Attitude Tracking of Rigid Bodies on the Special Orthogonal Group ...
Joint 48th IEEE Conference on Decision and Control and 28th Chinese Control Conference Shanghai, P.R. China, December 16-18, 2009
WeCIn4.4
Attitude Tracking of Rigid Bodies on the Special Orthogonal Group with Bounded Partial State Feedback Sylvain Bertrand, Tarek Hamel, H´el`ene Piet-Lahanier and Robert Mahony Abstract— A solution to the attitude tracking problem of rigid bodies with kinematic representation directly on the special orthogonal group SO(3) of rotation matrices is proposed. A dynamic partial state feedback controller is designed to address the case where no angular velocity measurements are available. In addition, the gains in the control design can be tuned in advance to ensure that the torque inputs satisfy arbitrary saturation bounds. Stability conditions are provided based on Lyapunov function analysis and Barbalat’s lemma. Simulation results are presented to illustrate the performance of the proposed control scheme.
I. INTRODUCTION The rigid-body attitude control problem has been widely investigated in the last decades by the control community. Motivations for such works are, for example, in aeronautics, spacecraft positioning, control of autonomous aircrafts, or in robotics, control of pan and tilt cameras or objects held by a robot arm. Early results concerned the regulation problem where the attitude of the body has to be stabilized around a fixed equilibrium point. Controllability conditions were derived in [6] for the attitude stabilization of a spacecraft with momentum exchange actuators or gas jets yielding three or two independent control torques. An attitude regulator based on a unit-quaternion representation is proposed in [17] for a spacecraft, while a controller using pseudo-Euler’s angles is presented in [7] in the case of two momentum wheel actuators. The case of the attitude stabilization of a rigid spacecraft with a reduced number of control inputs is also investigated in [12]. Other results addressing the problem of attitude tracking of time-varying references have also been obtained. Globally stabilizing control laws can for example be found in [16]. An interesting result can be found in [3], where a coordinate-free procedure is proposed to design tracking controllers for fully actuated mechanical systems. In all these works, full state measurements are assumed. When no measurements of angular velocity are available, observers can be used to estimate the non measured states [4], [10], [13]. In [11], a coordinate-free control procedure is presented to solve the reference tracking problem of S. Bertrand is with ONERA-DPRS, Chˆatillon, France (e-mail: [email protected]) T. Hamel is with I3S-UNSA-CNRS, Sophia Antipolis, France (e-mail: [email protected]) H. Piet-Lahanier is with ONERA-DPRS, Chˆatillon, France (e-mail: [email protected]) R. Mahony is with Department of Engineering, Australian National University, Canberra ACT, 0200, Australia (e-mail: [email protected])
978-1-4244-3872-3/09/$25.00 ©2009 IEEE
mechanical systems on a general class of Lie groups. The output feedback case is addressed by using a velocity observer and a separation-like principle is established to prove the convergence of the observer-based control strategy. Nevertheless, to avoid the design of an observer, more recent works have proposed partial state feedback strategies. In [18], a linear filter is used to generate a velocity-related signal from attitude measurements and solve the attitude tracking problem of a rigid spacecraft. Modified Rodrigues parameters have been chosen in this work for the attitude representation. In [5] a nonlinear filter is used for the same purpose, but a unit-quaternion representation has been chosen for control design. A dynamic output feedback controller is presented in [1] for the attitude tracking of rigid bodies by using modified Rodrigues parameters. First-order dynamic output feedback controllers based on unit-quaternion representations are also proposed in [2] and [14] for the inertial pointing of magnetically actuated spacecrafts and attitude tracking of rigid bodies. A passivity-like property of the system is used in [8] and [15] to design attitude feedback controllers respectively based on unit-quaternions and Rodrigues parameters. In most of the aforementioned works (except [3], [11]), the attitude representation based on orientation matrices is often considered to be too complex to work with, and control laws are designed by using another parameterization. In this paper, a solution to the attitude tracking problem is directly proposed on the special orthogonal group SO(3) of rotation matrices. A partial state feedback controller is designed for attitude tracking of rigid bodies, based on the introduction of two virtual states in the orientation dynamics. By this method, the complexity associated to the design of an observer is avoided. The proposed control law also takes into account saturations on the control inputs by guaranteeing a tunable bound on the resulting control torque. The paper is organized as follows. The next section introduces the notations as well as several mathematical identities that will be used in the rest of the paper. In Section 3, rigid body orientation dynamics are presented and then re formulated in Section 4 for control design in terms of tracking errors. The control law is described in Section 5 with a stability analysis, and guaranteed bounds on the resulting control torque are computed in Section 6. In Section 7, simulation results are presented to illustrate the proposed approach. The last part of the paper is devoted to concluding remarks.
2972
WeCIn4.4 II. N OTATIONS
III. R IGID B ODY O RIENTATION DYNAMICS
Let I3 denote the identity matrix of R3×3 . The special orthogonal group of R3×3 is defined by SO(3) = A ∈ R3×3 |AT A = I3 , det(A) = 1 (1)
Consider a rigid body and denote by J (= J T > 0) its tensor of inertia. To describe the attitude dynamics of the body, two reference frames are introduced: an inertial reference frame (I) associated with the vector basis (e1 , e2 , e3 ), and a body frame (B) attached to the rigid body and associated with the vector basis (eb1 , eb2 , eb3 ). The attitude of the rigid body is represented by the orientation matrix R ∈ SO(3) from (I) to (B). Let T Ω = [ωp ωq ωr ] denote in (B) its angular velocity vector. T We assume that a control torque Γ = [Γ1 Γ2 Γ3 ] defined in (B) is applied to the rigid body.
Let so(3) denote the group of antisymmetric matrices of R3×3 . We define by (.)× the operator from R3 → so(3) such that 0 −b3 b2 0 −b1 (2) ∀b ∈ R3 , b× = b3 −b2 b1 0
where bi denotes the ith component of the vector b. Let vex(.) be the inverse operator of (.)× , defined from so(3) → R3 , such that ∀b ∈ R3 , vex(b× ) = b
∀B ∈ so(3), vex(B)× = B (3)
With that definition, one has the property ∀A ∈ SO(3), ∀b ∈ R3 , (Ab)× = Ab× AT
(4)
n×n
For matrices of R , the Euclidian matrix inner product and the Frobenius norm are defined by ∀A, B ∈ Rn×n , hA, Bi = tr(AT B) n×n
∀A ∈ R
, ||A||F =
p
hA, Ai =
q
(5)
tr(AT A)
M + MT Ps (M ) = 2
The movement of the rigid body around its center of mass can be described by the following system: ( R˙ = R Ω× (14) J Ω˙ = −Ω× JΩ + Γ Consider a reference trajectory for the system (14), given in terms of a desired orientation Rd (t). The attitude tracking problem consists in designing a feedback control law for the torque Γ, such that the orientation R of the body converges to Rd and such that its angular velocity vector Ω converges to the desired value Ωd defined by R˙ d = Rd Ωd×
(6)
For a given vector b ∈ R3 and a given matrix M ∈ R3×3 , let us consider the following notations and identities: M − MT Pa (M ) = 2
Remark 1: The orientation matrix R can be usually parametrized by Euler angles ψ, θ, φ (yaw, pitch, roll).
(15)
To design such a control law, the system (14) will be reformulated in terms of tracking errors in the next section. IV. T RACKING ERRORS DYNAMICS
(7)
Consider the system dynamics (14). For a given desired orientation Rd , we define the deviation matrix
tr(Pa (M )Ps (M )) = 0
(8)
˜ = (Rd )T R R
1 tr(b× M ) = −bT vex(Pa (M )) 2
(9)
Using the first equation of (14) and (15), the time derivative ˜ can be expressed as: of R ˜˙ = −Ωd× R ˜ + RΩ ˜ × R
The following identity will also be used: 1 2 ∀A ∈ so(3), tr(AT A) = kvex(A)k 2
(10)
Denote by (γR , nR ) the angle-axis coordinates of a given matrix R ∈ SO(3). One has:
(16)
(17)
Since ∀A ∈ SO(3), AAT = I3 , one has:
˜ + RΩ ˜ × ˜˙ = −R ˜R ˜ T Ωd× R R
(18)
Using identity (4), we get: ˜˙ = −R( ˜ R ˜ T Ωd )× + RΩ ˜ × = R(Ω ˜ −R ˜ T Ωd )× R
(19)
∀R ∈ SO(3), tr(I3 − R) = 2(1 − cos(γR ))
(11)
∀R ∈ SO(3), kvex(Pa (R))k = |sin(γR )|
(12)
¯d = R ˜ T Ωd Ω
(20)
, we
˜ =Ω−Ω ¯d Ω
(21)
˜˙ = R ˜Ω ˜× R
(22)
¯ d and Ω ˜ defined by Let us introduce Ω 3×3
Finally, for a given positive definite matrix M ∈ R denote by λi (M ) its ith eigenvalue and introduce
to finally obtain: λmax (M ) = max {|λi (M )| , i = 1, 2, 3}
(13)
2973
WeCIn4.4 With these notations, we also have ¯˙ d ¯˙ d ) = −Ω× JΩ + Γ − J Ω ˜˙ = J(Ω˙ − Ω JΩ
(23)
¯˙ d = −Ω ˜ ×Ω ¯d + R ˜ T Ω˙d Ω
(24)
with
Using (22), (23) and (24), the system dynamics (14) are rewritten as: R ˜˙ = R ˜Ω ˜×
JΩ ˜˙ = −(Ω ˜ +Ω ¯ d )× J(Ω ˜ +Ω ¯ d) + Γ + J Ω ˜ ×Ω ¯d − JR ˜ T Ω˙d (25) The attitude tracking problem consists now in assigning a feedback control law for the torque Γ that will ensure the ˜ Ω) ˜ to the equilibrium convergence of the tracking error (R, value (I3 , 0). V. C ONTROL LAW DESIGN AND STABILITY ANALYSIS Designing a state feedback control law would require measurements on the orientation R and on the angular velocity Ω. In this paper, we consider the case where no angular velocity measurements are available for control design. To avoid the design of an observer, a partial state feedback controller is proposed. Let us introduce two virtual states Q ∈ SO(3), W ∈ R3 and a virtual input ∆ ∈ R3 for the tracking errors dynamics (25), such that: ˙ ˜=R ˜Ω ˜× R ˜˙ ˜ +Ω ¯ d )× J(Ω ˜ +Ω ¯ d) + Γ + J Ω ˜ ×Ω ¯d − JR ˜ T Ω˙d J Ω = −(Ω Q˙ = −Q W× ˙ =∆ W
Remark 2: From Assumption 1 and from equation (15) it is straightforward to show that the desired angular velocity Ωd and its time derivative are also uniformly continuous and bounded. For control design and stability analysis, let us introduce the Lyapunov function candidate 1 ˜ + 1 kω Ω ˜T JΩ ˜ + 1 k1 tr(I3 − Q) ˜ L = kr tr(I3 − R) 2 2 2 (29) ˜ − 1) + k2 (N (W, Q)
where kr , kω , k1 and k2 are strictly positive scalars, and ˜ is defined by: where the term N (W, Q) q ˜ 2 +1 ˜ = ||W× + Pa (Q)|| (30) N (W, Q) F
Let L(0) denote the initial value of L.
Proposition 1: Consider the system dynamics (28). We define the control torque kr ˜ − k1 vex(Pa (Q)) ˜ vex(Pa (R)) kω kω k2 vex(Pa (M )) + vex(Pa (N )) ¯ d ¯ d ˜ T Ω˙d + + Ω× J Ω + J R ˜ kω N (W, Q) (31) and the virtual input ˜ N (W, Q) ˜ + k3 W× ∆=− vex (k1 + k3 )Pa (Q) 2k2 (32) 1 ˜+Q ˜ T W× − vex W× Q 2 where the matrices M and N are respectively defined by Γ=−
(26)
Defining
˜ = QT R ˜ Q
(27)
the system dynamics become ˜˙ = R ˜Ω ˜× R ˜˙ ˜ +Ω ¯ d )× J(Ω ˜ +Ω ¯ d) + Γ + J Ω ˜ ×Ω ¯d − JR ˜ T Ω˙d J Ω = −(Ω ˜˙ ˜+Q ˜Ω ˜× Q = W× Q ˙ W =∆ (28) Using that new formulation, the attitude tracking problem will be solved by designing a control law (only depending on orientation measurements and on the virtual states) for the ˜ Ω, ˜ Q, ˜ W) torque Γ that will ensure the convergence of (R, to the equilibrium (I3 , 0, I3 , 0). We suppose that the reference to be tracked satisfies the following assumption: Assumption 1: The reference orientation Rd , and its first and second time derivatives are uniformly continuous and bounded.
˜ TQ ˜ M = (W× + Pa (Q))
(33)
˜ T (W× + Pa (Q)) ˜ T N =Q
(34)
and where kr , kω , k1 , k2 and k3 are strictly positive scalars with kr < k1 (35) Consider the Lyapunov function candidate (29). ˜ ˜ Under Assumption 1, for any initial condition R(0), Ω(0), ˜ with Q(0) = R(0) and W (0) = 0, such that L(0) < 2kr
(36)
the control torque (31), along with the virtual input (32), asymptotically stabilizes the system dynamics (28). Proof: Consider the Lyapunov function L defined by (29). It’s time derivative along the trajectories of (28) is given by: 1 ˜Ω ˜ × ) − 1 k1 tr(W× Q ˜+Q ˜Ω ˜ ×) L˙ = − kr tr(R 2 2 ˜ T (−(Ω ˜ +Ω ¯ d )× J(Ω ˜ +Ω ¯ d) + Γ + J Ω ˜ ×Ω ¯ d) + kω Ω ˜ T (J R ˜ T Ω˙d ) − kω Ω . z }| { k2 ˜ T (∆× + Pa (Q)) ˜ + tr 2(W× + Pa (Q)) ˜ 2 N (W, Q) (37)
2974
WeCIn4.4 where .
z }| { ˜+Q ˜Ω ˜× + Q ˜ T W× + Ω ˜ ×Q ˜T ) ˜ = 1 (W× Q Pa (Q) 2
(38)
Using identity (8) and recalling that ∀a, b ∈ R3 , aT (a× b) = 0, we get: 1 ˜ × Pa (R)) ˜ − 1 k1 tr(W× Pa (Q)) ˜ L˙ = − kr tr(Ω 2 2 1 ˜ ˜ × Pa (Q)) − k1 tr(Ω 2 ˜ T (−Ω ¯ d× J Ω ¯d + Γ − JR ˜ T Ω˙d ) + kω Ω ˜ T (−Ω ¯d JΩ ˜ + JΩ ˜ ×Ω ¯ d) + kω Ω × o n 1 k2 ˜ T (Q ˜Ω ˜× + Ω ˜ ×Q ˜T ) + tr (W× + Pa (Q)) ˜ 2 N (W, Q) n o k2 1 ˜ T (W× Q ˜+Q ˜ T W× ) tr (W× + Pa (Q)) + ˜ 2 N (W, Q) o n k2 ˜ T ∆× tr (W× + Pa (Q)) + ˜ N (W, Q) (39) Using (33), (34) and identity (9), we obtain:
Taking ∆ as defined in (32) yields: n o 1 ˜ T Pa (Q) ˜ L˙ = − k1 tr Pa (Q) 2 n o (43) 1 ˜ T (W× + Pa (Q)) ˜ − k3 tr (W× + Pa (Q)) 2 Using identity (10), we finally obtain:
2
2
˜ ˜ L˙ = −k1 vex(Pa (Q))
− k3 vex(W× + Pa (Q))
(44)
Therefore we have L(t) ≤ L(0) and, under Assumption ˜ are uniformly bounded. Using (38) to 1, W× and Ω compute the time derivative of L˙ shows that L¨ is uniformly bounded as well. Hence, L˙ is uniformly continuous and ˜ → 0 Barbalat’s lemma can be applied, leading to Pa (Q) ˜ and W× → −Pa (Q), i.e. W× → 0 and W → 0. ˜ Denote by (γQ˜ , nQ˜ ) the angle-axis coordinates of Q. Using (11) one has: 1 ˜ ≤L k1 tr(I3 − Q) (45) 2 Since L is decreasing, we have L ≤ L(0). Using (36) it yields: k1 (1 − cos(γQ˜ )) ≤ L ≤ L(0) ≤ 2kr (46) k1 (1 − cos(γQ˜ )) =
Using (35), we get: ˜ + k1 Ω ˜ T vex(Pa (Q)) ˜ ˜ T vex(Pa (R)) L˙ =kr Ω kr k2 k2 ˜ T vex(Pa (M )) − ˜ T vex(Pa (N )) (47) 1 − cos(γQ˜ ) < 2 < 2 Ω Ω − ˜ ˜ k1 N (W, Q) N (W, Q) ˜ → 0, we have γ ˜ → 0 or γ ˜ → ±π. The ˜ T (−Ω ¯d JΩ ¯d + Γ − JR ˜ T Ω˙d ) From Pa (Q) + kω Ω × Q Q second possibility is excluded by (47). Therefore we have 1 T d d ˜ ˜ (Ω ¯ J + JΩ ¯ )Ω ˜ − k1 tr(W× Pa (Q)) − kω Ω ˜ → I3 . By (27), it yields R ˜ → Q. By continuity, we get × × Q 2 n o ˙ ˜ ˙ k2 R → Q. Using respectively the first and third equations ˜ T ∆× tr (W× + Pa (Q)) + ˜Ω ˜ × → −QW× . Since R ˜ is ˜ of (28) and (26), one has R N (W, Q) T ˜ ˜ n o orthogonal, we get Ω× → −R QW× . Using W× → 0 k2 ˜ T (W× Q ˜+Q ˜ T W× ) tr (W× + Pa (Q)) + ˜ × → 0, Ω ˜ → 0, and R ˜˙ → 0 from the first it yields Ω ˜ 2N (W, Q) ˙ ˜ → 0 and then, by the (40) equation of (28). By continuity Ω ¯ d ) is antisymmetric second equation of system (28), Γ → (Ω ¯d J + JΩ ¯d + JR ˜ T Ω˙d ). ¯d JΩ Since J = J T , the matrix (Ω × × × ˜ T (Ω ¯d J + JΩ ¯ d )Ω ˜ = 0. Choosing Γ Knowing that P (Q) ˜ and therefore we have Ω and W× converge to zero, one can × × a according to (31) leads to: ensure that, respectively from (33) and (34), M and N converge to zero. Combining the above discussion with the 1 ¯d + JR ˜ T Ω˙d ), equation (31) ensures ¯d JΩ fact that Γ → (Ω ˜ ˙ × L = − k1 tr(W× Pa (Q)) ˜ 2 that Pa (R) → 0. o n k2 T ˜ tr (W× + Pa (Q)) ∆× + ˜ ˜ let us denote N (W, Q) Similarly to the previous analysis on Q, n o ˜ k2 (γR˜ , nR˜ ) the angle-axis coordinates of R. One has: ˜ T (W× Q ˜+Q ˜ T W× ) + tr (W× + Pa (Q)) ˜ 2N (W, Q) 1 ˜ ≤ L ≤ L(0) < 2kr (48) kr (1 − cos(γR˜ )) = kr tr(I3 − R) (41) 2 T ˜ As W× = −W× , and introducing Pa (Q) in the first term: It yields 1 − cos(γR˜ ) < 2 (49) 1 ˜ − Pa (Q)) ˜ T Pa (Q)) ˜ L˙ = k1 tr((W× + Pa (Q) ˜ → 0, we have γ ˜ → 0 or γ ˜ → ±π. The 2 From Pa (R) R R o n k2 T second possibility is excluded by (49). Therefore, we finally ˜ ∆× + tr (W× + Pa (Q)) ˜ ˜ → I3 and R → Rd . Using Ω ˜ → 0 we get Ω → Ω ¯ d, N (W, Q) have R n o T d ˜ Ω . From R ˜ → I3 we finally have Ω → Ωd . k2 i.e. Ω → R ˜ T (W× Q ˜+Q ˜ T W× ) tr (W× + Pa (Q)) + ˜ 2N (W, Q) (42)
2975
WeCIn4.4 Remark 3: The result presented in this paper ensures local asymptotic stability. However, almost global asymptotic stability can be achieved by the proposed control approach. Indeed, it can be shown that, removing conditions (35) and (36), asymptotic closed-loop stability is guaranteed for almost all initial conditions ˜ ˜ ˜ with (R(0), Ω(0), Q(0), W (0)) ∈ / U, where the set 3 3 U ⊂ SO(3) × R × SO(3) × R is defined as Uo = n ˜ Ω, ˜ Q, ˜ W )|tr(R) ˜ = −1, Ω ˜ = 0, tr(Q) ˜ = −1, W = 0 . (R, The complete proof corresponding to this result is too long to be presented in this paper, but arguments are similar to those used in previous work done by some of the authors and can be found in [9]. VI. G UARANTEED B OUND ON Γ By the proposed control law, a bound on the resulting control torque Γ can be guaranteed, as presented in the following Proposition. Proposition 2: Under Assumption 1, the control torque defined by (31) is uniformly bounded. Proof: From (31) and by triangular inequality, we get:
k
kr
1 ˜ ˜ kΓk ≤
vex(Pa (R))
vex(Pa (Q))
+
kω kω k2 (kvex(Pa (M ))k + kvex(Pa (N ))k) + ˜ kω N (W, Q)
T d
d ¯ JΩ ¯ d + ˜ Ω˙ + Ω
J R × (50) Using the angular-axis notations previously defined for ma˜ and Q, ˜ along with (12), it yields: trices R kr k1 kΓk ≤ |sin(γR˜ )| + sin(γQ˜ ) kω kω k2 + (kvex(Pa (M ))k + kvex(Pa (N ))k) ˜ kω N (W, Q)
d
T d ¯ ×J Ω ¯ d + ˜ Ω˙ + Ω
J R
(51)
Under Assumption 1 and using notation (13), we get:
k1 k2 kvex(Pa (M ))k + kvex(Pa (N ))k kr + + ˜ kω kω kω N (W, Q)
d 2
+ λmax (J) sup Ω + λmax (J) sup Ω˙ d (52) ˜ Using identity Consider the term kvex(Pa (M ))k /N (W, Q). (10) and definition (6) one has: 1 1 2 kvex(Pa (M ))k = tr(Pa (M )T Pa (M )) = ||Pa (M )||F2 2 2 (53) From identity (8), we have:
From definitions (33) and (6), and using the fact that ∀A, B ∈ ˜ = I3 , we obtain: ˜T Q Rn×n , tr(AB) = tr(BA) along with Q 1 kvex(Pa (M ))k ≤ √ 2
(54)
Substituting in equation (53), it yields: 1 1 2 ||M ||F2 − ||Ps (M )||F2 ≤ ||M ||F2 kvex(Pa (M ))k = 2 2 (55)
(56)
and finally ˜ 1 1 kvex(Pa (M ))k ||W× + Pa (Q)|| F ≤√ q ≤√ ˜ 2 ||W + P (Q)|| 2 N (W, Q) ˜ 2 +1 × a F
(57) In the same way, an upper bound can be found for the term ˜ Hence, expression (52) leads to kvex(Pa (N ))k /N (W, Q). kΓk ≤
2 2k2 kr + k1
˙ d
+ √ + λmax (J) sup Ωd + sup Ω
kω kω 2 (58)
and the control torque Γ is uniformly bounded.
Remark 4: It is clear from (58) that the gains of the control law can be tuned to satisfy any bound B > 0 on the con trol torque that verifies B > λmax sup ||Ωd ||2 + sup ||Ω˙ d || . The strict inequality provides sufficient margin for a choice of gains kr , k1 and k2 small and kω large. In practice, it is also required that kr < k1 (a trivial choice) and that L(0) < 2kr . The second condition is more difficult to guarantee, since increasing kω increases L(0). This combination of conditions does place an effective bound on the ball of initial conditions that we guarantee to lie in the domain of attraction of the controller. The lower bound on required torque ensures that the rigid-body has sufficient control authority to follow the demanded trajectory Rd . If sufficient control authority is not available, the simplest alternative is to choose a less aggressive trajectory Rd . This could be d done, for example, by adjusting the time-scale for Rnew (t) = d d 2 d R (µt) for µ ≤ 1. One has sup ||Ωnew || + sup ||Ω˙ new || ≤ µ2 sup ||Ωd ||2 + sup ||Ω˙ d || , where Ωdnew is defined according to (15). By choosing a sufficiently slow reference trajectory then all the terms on the right hand side of (58) may be adjusted arbitrarily and it is possible to find gains kr , k1 , k2 , kω and scaling factor µ such that the theorem conditions are satisfied for arbitrary initial conditions.
kΓk ≤
||M ||F2 = ||Pa (M )||F2 + ||Ps (M )||F2
r n o ˜ T (W× + Pa (Q)) ˜ tr (W× + Pa (Q))
VII. S IMULATIONS Simulation results are provided in this section to illustrate the stabilizing property of the proposed approach. The following values have been considered for the rigid body inertia and the control law parameters: J = diag(9.3, 9.3, 6.3) kg.m2 , kr =4, kω = 0.9, k1 = 14, k2 =1.6, k3 =14. We assume that the reference trajectory to be tracked is defined for all t ≥ 0 by θ(t) = φ(t) = a sin(b t) and ψ(t) = c cos(d t), with a =10 deg, b =0.3 rad.s−1 , c = 5 deg and d = 0.15 rad.s−1 . The initial conditions for attitude angles and angular velocities are φ0 =10 deg, θ0 = -10 deg, ψ0 = -5 deg, ωp0 = -0.5 deg.s−1 , ωq0 = -2 deg.s−1 , ωr0 = 3 deg.s−1 . Attitude angles and angular velocities (solid curves) are represented on Figure 1 along with their reference values (dashed curves). Tracking errors
2976
WeCIn4.4 10
−5
0 −2 −4
ωp − ωdp (deg/s)
0
2 φ − φd (deg)
ωp, ωdp (deg/s)
d
φ, φ (deg)
5
5
0
0
−5
−6
−10 0
5
4
10
5
10
15
−8 0
20
15
5
10
15
−5 0
20
15
5
10
15
−10 0
20
5
5
10
15
20
5
10
15
20
5
10
15
20
10
0 −5
10 5
0
0
ωq − ωdq (deg/s)
5
θ − θd (deg)
ωq, ωdq (deg/s)
d
θ, θ (deg)
10
−5 −10
5
0
−10 −15 0
5
10
15
−5 0
20
5
10
15
−15 0
20
15
5
10
15
−5 0
20
10
15
10
15
20
−5 0
5
t (s)
Fig. 1.
−5 −10
10
15
−15 0
20
5
10
t (s)
15
10 5 0 −5 0
20
t (s)
Attitude angles and angular velocities
t (s)
Fig. 2.
and components of the control torque are respectively plotted in Figures 2 and 3. As can be seen, stability around the reference trajectory is achieved with a satisfying closed loop behavior.
5
4
1
4
3
0
3
2
−1 −2 −3 −4 0
VIII. C ONCLUSION
2 1 0
5
10 15 t (s)
20
−1 0
Fig. 3.
A solution to the attitude tracking problem of rigid bodies has been presented in this paper. Contrary to most of existing works, the controller has been designed by using an attitude representation directly on the special orthogonal group of rotation matrices. For control design, we have considered the case where no angular velocity measurements are available. A dynamic partial state feedback controller has been proposed based on the introduction of two virtual states in the system dynamics. Using Lyapunov analysis, stability conditions have been derived. A guaranteed bound on the resulting control torque has also been computed. Simulations results have been presented to illustrate the stabilizing property of the proposed controller. R EFERENCES [1] M. R. Akella, ”Rigid Body Attitude Tracking without Angular Velocity Feedback”, in Systems & Control Letters, vol. 42, pp 321-326, 2001. [2] A. Astolfi and M. Lovera, ”Global Spacecraft Attitude Control using Magnetic Actuators”, in Proceedings of the American Control Conference, Anchorage, USA, pp 1331-1335, 2002. [3] F. Bullo and R. M. Murray, ”Tracking for Fully Actuated Mechanical Systems: a Geometric Framework”, in Automatica, vol. 35, pp 17-34, 1999. [4] F. Caccavale and L. Villani, ”Output Feedback Control for Attitude Tracking”, in Systems & Control Letters, vol. 38, pp 91-98, 1999. [5] B. T. Costic, D. M. Dawson, M. S. de Queiroz and V. Kapila, ”A Quaternion-Based Adaptive Attitude Tracking Controller Without Velocity Measurements”, in Proceedings of the 39th IEEE Conference on Decision and Control, Sydney, Australia, pp 2424-2429, 2000. [6] P. E. Crouch, ”Spacecraft Attitude Control and Stabilization: Applications of Geometric Control Theory to Rigid Body Models”, in IEEE Transactions on Automatic Control, vol. 29, no. 4, pp 321-331, 1984. [7] H. Krishnan, N. H. McClamroch and M. Reyhanoglu, ”Attitude Stabilization of a Rigid Spacecraft Using Two Momentum Wheel Actuators”, in Journal of Guidance, Control, and Dynamics, vol. 18, no. 2, pp 256-263, 1995.
Tracking errors
2
Γ2 (N.m)
5
0
0
Γ1 (N.m)
−10 0
5
Γ3 (N.m)
0 −5
5
10
ωr − ωdr (deg/s)
5
ψ − ψd (deg)
ωr, ωdr (deg/s)
d
ψ, ψ (deg)
10
1 0 −1
5
10 15 t (s)
20
−2 0
5
10 15 t (s)
20
Control inputs
[8] F. Lizarralde and J. T. Wen, ”Attitude Control without Angular Velocity Measurement: A Passivity Approach”, IEEE Transactions on Automatic Control, vol. 41, no. 3, pp 468-472, 1996. [9] R. Mahony, T. Hamel and J. M. Pflimlin, ”Nonlinear Complementary Filters on the Special Orthogonal Group”, in IEEE Transactions on Automatic Control, vol. 53, no. 5, pp 1203-1217, 2008. [10] D. H. S. Maithripala, J. M. Berg and W. P. Dayawansa, ”An Intrisic Observer for a Class of Simple Mechanical Systems on a Lie Group”, in Proceedings of the 2004 American Control Conference, Boston, USA, pp 1546-1551, 2004. [11] D. H. S. Maithripala, J. M. Berg and W. P. Dayawansa, ”AlmostGlobal Tracking of Simple Mechanical Systems on a General Class of Lie Groups”, in IEEE Transactions on Automatic Control, vol. 51, no. 1, pp 216-225, 2006. [12] P. Morin, C. Samson, J.-B. Pomet and Z.-P. Jiang, ”Time-varying Feedback Stabilization of the Attitude of a Rigid Spacecraft with Two Controls”, in System & Control Letters, vol. 25, pp 375-7385, 1995. [13] S. Salcudean, ”A Globally Convergent Angular Velocity Observer for Rigid Body Motion”, in IEEE Transactions on Automatic Control, vol. 36, no. 12, pp 1493-1497, 1991. [14] A. Tayebi, ”Unit Quaternion-Based Output Feedback for the Attitude Tracking Problem”, in IEEE Transactions on Automatic Control, vol. 53, no. 6, pp 1516-1520, 2008. [15] P. Tsiotras, ”Further Passivity Results for the Attitude Control Problem”, IEEE Transactions on Automatic Control, vol. 43, no. 11, pp 1597-1600, 1998. [16] J. T.-Y. Wen and K. Kreutz-Delgado, ”The Attitude Control Problem”, in IEEE Transactions on Automatic Control, vol. 36, no. 10, pp 11481162, 1991. [17] B. Wie, H. Weiss and A. Arapostathis, ”A Quaternion Feedback Regulator for Spacecraft Eigenaxis Rotations”, AIAA Guidance, Navigation and Control Conference, Minneapolis, USA, 1988. [18] H. Wong, M. S. de Queiroz and V. Kapila, ”Adaptive Tracking Control Using Synthesized Velocity from Attitude Measurements”, in Automatica, vol. 37, no. 6, pp 947-953, 2001.
2977
WeCIn4.4
Attitude Tracking of Rigid Bodies on the Special Orthogonal Group with Bounded Partial State Feedback Sylvain Bertrand, Tarek Hamel, H´el`ene Piet-Lahanier and Robert Mahony Abstract— A solution to the attitude tracking problem of rigid bodies with kinematic representation directly on the special orthogonal group SO(3) of rotation matrices is proposed. A dynamic partial state feedback controller is designed to address the case where no angular velocity measurements are available. In addition, the gains in the control design can be tuned in advance to ensure that the torque inputs satisfy arbitrary saturation bounds. Stability conditions are provided based on Lyapunov function analysis and Barbalat’s lemma. Simulation results are presented to illustrate the performance of the proposed control scheme.
I. INTRODUCTION The rigid-body attitude control problem has been widely investigated in the last decades by the control community. Motivations for such works are, for example, in aeronautics, spacecraft positioning, control of autonomous aircrafts, or in robotics, control of pan and tilt cameras or objects held by a robot arm. Early results concerned the regulation problem where the attitude of the body has to be stabilized around a fixed equilibrium point. Controllability conditions were derived in [6] for the attitude stabilization of a spacecraft with momentum exchange actuators or gas jets yielding three or two independent control torques. An attitude regulator based on a unit-quaternion representation is proposed in [17] for a spacecraft, while a controller using pseudo-Euler’s angles is presented in [7] in the case of two momentum wheel actuators. The case of the attitude stabilization of a rigid spacecraft with a reduced number of control inputs is also investigated in [12]. Other results addressing the problem of attitude tracking of time-varying references have also been obtained. Globally stabilizing control laws can for example be found in [16]. An interesting result can be found in [3], where a coordinate-free procedure is proposed to design tracking controllers for fully actuated mechanical systems. In all these works, full state measurements are assumed. When no measurements of angular velocity are available, observers can be used to estimate the non measured states [4], [10], [13]. In [11], a coordinate-free control procedure is presented to solve the reference tracking problem of S. Bertrand is with ONERA-DPRS, Chˆatillon, France (e-mail: [email protected]) T. Hamel is with I3S-UNSA-CNRS, Sophia Antipolis, France (e-mail: [email protected]) H. Piet-Lahanier is with ONERA-DPRS, Chˆatillon, France (e-mail: [email protected]) R. Mahony is with Department of Engineering, Australian National University, Canberra ACT, 0200, Australia (e-mail: [email protected])
978-1-4244-3872-3/09/$25.00 ©2009 IEEE
mechanical systems on a general class of Lie groups. The output feedback case is addressed by using a velocity observer and a separation-like principle is established to prove the convergence of the observer-based control strategy. Nevertheless, to avoid the design of an observer, more recent works have proposed partial state feedback strategies. In [18], a linear filter is used to generate a velocity-related signal from attitude measurements and solve the attitude tracking problem of a rigid spacecraft. Modified Rodrigues parameters have been chosen in this work for the attitude representation. In [5] a nonlinear filter is used for the same purpose, but a unit-quaternion representation has been chosen for control design. A dynamic output feedback controller is presented in [1] for the attitude tracking of rigid bodies by using modified Rodrigues parameters. First-order dynamic output feedback controllers based on unit-quaternion representations are also proposed in [2] and [14] for the inertial pointing of magnetically actuated spacecrafts and attitude tracking of rigid bodies. A passivity-like property of the system is used in [8] and [15] to design attitude feedback controllers respectively based on unit-quaternions and Rodrigues parameters. In most of the aforementioned works (except [3], [11]), the attitude representation based on orientation matrices is often considered to be too complex to work with, and control laws are designed by using another parameterization. In this paper, a solution to the attitude tracking problem is directly proposed on the special orthogonal group SO(3) of rotation matrices. A partial state feedback controller is designed for attitude tracking of rigid bodies, based on the introduction of two virtual states in the orientation dynamics. By this method, the complexity associated to the design of an observer is avoided. The proposed control law also takes into account saturations on the control inputs by guaranteeing a tunable bound on the resulting control torque. The paper is organized as follows. The next section introduces the notations as well as several mathematical identities that will be used in the rest of the paper. In Section 3, rigid body orientation dynamics are presented and then re formulated in Section 4 for control design in terms of tracking errors. The control law is described in Section 5 with a stability analysis, and guaranteed bounds on the resulting control torque are computed in Section 6. In Section 7, simulation results are presented to illustrate the proposed approach. The last part of the paper is devoted to concluding remarks.
2972
WeCIn4.4 II. N OTATIONS
III. R IGID B ODY O RIENTATION DYNAMICS
Let I3 denote the identity matrix of R3×3 . The special orthogonal group of R3×3 is defined by SO(3) = A ∈ R3×3 |AT A = I3 , det(A) = 1 (1)
Consider a rigid body and denote by J (= J T > 0) its tensor of inertia. To describe the attitude dynamics of the body, two reference frames are introduced: an inertial reference frame (I) associated with the vector basis (e1 , e2 , e3 ), and a body frame (B) attached to the rigid body and associated with the vector basis (eb1 , eb2 , eb3 ). The attitude of the rigid body is represented by the orientation matrix R ∈ SO(3) from (I) to (B). Let T Ω = [ωp ωq ωr ] denote in (B) its angular velocity vector. T We assume that a control torque Γ = [Γ1 Γ2 Γ3 ] defined in (B) is applied to the rigid body.
Let so(3) denote the group of antisymmetric matrices of R3×3 . We define by (.)× the operator from R3 → so(3) such that 0 −b3 b2 0 −b1 (2) ∀b ∈ R3 , b× = b3 −b2 b1 0
where bi denotes the ith component of the vector b. Let vex(.) be the inverse operator of (.)× , defined from so(3) → R3 , such that ∀b ∈ R3 , vex(b× ) = b
∀B ∈ so(3), vex(B)× = B (3)
With that definition, one has the property ∀A ∈ SO(3), ∀b ∈ R3 , (Ab)× = Ab× AT
(4)
n×n
For matrices of R , the Euclidian matrix inner product and the Frobenius norm are defined by ∀A, B ∈ Rn×n , hA, Bi = tr(AT B) n×n
∀A ∈ R
, ||A||F =
p
hA, Ai =
q
(5)
tr(AT A)
M + MT Ps (M ) = 2
The movement of the rigid body around its center of mass can be described by the following system: ( R˙ = R Ω× (14) J Ω˙ = −Ω× JΩ + Γ Consider a reference trajectory for the system (14), given in terms of a desired orientation Rd (t). The attitude tracking problem consists in designing a feedback control law for the torque Γ, such that the orientation R of the body converges to Rd and such that its angular velocity vector Ω converges to the desired value Ωd defined by R˙ d = Rd Ωd×
(6)
For a given vector b ∈ R3 and a given matrix M ∈ R3×3 , let us consider the following notations and identities: M − MT Pa (M ) = 2
Remark 1: The orientation matrix R can be usually parametrized by Euler angles ψ, θ, φ (yaw, pitch, roll).
(15)
To design such a control law, the system (14) will be reformulated in terms of tracking errors in the next section. IV. T RACKING ERRORS DYNAMICS
(7)
Consider the system dynamics (14). For a given desired orientation Rd , we define the deviation matrix
tr(Pa (M )Ps (M )) = 0
(8)
˜ = (Rd )T R R
1 tr(b× M ) = −bT vex(Pa (M )) 2
(9)
Using the first equation of (14) and (15), the time derivative ˜ can be expressed as: of R ˜˙ = −Ωd× R ˜ + RΩ ˜ × R
The following identity will also be used: 1 2 ∀A ∈ so(3), tr(AT A) = kvex(A)k 2
(10)
Denote by (γR , nR ) the angle-axis coordinates of a given matrix R ∈ SO(3). One has:
(16)
(17)
Since ∀A ∈ SO(3), AAT = I3 , one has:
˜ + RΩ ˜ × ˜˙ = −R ˜R ˜ T Ωd× R R
(18)
Using identity (4), we get: ˜˙ = −R( ˜ R ˜ T Ωd )× + RΩ ˜ × = R(Ω ˜ −R ˜ T Ωd )× R
(19)
∀R ∈ SO(3), tr(I3 − R) = 2(1 − cos(γR ))
(11)
∀R ∈ SO(3), kvex(Pa (R))k = |sin(γR )|
(12)
¯d = R ˜ T Ωd Ω
(20)
, we
˜ =Ω−Ω ¯d Ω
(21)
˜˙ = R ˜Ω ˜× R
(22)
¯ d and Ω ˜ defined by Let us introduce Ω 3×3
Finally, for a given positive definite matrix M ∈ R denote by λi (M ) its ith eigenvalue and introduce
to finally obtain: λmax (M ) = max {|λi (M )| , i = 1, 2, 3}
(13)
2973
WeCIn4.4 With these notations, we also have ¯˙ d ¯˙ d ) = −Ω× JΩ + Γ − J Ω ˜˙ = J(Ω˙ − Ω JΩ
(23)
¯˙ d = −Ω ˜ ×Ω ¯d + R ˜ T Ω˙d Ω
(24)
with
Using (22), (23) and (24), the system dynamics (14) are rewritten as: R ˜˙ = R ˜Ω ˜×
JΩ ˜˙ = −(Ω ˜ +Ω ¯ d )× J(Ω ˜ +Ω ¯ d) + Γ + J Ω ˜ ×Ω ¯d − JR ˜ T Ω˙d (25) The attitude tracking problem consists now in assigning a feedback control law for the torque Γ that will ensure the ˜ Ω) ˜ to the equilibrium convergence of the tracking error (R, value (I3 , 0). V. C ONTROL LAW DESIGN AND STABILITY ANALYSIS Designing a state feedback control law would require measurements on the orientation R and on the angular velocity Ω. In this paper, we consider the case where no angular velocity measurements are available for control design. To avoid the design of an observer, a partial state feedback controller is proposed. Let us introduce two virtual states Q ∈ SO(3), W ∈ R3 and a virtual input ∆ ∈ R3 for the tracking errors dynamics (25), such that: ˙ ˜=R ˜Ω ˜× R ˜˙ ˜ +Ω ¯ d )× J(Ω ˜ +Ω ¯ d) + Γ + J Ω ˜ ×Ω ¯d − JR ˜ T Ω˙d J Ω = −(Ω Q˙ = −Q W× ˙ =∆ W
Remark 2: From Assumption 1 and from equation (15) it is straightforward to show that the desired angular velocity Ωd and its time derivative are also uniformly continuous and bounded. For control design and stability analysis, let us introduce the Lyapunov function candidate 1 ˜ + 1 kω Ω ˜T JΩ ˜ + 1 k1 tr(I3 − Q) ˜ L = kr tr(I3 − R) 2 2 2 (29) ˜ − 1) + k2 (N (W, Q)
where kr , kω , k1 and k2 are strictly positive scalars, and ˜ is defined by: where the term N (W, Q) q ˜ 2 +1 ˜ = ||W× + Pa (Q)|| (30) N (W, Q) F
Let L(0) denote the initial value of L.
Proposition 1: Consider the system dynamics (28). We define the control torque kr ˜ − k1 vex(Pa (Q)) ˜ vex(Pa (R)) kω kω k2 vex(Pa (M )) + vex(Pa (N )) ¯ d ¯ d ˜ T Ω˙d + + Ω× J Ω + J R ˜ kω N (W, Q) (31) and the virtual input ˜ N (W, Q) ˜ + k3 W× ∆=− vex (k1 + k3 )Pa (Q) 2k2 (32) 1 ˜+Q ˜ T W× − vex W× Q 2 where the matrices M and N are respectively defined by Γ=−
(26)
Defining
˜ = QT R ˜ Q
(27)
the system dynamics become ˜˙ = R ˜Ω ˜× R ˜˙ ˜ +Ω ¯ d )× J(Ω ˜ +Ω ¯ d) + Γ + J Ω ˜ ×Ω ¯d − JR ˜ T Ω˙d J Ω = −(Ω ˜˙ ˜+Q ˜Ω ˜× Q = W× Q ˙ W =∆ (28) Using that new formulation, the attitude tracking problem will be solved by designing a control law (only depending on orientation measurements and on the virtual states) for the ˜ Ω, ˜ Q, ˜ W) torque Γ that will ensure the convergence of (R, to the equilibrium (I3 , 0, I3 , 0). We suppose that the reference to be tracked satisfies the following assumption: Assumption 1: The reference orientation Rd , and its first and second time derivatives are uniformly continuous and bounded.
˜ TQ ˜ M = (W× + Pa (Q))
(33)
˜ T (W× + Pa (Q)) ˜ T N =Q
(34)
and where kr , kω , k1 , k2 and k3 are strictly positive scalars with kr < k1 (35) Consider the Lyapunov function candidate (29). ˜ ˜ Under Assumption 1, for any initial condition R(0), Ω(0), ˜ with Q(0) = R(0) and W (0) = 0, such that L(0) < 2kr
(36)
the control torque (31), along with the virtual input (32), asymptotically stabilizes the system dynamics (28). Proof: Consider the Lyapunov function L defined by (29). It’s time derivative along the trajectories of (28) is given by: 1 ˜Ω ˜ × ) − 1 k1 tr(W× Q ˜+Q ˜Ω ˜ ×) L˙ = − kr tr(R 2 2 ˜ T (−(Ω ˜ +Ω ¯ d )× J(Ω ˜ +Ω ¯ d) + Γ + J Ω ˜ ×Ω ¯ d) + kω Ω ˜ T (J R ˜ T Ω˙d ) − kω Ω . z }| { k2 ˜ T (∆× + Pa (Q)) ˜ + tr 2(W× + Pa (Q)) ˜ 2 N (W, Q) (37)
2974
WeCIn4.4 where .
z }| { ˜+Q ˜Ω ˜× + Q ˜ T W× + Ω ˜ ×Q ˜T ) ˜ = 1 (W× Q Pa (Q) 2
(38)
Using identity (8) and recalling that ∀a, b ∈ R3 , aT (a× b) = 0, we get: 1 ˜ × Pa (R)) ˜ − 1 k1 tr(W× Pa (Q)) ˜ L˙ = − kr tr(Ω 2 2 1 ˜ ˜ × Pa (Q)) − k1 tr(Ω 2 ˜ T (−Ω ¯ d× J Ω ¯d + Γ − JR ˜ T Ω˙d ) + kω Ω ˜ T (−Ω ¯d JΩ ˜ + JΩ ˜ ×Ω ¯ d) + kω Ω × o n 1 k2 ˜ T (Q ˜Ω ˜× + Ω ˜ ×Q ˜T ) + tr (W× + Pa (Q)) ˜ 2 N (W, Q) n o k2 1 ˜ T (W× Q ˜+Q ˜ T W× ) tr (W× + Pa (Q)) + ˜ 2 N (W, Q) o n k2 ˜ T ∆× tr (W× + Pa (Q)) + ˜ N (W, Q) (39) Using (33), (34) and identity (9), we obtain:
Taking ∆ as defined in (32) yields: n o 1 ˜ T Pa (Q) ˜ L˙ = − k1 tr Pa (Q) 2 n o (43) 1 ˜ T (W× + Pa (Q)) ˜ − k3 tr (W× + Pa (Q)) 2 Using identity (10), we finally obtain:
2
2
˜ ˜ L˙ = −k1 vex(Pa (Q))
− k3 vex(W× + Pa (Q))
(44)
Therefore we have L(t) ≤ L(0) and, under Assumption ˜ are uniformly bounded. Using (38) to 1, W× and Ω compute the time derivative of L˙ shows that L¨ is uniformly bounded as well. Hence, L˙ is uniformly continuous and ˜ → 0 Barbalat’s lemma can be applied, leading to Pa (Q) ˜ and W× → −Pa (Q), i.e. W× → 0 and W → 0. ˜ Denote by (γQ˜ , nQ˜ ) the angle-axis coordinates of Q. Using (11) one has: 1 ˜ ≤L k1 tr(I3 − Q) (45) 2 Since L is decreasing, we have L ≤ L(0). Using (36) it yields: k1 (1 − cos(γQ˜ )) ≤ L ≤ L(0) ≤ 2kr (46) k1 (1 − cos(γQ˜ )) =
Using (35), we get: ˜ + k1 Ω ˜ T vex(Pa (Q)) ˜ ˜ T vex(Pa (R)) L˙ =kr Ω kr k2 k2 ˜ T vex(Pa (M )) − ˜ T vex(Pa (N )) (47) 1 − cos(γQ˜ ) < 2 < 2 Ω Ω − ˜ ˜ k1 N (W, Q) N (W, Q) ˜ → 0, we have γ ˜ → 0 or γ ˜ → ±π. The ˜ T (−Ω ¯d JΩ ¯d + Γ − JR ˜ T Ω˙d ) From Pa (Q) + kω Ω × Q Q second possibility is excluded by (47). Therefore we have 1 T d d ˜ ˜ (Ω ¯ J + JΩ ¯ )Ω ˜ − k1 tr(W× Pa (Q)) − kω Ω ˜ → I3 . By (27), it yields R ˜ → Q. By continuity, we get × × Q 2 n o ˙ ˜ ˙ k2 R → Q. Using respectively the first and third equations ˜ T ∆× tr (W× + Pa (Q)) + ˜Ω ˜ × → −QW× . Since R ˜ is ˜ of (28) and (26), one has R N (W, Q) T ˜ ˜ n o orthogonal, we get Ω× → −R QW× . Using W× → 0 k2 ˜ T (W× Q ˜+Q ˜ T W× ) tr (W× + Pa (Q)) + ˜ × → 0, Ω ˜ → 0, and R ˜˙ → 0 from the first it yields Ω ˜ 2N (W, Q) ˙ ˜ → 0 and then, by the (40) equation of (28). By continuity Ω ¯ d ) is antisymmetric second equation of system (28), Γ → (Ω ¯d J + JΩ ¯d + JR ˜ T Ω˙d ). ¯d JΩ Since J = J T , the matrix (Ω × × × ˜ T (Ω ¯d J + JΩ ¯ d )Ω ˜ = 0. Choosing Γ Knowing that P (Q) ˜ and therefore we have Ω and W× converge to zero, one can × × a according to (31) leads to: ensure that, respectively from (33) and (34), M and N converge to zero. Combining the above discussion with the 1 ¯d + JR ˜ T Ω˙d ), equation (31) ensures ¯d JΩ fact that Γ → (Ω ˜ ˙ × L = − k1 tr(W× Pa (Q)) ˜ 2 that Pa (R) → 0. o n k2 T ˜ tr (W× + Pa (Q)) ∆× + ˜ ˜ let us denote N (W, Q) Similarly to the previous analysis on Q, n o ˜ k2 (γR˜ , nR˜ ) the angle-axis coordinates of R. One has: ˜ T (W× Q ˜+Q ˜ T W× ) + tr (W× + Pa (Q)) ˜ 2N (W, Q) 1 ˜ ≤ L ≤ L(0) < 2kr (48) kr (1 − cos(γR˜ )) = kr tr(I3 − R) (41) 2 T ˜ As W× = −W× , and introducing Pa (Q) in the first term: It yields 1 − cos(γR˜ ) < 2 (49) 1 ˜ − Pa (Q)) ˜ T Pa (Q)) ˜ L˙ = k1 tr((W× + Pa (Q) ˜ → 0, we have γ ˜ → 0 or γ ˜ → ±π. The 2 From Pa (R) R R o n k2 T second possibility is excluded by (49). Therefore, we finally ˜ ∆× + tr (W× + Pa (Q)) ˜ ˜ → I3 and R → Rd . Using Ω ˜ → 0 we get Ω → Ω ¯ d, N (W, Q) have R n o T d ˜ Ω . From R ˜ → I3 we finally have Ω → Ωd . k2 i.e. Ω → R ˜ T (W× Q ˜+Q ˜ T W× ) tr (W× + Pa (Q)) + ˜ 2N (W, Q) (42)
2975
WeCIn4.4 Remark 3: The result presented in this paper ensures local asymptotic stability. However, almost global asymptotic stability can be achieved by the proposed control approach. Indeed, it can be shown that, removing conditions (35) and (36), asymptotic closed-loop stability is guaranteed for almost all initial conditions ˜ ˜ ˜ with (R(0), Ω(0), Q(0), W (0)) ∈ / U, where the set 3 3 U ⊂ SO(3) × R × SO(3) × R is defined as Uo = n ˜ Ω, ˜ Q, ˜ W )|tr(R) ˜ = −1, Ω ˜ = 0, tr(Q) ˜ = −1, W = 0 . (R, The complete proof corresponding to this result is too long to be presented in this paper, but arguments are similar to those used in previous work done by some of the authors and can be found in [9]. VI. G UARANTEED B OUND ON Γ By the proposed control law, a bound on the resulting control torque Γ can be guaranteed, as presented in the following Proposition. Proposition 2: Under Assumption 1, the control torque defined by (31) is uniformly bounded. Proof: From (31) and by triangular inequality, we get:
k
kr
1 ˜ ˜ kΓk ≤
vex(Pa (R))
vex(Pa (Q))
+
kω kω k2 (kvex(Pa (M ))k + kvex(Pa (N ))k) + ˜ kω N (W, Q)
T d
d ¯ JΩ ¯ d + ˜ Ω˙ + Ω
J R × (50) Using the angular-axis notations previously defined for ma˜ and Q, ˜ along with (12), it yields: trices R kr k1 kΓk ≤ |sin(γR˜ )| + sin(γQ˜ ) kω kω k2 + (kvex(Pa (M ))k + kvex(Pa (N ))k) ˜ kω N (W, Q)
d
T d ¯ ×J Ω ¯ d + ˜ Ω˙ + Ω
J R
(51)
Under Assumption 1 and using notation (13), we get:
k1 k2 kvex(Pa (M ))k + kvex(Pa (N ))k kr + + ˜ kω kω kω N (W, Q)
d 2
+ λmax (J) sup Ω + λmax (J) sup Ω˙ d (52) ˜ Using identity Consider the term kvex(Pa (M ))k /N (W, Q). (10) and definition (6) one has: 1 1 2 kvex(Pa (M ))k = tr(Pa (M )T Pa (M )) = ||Pa (M )||F2 2 2 (53) From identity (8), we have:
From definitions (33) and (6), and using the fact that ∀A, B ∈ ˜ = I3 , we obtain: ˜T Q Rn×n , tr(AB) = tr(BA) along with Q 1 kvex(Pa (M ))k ≤ √ 2
(54)
Substituting in equation (53), it yields: 1 1 2 ||M ||F2 − ||Ps (M )||F2 ≤ ||M ||F2 kvex(Pa (M ))k = 2 2 (55)
(56)
and finally ˜ 1 1 kvex(Pa (M ))k ||W× + Pa (Q)|| F ≤√ q ≤√ ˜ 2 ||W + P (Q)|| 2 N (W, Q) ˜ 2 +1 × a F
(57) In the same way, an upper bound can be found for the term ˜ Hence, expression (52) leads to kvex(Pa (N ))k /N (W, Q). kΓk ≤
2 2k2 kr + k1
˙ d
+ √ + λmax (J) sup Ωd + sup Ω
kω kω 2 (58)
and the control torque Γ is uniformly bounded.
Remark 4: It is clear from (58) that the gains of the control law can be tuned to satisfy any bound B > 0 on the con trol torque that verifies B > λmax sup ||Ωd ||2 + sup ||Ω˙ d || . The strict inequality provides sufficient margin for a choice of gains kr , k1 and k2 small and kω large. In practice, it is also required that kr < k1 (a trivial choice) and that L(0) < 2kr . The second condition is more difficult to guarantee, since increasing kω increases L(0). This combination of conditions does place an effective bound on the ball of initial conditions that we guarantee to lie in the domain of attraction of the controller. The lower bound on required torque ensures that the rigid-body has sufficient control authority to follow the demanded trajectory Rd . If sufficient control authority is not available, the simplest alternative is to choose a less aggressive trajectory Rd . This could be d done, for example, by adjusting the time-scale for Rnew (t) = d d 2 d R (µt) for µ ≤ 1. One has sup ||Ωnew || + sup ||Ω˙ new || ≤ µ2 sup ||Ωd ||2 + sup ||Ω˙ d || , where Ωdnew is defined according to (15). By choosing a sufficiently slow reference trajectory then all the terms on the right hand side of (58) may be adjusted arbitrarily and it is possible to find gains kr , k1 , k2 , kω and scaling factor µ such that the theorem conditions are satisfied for arbitrary initial conditions.
kΓk ≤
||M ||F2 = ||Pa (M )||F2 + ||Ps (M )||F2
r n o ˜ T (W× + Pa (Q)) ˜ tr (W× + Pa (Q))
VII. S IMULATIONS Simulation results are provided in this section to illustrate the stabilizing property of the proposed approach. The following values have been considered for the rigid body inertia and the control law parameters: J = diag(9.3, 9.3, 6.3) kg.m2 , kr =4, kω = 0.9, k1 = 14, k2 =1.6, k3 =14. We assume that the reference trajectory to be tracked is defined for all t ≥ 0 by θ(t) = φ(t) = a sin(b t) and ψ(t) = c cos(d t), with a =10 deg, b =0.3 rad.s−1 , c = 5 deg and d = 0.15 rad.s−1 . The initial conditions for attitude angles and angular velocities are φ0 =10 deg, θ0 = -10 deg, ψ0 = -5 deg, ωp0 = -0.5 deg.s−1 , ωq0 = -2 deg.s−1 , ωr0 = 3 deg.s−1 . Attitude angles and angular velocities (solid curves) are represented on Figure 1 along with their reference values (dashed curves). Tracking errors
2976
WeCIn4.4 10
−5
0 −2 −4
ωp − ωdp (deg/s)
0
2 φ − φd (deg)
ωp, ωdp (deg/s)
d
φ, φ (deg)
5
5
0
0
−5
−6
−10 0
5
4
10
5
10
15
−8 0
20
15
5
10
15
−5 0
20
15
5
10
15
−10 0
20
5
5
10
15
20
5
10
15
20
5
10
15
20
10
0 −5
10 5
0
0
ωq − ωdq (deg/s)
5
θ − θd (deg)
ωq, ωdq (deg/s)
d
θ, θ (deg)
10
−5 −10
5
0
−10 −15 0
5
10
15
−5 0
20
5
10
15
−15 0
20
15
5
10
15
−5 0
20
10
15
10
15
20
−5 0
5
t (s)
Fig. 1.
−5 −10
10
15
−15 0
20
5
10
t (s)
15
10 5 0 −5 0
20
t (s)
Attitude angles and angular velocities
t (s)
Fig. 2.
and components of the control torque are respectively plotted in Figures 2 and 3. As can be seen, stability around the reference trajectory is achieved with a satisfying closed loop behavior.
5
4
1
4
3
0
3
2
−1 −2 −3 −4 0
VIII. C ONCLUSION
2 1 0
5
10 15 t (s)
20
−1 0
Fig. 3.
A solution to the attitude tracking problem of rigid bodies has been presented in this paper. Contrary to most of existing works, the controller has been designed by using an attitude representation directly on the special orthogonal group of rotation matrices. For control design, we have considered the case where no angular velocity measurements are available. A dynamic partial state feedback controller has been proposed based on the introduction of two virtual states in the system dynamics. Using Lyapunov analysis, stability conditions have been derived. A guaranteed bound on the resulting control torque has also been computed. Simulations results have been presented to illustrate the stabilizing property of the proposed controller. R EFERENCES [1] M. R. Akella, ”Rigid Body Attitude Tracking without Angular Velocity Feedback”, in Systems & Control Letters, vol. 42, pp 321-326, 2001. [2] A. Astolfi and M. Lovera, ”Global Spacecraft Attitude Control using Magnetic Actuators”, in Proceedings of the American Control Conference, Anchorage, USA, pp 1331-1335, 2002. [3] F. Bullo and R. M. Murray, ”Tracking for Fully Actuated Mechanical Systems: a Geometric Framework”, in Automatica, vol. 35, pp 17-34, 1999. [4] F. Caccavale and L. Villani, ”Output Feedback Control for Attitude Tracking”, in Systems & Control Letters, vol. 38, pp 91-98, 1999. [5] B. T. Costic, D. M. Dawson, M. S. de Queiroz and V. Kapila, ”A Quaternion-Based Adaptive Attitude Tracking Controller Without Velocity Measurements”, in Proceedings of the 39th IEEE Conference on Decision and Control, Sydney, Australia, pp 2424-2429, 2000. [6] P. E. Crouch, ”Spacecraft Attitude Control and Stabilization: Applications of Geometric Control Theory to Rigid Body Models”, in IEEE Transactions on Automatic Control, vol. 29, no. 4, pp 321-331, 1984. [7] H. Krishnan, N. H. McClamroch and M. Reyhanoglu, ”Attitude Stabilization of a Rigid Spacecraft Using Two Momentum Wheel Actuators”, in Journal of Guidance, Control, and Dynamics, vol. 18, no. 2, pp 256-263, 1995.
Tracking errors
2
Γ2 (N.m)
5
0
0
Γ1 (N.m)
−10 0
5
Γ3 (N.m)
0 −5
5
10
ωr − ωdr (deg/s)
5
ψ − ψd (deg)
ωr, ωdr (deg/s)
d
ψ, ψ (deg)
10
1 0 −1
5
10 15 t (s)
20
−2 0
5
10 15 t (s)
20
Control inputs
[8] F. Lizarralde and J. T. Wen, ”Attitude Control without Angular Velocity Measurement: A Passivity Approach”, IEEE Transactions on Automatic Control, vol. 41, no. 3, pp 468-472, 1996. [9] R. Mahony, T. Hamel and J. M. Pflimlin, ”Nonlinear Complementary Filters on the Special Orthogonal Group”, in IEEE Transactions on Automatic Control, vol. 53, no. 5, pp 1203-1217, 2008. [10] D. H. S. Maithripala, J. M. Berg and W. P. Dayawansa, ”An Intrisic Observer for a Class of Simple Mechanical Systems on a Lie Group”, in Proceedings of the 2004 American Control Conference, Boston, USA, pp 1546-1551, 2004. [11] D. H. S. Maithripala, J. M. Berg and W. P. Dayawansa, ”AlmostGlobal Tracking of Simple Mechanical Systems on a General Class of Lie Groups”, in IEEE Transactions on Automatic Control, vol. 51, no. 1, pp 216-225, 2006. [12] P. Morin, C. Samson, J.-B. Pomet and Z.-P. Jiang, ”Time-varying Feedback Stabilization of the Attitude of a Rigid Spacecraft with Two Controls”, in System & Control Letters, vol. 25, pp 375-7385, 1995. [13] S. Salcudean, ”A Globally Convergent Angular Velocity Observer for Rigid Body Motion”, in IEEE Transactions on Automatic Control, vol. 36, no. 12, pp 1493-1497, 1991. [14] A. Tayebi, ”Unit Quaternion-Based Output Feedback for the Attitude Tracking Problem”, in IEEE Transactions on Automatic Control, vol. 53, no. 6, pp 1516-1520, 2008. [15] P. Tsiotras, ”Further Passivity Results for the Attitude Control Problem”, IEEE Transactions on Automatic Control, vol. 43, no. 11, pp 1597-1600, 1998. [16] J. T.-Y. Wen and K. Kreutz-Delgado, ”The Attitude Control Problem”, in IEEE Transactions on Automatic Control, vol. 36, no. 10, pp 11481162, 1991. [17] B. Wie, H. Weiss and A. Arapostathis, ”A Quaternion Feedback Regulator for Spacecraft Eigenaxis Rotations”, AIAA Guidance, Navigation and Control Conference, Minneapolis, USA, 1988. [18] H. Wong, M. S. de Queiroz and V. Kapila, ”Adaptive Tracking Control Using Synthesized Velocity from Attitude Measurements”, in Automatica, vol. 37, no. 6, pp 947-953, 2001.
2977
