# Second-Order Geometric Sliding Mode Attitude Observer with Application to Quadrotor on a Test Bench

Authors: Honglei An; Jie Li; Jian Wang; Jianwen Wang; Hongxu Ma
Journal: Mathematical Problems in Engineering
 (2013)

---

## Abstract

A sliding mode observer design framework is proposed based on the Lie group method of numerical integration on manifolds, and a Second-Order Geometric Sliding Mode Attitude Observer (SOGSMAO) is designed for angular velocity estimation of quadrotor attitude. The algorithm constructs feedback in the angular velocity space and the space of equivalent Lie algebra of unit quaternion space, respectively. It avoids not only the complexity of constructing feedback in unit quaternion space but also the process of mandatory rescaling which is seen to deteriorate the accuracy of the angular velocity estimates during sliding. The performance of SOGSMAO is compared with traditional quaternion based sliding mode observer in which multiplicative quaternion correction is used and the results show that SOGSMAO gains better tracking performance. Then SOGSMAO is realized on a test bed and the effectiveness of the observer algorithm is verified by experimental studies.


---

## Body

## 1. Introduction

The rotational motion of a rigid body describes the orientation (attitude) of the rigid body with respect to inertial reference. And attitude control is the process of orienting the rigid body in a specified predetermined direction. Attitude control system is an important subsystem in aircrafts, spacecrafts, and satellites and is responsible for pointing and slewing [1–5]. In the case of quadrotor control, translational outer loop is usually realized on the basis of rotational inner loop, as a result of its property of underactuatation [6–9]. Compared with fixed-wing aircraft, rotorcraft has special advantages in the city and indoor environment due to the ability of VTOL (vertical take-off and landing) and hovering flight. As a new kind of rotorcraft, quadrotor has some unique advantages over other traditional rotorcrafts. Firstly, it is more convenient that swash plate and mechanical linkages are not necessary. Secondly, it is much safer in the indoor flying because its propellers are smaller and can be wrapped up. In addition, it has greater thrust-weight ratio and better maneuver performance. Therefore, a great deal of work has been done in quadrotor control.Most of the attitude control algorithms require the attitude and angular rate. These measurements are obtained from a variety of sensors and estimation techniques. However, the tendency of building smaller satellites and aircrafts limits the number of sensors and the on-board processing capability. So attitude control system requires computationally simple yet effective algorithms without sophisticated measurement instrumentation, particularly gyroscopes [10–14]. Gyroscopes provide direct angular rate measurements but are costly and heavy require significant power consumption, and have high failure tendencies. Lizarralde and Wen use a velocity filter to stabilize attitude without angular velocity [12]. Another solution is to estimate the angular velocity using an extended Kalman filter [15] or a nonlinear observer [13, 14]. In this paper the problem of estimating angular velocities of quadrotor is addressed by nonlinear observers. Estimating angular velocities eliminates the use of gyros and can also confirm the accuracy of gyro readings in case of possible gyro failure and attitude estimates when sensor measurements are not available.In all kinds of nonlinear observers, sliding mode observer has special advantages. Sliding mode technique is widely known because of its high accuracy and robustness with respect to various internal and external disturbances [16]. Its application is not limited to mechanical systems. Wu and Ho design an SMC for singular systems which can be a natural representation of economic systems, power systems and circuits systems [17]. In addition, there are a wide range of analysis methods such as the Lyapunov method, equivalent control method and dissipativity based method [18, 19]. In the area of observer application, the sliding mode observer has the feature of finite-time convergence, so it is not needed to prove the separation principle when implementing a sliding mode observer in a control system.In all representations of the attitude, the unit quaternion is a pervasive one [20–22]. Different from minimal attitude representations, such as Euler angles, which have the disadvantages of singularities and complicated trigonometric expressions, the unit quaternion provides a globally nonsingular representation of the attitude. Compared with classical direction cosine matrix, it has only 4 numbers so that the multiplication and propagation rule can be calculated more efficiently. However, unit quaternion is regarded only as an algebraic tool in traditional sliding mode observer algorithms without considering its geometric structure [9, 10]. The classical discrete integral method is used directly on each element of unit quaternion, and the quaternion vector must be normalized in each estimation cycle. In [11, 23] the multiplicative quaternion correction is used instead of the additive quaternion correction, but the discontinuous feedback term is applied in unit quaternion space and it must be normalized too.Actually, unit quaternion keeps the structure ofSU(2), the special unitary group, which is a 2 to 1 cover of SO(3) [21]. The add operation is not a group operation of unit quaternion, so the group structure is not conserved when additive discrete integral method is used. And the normalization is seen to deteriorate the accuracy of the angular velocity estimates during sliding [10]. So a main motivation of this work is to construct an observer design frame from the geometric point of view, such that it is unnecessary to construct sliding mode feedback in unit quaternion space directly or the process of mandatory rescaling can be avoided.Inspired by the ideological of equivariant mapping in the Lie group method of numerical integration, this work extends the design method of sliding mode observer based on unit quaternion. The kinematic equation in unit quaternion space is transformed to an equivalent differential equation evolving in the Lie algebra space of unit quaternion, and the feedback is constructed in the Lie algebra space. An appealing feature of Lie algebra space is that it is a vector space in which the classical numerical method can be applied in approximately solving the transformed equation. So it is unnecessary to construct sliding mode feedback in unit quaternion space directly in the proposed algorithm. In addition, the process of mandatory rescaling which the traditional methods need in each integration step can be also avoided.The main contributions of this work are the following.First, a sliding mode observer design framework is proposed based on the Lie group method of numerical integration on manifolds. In this frame work, the feedback is constructed in vector spaces, so traditional sliding mode observer can be extended easily.Second, a Second-Order Geometric Sliding Mode Attitude Observer (SOGSMAO) is designed for angular velocity estimation of quadrotor attitude. The effectiveness of SOGMAO algorithm is exemplified by simulation and experiment.This paper is organized as follows. Based on the assumptions of single rigid body and symmetrical structure, a mathematical model of quadrotor on the attitude platform is given in Section2. Then SOGSMAO is designed using Lie group method of numerical integration and sliding mode technique in Section 3. Section 4 conducts a simulation in which a comparison is made between SOGSMAO and traditional quaternion based sliding mode observer with multiplicative quaternion correction. In Section 5, the performance of the proposed method is evaluated with real experimental data. At last, the conclusion is outlined in Section 6.


