Method and system for precision operational control of automated machines

This is a regular application of a provisional application having an application Ser. No. of 60/266,972 and a filing date of Feb. 6, 2001.

1. Field of the Invention

The present invention relates to a control method and system for automated machines, and more particularly to a precision operational control method and system for an automated machine, wherein output data of an inertial sensor package installed in the grip or claw of the automated machine are received and processed in a central control processor for the automated machine , with electromechanical servo actuator valves and the surrounding loop controls to precisely control the movement of a motion element such as a grip or claw of the automated machine.

2. Description of Related Arts

Various automated machines, such as robots, smart cranes, etc., are being developed to facilitate automated operations, production, and processes. However, it's still a challenge to design highly accurate operational controls for an automated machine.

The automated machine is configured with several major linkages, which produce the gross motion of the end effector. Without feeding back the motion sensed at the end effector, manipulation of its position and orientation has to be done at a slow speed and requires extensive operators'assistance.

It is a main objective of the present invention to provide a method and system for precision operational control of an automated machine, wherein a micro IMU(Inital Measuring Unit)(such as the U.S. patent application Ser. No. 09/477,151, filed Jan. 4, 2000, entitled “micro IMU”, now allowed, and the U.S. Patent pending application Ser. No. 09/624,366, filed Jul. 25, 2000, entitled “coremicro® IMU”) is incorporated, so that the acceleration, velocity, angle rate, and angle of the grip or claw of the automated machine are all commanded with high accuracy, and effects from the mechanical portions of the loop, such as the mechanical flexing and bending due to loading, and nonlinear torques due to hydraulic components are minimized.

Another objective of the present invention is to provide a method and system for precision operational control of an automated machine that enables autonomous/intelligent control of the automated machine's end effector by incorporating an 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 as mechanical flexing and bending due to loading and nonlinear torques due to hydraulic components.

Another objective of the present invention is to provide a method and system for precision operational control of an automated machine, wherein control loops are formed through the use of actuators and outputs of the IMU linked with software for enhanced performance and agile operation of the automated machine.

Another objective of the present invention is to provide a method and system for precision operational control of an automated machine, wherein the application aspects of the IMU is designed to enhance manual and automatic control of the machine. The use of the IMU was applied within the context of control loops that can enhance the machine's motion profiles.

Another objective of the present invention is to provide a method and system for precision operational control of an automated machine, wherein the IMU is totally self sufficient to provide all data necessary for motion measurement of the machine.

Another objective of the present invent is to provide a method and system for precision operational control of an automated machine, wherein the control loops are configured starting with an inner loop and expanding the functionality With successive layers of outer loops.

In order to accomplish the above objectives, an IMU is installed at an end effector of a motion element of an automated machine, fast-response feedback control for both position and angle servo-loops (for the end effector) greatly decreases the operational time needed to complete a preplanned trajectory. In addition, the closed-loop control design provides stabilization and isolation of the end effector from external disturbances

The application of the IMU as a sensor for providing velocity and position of the tool with respect to the base of the automated machine expressed in the navigation (earth fixed) frame requires the use of specific processing methods and software in order to form the navigation solution.

This unique navigation solution is based upon the use of a set of equations performing an open loop computation with the inertial data as its input. This formulation of equations requires a periodic update of the open loop solution in order to bound the growth of system errors. The source of this update is the automated machine position measurement derived from the mechanical sensors in the system.

The performance enhancements gained by obtaining an inertially referenced solution to the position data is to increase the devices bandwidth, remove the effects of mechanical compliance and resonance, and to provide an earth fixed reference for the sensed motion of the automated machine.

FIG. 1 is a block diagram illustrating a generic system configuration according to a preferred embodiment of the present invention.

FIG. 2 is an exploded perspective view illustrating an inertial sensor package according to the above preferred embodiment of the present invention.

FIG. 3 is a schematic view illustrating an application of the present invention for automated machine pointing stabilization.

