A quaternion-based Complementary Sliding Mode Observer for attitude estimation: application in free-ranging animal motions

Transcription

1 A quaternion-based Complementary Slidin Mode Observer for attitude estimation: application in free-ranin animal motions Hassen Fourati, oureddine Manamanni, Lissan Afilal, Yves Handrich o cite this version: Hassen Fourati, oureddine Manamanni, Lissan Afilal, Yves Handrich. A quaternion-based Complementary Slidin Mode Observer for attitude estimation: application in free-ranin animal motions. 49th IEEE Conference on Decision and Control, Dec, Atlanta, United States. pp ,. HAL Id: hal Submitted on 9 ov HAL is a multi-disciplinary open access archive for the deposit and dissemination of scientific research documents, whether they are published or not. he documents may come from teachin and research institutions in France or abroad, or from public or private research centers. L archive ouverte pluridisciplinaire HAL, est destinée au dépôt et à la diffusion de documents scientifiques de niveau recherche, publiés ou non, émanant des établissements d enseinement et de recherche français ou étraners, des laboratoires publics ou privés.

2 A quaternion-based Complementary Slidin Mode Observer for attitude estimation: application in free-ranin animal motions H. Fourati,, Student, IEEE,. Manamanni, Member, IEEE, A.. Jemaa, L. Afilal and Y. Handrich Abstract his paper proposes a sensin method usin a module of GAM sensors (-axis Gyroscope, -axis Accelerometer, -axis Manetometer) for more accurate measurement of the marine animal s posture. he application in mind is that of io-loin. A quaternion-based alorithm is developed throuh the complementary filterin theory in order to combine all information sources and to obtain an accurate estimation of the attitude. y considerin the riid body kinematic model, a Complementary Slidin Mode Observer (CSMO) is proposed, takin advantaes from strapdown inertial measurements and or observations. he proposed alorithm is based on the multiplicative quaternion correction technique. he performance of the CSMO is shown with a set of preliminary experiments on domestic animals throuh sensor measurements provided by an Inertial Measurement Unit. I. IRODUCIO owadays, the attitude estimation of a riid body has attracted a stron interest in many fields such as Walkin Robots [], Micro Air Vehicles [], Marine Vehicles [] and others. he need of reliable and precise attitude information is one of the fundamental requirements in these areas. ote that, the proress in Micro Electro-Mechanical System (MEMS) has enabled the development of low-cost inertial and manetic sensors, namely, the yroscopes, the accelerometers, and the manetometers. herefore, the use of these sensors in the attitude estimation problem has attracted a stron interest. he attitude determination process usually involves some errors (e.., sensor noise, bias, and inaccuracy in predictions). For this reason, most of the attitude determination methods aim to combine two or more sensor measurements in an optimal way and use state estimation techniques to increase the accuracy of the solutions. Quaternion has been the subject of studies in many attitude determination systems usin various filterin theories. Due to the unconventional nature of quaternion kinematics, filter models have been synthesized in two different ways related to the objectives, the formulation of the measurement error or and the update of the state estimates. he first way is based on additive quaternion correction []-[6]. his approach is easy to implement but it is considered as Manuscript received March,. his work was supported by Alsace and Champane-Ardenne Reions within the framework of the project avimeles. H. Fourati, A. en Jemaa,. Manamanni and L. Afilal are with CReSIC-URCA, Reims Champane Ardenne University, France noureddine.manamanni@univ-reims.fr Y. Handrich and H. Fourati are with IPHC-DEPE-CRS, Strasbour University, France. yves-jean.handrich@c-strasbour.fr localized approximation since it is valid only for small attitude chanes. he second way uses multiplicative quaternion correction [7]-[9] and can be applied for larer attitude maneuvers. his paper focuses on the development of a robust method recoverin the full attitude of a movin riid body and usin the multiplicative quaternion correction. he application in mind is that of io-loin. It is one of the recent interestin areas, at the intersection of the animal behavior and bioenineerin, where the attitude estimation is becomin a requirement for bioloists. It aims to obtain new information on the natural world and provides new insihts into the hidden lives of animal species []. io-loin enerally involves attachin an electronic device (bioloer) to an animal in order to record different aspects of its bioloy and environment. Durin the last years, a considerable number of studies have been undertaken to estimate the posture or orientation of marine mammals usin only accelerometers and manetometers []-[]. However, these works are not well suited in dynamic situations observed underwater for marine animals, e.. the phases of prey pursuit. herefore, the estimation performance is sinificantly deraded. his paper proposes an alternative approach to estimate the attitude in free-ranin marine animals durin their movements to overcome the drawbacks of mentioned papers in this area []-[]. he main idea is to use a Complementary Slidin Mode Observer (CSMO) that exploits the multiplicative quaternion correction technique to recover the full riid body orientation. he nonlinear observer is fed with inertial and manetic measurements and takes into account the complementary spectra of these sinals. In fact, the estimation alorithm idea uses -axis yroscope measurements to derive the attitude. he correction was performed usin a quaternion error continuously derived from a -axis accelerometer and -axis manetometer data fusion method. his reduces the interation drift that oriinates from the anular velocity. his paper is oranized as follows: Section II ives some backrounds of the riid body attitude representation. he formulation of the CSMO is proposed in section III. Section IV is devoted to preliminary experimental results to illustrate the efficiency of the proposed approach. Section V summarizes the main conclusions of the paper. II. RIGID ODY AIUDE DESCRIPIO As known the riid body attitude is the orientation X, Y, Z and local relation between the body frame

