Unified control of a work implement

[0002] The present invention generally relates to controlling a work implement. More particularly, the invention relates to a unified control system for controlling the motion of a work implement such as a bucket coupled to a work vehicle such as a backhoe or an excavator.

[0003] A typical backhoe includes an elongated boom with a dipper stick assembly articulately connected to the distal end of the boom. A work implement such as a bucket, or the like, is connected to the distal end of the dipper stick assembly. The boom, the dipper stick assembly, and the work implement are relatively massive components that develop substantial inertia as they move from one position to another.

[0004] As will be appreciated by those skilled in the art, a cylinder end of each hydraulic cylinder is pivotally connected to the implement frame to allow pivotal movement of the drivers in response to movements of the backhoe apparatus to opposite sides of the implement or machine. As is conventional, each hydraulic cylinder has a piston rod that linearly extends from the cylinder end of the driver. The rod end of each cylinder is articulately connected to the swing tower as by a pin passing endwise through a weldment. The pins that connect the rod ends of the cylinders to the swing tower each extend along an axis that is parallel to the vertical swing axis of the swing tower.

[0005] The backhoe bucket is conventionally controlled by a set of operator controls. The typical operator controls provide either extension or retraction of hydraulic cylinders, or rotation of the joints connecting the backhoe members. Conventional backhoes are not configured to accept operator inputs that correspond to motions of the backhoe bucket in Cartesian space.

[0006] Traditional controls for backhoes typically only control the motions of the backhoe apparatus disregarding any constraints on the backhoe apparatus such as the avoidance of any obstacles, controlling the force applied by the backhoe apparatus, minimizing time to travel, minimizing gravity torque, preventing engine stall or backhoe tippage, or other user defined constraints. In traditional controls, such constraints are met only through the combination of skill and experience exhibited by the operator in manipulating the controls, thereby preventing operation by unskilled operators and causing erroneous operation by even skilled operators.

[0007] Many conventional backhoe controls do not control the flow of hydraulic fluid into the cylinder; rather, many conventional controls attempt to control the angular position of the backhoe apparatus joints. Also, many conventional controls do not include inertial parameters in the control design.

[0008] Conventional backhoe controls are not configured to account for the inertial parameters and forces on the backhoe members. Nor are conventional backhoe controls configured to provide hydraulic fluid flow control signals as opposed to angular rate or velocity signals. Furthermore, conventional backhoe controls are not configured to adapt to changes in available hydraulic fluid flow conditions, or changes in the linkage dynamics of the system.

[0009] Thus, there is a need and desire for a unified control for a work implement such as a bucket coupled to a work vehicle such as an excavator or a backhoe. There is also a need and desire for a unified control for a backhoe or excavator that controls the work implement tip position. There is also a need and desire for a unified control that accounts for predetermined constraints during control of the backhoe or excavator apparatus. Further still, there is a need and desire for a unified control that incorporates the positional control and constraints into the minimization of an objective criterion.

[0010] Further still, there is a need and desire for a unified control that accounts for inertial parameters and forces on the backhoe or excavator members. Further still, there is a need and desire for a unified backhoe or excavator control that provides hydraulic fluid flow control signals. Further still, there is a need and desire for a unified backhoe or excavator control that is configured to adapt to changes in available hydraulic fluid flow conditions, or changes in the linkage dynamics of the system.

[0011] The present invention relates to an apparatus for controlling a work implement. The apparatus includes a means for defining the posture of a work implement. The apparatus also includes a means for defining a unified vector, including the implement posture. Further, the apparatus includes a means for providing kinematic control signals for the implement posture. The kinematic control signal is provided by a kinematic controller configured to control a system that is any one of a redundant system, an exact system, and an overdetermined system.

[0012] The present invention also relates to a method for controlling a work implement for a work vehicle. The method includes defining the posture of the work implement. The method also includes defining a unified vector, the unified vector including a bucket posture. The method further includes defining a control objective, in terms of the unified vector. Further still, the method includes providing a kinematic control signal formulated to minimize the control objective, by applying a transformation that can be used on any one of a redundant system, an exact system, and an overdetermined system.

