Method and system for precision operational control of automated machines

• US 6,463,357 B1
• Filed: 02/06/2002
• Issued: 10/08/2002
• Est. Priority Date: 02/06/2001
• Status: Expired due to Term

• Current Assignee: American GNC Corporation
• Original Assignee: American GNC Corporation
• Inventors: An, Dong, Li, ZongKe, McCall, Hiram, Lin, Ching-Fang
• Primary Examiner(s): Louis-Jacques, Jacques H.
• Assistant Examiner(s): Marc, McDieunel
• Application Number:
  US10/072,835
• Publication Number:
  US 20020147527A1
• Time in Patent Office: 244 Days
• Field of Search: 700/187, 700/245, 700/255, 700/172, 700/176, 700/180, 700/189, 700/190, 700/7, 700/76, 700/23, 700/50, 700/48, 700/33, 700/36, 700/29, 700/35, 318/567, 714/11, 714/819, 111/177
• US Class Current: 700/245
• CPC Class Codes: B25J 9/1651 acceleration, rate control
• CPC Class Codes: G05B 2219/39505 Control of gripping, grasping, contacting force, force distribution

Abstract

A system and method for precision operational control of automated machines includes a motion element, an IMU (Inital Measuring Unit) installed at an end effector of the motion element for sensing and providing a motion measurement of the motion element, a central control processor receiving output of the IMU and producing commands, and a motion actuator receiving the commands from the central control processor to control the movement of the end effector of the motion element, so as to enable autonomous/intelligent control of the automated machine'"'"'s end effector by incorporating the IMU to permit direct servo-control of the end effector'"'"'s acceleration, velocity, angular rate. and angle—this closed-loop system minimizes effects of such disturbances like mechanical flexing and bending due to loading and nonlinear torques due to hydraulic components.

Claims

1. An operational control system of an automated machine comprising a motion element having an end effector wherein said operational control system comprises:

an inertial sensor package, which is installed at said end effector of said motion element, sensing a motion and motion changes of said end effector and providing a motion measurement of said end effector through a navigation processing of said inertial sensor package to obtain measurement data;

a central control processor receiving said measurement data from said inertial sensor package and comparing said measurement data with a command input to form error data which is received in said central control processor to produce a control signal in said central control processor; and

a motion actuator receiving said control signal from said central control processor to control speed force outputs of said motion actuator and driving said end effector of said motion element by said motion actuator according to said control signal, wherein errors between said motion being measured and said command input converges to zero, so as to ensure said end effector of said motion element moves along a trajectory as said command input requires.

2. The system, as recited in claim 1, wherein said motion actuator includes one or more electromechanical servo actuator valves and one or more hydraulic actuators which are controlled b) said electromechanical servo actuator valves, wherein said central control processor receives output of said inertial sensor package and produces commands to said electromechanical servo actuator valves to control said hydraulic actuators for making said motion element move.

3. The system, as recited in claim 2, wherein said control signal is sent to said electromechanical servo actuator valves to control a hydraulic flow to said hydraulic actuators so that speed and force outputs of said hydraulic actuators are controlled.

4. The system, as recited in claim 3, wherein said end effector of said motion element is driven by said hydraulic actuators according to said control signal.

5. The system, as recited in claim 1, further comprising an acceleration loop control which comprises accelerometer of said inertial sensor package for measuring real motion, a converter for converting delta velocity data to acceleration data, a first limit for restricting said magnitude of said force, a first comparator for comparing said acceleration command and measured acceleration, a first amplifier for signal amplifying, and an integrator for signal interation, wherein a produced voltage signal is sent to the motion actuator for driving said end effector.

6. The system, as recited in claim 5, wherein said acceleraterometers of said inertial sensor package measures acceleration of said end effector and produces delta velocity data, wherein said delta velocity data is sent to said converter in said central control processor to convert to acceleration data, wherein said acceleration data is inputted in said first limit so as to limit said acceleration data and produce acceleration commands, wherein said limited input acceleration commands are compared with said measured acceleration to produce an acceleration error signal by said first comparator, wherein said acceleration error signal is simplified by said first amplifier to form an amplified signal which is then integrated by said integrator. wherein an output of said integrator is converted to an analog voltage signal which is sent to said motion actuator to produce force according to said analog voltage signal by said motion actuator to drive said end effector to move while said acceleration error converges to zero.

