Institutionen för systemteknik Department of Electrical Engineering Examensarbete

Institutionen för systemteknik Department of Electrical Engineering Examensarbete
Institutionen för systemteknik
Department of Electrical Engineering
Examensarbete
Modelling and control of a hexarotor UAV
Examensarbete utfört i Reglerteknik
vid Tekniska högskolan vid Linköpings universitet
av
Simon Lindblom & Adam Lundmark
LiTH-ISY-EX--15/4844--SE
Linköping 2015
Department of Electrical Engineering
Linköpings universitet
SE-581 83 Linköping, Sweden
Linköpings tekniska högskola
Linköpings universitet
581 83 Linköping
Modelling and control of a hexarotor UAV
Examensarbete utfört i Reglerteknik
vid Tekniska högskolan vid Linköpings universitet
av
Simon Lindblom & Adam Lundmark
LiTH-ISY-EX--15/4844--SE
Handledare:
Daniel Simon
isy, Linköpings universitet
Staffan Sjöqvist
Intuitive Aerial
Examinator:
Johan Löfberg
isy, Linköpings universitet
Linköping, 11 juni 2015
Avdelning, Institution
Division, Department
Datum
Date
Division of Automatic Control
Department of Electrical Engineering
SE-581 83 Linköping
2015-06-11
Språk
Language
Rapporttyp
Report category
ISBN
Svenska/Swedish
Licentiatavhandling
ISRN
Engelska/English
Examensarbete
C-uppsats
D-uppsats
—
LiTH-ISY-EX--15/4844--SE
Serietitel och serienummer
Title of series, numbering
Övrig rapport
ISSN
—
URL för elektronisk version
http://urn.kb.se/resolve?urn=urn:nbn:se:liu:diva-XXXXX
Titel
Title
Modellering och reglering av en hexarotor
Författare
Author
Simon Lindblom & Adam Lundmark
Modelling and control of a hexarotor UAV
Sammanfattning
Abstract
This thesis is a study of modelling and control of a multirotor unmanned aerial vehicle
(UAV). On behalf of Intuitive Aerial, a model of their hexarotor aircraft has been developed
as a tool in further development and testing of their product. The potential of using Model
Predictive Control (MPC) as control method for multirotor UAV:s has also been evaluated.
The model was successfully implemented in MATLAB/Simulink, as was the Model Predictive Controller. Quaternion angle representation has been used to avoid singularities in the
model and non-linear dynamics have been included in the simulation model. Unknown
model parameters have been estimated with data acquired from testing. Model validity has
also been evaluated with flight data gathered from flights using a test vehicle. A basic MPC
formulation has been expanded to include reference tracking, integral action and to ensure
stability.
Assessment proved the model to be feasible but in need of more rigorous evaluation to guarantee good performance. The MPC controller showed promising performance compared to
a linear feedback controller.
Nyckelord
Keywords
multirotor, hexarotor, hexacopter, mpc, automatic control, modelling
Abstract
This thesis is a study of modelling and control of a multirotor unmanned aerial
vehicle (UAV). On behalf of Intuitive Aerial, a model of their hexarotor aircraft
has been developed as a tool in further development and testing of their product.
The potential of using Model Predictive Control (MPC) as control method for
multirotor UAV:s has also been evaluated.
The model was successfully implemented in MATLAB/Simulink, as was the Model
Predictive Controller. Quaternion angle representation has been used to avoid
singularities in the model and non-linear dynamics have been included in the
simulation model. Unknown model parameters have been estimated with data
acquired from testing. Model validity has also been evaluated with flight data
gathered from flights using a test vehicle. A basic MPC formulation has been
expanded to include reference tracking, integral action and to ensure stability.
Assessment proved the model to be feasible but in need of more rigorous evaluation to guarantee good performance. The MPC controller showed promising
performance compared to a linear feedback controller.
iii
Acknowledgments
First off we would like to thank everyone at Intuitive Aerial for giving us the
chance to work with this thesis during these months. In particular we would like
to devote a few lines to our fellow adviser Staffan Sjöqvist at IA, who patiently has
had the time to contribute with invaluable advise even during his busiest hours.
Furthermore we would like to thank Daniel Simon, our academic supervisor at
ISY, who, with his approachability and knowledge, has been of great assistance
to us. Our examiner, Johan Löfberg, is also highly deserving of our gratitude for
always being available and courteous.
For the amazing five long, but short, years at LiU we would like to thank our
good friends and colleagues for maintaining unity and a good atmosphere, and
for some energising fiddling, even while Rome was burning.
Simon Lindblom
To my mother and father who, unconditionally, have supported me along the way.
To my sisters and my brother who, with great perseverance, have shared with me
much banter, energy and joy. To the rest of my family, by blood or by heart, who
have, in whatever way, helped me forward. Thank you all for standing by my side
and for shaping my academic days to the better.
Adam Lundmark
This is the end of the road. This is what I have, more or less intentionally, worked
for a long time. Every person I have met and shared moments with, good and
bad, have implicitly helped me down this road. You, whomever you may be, have
been part of this. That is why I would like to extend my sincere gratitude to you
from the bottom of my heart.
Linköping, June 2015
Simon Lindblom and Adam Lundmark
v
Contents
Notation
1 Introduction
1.1 Background . . . . .
1.2 Problem formulation
1.3 Limitations . . . . . .
1.4 Related work . . . . .
1.5 Thesis outline . . . .
xi
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
1
1
2
2
3
3
2 Mathematical Preliminaries
2.1 Coordinate systems . . . . . . . . . .
2.2 Kinematics . . . . . . . . . . . . . . .
2.3 Rotation matrices . . . . . . . . . . .
2.4 Quaternions . . . . . . . . . . . . . .
2.4.1 Introduction . . . . . . . . . .
2.4.2 Quaternion properties . . . .
2.4.3 Rotation in three dimensions
2.4.4 Dynamics of quaternions . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
5
5
6
6
9
9
10
11
12
3 Hardware Systems
3.1 Aerigon system . . . . . .
3.1.1 Aerigon helicopter
3.1.2 Aerigon gimbal . .
3.2 Test vehicle . . . . . . . . .
3.3 Test rig . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
13
13
14
14
15
16
.
.
.
.
.
.
17
17
17
18
22
22
23
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
4 Hexarotor Model
4.1 Modelling preliminaries . . . . . .
4.1.1 Rotor specifics . . . . . . . .
4.1.2 Basic manoeuvring concepts
4.2 States . . . . . . . . . . . . . . . . .
4.3 Forces . . . . . . . . . . . . . . . . .
4.3.1 Gravity . . . . . . . . . . . .
vii
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
viii
Contents
4.3.2 Thrust force . . . . . . . . . . . .
4.3.3 Complete force description . . .
4.4 Torques . . . . . . . . . . . . . . . . . . .
4.4.1 Torque induced by gravity . . . .
4.4.2 Torque induced by thrust forces .
4.4.3 Reaction torque . . . . . . . . . .
4.4.4 Inertial counter torque . . . . . .
4.4.5 Gyroscopic rotor effects . . . . . .
4.4.6 Complete torque description . . .
4.5 The complete non-linear dynamic model
4.6 Linearisation . . . . . . . . . . . . . . . .
5 Control Strategies
5.1 Control structure . . . . .
5.2 Model Predictive Control .
5.2.1 Linear MPC . . . .
5.2.2 Stability . . . . . .
5.2.3 Reference tracking
5.2.4 Integral action . . .
5.2.5 Final formulation .
5.3 Linear Quadratic Control .
5.3.1 Reference tracking
5.3.2 Integral action . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
23
23
24
25
26
26
27
28
28
29
29
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
33
33
33
34
35
35
36
37
37
38
38
6 Parameter estimation
6.1 Thrust coefficient estimation . .
6.1.1 Single setup . . . . . . .
6.1.2 Coaxial setup . . . . . .
6.1.3 Test vehicle single setup
6.2 Torque coefficient estimation . .
6.2.1 Single setup . . . . . . .
6.2.2 Coaxial setup . . . . . .
6.2.3 Test vehicle single setup
6.3 Motor dynamics estimation . .
6.3.1 Motor model estimation
6.3.2 Motor model validation
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
39
39
40
41
42
43
43
44
44
46
47
48
7 Results
7.1 Simulation environment . . . . .
7.2 Model assessment . . . . . . . . .
7.3 Analysis of the linear system . . .
7.4 Control evaluation . . . . . . . . .
7.4.1 Controller design choices .
7.4.2 Flight simulations . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
51
51
53
58
60
60
63
8 Conclusions and Future Work
8.1 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
73
73
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
Contents
8.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
ix
74
A Linearisation
77
B Analytical QP-formulation of MPC
81
Bibliography
85
Notation
Abbreviations
Abbreviation
cog
cpu
esc
ia
lq
mpc
pid
pwm
rpm
vtol
Definition
Center of gravity
Central processing unit
Electronic speed controller
Intuitive Aerial
Linear Quadratic Control
Model Predictive Control
Proportional, integral, differential (regulator)
Pulse-width modulation
Revolutions per minute
Vertical take-off and landing
xi
xii
Notation
Notations
Notation
E
B
N
ξE
ηE
φ
θ
ψ
VB
ωB
R
Q
m
g
CT
ρ
A
rprop
Ω
T
τ
l
h
N
U
e
0
I
kxk2
kxk2W
Definition
Earth fixed coordinate system
Body fixed coordinate system
Natural numbers
Translational position in E
The orientation of B in relation to E
Roll angle
Pitch angle
Yaw angle
Linear velocity of the hexarotor
Angular velocity of the hexarotor
Rotation matrix
Quaternion rotation matrix
Overall mass
Gravitational acceleration
Thrust coefficient
Air density
Propeller disk area
Rotor radius
Propeller angular velocity
Thrust force
Torque
Arm length
Vertical distance from origin to propeller center
Prediction horizon
Control inputs
Unit vector
Zero vector
Identity matrix
Euclidean norm
Weighted quadratic function, kxk2W = xT Wx
1
Introduction
This master’s thesis involves modelling and control of an unmanned aerial vehicle (UAV) in a simulation environment. The vehicle is a multirotor which is a
rotorcraft with more than two rotors. Its rotors are fixed so the multirotor’s direction of thrust, and thus its translational motions, cannot be controlled by tilting
the rotors. This is instead achieved by varying each rotor’s angular rate.
This introductory chapter will describe the background, goals and proceedings
of this thesis, which was performed with the cooperation of Intuitive Aerial (IA).
1.1
Background
In recent years the interest in UAV:s in general, and multirotors in particular,
has increased significantly due to their many fields of application. The idea of
vertical take-off and landing (VTOL) vehicles, however, has existed for a long
time and the very first VTOL vehicles were created in the 1920s [Leishman, 2006].
These designs were unsuccessful due to their limited performance but when the
hardware development eventually caught up with the theory both helicopters
and later multirotors became highly popular fields of research and subsequently
also used in practice.
Multirotors are versatile mainly due to their simple structure and control. This
makes them suitable vehicles for applications in many areas, such as surveillance,
media, transportation, hobbyism and for operations in difficult or otherwise dangerous terrain. While multirotors are already capable vehicles in some areas there
is still much room for improvement, such as better stability, control, power and
operation time. Another interesting aspect of UAV:s is autonomous flight which
most likely will broaden the vehicles’ application area even more.
1
2
1
Introduction
Intuitive Aerial is developing airborne camera platforms. Their current platform,
which is in focus in this thesis, is the Aerigon system. Swedish legislation does
not allow fully autonomous flight of these kinds of aircraft at present. However,
to follow predefined paths could be interesting for IA in the future. For this
to be possible the flight control system of the vehicle should operate with high
accuracy. Testing and evaluation of new control algorithms would be simplified
if a simulation environment was available. To quote Mårten Svanfeldt at IA, "It’s
easy to make something fly, but it’s hard to make something fly well".
1.2
Problem formulation
The task of this thesis is to develop a simulation environment in which the flight
control performance of an unmanned multirotor can be tested. Such a simulation
environment will create a cheap and easy way of testing, and thus improving,
the aircraft’s performance under certain given conditions. To achieve this it is
necessary to create a model for the multirotor. It is also vital to implement a flight
control algorithm that can control the model in the simulation environment.
The multirotor is based on Intuitive Aerial’s Aerigon system and it needs to be
modelled accurately in order to give a fair representation of reality. The model
should be dynamic and adjustable and consider the fact that the centre of gravity
is not necessarily in the centre of lift.
The simulation environment should provide a visual representation of the vehicle’s movements during the simulation. Additionally it should be able to simulate
and show effects of turbulence, wind, atmospheric conditions, GPS accuracy and
latency of sensor data.
The flight control algorithm will control and fly the model in the simulation. It
should be implemented as a Model Predictive Controller (MPC) which should
consider the structural and aerodynamic limitations of the helicopter. An investigation of the suitability of MPC controllers in multirotor applications should
also be performed.
1.3
Limitations
The simulation environment will be created in MATLAB Simulink. Since the
Aerigon system is a complex machine it is not possible to model all of its dynamics during the time period assigned to this thesis. Therefore the focus will be on
modelling and controlling the most basic dynamic behaviour and successively
adding more complexity as the work progresses. Other subjects, such as signal
processing, will have to be touched upon to reach the modelling and control goals.
Such subjects will only be used as tools and will not be explained in detail, as that
would deviate too much from the purpose of this thesis.
1.4
Related work
1.4
3
Related work
Since multirotors has been a popular field of research for the past decade there
are many papers regarding modelling, simulation and control of them. However,
as the field of research is quite young, there is still a lot of room for improvement.
This is partly what makes it such an interesting field to investigate.
The most thoroughly researched field concerning multirotors is modelling. Whereas control algorithms are to some extent limited to function within the limitations of in-flight hardware, the modelling of multirotors is not. Therefore most
multirotor papers include a more or less detailed mathematical dynamic model
description. Some overall thorough modelling sections can be found in [Bouabdallah, 2007, Rinaldi et al., 2013] and the master’s theses [Barsk, 2012, Bergman
and Ekström, 2014, Magnusson, 2014]. Other literature focuses on the modelling
of specific dynamics. The dynamics of rotors is handled in [Leishman, 2006, Bristeau et al., 2009] and the modelling of a multirotor with an off-centred centre of
gravity is treated in [Raharijaoana and Bateman, 2013].
The controller most commonly used in literature is a
Proportional-Integral-Derivative (PID) controller as its simple algorithm and adequate performance often is desirable in multirotor applications. Authors such
as [Pounds et al., 2010, Magnusson, 2014], among many others, have achieved
PID controllers that provide promising results on multirotor platforms. A controller that is a bit more complex and also a viable option for multirotors is the
Linear Quadratic (LQ) controller. Such a controller in a multirotor application is
discussed in [Rinaldi et al., 2013, Liu et al., 2013]. An even more complex control strategy is the MPC controller and its basic structure is treated in [Simon,
2014]. MPC controllers that are specifically designed and used for multirotor applications are discussed in [Alexis et al., 2011, Barsk, 2012, Abdolhosseini et al.,
2013]. Additionally, [Bouabdallah, 2007] compares many control concepts for
multirotors, among them a PID and an LQ controller.
1.5
Thesis outline
The thesis is structured as follows:
• Chapter 2 presents coordinate systems and definitions that are used in the
rest of the thesis.
• Chapter 3 describes the hardware that was used for testing and verification
during the thesis.
• Chapter 4 describes the development of the non-linear dynamic model of
the hexarotor.
• Chapter 5 presents the controllers used to control the hexarotor in the simulation environment.
4
1
Introduction
• Chapter 6 describes how the estimation of certain unknown coefficients and
properties was executed.
• Chapter 7 contains the results of the thesis.
• Chapter 8 summarizes the thesis with conclusions that could be drawn from
the results.
2
Mathematical Preliminaries
This chapter acts as a theoretical foundation for the entire thesis as it introduces
coordinate systems, definitions and concepts that are used throughout.
2.1
Coordinate systems
Two coordinate systems are needed to describe the motion of a hexarotor: an
earth fixed frame and a body fixed frame. These are illustrated in Figure 2.1.
Figure 2.1: The two coordinate systems that are used to describe the hexarotor’s motions.
5
6
2
Mathematical Preliminaries
The earth fixed frame, E, is denoted with subscript E and is a system that uses
NED (North, East, Down) coordinates. Its origin, denoted OE , is fixed on the
earth’s surface in the hexarotor’s starting position. Since the earth fixed frame
acts as an inertial frame the hexarotor’s absolute linear position can be defined in
this frame. The axes are denoted XE , YE and ZE respectively and their directions
are shown in Figure 2.1.
The body fixed coordinate system, B, is denoted with subscript B and its origin
OB is fixed in the centre of the hexarotor’s airframe. Consequently, the body
fixed frame moves relative to the earth fixed frame when the hexarotor moves.
The body fixed x-axis points in the forward direction, the y-axis points to the
right and the z-axis points downwards. These axes are denoted XB , YB and ZB
respectively and are shown in Figure 2.1. This is the body fixed coordinate system
convention used by IA and is thus also used in this thesis.
2.2
Kinematics
The position of the body fixed frame in the earth fixed frame is defined as
ξ E = (x, y, z)T
(2.1)
and the body frame’s orientation in relation to the earth frame is described by the
vector
η E = (φ, θ, ψ)T
(2.2)
where the angles φ, θ and ψ are called roll, pitch and yaw respectively. These
angles are the Euler rotation angles.
The linear velocities of the hexarotor in the body frame are defined as
V B = (u, v, w)T
(2.3)
and its angular velocities are defined as
ω B = (p, q, r)T .
2.3
(2.4)
Rotation matrices
In flight mechanics, the Euler angles are often used for transformations between
coordinate systems. These transformations are achieved with rotation matrices
which consist of terms of Euler angles. A multiplication of a rotation matrix and
a vector in one coordinate system transforms that vector to another coordinate
system. This section presents the rotation matrices that transform linear quantities between the two coordinate systems E and B. Note that the notation c is
used for cos, s for sin and t for tan.
Consider that the hexarotor has changed its roll, pitch and yaw angle in relation
to the earth fixed frame. To describe this rotation each angle rotation is treated
2.3
7
Rotation matrices
successively. A rotation about the earth fixed z-axis by a yaw angle ψ results in
the new reference frame A0 . Thus,
 
  
xA0 
xE   cψ
 0 
  
 yA  = R(ψ)  yE  = −sψ
 
  
