Professional Documents
Culture Documents
H. Rodrguez-Cortes
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 = [ ]
= 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 ]
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 ]
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
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