Tracking Control of a Class of Hamiltonian Mechanical Systems with Disturbances

Transcription

1 Proceeings of Australasian Conference on Robotics an Automation, -4 Dec 4, The University of Melbourne, Melbourne, Australia Tracking Control of a Class of Hamiltonian Mechanical Systems with Disturbances Alejanro Donaire, Tristan Perez, Nathan Bartlett School of Engineering, The University of Newcastle, Australia Alejanro.Donaire@newcastle.eu.au, Nathan.Bartlett@uon.eu.au School of Electrical Eng. an Comp. Sc., Queenslan University of Technology, Australia Tristan.Perez@qut.eu.au Abstract This paper presents a trajectory-tracking control strategy for a class of mechanical systems in Hamiltonian form. The class is characterise by a simplectic interconnection arising from the use of generalise coorinates an full actuation. The tracking error ynamic is moelle as a port-hamiltonian Systems (PHS). The control action is esigne to take the error ynamics into a esire close-loop PHS characterise by a constant mass matrix an a potential energy with a minimum at the origin. A transformation of the momentum an a feeback control is exploite to obtain a constant generalise mass matrix in close loop. The stability of the close-loop system is shown using the close-loop Hamiltonian as a Lyapunov function. The paper also consiers the aition of integral action to esign a robust controller that ensures tracking in spite of isturbances. As a case stuy, the propose control esign methoology is applie to a fully actuate robotic manipulator. Introuction Mechanical systems exhibit equilibrium points where the potential energy is stationary, an the stable equilibrium points are characterise by a minimum of the potential energy - this can be escribe using the Principle of Virtual Work Greenwoo,. When esigning a motion control law for a mechanical system it is natural to exploit energy concepts an target a close-loop energy with esirable characteristics such as having a minimum of the potential energy at the esire equilibrium point an the kinetic energy shape to achieve certain ynamic behaviours. This has lea to formulations of control problems an solutions in terms of energy-base moels such as Euler-Lagrange Ortega et al., 998 an Hamiltonian van er Schaft, 6. In this paper, we follow the latter. We aress the trajectory tracking control esign problem for a class of mechanical systems with particular structure that arises from the use of generalise co-orinates an full actuation. Many mechanical systems fall within this class of particular importance are fully actuate robotic manipulators. The problem of trajectory tracking for these robotic manipulators has been aresse in term of Euler-Lagrange moels by Ortega an Spong 989 an by Wen an Bayar 988. Our formulation in terms of Hamiltonian moels provies an alternative, which exploits the rich geometrical structure of the Hamiltonian formulation. In particular, we use a change of momentum co-orinates an a feeback control that allow to set a constant generalise mass matrix in the target close-loop system. This can be exploite for the tuning of the control law. We also consier the aition of integral action, for cases where constant isturbances must be rejecte. We explote the proceure propose in Donaire an Junco, 9; Ortega an Romero, ; Romero et al., for regulation problem, an we applie to the tracking problem. Finally, we illustrate the use of the propose control esign methoology on a fully-actuate robotic manipulator. Mechanical Systems an Port Hamiltonian Moels The ynamics of mechanical systems, such as a robotic manipulator, can be escribe using the Euler-Lagrange equation Lanczos, 986: t ql(q, q) q L(q, q) = τ, () where q an q are the n-imensional vectors of generalise coorinates an velocities respectively, an τ is the vector of generalise forces. The Lagrangian L(q, q) is the ifference between the kinetic co-energy an the potential energy of the system. For systems within the realm of classical mechanics, the

