# Patent

## Title
Coordinated joint motion control system with position error correction

## Abstract

Disclosed are an articulated hydraulic machine supporting, control system and control method for same. The articulated hydraulic machine has an end effector for performing useful work. The control system is capable of controlling the end effector for automated movement along a preselected trajectory. The control system has a position error correction system to correct discrepancies between an actual end effector trajectory and a desired end effector trajectory. The correction system can employ one or more absolute position signals provided by one or more acceleration sensors supported by one or more movable machine elements. Good trajectory positioning and repeatability can be obtained. A two joystick controller system is enabled, which can in some cases facilitate the operator's task and enhance their work quality and productivity.

## Background
CROSS-REFERENCE TO RELATED APPLICATIONS
This is a divisional application of U.S. patent application Ser. No. 14/105,044, filed Dec. 12, 2013, which is a divisional application of U.S. patent application Ser. No. 13/271,774, filed Oct. 12, 2011, now abandoned, which is a divisional application of U.S. patent application Ser. No. 11/333,994, filed Jan. 18, 2006, now U.S. Pat. No. 8,065,060, all of which applications are hereby incorporated by reference.


ACKNOWLEDGEMENT OF GOVERNMENT SUPPORT
This invention was made with government support under DOE Grant No. DE-FC26-04NT42087 awarded by the U.S. Department of Energy. The government has certain rights in the invention.


STATEMENT REGARDING FEDERALLY SPONSORED RESEARCH OR DEVELOPMENT
(Not applicable.)
FIELD OF THE INVENTION
The present invention relates to a computerized control system for controlling hydraulic machinery of the type having an articulated arm carrying an end effector or tool at its extremity The invention is particularly, although not exclusively, applicable to control of coordinated joint systems, for example as employed in excavating equipment and certain three-dimensional image, generators such as anatomic simulators. One non-limiting example of a machine that can benefit from the control systems and methods of the invention is a power shovel having an articulated arm supported from a rotatable turret and carrying a shovel or bucket at its extremity.
BACKGROUND OF THE INVENTION
Computer-assisted operator control of hydraulic machinery requires stable, smooth machine response. Danko U.S. patent application Ser. No. 10/488,011, Publication No. 20040267404, “referenced Danko '404” hereinafter, entitled “Coordinated joint motion control system”, the entire disclosure of which application is hereby incorporated herein by reference thereto, discloses a coordinated joint motion control system for hydraulic machinery and the like. The disclosed control system employs a software-based kinematics reconfiguration system to assist the operator with various tasks. Danko '404's disclosure includes a differential, forward-prediction kinematics solution for adjusting the as-built machine hardware kinematics to an as-desired kinematics which provides desirable, reference actuator velocities and can yield a resultant motion trajectory that is well suited for the task specified. The systems and methods disclosed in Danko '404 are believed useful for their intended purposes, but, as is understood by the present invention the do not address the question of how to compensate for imperfect control system components.
Implementation of kinematics transformation from a virtual model in the digital domain to a real world tool trajectory is a difficult task because of the reliance on imperfect mechanical sensors and actuators. Energy loss reduction is a significant consideration for heavy equipment, mobile hydraulics control systems for excavators and the like may employ simplified control loops having higher energy efficiencies than do other control systems, for example hydraulic robot control systems. However, response time and precision may be compromised in such simplified hydraulic control loops. Generally, it is to be expected that the load-dependent valve and hydraulics control response characteristics will be non-linear. What are known as “measured valves” have been proposed to linearize the control valves and eliminate load-dependency by applying closed control loops around the valves. In Danko '404 a drawback of this approach, is that the hydraulic circuit may not effectively and timely apply the desired control velocity to the machine elements or links, owing to hydraulic flow inertia or pressure deficiencies that may arise.
Pursuant to the present invention it can be understood that it would be desirable to have accurate position information regarding the moving machine elements and especially regarding the end effector or tool. Further, it is an understanding of the invention that such position information for a given point in time and provided continuously throughout machine operation could be useful in enhancing control system performance.
A number of solutions have been proposed to the general problem of system identification i.e. determination of a system space-state at a given point in time from a noisy observation signal. For example Kalman, R. E., in “A New Approach to Linear Filtering and Prediction Problems,” Journal of Basic Engineering, 82 (Series D), 1960, pp. 35-45, describes an iterative method for developing a state-space system model during the solution. Various methods for identifying a causal linear or non-linear system are given by Schetzen, M., The Volterra and Wiener Theories of Nonlinear Systems, John Wiley and Sons, 1980, including convolution and Volterra series-based solutions.
Hypothetically, it might be possible to construct a priori a multi-variable, nonlinear machine model, at least for an unloaded machine, that could predict the motion of the machine under arbitrary control of all actuators. A non-iterative, Volterra-series, linear or non-linear, multi-variate system identification method could be applied. A drawback of such methods is that they may be computationally too extensive to be effectively used in embedded real-time control applications, where model computation time is significantly limited. A priori system model developments are described by Juang, J.-N. and Phan, M. in “Identification of System, Observer, and Controller from Closed-loop Experimental Data,” Journal of Guidance, Control, and Dynamics, Vol. 17, No. 2, January-February 1994, pp. 91-96. and by Phan et al. in “Improvement of Observer/Kalman Filter Identification (OKID) by Residual Whitening,” Journal of Vibrations and Acoustics, Vol. 117, April 1995, pp. 223-238. An observer-controller identification method from measurements under closed-loop control is given by Juang and Phan [1994]. A state-space dynamic system model identification from measured input-output data processed first as observer Markov parameters is described by Phan et al., “Markov Parameters in System Identification: Old and New Concepts,” Structronic Systems: Smart Structures, Devices, and Systems, Vol. 2, Tzou, H. S. and Guran A. (eds.), World Scientific, Singapore, 1997, pp. 263-293.
A direct, adaptive system identification approach is described by Phan et al. in “Unifying Input-Output and State-Space Perspectives of Predictive Control,” Department of Mechanical and Aerospace Engineering Technical Report No. 3044, Princeton University, September, 1998 for predictive control of a flexible and lightly damped system with complex dynamics. Such an adaptive/predictive state-space control system component may be identified in the presence of unknown disturbances from measurements, as demonstrated by Goodzeit, N. E. and Phan, M. Q. in “System and Periodic Disturbance Identification for Feedforward-Feedback Control of Flexible Spacecraft,” Proceedings of the 35th AIAA Aerospace Science Meeting and Exhibit, Reno, Nev., January 1997. Goodzeit N. E. and Phan point to advantages that may be obtained by simplifying a control compensation task by excluding responses to selected disturbances. Such methods can avoid the saturation of the adaptive control system with too many disturbance-corrupted signals and can be described as “clear-box” adaptive control methods. However, none of these proposals is entirely adequate to compensate for the mechanical inefficiencies that may arise in articulated hydraulic machinery under time- and task-variable, external loads.
The foregoing description of background art may include insights, discoveries, understandings or disclosures, or associations together of disclosures, that were not known to the relevant art prior to the present invention but which were provided by the invention. Some such contributions of the invention may have been specifically pointed out herein, whereas other such contributions of the invention will be apparent from their context. Merely because a document may have been cited here, no admission is made that the field of the document, which may be quite different from that of the invention, is analogous to the field or fields of the present invention.
BRIEF SUMMARY OF THE INVENTION
In one aspect the invention provides an articulated hydraulic machine supporting an end effector for performing useful work, the articulated hydraulic machine comprising a control system capable of controlling the end effector for automated movement along a preselected trajectory. Beneficially, the control system comprises a position error correction system to correct discrepancies between an actual end effector trajectory and a desired end effector trajectory. The error correction module can:

    
    
        a) compares the actual end effector position at a given point in time with a reference position appropriate for the desired end effector trajectory;
        b) generates a difference between the actual end effector position and the reference position; and
        c) applies a correction signal to the machine to reduce the difference.
    
    