z A0
zE
0
sψ
cψ
0
 
0 xE 
 
0  yE  .
 
1 zE
(2.5)
Similarly, a rotation about the YA0 axis by a pitch angle θ results in the new reference frame A00 which gives

  
 

xA00 
xA0  cθ 0 −sθ  xA0 
 
 yA00  = R(θ)  yA0  =  0 1
0   yA0  .
(2.6)


  
 
zA00
zA0
sθ 0 cθ zA0
A rotation about XA00 , which coincides with XB , by a roll angle φ then yields the
body fixed frame with the relation
 

 


0
0  xA00 
xB 
xA00  1
 yB  = R(φ)  yA00  = 0 cφ sφ  yA00  .
(2.7)
 

 

 
zB
zA00
0 −sφ cφ zA00
All of the above described rotations are illustrated in Figure 2.2.
(a) A rotation about ZE
with yaw angle ψ yields
the new reference frame
A0 .
(b) A rotation about YA0
with pitch angle θ yields
the new reference frame
A00 .
(c) A rotation about
XA00 with roll angle φ
finally yields the body
fixed frame B.
Figure 2.2: Illustrations of the subsequent rotations used to transform a linear quantity from E to B.
Finally, equations (2.5)-(2.7) yield
 
  
cθcψ
xB 
xE  
 yB  = REB  yE  = sφsθcψ − cφsψ
 
  
zB
zE
cφsθcψ + sφsψ
cθsψ
sφsθsψ + cφcψ
cφsθsψ − sφcψ
 
−sθ  xE 
 
sφcθ   yE 
 
cφcθ zE
(2.8)
8
2
Mathematical Preliminaries
where REB = R(φ)R(θ)R(ψ) is the rotation matrix that transforms a linear quantity from earth fixed coordinates to body fixed coordinates. The inverse of REB
yields the rotation matrix RBE that transforms linear quantities from body fixed
coordinates to earth fixed coordinates. Since REB is orthogonal its inverse is given
by its transpose, which gives
T
RBE = R−1
EB = REB

cθcψ

=  cθsψ

−sθ
sφsθcψ − cφsψ
sφsθsψ + cφcψ
sφcθ

cφsθcψ + sφsψ 

cφsθsψ − sφcψ  .

cφcθ
(2.9)
The translational velocities of the body frame are linear. The transformation of
these velocities from the body frame to the earth frame is thus described by
ξ̇ E = RBE V B .
(2.10)
However, the transformation from body frame to earth frame is not as straightforward for ω B as for V B . The approach used to calculate the relationship between
η̇ E and ω B is similar to the previous rotation case.
Initially, it is worth noting that the earth fixed frame does not rotate, resulting in
 
0
 
ω E = 0 .
 
0
(2.11)
Firstly, a rotation about the body fixed z-axis by an angle ψ yields an angular
velocity ψ̇. The angular velocity in the reference frame A0 is thus given by
ω A0
   

 0    0 

   
= R(ψ)ω E +  0   =  0  .
 
 
ψ̇
ψ̇
(2.12)
Secondly, by adding an angular velocity in the pitch angle
ω A00

  

 0   −ψ̇sθ 






= R(θ)ω A0 + θ̇   =  θ̇  .

 

0
ψ̇cθ
(2.13)
Finally, the relationship between ω B and η̇ E is given by

  


φ̇   φ̇ − ψ̇sθ





ω B = R(φ)ω A00 +  0   =  θ̇cφ + ψ̇sφcθ  = Rvel,EB η̇ E
 


0
−θ̇sφ + ψ̇cφcθ
where
(2.14)
2.4
9
Quaternions
Rvel,EB

1

= 0

0
0
cφ
−sφ

−sθ 

sφcθ 

cφcθ
(2.15)
is the transfer matrix that transforms the time derivatives of the Euler angles in
the earth fixed frame to the angular velocities in the body fixed frame. Inversion
of Rvel,EB gives
Rvel,BE

1

= 0

0
sφtθ
cφ
sφ
cθ

cφtθ 

−sφ 

cφ 
(2.16)
cθ
which relates ω B and η̇ E as
η̇ E = Rvel,BE ω B .
(2.17)
The matrix (2.16) includes four divisions by cos(θ), two of which in tan(θ). This
is not desirable since it results in a singularity in θ = ±π/2. This singularity is
often called gimbal lock. This is usually not an issue because if the multirotor is
pointing straight upwards or downwards something is already very wrong. However, being able to safely test these extreme cases is one of many benefits of using
a simulation environment. A more robust alternative is therefore needed when
modelling the hexarotor’s motions. Such an approach is presented in Section 2.4.
2.4
Quaternions
This section gives a brief introduction of quaternions and describes basic properties and uses in rotation representation. The theory of this section can be seen in
[Kuipers, 1999].
2.4.1
Introduction
In 1844 Sir William Rowan Hamilton extended the imaginary space with two
additional complex numbers and thus creating quaternions [Hamilton, 1847]. At
the time, Sir Hamilton did not know that quaternions are a good way to represent
rotations in a three dimensional space. In difference to Euler angles quaternions
do not suffer from discontinuities and give a more unique solution. A quaternion
is defined as a scalar part which is real, and vector part with components in the
complex space as q = q0 + iq1 + jq2 + kq3 , or as


 q0 
!
 iq 
q0
 1 
q = 
=
 jq2 
q


kq3
(2.18)
10
2
Mathematical Preliminaries
which can be seen as a quadruple real number as an element of R4 , from here on
denoted
 
q0 
q 
 
q =  1  , or q = q0 + q
q2 
 
q3
(2.19)
where q0 , q1 , q2 and q3 are real numbers or scalars [Kuipers, 1999].
2.4.2
Quaternion properties
A crucial part of Hamilton’s discovery was the rule
i2 = j 2 = k2 = ijk = −1
(2.20)
or so called quaternion products. Further results of (2.20) gives the relations
ij = k = −ji,
jk = i = −kj,
(2.21)
ki = j = −ik.
Because of the properties of (2.20) and (2.21) quaternion multiplication (denoted
) is not commutative. Consider two arbitrary quaternions q = q0 + iq1 + jq2 + kq3
and p = p0 + ip1 + jp2 + kp3 . Then the product of these two can be written as
p q = (p0 + ip1 + jp2 + kp3 )(q0 + iq1 + jq2 + kq3 ) =
= p0 q0 + ip0 q1 + jp0 q2 + kp0 q3 +
ip1 q0 + i2 p1 q1 + ijp1 q2 + ikp1 q3 +
jp2 q0 + ijp2 q1 + j 2 p2 q2 + jkp2 q3 +
kp3 q0 + ikp3 q1 + jkp3 q2 + k2 p3 q3 =
= p0 q0 − (p1 q1 + p2 q2 + p3 q3 ) +
p0 (iq1 + jq2 + kq3 ) + q0 (ip1 + jp2 + kp3 ) +
i(p2 q3 − p3 q2 ) + j(p3 q1 − p1 q3 ) + k(p1 q2 − p2 q1 ).
On close inspection one can identify the scalar product (p · q = p1 q1 + p2 q2 + p3 q3 )
and cross product (p × q = i(p2 q3 − p3 q2 ) + j(p3 q1 − p1 q3 ) + k(p1 q2 − p2 q1 )) of the
complex vectors p and q. In short, the quaternion product is defined as
pq=
!
!
!
p0
q
p0 q 0 − p · q
0 =
.
p
q
p0 q + q0 p + p × q
(2.22)
2.4
11
Quaternions
Other properties of the quaternion are the complex conjugate
q = q0 + q ⇔ q∗ = q0 − q,
(q p)∗ = q∗ p∗ ,
(2.23)
the norm
p
and the inverse
2.4.3
q∗ q = kqk2 ,
q−1 q = 1 + 0 ⇔
q∗
q−1 =
.
kqk22
(2.24)
(2.25)
Rotation in three dimensions
Euler’s rotation theorem states that "When a sphere is moved around its centre
it is always possible to find a diameter whose direction in the displaced position
is the same as in the initial position" [Euler, 1776]. In short, any rotation can be
represented with a three dimensional unit vector e and an angle α. The Euclidean
vector e can also be written as e = ie1 + je2 + ke3 , where the complex values i,
j and k represents the three Cartesian axis. A rotation α about e can then be
represented by a quaternion
α
q = e 2 (ie1 +je2 +ke3 ) = cos
α
α
+ (ie1 + je2 + ke3 ) sin ,
2
2
(2.26)
where proof can be seen in [Kuipers, 1999]. The vector form of (2.26) is denoted
q=
!
cos(α/2)
.
sin(α/2)e
(2.27)
Body rotation in the earth frame can be represented with quaternions for each
rotation about each axis. Using the rotation sequence as in Figure 2.2 gives the
following representation
  c( φ )c( θ )c( ψ ) + s( φ )s( θ )s( ψ )
2
2
2 
q0   φ2 2 ψ2
q  s( )c( θ )c( ) − c( φ )s( θ )s( ψ )
 1   2 2 2
 .
2
2
2
q =   =  φ θ ψ
q2  c( )s( )c( ) + s( φ )c( θ )s( ψ )
   2 2 2
2
2
2 
φ
ψ
φ
ψ 
q3
c( 2 )c( θ2 )s( 2 ) − s( 2 )s( θ2 )c( 2 )
(2.28)
Similarly, a conversion from quaterions to Euler angles is given by


   arctan ( 2q22q3 +2q2 0 q1 ) 


φ
2q0 +2q3 −1
  

  
.
θ
arcsin
(2q
q
−
2q
q
)
  = 
0 2
1 3 

  
 arctan ( 2q12q2 +2q2 0 q3 ) 
ψ
2q0 +2q1 −1
(2.29)
12
2
Mathematical Preliminaries
As in Section 2.3 transformation from the body reference frame to the earth reference frame is done as
QBE
 2
 2q0 + 2q12 − 1

= 2q1 q2 + 2q0 q3

2q1 q3 − 2q0 q2
2q1 q2 − 2q0 q3
2q02 + 2q22 − 1
2q2 q3 + 2q0 q1

2q1 q3 + 2q0 q2 

2q2 q3 − 2q0 q1 

2q02 + 2q32 − 1
(2.30)
and from earth reference frame to body reference frame as
QEB = QTBE
2.4.4
 2
 2q0 + 2q12 − 1

= 2q1 q2 − 2q0 q3

2q1 q3 + 2q0 q2
2q1 q2 + 2q0 q3
2q02 + 2q22 − 1
2q2 q3 − 2q0 q1

2q1 q3 − 2q0 q2 

2q2 q3 + 2q0 q1  .

2q02 + 2q32 − 1
(2.31)
Dynamics of quaternions
Let qB (t) be the orientation of an arbitrary system B in the earth reference frame
E. Also let the angular velocity of B be in the direction of the unit vector s with
magnitude ω. Then δqB describes the rotation of B about s during a small time
interval δt. Using small angle approximation and (2.27), quaternion perturbation
can be described as
qB (t + δt) = qB (t)δr(t)
(2.32)
where
δr(t) = cos(δα/2) + s(t) sin(δα/2) .
(2.33)
The time derivative of qB (t) can then be expressed as
q (t + δt) − qB (t)
dqB (t)
= lim B
=
dt
δt
δt→0
q (t)δr(t) − qB (t)
=
= lim B
δt
δt→0
q (t)s(t)δα
= lim B
=
2δt
δt→0
1
1
= qB (t)s(t)ω(t) = qB (t)ω(t).
2
2
(2.34)
(2.35)
(2.36)
(2.37)
The matrix form of the quaternion time derivative is derived by

 0
dqB (t) 1 ω1
= 
dt
2 ω2
ω3
−ω1
0
−ω3
ω2
−ω2
ω3
0
−ω1
 

−ω3  q0 
q0
−ω2  q1  1 q1
  = 
ω1  q2  2 q2
 

0
q3
q3
−q1
q0
q3
−q2
−q2
−q3
q0
q1
 
−q3   0 
q2  ω1 
 .
−q1  ω2 
 
