The Trajectory Tracking Problem For Quadrotor System: Na Zhang and Qing-He Wu

Download as pdf or txt
Download as pdf or txt
You are on page 1of 6

16TH INTERNATIONAL CONFERENCE ON MECHATRONICS TECHNOLOGY, OCTOBER 16-19, 2012, TIANJIN, CHINA

The Trajectory Tracking Problem for Quadrotor System



Na Zhang
*
and Qing-He Wu
Automation Department, Beijing Institute of Technology, Beijing, China
*Corresponding: [email protected]

AbstractThis paper presents the design of a nonlinear controller
based on integral backstepping control technique combined with
the super twisting control algorithm for trajectory tracking of a
quadrotor helicopter. A simple version of dynamical system based
on Newton-Euler formalism is used. The performance and
effectiveness of the proposed controller are tested in a simulation
study taking into account external disturbances.
Keywordsquadrotor system, trajectory tracking, integral
backstepping, supertwist algorithm
1 INTRODUCTION
Over the last decade, autonomous unmanned aerial
vehicles (UAV) have gained considerable attention among
researchers. Recent advances in sensor technology, signal
processing and integrated actuators have taken the
development of small UAVs one step further. Quadrotor, as a
popular type of UAV, is a kind of Vertical Take-Off and
Landing (VTOL) vehicle which could be launched from any
place and hover above any target. In comparison with classical
helicopters conventionally equipped with a main rotor and a
tail rotor, quadrotors possess a larger payload capacity and
higher versatility and maneuverability to carry out various
types of tasks such as rescue, near-area surveillance and
exploration. However the dynamics of the quadrotor are
nonlinear, multivariable and under actuated with four control
inputs and six degrees of freedom (DOF). Besides, variables
are strongly coupled with each other. All these characteristics
make the control of quadrotors increasingly challenging. A
great number of efforts have been made by researchers in
designing the control methodologies that will make an
unmanned aerial autonomous. Popular control methods include
input-output feedback linearization [11,12], backstepping
[1,10], sliding mode control [1,4,7,10] and so on.
The quadrotor is a kind of rotorcraft, whose mechatronic
system consists of four rotors in a cross configuration. Rotors
are paired to spin in opposite directions in order to cancel out
gyroscopic effects and aerodynamic torques in trimmed flight.
The motion of a quadrotor can be divided into two parts:
translational subsystem and rotational subsystem. As a
difference of speed is applied between opposing rotors, the
helicopter tilts towards the slowest one, which results in
acceleration along that direction. In other words, the rotational
and translational motions are closely related.
The main work of this paper aims to use nonlinear control
techniques to perform the stabilization and tracking control of
a quadrotor. The controller for translational subsystem is
designed to stabilize the altitude and gives the desired roll and
pitch angles to the rotational subsystem controller in the
meanwhile.


Fig. 1: The quadrotor system

2 QUADROTOR MODEL
Generally speaking, the control strategies are based on
simplified models which have both a minimum number of
states and a minimum number of inputs. These reduced models
should retain the main features that must be considered when
designing control laws for real aerial vehicles [5].
The quadrotor model consists of four rotors powered by
electric motors and mounted at each end of an X-shaped frame
as shown in Fig. 1. The collective input is the sum of the
thrusts of each motor. Increasing or decreasing the speeds of
four motors together generates the vertical motion. Pitch
movement is obtained by increasing (reducing) the speed of
the rear motor and reducing (increasing) the speed of the front
motor. The roll movement is generated using the lateral motors
in the similar way. Yaw motion results from the difference in
the counter-torque between each pair of propellers.
Let E denote an inertial frame, and B be a body fixed
frame shown in Fig. 2. The generalized coordinates of the
quadrotor can be described as q=(x, y, z, , , ), where (x, y, z)
denote the position of the center of gravity of the vehicle in
frame E while three Euler angles (, , ) (roll, pitch and yaw
angles) represent its angular velocity in frame B and give the
orientation of the quadrotor. Therefore, the model is naturally
divided into translational coordinates =(x, y, z)
T
and rotational
coordinates =(, , )
T
.
221





Fig. 2: Coordinate system

The transformation of vectors from the body fixed frame
to the inertial frame is given by the rotation matrix R:
|
|
|
.
|

\
|

+
+
=
u u u
u u u u
u u u
c c s c s
s c c s s c c s s s c s
s s c s c c s s s c c c
R

