推土機(jī)的工作裝置結(jié)構(gòu)及液壓系統(tǒng)設(shè)計(jì),推土機(jī)的工作裝置結(jié)構(gòu)及液壓系統(tǒng)設(shè)計(jì),推土機(jī),工作,裝置,結(jié)構(gòu),液壓,系統(tǒng),設(shè)計(jì) .Automation in Construction 9 2000 421435 rlocaterautcon Impedance control of a hydraulically actuated robotic excavator Q.P. Ha ) , Q.H. Nguyen, D.C. Rye, H.F. Durrant-Whyte Australian Center for Field Robotics, The Uniersity of Sydney, J07, Sydney, 2006 NSW, Australia Abstract In robotic excavation, hybrid positionrforce control has been proposed for bucket digging trajectory following. In hybrid positionrforce control, the control mode is required to switch between position- and force-control depending on whether the bucket is in free space or in contact with the soil during the process. Alternatively, impedance control can be applied such that one control mode is employed in both free and constrained motion. This paper presents a robust sliding controller that implements impedance control for a backhoe excavator. The control law consists of three components: an equivalent control, a switching control and a tuning control. Given an excavation task in world space, inverse kinematic and dynamic models are used to convert the task into a desired digging trajectory in joint space. The proposed controller is applied to provide good tracking performance with attenuated vibration at bucketsoil contact points. From the control signals and the joint angles of the excavator, the piston position and ram force of each hydraulic cylinder for the axis control of the boom, arm, and bucket can be determined. The problem is then how to find the control voltage applied to each servovalve to achieve force and position tracking of each electrohydraulic system for the axis motion of the boom, arm, and bucket. With an observer-based compensation for disturbance force including hydraulic friction, tracking of the piston ram force and position is guaranteed using robust sliding control. High performance and strong robustness can be obtained as demonstrated by simulation and experiments performed on a hydraulically actuated robotic excavator. The results obtained suggest that the proposed control technique can provide robust performance when employed in autonomous excavation with soil contact considerations. q 2000 Elsevier Science B.V. All rights reserved. Keywords: Robotic excavator; Hybrid positionrforce control; Sliding controller 1. Introduction The usual task of a backhoe excavator is to free and remove material from its original location and to transfer it to another location by lowering the bucket, digging by dragging the bucket through the soil, then ) Corresponding author. Department of Mechanics and Mecha- tronic Engineering, The University of Sydney, J07, 2006 NSW, Australia. Tel.: q61-2-9351-3098; fax: q61-2-9351-7474. .E-mail address: quang.hamech.eng.usyd.edu.au Q.P. Ha . lifting, slewing and dumping the bucket. In moving towards automatic excavation, there is a need for the development of a controller that is robust to uncer- wx tainties associated with these operations 1 . For control purposes, kinematic and dynamic models of excavators that assume the hydraulic actuators act as infinitely powerful force sources are presented in wx Refs. 24 . Position control with a conventional proportional and derivative controller is used in Refs. wx 4,5 for simulation of the digging process with limited soil interaction. Excavators are, however, subject to a wide varia- tion of soiltool interaction forces. When digging, 0926-5805r00r$ - see front matter q 2000 Elsevier Science B.V. All rights reserved. .PII: S0926-5805 00 00056-X ()Q.P. Ha et al.rAutomation in Construction 9 2000 421435422 the bucket tip motion is effectively force-constrained by the nonlinear constitutive equations of the envi- ronment, and by the hydraulic forces. Compliance control approaches may therefore be considered to be more suitable than position control for the shap- ing of excavator dynamics. Compliant motion con- trol can generally be classified into two broad classes: hybrid positionrforce control and interactive control or impedanceradmittance control. In hybrid forcerposition control, the Cartesian space of the end-effector co-ordinates is decomposed into a posi- tion sub-space and a force sub-space. Separate posi- tion- and force-trajectory tracking objectives are specified in each sub-space. Excessive force tran- sients may, however, occur at the instant of contact between the tool and the environment. Rather than tracking desired position and force trajectories, inter- active control seeks to regulate the relationship be- tween the end-effector position and the interaction force. It is known that impedance control provides a unified approach to both unconstrained and con- wx strained motion 6 . If hybrid positionrforce control is adopted, the control mode should be switched between position control and force control according to whether the excavator bucket is in free space or in contact with the soil during an excavation task. Impedance control is believed to be better suited to excavation tasks in the sense that it can be applied continuously to both free and constrained motions wx 1 . An impedance controller has recently been re- wx ported for an excavator arm 7 . This paper proposes a robust sliding mode control technique to imple- ment impedance control for an excavator using gen- eralised excavator dynamics. The bucket tip is con- trolled to track a desired digging trajectory in the presence of environment and system parameter un- certainties. In impedance control of hydraulic exca- vators, the piston position and the ram force of each hydraulic cylinder for the axis control of the boom, arm, and bucket can be determined. The problem is then how to find the control voltage applied to the servovalves to track these desired commands. Taking into account friction and nonlinearities, a discontinu- ous observer is developed for estimating both piston displacement velocity and disturbance including load force and friction. With an observer-based compen- sation for the disturbance force, robust tracking the piston ram force and position is guaranteed using robust sliding mode controllers for electrohydraulic systems. The validity of the proposed method is verified through simulation and filed tests performed on the Komatsu PC-05 mini-excavator. The remain- der of this paper is organised as follows. Section 2 is devoted to the derivation of the excavator dynamic model. The problem formulation and the develop- ment of impedance control for excavator dynamics are presented in Section 3. The control of electrohy- draulic systems is addressed in Section 4. The hard- ware organisation for the robotic excavator is de- scribed in Section 5 together with computer simula- tions and experimental results. Finally, conclusions are provided in Section 6. 2. Excavator dynamics The equations of motion for a generic excavator can be derived by applying the EulerLagrange equations to a Lagrangian energy function, or by writing the NewtonEuler equations successively for each link of the machine. In the latter approach, the dynamics of each link are described by equations that recurse through the link index. The driving joint torques of the boom, arm, and bucket are generated by the forces of the hydraulic ram actuators. The translational and rotational motions of these links are described by the dynamic model of the excavator system. Dynamic models of excavators are presented wx wx in Ref. 2 with refinements in Ref. 4 . Firstly, a 4 Cartesian co-ordinate frame Oxyz fixed to the 0000 excavator body is chosen. Other Cartesian co-ordinate frames are systematically assigned by applying the wx Denavit and Hartenberg procedure as in Refs. 24 . 4 4 The frames Oxyz , Oxyz , Oxyz , 1111 2 222 3333 4 and Oxyz are respectively attached to the 4444 boom, arm, bucket, and bucket tip as seen in Fig. 1. Note that the movements of the excavator mecha- nism during digging usually occur in the vertical plane. It is therefore assumed that no boom-swing motion occurs during excavation, so that the boom . swing angle u is therefore held constant u s0 11 during digging. The model equations can be written for each link of the excavator by considering the link as a rigid free body. By combining Newton and Euler equations for all links, the dynamical model for the excavator can be expressed concisely in a ()Q.P. Ha et al.rAutomation in Construction 9 2000 421435 423 Fig. 1. Excavator joint variables. well-known form of manipulator equations of motion wx 4: D uuqC u ,uuqB u qG u . . . . sA u FyT F ,F ,1 . . . Ltn wx T where us uuu is the vector of measured 234 shaft angles: u for the boom joint, u for the arm joint, and u for the bucket joint; T represents the 4L load torques as functions of the tangential and nor- mal components, F and F , of the soil reaction force tn at the bucket, and F are the ram forces of the hydraulic actuators that produce the torques acting on the joint shafts. The tangential component, F , t which is parallel to the digging direction, represents the resistance to the digging of ground by excavators bucket teeth. This resistance is considered as the sum of soils resistance to cutting, the friction between the bucket and the ground, and the resistance to movement of the prism of soil and soil movement in the bucket. The tangential component can be calcu- wx lated according to Ref. 8 as F skbh,2 . t1 w y2 x where k is the specific digging force N m , and 1 h and b are respectively the thickness and width of wx the cut slice of soil m . The normal component, F , n is calculated as F sC F ,3 . nt . where cs 0.10.45 is a factor depending on the digging angle, digging conditions, and the wear and wx tear of the cutting edge 8 . The determination of the . matrices of inertia, D u , of Coriolis and centripetal . . effects, C u,uu, of gravity forces, G u , and of . functions of the moment arms, A u , is comprehen- wx sively described in Refs. 24 . All entries in these wx matrices are given in Ref. 4 . The 3=1-matrix of . viscous friction B u is treated in this paper as a source of uncertainty. . In the digging plane, the Jacobian J u defined as xsJ uu,4 . . wx w can be obtained from Ref. 3 , where xs xz 44 x T u represents the Cartesian co-ordinates and orien- O 4 . tation of the bucket tip O with respect to 4 4 . Oxyz . Assuming that the Jacobian matrix J e 0000 . is non-singular, Eq. 1 in joint space can be rewrit- ten in Cartesian space as: H xxqC x, xxqB x qG x . . . . xx yT sJAFyF ,5 . e where HsJ yT DJ y1 , C sJ yT CyDJ y1 JJ y1 , . x G sJ yT G, B sJ yT B,6 . xx and F sJ I T T denotes the generalised forces of eL . interaction between the end-effector bucket tip and . the environment soil . They consist of digging forces acting on the bucket with force entries for the co- . ordinates x , z and a torque entry around y . 44 4 The determination of the forward and inverse . y1 . kinematic relationships, xsL u and usL x wx . is detailed in Ref. 3 . As Eq. 5 has the form of the generalised robotic manipulator dynamics where x is a vector of the co-ordinates of the contact point of the manipulator with the environment, below and in the next section, we will consider in general xgR n and ugR n . We assume A1: HsHqAH, C sC qDC , xx x G sG qDG , AsAqD A, F sF qD F , xx x ee e 7 . where matrices H, C , G , and A are known, F is to xx e be measured by force sensors such as load pins, and DH, DC , DG , D A, and D F are uncertainties. xx e Denoting friction and uncertainties by Df x, x, x sDH xqDC xqDG qB x . xx yT qD F yJ D AF,8 . e ()Q.P. Ha et al.rAutomation in Construction 9 2000 421435424 . Eq. 5 can be rewritten as H xxqC x, xxqG x . . . xx suyF yDf x, x, x ,9 . . e where yT usJAF 10 . is the control input. . Remark 1. As D u is a 3=3-symmetric positive- definite matrix satisfying the skew symmetric prop- wx . . erty 9 , for the nominal dynamics Df x, x, x s0 w . .x of the excavator, H x -2C x, x is also a skew- x symmetric matrix, i.e. T xHxy2C x, xxs0, ;x.11 . . . x 3. Excavator dynamics impedance control 3.1. Problem formulation One of the excavating task elements is penetration of the soil by an excavator bucket to follow a pre-planned digging trajectory. During digging, three main tangential resistance forces arise: the resistance to soil cutting, the frictional force acting on the bucket surface in contact with the soil, and the resistance to movement of the prism of soil ahead of and in the bucket. The magnitude of the digging resistance forces depends on many factors such as the digging angle, volume of the soil prism, volume of material ripped into the bucket, and the specific resistance to cutting. These factors are generally variable and unavailable. Moreover, due to soil plas- ticity, spatial variation in soil properties, and poten- tial severe inhomogeneity of material under excava- tion, it is impossible to exactly define the force needed for certain digging conditions. The objective of impedance control is to establish a desired dynamical relationship between the end-ef- . fector bucket tip position and the contact force. This dynamical relationship is referred to as the . target impedance. Let x t be the desired trajectory r of the end-effector. Typically, the target impedance is chosen as a linear second-order system to mimic mass-spring-damper dynamics: Z ses M s 2 qB sqK e . . t P tttP sM e qB e qK e se ,12 . t P t P t PF where s is the derivative operator and the constant positive-definite n=n-matrices M , B and K are tt t respectively the matrices of inertia, damping and stiffness. The position error, e , and the force error, P e , are defined as F e sx yx, e sF yyF ,13 . . P r F r e . where F t sM x qB x qK x is the force rtrtrtr set-point. The control problem is to asymptotically drive the . system state to implement the target impedance 12 even in the presence of uncertainty. If the position error e approaches zero, the force error e also PF approaches zero and vice versa, according to a speci- fied dynamical relationship defined by the numerical . values of the matrices M , B and K in Eq. 12 . In tt t some contact tasks, the force set-point, F , will be r specified to be constant rather than time-varying. During free-space motion where there is no contact with the environment, F syF s0, so e tends to r e P . 2 zero since Z s sM s qB sqK is stable. The tttt choice of the matrices M , B and K will determine tt t the shape of the desired transient response of the system. When the end-effector contacts the environ- ment, the interaction is characterised by the target . impedance 12 , which results in a compromise be- tween the position error and the force error. If the end-effector position tracks the desired trajectory . xx then the contact force follows the force r . set-point yF F . e r 3.2. Controller deelopment Consider a manipulator dynamical model of the . . form 5 with uncertainty satisfying condition 7 . It wx is well known 10 that robustness is the most distin- guished feature of variable structure control with sliding mode. In this section, a robust sliding mode controller will be developed for the manipulator . . dynamics 5 . As Eq. 5 represents a 2n-dimen- ()Q.P. Ha et al.rAutomation in Construction 9 2000 421435 425 sional system with an n-dimensionalcontrol input, a sliding surface in the state space will be a manifold wx w of dimension 2nynsn 10 . Let us define ss s 1 . . .x T x , s x ,., s x , the sliding functions, as 2 n wx follows 11 : ssye yM y1 B e yM y1 K e dt H P ttP ttP qM y1 e dts xyx ,14 . H t F s where x sx qM y1 B e qM y1 K e dtyM y1 e dt . HH sr ttP ttP t F 15 . The existence of a sliding mode, ss0, requires that ssye yM y1 B e yM y1 K e qM y1 e s0. P ttP ttP t F 16 . It can be seen that once the system state is in the . . sliding mode associated with Eq. 14 , condition 16 . guarantees that the target impedance 12 is reached. . . Thus, in the sliding mode s x s0, is1,2, . . . ,n , i . the force error tends to zero. The magnitudes s x i . is1,2, . . . ,n represent then the deviations of the system state from the sliding surface. We assume further that: . A2: Each entry of the uncertainty Df x, x, x is bounded: D f x, x, x Fb , . ii ; x, x, x is1,2,.,n .17 . . . Let us now define the control input usuyQ sgn s ,18 . . where usH x qC x qG qF ,19 . sxsx e T Qs Q sgn s ,.,Q sgn s , . . 11 nn Q )b is1,2,.,n .20 . ii . Remark 2. The control law 18 consists of two . components. The component 19 , calculated with the nominal system dynamic model, is called the equialent control. The other component, with the . discontinuous gain given in Eq. 20 , is called the switching control. () Theorem 1. Consider the system of Eq. 5 associ- () ated with sliding functions 14 and the target () impedance 12 . If the assumptions A1 and A2 are () satisfied, and the control law 18 is employed, then () the impedance error 14 asymptotically conerges to zero 9 . Implementation implies that sufficiently large switching gains, Q , are available. Large values of i Q will, however, tend to excite chattering. To accel- i erate the reaching phase and to reduce chattering, the . control law 18 is added by a tuning component: usuyQ sgn s yKs,21 . . where Ksdiag Ks is1,2,.,n .22 . . . ii Employing the fuzzy tuning technique proposed in wx . Ref. 12 , the expressions for Ks)0 are chosen ii as: K sK 1yexp y s rd . iimax ii is1,2,.,n ,23 . . where K and d are some positive constants. i max i () Theorem 2. Consider the system of Eq. 5 associ- () ated with the sliding functions 14 and the target () impedance 12 . If the assumptions A1 and A2 are () satisfied, and the control law 21 is employed then () the impedance error 14 asymptotically conerges to zero 9 . Remark 3. The switching component is for ensuring robust stability only. It can be omitted in practice. Note that from the geometry of the excavator, there exists a trigonometric mapping between each joint angle u and the corresponding linear displace- i ment y of each hydraulic cylinder piston, is2,3,4 i wx . 13 . Using this relationship and Eq. 10 , the cylin- wx T der positions ys yyy and the ram forces F 234 of the hydraulic actuators can be determined. They are considered as the control references to the exca- vator hydraulic systems. The following section is devoted to the control of electrohydraulic systems in order to track these desired commands. ()Q.P. Ha et al.rAutomation in Construction 9 2000 421435426 4. Electrohydraulic systems control 4.1. Hydraulic modelling . The control 10 requires the ram force generated at each cylinder of the excavator arms follow a desired function of time when executing digging tasks in impedance control. Nonlinear effects occur- ring during the toolsoil interaction, and in the hydraulic system itself, complicate the control strat- egy requirements. It is known that gravitational and friction between the piston and cylinder should be compensated for to achieve high performance of heavy-duty hydraulic machines, such as excavators wx 13 . Furthermore, oil viscosity, oil flow through the hydraulic servovalve, and variable loading, will cause hydraulic control systems to suffer from highly non- linear time-variant dynamics, load sensitivity, and wx parameter uncertainty 14 . Thus, these factors have to be taken into account in servo hydraulic modelling and control. The hydraulic actuators incorporated in the blade, boom swing, boom, arm, and bucket attachments of the excavator are axial hydraulic cylinders. The flow of hydraulic oil to the cylinder is regulated by a direct drive servovalve with an elec- trically controlled closed loop that controls spool position. This system could be generally described by a six-order differential equation. For simplicity, the following linear expression can be used with little loss of accuracy for frequencies up to 200 Hz: x sKu,24 . vvv where x is the spool valve displacement and u is the valve input voltage. Thus, a nonlinear state model wx can be obtained as in Ref. 15 , based on the relation- ship between the valve displacement x and the