You are on page 1of 6

Path following control for the Cartesian

position of the Quadrotor



Jose J. Corona-Sanchez

H. Rodrguez-Cortes

Centro de Investigacion y de Estudios Avanzados del Instituto


Politecnico Nacional, Av. IPN 2508, San Pedro Zacatenco, 07360,
Mexico, D.F., Mexico (e-mail: hrodriguez@ cinvestav.mx).
Abstract: In this paper, we propose a path following controller for the Cartesian position of
the quadrotor mobile robot. The control approach is based on a combination of a backstepping
inspired controller and an output maneuvering controller, as introduced in (R. Skjetne and
Kokotovic (2004)). It is shown that the closedloop system is locally asymptotically stable
and the path following error converges asymptotically to zero when an accurate quadrotor
localization system is available, and it is shown that when the quadrotor position is not
accurately estimated (for instance, under the inuence of noise measurements) the closed
loop dynamics remains bounded. Numerical simulations are performed to show the overall
performance of the proposed scheme in both cases.
Keywords: Quadrotor, Nonlinear control, Path following.
1. INTRODUCTION
In recent years, autonomous aerial robots have demon-
strated their usefulness in dierent applications ranging
from very expensive military applications to relatively
cheap and mass produced home applications. To increase
their civilian applications, for instance in surveillance ap-
plications, their capability to plan paths and to follow
them accurately, specially when non expensive position
sensors are available, is of great importance.
Autonomous robot localization is a fundamental problem
in autonomous robot applications, the many partial solu-
tions can roughly be categorized into two groups: relative
and absolute position measurements, (J. Borenstein and
Wehe (1997)). For indoor applications both solutions, in
particular odometry, inertial navigation, active beacons
and landmark navigation, provide relatively accurate po-
sition measurements mainly because the environment is
in some sense controlled. For instance, the terrain can
be carefully selected to reduce slipping for a successful
odometry application. However, for outdoor applications
the autonomous robot localization problem has not many
successful solutions; since it is dicult to control the
environment. Some promising solutions for autonomous
robot localization have been obtained by fusing global
positioning systems (GPS), inertial measurement units
(IMU) and compass sensors (CS) signals. However, due to
the deliberated small errors in timing and satellite position
of the GPS, there is a bias error associated to the GPS,
IMU and CS based position measurement.
Current position measurement systems available in the
civil market, based on GPS, IMU and MC, have an
associated bias of approximately 5 m virtually unchanging
over a long period of time and a random error in the

This work has been partially funded by the Instituto de Ciencia y


Tecnologa del Distrito Federal under grant ICyTDF/263/2010
range of 2 3 m. A successful control strategy to perform
trajectory tracking or path following must be robust
against this position measurement errors. For instance,
in (D. R. Nelson and Beard (2007)) a successful path
following scheme for miniature air vehicles is achieved
in the presence of this kind of localization errors. Even
though, the closedloop stability analysis in the presence
of localization errors is not performed, the experimental
results show that the controller is able to handle such
localization errors.
Control strategies for trajectory tracking have the objec-
tive of driving the mobile robot at a certain point of the
trajectory at a particular time. Hence, trajectory tracking
control strategies rely on precise mobile robot localization
algorithms. The trajectory tracking error will increase
without taking into account the actual position of the mo-
bile robot. Trajectory tracking controllers for the quadro-
tor has been achieved using control schemes ranging from
PID control (G. M. Homan and Tomlin (2008)) to non-
linear designs such as feedback linearization (Mokhatari
and Benallegue (2004)), backstepping (T. Hamel T. and
Ostrowski (2002)), (S. A. Araujo-Estrada and Rodrguez-
Cortes (2009)), (N. Guenard and Mahony (2008)), nested
saturations (P. Castillo and Dzul (2005)), Lyapunov based
output feedback schemes (L. DongBin and Dawson (2007))
and nonlinear H

control (G. V. Rao and Rubio (2010)).