Various useful features can be employed in practicing the invention to facilitate the achievement of one or more of its objectives. For example, the reference position can be modeled for a hypothetical model machine under identical control command to that of the real machine, save for the corrections. The control system can comprise two operational parts respectively capable of separately processing a reference position vector and an actual position vector and these may be connected using a closing control loop which compares the actual position vector with the reference position vector and generates the correction signal.
In one embodiment, the machine provides to the control system absolute position information regarding at least one movable machine element to facilitate determination of the end effector position. The machine can have multiple movable machine components contributing to movement of the end effector and the absolute position information can comprise absolute position information regarding each movable machine element. The position error correction system can comprise one or more of a joint control model, a machine actuators model and a dynamic machine model. If desired, the position error correction system can be effective to correct velocity error.
The machine can include one or more acceleration sensors, each supported by a movable machine element to provide absolute position signals to the control system regarding the respective machine element. If desired, an accelerometer can be provided at or on each movable machine element that contributes to end effector positioning.
In a further embodiment, the control system employs model-based forward prediction to predict a next movement of a machine element and scale the predicted movement to provide desired movement. The control system can apply the correction signal kinematically, providing continual incremental adjustments while the machine is moving to execute a specified end effector trajectory.
The machine can be a mining machine, a construction machine, an excavator, a front loader a mechanical shovel or other machine capable of benefiting from the novel control system described herein. In general, the invention can be utilizes with machines employing operator-directed repetitive motion of an articulated limb. Such machines may be intended for any of a wide variety of uses including industrial, domestic and personal uses.
The invention also provides a control system for a machine as described, which control system comprises a position error correction module to correct discrepancies between the actual end effector trajectory and the desired end effector trajectory. The error correction module can compare the actual end effector position at a given point in time with a reference position appropriate for the desired end effector trajectory. Also, the error correction module can generate a difference signal indicating a difference between the actual end effector position and the reference position and can apply a correction signal to the machine to reduce the difference.
In another aspect the invention provides a method of controlling a machine such as is described above. The method can comprise prescribing a desired tool or end effector position to the controlled machine, comparing one or more reference positions appropriate for achieving the desired tool position with one or more actual machine element positions determined from an absolute sensor signal, generating a correction and applying the correction to improve the actual position.
The present invention recognizes, inter alia, that benefits can be obtained by providing error compensation in the execution of the reference velocities and trajectory in the methods of Danko '404, and provides an embedded, dynamic model that can be applied for this task.
The control systems and methods of some embodiments of the invention enable the automated generation of useful working tool trajectories by articulated hydraulic machines and the like with considerable precision and repeatability. Such novel control systems can relieve an operator of a number of tedious, repetitive control functions permitting the operator to concentrate on overall machine positioning, trajectory selection and fine-tuning or trimming of tool action, and other concomitant functions.
The novel systems and methods of the invention enable the achievement of precise control with good absolute spatial positioning and good motion trajectory, as well as accurate repeatability by means of the application of correction feedback techniques. Danko '404 refers to correcting or linearizing individual control elements independently. More general correction with fewer independent correction feedback component loops is desirable and attainable in embodiments of the present invention.
Furthermore, the invention contemplates providing additional sensory inputs from absolute, as opposed to relative, link positions to facilitate adaptive control error compensation. Pursuant to clear box concepts, other signal errors, regarded as not consequential, may be tolerated by design.



BRIEF DESCRIPTION OF THE SEVERAL VIEWS OF THE DRAWING
Some embodiments of the invention, and of making and using the invention, as well as the best mode contemplated of carrying out the invention, are described in detail below, by way of example, with reference to the accompanying drawings, in which like reference characters designate like elements throughout the several views, and in which:
FIG. 1 is a schematic side elevation of a known articulated hydraulic machine, in this case a powered shovel, which machine is suitable for being controlled by a coordinated joint motion control system according to the present invention;
FIG. 2 is a schematic view of a known motion control system such as is disclosed in Danko '404 which is useful for controlling the powered shovel shown in FIG. 1;
FIG. 3 is a block flow diagram of a continuous differential control architecture according to Danko '404 for operating the motion control system illustrated in FIG. 2;
FIG. 4 is a block flow diagram of a differential control architecture including dynamic machine modeling for position error correction according to one embodiment of the present invention, which control architecture is also useful for operating the motion control system illustrated in FIG. 2;
FIG. 5 is a side elevation of the trajectory of a small component of the end effector of the shovel shown in FIG. 1, with imperfect and uncompensated motion control values, as it might appear when operated by a known control architecture such as that shown in FIG. 3;
FIG. 6 is a side elevation of the trajectory of a small component of the end effector of the shovel shown in FIG. 1, with imperfect and uncompensated motion control values, as it might appear when operated by a control architecture according to an embodiment of the present invention, such as that shown in FIG. 4;
FIG. 7 is a schematic view of a bucket manipulable by a hydraulic machine such as that shown in FIG. 1 and of an undulating trajectory exhibited by an edge of the bucket as the bucket is moved;
FIG. 8 is a perspective view of a lefthand joystick useful for manipulating the bucket in the manner indicated in FIG. 7; and
FIG. 9 is a perspective view of a righthand joystick useful for manipulating the bucket in the manner indicated in FIG. 7.



