Nonlinear Control for Trajectory Tracking of a

Quadrotor Unmanned Vehicle

Manuel Jesús Rodrı́guez-Rodrı́guez and Hugo Rodrı́guez-Cortés
Departamento de Ingenierı́a Eléctrica. CINVESTAV-IPN
Av. Instituto Politécnico Nacional 2508. 07300 México D.F., México
E-mail:{mrodriguez,hrodriguez}@cinvestav.mx

Abstract— This paper presents a new solution to
the motion control problem for a rotary wing vehicle
powered by four rotors. The proposed control struc-
ture is a parallel scheme consisting of a partial state
saturated feedback to control the vehicle altitude and
a partial feedback linearizing feedback to control the
planar vehicle’s position. It is shown that the resulting
closed–loop dynamics has a local asymptotic stability
property. Numerical simulations show the performance
of the proposed controller.
Keywords: Nonlinear control, bounded control, un-
manned aerial vehicle, rotary wing vehicle, trajectory
tracking
I. INTRODUCTION
In recent years, the development of unmanned aerial
vehicles (UAV) has had significant growth, this is due
to several factors: the development of new technologies
in sensors and actuators as well as the miniaturization
of computers have enabled the development of more so-
phisticated control techniques that help improve vehicle
performance.
The goal of this paper is to address and solve the
trajectory tracking problem. The proposed control struc-
ture is a parallel scheme consisting of a partial state
saturated feedback to control the vehicle altitude and a
partial feedback linearizing feedback to control the planar
vehicle’s position.
The paper is organized as follows. In Section II the
rotor-craft dynamical model and a precise definition of
the control problem of interest are defined. Section III is
devoted to the design of the proposed controller. In Section
IV the effectiveness of the control design is shown through
a series of numerical simulations. Finally, in Section V
some concluding remarks are presented.
II. The Model
XB
YB

Another factor driving the development of UAVs is the

foray into civilization and military tasks such as traffic
ZB
monitoring, recognition and surveillance vehicles. Law
enforcement organizations use UAVs for hostage rescue, YE
XE
border patrol, traffic surveillance and riot control [1].
Existing UAVs can be classified in two classes: rotary
wing vehicles and fixed wing vehicles. For missions requir- ZE
ing the vehicle to remain stationary (hover) or to maneu-
ver in tightly constrained environments rotary wing vehi-
cles have significant advantages over fixed wing vehicles.
For example, traffic surveillance around buildings requires
a hovering vehicle with good maneuverable characteristics. Fig. 1: Rotary wing vehicle
It is important to point out that hover flight consumes
approximately twice the power of a similarly loaded fixed The rotary wing vehicle is shown in Figure 1. This
wing vehicle moving forward. However, it is expected that vehicle is powered by four non-tilting rotors attached to
new power technologies will allow achieving reasonable a rigid frame. The dynamical model of the rotary wing
endurance for rotary wing vehicles. vehicle considered can be obtained as follows. Let 0xe y e z e
In this paper a rotorcraft powered by four non-tilting denote a right-hand inertial frame (earth frame) such that
rotors known as the X4-flier [3] or the Dragan-flyer is z e points downwards into the earth and 0xb y b z b a right-
considered. Besides its practical relevance, this system is hand frame fixed to the centre of mass of the aircraft
an interesting case of study. E. g. it is a six degrees of structure (body frame). The vehicle dynamics in the body
freedom mechanical system whose dynamics is described frame is described by [5]
by an under-actuated twelfth order highly coupled nonlin-
b b
ear model. m V̇CM + m Ω × VCM = Feb (1)
 J Ω̇ + Ω × J Ω = Meb (2) The external applied moments in the body frame are
b defined as follows. The pitching motion is actuated by
where m represents the vehicle mass, VCM =
 ⊤ the moment around y b produced by increasing the thrust
u v w denotes the linear velocity of the of rotor 1 and reducing the thrust of rotor 3. The roll
vehicle centre of mass  expressed in the body axis movement is generated in a similar way, that is, by pro-
⊤
frame, Ω = p q r denote the angular velocity ducing a differential thrust between rotors 2 and 4. Due
of the body frame, I is the vehicle inertia matrix1 Feb to the torque applied to the rotor shaft by the motors
represents the external applied forces expressed in the a reactive torque of the same magnitude but opposite
body frame and Meb represents the external applied direction is experienced on the structure of the vehicle.
moments expressed in the body frame. By manipulating these reaction torques it is possible to
As it is usual in aeronautics, the traslational vehicle control the yaw moment. That is,
dynamics (1) must be expressed in earth axis coordinates.    
Considering the Euler yaw-pitch-roll rotation sequence. L (T3 − T1 ) ℓ
The rotation matrix that describes the orientation of the Meb =  M  =  (TP 2 − T4 ) ℓ  (8)
4
body reference frame relative to the earth frame is given N Q
i=1 i
by
  where ℓ is the distance between the rotor rotation axis and
