ScienceDirect ScienceDirect

5th IFAC Workshop on Lagrangian and Hamiltonian Methods 5th IFAC Workshop on Lagrangian and Hamiltonian Methods 5th IFACLinear Workshop on Lagrangian and Hamiltonian Methods for Non Control 5th IFACLinear Workshop on Lagrangian and Hamiltonian Methods for Non Control Available online at www.sciencedirect.com July 4-7, 2015. Lyon, France for Non Linear Control for Non Control July 4-7,Linear 2015. Lyon, France July July 4-7, 4-7, 2015. 2015. Lyon, Lyon, France France

ScienceDirect

IFAC-PapersOnLine 48-13 (2015) 129–134

Passivity-Based Tracking Controllers Passivity-Based Tracking Controllers Passivity-Based Tracking Controllers Passivity-Based Trackingwith Controllers Mechanical Systems Active Mechanical Systems with Active Mechanical Systems with Active Mechanical Systems with Active Disturbance Rejection Disturbance Rejection Disturbance Rejection Disturbance Rejection

for for for for

∗ ∗∗ Jose ∗ Alejandro Donaire ∗∗ Jose Guadalupe Guadalupe Romero Romero∗∗∗ Alejandro Donaire ∗ ∗∗ ∗∗∗∗ ∗ Alejandro ∗∗ Jose Romero Donaire David Navarro-Alarcon Victor Ramirez ∗∗∗∗ Jose Guadalupe Guadalupe Romero∗∗∗ Donaire David Navarro-Alarcon Victor Ramirez ∗∗∗Alejandro ∗∗∗∗ David Navarro-Alarcon Victor Ramirez David Navarro-Alarcon ∗∗∗ Victor Ramirez ∗∗∗∗ ∗ ∗ Laboratoire d’Informatique, de Robotique et de Microlectronique de ∗ Laboratoire d’Informatique, de Robotique et de Microlectronique de ∗ Laboratoire d’Informatique, de et de Montpellier, Montpellier, France. Laboratoire d’Informatique, de Robotique Robotique de Microlectronique Microlectronique de de Montpellier, Montpellier, France.et(e-mail:Jose. (e-mail:Jose. Montpellier, Montpellier, France. (e-mail:Jose. [email protected]) Montpellier, Montpellier, France. (e-mail:Jose. [email protected]) ∗∗ [email protected]) The University of Newcastle, Newcastle, ∗∗ School of Engineering, [email protected]) The University of Newcastle, Newcastle, ∗∗ School of Engineering, School of Engineering, The University of Newcastle, Newcastle, ∗∗ NSW, Australia (e-mail: [email protected]). School of Engineering, The University of Newcastle, Newcastle, NSW, Australia (e-mail: [email protected]). ∗∗∗ NSW, Australia (e-mail: [email protected]). Department of Mechanical and Automation Engineering, ∗∗∗ NSW, Australia (e-mail: [email protected]). Department of Mechanical and Automation Engineering, The The ∗∗∗ ∗∗∗ Department of Mechanical and Automation Engineering, The Chinese University Hong N.T. Kong Department of of Mechanical andShatin Automation Engineering, The Chinese University of Hong Kong, Kong, Shatin N.T. Hong Hong Kong (e-mail: (e-mail: Chinese University of Hong Kong, Shatin N.T. Hong Kong (e-mail: [email protected] ) Chinese University of Hong Kong, Shatin N.T. Hong Kong (e-mail: [email protected] ) ∗∗∗∗ [email protected] )) ıa o ∗∗∗∗ Unidad de Energ´ [email protected] Unidad de Energ´ ıa Renovable, Renovable, Centro Centro de de Investigaci´ Investigaci´ on n Cientifica Cientifica ∗∗∗∗ ∗∗∗∗ Unidad de Energ´ ıa Renovable, Centro de Investigaci´ o n de Yucat´ a n A.C, Yucat´ a n, Mexico (e-mail: [email protected] )) Unidad de Energ´ ıa Renovable, Centro de Investigaci´ o n Cientifica Cientifica de Yucat´ an A.C, Yucat´ an, Mexico (e-mail: [email protected] de Yucat´ a n A.C, Yucat´ a n, Mexico (e-mail: [email protected] de Yucat´ an A.C, Yucat´ an, Mexico (e-mail: [email protected] )) Abstract: Abstract: The The main main purpose purpose of of this this paper paper is is to to investigate investigate the the robustness robustness of of tracking tracking controllers controllers Abstract: The main purpose of this paper is to investigate the robustness of tracking controllers for mechanical systems vis–` a –vis external disturbances. The controllers designed are obtained by Abstract: Thesystems main purpose of this paperdisturbances. is to investigate robustness of tracking controllers for mechanical mechanical vis–` a–vis –vis external Thethe controllers designed are obtained obtained by for systems vis–` a external disturbances. The controllers designed are by modifying a change of coordinates recently proposed for robust energy shaping controller. This for mechanical systems vis–` a –vis external disturbances. The controllers designed are obtained by modifying a change of coordinates recently proposed for robust energy shaping controller. This modifying a of recently proposed for energy shaping controller. This type of allows to damping in states when the loop modifying a change change of coordinates coordinates recently proposed for robust robust energy shaping type of of change change of coordinates coordinates allows to assign assign damping in all all the the states when controller. the closed closed This loop type of change of coordinates allows to assign damping in all the states when the closed loop is written in port–Hamiltonian form. This feature simplify the stability analysis by ensure type of change of coordinates allows to assign damping in all the states when the closed loop is written in port–Hamiltonian form. This feature simplify the stability analysis by ensure that that is written in form. This simplify the stability analysis by that the Hamiltonian function is strict function of the loop. Moreover, robustness is in port–Hamiltonian port–Hamiltonian form.Lyapunov This feature feature simplify theclosed stability analysis by ensure ensure that thewritten Hamiltonian function is is aa a strict strict Lyapunov function of the the closed loop. Moreover, robustness the Hamiltonian function Lyapunov function of closed loop. Moreover, robustness of closed against disturbances is This is for the Hamiltonian function a strict Lyapunov function of the closed loop. Moreover, robustness of the the closed loop loop againstis external external disturbances is ensured. ensured. This result result is also also extended extended for of the closed loop against external disturbances is ensured. This result is also extended for mechanical systems interacting with elastic environments and linear deformation. The robust of the closed loop against external disturbances is ensured. This result is also extended for mechanical systems interacting with elastic environments and linear deformation. The robust mechanical systems interacting with elastic environments and linear deformation. The properties preserved adding terms in control the new of mechanical systems interacting and deformation. The robust robust properties are are preserved adding with termselastic in the the environments control law law via via thelinear new change change of coordinates. coordinates. properties are preserved adding terms in the control law via the new change of coordinates. properties are preserved adding terms in the control law via the new change of coordinates. © 2015, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Keywords: Keywords: Passivity-based Passivity-based control; control; Trajectory Trajectory tracking; tracking; Mechanical Mechanical systems; systems; Robust Robust Keywords: Passivity-based control; Trajectory tracking; Mechanical systems; Robust stabilization Keywords: stabilizationPassivity-based control; Trajectory tracking; Mechanical systems; Robust stabilization stabilization 1. On 1. INTRODUCTION INTRODUCTION On the the other other hand, hand, the the force force control control of of position/velocity position/velocity 1. INTRODUCTION On the other hand, the force control of problem of compliant mechanical systems has 1. INTRODUCTION On the other hand, the force control of position/velocity position/velocity Passivity-based control (PBC) is a well-known technique problem of compliant mechanical systems has been been studied studied Passivity-based control control (PBC) (PBC) is is aa well-known well-known technique technique problem of compliant mechanical systems has been studied for a long time. The reader is referred to (Roy and problem of compliant mechanical systems has been studied Passivity-based used for stabilization of nonlinear systems via shaping for a long time. The reader is referred to (Roy and Passivity-based control (PBC) is a well-known technique used for stabilization of nonlinear systems via shaping for aa long time. The reader is referred to (Roy and Whitcomb , 2002), (Yao and Tomizuka, 1998), (Chianfor long time. The reader is referred to (Roy and used for stabilization of nonlinear systems via shaping the energy function of the system (Ortega et al., 1998). Whitcomb , 2002), (Yao and Tomizuka, 1998), (Chianused for stabilization of nonlinear systems via shaping the energy function of the system (Ortega et al., 1998). Whitcomb , 2002), (Yao Tomizuka, 1998), (ChianSong for review of Whitcomb 2002), (Baspiner, (Yao and and 2012) Tomizuka, the energy function of the system (Ortega et Recently, problem designing Song et et al., al.,, 2004), 2004), (Baspiner, 2012) for aaa 1998), review(Chianof this this the energythe function system robust (Ortegapassivity-based et al., al., 1998). 1998). issue. Song et al., 2004), (Baspiner, 2012) for review of this Recently, the problemofof ofthe designing robust passivity-based However, the most of this solutions so far have been Song et al., 2004), (Baspiner, 2012) for a review of this Recently, the problem of designing robust passivity-based controller in face of external disturbances has been adissue. However, the most of this solutions so far have been Recently, problem of designing robust passivity-based controller the in face of external external disturbances has been been adad- issue. However, the most this so have proposed for external disturbances issue. However, the contact most of of without this solutions solutions so far far have been been controller in disturbances has dressed Junco., 2009; and Romero, proposed for rigid rigid contact without external disturbances controller in face face of ofand external ad- proposed dressed in in (Donaire (Donaire and Junco.,disturbances 2009; Ortega Ortegahas and been Romero, for rigid contact without external disturbances and only considering Euler-Lagrange dynamics. Indeed, proposed for rigid contact without external disturbances dressed in (Donaire and Junco., 2009; Ortega and Romero, 2012). approach robust PBC for asympand only considering Euler-Lagrange dynamics. Indeed, dressed in (Donairean and Junco., for 2009; Ortega and 2012). Afterwards, Afterwards, an approach for robust PBC forRomero, asymp- and only considering Euler-Lagrange dynamics. Indeed, these force controllers cannot the of and Indeed, 2012). Afterwards, an approach for robust PBC for asymptotic stailization of port-Hamiltonian (PH) mechanical theseonly forceconsidering controllersEuler-Lagrange cannot exploit exploit dynamics. the properties properties of 2012). Afterwards, an approach for robust PBC for asympthese force controllers cannot exploit the properties of totic stailization of port-Hamiltonian (PH) mechanical PH systems, such as the direct construction of Lyapunov these force controllers cannot exploit the properties of totic stailization of port-Hamiltonian (PH) mechanical systems have been proposed in (Romero et al., 2013a). PH systems, such as the direct construction of Lyapunov totic stailization port-Hamiltonian (PH) mechanical systems have been beenof proposed proposed in (Romero et al., 2013a). PH systems, such as construction of functions, or the of dissipative that PH systems, as the the direct direct of Lyapunov Lyapunov systems have in et 2013a). This was extended to in functions, or such the modulation modulation of construction dissipative elements elements that systems have proposed in (Romero (Romerostability et al., al., (ES) 2013a). This work work wasbeen extended to exponential exponential stability (ES) in functions, or the modulation of dissipative elements that only appear with PH dynamics (Van der Schaft , 1999). functions, or the modulation of dissipative elements that This work was extended to exponential stability (ES) in (Romero et al., 2013b). However, the robustness proponly appear with PH dynamics (Van der Schaft , 1999). This work was extended to exponential stability (ES) in (Romero et al., 2013b). However, the robustness proponly PH (Van der Schaft ,, 1999). only appear appearbywith with PH dynamics dynamics (Van der Schaft 1999). (Romero et However, the robustness property tracking controller PH the above discussion, in this note we present (Romero et al., al., 2013b). 2013b). prop- Motivated erty of of trajectory trajectory trackingHowever, controllerthefor forrobustness PH mechanical mechanical Motivated by the above discussion, in this note we present erty of trajectory tracking controller for PH mechanical systems with disturbances has not been totally addressed. Motivated by the above discussion, in this note we present a new passivity based tracking controller (PBTC) methoderty of trajectory trackinghas controller for PH mechanical Motivated by the above discussion, in this note we present systems with disturbances not been totally addressed. a new passivity based tracking controller (PBTC) methodsystems with disturbances has not been totally addressed. aology new based tracking controller (PBTC) methodthat exponential to systems with disturbances has not been totally addressed. aology new passivity passivity basedglobal tracking controllerstability (PBTC)(GES) methodIn that enforces enforces global exponential stability (GES) to ology that enforces global exponential stability (GES) to In (Fujimoto (Fujimoto et et al., al., 2003) 2003) aa tracking tracking control control for for PH PH PH mechanical system under constant external disturology that enforces global exponential stability (GES) to In (Fujimoto et al., 2003) a tracking control for PH systems has been presented using PBC and canonical PH mechanical system under constant external disturIn (Fujimoto et al., 2003) ausing tracking for PH PH systems has been presented PBCcontrol and canonical mechanical system under constant external disturbances. To robustness presence of PH mechanical system underin distursystems has presented using PBC transformation. Based in on bances. To provide provide robustness inconstant presenceexternal of time-varying time-varying systems has been been presented using transformation PBC and and canonical canonical transformation. Based in coordinate coordinate transformation on the the bances. To provide robustness in presence of time-varying (bounded) disturbance, we adopt the well-known formalism bances. To provide robustness in presence of time-varying transformation. Based in coordinate transformation on the momenta, a curve-tracking controller for a fully actuated (bounded) disturbance, we adopt the well-known formalism transformation. Based in coordinate transformation on the (bounded) momenta, a curve-tracking controller for a fully actuated disturbance, we adopt well-known formalism of stability ,, 2007). This ap(bounded) disturbance, we (Sontag adopt the the well-known formalism momenta, aa system curve-tracking controller for aa(Duindam fully mechanical has been designed and of input-to-state input-to-state stability (Sontag 2007). This design design apmomenta, curve-tracking controller forin fully actuated actuated of input-to-state stability (Sontag ,, 2007). This design apmechanical system has been designed in (Duindam and proach is adapted for perturbed mechanical system interof input-to-state stability (Sontag 2007). This design apmechanical system has been designed in (Duindam and Stramigioli, 2004). Making use of energy shaping, damping proach is adapted for perturbed mechanical system intermechanical has been (Duindam and proach Stramigioli, system 2004). Making usedesigned of energy in shaping, damping is for perturbed system interacting elastic environments. An term in proach is adapted adapted perturbed mechanical mechanical system Stramigioli, 2004). Making use shaping, assignment dynamic aa trajectory tracking acting with with elastic for environments. An additional additional terminterin aa Stramigioli, 2004). Makingextension, use of of energy energy shaping, damping damping assignment and and dynamic extension, trajectory tracking acting with elastic environments. An additional term in change of coordinates is needed to ensure GES for constant acting elastic environments. An additional in aa assignment and dynamic extension, aa trajectory tracking control (Donaire et to vehicles changewith of coordinates is needed to ensure GES forterm constant assignment andin trajectory control is is given given indynamic (Donaireextension, et al., al., 2011) 2011) to marine marine tracking vehicles change of coordinates is needed to ensure GES for constant disturbances and ISS for time-varying disturbances. change of coordinates to ensure GES for constant control is in (Donaire et 2011) marine vehicles manoeuvring at low speed modelled in PH disturbances and ISS is forneeded time-varying disturbances. control is given given (Donaire et al., al., 2011) to to vehicles disturbances manoeuvring atin low speed and and modelled in marine PH framework. framework. and ISS for time-varying disturbances. disturbances and ISS for time-varying disturbances. manoeuvring at low speed and modelled in PH framework. In these works, the controller guarantees global asymptotic This novel approach enlarges the applicability manoeuvring lowcontroller speed andguarantees modelled global in PH framework. In these works,atthe asymptotic This novel approach enlarges the applicability of of the the In these works, the controller guarantees global asymptotic stability for the case of undisturbed system, and input-toThis novel approach enlarges the applicability of the coordinate transformation developed in (Romero et al., In these works, the controller guarantees global asymptotic novel transformation approach enlarges the applicability of the stability for the case of undisturbed system, and input-to- This coordinate developed in (Romero et al., stability for the case of undisturbed system, and input-tostate stability (ISS) when external forces are considered. coordinate transformation developed in (Romero et al., stability for the case of undisturbed system, and input-tocoordinate transformation developed in (Romero et al., state stability (ISS) when external forces are considered. state state stability stability (ISS) (ISS) when when external external forces forces are are considered. considered. 129 Hosting by Elsevier Ltd. All rights reserved. 2405-8963 ©© 2015, IFAC (International Federation of Automatic Control) Copyright 2015 IFAC 129 Copyright © 2015 IFAC 129 Peer review under responsibility of International Federation of Automatic Copyright © 2015 IFAC 129Control. Copyright © 2015 IFAC 10.1016/j.ifacol.2015.10.226

IFAC LHMNC 2015 130 July 4-7, 2015. Lyon, France

Jose Guadalupe Romero et al. / IFAC-PapersOnLine 48-13 (2015) 129–134

2013a) to robust tracking controllers. The proposed control scheme consist of an extended closed-loop system that preserves the port Hamiltonian form. The state vector is extended by the addition of integral action, which is reflected on the extra entries in the interconnection matrix. These entries result from the system’s gyroscopic terms. Exponential stability and disturbance rejection are guaranteed by the fact that the resulting system is constructed with dissipative elements in every coordinate. Thus, our method does not rely on high gains injection or complicated bounded functions to establish stability. The paper is organized as follows. In Section II, we formulate the problem. In Section III, we present the main results, namely tracking controller for mechanical systems in free motion and in contact with elastic environment. Both controllers are derived following the same procedure, but using joint and cartesian coordinates respectively. Hence, the decision to split the material in this form is to enhance readability. In Section IV, we present a numerical example for unconstrained motion. Finally, we give final conclusions and future research in section V. Notation. For x ∈ Rn , S ∈ Rn×n , S = S > 0 we denote the Euclidean norm |x|2 := x x, and the weighted-norm x2S := x Sx. Given a function f : Rn → R we define the operators 2 ∂ f ∂f ∂f 2 , ∇ f := , ∇xi f := , ∇f := ∂x ∂xi ∂x2 2. PROBLEM FORMULATION

m1 In ≤ M −1 (q) ≤ m2 In , where In is the n × n identity matrix, and mi > 0 ∈ R. 2.2 Problem statement The control objective is to design an energy shaping tracking (smooth) controller such that, the closed-loop system ensures stability in spite of the presence of the external disturbances d(t). In particular, we are interested in the following: • Exponential stability (almost global) with respect to constant matched disturbances. • Input-to-State Stability with time varying disturbances. To satisfy these objectives, besides the inclusion of a suitable integral action, we exploit the nonlinear gyroscopic properties of the system as in (Romero et al., 2013b). 3. MAIN RESULT The design of the PBTC proceeds in two steps. First, a change of generalizad momenta coordinates similar to (Venkatraman et al., 2010) is used to assign a constant inertia matrix in the energy function. Second, the error tracking proposed in (Romero et al., 2015) and the change of coordinates given in (Romero et al., 2013a) to add integral actions to mechanical systems is combined with a PBTC to assign a PH closed-loop with a desired energy function.

2.1 Port-Hamiltonian modelling

3.1 A suitable PH representation

Consider a perturbed fully-actuated mechanical system with n-degree of freedom (DOF). The dynamic is described in the following PH form (Van der Schaft , 1999) 0 0 q˙ 0 In , (1) u+ ∇H(q, p) + = −In 0 In p˙ A (q)d with Hamiltonian function 1 H(q, p) = p M −1 (q)p + P (ς(q)), (2) 2 where q, p ∈ Rn respectively represent the generalized positions and momenta (assumed to be measurable), u ∈ Rn denotes the control input, P (ς(q)) ∈ R represents the potential/elastic energy function of the system, ς : Rn → Rm represents a smooth vector functional (which will be defined in later sections), d ∈ Rn is a unknown external disturbances and possibly time varying, due to measurement or system noise, the input matrix A(q) ∈ Rn×n is assumed to be exactly known (full rank) and define the following two scenarios:

As shown in (Venkatraman et al., 2010) and (NavarroAlarcon et al., 2014), the change of coordinates (q, p) → (x(q), T (q(x))A(q(x))− p), with A(q) ∈ Rn×n a full rank matrix and T (q) ∈ Rn → Rn×n satisfying

(i) For unconstrained motion, we have that A(q) = In , such that, the vector d directly perturbs all joints (see (Romero et al., 2013a)). (ii) If the end-effector of the mechanism physically interacts with a passive environment, the matrix A(q) represents the Jacobian matrix that maps joint velocities to end-effector velocities. The standard known inertia matrix is denoted by M (q) = M (q) > 0 ∈ Rn×n , and satisfies 130

A(q)M −1 (q)A (q) := T (q) T (q), M (q)

−1

= A(q)

−1

(3)

T (q) T (q)A(q)

−

,

= L(q) L(q), with L(q) = T A

−

. Then, the change of coordinates

(q, p) → (x(q), T (q(x))A(q(x))− p).

(4)

for x ∈ Rn as a known function of q, transforms (1) into x˙ 0 0 0 T (q) = v+ , (5) ∇W + p˙ T (q)d In −T (q) J2 (q, p)

with v := L(q)u the new control signal, the new Hamiltonian function 1 W (x, p) = |p|2 + P (x), (6) 2 and the gyroscopic forces matrix 1 J2 (q, p) =

n i=1

1

∇qi (L(q))L−1 p (L(q)ei ) −

−L(q)ei ∇qi (L(q))L(q)−1 p

Clearly, J2 (q, p¯) = −J2 (q, p¯).

(7)

IFAC LHMNC 2015 July 4-7, 2015. Lyon, France


z˙1 = q˜˙ := T p˜

3.2 Mechanical systems in unconstrained motion In this scenario, the control of the mechanical system is given in the original joint position coordinates, such that x(q) = q and P (x) = Vg (q) is the gravitational potential. Hence, the matrix A(q) = In directly map disturbances into joint space. Proposition 1. Consider the system (5) with A(q) = In and x = q , in closed-loop with the control law 2 v = −T [In + Rz−2 ]M −1 q˜ + T Rz−1 [J2 − R2 − R3 ]z3 − T Rz−1 [R2 − J2 ]Rz T −1 p˜ + M R1 q˜ + p˙ d − T Rz−1 R3 Rz [T −1 p˜ + M R1 q˜] − T

131

d {T −1 }˜ p dt

d {M }R1 q˜ − T − R1 T p˜ − J2 p + T ∇q Vg dt z˙3 = Rz−1 M −1 q˜ + R3 Rz [T −1 p˜ + M R1 q˜] (8) −T

with pd = T − q˙d and R1 , R2 , R3 , Rz ∈ Rn×n positive definite gain matrices. (i) The closed-loop dynamics expressed in the coordinates (9) z1 = q˜, z2 = Rz S −1 {T p˜ + R1 q˜} + z3 , where q˜ = q − qd (t), p˜ = p − pd (t), with qd (t) and pd (t) as the desired positions and momenta respectively, takes the following PH form 0 z˙ = F (z)∇U + Rz d(t) , (10) 0 with an interconnection–dissipation matrix   −R1 SRz−1 −SRz−1 F =  −Rz−1 S −R2 + J2 −R3  , (11) −1 R3 −R3 Rz S with S(q) ∈ Rn×n full rank matrix (to be defined later) and energy function 1 1 1 (12) U (z) := |z1 |2 + |z2 |2 + |z3 |2 . 2 2 2 (ii) The error of the dynamic system (10) is ISS with respect to respect to time varying disturbances. (iii) Considering that the external force d is constant and with change of coordinates (13) z˜1 = z1 , z˜2 = (z2 − d), z˜3 = z3 , the PH system take the form z˜˙ = F ∇Ud (14) with energy function 1 1 1 z1 |2 + |˜ z2 |2 + |˜ z3 − d|2 . z ) = |˜ (15) Ud (˜ 2 2 2 Then, the zero equilibrium point of (14) is UGES z ). Consequently, with Lyapunov function Ud (˜ (˜ q (t), p˜(t)) → 0 and z3 → d exponentially fast. Proof 1. (i) To obtain the PH closed-loop system (10), first, we take the time derivative of the first row of (9) and replacing T p˜ via the definition of z2 (second row of (9)), we get the z1 dynamic of (10) . 2

The arguments of the matrices S(q), M (q) and T (q) have been omitted because of space limitations.

131

= SRz−1 (z2 − z3 ) − R1 ∇z1 U

= −R1 ∇z1 U + SRz−1 ∇z2 U − SRz−1 ∇z3 U.

Second, taking the time derivative of the second row of (9) and using p˙ and control law (8) expressed in the z coordinates with S = M −1 = T (q) T (q), yields the second row of the closed loop (10). d z˙2 = Rz {S −1 T }˜ p + S −1 T p˜˙ + z˙3 + dt d −1 {S }R1 ∇z1 U + S −1 R1 ∇2z1 U T p˜ +Rz dt = −Rz−1 Sz1 + (J2 − R2 )z2 − R3 z3 + Rz d(t). Finally with T p˜ = SRz−1 (z2 − z3 ) − R1 ∇z1 U given by the definition of z2 , we have that the z3 dynamic of the controller (8) is rewritten as z˙3 = Rz−1 S∇z1 Vd + R3 Rz S −1 [T p˜ + R1 ∇x˜ Vd ] = Rz−1 Sz1 + R3 z2 − R3 z3

and that corresponds to the third row of (10). (ii) Taking (12) as candidate ISS-Lyapunov function and computing its time derivative along (10) with Rz = In , yields U˙ ≤ −z1 2R1 − z2 2R2 − z3 2R3 + |z2 ||d(t)|, 1 1 ≤ −z1 2R1 − z2 2R2 − z3 2R3 + |z2 |2 + |d(t)|2 2 2 1 ≤ −λU (z) + |d(t)|2 , (16) 2 with λ = 2 min{λmin (R1 ), λmin (R2 ; 12 ), λmin (R3 )}. Then, 1 d (17) U˙ ≤ −(1 − θ)λU (z) ∀z ≥ √ 2θλ with 0 < θ < 1. From (17) and the fact that U (z) is a ISS-Lyapunov function, we have that the closed loop system (10) is ISS. (iii) First, taking the time derivative of (13) and choosing R3 = J2 +R3a , Rz = (R2 +R3a ), with free gain matrix R3a = R3a > 0 and energy function Ud (˜ z ) given by (15), we get the closed–loop system given by (14). On the other hand, with (15) as Lyapunov function and computing its time derivative along (14) yields z1 2R1 − ˜ z2 2R2 − ˜ z3 − d2R3a U˙ d ≤ −˜ ≤ −δ0 Ud (z)

(18)

with a bounding scalar satisfying δ0 := 2 min{λmin (R1 ), λmin (R2 ), λmin (R3a )}. Remark 1. Note that the inclusion of gyroscopic terms in the interconnection matrix do not affect the stability proof. This point is clear from the (3, 3) block, which shows that the skew-symmetric matrix J2 (q, p) has no impact on the time derivative of the Lyapunov function. 3.3 Mechanical systems interacting with elastic environments In this section we derive a robust tracking controller, such that, following proposition 1, GES and ISS properties can be ensured for both force and position trajectories.



For this scenario, the displacements coordinate x = x(q) ∈ Rn represents the end-effector position of the mechanism, respectively. It is assumed that the end-effector is continuously interacting with a lossless environment, which imposes m elastically constrained directions (where m ≤ n). Physically, m = n represents a manipulator that can apply force to the environment in any direction, while m = 1 represents point interaction with one deformable surface. The displacements coordinate can be decomposed as (19) x(q) = [r(q) , s(q) ] , where r ∈ Rm and s ∈ Rn−m are the constrained and unconstrained independent directions (Raibert and Craig, 1981). Therefore, the linear velocity of the end-effector is computed as x˙ = A(q)q˙ with A(q) = ∇q r , ∇q s (20) as the Jacobian matrix. For the elastically constrained mechanism, the natural potential energy is named Vc (r(q)) and represents the elastic energy function imposed by the compliant environment. Assumption 1. From (Arimoto et al., 1999; Doulgeri and Karayiannidis, 2011), the elastic energy Vc (∆r) induced by the lossless environment is a smooth positive definite potential function, with ∆r = r − r0 and has an unique constant unforced equilibrium r0 at which its gradient ∇r Vc (∆r) is strictly increasing, and the Hessian matrix satisfies ∇2r V (∆r) > 0. As in (Navarro-Alarcon et al., 2014), we assume that the end-effector of the manipulator is provided with a force transducer, located at the contact point that measures the vector of the interaction forces along the m elastically constrained directions, and it is given by the gradient of the elastic potential function f = ∇r Vc (∆r) ∈ Rm . To practical purposes, we assume that the deformation given by the interaction with the end effector robot is linear. Then, the measured force satisfy f = ∇r Vc (∆r) ≈ K∆r, such that, the elastic energy is given by 1 Vc ≈ ∆r K∆r, (21) 2 where K = K : Rm×m > 0 represents the stiffness matrix. An auxiliary error PH system: First we presents an auxiliar dynamical model helpful to give the robustness of the force tracking controllers. Taking the transformed system (5) with A(q) as the Jacobian matrix given by (20), P (ς(q)) = Vc (x) and the error coordinates (22) x ˜ = x − xd (t), p˜ = p − pd (t), the closed loop system with controller ∇r Vc (∆r) + p˙ d v = −T ∇x Wd (x, t) − J2 pd + u1 + T 0 ˜ f = −T (kf + 1)f (t) − J2 pd + T + p˙d + u1 , 0 ks s˜

where f˜(t) = f − fd (t), fd (t) = K∆rd (t) and pd = T − x˙ d yields:

132

x ˜˙ 0 0 0 T (q) ∇W = u + , + 1 d T (q)d In −T (q) J2 (q, p) p˜˙ (23) with Hamiltonian function 1 1 2 p| , s|2 + |˜ (24) Wd = Vr (∆r, t) + ks |˜ 2 2 where ks > 0 and elastic energy defined as Vr = (kf + 1) Vc (∆r) − Vc (∆rd (t)) − r˜ ∇r Vc (∆rd (t)) , (25) which has a zero equilibrium on the desired configuration namely ∆rd (t) = (rd (t) − r0 ) and sd (t). Furthermore, with the expression of the elastic energy function (21), we get a simple form for Vr , given by 1 r) = (kf + 1)˜ r K r˜. (26) Vr (˜ 2

Finally, the hamiltonian function Wd can be written as 1 2 Wd (˜ p| , x, p˜) = Vd (˜ x) + |˜ (27) 2 with 1 Vd (˜ x) = Vr (˜ r) + ks |˜ s |2 . 2 Remark 2. In is interesting to note that, by using (24) as Lyapunov function, we cannot provide none stability property of the system (23). Proposition 2. Consider the mechanical system (23) with S(q) = A(q)M (q)A (q) in closed-loop with the control law x) + T Rz−1 [J2 − R2 − R3 ]z3 u1 = T [In − S − Rz−2 S]∇x˜ Vd (˜ − J2 p˜ − T Rz−1 [R2 − J2 ]Rz T −1 p˜ + S −1 R1 ∇˜ xV d − T Rz−1 R3 Rz [T −1 p˜ + S −1 R1 ∇x˜ Vd ] − T

d {T −1 }˜ p dt

d −1 {S }R1 ∇x˜ Vd − T − R1 ∇2x˜ Vd T p˜ dt z˙3 = Rz−1 S∇x˜ Vd + R3 Rz S −1 [T p˜ + R1 ∇x˜ Vd ]. −T

(28)

(i) The closed-loop dynamics expressed in the coordinates ˜, z1 = x

z2 = Rz S −1 {T p˜ + R1 ∇x˜ Vd } + z3 , (29) takes the following perturbed PH form 0 (30) z˙ = F ∇U − Rz d(t) 0 with interconnection matrix F given by (11) and energy storage function 1 1 (31) U (z) := |z2 |2 + Vd (z1 ) + |z3 |2 . 2 2 (ii) For time varying disturbances d(t), the perturbed system is endowed with the strong ISS property. (iii) For constant external forces, the zero equilibrium of the transformed system z˜˙ = F ∇Ud (32) is GES with change of coordinates proposed in (13) and energy function. 1 1 z2 |2 + Vd (˜ z3 − d|2 z ) = |˜ z1 ) + |˜ (33) Ud (˜ 2 2

IFAC LHMNC 2015 July 4-7, 2015. Lyon, France


Then, (˜ x(t), p˜(t)) → 0 and z3 → d exponentially fast. Consequently from the fact that r˜(t) → 0, we ensure that f˜(˜ r(t)) → 0 converge also exponentially. Proof 2. (i) The PH closed-loop system (30) is obtained via direct computations of the change of coordinates (29) and using the controller proposed in (28). This point follows the proceeding of the first point into of the proof at Proposition 1. (ii) Consider the ISS Lyapunov function 1 1 U (z) = Vd (z1 )+ |z2 |2 +|z3 | := z1 N z1 +|z2 |2 +|z3 | 2 2 (34) with N = diag[(kf + 1)K, ks In−m ]. Computing its time derivative along the solutions of (30) with Rz = In yields U˙ = −z1 N R1 N z1 − z2 R2 z2 − z3 R3 z3 − z2 d(t)

133

cally defined and analytically easy to compute (Lancaster and Tismenetsky, 1985). 4. NUMERICAL EXAMPLE In this section, we present a numerical example of the proposed control design for unconstraint motion. We consider the classical 2-DOF revolute robotic manipulator, with q1 and q2 as the joint positions. The Hamiltonian model of the system, can be described by (5) with inertia matrix c1 + c2 + 2c3 C2 c2 + c3 C2 a1 a2 := , M= a2 a3 c2 + c3 C2 c2

1

2

z) < δ1 Ud (˜ with a bounding scalar satisfying λmin (N T N ) δ1 = 2 min{ , λmin (R2 ), λmin (R3a )}, λmax (N ) establishing the exponential stability of the perturbed mechanical systems interacting with elastic environments. Remark 3. The main modifications of this work with (Romero et al., 2013b) is the extension to PBTC for perturbed mechanical systems, based in the error tracking for PH systems introduced in (Romero et al., 2015). The proposed change of coordinates, assign positive definite matrices in the blocks of the damping matrix of the closed– loop PH system (11). To achieve this end it is necessary also to include the matrix S −1 in the change of coordinates (9). Furthermore, in this paper we use the Cholesky decomposition of M −1 that, in contrast to the the square root matrix proposed in (Romero et al., 2013b), is univo-

133

q and q d

potential energy function Vg (q1 , q2 ) = −c4 gC1 − c5 gC12 and Cholesky factorization T (q) given as   1 0   √a 1 1 1 , T (q) =  ≤ −z1 2N R1 N − z2 2R2 − z3 2R3 + |z2 |2 + |d(t)|2 .  a2 a1  2 2 −√ (35) det a2 det Then, considering the following bounds with R3 = where, to simplify the notation, we have defined C1 = R3 > 0 cos(q1 ), C2 := cos(q2 ), C12 = cos(q1 + q2 ), det := a1 a3 − 2 1 λmin (N T N ) , λmin (R2 ; ), λmin (R3 )} (36) a2 and the definition of all constants may be found in δ = 2 min{ λmax (N ) 2 (Sandoval et al., 2008). we can express (35) as According to Proposition 1, the control gains are se1 lected as R1 = diag[3.3, 3.1], R2 = diag[22, 16] and 2 (37) R = diag[2.3, U˙ ≤ −δU(z) + d(t) . 3.6]. We carry out simulations for con3a 2 stant unknown disturbances, whit d = col(−100, 20). Furthermore, The transient behavior of the error signals q˜, p˜ and z3 1 d (38) are shown in Fig.1–Fig.3 with initial conditions (qi , pi , U˙ ≤ −(1 − θ1 )δU (z) ∀z ≥ √ 2θ1 δ z3i )= [−π, π6 ; 0.4, 0.12; 4, 5] and desired trajectory qd = with 0 < θ1 < 1. From (38) and the fact that U (z) [0.2 + 1.535 sin(0.8t); 0.2 − 4.67 cos(0.8t)] . As seen from is an ISS-Lyapunov function, we have that the closed the figures, besides the exponential convergence to zero of loop system (30) (error trajectories) is ISS. the error trajectories of positions and momenta predicted (iii) Using the change of coordinates (13) and considering by the theory, we also observe that the state z converges 3 R3 = J2 + R3a and Rz = (R2 + R3a ), with free to the disturbance value. Thus, it is ensured that the gain matrix R3a = R3a > 0, we get the transformed disturbances have no effect on the state q and p during closed–loop system (32). the tracking of its desired trajectory. On the other hand, proposing as Lyapunov function (33) and taking its time derivative along solutions of 5 q 1 (32) yields q2 q 1d U˙d = −˜ z3 − d) R3a (˜ z3 − d), z N R1 N z˜1 − z˜ R2 z˜2 − (˜ q2d

0

−5 0

5

Time (sec)

10

15

Fig. 1. Transient behaviour of q and qd 5. CONCLUSIONS AND FUTURE RESEARCH In this paper, we have presented a robust control methodology to solve the problem of global exponential tracking of mechanical systems in presence of constant external disturbances. In addition, for bounded time-varying disturbances has been established boundedness of the error trajectories via the ISS properties. The presented result is also applicable to force tracking task with passive environments and lineal deformation. Robustness of the



50 z31

z 3 and d

0

z32 d1

−50

d2

−100 −150 −200 0

0.5

1

1.5

2

2.5

Time (sec)

3

3.5

4

Fig. 2. Transient behaviour of z3 7 6

15

5

| p˜| 2

| q˜| 2

4 3 2

10

5

1 0 −1 0

0 1

2

3

Time (sec)

4

5

0

1

2

3

Time (sec)

4

5

Fig. 3. Transient behaviour of |˜ q | and |˜ p| system is achieved with a dynamic state-feedback controller that incorporate integral actions while preserving the PH structure. It is important to note that none of the presented controllers cancelled the nonlinearities of the system. Moreover, it made used of these nonlinear terms (skew-symmetric) to achieve the robustification objective. Current research is focused on the experimental validation of the controllers proposed. Also, the extension of the robust control design without velocity measurements is under research. Preliminary calculations show that it is possible to prove (local) convergence to a residual set. REFERENCES S. Arimoto, H. Han, C. Cheach and S. Kawamura Extension of impedance matching to nonlinear dynamics of robotic tasks. Syst. Control Lett., 32 (2), pp. 109–119, 1999. C. Baspiner. On robust position/force control of robot manipulators with constraint uncertainties, 10th IFAC Symposium on Robot Control, pp. 555–560, 2012. C. Chian-Song, L. Kuang-Yow and W. Tsu-Cheng. Robust adaptive motion/force tracking control design for uncertain constrained robot manipulators, Automatica, 40 (12), pp. 2111–2119, 2004. A. Donaire and S. Junco. On the addition of integral action to port-controlled Hamiltonian systems, Automatica, 45 (8), pp. 1910–1916, 2009. A. Donaire, T. Perez, and C. Renton. Manoeuvring control of fully-actuated marine vehicles. A Port-Hamiltonian system approach to tracking, Australian Control Conference, pp. 32-37, 2011. Z. Doulgeri and K. Karayiannidis. Force position control for a robot finger with a soft tip and kinematic uncertainties, Robotics and Autonomous Systems, 55 (2), pp. 328–336, 2011. V. Duindam, and S. Stramigioli. Port–based asymptotic curve tracking for mechanical systems, European Journal of Control, 10 (5), pp. 411-420, 2004. 134

K. Fujimoto, K. Sakurama and T. Sugie. Trajectory tracking control of port-controlled Hamiltonian systems via generalized canonical transformations, Automatica, 39 (12), pp. 2059–2069, 2003. H.K Khalil. Nonlinear Systems. Third ed., Prentice Hall, 1994. P. Lancaster and M. Tismenetsky. Theory of Matrices. Academic Press, 1985. R. M. Murray, Z. Li and S. S. Sastry. A Mathematical Introduction to Robotic Manipulation. CRC Press, 1994. D. Navarro-Alarc´on, Y. Liu, J. G. Romero and P. Li. Energy Shaping Methods for Asymptotic Force Regulation of Compliant Mechanical Systems systems, IEEE Trans. on Control Systems Technology, 22 (6), pp. 2376–2383, 2014. R. Ortega, A. Loria, P. J. Nicklasson and H. Sira– Ramirez. Passivity-based control of Euler–Lagrange systems. Communications and Control Engineering, Springer-Verlag, Berlin, 1998. R. Ortega and J. G. Romero. Robust integral control of port-Hamiltonian systems: The case of non-passive outputs with unmatched disturbances, Systems & Control Letters, 61 (1), pp. 11–17, 2012. M. H. Raibert and J. J. Craig. Hybrid position/force control of manipulators, ASME J. of Dynamic Systems, Measurement, and Control , 103 (2), pp. 126–133, 1981. J. G. Romero, A. Donaire and R. Ortega. Robust energy shaping control of mechanical systems, Systems & Control Letters, 62 (9), pp. 770–780, 2013a. J. G. Romero, D. Navarro-Alarc´on and E. Panteley. Robust globally exponentially stable control for mechanical systems in free/constrained-motion tasks, Proc. IEEE Conf. Decision Control, pp. 3067–3072, 2013b. J. G. Romero, R. Ortega and I. Sarras. A Globally Exponentially Stable Tracking Controller for Mechanical Systems Using Position Feedback, IEEE Trans. Automatic Control , 60 (3), pp. 818–823, 2015. J. Roy and L. Whitcomb. Adaptive Force Control of Position/Velocity Controlled Robots: Theory and Experiment, IEEE Transaction on Robotics and Automation, 18 (2), pp. 121–137, 2002. J. Sandoval, R. Ortega and R. Kelly. Interconnection and Damping Assignment Passivity–Based Control of the Pendubot, Proc; of the 17th World Congress, International Federation of Automatic Control, pp. 7700–7704, 2008. E. Sontag. Input-to-state stability: basic concepts and results, in: P. Nistri, G. Stefani (Eds.), Nonlinear and Optimal Control Theory, Springer-Verlag. Berlin, pp. 163–220, 2007. A. Van der Schaft. L2 –Gain and Passivity Techniques in Nonlinear Control. Springer, Berlin, 1999. A. Venkatraman, R. Ortega, I. Sarras, and A. Van der Schaft. Speed observation and position feedback stabilization of partially linearizable mechanical systems, IEEE Trans. Automatic Control, 55 (5), pp. 1059–1074, 2010. B. Yao, B and M. Tomizuka. Adaptive Robust Motion and Force Tracking Control of Robot Manipulators in Contact with Stiff Surfaces, ASME J. of Dynamic Systems, Measurement, and Control , 120 (2), pp. 232– 240, 1998.