DETAILED DESCRIPTION OF THE INVENTION
To provide an enhanced control system for articulated hydraulic machines such as power shovels and excavators, the invention uses a simple, approximate, dynamic machine model useful for real-time control applications. Some embodiments of the novel control system of the invention provide a simple predictive control with embedded system identification. The system identification can provide real-time identification of actual positions of movable elements of the machine at given points in time. The technique does not require pre-calibration or a priori model formulation, although such methods may be employed, if desired.
The invention is particularly, although not exclusively, applicable to controlling an articulated hydraulic machine comprising a coordinated joint motion system which coordinated joint motion system includes: a support; multiple links; multiple joints, optionally revolute or prismatic joints, connecting the links one to another and to a support, each joint permitting relative movement between the connected members; multiple actuators to effect the relative movement between the connected members, the multiple actuators being controlled by the coordinate joint control system; and an end effector supported by the jointed links for movement relative to the support.
Usefully, in such an exemplary application, the coordinated joint motion system can be capable of execution of an automated end effector trajectory without human intervention. Also, if desired, the coordinated joint control system can comprise an operator interface enabling a human supervisor to change the end effector motion or position during execution of the automated trajectory.
The machine's control system can comprise an internal feedback loop to determine a mathematical model of the coordinated joint motion system and provide a model-based forward predictor for directly controlling the joint actuators, optionally by employing a differential control architecture.
In one inventive embodiment, the dynamic machine model is used in a differential control architecture in which the motion kinematics of the machine is software controlled. The control method retains the advantages of the differential kinematics transformation with which continuous operator control is provided, such as is disclosed and claimed by Danko '404, by controlling transformed actuator velocities. At the same time, in this embodiment of the invention, closed-loop position control can provide for the cancellation of motion error caused by machine inertia, control system-component imperfections, repeatability and other errors.
The dynamic machine model can be continuously updated during motion, providing for adjustments to model errors caused by the approximations inherent in the modeling process. Some of these approximations may include: the replacement of the true cross-effects between machine links with time dependent adjustments; time-averaging of the model coefficients; the treatment of non-linearity; and approximate solution techniques.
One useful embodiment of the model space-state identification process applies least square fit between modeled and measured link positions, resulting in a natural depression of measurement noise. This simple technique does not require any pre-assumption about noise statistics.
Referring to FIG. 1, the powered shovel shown, to which the control systems and methods of the invention can be applied, which in this case is a front end shovel loader, comprises a tracked motive under carrier or ground-engaging system 101 supporting a variable-slew rotatable turret 102. Rotatable turret 102 has a cab 103 to house a human supervisory operator and supports one end of an articulated positioning unit 104 for manipulating a tool or other end effector supported at the other end of positioning unit 104.
Positioning unit 104 comprises a boom 105 articulated to turret 102 for rotation about a pivot 106, thereby providing a boom joint. Positioning unit 104 further comprises a lever arm 108 one end of which is articulated to boom 104 for rotation about a pivot 110, thereby providing an arm joint, while the other end is articulated to and supports a tool, in this case a shovel or bucket 112, for rotation about a bucket pin 114, thereby providing a tool joint. The described rotations are to be understood as all being about axes perpendicular to the plane of the paper. While the example illustrated in the drawings comprises revolute joints having one or multiple degrees of freedom, other joints may be utilized, for example prismatic joints.
The tilt or orientation of shovel 112 is preferably also variable, for which purpose shovel 112 can be mounted with two further degrees of rotational freedom about axes in the plane of the paper by means (not shown). Such further shovel rotational means may comprise, for example, a first pivotal attachment to the distal end of arm 108 providing rotation about a longitudinal arm axis extending through pivots 110 and 114 and a second axis perpendicular thereto, also extending through bucket pin 114.
Piston-and-cylinder hydraulic actuators 116, 118 and 120 control the motion and position of boom 105, arm 108 and shovel or bucket 112, respectively. Each actuator 116-120 is pivotally secured at its ends and cylinder 120 is connected to shovel 112 through an articulated link 122 to enhance its leverage. One or more leveling, guide or ancillary cylinders such as leveling cylinder 124 may be provided if desired, but may be rendered unnecessary by the software kinematics configuration capabilities of the present invention.
Referring to FIG. 2, other actuators such as 126 can optionally also be provided for moving and positioning other elements or systems, for example turret 102, and ground-engaging system 101, as will be further described hereinbelow.
In the motion control system illustrated in FIG. 2, each actuator 116-120 and 126 is operated by its own, local hydraulic control unit 130-136, respectively, which control unit receives a signal from a joint control coordinator 138b and controls its associated actuator 116, 118, 120 or 126 to provide a suitable output. One example of a suitable output comprises a desired position and/or velocity of the associated articulated element 104, 108, 112 or 128. This output is determined by the joint control coordinator 138b based on the forward input from the differential control kinematics generator 138a and the feedback input from the machine differential kinematics feedback generator 138c. 
Each articulated element 104, 108, 112 or 128 is provided with a transducer sensor 141, 143, 145 or 147, respectively, to detect preferably at least the position of the respective articulated element 104, 108, 112 or 128 with respect to its supporting element, and report the sensed position, preferably as a continuous or time-divided signal, to the machine differential kinematics feedback generator 138c. 
A control box 140 comprises a suitable number of manually operable control members or joysticks 142 which can be moved, pressured or otherwise manipulated by a human supervisor 144 to move the boom 104 and/or arm 108 and/or shovel 112, and/or other elements, according to a desired trajectory. Joysticks 142 may be the same or different and may comprise any convenient and suitable manually operable input member such as a pivotable lever, wheel, slide, key, button or set of keys or buttons or comprise a pointing device such as a mouse, trackball or the like. In mining and construction industries, pivoted levers are common. Conventional powered shovels and other such articulated hydraulic machinery employ at least one joystick for each powered joint. This number may be varied, as will become apparent hereinbelow. Additional joysticks may be provided for operation of rotatable turret 102 and, possibly also for the drive to ground-engaging system 101 to move the powered shovel around, or to or from a work site, or for other desired purposes.
The signal outputs from control box 140 are supplied to the differential control kinematics generator 138a that is provided with suitable data storage means, such as a hard drive, optical drive, flash memory and the like. Suitable software and/or programs, for implementing the control processes may be stored on such data storage means, or may be remotely accessed by central computer 138.
Preferably, but optionally, the differential control kinematics generator 138a is also provided with conventional input/output devices such as mouse, keyboard, touch pad or touch screen, monitor, printer data transfer means such as radio wave input/output, removable data storage media, and so on, enabling human supervisor 144 to interface with the unit and mediate the operation of, for example, positioning unit 104, if desired. Such mediation or intervention by human supervisor 144 enables supervisor 144 to change the response of the control system to one or more inputs received by control box 140 to modify a programmed response or to choose a desired response from multiple possible preprogrammed responses, as will be further described hereinbelow.
The differential control kinematics generator 138a, joint control coordinator 138b, and machine differential kinematics feedback generator 138c are preferably realized in a common central computer 138.
Preferably also, the central computer 138 provides a visual display which can include schematic representations of the real machine's configuration and virtual machine configurations that can be employed by human supervisor 144. In addition, calculated and learned trajectories which can be made available for selection by human supervisor 144. Optionally also, machine 100 may be equipped with cameras providing one or more views of the tool and the work area, which views can be displayed by the computer, and optionally integrated with the control programs to enhance the control system.
Referring to FIGS. 2-3, employing the control architecture illustrated in FIG. 3, human supervisor 144 uses computer 138 to input, or select, a predetermined shovel trajectory for processing by trajectory generator 146. Trajectory generator 146 employs a differential time base 148 to generate a differential trajectory with transformation which outputs to differential inverse kinematics model 150 which is generated based on a Jacobian generator 152. Differential inverse kinematics model 150 provides a value, series of values or continuous signal for each machine joint to be moved. This may for example be the double differential with respect to time of the particular joint angle θ.
Employing an integrator 153, a matrix of angular acceleration and velocity signals for each joint to be moved is applied to the joint control system 154 in FIG. 3. Joint control system 154 can comprise relevant processing components of central computer 138 and control units 130-136. Operation of control units 130-136 imparts suitable positioning and velocity to actuators 116-120 and 126, thereby providing a desired machine kinematics configuration 156 to the machine 100.
Sensors 141, 143, 145 and 147 read the positions, and optionally the velocities, of actuators 116-120 and 126 which values or signals are applied to Jacobian generator 152 both directly and after forward kinematics processing 158.
Human supervisor 144 observes the machine, notably the performance of the end effector, in this case, shovel 112, and also the configuration of the boom 105 and the articulated arm 108. If desired, supervisor 144 can employ visual feedback 160 to control trajectory generator 146, or to apply tool trajectory adjustments using joysticks 142 or to select a different or modified trajectory using central computer 138.
An optional artificial intelligence component 162, receiving input from machine actuators 116-120, 126 and human supervisor 144, can be used to provide to trajectory generator 146 a desirable or optimum trajectory family for typical, repetitive tasks.
As used herein, the term “trajectory” is to be understood to include reference to a series of motion coordinates of an end effector, e.g. a tool or shovel 112, which represent a number of consecutive motion positions or a locus of the motion of the end effector within a time frame. Also, as used herein, the term “machine kinematics” is to be understood to include reference to the relationship between the actuators' positions or movements and the end effector's position or movement;
As described above, the signal generated by an individual joystick 142 can be distributed to more than one actuator 116-120 and 126. Preferably, the distribution of the joystick control is accomplished dynamically, while the system is in motion, using a pre-programmed trajectory family, or pre-defined coordinate system, or a pre-defined control kinematics.
Employing the control architecture disclosed in Danko '404, it is possible to provide efficient control of coordinated joint motion without generating an integrated path in the control computer model-space and then differentiating the trajectory for joint variable control during real-time machine control. Thus, the control system illustrated in FIG. 3 employing differentiating control architecture, does not require the use of closed joint position control loops. Instead, a main, Cartesian position control loop is, in this embodiment, applied to each motion component through visual feedback from the human supervisor 144.
Although not an inherent or required component for the purpose of kinematics re-configuration in the control architecture, position feedback loops may optionally be employed for linearization, backlash compensation and/or to achieve added stability. Such optional feedback loop in each control element may be useful for achieving acceptable control performances when imperfect machine elements are employed. The present invention describes one example of an overall, closed feedback control loop for each link that comprises many individual control elements. Others will be apparent to those skilled in the art in the light of this disclosure.
While recognizing that modified control architectures based upon other control parameters would be, or become, apparent to those skilled in the art, Danko '404 discloses that a particularly useful position-control signal is control velocity. This is a common example characteristic of hydraulic power machinery. Using velocity control, applied by joysticks 142, the human supervisor 144 can adjust and correct the tool position. An internal feedback loop is applied via sensors 140-146, forward kinematics 158 and Jacobian generator 152 to generate differential inverse kinematics model 150 of the machine configuration for a given tool position. In other words, for a given tool position a model is generated of how the machine should be configured, which model specifies parameters such as the angular position and velocity of each joint. As disclosed, the model can be, but is not limited to, an inverse, or pseudo inverse, Jacobian, a sensitivity matrix that relates the joint-space θ variables as a vector, to an input vector of Cartesian variables xi.
Referring now to FIG. 4, the inventive control architecture there shown is intended to be useful for enhancing or correcting the differential control of a complex system for operating hydraulic machinery that includes imperfect hydraulic valves, actuators, and machine links which have inertia and mechanical inefficiencies causing control response delays and other disturbances.
In FIG. 4: “[θiref]” is used to denote the modeled reference positions or position vector; “[θi]” is used to denote the actual positions or position vector of the real machine; “[θi]” denotes a differential control signal vector; “[θic]” denotes a correction signal vector; “[φiref]” denotes a reference actuator velocity control signal; and “[φi]” denotes real machine actuator velocity control signal.
The present invention provides control system embodiments wherein system processing architecture such as that illustrated in FIGS. 1-3, is split or cut open into two parts to separately process reference position signals and real time position signals. The split signals can be processed to generate an adjustment or correction for the control system which will better move the end effector on a desired trajectory, and can then be reconnected.
Unlike Danko '404, the illustrated embodiment of the present invention provides means to communicate to the control system what may be termed “absolute” position information regarding the movable machine elements or links, as is further described hereinbelow, enabling the end effector position at any given moment to be accurately determined. Typical embodiments of the Danko '404 disclosure employ relative position sensors whose output is not suitable for clear identification in an absolute, ground-related coordinate system of the end effector position at any given moment.
Whereas known systems may be based upon an expectation that the machine will do what it was directed to do, i.e. that the machine will more or less faithfully execute a trajectory supplied to the control system, in practice, mechanical imperfections and inertia in the actuators and components of articulated positioning unit 104, may result in significant variances from the desired trajectory. In one aspect, the present invention generates a reference model, which may be a dynamic, continuously updated model, of what the machine should be doing, senses what the machine is actually doing and provides corrections to move the machine toward the model.
In a method known from Danko '404, the control system employs model-based forward prediction wherein a next movement of a machine component is predicted and scaled to provide desired movement. Velocity-based control is employed based with the expectation that machine components will accurately follow instructions applied to the actuators, possibly with the assistance of embedded, independent correction feedback loops for each component.
In contrast, the present invention provides, in some embodiments, robust error detection obtained by “looking at” or system-determining the actual or absolute positions of the end effector and the machine links. Desirably, the position of each machine link or element that contributes to movement of the end effector is determined. The control system can be responsive to one or more of the actual positions.
One method embodiment of the present invention comprises prescribing a desired tool or end effector position to the controlled machine, comparing one or more reference positions appropriate for achieving the desired tool position with one or more actual machine element positions determined from an absolute sensor signal, generating a correction and applying the correction to improve the actual position.
In an embodiment of the inventive control architecture, corrections or adjustments are effected kinematically, providing continual incremental adjustments while the machine is moving to execute a specified end effector trajectory. For example, the adjustments, or corrections, an be made at system-determined timed intervals. The length of the time intervals may be selected in accordance with the precision and frequency of position signals output from sensors 141, 143, 145 and 147, the response times of the hydraulic and mechanical machine components, for example, actuators 116-120, 126, components 105-114 and system processing capabilities. Other appropriate considerations can also, or alternatively, be employed to determine system processing intervals.
Broadly stated, kinematic control correction can be provided, in accordance with the invention, by determining the actual positions of a hydraulic machine's, or comparable machine's actuators, at a given point in time and comparing those actual actuator positions with reference positions modeled for a hypothetical model machine. The hypothetical model machine is under differential coordinated joint motion control, according to Danko '404. In practicing the present invention, control can be identical to that of the real machine, save for the corrections.
In another embodiment of the invention, an objective of the control correction task is to reduce, minimize or eliminate the difference between [θiref] and [θi]. Other embodiments of the invention can employ different relationships between [θiref] and [θi] to obtain other desired objectives. For example, it may be desired to adjust [θi] to obtain a constant or predictably variable relationship to [θiref].
Pursuant to the aforementioned embodiment of the invention, a useful reduction, minimization or elimination of the difference between [θiref] and [θi] can be accomplished by using negative feedback methodology. To this end, the actual link position vector [θi] can be separated from the reference position vector [θiref] vector, which is provided as an output signal from suitable system control architecture such as the differential control architecture disclosed and claimed in Danko '404. Danko '404 discloses the use of transducer sensors 140-146 to detect the positions of articulated elements 104, 108, 112 and 128 with respect to their supporting elements. However, Danko '404 does not disclose or suggest use of a dynamic machine model such as model 184 and absolute position sensing.
Pursuant to further useful embodiments of the present invention, the [θiref] and [θi] vectors can be actively re-connected using a closing control loop which compares [θi] with [θiref] and generates a correction signal vector for minimizing, reducing or eliminating the [θi]-[iref] difference.
Other embodiments of the present invention can include a dynamic machine motion correction model in the closing control loop. This dynamic machine motion correction model can process input signals from the position sensors of the machine links, and evaluate those signals to provide an enhanced control signal to actuators 116-120 and 126 in order to reduce or minimize the difference between [θiref] and [θi] during motion. The machine motion correction model may incorporate system identification, and an intelligent, predictive control element, if desired.
Referring now to the machine control system shown in FIG. 4, this system adds to the system shown in FIG. 3 a position error correction system comprising a number of modeling components which can be used to provide a reference model against which actual machine performance can be compared, together with an error processing module. The added modeling components comprise a joint control model 180, a machine actuators model 182 and a dynamic machine model 184. Error processing is effected by a position and velocity error cancellation module 186.
Advantages can be realized with the addition of acceleration sensors or accelerometers 190 to provide absolute position signals. An accelerometer 190 can be provided at or on each of the boom 104, arm 108, shovel 112, other elements 128 and shovel or bucket 112 to monitor the acceleration of its respective host element. The invention includes embodiments wherein each accelerometer 190 can detect and report acceleration in one dimension, in two dimensions, or in three dimensions, depending upon the host machine element and the application, .i.e. the machine utilized and the operation performed. Desirably, acceleration is monitored continuously throughout operation of the machine to execute a trajectory prescribed by the control system and is reported to the control system by wire or wirelessly. The sensed acceleration can be processed locally, at the machine element, or centrally, at the control system to yield an absolute position signal which indicates the position of the respective element in space and time independently of the mechanical condition of the machine element or the machine articulations or joints.
Accelerometers 190 can be mounted on the machine links and can be used as absolute motion sensors, directly providing acceleration signals that can be integrated first to obtain velocities, and subsequently integrated again to obtain absolute positions. Sensors 141-147 of Danko that report the position of one moving machine element relative to another machine element which may itself also be moving are not generally suitable for providing precise and reliable position information in the absolute (ground) coordinate system. Mechanical inefficiencies such as machine link elasticity resulting in machine link sag, vibrations and other factors that cannot be detected by typical relative link position sensors are problematic.
Accelerometers 190 or other suitable sensing or observation means can be employed in embodiments of the invention to provide actuator and/or machine element position information relative to a base reference which is fixed in relation to the articulated elements of the machine. The base reference can be the ground or other machine support surface or work surface, for example, a boat deck or ice floe. Alternatively, an axis through rotatable turret 102 or ground-engaging system 101, or other suitable machine component, could be employed as a reference. The absolute position sensors on the machine can be employed to provide actual, real-world position signals of the articulated machine elements. If desired, a GPS signal can be used to provide, facilitate or supplement the absolute position information.
As illustrated in FIG. 4, joint control model 180 employs a differential control signal vector [θiu] generated by differential inverse kinematics model 150 to provide a model or reference velocity control signal [φiref] containing velocity information for any one or more of actuators 116-120 and 126. The velocity information is calculated to enable the respective actuator 116-120 or 126 to operate its associated joint in a manner which will properly assist in moving the end effector along the model reference trajectory. Differential control signal vector [θiu], from differential inverse kinematics model 150, is also supplied to dynamic machine model 184.
Machine actuators model 182 receives reference velocity control signal [φiref] from joint control model 180 and employs same to generate a set of one or more reference positions [θiref] to which one or more actuators 116-120 and 126 is to be moved. Reference positions [θiref] may, for example, be the link positions or other element positions of a theoretical perfectly tuned machine with no dynamic response error. Reference positions [θiref] are output by machine actuators model 182 to dynamic machine model 184, to error cancellation module 186, and also to forward kinematics processing 158 and Jacobian generator 152.
As stated, differential control signal vector [θiu] and reference positions [θiref] are input to dynamic machine model 184. In addition, dynamic machine model 184 receives actual link or element positions [θi] of the real machine. From these or other suitable inputs, dynamic machine model 184 constructs a dynamic model of the instantaneous configuration that an ideal machine performing a desired trajectory should have. This model changes with time. In a further embodiment of the invention dynamic machine model 184 provides a logical picture of a desired pattern of movement of relevant machine elements, over time, for performance of a selected end effector trajectory. In this and other embodiments of the invention the dynamic machine model can include the position, or the velocity of each machine actuator 116-120 and 126, or both the position and the velocity. It will be understood that the control system may of course simply include information indicative of the respective position and/or velocity. If desired, the dynamic model can also or alternatively include position or velocity or position and velocity information for each of the movable components of boom 105, or other articulated structure, supporting shovel or bucket 112 or another end effector.
Input into position and velocity error cancellation module 186 are both the modeled reference positions [θiref], from machine actuators model 182 and the actual positions [θi] of the real machine. Error cancellation module 186 also receives the modeled input from dynamic machine model 184. From these several inputs, error cancellation module 186 generates and outputs a correction signal vector [θic] to integrator 188 and thence to joint control system 154.
To generate correction signal vector [θic], error cancellation module 186 compares the actual machine element positions [θi] with the respective modeled reference positions [θiref] and calculates suitable corrections or adjustments to the actual or real-world machine link or element positions [θi]. The corrections or adjustments can, for example, be selected within the error cancellation module to bring the actual positions [θi] into correspondence with, or closer to, the modeled reference positions [θiref], or effect some other useful adjustment of the actual positions [θi].
Integrator 188 applies correction signal vector [θic] as an adjustment to differential control signal vector [θiu] received from differential inverse kinematics model 150, which has been calculated to cause the end effector to follow a trajectory specified in trajectory generator 146. Summator 188 effects the adjustment to differential control signal vector [θiu] for each active one of machine actuators 116-120 and 126. The adjusted differential control signal vector [θiu] is supplied to joint control system 154 which outputs a real machine actuator velocity control signal [φi], incorporating the adjustments to machine actuators 116-120 and 126.
Machine actuators 116-120 and 126 move their associated movable machine elements, for example, boom 104, arm 108, bucket or shovel 112 and/or other elements 128 to have positions and/or velocities which are adjusted compared with what their values would have been without the application of correction signal vector [θic] to bring the actual end effector trajectory closer to the reference model trajectory, or to effect other desired adjustment.
In a further embodiment of the invention, link positions signal [θiref] and velocity control signal [φiref] are idealized signals that can produce a desired trajectory in a perfectly tuned machine with no dynamic response error. In an alternative embodiment, dynamic machine model 184 is adapted to approximate a given real-world machine, by modifying link positions signal [θiref] and velocity control signal [φiref] to approximate a specific host machine. Such modification can be provided by machine learning, artificial intelligence, learning from error correction or in other appropriate manner. A benefit of the use of such an adaptive model is a closer approximation of the model to the actual signals and a reduction in amplitude of the correction vector.
A still further embodiment of the invention supplements any one or more of the above-described embodiments of the invention with an artificial intelligence component such as is referenced “162” in Danko '404 to receive input from machine actuators 116-120, 126 and human supervisor 144 to provide to trajectory generator 146 one or more trajectory families for executing desired tasks, including repetitive tasks.
As described in Danko '404, human supervisor 144 can observe the machine, and provide visual feedback 160, using joysticks 142 or other suitable input means, to adjust the tool trajectory or to select a different or modified trajectory. In another embodiment of the invention, such visual feedback can be applied to dynamic machine model 184 to provide an adjusted or re-selected machine model trajectory for reference in generating corrections.
In some embodiments of the invention, a hydraulic unit and any link or other members attached or articulated thereto which connects two machine members together across one of the pivots 106, 110 or 114 is referenced as a “link”. In this case, the spatial configuration of the articulated components of the machine can be specified in terms of the angular link positions, which may be used to describe, respectively: the angle of boom 105 to turret 102; the angle of lever arm 108 to boom 105; and the angle of bucket 112 to lever arm 108. Additional angular descriptors may optionally include the rotational angle of turret 102 about a vertical axis, called “slew”; the angle of ground-engaging system 101 to the horizontal about a transverse axis (perpendicular to the paper); and, possibly, the angle of ground-engaging system 101 to the horizontal about a direction of travel axis (in the plane of the paper). Further angular descriptors may be employed for additional movable, linked mechanical structure, if desired. Any or all of such angular position descriptors may be included or implicitly referenced in the position vectors, functions or signals referenced herein, for example in reference positions [θiref], actual machine link positions [θi], differential control signal vector [θiu], correction signal vector [θic], reference actuator velocity control [φiref] and real machine actuator velocity control signal [φi].
One example of the application of the dynamic model control system of the invention to machine control implements the technique on a small excavator having electro-hydraulic valves, link sensors, on-board control computer, and differential control architecture as described herein. Kinematics transformation including a virtual, revolute-to-Cartesian machine re-configuration and feedback control algorithms can be tested. Test results with such an excavator show that the machine is capable of performing complex tasks according to software-transformed, adjustable kinematics.
FIGS. 5 and 6 show bucket pin trajectories that can be measured using a camera-based, trajectory tracking method Danko et al., “Model-based camera vision evaluation of mining machine motion,” Thirty Second International Symposium of the Application of Computers and Operational Research in the Mineral Industry, Tucson, Ariz., 2005b.
FIG. 5 shows the measured trajectory of the bucket pin during open-loop control operation. The test task is to move the bucket pin along a square at a constant speed. Three consecutive repetitions of the same square target trajectory are made. As shown, the task is executed quite well, considering that the kinematics reconfiguration control works with open-loop hydraulic cylinder velocity control. Repeatability, however, is poor with drifting squares, due to lack of control correction means in the machine's hydraulic elements or an active overall position control. The trajectory can be seen to be somewhat noisy, which noise may largely be attributed to tracking error in the optical measurement.
The task is repeated with closed-loop control, according to the invention, in which the differences between the reference and the actual actuator positions are reduced by the controller. FIG. 6 shows the bucket pin movement with position error cancellation during motion. Repeatability of the motion trajectory is greatly improved as the overlapping movement paths in FIG. 6 demonstrate the invention's ability to correct error between relative iterations. In closed-loop control, the machine retains its good dynamic response. The dynamic response of the machine model is identified, and the model is applied in a predictive mode real-time during machine operation. The machine model is continuously updated with time every few milliseconds, providing for adjustment to link positions, to cross-effects between links, to non-linear effects, and to the errors caused by the approximations used in the model derivation.
FIGS. 7-9 illustrate a simple operator input device system made possible by the control architecture disclosed in Danko '404 and herein which can provide sophisticated manipulation of a tool such as a shovel or a bucket with only two operator input devices, in this case joysticks.
It will be understood that more input devices can be employed, if desired: the use of two joysticks is advantageous and illustrative of some of the benefits attainable with the above-described invention. For an operator, greater efficiency, accuracy and productivity is likely to be obtainable if they can focus their attention on a small number of controls. The present invention accordingly includes embodiments of control system and hydraulic and other machines which utilize two manually operable input devices to achieve some or all of the functionality described herein.
FIG. 7 illustrates some of the more sophisticated maneuvers that a hydraulically powered bucket or other tool may be called upon to perform. Referring to FIG. 7, a bucket 112 is supported on the viewed side by a pivot pin 114 and is pivoted or rotated about pin 114 by a hydraulic actuator, for example shovel rotator 120 (not shown in this figure), coupled to aperture 192. Bucket 112 has a leading edge 194 which engages the work material, ground, rock face, bulk deposit or the like.
Bucket edge 194 moves along an automated trajectory 196 advancing toward the host machine (not shown) with a velocity vt read as a tangent to trajectory 196. The angle of the trajectory tangent to the horizontal is designated “α”. Bucket edge 194 addresses the work material at a desired cutting angle β which is measured with respect to the trajectory tangent. In one embodiment of the invention, angle β is selected in the software and remains constant until, or unless, modified by the operator using a joystick as will be further described hereinbelow.
The proposed bucket steering kinematics will allow an operator to move the bucket's cutting edge along an arbitrary trajectory using three joystick axes configured to independently control speed, moving direction, and cutting edge angle relative to the movement direction.
The control system software can be effective to move bucket 112 along a selected trajectory such as 196 without further operator input, leaving. In this case, the operator can perform a useful range of adjustments or fine-tuning employing as few as two joystick, or comparably effective controls, for example as shown in FIGS. 7-8.
Pursuant to the invention, one possible joystick configuration for a hydraulic excavator uses two electronic joysticks each with two independently controlled axes, as is illustrated in FIGS. 7-8.
Referring to FIG. 7, the control shown which is suggested for lefthand use comprises a joystick 198 capable of pivotal movement in two orthogonal, generally horizontal, directions. Back-and-forth movement can be used to control trajectory angle α, as indicated by arrow 200. Left-to-right movement can be used to control the slew angle which is the angle of rotation of the turret on the machine base, as indicated by arrow 202.
Referring to FIG. 8, the control shown which is suggested for righthand use comprises a joystick 204 also capable of pivotal movement in two orthogonal, generally horizontal, directions. Back-and-forth movement can be used to control the bucket velocity vt along the trajectory 196, as indicated by arrow 206. Left-to-right movement can be used to control, or override, the cutting angle β, as indicated by arrow 208.
The lefthand and righthand functions can, of course, be reversed, if desired. It will also be apparent that the functions shown can be assigned differently, if desired, and that different functions may be assigned, and that different input devices can be used to provide more, fewer or different functions, if desired.
Because the major control functions required to efficiently and controllably maintain and repeat the desired trajectory are assumed by the control system, automating trajectory execution, the operator is relieved of same and can concentrate on work piece selection and fine-tuning the tool movements providing better, more efficient output with reduced operator effort.
Employing the novel error-corrected control system of the invention, a coordinated system of tool trajectory curves can be efficiently machine-generated. Using the above-described, or equivalent, or comparable joystick or other suitable input device configuration, the operator may work in Cartesian space to effect software-controlled adjustments of the tool velocity, the trajectory angle, the cutting angle, and/or other parameters programmed into the joysticks or other controllers.
For example the angle of orientation of a trajectory, such as the generally square trajectory illustrated in FIG. 6, or other useful trajectory, to the ground or other work surface may be trimmed so that the tool angle, of bucket or shovel 112 or the like, is a few degrees sharper, e.g. 5° sharper, than the trajectory angle.
The error-corrected control system of the invention is useful for controlling a wide range of machinery and comparable equipment, as described in Danko '404, including, by way of example, articulated arm front loaders, mechanical shovels and the like useful for construction, digging, mining, sand, gravel, shale, limestone, ore, and other bulk material handling, open-cast mining, pit mining and the like. The invention includes such machinery and equipment embodying the novel control systems and methods described herein.
A theoretical example of the practice of the invention will now be described for illustrative purposes, which example is not intended to limit the scope of the invention which it will be understood can be practiced in a variety of other ways.
EXAMPLE
Approximate Causal System Model
For an n-link machine, the joint angular positions x1, x2, . . . , xn can be denoted by the n-vector x=[x1, x2, . . . , xn]. A simplified motion equation can be written as a second-order vector differential equation [ref] between positions x, velocities dx/dt, and accelerations d2x/dt2:




  
    
      
        
          
            
              M
              ⁡
              
                (
                x
                )
              
            
            ⁢
            
              
                
                  d
                  2
                
                ⁢
                x
              
              
                d
                
                  t
                  2
                
              
            
          
          +
          
            
              D
              ⁡
              
                (
                x
                )
              
            
            ⁢
            
              
                d
                x
              
              
                d
                t
              
            
          
          +
          
            
              S
              ⁡
              
                (
                x
                )
              
            
            ⁢
            x
          
        
        =
        
          A
          ⁡
          
            (
            
              
                
                  d
                  x
                
                
                  d
                  t
                
              
              ,
              x
            
            )
          
        
      
    
    
      
        (
        1
        )
      
    
  