## 2. Attitude Dynamics of Quadrotor

In this section the mathematical model of quadrotor on the test bench is given. The model consists of the kinematics of unit quaternion and the dynamics of single rigid body. In simulation this model is used for providing ideal attitude and angular velocity. And in the process of observer design, the model is a necessity.


### 2.1. Unit Quaternion Kinematics

A unit quaternionq→ is a complex with four parameters. It has the norm of constant one and can be considered as a point of the unit sphere S3(1)q→=q0+q1i+q2j+q3k,q02+q12+q22+q32=1.Letq~ represent its vector part, that is, q~=q1i+q2j+q3k. Then unit quaternion can be written in the following form:
(2)q→=(q0,q~).Given a unit quaternion, there is a correspondence between unit quaternion and rotation transformation(3)R(q0,q~)=(q02-q~Tq~)I+2q0S(q~)+2q~q~T,
where
(4)S(q~)=[0-q3q2q30-q1-q2q10].Noticing thatR(q0,q~)=R(-q0,-q~), so the unit sphere S3 represented by unit quaternion is a double cover of the attitude space SO(3). The phenomenon is more clearly using the angle form of unit quaternion. Euler’s theorem states that any rotation in space can be achieved through rotating θ angle around an eigenaxis of n→, then unit quaternion can be written as
(5)q→=(cos(θ2),sin(θ2)n→),θ∈[0,2π].It is obvious that rotatingθ0(0≤θ0≤2π) angle around eigenaxis n→ is equivalent to a rotation of 2π-θ0 angle around eigenaxis of -n→. In Figure 1, X point and O point are different by quaternion representation but have the same physical meaning. Actually, q→=(cos(θ0/2),sin(θ0/2)n→) and -q→=(cos(π-θ0/2),sin(π-θ0/2)n→) have the same physical meaning.Figure 1
Quaternion representation along some fixed axis.The conjugate of a unit quaternion is(6)q→*=(q0,-q~).Given two unit quaternionq→1=(q10,q~1) and q→2=(q20,q~2), the multiplication ⊗ is defined as
(7)q→1⊗q→2=[q10q20-q~1Tq~2q10q~2+q20q~1+S(q~1)q~2].Unit quaternion multiplication is equivalent to the multiplication of rotation matrix. Ifq→1 and q→2 correspond to rotation matrices R1 and R2 in SO(3), then there are the following correspondences:
(8)R1R2⟷±q→1⊗q→2,R1v⟷q→1⊗v-⊗q→1*,
where v-=(0,v→).When a rigid body is rotating with angular velocity vectorw→, the time derivative of unit quaternion can be given by the unit quaternion propagation rule as follows:
(9)q→˙=12q→⊗w-.