2 Proceeings of Australasian Conference on Robotics an Automation, -4 Dec 4, The University of Melbourne, Melbourne, Australia Lagrangian takes the form L(q, q) = T (q, q) V (q) = qt M(q) q V (q) () where the generalise mass matrix M(q) > is symmetric for all q. For these systems, the conjugate generalise momentum is p = q L(q, q) = M(q) q. Using the momentum an the generalise coorinate vector, the set of n secon-orer ifferential equations arising from () can be transforme, using the Legenre s transformation, into a set of n first-orer ifferential equations Lanczos, 986: q = p H(p, q), () ṗ = q H(p, q) + τ (4) where the Hamiltonian is the sum of the kinetic energy an the potential energy: H(p, q) = T (q, p) + V (q) = pt M (q)p + V (q). (5) This function represents the total energy of the system. The equations ()-(4) are calle the Hamilton s canonical equations of motion. In the control literature, the Hamiltonian moel ()- (4) has been generalise to what is known as a port- Hamiltonian system (PHS) (e.g. van er Schaft, 6): ẋ = J(x) R(x) H(x) + g(x) u, (6) y = g T (x) H(x), (7) where x is the state vector an the pair u, y are the input an output m-imensional vectors. These are conjugate variables; that is, their inner prouct represents the power exchange between the system an the environment. The function J(x) = J T (x) escribes the power preserving interconnection structure through which the components of the system exchange energy. The symmetric function R(x) captures issipative phenomena in the system. The function g(x) weighs the action of the input on the system an efines the conjugate output. From (6)-(7), it follows that Ḣ(x) = y T u H(x) T R(x) H(x) y T u, (8) which shows passivity of the PHS moel van er Schaft,. In this paper, we consier fully-actuate mechanical systems with ynamics represente as follows q = ṗ In I n H(q, p) + I n (τ + τ ), (9) where H(q, p) is given in (5), I n is the n n ientity matrix, τ is the vector of control forces, an τ is the vector of isturbances forces. The isturbances can be ecompose as τ = + (t), where is a constant vector an (t) is a time-varying vector. In this moel, the generalise force vector τ is mappe irectly into the momentum equation ue to the ientity matrix. If the components of τ are inepenent, the system is within the class of fully actuate systems since im τ = im p = n. For a efinition of the complete class see Bullo an Lewis 4. Note also that this moel uses generalise an not quasi-coorinates, which gives the simplectic form for the interconnection in the first term on the righthan sie of (9), namely, In J(x) =. I n Tracking Control of Fully-actuate Mechanical Systems We consier the ynamics of the system (9), an given a (possible) time-varying reference q (t) an its time erivatives q (t) an q (t), then the control problem is to esign a controller that ensures t q (t). Proposition. Consier the isturbance free (τ = ) port-hamiltonian system (9) in close loop with the control law τ = q H(q, p) M M (q) Ṽ ( q) + (J R )M p + + M(q) q + Ṁ(q) q K Ṽ ( q) M (q)k Ṽ ( q) + M (q) p, () where q = q q, () p = p M q + K Ṽ, () the n n constant matrices K an M, an the n n matrices J an R satisfy M K + K T M >, () M = M T >, (4) R = R T >, (5) J = J T, (6) an the function Ṽ ( q) has a (global) minimum at the origing, i.e. arg min Ṽ ( q) =. Then, i) PHS form. The ynamics of the close loop can be written as a PHS as follows q M = K M M p M M H J R ( q, p), (7) To simplify the notation, we rop the epenency of matrices an function on the inepenent variable, e.g. we note M an Ṽ instea of M(q) an Ṽ ( q).