cθ cψ cθ sψ −sθ the aircraft centre of mass and Qi is the reactive moment
R =  cψ sθ sφ − sψ cφ sψ sθ sφ + cψ cφ cθ sφ  produced by rotor i. This reactive moment is given as [6]
cψ sθ cφ + sψ sφ sψ sθ cφ − cψ sφ cθ cφ
(3) Qi = CQi π ri5 ρ ωi2
 ⊤
where ǫ = φ θ ψ are the Euler angles with φ the As shown in [3] there exist a globally defined
roll angular displacement, θ the pitch angular displace- change of coordinates from

−TT L M N
⊤
ment, ψ the yaw angular displacement and cx = cos(x) ⊤
to ω1 ω22 ω32 ω42
 2 
for CTi > 0 and CQi > 0.
and sx = sin(x). Moreover, Ω is related to the Euler angles
The control objective is to asymptotically track pre-
velocity as follows [5]
  scribed trajectories for the vehicle spatial position (x, y, z)
1 0 −sθ and the yaw orientation ψ.
Ω = W ǫ̇ =  0 cφ cθ sφ  ǫ̇ (4) In what follows we show that the considered control
0 −sφ cθ cφ objective is achievable with a nonlinear time variant state
From equation (3) we have that the relationship between feedback under the following standing assumption
the velocity components in the earth frame and the veloc- A1 All system states are measurable and all system phys-
ity components in the body frame is defined as ical parameters are known.
b e
VCM = R VCM (5) III. Control Design
e
  ⊤ In this section we present the control strategy which
where VCM = ẋ ẏ ż is the linear velocity of the
vehicle centre of mass expressed in the earth frame. Thus, makes use of a bounded function characterized by the
the traslational vehicle dynamics expressed in the earth following definition.
frame is described by Definition 1: Let γ+ : ℜ → ℜ+ be a bounded function
defined as h  s i
e
m V̇CM = R⊤ Feb (6) γ+ (s) = σ δ + tanh
2σ
The external applied forces expressed in the body frame
where σ and δ are positive constants, with δ > 1.
are the vehicle weight and the total thrust produced by the
Now, we describe the control strategy. First, we design
four rotors, that is,
    a controller for the vehicle vertical motion taking into
−m g sin(θ) 0 account that the vertical velocity is not available. Then,
Feb =  m g cos(θ) sin(φ)  +  0  (7) we perform a partial feedback linearization control design
m g cos(θ) cos(φ) −TT to the resulting closed–loop dynamics in order to achieve
P4
where TT = i=1 Ti with Ti the thrust of each rotor. It is trajectory tracking in the 0xe y e plane. As it is usual in this
shown in [6] that the thrust generated by each rotor can aerial vehicle, the motion in the yaw direction is controlled
be expressed as: independently.
Equation (6) can be written as follows
Ti = CTi π ri4 ρ ωi2
m ẍ = TT sθ
where CTi is the rotor i thrust coefficient, ρ is the air m ÿ = −TT cθ sφ (9)
density, ri is the radius of rotor i and ωi is the angular m z̈ = −TT cθ cφ + m g
velocity of rotor i.
Defining
1 As the vehicle has two symmetry axes J = diag{Jxx , Jyy , Jzz }. z1 = z − zd , z2 = ż − żd
the third equation in (9) can be written as which can be upper bounded as follows
ż1 = z2 V̇ ≤ − h[1 − 2λmax (P ) κ1 ] |χ|2i
(10)
ż2 = −cθ cφ TmT + g − z̈d (4)
+ 2λmax (P ) |zd | + κ0 |χ|
Vertical motion control can be obtained by defining
where we have used the fact that
mγ+ (s)
TT = (11) |s − γ+ (s)| ≤ κ0 + κ1 |χ|
cθ cφ
where for some positive constants κ0 , κ1 . Defining the constants
µ1 and µ2 such that
s = kp z1 + kd (−ẑ2 − Γ11 z1 ) − (σ11 + Γ12 z1 )
0 < µ1 < 1, 0 < µ2 < µ1
together with the dynamic system
the upper bound on V̇ can be expressed as
TT
ẑ˙2
= cθ cφ + σ11 + Γ12 z1 + Γ11 (ẑ2 + Γ11 z1 ) V̇ ≤ − (µ1 − µ2 ) |χ|2
m
σ̇11 = σ12 + Γ13 z1 + Γ12 (ẑ2 + Γ11 z1 ) − µ2 |χ| − 2λmax (P ) |zd |(4) − κ0 |χ|