3 naviation frame (,, ) X Y Z (see Fi. ). he former is fixed to the riid-body. he latter is tanent to the Earth s surface (Local anent Plane, LP) and it is particularly useful to express the attitude of a movin riid body on the surface of the earth [4]. he X -axis points towards the true north. he Z -axis points towards the interior of the Earth, perpendicular to the reference ellipsoid. he Y -axis completes the riht-handed coordinate system, pointin east (ED: orth, East, Down). (East) Y Y O Z () Z (Down) () Riid-body X X (orth) Fi.. he coordinate system () (solid line) of a riid body represented in the Earth-fixed frame () (dashed line) Recall that the unit quaternion is the mathematical representation of the riid body attitude between the frames and. he quaternion, denoted by q, is a hypercomplex number of rank 4 such that: φ φ q q = q + qi + q j + qk = cos u sin = q () where q, q, q and q are real numbers. i, j, and k are the components of the or u (Euler axis), φ is the rotation anle and q = q q q R is the imainary or. More details about quaternion alebra can be founded in [5]. III. COMPLEMEARY SLIDIG MODE OSERVER DESIG In this section, the objective is to desin a riid body attitude estimation alorithm based on inertial and manetic MEMS sensors. y considerin the riid body kinematic model, a CSMO is proposed in order to take advantae from the ood short term precision iven by rate yros interation and the reliable lon term accuracy provided by accelerometers and manetometers measurements. Hence, the CSMO exploits the low-frequency reion where the attitude observations are typically more accurate, and the hih-frequency reion where the interation of the rate yro yields better attitude estimates. It is important to note that the resultin structure is complementary: hih bandwidth rate yro measurements are combined with low bandwidth or observations. A. Riid body kinematic model he attitude kinematic equation describes the time rate of chane of the attitude q, in a quaternion term, as a result of the riid body anular rates measured by the yroscope. herefore, the quaternion derivative that represents the kinematic differential equation is iven as in [6] as follows: q = ω q () t where ω = ω is the quaternion form of the anular velocity or ω = ω ω ω. x y z he anular velocities measurements ω are often corrupted with noises and bias w( t ) [7]. herefore, includin yroscope disturbances and usin the quaternion product iven in [5], () can be written such as: where as []: and q q = ω + w( t) () t I q + q q represents the skew-symmetric matrix defined q q q q = q = q q q q q I is the identity matrix of dimension. Hence, let us consider the followin nonlinear system composed of () and the linear measurement model y : q q = ω + w t t I q + q y = Cq + ν where = [ ] (4) (5) q q q q q represents the quaternion or. he linear measurement model y is taken as follows 4 y = q m R and C = I 4 4. ν represents the or of the measured noises. It is important to note that the riid body quaternion measurements q m may be determined from accelerometer and manetometer readins.. Quaternion q m calculation from accelerometer and manetometer measurements he problem of optimal attitude determination alorithm usin or observations is known in the literature as Wahba s problem [8], [9]. he manetometer provides the manetic field or in the frame. his quantity is locally constant in the frame and therefore it represents a or observation. For sufficiently low frequency bandwidths, the ravitational field also dominates the accelerometer measurements, as discussed in []. his would provide a second or observation. For this purpose, a Levenber Marquardt Alorithm (LMA) is proposed to recover complete posture information, expressed in quaternion term, by usin a -axis accelerometer and a -axis manetometer [9].