q0 ω3
(2.38)
3
Hardware Systems
The simulation environment has been created to enable simple and cheap testing
of different configurations, such as more powerful motors or a different control
algorithm, for Aerigon. It has never been viable to test such configurations on the
real Aerigon system during this master’s thesis project due to the limited amount
of time that has been assigned to it. There simply was not enough time to create,
and beforehand guarantee, a stable enough control algorithm to warrant testing it
on such an expensive system as the Aerigon. The current Aerigon logs are tough
to obtain in a usable format, which makes it unnecessarily difficult to validate the
hexarotor model with that system. For these two reasons a test vehicle similar to
the Aerigon has been used for testing purposes. Additionally, a test rig has been
used in order to identify certain parameters and coefficients that are otherwise
difficult to determine. This chapter presents these three hardware systems that
were used throughout this thesis.
3.1
Aerigon system
The Aerigon system is a large multirotor system used for filming in the professional media industry. It consists of two main components, the helicopter and
the gimbal, described further in Sections 3.1.1 and 3.1.2. The gimbal is optional
which means that the helicopter can be used with or without it. Note that both
the helicopter and the gimbal are only described with the level of detail required
to properly model the system. The Aerigon system is shown in Figure 3.1.
13
14
3
Hardware Systems
Figure 3.1: The Aerigon system consisting of the Aerigon IAH 3 Helicopter
and the Aerigon IAG 3 Gimbal.
3.1.1
Aerigon helicopter
The Aerigon IAH 3 Helicopter has six arms. Each arm has two coaxial rotors
powered by brushless DC motors. The rotors on each arm rotate in opposite
directions. The helicopter weighs 11 kg including batteries and the diameter of
its airframe is 1050 mm. The diameters of the rotors are 457.2 mm. Although
the Aerigon helicopter has twelve rotors it essentially resembles a hexarotor, only
with coaxial rotors. The Aerigon IAH 3 Helicopter is therefore referred to as a
hexarotor throughout the thesis. The hexarotor can, together with the gimbal, be
seen in Figure 3.1.
3.1.2
Aerigon gimbal
The Aerigon IAG 3 Gimbal’s purpose is to stabilize and rotate the camera, which
it does with a stabilisation system powered by four brushless DC motors. It can
hold any reasonably sized camera payload that weighs in between 2 and 9 kg.
The gimbal itself weighs 4.7 kg including batteries and is mounted to the bottom
of the hexarotor’s centre of lift in a damped attachment around which the gimbal
can rotate. It has a width of 620 mm, a height of 610 mm and a length of 614 mm.
A picture of the gimbal can be seen in Figure 3.2.
3.2
15
Test vehicle
Figure 3.2: The Aerigon gimbal.
3.2
Test vehicle
The test vehicle’s structure is similar to the Aerigon hexarotor’s as it is also a
hexarotor. Unfortunately it does not have coaxial rotors and its frame is a bit
smaller with a diameter of 1040 mm. The other major differences between the
two systems are the weight and the power. With a weight of 6.4 kg the test vehicle weighs less than the Aerigon hexarotor. Also, the test vehicle’s motors are
less powerful and its rotors smaller than the Aerigon hexarotor’s since the test
vehicle is not created to carry any heavy payload, such as a gimbal and a camera.
Even though there are some structural differences between the two vehicles the
test vehicle is still regarded similar enough to make it adequate for model validation purposes. To be able to validate the model it is necessary to log the model
states and control signals of the test vehicle. This is done with a 3DR Pixhawk
which is an advanced autopilot system. Aside from being able to control the test
vehicle it is also equipped with a multitude of sensors and a microSD card for
high-rate logging with which it can log most quantities that are needed for the
model validation. A picture of the test vehicle can be seen in Figure 3.3.
Figure 3.3: The test vehicle used for verification of the simulation model.
16
3.3
3
Hardware Systems
Test rig
The test rig that was used for all tests in Chapter 6 was developed in an earlier
Bachelor’s thesis also carried out at IA. It mainly consists of a wooden frame
and an Aerigon arm including motors and propellers similar to Aerigon’s, and a
string attached between the two. Among its measuring components are a load
cell which measures the load exerted by the motor on the string and the micro
controller Arduino Yún which logs the load and controls the motor speed by a
preset sequence. The test rig is also equipped with the logging unit Eagle Tree
elogger v4 which measures and logs many quantities, for instance motor current,
voltage, power and engine speed in revolutions per minute (RPM). The test rig is
shown in Figure 3.4. For a full description of the test rig, see the Bachelor’s thesis
[Carlbring and Karlsson, 2014].
Figure 3.4: The test rig used to perform both the coefficient estimation and
the motor model estimation tests. The picture is taken from the Bachelor’s
thesis [Carlbring and Karlsson, 2014].
4
Hexarotor Model
To successfully simulate the movements of any system it is crucial to find an
accurate model of the system. In this thesis, this is done by expressing the system’s states with mathematically derived differential equations and, when that is
difficult, using system identification to describe certain behaviours. This chapter, however, focuses solely on how the mathematical modelling of the Aerigon
system was done. Note that the modelling of a multirotor can be done more generally, but that it in this chapter is done for the Aerigon system.
4.1
Modelling preliminaries
Before any modelling can be done one must specify some system details, such
as the numbering of the rotors. It is also important to fully understand how a
hexarotor manoeuvres in order to construct a well thought-out dynamic model.
This section presents these topics and so acts as a preparatory segment for the
rest of this chapter.
4.1.1
Rotor specifics
The rotors of the Aerigon hexarotor are numbered clockwise starting from the
positive XB axis so the upper rotors are numbered 1-6 and the lower ones 7-12
meaning that, for example, rotors 1 and 7 are a coaxial pair. The odd-numbered
upper rotors rotate in the positive direction about ZB and the even-numbered upper rotors rotate in the negative direction. Both a negative and a positive rotation
is illustrated in Figure 4.1.
17
18
4
(a) Illustration of a negative
rotation.
Hexarotor Model
(b) Illustration of a positive
rotation.
Figure 4.1: The two rotation directions of the rotors. A negative rotation is
represented by a red circular arrow and a positive rotation by a blue circular
arrow.
The rotors of a pair rotate in opposite directions to, when a yaw motion is not desirable, cancel out the reaction torque resultant of that pair. The reaction torque
and the yaw motion are treated further in the following section. All rotors’ rotation directions and numbering is shown in Figure 4.2. It is also noteworthy that
the rotors’ motors are mounted to rotor shafts, meaning that each rotor is placed
either a little above or below the (XB , YB )-plane.
Figure 4.2: The numbering and rotation directions of the rotors of the
Aerigon hexarotor.
4.1.2
Basic manoeuvring concepts
In a situation where a rotor is rotating with a constant angular velocity it produces both a force and a torque. The force is called a thrust force and is always
pointing in the negative ZB direction since the rotors are fixed. It is treated in
more detail in Section 4.3.2. The torque is called a reaction torque and is exerted
on the rotor shaft in the opposite direction to the rotor’s rotation direction. See
Section 4.4.3 for a more detailed description of the reaction torque. Figure 4.3
illustrates the thrust forces Ti and reaction torques τQ,i produced by a hexarotor
4.1
19
Modelling preliminaries
that is currently hovering at a stationary height. Note that the manoeuvring concepts in this section are presented for a general hexarotor instead of the Aerigon
hexarotor to make the concepts and figures more comprehensible.
Figure 4.3: The thrust forces and reaction torques for a general hexarotor.
The thrust forces are represented by the big black straight arrows and the
reaction torques by the black circular arrows below the rotors’ coloured circular rotation direction arrows. The bigger the black arrows are, the more
thrust force and reaction torque is produced by the rotors.
A hexarotor is in stationary flight, also known as hover, when the resultants of all
forces and torques acting on it are zero. This is achieved when all thrust forces
are equal and the sum of all thrust forces is equal to the gravitational force. The
thrust force produced by rotor i in stationary flight is therefore defined as
T0,i =
mg
.
6
(4.1)
Upward or downward flight is then achieved by simply increasing or decreasing
the angular velocities, and consequently the thrust forces, of all rotors equally
and simultaneously. To achieve forward flight, sideward flight and a controlled
change of attitude is a bit more complicated, though, as it is then necessary to
individually change each rotor’s angular velocity. During such movements it is
generally desirable to maintain the same altitude. This means that when some
rotor’s angular velocity is decreased, another’s is increased by the same amount.
Since the thrust force vectors no longer points straight upwards when the hexarotor is tilted the overall angular velocities will also have to increase to maintain
altitude. The different rotations that are used to generate these movements are
described individually below.
The roll is the rotation about the XB axis. Since all rotors are positioned at a distance larger than zero from XB in the (XB , YB )-plane they each produce a torque
about that axis. When the resultant of these torques is non-zero the hexarotor
20
4
Hexarotor Model
will roll. A positive roll is hence achieved by increasing the angular velocities of
rotors 4-6 while decreasing the angular velocities of rotors 1-3. Such a roll will in
turn partly generate an acceleration in the positive YB direction. The roll is therefore the rotation that is used to achieve sideward flight. A positive roll command
is illustrated in Figure 4.4.
Figure 4.4: Illustration of how a hexarotor produces a positive roll motion
by increasing the angular velocities, and thus also the thrust forces, of rotors
4-6 and decreasing the angular velocities of rotors 1-3.
The pitch is the rotation about the YB axis. A positive pitch is generated by increasing the angular velocities of rotors 1 and 6 while decreasing the angular
velocities of rotors 3 and 4. Rotors 2 and 5 do not produce a torque about the YB
axis since they are positioned right on it in the (XB , YB )-plane. As a result of the
positive pitch the hexarotor will partly accelerate in the negative XB direction.
The pitch is consequently used to perform forward and backward flight. Figure
4.5 illustrates what a positive pitch command looks like.
The yaw is the rotation about the ZB axis. In contrary to roll and pitch the yaw
motion is not directly dependent of the thrust forces. Instead it depends on the
reaction torques produced by the rotors. In stationary flight when all rotors’ angular velocities are equal the resultant of all reaction torques is zero. If an evennumbered rotor, which is exerting a positive torque about ZB on the airframe,
should divert from stationary flight and increase its angular velocity the hexarotor would, among other things, begin to yaw since the resultant of all reaction
torques would be positive. A controlled positive yaw is therefore achieved when
the angular velocities of rotors 2, 4 and 6 are increased while the angular velocities of rotors 1, 3 and 5 are decreased. The yaw is hence used to control the
hexarotor’s heading, i.e. its rotation about ZB . An illustration of a positive yaw
command can be seen in Figure 4.6.
4.1
Modelling preliminaries
21
Figure 4.5: Illustration of how a hexarotor produces a positive pitch motion
by increasing the angular velocities, and thus also the thrust forces, of rotors
1 and 6, and decreasing the angular velocities of rotors 3 and 4. The angular
velocities of rotors 2 and 5 remain unchanged as they do not affect the pitch.
Figure 4.6: Illustration of how a hexarotor produces a positive yaw motion
by increasing the angular velocities, and thus also the reaction torques, of
rotors 2, 4 and 6 while decreasing the angular velocities of rotors 1, 3 and 5.
22
4.2
4
Hexarotor Model
States
The states used to describe the position, velocity and attitude of the hexarotor are
 
 x 
 y 
 
 z 
 
 
 ẋ 
   ẏ 
 ξ E   ż 
 ξ̇   
 
X =  E  = q0  .
 q   
  q1 
ωB
 
q2 
 
q3 
 
 p 
 
 q 
 
r
(4.2)
These states are, with the exception of the quaternion representation instead of
Euler angles, used in many multirotor applications and throughout the work related to this thesis.
4.3
Forces
Newton’s second law of motion states that
F = ma
(4.3)
which in the hexarotor’s three dimensional earth fixed frame is equal to
 
mẍ
 
F = mξ̈ E = mÿ  .
 
mz̈
(4.4)
The complete dynamic model of the hexarotor requires expressions for the derivatives of the states. ξ̈ E is thus a sought after quantity which, by rewriting (4.4), can
be expressed as
 
ẍ F
 
ξ̈ E = ÿ  =
  m
z̈
(4.5)
where F consists of the external forces acting on the hexacopter. These forces
are gravity and the thrust forces produced by the rotors and are described in the
following subsections.
4.3
23
Forces
4.3.1
Gravity
Whereas most forces, torques and other quantities that are acting on the hexarotor are naturally expressed in the body fixed frame, gravity is not one of them. It
is thus easily defined in the earth fixed frame as
F g,E
 
 0 
 
=  0  .
 
mg
(4.6)
Sometimes, however, it might be desirable to express the gravitational force in
the body fixed frame. It can then be expressed as
F g,B
4.3.2
  

 0  2(q1 q3 − q0 q2 )
  2(q q + q q )
= QEB  0  =  2 3
0 1 
 mg.
   2
mg
2q0 + 2q32 − 1
(4.7)
Thrust force
The thrust force generated by rotor i is defined as
2
Ti = CT ρArprop
Ω2i
(4.8)
where CT is a thrust coefficient, ρ is the air density, A is the rotor disk area, rprop
is the rotor radius and Ωi is the rotor angular velocity of rotor i [Leishman, 2006].
Hence, the total thrust force acting on the hexarotor can be described by
TB =
12
X
Ti .
(4.9)
i=1
Each thrust force is considered to be pointing in the negative ZB direction. However, in order to properly express these forces in the earth fixed frame they must
first be transformed to that frame. As explained in the Chapter 2 each linear
quantity can be transformed from the body fixed frame to the earth fixed frame
with the quaternion rotation matrix QBE . This gives
T E = QBE T B
4.3.3


 
 0   −(2q1 q3 + q0 q2 ) 

  −(2q q − q q ) 
= QBE  0  = 
2 3
0 1 
 TB .

 
−TB
−(2q02 + 2q32 − 1)
(4.10)
Complete force description
To control the altitude of the hexarotor the total thrust TB is in this thesis used as
a control signal denoted uT = TB . Using this and adding the gravity and the total
thrust force gives
24
4
F = F g,E + T E



= 


−2(q1 q3 + q0 q2 )uT 
−2(q2 q3 − q0 q1 )uT 

mg − (2q02 + 2q32 − 1)uT
Hexarotor Model
(4.11)
which inserted in (4.5) gives

  
1
ẍ  −2(q1 q3 + q0 q2 ) m uT 
  
1
ξ̈ E = ÿ  =  −2(q2 q3 − q0 q1 ) m
uT  .
  

1
2
2
z̈
g − (2q0 + 2q3 − 1) m uT
4.4
(4.12)
Torques
The torque M E of a rigid body is defined as
ME =
dL
dt
(4.13)
where L = I ω B is the angular momentum and I is the inertia matrix which is
defined as

Ixx

I = Iyx

Izx
Ixy
Iyy
Izy

Ixz 

Iyz  .

Izz
(4.14)
Since the hexarotor’s mass distribution is considered symmetrical, resulting in
Iij = 0 ∀ i , j, the inertia matrix can be simplified to

Ixx

I =  0

0
0
Iyy
0

0 
0  .

Izz
(4.15)
Note that this assumption holds only for the hexarotor and is not valid when
the gimbal is mounted to it as the mass distribution in both the (x, z)-plane and
the (y, z)-plane then becomes asymmetrical. Even though this asymmetry yields
a non-diagonal inertia matrix it is still always assumed diagonal in this thesis.
This is based on two reasons. Firstly a non-diagonal inertia matrix complicates
the modelling and since the hexarotor, and not the gimbal, is in focus such a
complication would deviate too far from the purpose of the thesis. Secondly the
test vehicle is not compatible with a gimbal which makes an assessment of the
validity of the gimbal dynamics impossible.
To make future calculations easier it is preferred to approach the torques in the
body fixed frame instead of in the earth fixed frame. Such a transformation
means that the Coriolis effect has to be added to (4.13), resulting in
4.4
25
Torques
MB =
dL
+ ω B × L.
dt
(4.16)
Inserting L = I ω B gives
dω B
MB = I
+ ω B × (I ω B ) = I ω̇ B + ω B × (I ω B ) =
dt


 
Ixx ṗ  (Izz − Iyy )qr 


= Iyy q̇ +  (Ixx − Izz )pr  .

 
(Iyy − Ixx )pq
Izz ṙ
(4.17)
Then, by rewriting (4.17), ω̇ B can be expressed as
   1
ṗ  Ixx
 
ω̇ B =  q̇  =  0
  
ṙ
0
0
1
Iyy
0
0
0
1
Izz



 
 (Iyy − Izz )qr  
 
 M +  (I − I )pr  .
  B  zz

xx

(I − I )pq 
xx
yy
(4.18)
In order to fully describe ω̇ B an expression of M B must be derived. There are
several different torques that are included in M B . One torque is induced by gravity when the gimbal is mounted to the hexarotor, and another is induced by the
thrust forces. The additional torques consist of the reaction torque, which is exerted on a rotor shaft by its motor, the inertial counter torque, which is produced
by a change in the rotors’ angular velocities, and finally the gyroscopic torque,
which is exerted on the body by simultaneous spin and precession motions. All
of these torques are treated in detail in the following subsections.
4.4.1
Torque induced by gravity
The torque that is induced by the gravitational force is defined as
τ g = r g × F g,B
(4.19)
where r g is the vector from OB to the centre of gravity (COG) in the body fixed
frame, here denoted COGB . These two coincide for a coaxial setup without gimbal which yields τ g = 0. For all other setups τ g is a non-zero vector. It is assumed
that an addition of the gimbal only displaces COGB in the positive ZB -direction
since the displacements in XB and YB are minimal in comparison. This gives

  


τg,φ   0 
−2(q2 q3 + q0 q1 )

  


τ g =  τg,θ  =  0  × F g,B =  2(q1 q3 − q0 q2 )  mgzg


τ  z 
0
g,ψ
(4.20)
g
where zg is the distance between OB and COGB . Since the Aerigon system normally consists of three bodies (the hexarotor, the gimbal and the camera) it is
26
4
Hexarotor Model
necessary to know both the mass and the position of COGB in relation to OB
for each body in order to calculate zg . Given this information zg is calculated
according to
P3
zi mi
zg = Pi=1
3
i=1 mi
(4.21)
where mi is the mass and zi is the displacement of COGB in B for body i.
4.4.2
Torque induced by thrust forces
Since none of the thrust force vectors go through the centre of gravity they create
a torque. The torque generated by rotor i is calculated by taking the cross product
of the vector r i from OB to the rotor and the rotor’s force T i,B . Thus, the torque
τ T ,i is given by
τ T ,i = r i × T i,B .
(4.22)
Insertion of the vector r i , where h is the height of the rotor shaft, for the upper
rotors gives the torques
τ T ,i
 