σ̇12 = Γ13 (ẑ2 + Γ11 z1 )
(12) therefore
The closed–loop dynamics (10)-(11)-(12) expressed in the (4)
2λmax (P ) |zd | + κ0

Z = z1 z2
⊤
coordinates and the estimator coordi- V̇ ≤ − (µ1 − µ2 ) |χ|2 ∀ |χ| >
µ2
nates
Hence, we conclude that the closed–loop dynamics (13)
z2 + ẑ2 + Γ11 z1
   
ξ11 is ultimately bounded. Now, we design the controller to
Ξ =  ξ12  =  σ11 + Γ12 z1 + g − z̈d  perform trajectory tracking of the planar coordinates. For,
(3)
ξ13 σ12 + Γ13 z1 − zd we define the following linearizing feedback
reads as Meb = −Ω × JΩ + Jτ (14)
  (4)
χ̇ A − BZ K̄ χ +
= BΞ z d
+ BZ [s − γ+ (s)] (13) and the new coordinates
⊤    
where χ = Z ⊤ Ξ⊤

, x xd
X= , Xd = , X1 = X − Xd
  y yd
0 1 0 0 0
 −kp −kd kd 1 0  Thus, the vehicle dynamics in closed loop with the con-
  troller (11) is described by the following equations
A= 0 0 Γ11 1 0  ,
 0 0 Γ12 0 1  Ẋ1 = X2
0 0 Γ13 0 0 Ẋ2 = γ+ (s)N − Ẍd

BΞ = 0 0 0 0 −1
⊤
, (15)
ǫ̇ = W −1 Ω
 ⊤ Ω̇ = τ
BZ = 0 1 0 0 0 ,
⊤
and we have expressed s as follows τp
with τ =  τq  .
s = K̄χ + g − z̈d τr
Let X3 = Ẋ2 and
with
(3)
 
K̄ = kp kd −kd −1 0 X4 = Ẋ3 = γ̇+ (s)N + γ+ (s)M Ω − Xd
Performing involved but standard computations it is pos- where ⊤
sible to verify the gains kp , kd and Γ1i , i = 1, 2, 3 can tθ

N = −tφ
be selected in such a way that the matrix A is Hurtwitz. cφ
Hence, there exists a positive definite matrix P such that ∂N −1  
the Lyapunov function M = W = M̄ m̄
∂ǫ
V = χ⊤ P χ with  
sφ sθ (1−cθ )2 +c2φ c2θ
has the following time derivative along the trajectories of c2φ cθ c2φ c2θ
M̄ =  s s

the closed–loop system (13) − c12 − cφ2 cθθ
φ φ
h i⊤
tθ
(4)
V̇ = −χ⊤ χ + 2χ⊤ P BΞ zd + 2χ⊤ P BZ [s − γ+ (s)] m̄ = −tφ − cφ
and tx = tan(x). Hence, in terms of the Xi , i = 1, · · · , 4 with Q a positive definite matrix. Second, notice that the
coordinates the planar and partial rotational dynamics of νi , i = 1, 2, 3 functions are bounded as follows
the rotary wing vehicle read as
|ν1 | ≤ κ2 |χ| + κ3 |χ|2
Ẋ1 = X2 |ν2 | ≤ κ4 + κ5 |χ|
Ẋ2 = X3 |ν3 | ≤ κ6 + κ7 |χ| + κ8 |χ|2
Ẋ3 = X
4 2
γ+ γ̈+ −2γ̇+
 for some positive constants κi , i = 2, · · · , 8. Consider now
Ẋ4 = γ+2 X3 + 2 γ̇γ+
+
X4 the following Lyapunov function
h i
+γ+ Ṁ Ω + M τ V = Θ⊤ P̄ Θ
 2 
γ+ γ̈+ −2γ̇+ (3) (4)
+ γ2
Ẍd + 2 γ̇γ+
+
Xd − Xd whose time derivative along the trajectories of the dynamic
+
system (17) is given as
Defining the feedback
V̇ = −Θ⊤ QΘ + 2Θ⊤ P̄ B̄ (ν1 X3 + ν2 X4 + ν3 )
   
