US20140107832A1 - Coordinated joint motion control system with position error correction - Google Patents

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.

Description

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 the35^thAIAA 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 inFIG. 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 inFIG. 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 inFIG. 2;
• FIG. 5 is a side elevation of the trajectory of a small component of the end effector of the shovel shown inFIG. 1, with imperfect and uncompensated motion control values, as it might appear when operated by a known control architecture such as that shown inFIG. 3;
• FIG. 6 is a side elevation of the trajectory of a small component of the end effector of the shovel shown inFIG. 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 inFIG. 4;
• FIG. 7 is a schematic view of a bucket manipulable by a hydraulic machine such as that shown inFIG. 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 inFIG. 7; and
• FIG. 9 is a perspective view of a righthand joystick useful for manipulating the bucket in the manner indicated inFIG. 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 toFIG. 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-engagingsystem101 supporting a variable-slewrotatable turret102.Rotatable turret102 has acab103 to house a human supervisory operator and supports one end of an articulatedpositioning unit104 for manipulating a tool or other end effector supported at the other end ofpositioning unit104.
• Positioning unit104 comprises aboom105 articulated to turret102 for rotation about apivot106, thereby providing a boom joint.Positioning unit104 further comprises alever arm108 one end of which is articulated to boom104 for rotation about apivot110, thereby providing an arm joint, while the other end is articulated to and supports a tool, in this case a shovel orbucket112, for rotation about abucket pin114, 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 ofshovel112 is preferably also variable, for whichpurpose shovel112 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 ofarm108 providing rotation about a longitudinal arm axis extending throughpivots110 and114 and a second axis perpendicular thereto, also extending throughbucket pin114.
• Piston-and-cylinderhydraulic actuators116,118 and120 control the motion and position ofboom105,arm108 and shovel orbucket112, respectively. Each actuator116-120 is pivotally secured at its ends andcylinder120 is connected to shovel112 through an articulatedlink122 to enhance its leverage. One or more leveling, guide or ancillary cylinders such as levelingcylinder124 may be provided if desired, but may be rendered unnecessary by the software kinematics configuration capabilities of the present invention.
• Referring toFIG. 2, other actuators such as126 can optionally also be provided for moving and positioning other elements or systems, forexample turret102, and ground-engagingsystem101, as will be further described hereinbelow.
• In the motion control system illustrated inFIG. 2, each actuator116-120 and126 is operated by its own, local hydraulic control unit130-136, respectively, which control unit receives a signal from ajoint control coordinator138band controls its associatedactuator116,118,120 or126 to provide a suitable output. One example of a suitable output comprises a desired position and/or velocity of the associated articulatedelement104,108,112 or128. This output is determined by thejoint control coordinator138bbased on the forward input from the differentialcontrol kinematics generator138aand the feedback input from the machine differentialkinematics feedback generator138c.
• Each articulatedelement104,108,112 or128 is provided with atransducer sensor141,143,145 or147, respectively, to detect preferably at least the position of the respective articulatedelement104,108,112 or128 with respect to its supporting element, and report the sensed position, preferably as a continuous or time-divided signal, to the machine differentialkinematics feedback generator138c.
• Acontrol box140 comprises a suitable number of manually operable control members orjoysticks142 which can be moved, pressured or otherwise manipulated by ahuman supervisor144 to move theboom104 and/orarm108 and/or shovel112, and/or other elements, according to a desired trajectory.Joysticks142 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 ofrotatable turret102 and, possibly also for the drive to ground-engagingsystem101 to move the powered shovel around, or to or from a work site, or for other desired purposes.
• The signal outputs fromcontrol box140 are supplied to the differentialcontrol kinematics generator138athat 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 bycentral computer138.
• Preferably, but optionally, the differentialcontrol kinematics generator138ais 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, enablinghuman supervisor144 to interface with the unit and mediate the operation of, for example, positioningunit104, if desired. Such mediation or intervention byhuman supervisor144 enablessupervisor144 to change the response of the control system to one or more inputs received bycontrol box140 to modify a programmed response or to choose a desired response from multiple possible preprogrammed responses, as will be further described hereinbelow.
• The differentialcontrol kinematics generator138a,joint control coordinator138b,and machine differentialkinematics feedback generator138care preferably realized in a commoncentral computer138.
• Preferably also, thecentral computer138 provides a visual display which can include schematic representations of the real machine's configuration and virtual machine configurations that can be employed byhuman supervisor144. In addition, calculated and learned trajectories which can be made available for selection byhuman supervisor144. Optionally also,machine100 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 toFIGS. 2-3, employing the control architecture illustrated inFIG. 3,human supervisor144 usescomputer138 to input, or select, a predetermined shovel trajectory for processing bytrajectory generator146.Trajectory generator146 employs adifferential time base148 to generate a differential trajectory with transformation which outputs to differentialinverse kinematics model150 which is generated based on aJacobian generator152. Differentialinverse kinematics model150 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 integrator153, a matrix of angular acceleration and velocity signals for each joint to be moved is applied to thejoint control system154 inFIG. 3.Joint control system154 can comprise relevant processing components ofcentral computer138 and control units130-136. Operation of control units130-136 imparts suitable positioning and velocity to actuators116-120 and126, thereby providing a desiredmachine kinematics configuration156 to themachine100.
• Sensors141,143,145 and147 read the positions, and optionally the velocities, of actuators116-120 and126 which values or signals are applied toJacobian generator152 both directly and after forward kinematics processing158.
• Human supervisor144 observes the machine, notably the performance of the end effector, in this case,shovel112, and also the configuration of theboom105 and the articulatedarm108. If desired,supervisor144 can employvisual feedback160 to controltrajectory generator146, or to apply tool trajectoryadjustments using joysticks142 or to select a different or modified trajectory usingcentral computer138.
• An optionalartificial intelligence component162, receiving input from machine actuators116-120,126 andhuman supervisor144, can be used to provide to trajectory generator146 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 shovel112, 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 anindividual joystick142 can be distributed to more than one actuator116-120 and126. 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 inFIG. 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 thehuman supervisor144.
• 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 byjoysticks142, thehuman supervisor144 can adjust and correct the tool position. An internal feedback loop is applied via sensors140-146,forward kinematics158 andJacobian generator152 to generate differentialinverse kinematics model150 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 x_i.
• Referring now toFIG. 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.
• InFIG. 4: “[θ_i^ref]” 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^u]” denotes a differential control signal vector; “[θ_i^c]” denotes a correction signal vector; “[φ_i^ref]” 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 inFIGS. 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 articulatedpositioning unit104, 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 fromsensors141,143,145 and147, the response times of the hydraulic and mechanical machine components, for example, actuators116-120,126, components105-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 [θ_i^ref] and [θ_i]. Other embodiments of the invention can employ different relationships between [θ_i^ref] 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 [θ_i^ref].
• Pursuant to the aforementioned embodiment of the invention, a useful reduction, minimization or elimination of the difference between [θ_i^ref] 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 [θ_i^ref] 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 sensors140-146 to detect the positions of articulatedelements104,108,112 and128 with respect to their supporting elements. However, Danko '404 does not disclose or suggest use of a dynamic machine model such asmodel184 and absolute position sensing.
• Pursuant to further useful embodiments of the present invention, the [θ_i^ref] and [θ_i] vectors can be actively re-connected using a closing control loop which compares [θ_i] with [θ_i^ref] and generates a correction signal vector for minimizing, reducing or eliminating the[θ_i]−[θ_i^ref] 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 actuators116-120 and126 in order to reduce or minimize the difference between [θ_i^ref] 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 inFIG. 4, this system adds to the system shown inFIG. 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 ajoint control model180, amachine actuators model182 and adynamic machine model184. Error processing is effected by a position and velocityerror cancellation module186.
• Advantages can be realized with the addition of acceleration sensors oraccelerometers190 to provide absolute position signals. Anaccelerometer190 can be provided at or on each of theboom104,arm108,shovel112,other elements128 and shovel orbucket112 to monitor the acceleration of its respective host element. The invention includes embodiments wherein eachaccelerometer190 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.
• Accelerometers190 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. Sensors141-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.
• Accelerometers190 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 throughrotatable turret102 or ground-engagingsystem101, 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 inFIG. 4,joint control model180 employs a differential control signal vector [θ_i^u] generated by differentialinverse kinematics model150 to provide a model or reference velocity control signal [φ_i^ref] containing velocity information for any one or more of actuators116-120 and126. The velocity information is calculated to enable the respective actuator116-120 or126 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 [θ_i^u], from differentialinverse kinematics model150, is also supplied todynamic machine model184.
• Machine actuators model182 receives reference velocity control signal [φ_i^ref] fromjoint control model180 and employs same to generate a set of one or more reference positions [θ_i^ref] to which one or more actuators116-120 and126 is to be moved. Reference positions [θ_i^ref] may, for example, be the link positions or other element positions of a theoretical perfectly tuned machine with no dynamic response error. Reference positions [θ_i^ref] are output bymachine actuators model182 todynamic machine model184, to errorcancellation module186, and also to forward kinematics processing158 andJacobian generator152.
• As stated, differential control signal vector [θ_i^u] and reference positions [θ_i^ref] are input todynamic machine model184. In addition,dynamic machine model184 receives actual link or element positions [θ_i] of the real machine. From these or other suitable inputs,dynamic machine model184 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 inventiondynamic machine model184 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 actuator116-120 and126, 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 ofboom105, or other articulated structure, supporting shovel orbucket112 or another end effector.
• Input into position and velocityerror cancellation module186 are both the modeled reference positions [θ_i^ref], frommachine actuators model182 and the actual positions [θ_i] of the real machine.Error cancellation module186 also receives the modeled input fromdynamic machine model184. From these several inputs,error cancellation module186 generates and outputs a correction signal vector [θ_i^c] to integrator188 and thence tojoint control system154.
• To generate correction signal vector [θ_i^c], error cancellation module1.86 compares the actual machine element positions [θ_i] with the respective modeled reference positions [θ_i^ref] 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 [θ_i^ref], or effect some other useful adjustment of the actual positions [θ_i].
• Integrator188 applies correction signal vector [θ_i^c] as an adjustment to differential control signal vector [θ_i^u] received from differentialinverse kinematics model150, which has been calculated to cause the end effector to follow a trajectory specified intrajectory generator146. Summator188 effects the adjustment to differential control signal vector [θ_i^u] for each active one of machine actuators116-120 and126. The adjusted differential control signal vector [θ_i^u] is supplied tojoint control system154 which outputs a real machine actuator velocity control signal [φ_i], incorporating the adjustments to machine actuators116-120 and126.
• Machine actuators116-120 and126 move their associated movable machine elements, for example,boom104,arm108, bucket or shovel112 and/orother elements128 to have positions and/or velocities which are adjusted compared with what their values would have been without the application of correction signal vector [θ_i^c] 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 [θ_i^ref] and velocity control signal [φ_i^ref] 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 model184 is adapted to approximate a given real-world machine, by modifying link positions signal [θ_i^ref] and velocity control signal [φ_i^ref] 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 actuators116-120,126 andhuman supervisor144 to provide totrajectory generator146 one or more trajectory families for executing desired tasks, including repetitive tasks.
• As described in Danko '404,human supervisor144 can observe the machine, and providevisual feedback160, usingjoysticks142 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 todynamic machine model184 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 thepivots106,110 or114 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 ofboom105 toturret102; the angle oflever arm108 to boom105; and the angle ofbucket112 tolever arm108. Additional angular descriptors may optionally include the rotational angle ofturret102 about a vertical axis, called “slew”; the angle of ground-engagingsystem101 to the horizontal about a transverse axis (perpendicular to the paper); and, possibly, the angle of ground-engagingsystem101 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 [θ_i^ref], actual machine link positions [θ_i], differential control signal vector [θ_i^u], correction signal vector [θ_i^c], reference actuator velocity control [φ_i^ref] 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 inFIG. 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 toFIG. 7, abucket112 is supported on the viewed side by apivot pin114 and is pivoted or rotated aboutpin114 by a hydraulic actuator, for example shovel rotator120 (not shown in this figure), coupled toaperture192.Bucket112 has aleading edge194 which engages the work material, ground, rock face, bulk deposit or the like.
• Bucket edge194 moves along anautomated trajectory196 advancing toward the host machine (not shown) with a velocity v_tread as a tangent totrajectory196. The angle of the trajectory tangent to the horizontal is designated “α”.Bucket edge194 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 movebucket112 along a selected trajectory such as196 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 inFIGS. 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 inFIGS. 7-8.
• Referring toFIG. 7, the control shown which is suggested for lefthand use comprises ajoystick198 capable of pivotal movement in two orthogonal, generally horizontal, directions. Back-and-forth movement can be used to control trajectory angle α, as indicated byarrow200. 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 byarrow202.
• Referring toFIG. 8, the control shown which is suggested for righthand use comprises ajoystick204 also capable of pivotal movement in two orthogonal, generally horizontal, directions. Back-and-forth movement can be used to control the bucket velocity v_talong thetrajectory196, as indicated byarrow206. Left-to-right movement can be used to control, or override, the cutting angle β, as indicated byarrow208.
• 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 inFIG. 6, or other useful trajectory, to the ground or other work surface may be trimmed so that the tool angle, of bucket or shovel112 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.
EXAMPLEApproximate Causal System Model
• For an n-link machine, the joint angular positions x₁, x₂, . . . , x_ncan be denoted by the n-vector x=[x₁, x₂, . . . , x_n]. A simplified motion equation can be written as a second-order vector differential equation [ref] between positions x, velocities dx/dt, and accelerations d²x/dt²:
• $\begin{array}{cc}M\left(x\right)\frac{{}^2x}{t^2}+D\left(x\right)\frac{x}{t}+S\left(x\right)x=A\left(\frac{x}{t},x\right)& \left(1\right)\end{array}$
• In Eq (1), M(x), an n x n matrix that represents the moment of inertia and mass reduced to the joint axes. D(x), S(x) are n x 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 x_i(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:
• $\begin{array}{cc}M\left(t\right)\frac{{}^2x}{t^2}+D\left(t\right)\frac{x}{t}+S\left(t\right)x=A\left(u,\frac{x}{t},t\right)& \left(2\right)\end{array}$
• A first-order, linear approximation (as the first term of the Taylor series) is applied to A:
• $\begin{array}{cc}A\left(u,x,\frac{x}{t},t\right)=\mathrm{Ju}\left(A\right)u+\mathrm{Jx}\left(A\right)x+\mathrm{Jdx}\left(A\right)\frac{x}{t}+T\left(t\right)& \left(3\right)\end{array}$
• Where Ju(A), Jx(A), and Jdx(A) are the following Jacobian matrices:
• $\begin{array}{cc}\mathrm{Ju}\left(A\right)=\left[\begin{array}{ccc}\frac{\partial A_{1,1}}{\partial u_1}& \dots & \frac{\partial A_{1,n}}{\partial u_n}\\ ⋮& & \\ \frac{\partial A_{n,1}}{\partial u_1}& \dots & \frac{\partial A_{n,n}}{\partial u_n}\end{array}\right]\mathrm{Jx}\left(A\right)=\left[\begin{array}{ccc}\frac{\partial A_{1,1}}{\partial x_1}& \dots & \frac{\partial A_{1,n}}{\partial x_n}\\ ⋮& & \\ \frac{\partial A_{n,1}}{\partial x_1}& \dots & \frac{\partial A_{n,n}}{\partial x_n}\end{array}\right]\mathrm{Jdx}\left(A\right)=\left[\begin{array}{ccc}\frac{\partial A_{1,1}}{\partial {\stackrel{.}{x}}_1}& \dots & \frac{\partial A_{1,n}}{\partial {\stackrel{.}{x}}_n}\\ ⋮& & \\ \frac{\partial A_{n,1}}{\partial {\stackrel{.}{x}}_1}& \dots & \frac{\partial A_{n,n}}{\partial {\stackrel{.}{x}}_n}\end{array}\right]& \left(4\right)\end{array}$
• In Eq (3), T(t) represents a time-dependent torque vector. Substituting (3) into (2), after simplification, gives:
• $\begin{array}{cc}M\left(t\right)\frac{{}^2x}{t^2}+\left[D\left(t\right)-\mathrm{Jdx}\left(A\right)\right]\frac{x}{t}+\left[S\left(t\right)-\mathrm{Jx}\left(A\right)\right]x=\mathrm{Ju}\left(A\right)u+T\left(t\right)& \left(5\right)\end{array}$
• Eq (5) can be re-arranged. For the x_i(i=1 . . . n) component, it reads:
• $\begin{array}{cc}{M}_{i,i}\frac{{}^2x_i}{t^2}+\left(D_{i,i}-{\mathrm{Jdx}}_{i,i}\right)\frac{x_i}{t}+\left(S_{i,i}-{\mathrm{Jx}}_{i,i}\right)x_i=F\left(t,x_j,u_j\right)+{\mathrm{Ju}}_{i,i}u_i& \left(6\right)\end{array}$
• Where F(t, x_j, u_j) (j≠i) is:
• $\begin{array}{cc}F\left(t,x_i,x_j,u_j\right)=-\sum _{j=1j\ne i}^{n}\left[M_{i,j}\frac{{}^2x_j}{t^2}+\left(D_{i,j}-{\mathrm{Jdx}}_{i,j}\right)-\frac{x_j}{t}+\left(S_{i,j}-{\mathrm{Jx}}_{i,j}\right)x_j+{\mathrm{Ju}}_{i,j}u_j\right]+T_i& \left(7\right)\end{array}$
• 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 M_{i, i}, D_{i, i}, Jx_{i, i}, Jdx_{i, i}, Ju_{i, i}, S_{i, i}and T_i(i=1 . . . n) are a best match to the predicted x_iangular positions from a model with observed (measured) x_i^Mvalues over a sufficiently long ΔT time period, for a given u_ias known control input. While alternative methods can be employed, for example, direct x^Msignal filtering during model building [Kalman], in the instant example, a simple least-square-fit match is applied:
• $\begin{array}{cc}{\int }_t^{t+\Delta T}{\left[x_i\left(t\right)-x_i^M\left(t\right)\right]}^2t=\mathrm{minimum}i=1\dots n& \left(8\right)\end{array}$
• 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 x_iand x_j, u_j(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:
• $\begin{array}{cc}{m}_i\frac{{}^2x_i}{t^2}+k_i\frac{x_i}{t}+q_ix_i=f_i\left(t\right)+a_iu_i& \left(9\right)\end{array}$
• In Eq (9) m_{i, i}=M_{i, i}, k_i=D_{i, i}−Jx_{i, i}, −Jx_{i, j}, and f_i=F(t, x_j, u_j), a_i=Ju_{i, i}notations are used; f_i(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 F_i(t) only, it is subject to identification. Eliminating indexing for simplification in notation when applying Eq (9) for each link independently, it gives:
• $\begin{array}{cc}\frac{{}^2x_i}{t^2}+P\frac{x}{t}+\mathrm{Cx}=F\mathrm{Where}& \left(10\right)\\ \begin{array}{c}\begin{array}{c}P=\frac{k_i}{m_i}\\ C=\frac{q_i}{m_i}\end{array}\\ F=\left(\frac{f_i}{m_i}+\frac{a_iu}{m_i}\right)\end{array}\}& \left(11\right)\end{array}$
• 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:
• $\begin{array}{cc}u=b{\int }_{t-\Delta \mathrm{Tv}}^tV\tau +d& \left(12\right)\end{array}$
• 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:
• $\begin{array}{cc}F=A{\int }_{t-\Delta \mathrm{Tv}}^tV\tau +B& \left(13\right)\end{array}$
• where the new constants are:
• $\begin{array}{cc}\begin{array}{c}A=b\frac{a_i}{m_i}\\ B=d+\frac{f_i}{m_i}\end{array}\}& \left(14\right)\end{array}$
• Substituting Eq (13) into Eq (10) gives:
• $\begin{array}{cc}\frac{{}^2x_i}{t^2}+P\frac{x}{t}+\mathrm{Cx}=A{\int }_{t-\Delta \mathrm{Tv}}^tvc+B& \left(15\right)\end{array}$
• 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 x^Musing 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 x_rseries of samples (r=1 . . . N) allows for re-writing Eq (10) into a finite-difference equation:
• $\begin{array}{cc}\frac{x_{r+1}+x_{r-1}-2x_r}{\Delta t^2}+P\frac{x_{r+1}-x_{r-1}}{2\Delta t}+{\mathrm{Cx}}_r=A\sum _{i=r-N_r}^rV_i+\Delta t+B\mathrm{Where}\text{:}\mathrm{Nv}=\frac{\Delta \mathrm{Tv}}{\Delta t}& \left(16\right)\end{array}$
• Matching x_rwith the measured x_r^Mvalues in a least-squares-fit sense over N samples is equivalent of replacing x_rwith x_r^Min Eq (16) and solving for P, A, B, C from an over-determined set of N−2 equations (if N>6):
• $\begin{array}{cc}\left[\begin{array}{c}\frac{x_3^M+x_1^M-2x_2}{\Delta t^2}\\ ⋮\\ \frac{x_N^M+x_{N-2}^M-2x_{N-1}}{\Delta t^2}\end{array}\right]=\left[\begin{array}{cccc}\frac{x_1^M-x_3^M}{2\Delta t^2}& \sum _{i=2-\mathrm{Nv}}^2V_i& 1& -x_2^M\\ ⋮& & & \\ \frac{x_{N-2}^M-x_N^M}{2\Delta t^2}& \sum _{i=N-\mathrm{Nv}-1}^2V_i& 1& -x_{N-1}^M\end{array}\right]\left[\begin{array}{c}P\\ A\\ B\\ C\end{array}\right]& \left(17\right)\end{array}$
• 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 x_rfrom an initial position. The solution can be used for position interpolation (when t<t_present), filtering (when t=t_present), or prediction (when t>t_present), with control vector V, which can include forward-predicted elements for t>t_present. 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 (19)

1. 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 wherein the control system comprises the generation of reference positions for each of multiple controlled movable machine elements according to a software-defined differential kinematics configuration, and applies a position error correction system to correct discrepancies between the actual and the desired machine element trajectories and wherein the error correction module:

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.

2. A machine according toclaim 1 wherein the reference position is modeled for a hypothetical model machine, using a software-defined differential kinematics configuration, under identical control command to that of the real machine, save for the corrections

3. A machine according toclaim 1 wherein the control system comprises two operational parts respectively capable of separately processing a reference position vector according to a software-defined differential kinematics and an actual position vector.

4. A machine according toclaim 3 wherein the reference position vector and the actual position vector are actively connected using a closing control loop which compares the actual position vector with the reference position vector and generates the correction signal.

5. A machine according toclaim 1 wherein 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.

6. A machine according toclaim 1 wherein the machine has multiple movable machine components contributing to movement of the end effector and the absolute position information comprises absolute position information regarding each movable machine element.

7. A machine according toclaim 1 wherein the position error correction system comprises at least one of a joint control model, a machine actuators model and a dynamic machine model.

8. A machine according toclaim 1 wherein the position error correction system comprises a joint control model, a machine actuators model and a dynamic machine model.

9. A machine according toclaim 1 wherein the position error correction system can operate to correct velocity error.

10. A machine according toclaim 1 comprising at least one acceleration sensor, the or each acceleration sensor being supported by a machine element, to provide absolute position signals regarding the respective machine element.

11. A machine according toclaim 6 wherein an accelerometer is provided at or on each movable machine element.

12. A machine according toclaim 1 wherein 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.

13. A machine according toclaim 1 wherein the control system applies the correction signal kinematically, providing continual incremental adjustments while the machine is moving to execute a specified end effector trajectory.

14. A machine according toclaim 1 comprising a coordinated joint motion system including:

a) support;

b) multiple links;

c) 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;

d) multiple actuators to effect said relative movement between the connected members, the multiple actuators being controlled by the coordinate joint control system; and

e) an end effector supported by the jointed links for movement relative to the support;

wherein the coordinated joint motion system is capable of execution of an automated end effector trajectory without human intervention wherein the coordinated joint control system comprises an operator interface enabling a human supervisor to change the end effector motion or position during execution of the automated trajectory.

15. A machine according toclaim 14 comprising 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.

16. A machine according toclaim 15 comprising a mining machine, a construction machine, an excavator, a front loader or a mechanical shovel.

17. A machine according toclaim 16 wherein the multiple links comprise a boom revolutely connected to the machine, an arm revolutely connected to the boom and a tool, optionally a bucket, revolutely connected to the arm.

18. A control system for a machine according toclaim 1, the control system comprising a position error correction module to correct discrepancies between the actual end effector trajectory and the desired end effector trajectory, wherein the error correction module:

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.

19. A method of controlling a machine according toclaim 1, the method comprising 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.

Images