7. The system, as recited in claim 6, further comprising a velocity loop control which makes use of said acceleration loop control as an inner loop control, wherein said velocity processing loop control comprises said inertial sensor package and a navigation module thereof for obtaining a real velocity of said end effector, a second limit for restricting said magnitude of said velocity, a second comparator for comparing a velocity command and said measured velocity, and a second amplifier for signal amplifying, wherein velocity processing produced data are sent to said acceleration loop control as said input acceleration command for driving said end effector, wherein said velocity loop control acts as an integrator to transform acceleration to velocity.

8. The system as recited in claim 7, wherein said velocity of said end effector is measured by said navigation module in said inertial sensor package, wherein said out put data of said inertial sensor package is processed by using said navigation module to produce velocity measurement of said end effector. wherein an input velocity signal is limited by said second limit to produce limited velocity data wherein said limited velocity data is compared with said measured velocity from said inertial sensor package by said second comparator to produce a velocity error signal, wherein said velocity error signal is amplified by said second amplifier, wherein an output of said second amplifier is sent to an input of said acceleration loop control to produce force by said motion actuator according to said input signal, wherein through said acceleration loop control and driving to said end effector said motion is generated while said velocity error converges to zero.

9. The system. as recited in claim 8, further comprising a position control loop control which makes use of said velocity loop control as an inner loop wherein said position processing loop comprises said inertial sensor package and a processing for obtaining a real position of said end effector, a third limit for restricting said magnitude of said position, a third comparator for comparing said position command and measured position, and a third amplifier for error signal amplifying. Said position processing produced data are sent to said velocity loop control as said input velocity command for driving said end effector in which said position loop acts as an integrator to transform velocity to position.

10. The system. as recited in claim 9, wherein a position of said end effector is measured by said inertial sensor package so as to estimating said position by using a fixed lever arm parameter of said inertial sensor package. where in said output data of said inertial sensor package is processed to produce a position measurement of said end effector, wherein said position measurement is limited by said third limit to produce limited position data, wherein said limited position data is compared with said measured position from said inertial sensor package by said third comparator to produce a position error signal which is amplified by said third amplifier. wherein an output of said third amplifier is sent to an input of said velocity loop control, wherein through said velocity loop control, said motion actuator produces force according to said input signal and drives said end effector to move while said position error converges to zero.

11. The system as recited in claim 10, further comprising an angular rate loop control which comprises said inertial sensor package and gyros thereof for obtaining a real angular rate of said end effector, a fourth limit for restricting said magnitude of said angular rate, a fourth comparator for comparing said angular rate command and measured angular rate, and a fourth amplifier for signal amplifying, wherein angular rate processing produced data are sent to said end effector for driving said end effector wherein in said angular rate loop control acts as an integrator to transform angular acceleration to angular rate.

12. The system, as recited in claim 11, wherein an angular motion of said end effector is measured by said gyros in said inertial sensor package to output angular data in forms of delta angles, wherein said delta angle data is converted by an angular rate converter to angular rate data which is limited by said fourth limit to limited angular rate data. wherein said limited angular rate data in compared with said measured angular rate from said angular rate converter by said fourth comparator to produce an angular rate error signal, wherein said angular rate error signal is amplified by said fourth amplifier, wherein an output of said fourth amplifier is converted to analog signal and sent to said input of said motion actuator to produce torque and force that exerts on said end effect by said motion actuator and produce an angular acceleration that makes said angular rate error converges to zero, wherein said angular acceleration is accumulated by said angular rate loop control to produce angular rate.

13. The system, as recited in claim 12, further comprising an angle control loop control which makes use of said angular rate loop control as an inner loop, wherein said angle loop control comprises said inertial sensor package and an AHRS (Altitude Heading Reference System) module for obtaining a real angle of said end effector a Fifth limit for restricting said magnitude of said angle a fifth comparator for comparing said angle command and measured angle. and a fifth amplifier for signal amplifying. Said angle processing produced data are sent to said angular rate loop control for driving said end effector in which said angle loop acts as an integrator to transform angular rate to angle.

14. The system, as recited in claim 13, wherein an angular motion of said end effector is measured by said inertial sensor package, wherein output data of said gyros is processed by said AHRS (Altitude Heading Reference System) module to produce angle data of said end effector which is limited by said fifth limit to produces limited angle data, wherein said limited angle data is compared with said measured angle from said inertial sensor package by said fifth wherein an output of said fifth amplifier is sent to said angular rate loop control to produce a torque and force by said angular rate loop control that exerts on said end effect and produces angular rate that makes an angle error converges to zero.