4 he -axis accelerometer measures the specific force f in the body frame as follows: x y z f [ ] f = f f f = M q a + + δ where = [ 9.8] and (6) a R represent respectively the ravity or and the inertial acceleration of the body, expressed in the naviation frame. R is the or of zero-mean white Gaussian noise. he rotation matrix M q is expressed as: δ f q + q ( q q + qq ) ( q q qq ) ( ) ( ) ( q q + q q ) ( q q q q ) q + q M ( q) = q q q q q + q q q + q q (7) he -axis manetometer ives the direction of the Earth s manetic field in the body frame as follows: where x y z h h = h h h = M q m + δ ( θ) ( θ) m = mx mz = m cos m sin (9) m represents the manetic field or measured in the naviation frame. he theoretical model of the manetic field nearest to reality considers a manetic field or with an inclination anle θ = 6 and a norm or m =.5 Gauss []. h (8) δ R is a or of zero-mean white Gaussian noise and the rotation matrix M q is as expressed in (7). he LMA is an estimator that uses the Earth s manetic field m and the ravity or as or observations to deduce the attitude q m. his alorithm is based on the followins steps: ) Measure accelerometer and manetometer readins f and h, respectively. ) Calculate fˆ = f and do the same for h ˆ. ote that ˆ f and h ˆ represent the estimated acceleration and manetic field values in the naviation frame. ote also that q ˆ = q ˆ q ˆ q ˆ q ˆ represents the quaternion inverse of ˆq. ) Calculate the naviation error ˆ f = f and do the same for h = m hˆ in order to form z = [ h f ]. 4) Calculate the Jacobian matrix: ˆ ˆ J h f = 5) Calculate the pseudo-inverse O = J J + λ I J. 6) Calculate the quaternion error such as: q t = α O z. α is a smooth parameter chosen between and []. er 7) Calculate m q such as: ˆ q t = q t q t. m It is important to note that this alorithm provides a lon term accuracy but still limited to the static or quasi-static situations (weak linear acceleration) []. herefore, the values of q m are disturbed in dynamic situations (durin hih linear acceleration periods). In this paper, the introduction aim of the term q m is to correct the attitude errors caused by disturbances in the rate yro measurements. C. Observer desin he main emphasis of the proposed observer is based on the use of the multiplicative correction technique [5]: q ' = δ q q () Quaternion multiplication is used in () to correct and update the quaternion calculation process. δ q is the correction term which is a function of the measurement error. o perform measurement update of the quaternion estimates, () can be transformed into the followin form: + + = δ () where q ˆ+ and q ˆ represent the post and pre-update values of the quaternion estimates, respectively. Finally, the Complementary Slidin Mode Observer (CSMO) can be desined by usin () with the choice of a slidin manifold based on the error quaternion. he proposed observer is iven as follows: ˆ ˆ q q CSMO : = δ K δ K ω () t I + where ˆq is the estimated quaternion at time t and q ˆ is defined as in (4). represents the switchin correction δ K term and δ K is the linear correction term. In order to perform the quaternion multiplication, each correction term should be converted into a quaternion. his conversion is obtained usin the forced normalization method iven in []. δ and K where δ K are computed such as follows: δ = χ ; K χ δ K χ, q e = K sat ;, χ ρ with k K = ; K k k er = χ () χ = K q e (4) k4 = k5 k6 (5)