On the other hand, control strategies for path following
have the primary objective of driving the mobile robot to
the path unrelated to time and as a secondary objective
to satisfy an additional dynamic specication, for instance
the speed along the path.
The rest of the paper is organized as follows. In Section
2 we describe the dynamic model of the quadrotor mobile
robot and formulate the path following control problem.
Section 3 presents a nonlinear control law to solve the path
following problem and discusses the stability of the result-
ing closedloop system in the case of accurate quadrotor
mobile robot localization. In Section 4 we illustrate the
main features of the proposed control scheme through
numerical simulations. Finally, in Section 5 the paper
concludes with remarks about the proposed results and
recommendations for further research.
2. THE QUADROTOR MODEL
The Quadrotor vehicle is shown in Figure 1. This vehicle
is powered by four non-tilting rotors attached to a rigid
frame. The dynamical model of the rotary wing vehicle can
be described as follows. Let 0x
e
y
e
z
e
denote a right-hand
inertial frame (earth frame) such that z
e
points downwards
into the earth and 0x
b
y
b
z
b
a right-hand frame xed to
the center of mass of the aircraft structure (body frame).
The vehicle dynamics in the body frame is described by
(Roskam (1982))
m

V
b
+m V
b
= F
b
(1)
J

+ J = M
b
(2)
where m represents the vehicle mass, V
b
= [ u v w ]

denotes the linear velocity of the vehicle center of mass


expressed in the body axis frame, = [ p q r ]

denote
the angular velocity of the body frame, J is the vehicle
inertia matrix
1
, F
b
represents the external applied forces
expressed in the body frame and M
b
represents the exter-
nal applied moments expressed in the body frame. In order
Fig. 1. Rotary wing vehicle
to perform trajectory tracking the translational vehicle
dynamics (1) must be expressed in earth axis coordinates.
Considering the Euler yaw-pitch-roll rotation sequence.
The rotation matrix that describes the orientation of the
body reference frame relative to the earth frame is given
by
R =
_
c

+ c

+s

_
(3)
where = [ ]

are the Euler angles with the roll


angular displacement, the pitch angular displacement,
the yaw angular displacement, and c
x
= cos(x) and
1
As the vehicle has two symmetry axes J = diag{Jxx, Jyy, Jzz}.
s
x
= sin(x). Moreover, is related to the Euler angles
velocity as follows (Roskam (1982))

= W()
1
=
_

_
1 t

0 c

0
s

_ (4)
with t
x
= tan(x). From equation (3) we have that the
relationship between the velocity components in the earth
frame and the velocity components in the body frame is
dened as
V
b
= RV
e
(5)
where V
e
= [ x y z ]

is the linear velocity of the vehi-


cle center of mass expressed in the earth frame. Thus,
the translational vehicle dynamics expressed in the earth
frame is described by
m

V
e
= R

F
b
e
(6)
The external applied forces expressed in the body frame
are the vehicle weight and the total thrust produced by
the four rotors, that is,
F
b
e
=
_
mg s

mg c

mg c

_
+
_
0
0
T
T
_
(7)
where T
T
=

4
i=1
T
i
with T
i
the thrust of each rotor.
The external applied moments in the body frame are
dened as follows. Pitch motion is driven by the moment
around y
b
produced by increasing the thrust of rotor 1 and
reducing the thrust of rotor 3. Roll motion is generated in
a similar way, that is, by producing a dierential thrust
between rotors 2 and 4. Due to the torque applied to the
rotor shaft by the motors a reactive torque of the same
magnitude but opposite direction is experienced on the
structure of the vehicle. To drive yaw motion a pair of
rotors rotates in opposite direction to the other pair of
rotors. Hence, the external moments applied to airframe
can be dened as follows
M
b
e
=
_
L
M
N
_
=
_
(T
3
T
1
)
(T
2
T
4
)
Q
1
Q
2
+Q
3
Q
4
_
(8)
where is the distance between the rotor rotation axis and
the aircraft center of mass and Q
i
is the reactive moment
produced by rotor i.
It is shown in (A. Gessow(1978)) that the thrust generated
by each rotor and the reactive moment can be expressed
as
T
i
= C
Ti
r
4
i

2
i
Q
i
= C
Qi
r
5
i

2
i
where C
Ti
is the rotor i thrust coecient, is the air
density, r
i
is the radius of rotor i and
i
is the angular
velocity of rotor i. Note that
Q
i
=
C
Qi
C
Ti
r
i
T
i
thus, dening
i
= r
4
i
and assuming that all rotors have
the same characteristics we have that
_