τp
h
= − γ1+ M̄ −1 −Ṁ Ω − m̄τr + Kj X4
τq The time derivative of the Lyapunov function can be upper
+Ka X3 + Kd X2 + Kp X1 (16) bounded as follows
γ̇+  
V̇ ≤ −|Θ|2 + 2λmax P̄ |Θ| [γ1 (|χ|)|Θ| + γ2 (|χ|)]


−Kj X3 + Ẍd
γ+
we obtain the closed–loop dynamics Because |χ| is globally ultimately bounded, we know that
|χ| converges to a bounded value. This implies that there
Θ̇ = ĀΘ + B̄ (ν1 X3 + ν2 X4 + ν3 ) (17) exist positive constants κ7 and κ8 such that
where
V̇ ≤ −|Θ|2 + 2λmax P̄ |Θ| [κ7 |Θ| + κ8 ]


 ⊤
Θ = X1 X2 X3 X4
  thus, we can find constants µ3 and µ4 such that
0 I 0 0
 0 0 I 0  0 < µ3 < 1, 0 < µ4 < µ3
Ā = 
 0

0 0 I  such that
−Kp −Kd −Ka −Kj

 ⊤ 2λmax P̄
B̄ = 0 0 0 I V̇ ≤ − (µ3 − µ4 ) |Θ|, ∀ |Θ| >
µ4
with Kp , Kd , Ka , Kj positive definite matrices, and
 2 
and the proof is concluded.
γ+ γ̈+ −2γ̇+
ν1 = γ 2 I + Kj γ̇γ+
+
Remark 1: Notice that we do not have access to γ̇+ , γ̈+
+
because these time derivatives of the saturation function
ν2 = −Kj + 2 γ̇γ+
+
I depend on z2 , which is not a measurable state. This fact
(18)
2 
explains the last term of the feedback (16).

γ+ γ̈+ −2γ̇+ (3) (4)
ν3 = γ+2 Ẍd + 2 γ̇γ+
+
Xd − Xd
Remark 2: Since the yaw rotational dynamics is com-
γ̇+
+Kj γ+ Ẍd pletely independent of the rest of the vehicle dynamics,
we propose
The main stability properties of the closed–loop dynamics
are summarized in the following τr = −kp ψ − kd r
Proposition 1: Consider the rotary wing vehicle dynam-
IV. Simulation results
ics described by equations (1)-(2) in closed–loop with
the controller defined by equations (11)-(14)-(16). Assume Numerical simulations were carried out to asses the
that the desired trajectories zd , xd and yd are bounded and performance of the controller proposed. The numerical
have bounded time derivatives. Then, there exist constants value of the vehicle parameters are presented in Table 1.
σ and δ, and gains kp , kd , Γ1i , i = 1, 2, 3, Kp , Kd , Ka Parameter Value Parameter Value
and Kj such that the closed–loop system (13)-(17) and is
m (kg) 0.74 CQi , i = 1, · · · , 4 0.1
globally ultimately bounded.
g (m/s2 ) 9.81 ℓ (m) 0.25
Proof: First, notice that by adequately selecting the
gain matrices Kp , Kd , Ka and Kj the matrix Ā will be r (m) 0.15 Ixx (kg m2 ) 0.004
Hurtwitz. Thus, there exists a positive definite matrix P̄ ρ (kg/m3 ) 1 Iyy (kg m2 ) 0.004
such that CTi , i = 1, · · · , 4 0.5 Izz (kg m2 ) 0.0084
P̄ Ā + Ā⊤ P̄ = −Q Table 1. Rotary wing vehicle parameters
Note that we consider similar characteristics for all rotors.
 The desired trajectory is defined as follows
Tracking error (ez)
xd = 10 sin(0.2 t) 0.5

yd = 10 cos(0.2 t)
(19)
zd = .2 + 10 cos2 (0.2 t)

Z position error (m)

0

ψd = sin(0.2 t)
We consider the following initial conditions −0.5

10, 0, 0, π4 , − π4 , 0 . Finally, the

(x, y, z, φ, θ, ψ) =
controller parameters are presented in Table 2.
−1
Parameter Value Parameter Value 0 5 10 15 20
Time (s)
25 30 35 40

kp , kpψ 2.625 kd 3.25 0.1

Tracking error (ephi)

Γ11 −5.75 Γ12 −10.75 0

Γ13 −6.5625 Kj 3 −0.1

Orientation error (rad)

−0.2
Ka 3.1875 Kd 1.4375 −0.3

Kp 0.234375 kdψ 3.5 −0.4