[0013] The present invention further relates to an apparatus for controlling a work implement for a work vehicle. The work implement includes a plurality of actuatable joints, actuatable by a plurality of hydraulic actuators. The apparatus includes a kinematic controller receiving a command signal representative of a command posture signal and a measured posture signal and providing a first output signal representative of the angular velocity of the joints of the work implement. The first output signal is generated based on the mathematical optimization of an objective criterion. The present invention still further relates to a flow controller receiving the first output signal from the kinematic controller and one of a signal representative of the actual flow and an estimated flow signal. The flow controller provides a signal representative of the stem displacement of the plurality of hydraulic actuators.

[0014] The present invention still further relates to an apparatus for controlling a work implement for a work vehicle. The work implement includes a plurality of actuatable joints, actuatable by a plurality of hydraulic actuators. The apparatus includes a kinematic controller receiving a command signal representative of a command posture signal and receiving a measured posture signal. The kinematic controller provides a first output signal representative of the angular velocity of the joints of the work implement. The first output signal is generated based on the mathematical optimization of an objective criterion. The apparatus also includes an adaptive controller for generating a cylinder force control signal.

[0015] Still further the present invention relates to a method for controlling a work implement. The work implement has n actuatable joints. The method includes defining an m-by-1 posture vector to represent bucket position and orientation with respect to a fixed Cartesian Coordinate System, defining a k-by-1 additional feature vector, and defining an objective criterion that is a function of the posture vector and the additional feature vector. The method also includes obtaining a desired n-by-1 joint angle velocity vector based on minimization of the object criterion.

[0016] The invention will become more fully understood from the following detailed description, taken in conjunction with the accompanying drawings, wherein like reference numerals refer to like parts, in which:

[0017] FIG. 1 is a fragmentary perspective view of an off-highway implement having a backhoe apparatus mounted thereto; and

[0018] FIG. 2 is a block diagram of a kinematic and dynamic controller.

[0019] Referring to FIG. 1, there is depicted an off-highway work vehicle 10, having a backhoe apparatus 12, attached thereto. Implement 10 includes a fore-and-aft extending frame 14 that is suitably supported for movement across a field. Backhoe apparatus 12 typically includes a swinging boom 16, a dipper stick 18, and a work implement depicted as bucket 20. Movement of bucket 20 is controlled by a set of controls 22. Bucket 20 is moved by a hydraulic cylinder 24. Dipper stick 18 is moved by a hydraulic cylinder 26. Boom 16 is likewise moved by a hydraulic cylinder 28. The horizontal swinging action of boom 16 is controlled by a set of hydraulic cylinders 30.

[0020] In a preferred embodiment of the present invention, controls 22 control all of hydraulic cylinders 24, 26, 28, and 30. Further, controls 22 work in conjunction with unified controller electronics (to be discussed herein) to provide the desired movement of bucket 20.

[0021] Referring now to FIG. 2, a unified controller 40 is depicted. Unified controller 40 includes a kinematic controller 45 and a dynamic controller 50. Unified controller 40 is adapted to control a backhoe or excavator such as backhoe apparatus 12, or the like. Backhoe apparatus 12 may be kinematically exact, redundant, or overdetermined. A backhoe or like apparatus is kinematically exact if the number of posture component (m) is equal to the number of moveable joints (n) that make up the backhoe or work implement. Thus, a backhoe is kinematically redundant if m<n, and a backhoe is kinematically overdetermined if m>n. When a backhoe arm is kinematically exact, control design is substantially simplified because there is only one set of joint movements that lead to a desired end effect. If a backhoe is kinematically overdetermined, some end effects may not be able to be achieved and therefore an optimization must be utilized to fit a best possible control in order to most closely achieve the desired end effect. If a backhoe arm is kinematically redundant, an infinite number of joint motions can lead to the same end effect (i.e., the solution to the control problem is not unique), therefore control design is decidedly more difficult because it is necessary to decide between a number of equivalent end effect objectives.

[0022] Because it is possible to design the hardware for a backhoe system that is kinematically exact, kinematically redundant, or kinematically overdetermined, it is beneficial to design a controller that can accommodate any of these situations so that the controller can easily be adapted thereto.

[0023] The present invention relates to a unified control system for a backhoe. The forward kinematic model for the backhoe can be represented as

[0024] P_m×l=f(θ_n×l)  (1)

[0025] where P=Posture vector (bucket position, preferably in Cartesian coordinates and bucket orientation) and θ=vector of joint angles (alternatively, the posture vector may include any representative state variables or transformed state variables, including but not limited to position, velocity, angular velocity, acceleration, angular acceleration, or linear and nonlinear combinations thereof).