_
T
T
L
M
N
_

_ = H
_

2
1

2
2

2
3

2
4
_

_
with
H =
_

_
C
T
C
T
C
T
C
T

C
T
0 C
T
0
0 C
T
0 C
T

C
Q

C
T
r
C
Q

C
T
r
C
Q

C
T
r
C
Q

C
T
r
_

_
It is a common practice to use brushless motors to power
the propeller. These brushless motors are commanded
by electronic speed controllers driven by pulse width
modulated signals (PWM) which are the actual control
signal. Therefore it is necessary to identify a function
relating the PWM signal with the angular velocity of
rotors.
Note that because of the selected attitude parameteriza-
tion the vehicle state vector
[ x y z x y z p q r ]

where (x, y, z) are the position coordinates in 0x


e
y
e
z
e
, is
constrained to the set
D =
_
R
3
R
3

2
,

2
_

2
,

2
_
(, ) R
3
_
in what follows we will implicitly assume that the state
remains in D for all t.
The control objective is to asymptotically track a pre-
scribed path for the vehicle Cartesian position (x, y), a
desired vehicle altitude z and a desired yaw angle .
In what follows we show that the considered control objec-
tive is achievable under the following standing assumption
A1 All system states are measurable and all system
physical parameters are known.
3. CONTROL DESIGN
This Section describes the control design procedure. We
have divided this task in three parts. First, the altitude
controller is designed, then by means of virtual control
signals the 0x
e
y
e
plane controller is developed and nally,
the attitude controller is designed to produce the necessary
virtual control signals.
3.1 Altitude control
Equation (6) can be written as follows
m x = T
T
(c

+s

)
m y = T
T
(c

)
m z = T
T
c

+mg
(9)
Dening
Z =
_
z
1
z
2
_
=
_
z z
d
z z
d
_
the third equation in (9) can be written as
z
1
= z
2
z
2
= c

T
T
m
+ g z
d
(10)
Vertical motion control can be obtained by dening
T
T
=
m(g
z
)
c

(11)
where
z
is a function of Z and time such that the dynamic
system
z
1
= z
2
z
2
=
z
z
d
(12)
is asymptotically stable.
3.2 0x
e
y
e
plane control
Replacing (11) into the rst two equations of (9) we obtain
_
x
y
_
= (g
z
)
_
c

_
_
t

_
(13)
In terms of the path following error coordinates
X
1
=
_
x
y
_
X
d
(
1
),
X
2
=
_
x
y
_

1
X
d
(
1
)
1
the dynamic equations (13) read as

X
1
= X
2

X
2
= (g
z
)
_
c

_
_
t

d
t

d
c

_
(g
z
)
_
c

_
_
t

d
t

d
c

2
1
X
d
(
1
)(v
s

2
)
2
+
1
X
d
(
1
)
2
(14)
where we have added
d
and
d
as the virtual control
inputs and taken into account that

1
= v
s

2
(15)
with v
s
the desired velocity on the path. Dening

d
= arctan
_
a
1
a
3
_

d
= arctan
_
a
2
a
3
_ (16)
where a
1
= c

(s

x
c

y
), a
2
= c

x
+s

y
, a
3
= g
z
and
_

y
_
= K
PX
X
1
+K
DX
X
2

2
1
X
d
(
1
)(v
s

2
)
2
(17)
with K
PX
and K
DX
positive denite matrices. Substitut-
ing (16) into (14) we obtain

X
1
= X
2

X
2
= K
PX
X
1
K
DX
X
2
a
3
_
c

_
_
t

d
t

d
c

_
+
1
X
d
(
1
)
2
(18)
Notice that a corner stone in the controller design for the
0x
e
y
e
plane dynamics is the fact that the signal (g
z
)
needs to be bounded away from zero. To achieve this we
propose

z
= z
d


z
2
_
tanh
_
2
z1

z
z
1
_
+
1
2
tanh
_
2
z2

z
z
2
__
which as stated in (Kaliora and Astol (2004)) achieves
local exponential stability of (12), for adequately selected
positive constants
z1
,
z2
and
z
, and bounds away from
zero the term (g
z
), provided | z
d
| is bounded, since
|
z
| | z
d
| +
3
4