In Eq (1), M(x), an n×n matrix that represents the moment of inertia and mass reduced to the joint axes. D(x), S(x) are n×n matrices of velocity-proportional drag and position-proportional spring torque coefficients, respectively. A(dx/dt, x) is an n-vector, representing an active torque vector, generated by the joint actuators. This term is a function of control signal u, velocity, and position vector variables.
Since all xi (i=1 . . . n) are functions of time t, the non-linear Eq (1) can be conveniently converted into a time-variant motion equation by eliminating x from the coefficient matrices:




  
    
      
        
          
            
              M
              ⁡
              
                (
                t
                )
              
            
            ⁢
            
              
                
                  d
                  2
                
                ⁢
                x
              
              
                d
                
                  t
                  2
                
              
            
          
          +
          
            
              D
              ⁡
              
                (
                t
                )
              
            
            ⁢
            
              
                d
                x
              
              
                d
                t
              
            
          
          +
          
            
              S
              ⁡
              
                (
                t
                )
              
            
            ⁢
            x
          
        
        =
        
          A
          ⁡
          
            (
            
              
                
                  d
                  x
                
                
                  d
                  t
                
              
              ,
              t
            
            )
          
        
      
    
    
      
        (
        2
        )
      
    
  




A first-order, linear approximation (as the first term of the Taylor series) is applied to A:




  
    
      
        
          A
          ⁡
          
            (
            
              u
              ,
              
                x
                ⁢
                
                  
                    d
                    x
                  
                  
                    d
                    t
                  
                
              
              ,
              t
            
            )
          
        
        =
        
          
            
              Ju
              ⁡
              
                (
                A
                )
              
            
            ⁢
            u
          
          +
          
            
              Jx
              ⁡
              
                (
                A
                )
              
            
            ⁢
            x
          
          +
          
            
              Jdx
              ⁡
              
                (
                A
                )
              
            
            ⁢
            
              
                d
                x
              
              
                d
                t
              
            
          
          +
          
            T
            ⁡
            
              (
              t
              )
            
          
        
      
    
    
      
        (
        3
        )
      
    
  




