Global exponential attitude and gyro bias estimation from vector measurements

07/11/2017
Publication GSI2017
OAI : oai:www.see.asso.fr:17410:22546
contenu protégé  Document accessible sous conditions - vous devez vous connecter ou vous enregistrer pour accéder à ou acquérir ce document.
- Accès libre pour les ayants-droit
 

Résumé

We consider the classical problem of estimating the attitude and gyro biases of a rigid body from at least two vector measurements and a triaxial rate gyro. We propose a solution based on a dynamic nonlinear estimator designed without respecting the geometry of SO(3), which achieves uniform global exponential convergence. The convergence is established thanks to a dynamically scaled Lyapunov function.

Global exponential attitude and gyro bias estimation from vector measurements

Collection

application/pdf Global exponential attitude and gyro bias estimation from vector measurements Philippe Martin, Ioannis Sarras
Détails de l'article
contenu protégé  Document accessible sous conditions - vous devez vous connecter ou vous enregistrer pour accéder à ou acquérir ce document.
- Accès libre pour les ayants-droit

Global exponential attitude and gyro bias estimation from vector measurements

Métriques

0
0
299.76 Ko
 application/pdf
bitcache://eca5d8f20684d23a006a43090042f442c0315506

Licence

Creative Commons Aucune (Tous droits réservés)

Sponsors

Sponsors Platine

alanturinginstitutelogo.png
logothales.jpg

Sponsors Bronze

logo_enac-bleuok.jpg
imag150x185_couleur_rvb.jpg

Sponsors scientifique

logo_smf_cmjn.gif

Sponsors

smai.png
logo_gdr-mia.png
gdr_geosto_logo.png
gdr-isis.png
logo-minesparistech.jpg
logo_x.jpeg
springer-logo.png
logo-psl.png

Organisateurs

logo_see.gif
<resource  xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
                xmlns="http://datacite.org/schema/kernel-4"
                xsi:schemaLocation="http://datacite.org/schema/kernel-4 http://schema.datacite.org/meta/kernel-4/metadata.xsd">
        <identifier identifierType="DOI">10.23723/17410/22546</identifier><creators><creator><creatorName>Philippe Martin</creatorName></creator><creator><creatorName>Ioannis Sarras</creatorName></creator></creators><titles>
            <title>Global exponential attitude and gyro bias estimation from vector measurements</title></titles>
        <publisher>SEE</publisher>
        <publicationYear>2018</publicationYear>
        <resourceType resourceTypeGeneral="Text">Text</resourceType><dates>
	    <date dateType="Created">Wed 7 Mar 2018</date>
	    <date dateType="Updated">Wed 7 Mar 2018</date>
            <date dateType="Submitted">Fri 17 Aug 2018</date>
	</dates>
        <alternateIdentifiers>
	    <alternateIdentifier alternateIdentifierType="bitstream">eca5d8f20684d23a006a43090042f442c0315506</alternateIdentifier>
	</alternateIdentifiers>
        <formats>
	    <format>application/pdf</format>
	</formats>
	<version>37259</version>
        <descriptions>
            <description descriptionType="Abstract">We consider the classical problem of estimating the attitude and gyro biases of a rigid body from at least two vector measurements and a triaxial rate gyro. We propose a solution based on a dynamic nonlinear estimator designed without respecting the geometry of SO(3), which achieves uniform global exponential convergence. The convergence is established thanks to a dynamically scaled Lyapunov function.
</description>
        </descriptions>
    </resource>
.