z
=
z
Note that the closed-loop dynamics (18)-(17) can be
written as folllows

X = AX a
3
G+F
2
(19)
where
X =
_
X
1
X
2
_
, G =
_
0
2
c

__
t

d
t

d
c

_
A =
_
0
2
I
2
K
PX
K
DX
_
, F =
_
0
2

1
X
d
(
1
)
_
with 0
2
and I
2
the zero matrix and the identity matrix of
dimension 2 2.
Consider now the Lyapunov function
V
X
= X

PX +
1
2

2
2
, > 0
whose time derivative along the trajectories of (19) is given
by

V
X
= X

(PA +A

P)X 2a
3
X

PG
+(2X

P F +
2
)
2
dening

2
= (2X

P F +
2
) (20)
and selecting the gains K
PX
and K
DX
in such a way that
A is Hurtwitz we obtain

V
X
= X

QX 2a
3
X

PG
(X

P F +
2
)
2
for positive denite matrices P and Q.
At this point it is not possible to conclude about the
stability properties of the vehicles Cartesian dynamics
because of the term G. In the next section the attitude
dynamics will be commanded to track the angles dened in
(16). This will allow us to establish the stability properties
of the vehicles dynamics.
3.3 Attitude control
It is well known that there exists a passive map between
the control inputs and the rigid body angular velocity
in the attitude dynamics. This fact has been exploited
to design passivity based schemes to control the attitude
dynamics (Lizarralde and Wen (1996)) even with bounded
controllers (J. F. Guerrero-Castellanos and Lesecq (2012)).
Here we will also exploit the passivity properties of the
attitude dynamics.
Dene the following error signals

=
d

=
d
(21)
Expressing the attitude kinematics (4) and attitude dy-
namics (2) in terms of the error signals (21) one has

= W()
1

+W()
1

d
J

= S(J)

+S(J

)
d
+

M
b
(22)
where we have dened S() as the matrix that performs
the vectors cross product, that is, d
1
d
2
= S(d
1
)d
2
, we
have considered that the following equation holds
J

d
= S(J
d
)
d
+M
b
d
(23)
and nally we have dened

M
b
= M
b
M
b
d
(24)
Given (16) we have

d
=
1
(X, , Z, ) +
2
(X, Z, , )W()
1

with
= [
1

2
]

1
=
_

_
c

[a
3
(s


x
c


y
) +a
1

z
]
a
2
3
+a
2
1
a
3
(c


x
+s


y
) +a
2

z
a
2
3
+a
2
2

d
_

2
=
1
a
_
0 s

(s

x
+c

y
) c

(c

x
s

y
)
0 0 s

x
c

y
0 0 0
_
and
a = a
3
_
1 +
c
2

(s

x
+c

y
)
2
a
2
3
_
We dene the attitude dynamics controller as follows

d
= W()
_
K
P

+
1
+
2
W()
1

M
b
= S(J

)
d
K
D

W()
T

e
(25)
with K
P
and K
D
positive denite matrices.
Observe that to implement the attitude controller dened
in (25) it is necessary to compute

d
in order to obtain M
b
d
from (23). However, since
d
and
d
depend on the pitch
and yaw angles, and these angles have a relative degree
equal 2, the rst time derivative of
d
has the following
form

d
=
3
(, , X, , Z)
+W()
2
W()
1
J
1
M
b
therefore, M
b
e
is computed from (24) as follows
M
b
e
=
1
4
_
S(J
d
)
d
+J
3
S(J

)
d
K
D

_ (26)
with I the 3 3 identity matrix and

4
=
_
I +JW
2
W
1
J
1

It is easy to verify that


det (
4
) = 1 (27)
The properties of the proposed controller can be stated as
follows:
Proposition 1. Under assumption A1. Consider the rotary
wing vehicle dynamics described by equations (1)-(2) in
closedloop with the controller dened by equations (11),
(17), (20) and (26). Assume that the desired path X
d
(
1
)
is bounded and has bounded derivatives with respect to
1
.
Then, there exists positive constants
z1
,
z2
, and
z
and
positive denite matrices K
PX
, K
DX
, K
P
and K
D
such
that the closedloop dynamics is locally asymptotically
stable in D.
Proof. The closedloop dynamics (1)-(2)-(11)- (17)-(20)-
(26) expressed in terms of the error coordinates reads as