[0026] The time derivative of the forward kinematic model may be represented as

[0027] {dot over (P)}_m×l=J_P(θ){dot over (θ)}_n×l  (2)

[0028] where J_P(θ)=Jacobian matrix of the posture vector.

[0029] Additional features may be incorporated into the design of the controller, especially in machines having redundancies (e.g., some backhoe models include a linearly extended hole). Additional features can include obstacle avoidance, force control, and kinematic optimization. Machines without redundancies, may still be provided with the additional features of this control system design.

[0030] The additional features may be represented as

[0031] A_k×l=g(θ_n×l)  (3)

[0032] where A=Additional features vector and where k=the number of additional features.

[0033] The time derivative of the additional features is represented by

[0034] {dot over (A)}_k×l=J_A(θ){dot over (θ)}_n×l  (4)

[0035] where J_A=Jacobian matrix of the additional features vector.

[0036] A unified vector may be defined as $\begin{array}{cc}{U}_{\left(k+m\right)\times 1}={\left[\begin{array}{c}{P}_{m\times 1}\\ A_{k\times 1}\end{array}\right]}_{\left(m+k\right)\times 1}=h_{l\times 1}\left(\theta \right)& \left(5\right)\end{array}$

[0037] where U=Unified vector, and l=k+m.

[0038] The time derivative of the unified vector is given as $\begin{array}{cc}{\stackrel{.}{U}}_{l\times 1}=\left[\begin{array}{c}{J}_{p_{m\times m}}\\ J_{A_{k\times n}}\end{array}\right]\stackrel{.}{\theta }=J_{l\times n}\left(\theta \right){\stackrel{.}{\theta }}_{n\times 1}& \left(6\right)\end{array}$

[0039] where J(θ)=Unified Jacobian matrix.

[0040] An operator of backhoe 10 specifies a desired unified vector either by predetermining a number of constraints and/or by inputting control commands. In an alternative embodiment of the present invention the unified vector may be specified by an automated site planning and positioning system, that automatically controls the operations carried out at a work site without any, or with limited, operator interaction. The desired unified vector is defined as $\begin{array}{cc}{U}_d=\left[\begin{array}{c}{P}_d\\ A_d\end{array}\right]& \left(7\right)\end{array}$

[0041] where P_d=desired posture vector and A_d=desired additional feature vector.

[0042] The controller is configured to achieve

[0043] U(t)→U_d  (8)

[0044] in an optimal manner to satisfy both the desired posture and the desired additional feature.

[0045] Referring again to FIG. 2, unified kinematic control 45 is formulated as an optimal control problem with a set of constraints. The unified kinematic control is formulated as

[0046] MinΦ={dot over (E)}^TW{dot over (E)}+λE_Q²,  (9)

[0047] where λ is a constraint weighting factor such that λ=0 if E_Q≦0 and λ>0 if E_Q>0 (or there is insufficient available flow from the hydraulic pump system mounted on vehicle 10, which includes both a small pump and a large pump, discussed in detail later), and in an exemplary embodiment having three controlled hydraulic cylinders (alternatively, any number of cylinders could be controlled),

[0048] E=U_d−U,  (10)

[0049] E_Q=(Q₁+Q₂+Q₃−Q_S1−Q_S2),  (1)

[0050] W=Weighting factor,  (12)

[0051] Q₁=K₁{dot over (θ)}₁=Boom cylinder flow,  (13)

[0052] Q₂=K₂{dot over (θ)}₂=Dipper stick cylinder flow,  (14)

[0053] Q₃=K₃{dot over (θ)}₃=Bucket cylinder flow,  (15)

[0054] Q_S1=small pump flow, and  (16)

[0055] Q_S2=large pump flow.  (17)

[0056] An objective of the unified kinematic control is to find the angular rate hence the cylinder flow that minimizes the objective function Φ. To do this, an expression for the derivative of the objective function Φ with respect to the angular rates is $\begin{array}{cc}\frac{\partial \Phi }{\partial \stackrel{.}{\theta }}={\hspace{0.17em}}^{-}2J^{T}W{\stackrel{.}{U}}_d+2J^T\mathrm{WJ}\stackrel{.}{\theta }+2K\stackrel{.}{\theta }-2S& \left(18\right)\end{array}$