### 2.2. System Model

Two coordinate frames, the inertial coordinate frame and the body coordinate frame, are employed for modeling the quadrotor attitude system. As seen in Figure2, the origin of the body coordinate frame Ob-XbYbZb is located in the center of the crossbeam of quadrotor, axis X points towards propeller 1, axis Z is perpendicular to the crossbeam plane, and axis three is to form a right-handed coordinate system.Figure 2
Coordinate frames of quadrotor.The quadrotor dynamics are based on two assumptions.(1)
Quadrotor is a rigid body.(2)
The small errors in manufacturing and installing the vehicle bodies are not considered, so the quadrotor is assumed to be symmetric.Based on assumption one, the following mathematical model can be derived from Newton-Euler equations and quaternion kinematics:(10a)Jw→˙=M→-w→×(Jw→),(10b)q~˙=12(q0I+S(q~))w→,q˙0=-12q~Tw→,where q→=[q0q~T]T represents attitude quaternion of body coordinate frame with respect to inertial coordinate frame, w→ is angular velocity in body coordinate frame, and M→ is the torque acting on the quadrotor.From the second assumption, the correspondence between four propeller thrustsfi(i=1,2,3,4) and the torques Mi(i=1,2,3) acting on the quadrotor is given as follows:
(11)[fM1M2M3]=[11110d0-d-d0d0-cτfcτf-cτfcτf][f1f2f3f4],
where f is the total thrust, d is the distance between axis of propeller and the center of crossbeams, and cτf is the propeller torque thrust ratio.Our model does not include the propeller gyroscopic torque and flapping dynamic term, thusM→ consists of three axis control torque M1, M2, M3(12)M→=[M1M2M3]T.Considering the low-speed characteristic of quadrotor, the following simplified equation is used to derive the propeller thrust:(13)fi=cfΩi2,
where cf is a constant coefficient.


## 2.1. Unit Quaternion Kinematics

A unit quaternionq→ is a complex with four parameters. It has the norm of constant one and can be considered as a point of the unit sphere S3(1)q→=q0+q1i+q2j+q3k,q02+q12+q22+q32=1.Letq~ represent its vector part, that is, q~=q1i+q2j+q3k. Then unit quaternion can be written in the following form:
(2)q→=(q0,q~).Given a unit quaternion, there is a correspondence between unit quaternion and rotation transformation(3)R(q0,q~)=(q02-q~Tq~)I+2q0S(q~)+2q~q~T,
where
(4)S(q~)=[0-q3q2q30-q1-q2q10].Noticing thatR(q0,q~)=R(-q0,-q~), so the unit sphere S3 represented by unit quaternion is a double cover of the attitude space SO(3). The phenomenon is more clearly using the angle form of unit quaternion. Euler’s theorem states that any rotation in space can be achieved through rotating θ angle around an eigenaxis of n→, then unit quaternion can be written as
(5)q→=(cos(θ2),sin(θ2)n→),θ∈[0,2π].It is obvious that rotatingθ0(0≤θ0≤2π) angle around eigenaxis n→ is equivalent to a rotation of 2π-θ0 angle around eigenaxis of -n→. In Figure 1, X point and O point are different by quaternion representation but have the same physical meaning. Actually, q→=(cos(θ0/2),sin(θ0/2)n→) and -q→=(cos(π-θ0/2),sin(π-θ0/2)n→) have the same physical meaning.Figure 1
Quaternion representation along some fixed axis.The conjugate of a unit quaternion is(6)q→*=(q0,-q~).Given two unit quaternionq→1=(q10,q~1) and q→2=(q20,q~2), the multiplication ⊗ is defined as
(7)q→1⊗q→2=[q10q20-q~1Tq~2q10q~2+q20q~1+S(q~1)q~2].Unit quaternion multiplication is equivalent to the multiplication of rotation matrix. Ifq→1 and q→2 correspond to rotation matrices R1 and R2 in SO(3), then there are the following correspondences:
(8)R1R2⟷±q→1⊗q→2,R1v⟷q→1⊗v-⊗q→1*,
where v-=(0,v→).When a rigid body is rotating with angular velocity vectorw→, the time derivative of unit quaternion can be given by the unit quaternion propagation rule as follows:
(9)q→˙=12q→⊗w-.