σ 10 δ 1.02 −0.5

−0.6
Table 2. Controller parameters
−0.7

Figures 2 and 3 show the performance of the controller of −0.8

0 5 10 15 20 25 30 35 40
Time (s)
Proposition 1. As it can be observed, the tracking errors
converge to zero. Figures 4 and 5 display time histories of Fig. 3: Tracking error ez (top) and tracking error eψ (bottom).
Tracking error
30
ex
20 ey

10
Position error (m)

0 Total thrust
45
−10
40
−20
35
−30
Applied force (N)

30
−40
25
−50
20
−60
0 5 10 15 20 25 30 35 40
15
Time (s)
10

Fig. 2: Tracking error ex (continuous line) and tracking error 0

0 5 10 15 20 25 30 35 40
Time (s)
ey (dashed line). Moment around x
0.2

the control signals. Finally, in Figure 6 the time histories 0.1

Applied moment (Nm)

of the states (φ, θ) and its time derivatives are shown. 0

−0.1

V. Conclusions −0.2

The trajectory tracking problem for a rotary wing −0.3

vehicle powered by four rotors has been addressed and −0.4

solved by means of a full information control law, which −0.5

0 5 10 15 20 25 30 35 40
is based on the backstepping technique. Simulations have Time (s)

been proposed to illustrate the properties of the closed

Fig. 4: Total thrust TT (top) and moment applied in the roll
loop system. direction L (bottom).
The advantage of the proposed control is that the speed
of elevation is not measured. In the other hand, the
 disadvantage is that the control scheme does not take into
Moment around y
0.1 account the problem of saturation of the control signals.
0.05
A few issues are left open in the present paper. First, all
0
the system parameters are assumed to be known. Second,
Applied moment (Nm)

−0.05

−0.1
all the states are assumed to be measurable, hence further
−0.15
work is necessary to relax (or avoid) these assumptions.
−0.2

−0.25
References
−0.3 [1] Davis Jr. W. R., Kosicki B.B., Boroson D.M. and Kostishack
−0.35 D.F., Micro Air Vehicles for Optical Surveillance, The Lincoln
0 5 10 15 20 25 30 35 40
Time (s) Laboratory Journal, Vol. 9, No. 2, 1998.
Moment around z [2] Kendoul F., Fantoni I and Lozano R., Modelling and control
0.14
of small autonomous aircraft having two tilting rotors, Proc. of
0.12
the 44th Conference on Decision and Control and the European
Control Conference 2005, Seville, Spain, December 12-15, 2005.
Applied moment (Nm)

0.1

0.08
[3] Hamel T., Mahony R. and Lozano R., Dynamic modelling and
configuration stabilization for an X4-flyer, Proc. of the 15th
0.06
Triennial World IFAC Congress, Barcelona, Spain, 2002.
0.04 [4] Salazar-Cruz S., Palomino A. and Lozano R., Trajectory track-
0.02
ing for a four rotor mini-craft, Proc. of the 44th Conference
on Decision and Control and the European Control Conference
0
2005, Seville, Spain, December 12-15, 2005.
−0.02
0 5 10 15 20 25 30 35 40 [5] Roskam J., Airplane Flight Dynamics and Automatic
Time (s)
Flight Controls, Part I, Roskam Aviation and Engineering
Corporation, USA, 1982.
Fig. 5: Moment applied in the pitch direction M (top) and [6] Gessow A. and Myers C. G. Jr., Aerodynamics of the Heli-
moment applied in the yaw direction N (bottom). copter, FREDERICK UNGAR PUBLISHING CO., New York,
Fifth Printing, 1978.
[7] Sepulchre, R., Jankovic M. and Kokotovic P., Constructive
Nonlinear Control, Springer Verlag, London, 1997.
[8] Kaliora G. and Astolfi A., A simple design for the stabilization of
1.5
Orientation around x and y cascaded nonlinear systems with bounded control, Proc. of the
phi
theta
40th Conference on Decision and Control, Orlando, FL, pages
1 3784-3789, 2001.
Orientation (rad)

0.5

−0.5

−1

−1.5
0 5 10 15 20 25 30 35 40
Time (s)
Angular rate around x and y
4
p
q
3
Angular rate (rad/s)

−1

−2
0 5 10 15 20 25 30 35 40
Time (s)

Fig. 6: Roll angle φ (continuous line) and Pitch angle θ (dashed

line) (top). Roll angular velocity φ̇ (continuous line) and pitch
angular velocity θ̇ (dashed line) (bottom).