π
l cos( 6 (2i − 1))  0 




=  l sin( π6 (2i − 1))  ×  0  ,
 


−h
−Ti
i = 1, . . . , 6.
(4.23)
The lower rotors induce the same torque distribution as their respective upper
rotors, except with their own thrust force Ti . This means that
τ T ,1
τ T ,2
τ T ,3
T , τ
=
T , τ
=
T ,
T1 7 T ,8
T2 8 T ,9
T3 9
τ T ,4
τ T ,5
τ T ,6
=
T10 , τ T ,11 =
T11 , τ T ,12 =
T .
T4
T5
T6 12
τ T ,7 =
τ T ,10
(4.24)
(4.25)
The sum of all torques induced by thrust forces on the hexarotor is thus
τ T ,tot =
12
X
τ T ,i .
(4.26)
i=1
4.4.3
Reaction torque
The reaction torque is exerted on the rotor shaft when the motor keeps the rotor
rotating. In contrary to the torques created by thrust forces, this results in a
torque about the ZB axis. The reaction torque exerted by motor i is defined as
4.4
27
Torques
3
τQ,i = CQ ρArprop
Ω2i
(4.27)
where CQ is the reaction torque coefficient [Leishman, 2006]. The resulting reaction torque on the shaft, and consequently on the hexarotor’s airframe, is of opposite direction and equal magnitude in comparison to the motor’s torque which
means that a rotor that rotates clockwise will exert a counter-clockwise torque on
the hexarotor. This means that the total reaction torque on the hexarotor is given
by
τ Q,tot



= 
P6
i
i=1 (−1) τQ,i

0


 .
0

P12
+ i=7 (−1)i+1 τQ,i
(4.28)
The reaction torque would hence be zero if both rotors in a coaxial pair were controlled by the same control signal. That is not the case, though, as each rotor is
individually controlled. The reaction torque thus enables control of the hexarotor’s yaw motion.
4.4.4
Inertial counter torque
In most papers regarding multirotors, such as [Pounds et al., 2010, Alexis et al.,
2011], the inertial counter torque is not brought up at all and when it is mentioned it is usually neglected. [Bouabdallah, 2007] first includes it in his thorough modelling but later neglects it with the motivation that a simplified model
is preferable in control applications. This is reasonable since for small multirotors with small motors the inertial counter torque will be insignificant in comparison to the other torques acting on the multirotor. This is not an assumption
that can be made for the Aerigon though since its motors are much heavier than
the ones used in standard multirotor applications. The inertial counter torque,
which is produced by a change in the rotor angular velocities, is modelled as
τ ct



= 


0 
0 

−Ir Ω̇r
(4.29)
where Ir is the rotor inertia around the rotor shaft and
Ωr =
6
X
i=1
(−1)i+1 Ωi +
12
X
i=7
is the resultant of the rotor angular velocities.
(−1)i Ωi
(4.30)
28
4.4.5
4
Hexarotor Model
Gyroscopic rotor effects
A gyroscopic effect consists of three motions: spin, precession and torque. With
the spin always rotating about the same body fixed axis, the torque, which is
sought after in this case, can be expressed as
τ rotor = ω p × I r ω s
(4.31)
where ωp is the precession, ωs is the spin and I r is the inertia matrix of the rotors.
This is derived using the theory in [Christensen, 2011]. Because, in this case, Ωr is
the spin and ωB , the rotational velocity of the airframe, represents the precession,
the gyroscopic torque that is exerted on the body by the rotors is given by
τ rotor
   
 
 0  p
 0 
   
 
= ω B × I r  0  =  q  × I r  0  .
   
 
Ωr
r
Ωr
(4.32)
Since the rotors can be considered symmetrical I r can be assumed diagonal. The
expression of the torque then becomes
τ rotor
  
 

p  0   Ir qΩr 
  
 

=  q  ×  0  = −Ir pΩr 
  
 

r
Ir Ωr
0
(4.33)
where the scalar Ir is the rotor inertia around the rotor shaft. The rotor gyroscopic
effects therefore only appear when the hexarotor body rotates about XB or YB
while Ωr is non-zero.
4.4.6
Complete torque description
To summarise, M B in (4.18) is given by
M B = τ g + τ T ,tot + τ Q,tot + τ ct + τ rotor
   
τφ  uφ 
   
=:  τθ  +  uθ 
τ  u 
ψ
(4.34)
ψ
where
τφ = − 2(q2 q3 + q0 q1 )mgzg + Ir qΩr ,
τθ = 2(q1 q3 − q0 q2 )mgzg − Ir pΩr ,
(4.35)
τψ = − Ir Ω̇r
and the control signals uφ , uθ and uψ used to control the hexarotor’s attitude are
defined as
4.5
29
The complete non-linear dynamic model
uφ =
6
X
i=1
uθ =
uψ =
6
X
i=1
6
X
π
−l sin( (2i − 1))(Ti + Ti+6 ),
6
π
l cos( (2i − 1))(Ti + Ti+6 ),
6
(−1)i τQ,i +
i=1
12
X
(4.36)
(−1)i+1 τQ,i .
i=7
By inserting (4.34) in (4.18) ω̇ B can finally be expressed as
   1 ((I − I )qr + τ + u ) 
zz
φ
φ 
ṗ  Ixx yy
   1 ((I − I )pr + τ + u ) 
ω̇ B =  q̇  =  Iyy zz
xx
θ
θ 
 .
   1
ṙ
((Ixx − Iyy )pq + τψ + uψ )
(4.37)
Izz
4.5
The complete non-linear dynamic model
The complete non-linear dynamic hexarotor model can be put together using the
expressions in (2.38), (4.12) and (4.37) which results in


ẋ
  

x


  
ẏ

 y  

  

ż
 z  

  
1

   −2(q1 q3 + q0 q2 ) m uT

 ẋ  

   −2(q2 q3 − q0 q1 ) 1 uT

 ẏ  
m
   g − (2q2 + 2q2 − 1) 1 u 
 ż  
T

0
3
m
  

1
(−pq1 − qq2 − rq3 )
˙ = d q0  = 
 .
2
X




1

dt   

(pq
+
rq
−
qq
)
0
2
3
2
q1  

1

q2  
(qq
−
rq
+
pq
)
0
1
3
2

  
1

q  
(rq
+
qq
−
pq
)

0
1
2
 3  
2
 p   I1 ((Iyy − Izz )qr + τφ + uφ ) 
   xx
 q   1 ((I − I )pr + τ + u ) 
   Iyy zz
xx
θ
θ 

 1
r
((I − I )pq + τ + u )
Izz
4.6
xx
yy
ψ
(4.38)
ψ
Linearisation
Since a non-linear controller is beyond the limitations of this thesis a linearised
version of the helicopter model which the linear controller can work with needs
to be derived. Assuming the system will have a linear behaviour in close proximity to a steady state this can be achieved. The linearisation in this section is based
on the theory in [Glad and Ljung, 2004].
30
4
Hexarotor Model
Angle operations in the quaternion domain are non-linear since, for instance, a
subtraction between two quaternions does not yield the true orientation difference between them. This causes trouble for the controller, especially when it
comes to reference tracking. Also the case of gimbal lock will be deemed as unlikely and uncontrollable enough for Euler angle representation to be satisfactory
for the controller. This gives a model of the dynamics as follows


  
ẋ

 x  

ẏ

 y  

  
ż

 z  
  −(cos φ sin θ cos ψ + sin φ sin ψ) 1 u 
 ẋ  
m T

  
1
 ẏ  −(cos φ sin θ sin ψ − sin φ cos ψ) m
uT 

  
1

uT
g − cos φ cos θ m
 ż  
d




˙ =
  = 

X
p + sin φ tan θq + cos φ tan θr

dt φ 

  
cos
φq
−
sin
φr

 θ  

sin φ
cos φ
  
q
+
r
ψ  

cos
θ
cos
θ
  

1
 p  

((I
−
I
)qr
+
τ
+
u
)
yy
zz
φ
φ
Ixx
  

1
 q  

((I
−
I
)pr
+
τ
+
u
)
  

zz
xx
θ
θ
Iyy



r
1
((I − I )pq + τ + u )
Izz
xx
yy
ψ
(4.39)
ψ
where τ g , which is included in τφ and τθ , also has been rewritten to an expression
with Euler angle representation instead of quaternions. The rewritten expression
is given by


−sφcθ 


τ g =  −sθ  mgzg .


0
(4.40)
For simplicity, the states and the control signals are in this section denoted as
   
   x1 
uT  u1 
 ξE   
   
 ξ̇   x2 
 u  u 
 
x =  E  =  .  , u =  φ  =  2 
.
 uθ  u3 
 ηE   . 
 
 
u4
uψ
ωB
x12
(4.41)


 f1 (x1 , . . . , x12 , u1 , . . . , u4 ) 
 f2 (x1 , . . . , x12 , u1 , . . . , u4 ) 


 .
ẋ = f (x, u) = 
..


.



f12 (x1 , . . . , x12 , u1 , . . . , u4 )
(4.42)
and (4.39) as
Defining x = a as a steady state of the multirotor with the constant control signals
u = b gives f (a, b) = 0. Assuming that f has continuous partial derivatives in a
close proximity to the steady state one can rewrite fk , ∀k = 1, 2, . . . , 12, as
4.6
31
Linearisation
∂fk
∂fk
(a, b)(x1 − a1 ) + . . . +
(a, b)(x12 − a12 )+
∂x1
∂x12
∂f
∂f
+ k (a, b)(u1 − b1 ) + . . . + k (a, b)(u4 − b4 ) + rk (x − a, u − b)
∂u1
∂u4
fk (x, u) = fk (a, b) +
(4.43)
where rk is the remainder of the expression. The remainder includes higher order terms which will approach zero faster than the first order terms when the
distance to the steady state is approaching zero. Using the notations
 ∂f
 1
 ∂x1

A =  ...

 ∂f12
∂x1
...
...
∂f1

∂x12 

 ∂f
 1
 ∂u1
..  , B =  ..
 .
. 

 ∂f
∂f12 

 12

∂x12
∂u1
...
...
∂f1
∂u4



..  ,
. 

∂f12 

