Professional Documents
Culture Documents
Quadrotor Report
Quadrotor Report
Semester-Thesis
Supervised by:
H
urzeler Christoph
Nikolic Janosch
Author:
R
uesch Andreas
Abstract
Autonomous mobile robots arouse a lot of interest in the past years, often with
the goal to support human beings in numerous activities, likewise in the project
AIRobots.
Within this project, an airframe of a quadrotor was constructed and equipped
with flight electronics. This thesis presents a complete system modeling and identification process of the dynamics of the quadrotor, motivated by the posterior
derivation of a flight controller. For the sake of which flight data were gathered
during manual controlled flights and the prediction error method was applied for
the parameter identification.
After the identification, a position controller was designed based on the model.
The controller was successfully applied to the quadrotor without retuning and allows the helicopter to hover smoothly. The control algorithm is based on an optimal
control problem formulation, namely a linear quadratic regulator method, with integral extensions and anti-reset-windup add-ons, as well as a Kalman observer for
the states estimation were applied.
Key words: UAV, Dynamical System Modeling and Identification, Position Control, LQR(I), Anti Reset Windup, Kalman Observer.
ii
Acknowledgment
This semester thesis was evolved at the Autonomous Systems Lab (ASL) of the
Swiss Federal Institute of Technology (ETH) Zurich. The author is very grateful to
his supervisors, Christoph H
urzeler and Janosch Nikolic for their great support and
guidance during the whole process of this project, their assistance was essential for
the accomplishment of this thesis.
Expressing gratitude to Prof. Roland Y. Siegwart, the head of the Autonomous
Systems Lab, for the opportunity to work at his laboratory under ideal conditions
on a very exciting topic.
Sincere thanks are given to the PhD students of the ASL team, who all the
time had a sympathetic ear to any question about flying robots. Last but not least,
many thanks to all Master students of the ASL flying room, for their sustained
support and our interesting discussions.
iii
iv
Contents
List of Tables
vii
List of Figures
viii
List of Symbols
ix
1 Introduction
1.1 Problem Formulation and Context . . . . . . . . . . . . . . . . . . .
1.2 Prior and Related Work . . . . . . . . . . . . . . . . . . . . . . . . .
1
1
2
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
3
3
4
4
5
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
7
7
7
7
8
8
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
9
9
9
10
11
12
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
13
13
14
14
16
18
18
18
19
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
21
21
22
22
22
22
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
23
23
26
26
27
29
29
29
A Parameters
A.1 Model Parameters . . . . . . . . . . . . . . .
A.2 Controller Parameters . . . . . . . . . . . . .
A.3 Controller Parameters - Prior PID Approach
A.4 Angle Offsets . . . . . . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
31
31
31
32
32
B Matlab/Simulink Documentation
B.1 Source Folder Overview . . . . . . .
B.2 Model Identification and Validation .
B.2.1 Submodel Identification . . .
B.2.2 Complete Model Validation .
B.3 Position Control . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
33
33
33
33
34
34
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
37
Bibliography
39
vi
List of Tables
2.1
5.1
20
7.1
26
vii
List of Figures
2.1
2.2
2.3
4
5
5
3.1
4.1
4.2
4.3
10
11
12
5.1
5.2
5.3
5.4
5.5
5.6
. . . . . . . . .
state feedback.
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
13
15
16
18
19
20
6.1
22
7.1
7.2
7.3
7.4
7.5
7.6
7.7
7.8
7.9
7.10
7.11
7.12
Results
Results
Results
Results
Results
Results
Results
Results
Results
Results
Results
Results
23
24
24
25
25
26
27
27
28
28
28
28
from
from
from
from
from
from
from
from
from
from
from
from
viii
List of Symbols
Physical Variables
Ae x
Aey
Ae z
RAB
r OP
F
T
dsharpL
dsharpR
dsharpS
dr
b
l
Controller Variables
x(t)
xk
Ts
A, B, C, D
F, G, C, D
K, KI
Q, R,
Kf
P
wd
wn
uk
u
k
yk
ix
Chapter 1
Introduction
Autonomous flight control of unmanned aerial vehicles (UAVs) presents a challenging task. The high agility and autonomy of small-scaled UAVs lead to a huge
area of applications. For example, industrial tasks like inspection, measurements
or exploration as well as search and rescue tasks or even military applications are
conceivable. The successful accomplishment of such applications requires that the
flight controller is able to stabilize the motion of the UAV. Thus, the completion of
maneuvers such as hovering, obstacle avoidance, autonomous navigation, docking
and sensing, is essential.
Besides a translational and rotational acceleration measurement, which is accomplished by the inertial measurement unit (IMU) and the gyroscopes, a position
control algorithm of any autonomous mobile robot requires information about the
position itself, to avoid any drifting errors or integration inaccuracies. The variety
of such exteroceptive sensors is wide, e.g., infrared rangefinders, 3D laser rangers,
ultrasonic or pressure sensors, global positioning system (GPS) sensors, cameras
and many more.
For indoor applications, the usage of GPS sensors is strongly limited, in contrast
3D laser rangefinders provide very precise distance measurements, but are usually
expensive and due to their high power consumption and weight, the usage on micro
aerial vehicles (MAV) is limited. Alternatively, computer vision, which utilizes
simple low cost cameras, can provide a good description of the MAV surroundings.
In practice though, the testing process of new visual servoing algorithms on a MAV
is often a challenging task.
This motivates us to develop a test platform, whose position is controlled by
much simpler sensors, such as infrared rangefinders and ultrasonic sensors. The
exteroceptive sensors controlled testing platform allows, in a further development
phase, a step by step procedure, to replace single degrees of freedom with visual
servoing algorithms.
1.1
Chapter 1. Introduction
our goal to derive a good and validated model of the vehicles flight dynamics and
implement a position controller based on infrared and sonar range sensors. The
autopilot is assumed to be able to hover smoothly in front of a corner.
1.2
As commonly known, aerial vehicles are usually representing a highly unstable and
nonlinear system. The six degrees of freedom (DOF) of an unconstrained aerial
vehicle present a challenging control task. A common approach is to use a cascaded
control structure, where an inner loop stabilizes the rotational DOF of the UAV
and an outer loop is used for the pose control.
Within the project OS4, Bouabdallah et al. [BS07], [BNS04], [BS05] analyzed
and applied several control techniques. For simulation purposes Bouabdallah et
al. [BS07] evolved a complete physical model of the quadrotors dynamics. With
an adoption of the integral back-stepping techniques a superior performance was
achieved, for the attitude and pose control.
As in the sFly project [BSWS] also in this thesis, the quadrotor was equipped
with an on-board attitude controller provided by Ascending Technologies [Asc],
which is basically a PD-controller. Blosch, Scaramuzza, Weiss et al. proposed
a black box modeling approach to model the system dynamics. Furthermore they
applied a visual SLAM algorithm for the position estimation and a LQG/LTR based
controller for stabilizing the vehicle at a desired set point.
Shin et al. [SNFH05] proposed a model-based MIMO-control system for an
unmanned helicopter, as well based on a cascaded control structure by applying
Kalman filter-based LQI controllers. They have shown that a position controller,
which considers the dynamics of the attitude closed-loop dynamics, is very useful.
The work of Skogestad & Postlethwaite [SP96], Lewis [LK92] and Guzzella
[Guz09] proved to be strong references for background information in control theory.
Chapter 2
2.1
AIRobots Quadrotor
The quadrotor used within the scope of this project is called AIRobots Quadrotor.
Its airframe was developed in a former student project at the ASL (Autonomous
System Lab) [Roo09], furthermore it is equipped with the flight electronics of the
AscTec Hummingbird quadrotor developed by Ascending Technologies GmbH [Asc].
The AIRobots Quadrotor is equipped with a quite simple and compact mechanical design, so that, in the worst case of a crash, it proves to be remarkably robust.
Thanks to the ease of modifying the quadrotor, for example by mounting additional
sensors or cameras, and its relatively low-cost, the AIRobots Quadrotor qualifies as
an ideal testing platform.
Besides autonomous flight, also manually controlled flights are possible, which
proves to be useful to gain flight data, for example to validate the system model.
In Figure 2.1 the CAD model (computer-aided-design) of the AIRobots Quadrotor
is illustrated. Table 2.1 provides the most important facts about the AIRobots
quadrotor and the AscTec Hummingbirds flight electronics.
nominal mass
payload capacity
total diameter
rotor diameter
flight time
maximal speed
micro processor
536 g
200 g
60 cm
20 cm
continuous
50 km/h
1 x ARM7
1 x TMS C2000
2.2
The AIRobots Quadrotor is equipped with an on-board attitude controller, by Ascending Technologies [Asc], which stabilizes the three rotational degrees of freedom
of the helicopter basically with PD controllers, see [BSWS]. Thus, no direct input signals to the rotors are needed, instead the desired tilt angles can directly be
transmitted. For more details about the flight control see chapter 5.
2.3
Development Environment
To derive the different controllers during this project the software package MATLAB/Simulink of MathWorks1 was used. For an automatic code generation from
the Simulink models into the programming language c, the MathWorks products:
Real-Time Workshop, Real-Time Workshop Embedded Coder, the Target Support
Package as well as the software tool Code Composer Studio of Texas Instruments2
were used. This setup, as illustrated in Figure 2.2, allows a direct code generation
and compilation, and furthermore the download of the software to the quadrotor.
For on-line recording and logging of IMU or sensor data, as well as controller
parameters or observer values, also MATLAB/Simulink was used. Using this setup,
the control structure is quickly and easily changeable. The on-line recording of any
data proved itself very valuable for the purpose of debugging.
1 MATLAB:
2 Code
version R2009b, August 12, 2009 for Unix and MS Windows systems
Composer Studio: version 3.3.83.19, exclusively on MS Windows systems
2.4
Before this semester project started some prior work had already been accomplished
by the ASL team.
The quadrotor had yet been equipped with three infrared rangefinders as well
as a sonar ultra sonic sensor. These exteroceptive sensors are used to estimate the
position of the helicopter in the room, namely its distance to the front and side
wall, its heading angle relatively to the front wall and its altitude. The positioning
of the sensors is illustrated in Figure 2.3.
A position controller, based on four decoupled PID controllers for the three
translational degrees of freedom plus for the helicopters heading angle relatively to
the front wall, had already been implemented and tested. The PID-approach lead
to average results, which encouraged to evolve and test an optimal control approach
as well as derive a validated model of the helicopter. For the PID-parameters see
appendix A.3.
Sharp L
Sharp R
'$ '$
@
@s
s
@
@
&%
&%
@
@
@
@
'$@'$
sUS sensor
@
s
@s
@ Sharp side
&% &%
@
3 Source:
MathWorks http://www.mathworks.com/products/target-package/
Chapter 3
3.1
Notation
Physical Variables
Av
RAB
, ()
()
Controller Variables
x(t)
Continuous signal for the states
xk
Discrete signal for the states, with xk = x(k Ts ) where Ts
denotes the sampling time
A, B, C, D Continuous state space matrices
F, G, C, D Discrete state space matrices
K, KI
LQR gain respectively LQRI gain
Q, R,
LQRI tuning parameters
3.2
Coordinate Frames
Within the scope of this project two coordinate frames were used, on the one hand
the ground-fixed inertial frame and on the other hand the helicopter-fixed body
system. In Figure 3.1 both system are illustrated.
3.2.1
Inertial Frame
The inertial frame is defined according to a common North-East-Down (NED) system. Since in this project an indoor MAV without a geographical environment was
used, the north direction was redefined perpendicular to the front wall, pointing
into the wall. The third axis was defined normal to the ground, pointing downwards. Due to the right-hand-system, the east direction points perpendicularly
7
6
Iex N
h
Iez D
Iey
into the side wall. The origin of the inertial frame is laid in the corner, thus only
flights in the 8th octant are possible.
3.2.2
Body Frame
The body frame is defined to be fixed on the quadrotor. Its origin is located in
the center of gravity (C.G.) of the helicopter. The quadrotors first, respectively
second axis are defined to be coaxial with the quadrotors arms. The third axis is
normal to the helicopters plane and points downwards. In Figure 3.1 the definition
is illustrated.
3.2.3
Transformation Matrices
The transformation from the ground-fixed inertial frame into the body-frame, applied within the scope of this thesis, was defined according to the Tait-Bryan convention [Glo07]. Thus, for the rotation matrix RBI follows,
RBI (, , )
=
=
R R R
1
0
0
c 0 s
c s 0
s c 0 0 c s 0
1 0
0
0 1
0 s c
s 0 c
c c
c s
s
s s c c s s s s + c c s c ,
(3.1)
c s c + s s c s s s c c c
where c(. . .) denotes the cos(. . .) function, and s(. . .) analogously the sin(. . .) function. The following relation
1
T
RIB (, , ) = RBI
= RBI
,
(3.2)
holds for the transformation from the body frame into the inertial frame, due to
the orthogonality of rotation matrices [Glo07].
Chapter 4
4.1
Point-Mass Model
Due to the quadrotors small size and symmetry, it can be modeled as a pointmass. According to the principle of linear momentum (Newtons 2nd Law) and
the assumption that the mass of the quadrotor is constant, for the change of the
momentum follows
X
m I rOP =
(4.1)
IF i,
i
where rOP denotes the acceleration of the center of gravity (C.G.) and F i represents
any outer force acting on the point-mass.
Applied to the quadrotor (see Figure 4.1) for the equation of motion of the
vehicle follows
0
0
1
= RIB (, , ) 0 + 0 ,
(4.2)
Ir
m
T
g
with T represents the total thrust moment, pointing in the opposing direction of the
helicopters down axis, and the rotation matrix RIB (, , ) from the body frame
into the inertial frame as defined in section (3.2.3).
4.2
An inner controller for the attitude is implemented on-board of the quadrotor (for
further information see section 2.2) . Hence, the unknown parameters in the equation (4.2), explicitly the Euler angles and the thrust momentum, are output variables of the attitude controller. Since no exact description of the attitude controller
exists and the control parameters are unknown, the dynamics of the inner loop are
also identified in the system modeling task. For this purpose a black box modeling
approach is used.
9
10
T
Be z
: Bex
Y
QH
H
QHH
Q H
Q H
s eH
Q
B y H
HH
HH
?
F G = mg
wall
XXfront
XX
XX
H
HH I r OP
H
HH
H
XXX
e
XXX HHH
*I x
XXX H
HH
XXX
XX
H
XX
XXX
z
X
Iey
?
Iez
side wall
The main idea of black box modeling is to approximate the dynamics of the
system with an educated guess. The system parameters are identified and validated
with an adequate method using flight data sets. Furthermore, this modeling method
avoids a sumptuous and expensive identification of the rotor dynamic, or an exact
description of the quadrotors aerodynamics. Because of the quadrotors mechanical
simplicity (i.a. non-existence of a swashplate, simple design) a black box approach
is adequate and leads to suitable results, as can be seen in section 7.1.
The following part describes the four decoupled subsystems, used to model the
roll, pitch, yaw and thrust dynamics. Furthermore a brief overview of the used
method for the parameter identification is given.
4.2.1
Decoupled Submodels
As illustrated in Figure 4.2, the dynamics of the attitude controller are divided into
four subsystems, which are identified separately.
Roll Dynamics
The roll dynamics are approximated with a 2nd order system and a delay
element. Its input variable is the desired roll angle, and its output the current
roll angle.
Pitch Dynamics
The pitch dynamics are described likewise with a 2nd order system and a delay
element. The input respectively output variables are analogously the desired
and the actual pitch angle.
Yaw Dynamics
For the yaw dynamics a 1st order system and as well a delay element is used.
In contrast to the roll and pitch dynamics, the desired angle rate is the input
to this subsystem, respectively the current yaw rate its output variable.
11
k02
s2 +2 0 s+02
esT
k02
s2 +2 0 s+02
esT
k
T s+1
esT
k
T s+1
esT
Thrust Dynamics
The thrust dynamics are as well modeled with a 1st order system and a delay
element, here too, the desired and current thrust momentum are the input
respectively output values of the system. Please note that after the 1st order
element and the delay element, the nominal thrust moment of m g, required
for hovering, is added.
4.2.2
To estimate the model parameters the Prediction Error Method was used. This
method uses an iterative nonlinear least-squares algorithm to minimize a cost function, which consists of a weighted sum of the squares of the errors. According to
[MAT10] the cost function is defined as follows:
VN (G, H) =
N
X
e2 (t),
(4.3)
i=1
where e(t) denotes the error between the estimated and the measured values. For
linear systems the error can be expressed as,
e(t) = H 1 (q)[y(t) G(q)u(t)].
(4.4)
As can be seen in the definition of the cost function, the estimation becomes the
more accurate the more samples are taken. The measured data sets used in this
project were gained during manual controlled flight maneuvers. (see appendix C)
To apply this parameter identification method, the software package MATLAB
was used. The MATLAB command pem computes the prediction error estimate of
a general linear system, wherefore the delay elements were manually estimated and
iteratively varied. To increase the accuracy of the estimation, the algorithm was
applied several times, using the prior estimation as the new initial guess.
For more detailed information about the prediction error method the interested
reader is referred to [MAT10] and [Sha03], for more details about the application,
see appendix B.2.
4.3
12
Eventually, the final system of the complete model of the AIRobots quadrotor is
illustrated in Figure 4.3. The model was used to build a simulator where for example
control algorithms can be tested before they are implemented on the real platform.
The identified parameters can be found in appendix A.1, the results of the model
validation are provided in chapter 7.
Helicopter Device
point-mass model
k02
s2 +2 0 s+02
esT
k02
s2 +2 0 s+02
esT
I r
k
T s+1
esT
k
T s+1
esT
0
0
= m1 RIB(, ,) 0 + 0
T
g
I r
I r
Ir
Chapter 5
, , , T
Position
Controller
AscTec
attitude
controller
Quadrotor
plant
Ir
, , , T
5.1
Controller Design
Because of near hovering, the cross coupling effects between the roll, pitch, yaw
and altitude dynamics are insignificant and thereby negligible, a decoupled control
approach is conceivable. As mentioned in section 2.4 a former position controller,
applying four decoupled PID controllers led to average results. Thus, for this project
an optimal control approach was chosen.
At the source of any optimal control approach the formulation of an optimization problem is to be found. According to Skogestad & Postlethwaite [SP96] and
Guzzella [Guz09], two main groups of optimization problems are distinguished in
modern control theory. The first method is the so-called H2 method, with the goal
to minimize the mean value of the so-called error signal over all frequencies. On
the other hand, a worst-case formulation where the cost function is defined through
the worst possible value of the error signal over all frequencies is conceivable, it is
called the H method.
In this project a linear quadratic regulator (LQR) is applied, which is a member
of the H2 optimization group. It is described in more detail in the following.
13
5.1.1
14
For any LQR control, it is assumed that the plant dynamics are linear and known,
furthermore it is assumed that all states of the plant are available. Thus, the system
is given in the state space notation as
xk+1
F xk + G uk
(5.1)
yk
C xk + D uk
(5.2)
As Skogestad & Postlethwaite [SP96] pointed out, the LQR control problem is
defined as a deterministic initial value problem, i.e. to find the control input u(t),
which brings the linear system with the non-zero initial state x(0) to the origin so
that the following cost function is minimized
Z
x(t)T Q x(t) + u(t)T R u(t) dt,
(5.3)
J(u) =
0
xTk Q xk + uTk R uk .
(5.4)
k=0
The corresponding weighting matrices Q and R, or in other words the design parameters, have to be chosen suitably, such that Q = QT 0 respectively R = RT 0.
The optimal solution to this regulator problem is the control signal
uk = K xk ,
(5.5)
(5.6)
(5.7)
For more mathematical detail, interested readers are referred to the work of Skogestad & Postlethwaite [SP96], Guzzella [Guz09] and Lewis [LK92].
In the scope of this project the gain matrix K was determined by solving the
DARE (5.7) with the solver provided by MATLAB. Figure 5.2 illustrates the LQR
state feedback controlled closed-loop system.
5.1.2
yk
u
F x
k + G
k
I C x
k
(5.8)
(5.9)
15
Plant
uk
xk+1
z1
xk
yk
Figure 5.2: Block diagram of the closed-loop system with a LQR state feedback.
T
with the new state vector x
k = vk x k
, an additional tuning parameter and
the new system matrices are defined as
"
#
"
#
I C
0
=
F =
,
G
.
(5.10)
0 F
G
for the linear quadratic regulator problem for the extended
The solution K
R}
can be found analogously to the standard regulator problem
C T C,
system {F , G,
applying the equations (5.6) and (5.7), see section 5.1.1. The new gain matrix has
the following form
= K KI ,
K
(5.11)
the first element K is the solution of the standard LQR problem, the second element
is the gain matrix for the integral extension, see Figure 5.3.
Anti Reset Windup Add-on
The output range of any real actuator is limited through physical constraints. Thus,
in the case of a limited plant input, i.e. the input required by the controller is
larger than the maximal possible input, the system typically responds with a huge
overshoot and a very slow return to the reference value, e.g. pre-takeoff.
This phenomenon is well known from PI(D) control applications, and there are
several possibilities of appropriate anti reset windup additions to PI controllers.
The interested reader is referred to [LK92] and [GS03].
Since the LQR controller was augmented with a (P)I controller on each output
(see above) also the LQRI controller required an anti reset windup addition, to
avoid a large overshoot in the saturated case. For this purpose the plant input uk
is limited by a saturation model of the actuator, i.e.
umax
u
k
uk =
umin
if
u
k > umax
if
u
k [umin , umax ]
if
u
k < umin
(5.12)
where u
k is the required input of the controller. Since a discrete LQRI controller is
applied, the anti reset windup is quite simple. The difference between the required
u
k and saturated uk input signal is negatively fed back to the integrator, thus in
the following time step the integrator is reset. Figure 5.3 illustrates the integral
extension combined with the anti reset windup.
16
Plant
rk
KI
vk+1
z1
vk +
Integral Extension
uk
uk
xk+1
z1
xk
yk
anti reset
windup
F
Figure 5.3: LQR system with integral extension at the output and anti reset windup
5.1.3
As mentioned in the beginning of this chapter, cross coupling effects are negligible
near hovering flight, thus a decoupled control approach is conceivable. Below the
four decoupled LQRI controllers are briefly described, please note that all of them
correspond to the control schema derived above.
Altitude Control
The subsystem of the quadrotors altitude simply consists of the black box model
of the thrust and basically a double integrator, see section 4.2.1. This leads to the
following form for the transfer function of the linear altitude submodel
Palt (s) =
s3
b0
,
+ a2 s2
(5.13)
where b0 and a2 represent the constant gains, the numerical values can be found in
the appendix A.2. The transfer function, (5.13) expressed in the canonical form,
leads to the following state space representation
0 1
0
0
"
#
Aalt Balt
0 0
1
0
=
(5.14)
0 0 a2 1 .
Calt Dalt
b0 0
0
0
After the discretization, the discrete system matrices {Falt , Galt , Calt , Dalt } can be
applied straightforward to the LQRI problem formulation respectively to the DARE
equation (5.7).
Front and Side Distance Control
To control the distance to the wall in front and on the side of the quadrotor the
forces in the corresponding directions can be derived from the principle of linear
momentum (4.1). The principle of linear momentum can be rewritten and solved
for the total force vector acting on the quadrotor, thus the following expression
holds:
0
0
I Fx
I Fy = RIB (, , ) 0 + 0 .
(5.15)
T
mg
I Fz
17
According to the Tait-Bryan convention (see (3.1), (3.2)), for the forces in front
and side direction follows
I Fx
(5.16)
I Fy
(5.17)
I Fz
(5.18)
Under the assumption, that cos and cos are different form zero, which always is
the case around hovering flight, follows that
I Fx
I Fy
I Fz mg
(cos sin cos + sin sin ),
cos cos
I Fz mg
(sin sin cos cos sin ).
cos cos
(5.19)
(5.20)
As can be seen in the equations (5.19), (5.20) and outlined above, cross coupling
effects are insignificant around the hovering point, thus the following approximations
I Fx
mg tan ,
(5.21)
I Fy
mg tan .
(5.22)
are obtained.
Due to the point mass model of the quadrotor (see equation (4.1)) the components of the force vector can be expressed as
I Fi
= m I ri ,
(5.23)
thus for the subsystem of the helicopters plant in front direction with the input
signal I Fx and output signal I x holds
"
# 0 1
0
Adist Bdist
= 0 0 1/m ,
(5.24)
Cdist Ddist
1 0
0
and for the subsystem in the side direction with the input signal I Fy and output
signal I y analogously
"
# 0 1
0
Aside Bside
= 0 0 1/m .
(5.25)
Cside Dside
1 0
0
After a discretization of the linear systems above, the LQRI control method is
applied.
Heading Control
Last but not least the subsystem for the quadrotors heading angle has to be controlled. The black box model for the dynamics of the yaw rate is simply extended
with a integrator to derive the yaw angle. Hence, the transfer function of the linearized subsystem is expressed as
Pyaw (s) =
s2
b0
+ a1 s
(5.26)
where b0 and a1 represent the corresponding gains. The numerical values are as
well to be found in the appendix A.2. Thus, the state space representation of the
yaw subsystem expressed in the canonical form is given as
"
# 0
1
0
Ayaw Byaw
(5.27)
= 0 a1 1 .
Cyaw Dyaw
b0
0
0
18
Also for the heading control, after a discretization of the linear subsystem, it is
applied to the LQRI control method to derive a heading controller.
5.2
Observer Design
In distinction from a simulator, on a real platform are usually not all states directly
available. Hence, for an optimal control approach, as described above, a observer
is required to estimate the unmeasurable states.
For example, the distance of the quadrotor to the wall can be measured through
a infrared sensor, its acceleration in the corresponding direction is provided by the
IMU, but its velocity is not directly measured.
The field of different observer methods is huge. A complete treatment is beyond
the scope of this project. For the estimation of the states a standard Kalman
filter approach was applied in the context of this project. This approach is briefly
introduced below, for further information the interested reader is referred to [DW01].
5.2.1
Kalman Filter
The Kalman filter (also called Kalman estimator) has the structure of a standard
state observer, as illustrated in Figure 5.4. The difference between the measured
system output and the estimated system output is scaled by the Kalman filter gain
Kf and fed back to the observer. The Kalman filter gain Kf itself, is the solution of
an optimization problem under the assumption that the process and measurement
noise are uncorrelated white noise signals. I.e. the gain matrix, that minimizes the
expectation of the estimation error E[(x x
)T (x x
)], is chosen. As for the linear quadratic regulator problem, the optimal solution for the Kalman optimization
problem is found by solving a discrete algebraic Riccati equation. The solver provided by the software package MATLAB allows a straightforward implementation
of a Kalman estimator. For further information see [DW01] or [SP96].
wd k
uk
yk
Plant
+
+
wnk
Kf
G
+ + xk+1
z1
xk
yk
xk
F
Kalman Estimator
5.2.2
Implemented Observers
The meauserment of the accelerations with the IMU contains a bias, caused through
a misalignment between the IMU plane and the helicopters hovering plane. Therefore, to derive the observers for each LQRI controller, the state space representations
of the corresponding subsystems are augmented with an additional state to estimate
19
these biases. Furthermore the process noise wd and measurement noise wn are no
longer neglected, i.e. the submodels of the plant have the following continuous form
x =
A x + B u + G wd
(5.28)
C x + D u + wn
(5.29)
where the process noise signal wd and measurement noise signal wn are assumed to
be uncorrelated zero-mean Gaussian signals.
For the bias estimation in each subsystem an additional state is included, which
is modeled as a so-called Random walk process, see [TR05]. Based on the assumption, that the biases of the acceleration measurements are constant over the time
but priorly unknown, a Random walk model is conceivable. I.e. the additional state
does not depend on any prior state, it depends only on the process noise, so that
for the variation of the state the following expression holds
x bias = wd .
(5.30)
Due to the Random walk model, the Kalman filter is able to adjust the bias estimation slowly, using the knowledge gained through the measurments. For further
information see [DW01] and [TR05].
The extended state space matrices are provided in Table 5.1. Before the corresponding optimization problem can be solved, the subsystems have to be discretized.
5.3
In the next step the LQRI controllers found, in section 5.1, are combined with
the state observers, found in section 5.2. Thus, the structure of each decoupled
controller corresponds to the block diagram shown in Figure 5.5.
wd k
rk
KI
vk+1
z1
vk +
uk
uk
yk
Plant
Integral Extension
anti reset
windup
wnk
Kf
G
+ + xk+1
z1
xk
F
Kalman Estimator
K
xk
Figure 5.5: Single LQRI controller combined with a Kalman state estimator
In Figure 5.6 the complete closed-loop control structure, which was implemented
on the AIRobots quadrotor, is schematically illustrated.
20
Altitude Observer
A
0
0
0
0
1
0
1
0
0
CT
0
1
0
0
1
1
1
0
0
0
CT
1
0
0
0
CT
1
0
0
0
CT
1
0
0
0
0
0
0
1
0
1
0
0
0
1
0
0
1
0
0
0
1
0
0
0
0
1
0
1
0
0
0
1
0
0
1
0
0
0
1
B
1
0
1
0
G
1
0
0
1
Table 5.1: Extended state space matrices for the bias estimation
Helicopter Device
Position Controller
Set Point
LQRI front
( , , , T )
LQRI side
(x,
y,
z, )
LQRI heading
Attitude Controller
LQRI altitude
Observer
Kalman
Estimator
IR/US
sensors
Hqaud
IMU
Chapter 6
Hardware Implementation
and Testing
Some preliminary remarks to the implementation. The controllers were derived in
MATLAB/Simulink, for the code generation the setup described in section 2.3 was
used. The manual [Asc10] provides a lot of information about the Hummingbird
flight electronics, which was mounted on the AIRobots quadrotor.
6.1
Plane Estimation
The quadrotor is equipped with three infrared sensors, for the positioning see Figure
2.3. With the two sensors pointing to the front wall, the helicopters heading angle
as well as its distance to the wall were calculated by applying some simple geometric
coherences. On the assumption that the quadrotors plane is not tilted, i.e. that it
is parallel to the ground the heading angle is calculated with
dsharpL dsharpR
(6.1)
= arctan
b
where dsharpL denotes the distance measured with the left infrared sensor, dsharpR
denotes analogously the distance measured with the right infrared sensor and b
denotes the absolute distance between the two sensors. For the x-component of the
position vector I r of the quadrotor the following expression holds
dsharpL + dsharpR
x=
+ l cos()
(6.2)
2
where l represents the gap between the C.G. and the sensors.
The y-component is derived in a similar way, please note since only one sensor
is used for the measurement of the side distance, the sideways displacement has to
be taken into account. The absolute distance between the infrared sensor and the
C.G. is denoted with dr . Remark: the sensor is assumed to be mounted exactly on
the quadrotor arm, thus the angle between the sensor and the y-direction is equal
to 45 . The side distance is expressed through
!
2
2
y = dsharpS +
dr cos()
dr sin(),
(6.3)
2
2
where the sideways measured distance is represented by dsharpS .
The z-component is simply the negative value of the measured value by the
ultra sonic sensors, since the 3rd axis is pointing downwards. Please note that the
21
22
ultra sonic sensor is insensitive to little tilt angles, due to the fact that it selects
the smallest distance of the measure conus for its altitude measurement.
6.2
6.2.1
Coordinate System
Experiments with the AIRobots quadrotor test platform have shown, that the AscTec attitude controller does not satisfy the definition of the body frame as stated
in section 3.2. In fact, it corresponds to the definition for the y-axis, but the x-axis
is pointing towards the opposite direction, what is probably caused by a right-hand
coordinate frame with a third axis pointing in direction of the thrust vector (not
downwards). Within this project the body coordinate frame, as defined in section
3.2, was applied. To avoid any problems with the attitude controller, the sign of
the input and output roll angle were changed.
6.2.2
According to the manual of AscTec [Asc10], the estimated angles of the attitude
controller are stored in an unsigned 8-bit integer value with the resolution of a tenth
of a degree. An unsigned 8-bit integer data type provides a range of [0...255]
where the neutral point is defined to be at 127.
The input command to the attitude controller, i.e. the desired euler angles, are
stored in an unsigned 8-bit integer value as well. Its resolution is not explicitly
indicated in the manual [Asc10]. Through experiments the resolution of a third of
a degree was found, see Figure 6.1.
Estimate Pitch command conversion
40
40
measurement
fitted conversion
measurement
fitted conversion
20
Measured roll angle [deg/10]
20
20
40
60
80
110
20
40
60
115
120
125
130
Input command [uint8]
135
140
80
110
115
120
125
130
Input command [uint8]
135
140
6.2.3
Angle Offsets
The hovering plane of the quadrotor does not exactly correspond to the plane of the
attitude controller. Hence biases arise between the quadrotors euler angles and the
attitude controllers euler. With different experiments the offsets were estimated
and subtracted from the corresponding position controller outputs, the resulting
offsets are denoted in appendix A.4. As a possible improvement, an estimation
process, which varies the input commands automatically, is conceivable.
Chapter 7
A quarter of the flight data gained during manual controlled flight (see appendix
C) was used for the parameter identification during the system modeling process,
described in chapter 4. The other three quarters were used for the model validation,
i.e. comparing the flight data with the response of the model stimulated with the
corresponding input data.
The Figures (7.1 - 7.6) show the results of the system validation. Please note
that, for a comparison between the simulated acceleration values and the acceleration values measured by the IMU a coordination transformation from the body
frame into the inertial frame was required, the transformation matrix is provided
in section 3.2.3.
xacc I [m/s2]
accleration [m/s2]
1
measured
simulated
0.5
0
0.5
1
2
10
time [sec]
12
14
16
18
yacc I [m/s2]
accleration [m/s2]
0.5
measured
simulated
0
0.5
1
2
10
time [sec]
12
14
16
18
zacc I [m/s2]
accleration [m/s2]
1
measured
simulated
0.5
0
0.5
1
2
10
time [sec]
12
14
16
18
20
23
24
pitch angle
0.05
[rad]
measured
simulated
0
0.05
2
10
12
14
16
18
roll angle
measured
simulated
[rad]
0.05
0.05
10
12
14
16
18
yaw rate
0.3
measured
simulated
[rad/s]
0.2
0.1
0
0.1
0.2
2
10
12
14
16
18
xacc I [m/s2]
accleration [m/s2]
1
measured
simulated
0.5
0
0.5
1
0
10
15
time [sec]
20
25
30
yacc I [m/s2]
accleration [m/s2]
1
measured
simulated
0
2
0
10
15
time [sec]
20
25
30
zacc I [m/s2]
accleration [m/s2]
4
measured
simulated
2
0
2
4
0
10
15
time [sec]
20
25
30
25
pitch angle
0.1
measured
simulated
[rad]
0.05
0
0.05
0.1
0
10
15
20
25
30
roll angle
0.2
measured
simulated
[rad]
0.1
0
0.1
0.2
0
10
15
20
25
30
yaw rate
1
[rad/s]
measured
simulated
0.5
0.5
0
10
15
20
25
30
xacc I [m/s2]
accleration [m/s2]
measured
simulated
2
2
10
time [sec]
12
14
16
18
yacc I [m/s2]
accleration [m/s2]
measured
simulated
2
2
10
time [sec]
12
14
16
18
zacc I [m/s2]
2
accleration [m/s2]
measured
simulated
1
0
1
2
2
10
time [sec]
12
14
16
18
26
pitch angle
[rad]
0.1
0
measured
0.1
simulated
2
10
12
14
16
18
roll angle
0.1
[rad]
0
0.1
measured
0.2
simulated
2
10
12
14
16
18
yaw rate
[rad/s]
0.2
0
measured
0.2
simulated
2
10
12
14
16
18
As can be seen in the plots of the validation, the dynamical model evolved in
chapter 4 approximates the behavior of the real plant of the helicopter quite well.
Especially for the roll and pitch dynamics great results were achieved, but also for
the acceleration in all directions the obtained results are satisfying. For the rate of
the yaw angle, the simulated result is a lot less noisy than the measured signal, but
contains an offset.
7.2
Flight Control
In the second part of this chapter, the result of the autonomous controlled flights
are presented.
7.2.1
Hovering Flight
The Figures (7.7 - 7.9) show the results of a hovering flight at a constant set-point at
T
front x
side y
altitude x
heading
desired value
RMS
0.3m
0.5m
0.3m
0
0.0207m
0.0316m
0.0117m
0.1245
Table 7.1: Results from Flight Control: RMS values of a hovering flight
27
0.24
x estimated
ref value
0.26
x [m]
0.28
0.3
0.32
0.34
0.36
35
40
45
50
55
time [sec]
60
65
70
75
80
0.4
y estimated
ref value
y [m]
0.45
0.5
0.55
0.6
35
40
45
50
55
time [sec]
60
65
70
75
80
Figure 7.7: Results from Flight Control: hovering flight (front and side distance)
0.26
z measured (US sensor)
ref value
altitude [m]
0.28
0.3
0.32
0.34
0.36
35
40
45
50
55
time [sec]
60
65
70
75
7.2.2
Step responses
The following plots (Figure 7.10 - 7.11) illustrate the response of the quadrotor to
a step-signal as its input. As can be seen, the controller reacts on a step of 30
centimeters in a horizontal direction approximately in 3 seconds, without a huge
overshoot. The performance of the altitude controller is even slightly better.
In Figure 7.12 the system response to a squared reference trajectory, is shown.
As can be seen, the designed position controller allows the quadrotor to follow the
simple trajectory. Please note, for more complex maneuvers and a higher performance an extension of the position controller would be required, which was beyond
the scope of this thesis, see section 8.2.
28
0.4
yaw measured
ref value
heading angle [deg]
0.2
0.2
0.4
35
40
45
50
55
time [sec]
60
65
70
75
80
Figure 7.9: Results from Flight Control: hovering flight (heading angle)
0.2
x estimated
ref value
0.3
0.2
x [m]
x [m]
0.4
0.4
0.5
0.6
x estimated
0.6
ref value
0.8
0.7
165
170
175
180
time [sec]
185
190
120
195
0.2
125
130
time [sec]
135
140
0.2
y estimated
ref value
y estimated
ref value
0.4
y [m]
y [m]
0.4
0.6
0.8
0.6
0.8
1
165
170
175
180
time [sec]
185
190
195
120
125
130
time [sec]
135
140
Figure 7.10: Results from Flight Control: step response (front and sideways)
0.2
0.2
0.3
altitude [m]
0.6
0.8
0.4
0.5
0.6
z measured (US sensor)
0.7
ref value
0.8
1
110
115
120
125
130
135
140
145
170
175
180
185
time [sec]
190
195
time [sec]
200
205
210
215
0.1
0.2
altitude [m]
0.4
0.3
0.4
0.5
0.6
0.7
0.8
0.2
0.4
0.25
0.3
0.6
0.4
0.35
0.45
0.5
0.8
y direction (side) [m]
0.55
1
0.6
0.65
Chapter 8
Conclusion
The simulator evolved during this project (chapter 4) represents the dynamics of the
real quadrotor system quite well. Thus, in MATLAB/Simulink derived controllers
can be applied to the real platform without retuning.
The implementation (as described in chapter 6) of the position controller on
the quadrotor was more complex than assumed, e.g. caused by the rough bias
approximation of the attitude controller input signal or by the roughly estimated
input conversion. Although the controller, based on the optimal control approach,
allows the quadrotor to hover in a smoother way than the former PID approach, it
still did not turn out as smooth as priorly expected.
8.2
Outlook
As outlined above, the interface between the inner and outer control loops is crucial
for a good flight control performance and offers room for improvement.
Although, the AIRobots quadrotor equipped with the position controller, derived within this project, allows a step-by-step replacement of single degrees of
freedom with vision-based control algorithms.
An extension of the LQRI controller with a feed-forward addition, as proposed
by Guzzella [Guz09], may improve the performance of tracking a desired reference
signal, i.e. not only hovering on a given set point. Furthermore other control
approaches, as H or nonlinear control algorithms as back-stepping are conceivable
to fly more complex maneuvers.
29
30
Appendix A
Parameters
A.1
Model Parameters
Ppitch (s) =
es0.06
Proll (s) =
13.11
s+9.404
es0.06
es0.22
Pthrust (s) =
3.265
s+77.23
es0.02
Point-Mass Model
mass m
gravity constant g
A.2
0.536 kg
9.81 sm2
Controller Parameters
Appendix A. Parameters
A.3
70
front
150
150
side
150
150
yaw
0.2
alt
A.4
Angle Offsets
command
pitch
32
bias
1.996
roll
0.7047
yaw
1.667
Appendix B
Matlab/Simulink
Documentation
B.1
All MATLAB source files can be found in the folder src, which is structured in the
following way
flightdata
HquadModel
IR_control_JN
IR_control_lqr
IR_control_Model_Identification
B.2
Pleas note that, all Simulink models are stored in the Simulink library files
SimulinkModel_Hquad_submodels_LIB.mdl respectively
SimulinkModel_Hquad_compmodel_LIB.mdl, thus the single blocks can easily be
added to any Simulink model with drag and drop. Global changes can only be
accomplished in those library files.
B.2.1
Submodel Identification
B.2.2
34
B.3
Position Control
This section gives a short overview of the contains of the folder IR_control_lqr,
which is used for the position control task. The folder has the following sub-folders
Hquad_CTRL_OBSV_parameters
contains the derived parameters of the controllers and observers
plot_hquad_val
contains plots of the autonomous flights
test_IMU
contains the experiments of the input conversion 6.2.2
furthermore it contains some help functions and files which are not depict in the
following.
m-Files
In the following, the most important files, for the derivation of the LQRI position
controller, are briefly introduced
lqr_main.m
main file for the position control
lqr_sim.m
runs the simulator
lqr_alt_calc_ctrl.m
to calculate the altitude controller
lqr_dist_calc_ctrl.m
to calculate the front and side distance controllers
lqr_yaw_calc_ctrl.m
to calculate the heading controller
lqr_obsv_calc.m
to calculate the state observers
flight_results_analyze.m
for the analyze and illustration of the flight data sets
35
Simulink Library-Files
Hquad_model_LIB.mdl
provides the models of the quadrotors dynamics, used for the simulator.
Hquad_control_LIB.mdl
contains all decoubled controllers.
Simulink Model-Files
host-models
This files are used for on-line recording and logging during the flight with the
quadrotor. The host-models run on a PC-station and are wireless (XBee)
connected to the helicopter.
QuadIf-models
The Simulink models for the position controller. The directly built and compiled code from these models runs on the embedded system on the quadrotor;
for more infromation about the code generation see section 2.3.
Simulator_hquad_lqr-models
To test the derived controllers without the real platform. Two simulators are
provided, one for a simulation without an observer, one for tests with the
states estimation.
36
Appendix C
model
model
model
model
validation
identification
validation
validation
flight_2010_04_21/flight2.mat
flight_2010_04_22/flight3.mat
37
38
Bibliography
[Asc]
[Asc10]
[BNS04]
[BS05]
S. Bouabdallah and R. Siegwart. Backstepping and Sliding-mod Techniques Applied to an Indoor Micro Quadrotor. International Conference
on Robotics and Automation, pages 22472252, April 2005.
[BS07]
S. Bouabdallah and R. Siegwart. Advances in Unmmande Aerial Vehicles, chapter Design and Control of a Miniature Quadrotor. Springer,
2007.
[BSWS]
M. Bl
osch, D. Scaramuzza, S. Weiss, and R. Siegwart. Vision Based
MAV Navigation in Unknown and Unstructured Environments. not yet
published.
[DW01]
[Glo07]
[GS03]
[Guz09]
[LK92]
C. Roos. Design of a Collision-Tolerant Airframe for Quadrotors, December 2009. Semester Thesis.
[Sha03]
E. Shafai. Einf
uhrung in die Adaptive Regelung. IMRT Press, ETH
Zurich, 2003.
[SNFH05] J. Shin, K. Nonami, D. Fujiwara, and K. Hazawa. Model-based optimal attitude and postioning control of small-scale unmanned helicopter.
Robotica, 23:5163, 2005.
39
Bibliography
40
[SP96]
S. Skogestad and I. Postlethwaite. Multivariable feedback control analysis and design. New York, 1996.
[TR05]