Nonlinear H_infinity control with PID structure for robot manipualtors

NONLINEAR H, CONTROL WITH PID STRUCTURE FOR ROBOT MANIPULATORS M.G. Ortega, F.R. Rubio and M. Romdn' * Depto. Ingenieria de Sistemas y Autombtica Escuela Superior de Ingenieros, Universidad de Sevilla Camino de 10s Descubrimientos, s/n - 41092 Sevilla (Spain) { ortega, rubio, manuelr} @cartuja.us. es Abstract: In this paper the nonlinear H, control for robot manipulators introduced in (Feng, W., Postlethwaite, I., 1994) is extended. An additional integral term is included to cope with persistent disturbances, such as constant weights at the endefecctor. The extended controller is interpreted like a computed torque control with and external PID, whose gain matrices vary with the position and velocity of the robot joints. A particular case of the cost variable weighting matrix is studied in which the resulting external nonlinear PID does not depend on the attenuation level y of the H, formulation. Finally, experimental results are presented for the RM-10 industrial robot. Keywords: Robot manipulators, nonlinear H, control, disturbance rejection. 1. INTRODUCTION In automatic control, despite considerable effort to minimize system modeling errors, uncertainties are usually present and sometimes are significant. Research on the motion control of rigid robot manipulators has known significant progress over the last few years. An interesting question is whether control laws possess desirable rejection properties even if perfect models are assumed to be available. In this paper, a vector of disturbance signals acting on the input channels (torques) of the robot is used to represent the combined effect of modeling errors and external disturbances. The control system ability to reject these disturbances and maintain small tracking error (without excessive control effort) is measured in a L2 gain sense. A control design --formulated into a nonlinear H, optimization problem -is proposed to achieve optimal disturbance rejection. Based on this formulation, a nonlinear H, suboptimal control law is derived which consists of a feedforward/feedback structure. The remainder of the paper is organized as follows: An approach upon the concepts of L2 gain and H, optimization in the context of nonlinear systems are introduced in Section 2. In Section 3 a suboptimal nonlinear controller is derived to maximize the robot manipulator ability to reject external disturbances acting on the input channel, assuming a perfect system model. In Section 4, the nonlinear H, controller is expressed in the form of a computed torque control with an external nonlinear PID controller. A nonlinear H, controller for the RM-10 robot manipulator is designed and experimental results are shown in Section 5. Finally, the major conclusions to be drawn are given in Section 6. 2. NONLINEAR H, CONTROL APPROACH The dynamical equation of a nth order smooth nonlinear system which is affected by an unknown disturbance can be expressed as follows 3 = f (5, t) + G (z, t) u + K (z, t) d (1) Copyright © 2002 IFAC www.elsevier.com/locate/ifac Copyright © 2002 IFAC 15th Triennial World Congress, Barcelona, Spain 407 where TL E SRP is the vector of control inputs, d E 8g is the vector of external disturbances and x E Xn is the vector of states. Performance can be defined using the cost variable z E ?J?(m+p) given by the expression . = w [ h!;)] where h(x) E SRm is the error vector to be controlled and W E SR(m+p) (m+p) is a weighting matrix. If we assume that the states x are available for measurement then the optimal H, problem can be posed as follows (van der Schaft, A., 1992): Find the smallest value y* 2 0 such that for any y 2 y* there exists a state feedback TL = u (x, t) such that the L2 gain from d to z is less than or equal to y, that is The integral expression on the left-hand side of expression (3) can be written as 11z11; = zTz = [ h'(x) u'] WTW [ and matrix WTW can be partitioned as follows where Qi Qi2 Qi3 Qi3 Q23 Q3 Q = [ Q12 Q2 Q23] C'= [ 21 The matrices Q and R are symmetric positive definite and such that Q - C'R"C'' > 0. An optimal control signal u* may be computed for system (1) if there exists a smooth solution V(x,t), with V(x0,t) 0 for t 2 0, to the following Hamilton-Jacobi equation: av aTv iaTv 1 - + -f(x,t) + -- --K(z,t)KT(z,t) at ax 2 ax [-p for each y > dq 2 0. In such case, the optimal state feedback control law -see (Feng, W., Postlethwaite, I., 1994)- is derived as U* = -R-I (CTh(x) + GT (x, t) ") (5) ax 3. NONLINEAR H, OPTIMIZATION IN MANIPULATOR MOTION CONTROL The following Euler-Lagrange equations of motion are used to describe the behavior of a n degree- of-freedom (DOF) robot manipulator where q is the vector of joint variables (joint positions) and q is its temporal derivative (joint speeds). It is supposed that these two vectors are available for measurements. The vector 7 (torques applied on the axis of the joints) is the signal input of the system and dT represents the total effect of system modelling errors and the external disturbances. The inertia matrix A4 (q) is symmetric and positive definite, V (q,q) is the vector of centripetal and Coriolis terms and G (q) is a vector which consists of the gravitational terms. Denoting by qT, qT and qT the desired position, speed and acceleration of the joints, respectively, the tracking error vector x and its derivative x are defined as follows: 2= [ :] and .=[!I (7) Sedt where = qqT, e =q-qT, Sedt = ) (q - qT) dt. =q-qT, 0 For system (6) a control law of the following structure is considered 7 = M(q)G+ v (q,4) + G(q) - (8) 1 1 -- (A4 (q) TX + C(q,Q)Tx) + -U P P and the matrices A? (q) and N (q,q) computed through the following expressions: must be Note that the N matrix is skew-symmetric. We will use this propriety in the Proofs. 408 It can be shown that It should be noted how vector u in control law (8) represents the additional control effort necessary for attenuating the disturbances. Matrix T in equation (8) can be partitioned as follows: T= [Ti T2 G] (11) with TI = PI, where p is a positive scalar and I is the nth-order identity matrix. On substituting the expression of the control law from (8) into the model equation of the robot and defining d = pd, yields MTx+ CTx = u + d (12) which is a 3nth order equation of the nonlinear dynamics of the error. Thereby, the control problem is to minimize the tracking error z in the presence of d without excessive control effort u. Equation (12) can be rewritten into the standard form of equation (1) as follows: f (2, t) = To-’ P Toz (13) where r-M-’C 0 0 1 and r M-’ 1 L:J G (z, t) = K (z, t) = Tl’ where I is the identity matrix, 0 the zero matrix, both of n-th order and If function h (z) in expression (2) is chosen equal to the error vector z, then the following expression constitutes a solution of the Hamilton-Jacobi equation (4): where the matrices Y, S, 2 and T = [TI T2 T3 ] can be obtained solving the equation - (CT + T)T R-’ (C’ + T) = 0 The proof of this result can be found in appendix A at the end of the paper. The algorithm to obtain matrix T is the following: (1) Compute TI and T3 solving the following Riccati algebraic equations: TF ($I - R-‘) T3 - C3R-’T3- -T,TR-’CT - C3~-’ C3 -T +Q3 =O 3 (2) Compute matrix S given by -TFR-’C; - el R-T; + Q~~ ) (3) Compute T2 solving the Riccati algebraic equation: TT (-$I - R-I) T2 - C2R-’T2- -T~R-’c~-e~R-’c~+Q2f2S=O Substituting for the value of V (2, t) into equation (5), the control law u* which corresponds to the H, optimal index y is u* = (CT + T) z 4. THE CONTROL LAW LIKE A NONLINEAR PID There exists in the literature several control methods in which robot controllers have been posed as PID control -see, for example, (Ramirez J.A., Cervantes I., Kelly R., 2000). In this section it is shown how the obtained control law may be interpreted like a computed torque control with an external nonlinear PID controller. Substituting for the expressions of T, x and u* in (8), and after some computations the optimal control law can be written as: P 409 1 P + CTx) - -R-' (C' + T) x Keeping in mind the definition of x, the control law can be rewritten as follows 1 P 7*=M&+V+G--(M[Tz T3 O]+ +CT + R-' (C' + T)) x or in a more compact expression: T* = M (q) & + V (q, 4) + G (4) - - (KD~ + Kpe + KIJedt) where 1 P KD = - ( MT~ + ( i~ + N) T~ + R- 1 (c~ + T, ) ) Kp = (MT. + (f iI+ N) T2 + R-I (CF + P 1 P '4, = - ((+ + N) T3 + R-1 ((7; +q) A particular case is obtained if components of the weighting matrix W'W satisfy Q1 = w:I, Q2 = wZI, Q3 = w~I, R = w~I, Qi2 = Qi3 = Q23 = 0, and C1 = C2 = C3 = 0. In this case the gain matrices take the form These expressions have an important property: they do not depend on the parameter y. Thereby we have algebraic expressions to compute the general optimal solution for this particular case. 5. EXPERIMENTAL RESULTS FOR THE RM-10 INDUSTRIAL ROBOT The RM-10, shown in Figure 1, is a six-degree-of- freedom revolute joint manipulator arm. All the six joints are driven by DC-brushless lowinertia electric motors which provide a uniform torque for all joint positions, and enables high control torque peaks. Torque is delivered to the joint axis through gear reductions, thus RM-10 is an indirect-drive manipulator. The joints also provide an electric brake to block the manipulator arm in any position -see (System Robot, 1991). Coupled to each motor axis there is a twopole resolver device which provides an accurate measurement of the correspondent joint position. qz (3 d I Fig. 1. The RM-10 Robot Manipulator These measures will allow, as usual, the closedloop control of the system. The RM-10 system employs a VME bus based architecture, providing independent control boards for every joint. Particularly, the real-time DS1103 control board, dSPACE trade, was employed (Implementation Guide, 1999), (MOO, 1998), (MOO, 1990). The control board was plugged into the expansion bus of a commercial PC, holding a 333MHz PowerPC as a main processor and an additional DSP as an input/output processor. Before accomplishing the design of a controller it is necessary to obtain a dynamic model of the robot manipulator. According to the Euler- Lagrange formulation (Craig, J.J., 1989), the dynamic model of a general n-link rigid-body robot is a second order nonlinear equation, as shown in Equation 6. In this case, the motion equation is complex and contains a number of hard-to-handle nonlinear terms (Perez, C., 1999). In order to simplify the controller design, friction terms in equation (6) have been neglected, and included as model uncertainty. This yields a very simplified model that only takes into account diagonal terms. A number of additional parameters are required to characterize the dynamic model of the robot manipulator, such as link masses and inertias. These parameters have been estimated by geometric measurements and dynamical experiments of the robot arm. In Table 1, the estimated masses of the different links of the robot are shown. These values may help to make an idea of the characteristics of the robot. link mass (Kg) 1 38.65 2 51.80 3 84.10 4 33.89 5 7.36 6 5.00 Table 1. Estimated masses of the links 410 A diagonal WTW weighting matrix has been considered to design the controller. Table 2 shows the values for the diagonal weighting submatrices used for the RM-10 control synthesis. Signal to minimize Weighting matrix Speed error C Position error e QI = ($)'I Q2 = I Integral error Jedt Additional control effort u Q3 = 321 R = 0.421 Table 2. Weights for the controller In the experiments presented in this paper, the position references (which are computed by a trajectory generator) are fifth degree polynomials between the initial position [ql q2 q3 44 45 q6] = [O.O 0.0 0.0 0.0 0.0 0.0 ] rad to final position equal to [0.6 - 0.3 - 0.3 0.6 0.6 0.61 rad with initial and final speeds and accelerations equal to zero. The transition time is 4.3 seconds. The position errors and speed errors are shown in Figure 2 and 3 respectively, while the control signals generated by the controller are presented in Figure 4. It can be seen how the performance is aceptable for all axes, with small errors in both angular positions and velocities. In the same way, it should be noted how the position error does not tend to zero despite of the integral action. This effect is due to a dead zone because of friction in the actuators, which is not compensated in this work. This fact agrees with the evolution of some control signals, whose magnitudes increase until their respective actuators leave their dead zones. -0otL Fig. 2. Position errors in the six axes of the RM-10 6. SUMMARY In this paper a nonlinear H, control for robot manipulators has been developed which copes with persistent disturbances due to the inclusion of an integral term. This controller can be interpreted like a computed torque control with an external nonlinear PID controller. A particular case has been obtained in which the nonlinear PID gains do Fig. 3. Speed errors in the six axes of the RM-10 Fig. 4. Control signals generated by the controller not depend on the value of the attenuation level y. Finally, experimental results have been presented by using an industrial robot which proves the good performance supplied by this controller. Acknowledgments: The authors would like to acknowledge CICYT and FEDER for funding this work under grants TAP98-0541, DPI2000-1218- COl and 1FD97-0783. Likewise, the authors appreciate the work by Rafael Romero Vicente in the realization of the real experiments. 7. REFERENCES Craig, 5.3. (1989). Introducciton to Robotics. Mechanics and Control. second ed.. Addison- Wesley Publishing Company. Feng, W., Postlethwaite, I. (1994). Robust Nonlinear H,/Adaptative Control of Robot Manipulation Motion. Proc. Instn. Mech. Engrs. Implementation Guide (1999). Real Time Inter- MOO (1990). T158-11 Controler User Manual. MOO (1998). DS1103 PPc Controler User Manual. Perez, C. (1999). Simulacidn y Control de un Robot Manipulador. Proyecto Fin de Carrera. 208, 221-230. face (RTI and RT-MP). dSPACE. 411 Escuela Superior de Ingenieros. Universidad de Sevilla. Ramirez J.A., Cervantes I., Kelly R. (2000). PID Regulation of Robot Manipulators: Stability and Performance. Systems & Control Letters System Robot (1991). RM-10 Manuale d'Uso. R.ob6tica Industriale. van der Schaft, A. (1992). Lz-Gain Analysis of Nonlinear Systems and Nonlinear State Feedback Control. IEEE Trans. Automat. Control 41(2), 73-83. 37(6), 770-784. X Appendix A. PROOF OF RESULTS IN SECTION 3 1 1 1 -Y S - -YT2 -5' - -Y (T3 - Tz) P P P Tox. S+Z- -(S+Z)- 1 P P 1 P -(s - y, 1 (S - Y)T* - (S - Y) (T3 - Tz) - We will show that the scalar function V (x, t) - equation (14)- is the solution for the Hamilton- Jacobi equation (4). The gradient of V(x,t) is given by 1 2 +- pixn w Oixn] with w E %Ixn and dTM 0 0 wi =x To 1 Tox, i = l...n. It is easy to validate that [Oixn w Oixn] (G(x,t)u+K(x,t)d) =O. Making some computations we can obtain this expression 1 2 +- [OIX, w O1xn] X. Using equation (13) the first term of the last expression can be written xTTT 0 Y S-Y Tof = x~TTx [::guy ZYY] X s+z- - (S + Z) 1 - (S - Y) TZ P P 1 P -(S-Y) 1 - - (S - Y) (T3 - T2) and for the second term we have 1 1 - [Oixn w Olxn] X = -x~TTx 2 2 The derivative of V (x, t) with respect to the time is Adding the previous expressions we arrive at r -N 0 0 1 Since matrix N is skew-symmetric and due to the particular structure of To we can write f = -xT Y 2s z+2s 2. at dX [" sz+2s 0 "1 dV dVT(x,t) 1 -+ = Tx, Also we can compute GT (x, t) ~ and since G (x, t) = K (x, t) we have dV (2, t) dX = -xTTT 1 ($I - R-I) Tx. 2 Using the computed expressions for the terms of the Hamilton-Jacobi equation given in (4) and the value of h (x) = x we have 0Y sz+2s 0 +ix~T~ ($1 - R-1) T~ - x~~~~-~~~x+ 2 1 +5xT (Q - CR-'CT) x = 0, TOG and by simplifying we obtain expression (15). 412