Backstepping Control for a Quadrotor Helicopterç«ï½¬ - Semantic Scholar
Proceedings of the 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems October 9 - 15, 2006, Beijing, China
Backstepping Control for a Quadrotor Helicopter Tarek Madani and Abdelaziz Benallegue Laboratoire d’Ing´enierie des Syst`emes de Versailles 10-12, avenue de l’Europe, 78140 V´elizy - FRANCE E-mail: {madani and benalleg}@lisv.uvsq.fr Abstract— This paper presents a nonlinear dynamic model for a quadrotor helicopter in a form suited for backstepping control design. Due to the under-actuated property of quadrotor helicopter, the controller can set the helicopter track three Cartesian positions (x, y, z) and the yaw angle to their desired values and stabilize the pitch and roll angles. The system has been presented into three interconnected subsystems. The first one representing the under-actuated subsystem, gives the dynamic relation of the horizontal positions (x, y) with the pitch and roll angles. The second fully-actuated subsystem gives the dynamics of the vertical position z and the yaw angle. The last subsystem gives the dynamics of the propeller forces. A backstepping control is presented to stabilize the whole system. The design methodology is based on the Lyapunov stability theory. Various simulations of the model show that the control law stabilizes a quadrotor with good tracking.
I. I NTRODUCTION Unmanned Aerial Vehicles (UAV) have unmatched qualities that make them the only effective solution in specialized tasks where risks to pilots are high, where beyond normal human endurance is required, or where human presence is not necessary. UAVs are being used more and more in civilian applications such as traffic monitoring, recognition and surveillance vehicles, search and rescue operations [1]. They are highly capable to flown without an on-board pilot. These robotic aircrafts are often computerized and fully autonomous. The first reported quadrotor helicopter (Gyroplane No.1) was built in 1907 by the Breguet Brothers. However, there was no means of control provided to the pilot other than a throttle for the engine to change the rotor speed, and the stability of the machine was found to be very poor. The machine was subsequently tethered so that it could move only vertically upward. The automatic control of a quadrotor helicopter has attracted the attention of many researches in the past few years [2][3][4][5]. Generally, the control strategies are based on simplified models which have both a minimum number of states and minimum number of inputs. These reduced models should retain the main features that must be considered when designing control laws for real aerial vehicles. In this work, we present the model of a quadrotor helicopter whose dynamical model is obtained via Newton’s laws. A backstepping control strategy is proposed having in mind that the quadrotor can be seen as the interconnected of three subsystems: under-actuated subsystem, fully-actuated subsystem
1-4244-0259-X/06/$20.00 ©2006 IEEE
and propeller subsystem. The output of the under-actuated subsystem are the x and y motion. In order to control them, tilt angles (pitch and roll) need to be controlled. It appeared judicious to us to use a backstepping technique to solve this problem. The idea is to stabilize the system in two phases. Firstly, the positions (x, y) are controlled by a virtual control based on the tilt angles. Secondly, the tilt angles are controlled by varying the rotor speeds, thereby changing the lift forces. The backstepping technique will be used to control the z motion and the yaw angle of the fully-actuated subsystem. The paper is organized as follows: in section II, a dynamic model for a quadrotor helicopter is developed. Based on this nonlinear model, we design in section III a backstepping control law. In section IV, some simulations are carried out to show the performance and stability of the proposed controller. Finally, in section V, our conclusions are presented. II. DYNAMIC MODELING OF A QUADROTOR HELICOPTER The quadrotor helicopter is shown in figure 1. The two pairs of rotors (1, 3) and (2, 4) turn in opposite direction in order to balance the moments and produce yaw motions as needed. On varying the rotor speeds altogether with the same quantity the lift forces will change affecting in this case the altitude z of the system. Yaw angle is obtained by speeding up the clockwise motors or slowing down depending on the desired angle direction. The motion direction according (x, y) axes depends on the sense of tilt angles (pitch and roll) whether they are positive or negative.
3255
Fig. 1.
Quadrotor helicopter
The equations describing the attitude and position of a quadrotor helicopter are basically those of a rotating rigid body with six degrees of freedom [6] [7]. They may be separated into kinematic equations and dynamic equations [8]. Let be two main reference frames (see Fig. 1): a • the earth fixed inertial reference frame E : → → → a − a − a − a (O , e1 , e2 , e3 ). − → − → → m m − m • the body fixed reference frame E : (Om , em 1 , e2 , e3 ) regidly attached to the aircraft. Let the vector ζ [x, y, z]T and η [φ, θ, ψ]T denote respectively the altitude positions and the attitude angles of the quadrotor (frame E m ) in the frame E a relative to a fixed origin Oa . The attitude angles {φ, θ, ψ} are respectively called pitch angle (− π2 < φ < π2 ), roll angle (− π2 < θ < π2 ) and yaw angle (−π ≤ ψ < π). The quadrotor is restricted with the six degrees of freedom according to the reference frame E m : Three translation velocities V = [V1 , V2 , V3 ]T and three rotation velocities Ω = [Ω1 , Ω2 , Ω3 ]T . The relation existing between the velocities ˙ η) vectors (V, Ω) and (ζ, ˙ are: ζ˙ = Rt V (1) Ω = Rr η˙ where Rt and Rr are respectively the transformation velocity matrix and the rotation velocity matrix between E a and E m such as: ⎤ ⎡ Cφ Cψ Sφ Sθ Cψ − Cφ Sψ Cφ Sθ Cψ + Sφ Sψ Rt = ⎣ Cθ Sψ Sφ Sθ Sψ + Cφ Cψ Cφ Sθ Sψ − Sφ Cψ ⎦ −Sφ Sφ Cθ Cφ Cθ (2) and ⎤ ⎡ 1 0 −Sθ Rr = ⎣ 0 Cφ Cθ Sφ ⎦ (3) 0 −Sφ Cφ Cθ where S(.) and C(.) are the respective abbreviations of sin(.) and cos(.). One can write R˙ t = Rt S(Ω) where S(Ω) denotes the skewsymmetric matrix such that S(Ω)v = Ω × v for the vector cross-product × and any vector v ∈ R3 . In other words, for a given vector Ω,the skew-symmetric matrix S(Ω) is defined as follows: ⎡ ⎤ 0 −Ω3 Ω2 0 −Ω1 ⎦ S(Ω) = ⎣ Ω3 (4) −Ω2 Ω1 0 The derivation of (1) with respect to time gives ˙ ˙ Rt S(Ω)V = Rt (V˙ + Ω × V ) ζ¨ = Rt V˙ + R t V = Rt V + ∂Rr ˙ r ˙ Ω˙ = Rr η¨ + ∂R ∂φ φ + ∂θ θ η˙ (5) Using the Newton’s laws in the reference
frame E m , about the quadrotor helicopter subjected to forces Fext and
moments Text applied to the epicenter, one obtains the dynamic equation motions:
F = mV˙ + Ω × (mV )
ext (6) Text = IT Ω˙ + Ω × (IT Ω)
where m and IT = diag[Ix , Iy , Iz ] are respectively the
mass and the total inertia matrix of helicopter, Fext and Text includes the external forces/torques developed in the epicenter of a quadrotor according to the direction of the reference frame E m , such as:
Fext = F − Faero − Fgrav (7) Text = T − Taero where the forces {F, Faero , Fgrav } and the torques {T, Taero } are explained in the table I where G = [0, 0, g]T is the gravity vector (g = 9.81m.s−2 ), {Kt , Kr } are two diagonal aerodynamic friction matrices. Model F = [0, 0, F3 ]T T = [T1 , T2 , T3 ]T Faero = Kt V Taero = Kr Ω Fgrav = mRtT G
Source propeller system aerodynamic friction gravity effect
TABLE I M AIN PHYSICAL EFFECTS ACTING ON A QUADROTOR
The forces F and torques T produced by the propeller system of a quadrotor are: ⎤ ⎤ ⎡ ⎡ 0 d (F2 − F4 ) ⎦, T = ⎣ ⎦ d (F − F1 ) F =⎣ 0 (8)
4 3 4 i+1 F (−1) F c i i i=1 i=1 where d is the distance from the epicenter of a quadrotor to the rotor axes and c > 0 is the drag factor. Using (5), (6) and (7) allows to give the equation of the dynamics of rotation of the quadrotor expressed in the reference frame E a : ⎧ T¨ T ˙ T ⎪ ⎨ F = mRt ζ + Kt Rt ζ + mRt G ∂Rr ˙ ∂Rr ˙ (9) T = IT Rr η¨ + IT ∂φ φ + ∂φ θ η˙ ⎪ ⎩ ˙ × (IT Rr η) ˙ +Kr Rr η˙ + (Rr η) The dynamic model (9) of the quadrotor helicopter has six outputs {x, y, z, φ, θ, ψ} while it has only four independent inputs. Therefore the quadrotor is an under-actuated system. We are not able to control all of the states at the same time. A possible combination of controlled outputs can be {x, y, z, ψ} in order to track the desired positions, more to an arbitrary heading and stabilize the other two angles, which introduces stable zero dynamics into the system [5]. A good controller should be able to reach a desired position and a desired yaw angle while guaranteeing stability of the pitch and roll angles. III. BACKSTEPPING CONTROL DESIGN The main objective of this section is to design a backstepping controller of a quadrotor helicopter ensuring that the position {x(t), y(t), z(t), ψ(t)} tracks the desired trajectory {xd (t), yd (t), zd (t), ψd (t)} asymptotically. This can be done in two phases:
3256
The dynamic model is written in an appropriate form. Indeed, it is divided into three subsystems: an underactuated subsystem, a fully-actuated subsystem and a propeller subsystem. • Synthesis of the control law via backstepping technique in seven steps. Let u = [F˙1 , F˙2 , F˙3 , F˙4 ]T the control input. The dynamics equations (9) can be rewritten in a state-space form according to the following state vectors: ⎡ ⎤ x φ ψ F1 x1 = , x3 = , x5 = , ⎢ ⎥ y θ z x7 = ⎢ F2 ⎥ . ˙ ˙ ⎣ F3 ⎦ x˙ φ ψ , x2 = , x4 = = , x 6 F4 y˙ z˙ θ˙ (10) We obtain the stat-space equations of an under-actuated subsystem S1 , a fully-actuated subsystem S2 and a propeller subsystem S3 : ⎧ x˙ 1 = x2 ⎪ ⎪ ⎨ x˙ 2 = f0 (x2 , x3 , x5 , x6 ) + g0 (x5 , x7 )ϕ0 (x3 ) S1 : x˙ 3 = x4 ⎪ ⎪ ⎩ x˙ 4 = f1 (x3 , x4 , x6 , x7 ) + g1 (x3 )ϕ1 (x7 ) x˙ 5 = x6 (11) S2 : x˙ 6 = f2 (x3 , x4 , x6 , x7 ) + g2 (x3 )ϕ2 (x7 ) S3 : x˙ 7 = u •
where the matrices gi (i = 0, 1, 2) are1 1 P4 Cψ Sψ Ix i=1 Fi , g g0 = = 1 m −Cψ Sψ 0 1 Cφ Seθ 0 I z g2 = 1 0 m Cφ Cθ
1 Iy Sφ T θ 1 Iy Cφ
the vectors ϕi (i = 0, 1, 2) are Sφ d(F2 − F4 ) ϕ0 = , ϕ1 = d(F3 − F1 ) Cφ Sθ c(F1 − F2 + F3 − F4 ) ϕ2 = F1 + F2 + F3 + F4 and the vectors fi (i = 0, 1, 2) are fx fφ fψ f0 = , f1 = , f2 = fy fθ fz with ⎡ fx ⎣ fy fz ⎡ fφ ⎣ fθ fψ
1T θ
Using the backstepping approach [9], one can synthesize the control law forcing the subsystems S1 , S2 and S3 to follow the desired trajectories. In this purpose, we will build the control law by the seven following steps: • Step 1: For the first step we consider the virtual system x˙ 1 = v1 Let the first tracking error z1 = x1d − x1
V˙ 1 = z1T z˙1 = z1T (x˙ 1d − x˙ 1 ) = z1T (x˙ 1d − v1 )
(19)
The stabilization of z1 can be obtained by introducing a first virtual control input v1 : v1 = A1 z1 + x˙ 1d
(20)
with A1 ∈ R2×2 is a positive definite matrix. The equation (19) is then V˙ 1 = −z1T A1 z1 < 0. Step 2: For the second step we consider the following new virtual system x˙ 2 = f0 (x2 , x3 , x5 , x6 ) + g0 (x5 , x7 )v2
(21)
where v2 is a second virtual control input. Let us proceed to variable change by making
z2 = v1 − x2 = A1 z1 + x˙ 1d − x˙ 1
(22)
z˙1
(12)
(13)
hence z˙1 = −A1 z1 + z2 . For the second step we consider the augmented Lyapunov function: 1 2 V2 = ziT zi (23) i=1 2 The time derivative of (23) is V˙ 2 = z1T z˙1 + z2T z˙2 = z1T (−A1 z1 + z2 ) + z2T (v˙ 1 − x˙ 2 ) = −z1T A1 z1 + z2T (z1 + v˙ 1 − f0 − g0 v2 )
(14)
(15)
(24)
The stabilisation of z2 can be obtained by introducing the following augmented virtual control v2 = g0−1 (z1 + A2 z2 + v˙ 1 − f0 )
˙ η˙ ⎦ = −(IT Rr )−1 [ IT ( ∂Rr φ˙ + ∂Rr θ) ∂φ ∂φ ˙ × (IT Rr η)] ˙ −K R η˙ − (R η) ⎡
4 r r i+1 r ⎤ c Fi I Cφ Tθ i=1 (−1) ⎥ ⎢ z 4 + ⎣ − Icz Sφ i=1 (−1)i+1 Fi ⎦ d Iy Sφ Seθ (F3 − F1 )
and Se(.) are respectively the abbreviations of tan(.) and
(17)
and considering the Lyapunov function positive definite 1 (18) V1 = z1T z1 2 Its time derivative is
⎤ ⎦ = − 1 Rt Kt RtT ζ˙ − G m ⎤
(16)
(25)
2×2
is a positive definite matrix. with A2 ∈ R Remark 1:It is worthy noting that the determinant of the 2
4
4 1 matrix g0 is m > 0, if i=1 Fi = 0. Therefore, i=1 Fi the matrix
4g0 is nonsingular in general operation condition because i=1 Fi represents the total thrust on the body in the z-axis and is generally nonzero to overcome the gravity. By using the equation (24) is comes V˙ 2 =
2 − i=1 ziT Ai zi < 0. • Step 3: For the third step we consider the virtual system
1 cos(.)
x˙ 3 = v3 3257
(26)
Let z3 = v2 − ϕ0 (x3 ) = g0−1 (z1 + A2 z2 + v˙ 1 − f0 ) − ϕ0 (x3 ) (27) then g0 z3 = z1 + A2 z2 + v˙ 1 − g0 ϕ0 z˙2
hence z˙2 = −z1 − A2 z2 + g0 z3 . For the third step we consider the Lyapunov function: 1 3 ziT zi (28) V3 = i=1 2 The derivatives with respect to time of (28) is V˙ 3 = z1T z˙1 + z2T z˙2 + z3T z˙3 = =
(29)
z1T (−A1 z1 + z2 ) + z2T (−z1 − A2 z2 + g0 z3 ) +z3T (v˙ 2 − ϕ˙ 0 ) −z1T A1 z1 − z2T A2 z2 + z3T (g0T z2 + v˙ 2 − J0 v3 )
where J0 is the Jacobian matrix of ϕ0 such as: ∂ϕ0 (x3 ) 0 Cφ = J0 = −Sφ Sθ Cφ Cθ ∂x3
with A4 ∈ R2×2 is a positive definite matrix. Remark 3: Since the condition (− π2 < φ < π2 ) is generally satisfied, then the matrix g1 given by (12) is nonsingular. While introducing the control (37) in equation (36) one ob 4 tains V˙ 4 = − i=1 ziT Ai zi < 0. Consequently, the subsystem S1 is asymptotically stable with the virtual control inputs v1 , v2 , v3 and v4 . After having stabilized an under-actuated subsystem S1 , there remains to us the stabilization of a fully-actuated subsystem S2 . To carry out this task, we will devote the two next steps. • Step 5: In this step we consider the virtual system
(31)
(32)
1 T z z5 2 5 The time derivative of (40) is V5 =
V˙ 4 = z1T z˙1 + z2T z˙2 + z3T z˙3 + z4T z˙4 = z1T (−A1 z1 + z2 ) + z2T (−z1 − A2 z2 + g0 z3 ) +z3T (−g0T z2 − A3 z3 + J0 z4 ) + z4T (v˙ 3 − x˙ 4 ) = −z1T A1 z1 − z2T A2 z2 − z3T A3 z3 +z4T (J0T z3 + v˙ 3 − f1 − g1 v4 )
(40)
V˙ 5 = z5T z˙5 = z5T (x˙ 5d − x˙ 5 ) = z5T (x˙ 5d − v5 )
(41)
The stabilization of z5 can be obtained by introducing a virtual control: v5 = A5 z5 + x˙ 5d (42) with A5 ∈ R2×2 is a positive definite matrix. By using (42) in (41) one obtains V˙ 5 = −z5T A5 z5 < 0. • Step 6: For this step we consider the system x˙ 6 = f2 (x3 , x4 , x6 , x7 ) + g2 (x3 )v6
(43)
Let
(33)
z6 = v5 − x6 = A5 z5 + x˙ 5d − x˙ 5
(44)
z˙5
(34)
Then z˙5 = −A5 z5 + z6 . The augmented Lyapunov function for this step is
z˙3
hence z˙3 = −g0T z2 − A3 z3 + J0 z4 . The global Lyapunov function of a subsystem S1 is 1 4 ziT zi V4 = i=1 2 Its time derivative is
(39)
and the Lyapunov function:
we get J0 z4 = g0T z2 + A3 z3 + v˙ 2 − J0 x4
(38)
z5 = x5d − x5
(30)
Putting z4 = v3 − x4 = J0−1 (g0T z2 + A3 z3 + v˙ 2 ) − x4
(37)
Let the tracking error
with A3 ∈ R2×2 is a positive definite matrix.
3 Using (29), we get V˙ 3 = − i=1 ziT Ai zi < 0. • Step 4: we consider the final virtual system of an underactuated subsystem S1 : x˙ 4 = f1 (x3 , x4 , x6 , x7 ) + g1 (x3 )v4
v4 = g1−1 (J0T z3 + A4 z4 + v˙ 3 − f1 )
x˙ 5 = v5
Remark 2: It should be noted that the determinant of the jacobian matrix J0 (x3 ) is Cφ2 Cθ . Therefore, J0 (x3 ) is nonsingular when (− π2 < φ < π2 ) and (− π2 < θ < π2 ), which is satisfied generally. The stabilization of z3 can be obtained by introducing a virtual control: v3 = J0−1 (g0T z2 + A3 z3 + v˙ 2 )
The stabilization of subsystem S1 can be obtained by introducing a following virtual control law:
V6 =
1 T (z z5 + z6T z6 ) 2 5
Its time derivative is
(35)
V˙ 6 = z5T z˙5 + z6T z˙6 = z5T (−A5 z5 + z6 ) + z6T (v˙ 5 − x˙ 6 ) = −z5T A5 z5 + z6T (z5 + v˙ 5 − f2 − g2 v6 )
(36)
(45)
(46)
The stabilization of S2 can be obtained by introducing a following virtual control: v6 = g2−1 (z5 + A6 z6 + v˙ 5 − f2 ) with A6 ∈ R
3258
2×2
is a positive definite matrix.
(47)
Remark 4: Knowing that (− π2 < φ < π2 ) and (− π2 < θ < it is easy to show in (12) that the matrix g2 is nonsingular. The introducing the control (47) in equation (46) one obtains V˙ 6 = −z5T A5 z5 − z6T A6 z6 < 0. One can thus conclude that the subsystem S2 is asymptotically stable. Now, in the following step, we will develop the real control which stabilizes the whole system. • Step 7: For the final step we consider the propeller subsystem (48) x˙ 7 = u π 2 ),
Let
z7 = ⎡ ⎢ =⎢ ⎣
v4 − ϕ1 (x7 ) v6 − ϕ2 (x7 )
(49)
g1−1 (J0T z3 + A4 z4 + v˙ 3 − f1 − g1 ϕ1 ) g2−1 (z5
z˙4
+ A6 z6 + v˙ 5 − f2 − g2 ϕ2 )
⎤ ⎥ ⎥ ⎦
z˙6
hence z˙4 = g1∗ z7 − J0T z3 − A4 z4 and z˙6 = g2∗ z7 − z5 − A6 z6 where g1∗ = [g1 , 02×2 ] and g2∗ = [02×2 , g2 ] with 02×2 is a null matrix in R2×2 . The Lyapunov function candidate of the whole system {S1 , S2 , S3 } is 1 7 V7 = ziT zi (50) i=1 2 Its time derivative is given
7 T V˙ 7 = i=1 zi z˙i =
(51)
z1T (−A1 z1 + z2 ) + z2T (−z1 − A2 z2 + g0 z3 ) +z3T (−g0T z2 − A3 z3 + J0 z4 ) T +z4T (−J0T z3 − A4 z4 + g1∗ z7 ) + z 5 (−A5z5 + z6 )
v˙ 4 ϕ˙ 1 − +z6T (−z5 − A6 z6 + g2∗ z7 ) + z7T v˙ 6 ϕ˙ 2 T
6 02×2 g1 z4 T T = − i=1 zi Ai zi + z7 02×2 g2 z6 ⎞ v˙ 4 J1 + − x˙ 7 ⎠ v˙ 6 J2 u
1 (x7 ) where J1 = ∂ϕ∂x 7 matrices of ϕ1 and ϕ2
J1 =
0 d −d 0
∂ϕ2 (x7 ) ∂x7
and J2 = are the Jacobian such as: 0 −d c −c c −c , J2 = (52) d 0 1 1 1 1
Therefore, the stabilization of the whole system can be obtained by introducing a following control law: −1 T 02×2 g1 J1 z4 (53) u= J2 02×2 g2 z6 v˙ 4 + A7 z7 + v˙ 6 with A7 ∈ R4×4 is a positive definite matrix.
Remark 5: It should be noted that the determinant of the J1 is 8cd2 . Therefore, this matrix is nonsingular matrix J2 when c > 0 and d > 0. While introducing
7 the control (53) in equation (51) one obtains V˙ 7 = − i=1 ziT Ai zi < 0. Consequently, by using (17, 20, 22, 25, 27, 31, 33, 37, 39, 42, 44, 47 and 53), the whole system (11) is asymptotically stable with the following control law: ⎧ v1 = A1 (x1d − x1 ) + x˙ 1d ⎪ ⎪ ⎪ −1 ⎪ ⎪ v2 = g0 [(x1d − x1 ) + A2 (v1 − x2 ) + v˙ 1 − f0 ] ⎪ ⎪ ⎪ v3 = J0−1 [g0T (v1 − x2 ) + A3 (v2 − ϕ0 ) + v˙ 2 ] ⎪ ⎪ ⎪ ⎪ v4 = g1−1 [J0T (v2 − ϕ0 ) + A4 (v3 − x4 ) + v˙ 3 − f1 ] ⎪ ⎪ ⎪ ⎨ v5 = A5 (x5d − x5 ) + x˙ 5d v6 = g2−1 [(x5d − x5 ) + A6 (v5 − x6 ) + v˙ 5 − f2 ] ⎪ ⎪ −1 T ⎪ ⎪ ⎪ 02×2 g1 v3 − x4 J1 ⎪ ⎪ u= ⎪ ⎪ J2 02×2 g2 v5 − x6 ⎪ ⎪ ⎪ ⎪ ⎪ v4 − ϕ1 v˙ 4 ⎪ ⎩ + A7 + v˙ 6 v6 − ϕ2 (54) IV. S IMULATION RESULT In order to verify the effectiveness and the efficiency of the proposed backstepping control law, an application to quadrotor helicopter is conducted by simulation using RungKutta’s method with variable step. The nominal parameters for quadrotor helicopter are: m = 2kg, Ix = Iy = Iz /2 = 1.2416N m.s2 /rad, d = 0.2m, c = 0.01m, g = 9.81m/s2 , Kt = diag[10−2 , 10−2 , 10−2 ]N.s/m and Kr = diag[10−3 , 10−3 , 10−3 ]N m.s/rad. The following controller parameters are used: A1 = . . . = A6 = diag[3, 3] and A7 = diag[3, 3, 3, 3]. The proposed backstepping control law (54) requires the knowledge of v˙ 1 , v˙ 2 , v˙ 3 , v˙ 4 , v˙ 5 and v˙ 6 . In order to avoid analytical derivation difficulties, we estimate them by using the following finite difference time approximation: Δvi v˙ i = for i = 1, . . . , 6 (55) Δt where Δvi is the change in input value and Δt is the change in time since the previous simulation time step. The helicopter is initially in hover flight and the initial conditions are: x1 (0) = T . . . = x6 (0) = [0, 0]T and x7 (0) = mg 4 [1, 1, 1, 1] . The reference trajectory chosen for xd (t), yd (t), zd (t) and ψd (t) is that of the step response of the following transfer function: 1 H(s) = (56) (s + 1)6 with s is the Laplace variable. Figures 2 and 3 show the positions and the tracking errors of a quadrotor helicopter respectively. It can be seen the good tracking of the desired trajectory. Moreover, we can notice in figure 2 an optimization of tilt angles (pitch and roll) and consequently the use of minimal energy. The behavior of all virtual controls is given in figure 4. We note that their evolutions are smooth and derivable. The obtained input control signals (see figure 5) are acceptable and physically realizable.
3259
Fig. 2.
Fig. 5.
Position outputs
Force control inputs
V. C ONCLUSION In this paper, we have presented the dynamic modeling of quadrotor helicopter and introduced a new approach of a backstepping control. This process is under-actuated system because it has six outputs while it has only four inputs. The whole system of the quadrotor helicopter is divided into three subsystems: an under-actuated subsystem, fully-actuated subsystem and a propeller subsystem. A backstepping control algorithm is proposed to stabilize the whole system and is able to drive a quadrotor to the desired trajectory of Cartesian position and yaw angle. The simulation results show the good performance of the proposed control approach. R EFERENCES Fig. 3.
[1] O. Shakernia, Y. Ma, T. J. Koo and S. S. Sastry, ”Landing an Unmanned Air Vehicle: Vision-based Motion Estimation and Nonlinear Control”, Asian J. Control, vol. 1, no. 3, 1999. [2] A. Mokhtari, A. Benallegue and A. Belaidi, ”Polynomial Linear Quadratic Gaussian ans Sliding Mode Observer for a Quadrotor Unmanned Aerial Vehicle”, Journal of Robotics and Mechatronics, vol. 17, no. 4, pp. 483495, 2005. [3] S. Bouabdallah and R. Siegwart, ”Backstepping and Sliding-mode Techniques Applied to an Indoor Micro Quadrotor”, Proceedings of the 2005 IEEE International Conference on Robotics and Automation, pp. 22592264, 2005. [4] P. Castillo, A. Dzul and R. Lozano, ”Real-Time Stabilization and Tracking of Four-Rotor Mini Rotorcraft”, IEEE Transactions on Control Systems Technology, vo. 12, no. 4, pp. 510-516, 2004. [5] E. Altug, J. P. Ostrowski and C. J. Taylor,”Quadrotor Control using Dual Cameral Visual Feedback”, Proceedings of the 2003 IEEE International Conference on Robotics and Automation, vol. 3, pp. 4294-4299, 2003. [6] M. Vukobratovic, Applied Dynamics of Manipulation Robots: Modelling, Analysis and Examples, Berlin: Springer-Verlag, 1989. [7] S. B. V. Gomes and J. J. Jr. G.Ramas, ”Airship dynamic modelling for autonomous operation”, IEEE International Conference on Robotics and Automation, 1998. [8] H. Nijmeijer and AJ Van Der Shaft, Nonlinear Dynamic Control Systems, Springer Verlag, 1990. [9] I. Fantoni and R. Lozano, Non-linear control for underactuated mechanical systems, Springer, 2002.
Tracking errors for x, y, z and ψ
Fig. 4. Evolution of the virtual controls: solid line with the left y-axis denote the first element and the dashed line with the right y-axis denote the second element.
3260
Backstepping Control for a Quadrotor Helicopter Tarek Madani and Abdelaziz Benallegue Laboratoire d’Ing´enierie des Syst`emes de Versailles 10-12, avenue de l’Europe, 78140 V´elizy - FRANCE E-mail: {madani and benalleg}@lisv.uvsq.fr Abstract— This paper presents a nonlinear dynamic model for a quadrotor helicopter in a form suited for backstepping control design. Due to the under-actuated property of quadrotor helicopter, the controller can set the helicopter track three Cartesian positions (x, y, z) and the yaw angle to their desired values and stabilize the pitch and roll angles. The system has been presented into three interconnected subsystems. The first one representing the under-actuated subsystem, gives the dynamic relation of the horizontal positions (x, y) with the pitch and roll angles. The second fully-actuated subsystem gives the dynamics of the vertical position z and the yaw angle. The last subsystem gives the dynamics of the propeller forces. A backstepping control is presented to stabilize the whole system. The design methodology is based on the Lyapunov stability theory. Various simulations of the model show that the control law stabilizes a quadrotor with good tracking.
I. I NTRODUCTION Unmanned Aerial Vehicles (UAV) have unmatched qualities that make them the only effective solution in specialized tasks where risks to pilots are high, where beyond normal human endurance is required, or where human presence is not necessary. UAVs are being used more and more in civilian applications such as traffic monitoring, recognition and surveillance vehicles, search and rescue operations [1]. They are highly capable to flown without an on-board pilot. These robotic aircrafts are often computerized and fully autonomous. The first reported quadrotor helicopter (Gyroplane No.1) was built in 1907 by the Breguet Brothers. However, there was no means of control provided to the pilot other than a throttle for the engine to change the rotor speed, and the stability of the machine was found to be very poor. The machine was subsequently tethered so that it could move only vertically upward. The automatic control of a quadrotor helicopter has attracted the attention of many researches in the past few years [2][3][4][5]. Generally, the control strategies are based on simplified models which have both a minimum number of states and minimum number of inputs. These reduced models should retain the main features that must be considered when designing control laws for real aerial vehicles. In this work, we present the model of a quadrotor helicopter whose dynamical model is obtained via Newton’s laws. A backstepping control strategy is proposed having in mind that the quadrotor can be seen as the interconnected of three subsystems: under-actuated subsystem, fully-actuated subsystem
1-4244-0259-X/06/$20.00 ©2006 IEEE
and propeller subsystem. The output of the under-actuated subsystem are the x and y motion. In order to control them, tilt angles (pitch and roll) need to be controlled. It appeared judicious to us to use a backstepping technique to solve this problem. The idea is to stabilize the system in two phases. Firstly, the positions (x, y) are controlled by a virtual control based on the tilt angles. Secondly, the tilt angles are controlled by varying the rotor speeds, thereby changing the lift forces. The backstepping technique will be used to control the z motion and the yaw angle of the fully-actuated subsystem. The paper is organized as follows: in section II, a dynamic model for a quadrotor helicopter is developed. Based on this nonlinear model, we design in section III a backstepping control law. In section IV, some simulations are carried out to show the performance and stability of the proposed controller. Finally, in section V, our conclusions are presented. II. DYNAMIC MODELING OF A QUADROTOR HELICOPTER The quadrotor helicopter is shown in figure 1. The two pairs of rotors (1, 3) and (2, 4) turn in opposite direction in order to balance the moments and produce yaw motions as needed. On varying the rotor speeds altogether with the same quantity the lift forces will change affecting in this case the altitude z of the system. Yaw angle is obtained by speeding up the clockwise motors or slowing down depending on the desired angle direction. The motion direction according (x, y) axes depends on the sense of tilt angles (pitch and roll) whether they are positive or negative.
3255
Fig. 1.
Quadrotor helicopter
The equations describing the attitude and position of a quadrotor helicopter are basically those of a rotating rigid body with six degrees of freedom [6] [7]. They may be separated into kinematic equations and dynamic equations [8]. Let be two main reference frames (see Fig. 1): a • the earth fixed inertial reference frame E : → → → a − a − a − a (O , e1 , e2 , e3 ). − → − → → m m − m • the body fixed reference frame E : (Om , em 1 , e2 , e3 ) regidly attached to the aircraft. Let the vector ζ [x, y, z]T and η [φ, θ, ψ]T denote respectively the altitude positions and the attitude angles of the quadrotor (frame E m ) in the frame E a relative to a fixed origin Oa . The attitude angles {φ, θ, ψ} are respectively called pitch angle (− π2 < φ < π2 ), roll angle (− π2 < θ < π2 ) and yaw angle (−π ≤ ψ < π). The quadrotor is restricted with the six degrees of freedom according to the reference frame E m : Three translation velocities V = [V1 , V2 , V3 ]T and three rotation velocities Ω = [Ω1 , Ω2 , Ω3 ]T . The relation existing between the velocities ˙ η) vectors (V, Ω) and (ζ, ˙ are: ζ˙ = Rt V (1) Ω = Rr η˙ where Rt and Rr are respectively the transformation velocity matrix and the rotation velocity matrix between E a and E m such as: ⎤ ⎡ Cφ Cψ Sφ Sθ Cψ − Cφ Sψ Cφ Sθ Cψ + Sφ Sψ Rt = ⎣ Cθ Sψ Sφ Sθ Sψ + Cφ Cψ Cφ Sθ Sψ − Sφ Cψ ⎦ −Sφ Sφ Cθ Cφ Cθ (2) and ⎤ ⎡ 1 0 −Sθ Rr = ⎣ 0 Cφ Cθ Sφ ⎦ (3) 0 −Sφ Cφ Cθ where S(.) and C(.) are the respective abbreviations of sin(.) and cos(.). One can write R˙ t = Rt S(Ω) where S(Ω) denotes the skewsymmetric matrix such that S(Ω)v = Ω × v for the vector cross-product × and any vector v ∈ R3 . In other words, for a given vector Ω,the skew-symmetric matrix S(Ω) is defined as follows: ⎡ ⎤ 0 −Ω3 Ω2 0 −Ω1 ⎦ S(Ω) = ⎣ Ω3 (4) −Ω2 Ω1 0 The derivation of (1) with respect to time gives ˙ ˙ Rt S(Ω)V = Rt (V˙ + Ω × V ) ζ¨ = Rt V˙ + R t V = Rt V + ∂Rr ˙ r ˙ Ω˙ = Rr η¨ + ∂R ∂φ φ + ∂θ θ η˙ (5) Using the Newton’s laws in the reference
frame E m , about the quadrotor helicopter subjected to forces Fext and
moments Text applied to the epicenter, one obtains the dynamic equation motions:
F = mV˙ + Ω × (mV )
ext (6) Text = IT Ω˙ + Ω × (IT Ω)
where m and IT = diag[Ix , Iy , Iz ] are respectively the
mass and the total inertia matrix of helicopter, Fext and Text includes the external forces/torques developed in the epicenter of a quadrotor according to the direction of the reference frame E m , such as:
Fext = F − Faero − Fgrav (7) Text = T − Taero where the forces {F, Faero , Fgrav } and the torques {T, Taero } are explained in the table I where G = [0, 0, g]T is the gravity vector (g = 9.81m.s−2 ), {Kt , Kr } are two diagonal aerodynamic friction matrices. Model F = [0, 0, F3 ]T T = [T1 , T2 , T3 ]T Faero = Kt V Taero = Kr Ω Fgrav = mRtT G
Source propeller system aerodynamic friction gravity effect
TABLE I M AIN PHYSICAL EFFECTS ACTING ON A QUADROTOR
The forces F and torques T produced by the propeller system of a quadrotor are: ⎤ ⎤ ⎡ ⎡ 0 d (F2 − F4 ) ⎦, T = ⎣ ⎦ d (F − F1 ) F =⎣ 0 (8)
4 3 4 i+1 F (−1) F c i i i=1 i=1 where d is the distance from the epicenter of a quadrotor to the rotor axes and c > 0 is the drag factor. Using (5), (6) and (7) allows to give the equation of the dynamics of rotation of the quadrotor expressed in the reference frame E a : ⎧ T¨ T ˙ T ⎪ ⎨ F = mRt ζ + Kt Rt ζ + mRt G ∂Rr ˙ ∂Rr ˙ (9) T = IT Rr η¨ + IT ∂φ φ + ∂φ θ η˙ ⎪ ⎩ ˙ × (IT Rr η) ˙ +Kr Rr η˙ + (Rr η) The dynamic model (9) of the quadrotor helicopter has six outputs {x, y, z, φ, θ, ψ} while it has only four independent inputs. Therefore the quadrotor is an under-actuated system. We are not able to control all of the states at the same time. A possible combination of controlled outputs can be {x, y, z, ψ} in order to track the desired positions, more to an arbitrary heading and stabilize the other two angles, which introduces stable zero dynamics into the system [5]. A good controller should be able to reach a desired position and a desired yaw angle while guaranteeing stability of the pitch and roll angles. III. BACKSTEPPING CONTROL DESIGN The main objective of this section is to design a backstepping controller of a quadrotor helicopter ensuring that the position {x(t), y(t), z(t), ψ(t)} tracks the desired trajectory {xd (t), yd (t), zd (t), ψd (t)} asymptotically. This can be done in two phases:
3256
The dynamic model is written in an appropriate form. Indeed, it is divided into three subsystems: an underactuated subsystem, a fully-actuated subsystem and a propeller subsystem. • Synthesis of the control law via backstepping technique in seven steps. Let u = [F˙1 , F˙2 , F˙3 , F˙4 ]T the control input. The dynamics equations (9) can be rewritten in a state-space form according to the following state vectors: ⎡ ⎤ x φ ψ F1 x1 = , x3 = , x5 = , ⎢ ⎥ y θ z x7 = ⎢ F2 ⎥ . ˙ ˙ ⎣ F3 ⎦ x˙ φ ψ , x2 = , x4 = = , x 6 F4 y˙ z˙ θ˙ (10) We obtain the stat-space equations of an under-actuated subsystem S1 , a fully-actuated subsystem S2 and a propeller subsystem S3 : ⎧ x˙ 1 = x2 ⎪ ⎪ ⎨ x˙ 2 = f0 (x2 , x3 , x5 , x6 ) + g0 (x5 , x7 )ϕ0 (x3 ) S1 : x˙ 3 = x4 ⎪ ⎪ ⎩ x˙ 4 = f1 (x3 , x4 , x6 , x7 ) + g1 (x3 )ϕ1 (x7 ) x˙ 5 = x6 (11) S2 : x˙ 6 = f2 (x3 , x4 , x6 , x7 ) + g2 (x3 )ϕ2 (x7 ) S3 : x˙ 7 = u •
where the matrices gi (i = 0, 1, 2) are1 1 P4 Cψ Sψ Ix i=1 Fi , g g0 = = 1 m −Cψ Sψ 0 1 Cφ Seθ 0 I z g2 = 1 0 m Cφ Cθ
1 Iy Sφ T θ 1 Iy Cφ
the vectors ϕi (i = 0, 1, 2) are Sφ d(F2 − F4 ) ϕ0 = , ϕ1 = d(F3 − F1 ) Cφ Sθ c(F1 − F2 + F3 − F4 ) ϕ2 = F1 + F2 + F3 + F4 and the vectors fi (i = 0, 1, 2) are fx fφ fψ f0 = , f1 = , f2 = fy fθ fz with ⎡ fx ⎣ fy fz ⎡ fφ ⎣ fθ fψ
1T θ
Using the backstepping approach [9], one can synthesize the control law forcing the subsystems S1 , S2 and S3 to follow the desired trajectories. In this purpose, we will build the control law by the seven following steps: • Step 1: For the first step we consider the virtual system x˙ 1 = v1 Let the first tracking error z1 = x1d − x1
V˙ 1 = z1T z˙1 = z1T (x˙ 1d − x˙ 1 ) = z1T (x˙ 1d − v1 )
(19)
The stabilization of z1 can be obtained by introducing a first virtual control input v1 : v1 = A1 z1 + x˙ 1d
(20)
with A1 ∈ R2×2 is a positive definite matrix. The equation (19) is then V˙ 1 = −z1T A1 z1 < 0. Step 2: For the second step we consider the following new virtual system x˙ 2 = f0 (x2 , x3 , x5 , x6 ) + g0 (x5 , x7 )v2
(21)
where v2 is a second virtual control input. Let us proceed to variable change by making
z2 = v1 − x2 = A1 z1 + x˙ 1d − x˙ 1
(22)
z˙1
(12)
(13)
hence z˙1 = −A1 z1 + z2 . For the second step we consider the augmented Lyapunov function: 1 2 V2 = ziT zi (23) i=1 2 The time derivative of (23) is V˙ 2 = z1T z˙1 + z2T z˙2 = z1T (−A1 z1 + z2 ) + z2T (v˙ 1 − x˙ 2 ) = −z1T A1 z1 + z2T (z1 + v˙ 1 − f0 − g0 v2 )
(14)
(15)
(24)
The stabilisation of z2 can be obtained by introducing the following augmented virtual control v2 = g0−1 (z1 + A2 z2 + v˙ 1 − f0 )
˙ η˙ ⎦ = −(IT Rr )−1 [ IT ( ∂Rr φ˙ + ∂Rr θ) ∂φ ∂φ ˙ × (IT Rr η)] ˙ −K R η˙ − (R η) ⎡
4 r r i+1 r ⎤ c Fi I Cφ Tθ i=1 (−1) ⎥ ⎢ z 4 + ⎣ − Icz Sφ i=1 (−1)i+1 Fi ⎦ d Iy Sφ Seθ (F3 − F1 )
and Se(.) are respectively the abbreviations of tan(.) and
(17)
and considering the Lyapunov function positive definite 1 (18) V1 = z1T z1 2 Its time derivative is
⎤ ⎦ = − 1 Rt Kt RtT ζ˙ − G m ⎤
(16)
(25)
2×2
is a positive definite matrix. with A2 ∈ R Remark 1:It is worthy noting that the determinant of the 2
4
4 1 matrix g0 is m > 0, if i=1 Fi = 0. Therefore, i=1 Fi the matrix
4g0 is nonsingular in general operation condition because i=1 Fi represents the total thrust on the body in the z-axis and is generally nonzero to overcome the gravity. By using the equation (24) is comes V˙ 2 =
2 − i=1 ziT Ai zi < 0. • Step 3: For the third step we consider the virtual system
1 cos(.)
x˙ 3 = v3 3257
(26)
Let z3 = v2 − ϕ0 (x3 ) = g0−1 (z1 + A2 z2 + v˙ 1 − f0 ) − ϕ0 (x3 ) (27) then g0 z3 = z1 + A2 z2 + v˙ 1 − g0 ϕ0 z˙2
hence z˙2 = −z1 − A2 z2 + g0 z3 . For the third step we consider the Lyapunov function: 1 3 ziT zi (28) V3 = i=1 2 The derivatives with respect to time of (28) is V˙ 3 = z1T z˙1 + z2T z˙2 + z3T z˙3 = =
(29)
z1T (−A1 z1 + z2 ) + z2T (−z1 − A2 z2 + g0 z3 ) +z3T (v˙ 2 − ϕ˙ 0 ) −z1T A1 z1 − z2T A2 z2 + z3T (g0T z2 + v˙ 2 − J0 v3 )
where J0 is the Jacobian matrix of ϕ0 such as: ∂ϕ0 (x3 ) 0 Cφ = J0 = −Sφ Sθ Cφ Cθ ∂x3
with A4 ∈ R2×2 is a positive definite matrix. Remark 3: Since the condition (− π2 < φ < π2 ) is generally satisfied, then the matrix g1 given by (12) is nonsingular. While introducing the control (37) in equation (36) one ob 4 tains V˙ 4 = − i=1 ziT Ai zi < 0. Consequently, the subsystem S1 is asymptotically stable with the virtual control inputs v1 , v2 , v3 and v4 . After having stabilized an under-actuated subsystem S1 , there remains to us the stabilization of a fully-actuated subsystem S2 . To carry out this task, we will devote the two next steps. • Step 5: In this step we consider the virtual system
(31)
(32)
1 T z z5 2 5 The time derivative of (40) is V5 =
V˙ 4 = z1T z˙1 + z2T z˙2 + z3T z˙3 + z4T z˙4 = z1T (−A1 z1 + z2 ) + z2T (−z1 − A2 z2 + g0 z3 ) +z3T (−g0T z2 − A3 z3 + J0 z4 ) + z4T (v˙ 3 − x˙ 4 ) = −z1T A1 z1 − z2T A2 z2 − z3T A3 z3 +z4T (J0T z3 + v˙ 3 − f1 − g1 v4 )
(40)
V˙ 5 = z5T z˙5 = z5T (x˙ 5d − x˙ 5 ) = z5T (x˙ 5d − v5 )
(41)
The stabilization of z5 can be obtained by introducing a virtual control: v5 = A5 z5 + x˙ 5d (42) with A5 ∈ R2×2 is a positive definite matrix. By using (42) in (41) one obtains V˙ 5 = −z5T A5 z5 < 0. • Step 6: For this step we consider the system x˙ 6 = f2 (x3 , x4 , x6 , x7 ) + g2 (x3 )v6
(43)
Let
(33)
z6 = v5 − x6 = A5 z5 + x˙ 5d − x˙ 5
(44)
z˙5
(34)
Then z˙5 = −A5 z5 + z6 . The augmented Lyapunov function for this step is
z˙3
hence z˙3 = −g0T z2 − A3 z3 + J0 z4 . The global Lyapunov function of a subsystem S1 is 1 4 ziT zi V4 = i=1 2 Its time derivative is
(39)
and the Lyapunov function:
we get J0 z4 = g0T z2 + A3 z3 + v˙ 2 − J0 x4
(38)
z5 = x5d − x5
(30)
Putting z4 = v3 − x4 = J0−1 (g0T z2 + A3 z3 + v˙ 2 ) − x4
(37)
Let the tracking error
with A3 ∈ R2×2 is a positive definite matrix.
3 Using (29), we get V˙ 3 = − i=1 ziT Ai zi < 0. • Step 4: we consider the final virtual system of an underactuated subsystem S1 : x˙ 4 = f1 (x3 , x4 , x6 , x7 ) + g1 (x3 )v4
v4 = g1−1 (J0T z3 + A4 z4 + v˙ 3 − f1 )
x˙ 5 = v5
Remark 2: It should be noted that the determinant of the jacobian matrix J0 (x3 ) is Cφ2 Cθ . Therefore, J0 (x3 ) is nonsingular when (− π2 < φ < π2 ) and (− π2 < θ < π2 ), which is satisfied generally. The stabilization of z3 can be obtained by introducing a virtual control: v3 = J0−1 (g0T z2 + A3 z3 + v˙ 2 )
The stabilization of subsystem S1 can be obtained by introducing a following virtual control law:
V6 =
1 T (z z5 + z6T z6 ) 2 5
Its time derivative is
(35)
V˙ 6 = z5T z˙5 + z6T z˙6 = z5T (−A5 z5 + z6 ) + z6T (v˙ 5 − x˙ 6 ) = −z5T A5 z5 + z6T (z5 + v˙ 5 − f2 − g2 v6 )
(36)
(45)
(46)
The stabilization of S2 can be obtained by introducing a following virtual control: v6 = g2−1 (z5 + A6 z6 + v˙ 5 − f2 ) with A6 ∈ R
3258
2×2
is a positive definite matrix.
(47)
Remark 4: Knowing that (− π2 < φ < π2 ) and (− π2 < θ < it is easy to show in (12) that the matrix g2 is nonsingular. The introducing the control (47) in equation (46) one obtains V˙ 6 = −z5T A5 z5 − z6T A6 z6 < 0. One can thus conclude that the subsystem S2 is asymptotically stable. Now, in the following step, we will develop the real control which stabilizes the whole system. • Step 7: For the final step we consider the propeller subsystem (48) x˙ 7 = u π 2 ),
Let
z7 = ⎡ ⎢ =⎢ ⎣
v4 − ϕ1 (x7 ) v6 − ϕ2 (x7 )
(49)
g1−1 (J0T z3 + A4 z4 + v˙ 3 − f1 − g1 ϕ1 ) g2−1 (z5
z˙4
+ A6 z6 + v˙ 5 − f2 − g2 ϕ2 )
⎤ ⎥ ⎥ ⎦
z˙6
hence z˙4 = g1∗ z7 − J0T z3 − A4 z4 and z˙6 = g2∗ z7 − z5 − A6 z6 where g1∗ = [g1 , 02×2 ] and g2∗ = [02×2 , g2 ] with 02×2 is a null matrix in R2×2 . The Lyapunov function candidate of the whole system {S1 , S2 , S3 } is 1 7 V7 = ziT zi (50) i=1 2 Its time derivative is given
7 T V˙ 7 = i=1 zi z˙i =
(51)
z1T (−A1 z1 + z2 ) + z2T (−z1 − A2 z2 + g0 z3 ) +z3T (−g0T z2 − A3 z3 + J0 z4 ) T +z4T (−J0T z3 − A4 z4 + g1∗ z7 ) + z 5 (−A5z5 + z6 )
v˙ 4 ϕ˙ 1 − +z6T (−z5 − A6 z6 + g2∗ z7 ) + z7T v˙ 6 ϕ˙ 2 T
6 02×2 g1 z4 T T = − i=1 zi Ai zi + z7 02×2 g2 z6 ⎞ v˙ 4 J1 + − x˙ 7 ⎠ v˙ 6 J2 u
1 (x7 ) where J1 = ∂ϕ∂x 7 matrices of ϕ1 and ϕ2
J1 =
0 d −d 0
∂ϕ2 (x7 ) ∂x7
and J2 = are the Jacobian such as: 0 −d c −c c −c , J2 = (52) d 0 1 1 1 1
Therefore, the stabilization of the whole system can be obtained by introducing a following control law: −1 T 02×2 g1 J1 z4 (53) u= J2 02×2 g2 z6 v˙ 4 + A7 z7 + v˙ 6 with A7 ∈ R4×4 is a positive definite matrix.
Remark 5: It should be noted that the determinant of the J1 is 8cd2 . Therefore, this matrix is nonsingular matrix J2 when c > 0 and d > 0. While introducing
7 the control (53) in equation (51) one obtains V˙ 7 = − i=1 ziT Ai zi < 0. Consequently, by using (17, 20, 22, 25, 27, 31, 33, 37, 39, 42, 44, 47 and 53), the whole system (11) is asymptotically stable with the following control law: ⎧ v1 = A1 (x1d − x1 ) + x˙ 1d ⎪ ⎪ ⎪ −1 ⎪ ⎪ v2 = g0 [(x1d − x1 ) + A2 (v1 − x2 ) + v˙ 1 − f0 ] ⎪ ⎪ ⎪ v3 = J0−1 [g0T (v1 − x2 ) + A3 (v2 − ϕ0 ) + v˙ 2 ] ⎪ ⎪ ⎪ ⎪ v4 = g1−1 [J0T (v2 − ϕ0 ) + A4 (v3 − x4 ) + v˙ 3 − f1 ] ⎪ ⎪ ⎪ ⎨ v5 = A5 (x5d − x5 ) + x˙ 5d v6 = g2−1 [(x5d − x5 ) + A6 (v5 − x6 ) + v˙ 5 − f2 ] ⎪ ⎪ −1 T ⎪ ⎪ ⎪ 02×2 g1 v3 − x4 J1 ⎪ ⎪ u= ⎪ ⎪ J2 02×2 g2 v5 − x6 ⎪ ⎪ ⎪ ⎪ ⎪ v4 − ϕ1 v˙ 4 ⎪ ⎩ + A7 + v˙ 6 v6 − ϕ2 (54) IV. S IMULATION RESULT In order to verify the effectiveness and the efficiency of the proposed backstepping control law, an application to quadrotor helicopter is conducted by simulation using RungKutta’s method with variable step. The nominal parameters for quadrotor helicopter are: m = 2kg, Ix = Iy = Iz /2 = 1.2416N m.s2 /rad, d = 0.2m, c = 0.01m, g = 9.81m/s2 , Kt = diag[10−2 , 10−2 , 10−2 ]N.s/m and Kr = diag[10−3 , 10−3 , 10−3 ]N m.s/rad. The following controller parameters are used: A1 = . . . = A6 = diag[3, 3] and A7 = diag[3, 3, 3, 3]. The proposed backstepping control law (54) requires the knowledge of v˙ 1 , v˙ 2 , v˙ 3 , v˙ 4 , v˙ 5 and v˙ 6 . In order to avoid analytical derivation difficulties, we estimate them by using the following finite difference time approximation: Δvi v˙ i = for i = 1, . . . , 6 (55) Δt where Δvi is the change in input value and Δt is the change in time since the previous simulation time step. The helicopter is initially in hover flight and the initial conditions are: x1 (0) = T . . . = x6 (0) = [0, 0]T and x7 (0) = mg 4 [1, 1, 1, 1] . The reference trajectory chosen for xd (t), yd (t), zd (t) and ψd (t) is that of the step response of the following transfer function: 1 H(s) = (56) (s + 1)6 with s is the Laplace variable. Figures 2 and 3 show the positions and the tracking errors of a quadrotor helicopter respectively. It can be seen the good tracking of the desired trajectory. Moreover, we can notice in figure 2 an optimization of tilt angles (pitch and roll) and consequently the use of minimal energy. The behavior of all virtual controls is given in figure 4. We note that their evolutions are smooth and derivable. The obtained input control signals (see figure 5) are acceptable and physically realizable.
3259
Fig. 2.
Fig. 5.
Position outputs
Force control inputs
V. C ONCLUSION In this paper, we have presented the dynamic modeling of quadrotor helicopter and introduced a new approach of a backstepping control. This process is under-actuated system because it has six outputs while it has only four inputs. The whole system of the quadrotor helicopter is divided into three subsystems: an under-actuated subsystem, fully-actuated subsystem and a propeller subsystem. A backstepping control algorithm is proposed to stabilize the whole system and is able to drive a quadrotor to the desired trajectory of Cartesian position and yaw angle. The simulation results show the good performance of the proposed control approach. R EFERENCES Fig. 3.
[1] O. Shakernia, Y. Ma, T. J. Koo and S. S. Sastry, ”Landing an Unmanned Air Vehicle: Vision-based Motion Estimation and Nonlinear Control”, Asian J. Control, vol. 1, no. 3, 1999. [2] A. Mokhtari, A. Benallegue and A. Belaidi, ”Polynomial Linear Quadratic Gaussian ans Sliding Mode Observer for a Quadrotor Unmanned Aerial Vehicle”, Journal of Robotics and Mechatronics, vol. 17, no. 4, pp. 483495, 2005. [3] S. Bouabdallah and R. Siegwart, ”Backstepping and Sliding-mode Techniques Applied to an Indoor Micro Quadrotor”, Proceedings of the 2005 IEEE International Conference on Robotics and Automation, pp. 22592264, 2005. [4] P. Castillo, A. Dzul and R. Lozano, ”Real-Time Stabilization and Tracking of Four-Rotor Mini Rotorcraft”, IEEE Transactions on Control Systems Technology, vo. 12, no. 4, pp. 510-516, 2004. [5] E. Altug, J. P. Ostrowski and C. J. Taylor,”Quadrotor Control using Dual Cameral Visual Feedback”, Proceedings of the 2003 IEEE International Conference on Robotics and Automation, vol. 3, pp. 4294-4299, 2003. [6] M. Vukobratovic, Applied Dynamics of Manipulation Robots: Modelling, Analysis and Examples, Berlin: Springer-Verlag, 1989. [7] S. B. V. Gomes and J. J. Jr. G.Ramas, ”Airship dynamic modelling for autonomous operation”, IEEE International Conference on Robotics and Automation, 1998. [8] H. Nijmeijer and AJ Van Der Shaft, Nonlinear Dynamic Control Systems, Springer Verlag, 1990. [9] I. Fantoni and R. Lozano, Non-linear control for underactuated mechanical systems, Springer, 2002.
Tracking errors for x, y, z and ψ
Fig. 4. Evolution of the virtual controls: solid line with the left y-axis denote the first element and the dashed line with the right y-axis denote the second element.
3260