(4.44)
∂u4
where the partial derivatives are evaluated in x = a and u = b, (4.43) can be
written as
f (x, u) = f (a, b) + A(x − a) + B(u − b) + r(x − a, u − b)
(4.45)
which, by using f (a, b) = 0 and disregarding the higher order terms in r(x − a, u −
b), is a linear model of (4.39).
A calculation of the partial derivatives in (4.44) can be seen in appendix A.
5
Control Strategies
Most advanced vehicular systems require some sort of control to perform at least
adequately. For multirotors, which are inherently unstable, it is particularly important to have a well-functioning control system. This chapter describes the
control strategy for the hexarotor model described in chapter 4.
5.1
Control structure
As stated in [Simon, 2014] Model Predictive Control (MPC) is a control strategy
with great potential. It is however not yet established in aeronautical applications. Evaluating this strategy as a flight controller is one objective of this thesis.
Other control strategies will also be evaluated since, as stated in [Bouabdallah,
2007, Liu et al., 2013, Alexis et al., 2011, Rinaldi et al., 2013, Abdolhosseini et al.,
2013], simpler feedback control such as PID-control or LQ control may prove to
be sufficient. LQ-control will be used as benchmark to compare the MPC controller with a linear feedback controller.
5.2
Model Predictive Control
Model Predictive Control emerged in the process industry in the 1970s. The first
applications were designed to control refining processes which had a sampling
time of many minutes. Many advances have been made since then and today
MPC controllers shows great potential in controlling constrained multivariable
systems. While MPC controllers are still mainly used for slow and predictable
processes it is making its way to faster systems that require fast computations.
33
34
5
Control Strategies
One of the aims of this thesis is thus to formulate an MPC controller and evaluate
its capability in multirotor applications. [Borrelli et al., 2014]
...
k+N
Figure 5.1: A basic working principle of Model Predictive Control (By Martin Behrendt, via Wikimedia Commons)
The principle of Model Predictive Control is to use a model of the system dynamics and current state measurements to calculate optimal control signals which
will bring the system to its origin. This is done for a predefined time interval,
a so-called prediction horizon, from the sampling instant. Since the process is
repeated for each sampling instant the prediction horizon is receding into the
future, see Figure 5.1. This is performed in each sampling instant, and the immediate optimal control signal is used as control law until the next sampling instant.
This section will give a brief description of the basic Linear MPC formulation and
aspects as reference tracking, integral action and stability. The formulation will
be in the discrete domain at time instant k.
5.2.1
Linear MPC
In this thesis the MPC problem is solved as a finite horizon, convex optimisation
problem with constrained variables. Although this is a non-linear optimisation
problem, it is still referred to as a linear MPC since the system dynamics are
modelled as linear. A basic linear MPC formulation can be expressed as
min Ψ (xN ) +
xk+i ,uk+i
N
−1
X
kxk+i k2Q1 + kuk+i k2Q2
(5.1a)
i=0
s.t. xk+i+1 = Axk+i + Buk+i
(5.1b)
xk+i ∈ X
(5.1c)
uk+i ∈ U
(5.1d)
xk+N ∈ T .
(5.1e)
Here the objective function is defined as a quadratic cost function with penalties
on states and control signals, linear dynamics and constraints defined as X =
5.2
35
Model Predictive Control
{x | FX x ≤ bX }, U = {u | FU u ≤ bU }. The terminal state constraint is defined
as xk+N ∈ T , and T and Ψ (xN ) will be further explained in the section below.
Henceforth the objective function is denoted as l(xk , uk ) = kxk k2Q1 + kuk k2Q2 .
5.2.2
Stability
Stability is an important factor of feedback control. In the case of MPC controllers
one can ensure stability as described in [Mayne et al., 2000]. The method mentioned by the authors is a so called dual mode MPC controller. This is done by
letting a local controller u = k(xN ) stabilize the system in a close region T to
the system origin. The purpose of the MPC controller is then to steer the system
to this region in finite time, where in theory the stabilizing feedback can kick-in.
This will however never occur because of the receding horizon properties of MPC
controller.
The terminal cost Ψ (xN ) in (5.1a) is the truncated part of the infinite cost function,
∞
X
l(xk+i , uk+i ) =
i=0
N
−1
X
l(xk+i , uk+i ) +
i=0
∞
X
l(xk+i , uk+i )
i=N
where the upper sum can be assumed to be bounded by
∞
X
l(xk+i , uk+i ) ≤ Ψ (xN ).
i=N
If one lets the local state feedback u = k(xN ) stabilize the system in the region T ,
and chooses the truncated upper part of the cost function as
T
Ψ (xN ) = xN
P xN
(5.2)
where k(xN ) = K xN is the state feedback that minimizes the LQ cost function
and P is the solution to the Riccati equation. The closed-loop system will be
asymptotically stable, according to [Mayne et al., 2000].
For simplicity the region T will for this thesis be chosen as the origin. This makes
the controller force the system to its origin within the prediction horizon, thereby
ensuring stability.
5.2.3
Reference tracking
More than often it is required by the controller to maintain the system at another
state than the origin. One can then assume that the system is driven to the stationary reference state
36
5
Control Strategies
xr = Axr + Bur
r = Cxr + Dur
where xr and ur denotes states and control signals at the stationary state. Similarly this can be expressed as
A−I
C
B
D
!
!
!
xr
0
=
ur
r
where the distance to the desired steady state is penalised instead. The modified
cost function is then expressed as
min Ψ (xN − xr ) +
xk+i ,uk+i
N
−1
X
l(xk+i − xr , uk+i − ur ).
i=0
This, however, can lead to infeasibility in the optimisation due to insufficient
prediction horizon for some reference values. This is emphasised in [Rossiter,
2006], in which a solution proposed. Instead of using the actual reference value
r a pseudo reference r̄ can be used. This gives the equivalent steady state
A−I
C
B
D
!
!
!
x̄
0
=
ū
r̄
where x̄,ū and r̄ are used as variables in the optimisation to ensure feasibility. To
ensure that the pseudo reference converges to the desired reference the difference
between r and r̄ is penalised in the objective function as
min
uk+i ,xk+i ,x̄,ū,r̄
Ψ (xN − x̄) +
N
−1
X
l(xk+i − x̄, uk+i − ū) + Φ(r̄ − r)
i=0
where Φ(r) is a positive definite function.
5.2.4
Integral action
Due to insufficiencies of the internal model of the MPC controller and external
disturbances the system is bound to deviate from the expected states. Integral
action can counter these effects. As proposed by [Muske and Rawlings, 1993] the
use of a disturbance observer may result in integral action. Expanding the linear
system model with a constant disturbance gives
xk+1 = Axk + Buk + Edk
dk+1 = dk
5.3
37
Linear Quadratic Control
which means a disturbance observer can be expressed as
!
x̂k+1
A
=
0
dˆk+1
E
I
!
!
!
x̂k
B
+
u + Kd (y − C x̂k )
0 k
dˆk
where Kd is the observer gain and the disturbance dˆk is estimated outside the
MPC controller with the proposed disturbance observer. The resulting steady
state
A−I
C
B
D
!
!
!
x̄k
−E dˆk
=
ūk
r̄k
compensates for the disturbance.
5.2.5
Final formulation
To sum up previous sections, the reformulation of the MPC problem at time instance k is
min
uk+i ,xk+i ,x̄k ,ūk ,r̄k
s.t.
Ψ (xk+N − x̄k ) +
N
−1
X
l(xk+i − x̄k , uk+i − ūk ) + Φ(r̄k − r)
(5.3a)
i=0
xk+i+1 = Axk+i + Buk+i + E dˆk
xk+i ∈ X
(5.3b)
(5.3c)
uk+i ∈ U
(5.3d)
xk+N ∈ T (x̄k )
!
!
!
A − I B x̄k
−E dˆk
=
.
C
D ūk
r̄k
(5.3e)
(5.3f)
Formulating (5.3) as a standard quadratic programming problem is done in appendix B.
5.3
Linear Quadratic Control
One systematic approach in the design of controllers is the use of optimal control
theory. In difference to ad-hoc methods, this theory can be used for a more systematic approach to derive an optimal control law. This section will give a brief
overview of the theory of linear quadratic (LQ) control.
Consider a linear state space representation with a linear quadratic cost function
38
5
J(x, u) =
∞
X
(xkT Qxk + ukT Ruk )
Control Strategies
(5.4)
k=0
s.t. xk+1 = Axk + Buk
x0 given.
(5.5)
(5.6)
The resulting optimal feedback law is then
u ∗ = −R−1 BT P x = −Lx
(5.7)
where P is the solution to the Riccati equation
P˙ + Q − P BR−1 BT P + P A + AT P = 0.
(5.8)
For more background to the solution see [Jönsson et al., 2003, Glad and Ljung,
2003].
5.3.1
Reference tracking
This control law can be supplemented with reference tracking. Following the
reasoning by [Glad and Ljung, 2003] the optimal controller can be expressed as
u = −Lx + Lr r
(5.9)
where is the feedback gain L defined as above, and Lr such that the static gain of
the system is I. If y = Cx then Lr can be expressed as
Lr = (C(BL − A)−1 B)−1 ,
(5.10)
and proof of this can be seen in [Glad and Ljung, 2003].
5.3.2
Integral action
Since model errors and unknown disturbances will trouble the LQ-controller as
well, integral action will be necessary to avoid steady state errors. One way to
handle this is to add additional states defined as the integral of the tracking error,
as
Z
xe =
(y − r) dt ⇒ ẋe = y − r = Cx − r.
These additional states will be forced to the origin by the controller so that steady
state errors are minimised.
6
Parameter estimation
It is not uncommon that a model includes parameters that are difficult to determine analytically. Also, some coefficients are highly system dependent. Therefore, some parameters need to be retrieved through tests and estimations. This
chapter describes how these tests were performed as well as how the processing
of the test data was carried out.
6.1
Thrust coefficient estimation
Recall that a thrust force is defined by
2
T = CT ρArprop
Ω2
(6.1)
which means that it is proportional to the squared angular velocity. While some
coefficients, such as A and rprop , are easily measurable, some are not. It is of
course possible to measure the air density ρ which means that the coefficient CT ,
the only remaining unknown constant, can be determined through testing. Often,
however, it is easier to simply lump all these coefficients together. This gives
T = kΩ2
(6.2)
where k is the unknown thrust coefficient that needs to be determined through
testing with the test rig described in Section 3.3. To determine k with acceptable
accuracy the test needs to yield the thrust force for a sufficient number of different
angular velocities. A preset control sequence consisting of multiple steps in Ω
was thus uploaded to the micro controller. There were 15 seconds in between the
39
40
6
Parameter estimation
steps to ensure that the angular velocity could get stabilised in time to retrieve
enough valid data at each angular velocity level. With the data collected, the
means of both the thrust and the angular velocity at each level were calculated
and used to create data points. Afterwards a linear model, which slope is the
sought after k, was fitted to the data points. This model was the best fit in a
linear least-squares sense. It must be noted that the first few data points, for
Ω < 220 rad/s, were excluded from the model for two reasons. Firstly, they
are outside of the hexarotor’s standard operating range during flight, which for
the hexarotor without gimbal is 270 < Ω < 470 rad/s. Secondly, they cause a
deviation of the T to Ω2 ratio, causing the slope to decrease or increase and thus
resulting in a value of k that is not as accurate for the most common flying cases.
This test was executed with several different motor and rotor configurations and
the results are presented in the following sections.
6.1.1
Single setup
Even if all motors of the same kind should behave alike, that is not always the
case and some minor deviations in performance between otherwise equal motors
is to be expected. Two motors similar to Aerigon’s were hence tested separately,
yielding the linear models in Figure 6.1 and the k-values
k1 = 8.8511 · 10−5 and k2 = 8.8058 · 10−5 .
25
(6.3)
25
20
20
Data
Model
15
T [kgm/s 2 ]
T [kgm/s 2 ]
Data
Model
10
5
15
10
5
0
0
0
0.5
1
1.5
Ω2 [rad 2 /s2 ]
2
2.5
3
×10 5
(a) Test data and linear model for motor 1.
0
0.5
1
1.5
Ω2 [rad 2 /s2 ]
2
2.5
3
×10 5
(b) Test data and linear model for motor 2.
Figure 6.1: The test data means (blue stars) and the fitted linear model (red
line) for both motors. The data was collected using the test rig with the single
setup. The slopes of the linear models are the estimated values of k1 and k2
respectively.
As the values of k1 and k2 are similar it is likely safe to assume that each individual motor behaves similarly. Therefore the k-value that is used in the model in
6.1
41
Thrust coefficient estimation
the simulation environment for all motor in a single setup is the mean value of
k1 and k2 , which is
k=
6.1.2
k1 + k2
= 8.8285 · 10−5 .
2
(6.4)
Coaxial setup
Testing a motor in a single setup and obtaining its k-value does not necessarily
mean that that value is valid for a coaxial setup as it creates additional aerodynamic effects that may or may not have an effect on the thrust force. Therefore
the two motors that were tested individually in Section 6.1.1 were also tested together in a coaxial setup. The resulting linear model is presented in Figure 6.2
and the numerical value of its slope is 1.4416 · 10−4 . The k-value for each motor
in the coaxial setup is then obtained as
kcoax =
1.4416 · 10−4
= 7.2079 · 10−5
2
(6.5)
since it is assumed that the thrust to angular velocity ratio is the same for both
motors. Since kcoax is significantly smaller than the k obtained in Section 6.1.1 it
is clear that a motor in a coaxial setup yields less thrust force at a certain angular
velocity compared to a motor in a single setup.
40
35
30
Data
Model
T [kgm/s 2 ]
25
20
15
10
5
0
0
0.5
1
1.5
Ω2 [rad 2 /s2 ]
2
2.5
3
×10 5
Figure 6.2: The test data means (blue stars) and the fitted linear model (red
line). The data was collected using the test rig with the coaxial setup. The
slope of the linear model is the estimated value of kcoax .
42
6
Parameter estimation
This estimation of kcoax is likely accurate when the two coaxially connected motors are run with similar angular velocities, which is often the case during normal
flight. To increase the robustness of the model it could be interesting to investigate whether the assumption of linear behaviour holds when the difference in
angular velocity between the two coaxial motors is large, or even when one of the
motors is not running. This is outside of the scope of this thesis, though.
6.1.3
Test vehicle single setup
A similar test was performed for the test vehicle’s motor and rotor single setup
with the only difference to the original test being that it lasted a bit longer. Since
this setup is less powerful than the one representing Aerigon’s it was necessary to
add a few more steps to cover a corresponding operating range to make the tests
comparable. The resulting data points and slope can be seen in Figure 6.3 and
the obtained k-value for the test vehicle is
ktest = 5.9448 · 10−5 .
(6.6)
20
18
16
Data
Model
T [kgm/s 2 ]
14
12
10
8
6
4
2
0
0.5
1
1.5
2
Ω2 [rad 2 /s2 ]
2.5
3
3.5
×10 5
Figure 6.3: The test data means (blue stars) and the fitted linear model (red
line). The data was collected using the test rig with the test vehicle single
setup. The slope of the linear model is the estimated value of ktest .
6.2
43
Torque coefficient estimation
6.2
Torque coefficient estimation
The reaction torque is given by
3
Ω2 ,
τQ = CQ ρArprop
(6.7)
which means that it, just like the thrust force, is proportional to the squared
angular velocity, but with a slight difference in the coefficients. Lumping these
together gives
τQ = bΩ2
(6.8)
where b is the unknown reaction torque coefficient. There is no way of directly
measuring the reaction torque τQ using the test rig, which means that τQ has
to be calculated through other measurable quantities. One such quantity is the
rotational power P , which is defined as
P = τΩ
(6.9)
where τ is the torque and Ω is the angular velocity [Nordling and Österman,
2006]. Combining (6.8) and (6.9) yields the relationship
P = bΩ3
(6.10)
which can be used to calculate b if the power and the angular velocity is known.
The same tests as the ones used to estimate the thrust coefficients could be used
to estimate b since the Eagle Tree elogger v4 also logs power. Consequently, the
exact same methodology was used for the data processing of the power data as
for the thrust data. This was done for the same test setups as in Section 6.1.
6.2.1
Single setup
For the single setup the two motors’ reaction torque coefficients are estimated to
be
b1 = 2.3464 · 10−6 and b2 = 2.4068 · 10−6
(6.11)
and the resulting linear models are shown in Figure 6.4. The b-value used in the
model in the simulation environment for a single setup is then
b=
b1 + b2
= 2.3766 · 10−6 .
2
(6.12)
6
400
400
350
350
300
300
250
P [kgm2 /s3 ]
P [kgm2 /s3 ]
44
Data
Model
200
150
Data
Model
250
200
150
100
100
50
50
0
Parameter estimation
0
0
5
10
Ω3 [rad 3 /s3 ]
15
0
5
×10 7
(a) Test data and linear model for motor 1.
10
Ω3 [rad 3 /s3 ]
15
×10 7
(b) Test data and linear model for motor 2.
Figure 6.4: The test data means (blue stars) and the fitted linear model (red
line) for both motors. The data was collected using the test rig with the
single setup. The slope of the linear model is the estimated value of b1 and
b2 respectively.
6.2.2
Coaxial setup
The resulting b-value estimation for each individual motor using the coaxial
setup is
bcoax =
4.3620 · 10−6
= 2.1810 · 10−6
2
(6.13)
since it is assumed that the power is equally distributed over the motors. Figure
6.5 shows the linear model whose slope is the above mentioned reaction torque
coefficient.
6.2.3
Test vehicle single setup
The test that was performed to obtain the test vehicle’s k-value was also used to
obtain its b-value. The resulting linear model and its slope can be seen in Figure
6.6 and the obtained numerical estimation is
btest = 1.6320 · 10−6 .
(6.14)
6.2
45
Torque coefficient estimation
700
600
P [kgm2 /s3 ]
500
Data
Model
400
300
200
100
0
0
5
10
15
Ω3 [rad 3 /s3 ]
×10 7
Figure 6.5: The test data means (blue stars) and the fitted linear model (red
line). The data was collected using the test rig with the coaxial setup. The
slope of the linear model is the estimated value of bcoax .
350
300
P [kgm2 /s3 ]
250
Data
Model
200
150
100
50
0
0
0.2
0.4
0.6
0.8
1
1.2
Ω3 [rad 3 /s3 ]
1.4
1.6
1.8
2
×10 8
Figure 6.6: The test data means (blue stars) and the fitted linear model (red
line). The data was collected using the test rig with the test vehicle single
setup. The slope of the linear model is the estimated value of btest .
46
6.3
6
Parameter estimation
Motor dynamics estimation
Brushless DC motors are used to drive the hexarotor’s rotors. In reality a motor
receives its control commands in the shape of pulse-width modulation (PWM)
signals from an electronic speed controller (ESC), which in turn receives its control commands from the central processing unit (CPU). Since the angular velocity
output does not instantaneously change in accordance with the control command
input from the CPU this dynamic needs to be described and modelled. According
to Chéron et al. [2010] a simulation model of an actuator composed of a brushless
controller, a brushless motor and a rotor can be represented by
Gmot (s) =
Kp
1 + Tp s
e−Td s
(6.15)
which is a first order transfer function with a time delay Td , a static gain Kp and
a time constant Tp .
In the simulation environment, however, the hexarotor model receives its angular velocities directly from the flight controller and thus neglects the ESC and the
communication preceding it. There are several reasons to this. Firstly, the communication produces the time delay Td which is very small in comparison to the
time constant Tp and is thus sufficiently small in order to be negligible. Secondly,
even if this delay was a bit larger it would still be very tough to estimate with the
test rig since the equipment used for logging, the Eagle Tree elogger v4 and the
Arduino, are not synchronised to millisecond precision, which is necessary when
estimating such small time delays. Thirdly, the conversion from PWM to angular
velocity is not particularly interesting to model as it is just a linear model. Therefore the motor model in the simulated hexarotor model has the rotor angular
velocity references from the flight controller as input and the generated angular
velocity as output. With Td = 0 the transfer function becomes
Gmot (s) =
Kp
1 + Tp s
.
(6.16)
Also, since the angular velocity acts as both input and output of the motor model,
the static gain Kp is, in theory, equal to 1. Thus the relationship between the
angular velocity reference input Ωref and the generated angular velocity output
Ωmot can be described by
Ωmot (s) = Gmot (s)Ωref (s) =
1
Ω (s).
1 + Tp s ref
What then remains to be determined is the time constant Tp .
(6.17)
6.3
47
Motor dynamics estimation
6.3.1
Motor model estimation
A commonly used method for estimating parameters of first order transfer functions is to give the system a step reference and analyse its step response. This
methodology was used to estimate Td and the step tests were performed using
the test rig with the test vehicle single setup. These tests were not performed
with any of the Aerigon setups because the Aerigon system is not equipped with
logging equipment. Therefore the retrieved motor models could not have been
used when assessing the validity of the complete simulation model. It was thus
concluded that it was more important to perform these tests with the test vehicle
single setup.
To begin with, one needs to collect data from which the motor model can be estimated. The test sequence that was used for this purpose was a series of positive
and negative steps in angular velocity. The negative steps are important to include as their step responses may not be symmetric to those of the positive ones.
The estimation test sequence and its response can be seen in Figure 6.7.
420
Ω
400
mot
Ωref
380
Ω [rad/s]
360
340
320
300
280
260
240
0
5
10
15
20
25
30
35
40
Time [s]
Figure 6.7: The estimation data used to estimate the motor model. The red
line is the angular velocity reference Ωref and the blue line is the, by the
motor, generated angular velocity Ωmot .
The motor model was then estimated by fitting a first order transfer function to
the data presented in Figure 6.7 using MATLAB. The resulting motor model was
Gmot (s) =
1.002
1 + 0.115s
(6.18)
48
6
Parameter estimation
which, by rounding the static gain to one, can be written as
Gmot (s) =
1
,
1 + 0.115s
(6.19)
thus yielding a time constant Tp = 0.115. The obtained motor model in (6.19)
achieved a 93% root mean square fit to the estimation data.
6.3.2
Motor model validation
To strengthen the validity of the motor model it was validated with another test
sequence which was constructed to elucidate any possible deviations in model fit
for different steps. The obtained motor model in (6.19) was then given the validation test reference sequence and its response can be seen in Figure 6.8 together
with the generated angular velocity. The motor model achieved a 89% root mean
square fit to the validation data.
550
Ωmot
Ω
500
mod
Ω [rad/s]
450
400
350
300
250
0
5
10
15
20
25
Time [s]
Figure 6.8: The motor model’s response, Ωmod , to the validation reference sequence. It can be compared with the real system’s generated response Ωmot .
With a visual inspection of Figure 6.8 one can see that the motor model is quite
accurate in modelling the positive steps, but performs worse when modelling
negative steps. To further strengthen these conclusions the relative model error,
which is the relative difference between Ωmod and Ωmot , was calculated and can
be seen in Figure 6.9. In the figure one can see that error spikes appear when
a step is initiated. These spikes appear to be smaller for smaller steps, which
6.3
49
Motor dynamics estimation
is promising since manoeuvres that involve large steps are very aggressive and
seldom used. It is also noteworthy how well the motor model performs in stationarity, although it becomes less accurate for higher angular velocities where
there is more noise in general. Finally, the difference in model fit between positive and negative steps becomes apparent in the figure as the relative model error
spikes of the positive steps decreases significantly faster than the error spikes
of the negative steps. This established difference between positive and negative
steps indicates that the true motor model is slightly non-linear. The motor model
(6.19), which is linear, is despite this considered satisfactory enough to use it in
the simulation environment.
10
9
Relative model error [%]
8
7
6
5
4
3
2
1
0
0
5
10
15
20
25
Time [s]
Figure 6.9: The relative model error between the motor model’s response
and the real system’s generated response to the validation reference sequence.
7
Results
This chapter presents the final simulation environment and assessments and evaluations of the performance of both the simulation model and the controller. It
also includes an analysis of the linearised system.
7.1
Simulation environment
The simulation environment was created in MATLAB/Simulink. It consists of
two main blocks: the hexarotor dynamics and the controller. An overview of the
simulation environment can be seen in Figure 7.1.
mot_sys
Rotor ang.v el.
x
Engine Dynamics
Helicopter Dynamics
x
Rotor ang.v el.
ref
Controller
ref
Reference values
Figure 7.1: Overview of the entire simulation environment in Simulink.
51
52
7
Results
The controller, which has the option to switch between an LQ controller and a
MPC controller, follows the predefined reference values and uses rotor angular
velocities as output to control the simulated hexarotor model. The hexarotor
dynamics in the simulation environment has been implemented based on the
theory in Chapter 4. However, before the angular velocities reach the hexarotor
model the motor model estimated in Section 6.3 simulates the dynamics of the
real motors. Additionally, the user of the simulation environment can observe
states, control signals and other desired quantities in plots in Simulink. The
user can also use the simulation data in order to create own plots with MATLAB
scripts.
The hexarotor dynamics block mostly uses the same notation as Chapter 4. A
few blocks, namely the "Body Torque" and "Body Rotation" blocks, do not use the
same notation, however, and need some further explanation. The "Body Torque"
block includes the inertial counter torque, the reaction torque and the torques
induced by thrust forces while the rest of the rotational dynamics are included
in the "Body Rotation" block. Note that the model is implemented as a general
multirotor. This enables the user to define parameters and dimensions of the
hexarotor so that he or she can choose to model either Aerigon, the test vehicle or
a completely different multirotor by simply changing some MATLAB variables.
The entire hexarotor dynamics block can be seen in Figure 7.2.
x
Rotor ang.v el.
y
z
x_dot
y _dot
Q_be
z_dot
Lateral Dynamics
1
1
Rotor ang.v el.
Body Torque
Q_be
q
q
eta
x
Rotor ang.vel.
Calculate Rot.matrix
Body Torque
Q_be tau_g
Quaternions to Euler angles
tau_g
q
Gravitational Torque
Body Torque
Body rot.v el.
Rotor ang.v el.
Body Rotation
Figure 7.2: The "Helicopter Dynamics" Simulink block.
As shown in Figure 7.2 the basic and most significant hexarotor modelling has
been implemented in the simulation model. Unfortunately the additional effects,
such as turbulence, wind, atmospheric conditions, GPC accuracy and latency of
sensor data, have yet to be implemented due to lack of time. The simulation
environment has, however, been created to enable future implementation of these
effects.
7.2
7.2
Model assessment
53
Model assessment
Simple assessments of the model in the simulation environment has been done
throughout the project as the model was expanded. To continuously test that
the simulation model performs according to the theoretical model is in fact a
necessity to avoid troublesome troubleshooting. While these tests are important
they only test the model’s performance in the simulation environment, which is
an incredibly simplified version of reality. Therefore it is also relevant to evaluate
how well reality and the simulation environment correspond.
To be able to perform this comparison, test data of a hexarotor’s movements during flight had to be collected. This was the sole purpose of the test vehicle. A
flight of the test vehicle, during which data was logged, was thus carried out with
the pilot performing different somewhat isolated manoeuvres. The GPS measurements were nowhere near accurate enough to enable comparisons in x and y position, though. The same goes for the barometer measurements that are used to
calculate the z position as these measurements were quite noisy. The assessment
of the simulation model has therefore been focused on the attitude. Figure 7.3
shows the roll, pitch and yaw angles during the entire flight, disregarding landing and take-off. To enable the comparison between the real and the simulated
hexarotor the model in the simulation environment was adapted to represent the
test vehicle.
The control signals that are logged during flights with the test vehicle are the
PWM signals of the ESC:s. Unfortunately, the test rig is not compatible with the
ESC:s of the test vehicle, so an accurate mapping of PWM to angular velocity
could not be made for the test flight data. This made it impossible to directly use
the logged flight control signals as inputs to the simulation model and then comparing the whole simulated flight with the real flight. Even if it was possible the
generated motions of the simulated model would in all likelihood diverge from
the real flight data in a matter of seconds. The most prominent reason behind
this is the lack of modelled noise and disturbance in the simulation environment.
The results from the simulated model will hence quickly diverge from the flight
data when the test vehicle’s control system compensates for wind, turbulence,
asymmetrical mass distribution and other irregularities. A simple way to bypass
this problem and still assess the simulation model is to only use short pieces of
flight data for verification of each basic motion. The pieces of flight data that are
used for roll, pitch and yaw motions respectively are marked with a red rectangle
in Figure 7.3.
54
7
Results
30
Roll angle [°]
20
10
0
-10
-20
-30
0
10
20
30
40
50
60
70
80
90
80
90
80
90
Time [s]
(a) The roll angle throughout the entire flight.
Pitch angle [°]
20
10
0
-10
-20
0
10
20
30
40
50
60
70
Time [s]
(b) The pitch angle throughout the entire flight.
60
Yaw angle [°]
50
40
30
20
10
0
10
20
30
40
50
60
70
Time [s]
(c) The yaw angle throughout entire flight.
Figure 7.3: The angles’ change throughout the whole test vehicle flight. The
data in the red rectangles was used for assessment of the simulation model’s
performance for respective angle.
7.2
55
Model assessment
Both the control signal and attitude estimation logs from the selected data pieces
were used to assess the simulation model’s attitude performance. The PWM control signals were scaled to a reasonable, but roughly estimated, level which somewhat represented angular velocities. These scaled control signals were then used
as inputs to the simulation model. In other words, the system was controlled as
an open loop system with the scaled control signals as inputs, and the states of the
simulated model as outputs. The simulation model used for model assessment
purposes can be seen in Figure 7.4. After running this simulation the resulting
simulated attitude could be compared with the attitude from the flight logs.
Omega
Input
mot_sys
Rotor ang.vel.
x
simout
Output
Engine Dynamics
Helicopter Dynamics
Figure 7.4: The Simulink simulation environment used for assessing the
simulation model. The inputs are scaled versions of the control signals from
the flight logs. The output is the state vector of the simulation model.
Note that the accuracy of the scaled control signals is not of utmost importance
as the purpose of the test is to assess whether or not the simulated and the logged
attitudes are similar when using similar control signals. In other words, one
should not focus on how well the two attitudes match up, but rather that they
initially resemble each other before the simulated attitude diverges.
First off is the assessment of the roll angle, which was done using the selected data
highlighted in Figure 7.3a. Both the resulting simulated roll angle and the flight
roll angle are shown in Figure 7.5. The two curves in the figure are initially quite
similar and it is apparent that a negative roll motion is initiated in both cases. The
simulated roll declines before the flight roll, however, which is an indication that
the time constant of the motor model may be a bit too small. After approximately
0.8 seconds the roll angle begins to increase in the simulation, while the flight roll
remains low. This divergence may be caused by some wind gust disturbance or
something as simple as that the batteries, which make up around 40% of the
the test vehicle’s total weight, are not completely centred. In conclusion, the
modelling of the roll angle in the simulation environment behaves as expected
with the two roll angles diverging from each other after a short period of time.
This deviation is most likely an effect of some irregularity that is not modelled.
56
7
Results
15
Flight roll
Simulated roll
10
Roll angle [°]
5
0
-5
-10
-15
-20
0
0.2
0.4
0.6
0.8
1
1.2
1.4
1.6
1.8
Time [s]
Figure 7.5: The flight roll angle (solid line) and the simulated roll angle
(dashed line).
The data used to assess the pitch angle is highlighted in Figure 7.3b and the resulting simulated pitch angle and flight pitch angle can be seen in Figure 7.6. The
two curves are very similar, other than that they are slightly separated in time. In
that sense, the pitch angle result is similar to that of the roll angle and thus also
likely to be an effect of a inaccurate motor model time constant estimation. All
in all, the modelling of the pitch angle seems to be surprisingly satisfactory.
2
0
Pitch angle [°]
-2
-4
-6
-8
-10
Flight pitch
Simulated pitch
-12
-14
0
0.2
0.4
0.6
0.8
1
1.2
1.4
1.6
Time [s]
Figure 7.6: The flight pitch angle (solid line) and the simulated pitch angle
(dashed line).
7.2
57
Model assessment
Finally, the yaw angle was assessed similarly to the roll and pitch angle, but by
instead using the data highlighted in Figure 7.3c. The logged flight yaw angle
and the resulting simulated yaw angle are presented in Figure 7.7. In the figure,
the two curves are moderately similar in the sense that they both increase and
decrease somewhat simultaneously. There’s still a slight time difference between
the two curves which is yet again likely caused by the inexact estimation of the
motor model’s time constant. It is slightly promising that the simulated yaw angle converges to the flight yaw angle at the end of the figure. The large difference,
though, is that the simulated yaw angle is largely scaled in comparison to the
logged flight yaw angle. This might be because of the inaccurate control signals
or perhaps due to inaccurate estimations of model parameters. It was later also
found that the attachments of the motors to the arms of the test vehicle were not
in good conditions and that this could cause the motors to slightly rotate around
their arms. This could explain that the simulated yaw angle diverges more from
the flight yaw angle when it decreases than when it increases. In short, the simulation model’s representation of the yaw angle can neither be confirmed nor
rejected.
150
Flight yaw
Simulated yaw
100
Yaw angle [°]
50
0
-50
-100
-150
-200
0
1
2
3
4
5
6
7
Time [s]
Figure 7.7: The flight yaw angle (solid line) and the simulated yaw angle
(dashed line).
In summary, the multirotor model that has been implemented in the simulation
environment produces in a sense promising results, even when being subject to
imperfect control signals. The representation of the yaw angle did not prove its
worth, though. A more extensive comparison to reality is therefore desirable.
58
7
7.3
Results
Analysis of the linear system
The operating point is chosen as a stationary hover at an arbitrary position. This
because the position does not affect the linearisation as long as the vehicle is
stationary. This gives the stationary states and control signals
h
x0 = 0 0 0 0 0 0
h
i
u0 = mg 0 0 0 .
0
0
0
0
0
i
0 ,
(7.1)
Inserting (7.1) into the framework found in appendix A, while neglecting gyroscopic torques and inertial counter torques, which have no effect in stationarity,
gives