where s() and c() denote sin() and cos(), respectively.
According to the formalism of Newton-Euler, the dynamic
equations are written in the following form:

+ =
=
=
=
-
-
- -
-
ext
ext
T J J
RS R
F m
V
e e e
e

) (

Where V=(u, v, w)
T
is the absolute velocity of the vehicle
expressed with respect to the inertial frame, m is the mass of
the structure, =(p, q, r)
T
is the angular velocity expressed in
B, the relationship between and Euler angles is as follows:
(
(
(
(
(

|
|
.
|

\
|

=
-
-
-

u
u
u
e
cos cos sin 0
sin cos cos 0
sin 0 1
(1)
S() is a skew-symmetric matrix for a given vector =(
1
,

2
,
3
)
T
, which is defined as:

.
0
0
0
) (
1 2
1 3
2 3
|
|
|
.
|

\
|

=
e e
e e
e e
e S

(2)
J is the inertia matrix given by

.
0 0
0 0
0 0
(
(
(

=
z
y
x
I
I
I
J
(3)
The coupling inertia is assumed to be zero because of the
symmetry of the geometric form of the quadrotor. The
notations F
ext
and T
ext
stand for the vector of external forces
and that of external torque, respectively.
Based on the force analysis of the system, we obtain
, 0
0
0
0
(
(
(

(
(
(

=
mg T
R F
ext

|
|
|
.
|

\
|
O O O + O
O O
O O
=
|
|
|
.
|

\
|
=
) (
) (
) (
2
3
2
1
2
4
2
2
2
1
2
3
2
2
2
4
d
lb
lb
T
ext

t
t
t

T is the total thrust produced by all four rotors, g is the
gravity constant (g = 9.81m/s
2
),
i
denotes the rotational
velocity of rotor i. l represents the distance from the rotors to
the center of gravity of the vehicle. b>0 is thrust factor and
d>0 is the drag factor. The thrust generated by rotor i is given
by T
i
=b
i
2
. So the total thrust can be described as
) (
2
4
2
3
2
2
2
1
O + O + O + O = b T
.
Consequently, complete dynamic model of the quadrotor is
as follows:


u u

t
t
t
u
u
u
A
I I
I I
pq r
A
I
l
I
I I
pr q
A
I
l
I
I I
qr p
A
m
T
g w
A
m
T
v
A
m
T
u
z z
y x
y y
x z
x x
z y
z
y
x
+ +

=
+ +

=
+ +

=
+ + =
+ =
+ + =
-
-
-
-
-
-
1
) (
) (
) (
) cos (cos
) cos sin sin sin (cos
) sin sin cos sin (cos
(4)




B
x
z
y
mg
T4
T3
T1
T2
X
Z
Y
E
222



where (A
x
, A
y
, A
z
, A

, A

, A

) represent disturbances in 6
degrees of freedom.
From the equation above, we can observe that the tilt
angles and the motion of the helicopter are closely related.
That is, the six degrees of freedom are strongly coupled with
each other and there are only four independent variables in
dynamic model equation, which makes trajectory tracking
control possible.
3 STATEMENT OF THE CONTROL PROBLEM
The control problem considered in this paper is to make
the system position variables x, y, z and the orientation
variable asymptotically track the desired references (the yaw
angle is set to be maintained at zero during flight) by designing
control in terms of torque commands to the propeller under the
assumption that orientation variables and always stay
inside the open set (-/2, /2) and T > 0.
4 CONTROL STRATEGY
The dynamic model of the quadrotor helicopter has six
dependent outputs while it has four independent inputs, so we
cannot control all the states at the same time. This problem can
be solved by backstepping method, which is a good way to
deal with underactuation problem. As a matter of fact, the
backstepping is well suited for the cascaded structure of the
quadrotor dynamics.
The overall control system has two loops and is depicted
in Fig. 3. The external inputs to the system are assumed to be
the desired position
d
and the desired yaw
d
. According to
the difference between the current quadrotor position and the
desired position, the position controller generates the desired

d
and
d
to ensure
d
. Subsequently, Euler angular
controller uses the information provided by position controller
to calculate the desired angular velocity
d
which ensures

d
. Then, the angular velocity controller calculates the
actual control inputs (T,

) which ensures
d
when
they are applied to the UAV system. The output vector is then
fed back into the controllers.
4.1 Translational subsystem controller design
The translational subsystem of the quadrotor is
represented by (4) and can be rewritten as follows:

+ =
=
+ =
=
=
=
-
-
-
-
-
-
m
T
g w
m
T
v
m
T
u
w z
v y
u x
) cos (cos
) cos sin sin sin (cos
) sin sin cos sin (cos
u
u
u
(5)
Now, we define a virtual control u
1
as:
.
) (
) (
) (
3 , 1
2 , 1
1 , 1
1
|
|
|
.
|

\
|
=
(
(
(
(
(
(

+
=
u
u
u
m
T
c c g
m
T
c s s s c
m
T
s s c s c
u
u
u
u

(6)
Then the original subsystem is transformed into a simpler
second order linear system. Backstepping is a recursive
procedure that interlaces the choice of a Lyapunov function
with the feedback control. It breaks a design problem for the
full system into a sequence of design problems for lower order
subsystems. By exploiting the extra flexibility that exists with
lower order subsystems, backstepping can often solve
stabilization, tracking, and robust control problems.
Considering the uncertainties and disturbances of quadrotor
model, we choose the IB (integral backstepping) to perform
the tracking command.
Reference
trajectory

Quadrotor
dynamic
Position
Controller
Euler
Angular
Controller
Angular
Velocity
Controller
d

d d
| u .,
|
|
|
.
|

\
|
d
d
d
r
q
p
|
|
|
.
|

\
|
d
d
d
z
y
x
|
|
|
.
|

\
|

u
|
t
t
t
T
|
|
|
.
|

\
|
z
y
x
|
|
|
.
|

\
|

u
|
|
|
|
.
|

\
|
r
q
p

Fig. 3: The overall control strategy of the quadrotor system

223



Firstly, we introduce the desired trajectory
d
=(x
d
, y
d
, z
d
)
T

for the position variable and define the tracking error as
.
1
=
d
e (7)
Using (5) and (7) we obtain its dynamics:
.
1
V
dt
de
d
=
-
(8)
We consider V as an input of the upper dynamics. To
stabilize the dynamics, the virtual control input V is chosen as:
, ) (
0
1 0 1 1
}
+ + =
-
t
a a d d
d e K e K V t t (9)
where K
a1
and K
a0
are diagonal matrices with positive
constants.
Then the velocity tracking error is defined by
V V e
d
=
2

It results in:
.
1 0 1 1 2
- - - - -
+ + = V e K e K e
a a d
(10)
So we rewrite the tracing error dynamics of e
1
:

2
0
0 1 1
1
) ( e d e K e K
dt
de
t
a a
+ =
}
t t (11)
After we apply (5), (6) and (11) to (10), the stabilization of
e
2
can be obtained by selecting the input u1 as
. ) (
) ( ) ( 1
0
0 1
2 1 1 1 0
2
1
t t

d e K K
e K K e K K I u
t
a a
a b a a d
}

+ + + + =
- -

The control input of the position subsystem is a function of
Euler angles and the total thrust T. As a result, we need to
invert u1 to compute the thrust and the Euler angle commands
that will be used as reference for the following subsystem.
As for (6), by squaring left and right sides of all three
equations in (6) and adding them together, we get the thrust T;
by subtracting the second equation multiplied by cos from
the first equation multiplied by sin, we get the desired pitch
angle d; the desired roll angle can be easily competed from
the first equation. Then the overall inversion is presented as:
(note that the yaw angle
d
is set to be maintained at zero
during flight)
.
)
cos cos
sin sin
arcsin(
)) cos sin ( sin(
) (
1 , 1
2 , 1 1 , 1
2
3 , 1
2
2 , 1
2
1 , 1

=
=
+ + + =
d d
d d
d
d d d
u
T
m
u u
T
m
ar
u g u u
m
T


u

(12)
4.2 Rotational subsystem controller design
The rotational subsystem of the quadrotor is denoted by (4)
and (1) which can be rewritten as:
|
|
|
.
|

\
|
|
|
|
|
|
.
|

\
|
=
|
|
|
|
|
.
|

\
|
-
-
-
r
q
p
c
c
c
s
s c
t c t s
u


u u

u
|
0
0
1
, e q M =
-
(13)
|
|
|
.
|

\
|
|
|
|
|
|
|
|
.
|

\
|
+
|
|
|
|
|
|
|
|
.
|

\
|

=
|
|
|
|
|
.
|

\
|
-
-
-

t
t
t
z
y
x
z
y x
y
x z
x
z y
I
I
l
I
l
I
I I
pq
I
I I
pr
I
I I
qr
r
q
p
1
) (
) (
) (


. ) ( EU f + =
-
e e

(14)
where t() denotes tan().
Attitude control is the heart of the control system; it keeps
the 3D orientation of the quadrotor to the desired value, hence,
prevents the vehicle from flipping over and crashing. As we
know, sliding mode controller is widely used due to its
attractive characteristics of finite-time convergence and
robustness to uncertainties, and super-twisting algorithm is an
important class of second order sliding modes. It ensures
robustness with respect to modeling errors and external
disturbances while reducing the chattering phenomenon caused
by all first order sliding mode controllers. [8] In this section,
were going to combine the super-twisting algorithm and
backstepping technique, which turns out to get a satisfactory
result in attitude stabilization.
In the previous section, desired Euler angles have been
calculated to act as the reference input of the rotational
subsystem. We set

q q =
d
z
1
,
then its first derivative becomes
.
1
e q q q M z
d d
= =
- - - -
(15)
To stabilize the above dynamics, the virtual control input
d

could be:

). ) ( (
0
1 0 1 1
1
t t q e d z K z K M
t
c c d d
}
+ + =
-

(16)
Again we define
) (
2
e e =
d
M z
,
and use (16) and (14). It yields
). ) ( ( ) (
1 0 1 1 2
EU f M z K z K z
c c d
+ + + =
- - - -
e q (17)
Then the dynamics of z
1
becomes:
. ) (
2
0
1 0 1 1 1
z d z K z K z
t
c c
+ =
}
-
t t (18)
We choose the sliding variable as s
1
= z
2
. Now we design
the control inputs applying the super twisting control algorithm
224



) (
) (
1 2
2 1
2
1
1 1
s sign s
s s sign s s
o

=
+ =
-
-
(19)
with control gains > 0 and > 0. The proof of finite time
convergence to zero of the variable s
1
and s
2
is explicitly given
in [6].
As a result, control input U should be designed as
) ( ) ) (
) (
) ( ( ) (
1
2 2
2
1
2
2 0
0
1
0 1 1
2
1 0
1
e
t t
q
f E s z sign z
z K
d z
K K z K K ME U
c
t
c c c c d

- -

+
+
+ =
}

5 SIMULATION RESULTS
In order to verify the performance of the proposed
controller, simulations with and without external disturbances
were made. As already noted, the controllers task is to make
the system position variables x, y, z asymptotically track the
desired references (the yaw angle is set to be maintained at
zero during flight). As we know, sinusoidal signal is one of the
typical input signals in control systems, and especially gives us
more challenges in tracking control problems. Therefore we
set the desired position trajectory as x = cos(t), y = sin(t) and z
= t. Initially, the quadrotor is placed at the origin and in a
horizontal position, that is, (x, y, z) = (0, 0, 0) and (, , ) =
(0, 0, 0), keeping constant.
5.1 Position tracking without external disturbances
Firstly we consider the situation of position tracking without
external disturbances. Fig. 4 shows the tracking of desired
trajectory, which clearly reflects the effectiveness of the
controller. Fig. 5 and Fig. 6 give a more distinct process of
tracking in 3 directions.

-1
-0.5
0
0.5
1
1.5
-1
0
1
2
0
5
10
15
20


refernce
real trajectory

Fig. 4: Position trajectory of the quadrotor without external
disturbances


Fig. 5: Position (x, y, z) tracking of the quadrotor without external
disturbances



Fig. 6: Orientation (, ) of the quadrotor without external
disturbances

5.2 Position tracking with external disturbances
The disturbances in position subsystem are A
x
= 0.02, A
y
=
0.02, A
z
= 0.02. Moreover, the moment disturbances in
rotational subsystem are A

= 0.1sin(0.1t), A

= 0.1cos(0.1t),
A

= 0.1sin(0.1t). The obtained results are depicted in Fig. 7,


Fig. 8 and Fig. 9. Its concluded from the simulation that the
good performance of the proposed control approach is
maintained.
-1
-0.5
0
0.5
1
1.5
-1
0
1
2
0
5
10
15
20

reference
actual trajectory

Fig. 7: Position trajectory of the quadrotor with external
disturbances


Fig. 8: Position (x, y, z) tracking of the quadrotor with external
disturbances

Fig. 9: Orientation (, ) of the quadrotor with external
disturbances
0 2 4 6 8 10 12 14 16 18 20
-0.2
-0.15
-0.1
-0.05
0
0.05
0.1
0.15


theta
0 2 4 6 8 10 12 14 16 18 20
-0.2
-0.15
-0.1
-0.05
0
0.05
0.1
0.15


phi
0 2 4 6 8 10 12 14 16 18 20
-1
-0.5
0
0.5
1
1.5


refernce
y
0 2 4 6 8 10 12 14 16 18 20
-1
-0.5
0
0.5
1
1.5


refernce
x
0 2 4 6 8 10 12 14 16 18 20
0
2
4
6
8
10
12
14
16
18
20


refernce
z
0 2 4 6 8 10 12 14 16 18 20
-0.2
-0.15
-0.1
-0.05
0
0.05
0.1
0.15


phi
0 2 4 6 8 10 12 14 16 18 20
-0.2
-0.15
-0.1
-0.05
0
0.05
0.1
0.15


theta
0 2 4 6 8 10 12 14 16 18 20
-1
-0.5
0
0.5
1
1.5


refernce
x
0 2 4 6 8 10 12 14 16 18 20
0
2
4
6
8
10
12
14
16
18
20


refernce
z
0 2 4 6 8 10 12 14 16 18 20
-1
-0.5
0
0.5
1
1.5


refernce
y
225



Although the simulation results under both situations are
satisfactory, we cannot achieve perfect tracking of sinusoidal
signal with zero steady-state error according to internal model
control theory. But perfect tracking of step signal can be
implemented by proposed algorithm, which has been proved
by simulations. Due to the limit of space, we wont show the
results here.
6 CONCLUSIONS
Integral backstepping control technique combined with the
super twisting control algorithm has been used to stabilize and
perform output tracking control on a simple version of
dynamical system. The super twisting control algorithm is
applied so as to improve robustness to external disturbances
and model uncertainties. Simulations performed on Matlab
Simulink show the ability of the controller to perform output
tracking control even external disturbances and model
uncertainties exist.
REFERENCES
[1] S. Bouabdallah, R. Riegwart, in: Proceedings of the 2005 IEEE
International Conference on Robotics and Automation,
Barcelona, Spain, 2005, pp. 22472252.
[2] A. Tayebi, S. McGilvray, Attitude stabilization of a VTOL
quadrotor aircraft, IEEE Transactions on Control Systems
Technology, vol. 14, no. 3, pp. 562-571, 2006.
[3] S. Bouabdallah, P. Murrieri and R. Riegwart, in: Proceedings of
the IEEE International Conference on Robotics and Automation,
New Orleans, US, 2004, vol. 5, pp. 4393-4398.
[4] A. Benallegue, A. Mokhtari and L. Fridman, High-order
sliding-mode observer for a quadrotor UAV, International
Journal of Robust and Nonlinear Control, vol. 18, no. 4, pp.
427-440, 2008.
[5] P. Castillo, A. Dzul and R. Lozano, Real-time stabilization and
tracking of a four rotor mini-rotorcraft, IEEE Transactions on
Control Systems Technology, vol. 12, no. 4, pp. 510-516, 2004.
[6] J. A. Moreno, M. Osorio, in: Proceedings of the 47th IEEE
Conference on Decision and Control, Cancun, Mexico, 2008, pp.
2856-2861.
[7] L. Luque-Vega, B. C. Toledo, A. G. Loukianov, Robust block
second order sliding mode control for quadrotor, Journal of the
Franklin Institute, vol. 349, no. 2, pp. 719-739, 2012.
[8] L. Derafa, A. Benallegue, L. Fridman, Super twisting control
algorithm for the attitude tracking of a four rotors UAV,
Journal of the Franklin Institute, vol. 349, no. 2, pp. 685-699,
2012.
[9] T. Dierks and S. Jagannathan, Output feedback control of a
quadrotor UAV using neural networks, IEEE Transaction on
Neural Networks, vol. 21, no. 1, pp. 50-66, 2010.
[10] H. Bouadi, M. Bouchoucha and M. Tadjine, Sliding mode
control based on backstepping approach for a UAV type-
quadrotor, International Journal of Applied Mathematics and
Computer Science, vol. 4, no. 1, pp. 22-27, 2007.
[11] A. Das, K. Subbarao and F. Lewis, Dynamic inversion with
zero-dynamics stabilization for quadrotor control, IET Control
Theory and Applications, vol. 3, no. 3, pp. 303-314, 2009.
[12] H. Voos, in: Proceedings of the 2009 IEEE International
Conference on Mechatronics, Malaga, Spain, 2009, pp. 1-6.
226

You might also like