## 2.2. System Model

Two coordinate frames, the inertial coordinate frame and the body coordinate frame, are employed for modeling the quadrotor attitude system. As seen in Figure2, the origin of the body coordinate frame Ob-XbYbZb is located in the center of the crossbeam of quadrotor, axis X points towards propeller 1, axis Z is perpendicular to the crossbeam plane, and axis three is to form a right-handed coordinate system.Figure 2
Coordinate frames of quadrotor.The quadrotor dynamics are based on two assumptions.(1)
Quadrotor is a rigid body.(2)
The small errors in manufacturing and installing the vehicle bodies are not considered, so the quadrotor is assumed to be symmetric.Based on assumption one, the following mathematical model can be derived from Newton-Euler equations and quaternion kinematics:(10a)Jw→˙=M→-w→×(Jw→),(10b)q~˙=12(q0I+S(q~))w→,q˙0=-12q~Tw→,where q→=[q0q~T]T represents attitude quaternion of body coordinate frame with respect to inertial coordinate frame, w→ is angular velocity in body coordinate frame, and M→ is the torque acting on the quadrotor.From the second assumption, the correspondence between four propeller thrustsfi(i=1,2,3,4) and the torques Mi(i=1,2,3) acting on the quadrotor is given as follows:
(11)[fM1M2M3]=[11110d0-d-d0d0-cτfcτf-cτfcτf][f1f2f3f4],
where f is the total thrust, d is the distance between axis of propeller and the center of crossbeams, and cτf is the propeller torque thrust ratio.Our model does not include the propeller gyroscopic torque and flapping dynamic term, thusM→ consists of three axis control torque M1, M2, M3(12)M→=[M1M2M3]T.Considering the low-speed characteristic of quadrotor, the following simplified equation is used to derive the propeller thrust:(13)fi=cfΩi2,
where cf is a constant coefficient.


## 3. Angular Rates Observer Design on Lie Group

Unit quaternion space is a nonlinear manifoldS3. In S3, classical numerical methods cannot conserve the geometric structure of the original manifold, so a rescaling process must be added which reduces the accuracy of the algorithm and increases the cost of computation. Actually, since the add operation is not a group operation of unit quaternion, it is meaningless in geometric point of view to apply classical numerical methods in S3.To solve this problem, a frame work of designing sliding mode observer is proposed using the Lie group method of numerical integration. On the basis of equivariant mapping ideological, the kinematic equation in unit quaternion space is transformed to an equivalent differential equation evolving on the Lie algebra space of unit quaternion, and the feedback is constructed in the Lie algebra space.


### 3.1. Lie Group Method of Numerical Integration

In this section, only main results of the approach are given, and more detailed material and proofs could be found in [24].Definition 1 (equivariant map).
Letℳ and 𝒩  be manifolds and let G be a Lie group which acts on ℳ by Φg:ℳ→ℳand on 𝒩 by Ψg:𝒩→𝒩. A smooth map f:ℳ→𝒩 is called equivariant with respect to these actions if, for all g∈G(14)f°Φg=Ψg°f.
That is, the following diagram commutes:(15)
From the definition of an actionΦ:G×ℳ→ℳ of G on ℳ, we can get an equivariant map Φm:G→ℳ with respect to the left translation action Lg of G on itself and an action Φg of G on ℳ(16)Φm°Lg=Φg°Φm.
It is known that there is a local coordinate mapf:𝔤→G on G where 𝔤 is the Lie algebra space of G, the most typical is exponential map exp. At this point, we need to find an action Bg of G on 𝔤 such that f will be an equivariant map with Bg and the left action of G on itself
(17)f°Bg=Lg°f.
In the case wheref is the exponential map, Bg is nothing else than the well-known Baker-Campbell-Hausdorff (BCH) formula
(18)Bg(u)=log(g·exp(u)),
where log is called the logarithm map.
Since composition of two equivariant maps is an equivariant map, we can construct an equivariant mapΦm∘f from 𝔤 to ℳ with respect to the action Bg on 𝔤 and Φg on ℳ. That is, the following diagram commutes
(19)Definition 2 (relatedness of vector fields).
Letϕ:ℳ→𝒩 be a smooth map between manifolds. The vector fields X on ℳ and Y on 𝒩 are called ϕ-related, denoted by X∽ϕY, if Tϕ∘X=Y∘ϕ.
The definition means that the following diagram commutes:(20)
LetΦexp(tξℳ) and Ψexp(tξ𝒩) be two flows on ℳ and 𝒩, respectively, generated by the elements ξ∈𝔤 and the action Φ on ℳ and Ψ on 𝒩 of the Lie group G. The main idea is to solve for a flow on a simpler manifold ℳ so that the effect is the same as if one solved for the flow on 𝒩.
Theorem 3.6 of [25] stated that if and only if ϕ is an equivariant map, then the infinitesimal generators of the action with respect to the same element ξ∈𝔤 areϕ-related vector fields. Thus, the infinitesimal generators of the flows of Bg and Φg are Φm∘f-related, that is,
(21)ξℳ∘Φm∘f=TΦm∘Tf∘ξ𝔤.
That is, the following diagram commutes:(22)
Now, since the infinitesimal generatorsξ𝔤 and ξℳ on 𝔤 and ℳ respectively is Φm∘f-related, the differential equation on ℳ can be translated to a equivalent system on 𝔤 with the selected ξ𝔤. So the differential equation in Lie algebra space 𝔤 can be solved firstly, and the solution of differential equation in corresponding manifold space can be gained.