3 Proceeings of Australasian Conference on Robotics an Automation, -4 Dec 4, The University of Melbourne, Melbourne, Australia with H ( q, p) = pt M p + Ṽ ( q) (8) ii) Stability. The tracking error q(t) asymptotically converges to zero, which ensures the control objective t q (t). Proof We first consier the tracking error q as in (), an we compute the time erivative as follows q = q q = M p q M K Ṽ + M p. (9) The last equality is satisfie for p as in (), which implies that the ynamics of q can be written as in the first line in (7). Secon, we compute the erivative of () p = ṗ M q Ṁ q K Ṽ q = q H + τ M q Ṁ q K Ṽ M K Ṽ + M p M M Ṽ + (J R )M p. () It can be verifie that the equality of the last two lines is satisfie by the control law (). Therefore, the ynamics of p can be written as in the secon line of (7). Then, the ynamics of the close loop in PHS form (7) follows irectly from (9) an (), which shows claim i). To show tracking of the time-varying reference q (t), we stuy the (global) asymptotic stability of the equilibrium ( q, p ) = (, ) of the close-loop ynamics (7). Since M is positive efinite an symmetric, an the function Ṽ has a (global) minimum at the origin, then the Hamiltonian H qualify as a Lyapunov caniate function. The erivative of H along the solutions of (7) is as follows Ḣ ( q, p) = ( q H ) T ( p H ) T q p = = ( q H ) T M K q H + ( q H ) T M M p H ( p H ) T M M q H + +( p H ) T (J R ) p H = ( qṽ )T (M K + K T M ) q Ṽ p T M T R M p < () where we use the properties ()-(6). Since the time erivative of the Lyapunov function is negative semiefinite, then the equilibrium ( q, p ) is (almost globally) asymptotically stable. This fact implies that q(t) zero as time goes to infinite, therefore lim t q(t) = q (t), which ensures the tracking of the reference. In the following proposition, we consier the robust tracking problem of a mechanical system (9) for a given a time-varying reference q (t) (an its time erivatives q (t) an q (t)). The control controller ensures t q (t), in spite of unknown constant isturbance τ. In case of boune isturbances (t), the controller propose ensures input-to-state-stability (ISS). This property has been use in control theory an control esign (see Sontag, 8 for a recent survey). Two nice robust property of ISS systems are that trajectories of the states are boune if the inputs are boune, an the states are convergent if the inputs are convergent. These two properties are easily attaine for linear systems, but it can be more ifficult to obtain for nonlinear systems. Proposition. Consier port-hamiltonian system (9) in close loop with the control law τ = q H(p, q) M M (q) Ṽ ( q) + (J R )M p M(q) q + K Ṽ ( q) M K z z (J R ) T where an K z z + M(q) q + Ṁ(q) q + M K z ż K Ṽ ( q) M (q)p + q, () ż = M M (q) + (J R )M K Ṽ ( q) + +(J R )M M(q) M (q)p q, () q = q q, (4) The n n constant matrices K an M, an the n n matrices J an R satisfy M (q)k + K T M (q) > ɛi n, (5) M = M T >, (6) K z = K T z >, (7) R = R T >, (8) R = R T >, (9) J = J T, () J = J T, () with ɛ R >, an the function Ṽ ( q) has a global minimum at the origin, i.e. arg min Ṽ ( q) =, an c q Ṽ ( q) c q, () c 4 q Ṽ ( q) c q, () Note that the control law ()-() requires no information about the value of the isturbance.

4 Proceeings of Australasian Conference on Robotics an Automation, -4 Dec 4, The University of Melbourne, Melbourne, Australia with c, c, c, c 4 R >. Then, i) PHS form. The ynamics of the close loop can be written as a PHS as follows q M K M M M M p = M M J R (J R ) T W + ż M M J R J R with + (t) (4) W ( q, p, z) = pt M p + Ṽ ( q) + (z α)t K z (z α), an α = Kz (J R J R ) (5) ii) Robust Exponential Stability. For the case of constant unknown isturbances an time-varying isturbance (t) =, the controller ()-() ensures that the tracking error q(t) exponentially converges to zero, therefore the control objetive is achieve, i.e. t q (t) ii) Input-to-State-Stability. For the case of constant unknown isturbances an time-varying isturbance (t), the controller ()-() ensures inputto-state-stability of the system with input (t). Proof We first consier the tracking error q as in (4), an we compute the time erivative as follows q = q q = M p q M K Ṽ + M p + M M K z (z α),(6) which is satisfie if p = p M q + K Ṽ M K z (z α), (7) therefor the ynamics of q can be written as in the first line in (4). Secon, we compute the erivative of (7) p = ṗ M q Ṁ q K Ṽ q M K z ż = q H + τ + + (t) M q Ṁ q K Ṽ q q M K z ż M M Ṽ + (J R )M p (J R ) T K z (z α) + (t) (8) It can be verifie that the equality of the last two lines is satisfie by the control law (). Therefore, the ynamics of p can be written as in the secon line of (4). Then, the ynamics of the close loop in PHS form (4) follows irectly from (6) an (8), which shows claim i). To show exponential tracking of the time-varying reference q (t) claime in ii), we stuy the global exponential stability of the equilibrium ( q, p, z ) = (,, α) of the close-loop ynamics (4). Since M is positive efinite an symmetric, an the function Ṽ has a global minimum at the origin an satisfies (), then the Hamiltonian W qualify as a Lyapunov caniate function, an it also satisfies β ( q, p, z α) W ( q, p, z) β ( q, p, z α) (9) with β, β R >. We compute the erivative of W along the solutions of (4) as follows Ẇ ( q, p, z) = ( q W ) T ( p W ) T ( z W ) q T p = ż = ( q W ) T M K q W + ( q W ) T M M p W + ( q W ) T M M z W ( p W ) T M M q W + ( p W ) T (J R ) p W ( p W ) T (J R ) T z W ( z W ) T M M q W + +( z W ) T (J R ) p W + ( z W ) T (J R ) z W + ( p W ) T (t) = ( qṽ )T (M K + K T M ) q Ṽ ( p W ) T R p W (z α) T K z R K z (z α) + ( p W ) T (t) = ɛ ( qṽ )T q Ṽ λ (z α) ( p W ) T R p W + ( p W ) T (t) ɛ q Ṽ λ (z α) λ pw + (t) λ ɛc q λ λ p λ (z α) + (t) λ γ β W ( q, p, z) + λ (t), (4) where λ = λ min (K z R K z ), λ = λ min (R ), λ = λ min (M ), an γ = min{ɛc, λ λ, λ }. The operator λ min (A) computes the minimum eigenvalue of the matriz A. To probe exponential stability of the close loop with constant isturbances, we set (t) = in (4). Then the inequality Ẇ ( q, p, z) γ β W ( q, p, z)

