PD+ Based Output Feedback Attitude Control of Rigid Bodies Rune Schlanbusch, Antonio Loria, Raymond Kristiansen and Per Johan Nicklasson
Abstract—We address the problem of output feedback attitude control of a rigid body in quaternion coordinate space via a modified PD+ based tracking controller. Angular velocity is replaced by a low-gain dynamic extension. The controller ensures fast convergence to the desired operating point during transient maneuvers, while keeping the gains small. This contributes to diminishing the sensitivity to measurement noise hence, energy consumption may be expected to drop along with a decrease of the residual. More precisely, we show uniform practical asymptotic stability of the equilibrium point for the closed loop system in the presence of unknown, bounded input disturbances. Simulation results illustrate the performance improvement with respect to PD+ based output feedback control with static gains.
I. INTRODUCTION Attitude control on the rotational sphere is an interesting theoretical problem since, due to the parameterization of the attitude by the unit quaternion, the mapping from S 3 to SO(3) is two-to-one. In other words, the open-loop system has two equilibria. To deal with multiple equilibria using continuous feedback one must choose a target equilibrium before starting a maneuver. However, if a target equilibrium is fixed before the maneuver the control design usually relies on the hypothesis that the sense of rotation does not change during the maneuver. Mathematically, this means that quaternion attitude coordinates can only take values in “half of the sphere” –see for instance [1]. Assuming that the sense of rotation remains constant is generally not restrictive –see [1]. It is an ad hoc hypothesis which may be showed to hold for a stabilizing feedback using standard Lyapunov arguments –see [2], under a simple restriction on the initial posture and the reference equilibrium which must be compatible with the quaternion constraint –see Section II-A. From a theoretical viewpoint, one is able to show asymptotic stability on a domain of attraction arbitrarily large strictly contained in “half of the sphere”. In other words, this is reminiscent of the property of semiglobal asymptotic stability for systems evolving in Rn . Obtaining stability on S 3 (which we call stability in the Large) or on a domain of attraction contained in it as we do in this paper, changes little for practical purposes since both properties imply asymptotic stability from “feasible” initial postures. In this note we do not assume that the sense of rotation cannot change. Smooth attitude tracking control (using continuous controllers) naturally builds upon a bulk of literature on tracking control of robot manipulators –see [3]. A classic in robot control literature is the PD+ controller of Paden and Panja [4] which, together with the Slotine and Li controller [5], was the first algorithm for which global asymptotic stability was demonstrated. A PD+ based controller for spacecraft was presented in [6], called model-dependent controller, and more recently for leader-follower spacecraft formation in [1]. An angular velocity observer for rigid-body motion is presented in [7] using unit quaternions; passivity-based control is used in [8] to ensure asymptotic stability without need of a model-based velocity Rune Schlanbusch, Raymond Kristiansen and Per Johan Nicklasson are with Department of Technology, Narvik University College, PB 385, AN8505 Narvik, Norway {runsch, rayk, pjn}@hin.no Antonio Lor´ıa is with LSS-SUPELEC, CNRS; 3, Rue Joliot Curie, 91192 Gif s/Yvette, France
[email protected]
observer. In [9] two schemes are presented, based on results for output control of robot manipulators (cf. [10]). In the first scheme, a second-order model-based observer is adopted for estimation of the angular velocity while the second scheme is based on a lead-filter for estimation of the angular velocity error. An alternative approach using Modified Rodrigues Parameters (MRP) is reported in [11]. Output feedback was further pursued for spacecraft control in [12], where the authors present an adaptive controller. Other unit-quaternion based results include [13] which includes an observer-based linear control law; asymptotic stability of the equilibrium point is showed. A generalization to almost “global” asymptotic stability1 is obtained for a similar approach in [14] and almost “global” exponential stability for stabilization on SE(3) in [15]. In this note we propose smooth modified PD+ based output feedback controllers, one of which includes nonlinear gains with exponential decay –cf. [16]. In the latter reference nonlinear gains are used to stabilize an inverted 3D pendulum with control saturation effects, via state-feedback. The control gains that we use here involve exponential functions but with a different purpose than in [16]; in particular, the gains of our controller take large values for large (attitude and estimation) errors in order to ensure fast convergence. Then, the control effort is reduced exponentially in a neighborhood of the reference operating point and damping is increased to reduce overshoot. Consequently, very little control effort is used in stationkeeping tasks, especially in the presence of sensor noise. This can be especially advantageous for an orbiting spacecraft since the major part of its lifetime typically is station-keeping. On the other hand, our approach can make the attitude error converge faster compared to the classical PD+ approach without ”stiffening” the system by increasing the controller gains close to the residual. In contrast, the gain scheduling in [16] involves increasing nonlinear gains of the velocity errors, possibly motivated by the problem studied in that reference, swing-up a 3D pendulum. We show that the origin of the closed-loop system is uniformly practically asymptotically stable with respect to perturbations. This means that there exists a neighborhood of the origin which is Lyapunov stable uniformly in the initial conditions and which also has the property of attracting other trajectories starting away from it, at a convergence rate independent of the initial conditions. The residual set, centered at the origin, depends on the magnitude of the perturbations however, uniform practical asymptotic stability should not be confused with ultimate boundedness which only implies the (non-uniform) convergence of the trajectories and not stability. The definition that we use is adapted from [17] for the purposes of this note, for other definitions see e.g. [18], [19] and [20] for discrete-time systems. Our theoretical findings are validated in simulation for an Earth orbiting spacecraft. Alternatively, allowing for the target equilibrium to change during the maneuver may be achieved at the price of additional complexity in the controller e.g., via hybrid control. See for instance [21] on control of an under-actuated non-symmetric rigid body and [22] where the authors present two quaternion-based hybrid controllers. 1 Global is used in accordance with the literature however a better terminology would be almost in the large, whence the quotes.
2
In the following section we present our main results, which are illustrated in simulation in Section III; concluding remarks are given in Section IV. II. M AIN RESULT A. The model The attitude of a rigid body is represented by a rotation matrix R ∈ SO(3) = {R ∈ R3×3 : R⊤ R = I, det(R) = 1}. which is the special orthogonal group of order three. Quaternions are used to parameterize members of SO(3) where the unit quaternion is defined as q = [η, ǫ⊤ ]⊤ ∈ S 3 = {x ∈ R4 : kxk = 1}, η ∈ R and ǫ ∈ R3 . The system’s kinematics is modeled by the differential equation q˙ i,b = T(qi,b )ω bi,b ,
(1)
where the symbol ω cb,a denotes angular velocity of frame a relative to frame b, expressed on the frame c and 1 −ǫ⊤ i,b T(qi,b ) = ∈ R4×3 , (2) 2 ηi,b I + S(ǫi,b ) where S(a)b denotes the cross product operation a × b hence, S(·) is skew-symmetric. The dynamic model of a rigid body is derived from Euler’s moment equation, which describes the relationship between applied torque and angular momentum on a rigid body as –see [23], Jω ˙ bi,b = −S(ω bi,b )Jω bi,b + τ b
(3)
where J = diag{jx , jy , jz } ∈ R3×3 is the rigid body inertia matrix and τ b ∈ R3 is the total input torque expressed on the body frame. The total torque working on the body is τ b = τ ba + τ bd , where τ bd is the disturbance torque, and τ ba is the actuator (control) torque. B. Problem Formulation ⊤ Let qi,d = [ηi,d , ǫ⊤ i,d ] , t 7→ qi,d denote a reference attitude trajectory, and let the following hypothesis hold. Assumption 2.1: The desired trajectory qi,d satisfies the quater2 nion constraint (i.e., ηi,d +ǫ⊤ i,d ǫi,d = 1). The desired angular velocity b ω i,d , the disturbance moments τ bd , the angular acceleration ω˙ bi,d and the inertia matrix J are bounded and the latter is symmetric positive definite. ˜ = [˜ The tracking error in quaternion coordinates, q η , ˜ǫ⊤ ]⊤ is ηi,d ηi,b + ǫ⊤ i,d ǫi,b ˜ := q ¯ i,d ⊗ qi,b = q , (4) ηi,d ǫi,b − ηi,b ǫi,d − S(ǫi,d )ǫi,b
¯ = [η, − ǫ⊤ ]⊤ corresponds to the inverse rotation of q. where q ˜ and −˜ The quaternion representation, q q represent the same physical attitude but correspond to different equilibria. Direct computations show that k˜ qk satisfies the quaternion constraint i.e., η˜2 + ˜ ǫ⊤ ˜ǫ = 1. The desired angular velocity of a rigid body is usually given with reference to the inertial frame as ω ii,d . By rotating to the body frame we obtain ω bi,d = Rbi ω ii,d (5) hence, the quaternion reference velocities satisfy q˙ i,d = T(qi,d )ω bi,d and the quaternion error velocities may be expressed as –see [24], ˜˙ = T(˜ q q) ω bi,b − ω bi,d . (6) Now, for the purpose of establishing meaningful stability properties we define eq± = [1 ∓ η˜, ˜ǫ⊤ ]⊤ ,
eω = ω bi,b − ω bi,d .
(7)
so the error trajectories satisfy the kinematics relation 1 ±˜ǫ⊤ . e˙ q± = Te (eq± )eω , Te (eq± ) = 2 η˜I + S(˜ǫ)
(8)
We define two sets, each one of which corresponds to the rotational 3 ˜ ∈ S 3 } and sphere, as eq+ ∈ Se+ := {[1 − η˜, ˜ǫ⊤ ]⊤ : η˜ ≥ 0, q 3 ⊤ ⊤ 3 ˜ ∈ S }. Then, eq± ∈ eq− ∈ Se− := {[1 + η˜, ˜ ǫ ] : η˜ < 0, q 3 3 ˜ ∈ S 3 }. We pursue the Se+ ∪ Se− = Se3 := {[1 − |˜ η |, ˜ ǫ⊤ ]⊤ : q following stability property for the closed-loop system with tracking errors defined in (7). The enunciated definition given below is adapted from [17, Definition 8] for the purposes of this note. Definition 2.1: Let Θ ⊂ Rm be a set of parameters. The system x˙ = f (t, x, θ) , where x ∈ Rn , t ≥ 0, θ ∈ Rm is a constant parameter and f (t, x, θ) is locally Lipschitz in x and piecewise continuous in t for all θ ∈ Rm , is said to be uniformly practically asymptotically stable if given any ∆ > δ > 0, there exists θ⋆ (δ, ∆) ∈ Θ such that Bδ is uniformly asymptotically stable on B∆ for the system2 x˙ = f (t, x, θ⋆ ). Remark 2.1: The property above implies but is not implied by ultimate boundedness. The definition is adapted from [17] –we removed the qualifier “semiglobal” which pertains to systems evolving in Rn . However, note that the parameter θ depends on the residual set Bδ and the estimate of the domain of attraction B∆ . In the context of the present note θ corresponds to design parameters. C. Controller-Observer Design Following [6] a logical choice of state-feedback PD+ control law for the rigid body is τ ba = Jω˙ bi,d − S(Jω bi,b )ωbi,d − kp T⊤ e e q − kd e ω
(9)
where, from (5), ω˙ bi,d = −S(ωbi,b )ω bi,d + Rbi ω˙ ii,d which depends on the angular velocities ω bi,b . It may be showed that the previous control law stabilizes the origin of the closed-loop system, (eq , eω ) = (0, 0) provided that kp and kd are positive. In anticipation to our main result, we introduce two important changes to the control law in (9). Firstly, we replace the constant gains kp and kd by a particular choice of positive definite functions: ⊤ ⊤ kp ek1 eq eq and kd e−k2 eq eq , where k1 and k2 determine the rapidity of change in the variable gains. Secondly, for the purpose of positionfeedback control we use the modified acceleration vector ad = −S(ω bi,d )ω bi,d + Rbi ω˙ ii,d = Rbi ω˙ ii,d
(10)
which is independent of the unmeasured velocities –cf. [9]. Then, consider the control law without velocity measurements, ⊤
⊤
−k2 eq eq b ω d,e τ ba = Jad − S(Jω bi,e )ωbi,d − kp ek1 eq eq T⊤ e e q − kd e (11)
where ω bd,e = ω bi,e − ω bi,d and ω bi,e is a velocity estimate generated by the observer ⊤ k1 e ⊤ q eq T⊤ e z˙ =ad + J−1 lp ek3 eeq eeq T⊤ (12) eq eeq − kp e e q , ω bi,e =z + 2J−1 ld T⊤ eq eeq ,
lp , ld , k3 ∈ R>0 .
(13)
Remark 2.2: In [25], [16] the authors propose a controller involving proportional and derivative feedback via control gains that consist of positive definite functions of the positions and velocities respectively. The case of exponential functions is underlined in [25] however, the exponential in the latter reference increases with the 2 The set B is uniformly asymptotically stable on B ∆ if there exists a δ number ∆ and a class KL function β such that kx(t0 )kδ < ∆, t0 ∈ R>0 =⇒ kx(t)kδ ≤ β(kx(t0 )kδ , t − t0 ), where kxkδ := inf z∈Bδ kx − zk.
3
norm of the velocities –see also3 [26]. Here, we use a damping gain which decreases with the norm of attitude errors as opposed to the exponentially increasing function of velocity errors, used in [25]. Our strategy ensures small damping far from the origin and higher damping close to it hence, diminishing overshoot. Nonetheless, the damping policy is related to the nature of the problem at hand and the system under analysis; the approach of [25] is specific to the swing-up problem of the 3D pendulum. We are ready to present our main result. Define the velocity estimation error eeω = ω bi,b − ω bi,e and the attitude estimation error ⊤ ¯ i,e ⊗ qi,b . Again, in quaternion coordinates qe,b = [ηe,b , ǫ⊤ e,b ] = q ⊤ 3 for the purpose of analysis, we define eeq = [1 − ηe,b , ǫ⊤ e,b ] ∈ Se+ which satisfies a kinematic equation of the form (8) with Te replaced by Teq and other obvious substitutions. Note that we do not define a negative counterpart for eeq , the filter can always be initialized such that ηe,b (t0 ) = 1 since the state is measured. Proposition 2.1: Let Assumption 2.1 hold and let eq be defined either by eq = eq+ or eq = eq− . Then, the closed-loop system is uniformly practically asymptotically stable on R7>0 . More precisely, for any positive numbers δ, a, b, any δη ∈ (0, 1) and any subset Γ 3 of R = {x ∈ Se3 × R3 × Se+ × R3 : |˜ η | > 0, keω k ∈ [0, a), ηe,b ≥ δη > 0, keeω k ∈ [0, b)} there exist control gains in R7>0 such that the set Bδ = {x ∈ R : kxk < δ} is uniformly stable for all initial conditions in Γ. Furthermore, all trajectories originating in Γ converge to Bδ asymptotically and uniformly in the initial conditions. Proof: Without loss of generality let eq = eq+ , Te = Te (eq+ ), ⊤ ⊤ ⊤ ⊤ and x := [e⊤ q , eω , eeq , eeω ] . The error dynamics takes the form x˙ = f (t, x) with h i⊤ f (t, x) = (Te eω )⊤ (J−1 ξ1 )⊤ (Teq eeω )⊤ (J−1 ξ2 )⊤ , ξ1 =S(Jω bi,b )eω + S(Jeeω )ω bi,d − kp e − kd e
−k2 e⊤ q eq
k1 e ⊤ q eq
T⊤ e eq
ξ2 =S(Jω bi,b )eω −kd e
eω +S(Jeeω )ω bi,d +kd e
− ld [ηe,b + S(ǫe,b )]eeω − lp e
k3 e ⊤ eq eeq
T⊤ eq eeq
−k2 e⊤ q eq
eeω
i,d
i,d
x∈R14 2
we have eckxk − 1 ≥ kxk2 hence V(x) ≤ α(x) holds with 2 α(x) := c1 ec2 kxk − 1
(15)
where c1 := 2 max {kp /k1 , lp /k3 , 2βJ , 4λβJ } and c2 := max{k1 , k3 , 1}. Next, we remark that ex ≥ 1 + x hence ⊤ e k 1 e q e q − 1 ≥ k1 e ⊤ (16) q eq , ⊤
idem for ek3 eeq eeq − 1. Then, α(x) ≤ V(x) with α(x) := pm kxk2 where pm > 0 is a lower bound on the induced norm of kp I λTe J 0 0 1 λJT⊤ J 0 0 e . (17) P := 0 0 lp I λTeq J 2 ⊤ 0 0 λJTeq J
Now we evaluate the total time derivative of V along the closedloop trajectories. To that end, we first compute the derivative of V . We have ⊤
⊤ b V˙ = − kd e−k2 eq eq e⊤ ω eω + eω S(Jeeω )ω i,d
−
eω JS(ωbb,d )ωbi,d
+
b e⊤ eω S(Jω i,b )eω
+ eeω S(Jeeω )ω bi,d − ld ηe,b − kd e
(18)
+
e⊤ ω
−k2 e⊤ q eq
Since the matrix S(·) is linear in its arguments,
e⊤ eω
+ τ d. e⊤ eω eeω
kS(Ja)bk ≤ βJ kakkbk.
(19)
1 2 2 b e⊤ ω S(Jeeω )ω i,d ≤ βJ βω b (keω k + keeω k ) i,d 2 eω JS(ω bb,d )ω bi,d ≤βJ βωb keω k2
(20) (21)
i,d
+ τ d.
The rest of the proof consists in showing that the conditions of [17, Theorem 10] hold restricted to the domain of interest4 . Let Assumption 2.1 generate positive real numbers βj , βJ , βd , βωb , βω˙ b
ln(kxk2 + 1) =1 kxk2
c ≥ sup
Then, using (19), Young’s inequality and Assumption 2.1 we obtain
(eω − eeω ) − JS(ω bb,d )ω bi,d + τ d , −k2 e⊤ q eq
Furthermore, for any
such that βj ≤ kJk ≤ βJ , kτ bd (t)k ≤ βd , kω bi,d (t)k ≤ βωb
i,d
and kω˙ bi,d (t)k ≤ βω˙ b for all t ≥ t0 ≥ 0. Given any δ, δη , a and i,d b let them generate a set R as in the statement and consider the Lyapunov function candidate V : R → R, V(x) = V (x) + λW (x) where 1 h kp k1 e⊤ V (x) = e q eq − 1 + e⊤ ω Jeω 2 k1 i l ⊤ p k3 eeq eeq e − 1 + e⊤ + eω Jeeω , k3 ⊤ W (x) = e⊤ q Te Jeω + eeq Teq Jeeω
We show that there exist functions α, α ∈ K∞ such that α(x) ≤ V(x) ≤ α(x). Firstly, we observe that 2 kp l p emax{k1 ,k3 }kxk − 1 + kxk2 . , , 2βJ , 4λβJ V ≤ max k1 k3 (14) 3 In this reference it is assumed that full-state feedback is available from measurement. 4 That is, with the obvious modifications. Strictly speaking, we cannot show that the conditions of [17, Theorem 10] (which is tailored for systems defined on Rn ) hold for arbitrarily large initial conditions in view of the topology of S3.
1 b 2 2 e⊤ eω S(Jω i,b )eω ≤ βJ (keω k + keeω k )(keω k + βω b ) i,d 2 eeω S(Jeeω )ω bi,d ≤βJ βωb keeω k2 . i,d
(22) (23)
Next, we invoke the quaternion constraint to observe that e⊤ q eq ≤ 2 and we insert the bounds (20)–(23) into (18) to obtain V˙ ≤ − φ(kd , keω k)keω k2 − ψ(kd , ld , keω k)keeω k2 + βd (keω k + keeω k), φ(kd , keω k) ψ(kd , ld , keω k) We see that φ and ψ |ηe,b | ≥ δη . Further derivative of V along
=
(24)
kd e−2k2 − βJ 6βωb + keω k (25a) i,d
1 = ld δη − kd − βJ (2βωb + keω k).(25b) i,d 2 are positive definite for all keω k ∈ [0, a) and direct computations show that the total time the closed-loop trajectories yields5
˙ V(x) ≤= −x⊤ Q(ω bi,b )x + 2βd kxk where Q = [qij ], i, j = 1, 2, 3, 4, q24 = q11 = λTe kp e q12 = q⊤ 21 =
k1 e ⊤ q eq
q⊤ 42
= q13 =
T⊤ e , q33 = λTeq lp e
(26) q⊤ 31
k3 e ⊤ eq eeq
=0
T⊤ eq
i ⊤ λ h Te kd e−k2 eq eq I − S(Jω bi,b ) − JS(ω bi,d ) 2
5 Clearly, Q depends on other variables besides ω b i,b however, only the dependence on the latter is made explicit for further use and to avoid a cumbersome notation.
4
h i ⊤ λ Te S(ω bi,d )J − kd e−k2 eq eq I 2 λ q22 = φ(kd , keω k) − [˜ η I + S(˜ǫ)] J 2 i ⊤ λh q23 = q⊤ kd e−k2 eq eq I − S(Jω bi,b ) T⊤ 32 = eq 2 n o ⊤ λ Teq S(ω bi,d )J+ld [ηe,b I+S(ǫe,b )]−kd e−k2 eq eq I q34 = q⊤ 43 = 2 λ q44 = ψ(kd , ld , keω k) − [ηe,b I + S(ǫe,b )]J. 2 We claim that there exist lower and upper bounds qij,m and qij,M on the norms of each sub-block qij of Q defined above. This follows under Assumption 2.1, for all x ∈ R and in view of the fact that 1 ⊤ ⊤ e eq (27) e⊤ q Te Te eq ≥ 8 q which is implied by the quaternion constraint. On one hand, keω k ∈ [0, a) and Assumption 2.1 imply that ω bi,b = eω + ω bi,d satisfies kω bi,b k ≤ ∆ with ∆ := ∆ω + βωb . On the other hand, to see that i,d (27) holds note that 1 ⊤ ⊤ ˜ e⊤ ǫ ˜ǫ (28) q Te Te eq = 4 and, in view of (7), 1 1 (1 − η˜)2 + ˜ǫ⊤ ˜ ǫ = e⊤ eq . (29) 8 8 q Now assume that (27) does not hold that is, from (28) and (29) 1 ⊤ 1 ˜ǫ ˜ǫ < (1 − η˜)2 + ˜ǫ⊤ ˜ǫ (30) 4 8 which is equivalent to (1 − η˜)2 > ˜ǫ⊤ ǫ˜ and, in view of the quaternion constraint, holds if and only if 2˜ η (1 − η˜) > 0. In its turn, the latter holds only if η˜ 6∈ [0, 1] which is impossible. Therefore, the claim follows by applying the triangle inequality on each cross term x⊤ k qi,j xl for all suitable indexes. h i Next, define kp⋆ := 2jM ∆, lp⋆ := 2 jm (∆ + βωb ) + ld ; let i,d kp > kp⋆ , lp > lp⋆ , ( φ(kd , ∆ω ) λ ≤ min , kd + 12 jM (1 + 2∆ + βωb ) i,d ) ψ(kd , ld , ∆ω ) ,1 , kd + 12 jM (1 + 2βωb + ld ) q14 = q⊤ 41 =
i,d
and let qm (∆) > 0 be a lower bound on the smallest eigenvalue of Q(∆). It follows that V˙ ≤ −qm kxk2 + 2βd kxk
(31)
hence, provided that qm ≥ 2βd /δ we have V˙ < 0 for all x ∈ R ∩ H 3 where H := {x ∈ Se3 × R3 × Se+ × R3 δ ≤ kxk ≤ ∆}. According to [17, Theorem 10] we verify the growth conditions on α−1 ◦ α(s) and α−1 ◦ α(s). Given any positive constants δ ⋆ , ∆⋆ such that δ ⋆ < ∆⋆ , there exist control gains generating 0 < δ < ∆ such that s c1 (∆) ec2 δ2 − 1 −1 α ◦ α(δ) = ≤ δ⋆ . (32) qm This holds because δ decreases while c1 and qm increase monotonically with the gains, and c2 is independent of δ, c1 and qm . On the other hand, ∆, c1 and qm may be increased with the control gains to satisfy v u u ln qm ∆2 +1 t c1 (∆) −1 ≥ ∆⋆ (33) α ◦ α(∆) = c2
The statement follows from [17, Theorem 10] by restricting the domain of attraction. The proof for the negative equilibrium point, determined by eq− , Te (eq− ) follows along similar lines. Remark 2.3: The choice of exponential functions in the definition of the control gains kp and kd is made for convenience and in view of the context –see the simulation results. It is clear from the previous proof that beyond this particular choice, the main results hold if the ⊤ ⊤ terms kp ek1 eq eq and kd e−k2 eq eq are replaced by positive definite functions kp (·) and kd (·) as in [25], [16]. However, we sacrifice generality for clarity of exposition. For the purpose of comparison we have the following proposition for a controller with constant proportional and derivative gains. Corollary 2.1: Consider the controller of Proposition 2.1 under the same conditions and let k1 = k2 = k3 = 0. Then, the closed-loop system is uniformly practically asymptotically stable. Remark 2.4: Note that instead of using ω bi,d in the modified acceleration vector (10), the estimated angular velocity ω bi,e might be used at the price of restricting ld further. III. SIMULATION RESULTS We present some simulation results for a spacecraft in an elliptic Low Earth Orbit (LEO). The objective of the simulation study is to compare the output feedback controller with state-dependent gains –see Proposition 2.1 against the output feedback controller with static gains –see Corollary 2.1 and the state feedback variant – see (9). For the sake of a fair comparison the parameters have been purposely tuned so as to recover similar performance for all three controllers, in the absence of noise and perturbations. The performance improvement of the output feedback controller with state-dependent gains compared to the controllers with static gains is clear from the second test, where noise and perturbations are considered. We use the functionals Z tf Z tf Z tf ˜ Jq = ǫ⊤ ˜ǫdt, Jeq = ǫ⊤ e⊤ e,b ǫe,b dt, Jω = ω eω dt, t0 t0 t0 Z tf Z tf b τ b,⊤ e⊤ Jeω = a τ a dt, eω eeω dt, Jp = t0
t0
where t0 and tf defines the start and end of the simulation window, respectively. The functionals Jq /Jω and Jeq /Jeω correspond to the integral functional errors of the attitude/angular velocity between body and desired frame, and between body and estimated frame, respectively. Jp denotes the integral applied squared control torque. The simulations were performed in Simulink using a fixed sampletime Runge-Kutta ODE4 solver with 10−2 s step size. The moments of inertia were J = diag{4.35, 4.33, 3.664} kgm2 , and the spacecraft orbit had perigee at 600 km, apogee at 750 km, inclination at 71◦ , and the argument of perigee and the right ascension of the ascending node at 0◦ . We chose the initial conditions qi,b = qi,e = [0.3772, − 0.4329, 0.6645, 0.4783]⊤ , ω bi,b = [0.1, 0.2, − 0.3]⊤ rad/s, z = [0 0 0]⊤ , t0 = 0 s and tf = 15 s. The control laws were tuned to achieve similar performance in the absence of disturbances. Hence, for the controller in Proposition 2.1 we used kp = 10, kd = 7, lp = 100, ld = 75, and k1 = k2 = k3 = 1 while for the controller of Corollary 2.1 we used kp = 49, kd = 11, lp = 240 and ld = 150. The spacecraft was commanded to follow smooth sinusoidal trajectories around the origin with velocity profile ω ii,d = [3.2 cos(2 × 10−3 t), 0.12 sin(1 × 10−3 t), −3
− 3.2 sin(4 × 10
⊤
−6
t)] × 10
rad/s.
(34)
5
TABLE I VALUES OF PERFORMANCE FUNCTIONALS FOR ATTITUDE MANEUVER .
η˜
2
Jeq 0.0134 0.0131
Jp 96.0 95.9 90.0
0
Jeq 0.013 0.013
Jp 241.1 157.2 209.0
TABLE III AVERAGE VALUE OF PERFORMANCE FUNCTIONALS FOR RIGID - BODY OVER 10, 000 SIMULATIONS .
Static gains Variable gains Controller (9)
Jq 1.674 0.898 1.495
Jω 22.623 24.706 13.896
Jeq 0.176 0.028
Jeω 10.531 13.500
Jp 3692.6 4049.5 3251.1
5
10
15
Static Variable Controller (9)
2 0
0
5
10
15
0
5
10
15
5
10
15
0.2 0.1 0
100 50
Rt 0
0
τ a - Static
Time (s) Fig. 1. Attitude, angular velocity and angular velocity estimation error, and power consumption using PD+ based output feedback with static and variable gains, and state feedback controller (9), during spacecraft attitude maneuver. 20
τx τy τz
0 −20
τ a - Controller (9) τ a - Variable
Simulation results without orbital perturbations and measurement noise are summarized in Table I and visualized in Figure 1. Although the control gains have been purposely tuned to obtain similar performance it is noticeable that the state feedback controller (9) leads to slightly faster transient and lower power consumption compared to both the output feedback controllers. Furthermore, we see from Figure 2 that the output feedback controller with variable gains utilizes lower maximum torque compared to the output feedback controller using static gains, and slightly lower compared to (9). See also the simulations presented in [16] where a similar control law was used except that in the latter reference the derivative gain had exponential growth in the velocity measurements –cf. Eq. (11). In a second run of simulations we introduced measurement noise as σBn = {x ∈ Rn : kxk ≤ σ}, which was added to the ˜q = (eq + 0.01B4 )/keq + 0.01B4 k and error states according to e 3 ˜ eω = eω + 0.005B . Since we applied a slightly elliptic LEO, we only considered the disturbance torques which are major contributors to such kind of orbits; namely, gravity gradient torque –see [23] and torques caused by atmospheric drag –see [27] and J2 effect –see [28]. The J2 effect is caused by non-homogeneous mass distribution of a planet, and the torques generated by atmospheric drag and J2 are induced because of a rbc = [0.1, 0, 0]⊤ meters displacement of the center of mass. All disturbances were considered continuous and bounded. The simulation results under noise and perturbations for one orbital period (5896 s) are presented in Table II. The performance improvement for both controllers using state-dependent gains is evident from the data concerning the index Jp and is illustrated by the plots in Figures 3 and 4. In table III we present simulation results from a wide number of simulations for a general rigid-body without disturbances and noise with controller gains kp = 30, kd = 15 for all three control laws and in addition lp = 100 and ld = 50 for the output feedback controllers and k1 = k2 = 0.5 and k3 = 100 for the variable gains controller. We used random initial values for the quaternion vector, while the L2 norm of the initial angular velocity vector was randomly found with standard deviation in equal steps from 0.01 to 10 rad/s during 10, 000 consecutive runs. Furthermore, we let qi,e (t0 ) = qi,b (t0 ) and z(t0 ) = [0, 0, 0]⊤ . What we see from the results is that the output feedback variable controller overall performs better than the static controller with
0
4
t0
τ ⊤τ
Jq 0.779 0.800 0.781
b ω b,⊤ e,b ω e,b
TABLE II VALUES OF PERFORMANCE FUNCTIONALS FOR ATTITUDE MANEUVER OVER ONE ORBITAL PERIOD (5896 s).
Static gains Variable gains Controller (9)
1
b ω b,⊤ d,b ω d,b
Static gains Variable gains Controller (9)
Jq 0.778 0.800 0.780
0
5
10
10
τx τy τz
0 −10 −20
0
5
10
10
15
τx τy τz
0 −10 −20
15
0
5
10
15
Time (s)
Fig. 2. Control torque for PD+ based output feedback with static and variable gains, and state feedback controller (9), during spacecraft attitude maneuver.
respect to attitude error transients and power consumption, that is, according to our measure, the body converges nearly twice as fast while the estimate converges over six times faster at the price of only about 10% increase in power consumption. On the other hand we see that the integral of the angular velocity error is higher which is expected because of the derivative gain which introduces less damping for large attitude error, thus larger angular velocities are achieved during stabilization. The state feedback controller leads to a faster transient of the angular velocity error, which should be expected based on its nature, along with a slightly faster convergence of the attitude error and lower power consumption. We see that the variable output feedback controller is about 40% faster with respect to attitude transient with an increase in power consumption of 20% compared to the state feedback controller. IV. CONCLUSION We proposed two PD+ based output feedback control laws for attitude tracking control of a rigid body, one using constant gains while variable gains for the other. The equilibrium points for the closed-loop system with unknown but bounded disturbances are uniformly practically asymptotically stable. The latter is illustrated
6
−3
x 10
0.02
ǫ˜x ǫ˜y ǫ˜z
ω bd,b - Static
˜ǫ - Static
5
0
−5 1000
1500
xi yi zi
0.01 0 −0.01 −0.02 1000
2000
1500
2000
−3
x 10
0.02
ǫ˜x ǫ˜y ǫ˜z
ω bd,b - Variable
˜ǫ - Variable
5
0
−5 1000
1500
xi yi zi
0.01 0 −0.01 −0.02 1000
2000
1500
2000
x 10
ω bd,b - Controller (9)
˜ǫ - Controller (9)
−3
5
ǫ˜x ǫ˜y ǫ˜z
0
−5 1000
1500
2000
0.02
xi yi zi
0.01 0 −0.01 −0.02 1000
1500
2000
Time (s) Time (s) Fig. 3. Attitude and angular velocity error for output feedback using static gains (top), variable gains (middle) and state feedback using static gains (bottom). −3
−3
x 10
1
ǫe,b,x ǫe,b,y ǫe,b,z
0.5
ǫe,b - Variable
ǫe,b - Static
1
0 −0.5 −1 1000
1500
2000
0 −0.5 −1 1000
xii
y zi
0.5 0 −0.5
1500
1
ω be,b - Variable
ω be,b - Static
1500
2000
−3
x 10
−1 1000
ǫe,b,x ǫe,b,y ǫe,b,z
0.5
−3
1
x 10
2000
x 10
xi yi zi
0.5 0 −0.5 −1 1000
Time (s)
1500
2000
Time (s)
Fig. 4. Attitude and angular velocity estimation error for output feedback using static gains (left) and variable gains (right).
in simulations; in particular it is showed that the controlled system is less sensitive to noise and perturbations under the action of the controller with state-dependent gains. R EFERENCES [1] R. Kristiansen, Dynamic Synchronization of Spacecraft - Modeling and Coordinated Control of Leader-Follower Spacecraft Formations. PhD thesis, Department of Engineering Cybernetics, Norwegian University of Science and Technology, Trondheim, Norway, 2008. [2] J. R. Lawton and R. W. Beard, “Synchronized multiple spacecraft rotations,” Automatica, vol. 38, pp. 1359–1364, Aug. 2002. [3] R. Kelly, V. Santib´an˜ ez, and A. Loria, Control of robot manipulators in joint space. Series Advanced textbooks in control engineering, London, U.K.: Springer-Verlag, 2005. ISBN: 1-85233-994-2.
[4] B. Paden and R. Panja, “A globally asymptotically stable ’PD+’ controller for robot manipulator,” Int. J. Control, vol. 47, no. 6, pp. 1697– 1712, 1988. [5] J. J.-E. Slotine and W. Li, “On the adaptive control of robot manipulators,” Int. J. Robot. Res., vol. 6, no. 3, pp. 49–59, 1987. [6] J. T.-Y. Wen and K. Kreutz-Delgado, “The attitude control problem,” IEEE Transactions on Automatic Control, vol. 36, no. 10, pp. 1148– 1162, 1991. [7] S. E. Salcudean, “A globally convergent angular velocity observer for rigid body motion,” IEEE Transactions on Automatic Control, vol. 36, no. 12, pp. 1493–1497, 1991. [8] F. Lizarralde and J. T. Wen, “Attitude control without angular velocity measurement: a passivity approach,” IEEE Transactions on Automatic Control, vol. 41, pp. 468–472, Mar. 1996. [9] F. Caccavale and L. Villani, “Output feedback control for attitude tracking,” Syst. Contr. Lett., vol. 38, pp. 91–98, Oct. 1999. [10] H. Berghuis and H. Nijmeijer, “A passivity approach to controllerobserver design for robots,” IEEE Trans. Robot. Automat., vol. 9, pp. 740–754, Dec. 1993. [11] M. R. Akella, “Rigid body attitude tracking without angular velocity feedback,” Syst. Contr. Lett., vol. 42, pp. 321–326, Apr. 2001. [12] B. Costic, D. Dawson, M. de Queiroz, and V. Kapila, “A quaternionbased adaptive attitude tracking controller without velocity measurements,” in Proceedings of the IEEE Conference on Decision and Control, 2000. [13] A. Tayebi, “Unit quaternion observer based attitude stabilization of a rigid spacecraft without velocity measurement,” in Proceedings of the IEEE Conference on Decision and Control, 2006. [14] A. Tayebi, “Unit quaternion-based output feedback for the attitude tracking problem,” IEEE Transactions on Automatic Control, vol. 53, pp. 1516–1520, Jul. 2008. [15] R. Cunha, C. Silvestre, and J. Hespanha, “Output-feedback control for stabilization on SE(3),” Systems & Control Letters, vol. 57, pp. 1013– 1022, Dec. 2008. [16] N. Chaturvedi and N. McClamroch, “Global stabilization of an inverted 3D pendulum including control saturation effects,” in Proceedings of the IEEE Conference on Decision and Control, 2006. [17] A. Chaillet and A. Lor´ıa, “Uniform semiglobal practical asymptotic stability for non-autonomous cascaded systems and applications,” Automatica, vol. 44, pp. 337–347, Feb. 2008. [18] Y. Lin, E. D. Sontag, and Y. Wang, “A smooth converse Lyapunov theorem for robust stability,” SIAM J. Contr. Optimiz., vol. 34, no. 1, pp. 124–160, 1996. [19] A. Teel and L. Praly, “A smooth Lyapunov function from a class-KL estimate involving two positive semidefinite functions,” ESAIM COCV, vol. 5, pp. 313–368, 2000. [20] D. Neˇsi´c, A. Teel, and E. D. Sontag, “Formulas relating KL stability estimates of discrete-time and sampled-data nonlinear systems,” Syst. Contr. Lett., vol. 38, pp. 49–60, Sep. 1999. [21] D. Casagrande, A. Astolfi, and T. Parisini, “Global asymptotic stabilization of the attitude and the angular rates of an underactuated nonsymmetric rigid body,” Automatica, vol. 44, pp. 1781–1789, Jul. 2008. [22] C. G. Mayhew, R. G. Sanfelice, and A. R. Teel, “Robust global asymptotic attitude stabilization of a rigid body by quaternion-based hybrid feedback,” in Proceedings of the IEEE Conference on Decision and Control, 2009. [23] M. J. Sidi, Spacecraft Dynamics and Control. New York: Cambridge University Press, 1997. ISBN 0-521-78780-7. [24] O.-E. Fjellstad, Control of Unmanned Underwater Vehicles in Six Degrees of Freedom. PhD thesis, Department of Engineering Cybernetics, Norwegian University of Science and Technology, Trondheim, Norway, 1994. [25] N. Chaturvedi and N. McClamroch, “Attitude stabilization of the inverted 3D pendulum on TSO(3) with control saturation,” in Proceedings of the IEEE Conference on Decision and Control, 2007. [26] R. Schlanbusch, A. Lor´ıa, R. Kristiansen, and P. J. Nicklasson, “PD+ attitude control of rigid bodies with improved performance,” in Proceedings of the IEEE Conference on Decision and Control, 2010. [27] J. R. Wertz, ed., Spacecraft Attitude Determination and Control. Dordrecht, The Netherlands: Reidel, 1978. ISBN 90-277-0959-9. [28] O. Montenbruck and E. Gill, Satellite Orbits. Models, methods, applications. Berlin, Germany: Springer-Verlag, 2001. First edition, corrected second printing, ISBN 3-540-67280-X.