15. The system, as recited in claim 1, further comprising an angular rate loop control which comprises said inertial sensor package and gyros thereof for obtaining a real angular rate of said end effector. a fourth limit for restricting said magnitude of said angular rate, a fourth comparator for comparing said angular rate command and measured angular rate, and a fourth amplifier for signal amplifying, wherein angular rate processing produced data are sent to said end effector for driving said end effector wherein in said angular rate loop control acts as an integrator to transform angular acceleration to angular rate.

16. The system, as recited in claim 15, wherein an angular motion of said end effector is measured by said gyros in said inertial sensor package to output angular data in forms of delta angles, wherein said delta angle data is converted by an angular rate converter to angular rate data which is limited by said fourth limit to limited angular rate data. Wherein said limited angular rate data in compared with said measured angular rate from said angular rate converter by said fourth comparator to produce an angular rate error signal, wherein said angular rate error signal is amplified by said fourth amplifier. wherein an output of said fourth amplifier is converted to analog signal and sent to said input of said motion actuator to produce torque and force that exerts on said end effect by said motion actuator and produce an angular acceleration that makes said angular rate error converges to zero, wherein said angular acceleration is accumulated by said angular rate loop control to produce angular rate.

17. The system, as recited in claim 16, further comprising an angle control loop control which makes use of said angular rate loop control as an inner loop, wherein said angle loop control comprises said inertial sensor package and an AHRS (Altitude Heading Reference System) module for obtaining a real angle of said end effector. a fifth limit for restricting said magnitude of said angle, a fifth comparator for comparing said angle command and measured angle, and a fifth amplifier for signal amplifying. Said angle processing produced data are sent to said angular rate loop control for driving said end effector in which said angle loop acts as an integrator to transform angular rate to angle.

18. The system as recited in claim 17, wherein an angular motion of said end effector is measured by said inertial sensor package, wherein output data of said gyros is processed by said AHRS (Altitude Heading Reference System) module to produce angle data of said end effector which is limited by said fifth limit to produces limited angle data, wherein said limited angle data is compared with said measured angle from said inertial sensor package by said fifth comparator to produce an angle error signal which is amplified by said fifth amplifier, wherein an output of said fifth amplifier is sent to said angular rate loop control to produce a torque and force by said angular rate loop control that exerts on said end effect and produces angular rate that makes an angle error converges to zero.

19. A method of operational control of an automated machine which comprises a motion element having an end effector, wherein said method comprises the steps of:

(a) sensing a motion and motion changes of said end effector of said motion element by an inertial sensor package installed at said end effector,

(b) providing a motion measurement of said end effector of said motion element through a navigation processing by said inertial sensor package to obtain measurement data;

(c) sending said measurement data from said inertial sensor package to a central control processor;

(d) receiving said measurement data from said inertial sensor package by said central control processor;

(e) comparing said measurement data with a command input to form error data;

(f) receiving said error data in said central control processor;

(h) producing a control signal by using a controller algorithm in said central control processor;

(i) sending said control signal to a motion actuator to control speed and force outputs of motion actuator; and

(j) driving said end effector of said motion element by said motion actuator according to said control signal, wherein an error between said measured motion and said command input of said motion actuator converges to zero, so as to ensure said end effector of said motion element moves along a trajectory as said command input requires.

20. The method, as recited in claim 19, wherein said motion actuator comprises one or more hydraulic actuators and one or more electronmechical servo valves, wherein said control signal is sent to said electromechanical servo actuator valves to control a hydraulic flow to said hydraulic actuators so as to control speed and force outputs of said hydraulic actuators.

21. The method, as recited in claim 19, further comprising an acceleration loop control which comprises the steps of:

(a.1) measuring an acceleration of said end effector and producing delta velocity data by accelerometer provided in said inertial sensor package;

(a.2) sending said delta velocity data to a converter in said central control processor;

(a.3) converting said delta velocity data to acceleration data;

(a.4) inputting and limiting said acceleration data with a first limit and producing limited acceleration commands;

(a.5) comparing said limited input acceleration command with said measured acceleration and producing an acceleration error signal by a first comparator;

(a.6) simplifying said acceleration error signal by a first amplifier and then integrating said amplified signal by an integrator;

(a.7) converting an output of said integrator to an analog voltage signal and sending said analog voltage signal to said motion actuator; and

(a.8) producing a force according to said analog voltage signal by said motion actuator and driving said end effector to move while an acceleration error converges to zero.

22. The method, as recited in claim 21, further comprising a velocity loop control which makes use of said acceleration loop control as an inner loop control and comprises the steps of:

(b.1) measuring a velocity of said end effector by a navigation module of said inertial sensor package;

(b.2) processing said output data of inertial sensor package by using said navigation and producing velocity measurements of said end effector;

(b.3) limiting said velocity measurements by a second limit and producing limited velocity data;