5 Proceeings of Australasian Conference on Robotics an Automation, -4 Dec 4, The University of Melbourne, Melbourne, Australia together with (9) ensure (global) exponential stability of the equilibrium ( q, p, z ) = (,, α) (see e.g. Khalil, ). This fact implies that q(t) exponential converge to zero as time goes to infinite, therefore lim t q(t) exponentially converge to q (t), in spite of the presence of unknown constant isturbances. The input-to-statestability property follows consiering (t) in (4) as follows Ẇ ( q, p, z) γ β W ( q, p, z) + λ (t), which show W is an ISS-Lyapunov function (Sontag, 8; Khalil, ). 4 Case of Stuy In this section, we present simulation results of a robotic manipulator to evaluate the performance of the control esign. We consier a two-link DoF serial-robotic manipulator shown in Figure. The generalise coorinate vector is q = (, q ). The mass matrix is M(q) = m lc + m l m l c l cos( q ) m l c l cos( q ) m lc (4) where the parameters l ci, m i are the offset to the centre of mass an the mass of the link i respectively, an l is the length of the link. The potential energy is V (q) = (m c + m l )g sin( ) + m c g sin(q ). (4) To ensure stability, we choose the esire potential energy on the position error as follows Ṽ ( q) = qt Q q, (4) The reference vector is q = (q, q), where q(t) an q(t) are two time-varying, smooth an sinusoial-type signals provie to the controller. The simulation starts with the two links in the horizontal position, with two constant isturbances acting on link at t=5s an on link at t=s. In this scenario, we test the control esigns from propositions. an., ientifie here as controller an controller. Figures, 6, 4 an 8 isplay the angle an angular velocity references together with the current angle an angular velocities of the two links for controller an controller. It can be seen from these plots that the control law tracks the reference throughout the entire simulation, with the isturbances affecting the angles an angular velocities of the manipulator for only a small interval of time. Although very small, there is a small tracking error in the controller ue to the isturbance as expecte (see figure ). The controller compensates the constant isturbance an the tracking error converge to zero (see figure 7). Figure 5 an 9 show the compute torques supplie by the controller an to the robotic manipulator, respectively. As can be seen, the control torque are smooth an within acceptable bouns. Finally, Figure shown the time history of the state of the controller, namely the integral action. Angle ra.5.5 q q where Q > is a iagonal constant matrix. Note that since V ( q) is a quaratic function, then it has a global minimum at the origin, Ṽ ( q) = Q q an Ṽ ( q) = Q. In this example, the matrices M, R, R an K z are constant iagonal matrices, K = ki n with k R >, an J = J = Figure : Link angles an its references (controller ). Figure : Two-link robotic manipulator 5 Conclusions We present a control esign for general mechanical systems that ensures tracking of time-varying references. The first controller shown in proposition. ensures asymptotic tracking for time-varying references. The secon controller, presente in proposition., is augmente with integral action, which ensures exponential convergency of the tracking error to zero in spite of con-