### 3.2. Cayley Transform

The kinematics of unit quaternion can be rewritten as(23)q→˙=A(w→)q→,
where A(w→) is the Lie algebra of unit quaternion
(24)A(w→)=12[0-w3-w2-w1w30w3-w2w2-w30w1w1w2-w10].Then the solution of the differential equation (23) is
(25)q→(t)=exp(A(w→(t)))·q→0.From the ideological of equivariant mapping, the Lie algebra space of unit quaternion is equivalent to unit quaternion space through equivariant mappingexp(tξ). And A(w→(t)) satisfies the following differential equation:
(26)A˙(w→(t))=dexpA(w→(t))-1(Ω(t)).In every integral step, the maps ofexp(tξ) and dexpu-1(v) must be solved. But in engineering practice the exact numerical solution of the two maps cannot be computed, and it is usually replaced by some approximate solution. This paper develops integration based on Cayley transform which would entail savings with respect to computational costs because of the less expense in computing the Cayley transform as compared to the exponential map.The Cayley transform,cay:𝔤→G, is defined as
(27)cay(u)=1+u/21-u/2.The corresponding vector field on the Lie algebradcayu-1 is calculated to be
(28)dcayu-1(v)=v-12[u,v]-14u·v·u,
where [·,·] denotes the matrix commutator and “·” denotes matrix multiplication.


### 3.3. Second-Order Geometric Sliding Mode Observer Design