0
0


0

0
0


0
A = 
0

0

0

0

0

0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
0
0
0
0
0
0
g
0
0
0
0
0
0
0
0
0
0
−g
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
0
0
0
0
0
1
0
0
0
0


 0
0
 0



0


 0
0


 0
0

 0



0
− 1

 m
0
 , B =  0


0
 0


0
 0


1


 0

0


 0
0



0
0
0
0
0
0
0
0
0
0
0
1
Ixx
0
0
0
0
0
0
0
0
0
0
0
1
Iyy
0
0
0
0
0
0
0
0
0
0
0
0
0
1
Izz













 .












All poles of this linear system are located in the origin. This makes the system
marginally stable.
This means that the system will remain in the origin if and only if the inputs
remain zero at all time. A bounded input will not cause the system to diverge
into infinity instantaneously. However the states will drift indefinitely without
oscillations since the poles lack imaginary value. This means a step response
will not result in a steady state. This is confirmed by evaluating the open loop
response of a step input in control signal uφ , which can be seen in Figure 7.8.
Deviations between the linearised model and the non-linear model can also be
seen in Figure 7.8. Since the linear system does not consider cross dependencies
between states the roll motion in Figure 7.8a is not affected by the yaw motion in
the linear case. This is not the case for the non-linear model as the simultaneous
roll and yaw motions cause a pitch motion, and consequently also a change in
position in x.
7.3
59
Analysis of the linear system
90
20
Linear
Nonlinear
Linear
Nonlinear
80
0
70
-20
Position X [m]
Roll angle [°]
60
50
40
-40
-60
30
-80
20
-100
10
0
-120
0
5
10
15
0
5
Time [s]
10
15
Time [s]
(a) The roll angle.
(b) The position in x.
140
20
Linear
Nonlinear
18
Linear
Nonlinear
120
16
100
Position Y [m]
Pitch angle [°]
14
12
10
8
80
60
40
6
20
4
0
2
0
-20
0
5
10
15
Time [s]
0
5
10
Time [s]
(c) The pitch angle.
(d) The position in y.
600
Linear
Nonlinear
500
Yaw angle [°]
400
300
200
100
0
0
5
10
15
Time [s]
(e) The yaw angle.
Figure 7.8: System outputs of a linear (solid line) and a non-linear (dashed
line) system with step in uφ at t = 5.
15
60
7.4
7
Results
Control evaluation
The MPC controller was evaluated in different flight cases and compared with
a linear feedback. This section will present the resulting performance of these
controllers. Tuning has only been done to the extent of achieving satisfactory
performance. The LQ controller was tuned with the same weighting matrices as
the MPC controller. Except for the final flight case, all reference values will be
assumed instantaneous and unforeseen because the hexarotor systems developed
at IA are exclusively pilot controlled.
7.4.1
Controller design choices
The design choices of the MPC controller, that have already been discussed in
Section 5, are Ψ (xN ) and T (x̄). The function Φ(r̄) is simply chosen as the Euclidean norm, and model errors and disturbances are assumed to occur on all
states, which results in E = I. Because of the high requirements of processing resources a longer sampling period Ts has been chosen to evaluate if this can affect
the system stability. A prediction horizon of ten samples is chosen as an arbitrary
trade-off between solution complexity and the controller’s ability of prediction.
The design variables used by the controller are
T
Ts = 0.1, N = 10 , Ψ (xN ) = xN
P xN , Φ(r̄) = kr̄k2 , T (x̄) = x̄ , E = I.
Limitations on states are based on the recommendations of flight performance for
the Aerigon system. Maximum thrust Tmax is limited by the highest engine speed
and the engines are assumed to only being able to rotate in one direction. For this
evaluation a maximum thrust corresponding to 5000 RPM will be assumed. The
other control signals were given more arbitrary, but realistic, limitations. State
limitations are derived from flight recommendations of the Aerigon with limitless capabilities in position and height from the ground. Effects of reduced air
density will not be considered in this case. All state and control signal limitations are given by
 


 ∞ 
 −∞ 
 ∞ 
 −∞ 
 


 


 0 
 −∞ 
 


−14
14
 


 


14
−14
 0 
Tmax 
 


−4
 4 
 −6 
 2 
 


 


−35 ≤ x ≤ 35 and −4 ≤ u ≤  4  .


 
 


−35
35
−2
2
 


−90
90
 