Where Ju(A), Jx(A), and Jdx(A) are the following Jacobian matrices:




  
    
      
        
          
            Ju
            ⁡
            
              (
              A
              )
            
          
          =
          
            [
            
              
                
                  
                    
                      ∂
                      
                        A
                        
                          1
                          ,
                          1
                        
                      
                    
                    
                      ∂
                      
                        u
                        1
                      
                    
                  
                
                
                  ⋯
                
                
                  
                    
                      ∂
                      
                        A
                        
                          1
                          ,
                          n
                        
                      
                    
                    
                      ∂
                      
                        u
                        n
                      
                    
                  
                
              
              
                
                  ⋮
                
                
                  
                    
                  
                
                
                  
                    
                  
                
              
              
                
                  
                    
                      ∂
                      
                        A
                        
                          n
                          ,
                          1
                        
                      
                    
                    
                      ∂
                      
                        u
                        1
                      
                    
                  
                
                
                  ⋯
                
                
                  
                    
                      ∂
                      
                        A
                        
                          n
                          ,
                          n
                        
                      
                    
                    
                      ∂
                      
                        u
                        n
                      
                    
                  
                
              
            
            ]
          
        
        ⁢
        
          

        
        ⁢
        
          
            Jx
            ⁡
            
              (
              A
              )
            
          
          =
          
            [
            
              
                
                  
                    
                      ∂
                      
                        A
                        
                          1
                          ,
                          1
                        
                      
                    
                    
                      ∂
                      
                        x
                        1
                      
                    
                  
                
                
                  ⋯
                
                
                  
                    
                      ∂
                      
                        A
                        
                          1
                          ,
                          n
                        
                      
                    
                    
                      ∂
                      
                        x
                        n
                      
                    
                  
                
              
              
                
                  ⋮
                
                
                  
                    
                  
                
                
                  
                    
                  
                
              
              
                
                  
                    
                      ∂
                      
                        A
                        
                          n
                          ,
                          1
                        
                      
                    
                    
                      ∂
                      
                        x
                        1
                      
                    
                  
                
                
                  ⋯
                
                
                  
                    
                      ∂
                      
                        A
                        
                          n
                          ,
                          n
                        
                      
                    
                    
                      ∂
                      
                        x
                        n
                      
                    
                  
                
              
            
            ]
          
        
        ⁢
        
          

        
        ⁢
        
          
            Jdx
            ⁡
            
              (
              A
              )
            
          
          =
          
            [
            
              
                
                  
                    
                      ∂
                      
                        A
                        
                          1
                          ,
                          1
                        
                      
                    
                    
                      ∂
                      
                        
                          x
                          .
                        
                        1
                      
                    
                  
                
                
                  ⋯
                
                
                  
                    
                      ∂
                      
                        A
                        
                          1
                          ,
                          n
                        
                      
                    
                    
                      ∂
                      
                        
                          x
                          .
                        
                        n
                      
                    
                  
                
              
              
                
                  ⋮
                
                
                  
                    
                  
                
                
                  
                    
                  
                
              
              
                
                  
                    
                      ∂
                      
                        A
                        
                          n
                          ,
                          1
                        
                      
                    
                    
                      ∂
                      
                        
                          x
                          .
                        
                        1
                      
                    
                  
                
                
                  ⋯
                
                
                  
                    
                      ∂
                      
                        A
                        
                          n
                          ,
                          n
                        
                      
                    
                    
                      ∂
                      
                        
                          x
                          .
                        
                        n
                      
                    
                  
                
              
            
            ]
          
        
      
    
    
      
        (
        4
        )
      
    
  