The system model is(29)q→˙=12q→⊗w-,Jw→˙=M→-w→×(Jw→).Define the error of attitude between estimated attitude and the actual one(30)q→e≜q→*⊗q→^.When estimated value is equivalent to the actual one, that is,q→^=q→, q~e=0, and when q→^≠q→, q~e=sin(θ/2)n→ represent a rotation of θ angle around axis n→, then q~e is considered as feedback variable. Since the attitude of quadrotor is the only measurable output, the sliding manifold is taken as s=q~e.Consider the first derivative of the sliding manifold(31)q~˙e=q0˙q^~-[q^0I-S(q^~)]q~˙-q^˙0q~+[q^0I+S(-q~)]q^~˙=-12(q~Tw)q^~-q^0q~˙+S(q^~)q~˙+12(q^~Tw^)q~+q^0q^~˙-S(q~)q^~˙=-12(q~Tw)q^~-12q^0(q0I+S(q~))w+12S(q^~)(q0I+S(q~))w+12(q^~Tw^)q~+12q^0(q^0I+S(q^~))w^-12S(q~)(q^0I+S(q^~))w^.It should be noted thatM→ is not included explicitly. But w→ is included in the first derivative of the sliding manifold, so M→ will be included in the second derivative of the sliding manifold, and its sliding order is 2.The Second-Order Sliding Mode Observer structure in [22] is
(32)x^˙1=x^2+z1,x^˙2=f(t,x1,x^2,u)+z2,
where
(33)z1=λ|x1-x^1|1/2sgn(x1-x^1),z2=αsgn(x1-x^1).Then the structure of unit quaternion based SOGSMAO can be designed as(34)q→^˙=12q→^⊗(w→^-Z1),Jw→^˙=-w→^×(Jw→^)-Z2,
where
(35)Z1=k1|q~e|1/2·SGN(q~e),Z2=k2SGN(q~e).TheSGN function is an extended sign function for multidimensional situation. Since the sign function can be seen as the direction function of one dimension, that is,
(36)sgn(λ)={λ|λ|λ≠00λ=0,
we extend it to the situation of three dimensions as
(37)SGN(X)={X∥X∥X≠[000][000]TX=[000].Based on the Lie group method of numerical integration, the algorithm of SOGSMAO can be transformed to the discrete form as follows:(38)q→^(+)=cay(ξ^)q→^(-),ξ^(+)=ξ^(-)+h·dcayξ-1(A(w→^-k1|q~e|1/2·SGN(q~e))),w→^(+)=w→^(-)+h·(M→-w→^×(Iw→^)-k2sgn(q~e)),
where h represents time interval between integral steps.


## 3.1. Lie Group Method of Numerical Integration

In this section, only main results of the approach are given, and more detailed material and proofs could be found in [24].Definition 1 (equivariant map).
Letℳ and 𝒩  be manifolds and let G be a Lie group which acts on ℳ by Φg:ℳ→ℳand on 𝒩 by Ψg:𝒩→𝒩. A smooth map f:ℳ→𝒩 is called equivariant with respect to these actions if, for all g∈G(14)f°Φg=Ψg°f.
That is, the following diagram commutes:(15)
From the definition of an actionΦ:G×ℳ→ℳ of G on ℳ, we can get an equivariant map Φm:G→ℳ with respect to the left translation action Lg of G on itself and an action Φg of G on ℳ(16)Φm°Lg=Φg°Φm.
It is known that there is a local coordinate mapf:𝔤→G on G where 𝔤 is the Lie algebra space of G, the most typical is exponential map exp. At this point, we need to find an action Bg of G on 𝔤 such that f will be an equivariant map with Bg and the left action of G on itself
(17)f°Bg=Lg°f.
In the case wheref is the exponential map, Bg is nothing else than the well-known Baker-Campbell-Hausdorff (BCH) formula
(18)Bg(u)=log(g·exp(u)),
where log is called the logarithm map.
Since composition of two equivariant maps is an equivariant map, we can construct an equivariant mapΦm∘f from 𝔤 to ℳ with respect to the action Bg on 𝔤 and Φg on ℳ. That is, the following diagram commutes
(19)Definition 2 (relatedness of vector fields).
Letϕ:ℳ→𝒩 be a smooth map between manifolds. The vector fields X on ℳ and Y on 𝒩 are called ϕ-related, denoted by X∽ϕY, if Tϕ∘X=Y∘ϕ.
The definition means that the following diagram commutes:(20)
LetΦexp(tξℳ) and Ψexp(tξ𝒩) be two flows on ℳ and 𝒩, respectively, generated by the elements ξ∈𝔤 and the action Φ on ℳ and Ψ on 𝒩 of the Lie group G. The main idea is to solve for a flow on a simpler manifold ℳ so that the effect is the same as if one solved for the flow on 𝒩.
Theorem 3.6 of [25] stated that if and only if ϕ is an equivariant map, then the infinitesimal generators of the action with respect to the same element ξ∈𝔤 areϕ-related vector fields. Thus, the infinitesimal generators of the flows of Bg and Φg are Φm∘f-related, that is,
(21)ξℳ∘Φm∘f=TΦm∘Tf∘ξ𝔤.
That is, the following diagram commutes:(22)
Now, since the infinitesimal generatorsξ𝔤 and ξℳ on 𝔤 and ℳ respectively is Φm∘f-related, the differential equation on ℳ can be translated to a equivalent system on 𝔤 with the selected ξ𝔤. So the differential equation in Lie algebra space 𝔤 can be solved firstly, and the solution of differential equation in corresponding manifold space can be gained.


## 3.2. Cayley Transform

The kinematics of unit quaternion can be rewritten as(23)q→˙=A(w→)q→,
where A(w→) is the Lie algebra of unit quaternion
(24)A(w→)=12[0-w3-w2-w1w30w3-w2w2-w30w1w1w2-w10].Then the solution of the differential equation (23) is
(25)q→(t)=exp(A(w→(t)))·q→0.From the ideological of equivariant mapping, the Lie algebra space of unit quaternion is equivalent to unit quaternion space through equivariant mappingexp(tξ). And A(w→(t)) satisfies the following differential equation:
(26)A˙(w→(t))=dexpA(w→(t))-1(Ω(t)).In every integral step, the maps ofexp(tξ) and dexpu-1(v) must be solved. But in engineering practice the exact numerical solution of the two maps cannot be computed, and it is usually replaced by some approximate solution. This paper develops integration based on Cayley transform which would entail savings with respect to computational costs because of the less expense in computing the Cayley transform as compared to the exponential map.The Cayley transform,cay:𝔤→G, is defined as
(27)cay(u)=1+u/21-u/2.The corresponding vector field on the Lie algebradcayu-1 is calculated to be
(28)dcayu-1(v)=v-12[u,v]-14u·v·u,
where [·,·] denotes the matrix commutator and “·” denotes matrix multiplication.


## 3.3. Second-Order Geometric Sliding Mode Observer Design

The system model is(29)q→˙=12q→⊗w-,Jw→˙=M→-w→×(Jw→).Define the error of attitude between estimated attitude and the actual one(30)q→e≜q→*⊗q→^.When estimated value is equivalent to the actual one, that is,q→^=q→, q~e=0, and when q→^≠q→, q~e=sin(θ/2)n→ represent a rotation of θ angle around axis n→, then q~e is considered as feedback variable. Since the attitude of quadrotor is the only measurable output, the sliding manifold is taken as s=q~e.Consider the first derivative of the sliding manifold(31)q~˙e=q0˙q^~-[q^0I-S(q^~)]q~˙-q^˙0q~+[q^0I+S(-q~)]q^~˙=-12(q~Tw)q^~-q^0q~˙+S(q^~)q~˙+12(q^~Tw^)q~+q^0q^~˙-S(q~)q^~˙=-12(q~Tw)q^~-12q^0(q0I+S(q~))w+12S(q^~)(q0I+S(q~))w+12(q^~Tw^)q~+12q^0(q^0I+S(q^~))w^-12S(q~)(q^0I+S(q^~))w^.It should be noted thatM→ is not included explicitly. But w→ is included in the first derivative of the sliding manifold, so M→ will be included in the second derivative of the sliding manifold, and its sliding order is 2.The Second-Order Sliding Mode Observer structure in [22] is
(32)x^˙1=x^2+z1,x^˙2=f(t,x1,x^2,u)+z2,
where
(33)z1=λ|x1-x^1|1/2sgn(x1-x^1),z2=αsgn(x1-x^1).Then the structure of unit quaternion based SOGSMAO can be designed as(34)q→^˙=12q→^⊗(w→^-Z1),Jw→^˙=-w→^×(Jw→^)-Z2,
where
(35)Z1=k1|q~e|1/2·SGN(q~e),Z2=k2SGN(q~e).TheSGN function is an extended sign function for multidimensional situation. Since the sign function can be seen as the direction function of one dimension, that is,
(36)sgn(λ)={λ|λ|λ≠00λ=0,
we extend it to the situation of three dimensions as
(37)SGN(X)={X∥X∥X≠[000][000]TX=[000].Based on the Lie group method of numerical integration, the algorithm of SOGSMAO can be transformed to the discrete form as follows:(38)q→^(+)=cay(ξ^)q→^(-),ξ^(+)=ξ^(-)+h·dcayξ-1(A(w→^-k1|q~e|1/2·SGN(q~e))),w→^(+)=w→^(-)+h·(M→-w→^×(Iw→^)-k2sgn(q~e)),
where h represents time interval between integral steps.


## 4. Simulation

In this section, the performance of the SOGSMAO is examined in the environment of MATLAB/Simulink. And a comparison is made between SOGSMAO and traditional quaternion based sliding mode observer with multiplicative quaternion correction.The ideal attitude and angular velocity data is obtained from the model of quadrotor. The inertial parameters used in the simulation are(39)J=diag(8.9429.4587.787)*10-3.The ideal curves are gained from a simulation model in which the initial states are(40)q→(0)=[1000]T,w→(0)=[000].And the torques acting on the model are(41)M→=0.01*[sin(t)sin(t)0].The sampled data of system states is shown in Figure3 (the sample time is 0.02 second).Ideal system state curves ((a) attitude curves; (b) angular velocity curves).
(a)
(b)Observers use attitude data to estimate angular velocity of the quadrotor. The algorithm of SOGSMAO rewritten here is(42)q→^(+)=cay(ξ^)q→^(-),ξ^(+)=ξ^(-)+h·dcayξ-1(A(w→^-k1|q~e|1/2·SGN(q~e))),w→^(+)=w→^(-)+h·(M→-w→^×(Iw→^)-k2sgn(q~e)).And the simulation result of SOGSMAO algorithm is shown in Figure4.The angular velocity estimating result of SOGSMAO (red: ideal angular velocity curves; green: estimated angular velocity curves).
(a)
(b)
(c)The traditional quaternion based sliding mode observer algorithm with multiplicative quaternion correction can be given as(43)δq’K=k1|q~e|1/2·SGN(q~e),δqK=1(1+δq’K12+δq’K22+δq’K32)1/2×[1δq’K1δq’K2δq’K3]T,q→^(+)=δqK⊗q→^(-),w→^(+)=w→^(-)+h·(M→-w→^×(Iw→^)-k2sgn(q~e)).And the simulation result of traditional algorithm is shown in Figure5.The angular velocity estimating results of traditional algorithm (red: ideal angular velocity curves; green: estimated angular velocity curves).
(a)
(b)
(c)Comparing the results of SOGSMAO algorithm and traditional algorithm for estimating the angular velocity in Figures4 and 5, it is clear that SOGSMAO algorithm has a better tracking performance while it avoided the process of mandatory rescaling.Since the sign functionSGN(·) would cause a high frequency oscillation, a low pass filter must be added for removing chattering while applying the SOGSMAO algorithm. The filtered data of estimated angular velocity is compared with the ideal angular velocity in Figure 6.Comparison between the filtered data of SOGSMAO and the ideal angular velocity.
(a)
(b)
(c)


## 5. Experiment on the Test Bench Quadrotor

A real test was carried out to evaluate the performance of the proposed SOGSMAO. The test configuration is shown in Figure7. The equipped devices include a posture platform, aircraft body, onboard controller, attitude and heading reference system (AHRS), wireless serial communication interface, remote control and receiver, and a computer.Figure 7
Physical devices of the experimental system.Aircraft body is selected to be the XAircraft X-650 quadrotor which is made by carbon fiber material, equipped with four brushless DC motor and motor speed control device. The empty weight is 800 g, and maximum single propeller thrust is 4N. Attitude and Heading Reference System is the Xsens MTi inertial measurement unit (IMU) which can provide 3 axes drift-less orientation and 3 axes calibrated angular velocity. Its maximum update rate is 100 Hz with 0.5 degree attitude measure accuracy, 1 degree heading measure accuracy, and 300 degrees per second maximum measurable angular velocity. Remote control and receiver are WFLY FT06-A with 5-way continuous pulse channel and a switching channel. TI’s TMS320F2809 chip with 100 MHz core frequency is the core of onboard controller.The connection diagram of the experimental devices is shown in Figure8. In every control cycle (20 ms) F2809 chip is interrupted to receive orientation and angular velocity from IMU, then calculate the control pulse width and transfer it to motor speed control devices. Outside the control loop, F2809 is interrupted to turn parameters and send a set of state data to the computer every 100 ms. The remote control instruction is received in the same interrupter program.Figure 8
Signal connection diagram.In the experiment, the sampled data of attitude and angular velocity is gained through wireless serial port with a sample time of 0.02 second (Figures9 and 10).Quadrotor attitude curves.
(a)
(b)
(c)
(d)Quadrotor angular velocity curves.
(a)
(b)
(c)The filtered data of estimated angular velocity is compared with the actual angular velocity in Figure11. The results show good tracking performance of the observer, which can meet the requirements of control applications in computational burden.Comparison between the filtered data of SOGSMAO and the actual angular velocity.
(a)
(b)
(c)


## 6. Conclusions

The problem of estimating angular velocities of quadrotor is addressed with only attitude measurement in this work. Different from other quaternion based sliding mode observers in which the unit quaternion is considered as an algebraic tool, the proposed sliding mode observer uses the unit quaternion as a nonlinear manifold. Based on the ideological of equivariant mapping, the kinematic equation in unit quaternion space is transformed to an equivalent differential equation evolving on the Lie algebra space of unit quaternion, and the feedback is constructed in the Lie algebra space. The designed algorithm avoids not only the complex sliding mode feedback in unit quaternion space but also the process of mandatory rescaling which the traditional methods need in each integration step. The simulation and experimental results show that the algorithm of Second-Order Geometric Sliding Mode Observer is effective, and can meet the requirements of control applications in computational burden.The proposed observer design framework has a wide range of applications. A variety of nonlinear observers can be designed based on the framework. In addition, the proposed observer design framework cannot only design the angular rates observer but also design disturbance observer for feedforward compensation.



---

*Source: 328974-2013-07-02.xml*