Global exponential attitude and gyro bias estimation from vector measurements Philippe Martin1 and Ioannis Sarras2 1 Centre Automatique et Systèmes, MINES ParisTech, PSL Research University, 75006 Paris, France philippe.martin@mines-paristech.fr 2 ONERA – The French Aerospace Lab, F-91123 Palaiseau, France ioannis.sarras@onera.fr Abstract. We consider the classical problem of estimating the attitude and gyro biases of a rigid body from at least two vector measurements and a triaxial rate gyro. We propose a solution based on a dynamic nonlinear estimator designed without respecting the geometry of SO(3), which achieves uniform global exponential convergence. The convergence is established thanks to a dynamically scaled Lyapunov function. 1 Introduction Estimating the attitude of a rigid body from vector measurements (obtained for instance from accelerometers, magnetometers, sun sensors, etc.) has been for decades a problem of interest, because of its importance for a variety of technological applications such as satellites or unmanned aerial vehicles. The attitude of the body can be described by the rotation matrix R ∈ SO(3) from body to inertial axes. On the other hand, the (time-varying) measurement vec- tors u1, · · · , un ∈ R3 correspond to the expression in body axes of known and not all collinear vectors U1, · · · , Un ∈ R3 which are constant in inertial axes, i.e., uk(t) = RT (t)Uk. The goal then is to reconstruct the attitude at time t using only the knowledge of the measurement vectors until t. The solution to the problem would be very easy if the vector measurements were perfect and two of them were linearly independent: indeed, using for instance only the two vectors u1(t) and u2(t) and noticing that RT (x × y) = RT x × RT y since R is a rotation matrix, we readily find RT (t) = RT (t) · U1 U2 U1 × U2  · U1 U2 U1 × U2 −1 = u1(t) u2(t) u1(t) × u2(t)  · U1 U2 U1 × U2 −1 . But in real situations, the measurement vectors are always corrupted at least by noise. Moreover, the Uk’s may possibly be not strictly constant: for instance a triaxial magnetometer measures the (locally) constant Earth magnetic field, but is easily perturbed by ferromagnetic masses and electromagnetic perturbations; similarly, a triaxial accelerometer can be considered as measuring the direction 2 of gravity provided it is not undergoing a substantial acceleration (see e.g. [13] for a detailed discussion of this assumption and its consequences in the frame- work of quadrotor UAVs). That is why, despite the additional cost, it may be interesting to use a triaxial rate gyro to supplement the possibly deteriorated vector measurements. The current literature on attitude estimation from vector measurements can be broadly divided into three categories: i) optimization-based methods; ii) stochastic filtering; iii) nonlinear observers. Details on the various approaches can be found e.g. in the surveys [6,17] and the references therein. The first cat- egory sets the problem as the minimization of a cost function, and is usually referred to as Wahba’s problem. The attitude is algebraically recovered at time t using only the measurements at time t. No filtering is performed, and possibly available velocity information from rate gyros is not exploited. The second cat- egory mainly hinges on Kalman filtering and its variants. Despite their many qualities, the drawback of those designs is that convergence cannot in general be guaranteed except for mild trajectories. Moreover the tuning is not completely obvious, and the computational cost may be too high for small embedded pro- cessors. The third, and more recent, approach proposes nonlinear observers with a large guaranteed domain of convergence and a rather simple tuning through a few constant gains. These observers can be designed: a) directly on SO(3) (or the unit quaternion space), see e.g. [7,10,12,16]; b) or more recently, on R3×3 , i.e., deliberately “forgetting” the underlying geometry [2,3,8,14]. Probably the best- known design is the so-called nonlinear complementary filter of [10]; as noticed in [11], it is a special case of so-called invariant observers [5]. In this paper, we propose a new observer of attitude and gyro biases from gyro measurements and (at least) two measurement vectors. It also “forgets” the geometry of SO(3), which allows for uniform global exponential convergence (notice the observer of [10] is only quasi-globally convergent). This observer is an extension of the observer of [14] (which is uniformly globally convergent), itself a modification of the linear cascaded observer of [3] (which is uniformly globally exponentially convergent). The idea of the proof is nevertheless completely dif- ferent from the approach followed in [3]; it is much more direct, as it relies on a strict, dynamically scaled, Lyapunov function, see [1,9]. 2 The design model We consider a moving rigid body subjected to an angular velocity ω. Its orien- tation matrix R ∈ SO(3) is related to the angular velocity by the differential equation Ṙ = Rω×, (1) where the skew-symmetric matrix ω× is defined by ω×u := ω × u whatever the vector u ∈ R3 . The rigid body is equipped with a triaxal rate gyro measuring the angu- lar velocity ω, and two additional triaxial sensors (for example accelerometers, 3 magnetometers or sun sensors) providing the measurements of two vectors α and β. These vectors correspond to the expression in body axes of two known independent vectors αi and βi which are constant in inertial axes. In other words, α := RT αi β := RT βi. Since αi, βi are constant, we obviously have α̇ = α × ω β̇ = β × ω. To take full advantage of the rate gyro, it is wise to take into account that it is biased, hence rather provides the measurement ωm := ω + b, where b is a slowly-varying (for instance with temperature) unknown bias. Since the effect of this bias on attitude estimation may be important, it is worth determining this value. But being not exactly constant, it can not be calibrated in advance and must be estimated online together with the attitude. Our objective is to design an estimation scheme that can reconstruct online the orientation matrix R(t) and the bias b(t), using i) the measurements of the gyro and of the two vector sensors; ii) the knowledge of the constant vectors αi and βi. The model on which the design will be based therefore consists of the dynamics α̇ = α × ω (2) β̇ = β × ω (3) ḃ = 0, (4) together with the measurements ωm := ω + b (5) αm := α (6) βm := β. (7) 3 The observer We want to show that the state of (2)–(7) can be estimated by the observer ˙ α̂ = α̂ × (ωm − b̂) − kα(α̂ − αm) (8) ˙ β̂ = β̂ × (ωm − b̂) − kβ(β̂ − βm) (9) ˙ ξ = lα(ωm − b̂) × (α̂ × αm) + lβ(ωm − b̂) × (β̂ × βm) (10) + lαkαα̂ × αm + lβkββ̂ × βm (11) ṙ = −2ψ1(r − 1) + 2 lα|αm||α̂ − αm| + lβ|βm||β̂ − βm|  r, (12) 4 where b̂ := ξ + lαα̂ × αm + lββ̂ × βm (13) kα := k1 + r  1 2 + l2 α 1 r  |αm|2 (14) kβ := k2 + r  1 2 + l2 β 1 r  |βm|2 . (15) α̂, β̂, b̂ ∈ R3 are the estimates of α, β, b; ξ ∈ R3 is the state of the bias observer, and r ∈ R is a dynamic scaling variable; the (positive) constants lα, lβ, ψ1, k1, k2, , 1 are tuning gains. Defining the estimation errors as eα := α̂ − α eβ := β̂ − β eb := b̂ − b, the error system reads ėα = eα × ω − (α + eα) × eb − kα(r, α̂)eα (16) ėβ = eβ × ω − (β + eβ) × eb − kβ(r, β̂)eβ (17) ėb = (lαα2 × + lββ2 ×)eb + lαeα × (α × eb) + lβeβ × (β × eb) (18) ṙ = −2ψ1(r − 1) + 2 lα|α||eα| + lβ|β||eβ|  r; (19) (18) is obtained thanks to the Jacobi identity a×(b×c)+b×(c×a)+c×(a×b). The main result is the global exponential convergence of the observer. Theorem 1. Assume k1, k2, , 1 > 0, ψ1 > 1, and lα, lβ large enough so that −(lαα2 × + lββ2 ×) > (ψ1 + )I. Then the equilibrium point (ēα, ēβ, ēb, r̄) := (0, 0, 0, 1) of the error system (16)–(19) is uniformly globally exponentially stable. Remark 1 (see [4,15]). Since α and β are linearly independent, −(lαα2 × + lββ2 ×) is a (symmetric) positive definite matrix when lα, lβ > 0; moreover, sufficiently large lα, lβ yield −(lαα2 × + lββ2 ×) > µI whatever the given constant µ. Proof. First consider the candidate Lyapunov function for the (eα, eβ)-subsystem V (eα, eβ) := 1 2 |eα|2 + 1 2 |eβ|2 . Its time derivative satisfies V̇ = −heα, eα × ωi − kα|eα|2 − heβ, eβ × ωi − kβ|eβ|2 ≤ −kα|eα|2 − kβ|eβ|2 + √ r|α||eα| |eb| √ r + √ r|β||eβ| |eb| √ r ≤ −  kα − r|α|2 2  |eα|2 −  kβ − r|β|2 2  |eβ|2 + |eb|2 r ; 5 where we have used ha, a×bi = 0 to obtain the first line, and Young’s inequality ab ≤ a2 2 + b2 2 to obtain the second line. Now, the obvious candidate Lyapunov function Vb(eb) := 1 2 |eb|2 for the eb- subsystem satisfies V̇b = heb, (lαα2 × + lββ2 ×)ebi + lαheb, eα × (α × eb)i + lβheb, eβ × (β × eb)i ≤ −µ|eb|2 + lα|α||eα| + lβ|β||eβ|)|eb|2 , where we have used Remark 1. The term lα|α||eα| + lβ|β||eβ|)|eb|2 happens to be very difficult to dominate with a classical Lyapunov approach. To overcome the problem, we use instead the candidate Lyapunov function Ṽb(eb, r) := 1 2r |eb|2 , obtaining by dynamically scaling Vb with r defined by (19). Notice r(t) ≥ 1 for all positive t as soon as r(0) ≥ 1. We then have ˙ Ṽb := V̇b r − Ṽb ṙ r ≤ −µ |eb|2 r + lα|α||eα| + lβ|β||eβ|) |eb|2 r − |eb|2 2r ṙ r = −(µ − ψ1) |eb|2 r , where we have used r−1 r ≤ 1. We next consider the candidate Lyapunov function for the r-subsystem Vr(r) := 1 2 (r − 1)2 . Its time derivative satisfies V̇r = −2ψ1(r − 1)2 + √ 2(r − 1) √ 2rlα|α||eα| + √ 2(r − 1) √ 2rlβ|β||eβ| ≤ −2(ψ1 − 1)(r − 1)2 + r2 1 l2 α|α|2 |eα||2 + l2 β|β||eβ|2  , where the second line is obtained by Young’s inequality. Finally, consider the complete Lyapunov function W(eα, eβ, eb, r) := V (eα, eβ) + Ṽb(eb, r) + Vr(r). Collecting all the previous findings, its time derivative satisfies Ẇ ≤ −  kα − r|α|2 2  |eα|2 −  kβ − r|β|2 2  |eβ|2 + |eb|2 r − (µ − ψ1) |eb|2 r −2 (ψ1 − 1)(r − 1)2 + r2 1 l2 α|α|2 |eα|2 + l2 β|β||eβ|2  = −k1|eα|2 − k2|eβ|2 − (µ − ψ1 − ) |eb|2 r − 2(ψ1 − 1)(r − 1)2 . 6 Choosing k1, k2 > 0, ψ1 > 1, and lα, lβ large enough so that µ > ψ1 +  clearly guarantees the uniform global exponential stability of the equilibrium point (ēα, ēm, ēb √ r̄ , r̄) := (0, 0, 0, 1), hence of (ēα, ēm, ēb, r̄) := (0, 0, 0, 1). u t Remark 2. More than two vectors α and β can be used with a direct generaliza- tion of the proposed structure. Remark 3. The observer does not use the knowledge of the constant vectors αi and βi. This may be an interesting feature in some applications when those vectors for example are not precisely known and/or (slowly) vary. We then have the following corollary, which gives an estimate of the true orientation matrix R by using the knowledge of the inertial vectors αi and βi. Notice it is considerably simpler than the approach of [3], where the estimated orientation matrix is obtained through an additional observer of dimension 9. Corollary 1. Under the assumptions of Theorem 1, the matrix R̃ defined by R̃T :=  α̂ |αi| α̂×β̂ |αi×βi| α̂×(α̂×β̂) |αi×(αi×βi)|  · RT i Ri :=  αi |αi| αi×βi |αi×βi| αi×(αi×βi) |αi×(αi×βi)|  uniformly globally exponentially converges to R. Proof. By Theorem 1, eα(t) ≤ C|eα(0)|e−λt and eβ(t) ≤ C|eβ(0)|e−λt for some C, λ > 0. Therefore, |α̂ × β̂ − α × β| = |α × eβ + eα × β + eα × eβ| ≤ |α||eβ| + |β||eα| + |eα||eβ| ≤ C|αi||eβ(0)|e−λt + C|βi||eα(0)|e−λt + C2 |eα(0)||eβ(0)|e−λ2t ; a similar bound is readily obtained for |α̂×(α̂×β̂)−α×(α×β)|. As a consequence, all the coefficients of the matrix R̃T −  α |αi| α×β |αi×βi| α×(α×β) |αi×(αi×βi)|  · RT i globally exponentially converge to 0. The claim follows by noticing  α |αi| α×β |αi×βi| α×(α×β) |αi×(αi×βi)|  · RT i =  RT αi |αi| RT αi×RT βi |αi×βi| RT αi×(RT αi×RT βi) |αi×(αi×βi)|  · RT i = RT RiRT i = RT , where we have used RT (u × v) = RT u × RT v since R is a rotation matrix. u t Of course, R̃T has no reason to be a rotation matrix (it is only asymptotically so); it is nevertheless the product of a matrix with orthogonal (possibly zero) columns by a rotation matrix. If a bona fide rotation matrix is required at all times, a natural idea is to project R̃ onto the “closest” rotation matrix R̂, thanks to a polar decomposition. 7 References 1. A. Astolfi, D.Karagiannis, and R. Ortega. Nonlinear and adaptive control with applications. Springer-Verlag, London, 2008. 2. P. Batista, C. Silvestre, and P. Oliveira. A GES attitude observer with single vector observations. Automatica, 48(2):388–395, 2012. 3. Pedro Batista, Carlos Silvestre, and Paulo Oliveira. Globally exponentially stable cascade observers for attitude estimation. Control Engineering Practice, 20(2):148 – 155, 2012. 4. L. Benziane, A. Benallegue, Y. Chitour, and A. Tayebi. Velocity-free attitude stabilization with inertial vector measurements. International Journal of Robust and Nonlinear Control, 2015. 5. S. Bonnabel, P. Martin, and P. Rouchon. Symmetry-preserving observers. IEEE Transactions on Automatic Control, 53(11):2514–2526, 2008. 6. J. L. Crassidis, F. L. Markley, and Y. Cheng. Survey of nonlinear attitude estima- tion methods. Journal of Guidance, Control, and Dynamics, 30(1):12–28, 2007. 7. H. F. Grip, T. I. Fossen, T. A. Johansen, and A. Saberi. Attitude estimation using biased gyro and vector measurements with time-varying reference vectors. IEEE Transactions on Automatic Control, 57(5):1332–1338, 2012. 8. H. F. Grip, T. I. Fossen, T. A. Johansen, and A. Saberi. Globally exponentially stable attitude and gyro bias estimation with application to GNSS/INS integration. Automatica, 51:158–166, 2015. 9. D. Karagiannis, M. Sassano, and A. Astolfi. Dynamic scaling and observer design with application to adaptive control. Automatica, 45(12):2883–2889, 2009. 10. R. Mahony, T. Hamel, and J. M. Pflimlin. Nonlinear complementary filters on the special orthogonal group. IEEE Transactions on Automatic Control, 53(5):1203– 1218, 2008. 11. P. Martin and E. Salaün. Invariant observers for attitude and heading estimation from low-cost inertial and magnetic sensors. In IEEE Conference on Decision and Control, pages 1039–1045, 2007. 12. P. Martin and E. Salaün. Design and implementation of a low-cost observer-based attitude and heading reference system. Control Engineering Practice, 18(7):712– 722, 2010. 13. P. Martin and E. Salaün. The true role of accelerometer feedback in quadrotor control. In IEEE International Conference on Robotics and Automation, pages 1623–1629, 2010. 14. P. Martin and I. Sarras. A global attitude and gyro bias observer from vector measurements. IFAC-PapersOnLine, 50, 2017. IFAC 2017 World Congress. 15. A. Tayebi, A. Roberts, and A. Benallegue. Inertial vector measurements based velocity-free attitude stabilization. IEEE Transactions on Automatic Control, 58(11):2893–2898, 2013. 16. J.F. Vasconcelos, C. Silvestre, and P. Oliveira. A nonlinear observer for rigid body attitude estimation using vector observations. IFAC Proceedings Volumes, 41(2):8599 – 8604, 2008. 17. M. Zamani, J. Trumpf, and R. Mahony. Nonlinear Attitude Filtering: A Compar- ison Study. ArXiv e-prints, arXiv:1502.03990 [cs.SY], 2015.