FIG. 4 is a block diagram illustrating an acceleration loop according to the above preferred embodiment of the present invention.

FIG. 5 is a block diagram illustrating a velocity loop according to the above preferred embodiment of the present invention.

FIG. 6 is a block diagram illustrating a position loop according to the above preferred embodiment of the present invention.

FIG. 7 is a block diagram illustrating an angle rate loop according to the above preferred embodiment of the present invention.

FIG. 8 is a block diagram illustrating an angle loop according to the above preferred embodiment of the present invention.

Referring to FIGS. 1 to 8 , a system for precision operational control of automated machines according to a preferred embodiment of the present invention is illustrated, which comprises a motion element 5 such as a holding element like a grip or claw, an inertial sensor package 1 installed at an end effector of the motion element 5 for sensing and providing a motion measurement of the motion element 5 , a central control processor 2 receiving output of the inertial sensor package and producing commands. and a motion actuator 34 receiving the commands from the central control processor 2 to control the movement of the end effector of the motion element 5 .

According to the preferred embodiment as shown in FIG. 1, the motion actuator 34 is embodied to include one or more electromechanical servo actuator valves 3 and one or more hydraulic actuators 4 which is controlled by the electromechanical servo actuator valves 3 , wherein the central control processor 2 receives output of the inertial sensor package 1 and produces commands to the electromechanical servo actuator valves 3 to control the hydraulic actuators 4 for making the motion element 5 move.

Referring to FIG. 1, the operational process of the system comprises the steps of:

(1) sensing a motion and motion changes of the end effector 51 of the motion element 5 , i.e. the grip or claw, by the inertial sensor package 1 ;

(2) providing an accurate motion measurement of the end effector 51 of the motion element 5 through a navigation processing by the inertial sensor package to obtain measurement data;

(3) sending the measurement data from the inertial sensor package 1 to the central control processor 2 ;

(4) receiving the measurement data from the inertial sensor package 1 by the central control processor 2 ,

(5) comparing the measurement data with a command input to form error data;

(6) receiving the error data in the central control processor:

(7) producing a control signal by using a controller algorithm in the central control processor 2 ;

(8) sending the control signal to the motion actuator 34 to control speed and force outputs of motion actuator 34 ; and

(9) driving the end effector 51 of the motion element 5 (grip or claw) by the motion actuator 34 according to the control signal, wherein under the control, the error between the measured motion and the input command converges to zero. That is, the end effector of the motion element 5 moves along a trajectory as the input command requires. Because of a closed-loop control system and method are used, the interference to the end effector motion inside the loop is diminished.

According to the preferred embodiment, in the step (8), the control signal is sent to the electromechanical servo actuator valves 3 to control the hydraulic flow to the hydraulic actuators 4 (hydraulic motors) so that the speed and force outputs of the hydraulic actuators 4 are controlled.

Moreover, in the step (9), the end effector of the motion element 5 (grip or claw) is driven by the hydraulic actuators 5 according to the control signal, wherein under the control, the error

Referring to FIG. 1, with the addition of an inertial sensor package shown in FIG. 2, the electromechanical servo actuator valves A and the surrounding loop controls, the movement of the motion element 5 can be precisely controlled. Because the acceleration, velocity, angle rate, and angle are all commanded with high accuracy, effects from the mechanical portions of the loop control, such as the mechanical flexing and bending due to loading, and nonlinear torques due to hydraulic components are minimized. This enables an operator to obtain the best performance allowed by the mechanical segments of the mechanism.

FIG. 3 depicts an application of the present invention for pointing stabilization. The control system automatically reorients the end effector of the motion element 5 while translating the arm. So it keeps pointing to the object to be grasped, whether it is at position 1 , 2 or 3 . The inertial position is updated using only IMU's gyro/accelerometer outputs for feedback. The attitude errors (from an initial reference) are computed for roll/pitch/yaw servo-control. The benefits of a closed-loop control design using the IMU include: improved pointing stabilization for a vision system and speed of operations; ability to continuously orient the end effector of the motion element 5 to a fixed reference without a vision system; availability of inertial navigation solutions for other usages (e.g., collision avoidance, cargo handling, real-time trajectory-planning, etc.)