X = AX a
3
GF(2X

P F +
2
)

1
=
2

2
= (2X

P F +
2
)
z
1
= z
2
z
2
=

z
2
_
tanh
_
2
z1

z
z
1
_
+
1
2
tanh
_
2
z2

z
z
2
__
_

_
=
_
K
P
W()
1
W()
T
S(J) K
D
_ _

_
(28)
From (Kaliora and Astol (2004)) we know that the ver-
tical closed-loop dynamics, the fourth and fth equations
in (28) are locally exponentially stable, thus there exists a
Lyapunov function
V
z
=
1
2
Z

P
z
Z
such that locally

V
z
= Z

Z
holds. Consider now, the Lyapunov function
V = V
X
+
1
2

+
1
2

+V
z
whose time derivative along the trajectories of (28) is given
by

V = X

QX 2a
3
X

PG(X

P F +
2
)
2
Z

K
P


K
D

Considering that
|G| |

| (29)
the time derivative of the Lyapunov function can be upper
bounded as

V
min
(Q)|X|
2
+ 2
z

max
(P)|X||

min
(K
P
)|

|
2

min
(K
D
)|

|
2
|X

P F +
2
|
2
|Z|
2
From the Youngs inequality we have

V
_

min
(Q)
z

max
(P)

|X|
2

min
(K
P
)
1

max
(P)
z
_
|

|
2

min
(K
D
)|

|
2
|X

P F +
2
|
2
|Z|
2
for positive constant > 0. Hence, by selecting the gains
in such a way that

min
(Q)
max
(P)
z
> 0

min
(K
P
)
1

max
(P)
z
> 0
(30)
1.5 1 0.5 0 0.5 1
1.5
1
0.5
0
0.5
1
x [m]
y
[
m
]
Fig. 2. Path following. Desired path (dashed line), path
followed (continuous line).
hold, the proof is completed.
4. NUMERICAL SIMULATIONS
Numerical simulations were carried out to asses the per-
formance of the controller proposed. The numerical value
of the vehicle parameters are presented in Table 1.
Parameter Value Parameter Value
m (kg) 1.120 C
Q
0.1
g (m/s
2
) 9.81 (m) 0.214
r (m) 0.127 I
xx
(kg m
2
) 0.002
(kg/m
3
) 1 I
yy
(kg m
2
) 0.002
C
T
0.5 I
zz
(kg m
2
) 0.004
Table 1. Rotary wing vehicle parameters
The desired trajectory is dened as follows
x
d
= sin(
1
)
y
d
= cos(
1
)
z
d
= 2 + sin(t)

d
= 0
(31)
We consider the following initial conditions (x, y, z, , , ) =
(0, 0, 0, 0, 0, 0). The controller parameters are summarized
in Table 2.
Parameter Value
50

z
10
K
PX
diag{1,1.5}
K
DX
diag{0.85,0.75}
K
P
diag{1,1.5,0.5}
K
D
diag{0.85,0.75,0.4}
Table 2. Controller parameters
The path error signals are shown in Figure 2 The time
history of the control signals is shown in Figure 3.
We consider now the case of non zero localization errors.
We consider the following model for the position error
measurement

P
(t) =
1
(32)
where
1
represents the random error. Figure 4 shows the
time histories of the path following errors as it can be
observed the proposed controller is not able to drive it to
zero, however the path errors have a bounded behavior.
Figure 5 shows the time histories of the control inputs.
0 2 4 6 8 10 12 14 16 18 20
2000
3000
4000
5000
6000
7000
8000
9000
10000
11000
t [sec]

i 2
, i =
1
, 2
, 3
, 4
[
r
a
d
/s
e
c
]

Fig. 3. Control signals.
2.5 2 1.5 1 0.5 0 0.5 1 1.5 2
1.5
1
0.5
0
0.5
1
x [m]
y
[
m
]
Fig. 4. Path following. Desired path (dashed line), path
followed (continuous line).
0 2 4 6 8 10 12 14 16 18 20
2000
3000
4000
5000
6000
7000
8000
9000
10000
11000
t [sec]