−45
45


 


 
−45
45


 
−45
45
7.4
61
Control evaluation
Control signals need to be reformulated as motor rotation speed for compatibility
with the simulation environment. For simplicity, this reformulation is presented
for a single axis hexarotor. The control signal
uT = TB =
6
X
Ti ,
i=1
that is the sum of all propeller thrusts and used to control altitude, could be
expressed depending on rotation speed since Ti = kΩ2i , which means that
uT = k
6
X
Ω2i .
i=1
Since the hexarotor is assumed symmetric and equal thrust from all propellers
is needed to maintain, or accelerate, altitude and position all motors contribute
with an equal amount as
uT
= kΩ2i .
6
This gives a quadratic relation between uT and Ωi , but since uT ≥ 0 the motor
rotation speed that controls the altitude, Ωalt , is given as
r
Ωalt =
uT
.
6k
(7.2)
Control signals for attitude control (uφ , uθ , uψ ) are all assumed to be relatively
small adjustments in motor rotation speed compared to the rotation speed needed
for stationary flight Ω0 . Adjustments in the roll control signal uφ are expressed
as
uφ = k
6
X
ly,i Ω2i
i=1
where ly,i is the y-coordinate of the motor i in B and Ωi = Ω0 + ∆Ωi . The sign
of ly,i indicates direction of the roll torque induced by increased thrust of motor
i. Known parameters of the test vehicle gives ly,1 = ly,3 = −ly,4 , ly,4 = ly,6 , −ly,2 =
ly,5 . This gives
62
7
uφ =k
6
X
Results
ly,i (Ω0 + ∆Ωi )2 =
i=1
=k(ly,1 (Ω0 + ∆Ω1 )2 + . . . + ly,6 (Ω0 + ∆Ω6 )2 ) =
=k (ly,1 + . . . + ly,6 ) Ω20 + 2kΩ0 (ly,1 ∆Ω1 + . . . + ly,6 ∆Ω6 )+
|
{z
}
=0
(ly,1 ∆Ω21 + . . . + ly,6 ∆Ω26 )
|
{z
= 2kΩ0 (ly,1 ∆Ω1 + . . . + ly,6 ∆Ω6 )
}
=0
where the last equality holds if and only if
∆Ω1 = ∆Ω3 = −∆Ω4 = −∆Ω6 , −∆Ω2 = ∆Ω5 .
The same reasoning holds for uθ , but instead lx,i is the x-coordinate of the motor
i in B, where the sign of lx,i indicates the direction of pitch torque induced by
increased thrust of motor i. Known parameters of the test vehicle gives lx,1 =
lx,6 = −lx,3 = −lx,4 , lx,2 = lx,5 . Which means uθ can be expressed as
uθ = k
6
X
lx,i Ω2i = 2kΩ0 (lx,1 ∆Ω1 + . . . + lx,6 ∆Ω6 )
i=1
if and only if ∆Ω1 = ∆Ω6 = −∆Ω3 = −∆Ω4 . The same follows for uψ which gives
uψ =b
6
X
(−1)i Ω2i = b(−(Ω0 + ∆Ω1 )2 + (Ω0 + ∆Ω2 )2 − . . . + (Ω0 + ∆Ω6 )2 ) =
i=1
=b (−Ω20 + Ω20 − . . . + Ω20 ) +
|
{z
}
=0
+2bΩ0 (−∆Ω1 + ∆Ω2 − . . . + ∆Ω6 ) + b (−∆Ω21 + ∆Ω22 − . . . + ∆Ω26 )
|
{z
}
=0
=2bΩ0 (−∆Ω1 + ∆Ω2 − ∆Ω3 + ∆Ω4 − ∆Ω5 + ∆Ω6 )
where the last equality holds if and only if |∆Ωi | = |∆Ωj | ∀ i, j = 1, . . . , 6. The
results for (uφ , uθ , uψ ) can be expressed in matrix form as
7.4
63
Control evaluation
 

uφ 
kly,1
 
 uθ  = 2Ω0 klx,1
 

uψ
−b
kly,2
klx,2
b
kly,3
klx,3
−b
kly,4
klx,4
b
kly,5
klx,5
−b


∆Ω1 

 ∆Ω2 
kly,6  

 ∆Ω3 
.
klx,6  
 ∆Ω4 
b 

∆Ω5 
∆Ω6
The resulting linear matrix relation is not invertible and thereby lacking an explicit solution. However applying the left pseudo inverse matrix


∆Ω1 
∆Ω2 


∆Ω 
1
3

∆Ω  =
2Ω

4
0


∆Ω5 


∆Ω6

kly,1

klx,1

−b
kly,2
klx,2
b
kly,3
klx,3
−b
kly,4
klx,4
b
kly,5
klx,5
−b
†  
kly,6  uφ 
  
klx,6   uθ  ,
  
uψ
b
(7.3)
gives a best fit in a linear least-squares sense. Equation (7.2) and (7.3) gives the
resulting motor rotation speed

Ωalt
Ω
 alt

Ω
uΩ =  alt
Ωalt

Ωalt

Ωalt
7.4.2

+ ∆Ω1 
+ ∆Ω2 

+ ∆Ω3 
.
+ ∆Ω4 

+ ∆Ω5 

+ ∆Ω6
(7.4)
Flight simulations
In the first flight case the hexarotor was assigned to follow changes in altitude.
The performance of the two controllers differ, as seen in Figure 7.9.
64
7
Results
4.5
4
MPC
LQ
Ref.
3.5
Height [m]
3
2.5
2
1.5
1
0.5
0
10
20
30
40
50
60
Time [s]
Figure 7.9: Performance of the MPC controller and the LQ controller during
altitude reference tracking.
5000
u MPC
Engine rotation speed [rpm]
[rad/s]
4500
u LQ
4000
3500
3000
2500
2000
10
20
30
40
50
60
Time [s]
Figure 7.10: Control signals for the MPC controller and the LQ controller
during altitude reference tracking.
7.4
65
Control evaluation
The MPC controller is tracking the reference altitude better and controlling a bit
more aggressively than the LQ controller as seen in Figure 7.10. Tuning of the
two controllers may have an impact on this outcome. Constraints on the terminal state will, despite the pseudo reference, make the MPC controller attempt to
reach the reference state within the prediction horizon. A longer prediction horizon will possibly relax this constraint and make the control signals less aggressive. Since the MPC controller is aware of how the system behaves it is possible
that the MPC controller uses this information to achieve better performance. The
difference is most likely a combination of the reasons mentioned.
In Figure 7.11 the control signals for the time interval 15 to 25 seconds can be
seen. Here the effects of the slow sampling period of the MPC controller can be
seen. This, however, has no immediate effect on the simulated hexarotor.
5000
u MPC
u LQ
[rpm]
Engine rotation speed [rad/s]
4500
4000
3500
3000
2500
15
20
25
Time [s]
Figure 7.11: Close up of the MPC and the LQ controllers’ control signals
during altitude step reference.
For the next two flight cases the ability to control the hexarotor’s position was
tested. Figures 7.12 and 7.14 show little or no difference in performance in the
two flight cases. However the way the two controllers adjust their attitude to
correct the position is different. This can be seen in Figures 7.13 and 7.15.
66
7
Results
6
5
MPC
LQ
Ref.
4
Position X [m]
3
2
1
0
-1
-2
-3
10
20
30
40
50
60
Time [s]
Figure 7.12: Performance of the MPC controller and the LQ controller during x-position reference tracking.
20
MPC
LQ
15
Pitch angle [ °]
10
5
0
-5
-10
-15
-20
10
20
30
40
50
60
Time [s]
Figure 7.13: Control signals of the MPC controller and the LQ controller
during x-position reference tracking.
7.4
67
Control evaluation
2
1.5
1
Position Y [m]
0.5
0
-0.5
-1
-1.5
-2
MPC
LQ
Ref.
-2.5
-3
10
20
30
40
50
60
Time [s]
Figure 7.14: Performance of the MPC controller and the LQ controller during y-position reference tracking.
20
MPC
LQ
15
Roll angle [ °]
10
5
0
-5
-10
-15
10
20
30
40
50
60
Time [s]
Figure 7.15: Control signals of the MPC controller and the LQ controller
during y-position reference tracking.
68
7
Results
Next up is the control of the hexarotor’s orientation or yaw, seen in Figure 7.16.
The LQ controller performed as in the previous cases while the MPC controller
is behaving quite differently from the LQ controller.
80
MPC
LQ
Ref.
60
40
Yaw [ °]
20
0
-20
-40
-60
-80
-100
5
10
15
20
25
30
Time [s]
Figure 7.16: Performance of the MPC controller and the LQ controller during yaw reference tracking.
70
60
Yaw [ °]
50
40
30
20
MPC
LQ
Ref.
10
0
5
5.5
6
6.5
7
7.5
Time [s]
Figure 7.17: Performance of the MPC controller and the LQ controller during yaw reference tracking from 4.5 to 8 seconds.
7.4
69
Control evaluation
From 5 to 5.5 seconds in Figure 7.17 the limitation of 45◦ /s in yaw rotation speed
is exceeded by the LQ controller. This is beacuse the LQ controller is completely
unaware of system limitations. The MPC controller, however, is applying a constant torque during the whole process to maintain a constant rotation speed.
From 5 to 6 seconds in Figure 7.17 the MPC controller rotates the vehicle approximately 40◦ , which is within system boundaries. This gives a clear example
of that the MPC controller is able to take state limitations into consideration and
control accordingly.
The LQ controller’s control signals were almost instant and of great amplitude
while the MPC controller was applying a constant rotational speed of the airframe, see Figure 7.18 and 7.20.
The difference becomes more apparent when taking a closer look in the time interval 4.5 to 7.5 seconds of the simulation. These intervals can be seen in Figures
7.19 and 7.21.
3600
Odd nr. rotors
Even nr. rotors
3550
Engine speed [rpm]
3500
3450
3400
3350
3300
3250
3200
5
10
15
20
25
30
Time [s]
Figure 7.18: Control signals of the LQ controller during ψ-reference tracking.
70
7
Results
3600
Odd nr. rotors
Even nr. rotors
3550
Engine speed [rpm]
3500
3450
3400
3350
3300
3250
3200
4.5
5
5.5
6
6.5
7
7.5
Time [s]
Figure 7.19: The LQ controller’s control signals during the time interval
from 4.5 and 7.5 seconds of the ψ-reference tracking.
3600
Odd nr. rotors
Even nr. rotors
3550
Engine speed [rpm]
3500
3450
3400
3350
3300
3250
3200
5
10
15
20
25
30
Time [s]
Figure 7.20: Control signals of the MPC controller during ψ-reference tracking.
7.4
71
Control evaluation
3420
Odd nr. rotors
Even nr. rotors
3410
Engine speed [rpm]
3400
3390
3380
3370
3360
3350
4.5
5
5.5
6
6.5
7
7.5
Time [s]
Figure 7.21: The MPC controller’s control signals during the time interval
4.5 and 7.5 of the ψ-reference tracking.
The final flight case consisted of a step in altitude. For this case it was considered as a predefined path for the hexarotor to track. This means that the MPC
controller could use the known reference in the prediction and thereby be able to
foresee the step and take appropriate action. The difference in performance can
be seen in Figure 7.22.
2.5
MPC
LQ
Ref.
Height [m]
2
1.5
1
0.5
0
5
10
15
20
25
30
Time [s]
Figure 7.22: Altitude step response of the MPC controller and the LQ controller. The MPC controller is utilising the known reference.
8
Conclusions and Future Work
This chapter discusses and concludes the results presented in Chapter 7. It also
presents what an eventual future development of this thesis could concern.
8.1
Conclusions
Evaluating the model with real measurements had to be done in very short time
intervals since unmodelled dynamics and sensor disturbances made the model
diverge from the evaluation data. However under these short time periods the
model performed decently. Because of the relatively large uncertainties in the
test flight it is difficult to know whether or not the model is ready to be extended
with additional dynamics. A further and more rigorous evaluation of the model’s
performance is therefore needed. On another note, the model is also adjustable
as the inclusion of a gimbal is optional and all hardware configurations, such as
arm length, number of rotors and mass, are parametrised.
Using Model Predictive Control as flight controller for a hexarotor has shown
great promise. For all flight cases tested in this thesis the MPC controller performed at least as well as the linear feedback controller. For some flight cases it
even outperformed the LQ controller. This is probably partly due to the fact that
the MPC controller "knows" how aggressive it can be since it has access to the
model dynamics, which the LQ controller does not. Furthermore the MPC controller accounted for system limitations in a way that the linear feedback could
not. A great example of this was the control of yaw angle, seen in Figure 7.16 on
page 68.
73
74
8.2
8
Conclusions and Future Work
Future Work
As stated in Chapter 1 the multirotor field of research is relatively new so there
are plenty of future advancements to be made in general. This section focuses
solely on the future development of this project, though.
The most important first step when continuing the work of this thesis is to further evaluate the simulation model with data from real hexarotor flights. It is
then imperative to use a test vehicle which is both well-balanced and meticulously examined to avoid minor defects which could become prominent when
validating the model. It is also recommended to perform these data collection
flights in as disturbance-free environments as possible in order to minimise irregularities that are not modelled in the simulation environment. Finally, it is
critical to use sensors which produce reliable measurements. All of these precautions will enable more accurate comparisons between the simulated data and the
flight data.
The hexarotor model describes the dynamics of a real hexarotor promisingly.
However, there are several minor and generally less prominent dynamical effects that are not treated in this thesis and thus are yet to be modelled. These
include blade flapping, hub force unbalances and induced drag among several
more. While these effects might be interesting to look into it is probably more
important to focus the future development on producing more accurate estimations of the model parameters. One such improvement could be to use different
motor models for positive and negative steps. Another could be to dynamically
estimate the hexarotor’s inertia matrix.
The simulation environment as it is today does not include any external effects
other than gravity. In essence, it describes a hexarotor in an environment in
which no noise or disturbances exist and where the state measurements are perfect. Even though this is a good foundation it does not describe real flights during
which disturbances are frequent and significant particularly well. Therefore it
would be interesting to expand the simulation environment with effects such as
wind, turbulence and sensor uncertainties.
As both the MPC controller and the LQ controller perform promisingly in the
simulation environment the next step is to implement these on hardware and
test their capabilities on real systems. It is particularly interesting to investigate
whether or not the MPC formulation used in this thesis is computationally sound
to use on hardware. If not, it might be necessary to reformulate it in order to
comply with the current hardware limitations. Additionally it would be highly
interesting to test how the MPC controller performs in trajectory tracking applications, which might be its strongest area.
Appendix
A
Linearisation
The A and B matrices are given by

0
0

0

0

0

0
A(x, u) = 
0

0

0

0

0

0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
a1,4
0
0
0
0
0
0
0
0
0
0
0
0
a2,5
0
0
0
0
0
0
0
0
0
0
0
0
a3,6
0
0
0
0
0
0
0
0
0

 0
 0


 0

b4,1

b5,1

b
B(x, u) =  6,1
 0

 0

 0

 0

 0

0
0
0
0
0
0
0
0
0
0
a4,7
a5,7
a6,7
a7,7
a8,7
a9,7
a10,7
0
0
a4,8
a5,8
a6,8
a7,8
0
a9,8
a10,8
a11,8
0
a4,9
a5,9
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
b10,2
0
0
77
b11,3
0
0
0
0
0
0
0
0
0
0
0
0
b12,4
























0
0
0
0
0
0
0
0
0
0
0
0
a7,10
0
0
0
a11,10
0
a7,11
a8,11
a9,11
a10,11
0
a12,11
0
0
0
0
0
0












 ,
a7,12 

a8,12 

a9,12 
a10,12 

a11,12 