6 Proceeings of Australasian Conference on Robotics an Automation, -4 Dec 4, The University of Melbourne, Melbourne, Australia..8 Link Link.6 Angle s.4.. Torque Nm Figure : Angle errors q = q q (controller ). Figure 5: Control torques (controller ). Angular velocity ra/s.4... /t /t q /t q /t Angle ra q q Figure 6: Link angles an its references (controller ). Figure 4: Link velocities an its references (controller )..8.6 x Link stant isturbances. Also, the robustness of the controller with respect to isturbances is stuie in term of inputto-state-stability theory. Using the propose esign, the stability analysis of the close-loop system oes not require the use of Barbalat s lemma. Inee, since both close-loop systems (7) an (4) have amping in all the states, the Hamiltonians H an W are strict Lyapunov functions for the close loop ynamics. The performance of the controllers are illustrate in simulation with a robotic manipulator. The control esign propose in this paper requires shaping the form of both potential an kinetic energy functions. Our current research focus on control esigns that shape only the potential energy, whilst preserving the form of the open-loop mass matrix. Angle ra Figure 7: Angle errors q = q q (controller ).

7 Proceeings of Australasian Conference on Robotics an Automation, -4 Dec 4, The University of Melbourne, Melbourne, Australia Anglular velovity ra/s.4... /t /t q /t q /t x Link Figure 8: Link velocities an its references (controller ). Torque Nm Link 5 5 References Figure 9: Control torques (controller ). Bullo an Lewis, 4 F. Bullo an A. Lewis. Geometric control of mechanical systems, volume 49. Springer Verlag, New York-Heielberg-Berlin, 4. Donaire an Junco, 9 A. Donaire an S. Junco. On the aition of integral action to port-controlle Hamiltonian systems. Automatica, 45(8):9 96, 9. Greenwoo, D.T. Greenwoo. Avance Dynamics. Cambrige University Press,. Khalil, H. Khalil. Nonlinear Systems. Prentice- Hall, New Jersey,. Lanczos, 986 C. Lanczos. The variational principles of mechanics. Number 4. Dover Publications, 986. Ortega an Romero, R. Ortega an J.G. Romero. Robust integral control of port-hamiltonian Figure : State of the integral action control (controller ). systems: The case of non-passive outputs with unmatche isturbances. Systems & Control Letter, 6(): 7,. Ortega an Spong, 989 R. Ortega an M. Spong. Aaptive motion control of rigi robots: a tutorial. Automatica, 5(6): , 989. Ortega et al., 998 R. Ortega, A. Loria, P. J. Nicklasson, an H. Sira-Ramirez. Passivity-base Control of Euler Lagrange Systems. Springer, 998. Romero et al., J.G. Romero, A. Donaire, an R. Ortega. Robust energy shaping control of mechanical systems. System & Control Letters, 6(9):77 78,. Sontag, 8 E. Sontag. Input to state stability: Basic concepts an results. In Nonlinear an Optimal Control Theory, P. Nistri an G. Stefani eitors, Lecture Notes in Mathematics, chapter, pages 6. Springer Verlag, Berlin, 8. van er Schaft, A. van er Schaft. L-Gain an Passivity Techniques in Nonlinear Control. Communications an Control Engineering. Springer Verlag, Lonon,. van er Schaft, 6 A. van er Schaft. Port- Hamiltonian systems: an introuctory survey. In Proceeing of the International Congress of Mathematicians, volume, pages 9 65, 6. Wen an Bayar, 988 J. T. Wen an S. D. Bayar. A new class of control laws for robotic manipulators - part. International Journal of Control, 47(5):88, 988.