i 2
, i =
1
, 2
, 3
, 4
[
r
a
d
/s
e
c
] 2
Fig. 5. Control signals.
5. CONCLUSION
The problem of path following for the quadrotor mobile
robot has been addressed and solved by means of a nonlin-
ear controller based on a backstepping inspired controller
and an output maneuvering controller, as introduced in
(R. Skjetne and Kokotovic (2004)). We have analyzed the
proposed controller in two scenarios, the rst one assumes
that the quadrotor mobile robot Cartesian position is
accurately measured or estimated and the second case
considers that there is an error in the quadrotor Cartesian
localization system. As expected the performance of the
proposed controller is degraded due to the errors in the
localization system however it keeps all signals bounded.
Numerical simulations have been used to illustrate the
properties of the closedloop system.
Some issues are left open in this work. First, the analysis of
the stability in the case that the localization systems is not
accurate. Second, the experimental test of the proposed
controller and the inclusion of wind.
REFERENCES
A. Gessow, C.G.J.M. (1978). Aerodynamics of the He-
licopter. FREDERICK UNGAR PUBLISHING CO.,
New York, USA.
D. R. Nelson, D. B. Barber, T.W.M. and Beard, R.W.
(2007). Vector eld path following for miniature air
vehicles. IEEE Trans. on Robotics, 23, 519529.
G. M. Homan, S.L.W. and Tomlin, C. (2008). Quadrotor
helicopter trajectory tracking control. In Proceedings of
the AIAA Guidance, Navigation, and Control Confer-
ence. Honolulu, Hawaii USA.
G. V. Rao, M.G.O. and Rubio, F.R. (2010). An integral
predictive/nonlinear

control structure for a quadro-


tor helicopter. Automatica, 46, 2939.
J. Borenstein, H. R. Everett, L.F. and Wehe, D. (1997).
Mobile robot positioning- sensors and techniques. Jour-
nal of Robotic Systems, Special Issue on Mobile Robots,
14, 231249.
J. F. Guerrero-Castellanos, N. Marchand, A.H. and
Lesecq, A. (2012). Bounded attitude control of rigid
bodies: Real-time experimentation to a quadrotor mini-
helicopter. Control Engineering Practice, to appear.
Kaliora, G. and Astol, A. (2004). Nonlinear control of
feedforward systems with bounded signals. IEEE Trans.
on Automatic Control, 49, 19751990.
L. DongBin, T. C. Burg, B.X. and Dawson, M.D. (2007).
Output feedback tracking control of an underactuated
quad-rotor uav. In Proceedings of the American Control
Conference. New York, USA.
Lizarralde, F. and Wen, J.T. (1996). Attitude control
without angular velocity measurement: A passivity ap-
proach. IEEE Trans. on Automatic Control, 41, 1519
1529.
Mokhatari, A. and Benallegue, A. (2004). Dynamic feed-
back controller of euler angles and wind parameters
estimation for a quad-rotor unmanned aerial vehicle. In
Proceedings of the IEEE International Conference on
Robotics and Automation, 23592366.
N. Guenard, T.H. and Mahony, R. (2008). A practical
visual servo control for an unmanned aerial vehicle.
IEEE Transactions on Robotics, 24(2), 331340.
P. Castillo, R.L. and Dzul, A.E. (2005). Stabilization of a
mini rotorcraft with four rotors. IEEE Control Systems
Magazine, 4555.
R. Skjetne, T.I.F. and Kokotovic, P. (2004). Robust
output maneuvering for a class of nonlinear systems.
Automatica, 40, 373383.
Roskam, J. (1982). Airplane Flight Dynamics and Auto-
matic Flight Controls, Part I. Roskam Aviation and
Engineering Corporation, USA.
S. A. Araujo-Estrada, E.L.C. and Rodrguez-Cortes, H.
(2009). Nonlinear motion control of a rotary wing
vehicle powered by four rotors. Revista Ingeniera,
Investigacion y Tecnologa, X(4), 373383.
T. Hamel T., R. Mahony R., R.L.R. and Ostrowski, J.
(2002). Dynamic modelling and conguration stabiliza-
tion for an x4-yer. In Proceedings of the 15th Triennial
World IFAC Congress. Barcelona, Spain.