(b.4) comparing said limited velocity data with said measured velocity from said inertial sensor package by a second comparator and producing a velocity error

(b.5) amplifying said velocity error signal by a second amplifier;

(b.6) sending an output of said second amplifier to an input of said acceleration loop control; and

(b.7) producing a force by said motion actuator according to said in put signal, wherein through said acceleration loop control and driving to said end effector, a motion is generated while said velocity error converges to zero.

23. The method, as recited in claim 22, further comprising a position control loop control which makes use of said velocity loop control as an inner loop and comprises the steps of.

(c.1) measuring a position of said end effector by said inertial sensor package

(c.2) estimating said position by using a fixed lever arm parameter;

(c.3) processing said output of said inertial sensor package by using navigation algorithms and producing a position measurement of said end effector;

(c.4) limiting said position measurement by a third limit and producing limited position data;

(c.5) comparing said limited position data with said measured position from said inertial sensor package by a third comparator and producing a position error signal;

(c.6) amplifying said position error signal by a third amplifier; and

(c.7) sending an output of said third amplifier to an input of said velocity loop control, wherein through said velocity loop control, said motion actuator produces a force to drive said end effector to move while said position error converges to zero.

24. The method, as recited in claim 23, further comprising an angular rate control loop which comprises the steps of:

(d.1) measuring an angular motion of said end effector by gyros provided in said inertial sensor package;

(d.2) outputting angular data of said gyros in forms of delta angles;

(d.3) converting said delta angle data to angular rate data by an angular rate converter;

(d.4) limiting said angular rate data by a fourth limit and producing limited angular rate data;

(d.5) comparing said limited angular rate data with said measured angular rate from said angular rate converter by a fourth comparator and producing an angular rate error signal;

(d.6) amplifying said angular rate error signal by a fourth amplifier;

(d.7) converting an output of a fourth amplifier to an analog signal and sending said analog signal to an input of said motion actuator; and

(d.8) producing a torque and force that exerts on said end effect by said motion actuator and producing an angular acceleration that makes said angular rate error converges to zero.

25. The method, as recited in claim 24, further comprising an angle control loop control which makes use of said angular rate loop control as an inner loop and comprises the steps of:

(e.1) measuring an angular motion of said end effector by said inertial sensor package,

(e.2) processing said output data of said gyros by an AHRS (Altitude Heading Reference System) module provided in said inertial sensor package and producing angle data of said end effector;

(e.3) limiting said angle data by a fifth limit and producing limited angle data;

(e.4) comparing said limited angle data with said measured angle from said inertial sensor package by a fifth comparator and producing an angle error signal;

(e.5) amplifying said angle error signal by a fifth amplifier;

(e.6) sending an output of said fifth amplifier to said angular rate loop control; and

(e.7) producing a torque and force by said angular rate loop control that exerts on said end effect and producing an angular rate that makes said angle error converges to zero.

26. The method, as recited in claim 19, further comprising an angular rate control loop which comprises the steps of:

(d.1) measuring an angular motion of said end effector by gyros provided in said inertial sensor package;

(d.2) outputting angular data of said gyros in forms of delta angles;

(d.3) converting said delta angle data to angular rate data by an angular rate converter;

(d.4) limiting said angular rate data by a fourth limit and producing, limited angular rate data;

(d.5) comparing said limited angular rate data with said measured angular rate from said angular rate converter by a fourth comparator and producing an angular rate error signal;

(d.6) amplifying said angular rate error signal by a fourth amplifier;

(d.7) converting an output of a fourth amplifier to an analog signal and sending said analog signal to an input of said motion actuator; and

(d.8) producing a torque and force that exerts on said end effect of said motion actuator and producing an angular acceleration that makes said angular rate error converges to zero.

27. The method, as recited in claim 26, further comprising an angle control loop control which makes use of said angular rate loop control as an inner loop and comprises the steps of:

(e.1) measuring an angular motion of said end effector by said inertial sensor package;

(e.2) processing said output data of said gyros by an AHRS (Altitude Heading Reference System) module provided in said inertial sensor package and producing angle data of said end effector;

(e.3) limiting said angle data by a fifth limit and producing limited angle data;

(e.4) comparing said limited angle data with said measured angle from said inertial sensor package by a fifth comparator and producing an angle error signal;

(e.5) amplifying said angle error signal by a fifth amplifier;

(e.6) sending an output of said fifth amplifier to said angular rate loop control; and

(e.7) producing a torque and force by said angular rate loop control that exerts on said end effect and producing an angular rate that makes said angle error converges to zero.