a12,12
78
A
Linearisation
where all of the non-zero elements are given by
∂f1
∂f
∂f
= 1, a2,5 = 2 = 1, a3,6 = 3 = 1
∂x4
∂x5
∂x6
∂f4
1
= −(− sin φ sin θ cos ψ + cos φ sin ψ) uT
a4,7 =
∂x7
m
∂f4
1
= − cos φ cos θ cos ψ uT
a4,8 =
∂x8
m
∂f
1
a4,9 = 4 = −(− cos φ sin θ sin ψ − sin φ cos ψ) uT
∂x9
m
∂f
1
a5,7 = 5 = −(− sin φ sin θ sin ψ − cos φ cos ψ) uT
∂x7
m
∂f
1
a5,8 = 5 = − cos φ cos θ sin ψ uT
∂x8
m
∂f
1
a5,9 = 5 = −(cos φ sin θ cos ψ − sin φ sin ψ) uT
∂x9
m
∂f6
∂f6
1
1
a6,7 =
= sin φ cos θ uT , a6,8 =
= cos φ sin θ uT
∂x7
m
∂x8
m
∂f7
a7,7 =
= cos φ tan θq − sin φ tan θr
∂x7
2 sin φ
2 cos φ
∂f
a7,8 = 7 =
q+
r
∂x8
cos(2θ) + 1
cos(2θ) + 1
∂f7
∂f7
a7,10 =
= 1, a7,11 =
= sin φ tan θ
∂x10
∂x11
∂f7
∂f
= cos φ tan θ, a8,7 = 8 = − sin φq − cos φr
a7,12 =
∂x12
∂x7
∂f8
∂f8
a8,11 =
= cos φ, a8,12 =
= − sin φ
∂x11
∂x12
cos φ
sin φ
∂f
a9,7 = 9 =
q−
r
∂x7
cos θ
cos θ
2 sin φ sin θ
2 cos φ sin θ
∂f
a9,8 = 9 =
q+
r
∂x8
cos(2θ) + 1
cos(2θ) + 1
sin φ
cos φ
∂f9
∂f9
=
, a
=
=
a9,11 =
∂x11
cos θ 9,12 ∂x12
cos θ
∂f
−1
a10,7 = 10 =
mgzg cos φ cos θ
∂x7
Ixx
∂f
1
a10,8 = 10 =
mgzg sin φ sin θ
∂x8
Ixx
a1,4 =
79
a10,11 =
a11,10 =
∂f10
1
=
((I − Izz )r + Ir Ωr )
∂x11
Ixx yy
∂f
1
a10,12 = 10 =
(I − Izz )r
∂x12
Ixx yy
∂f
−1
mgzg cos θ
a11,8 = 11 =
∂x8
Iyy
∂f11
1
=
((I − Ixx )r − Ir Ωr )
∂x10
Iyy zz
a11,12 =
∂f12
1
=
(I − Iyy )q
∂x10
Izz xx
∂f
1
a12,11 = 12 =
(I − Iyy )p
∂x11
Izz xx
1
= −(cos φ sin θ cos ψ + sin φ sin ψ)
m
1
= −(cos φ sin θ sin ψ + sin φ cos ψ)
m
1
b6,1 = − cos ψ cos θ
m
1
b10,2 =
(I − Izz )
Ixx yy
1
b11,3 =
(I − Iyy )
Iyy zz
1
(I − Izz )
b12,4 =
Izz xx
a12,10 =
b4,1
b5,1
∂f11
1
=
(I − Ixx )p
∂x12
Iyy zz
B
Analytical QP-formulation of MPC
Consider the formulation of the MPC problem
min
uk+i ,xk+i ,x̄k ,ūk ,r̄k
s.t.
N
−1
X
l(xk+i − x̄k , uk+i − ūk ) + Φ(r̄k − r) + Ψ (xk+N − x̄k )
(B.1)
i=0
xk+i+1 = Axk+i + Buk+i + E dˆk
(B.2)
xk+i ∈ X
(B.3)
uk+i ∈ U
(B.4)
xk+N ∈ T (x̄k )
"
#" # "
#
A − I B x̄k
−E dˆk
=
C
D ūk
r̄k
(B.5)
where
l(x, u) = kxk2Q1 + kuk2Q2 ,
Φ(r) = krk2Z , Z ∈ N,
Ψ (x) = kxk2P .
Iterating (B.2) to time instant k = N gives
81
(B.6)
82
B
Analytical QP-formulation of MPC
xk+1 = Axk + Buk + E dˆk ⇒
xk+2 = Axk+1 + Buk+1 + E dˆk =
= A2 xk + ABuk + Buk+1 + (AE + E)dˆk ⇒
(B.7)
..
.
−1 j ˆ
xk+N = AN xk + AN −1 Buk + AN −2 Buk+1 + . . . + Buk+N −1 + ΣN
j=0 A E dk .
Expanding the sum in (B.1) using the results from (B.7) gives
min
uk+i ,xk+i ,x̄k ,ūk ,r̄k
(F xk + GU + E dˆk − X̄k )T Q1 (F xk + GU + E dˆk − X̄k )
+ (U − Ūk )T Q2 (U − Ūk ) + (r̄k − r)T Z(r̄k − r).
where
 




 
 uk 
 xk+1 
ūk 
x̄k 




 
 
 uk+1 
 xk+2 
ūk 
x̄k 












U =  .  , X =  .  , Ūk =  .  , X̄k =  . 
 .. 
 .. 
 .. 
 .. 



 
 

uk+N −1
xk+N
ūk
x̄k



F = 



0
 B


B

 AB
 , G =  .

 .
..

 .
.

AN
AN −1 B · · ·
···
···
..
.
A
..
.

Q1


Q1 = 


..
.
Q1
P
AB



E
0 



 AE + E 
0 



..
..  , E = 



.
. 
PN −1 j 

B
j=0 A E




Q2

 , Q = 

2




..
.
Q2



 ,


and state and control constraints matrices
T
Πx = bX

FX


FX = 


T
bX
···
..
bTT
T
.
FX
FT
Now define a new optimization variable
, Πu = bUT




FU

 , F = 
 U 



···
..
bUT
T
.
FU



 .


,
(B.8)
83
Γ = (uk uk+1 . . . uk+N −1 ūk x̄k r̄k )T
(B.9)
which gives
U = ∇U Γ
Ūk = ∇uk Γ
(B.10)
X̄k = ∇xk Γ
r̄k = ∇rk Γ
where ∇U , ∇uk , ∇xk and ∇rk are matrix operators that extracts the variables from
Γ.
Using (B.8), (B.9) and (B.10) one can write (B.1) as
min
Γ
(F xk + G∇U Γ + E dˆk − ∇xk Γ )T Q1 (F xk + G∇U Γ + E dˆk − ∇xk Γ )
+ (∇U Γ − ∇uk Γ )T Q2 (∇U Γ − ∇uk Γ ) + (∇rk Γ − r)T Z(∇rk Γ − r) =
= min (xkT F T Q1 F xk + xkT F T Q1 G∇U Γ + xkT F T Q1 E dˆk − xkT F T Q1 ∇xk Γ +
Γ
+ Γ T ∇TU G T Q1 F xk + Γ T ∇TU G T Q1 G∇U Γ + Γ T ∇TU G T Q1 E dˆk −
− Γ T ∇T G T Q1 ∇x Γ + dˆT E T Q1 F xk + dˆT E T Q1 G∇U Γ +
U
k
k
k
+ dˆkT E T Q1 E dˆk − dˆkT E T Q1 ∇xk Γ − Γ T ∇Txk Q1 F xk −
− Γ T ∇T Q1 G∇U Γ − Γ T ∇T Q1 E dˆk + Γ T ∇T Q1 ∇x Γ +
+
+
xk
xk
xk
k
T
T T
T T
Γ ∇U Q2 ∇U Γ − Γ ∇U Q2 ∇uk Γ − Γ ∇uk Q2 ∇U Γ +
Γ T ∇Tuk Q2 ∇uk Γ + Γ T ∇Trk Z∇rk Γ − Γ T ∇Trk Zr − r T Z∇rk Γ
T
+ r T Zr) .
which, by eliminating terms independent of Γ , is equal to
min (2Γ T ∇TU G T Q1 F xk − 2Γ T ∇Txk Q1 F xk +
Γ
+ Γ T ∇TU (G T Q1 G + Q2 )∇U Γ + 2Γ T ∇TU G T Q1 E dˆk − 2Γ T ∇TU G T Q1 ∇xk Γ −
− 2Γ T ∇xk Q1 E dˆk + Γ T ∇Txk Q1 ∇xk Γ − 2Γ T ∇TU Q2 ∇uk Γ +
+ Γ T ∇Tuk Q2 ∇uk Γ + Γ T ∇Trk Z∇rk Γ − 2Γ T ∇Trk Zr) .
This can be expressed as the quadratic programming problem
84
B
Analytical QP-formulation of MPC
min Γ T (∇TU (G T Q1 G + Q2 )∇U − 2∇TU G T Q1 ∇xk + ∇Txk Q1 ∇xk − 2∇TU Q2 ∇uk +
Γ
+ ∇Tuk Q2 ∇uk + ∇Trk Z∇rk )Γ
+ 2Γ T (∇TU G T Q1 F xk − ∇Txk Q1 F xk + ∇TU G T Q1 E dˆk − ∇xk Q1 E dˆk − ∇Trk Zr)
"
s.t.
#
"
#
FU ∇U
Πu
Γ ≤
FX G∇U
Πx − FX F xk
(B.11)
Bibliography
M. Abdolhosseini, Y. M. Zhang, and C. A. Rabbath. An efficient model predictive
control scheme for an unmanned quadrotor helicopter. Journal of Intelligent
& Robotic Systems, 70(1):27 – 38, 2013. Cited on pages 3 and 33.
K. Alexis, G. Nikolakopoulos, and A. Tzes. Switching model predictive attitude
control for a quadrotor helicopter subject to atmospheric disturbances. Control
Engineering Practice, 19:1195 – 1207, 2011. ISSN 0967-0661. Cited on pages
3, 27, and 33.
K-J. Barsk. Model Predictive Control of a Tricopter. [Elektronisk resurs].
Linköping : Univ., 2012, 2012. Cited on page 3.
K. Bergman and J. Ekström. Modeling, Estimation and Attitude Control of an Octorotor Using PID and L1 Adaptive Control Techniques. [Elektronisk resurs].
Linköping : Univ., 2014, 2014. Cited on page 3.
F. Borrelli, A. Bemporad, and M. Morari. Predictive Control for linear and hybrid
systems. 2014. Cited on page 34.
S. Bouabdallah. Design and control of quadrotors with application to autonomous flying., 2007. Cited on pages 3, 27, and 33.
P-J. Bristeau, P. Martin, E. Salaün, and N. Petit. The Role of Propeller Aerodynamics in the Model of a Quadrotor UAV., 2009. Cited on page 3.
J. Carlbring and S. Karlsson. Multirotor drivetrain efficiency. Bachelor’s thesis at
Linköping University, 2014. Cited on page 16.
P. Christensen. Elementär mekanik, del 2. Division of Mechanics, Linköpings
universitet, 2011. Cited on page 28.
C. Chéron, D. Aaron, S. Vardan, and Y. Chen. A multifunctional hil testbed for
multirotor vtol uav actuator. In Proceedings of 2010 IEEE/ASME International
Conference on Mechatronic and Embedded Systems and Applications, MESA
2010, pages 44–48, Department of Electrical and Computer Engineering, Center for Self-Organizing and Intelligent Systems, Utah State University, 2010.
Cited on page 46.
85
86
Bibliography
L. Euler. Formulae generales pro translatione quacunque corporum rigidorum.
Novi Commentarii academiae scientiarum Petropolitanae 20, pp. 189–207
(E478), 1776. Cited on page 11.
T. Glad and L. Ljung. Reglerteori : flervariabla och olinjära metoder. Lund :
Studentlitteratur, 2003, 2003. ISBN 9144030037. Cited on page 38.
T. Glad and L. Ljung. Modellbygge och simulering. Lund : Studentlitteratur,
2004, 2004. ISBN 9144024436. Cited on page 29.
W. R. Hamilton. Lxxv. on quaternions ; or on a new system of imaginaries in
algebra. Philosophical Magazine Series 3, 31(211):511, 1847. ISSN 19415966.
Cited on page 9.
U. Jönsson, C. Trygger, and P. Ögren. Optimal control. Lecture notes, Optimization and system theory, Dept. of Mathematics, Royal Institute of Technology
(KTH), Sweden, 2003. Cited on page 38.
J. B. Kuipers. Quaternions and rotation sequences : a primer with applications
to orbits, aerospace, and virtual reality. Princeton, N.J. : Princeton University
Press, cop. 1999, 1999. ISBN 0691058725. Cited on pages 9, 10, and 11.
J. G. Leishman. Principles of helicopter aerodynamics. Cambridge aerospace series. Cambridge : Cambridge University Press, 2006, 2006. ISBN 0521858607.
Cited on pages 1, 3, 23, and 27.
H. Liu, Y. Zhong, D. Derawi, and J. Kim. Robust optimal attitude control of
hexarotor robotic vehicles. Nonlinear Dynamics, 74(4):1155–1168, 2013. ISSN
0924090X. Cited on pages 3 and 33.
T. Magnusson. Attitude Control of a Hexarotor. [Elektronisk resurs]. Linköping :
Univ., 2014, 2014. Cited on page 3.
D. Q. Mayne, J. B. Rawlings, C. V. Rao, and P. O. M. Scokaert. Survey paper:
Constrained model predictive control: Stability and optimality. Automatica,
36:789 – 814, 2000. ISSN 0005-1098. Cited on page 35.
K. R. Muske and J. B. Rawlings. Model predictive control with linear models.
AIChE Journal (American Institute of Chemical Engineers); (United States),
page 262, 1993. ISSN 0001-1541. Cited on page 36.
C. Nordling and J. Österman. Physics handbook for science and engineering.
Lund : Studentlitteratur, 2006, 2006. ISBN 9144044534. Cited on page 43.
P. Pounds, R. Mahony, and P. Corke. Modelling and control of a large quadrotor
robot. Control Engineering Practice, 18(Special Issue on Aerial Robotics):691 –
699, 2010. ISSN 0967-0661. Cited on pages 3 and 27.
T. Raharijaoana and F. Bateman. Robust control for an off-centered quadrotor.,
2013. Cited on page 3.
Bibliography
87
F. Rinaldi, S. Chiesa, and F. Quagliotti. Linear quadratic control for quadrotors
uavs dynamics and formation flight. Journal of Intelligent & Robotic Systems,
70(1):203 – 220, 2013. Cited on pages 3 and 33.
J. A. Rossiter. A global approach to feasibility in linear mpc. UKACC ICC, 2006.
Cited on page 36.
D. Simon. Model Predictive Control in Flight Control Design. [Elektronisk
resurs] : Stability and Reference Tracking. Linköping Studies in Science and
Technology. Thesis: 1642. Linköping : Department of Electrical Engineering,
Linköping University, 2014, 2014. Cited on pages 3 and 33.
88
Bibliography
Upphovsrätt
Detta dokument hålls tillgängligt på Internet — eller dess framtida ersättare —
under 25 år från publiceringsdatum under förutsättning att inga extraordinära
omständigheter uppstår.
Tillgång till dokumentet innebär tillstånd för var och en att läsa, ladda ner,
skriva ut enstaka kopior för enskilt bruk och att använda det oförändrat för ickekommersiell forskning och för undervisning. Överföring av upphovsrätten vid
en senare tidpunkt kan inte upphäva detta tillstånd. All annan användning av
dokumentet kräver upphovsmannens medgivande. För att garantera äktheten,
säkerheten och tillgängligheten finns det lösningar av teknisk och administrativ
art.
Upphovsmannens ideella rätt innefattar rätt att bli nämnd som upphovsman
i den omfattning som god sed kräver vid användning av dokumentet på ovan
beskrivna sätt samt skydd mot att dokumentet ändras eller presenteras i sådan
form eller i sådant sammanhang som är kränkande för upphovsmannens litterära
eller konstnärliga anseende eller egenart.
För ytterligare information om Linköping University Electronic Press se förlagets hemsida http://www.ep.liu.se/
Copyright
The publishers will keep this document online on the Internet — or its possible replacement — for a period of 25 years from the date of publication barring
exceptional circumstances.
The online availability of the document implies a permanent permission for
anyone to read, to download, to print out single copies for his/her own use and
to use it unchanged for any non-commercial research and educational purpose.
Subsequent transfers of copyright cannot revoke this permission. All other uses
of the document are conditional on the consent of the copyright owner. The
publisher has taken technical and administrative measures to assure authenticity,
security and accessibility.
According to intellectual property law the author has the right to be mentioned when his/her work is accessed as described above and to be protected
against infringement.
For additional information about the Linköping University Electronic Press
and its procedures for publication and for assurance of document integrity, please
refer to its www home page: http://www.ep.liu.se/
© Simon Lindblom & Adam Lundmark
Was this manual useful for you? yes no
Thank you for your participation!

* Your assessment is very important for improving the work of artificial intelligence, which forms the content of this project

Related manuals

Download PDF

advertisement