In Eq (3), T(t) represents a time-dependent torque vector. Substituting (3) into (2), after simplification, gives:




  
    
      
        
          
            
              M
              ⁡
              
                (
                t
                )
              
            
            ⁢
            
              
                
                  d
                  2
                
                ⁢
                x
              
              
                d
                
                  t
                  2
                
              
            
          
          +
          
            
              [
              
                
                  D
                  ⁡
                  
                    (
                    t
                    )
                  
                
                -
                
                  Jdx
                  ⁡
                  
                    (
                    A
                    )
                  
                
              
              ]
            
            ⁢
            
              
                d
                x
              
              
                d
                t
              
            
          
          +
          
            
              [
              
                
                  S
                  ⁡
                  
                    (
                    t
                    )
                  
                
                -
                
                  Jx
                  ⁡
                  
                    (
                    A
                    )
                  
                
              
              ]
            
            ⁢
            x
          
        
        =
        
          
            
              Ju
              ⁡
              
                (
                A
                )
              
            
            ⁢
            u
          
          +
          
            T
            ⁡
            
              (
              t
              )
            
          
        
      
    
    
      
        (
        5
        )
      
    
  




Eq (5) can be re-arranged. For the xi (i=1 . . . n) component, it reads:




  
    
      
        
          
            
              M
              
                i
                ,
                i
              
            
            ⁢
            
              
                
                  d
                  2
                
                ⁢
                
                  x
                  i
                
              
              
                d
                
                  t
                  2
                
              
            
          
          +
          
            
              (
              
                
                  D
                  
                    i
                    ,
                    i
                  
                
                -
                
                  Jdx
                  
                    i
                    ,
                    i
                  
                
              
              )
            
            ⁢
            
              
                d
                
                  x
                  i
                
              
              
                d
                t
              
            
          
          +
          
            
              (
              
                
                  S
                  
                    i
                    ,
                    i
                  
                
                -
                
                  Jx
                  
                    i
                    ,
                    i
                  
                
              
              )
            
            ⁢
            
              x
              i
            
          
        
        =
        
          
            F
            ⁡
            
              (
              
                t
                ,
                
                  x
                  j
                
                ,
                
                  u
                  j
                
              
              )
            
          
          +
          
            
              Ju
              
                i
                ,
                i
              
            
            ⁢
            
              u
              i
            
          
        
      
    
    
      
        (
        6
        )
      
    
  




Where F(t, xj, uj) (j≠i) is:




  
    
      
        
          F
          ⁡
          
            (
            
              t
              ,
              
                x
                i
              
              ,
              
                x
                j
              
              ,
              
                u
                j
              
            
            )
          
        
        =
        
          
            -
            
              
                ∑
                
                  
                    j
                    =
                    1
                  
                  
                    j
                    ≠
                    i
                  
                
                n
              
              ⁢
              
                
              
              ⁢
              
                [
                
                  
                    
                      M
                      
                        i
                        ,
                        j
                      
                    
                    ⁢
                    
                      
                        
                          d
                          2
                        
                        ⁢
                        
                          x
                          j
                        
                      
                      
                        d
                        
                          t
                          2
                        
                      
                    
                  
                  +
                  
                    
                      (
                      
                        
                          D
                          
                            i
                            ,
                            j
                          
                        
                        -
                        
                          Jdx
                          
                            i
                            ,
                            j
                          
                        
                      
                      )
                    
                    ⁢
                    
                      
                        d
                        
                          x
                          j
                        
                      
                      
                        d
                        t
                      
                    
                  
                  +
                  
                    
                      (
                      
                        
                          S
                          
                            i
                            ,
                            j
                          
                        
                        -
                        
                          Jx
                          
                            i
                            ,
                            j
                          
                        
                      
                      )
                    
                    ⁢
                    
                      x
                      j
                    
                  
                  +
                  
                    
                      Ju
                      
                        i
                        ,
                        j
                      
                    
                    ⁢
                    
                      u
                      j
                    
                  
                
                ]
              
            
          
          +
          
            T
            i
          
        
      
    
    
      
        (
        7
        )
      
    
  