5 ote that q e, is the imainary or of the error quaternion q. q measures the discrepancy between the e e complementary estimated quaternion ˆq and the measured attitude q m ( q m is obtained from LMA, section III-), that is: q = q = q q (6) e m e e, he scalar parts of χ and χ are chosen very close to unit since the incremental quaternion corresponds to a small anle rotation []. he saturation function sat is used to avoid the hih frequency chatterin behavior around the slidin surface: qe, ρ qe, qe, sat = qe, ρ (7) ρ ρ qe, ρ he parameter ρ is the slidin surface boundary layer. It determines the slidin behavior in the vicinity of q e, =. o preserve the unit quaternion norm, the estimated quaternion ˆq in () must be normalized to avoid a diverence caused by round-off errors. ormalization is obtained such as: norm = (8) Fi. illustrates the attitude estimation alorithm performed by the proposed nonlinear observer. Fi.. Scheme of the estimation alorithm D. Performance analysis of the desined observer A frequency analysis of inertial and manetic sensors shows that sinals comin from the accelerometermanetometer pair and sinals from the yroscope have a complementary frequency spectrum [7]. herefore, the resultin structure of the slidin mode observer () blends the low frequency reion (low bandwidth) of the accelerometer and manetometer data, where the attitude is typically more accurate, with the hih frequency reion (hih bandwidth) of the yroscope data, where the interation of the anular velocity yields better attitude estimates. he proposed observer is derived from the complementary filterin theory []. It explores the sensor redundancy to reject measurement disturbances in complementary frequency reions without distortin the sinal. If the measurements have complementary spectral characteristics, transfer functions may be chosen in such way as to minimize the estimation error []. he eneral requirement is that one of the transfer functions complements the sum of the others. hus for n measurements of a sinal: ( s) ( s)... n ( s) = n ( s) (9) where s is the time interation Laplace operator. o study the performance of the CSMO let us show the transform domain block diaram of the linearized quaternion observer (see Fi. ). his diaram is obtained from Fi.. Fi.. lock diaram of the transform domain (Laplace) of the linearized quaternion estimation observer Let the Laplace transform of the quaternion q m (obtained from accelerometer and manetometer readins) be qm ( s ), while q ( s ) is the quaternion obtained by interatin yroscope sinals and sq s is the Laplace transform of q in (). ote that the accelerometer measures both t ravitational and linear accelerations and the yroscope q s are both suffers from bias. herefore, qm ( s ) and disturbed. From Fi., the filter transfer function F ( s ) based on accelerometer and manetometer inputs is iven by: ( s ) Ks K F ( s) = = = () q s + Ks s + K m ote that () has the form of first-order low-pass filter which proves that the perturbation effects due to hih frequency components of accelerometer sinal (linear q s. acceleration) are filtered from m he ain K can be written such as K = KK, where K and K are iven in (5). Similarly, from Fi., the filter transfer function based on yroscope input can be written such as: ( s) s F s = = = () q s + Ks s + K ote that () has the form of first-order hih-pass filter. he yroscope measurements are hih-pass filtered with ˆq s. So, the perturbations due to low respect to the output frequency components of yroscope sinal (the noises and q s. biases) are filtered from Finally, (9) can be verified:

6 s s K s + = + = q s q s s + K s + K m () ote that () confirms the complementary aspect of the CSMO. he lobal transfer function of the observer is: K s ( s) = qm ( s) + q ( s) s + K s + K () One can conclude from () that CSMO blends a low-pass filterin on the sinals from the accelerometer-manetometer pair, and a hih-pass filterin on the sinals from the yroscope. IV. EXPERIMEAL RESULS At the time of the paper s writin, the final prototype (biloer) is under conception. Experiments involvin the kin penuin will be carried out when the developed alorithm is implemented on the embedded prototype. herefore, we chose to carry out the first preliminary experiment on another animal like a do. he accuracy of the CSMO is evaluated durin the free motion of the animal. An Inertial Measurement Unit was used, namely the Mi-G from Xsens echnoloies [4]. It is important to note that the Mi-G serves as a tool for the evaluation of the CSMO efficiency and cannot be suitable for use in free-ranin animals due to its dependence on an enery source as well as its heavy weiht. he Mi-G outputs data at a rate of Hz. he Xsens device is a miniature, liht-weiht, and D diital output sensor. It outputs D acceleration from a -axis accelerometer ( ±5m/s ), D anular rate from a -axis yroscope ( ± /s ), and D manetic field data from a - axis manetometer ( ± 75mGauss ) with built-in bias, sensitivity, and temperature compensation. ote that the Mi-G is a GPS enhanced Attitude and Headin Reference System (AHRS). he Xsens M device can track the attitude in Euler anles and quaternion representations usin an internal alorithm based on Xsens Kalman Filter (XKF) [4]. efore startin experiments, the Mi-G is attached to the back of the do (see Fi. 4). US port Computer (Roll) (Pitch) Mi-G (Yaw) Fi. 4. he Mi-G attached to the back of the do Description of the do motion o represent more accurately the onoin posture of the do, some efforts are made to alin the three axes of the body frame (the subject) with respect to the sensor frame so that the estimated orientation by the CSMO or by the Mi-G represents the real attitude of the do. Durin the motion, the do performed straiht and curved trajectories in a football stadium (see Fi. 4). Its course is composed of natural aits such as a walk, a trot, and a allop. he recorded data from the Mi-G (D-specific forces, D-anular rates, and D- Earth s manetic fields (see Fi. 5)) are used as inputs to the CSMO. For an initial period of time (between -8sec), the do is at rest. hen, its accelerations represent only the ravity projection. When the animal starts the course (between 8sec-sec), the acceleration values increase and the variation rane is between m / s and m / s. f ( m/s ) x ω (rad/s) x h (Gauss) x ime (s) ime (s) ime (s) f (m/s ) y ω (rad/s) y h (Gauss) y ime (s) ime (s) Specific forces Anular rates Earth s manetic fields 5 ime (s) f (m/s ) z ω (rad/s) z h (Gauss) z - 5 im e(s) ime (s) Fi. 5. Inertial and manetic measurements from Mi-G - 5 ime (s) he quaternion estimation errors are shown in Fi. 6 to provide an overview of the overall performance of the nonlinear observer aainst a fusion method based only on accelerometer and manetometer measurements. hese errors are computed as the difference between quaternion estimates produced by the accelerometer/manetometer fusion method (Acc/Ma) (Levenber Marquardt Alorithm) (see Fi. 6(a)) [] and Mi-G or by the CSMO and Mi-G (see Fi. 6(b)). Fiure 6(a) shows lare amounts of hih errors because of the hih frequency noise (motion accelerations) that affects the acceleration measurements. Conversely, Fi. 6(b) shows a small attitude estimation error durin the motion of the animal. Hence, this fiure illustrates the performance of the CSMO in estimatin the attitude of the do even in dynamic situations (allop). For this specific movin test, the performance of the CSMO is also compared with the fusion method (based on accelerometer and manetometer measurements) by usin the Root Mean Square Error (RMSE) criteria. herefore, the RMSE differences on Euler anles between the CSMO and Mi-G outputs durin the free movement experiments are computed. he same criterion is used to calculate the RMS differences between the accelerometer/manetometer fusion method (Acc/Ma) and the Mi-G. he obtained results (see

7 able I) illustrate the performance level consistency of the CSMO in estimatin the Euler anles with a small error. q q q q Difference between (A/M) and (Mi-G) ime (s) (a) q q q q Difference between (CSMO) and (Mi-G) ime (s) Fi. 6. (a) Differences between quaternion estimates produced by the fusion method (Acc/Ma) and Mi-G (b) Differences between quaternion estimates produced by the CSMO and Mi-G ALE I RMSE DIFFERECES O EULER AGLES Fusion method (Acc/Ma) CSMO RMSE ( ) (roll) RMSE ( ) (pitch) 5.5. RMSE ( ) (yaw) V. COCLUSIO his paper focuses on the development of an alternative approach to recover the full attitude of a movin riid body with application in io-loin area. herefore, a Complementary Slidin Mode Observer (CSMO) is desined based on the complementary filterin theory and the multiplicative quaternion correction technique. y considerin the riid body kinematic model, the CSMO is proposed with a view to do a trade-off between a ood short term precision iven by rate yros interation and a reliable lon term accuracy provided by accelerometers and manetometers measurements. he efficiency of the proposed approach is shown with experiments on a do throuh sensor measurements provided by an Inertial Measurement Unit. he experimental results point out that the alorithm is able to track the accurate orientation under several motions. his alorithm will be used on the kin penuin to assess its correspondin mechanical work. REFERECES [] S. P.. Sinh, and K. J. Waldron, Attitude estimation for dynamic leed locomotion usin rane and inertial sensors, IEEE International Conference on Robotics and Automation, arcelona, Spain, April 5, pp [] R. Mahony,. Hamel, and J. M. Pflimlin, onlinear complementary filters on the special orthoonal roup,, IEEE ransactions on Automatic Control, vol. 5, no. 5, pp. -8, June 8. [] J. F. Vasconcelos, C. Silvestre, P. Oliveira, P. atista, and. Cardeira, Discrete time-varyin attitude complementary filter, (b) IEEE American Control Conference, St. Louis, USA, June 9, pp [4] J. hienel, and R.M. Sanner, A coupled nonlinear spacecraft attitude controller and observer with an unknown constant yro bias and yro noise, IEEE ransactions on Automatic Control, vol. 48, no., ovember. [5] P. Martin, and E. Salaun, Invariant observers for attitude and headin estimation from low-cost inertial and manetic sensors, IEEE Conference on Decision and Control, ew Orleans, USA, December 7, pp [6] A. M. Sabatini, Quaternion-based Extended Kalman Filter for determinin orientation by inertial and manetic sensin, IEEE rans. on iomedical En., vol. 5, no. 7, pp , June 6. [7] V. Van Acht, E. oners,. Lambert, and R. Verberne, Miniature Wireless Inertial Sensor for Measurin Human Motions, 9 th Annual International Conference of the IEEE EMS, Lyon, France, Auust 7, pp [8] H. Fourati,. Manamanni, L. Afilal, and Y. Handrich, onlinear attitude estimation based on fusion of inertial and manetic sensors: io-loin application, IFAC International Conference on intellient Control Systems and Sinal Processin, urkey, September 9. [9] H. Fourati,. Manamanni, L. Afilal, and Y. Handrich, A riid body attitude estimation for io-loin application: A quaternion-based nonlinear filter approach, IEEE/RSJ International conference on Intellient Robots and Systems IROS 9, St. Louis, USA, October 9, pp [] C. Rutz, and G. C. Hays, ew frontiers in io-loin science, ioloy letters, March 9. [] G.H. Elkaim, E.. Decker, G. Oliver, and. Wriht, Marine Mammal Marker (MAMMARK) dead reckonin sensor for In-Situ environmental monitorin,, IEEE Position Location and aviation Symposium, Monterey, USA, April 6, pp [] M. P. Johnson, and P. L. yack, A diital acoustic recordin ta for measurin the response of wild marine mammals to sound, IEEE Journal of Oceanic En., vol. 8, no., pp. -, January. [] R. P. Wilson, E. L. C. Shepard, and. Liebsch, Pryin into the intimate details of animal lives: use of a daily diary on animals, Endanered Species Research, vol. 4, pp. -7, December 7. [4] M. S. Grewal, L. R. Weill, and A. P. Andrews, Global positionin systems, inertial naviation, and interation, John Wiley & Sons,. [5] J.. Kuipers, Quaternion and Rotation Sequences, Princeton, J: Princeton University Press, 999. [6] M.D. Shuster, A survey of attitude representations, Journal of the Astronautical Science, vol. 4, no.4, pp , 99. [7] R.G. rown, and P.Y.C Hwan, Introduction to Random Sinal and Applied Kalman Filterin, rd Ed. ew York: John Wiley, 997. [8] G. Wahba, A least squares estimate of spacecraft attitude, SIAM, Review, vol. 7, no., pp. 49, 965. [9] H. Fourati,. Manamanni, L. Afilal, and Y. Handrich, Posture and body acceleration trackin by inertial and manetic sensin: Application in behavioral analysis of free-ranin animals, iomedical Sinal Processin and Control, in press, June, doi:.6/j.bspc [] Astrosurf, January. Available: [] J. Deutschmann, I. ar-itzhack, and K. Galal, Quaternion normalization in spacecraft attitude determination, Proceedins of the AIAA Astrodynamics Conference, 99, pp [] W. Hiins, A comparison of complementary and Kalman filterin, IEEE ransactions on Aerospace and Electronic Systems, vol., no., pp. -5, 975. [] H. Fourati,. Manamanni, L. Afilal, and Y. Handrich, onlinear Filterin Approach for the Attitude and Dynamic ody Acceleration Estimation ased on Inertial and Manetic Sensors: io-loin Application, IEEE Sensors Journal, in press, July, doi:.9/jse..55 [4] Xsens echnoloies, (, February). Available: