IDA-PBC for a Class of Underactuated Mechanical ... - Semantic Scholar
52nd IEEE Conference on Decision and Control December 10-13, 2013. Florence, Italy
IDA-PBC for a Class of Underactuated Mechanical Systems with Application to a Rotary Inverted Pendulum Mutaz Ryalat and Dina Shona Laila Abstract— We develop a method to simplify the partial differential equations (PDEs) associated to the potential energy for interconnection and damping assignment passivity based control (IDA-PBC) of a class of underactuated mechanical systems. Solving the PDEs, also called the matching equations, is the main difficulty in the construction and application of the IDA-PBC. With the proposed method, a simplification to the potential energy PDE is achieved through a particular parametrization of the closed-loop inertia matrix that appears as a coupling term with the inverse of original inertia matrix. The results are applied to the Quanser rotary inverted pendulum and illustrated through numerical simulations. Index Terms— Hamiltonian systems, passivity-based control, rotary inverted pendulum, underactuated systems.
I. I NTRODUCTION Passivity-based control (PBC) is a control method to achieve stabilization of a system by passivation of the closedloop dynamics. The objective is to render the closed-loop system passive with a desired storage function, usually qualifies as a Lyapunov function that has a minimum at the desired equilibrium. Asymptotic stability is ensured if detectability of the passive output is satisfied [1]. There are two main approaches of PBC; 1) Classical PBC, where a storage function is first selected and then a controller is designed to render the storage function non-increasing [2] and 2) Interconnection and damping assignment passivitybased control (IDA-PBC), where the control action is split into two: energy shaping where the desired energy function to the passive map is assigned, and damping injection to ensure asymptotic stability [1]. IDA-PBC is applied to systems represented in a port-controlled Hamiltonian (PCH) structure. The success of the IDA-PBC relies on solving the matching equations, a set of nonlinear PDEs which are in general hard to solve. A number of works devoted to solving and simplifying the matching equations have appeared in recent literature. In [3] a method based on total energy shaping was proposed to reduce these PDEs into a set of ODEs for a subclass of underactuated systems. The so called λ−method first proposed in [4] has been used in [5] to transform these nonlinear PDEs into a set of quadratic and linear PDEs. In [6] explicit solution to the PDEs has been presented for mechanical systems with underactuation degree one. Control of underactuated mechanical systems has been an important topic that attracts the attention of control engineers and researchers due to their broad real-life applications in Mutaz Ryalat and Dina Shona Laila are with the School of Engineering Sciences, Faculty of Engineering and the Environment, University of Southampton, Highfield, Southampton SO17 1BJ, United Kingdom. {d.laila, mmr3g11}@soton.ac.uk
978-1-4673-5716-6/13/$31.00 ©2013 IEEE
robotics, marine and air vehicles as well as satellites, and as benchmark to study complex nonlinear control systems. These systems are characterized by the fact that they have fewer control inputs than the degrees of freedom to be controlled. While this feature imposes a challenging task to achieve the desired control objectives with a lower number of actuators, under-actuation control has the advantages of reducing the cost and complexity of the control system, and ensuring the workability in the case of actuation failure [7]. However, the fact that underactuated mechanical systems have complex internal dynamics and are not fully feedback linearizable complicates the control design, because the nonlinear control methods proposed for general mechanical systems can not be applied directly to this class of systems. Energy based control of underactuated mechanical systems has been an active area of research. In [8], a pasivity and energy based technique has been proposed for general underactuated mechanical systems by applying partial feedback linearization. PBC methods have been developed for various underactuated systems, such as pendulum on a cart [9], inertia wheel and ball on beam [10], Pendubot [11] and Acrobot [12]. Other nonlinear control designs have been reported in [7]. A constructive stabilization for a class of underactuated mechanical systems based on a newly developed Immersion and Invariance technique has been proposed in [13]. In this paper, we address the problem of solving the matching PDEs within the IDA-PBC design for a class of underactuated mechanical systems. In [14], the IDAPBC method is adopted, and a constructive solution for the kinetic energy PDE has been provided by reparametrization of some matrices involved, which enables transforming the PDEs to a set of ODEs. A further simplification to the non-homogeneous terms of the kinetic energy PDEs has been achieved by a change of coordinates that eliminates or simplify the forcing term. In this paper, a strategy to simplify and solve the potential energy PDE for a class of underactuated mechanical systems is developed. The key idea is to parametrize the desired inertia matrix, appearing as the coupling term with the inverse of the original inertia matrix, in a way that make these PDEs easier to solve. By assigning the desired inertia matrix, we are able to shape the potential energy function that has a local minimum at the desired equilibrium. This strategy expands the class of underactuated mechanical systems that can be treated. We apply our result to a stabilization, as well as the swing up, of a rotary inverted pendulum, or a Furuta pendulum, whose dynamics are relatively more complex than those of most other commonly studied benchmark systems [7].
5240
The main contributions of the paper are threefold: 1) A constructive method to solve the potential energy PDEs for mechanical systems with underactuation degree one. The motivation of this is because most position stabilization problems can be solved by shaping the potential energy function [6]. For underactuated mechanical systems, kinetic energy function also need to be shaped. We have assigned the inertia matrix that shapes the kinetic energy, and used it to simplify the potential energy PDEs, and solve them to shape the potential energy function, thus reshaping the total energy function. 2) While most works in literature use normalized, linearized, or partial feedback linearized model of the rotary inverted pendulum to simplify the problem, we have employed a full nonlinear model of the system. 3) This work is the first that yields a globally asymptotically stabilizing controller within the IDA-PBC framework, for the rotary inverted pendulum in its full nonlinear dynamics.
A. Definitions and notations The set of real and natural numbers (including 0) are denoted respectively by R and N. Given an arbitrary matrix G, we denote the transpose and the Moore-Penrose inverse of G by G⊤ and G+ , respectively. G⊥ denotes the full rank left annihilator of G, i.e. G⊥ G = 0. We denote an n × n identity matrix with In . For any continuous function H(i, j), we define ∇i H(i, j) := ∂H(i, j)/∂i. We use a standard stability and passivity definitions for nonlinear systems [15]. Due to limited space the arguments of functions are often dropped whenever they are clear from the context.
1 T −1 p Md (q)p + Vd (q), (5) 2
with Md = MdT > 0 the desired inertia matrix and Vd (q) the desired potential energy, such that Hd has an isolated minimum at the desired equilibrium point qe , i.e. qe = arg minHd (q) = arg minVd (q).
(6)
The following conditions are required so that (6) to hold: C2.1: ∇q Vd (0) = qe . C2.2: ∇2q Vd (0) > qe , i.e. the Hessian of the function at the equilibrium point is positive. The main challenge in solving (4) is to solve the following set of partial differential equations (PDEs): G⊥ {∇q H − Md M −1 ∇q Hd + J2 Md−1 p} = 0,
(7)
G⊥ {∇q (pT M −1 p) − Md M −1 ∇q (pT Md−1 p)
+ 2J2 Md−1 p} = 0
G⊥ {∇q V − Md M −1 ∇q Vd } = 0
(8) (9)
If this set of PDEs are solved, in the sense that Md , Vd and J2 are obtained, then ues is given by ues =(G⊤G)−1 G⊤ ∇q H−Md M −1 ∇q Hd +J2 Md−1 p (10) = G+ ∇q H − Md M −1 ∇q Hd + J2 Md−1 p . 2) Damping Injection: The task is to find the damping injection (dissipation) controller
B. Review on IDA-PBC design We briefly review the general procedure of the IDA-PBC design as has been proposed for instance in [1], [2]. Consider a PCH systems whose dynamics can be written as 0 0 In ∇q H q˙ u, (1) + = −In 0 ∇p H G(q) p˙
where q, p ∈ Rn are the states and u ∈ Rm , m ≤ n, is the control action. If m = n the system is called fully actuated, whereas if m < n it is called underactuated. The Hamiltonian function, which is the total energy of the system, is defined as the sum of the kinetic energy and the potential energy
1 T −1 p M (q)p + V (q), (2) 2 where M (q) > 0 is the symmetric inertia matrix and V (q) is the potential energy function. IDA-PBC consists of two parts, which correspond to its design steps; the energy shaping and the damping injection controller parts, i.e. H(q, p) = K(q, p) + V (q) =
(3)
1) Energy Shaping: The task is to find ues such that the closed-loop system is lossless. This is achieved by solving
Hd (q, p) = Kd (q, p) + Vd (q) =
where J2 = −J2T is a free parameter. PDEs (7) can be separated into two subsets; kinetic energy PDEs (depend on p) and potential energy PDEs (independent of p)
II. P RELIMINARIES
u = ues + udi .
The desired total energy in closed-loop is assigned to be
0 M −1 Md ∇q Hd 0 0 I n ∇q H (4) + ues= −In 0 ∇p H G(q) −Md M −1 J2 (q, p) ∇p Hd
udi = −Kv GT ∇p Hd ,
Kv > 0
(11)
to add the damping to the closed-loop system. udi is applied via a negative feedback of the passive output to achieve asymptotic stability, provided that the system is detectable. III. M AIN R ESULTS The main challenge, thus the success of the IDA-PBC design relies on solving the matching PDEs (8) and (9). Various constructive techniques have been proposed in literature, for instance in [3], [4], [6], [14] to deal with different subclasses of port-controlled Hamiltonian systems, imposing particular conditions to satisfy. In this section, we propose an alternative approach, focusing on solving the set of PDEs associated with the potential energy (9). In [6], it was shown that the potential energy PDEs can be explicitly solved, provided that the inertia matrix M and the force induced by the potential energy (on the unactuated subspaces) depend only on the actuated subspaces. This method is applicable only to a subclass of underactuated mechanical systems that satisfy the following conditions: C3.1: The inertia matrix M and the potential energy function V do not depend on the unactuated subspaces. C3.2: The system has underactuation degree one, i.e. m = n − 1.
5241
Violating Condition C3.1, the term G⊥ ∇q (pT M −1 p) in (8) will not be eliminated and hence does not simplify the process of solving the PDEs. Here, we propose a new procedure to relax Condition C3.1, with the implication to also extend the subset of underactuated systems that can be that can be treated with this method. In the sequel we will restrict our discussion to the systems that has two degrees of freedom, i.e n = 2, m = 1. This is motivated by the fact that the majority of classical problems in underactuated control (such as all examples in the introduction) share this property. The following condition identifies the class of port-controlled Hamiltonian systems that we consider in this paper: C3.1R: The inertia matrix M depends only on one subspace, not necessarily the actuated subspaces. This Condition C3.1R is a relaxation to Condition C3.1, in the sense that this method can be applied to both cases; 1) M depends only on the actuated subspaces, 2) M depends only on the unactuated subspaces. The latter, is the case which applies to our case study (also applied to cart pole system), hence we again restrict our discussion to this case. Without loss of generality, we assume that the unactuated coordinate is q1 and hence G = e2 , otherwise we may reorder the coordinates to come up with this structure. Clearly, one of the reasons for the difficulty in solving (9) arises from the complex structure and dependencies on q of the inertia matrix, thus its inverse. Recognizing that in this PDE we have a coupling term Md M −1 , we can simplify the PDE by choosing the structure of Md in a certain way that eliminates some terms as follows. Let the inertia matrices k1 (q1 ) k2 (q1 ) M (q) = (12) k2 (q1 ) k3 (q1 ) and m1 (q1 ) m2 (q1 ) m ¯ 1 (q1 ) m ¯ 2 (q1 ) Md (q)= =∆ (13) m2 (q1 ) m3 (q1 ) m ¯ 2 (q1 ) m ¯ 3 (q1 ) with ∆ = det(M ) = k1 (q1 )k3 (q1 ) − k22 (q1 ). Notice that M is a function of q1 only, hence, we can simply take Md as a function of q1 too. Suppose Conditions C3.2 and C3.1R hold, the potential energy PDEs (9) can then be written as ∇q1 V (q1 ) 1 0 0 k3 −k2 ∇q1 Vd m ¯ 1∆ m ¯ 2∆ ∆ ∆ = 0, (14) − k1 2 ∇q2 Vd m ¯ 2∆ m ¯ 3 ∆ −k ∆ ∆ which further gives (m ¯ 1 k3 − m ¯ 2 k2 )∇q1 Vd + (−m ¯ 1 k2 + m ¯ 2 k1 )∇q2 Vd = ∇q1 V (q). Choosing m ¯2 =
k2 ¯ 1, k1 m
IV. A PPLICATION : T HE ROTARY I NVERTED P ENDULUM (15)
then (15) is further simplified to
(m ¯ 1 k3 − m ¯ 2 k2 )∇q1 Vd = ∇q1 V, i.e. k22 ∇q1 Vd = ∇q1 V, m ¯ 1 k3 − k1
that can be solved either as ODE or PDE. However, we have solved it as PDE for two reasons: 1) to satisfy C2.1 and C2.2, and 2) to keep track on the coordinate q2 . The procedure can now be summarized in the following preposition. Proposition 3.1: Consider the underactuated portcontroller Hamiltonian system (1) satisfying Conditions C2.1 and C2.2. Let the inertia matrix M (q) and the parametrized desired inertia matrix Md take the form (12) and (13), respectively. Then the potential energy PDE (9) can be written in its simplest form (16) by choosing ¯ 1. m ¯ 2 = kk12 m Remark 3.1: Using Proposition 3.1, PDEs (8) and (9) are simplified and their general solutions depend on the dynamics of the underactuated mechanical system. Clearly, the inclusion of the determinant ∆ is essential in the parametrization of Md . It simplifies the PDE by canceling out the ∆ in the denominator of the each element of M −1 , thus avoids any singularity in the solution. Remark 3.2: The elimination of the second term on the left hand side of (15) by choosing m ¯ 2 = kk12 m ¯ 1 is critical to make the potential energy PDE as simple as possible. The parametrizing of the matrix Md to assign m ¯ i , i = 1, 2, 3 depends mainly on the dynamics of the system. However, the choice of m ¯ 1 and subsequently m ¯ 2, m ¯ 3 is not free: first, it should satisfy the condition of positive definiteness of Md m ¯ 22 (i.e. m ¯ 1 > 0, and m ¯3 > m ¯ 1 ), and then the solution of (16) should guarantee that Vd has an isolated minimum. Once all of the above is satisfied, then the free skew-symmetric matrix J2 (q, p) is brought into play. Since we have fixed the Md , the kinetic energy (8) is no longer nonlinear and nonhomogeneous PDE but becomes an algebraic equation in the unknown J2 for a given Md . Equation (16) represents a simplified PDE which can be applicable to a wide range of underactuated mechanical systems such as a pendulum on a cart, and rotary pendulum. In the next section, we will show the effectiveness of this method applied to a rotary pendulum system. Remark 3.3: If we apply the construction proposed in [6] to a pendulum on a cart, because Condition C3.1 is not satisfied, a partial feedback linearization is used to change coordinates so that M became constant, hence does not depend on the unactuated subspaces. Using Proposition 3.1, we can provide a solution directly without linearization. We will apply our results to a rotary inverted pendulum that has a similar (even more complicated) structure (in terms of M , V , and G) to a pendulum on a cart, as it is the hardware that is available in our laboratory.
(16)
In this section, we will apply the IDA-PBC design to solve the stabilization problem of a rotary inverted pendulum, which is also known as a Furuta pendulum. We will apply the technique proposed in Section III to solve the PDEs. We show that this technique reduces the design complexity, while at the same time preserve the effectiveness of the IDA-PBC design to stabilize the pendulum at its upright position.
5242
A. Model We use the Quanser rotary inverted pendulum module (SRV02+ROTPEN) as shown, together with the simplified free body diagram of its mechanical part, in Figure 1. This system consists of an inverted pendulum which is attached at the end of a motor-driven horizontally-rotating arm. The pendulum is also free to rotate in a vertical plane. Thus, the system has 2-DOF: the angular position of the arm (α) and the angular position of the pendulum (θ). This system is underactuated because only the arm is subjected to an input torque (applied by a DC motor), and hence the system is 2DOF with only one control input. The equations of motion of
and introduce the following shorten notations for the parameters (to simplify the model) 1 1 γ = Jp + mp L2p , ρ = Jr + mp L2r + mr L2r , 4 4 1 1 σ = mp Lp Lr , κ = mp gLp . 2 2 Applying Newton’s Second Law for rotational motion, and by ignoring the effect of friction, from (17) and (18) we can extract the inertia matrix γ −σ cos(q1 ) M (q) = , (20) −σ cos(q1 ) ρ + γ sin2 (q1 ) and also the potential energy of the system V (q1 ) = κ (1 + cos(q1 )) .
(21)
The Hamiltonian function (2) of the system is then be obtained, and the Hamilton model of the rotary pendulum, can be described by (1) with the torque as the control input, i.e. u = τ , and the component T G = e2 = 0 1 . (22) B. Controller Design
Fig. 1. Quanser rotary inverted pendulum and its free body diagram [16].
the system, can be derived from the standard Euler-Lagrange method as [16] 1 1 − mp Lp Lr cos(α) θ¨ − mp L2p cos(α) sin(α) θ˙2 2 4 1 1 ¨ − mp Lp g sin(α) = −Bp α˙ (17) + Jp + mp L2p α 4 2 1 1 2 2 2 2 Jr + mp Lr + mr Lr + (Jp + mp Lp ) sin (α) θ¨ 4 4 1 1 mp Lp Lr cos(α) α ¨+ mp Lp Lr sin(α) α˙ 2 − 2 2 1 ˙ + mp L2p cos(α) sin(α) θ˙α˙ = τ − Br θ, (18) 2 where τ is the input torque applied at the base of the rotary arm, which is generated by the servo motor. Other parameters along with their physical values are described in Table I. TABLE I T HE S PECIFICATIONS OF THE ROTARY PENDULUM Symbol Description Value Unit mp Mass of pendulum 0.127 kg Lp Total length of pendulum 0.337 m Jp Moment of inertia of pendulum 0.0012 kg.m2 mr Mass of arm 0.257 kg Lr Total length of arm 0.216 m Jr Moment of inertia of arm 0.0020 kg.m2
To apply IDA-PBC design, we need to obtain the portHamiltonian representation of the system. We define the generalized coordinate q to be q α q= 1 = , (19) q2 θ
We apply the procedure given in Section III to design the controller for the system. The main objective is to asymptotically stabilize the rotary inverted pendulum system at its unstable equilibrium point q = (0, q2 ) for any q2 ∈ [0, 2π]. First we design the energy shaping controller ues and then adding the damping to the closed-loop system by designing the damping injection controller udi . 1) Reshaping the total energy: We start with parametrizing the inertia matrix Md , then solve the PDE of the potential energy. From (20), we obtain the inverse inertia matrix 1 ρ + γ sin2 (q1 ) σ cos(q1 ) −1 , (23) M (q) = σ cos(q1 ) γ ∆ where ∆ = det(M ) = γρ + γ 2 sin2 (q1 ) − σ 2 cos2 (q1 ). Because M depends on q1 , it is clear that M −1 is a complicated matrix. Solving directly the PDEs (8) and (9) will require also tedious computations, that will lead to an unreasonable form of the controller. Applying Proposition 3.1, we fix Md (q) in the form of " # 1 )+ǫ) (cos(q1 ) + ǫ) −σ cos(q1 )(cos(q γ Md = ∆ −σ cos(q1 )(cos(q1 )+ǫ) , (24) m3 γ where ǫ > 1 is to satisfy the condition (m1 > 0 and thus, Md > 0 ∀ q1 ∈ [0, 2π]. With this choice of Md (q), and having G⊥ = [1 0], the PDE (9) becomes −κ sin(q1 ) 1 0 − 0 # " −σ cos(q1 )(cos(q1 )+ǫ) (cos(q1 ) + ǫ) γ ∆ −σ cos(q1 )(cos(q1 )+ǫ) m3 γ 2 1 ρ + γ sin (q1 ) σ cos(q1 ) ∇q1 Vd × = 0, ∇q2 Vd σ cos(q1 ) γ ∆
5243
which further gives κ sin(q1 ) σ 2 cos2 (q1 ) 2 ∇q1 Vd = − ρ + γ sin (q1 ) − . γ (cos(q1 ) + ǫ)
Solving this PDE produces the desired potential energy Vd (q) = λ1 − λ2 tanh−1 λ2 cos(q1 ) + ln cos(q1 ) + ǫ + Ψ x(q2 ) , (25) with λ1 =
κγ γρ+γ 2 −σ 2 ǫ2 −γ 2 ǫ2 ,
λ2 = √
γ 2 +σ 2 γ(ρ+γ)(γ 2 +σ 2 )
< 1.
Here, the function Ψ(·) is an arbitrary differentiable function that must be chosen to satisfy condition (6). This condition, along with the conditions (m1 > 0, m1 m3 > m22 ), is satisfied K by choosing Ψ(x(q2 )) = 2p q22 , where Kp > 0 is the gain of energy shaping controller. This imposes σ 2 cos2 (q1 ) cos(q1 ) + ǫ . (26) m3 > 2 γ Remark 4.1: For this particular design, m1 is fixed as m1 = cos(q1 ) + ǫ. The term cos(q1 ) ensures that Vd is minimum at qe , and ǫ is added to guarantee the positivity of Md in the whole domain of attraction (DoA). Now, using Proposition 3.1, we have established the existence of a solution for the potential energy PDE. It remains to verify the existence of solution(s) to the kinetic energy PDEs (8). Solving (8) is essential for the completion of kinetic energy shaping, as well as, to find J2 (q, p), which contributes for the the last term on the right hand side of (10). Since we have fixed Md , the kinetic energy (8) becomes an algebraic equation and can be written as G⊥ 2J2 Md−1 p = G⊥ {Md M −1 ∇q Hd − ∇q H}.
sin(q1 ) cos(q1 ) ǫ + cos(q1 ) × B3 = − ∆d m3 γ 2 − ǫσ 2 cos2 (q1 ) − σ 2 cos3 (q1 ) 2(γ 2 + σ 2 ) m3 γ 2 − ǫσ 2 cos2 (q1 ) − σ 2 cos3 (q1 ) + σ 2 ∆ 2ǫ + 3 cos(q1 )
2 2 ϕ = ǫ + cos(q1 ) ρ + γ sin2 (q1 ) − σ cosγ (q1 ) , and the determinant ofMd 2 2 2 2 . m γ − σ cos (q ) ǫ + cos(q ) ǫ + cos(q ) ∆d = ∆ 2 3 1 1 1 γ We can now substitute all the terms into (10) to obtain the energy shaping controller σ cos(q1 ) γm3 − ρ + γ sin2 (q1 ) ǫ + cos(q1 ) ues = − γ 2 ǫλ1 λ2 sin(q1 ) λ1 sin(q1 ) B1 2 − + p + B 2 p1 p2 1 − λ22 cos2 (q1 ) ǫ + cos(q1 ) 2 1 σ 2 cos2 (q1 ) ǫ + cos(q1) B3 2 p − γm3 − + Kp q 2 2 2 γ ∆ γm3 p1 + σ cos(q1 ) ǫ + cos(q1 ) p2 . (30) − j2 γ∆d 2) Damping assignment: The damping injection controller follows the construction (11). Given Md that has been obtained when reshaping the total energy, we have
∇p Hd = Md−1 p " # σ cos(q1 )(ǫ+cos(q1 )) m3 ∆ p1 γ = 1 )) p2 ∆d σ cos(q1 )(ǫ+cos(q ǫ + cos(q ) 1 γ # " σ cos(q1 )(ǫ+cos(q1 )) m3 p 1 + p2 ∆ γ (31) = 1 )) ∆d σ cos(q1 )(ǫ+cos(q p + (ǫ + cos(q 1 1 ))p2 γ
(27)
After lengthy but straightforward calculations, J2 is evaluated as 0 j2 , (28) J2 = −j2 0 where,
Kj γ∆d × j2 = 2∆ (ǫ + cos(q1 )) (σ cos(q1 )p1 + γp2 ) (29) 2 2 (ϕB1−A1 )p1+2(ϕB2 − A2 )p1 p2+(ϕB3−A3 )p2
with 1) γ∆ − ρ + γ sin2 (q1 ) (γ 2 + σ 2 ) , A1 = sin(2q ∆2 1) A2 = − σ sin(q ∆ + 2 cos2 (q1 )(γ 2 + σ 2 ) , 2 ∆ 2 2 A3 = − γ(γ +σ∆)2sin(2q1 ) , m3 sin(q1 ) − 2γ 2 ∆d cos(q1 )(γ 2 + σ 2 ) B1 = (γ∆d )2 + ∆3 m3 γ 2 − ǫσ 2 cos2 (q1 ) − σ 2 cos3 (q1 ) − σ 2 ∆3 cos(q1 ) ǫ + cos(q1 ) 2ǫ + 3 cos(q1 ) , σ sin(q1 ) ǫ + cos(q1 ) × B2 = − γ∆d m3 γ 2 − ǫσ 2 cos2 (q1 ) − σ 2 cos3 (q1 ) σ 2 ∆ cos2 (q1 ) 2ǫ + 3 cos(q1 ) + 2 cos2 (q1 )(γ 2 + σ 2 ) + ∆ m3 γ 2 − ǫσ 2 cos2 (q1 ) − σ 2 cos3 (q1 ) ,
Substituting all the terms into (11), the damping injection controller is then Kv ∆ ǫ + cos(q1 ) udi = − σ cos(q1 )p1 + γp2 . (32) γ∆d
By choosing the right m3 that satisfies the condition of 2 2 (q1 ) , for instance m3 := ǫ + m3 > ǫ + cos(q1 ) σ cos γ2 σ2 cos2 (q1 ) cos(q1 ) +µ with a constant µ > 0 the dependence γ2 on q1 at the denominator of udi can easily be avoided. Moreover, from the positive definiteness of the inertia matrix M , its determinant ∆ is never be zero. We can conclude the IDA-PBC design for the rotary inverted pendulum by stating the following proposition. Proposition 4.1: The state feedback controller (30) and 2 2 (q1 ) (32), with m3 > ǫ + cos(q1 ) σ cos , Kp , Kv , Kj > γ2 0 and ǫ > 1 is an asymptotically stabilizing controller for the rotary pendulum inverted system (17)-(18) at its unstable equilibrium point q = (0, q2 ) for any q2 ∈ [0, 2π]. The proof of Proposition 4.1 can be established by verifying that Vd satisfies the Conditions (C2.1-C2.2:), and Md is positive and symmetric, thus, Hd qualifies as a Lyapunov function. Furthermore, by invoking LaSalle’s invariance principle asymptotic stability can be proven. (The proof is omitted due to the limited space.)
5244
V. S IMULATION R ESULTS
0
0
q2 (rad)
0.5
q1 (rad)
0.5
−0.5
−1
−0.5
0
0.5
1
1.5
−1
5 0
0.5
0
0.5
1
1.5
1
1.5
0
−10
−5
−10
0
10
Hd (J)
Torque (N.m.)
10
0
0.5
1 t (sec)
1.5
−20
t (sec)
Fig. 2. Simulation result for the rotary inverted pendulum with the initial condition [q, p] = [− π4 , −0.8, 0.033, 0.032].
extreme case, Figure 3 shows the simulation result obtained with the initial condition [q, p] = [ π2 , 0.5, −0.074, −0.001] and the controller parameters Kp = 0.37, Kv = 0.029, Kj = 1 × 10−11 , m3 = 263. q10 = π2 rad is chosen to show the advantage of the controller (30), (32) as it provides a much larger DoA compared to a linear controller (see [16]). This angular position π2 (as well as − π2 ), which is the horizontal position of the pendulum, is also a critical point where the pendulum losses its local controllability [17]. It is shown that, by allowing a small initial angular velocity on both the pendulum and the arm, the pendulum can still be stabilized at its upright position with a reasonable effort. VI. C ONCLUSION AND FUTURE WORKS We have proposed a method to simplify the matching PDEs for solving an IDA-PBC construction. This has significantly simplified the design computation and also yield a simpler form of the controller. The result has been applied to solve a global stabilization design of a rotary inverted pendulum. It has been shown by simulations that this design results in a very high closed-loop performance of the pendulum in its full nonlinear dynamics. The next obvious step is to implement the controller to the hardware to validate the design approach in a real experiment. As there are only two
1
0
−1
0
0.5
1
1.5
2
2.5
3
0.5
1
1.5 t (sec)
2
2.5
3
1 0 q2 (rad)
We present some simulation results obtained on the Quanser rotary pendulum shown in Figure (1), by applying the IDA-PBC controller (3) which consists of (30) and (32). The parameters of the rotary pendulum are listed in Table I. Throughout the simulation, we use g = 9.8 ms−2 . In the first simulation shown in Figure 2, we choose the initial condition [q, p] = [− π4 , −0.8, 0.033, 0.032] and the controller parameters Kp = 0.01, Kv = 0.04, Kj = 1 × 10−6 , m3 = 100. This initial state is more than twice the initial angular position of the pendulum that is recommended for the balancing experiment of the Quanser rotary inverted pendulum module (SRV02+ROTPEN) hardware using a linear state feedback controller obtained via a pole placement design. As expected, it is observed that the pendulum can easily be stabilized at its upright position using the controller (30), (32) with very little effort as shown by the low value of the control torque. To illustrate a more
q1 (rad)
2
−1 −2 −3
0
Fig. 3. Simulation result for the rotary inverted pendulum with the initial condition [q, p] = [ π2 , 0.5, −0.074, −0.001].
states, the pendulum angle and the arm angle, measured, an observer will be designed to estimate the momenta. R EFERENCES [1] R. Ortega, A. van der Schaft, B. Maschke, and G. Escobar, “Interconnection and damping assignment passivity-based control of portcontrolled Hamiltonian systems,” Automatica, vol. 38, pp. 585–596, 2002. [2] R. Ortega and E. Garcia-Canseco, “Interconnection and damping assignment passivity-based control: A survey,” European Journal of Control, vol. 10, pp. 432–450, 2004. [3] F. Gomez-Estern, R. Ortega, F. Rubio, and J. Aracil, “Stabilization of a class of underactuated mechanical systems via total energy shaping,” IEEE Conf on Decision and Control, vol. 41, pp. 1372–1388, 2001. [4] D. Auckly and L. Kapitanski, “On the λ−equations for matching control laws,” SIAM J. Contr. & Optim., vol. 41, pp. 1372–1388, 2002. [5] G. Blankenstein, R. Ortega, and A. van der Schaft, “The matching conditions of controlled Lagrangians and IDA-passivity based control,” International Journal of Control, vol. 75, pp. 645–665, 2002. [6] J. A. Acosta, R. Ortega, A. Astolfi, and A. D. Mahindrakar, “Interconnection and damping assignment passivity based control of mechanical systems with underactuation degree one,” IEEE Trans. on Automatic Control, vol. 50, pp. 1936–1955, 2005. [7] R. Olfati-Saber, “Nonlinear control of underactuated mechanical systems with application to robotics and aerospace vehicles,” Ph.D. dissertation, Massachusetts Institute of Technology, Department of Electrical Engineering and Computer Science, 2001. [8] M. Spong, “Energy based control of a class of underactuated mechanical systems,” IFAC World Congress, pp. 431–435, 1996. [9] J. van der Burg, R. Ortega, J. Scherpen, J. Acosta, and H. Siguerdidjane, “An experimental application of total energy shaping control: Stabilization of the inverted pendulum on a cart in the presence of friction,” European Control Conference, 2007. [10] R. Ortega, M. Spong, F. Gomez-Estern, and G. Blankenstein, “Stabilization of a class of underactuated mechanical systems via interconnection and damping assignment,” Automatic Control, IEEE Transactions on, vol. 47, pp. 1218 – 1233, 2002. [11] J. Sandova, R. Ortega, and R. Kelly, “Interconnection and damping assignment passivity–based control of the pendubot,” IFAC World Congress, pp. 7700–7704, 2008. [12] A. Mahindrakar, A. Astolfi, R. Ortega, and G. Viola, “Further constructive results on interconnection and damping assignment control of mechanical systems: the acrobot example,” Int. J. Robust Nonlinear Control, vol. 16, pp. 671–685, 2006. [13] I. Sarras, A. Acosta, R. Ortega, and A. Mahindrakar, “Constructive immersion and invariance stabilization for a class of underactuated mechanical systems,” 8th IFAC Sympossium on Nonlinear Control Systems, pp. 108 – 113, 2010. [14] G. Viola, R. Ortega, R. Banavar, J. A. Acosta, and A. Astolfi, “Total energy shaping control of mechanical systems: Simplifying the matching equations via coordinate changes,” IEEE Trans. Autom. Control, vol. 52, pp. 1093–1099, 2007. [15] H. Khalil, Nonlinear Systems, 2nd Ed. Prentice Hall, 1996. [16] Q. Inc., “Quanser rotary pendulum user manual and workbook,” 2011. [17] K. Astr¨om and K. Furuta, “Swing up a pendulum by energy control,” Automatica, vol. 36, pp. 287–295, 2000.
5245
IDA-PBC for a Class of Underactuated Mechanical Systems with Application to a Rotary Inverted Pendulum Mutaz Ryalat and Dina Shona Laila Abstract— We develop a method to simplify the partial differential equations (PDEs) associated to the potential energy for interconnection and damping assignment passivity based control (IDA-PBC) of a class of underactuated mechanical systems. Solving the PDEs, also called the matching equations, is the main difficulty in the construction and application of the IDA-PBC. With the proposed method, a simplification to the potential energy PDE is achieved through a particular parametrization of the closed-loop inertia matrix that appears as a coupling term with the inverse of original inertia matrix. The results are applied to the Quanser rotary inverted pendulum and illustrated through numerical simulations. Index Terms— Hamiltonian systems, passivity-based control, rotary inverted pendulum, underactuated systems.
I. I NTRODUCTION Passivity-based control (PBC) is a control method to achieve stabilization of a system by passivation of the closedloop dynamics. The objective is to render the closed-loop system passive with a desired storage function, usually qualifies as a Lyapunov function that has a minimum at the desired equilibrium. Asymptotic stability is ensured if detectability of the passive output is satisfied [1]. There are two main approaches of PBC; 1) Classical PBC, where a storage function is first selected and then a controller is designed to render the storage function non-increasing [2] and 2) Interconnection and damping assignment passivitybased control (IDA-PBC), where the control action is split into two: energy shaping where the desired energy function to the passive map is assigned, and damping injection to ensure asymptotic stability [1]. IDA-PBC is applied to systems represented in a port-controlled Hamiltonian (PCH) structure. The success of the IDA-PBC relies on solving the matching equations, a set of nonlinear PDEs which are in general hard to solve. A number of works devoted to solving and simplifying the matching equations have appeared in recent literature. In [3] a method based on total energy shaping was proposed to reduce these PDEs into a set of ODEs for a subclass of underactuated systems. The so called λ−method first proposed in [4] has been used in [5] to transform these nonlinear PDEs into a set of quadratic and linear PDEs. In [6] explicit solution to the PDEs has been presented for mechanical systems with underactuation degree one. Control of underactuated mechanical systems has been an important topic that attracts the attention of control engineers and researchers due to their broad real-life applications in Mutaz Ryalat and Dina Shona Laila are with the School of Engineering Sciences, Faculty of Engineering and the Environment, University of Southampton, Highfield, Southampton SO17 1BJ, United Kingdom. {d.laila, mmr3g11}@soton.ac.uk
978-1-4673-5716-6/13/$31.00 ©2013 IEEE
robotics, marine and air vehicles as well as satellites, and as benchmark to study complex nonlinear control systems. These systems are characterized by the fact that they have fewer control inputs than the degrees of freedom to be controlled. While this feature imposes a challenging task to achieve the desired control objectives with a lower number of actuators, under-actuation control has the advantages of reducing the cost and complexity of the control system, and ensuring the workability in the case of actuation failure [7]. However, the fact that underactuated mechanical systems have complex internal dynamics and are not fully feedback linearizable complicates the control design, because the nonlinear control methods proposed for general mechanical systems can not be applied directly to this class of systems. Energy based control of underactuated mechanical systems has been an active area of research. In [8], a pasivity and energy based technique has been proposed for general underactuated mechanical systems by applying partial feedback linearization. PBC methods have been developed for various underactuated systems, such as pendulum on a cart [9], inertia wheel and ball on beam [10], Pendubot [11] and Acrobot [12]. Other nonlinear control designs have been reported in [7]. A constructive stabilization for a class of underactuated mechanical systems based on a newly developed Immersion and Invariance technique has been proposed in [13]. In this paper, we address the problem of solving the matching PDEs within the IDA-PBC design for a class of underactuated mechanical systems. In [14], the IDAPBC method is adopted, and a constructive solution for the kinetic energy PDE has been provided by reparametrization of some matrices involved, which enables transforming the PDEs to a set of ODEs. A further simplification to the non-homogeneous terms of the kinetic energy PDEs has been achieved by a change of coordinates that eliminates or simplify the forcing term. In this paper, a strategy to simplify and solve the potential energy PDE for a class of underactuated mechanical systems is developed. The key idea is to parametrize the desired inertia matrix, appearing as the coupling term with the inverse of the original inertia matrix, in a way that make these PDEs easier to solve. By assigning the desired inertia matrix, we are able to shape the potential energy function that has a local minimum at the desired equilibrium. This strategy expands the class of underactuated mechanical systems that can be treated. We apply our result to a stabilization, as well as the swing up, of a rotary inverted pendulum, or a Furuta pendulum, whose dynamics are relatively more complex than those of most other commonly studied benchmark systems [7].
5240
The main contributions of the paper are threefold: 1) A constructive method to solve the potential energy PDEs for mechanical systems with underactuation degree one. The motivation of this is because most position stabilization problems can be solved by shaping the potential energy function [6]. For underactuated mechanical systems, kinetic energy function also need to be shaped. We have assigned the inertia matrix that shapes the kinetic energy, and used it to simplify the potential energy PDEs, and solve them to shape the potential energy function, thus reshaping the total energy function. 2) While most works in literature use normalized, linearized, or partial feedback linearized model of the rotary inverted pendulum to simplify the problem, we have employed a full nonlinear model of the system. 3) This work is the first that yields a globally asymptotically stabilizing controller within the IDA-PBC framework, for the rotary inverted pendulum in its full nonlinear dynamics.
A. Definitions and notations The set of real and natural numbers (including 0) are denoted respectively by R and N. Given an arbitrary matrix G, we denote the transpose and the Moore-Penrose inverse of G by G⊤ and G+ , respectively. G⊥ denotes the full rank left annihilator of G, i.e. G⊥ G = 0. We denote an n × n identity matrix with In . For any continuous function H(i, j), we define ∇i H(i, j) := ∂H(i, j)/∂i. We use a standard stability and passivity definitions for nonlinear systems [15]. Due to limited space the arguments of functions are often dropped whenever they are clear from the context.
1 T −1 p Md (q)p + Vd (q), (5) 2
with Md = MdT > 0 the desired inertia matrix and Vd (q) the desired potential energy, such that Hd has an isolated minimum at the desired equilibrium point qe , i.e. qe = arg minHd (q) = arg minVd (q).
(6)
The following conditions are required so that (6) to hold: C2.1: ∇q Vd (0) = qe . C2.2: ∇2q Vd (0) > qe , i.e. the Hessian of the function at the equilibrium point is positive. The main challenge in solving (4) is to solve the following set of partial differential equations (PDEs): G⊥ {∇q H − Md M −1 ∇q Hd + J2 Md−1 p} = 0,
(7)
G⊥ {∇q (pT M −1 p) − Md M −1 ∇q (pT Md−1 p)
+ 2J2 Md−1 p} = 0
G⊥ {∇q V − Md M −1 ∇q Vd } = 0
(8) (9)
If this set of PDEs are solved, in the sense that Md , Vd and J2 are obtained, then ues is given by ues =(G⊤G)−1 G⊤ ∇q H−Md M −1 ∇q Hd +J2 Md−1 p (10) = G+ ∇q H − Md M −1 ∇q Hd + J2 Md−1 p . 2) Damping Injection: The task is to find the damping injection (dissipation) controller
B. Review on IDA-PBC design We briefly review the general procedure of the IDA-PBC design as has been proposed for instance in [1], [2]. Consider a PCH systems whose dynamics can be written as 0 0 In ∇q H q˙ u, (1) + = −In 0 ∇p H G(q) p˙
where q, p ∈ Rn are the states and u ∈ Rm , m ≤ n, is the control action. If m = n the system is called fully actuated, whereas if m < n it is called underactuated. The Hamiltonian function, which is the total energy of the system, is defined as the sum of the kinetic energy and the potential energy
1 T −1 p M (q)p + V (q), (2) 2 where M (q) > 0 is the symmetric inertia matrix and V (q) is the potential energy function. IDA-PBC consists of two parts, which correspond to its design steps; the energy shaping and the damping injection controller parts, i.e. H(q, p) = K(q, p) + V (q) =
(3)
1) Energy Shaping: The task is to find ues such that the closed-loop system is lossless. This is achieved by solving
Hd (q, p) = Kd (q, p) + Vd (q) =
where J2 = −J2T is a free parameter. PDEs (7) can be separated into two subsets; kinetic energy PDEs (depend on p) and potential energy PDEs (independent of p)
II. P RELIMINARIES
u = ues + udi .
The desired total energy in closed-loop is assigned to be
0 M −1 Md ∇q Hd 0 0 I n ∇q H (4) + ues= −In 0 ∇p H G(q) −Md M −1 J2 (q, p) ∇p Hd
udi = −Kv GT ∇p Hd ,
Kv > 0
(11)
to add the damping to the closed-loop system. udi is applied via a negative feedback of the passive output to achieve asymptotic stability, provided that the system is detectable. III. M AIN R ESULTS The main challenge, thus the success of the IDA-PBC design relies on solving the matching PDEs (8) and (9). Various constructive techniques have been proposed in literature, for instance in [3], [4], [6], [14] to deal with different subclasses of port-controlled Hamiltonian systems, imposing particular conditions to satisfy. In this section, we propose an alternative approach, focusing on solving the set of PDEs associated with the potential energy (9). In [6], it was shown that the potential energy PDEs can be explicitly solved, provided that the inertia matrix M and the force induced by the potential energy (on the unactuated subspaces) depend only on the actuated subspaces. This method is applicable only to a subclass of underactuated mechanical systems that satisfy the following conditions: C3.1: The inertia matrix M and the potential energy function V do not depend on the unactuated subspaces. C3.2: The system has underactuation degree one, i.e. m = n − 1.
5241
Violating Condition C3.1, the term G⊥ ∇q (pT M −1 p) in (8) will not be eliminated and hence does not simplify the process of solving the PDEs. Here, we propose a new procedure to relax Condition C3.1, with the implication to also extend the subset of underactuated systems that can be that can be treated with this method. In the sequel we will restrict our discussion to the systems that has two degrees of freedom, i.e n = 2, m = 1. This is motivated by the fact that the majority of classical problems in underactuated control (such as all examples in the introduction) share this property. The following condition identifies the class of port-controlled Hamiltonian systems that we consider in this paper: C3.1R: The inertia matrix M depends only on one subspace, not necessarily the actuated subspaces. This Condition C3.1R is a relaxation to Condition C3.1, in the sense that this method can be applied to both cases; 1) M depends only on the actuated subspaces, 2) M depends only on the unactuated subspaces. The latter, is the case which applies to our case study (also applied to cart pole system), hence we again restrict our discussion to this case. Without loss of generality, we assume that the unactuated coordinate is q1 and hence G = e2 , otherwise we may reorder the coordinates to come up with this structure. Clearly, one of the reasons for the difficulty in solving (9) arises from the complex structure and dependencies on q of the inertia matrix, thus its inverse. Recognizing that in this PDE we have a coupling term Md M −1 , we can simplify the PDE by choosing the structure of Md in a certain way that eliminates some terms as follows. Let the inertia matrices k1 (q1 ) k2 (q1 ) M (q) = (12) k2 (q1 ) k3 (q1 ) and m1 (q1 ) m2 (q1 ) m ¯ 1 (q1 ) m ¯ 2 (q1 ) Md (q)= =∆ (13) m2 (q1 ) m3 (q1 ) m ¯ 2 (q1 ) m ¯ 3 (q1 ) with ∆ = det(M ) = k1 (q1 )k3 (q1 ) − k22 (q1 ). Notice that M is a function of q1 only, hence, we can simply take Md as a function of q1 too. Suppose Conditions C3.2 and C3.1R hold, the potential energy PDEs (9) can then be written as ∇q1 V (q1 ) 1 0 0 k3 −k2 ∇q1 Vd m ¯ 1∆ m ¯ 2∆ ∆ ∆ = 0, (14) − k1 2 ∇q2 Vd m ¯ 2∆ m ¯ 3 ∆ −k ∆ ∆ which further gives (m ¯ 1 k3 − m ¯ 2 k2 )∇q1 Vd + (−m ¯ 1 k2 + m ¯ 2 k1 )∇q2 Vd = ∇q1 V (q). Choosing m ¯2 =
k2 ¯ 1, k1 m
IV. A PPLICATION : T HE ROTARY I NVERTED P ENDULUM (15)
then (15) is further simplified to
(m ¯ 1 k3 − m ¯ 2 k2 )∇q1 Vd = ∇q1 V, i.e. k22 ∇q1 Vd = ∇q1 V, m ¯ 1 k3 − k1
that can be solved either as ODE or PDE. However, we have solved it as PDE for two reasons: 1) to satisfy C2.1 and C2.2, and 2) to keep track on the coordinate q2 . The procedure can now be summarized in the following preposition. Proposition 3.1: Consider the underactuated portcontroller Hamiltonian system (1) satisfying Conditions C2.1 and C2.2. Let the inertia matrix M (q) and the parametrized desired inertia matrix Md take the form (12) and (13), respectively. Then the potential energy PDE (9) can be written in its simplest form (16) by choosing ¯ 1. m ¯ 2 = kk12 m Remark 3.1: Using Proposition 3.1, PDEs (8) and (9) are simplified and their general solutions depend on the dynamics of the underactuated mechanical system. Clearly, the inclusion of the determinant ∆ is essential in the parametrization of Md . It simplifies the PDE by canceling out the ∆ in the denominator of the each element of M −1 , thus avoids any singularity in the solution. Remark 3.2: The elimination of the second term on the left hand side of (15) by choosing m ¯ 2 = kk12 m ¯ 1 is critical to make the potential energy PDE as simple as possible. The parametrizing of the matrix Md to assign m ¯ i , i = 1, 2, 3 depends mainly on the dynamics of the system. However, the choice of m ¯ 1 and subsequently m ¯ 2, m ¯ 3 is not free: first, it should satisfy the condition of positive definiteness of Md m ¯ 22 (i.e. m ¯ 1 > 0, and m ¯3 > m ¯ 1 ), and then the solution of (16) should guarantee that Vd has an isolated minimum. Once all of the above is satisfied, then the free skew-symmetric matrix J2 (q, p) is brought into play. Since we have fixed the Md , the kinetic energy (8) is no longer nonlinear and nonhomogeneous PDE but becomes an algebraic equation in the unknown J2 for a given Md . Equation (16) represents a simplified PDE which can be applicable to a wide range of underactuated mechanical systems such as a pendulum on a cart, and rotary pendulum. In the next section, we will show the effectiveness of this method applied to a rotary pendulum system. Remark 3.3: If we apply the construction proposed in [6] to a pendulum on a cart, because Condition C3.1 is not satisfied, a partial feedback linearization is used to change coordinates so that M became constant, hence does not depend on the unactuated subspaces. Using Proposition 3.1, we can provide a solution directly without linearization. We will apply our results to a rotary inverted pendulum that has a similar (even more complicated) structure (in terms of M , V , and G) to a pendulum on a cart, as it is the hardware that is available in our laboratory.
(16)
In this section, we will apply the IDA-PBC design to solve the stabilization problem of a rotary inverted pendulum, which is also known as a Furuta pendulum. We will apply the technique proposed in Section III to solve the PDEs. We show that this technique reduces the design complexity, while at the same time preserve the effectiveness of the IDA-PBC design to stabilize the pendulum at its upright position.
5242
A. Model We use the Quanser rotary inverted pendulum module (SRV02+ROTPEN) as shown, together with the simplified free body diagram of its mechanical part, in Figure 1. This system consists of an inverted pendulum which is attached at the end of a motor-driven horizontally-rotating arm. The pendulum is also free to rotate in a vertical plane. Thus, the system has 2-DOF: the angular position of the arm (α) and the angular position of the pendulum (θ). This system is underactuated because only the arm is subjected to an input torque (applied by a DC motor), and hence the system is 2DOF with only one control input. The equations of motion of
and introduce the following shorten notations for the parameters (to simplify the model) 1 1 γ = Jp + mp L2p , ρ = Jr + mp L2r + mr L2r , 4 4 1 1 σ = mp Lp Lr , κ = mp gLp . 2 2 Applying Newton’s Second Law for rotational motion, and by ignoring the effect of friction, from (17) and (18) we can extract the inertia matrix γ −σ cos(q1 ) M (q) = , (20) −σ cos(q1 ) ρ + γ sin2 (q1 ) and also the potential energy of the system V (q1 ) = κ (1 + cos(q1 )) .
(21)
The Hamiltonian function (2) of the system is then be obtained, and the Hamilton model of the rotary pendulum, can be described by (1) with the torque as the control input, i.e. u = τ , and the component T G = e2 = 0 1 . (22) B. Controller Design
Fig. 1. Quanser rotary inverted pendulum and its free body diagram [16].
the system, can be derived from the standard Euler-Lagrange method as [16] 1 1 − mp Lp Lr cos(α) θ¨ − mp L2p cos(α) sin(α) θ˙2 2 4 1 1 ¨ − mp Lp g sin(α) = −Bp α˙ (17) + Jp + mp L2p α 4 2 1 1 2 2 2 2 Jr + mp Lr + mr Lr + (Jp + mp Lp ) sin (α) θ¨ 4 4 1 1 mp Lp Lr cos(α) α ¨+ mp Lp Lr sin(α) α˙ 2 − 2 2 1 ˙ + mp L2p cos(α) sin(α) θ˙α˙ = τ − Br θ, (18) 2 where τ is the input torque applied at the base of the rotary arm, which is generated by the servo motor. Other parameters along with their physical values are described in Table I. TABLE I T HE S PECIFICATIONS OF THE ROTARY PENDULUM Symbol Description Value Unit mp Mass of pendulum 0.127 kg Lp Total length of pendulum 0.337 m Jp Moment of inertia of pendulum 0.0012 kg.m2 mr Mass of arm 0.257 kg Lr Total length of arm 0.216 m Jr Moment of inertia of arm 0.0020 kg.m2
To apply IDA-PBC design, we need to obtain the portHamiltonian representation of the system. We define the generalized coordinate q to be q α q= 1 = , (19) q2 θ
We apply the procedure given in Section III to design the controller for the system. The main objective is to asymptotically stabilize the rotary inverted pendulum system at its unstable equilibrium point q = (0, q2 ) for any q2 ∈ [0, 2π]. First we design the energy shaping controller ues and then adding the damping to the closed-loop system by designing the damping injection controller udi . 1) Reshaping the total energy: We start with parametrizing the inertia matrix Md , then solve the PDE of the potential energy. From (20), we obtain the inverse inertia matrix 1 ρ + γ sin2 (q1 ) σ cos(q1 ) −1 , (23) M (q) = σ cos(q1 ) γ ∆ where ∆ = det(M ) = γρ + γ 2 sin2 (q1 ) − σ 2 cos2 (q1 ). Because M depends on q1 , it is clear that M −1 is a complicated matrix. Solving directly the PDEs (8) and (9) will require also tedious computations, that will lead to an unreasonable form of the controller. Applying Proposition 3.1, we fix Md (q) in the form of " # 1 )+ǫ) (cos(q1 ) + ǫ) −σ cos(q1 )(cos(q γ Md = ∆ −σ cos(q1 )(cos(q1 )+ǫ) , (24) m3 γ where ǫ > 1 is to satisfy the condition (m1 > 0 and thus, Md > 0 ∀ q1 ∈ [0, 2π]. With this choice of Md (q), and having G⊥ = [1 0], the PDE (9) becomes −κ sin(q1 ) 1 0 − 0 # " −σ cos(q1 )(cos(q1 )+ǫ) (cos(q1 ) + ǫ) γ ∆ −σ cos(q1 )(cos(q1 )+ǫ) m3 γ 2 1 ρ + γ sin (q1 ) σ cos(q1 ) ∇q1 Vd × = 0, ∇q2 Vd σ cos(q1 ) γ ∆
5243
which further gives κ sin(q1 ) σ 2 cos2 (q1 ) 2 ∇q1 Vd = − ρ + γ sin (q1 ) − . γ (cos(q1 ) + ǫ)
Solving this PDE produces the desired potential energy Vd (q) = λ1 − λ2 tanh−1 λ2 cos(q1 ) + ln cos(q1 ) + ǫ + Ψ x(q2 ) , (25) with λ1 =
κγ γρ+γ 2 −σ 2 ǫ2 −γ 2 ǫ2 ,
λ2 = √
γ 2 +σ 2 γ(ρ+γ)(γ 2 +σ 2 )
< 1.
Here, the function Ψ(·) is an arbitrary differentiable function that must be chosen to satisfy condition (6). This condition, along with the conditions (m1 > 0, m1 m3 > m22 ), is satisfied K by choosing Ψ(x(q2 )) = 2p q22 , where Kp > 0 is the gain of energy shaping controller. This imposes σ 2 cos2 (q1 ) cos(q1 ) + ǫ . (26) m3 > 2 γ Remark 4.1: For this particular design, m1 is fixed as m1 = cos(q1 ) + ǫ. The term cos(q1 ) ensures that Vd is minimum at qe , and ǫ is added to guarantee the positivity of Md in the whole domain of attraction (DoA). Now, using Proposition 3.1, we have established the existence of a solution for the potential energy PDE. It remains to verify the existence of solution(s) to the kinetic energy PDEs (8). Solving (8) is essential for the completion of kinetic energy shaping, as well as, to find J2 (q, p), which contributes for the the last term on the right hand side of (10). Since we have fixed Md , the kinetic energy (8) becomes an algebraic equation and can be written as G⊥ 2J2 Md−1 p = G⊥ {Md M −1 ∇q Hd − ∇q H}.
sin(q1 ) cos(q1 ) ǫ + cos(q1 ) × B3 = − ∆d m3 γ 2 − ǫσ 2 cos2 (q1 ) − σ 2 cos3 (q1 ) 2(γ 2 + σ 2 ) m3 γ 2 − ǫσ 2 cos2 (q1 ) − σ 2 cos3 (q1 ) + σ 2 ∆ 2ǫ + 3 cos(q1 )
2 2 ϕ = ǫ + cos(q1 ) ρ + γ sin2 (q1 ) − σ cosγ (q1 ) , and the determinant ofMd 2 2 2 2 . m γ − σ cos (q ) ǫ + cos(q ) ǫ + cos(q ) ∆d = ∆ 2 3 1 1 1 γ We can now substitute all the terms into (10) to obtain the energy shaping controller σ cos(q1 ) γm3 − ρ + γ sin2 (q1 ) ǫ + cos(q1 ) ues = − γ 2 ǫλ1 λ2 sin(q1 ) λ1 sin(q1 ) B1 2 − + p + B 2 p1 p2 1 − λ22 cos2 (q1 ) ǫ + cos(q1 ) 2 1 σ 2 cos2 (q1 ) ǫ + cos(q1) B3 2 p − γm3 − + Kp q 2 2 2 γ ∆ γm3 p1 + σ cos(q1 ) ǫ + cos(q1 ) p2 . (30) − j2 γ∆d 2) Damping assignment: The damping injection controller follows the construction (11). Given Md that has been obtained when reshaping the total energy, we have
∇p Hd = Md−1 p " # σ cos(q1 )(ǫ+cos(q1 )) m3 ∆ p1 γ = 1 )) p2 ∆d σ cos(q1 )(ǫ+cos(q ǫ + cos(q ) 1 γ # " σ cos(q1 )(ǫ+cos(q1 )) m3 p 1 + p2 ∆ γ (31) = 1 )) ∆d σ cos(q1 )(ǫ+cos(q p + (ǫ + cos(q 1 1 ))p2 γ
(27)
After lengthy but straightforward calculations, J2 is evaluated as 0 j2 , (28) J2 = −j2 0 where,
Kj γ∆d × j2 = 2∆ (ǫ + cos(q1 )) (σ cos(q1 )p1 + γp2 ) (29) 2 2 (ϕB1−A1 )p1+2(ϕB2 − A2 )p1 p2+(ϕB3−A3 )p2
with 1) γ∆ − ρ + γ sin2 (q1 ) (γ 2 + σ 2 ) , A1 = sin(2q ∆2 1) A2 = − σ sin(q ∆ + 2 cos2 (q1 )(γ 2 + σ 2 ) , 2 ∆ 2 2 A3 = − γ(γ +σ∆)2sin(2q1 ) , m3 sin(q1 ) − 2γ 2 ∆d cos(q1 )(γ 2 + σ 2 ) B1 = (γ∆d )2 + ∆3 m3 γ 2 − ǫσ 2 cos2 (q1 ) − σ 2 cos3 (q1 ) − σ 2 ∆3 cos(q1 ) ǫ + cos(q1 ) 2ǫ + 3 cos(q1 ) , σ sin(q1 ) ǫ + cos(q1 ) × B2 = − γ∆d m3 γ 2 − ǫσ 2 cos2 (q1 ) − σ 2 cos3 (q1 ) σ 2 ∆ cos2 (q1 ) 2ǫ + 3 cos(q1 ) + 2 cos2 (q1 )(γ 2 + σ 2 ) + ∆ m3 γ 2 − ǫσ 2 cos2 (q1 ) − σ 2 cos3 (q1 ) ,
Substituting all the terms into (11), the damping injection controller is then Kv ∆ ǫ + cos(q1 ) udi = − σ cos(q1 )p1 + γp2 . (32) γ∆d
By choosing the right m3 that satisfies the condition of 2 2 (q1 ) , for instance m3 := ǫ + m3 > ǫ + cos(q1 ) σ cos γ2 σ2 cos2 (q1 ) cos(q1 ) +µ with a constant µ > 0 the dependence γ2 on q1 at the denominator of udi can easily be avoided. Moreover, from the positive definiteness of the inertia matrix M , its determinant ∆ is never be zero. We can conclude the IDA-PBC design for the rotary inverted pendulum by stating the following proposition. Proposition 4.1: The state feedback controller (30) and 2 2 (q1 ) (32), with m3 > ǫ + cos(q1 ) σ cos , Kp , Kv , Kj > γ2 0 and ǫ > 1 is an asymptotically stabilizing controller for the rotary pendulum inverted system (17)-(18) at its unstable equilibrium point q = (0, q2 ) for any q2 ∈ [0, 2π]. The proof of Proposition 4.1 can be established by verifying that Vd satisfies the Conditions (C2.1-C2.2:), and Md is positive and symmetric, thus, Hd qualifies as a Lyapunov function. Furthermore, by invoking LaSalle’s invariance principle asymptotic stability can be proven. (The proof is omitted due to the limited space.)
5244
V. S IMULATION R ESULTS
0
0
q2 (rad)
0.5
q1 (rad)
0.5
−0.5
−1
−0.5
0
0.5
1
1.5
−1
5 0
0.5
0
0.5
1
1.5
1
1.5
0
−10
−5
−10
0
10
Hd (J)
Torque (N.m.)
10
0
0.5
1 t (sec)
1.5
−20
t (sec)
Fig. 2. Simulation result for the rotary inverted pendulum with the initial condition [q, p] = [− π4 , −0.8, 0.033, 0.032].
extreme case, Figure 3 shows the simulation result obtained with the initial condition [q, p] = [ π2 , 0.5, −0.074, −0.001] and the controller parameters Kp = 0.37, Kv = 0.029, Kj = 1 × 10−11 , m3 = 263. q10 = π2 rad is chosen to show the advantage of the controller (30), (32) as it provides a much larger DoA compared to a linear controller (see [16]). This angular position π2 (as well as − π2 ), which is the horizontal position of the pendulum, is also a critical point where the pendulum losses its local controllability [17]. It is shown that, by allowing a small initial angular velocity on both the pendulum and the arm, the pendulum can still be stabilized at its upright position with a reasonable effort. VI. C ONCLUSION AND FUTURE WORKS We have proposed a method to simplify the matching PDEs for solving an IDA-PBC construction. This has significantly simplified the design computation and also yield a simpler form of the controller. The result has been applied to solve a global stabilization design of a rotary inverted pendulum. It has been shown by simulations that this design results in a very high closed-loop performance of the pendulum in its full nonlinear dynamics. The next obvious step is to implement the controller to the hardware to validate the design approach in a real experiment. As there are only two
1
0
−1
0
0.5
1
1.5
2
2.5
3
0.5
1
1.5 t (sec)
2
2.5
3
1 0 q2 (rad)
We present some simulation results obtained on the Quanser rotary pendulum shown in Figure (1), by applying the IDA-PBC controller (3) which consists of (30) and (32). The parameters of the rotary pendulum are listed in Table I. Throughout the simulation, we use g = 9.8 ms−2 . In the first simulation shown in Figure 2, we choose the initial condition [q, p] = [− π4 , −0.8, 0.033, 0.032] and the controller parameters Kp = 0.01, Kv = 0.04, Kj = 1 × 10−6 , m3 = 100. This initial state is more than twice the initial angular position of the pendulum that is recommended for the balancing experiment of the Quanser rotary inverted pendulum module (SRV02+ROTPEN) hardware using a linear state feedback controller obtained via a pole placement design. As expected, it is observed that the pendulum can easily be stabilized at its upright position using the controller (30), (32) with very little effort as shown by the low value of the control torque. To illustrate a more
q1 (rad)
2
−1 −2 −3
0
Fig. 3. Simulation result for the rotary inverted pendulum with the initial condition [q, p] = [ π2 , 0.5, −0.074, −0.001].
states, the pendulum angle and the arm angle, measured, an observer will be designed to estimate the momenta. R EFERENCES [1] R. Ortega, A. van der Schaft, B. Maschke, and G. Escobar, “Interconnection and damping assignment passivity-based control of portcontrolled Hamiltonian systems,” Automatica, vol. 38, pp. 585–596, 2002. [2] R. Ortega and E. Garcia-Canseco, “Interconnection and damping assignment passivity-based control: A survey,” European Journal of Control, vol. 10, pp. 432–450, 2004. [3] F. Gomez-Estern, R. Ortega, F. Rubio, and J. Aracil, “Stabilization of a class of underactuated mechanical systems via total energy shaping,” IEEE Conf on Decision and Control, vol. 41, pp. 1372–1388, 2001. [4] D. Auckly and L. Kapitanski, “On the λ−equations for matching control laws,” SIAM J. Contr. & Optim., vol. 41, pp. 1372–1388, 2002. [5] G. Blankenstein, R. Ortega, and A. van der Schaft, “The matching conditions of controlled Lagrangians and IDA-passivity based control,” International Journal of Control, vol. 75, pp. 645–665, 2002. [6] J. A. Acosta, R. Ortega, A. Astolfi, and A. D. Mahindrakar, “Interconnection and damping assignment passivity based control of mechanical systems with underactuation degree one,” IEEE Trans. on Automatic Control, vol. 50, pp. 1936–1955, 2005. [7] R. Olfati-Saber, “Nonlinear control of underactuated mechanical systems with application to robotics and aerospace vehicles,” Ph.D. dissertation, Massachusetts Institute of Technology, Department of Electrical Engineering and Computer Science, 2001. [8] M. Spong, “Energy based control of a class of underactuated mechanical systems,” IFAC World Congress, pp. 431–435, 1996. [9] J. van der Burg, R. Ortega, J. Scherpen, J. Acosta, and H. Siguerdidjane, “An experimental application of total energy shaping control: Stabilization of the inverted pendulum on a cart in the presence of friction,” European Control Conference, 2007. [10] R. Ortega, M. Spong, F. Gomez-Estern, and G. Blankenstein, “Stabilization of a class of underactuated mechanical systems via interconnection and damping assignment,” Automatic Control, IEEE Transactions on, vol. 47, pp. 1218 – 1233, 2002. [11] J. Sandova, R. Ortega, and R. Kelly, “Interconnection and damping assignment passivity–based control of the pendubot,” IFAC World Congress, pp. 7700–7704, 2008. [12] A. Mahindrakar, A. Astolfi, R. Ortega, and G. Viola, “Further constructive results on interconnection and damping assignment control of mechanical systems: the acrobot example,” Int. J. Robust Nonlinear Control, vol. 16, pp. 671–685, 2006. [13] I. Sarras, A. Acosta, R. Ortega, and A. Mahindrakar, “Constructive immersion and invariance stabilization for a class of underactuated mechanical systems,” 8th IFAC Sympossium on Nonlinear Control Systems, pp. 108 – 113, 2010. [14] G. Viola, R. Ortega, R. Banavar, J. A. Acosta, and A. Astolfi, “Total energy shaping control of mechanical systems: Simplifying the matching equations via coordinate changes,” IEEE Trans. Autom. Control, vol. 52, pp. 1093–1099, 2007. [15] H. Khalil, Nonlinear Systems, 2nd Ed. Prentice Hall, 1996. [16] Q. Inc., “Quanser rotary pendulum user manual and workbook,” 2011. [17] K. Astr¨om and K. Furuta, “Swing up a pendulum by energy control,” Automatica, vol. 36, pp. 287–295, 2000.
5245