The set of n simultaneous differential equations in Eq (6) defines the simplified dynamic model of an n-link machine. In one embodiment, the invention provides a dynamic machine model wherein the time variant coefficients Mi,i, Di,i, Jxi,i, Jdxi,i, Jui,i, Si,i and Ti (i=1 . . . n) are a best match to the predicted xi angular positions from a model with observed (measured) xiM values over a sufficiently long ΔT time period, for a given ui as known control input. While alternative methods can be employed, for example, direct xM signal filtering during model building [Kalman], in the instant example, a simple least-square-fit match is applied:




  
    
      
        
          
            
              ∫
              t
            
            
              t
              +
              
                Δ
                ⁢
                
                  
                
                ⁢
                T
              
            
          
          ⁢
          
            
              
                [
                
                  
                    
                      x
                      i
                    
                    ⁡
                    
                      (
                      t
                      )
                    
                  
                  -
                  
                    
                      x
                      i
                      M
                    
                    ⁡
                    
                      (
                      t
                      )
                    
                  
                
                ]
              
              2
            
            ⁢
            
              d
              t
            
          
        
        =
        
          
            minimum
            ⁢
            
              
            
            ⁢
            i
          
          =
          
            1
            ⁢
            
              
            
            ⁢
            ...
            ⁢
            
              
            
            ⁢
            n
          
        
      
    
    
      
        (
        8
        )
      
    
  




For real-time model identification applications in the control loop, computation time is limited and simplification of Eq (6) can be helpful. Pursuant to this example, simplification can be accomplished by de-coupling the set of differential equations and assuming that the cross-effects represented by xi and xj, uj (j≠i) on the left-hand side of Eq (7) are only time-dependent, yet unknown, functions. Furthermore, the time-variable coefficients in Eq (6) can be considered time-averaged constants within the sliding [t, t+ΔT] time interval. While more exact solutions may be possible, they are likely to require increased processing resources. Using such simplification, the decoupled, second-order differential equations with time-averaged coefficients for each link i are:




  
    
      
        
          
            
              m
              i
            
            ⁢
            
              
                
                  d
                  2
                
                ⁢
                
                  x
                  i
                
              
              
                d
                
                  t
                  2
                
              
            
          
          +
          
            
              k
              i
            
            ⁢
            
              
                d
                
                  x
                  i
                
              
              
                d
                t
              
            
          
          +
          
            
              q
              i
            
            ⁢
            
              x
              i
            
          
        
        =
        
          
            
              f
              i
            
            ⁡
            
              (
              t
              )
            
          
          +
          
            
              a
              i
            
            ⁢
            
              u
              i
            
          
        
      
    
    
      
        (
        9
        )
      
    
  




In Eq (9) mi=Mi,i, ki=Di,i−Jxi,i, qi=Si,i−Jxi,i, and fi=F(t,xj,uj), ai=Jui,i notations are used; fi(t) can be considered unknown, since it includes unknown system coefficient functions and parameters shown in Eq (7). Since the force balance of each link i is affected by the motion of the other links (j=1 . . . n, j≠i) throughout Fi(t) only, it is subject to identification. Eliminating indexing for simplification in notation when applying Eq (9) for each link independently, it gives:




  
    
      
        
          
            
              
                d
                2
              
              ⁢
              x
            
            
              d
              
                t
                2
              
            
          
          +
          
            P
            ⁢
            
              
                d
                x
              
              
                d
                t
              
            
          
          +
          Cx
        
        =
        F
      
    
    
      
        (
        10
        )
      
    
  




    
    
        Where
    
    






  
    
      
        
          
            
              
                P
                =
                
                  
                    k
                    i
                  
                  
                    m
                    i
                  
                
              
            
          
          
            
              
                C
                =
                
                  
                    q
                    i
                  
                  
                    m
                    i
                  
                
              
            
          
          
            
              
                F
                =
                
                  (
                  
                    
                      
                        f
                        i
                      
                      
                        m
                        i
                      
                    
                    +
                    
                      
                        
                          a
                          i
                        
                        ⁢
                        u
                      
                      
                        m
                        i
                      
                    
                  
                  )
                
              
            
          
        
        }
      
    
    
      
        (
        11
        )
      
    
  




The motion control velocity signal u contained in F can be assumed to be a response, for example, the output of an electro-hydraulic valve. The primary control signal is the input control voltage to the electro-hydraulic valve. Such a response is primarily integrated and/or delayed, and may have linearity and accuracy problems. Accordingly, in this example, these time-variable effects are included in the task of dynamic system identification. The equation for u with time-variable constants b and d to be identified is:




  
    
      
        u
        =
        
          
            b
            ⁢
            
              
                
                  ∫
                  
                    t
                    -
                    
                      Δ
                      ⁢
                      
                        
                      
                      ⁢
                      Tv
                    
                  
                
                t
              
              ⁢
              
                V
                ⁢
                
                  d
                  τ
                
              
            
          
          +
          d
        
      
    
    
      
        (
        12
        )
      
    
  




Where V is the time-variable voltage control signal to the actuator for each link. The ΔTv time interval can be assumed to be a suitable duration, characterizing the hydraulic system and the electro-hydraulic valve. Using the expression F from Eqs (11) and simplifying, gives:




  
    
      
        F
        =
        
          
            A
            ⁢
            
              
                
                  ∫
                  
                    t
                    -
                    
                      Δ
                      ⁢
                      
                        
                      
                      ⁢
                      Tv
                    
                  
                
                t
              
              ⁢
              
                V
                ⁢
                
                  d
                  τ
                
              
            
          
          +
          B
        
      
    
    
      
        (
        13
        )
      
    
  




where the new constants are:





  
    
      
        
          
            
              
                A
                =
                
                  b
                  ⁢
                  
                    
                      a
                      i
                    
                    
                      m
                      i
                    
                  
                
              
            
          
          
            
              
                B
                =
                
                  d
                  +
                  
                    
                      f
                      i
                    
                    
                      m
                      i
                    
                  
                
              
            
          
        
        }
      
    
    
      
        (
        14
        )
      
    
  




Substituting Eq (13) into Eq (10) gives:




  
    
      
        
          
            
              
                d
                2
              
              ⁢
              x
            
            
              d
              
                t
                2
              
            
          
          +
          
            P
            ⁢
            
              
                d
                x
              
              
                d
                t
              
            
          
          +
          Cx
        
        =
        
          
            A
            ⁢
            
              
                
                  ∫
                  
                    t
                    -
                    
                      Δ
                      ⁢
                      
                        
                      
                      ⁢
                      Tv
                    
                  
                
                t
              
              ⁢
              
                v
                ⁢
                
                  d
                  c
                
              
            
          
          +
          B
        
      
    
    
      
        (
        15
        )
      
    
  




In conclusion of the derivation, the time variant system identification task for a given machine link at time t is to determine P, A, B, and C time-averaged constants from Eq (15) based on matching x to the measured xM using the optimization condition in Eq (8) for a given ΔT time interval. A simplified optimization task is achieved by applying a numerical, finite difference solution scheme. Discretaziation of x with time into an xr series of samples (r=1 . . . N) allows for re-writing Eq (10) into a finite-difference equation:




  
    
      
        
          
            
              
                x
                
                  r
                  +
                  1
                
              
              +
              
                x
                
                  r
                  -
                  1
                
              
              -
              
                2
                ⁢
                
                  x
                  r
                
              
            
            
              Δ
              ⁢
              
                
              
              ⁢
              
                t
                2
              
            
          
          +
          
            P
            ⁢
            
              
                
                  x
                  
                    r
                    +
                    1
                  
                
                -
                
                  x
                  
                    r
                    -
                    1
                  
                
              
              
                2
                ⁢
                Δ
                ⁢
                
                  
                
                ⁢
                t
              
            
          
          +
          
            Cx
            r
          
        
        =
        
          
            A
            ⁢
            
              
                ∑
                
                  i
                  =
                  
                    r
                    -
                    
                      N
                      r
                    
                  
                
                r
              
              ⁢
              
                
              
              ⁢
              
                V
                i
              
            
          
          +
          
            Δ
            ⁢
            
              
            
            ⁢
            t
          
          +
          B
        
      
    
    
      
        (
        16
        )
      
    
  




    
    
        Where:
    
    






  Nv
  =
  
    
      Δ
      ⁢
      
        
      
      ⁢
      Tv
    
    
      Δ
      ⁢
      
        
      
      ⁢
      t
    
  