The feedback control system relies on the control processor to produce the commands to the electromechanical servo actuator valves 3 baser on data received from the inertial sensor package 1 ; the electromechanical servo actuator valves 3 control the hydraulic actuators 4 that make the end effector of the motion element 5 move, and the inertial sensor package 1 installed at the end effector of the motion element 5 senses the motion.

As an application example, the effector motion is described by the following specifications of its performance when sensed by the AGNC®- 2000 CMIMU coremicro® IMU (U.S. patent application, application Ser. No. 09/624,366, filed Jul. 25, 2000:

The velocity accuracy is derived assuming a 2 sec system update rate (0.5Hz). The assumed error in acceleration is 0.00125 g or 1.25 milig. Derived from the angular tilt of the leveling loop and the other long list of lessor errors.

0.00125 G*32.141 FT/sec**2/G*2 sec=0.0803 FT/SEC

The Position accuracy relative to the base is derived from the linear acceleration error of 0.001 G.

0.5*0.001 G*32.14 ft/sec/G*(2 sec)**2*12 in/ft=0.77 inch

The angular accuracy is derived from the RSS of the Angle rate noise, the axis misalignment, and the mounting angular misalignment.

The angle derived from the angle rate noise is 0.07 deg/sec/rt-Hz*(0.5 Hz)**0.5 *2 sec=0.0989 deg, where 0.5 Hz is the correlation time of 2 seconds or 0.5 Hz. Two seconds is the update rate for the time interval of the system. The mounting accuracy is 0.15 deg. The axis misalignment is derived from the uncompensated orthogonality error of 0.005 deg and the maximum turn rate of 100 deg/sec*2 sec for the update period.

0.005 deg*100 deg/sec*2 sec*0.017 rad/deg for the misalignment=0.017 Deg

Therefore the RSS of the terms is from 0.0989 deg, 0.15 deg, and 0.017 deg

SQRT ((0.0989**2)+(0.15**2)+(0.017**2))=0.18 deg of angular accuracy

The loop bandwidth is assumed to be limited by the characteristics of the electrohydraulic actuator valve in that the gyro bandwidth is about 700 Hz.

Note that performance enhancement can be realized without upgrading other costly components of the automated machine. By directly measuring the end effector motion, less costly linear and angular motion sensors for various links and joints can be utilized.

The incorporation of the IMU also engenders a variety of autonomous/intelligent operations for automated machines:

1. Optimized Trajectory Planning and Autonomous Operations—Making use of the motion state data for trajectory planning. After achieving stabilized response of the end effector of the motion element 5 , the operator no longer needs to closely monitor the motions of the machine, and input commands can be structured for semiautonomous/autonomous operations.

2. Safety/Fault Monitoring—Important safety monitoring can be achieved by comparing the short-term navigation solution derived from the inertial sensors package 1 with the position and velocity estimates derived from the mechanical sensors. When the two solutions differ by more than a small amount, the system can be shut down.

3. Inertial Reference Database—Navigation solution can be stored in a database for geographic referencing of handled cargo items. Inertial orientation of cargo can be determined by resolving optical image data into an earth-fixed navigation frame of reference. The data is also available for the operator's use in commanding the machine.

4. Adaptive Gain Scheduling—Cargo mass and moment of inertia can be determined by estimating the closed loop time constants online from motion command inputs and sensed motion outputs at the end effector of the motion element 5 . This information can then be used to schedule appropriate control gains to optimize performance.

5. Collision Avoidance—By making use of pre-existing navigation data, collision avoidance can be realized.

6. Robust Control—Operation of the end effector of the motion element 5 in a semiautonomous/autonomous mode is possible while the machine makes small maneuvering motions.

According to the preferred embodiment, the inertial sensor package 1 is embodied as an IMU which comprises an accelerometer 11 , a navigation module 12 , a fixed level arm 13 , a gyro 14 , and an AHRS (Altitude Heading Reference System) module 15 .

The application of the AGNC®-2000CMIMU coremicro® IMU or any other IMU to the automated machine includes the generation of control loops for enhanced performance and agile operation. These loops are formed through the use of actuators and the IMU linked with software. The inertial sensors package AGNC®-2000CMIMU coremicro® IMU for the automated machine application is totally self contained including miniature size IMU, wireless transmitter, case, and battery.

The actuation of the mechanical links of the crane is accomplished through hydraulic actuators that are controlled with electromechanical servo actuator valves. These valves are very low power devices with a large power gain. They are driven with the output of a typical op amp, with about 5 ma and −10 to +10 volts. These actuator valves can provide very predictable and linear source of pressure with a moderate bandwidth.

This configuration eliminates the inherent reliability problems found with cabling and connectors while lowering installation costs.

The loop controls are configured starting with an inner loop and expanding the functionality with successive layers of outer loop controls, comprising:

(a) an acceleration loop control;

(b) a velocity loop control;

(c) a position loop control;

(d) an angular rate loop control; and

(e) an angle loop control.

The primary inner loop control is a first order acceleration loop control as shown in FIG. 4 . The input consists of a commanded acceleration in three axes with the output acceleration sensed with the IMU. The input command is limited in order to restrict the magnitude of the force generated by the motion actuator 34 .

Referring to FIG. 4, the acceleration loop control 29 comprises accelerometer 11 of the IMU 1 for measuring real motion, a converter 21 for converting delta velocity data to acceleration data, a first limit 22 for restricting the magnitude of the force, a first comparator 23 for comparing the acceleration command and measured acceleration, a first amplifier 24 for signal amplifying, and an integrator 25 for signal interation. The processor produced voltage signal is sent to the motion actuator 34 for driving the end effector 51 .

The method of acceleration control processing comprises following steps:

(1) measuring an acceleration of the end effector 51 and producing delta velocity data by the accelerometer 11 of the IMU 1 ;

(2) sending the delta velocity data to the converter 21 in the central control processor 2 ;

(3) converting the delta velocity data to acceleration data;

(4) inputting the acceleration command signal and limiting it with the first limit 22 and producing bounded acceleration commands;

(5) comparing the limited input acceleration command with the measured acceleration and producing an acceleration error signal by the first comparator 23 ;

(6) simplifying the acceleration error signal by the first amplifier 24 and then integrating the amplified signal by the integrator 25 ;

(7) converting the output of the integrator 25 to an analog voltage signal and sending it to the motion actuator 34 ; and

(8) producing force according to the input signal by the motion actuator 34 and driving the end effector 51 to move so that the acceleration error converges to zero.

Based on the acceleration loop control 29 as shown in FIG. 4 . the velocity loop control 213 is built as shown in FIG. 5 . The velocity loop control 213 makes use of the acceleration loop control 29 as an inner loop control. This is the first order velocity loop control 213 as shown in FIG. 5 and is realized entirely within the processing module. With the implementation of this loop control. velocity is commanded with respect to the fixed base of the machine and is sensed by the combination of the IMU and navigation equations derived with the constraints of known mechanical links and with the aid of the angular rate loop control.

Referring to FIG. 5, the velocity processing loop control 213 comprises the IMU 1 and its navigation module 12 including processing algorithms for obtaining the real velocity of the end effector 51 , a second limit 26 for restricting the magnitude of the velocity, a second comparator 27 for comparing the velocity command and measured velocity, and a second amplifier 28 for signal amplifying. The velocity processing produced data are sent to the acceleration loop control 29 as the input acceleration command for driving the end effector 51 in which the velocity loop control 213 acts as an integrator to transform acceleration to velocity.

The method of velocity control processing comprises following steps:

(1) measuring a velocity of the end effector 51 by the navigation module of the IMU 1 ;

(2) processing the output data of IMU 1 by using the navigation processing algorithms of the navigation module 12 and producing velocity measurements of the end effector 51 ;

(3) limiting the input velocity command signal by the second limit 26 and producing limited velocity data;

(4) comparing the limited input velocity data with the measured velocity from the IMU 1 by the second comparator 27 and producing velocity error signal;

(5) amplifying the velocity error signal by the second amplifier 28 ;

(6) sending the output of the second amplifier 28 to the input of the acceleration loop control 29 ; and

(7) producing force by the motion actuator according to the input signal.

Through the acceleration loop control 29 and driving to the end effector 51 , motion is generated so that the velocity error converges to zero.

The position control loop control as illustrated in FIG. 6 makes use of the velocity loop control 213 as an inner loop. This is a first order position loop control as shown in FIG. 6 and is realized entirely within the processing module. With the implementation of this loop. position is commanded with respect to the fixed base of the machine and is sensed by the combination of the IMU 1 and navigation modules derived with the constraints of known mechanical links and the aid of the gimbal angles as sensed by the attitude derived in the AHRS equations.

Referring to FIG. 6, the position processing loop comprises the IMU 1 and its processing for obtaining the real position of the end effector 51 , a third limit 210 for restricting the magnitude of the position, a third comparator 211 for comparing the position command and measured position, and a third amplifier 212 for error signal amplifying. The position processing produced data are sent to the velocity loop control 213 as the input velocity command for driving the end effector 51 in which the position loop acts as an integrator to transform velocity to position.

The method of position control processing comprises following steps:

(1) measuring a position of the end effector 51 by the IMU 1 ;

(2) estimating the position by using a fixed lever arm parameter 13 ;

(3) processing the output data of the IMU 1 by using navigation algorithms and producing position measurement of the end effector 51 ;

(4) limiting the input position command signal by the third limit 210 and producing limited position data;

(5) comparing limited input position data with the measured position from the IMU 1 by the third comparator 211 and producing position error signal;

(6) amplifying the position error signal by the third amplifier 212 :

(7) sending the output of the third amplifier 212 to the input of the velocity loop control 213 .

Through the velocity loop control 213 , the motion actuator produces force according to the input signal and drives the end effector 51 to move so that the position error converges to zero.

The angular rate control loop consists of an inner loop and successive layers of outer loops. The primary inner loop is a first order angular rate loop as shown in FIG. 7 . The input consists of a commanded angular rate in three axis with the output angular rate sensed with the IMU. The input command is limited in order to restrict the magnitude of the torque generated by the motion actuator.

Referring to FIG. 7, the angular rate loop control 221 comprises the IMU 1 and its gyros 14 for obtaining a real angular rate of the end effector 51 , a fourth limit 215 for restricting the magnitude of the angular rate, a fourth comparator 216 for comparing the angular rate command and measured angular rate, and a fourth amplifier 217 for signal amplifying. The angular rate processing produced data are sent to the end effector 51 for driving the end effector 51 in which the angular rate loop control 221 acts as an integrator to transform angular acceleration to angular rate.

The method of angular rate control processing comprises following steps:

(1) measuring the angular motion of the end effector 51 by the gyros 14 in IMU 1 ;

(2) outputting angular data of the gyros in forms of delta angles;

(3) converting the delta angle data to angular rate data by an angular rate converter 214 ;

(4) limiting the input angular rate command signal by fourth limit 215 and producing limited angular rate data;

(5) comparing the limited input angular rate data with the measured angular rate from the angular rate converter 214 by the fourth comparator 216 and producing angular rate error signal;

(6) amplifying the angular rate error signal by the fourth amplifier 217 ;

(7) converting the output of the fourth amplifier 217 to analog signal and sending it to the input of motion actuator 34 : and

(8) producing torque/force that exerts on the end effect 51 by the motion actuator 34 and producing angular acceleration that makes the angular rate error converges to zero.

The angular acceleration is accumulated by the angular rate loop control 221 and produces angular rate. Through the angular rate loop control 221 , the motion actuator 34 produces force according to the input signal and drives the end effector 51 to move so that the angular rate error converges to zero.

The angle control loop control as shown in FIG. 8 makes use of the angular rate loop control 221 as an inner loop. This is the first order angle position loop as shown in FIG. 8 and is realized entirely within the processing module. With the implementation of this loop, angle is commanded with respect to the fixed base of the machine and is sensed by the combination of the IMU 1 and its AHRS module derived with the constraints of known mechanical limits on the gimbal angles and known gimbal angles on the links closer to the base.

Referring to FIG. 8, the angle loop control comprises the IMU 1 and its AHRS module for obtaining a real angle of the end effector 51 , a fifth limit 218 for restricting the magnitude of the angle, a fifth comparator 219 for comparing the angle command and measured angle, and a fifth amplifier 220 for signal amplifying. The angle processing produced data are sent to the angular rate loop control 221 for driving the end effector 51 in which the angle loop acts as an integrator to transform angular rate to angle.

The method of angle control processing comprises following steps:

(1) measuring the angular motion of the end effector 51 by the IMU 1 ;

(2) processing the output data of the gyros 14 by the AHRS module 15 and producing angle data of the end effector 51 ;

(3) limiting the input angle command signal by the fifth limit 218 and producing limited angle data;

(4) comparing the limited input angle data with the measured angle from the IMU 1 by the fifth comparator 219 and producing angle error signal;

(5) amplifying the angle error signal by the fifth amplifier 220 ;

(6) sending the output of the fifth amplifier 220 to the angular rate loop control 221 ; and

(7) producing torque/force by the angular rate loop control 221 that exerts on the end effect and producing angular rate that makes the angle error converges to zero.

The angular rate is accumulated by angle loop control which acts as an integrator and produces the angle. Through the angle loop control, the motion actuator produces force according to the input signal and drives the end effector 51 so that the angle error converges to zero.

The features of the present invention is summarized as following:

1. An IMU, such as the above mentioned AGNC®-2000CMIMU coremicro® IMU is used to provide data to greatly decrease operational time through the stabilization of the effector with respect to an earth fixed frame. Time can be minimized by doing trajectory planning which makes use of the motion state data. Stabilization and isolation of the effector from external motion disturbances can be achieved through the use of inertial data determined actuator response.

2. After achieving stabilized response of the effector, the operator no longer needs to be as closely involved in the motions of the crane. He can input commands that are structured for semiautonomous/autonomous operation.

3. Performance enhancement can be achieved without upgrading other costly components. By more closely monitoring effector motions, less costly linear and angular direct motion sensors can be utilized.

4. Important safety monitoring can be achieved by comparing the short term navigation solution derived from the mechanical sensors. This navigation solution will be very accurate over short two second intervals. When the two solutions differ by more than a small amount, the system can be shut down.

5. After forming a navigation solution, data is available for the operator's use in directing the machine.

6. Once navigation data is available, it can be stored into a data base each time an item of cargo is handled so that the geographic location of cargo can be retained for future reference.

7. Orientation of cargo can be determined by resolving optical image data into an earth fixed navigation frame of reference.

8. After the implementation of control loops with other existing system components, cargo mass can be determined by measuring loop time constants. This information can be used to schedule gains in order to optimize performance.

9. By making use of preexisting navigation data, collision avoidance can be realized.

10. Operation of the crane in a semiautonomous/autonomous mode is possible while the machine makes small maneuvering motions.