[0057] setting $\begin{array}{cc}\frac{\partial \Phi }{\partial \stackrel{.}{\theta }}=0& \left(19\right)\end{array}$

[0058] and solving for {dot over (θ)}_d, an expression for {dot over (θ)}_dmay be derived as

[0059] {dot over (θ)}_d=[J^TWJ+K]⁻¹[J^TW{dot over (U)}_d+S],  (20)

[0060] and may be augmented as

[0061] {dot over (θ)}_d=[J^TWJ+K]⁻¹[J^TW({dot over (U)}+K_eE)+S  (21)

[0062] where the K_eE term has been added to avoid the steady state error E. Further, $\begin{array}{cc}\begin{array}{c}K=\lambda \left[\begin{array}{ccc}{K}_1^2& K_1{K}_2& K_1{K}_3\\ K_1{K}_2& K_2^2& K_2{K}_3\\ K_1{K}_3& K_2{K}_3& K_3^2\end{array}\right]\\ =\mathrm{matrix}\mathrm{of}\mathrm{cylinder}\mathrm{flow}\mathrm{conversion}\mathrm{factors}\end{array}& \left(22\right)\end{array}$

[0063] and $\begin{array}{cc}S=\lambda \left[Q_{\mathrm{S1}}+Q_{\mathrm{S2}}\right]\left[\begin{array}{c}{K}_1\\ K_2\\ K_3\end{array}\right].& \left(23\right)\end{array}$

[0064] As the desired angular rates ({dot over (θ)}_id) are calculated, the flow rates may be calculated therefrom, as

[0065] Q_id=K_i{dot over (θ)}_id,  (24)

[0066] when i=1 , . . . , n, to provide the desired flow rates for each cylinder. The advantage of using an optimal kinematic control as provided above is that both trajectory and flow constraints are satisfied by some optimal combination of the trajectory and flow constraints. Further, additional features (represented by reference numeral 85 in FIG. 2) may be specified which are added as constraints in the unified kinematic control.

[0067] For example, a posture control may be specified as an additional feature 85. A posture control may be configured to maintain a constant bucket angle to achieve a desired slope of a dig. For example, if a desired angle for the slope is 45°, one would specify in the desired additional features 44,

[0068] A_d(t)=45°  (25)

[0069] where A(t)=θ₃and θ₃is the bucket angle. Alternatively, any joint angle or function representative of slope may be specified in a similar manner.

[0070] Another additional feature 85 that may be specified may be obstacle avoidance. Obstacle avoidance may be used to provide an avoidance zone near the cab or it may be used to provide an avoidance zone where there is limited space for maneuvering the backhoe. The avoidance zone may be defined as

[0071] S(θ)≧C  (26)

[0072] such that if S(θ)≧C the constraint is ignored. If S(θ)<C the constraint is active and S(θ) is set equal to C. Thus, additional feature 85 is defined as

[0073] A=S(θ)  (27)

[0074] with desired additional feature 44,

[0075] A_d=C.  (28)

[0076] A further additional feature 85 that may be specified is force control. Force control may be used when a uniform hardness on the excavation surface is required, or a specific slope compaction is required. To fulfill this objective, a constant force (F) is maintained and the joint torque needed to produce F is minimized. The joint torque may be defined as

[0077] T=J_p^T(θ)F.  (29)

[0078] The objective then is to minimize the joint torque by minimizing an objective criterion Φ_Twhere

[0079] Φ_T=∥T∥_W²=T^TWT.  (30)

[0080] The time derivative of Φ_Tis $\begin{array}{cc}\frac{d{\Phi }_T}{dt}=H^T\frac{\partial {\Phi }_T}{\partial \theta },& \left(31\right)\end{array}$

[0081] so to minimize Φ_Twith respect to time, it is desirable to drive $\frac{d{\Phi }_T}{dt}$

[0082] to 0 so desired additional feature 44, A_d(θ)=0 and $\begin{array}{cc}A\left(\theta \right)=H^T\frac{\partial {\Phi }_T}{\partial \theta }& \left(32\right)\end{array}$

[0083] where

[0084] J_pH=0.  (33)

[0085] A further additional feature 85 that may be specified is carry with load which provides a trajectory requiring a minimum time to travel while providing a minimum gravity torque due to the payload. This minimization may be formulated generally as

[0086] MinΦ_G=G^T(θ)WG(θ)  (34)

[0087] where G(θ)=Gravity Torque. So, similar to force control, to minimize Φ_Gwith respect to time, it is desirable to drive $\frac{d{\Phi }_G}{dt}$

[0088] to 0 so A_d(θ)=0 (provided in desired additional features 44) and $\begin{array}{cc}A\left(t\right)=H^T\frac{\partial \Phi }{\partial \theta }.& \left(35\right)\end{array}$

[0089] This additional carry with load feature helps to reduce the amount of stress on the backhoe vehicle joints.

[0090] Further still, an additional feature 85 that may be added to the backhoe control is a normal digging feature. A normal digging feature ensures that the engine does not stall due to overload and that the excavator/backhoe (vehicle) does not tip due to weight overloading. The additional feature is defined as

[0091] A(t)=0 if F_max−F≧0 and  (36)

[0092] A(t)=F_max−F if F_max−F<0  (37)

[0093] where,

[0094] F_max=min(F_max, F_max)*K,  (38)

[0095] such that

[0096] F_maxis the force that causes the engine to stall;

[0097] F_maxis the force that causes the backhoe/excavator to tip; and

[0098] K is a safety factor less than 1.

[0099] The objective is to obtain A_d(t)=0 provided by desired additional features 44. Alternatively, the engine overload and the anti-tipping features could be formulated as separate features.

[0100] Further, any other additional features 85 may be specified using the general formulation

[0101] Min ψ(θ)  (39)

[0102] where $A\left(t\right)=H^T\frac{\partial \psi }{\partial \theta },$

[0103] J_pH=0, with the desired additional feature 44, A_d=0.

[0104] For example, joint angular velocity or tip velocity may be minimized, however the additional features are not limited to any of those features disclosed above nor are the formulations for the additional features limited to those provided above. It is well known to those of ordinary skill in the art to formulate the objective criterion in numerous ways.

[0105] Once the kinematic control has supplied a signal representative of the flow necessary in each cylinder, the actuator effects and linkage dynamics are taken into account in unified dynamic controller 50 (which may be an adaptive controller). In a preferred embodiment, dynamic controller 50 is a model reference adaptive control utilizing a model of the linkage dynamics. The linkage dynamics may be represented as

[0106] [J(θ)+J_c(θ)]{umlaut over (θ)}+H{dot over (θ)}+[C(θ,{dot over (θ)})+C_c(θ,{dot over (θ)})]{dot over (θ)}  (40)

[0107] +G(θ)−N(θ)F=0

[0108] where J(θ)=the inertia matrix for the linkages.

[0109] J_c(θ)=the actuator affects on the linkage inertias

[0110] H=the linkage friction matrix

[0111] C(θ,{dot over (θ)})=the linkage Coriolis matrix

[0112] C_c(θ,{dot over (θ)})=the actuator effect on the linkage Coriolis matrix

[0113] G(θ)=the gravity effect

[0114] N(θ)=the force to torque conversion matrix

[0115] θ=[θ₁θ₂θ₃θ₄]^T=the joint angles

[0116] F=[F₁F₂F₃F₄]^T=the cylinder force vector

[0117] where F₁=P_i1A_i1−P_i2A_i2and

[0118] P_i1=pressure in the with cylinder at the head end

[0119] P_i2=pressure in the with cylinder at the rod end

[0120] A_i1=cross sectional area of the cylinder at the head end

[0121] A_i2=cross sectional area of the cylinder at the rod end.

[0122] The actuator dynamics may be given as the set of differential equations

[0123] {dot over (P)}₁₁=K₁{dot over (y)}₁+K₂Q₁₁  (41)

[0124] {dot over (P)}₁₂=K₃{dot over (y)}₁+K₄Q₁₂  (42)

[0125] {dot over (P)}₂₁=K₅{dot over (y)}₂+K₆Q₂₁  (43)

[0126] {dot over (P)}₂₂=K₇{dot over (y)}₂+K₈Q₂₂  (44)

[0127] {dot over (P)}₃₁=K₉{dot over (y)}₃+K₁₀Q₃₁  (45)

[0128] {dot over (P)}₃₂=K₁₁{dot over (y)}₃+K₁₂Q₃₂  (46)

[0129] {dot over (P)}₄₁=K₁₃{dot over (y)}₄+K₁₄Q₄₁  (47)

[0130] {dot over (P)}₄₂=K₁₅{dot over (y)}₄+K₁₆Q₄₂  (48)

[0131] where

[0132] {dot over (y)}_i=the cylinder velocity;

[0133] K_j=the cylinder coefficient; and

[0134] Q_ij=the flow in cylinder i at the j end (j=1=head end, j=2=rod end).

[0135] Standard hydraulic equations for hydraulic cylinders may be given as $\begin{array}{cc}{Q}_{\mathrm{i1}}=A_{\mathrm{i1}}\left(u_i\right)C_d\sqrt{\frac{2\left(P_s-P_{\mathrm{i1}}\right)}{\rho }}-A_{\mathrm{i2}}\left(u_i\right)C_d\sqrt{\frac{2P_{\mathrm{i1}}}{\rho }}& \left(49\right)\\ Q_{\mathrm{i2}}=A_{\mathrm{i3}}\left(u_i\right)C_d\sqrt{\frac{2P_{\mathrm{i2}}}{\rho }}-A_{\mathrm{i4}}\left(u_i\right)C_d\sqrt{\frac{2\left(P_s-P_{\mathrm{i2}}\right)}{\rho }}& \left(50\right)\end{array}$

[0136] where u_i=the stem displacement for cylinder i

[0137] C_d=a fluid property constant;

[0138] ρ=the flow density; and

[0139] P_s=the supply pressure.

[0140] In a preferred embodiment of the invention, the unified kinematic control 45 is configured to generate the desired joint angle θ_dthat will meet flow control requirements. Thus, based on the measured joint angle θ_aand the desired joint angle θ_d, unified dynamic controller 50 (adaptive controller) is utilized to generate a desired cylinder force F_dto guarantee that U is driven to U_dunder different conditions, such as payload changes, etc.

[0141] In one embodiment of the present invention, both head end and rod end pressures are measured in each cylinder (represented as valve+actuator block) 70. Based on both of these measured pressures, the actual cylinder force F_acan be calculated as described above. A variable structure controller 55 is then used to generate the spool displacement based on the cylinder force errors (e_F) the spool displacement command u_icauses cylinder 70 to move, thereby producing a force (F_i) on linkage 75.

[0142] In an alternative embodiment, only one pressure is measured for each cylinder 70 along with one supply pressure. Based on the desired cylinder force F_dand pressure P_i1, pressure P_i2can be calculated. Thus, having P_i1and P_i2, the desired flow rates Q_i1and Q_i2, the spool displacement for each cylinder can be calculated.

[0143] Once the unified kinematic control 45 generates desired angular velocities or flow rates for each cylinder, a flow controller 58 can be used to force the actual flow rate to follow the desired flow rate, for simplicity. A closed loop controller such as this has one shortcoming however, that is that the operator loses the normal feeling of ground conditions in the control mechanisms. Thus, in order to provide an operator with a feeling of ground conditions, spool displacement (u_i) can be used as an input to the actuator dynamics pressure estimator 62 to estimate the cylinder pressures. The estimated cylinder pressures can then be used to provide physical or visual indications to the operator, such as by providing force feedback in control stick 65, regarding the ground conditions.

[0144] As linkage 75 moves, the angular displacement of each link is measured and is communicated to forward kinematics 80 and additional features 85 to calculate the current unified vector (U), for feedback to kinematic controller 45.

[0145] In a preferred embodiment of the present invention, cylinder velocity constraints can be added to the kinematic optimal control loop, such that the kinematic control is formulated as $\begin{array}{cc}\mathrm{Min}\Phi ={\stackrel{.}{E}}^{T}W\stackrel{.}{E}+\lambda E_Q^2+\sum _{i=1}^4{{\lambda }_i\left(V_i-V_{i_{\max }}\right)}^2& \left(51\right)\end{array}$

[0146] where V_i=the measured cylinder velocity and V_i=the maximum cylinder velocity. Thus, the term $\begin{array}{cc}\sum _{i=1}^4{{\lambda }_i\left(V_i-V_{i_{\max }}\right)}^2& \left(52\right)\end{array}$

[0147] is the cylinder velocity constraint. Solving for the desired joint angular velocity {dot over (θ)}_d,

[0148] {dot over (θ)}_d=[J^TWJ+K+K_λ]⁻¹[J^TW({dot over (U)}_d+K_eE)+S+S_λ],  (53)

[0149] with $\begin{array}{cc}{K}_{\lambda }=\left[\begin{array}{cccc}{\lambda }_1{a}_1^2& 0& 0& 0\\ 0& {\lambda }_2{a}_2^2& 0& 0\\ 0& 0& {\lambda }_3{a}_3^2& 0\\ 0& 0& 0& {\lambda }_4{a}_4^2\end{array}\right],& \left(54\right)\end{array}$

[0150] and $\begin{array}{cc}{S}_{\lambda }=\left[\begin{array}{c}{\lambda }_1{a}_1{V}_{1\max }\\ {\lambda }_2{a}_2{V}_{2\max }\\ {\lambda }_3{a}_3{V}_{3\max }\\ {\lambda }_4{a}_4{V}_{4\max }\end{array}\right]& \left(55\right)\end{array}$

[0151] where

[0152] V_i=a_i{dot over (θ)}₁,

[0153] a_i=conversion factor from joint velocities to cylinder linear velocities, and $J=\text{augmented Jacobian matrix}=\left[\frac{J_p}{J_A}\right].$

[0154] Also in a preferred embodiment, the feedback gain K_ecan be gain scheduled to reduce oscillation. Defining the bucket error as $\begin{array}{cc}{E}_b=\sqrt{{\left(x_{\mathrm{ref}}-x_a\right)}^2+{\left(y_{\mathrm{ref}}-y_a\right)}^2+{\left(z_{\mathrm{ref}}-z_a\right)}^2}& \left(56\right)\end{array}$

[0155] an exemplary gain schedule may be given as $\begin{array}{cc}{K}_e=\left\{\begin{array}{ccc}{K}_{\mathrm{eo}}& \mathrm{if}& E_b\ge b_1\\ f_{\mathrm{ke}}& \mathrm{if}& b_2<E_b<b_1\\ 0& \mathrm{if}& E_b\le b_2\end{array}\right\}& \left(57\right)\end{array}$

[0156] where b₁>b₂and f_keis a smooth transition function that connects K_eowith 0. An exemplary choice for f_keis $\begin{array}{cc}{f}_{\mathrm{ke}}=K_{\mathrm{eo}}\left[1-\mathrm{Sin}\frac{\pi }{2}{E}_b-\frac{b_1}{b_2-b_1}\right].& \left(58\right)\end{array}$

[0157] Alternatively a B-spline curve that connects two endpoints K_eoat E_b=b₁and 0 at E_b=b₂by f_ke=d₃t³+d₂t²+d₁t+d₀.

[0158] The weighting factor λ may also be gain scheduled such that $\begin{array}{cc}\lambda =\left\{\begin{array}{ccc}{\lambda }_0& \mathrm{if}& E_Q\ge b_{1\lambda }\\ f_{k\lambda }& \mathrm{if}& b_{2\lambda }<E_Q<b_{1\lambda }\\ 0& \mathrm{if}& E_Q\le b_{2\lambda }\end{array}\right\}& \left(59\right)\end{array}$

[0159] where f_kλ is preferably a smooth function that connects λ₀with 0. An exemplary choice for f_kλ is $\begin{array}{cc}{f}_{k\lambda }=\frac{{\lambda }_0}{2}\left[1+\mathrm{Cos}\pi E_Q-\frac{b_{1\lambda }}{b_{2\lambda }-b_{1\lambda }}\right].& \left(60\right)\end{array}$

[0160] Alternatively a b-spline curve could be applied.

[0161] It is also advantageous to gain schedule the values of K₁thru K₄. An exemplary gain schedule is

[0162] K_i=l_i*[0*(|{dot over (θ)}_i|<b_ik)+1*({dot over (θ)}_i>b_ik)−1*({dot over (θ)}_i<−b_ik)] i=1, . . . , 4  (61)

[0163] Thus, the value of K_iequals one of l_i,0,−l_i. The values of b_ikare preferably chosen by simulation or experiment.

[0164] Further still, it is beneficial to gain schedule the weighting matrix of the additional task. The weighting matrix may be $\begin{array}{cc}W=\left[\begin{array}{cc}{W}_b& 0\\ 0& W_a\end{array}\right]& \left(62\right)\end{array}$

[0165] where W_b=weighting matrix of basic task=W_b0*I_n×n, and W_a=weighting matrix of additional task=W_a0*I_r×rwith n=no. of basic tasks and r=no. of additional tasks. Therefore, $\begin{array}{cc}{w}_{\mathrm{ao}}=\{\begin{array}{ccc}0& \mathrm{if}& E_b>b_{\mathrm{w1}}\\ f_w& \mathrm{if}& b_{\mathrm{w1}}<E_b<b_{\mathrm{w2}}\\ w_{\mathrm{bo}}& \mathrm{if}& E_2>b_{\mathrm{w2}}\end{array}& \left(63\right)\end{array}$

[0166] with b_w1>b_w2.

[0167] An exemplary choice of function f_wis a smooth function that connects zero with w_bo. One choice is $\begin{array}{cc}{f}_w=w_{b_0}\left[1-\sin \frac{\pi }{2}{E}_b-\frac{b_{\mathrm{w2}}}{b_{\mathrm{w1}}-b_{\mathrm{w2}}}\right].& \left(64\right)\end{array}$

[0168] Another choice would be to build a b-spline curve that connects the two end points 0 at b_w1and w_b0at b_w2.

[0169] When there is a conflict between flow control (k), the desired velocity vector ({dot over (X)}_d) and error vector E, or a conflict between basic task and additional task, the generated velocity vector {dot over (θ)}_dmay violate the flow constraint occasionally. In such a situation it is desirable, in a preferred embodiment, to increase the value of λ until the flow constraint is satisfied. This may be accomplished through a heuristic approach provided as

[0170] (1) Initialize λ₀

[0171] (2) Calculate {dot over (θ)}_d=[J^TWJ+K]⁻¹[J^TW({dot over (x)}_d+K_eE)+S] $\left(1\right)\mathrm{Initialize}{\lambda }_0\left(2\right)\mathrm{Calculate}{\stackrel{.}{\theta }}_d={\left[J^T\mathrm{WJ}+K\right]}^{-1}\left[J^{T}W\left({\stackrel{.}{x}}_d+K_{e}E\right)+S\right]\left(3\right)\mathrm{If}\left[Q_{1d}+Q_{2d}+Q_{3d}+Q_{4d}-\left(Q_{\mathrm{s1}}+Q_{\mathrm{s2}}\right)\right]>E_{\mathrm{qo}}\mathrm{Then}\mathrm{increase}{\lambda }_0\left(e.g.,\mathrm{by}5\%\right)\mathrm{Goto}\left(2\right)\mathrm{Else}{Q}_T=Q_{1d}+Q_{2d}+Q_{3d}+Q_{4d}\mathrm{if}\left[Q_T>\left(Q_{\mathrm{s1}}+Q_{\mathrm{s2}}\right)\right]\mathrm{then}{S}_f=\frac{Q_{\mathrm{s1}}+Q_{\mathrm{s2}}}{Q_T}\mathrm{else}{S}_f=1$ $\mathrm{end}\mathrm{if}$ $\mathrm{End}\mathrm{if}\left(4\right){\stackrel{.}{\theta }}_d={\stackrel{.}{\theta }}_d*S_f$

[0172] This set of adaptive heuristics aids in solving oscillation problems and performance problems introduced by the unified kinematic control.

[0173] In a preferred embodiment of the present invention, a low level control loop may be implemented to adapt to hydraulic actuator delay and to adapt to actuator nonlinearities. A low level control loop may be of the type including, but not limited to Smith predictor or internal mode control. Further, a nonlinear transformer or other applicable control technique may be applied to adapt to actuator nonlinearities.

[0174] The control described above may be applied to a variety of work vehicles including, but not limited to, loaders, backhoes, loader/backhoes, skid-steers, and any vehicles or work implements having controlled joint movements.

[0175] While the detailed drawings, specific examples, and particular formulations given describe preferred embodiments of the present invention, they serve the purpose of illustration only. For example, the control methodologies, algorithms, and mathematical models of physical systems may differ depending on chosen control characteristics and the physical characteristics of the work vehicle or implement. The apparatus of the invention is not limited to the precise details and conditions disclosed. Furthermore, other substitutions, modifications, changes, and omissions may be made in the design, operating conditions, and arrangement of the preferred embodiments without departing from the spirit of the invention as expressed in the appended claims.