Attitude Estimation with Gyros-Bias Compensation Using Low-Cost ...
Joint 48th IEEE Conference on Decision and Control and 28th Chinese Control Conference Shanghai, P.R. China, December 16-18, 2009
FrC03.4
Attitude estimation with gyros-bias compensation using low-cost sensors A. El Hadri and A. Benallegue
Abstract— We present in this paper a new algorithm for attitude and gyro-bias estimation of a rigid body in rotation in space using low-cost sensors. The algorithm is based on sliding mode nonlinear observer that provides in the same time estimates of the gyro-bias and the actual attitude of the rigid body. This algorithm was developed in order to address the wellknown problem of the weak dynamics of the attitude sensors (inclinometers), which can be modeled by low pass filters, and of the measurement bias of the gyros. In its design the observer uses biased gyro measurement and attitude measurements, provided by the low-cost sensors. The stability of the observer was proven using Lyapunov stability method. The effectiveness of the algorithm has been shown from experimental tests using a rotary platform equipped with several sensors with axes of rotation coincide with orientation of the rigid body.
I. INTRODUCTION The attitude control problem of rigid bodies (Walking Robots, Unmanned Aerial Vehicles, Autonomous Vehicles, ...) has been widely studied in literature (control, aerospace and robotics), and several control strategies have been proposed [4], [11], [9], [16], [19]. The effectiveness of these controls depends on the availability and reliability of measurements. In most applications in this field, these measurements are derived from sensors such as rate gyros, inclinometers, accelerometers and magnetometers. These sensors are used to perform the attitude estimation. If these sensors are of very high quality, then on the one hand the use of information from accelerometer or inclinometer and magnetometer can provide very accurate estimation of attitude that is valid only on low bandwidth. In the other hand, the rate gyros can be used to derive attitude by integrating the kinematic equations of the rigid body. Such high quality-sensors are very expensive and not suitable for commercial applications. Nowadays, the progress in micro electro-mechanical system (MEMS) and technology of the anisotropic magnetoresistive has enabled the development of low-cost inertial measurement units (IMU). However, these low-cost sensors (gyroscopes, accelerometers and magnetometers) are usually noisy and provide a biased measurement. The multiplication of the applications using low-quality sensors has lead to a strong interest in attitude estimation algorithms in order to improve the performance. Several authors in the literature proposed estimation algorithms providing an estimation of the bias assuming that the attitude is well known [12], [18]. In case of small angles variation, a linear complementary filtering technique can be used to provide relatively accurate A. El Hadri and A. Benallegue are with Laboratory of Systems Engineering (LISV), Versailles University, 10-12 Avenue de l’Europe, 78140 V´elizy, France [email protected] /
[email protected]
978-1-4244-3872-3/09/$25.00 ©2009 IEEE
attitude estimation obtained through the fusion process [2], [16]. A nonlinear complementary filtering approach with gyro-bias estimation has been proposed in [12]. A Survey of nonlinear attitude estimation methods is proposed in [5]. In [13], a high gain observer based on a low-pass sensors model and Euler equations of a rigid body has been studied for roll and pitch angles estimation by combining sensors data from gyros and inclinometers. An experimental evaluation of this algorithm compared with the standard extended Kalman filter is presented in [14]. In [1], the authors formulated the rigid body attitude control with state estimation using Rodrigues parameters and using measurements from rate gyros and low-pass inclinometers. For the problem of attitude estimation and stabilization of a rigid body in space using low-cost sensors, a model with quaternion parameterization using a first-order low-pass filter on a ”virtual” angular velocity is used in [17] to design an observer combined with complementary filter for providing estimates for the gyrobias and the actual attitude. Several other authors in literature have used the Kalman filter or extended Kalman filter to estimate the attitude of the rigid body with low-cost sensors (see for example [10], [20]). This paper deals with the problem of rigid body attitude estimation with gyro-bias compensation based on low-cost sensors. The objective is to improve the quality of measurements by using an algorithm that provides both the estimates of the real attitude and gyro-bias. Indeed, the measurements derived from tilt senors and gyros are used with an nonlinear observer based on its design on the kinematics equation of the rigid body and the dynamics of the attitude sensors.which can be modeled by low pass filters. In practice the orientation of the rigid body is obtained using low-pass sensors such as inclinometers (based on accelerometers) and magnetometers. These sensors with generally very close bandwidth provide relatively accurate attitude measurements at low frequencies. The angular velocity is provided from the gyroscopes, which often have high bandwidth, but the measure is biased. To cover a wide frequency range, the issue is to fuse these data while removing the imperfections of sensors (phase delay, bias, noise). The use of a linear complementary filter is not suitable in the case of combined movement with large variations in angles because of nonlinearities of the system. In order to covers a wide frequency range in the estimation of the real attitude by removing the imperfections of sensors, one must take into account in designing the estimation algorithm the dynamics sensors. Thus, it is considered in this work that the sensors measuring the attitude at low frequencies can be modeled as a low pass filter as proposed in [13]. Then, by using the measurements given by the attitude
8077
FrC03.4 sensors and the gyros, the filters modeling the sensors and the kinematics equation of the rigid body a nonlinear observer based on sliding mode approach (see [15]) is proposed to provide both the estimates of the real attitude and the gyrobias. This observer can be considered as an improved version of the observer proposed in [8]. Using Lyapunov stability analysis, the convergence of the estimation of the attitude and the gyro-bias to their actual values is proven. To evaluate the effectiveness of the algorithm a simulations and experimental tests are performed using a rotary platform equipped with several sensors and whose axes of rotation coincide with orientation of the rigid body. II. R IGID - BODY ATTITUDE DESCRIPTION The attitude control problems of rigid bodies such as the stabilization and navigation require the transformation of measured and computed quantities between various frames of references. The position and the attitude of a rigid-body is based on measurements from sensors attached to a rigidbody. Indeed, inertial sensors (accelerometer, gyro,. . . ) are attached to the body-platform and provide inertial measurements expressed relative to the instrument axes. In most systems, the instrument axes are nominally aligned with the body-platform axes. Since the measurements are performed in the body frame we describe in Fig. 1 the orientation of the body-fixed frame B(xm , ym , zm ) with respect to the inertial reference frame RI (xa , ya , za ). Various mathematical representation can be used to define the attitude of the rigidbody with respect to coordinate inertial reference frame. In this paper, we consider the Euler angles representation in which a transformation from one coordinate frame to another is defined by three successive rotations about different axes taken in turn. The Euler rotation angles used here corresponds to the following rotation sequence: yaw(ψ)-pitch(θ)roll (φ).
where c(·) and s(·) denote functions cos(·) and sin(·), respectively. The rotation matrix R ∈ SO(3) satisfies the following rigid body kinematic differential equation: R˙ = RS(Ω)
where S(Ω) is a skew-symmetric matrix such that S(Ω)V = Ω × V for any vector V ∈ R3 , where × is the vector cross product. Ω is the angular velocity vector of the body expressed in the body-fixed frame B. The roll, pitch and yaw angular rates (p, q, r) measured by gyros are the components of the angular velocity vector Ω. From the matrix equation (2) we can derive expression ˙ θ, ˙ ψ) ˙ to the equivalent which relates the Euler angle rates (φ, angular velocity (p, q, r) as follows: ˙ 1 tan θ sin φ tan θ cos φ φ p θ˙ = 0 cos φ − sin φ q (3) cos φ sin φ ˙ r 0 ψ cos θ cos θ The kinematic equation (3) is used to construct the observer in order to estimate the true attitude of a rigid body by canceling the phase delay caused by the time constant of the sensors.
III. P ROBLEM STATEMENT In this paper, the objective is to design a high-quality inertial measurement unit (IMU) based on low-cost sensors and using an algorithm estimation that is based on the kinematics equation of the rigid body and takes into account the dynamics sensors (see Fig. 2).
Fig. 2.
Fig. 1.
Coordinate system of a rigid body
In this case, the coordinate transformation relating body frame B to the inertial reference frame RI is given by the rotation matrix R and expressed as a function of the attitude: cθcψ −cφsψ + sθcψsφ sφsψ + cφsθcψ R = cθsψ cφcψ + sθsφsψ −cψsφ + cφsθsψ −sθ cθsφ cθcφ (1)
(2)
Scheme of the estimation algorithm
The low-cost sensors used to measure the orientation of the rigid body are generally characterized by close bandwidth and can provide a relatively accurate measure of attitude only at low frequencies. This measure can also be corrupted by noise. In this case, we assume that the attitude measure (φm , θm , ψm ) is related to the actual attitude (φ, θ, ψ) through the following first low order filters in matrix form: ˙ 1 0 0 φm φ − φm τφ 1 θ˙m = 0 0 θ − θm (4) τθ ˙ ψ − ψm 0 0 τ1 ψm ψ
These equations define the dynamics of the low-cost sensors used in IMU. The positive constants τi describe the time constants of the sensors.
8078
FrC03.4 The gyros used to obtain the angular velocities are often large bandwidth but the measurements are biased. Then one considers that the real angular velocity vector Ω, is written as: Ω = Ωm − b (5) where Ωm is a measurement provided by the gyros, b is the unknown gyro-bias which is considered constant but can vary at each use. Now, the state vectors Θ = (φ, θ, ψ)T and T Θm = (φm , θm , ψm ) are defined to represent respectively the actual and the measured attitude of the rigid-body. The equation (4) can be rewritten as: ˙ m = Π(Θ − Θm ) Θ
(6)
where matrix Π = diag( τ1φ , τ1θ , τ1ψ ) . By using kinematics equation (3) and relationship (5) one can write: ˙ = M (Θ)(Ωm − b) Θ (7)
and ¯ 6= Θ ˆ Θ otherwise (11) and Li (i = 1, 2, 3) are diagonal positive definite matrices and Sign represents the usual function sign(·) applied to each component of the vector. The function Signeq (·) is equivalent to Sign but with a low pass filter and anti-peaking structure [3]. Then for any initial states under the assumption that the pitch angle is bounded (|θ| < π2 ), there exist L1, L2 and L3 ˜ m , Θ, ˜ that ensure the boundedness of the estimation errors Θ ˜b and Θ(t)= ˜ ˜ m (t)= 0 and lim ˜b = 0. 0 for t > t1 and lim Θ t→∞ t→∞ ˜ m is exponential. The convergence of ˜b and Θ Proof: The error dynamics of the observer can be obtained by using (6), (7) and (9), it is given by: ¯ − Θ) ˆ = Sign(Θ
1 tan θ sin φ tan θ cos φ cos φ − sin φ M (Θ) = 0 sin φ cos φ 0 cos θ cos θ
(8)
Then given (6) and (7), one can propose a nonlinear observer that provide estimates of both the real attitude Θ and the gyro-bias b. ˜ m = Θm − Θ ˆm, Θ ˜ =Θ−Θ ˆ We also define the errors Θ ˜ ˆ and b = b − b which describe the observation errors of the measured attitude, the actual attitude and the actual bias respectively. Remark 1: Is is known that the singularity is inherent in the use of the representation of Euler angles. Thus, to avoid this problem associated with Euler angles, it is assumed that the pitch angle is kept in a range less then ± π2 . This applies to applications such as walking robot, autonomous vehicle, stabilized aero-vehicle, etc.
0 if ¯ − Θ) ˆ Signeq (Θ
˜˙ m = Π(Θ ˜ − L1 Θ ˜ m) Θ ˜˙ ¯ ˜b − L2 Sign(Θ ¯ − Θ) ˆ Θ=∆f − M (Θ) ˜b˙ = L3 M −1 (Θ)L ¯ − Θ) ˆ ¯ 2 Sign(Θ
where :
(12b) (12c)
¯ with the function f defined as where ∆f = f (Θ) − f (Θ), f : D ⊂ R3 → R3 as f (Θ) = M (Θ)Ω. Consider in the first step the observation error dynamics (9a) which is described by a first order linear model. This one can be expressed using transfer function representation as: 1 ˜ ˜ m (i) = L1i Θ Θ(i) (13) i 1 + Lτ1i s where the index i (i = 1, 2, 3) is used to denote successively the variables φ, θ, ψ and L1i represents the diagonal element i of the observer gain L1 . From the frequency analysis of (13), one can show that if the gain L1i is chosen large enough of (13) is greater or that the cut-off frequency ω ¯ i = Lτ1i i equal to rate gyros, then the relation bellow can be admitted: ˜ (i) = L1i Θ ˜ m (t)(i) Θ(t)
IV. D ESIGN OF THE NONLINEAR OBSERVER FOR ESTIMATING ATTITUDE AND GYROS - BIAS
(12a)
(14)
Using (10), one can write:
The aim of this section is to design a nonlinear observer for the attitude and gyro-bias estimation. This observer can be considered as an improved version of the observer proposed in [8]. Indeed, to improve the performance of the observer, the notion of finite time convergence is used to obtain step by step the estimation of the actual attitude and the gyro-bias. Theorem 1: Consider the following nonlinear observer designed based on (6) and (7): ˆ˙ m = Π(Θ ˆ − Θm + L1 Θ ˜ m) Θ ˆ ˆ˙ ¯ ¯ ˆ Θ=M (Θ)(Ω m − b) + L2 Sign(Θ − Θ) ˆb˙ = −L3 M −1 (Θ)L ¯ − Θ) ˆ ¯ 2 Sign(Θ
(9b)
¯ =Θ ˆ + L1 Θ ˜m Θ
(10)
¯ (i) − Θ(t) ˆ ˜ Θ(t) (i) = L1i Θm (t)(i)
˜ =Θ ¯ −Θ ˆ So from (14) and (15) one can conclude that Θ ¯ . In this case, the equaand therefore one can have Θ = Θ tions of the observation error dynamics (12b) and (12c) can be rewritten as follows: ˜˙ − M (Θ)˜b − L Sign(Θ) ˜ Θ= (16a) 2
˜b˙ = L3 M −1 (Θ)L2 Sign(Θ) ˜
(9a)
(9c)
where
(15)
(16b)
The obtained equations (16a) and (16b) correspond to a sliding mode system with sliding manifold based on the ˜ The function Sign(·) as defined by equation (11) error Θ. is used to maintain ˜b bounded before reaching the sliding ˜ = 0. Then, the convergence of the observer can manifold Θ be conducted in two steps. In the first step, the convergence
8079
FrC03.4 ˜ is shown and thereafter the to zero in finite time of Θ exponential convergence to zero of ˜b is proven. For the first step, one can obtain the following observation error dynamic: ! ˜˙ ˜ Θ −M (Θ)˜b − L2 Sign(Θ) (17) = 0 ˜b˙ ˜ = 0, consider In order to reach the manifold defined by Θ 1 ˜T ˜ the candidate Lyapunov function V1 = 2 Θ Θ. Then the time derivative of this Lyapunov function is given as: (18)
By choosing the observer gains
of the matrix L2 sufficiently
˜ large such that kM (Θ)k b < λmax (L2 ), the sliding mode ˜ = 0, occurs in finite time t1 along the sliding manifold Θ (see [15] for more details). So, as shown in [7] and according to the equivalent control method, the system in sliding mode ˜ = 0 and Θ ˜˙ = 0) behaves as if the discontinuous term (Θ ˜ L2 Sign(Θ) is replaced by its equivalent value which can ˜ = −M (Θ)˜b . This equivalent be deduced as L2 Signeq (Θ) term can be obtained in finite time t1 via a low pass filtering ˜ Thus, at time t1 , one can have Θ ˜ = 0 and of L2 Sign(Θ). ˙ ˜ = 0, the observation error (16) can be written as: Θ ! ˜˙ ˜ =0 Θ −M (Θ)˜b − L2 Sign(Θ) (19) = ˜b˙ −L3˜b Now, one wants to reach the manifold defined by ˜ = 0, ˜b = 0). Then, the following Lyapunov function is (Θ chosen as: 1 ˜ T ˜ 1 ˜T ˜ V2 = Θ Θ+ b b (20) 2 2 If the condition obtained in the first step holds for all t > t1 , ˜ = 0 and Θ ˜˙ = 0, and the time derivative of the then Θ Lyapunov function (20) gives V˙ 2 = −˜bT L3˜b. ˜ and ˜b globally bounded Thus, one can conclude that Θ ˜ and Θ(t)= 0 in finite time and lim ˜b = 0 exponentially. t→∞ ˜ m (t)= 0 Moreover, using the (13), one can deduce that lim Θ
Fig. 3.
VI. S IMULATION AND EXPERIMENTAL RESULTS To validate the proposed algorithm, we have simulated the low cost sensors by first order low-pass filters with a unit gain and time constant τφ = 0.05 sec , τθ = 0.1 sec and τψ = 0.5 sec . The input of the filters are the attitude given from potentiometer sensors of the platform. To generate data, the platform was operated by hands combining the movements of roll, pitch and yaw for coupling between the different dynamics. These tests combine slow and fast movements and small and large angle variations. For this simulation tests, the angular velocity of the platform is derived from real gyros. The measures of the rate gyros are presented in Fig (4). The gyros-bias are identified in 5 p q r
4
3
t→∞
exponentially.
Experimental platform: three rotational axis
ADXL330 Accelerometer ICs as inclinometers. All sensors of the platform are connected to a PC Pentium, equipped with a dSpace DS1103 PPC real-time controller board using Matlab and Simulink software. The sampling frequency has been fixed to 1 kHz.
Angular velocityΩ [rad/s]
˜˙ = Θ ˜ T (−M (Θ)˜b − L2 Sign(Θ)) ˜ ˜TΘ V˙ 1 = Θ
The result shows that it is possible to improve the quality of measurements derived from low-cost sensors using the nonlinear observer described in (9), (10). In the next section, the experimental validation of this observer is presented.
2
1
0
-1
-2
-3
-4
0
1
3
4
5
6
7
Time [s]
V. E XPERIMENTAL SETUP
Fig. 4.
For the experimental validation of the proposed observer, a rotational platform (see figure 3) was designed to generate a known motion in order to be able to compare the attitude estimation given by observer to the actual attitude of the platform. Yaw, pitch and roll angles are the three degrees of freedom of this platform. Each degree of freedom coincides with a rotational axis of the platform and all rotational joints are equipped with potentiometer sensors: Then, to evaluate the estimation algorithm, it was mounted on the platform a low-cost gyros IDG300 Gyro and
2
Measures of the rate gyros
order to be able to compare them with those estimated by the algorithm. The table I gives the identified gyrosbias. The gyros have a bandwidth of about fc = 1kHz. This value is used to define the observer gain L1 such L1i > 2πfc τi , (i = φ, θ, ψ). The gain L2 is chosen
observer
such as λmin (L2 ) > kM (Θ)k ˜b with in mind that the maximum bias of the gyros is 2 and the pitch angle is bounded as |θ| < π2 . The matrix gains L3 is chosen diagonal and positive, it ensure exponential convergence to zero of
8080
FrC03.4 TABLE I G YROS - BIAS
15
Yaw angle [deg]
Gyro − bias Roll rate (p) P itch rate (q) Y aw rate (r)
20
b −0.59 1.56 −0.85
10 5 0 Zoom -5 -10 -15 -20
0
1
2
3
4
5
6
7
Time [s] Actual ψ Observed ψ "Measured" ψ 1.5 1 0.5 Finite time convergence
0
the bias estimation. The observer is initialized as follows : ˆ m (0) = Θm (0), Θ(0) ˆ Θ = 0 and ˆb(0) = 0. In figures (5), (6) and (7), the results of the simulation test are presented. One can observe that the estimation attitude converge to the real attitude and the phase delay due to different time constants is corrected. One can also observe the convergence of these estimation in finite time. This result shows the establishment of the sliding mode and the convergence of the observation error to zero. In Fig. (8), the
-0.5 -1 0.05
Fig. 7.
0.1
0.15
0.2
0.25
0.3
0.35
Estimation of the yaw angle ψˆ
2 bias-roll bias-pitch bias-yaw 1.5
Gyro-bias
1
Roll angle [deg]
20
0.5
10 0 0 Zoom -10 -0.5 -20
-30
0
1
2
3
4
5
6
7
Time [s]
-1 Actual φ Observed φ
0
1
2
3
4
5
6
7
Time [s]
"Measured" φ
0 -0.5 -1
Fig. 8.
-1.5 -2
Estimation of the gyro bias ˆb
-2.5 -3 -3.5 -4 0.1
Fig. 5.
0.2
0.3
0.4
where the gains k and the time constants τ for each sensor are given in table II. The parameters of transfer functions
Estimation of the roll angle φˆ
TABLE II PARAMETERS OF THE LOW- PASS SENSORS 30
Rotational axis Roll angle φ P itch angle θ Y aw angles ψ
Pitch angle [deg]
20 10 0 Zoom -10 -20 -30
0
1
2
3
4
5
6
Time [s]
τ 0.034 0.026 0.041
7 Actual θ Observed θ "Measured" θ
3.5 3 2.5 Finite time convergence
2 1.5 1 0.5 0 -0.5 0
Fig. 6.
k 1.16 1.07 0.99
0.1
0.2
0.3
0.4
Estimation of the pitch angle θˆ
estimation of the real bias of the gyro is shown. One can observe that bias estimation converge to the value given in table (I). For the experimental test we have use the same conditions of the previous test but in this case the measured signal is provided by tilt sensors. However, some tests have been performed to identify the static and dynamic characteristics of the tilt sensors. The data analysis shows that these sensors can be modeled by low pass filters with a transfer function given by: k (21) H(s) = 1 + τs
identified previously (see table II) are used for implementation of the estimation algorithm defined by the observer (9) and (10) with the choice of matrix gains as described previously. The obtained results are shown in the following figures. In Fig. (9), the real attitude given by potentiometers is compared to that provided by tilt sensors. One can observe a phase delay of the tilt sensors responses. This explain the use of the model (6). In Fig. (10), the result of estimation of the real attitude ˆ is shown. This figure presents the attitude estimation Θ compared to actual attitude given by potentiometers. One can observe the convergence of these estimates to the real attitude. This result shows that phase delay of the tilt sensors was also compensated. The estimation of the gyro-bias is presented in Fig. (11). This corroborates the results obtained previously. These obtained results show the effectiveness of the observer to improve the quality of measurements provided by low-cost sensors.
8081
FrC03.4 30
2 bias-roll bias-pitch bias-yaw
Roll angle [deg]
20 10 1.5 0 -10 1
-30
0
1
2
3
4
5
6
Time [s]
Gyro-bias
-20
7 Real attitude measured attitude
30
Pitch angle [deg]
20
0.5
0
10 0 -0.5 -10 -20 -30
0
1
2
3
4
5
6
-1
7
0
1
Time [s]
Roll angle [deg]
20
10
0
-10
-20 2
3
4
5
6
7
Time [s]
Actual Θ Observed Θ
30
Pitch angle [deg]
20 10 0 -10 -20 -30
0
1
2
3
4
5
6
7
Time [s]
Fig. 10.
3
4
5
6
7
Time [s]
Fig. 9. Real attitude given by potentiometers, Θ and its measure provided by tilt sensors, Θm
1
2
ˆm The actual attitude Θ and its observation Θ
VII. CONCLUSIONS A new approach based on nonlinear observer algorithm has been proposed to estimate both the attitude and the gyrobias in order to improve the quality of measures obtained by using low-cost sensors. The algorithm was developed using the kinematics equation of a rigid body and by considering that the attitude sensors can be modeled as low-pass filters. The observer is designed based on the sliding-mode approach and uses the real measurements given by the attitude sensors and the gyros. The stability of the observer was proven using Lyapunov stability method and the notion of finite time convergence is used to obtain step by step the estimation of the actual attitude and the gyro-bias. The effectiveness of the algorithm has been shown from a simulations and experimental tests using a rotary platform equipped with several sensors with axes of rotation coincide with orientation of the rigid body. R EFERENCES [1] M. R. Akella, J. T. Halbert, and G. R. Kotamraju, ”Rigid body attitude control with inclinometer and low-cost gyro measurements”, Syst. Contr. Lett., Vol. 49, pp. 151–159, 2003. [2] A.J. Baerveldt and R. Klang, ”A low-cost and low-weight attitude estimation system for an autonomous helicopter”, Proc. IEEE Int. Conf. on Intelligent Engineering Systems, pp. 391-395, 1997. [3] J.P Barbot, T. Boukhobza and M Djemai, ”Sliding mode observer for triangular input form”, In proc. of the 35th IEEE Conference on Decision and Control, Kobe, Japan, dec. 11-13, 1996. [4] A. Benallegue, A. Mokhtari and L. Fridman, ”High-order sliding-mode observer for a quadrotor UAV”, Int. J. Robust Nonlinear Control 2008; 18:427–440.
Fig. 11.
Estimation of the gyro bias ˆb
[5] J.L. Crassidis, F. Landis Markley, and Y. Cheng, ”Survey of nonlinear attitude estimation methods”, Journal of Guidance, Control, and Dynamics, Vol. 30, No. 1, pp. 12–28, 2007. [6] P. Castillo, A. Dzul and R. Lozano, ”Real-time stabilization and tracking of a four rotor mini rotorcraft,” IEEE Trans. Control Syst. Technol., Vol 12, No. 4, pp. 510–516, Jul. 2004. [7] S. Drakunov and V. Utkin, ”Sliding mode observer: Tutorial”, In proc. of the 34th IEEE Conference on Decision and Control, New Orleans, Louisiana, USA, 1995. [8] A. El-Hadri, A. Benallegue, ”Sliding Mode Observer to Estimate Both the Attitude and the Gyro-Bias by Using Low-Cost Sensors”, Proc. IEEE Int. Conf. on Intelligent Robots and Systems, October 11-15, 2009, St. Louis, USA [9] S. M. Joshi, A. G.Kelkar, and J. T.-Y.Wen, ”Robust attitude stabilization of spacecraft using nonlinear quaternion feedback”, IEEE Transactions on Automatic Control, Vol. 40, No. 10, pp. 1800–1803, Oct. 1995. [10] D. Jurman, M. JanKovec, R. Kamnik and M. Topic, ”Calibration and data fusion solution for the miniature attitude and heading reference system”, Sensors and Actuators A 138, pp. 411–420, 2007. [11] T. Madani, A. Benallegue, ”Control of a quadrotor mini-helicopter via full state backstepping technique”, In: Proceedings of the 45th IEEE Conference on Decision and Control. pp. 1515–1520. [12] R.Mahony, T Hamel and J. Pflimlin, ”Nonlinear complementary filters on the special orthogonal group”, IEEE Transactions on Automatic Control, Vol. 53, No. 5, pp. 1203–1218, 2008. [13] H. Rehbinder, X. Hu, ”Nonlinear state estimation for rigid-body motion with low-pass sensors”, Systems Control Letter, 40 (2000) 183–190. ” [14] Henrik Rehbinder and Xiaoming Hu, Nonlinear Pitch and Roll Estimation for Walking Robots”, IEEE Int. Conf. on Robotics and Automation (ICRA 2000), San Francisco, CA, USA, April 24-28, 2000. [15] J.-J. E. Slotine, J. K. Hedrick and E. A. Misawa, ”On Sliding Observers for Nonlinear Systems”, in ASME Jornal of Dynamic Sytems, Measurement and Control, Vol. 109, pp. 245–252, 1987. [16] A. Tayebi and S. McGilvray, ”Attitude stabilization of a quadrotor aircraft”, IEEE Transactions on Control Systems Technology, Vol. 14, No. 3, pp. 562–571, May 2006. [17] A. Tayebi, S. McGilvray, A. Roberts and M. Moallem, ”Attitude estimation and stabilization of a rigid body using low-cost sensors”, In proc. of the 46th IEEE Conference on Decision and Control, New Orleans, LA, USA, Dec. 12-14, 2007. [18] J. Thienel and R.M. Sanner ”A coupled nonlinear spacecraft attitude controller and observer with an unknown constant gyro bias and gyro noise”, IEEE Transactions on Automatic Control, Vol. 48, No. 11, pp. 2011–2015, 2003. [19] J. T.-Y. Wen and K. Kreutz-Delgado, ”The attitude control problem”, IEEE Transactions on Automatic Control, Vol. 36, No. 10, pp. 1148– 1162, Oct. 1991. [20] R.Zhu, D. Sun, Z. Zhou and D. Wang ”A linear fusion algorithm for attitude determination using low cost MEMS-based sensors”, Measurement, Vol. 40, pp. 322–328, 2007.
8082
FrC03.4
Attitude estimation with gyros-bias compensation using low-cost sensors A. El Hadri and A. Benallegue
Abstract— We present in this paper a new algorithm for attitude and gyro-bias estimation of a rigid body in rotation in space using low-cost sensors. The algorithm is based on sliding mode nonlinear observer that provides in the same time estimates of the gyro-bias and the actual attitude of the rigid body. This algorithm was developed in order to address the wellknown problem of the weak dynamics of the attitude sensors (inclinometers), which can be modeled by low pass filters, and of the measurement bias of the gyros. In its design the observer uses biased gyro measurement and attitude measurements, provided by the low-cost sensors. The stability of the observer was proven using Lyapunov stability method. The effectiveness of the algorithm has been shown from experimental tests using a rotary platform equipped with several sensors with axes of rotation coincide with orientation of the rigid body.
I. INTRODUCTION The attitude control problem of rigid bodies (Walking Robots, Unmanned Aerial Vehicles, Autonomous Vehicles, ...) has been widely studied in literature (control, aerospace and robotics), and several control strategies have been proposed [4], [11], [9], [16], [19]. The effectiveness of these controls depends on the availability and reliability of measurements. In most applications in this field, these measurements are derived from sensors such as rate gyros, inclinometers, accelerometers and magnetometers. These sensors are used to perform the attitude estimation. If these sensors are of very high quality, then on the one hand the use of information from accelerometer or inclinometer and magnetometer can provide very accurate estimation of attitude that is valid only on low bandwidth. In the other hand, the rate gyros can be used to derive attitude by integrating the kinematic equations of the rigid body. Such high quality-sensors are very expensive and not suitable for commercial applications. Nowadays, the progress in micro electro-mechanical system (MEMS) and technology of the anisotropic magnetoresistive has enabled the development of low-cost inertial measurement units (IMU). However, these low-cost sensors (gyroscopes, accelerometers and magnetometers) are usually noisy and provide a biased measurement. The multiplication of the applications using low-quality sensors has lead to a strong interest in attitude estimation algorithms in order to improve the performance. Several authors in the literature proposed estimation algorithms providing an estimation of the bias assuming that the attitude is well known [12], [18]. In case of small angles variation, a linear complementary filtering technique can be used to provide relatively accurate A. El Hadri and A. Benallegue are with Laboratory of Systems Engineering (LISV), Versailles University, 10-12 Avenue de l’Europe, 78140 V´elizy, France [email protected] /
[email protected]
978-1-4244-3872-3/09/$25.00 ©2009 IEEE
attitude estimation obtained through the fusion process [2], [16]. A nonlinear complementary filtering approach with gyro-bias estimation has been proposed in [12]. A Survey of nonlinear attitude estimation methods is proposed in [5]. In [13], a high gain observer based on a low-pass sensors model and Euler equations of a rigid body has been studied for roll and pitch angles estimation by combining sensors data from gyros and inclinometers. An experimental evaluation of this algorithm compared with the standard extended Kalman filter is presented in [14]. In [1], the authors formulated the rigid body attitude control with state estimation using Rodrigues parameters and using measurements from rate gyros and low-pass inclinometers. For the problem of attitude estimation and stabilization of a rigid body in space using low-cost sensors, a model with quaternion parameterization using a first-order low-pass filter on a ”virtual” angular velocity is used in [17] to design an observer combined with complementary filter for providing estimates for the gyrobias and the actual attitude. Several other authors in literature have used the Kalman filter or extended Kalman filter to estimate the attitude of the rigid body with low-cost sensors (see for example [10], [20]). This paper deals with the problem of rigid body attitude estimation with gyro-bias compensation based on low-cost sensors. The objective is to improve the quality of measurements by using an algorithm that provides both the estimates of the real attitude and gyro-bias. Indeed, the measurements derived from tilt senors and gyros are used with an nonlinear observer based on its design on the kinematics equation of the rigid body and the dynamics of the attitude sensors.which can be modeled by low pass filters. In practice the orientation of the rigid body is obtained using low-pass sensors such as inclinometers (based on accelerometers) and magnetometers. These sensors with generally very close bandwidth provide relatively accurate attitude measurements at low frequencies. The angular velocity is provided from the gyroscopes, which often have high bandwidth, but the measure is biased. To cover a wide frequency range, the issue is to fuse these data while removing the imperfections of sensors (phase delay, bias, noise). The use of a linear complementary filter is not suitable in the case of combined movement with large variations in angles because of nonlinearities of the system. In order to covers a wide frequency range in the estimation of the real attitude by removing the imperfections of sensors, one must take into account in designing the estimation algorithm the dynamics sensors. Thus, it is considered in this work that the sensors measuring the attitude at low frequencies can be modeled as a low pass filter as proposed in [13]. Then, by using the measurements given by the attitude
8077
FrC03.4 sensors and the gyros, the filters modeling the sensors and the kinematics equation of the rigid body a nonlinear observer based on sliding mode approach (see [15]) is proposed to provide both the estimates of the real attitude and the gyrobias. This observer can be considered as an improved version of the observer proposed in [8]. Using Lyapunov stability analysis, the convergence of the estimation of the attitude and the gyro-bias to their actual values is proven. To evaluate the effectiveness of the algorithm a simulations and experimental tests are performed using a rotary platform equipped with several sensors and whose axes of rotation coincide with orientation of the rigid body. II. R IGID - BODY ATTITUDE DESCRIPTION The attitude control problems of rigid bodies such as the stabilization and navigation require the transformation of measured and computed quantities between various frames of references. The position and the attitude of a rigid-body is based on measurements from sensors attached to a rigidbody. Indeed, inertial sensors (accelerometer, gyro,. . . ) are attached to the body-platform and provide inertial measurements expressed relative to the instrument axes. In most systems, the instrument axes are nominally aligned with the body-platform axes. Since the measurements are performed in the body frame we describe in Fig. 1 the orientation of the body-fixed frame B(xm , ym , zm ) with respect to the inertial reference frame RI (xa , ya , za ). Various mathematical representation can be used to define the attitude of the rigidbody with respect to coordinate inertial reference frame. In this paper, we consider the Euler angles representation in which a transformation from one coordinate frame to another is defined by three successive rotations about different axes taken in turn. The Euler rotation angles used here corresponds to the following rotation sequence: yaw(ψ)-pitch(θ)roll (φ).
where c(·) and s(·) denote functions cos(·) and sin(·), respectively. The rotation matrix R ∈ SO(3) satisfies the following rigid body kinematic differential equation: R˙ = RS(Ω)
where S(Ω) is a skew-symmetric matrix such that S(Ω)V = Ω × V for any vector V ∈ R3 , where × is the vector cross product. Ω is the angular velocity vector of the body expressed in the body-fixed frame B. The roll, pitch and yaw angular rates (p, q, r) measured by gyros are the components of the angular velocity vector Ω. From the matrix equation (2) we can derive expression ˙ θ, ˙ ψ) ˙ to the equivalent which relates the Euler angle rates (φ, angular velocity (p, q, r) as follows: ˙ 1 tan θ sin φ tan θ cos φ φ p θ˙ = 0 cos φ − sin φ q (3) cos φ sin φ ˙ r 0 ψ cos θ cos θ The kinematic equation (3) is used to construct the observer in order to estimate the true attitude of a rigid body by canceling the phase delay caused by the time constant of the sensors.
III. P ROBLEM STATEMENT In this paper, the objective is to design a high-quality inertial measurement unit (IMU) based on low-cost sensors and using an algorithm estimation that is based on the kinematics equation of the rigid body and takes into account the dynamics sensors (see Fig. 2).
Fig. 2.
Fig. 1.
Coordinate system of a rigid body
In this case, the coordinate transformation relating body frame B to the inertial reference frame RI is given by the rotation matrix R and expressed as a function of the attitude: cθcψ −cφsψ + sθcψsφ sφsψ + cφsθcψ R = cθsψ cφcψ + sθsφsψ −cψsφ + cφsθsψ −sθ cθsφ cθcφ (1)
(2)
Scheme of the estimation algorithm
The low-cost sensors used to measure the orientation of the rigid body are generally characterized by close bandwidth and can provide a relatively accurate measure of attitude only at low frequencies. This measure can also be corrupted by noise. In this case, we assume that the attitude measure (φm , θm , ψm ) is related to the actual attitude (φ, θ, ψ) through the following first low order filters in matrix form: ˙ 1 0 0 φm φ − φm τφ 1 θ˙m = 0 0 θ − θm (4) τθ ˙ ψ − ψm 0 0 τ1 ψm ψ
These equations define the dynamics of the low-cost sensors used in IMU. The positive constants τi describe the time constants of the sensors.
8078
FrC03.4 The gyros used to obtain the angular velocities are often large bandwidth but the measurements are biased. Then one considers that the real angular velocity vector Ω, is written as: Ω = Ωm − b (5) where Ωm is a measurement provided by the gyros, b is the unknown gyro-bias which is considered constant but can vary at each use. Now, the state vectors Θ = (φ, θ, ψ)T and T Θm = (φm , θm , ψm ) are defined to represent respectively the actual and the measured attitude of the rigid-body. The equation (4) can be rewritten as: ˙ m = Π(Θ − Θm ) Θ
(6)
where matrix Π = diag( τ1φ , τ1θ , τ1ψ ) . By using kinematics equation (3) and relationship (5) one can write: ˙ = M (Θ)(Ωm − b) Θ (7)
and ¯ 6= Θ ˆ Θ otherwise (11) and Li (i = 1, 2, 3) are diagonal positive definite matrices and Sign represents the usual function sign(·) applied to each component of the vector. The function Signeq (·) is equivalent to Sign but with a low pass filter and anti-peaking structure [3]. Then for any initial states under the assumption that the pitch angle is bounded (|θ| < π2 ), there exist L1, L2 and L3 ˜ m , Θ, ˜ that ensure the boundedness of the estimation errors Θ ˜b and Θ(t)= ˜ ˜ m (t)= 0 and lim ˜b = 0. 0 for t > t1 and lim Θ t→∞ t→∞ ˜ m is exponential. The convergence of ˜b and Θ Proof: The error dynamics of the observer can be obtained by using (6), (7) and (9), it is given by: ¯ − Θ) ˆ = Sign(Θ
1 tan θ sin φ tan θ cos φ cos φ − sin φ M (Θ) = 0 sin φ cos φ 0 cos θ cos θ
(8)
Then given (6) and (7), one can propose a nonlinear observer that provide estimates of both the real attitude Θ and the gyro-bias b. ˜ m = Θm − Θ ˆm, Θ ˜ =Θ−Θ ˆ We also define the errors Θ ˜ ˆ and b = b − b which describe the observation errors of the measured attitude, the actual attitude and the actual bias respectively. Remark 1: Is is known that the singularity is inherent in the use of the representation of Euler angles. Thus, to avoid this problem associated with Euler angles, it is assumed that the pitch angle is kept in a range less then ± π2 . This applies to applications such as walking robot, autonomous vehicle, stabilized aero-vehicle, etc.
0 if ¯ − Θ) ˆ Signeq (Θ
˜˙ m = Π(Θ ˜ − L1 Θ ˜ m) Θ ˜˙ ¯ ˜b − L2 Sign(Θ ¯ − Θ) ˆ Θ=∆f − M (Θ) ˜b˙ = L3 M −1 (Θ)L ¯ − Θ) ˆ ¯ 2 Sign(Θ
where :
(12b) (12c)
¯ with the function f defined as where ∆f = f (Θ) − f (Θ), f : D ⊂ R3 → R3 as f (Θ) = M (Θ)Ω. Consider in the first step the observation error dynamics (9a) which is described by a first order linear model. This one can be expressed using transfer function representation as: 1 ˜ ˜ m (i) = L1i Θ Θ(i) (13) i 1 + Lτ1i s where the index i (i = 1, 2, 3) is used to denote successively the variables φ, θ, ψ and L1i represents the diagonal element i of the observer gain L1 . From the frequency analysis of (13), one can show that if the gain L1i is chosen large enough of (13) is greater or that the cut-off frequency ω ¯ i = Lτ1i i equal to rate gyros, then the relation bellow can be admitted: ˜ (i) = L1i Θ ˜ m (t)(i) Θ(t)
IV. D ESIGN OF THE NONLINEAR OBSERVER FOR ESTIMATING ATTITUDE AND GYROS - BIAS
(12a)
(14)
Using (10), one can write:
The aim of this section is to design a nonlinear observer for the attitude and gyro-bias estimation. This observer can be considered as an improved version of the observer proposed in [8]. Indeed, to improve the performance of the observer, the notion of finite time convergence is used to obtain step by step the estimation of the actual attitude and the gyro-bias. Theorem 1: Consider the following nonlinear observer designed based on (6) and (7): ˆ˙ m = Π(Θ ˆ − Θm + L1 Θ ˜ m) Θ ˆ ˆ˙ ¯ ¯ ˆ Θ=M (Θ)(Ω m − b) + L2 Sign(Θ − Θ) ˆb˙ = −L3 M −1 (Θ)L ¯ − Θ) ˆ ¯ 2 Sign(Θ
(9b)
¯ =Θ ˆ + L1 Θ ˜m Θ
(10)
¯ (i) − Θ(t) ˆ ˜ Θ(t) (i) = L1i Θm (t)(i)
˜ =Θ ¯ −Θ ˆ So from (14) and (15) one can conclude that Θ ¯ . In this case, the equaand therefore one can have Θ = Θ tions of the observation error dynamics (12b) and (12c) can be rewritten as follows: ˜˙ − M (Θ)˜b − L Sign(Θ) ˜ Θ= (16a) 2
˜b˙ = L3 M −1 (Θ)L2 Sign(Θ) ˜
(9a)
(9c)
where
(15)
(16b)
The obtained equations (16a) and (16b) correspond to a sliding mode system with sliding manifold based on the ˜ The function Sign(·) as defined by equation (11) error Θ. is used to maintain ˜b bounded before reaching the sliding ˜ = 0. Then, the convergence of the observer can manifold Θ be conducted in two steps. In the first step, the convergence
8079
FrC03.4 ˜ is shown and thereafter the to zero in finite time of Θ exponential convergence to zero of ˜b is proven. For the first step, one can obtain the following observation error dynamic: ! ˜˙ ˜ Θ −M (Θ)˜b − L2 Sign(Θ) (17) = 0 ˜b˙ ˜ = 0, consider In order to reach the manifold defined by Θ 1 ˜T ˜ the candidate Lyapunov function V1 = 2 Θ Θ. Then the time derivative of this Lyapunov function is given as: (18)
By choosing the observer gains
of the matrix L2 sufficiently
˜ large such that kM (Θ)k b < λmax (L2 ), the sliding mode ˜ = 0, occurs in finite time t1 along the sliding manifold Θ (see [15] for more details). So, as shown in [7] and according to the equivalent control method, the system in sliding mode ˜ = 0 and Θ ˜˙ = 0) behaves as if the discontinuous term (Θ ˜ L2 Sign(Θ) is replaced by its equivalent value which can ˜ = −M (Θ)˜b . This equivalent be deduced as L2 Signeq (Θ) term can be obtained in finite time t1 via a low pass filtering ˜ Thus, at time t1 , one can have Θ ˜ = 0 and of L2 Sign(Θ). ˙ ˜ = 0, the observation error (16) can be written as: Θ ! ˜˙ ˜ =0 Θ −M (Θ)˜b − L2 Sign(Θ) (19) = ˜b˙ −L3˜b Now, one wants to reach the manifold defined by ˜ = 0, ˜b = 0). Then, the following Lyapunov function is (Θ chosen as: 1 ˜ T ˜ 1 ˜T ˜ V2 = Θ Θ+ b b (20) 2 2 If the condition obtained in the first step holds for all t > t1 , ˜ = 0 and Θ ˜˙ = 0, and the time derivative of the then Θ Lyapunov function (20) gives V˙ 2 = −˜bT L3˜b. ˜ and ˜b globally bounded Thus, one can conclude that Θ ˜ and Θ(t)= 0 in finite time and lim ˜b = 0 exponentially. t→∞ ˜ m (t)= 0 Moreover, using the (13), one can deduce that lim Θ
Fig. 3.
VI. S IMULATION AND EXPERIMENTAL RESULTS To validate the proposed algorithm, we have simulated the low cost sensors by first order low-pass filters with a unit gain and time constant τφ = 0.05 sec , τθ = 0.1 sec and τψ = 0.5 sec . The input of the filters are the attitude given from potentiometer sensors of the platform. To generate data, the platform was operated by hands combining the movements of roll, pitch and yaw for coupling between the different dynamics. These tests combine slow and fast movements and small and large angle variations. For this simulation tests, the angular velocity of the platform is derived from real gyros. The measures of the rate gyros are presented in Fig (4). The gyros-bias are identified in 5 p q r
4
3
t→∞
exponentially.
Experimental platform: three rotational axis
ADXL330 Accelerometer ICs as inclinometers. All sensors of the platform are connected to a PC Pentium, equipped with a dSpace DS1103 PPC real-time controller board using Matlab and Simulink software. The sampling frequency has been fixed to 1 kHz.
Angular velocityΩ [rad/s]
˜˙ = Θ ˜ T (−M (Θ)˜b − L2 Sign(Θ)) ˜ ˜TΘ V˙ 1 = Θ
The result shows that it is possible to improve the quality of measurements derived from low-cost sensors using the nonlinear observer described in (9), (10). In the next section, the experimental validation of this observer is presented.
2
1
0
-1
-2
-3
-4
0
1
3
4
5
6
7
Time [s]
V. E XPERIMENTAL SETUP
Fig. 4.
For the experimental validation of the proposed observer, a rotational platform (see figure 3) was designed to generate a known motion in order to be able to compare the attitude estimation given by observer to the actual attitude of the platform. Yaw, pitch and roll angles are the three degrees of freedom of this platform. Each degree of freedom coincides with a rotational axis of the platform and all rotational joints are equipped with potentiometer sensors: Then, to evaluate the estimation algorithm, it was mounted on the platform a low-cost gyros IDG300 Gyro and
2
Measures of the rate gyros
order to be able to compare them with those estimated by the algorithm. The table I gives the identified gyrosbias. The gyros have a bandwidth of about fc = 1kHz. This value is used to define the observer gain L1 such L1i > 2πfc τi , (i = φ, θ, ψ). The gain L2 is chosen
observer
such as λmin (L2 ) > kM (Θ)k ˜b with in mind that the maximum bias of the gyros is 2 and the pitch angle is bounded as |θ| < π2 . The matrix gains L3 is chosen diagonal and positive, it ensure exponential convergence to zero of
8080
FrC03.4 TABLE I G YROS - BIAS
15
Yaw angle [deg]
Gyro − bias Roll rate (p) P itch rate (q) Y aw rate (r)
20
b −0.59 1.56 −0.85
10 5 0 Zoom -5 -10 -15 -20
0
1
2
3
4
5
6
7
Time [s] Actual ψ Observed ψ "Measured" ψ 1.5 1 0.5 Finite time convergence
0
the bias estimation. The observer is initialized as follows : ˆ m (0) = Θm (0), Θ(0) ˆ Θ = 0 and ˆb(0) = 0. In figures (5), (6) and (7), the results of the simulation test are presented. One can observe that the estimation attitude converge to the real attitude and the phase delay due to different time constants is corrected. One can also observe the convergence of these estimation in finite time. This result shows the establishment of the sliding mode and the convergence of the observation error to zero. In Fig. (8), the
-0.5 -1 0.05
Fig. 7.
0.1
0.15
0.2
0.25
0.3
0.35
Estimation of the yaw angle ψˆ
2 bias-roll bias-pitch bias-yaw 1.5
Gyro-bias
1
Roll angle [deg]
20
0.5
10 0 0 Zoom -10 -0.5 -20
-30
0
1
2
3
4
5
6
7
Time [s]
-1 Actual φ Observed φ
0
1
2
3
4
5
6
7
Time [s]
"Measured" φ
0 -0.5 -1
Fig. 8.
-1.5 -2
Estimation of the gyro bias ˆb
-2.5 -3 -3.5 -4 0.1
Fig. 5.
0.2
0.3
0.4
where the gains k and the time constants τ for each sensor are given in table II. The parameters of transfer functions
Estimation of the roll angle φˆ
TABLE II PARAMETERS OF THE LOW- PASS SENSORS 30
Rotational axis Roll angle φ P itch angle θ Y aw angles ψ
Pitch angle [deg]
20 10 0 Zoom -10 -20 -30
0
1
2
3
4
5
6
Time [s]
τ 0.034 0.026 0.041
7 Actual θ Observed θ "Measured" θ
3.5 3 2.5 Finite time convergence
2 1.5 1 0.5 0 -0.5 0
Fig. 6.
k 1.16 1.07 0.99
0.1
0.2
0.3
0.4
Estimation of the pitch angle θˆ
estimation of the real bias of the gyro is shown. One can observe that bias estimation converge to the value given in table (I). For the experimental test we have use the same conditions of the previous test but in this case the measured signal is provided by tilt sensors. However, some tests have been performed to identify the static and dynamic characteristics of the tilt sensors. The data analysis shows that these sensors can be modeled by low pass filters with a transfer function given by: k (21) H(s) = 1 + τs
identified previously (see table II) are used for implementation of the estimation algorithm defined by the observer (9) and (10) with the choice of matrix gains as described previously. The obtained results are shown in the following figures. In Fig. (9), the real attitude given by potentiometers is compared to that provided by tilt sensors. One can observe a phase delay of the tilt sensors responses. This explain the use of the model (6). In Fig. (10), the result of estimation of the real attitude ˆ is shown. This figure presents the attitude estimation Θ compared to actual attitude given by potentiometers. One can observe the convergence of these estimates to the real attitude. This result shows that phase delay of the tilt sensors was also compensated. The estimation of the gyro-bias is presented in Fig. (11). This corroborates the results obtained previously. These obtained results show the effectiveness of the observer to improve the quality of measurements provided by low-cost sensors.
8081
FrC03.4 30
2 bias-roll bias-pitch bias-yaw
Roll angle [deg]
20 10 1.5 0 -10 1
-30
0
1
2
3
4
5
6
Time [s]
Gyro-bias
-20
7 Real attitude measured attitude
30
Pitch angle [deg]
20
0.5
0
10 0 -0.5 -10 -20 -30
0
1
2
3
4
5
6
-1
7
0
1
Time [s]
Roll angle [deg]
20
10
0
-10
-20 2
3
4
5
6
7
Time [s]
Actual Θ Observed Θ
30
Pitch angle [deg]
20 10 0 -10 -20 -30
0
1
2
3
4
5
6
7
Time [s]
Fig. 10.
3
4
5
6
7
Time [s]
Fig. 9. Real attitude given by potentiometers, Θ and its measure provided by tilt sensors, Θm
1
2
ˆm The actual attitude Θ and its observation Θ
VII. CONCLUSIONS A new approach based on nonlinear observer algorithm has been proposed to estimate both the attitude and the gyrobias in order to improve the quality of measures obtained by using low-cost sensors. The algorithm was developed using the kinematics equation of a rigid body and by considering that the attitude sensors can be modeled as low-pass filters. The observer is designed based on the sliding-mode approach and uses the real measurements given by the attitude sensors and the gyros. The stability of the observer was proven using Lyapunov stability method and the notion of finite time convergence is used to obtain step by step the estimation of the actual attitude and the gyro-bias. The effectiveness of the algorithm has been shown from a simulations and experimental tests using a rotary platform equipped with several sensors with axes of rotation coincide with orientation of the rigid body. R EFERENCES [1] M. R. Akella, J. T. Halbert, and G. R. Kotamraju, ”Rigid body attitude control with inclinometer and low-cost gyro measurements”, Syst. Contr. Lett., Vol. 49, pp. 151–159, 2003. [2] A.J. Baerveldt and R. Klang, ”A low-cost and low-weight attitude estimation system for an autonomous helicopter”, Proc. IEEE Int. Conf. on Intelligent Engineering Systems, pp. 391-395, 1997. [3] J.P Barbot, T. Boukhobza and M Djemai, ”Sliding mode observer for triangular input form”, In proc. of the 35th IEEE Conference on Decision and Control, Kobe, Japan, dec. 11-13, 1996. [4] A. Benallegue, A. Mokhtari and L. Fridman, ”High-order sliding-mode observer for a quadrotor UAV”, Int. J. Robust Nonlinear Control 2008; 18:427–440.
Fig. 11.
Estimation of the gyro bias ˆb
[5] J.L. Crassidis, F. Landis Markley, and Y. Cheng, ”Survey of nonlinear attitude estimation methods”, Journal of Guidance, Control, and Dynamics, Vol. 30, No. 1, pp. 12–28, 2007. [6] P. Castillo, A. Dzul and R. Lozano, ”Real-time stabilization and tracking of a four rotor mini rotorcraft,” IEEE Trans. Control Syst. Technol., Vol 12, No. 4, pp. 510–516, Jul. 2004. [7] S. Drakunov and V. Utkin, ”Sliding mode observer: Tutorial”, In proc. of the 34th IEEE Conference on Decision and Control, New Orleans, Louisiana, USA, 1995. [8] A. El-Hadri, A. Benallegue, ”Sliding Mode Observer to Estimate Both the Attitude and the Gyro-Bias by Using Low-Cost Sensors”, Proc. IEEE Int. Conf. on Intelligent Robots and Systems, October 11-15, 2009, St. Louis, USA [9] S. M. Joshi, A. G.Kelkar, and J. T.-Y.Wen, ”Robust attitude stabilization of spacecraft using nonlinear quaternion feedback”, IEEE Transactions on Automatic Control, Vol. 40, No. 10, pp. 1800–1803, Oct. 1995. [10] D. Jurman, M. JanKovec, R. Kamnik and M. Topic, ”Calibration and data fusion solution for the miniature attitude and heading reference system”, Sensors and Actuators A 138, pp. 411–420, 2007. [11] T. Madani, A. Benallegue, ”Control of a quadrotor mini-helicopter via full state backstepping technique”, In: Proceedings of the 45th IEEE Conference on Decision and Control. pp. 1515–1520. [12] R.Mahony, T Hamel and J. Pflimlin, ”Nonlinear complementary filters on the special orthogonal group”, IEEE Transactions on Automatic Control, Vol. 53, No. 5, pp. 1203–1218, 2008. [13] H. Rehbinder, X. Hu, ”Nonlinear state estimation for rigid-body motion with low-pass sensors”, Systems Control Letter, 40 (2000) 183–190. ” [14] Henrik Rehbinder and Xiaoming Hu, Nonlinear Pitch and Roll Estimation for Walking Robots”, IEEE Int. Conf. on Robotics and Automation (ICRA 2000), San Francisco, CA, USA, April 24-28, 2000. [15] J.-J. E. Slotine, J. K. Hedrick and E. A. Misawa, ”On Sliding Observers for Nonlinear Systems”, in ASME Jornal of Dynamic Sytems, Measurement and Control, Vol. 109, pp. 245–252, 1987. [16] A. Tayebi and S. McGilvray, ”Attitude stabilization of a quadrotor aircraft”, IEEE Transactions on Control Systems Technology, Vol. 14, No. 3, pp. 562–571, May 2006. [17] A. Tayebi, S. McGilvray, A. Roberts and M. Moallem, ”Attitude estimation and stabilization of a rigid body using low-cost sensors”, In proc. of the 46th IEEE Conference on Decision and Control, New Orleans, LA, USA, Dec. 12-14, 2007. [18] J. Thienel and R.M. Sanner ”A coupled nonlinear spacecraft attitude controller and observer with an unknown constant gyro bias and gyro noise”, IEEE Transactions on Automatic Control, Vol. 48, No. 11, pp. 2011–2015, 2003. [19] J. T.-Y. Wen and K. Kreutz-Delgado, ”The attitude control problem”, IEEE Transactions on Automatic Control, Vol. 36, No. 10, pp. 1148– 1162, Oct. 1991. [20] R.Zhu, D. Sun, Z. Zhou and D. Wang ”A linear fusion algorithm for attitude determination using low cost MEMS-based sensors”, Measurement, Vol. 40, pp. 322–328, 2007.
8082
