Port-Hamiltonian Formulation of Rigid-Body Attitude Control

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

ScienceDirect

IFAC-PapersOnLine 48-13 (2015) 164–169

Port-Hamiltonian Formulation of Port-Hamiltonian Formulation of Port-Hamiltonian Formulation of Port-Hamiltonian Formulation of Rigid-Body Attitude Control Rigid-Body Attitude Control Rigid-Body Attitude Control Rigid-Body Attitude Control

∗∗∗ Paolo Forni ∗∗∗ Dimitri Jeltsema ∗∗ ∗∗ Gabriel A. D. Lopes ∗∗∗ Paolo ∗∗ Gabriel ∗∗∗ Paolo Forni Forni ∗∗ Dimitri Dimitri Jeltsema Jeltsema ∗∗ Gabriel A. A. D. D. Lopes Lopes ∗∗∗ ∗∗ Gabriel Paolo Forni Dimitri Jeltsema A. D. Lopes Paolo Forni Dimitri Jeltsema Gabriel A. D. Lopes ∗∗∗ ∗ ∗ Department of Electrical and Electronic Engineering, Imperial ∗ Department of Electrical and Electronic Engineering, Imperial ∗ Department ofExhibition Electrical Road, and Electronic Electronic Engineering, Imperial ∗ Department of Electrical and Engineering, Imperial College London, London SW7 2AZ, UK (e-mail: Department ofExhibition Electrical Road, and Electronic Engineering, Imperial College London, London SW7 2AZ, UK (e-mail: College London, Exhibition Road, London SW7 2AZ, UK (e-mail: College London, Exhibition Road, London SW7 2AZ, UK (e-mail: [email protected]). College London, Exhibition Road, London SW7 2AZ, UK (e-mail: [email protected]). ∗∗ [email protected]). [email protected]). Mathematics, Mekelweg 4, Delft University ∗∗ Delft Institute of Applied [email protected]). ∗∗ Delft Institute of Applied Mathematics, Mekelweg 4, Delft University ∗∗ Delft of Institute of Applied Applied Mathematics, Mekelweg 4, Delft Delft University ∗∗ Delft Institute of Mathematics, Mekelweg 4, University Technology, 2628 CD Delft, The Netherlands (e-mail: Delft of Institute of Applied Mathematics, Mekelweg 4, Delft University Technology, 2628 CD Delft, The Netherlands (e-mail: of Technology, Technology, 2628 2628 CD Delft, Delft, The The Netherlands Netherlands (e-mail: (e-mail: of CD [email protected]) of Technology, 2628 CD Delft, The Netherlands (e-mail: [email protected]) ∗∗∗ [email protected]) [email protected]) & Control, Mekelweg 2, Delft University ∗∗∗ Delft Center for Systems [email protected]) ∗∗∗ Center for Systems & Control, Mekelweg 2, Delft University ∗∗∗ Delft Delft Center for Systems & Control, Mekelweg 2, Delft University ∗∗∗ Delft Center for Systems & Control, Mekelweg 2, Delft University of Technology, 2628 CD Delft, The Netherlands (e-mail: Delft Center for Systems & Control, Mekelweg 2, Delft University of Technology, 2628 CD Delft, The Netherlands (e-mail: of Technology, Technology,[email protected]) 2628 CD CD Delft, Delft, The The Netherlands Netherlands (e-mail: (e-mail: of 2628 of Technology,[email protected]) 2628 CD Delft, The Netherlands (e-mail: [email protected]) [email protected]) [email protected]) Abstract: The aim of this paper is to present aa port-Hamiltonian (pH) formulation of the rigidAbstract: The aim this is port-Hamiltonian (pH) formulation of Abstract: The aim of ofproblem, this paper paper is to to present present port-Hamiltonian (pH)tools formulation of the the rigidrigidAbstract: The aim this is aaa port-Hamiltonian (pH) formulation of body attitude control therefore enhancing the set of available for its modeling and Abstract: The aim of ofproblem, this paper paper is to to present present port-Hamiltonian (pH)tools formulation of the the rigidrigidbody attitude control therefore enhancing the set of available for its modeling and body attitude control problem, therefore enhancing the set of available tools for its modeling and body attitude control problem, therefore enhancing the set of available tools for its modeling and control. First, a pH formulation of both dynamics and kinematics equations is presented. Second, body attitude control problem, therefore enhancing thekinematics set of available toolsisfor its modeling and control. First, a pH formulation of both dynamics and equations presented. Second, control. First, a pH formulation of both dynamics and kinematics equations is presented. Second, First, a pH formulation of both dynamics and kinematics equations is presented. Second, aacontrol. standard energy-balancing passivity-based controller (EB-PBC) is used for set-point tracking. control. First, a pH formulation of both dynamics and kinematics equations is presented. Second, standard energy-balancing passivity-based controller (EB-PBC) is used for set-point tracking. standard energy-balancing passivity-based controller (EB-PBC) istoused used for set-point set-point tracking. aaa standard energy-balancing passivity-based controller (EB-PBC) is for tracking. Third, the controlled system is endowed with aa dynamical extension achieve set-point tracking standard energy-balancing passivity-based controller (EB-PBC) istoused for set-point tracking. Third, controlled system is with extension set-point tracking Third, the the controlledthe system is endowed endowed with a dynamical dynamical extension toitachieve achieve set-point tracking Third, the controlled system is endowed with a dynamical extension to achieve set-point tracking without measuring angular velocities. As a conclusive remark, is showed under specific Third, the controlledthe system is endowed with a dynamical extension toitachieve set-point tracking without measuring angular velocities. As a conclusive remark, is showed under specific without measuring the angular velocities. As a conclusive remark, it is showed under specific without measuring the angular velocities. As a conclusive remark, it is showed under assumptions that these three results can be achieved regardless the coordinate representation without measuring the angular velocities. Asachieved a conclusive remark, itcoordinate is showed representation under specific specific assumptions that these three results can be regardless the assumptions that these three results can be achieved regardless the coordinate representation assumptions that these these threefollow results can be achieved achieved regardless thepH coordinate representation in use. Additional examples to motivate the adoption of the formulation. assumptions that three results can be regardless the coordinate representation in use. Additional examples follow to motivate the adoption of the pH formulation. in use. use. Additional Additional examples examples follow follow to to motivate motivate the the adoption adoption of of the the pH pH formulation. formulation. in in use. Additional examples follow to adoption of by theElsevier pH formulation. © 2015, IFAC (International Federation of motivate Automatic the Control) Hosting Ltd. All rights reserved. Keywords: Attitude control, port-Hamiltonian systems, nonlinear control systems Keywords: Attitude control, port-Hamiltonian systems, nonlinear control Keywords: Attitude Attitude control, port-Hamiltonian port-Hamiltonian systems, nonlinear nonlinear control systems systems Keywords: Keywords: Attitude control, control, port-Hamiltonian systems, systems, nonlinear control control systems systems 1. INTRODUCTION briefly recalling the essence of passivity-based control 1. INTRODUCTION briefly recalling the essence of passivity-based control 1. INTRODUCTION INTRODUCTION briefly and recalling the essence essence ofhow passivity-based control 1. briefly recalling the of passivity-based control (PBC) subsequently shows to naturally re-derive 1. INTRODUCTION briefly and recalling the essence ofhow passivity-based control (PBC) subsequently shows to naturally re-derive (PBC) and subsequently shows how to naturally re-derive (PBC) and shows to naturally re-derive the attitude controller proposed and Lewis, (PBC) and subsequently subsequently shows how howby to (Bullo naturally re-derive The attitude control problem, see e.g., (Bullo and Lewis, the attitude controller proposed by (Bullo and Lewis, The attitude control problem, see e.g., (Bullo and Lewis, the attitude attitude controller proposed by (Bullo (Bullo and Lewis, controller proposed by and Lewis, 2004). A major drawback of this controller is the necessity The attitude attitude control problem, see e.g., (Bullo and andproblem Lewis, the The control problem, see e.g., (Bullo Lewis, the attitude controller proposed by (Bullo and Lewis, 2004; Chaturvedi et al., 2011), is the well-known 2004). A major drawback of this controller is the necessity The attitude control problem, see e.g., (Bullo andproblem Lewis, 2004). 2004; Chaturvedi et al., 2011), is the well-known 2004). A major drawback of this controller is the necessity A major drawback of this controller is the necessity of velocity measurements. To overcome this issue, the 2004; Chaturvedi et al., 2011), is the well-known problem 2004; Chaturvedi et al., 2011), is the well-known problem 2004). A major drawback of this controller is the necessity of asymptotically stabilizing a rigid spacecraft about a of velocity measurements. To overcome this issue, the 2004; Chaturvedi etstabilizing al., 2011),aisrigid the well-known problem of asymptotically spacecraft about aa of of velocity velocity measurements. To overcome thisfeedback issue, the the measurements. To overcome this issue, dynamic extension approach for position-only of of asymptotically stabilizing a rigid spacecraft about of asymptotically stabilizing using a rigid rigid spacecraft about a dynamic of velocity measurements. To overcome thisfeedback issue, the desired reference orientation its thrusters. The dyextension approach for position-only of of asymptotically stabilizing a spacecraft about a desired reference orientation using its thrusters. The dydynamic extension approach for position-only feedback of dynamic extension approach for position-only feedback of (Dirksz et al., 2008) is exploited in Section 4. In Section, desired reference orientation using its thrusters. The dydesired reference orientation using its thrusters. The dydynamic extension approach for position-only feedback of namics of the velocities of a rigid body free to thumble (Dirksz et al., 2008) is exploited in Section 4. In Section, desired reference orientation using its thrusters. The dynamics of the velocities of a rigid body free to thumble (Dirksz et al., 2008) is exploited in Section 4. In Section, (Dirksz et al., 2008) is exploited in Section 4. In Section, it is show that, under specific assumptions, it is possible namics ofare thedescribed velocities by of aaEuler’s rigid body body free to to thumble namics of the velocities of rigid free thumble (Dirksz et al., 2008) is exploited in Section 4. In Section, in space equations of motion. it is show that, under specific assumptions, it is possible namics ofare thedescribed velocities by of aEuler’s rigid body free to thumble in space equations of motion. it is is show show that, that, under specific assumptions, assumptions, it is is possible under specific it possible to the pH formulation, the standard controller, in space space are are described described by Euler’s Euler’s equations of stabilize motion. it in by equations of motion. it isachieve show that, under specific assumptions, it is possible Together the velocities, it is also required to to achieve the pH formulation, the standard controller, in space with are described by Euler’s equations of stabilize motion. to Together with the velocities, it is also required to to achieve the pH formulation, the standard controller, the formulation, the controller, and the extended regardless the choice of the Together withkinematics the velocities, velocities, it is is also alsonamely requiredthe to stabilize stabilize Together with the it required to to achieve achieve the pH pHcontroller, formulation, the standard standard controller, the attitude equations, relation and the extended controller, regardless the choice of the Together with the velocities, it is also required to stabilize the attitude kinematics equations, namely the relation and the the extended extended controller, regardless the choice of the and controller, regardless the choice of the attitude parameterization. the attitude kinematics equations, namely the relation the attitude kinematics equations, namely the relation and the extended controller, regardless the choice of the between the velocities and the attitude rates of changes in attitude parameterization. the attitude kinematics equations, namely the relation between the velocities and the attitude rates of changes in attitude parameterization. attitude parameterization. between theattitude velocities and the the attitude rates of changes changes in attitude parameterization. × between the velocities and rates of in 3 the chosen parametrization. The literature is vast between theattitude velocities and the attitude attitude rates of changes in Notation: The ‘cross’ map (·) × : R3 → so(3) is defined as the chosen parametrization. The literature is vast × 3 → so(3) is defined as Notation: The ‘cross’ map (·) the chosen chosen attitude parametrization. The literature literature is vast vast Notation: × :: R the attitude parametrization. The is Notation: The ‘cross’ map (·) R → so(3) so(3) is defined defined as and spans many decades and applications in aerospace, × The ‘cross’ map (·) : R the chosen attitude parametrization. The literature is vast Notation: The ‘cross’ map (·) : R33 → and spans many decades and applications in aerospace, → so(3) is is defined as as and spans many decades and applications in aerospace, and spans many decades and applications in aerospace, underwater vehicles, and robotic systems. ] ] [ [ × and spans many decades and applications in aerospace, underwater vehicles, and robotic systems. ] ] [ [ × v 0 −v v underwater vehicles, vehicles, and and robotic robotic systems. systems. 3 v2 ] 1 ]× [ [ underwater [ 0 −v [ 3 v2 ] underwater vehicles, and robotic systems.control problem ]× [vv1112 ] × := [ v0 vv221 ] . 003 −v −v In this paper, we approach the attitude 00 333 −v (1) vv × × = v v −v In this paper, we approach the attitude control problem × 1 21 . v −v (1) := = 3 2 × In this paper, we approach the attitude control problem v 0 −v v (1) := = v In this paper, we approach the attitude control problem 32 v01 −v 1 .. 2 × = v2 v from a port-Hamiltonian perspective. Apart from offering (1) := v −v 0 3 1 3 In this paper, we approach the attitude control problem v 0 −v v . (1) = := v from a port-Hamiltonian perspective. Apart from offering 2 3 1 −v v 0 3 2 1 from a port-Hamiltonian perspective. Apart from offering −v v 0 v from a port-Hamiltonian perspective. Apart from offering 2 1 3 v −v v 0 a systematic and insightful framework for modelling and 3 2 1 from a port-Hamiltonian perspective. Apart from offering v −v v 0 aa systematic and insightful framework for modelling and 2 1 3 systematic and insightful framework for modelling and aanalysis and framework for and of multi-physics systems, port-Hamiltonian ∨ 3 a systematic systematic and insightful insightful framework for modelling modelling sysand analysis of multi-physics systems, port-Hamiltonian sys‘vee’ map (·) ∨ : so(3) → R3 denotes the inverse operaanalysis of multi-physics multi-physics systems, port-Hamiltonian sys- The ∨ 3 The ‘vee’ map (·) : so(3) → R the inverse operaanalysis of systems, port-Hamiltonian sys∨ 3 denotes tems theory provides a natural starting point for control; × ∨ The ‘vee’ map (·) : so(3) → R denotes the inverse analysis of multi-physics systems, port-Hamiltonian sys- tion ∨ : so(3) 3 a. The ‘vee’ map (·) R denotes the inverse operatems theory provides aa natural starting point for control; of ‘cross’, namely (a ))∨ = The gradient of aa operascalar ×→ The ‘vee’ map (·) : so(3) → R denotes the inverse operatems theory provides natural starting point for control; × ∨ tion of ‘cross’, namely (a = a. The gradient of tems theory provides a natural starting point for control; × )∨ = a. The gradient of a scalar see (van der Schaft and Jeltsema, (Duindam of ‘cross’, namely (a scalar temse.g., theory provides a natural starting 2014), point for control; tion × )the ∨ =operator tion of ‘cross’, namely (a a. The gradient of a scalar see e.g., (van der Schaft and Jeltsema, 2014), (Duindam function is denoted with ∇ := ∂/∂x. All x tion of ‘cross’, namely (a ) = a. The gradient of a scalar seeal., e.g., (van and der the Schaft and Jeltsema, Jeltsema, 2014), (Duindam function is denoted with the operator ∇ ∂/∂x. All see e.g., (van der Schaft and 2014), (Duindam x := et 2009), references therein. Especially in the function is denoted with the operator ∇ := ∂/∂x. All see e.g., (van der Schaft and Jeltsema, 2014), (Duindam x function is denoted with the operator ∇ := ∂/∂x. All et al., 2009), and the references therein. Especially in the vectors are considered as column vectors, the x including function is denoted with the operator ∇ := ∂/∂x. All et al., 2009), and the references therein. Especially in the x including the vectors are considered as column vectors, et al., 2009), and the references therein. Especially in the nonlinear case it is widely recognised that physical propvectors are considered as column vectors, including the et al., 2009), and the references therein. Especially in the vectors are considered as column vectors, including the nonlinear case it is widely recognised that physical propgradient of a scalar function. Hence, the Jacobian of aa vectors are considered as column vectors, including the nonlinear case it is widely recognised that physical propgradient of a scalar function. Hence, the Jacobian of nonlinear case it that physical properties of the as balance and conservation nfunction. m of aa scalar Hence, the Jacobian of a nonlinear casesystem—such it is is widely widely recognised recognised that physical prop- gradient gradient of scalar function. Hence, the Jacobian of erties of the system—such as balance and conservation vector function h : R → R is denoted as n m gradient of a scalar function. Hence, the Jacobian of aa ertiesand of the the system—such as balance balance and and conservation n m vector function h :: R R denoted as erties of system—such as conservation n → m is laws energy considerations—should be exploited and vector function h R → R is denoted as erties of the system—such as balance and conservation n m vector function h : R → R is laws and considerations—should be exploited and is denoted denoted as as laws and and energy energy considerations—should be and laws energy considerations—should be exploited exploited and vector function h : R → R  respected in the design of control laws which are robust ⊤   laws and energy considerations—should be exploited and respected in the design of control laws which are robust   ∇ ⊤ h1 (x)   respected in the design of control laws which are robust x ⊤ h1 (x)  ∇ respected in design  and physically interpretable. x ∇⊤ h.11 (x) (x)  respected in the the design of of control control laws laws which which are are robust robust  and physically interpretable. ∇ x xh and physically physically interpretable. interpretable. ∇⊤  Jac (x) := . (2) and   x h..1 (x)  h   and physically interpretable. Jach (x) (x) := :=  (2)    The contributions of the paper are outlined as follows. . Jac (2)    ⊤ ..  .... h (x) :=  Jac (2)  h The contributions of the paper are outlined as follows. Jac (x) := (2)   h The contributions of the paper are outlined as follows. ∇ h (x) ⊤ . m (x) The contributions of the paper are outlined as follows. x In Section 2, first some background on port-Hamiltonian ⊤ ∇ h TheSection contributions of the paper are on outlined as follows. ⊤ hm x In 2, first some background port-Hamiltonian ∇ (x) m ⊤ ∇ h (x) x In Section 2, first some background on port-Hamiltonian m In 2, first background on ∇xx hm (x) modeling is is how the wellIn Section Section is 2, provided. first some some Next, background on port-Hamiltonian port-Hamiltonian modeling is Next, is is shown shown how the wellmodeling is provided. provided. Next, is how the wellmodeling is provided. Next, of is is istheshown shown how the wellknown dynamical equations rigid body can be The notation tr(A) is used to represent the trace of aa modeling is provided. Next, is is shown how the wellknown dynamical equations of the rigid body can be tr(A) known dynamical dynamical equations of the the rigid rigid body body can by be The The notation notation tr(A) is is used used to to represent represent the the trace trace of of aa known equations of can be tr(A) extended to include the kinematics. 3 starts square matrix A. known dynamical equations of the Section rigid body can by be The The notation notation tr(A) is is used used to to represent represent the the trace trace of of a extended to include the kinematics. Section 3 starts square matrix A. extended to to include include the the kinematics. kinematics. Section Section 33 starts starts by by square matrix A. extended square matrix A. extended to include the kinematics. Section 3 starts by square matrix A. 164 Hosting by Elsevier Ltd. All rights reserved. 2405-8963 ©© 2015, IFAC (International Federation of Automatic Control) Copyright 2015 IFAC 164 Copyright © 2015 IFAC 164Control. 164 Copyright © 2015 IFAC Peer review under responsibility of International Federation of Automatic Copyright © 2015 IFAC 164 Copyright © 2015 IFAC 10.1016/j.ifacol.2015.10.233

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

Paolo Forni et al. / IFAC-PapersOnLine 48-13 (2015) 164–169

2. PORT-HAMILTONIAN FORMULATION 2.1 Background on Port-Hamiltonian Systems Port-Hamiltonian (pH) systems can be seen as the extension of the classical Hamiltonian equations of motion for mechanical systems to a more general category of systems, namely systems arising from the network modeling of physical systems. Fundamental in their formalization is the geometric notion of Dirac structure (Duindam et al. (2009)). In this paper, we consider the class of so-called input-state-output pH system with dissipation of the form: { x˙ = [J(x) − D(x)]∇x H(x) + g(x)u, Σ: (3) y = g ⊤ (x)∇x H(x), where x ∈ Rn represents the state vector, u ∈ Rm is the control input, y ∈ Rm is the corresponding output, J(x) = −J ⊤ (x) is a skew-symmetric interconnection matrix, D(x) = D⊤ (x) ≽ 0 a dissipation matrix, and H(x) the Hamiltonian. Differentiating the Hamiltonian along the trajectories of (3), we recover the fundamental power-balance ⊤ ⊤ H˙ = −∇⊤ (4) x H(x)D(x)∇x H(x) + y u ≤ y u. Hence, pH systems (3) are passive under the additional assumption that H(x) is bounded from below. Specifically, (3) is conservative (lossless) when D(x) = 0. 2.2 PH formulation for Attitude Control In this subsection, a pH formulation of both the attitude and angular velocities of a rigid-body is presented. The attitude is represented by rotation matrices, namely matrices within the special orthogonal group SO(3) = { } R ∈ R3×3 | RR⊤ = I, det R = 1 . A generalization over coordinate representation will be given in Section 5.

Choose an inertial reference frame and a body-fixed frame. Let R ∈ SO(3) denote the rotation matrix from the body-fixed frame to the inertial frame, thus describing the attitude of the rigid body with respect to a non-rotating object. Let ω ∈ R3 , I := diag(Ix , Iy , Iz ) ≻ 0, and u ∈ R3 denote the angular velocity vector, the inertia matrix, and the applied control torques of the rigid body with respect to the body-fixed frame, respectively. The equations of motion then read as: R˙ = Rω × (kinematics equation), (5a) × I ω˙ + ω Iω = u (dynamics equation). (5b) A pH formulation of the dynamics (5b) has already been presented in the literature (see for instance, Example 3.1.1 in van der Schaft and Jeltsema (2014)) and is given by: p˙ = p× ∇p H(p) + u, (6) where p := Iω denotes the vector of the angular momenta and the Hamiltonian 1 H(q, p) := p⊤ I −1 p (7) 2 corresponds to the total kinetic energy of the rotating rigid-body. Assume now that the elements of matrix R are defined as follows: [ ] Rx (8) R := Ry , Rz 165

165

where Rx , Ry , and Rz denote first, second, and third row of R, respectively. To the end of formulating both equations (5), the displacement vector q ∈ R9 is introduced as the flattened version of R, namely { } ⊤ (9) q := vec R⊤ = [Rx Ry Rz ] .

Let r(q) : R9 → R9×3 be the matrix  × Rx r(q) := Ry×  . (10) Rz× With the previously introduced notation, the pH system corresponding to equations (5) is then given as ][ ] [ ] [ ] [ 0 r(q) ∇q H(q, p) 0 q˙ + u, (11) = ΣA : ∇p H(q, p) I p˙ −r⊤ (q) p× ���� � �� �� �� � ���� x˙

∇x H(x)

J(x)

g(x)

with D(x) = 0 and passive outputs y = ∇p H(q, p) = ω. 3. STANDARD CONTROLLER 3.1 Background on Passivity-Based Control

Passivity-based control (PBC) is a control methodology that achieves the control objective by rendering a system passive with respect to a desired storage function (Ortega et al. (2008) and Ortega et al. (2001)). The standard assumption in PBC is that, for a general nonlinear system x˙ = f (x, u), y = h(x), the energy-balance equation ∫ t ( ) ( ) H x(t) − H x(0) = u⊤ (s)y(s)ds − D(t) 0

is satisfied with H(x) bounded from below and D(t) being a nonnegative function capturing the dissipation effects. Generally, the point where the open-loop energy H(x) is minimal is usually not the one of practical interest. Therefore the set-point stabilization objective requires the shaping of the energy function into a new desired one Hd (x) by means of a state-feedback u = β(x) + v. In fact, if Hd (x) has a strict global minimum at x∗ , Lyapunov stability of x∗ can be proven when v = 0. Assigning an energy function with a minimum at the desired equilibrium is usually referred to as energy shaping, whereas the modification of the dissipation is called damping injection that is achieved by feeding back the passive outputs, i.e., v = −C(x)y, (12) with C(x) ≽ εI for all x and some ε > 0. Damping injection is usually required to infer asymptotic stability under relatively mild additional assumptions (i.e., zerostate detectability 1 ). Energy-balancing (EB-PBC) is a methodology in which the closed-loop energy Hd is equal to the sum of the energy H stored in the plant and the energy Href supplied by the controller, namely Hd := H + Href . For the pH system (3), EB-PBC aims to find a state-feedback u = β(x) + v such that the closed-loop system still retains its pH form: { x˙ = [J(x) − D(x)]∇x Hd (x) + g(x)v, Σd : (13) y = g ⊤ (x)∇x Hd (x), where Hd has a strict local minimum at x∗ , thus rendering (13) Lyapunov stable by exploiting the passivity property 1

Recall that a system Σ is said to be zero-state detectable if u(t) ≡ 0 and y(t) ≡ 0, for all t ≥ 0, implies that limt→∞ x(t) = 0.

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

Paolo Forni et al. / IFAC-PapersOnLine 48-13 (2015) 164–169

(4). The function β(x) achieving this control objective is the solution of the PDE [J(x) − D(x)] ∇x Href (x) = g(x)β(x). (14) Additionally, if the closed-loop system (13) is fully damped (for instance by injecting damping as in (12)) and is zerostate detectable, then the equilibrium x∗ is locally asymptotically stable. Under the additional assumption of Hd (x) being radially unbounded (i.e., Hd (x) → ∞ as ∥x∥ → ∞), the equilibrium x∗ becomes globally asymptotically stable. 3.2 EB-PBC for Attitude Control Consider the system (5) and its pH formulation (11). Let Rref ∈ SO(3) denote the desired attitude configuration for the rigid body as follows ] [ Rref,x Rref := Rref,y , Rref,z where Rref,x , Rref,y , and Rref,z denote the first, second, and third row of Rref , respectively. The first step of the EB-PBC is to shape the closed-loop energy Hd (q, p) = H(q, p) + Href (q) (15) such that it has a minimum in (R∗ , p∗ ) = (Rref , 0). Following (Bullo and Lewis, 2004), this is achieved by selecting )] 1 [ ( ⊤ Href (q) := tr Kp I − Rref R , (16) 2 with controller gain Kp := diag(kpx , kpy , kpz ) ≻ 0 tuning the steepness of the energy function Href (q). Solving the PDE (14) gives the energy-shaping control input: ues = −r⊤ (q)∇q Href (q) (17) ]∨ 1[ ⊤ = − Kp Rref R − R⊤ Rref Kp⊤ . 2 By selecting the damping-injection control input as udi = −Kd I −1 p, with controller gain Kd := diag(kdx , kdy , kdz ) ≻ 0, the total applied control torque becomes u = ues + udi , which, in turn, yields the pH closed-loop system: ][ ] [ ] [ 0 r(q) ∇q Hd (q, p) q˙ . (18) = ΣA d : p˙ −r⊤ (q) p× − Kd ∇p Hd (q, p) 4. EXTENDED CONTROLLER 4.1 Background on PBC with Only Position Measurements In (Dirksz et al., 2008), a dynamic extension for position feedback of pH mechanical systems is studied to avoid the injection of damping by means of velocity measurement, thus overcoming the degradation of the derivative action on noisy signals. The underlying idea of (Dirksz et al., 2008) is to make the energy bounce between an Euler-Lagrange (EL) system—the plant—and a virtual EL system—the controller—and to dissipate this energy only when the controller retains it. In this way, dissipation is propagated to the plant. Let us consider a pH mechanical system Σ as (3). For ease of presentation, it is assumed here that system Σ has been brought to the form Σd (13) with desired Hamiltonian Hd (x) = 12 p⊤ M (q)−1 p + Vd (q). Note that in (13) the dissipation matrix D(x) is zero due to the prohibition of 166

feeding back the velocities by means of standard damping injection techniques. A virtual mechanical systems Σc in ⊤ the coordinates xc = [qc⊤ , p⊤ c ] is introduced, as follows: x˙ c = (Jc (xc ) − Dc (xc )) ∇xc Hc (xc ) , (19) ⊤ with interconnection matrix Jc (xc ) = −Jc (xc ), damping matrix Dc (xc ) = Dc⊤ (xc ) ≽ 0, and Hamiltonian Hc (xc ) = 1 ⊤ −1 pc + Vc (qc ), where the mass matrix Mc (qc ) 2 pc Mc (qc ) and the potential energy term Vc (qc ) have to be defined. The two systems Σd and Σc are coupled together by means a mixed potential energy term V˜d (q, qc ), as in the following. The extended closed-loop system reads as: ] ][ [ ] [ ˜d 0 ∇x H ˜ d : x˙ = J(x) Σ ˜ d , (20) 0 Jc (xc ) − Dc (xc ) ∇x H x˙ c c

˜ d (x, xc ) = 1 p⊤ M −1 (q) p+ 1 p⊤ M −1 (qc ) p+V˜d (q, qc ). with H c 2 2 c Note that the mixed energy term V˜d (q, qc ) accounts for the reshaping of Vd (q) and Vc (qc ), thus for the coupling between Σd and Σc . Define the equilibrium as (x⋆ , x⋆ ) = (qd , 0, qcd , 0) with qd , qcd being the desired reference positions for (q, qc ), respectively. The conditions for the closed˜ d to achieve asymptotic stability of (x⋆ , x⋆ ) loop system Σ are reported in the following theorem. Theorem 1. (Dirksz et al., 2008, Proposition 1) If ˜ d (q, p, qc , pc ) has its strict • (C1) the Hamiltonian H minimum at (qd , 0, qcd , 0) and hence satisfies ˜ d (qd , 0, qcd , 0) = 0; ∇q H • (C2) the PDE matching condition [ ] ˜ d (x, xc ) − ∇x H(x) = 0, g ⊤ (x)J(x) ∇x H

(21)

is fulfilled; • (C3) for ∇qc V˜d (q, qcd ) = 0 it holds that q is constant,

then the closed-loop system (20) is asymptotically stable, and the energy-shaping input takes the form: [ ] ˜ d (x, xc ) − ∇x H(x) , (22) ues = a(x)J(x) ∇x H )−1 ⊤ ( g (x). with a(x) := g ⊤ (x)g(x) 4.2 Attitude Control with only position measurements

Following the ideas provided in Subsection 4.1, a dynamic extension for (11) is designed in order to stabilize the attitude of the rigid body without directly using velocity measurements. To this end, assume that the energy shaping control input (17) has been applied to (11) to yield the closed-loop system (18) with Kd = 0. Consider now the virtual mechanical system (19) as another rotating rigid body with position coordinates qc and momenta pc . The coordinates qc ∈ R9 are assumed to be the flattened version of a rotation matrix Rc ∈ SO(3) in a similar fashion as in Subsection 2.2 for q and R. This virtual mechanical system and system ΣAd are coupled together ˜ d . By as in (20) through a new desired Hamiltonian H ˜ d as a function of (q, p, qc ) only, the dynamics selecting H of pc are automatically discarded, thus further simplifying the design. Moreover, by setting Jc (qc ) = 0, the following closed-loop system is obtained ] ][ [ ] [ ˜ d (x, qc ) J(x) 0 ∇x H x˙ (23) = ˜ d (x, qc ) , 0 −Dc (qc ) ∇q H q˙c c

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

Paolo Forni et al. / IFAC-PapersOnLine 48-13 (2015) 164–169

where x := (q ⊤ , p⊤ )⊤ is the state of ΣAd and Dc (qc ) ≽ 0 is specified later in this Subsection. We are now going to ˜ d (x, qc ) and Dc (qc ) in order to verify the properly design H conditions of Theorem 1, thus concluding the asymptotic stability of the controller. (C1) Define ˜ d (q, p, qc ) := H(q, p) + Href (q) +Hc (q, qc ), (24) H �� � � Hd (q,p)

with Href (q, p) as in (16) and )] 1 [ ( Hc (q, qc ) := tr Kc I − Rc⊤ R , (25) 2 ˜ d (q, p, qc ) with Kc = diag(kcx , kcy , kcz ) ≻ 0. Note that H has a strict local minimum at the equilibrium: (R∗ , p∗ , Rc∗ ) = (Rd , 0, Rd ). (26) (C2) Matching condition (21) indeed holds true and yields the control input (22) 1[ u = − Kp Rd⊤ R − R⊤ Rd Kp⊤ 2 (27) ]∨ + Kc Rc⊤ R − R⊤ Rc Kc⊤ . Condition (C3) cannot be met because, due to R ∈ SO(3), ⊤

∇qc Hc (q, qc ) = [kcx Rx kcy Ry kcz Rz ] cannot simultaneously have all zero components. However, we remark that (C3) is a LaSalle condition ensuring that all trajectories of (20) are trapped into the largest invariant set M that contains the equilibrium (qd , 0, qcd ) only. To overcome this issue, we first consider the time-derivative of (24) along the trajectories of (23). i.e., ˜˙ = −∇⊤ H (q, q ) D (q )∇ H (q, q ), H d

qc

c

c

c

c

qc

c

c

and then we select Dc (qc ) in such a way that

167

extended controller, regardless the choice of the attitude parameterization. The following lemma highlights an interesting property of all attitude parameterizations of SO(3), which will be used in the main result of this section. Specifically, we will show that the kinematics equation is linear in the angular velocity vector ω, regardless of the attitude parameterization in use. Lemma 2. For any attitude parameterization q ∈ Rnq and angular velocity trajectory ω(t) with t ∈ (a, b), it is always possible to write: q(t) ˙ = r(q(t))ω(t), (30) for some matrix r(q) : Rnq → Rnq ×3 , with q(t) being the attitude trajectory. Proof. This result stems from two facts: (i) Since SO(3) is a smooth manifold, any attitude parameterization can be seen as a smooth coordinate function φ : U → V with coordinate chart U ⊂ SO(3) and V ⊆ Rnq ; (ii) tangent spaces on a smooth manifold relate linearly via a Jacobian matrix. Let R : (a, b) → SO(3) be a differentiable attitude trajectory with t varying in (a, b). Consider f0 : SO(3) → R9 as the flattening map f0 (·) = vec {·}. Let f : SO(3) → Rnq be the function mapping R to coordinates q, i.e. f (R(t)) := q(t). By (Baker, 2002, Lemma 7.6) and d vec {R} = r(R(t))ω(t), with r(·) as in (10) recalling that dt and (11), it holds that q(t) ˙ = Jacf f −1 (q(t)) r(R(t)) ω(t). 0

With a slight abuse of notation, we can now re-define r : Rnq → Rnq ×3 as follows ( ) (31) r(q) := Jacf f −1 (q) r f −1 (q) , 0

• Dc (qc ) ∇qc Hc (q, qc ) ≡ 0 for all t implies that M contains (qd , 0, qcd ) only; • R(t) ∈ SO(3) for all t along integration.

Both items are automatically satisfied by selecting: Dc (qc ) := r(qc ) Kd r⊤ (qc ) ≻ 0. Consequently, the dynamics of qc becomes ) ( ] 1[ ⊤ ⊤ ⊤ ∨ . (28) q˙c = −r(qc ) Kd Kc R Rc − Rc RKc 2 In Subsection 2.2, R˙ = Rω × has been recasted into q˙ = r(q)ω. By using the converse procedure, the dynamics (28) turn into [ ( ] ∨ )× 1 R˙ c = Rc Kd Kc R⊤ Rc − Rc⊤ RKc⊤ . (29) 2 Finally, the form of the extended controller is given by the control input (27) and the dynamic extension (29). 5. GENERALIZATION OVER COORDINATE REPRESENTATION A number of coordinate representations, like for instance roll-pitch-yaw (RPY) angles, quaternions, and rotation matrices, serve as parameterizations of the attitude in a three dimensional space. 2 In this Section, we show that, under specific assumptions, it is possible to achieve the presented pH formulation, the standard controller, and the 2

The reader is referred to (Chaturvedi et al., 2011) for a comprehensive introduction on attitude parameterizations.

167

thus proving the lemma.

Q.E.D.

Now assume that the attitude of the rigid body is parametrized using rotation coordinates q ∈ V ⊆ Rnq , where a coordinate chart U and coordinate map f such that f : U → V have been defined. Let qref ∈ Rnq denote the desired orientation for the rigid body, with the same parameterization of q. The main result can then be formalized as follows. Theorem 3. If, for the given attitude parametrization q ∈ V , there exists an energy function Ψ(q, qref ) such that • Ψ(q, qref ) is defined for q ∈ QΨ with QΨ being a neighbourhood of qref ; • Ψ(q, qref ) is strictly positive-definite in QΨ with Ψ being equal to 0 only at q = qref , then the following statements hold for (6)-(30): • (T1) it can be formulated as a pH system of the form (3) with D(x) = 0; • (T2) there exists a EB-PBC controller of the form u = −r⊤ (q) ∇q Ψ(q, qref ) − Kd ω, (32) ⊤ with Kd = Kd ≻ 0 a positive definite gain, which renders the equilibrium (q ∗ , p∗ ) = (qref , 0) asymptotically stable in (QΨ ∩ Qr ) × R3 ; • (T3) there exists a dynamic controller of the form q˙c = −r(qc )Kd r⊤ (qc )∇qc Ψ(q, qc ), (33a) u = −r⊤ (q) [∇q Ψ(q, qref ) + ∇q Ψ(q, qc )] ,

(33b)

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

Paolo Forni et al. / IFAC-PapersOnLine 48-13 (2015) 164–169

which does not use p as an input and renders the equilibrium (q ∗ , p∗ , qc∗ ) = (qref , 0, qref ) (34) asymptotically stable in (QΨ ∩ Qr )×R3 ×(QΨ ∩ Qr ). Proof. (T1) PH formulation (3) follows immediately by selecting the state, the interconnection and input matrices as in (11), with q ∈ V and r(q) as given by Lemma 2. (T2) Let the closed-loop Hamiltonian be Hd (x) := H(p) + Href (q) with Href (q) := Ψ(q, qref ). Note that Hd (x) has a strict local minimum at (q ∗ , p∗ ) = (qref , 0). Solving PDE (14) yields ues := β(x) = −r⊤ (q) ∇q Ψ(q, qref ) as the energy-shaping part of the input. By means of damping injection udi = −Kd ∇p Hd (x) = −Kd I −1 p, we are able to retrieve the EB-PBC control law u = ues + udi as in (32). The proof for (T3) follows the track provided in Dirksz et al. (2008). Consider the EL controller: q˙c = −Dc (qc )∇qc Hc (qc ), (35) with Dc (qc ) and Hc (qc ) to be defined later in the proof. The closed-loop system then reads as: ] ][ [ ] [ ˜ d (x, qc ) J(x) 0 ∇x H x˙ (36) = ˜ d (x, qc ) , 0 −Dc (qc ) ∇q H q˙c

{[

} ]⊤ | q = qc . Evaluating (39c) in O gives q˙c ≡ q ⊤ p⊤ qc⊤ 0 and thus q˙ ≡ 0. Since r(q) has full column rank, from (39a) it can be inferred that p ≡ p˙ ≡ 0. Hence, (39b) reduces to 0 ≡ −r⊤ (q)∇q Ψ(q, qref ), which, in turn, implies q ≡ qref . Therefore, M = {(qref , 0, qref )} and the equilibrium (34) is asymptotically stable. Q.E.D. 6. APPLICATION As mentioned in Section 5, a variety of attitude parameterizations are available for rigid-body attitude control. Energy functions and kinematics equations for most common attitude parameterizations are depicted in Table 1.

˜ d is defined as: where the new closed-loop Hamiltonian H ˜ d (q, p, qc ) := H(p) + Ψ(q, qref ) + Ψ(q, qc ) . (37) H � �� � � �� �

Two simulations are carried out in order to show the effectiveness of the presented dynamic controller (33). In both simulations, the inertia of the rigid body is chosen as I = diag {1, 0.8, 1} , while initial conditions are chosen as [ ] [ ] 0 0 −1 −5 R(0) = cos π/3 − sin π/3 0 , p(0) = 4 , (40) − sin π/3 − cos π/3 0 −3 and Rc (0) = I. The desired attitude is as follows [ ] − cos π/4 sin π/4 0 Rref = sin π/3 cos π/3 0 . (41) 0 0 −1 In Simulation #1, the attitude parameterization is chosen as SO(3). To the end of tuning the steepness of the energy function Ψ [in Table 1, we)] use the follow( 1 ⊤ ing Ψ(q, qref ) = 2 tr Kp I − Rref R , with Kp = [ ( )] diag {2.5, 2, 2.5}, and Ψ(q, qc ) = 12 tr Kc I − Rc⊤ R , with Kc = diag {20, 20, 20}. The damping gain is chosen as Kd = diag {0.5, 0.5, 0.5} and the simulation results are shown in Figure 1.

and solving (22) indeed yields (33b). (C3) Depending on the coordinate representation and corresponding energy function Ψ, condition � ˜ d (q, qc )�� ∇q H = 0 at q = const only (38)

In Simulation #2, the attitude parameterization is chosen as the RPY angles. Again, in order to tune the steepness of the energy function Ψ in Table 1, we use the following k Ψ(q, qref ) = 2p e⊤ e, with kp = 8, and Ψ(q, qc ) = k2c e⊤ e, with kc = 35. The damping gain is chosen as Kd = diag {0.2, 0.2, 0.2}. The simulation results are shown in Figure 2.

c

Hc (q,qc )

Href (q)

The conditions of Theorem 1 are now checked to infer the ˜ d (q, p, qc ) asymptotic stability of equilibrium (34). (C1) H has a strict local minimum at (34) since H(p), Ψ(q, qref ) and Ψ(q, qc ) respectively have a strict local minimum at p = 0, q = qref and qc = q. (C2) Matching condition (21) holds true since ] [ ] [ ] [ r(q) 0 ∇q (Href + Hc ) Inq ×nq 0nq ×3 = 0, 0 −r⊤ (q) p×

c

q,qref

might not be satisfied. However, this condition arises from a LaSalle argument which is required in order for the asymptotic stability of the closed-loop system to be inferred. Equivalently, it can be required that the largest invariant set M{in } [ ]⊤ ˙ ˜ d (q, p, qc ) = 0 O := q ⊤ p⊤ q ⊤ |H c

consists of (34) only. This property is indeed satisfied by setting Dc (qc ) = r(qc )Kd r⊤ (qc ) as follows. The closedloop system reads as q˙ = r(q)I −1 p, (39a) ⊤

p˙ = −r (q)∇q [Ψ(q, qref ) + ∇q Ψ(q, qc )] + p˜ I

−1

p, (39b)

⊤

(39c) q˙c = −r(qc )Kd r (qc )∇qc Ψ(qc , q). ˜ Taking time derivative of the Hd along the trajectories of (39) yields ˜˙ (q, p, q ) = −ν(q, q )⊤ K ν(q, q ), H d

c

c

d

c

with ν(q, qc ) = r⊤ (qc )∇qc Ψ(qc , q). By hypothesis, the latter equals zero for q = qc only, namely O :=

168

It is interesting to observe that, although if the magnitude of the angular velocities is comparable in both simulations, the control input usage in the RPY case is approximately one order of a magnitude larger than the one in the SO(3) case. The behaviour of the RPY case is explained by the trajectory approaching the singularities of matrix r(q), namely θ ≃ ±π/2. 7. CONCLUSIONS We introduced a pH formulation of both dynamics and kinematics equations of the rigid-body attitude control problem. Within the proposed framework, we have presented a standard passivity-based controller and an extended controller which does not make use of angular velocity measurements. By means of both theory and illustrative examples, we have shown that such results are independent of the attitude parameterization in use, thus

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

Paolo Forni et al. / IFAC-PapersOnLine 48-13 (2015) 164–169

169

Table 1. Energy functions for most common attitude representations ϕ θ ψ

∈ R3 , qd :=

Tracking error

Ψ(q, qd ) =



1 0

R entries

1

10

20

0.5 0

10

20

1

1

1

0

0

−1 1

−1 0

10

−1

0

10

20

10 t [s]

20

[

Ψ(q, qd ) =

−1

r(q) :=

0

10 t [s] torques [N m]

py pz 0

5

10

−q⊤ v q4 I3 + q× v

20

10

−1

0

10

]

1 2

tr

(

)∨

I − Rd⊤ R

)

equation (10)

0.2

0

−0.5

0

10 t [s]

20

uz 5

10

q qc

−1 0.05

qref 0

uy

0

Rd⊤ R − R⊤ Rd

Ψ(q, qd ) =

0.5

R R20 c Rdes

−10

15

(

1

20

ux

t [s]

1 2

0.1

0

−20

e :=

1

−1

q := vec R with R ∈ SO(3) , qd := vec Rd with Rd ∈ SO(3)

0.15

0

0

px

0

[

1 ⊤ z z 2

]

1.5

10

5

−5

q4 q ∈ S3 , qd := d4 ∈ S3 qv qdv



1

0 0

Rotation matrices

]

0

10

momenta [N m s]

20

1

0 −1

q :=

[

q⊤ dv qv + q4 qd4 z := qd4 qv − q× q − q4 qdv dv v

1 ⊤ e e 2

0 0

∈ R3

[ ]

1 sin ϕ tan θ cos ϕ tan θ cos ϕ − sin ϕ  r(q) := 0 sin ϕ cos ϕ 0 cos θ cos θ

Kinematics equation

0

Unit quaternions

e := q − qd

Energy function

−1

ϕd θd ψd

5 t [s]

10

0

0

5 t [s]

20 py pz

0

15

10

−1.5

0

5 t [s]

10

100 px

10

−10

t [s]

0

torques [N m]

q :=

[ ]Roll-Pitch-Yaw [ ]

momenta [N m s]

Current / desired attitude

0

5 t [s]

10

ux uy

0

−100

uz 0

5 t [s]

10

Fig. 2.

Fig. 1. Controller (33) on the attitude dynamics of the rigid body in SO(3). Plot 1, from the top: evolution of the entries of R (solid blue) and Rc (dashed blue) versus desired attitude Rd (cyan). Plot 2: evolution of the angular momenta p. Plot 3: evolution of the control inputs u.

highlighting that the pH framework is very suitable for tackling the rigid-body attitude control problem. Hopefully, this paper will serve as a starting point for further extension of techniques applying to space manipulators and flexible rigid-body systems. REFERENCES Baker, A. (2002). Matrix Groups, An Introduction to Lie Group Theory. Springer. Bullo, F. and Lewis, A.D. (2004). Geometric Control of Mechanical Systems, volume 49 of Texts in Applied Mathematics. Springer Verlag, New York-HeidelbergBerlin. Chaturvedi, N., Sanyal, A., and McClamroch, N. (2011). Rigid-body attitude control. Control Systems, IEEE, 31(3), 30–51. Dirksz, D.A., Scherpen, J.M.A., and Ortega, R. (2008). Interconnection and damping assignment passivity-based control for port-hamiltonian mechanical systems with only position measurements. In Decision and Control, 2008. CDC 2008. 47th IEEE Conference on, 4957–4962. Duindam, V., Macchelli, A., Stramigioli, S., and Bruyninckx, H. (2009). Modeling and control of complex phys169

Controller (33) on the attitude dynamics of the rigid body with RPY angles. Plot 1, from the top: evolution of the RPY angles q (solid blue) and qc (dashed blue) versus desired attitude Rd (cyan). Plot 2: evolution of the angular momenta p. Plot 3: evolution of the control inputs u.

ical systems. The port-Hamiltonian approach. SpringerVerlag, Berlin. Ortega, R., van der Schaft, A., Castanos, F., and Astolfi, A. (2008). Control by interconnection and standard passivity-based control of port-hamiltonian systems. Automatic Control, IEEE Transactions on, 53(11), 2527–2542. Ortega, R., Van der Schaft, A., Mareels, I., and Maschke, B. (2001). Putting energy back in control. Control Systems, IEEE, 21(2), 18–33. van der Schaft, A. and Jeltsema, D. (2014). Porthamiltonian systems theory: An introductory overview. R Foundations and Trends ⃝in Systems and Control, 1(23), 173–378.