Matching xr with the measured xrM values in a least-squares-fit sense over N samples is equivalent of replacing xr with xrM in Eq (16) and solving for P, A, B, C from an over-determined set of N−2 equations (if N>6):




  
    
      
        
          [
          
            
              
                
                  
                    
                      x
                      3
                      M
                    
                    +
                    
                      x
                      1
                      M
                    
                    -
                    
                      2
                      ⁢
                      
                        x
                        2
                      
                    
                  
                  
                    Δ
                    ⁢
                    
                      
                    
                    ⁢
                    
                      t
                      2
                    
                  
                
              
            
            
              
                ⋮
              
            
            
              
                
                  
                    
                      x
                      N
                      M
                    
                    +
                    
                      x
                      
                        N
                        -
                        2
                      
                      M
                    
                    -
                    
                      2
                      ⁢
                      
                        x
                        
                          N
                          -
                          1
                        
                      
                    
                  
                  
                    Δ
                    ⁢
                    
                      
                    
                    ⁢
                    
                      t
                      2
                    
                  
                
              
            
          
          ]
        
        =
        
          
            [
            
              
                
                  
                    
                      
                        x
                        1
                        M
                      
                      -
                      
                        x
                        3
                        M
                      
                    
                    
                      2
                      ⁢
                      Δ
                      ⁢
                      
                        
                      
                      ⁢
                      
                        t
                        2
                      
                    
                  
                
                
                  
                    
                      ∑
                      
                        i
                        =
                        
                          2
                          -
                          Nv
                        
                      
                      2
                    
                    ⁢
                    
                      
                    
                    ⁢
                    
                      V
                      i
                    
                  
                
                
                  1
                
                
                  
                    -
                    
                      x
                      2
                      M
                    
                  
                
              
              
                
                  ⋮
                
                
                  
                    
                  
                
                
                  
                    
                  
                
                
                  
                    
                  
                
              
              
                
                  
                    
                      
                        x
                        
                          N
                          -
                          2
                        
                        M
                      
                      -
                      
                        x
                        N
                        M
                      
                    
                    
                      2
                      ⁢
                      Δ
                      ⁢
                      
                        
                      
                      ⁢
                      
                        t
                        2
                      
                    
                  
                
                
                  
                    
                      ∑
                      
                        i
                        =
                        
                          N
                          -
                          Nv
                          -
                          1
                        
                      
                      2
                    
                    ⁢
                    
                      V
                      i
                    
                  
                
                
                  1
                
                
                  
                    -
                    
                      x
                      
                        N
                        -
                        1
                      
                      M
                    
                  
                
              
            
            ]
          
          ⁡
          
            [
            
              
                
                  P
                
              
              
                
                  A
                
              
              
                
                  B
                
              
              
                
                  C
                
              
            
            ]
          
        
      
    
    
      
        (
        17
        )
      
    
  




In the application example, N=30 is used, while Nv=5 and ΔT=2 ms. Once P,A,B,C are determined from Eq (17), Eq (16) can be used for numerically solving the time variable position xr from an initial position. The solution can be used for position interpolation (when t<tpresent), filtering (when t=tpresent), or prediction (when t>tpresent), with control vector V, which can include forward-predicted elements for t>tpresent. In the application example, the dynamic machine mode is applied for a one time-step, forward prediction of the motion of each machine link.
In summary, the efficiency of a model-based, intelligent, adaptive control is limited. The invention provides a more efficient solution by employing a simplified system model-component combined with absolute sensory signal contribution. A simple method, based on first principles, is developed as an example for extracting a causal, second-order model from noisy, measured system responses. Although fifth or even higher order models are routinely expected in hydraulic system applications, as described for example by Walters, R. B., Hydraulic and Electric-Hydraulic Control Systems, Second Enlarged Edition, Kluwer Academic Publishing, Norwell, Mass., 2000, the invention is able, surprisingly, to provide an effective solution using low-order, but nevertheless time-variant, model application.
The present invention includes machines and mechanical systems equipped with the novel control systems described herein as well as methods of operation of machines and mechanical systems which employ the novel control methods disclosed herein.
The foregoing detailed description is to be read in light of and in combination with the preceding background and invention summary descriptions wherein information regarding the invention and its practice may be set forth and wherein modifications, alternative and useful embodiments of the invention may be suggested or set forth, as will be apparent to one skilled in the art.
While illustrative embodiments of the invention have been described above, it is, of course, understood that many and various modifications will be apparent to those of ordinary skill in the relevant art, or may become apparent as the art develops, in the light of the foregoing description. Such modifications are contemplated as being within the spirit and scope of the invention or inventions disclosed in this specification.

## Claims

1. A control system useable in a hydraulic machine with a mechanical arm that includes a plurality of joints, a plurality of actuators associated with the plurality of joints, and an end effector, the control system comprising a processor and data storage and configured to perform control operations comprising:
receiving user inputs that include:
first user input that specifies a movement direction of the end effector;
second user input that specifies a movement speed of the end effector in the movement direction; and
third user input to adjust a preset value of the angle of the end effector relative to the movement direction;

calculating a set of control signals for adjusting the plurality of actuators, the first set of control signals being calculated to enable the mechanical arm to achieve the end effector movement direction, movement speed, and angle of the end effector relative to the movement direction specified by the first, second, and third user inputs; and
outputting the set of control signals;
wherein the control system allows a user to steer the end effector by specifying the movement direction and movement speed of the end effector, with at least a portion of other movement aspects of the mechanical arm needed to accomplish the steering being automatically controlled by the control system.

2. The control system of claim 1, wherein the movement direction of the end effector and the speed of the end effector are directly and independently controlled through the first and second user inputs.

3. The control system of claim 1, wherein the angle of the movement direction of the end effector and the speed of the end effector are controlled in Cartesian space.

4. The control system of claim 1, wherein the first user input allows the user to move the end effector along an arbitrary trajectory.

5. The control system of claim 1, wherein the first and second user inputs allow the user to move the end effector along an arbitrary trajectory at an arbitrary velocity.

6. The control system of claim 1, wherein the at least a portion of other movement aspects of the mechanical arm needed to accomplish the steering being automatically controlled by the control system comprises calculating a control signal to maintain the preset value of the angle of the end effector relative to the movement direction.

7. The control system of claim 1, wherein the angle of a cutting edge of the end effector is automatically maintained by the control system at a previously selected cutting edge angle as the end effector is steered as directed by the first and second user inputs, unless the angle of the cutting edge of the end effector is modified according to the third user input.

8. The control system of claim 1, wherein the first user input specifies an angle of a tangent to the movement direction relative to horizontal.

9. The control system of claim 1, the control operations further comprising:
selecting a trajectory family for automated execution of the end effector movement, wherein receiving user inputs and outputting a set of control signals facilitates adjustment of the end effector movement.

10. The control system of claim 9, the control operations further comprising:
maintaining the trajectory family; and
repeating the trajectory for automated trajectory execution.

11. The control system of claim 1, wherein the control system uses a continuous differential control architecture.

12. The control system of claim 1, wherein the control system uses a continuous differential control architecture with dynamic motion modeling for position error correction.

13. The control system of claim 1 wherein the end effector is a bucket.

14. The control system of claim 1, wherein the first user input is received from a first user input device configured to allow a user to specify the direction of movement of the end effector and the second user input is received from a second user input device configured to allow a user to specify the movement speed of the end effector.

15. The control system of claim 14, the control operations further comprising receiving fourth user input specifying the slew angle of the mechanical arm.

16. The control system of claim 15, wherein the fourth user input is received from the first user input device, the first user input device being further configured to allow a user to specify the slew angle of the mechanical arm.

17. The control system of claim 14, the control operations further comprising receiving the third user input from the second user input device, the second user input device being further configured to allow a user to specify the angle of a cutting edge of the end effector relative to the movement direction of the end effector.

18. The control system of claim 1, the control operations further comprising receiving user input, including the first, second, and third user inputs, from first and second user input devices, the user input further comprising the slew angle of the mechanical arm and the movement speed of the end effector, the first and second user input devices being configured to allow a user to select trajectory parameters selected from the group consisting of direction of movement of the end effector, the angle of the cutting edge of the end effector relative to the movement direction of the end effector, the slew angle of the mechanical arm, and the movement speed of the end effector.

19. The control system of claim 18, further comprising the first and second user input devices.

20. The control system of claim 1, wherein the data storage stores computer readable instructions for carrying out the control operations.

21. The control system of claim 1, wherein the control system is incorporated into a hydraulic machine with a mechanical arm that includes an end effector and a plurality of joints.

22. A control system useable in a hydraulic machine with a mechanical arm that includes an end effector, the control system comprising a processor and data storage and configured to perform control operations comprising:
receiving user inputs that include:
first user input that specifies a movement direction of the end effector;
second user input that specifies a movement speed of the end effector in the movement direction; and
third user input to adjust a preset value of the angle of the end effector relative to the movement direction;

calculating a control signal for adjusting an actuator associated with a joint of the mechanical arm, the control signal being calculated to enable the mechanical arm to achieve the end effector movement direction, movement speed, and angle of the end effector relative to the movement direction specified by the first, second, and third user inputs; and
outputting the control signal;
wherein the control system allows a user to steer the end effector by specifying the movement direction and movement speed of the end effector, with at least a portion of other movement aspects of the mechanical arm needed to accomplish the steering being automatically controlled by the control system.

23. The control system of claim 22, wherein the mechanical arm includes a plurality of joints and a plurality of actuators, which include the joint and the actuator, and calculating a control signal includes calculating a control signal for the plurality of actuator.