Documenti di Didattica
Documenti di Professioni
Documenti di Cultura
.
Given the outputs provided by the available sensors, a Kalman lter was developed
capable of estimating the angular velocities and Euler angles of the quadrotor, being therefore
possible to have knowledge of its attitude. This lter was combined with a Linear-Quadratic Regulator
in order to make the
system follow a given attitude reference. Finally, the results of the simulati
ons and real implementation
in the quadrotor are presented.
ii
iii
Resumo
Existe actualmente uma variedade reduzida de veculos capazes de descolagem e ater
ragem vertical.
No entanto existe um interesse visvel por parte de investigadores e admiradores d
e Veculos Areos
No Tripulados num tipo especial de aeronave denominada de quadrirotor. Este tra
balho tem precisamente como objectivo desenvolver e controlar um prottipo de um quadrirotor, possu
indo apenas como
sensores um acelermetro triaxial e uma bssola.
Comeou-se por conceber o quadrirotor tendo em vista uma lista de ob
jectivos a cumprir pelo
mesmo. Seguiu-se a modelao da dinmica e cinemtica da aeronave, assim co
mo tambm dos
seus motores e sensores. Da resultou a implementao do sistema em ambiente de simu
lao computacional Simulink
.
Tendo em conta as sadas fornecidas pelos sensores disponveis, foi dese
nvolvido um
ltro de
Kalman capaz de estimar apenas as velocidades angulares e os ngulos de Euler do q
uadrirotor, sendo
portanto possvel ter conhecimento da atitude do mesmo. Este ltro foi combinado co
m um controlador
LQR por forma fazer o sistema seguir uma dada referncia de atitude.
Finalmente, os resultados das simulaes e da implementao real no quadrirotor so aprese
ntados.
iv
v
Keywords
Quadrotor modelling
Quadrotor simulation
Kalman lter
Linear-Quadratic Regulator
vi
vii
Acknowledgments
First of all, I would like to thank my teachers, Professor Jos Raul Azinheira and
Professor Alexandra
Bento Moutinho for their advice, support and availability to speak with me throu
ghout the course of this
thesis.
I must not forget to thank my colleague Tiago Rita, who shared with me the dif cul
ty of working with a
UAV, Tiago Gonalves also for offering his aid and knowledge on UAVs, and Mr. Rap
oseiro who helped
me with the quadrotor construction.
Last but not least, I want to give special thanks to my family for all their lov
e, support, and for encouraging me to work my way through college, a path that made me gain precious life
experiences, which i
would have missed otherwise.
viii
Contents
Abstract i
Resumo iii
Keywords v
Acknoledgments vii
List of Tables xiii
List of Figures xvii
Notation xix
Acronyms xix
Subscripts xix
Superscripts xix
List of symbols xxi
1 Introduction 1
1.1 De nition and purpose of a quadrotor . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . 1
1.2 Quadrotor operation . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . 1
1.3 Historical role of quadrotors . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . 2
1.4 Motivation . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . . . . 6
and Simulink
. . . . . . . . . . . . . . . . . . . .
. . 41
4 Kalman Filter 45
4.1 State-space system linearization . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . 46
4.2 System analysis . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . . 47
4.3 Mathematical formulation . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . 48
4.4 Kalman lter in MATLAB
and Simulink
. . . . . . . . . . . . . . . . . . . .
. . . . . . . . 49
4.5 Kalman lter for Arduino . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . 54
5 Optimal Control - The Linear-Quadratic Regulator 55
5.1 Mathematical formulation . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . 55
5.2 LQR control in MATLAB
and Simulink
. . . . . . . . . . . . . . . . . . . .
. . . . . . . . 56
5.3 LQR control in the Arduino . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . 58
6 Implementation and results 59
6.1 Full 12 states control . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . 60
6.2 6 states control . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . . . 62
6.2.1 LQG control with sensors . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
62
les 81
B.1 loadvars.m . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . . . 81
B.2 quadsys.m . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . . . 87
C Arduino source code 93
xii CONTENTS
List of Tables
3.1 Dead zone pulse width of each motor. . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
34
3.2 Data points gathered for understanding the motor-propeller dynamics. .
. . . . . . . . . . 34
3.3 Gains of the transfer functions describing the motors dynamics. . .
. . . . . . . . . . . . . 35
3.4 Quadrotors mass and main geometric variables. . . . . . . .
. . . . . . . . . . . . . . . . 37
3.5 Relevant accelerometer data. . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . 40
3.6 Relevant compass data . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . 41
5.1 Maximum state values for designing matrix
Q. . . . . . . . . . . . . . . . . . . .
. . . . . 56
5.2 Maximum input values for designing matrix
R. . . . . . . . . . . . . . . . . . . .
. . . . . 57
xiii
xiv LIST OF TABLES
List of Figures
1.2.1Yaw, pitch and roll rotations of a common quadrotor. . . . . .
. . . . . . . . . . . . . . . . 2
1.2.2Illustration of the various movements of a quadrotor. . . . . .
. . . . . . . . . . . . . . . . 2
2.4.1Thunderbird-9 ESC. . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . 18
2.4.2Electric DC Brushless motor Robbe ROXXY BL-Outrunner 2824-34. . . .
. . . . . . . . . 18
2.4.3Arduino Duemilanove. . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . 19
2.4.4XBee 802.15.4 1mW wireless module. . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . 19
2.4.5ADXL330 accelerometer. . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . 20
2.4.6HMC6352 compass. . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . 20
1
http://www.thingsmagazine.net/blogimages/
2
http://www.aviastar.org/helicopters\_eng/bothezat.php
3
http://www.aviastar.org/helicopters\_eng/convertawings.php
4
http://www.symscape.com/node/622
5
http://aero.ep .ch/page57689.html
xv
xvi LIST OF FIGURES
2.4.7Vislero LiPo 11,1V 2200mAh 20C battery. . . . . . . . . .
. . . . . . . . . . . . . . . . . . 21
2.5.1Example illustration of the angular velocity of a DC motor as a function of
power supply
voltage. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . . . 22
3.1.2NED and ABC reference frames. . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. 26
3.1.1Visualization of the North-East-Down reference frame. . . . . .
. . . . . . . . . . . . . . . 26
3.4.1Schematic of the input-output sequence for identi cation purposes. . . .
. . . . . . . . . . 32
3.4.2Microphone positioned to record the propeller sound. . . . . . .
. . . . . . . . . . . . . . . 33
3.4.3Propeller sound. Each step in amplitude is a consequence of a propeller bla
de passage. . 33
3.4.4Dynamic behaviour of the motors. The grey areas show a zoom on of the lin
earization
zone in red. . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . . 34
3.4.5Input-output data used with the system identi cation toolbox to calculate the
time constant. 35
3.4.6Blocks used in Simulink
to simulate motor 1. . . . . . . . . . . . . . . .
. . . . . . . . . . 36
3.5.1Distance from the motors to the center of gravity. . . . . . .
. . . . . . . . . . . . . . . . . 37
3.6.1The red dot in the center of the gure indicates the center of gravity. .
. . . . . . . . . . . 38
3.8.1Block diagram of quadrotor dynamics. . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
42
3.8.2Block diagram of the sensors. . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . 42
3.8.3Inside view of sensors Simulink
block. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . 43
4.4.1Kalman lter schematic. . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . 54
5.2.1LQR control. . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . . . 57
5.2.2LQG control block diagram in Simulink
. . . . . . . . . . . . . . . . . . . .
. . . . . . . . . 58
6.1.112 states LQR control loop. . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . 60
6.1.2Time response of the LQR control loop using with 12 states feedback. .
. . . . . . . . . . 61
6.1.3Linear positions with increasing weight matrix
Q. . . . . . . . . . . . . . . . . . . .
. . . . 62
6.1.4Linear positions with increasing weight matrix
R. . . . . . . . . . . . . . . . . . . .
. . . . 62
6.2.16 states Simulink
. . . 68
6.3.4Kalman lter estimation of the yaw angle in the quadrotor prototype. . .
. . . . . . . . . . 69
6.3.5Time response of the LQR control loop using 6 states, sensors and motors dy
namics with
PWM resolution. . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . 70
6.3.6Sensor noise data. . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . . 71
6.4.1Time response of the LQR control loop using 6 states and gyros while in the
presence of
very noisy sensor readings. . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . 73
A.1 Accelerometer wiring. . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . . . . 79
LIST OF FIGURES xvii
A.2 Compass and electronic speed controllers wiring. . . . . . . .
. . . . . . . . . . . . . . . . 80
A.3 Arduinos battery check circuit. . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . .
. . 80
xviii LIST OF FIGURES
Notation xix
Notation
Acronyms
ALIV - Autonomous Locomotion Individual Vehicle
COG - Center Of Gravity
DC - Direct Current
ESC - Electronic Speed Controller
GPS - Global Positioning System
LED - Light-emitting diode
LQR - Linear-Quadratic Regulator
LQ - Linear-Quadratic
LQG - Linear-Quadratic Gaussian
MIMO - Multiple-Input Multiple-Output
PD - Proportional-Derivative
PID - Proportional-Integral-Derivative
PWM - Pulse-Width Modulation
RC - Remote Control
SISO - Single-Input Single-Output
UAV - Unmanned Aerial Vehicle
VTOL - Vertical Take-Off and Landing
Subscripts
B - Regarding the aircrafts body centered coordinate frame
g - Regarding gravity
net - Resultant (e.g. resultant force)
Superscripts
T - Matrix transpose
xx Notation
Notation xxi
List of symbols
Symbol Domain Unit Description
R - Quat rnion angl of rotation
R rad Pitch angl
R rad.s
1
Pitch angle rate
R kg.m
3
Ai density at sea level and 20
C
R s Time consan
R rad Roll angle
R rad.s
1
Roll angle rae
R rad Yaw angle
R rad.s
1
Yaw angle rae
R ad.s
1
Popelle angula velocity
R
31
ad.s
1
Quadotos angula velocities
_
P Q R
_
1
,
2
,
3,
4
R ad.s
1
Angula velocity of each moto
A R
nn
State matix (n states)
A
K
R
nn
Kalman lte state matix (n states)
a
m
R
31
m.s
2
Acceleation measued by the acceleomete
a
p
R
31
m.s
2
Absolute acceleation at the point hee the senso is located,
a
p
=
_
a
px
a
py
a
pz
_
a
B
R
31
m.s
2
Acceleation in the quadotos body
B R
nm
Input matix (n states, m inputs)
B
K
R
nm
Kalman lte input matix
c
T
R none Thust coef cient of a popelle
c
P
R
Poe coef cient of a popelle
C R
qn
Output matix (q outputs, n states)
C
K
R
qn
Kalman lte output matix (q outputs, n states)
D
P
R m Popelle diamete
D R
qm
Feedfoad matix (q outputs, m inputs)
D
K
R
qm
Kalman lte feedfoad matix (q outputs, m inputs)
d
cg
R m Distance beteen the quadotos cente of gavity and a moto
e
x
R
n1
Eo of the dynamics deivative (n states)
e
y
R
q1
Output eo (q outputs)
F
net
R
31
N Combination of all the foces acting on the quadoto,
F
net
=
_
F
x
F
y
F
z
_
F
P
R
31
N Total thust geneated by the popelles,
F
P
=
_
F
Px
F
Py
F
Pz
_
f
t
R s Flight autonomy
f R Quadotos model function to lineaize
G(s) R
Moto dynamics tansfe function
G R
np
Obsevation matix (n states, p pocess noise measuements)
g R m.s
2
Eaths gavity (constant value of 9.81m.s
2
)
xxii Notation
H R
qp
Matix that elates the pocess noise ith the outputs of a
system in the state space fom (q outputs, p pocess noise
measuements)
I R
33
kg.m
2
Inetia matix of the quadoto
I R
nn
Identity matix (n states)
I
x
, I
y
, I
z
R kg.m
2
Elements of a motos inetia tenso espectively elated to u
x
,
u
y
and u
x
I
R
I
0
1
Popelles speed in evolutions pe minute pe unit of voltage
k R Time instant k
L
nq
R
Kalman gain (n states, q outputs)
M
net
R
31
Sum of all the moments acting on the quadoto,
M
net
=
_
M
x
M
y
M
z
_
M
P
R N.m Popelles moment
m R kg Mass of the quadoto
N
c
R ad Noth magnetic pole
N R Ode of the state space system
n R s
1
Popelles speed in evolutions pe second
n
1
, n
2
, n
3
R
Elements of a unit vecto used to build a quatenion
O
ABC
R
3
AicaftBodyCenteed efeence fame
O
NED
R
3
NothEastDon efeence fame
O R
nqn
Obsevability matix (n states, q outputs)
P R ad.s
1
Angula speed aound the u
x
axis of the quadoto
P R ad.s
2
Angula acceleation of the quadoto along the u
x
axis of the
inetial efeence fame
P
P
R W Popelle poe
P R
n1
Steadystate eo covaiance matix (n states)
p
R
Lineaization point of the system
P R
mq
Unique positivede nite solution fo the LQR contolle Riccati
equation (m inputs, q outputs)
P R s Pulse idth of the PWM signal diving a moto
P
0
R s PWM signal pulse idth of the motos dead zone
Q R ad.s
1
Angula speed aound the u
y
axis of the quadoto
Notation xxiii
Q R ad.s
2
Angula acceleation of the quadoto along the u
y
axis of the
inetial efeence fame
Q
R
Q
k
R
Q
kk
Q
nn
Q
k
R
Q
q
41
_
q
A.s
Diagonal elements of
R
Quatenion, q =
0
q
1
q
2
q
3
_
q R
41
Angula velocity quatenion
R R ad.s
1
Angula speed aound the u
z
axis of the quadoto
R
m
R Motos coppe coil electic esistance
R R ad.s
2
Angula acceleation of the quadoto along the u
z
axis of the
inetial efeence fame
R
() R
33
Roaion marix ha expresses he orienaion o he O
ABC
rame around he u
x
axis o O
NED
R
() R
33
Roaion marix ha expresses he orienaion o he O
ABC
rame around he u
x
axis o O
NED
R
() R
33
Roaion marix ha expresses he orienaion o he O
ABC
rame around he u
x
axis o O
NED
R
acc
R m.s
2
R R
mm
LQR conroller weighing marix (m inpus)
R
k
R
Diagonal elemens o
R
r R m Propeller radius
r R
31
m Posiion o he O
ABC
coordinae rame relaive o O
NED
S R
33
Roaion marix (also known as direcion cosine marix)
S
R
33
Quaernion euivalen o he roaion marix
T R N Propeller hrus
T R
33
Marix ha relaes he bod xed angular veloci vecor wih he
rae o change o he Euler angles
T
P
R s Time period beween he passage o wo propeller blades
s
R s Sample ime.
U R m.s
2
Linear acceleraion o he uadroor along he u
x
axis o he
inerial reerence rame
u
0
R m.s
1
Aircra igh veloci
u
x
R
Xaxis o he O
NED
u
x
reerence rame
Xaxis o he O
R
ABC
reerence rame
u
R
Yaxis o he O
NED
reerence rame
u
R
Yaxis o he O
ABC
reerence rame
u
z
R
Zaxis o he O
NED
reerence rame.
u
z
Zaxis o he O
R
ABC
reerence rame
u R
41
Vecor conaining he inpus o he ssem a an euilibrium poin
xxiv Noaion
u
K
R
41
LQR vecor o conrol acions
u
k,max
R
Highes olerable value or inpu k
V R m.s
2
Linear acceleraion o he uadroor along he u
axis o he
inerial reerence rame
V
m
R V Moors power suppl volage
V
0
R V Minimum power suppl reuired or propeller roaion
v
R
31
m.s
1
Vecor conaining he uadroors linear velociies
_
U V W
_
v
p
R
31
m.s
1
Vecor o linear velociies a he poin where he acceleromeer is
locaed
v
cg
R
31
m.s
1
Vecor o linear velociies o he uadroors cener o gravi
v R
i
Measuremen noise (i sensor measuremens)
W R m.s
2
Linear acceleraion o he uadroor along he u
z
axis o he
inerial reerence rame
w
v
R rad.s
1
.V
1
Propeller angular speed in S.I. unis per uni o volage
w R
p1
Process noise (p process noise measuremens)
X R m Posiion o he uadroors cener o gravi along he xaxis he
inerial reerence rame O
NED
X R m.s
1
Time derivaive i he O
ABC
rames posiion along u
x
x R
n1
Sae vecor (n saes)
x R
n1
Vecor conaining he saes o he ssem a an euilibrium poin
(n saes)
x
k,max
R
Highes olerable value or sae k
x R
n1
Esimaed saes b he Kalman ler (n saes)
Y R m Posiion o he uadroors cener o gravi along he axis he
Y R m.s
1
Time derivaive i he O
ABC
rames posiion along u
R
Oupu vecor
R
1
Esimaed oupus b he Kalman ler ( oupus)
Z R m Posiion o he uadroors cener o gravi along he zaxis he
inerial reerence rame O
NED
Z R m.s
1
Time derivaive i he O
ABC
rames posiion along u
z
Chaper 1
Inroducion
1.1 De niion and purpose o a uadroor
A uadroor, or uadroor helicoper, is an aircra ha becomes airborne due
o he li orce provided b
our roors usuall mouned in cross con guraion, hence is name. I is an enire
l dieren vehicle when
compared wih a helicoper, mainl due o he wa boh are conrolled. Helicop
ers are able o change
he angle o aack o is blades, uadroors canno.
A presen, here are hree main areas o uadroor developmen: miliar, ra
nsporaion (o goods
and people) and Unmanned Aerial Vehicles (UAVs) [5]. UAVs can be classi ed ino
wo major groups:
heavierhanair and ligherhanair. These wo groups sel divide in man ohe
r ha classi aircras ac
cording o moorizaion, pe o lio and man oher parameers. Verical Tak
eO and Landing (VTOL)
UAVs like uadroors have several advanages over xedwing airplanes. The can mo
ve in an direcion
and are capable o hovering and a low speeds. In addiion, he VTOL capabili
allows deplomen
in almos an errain while xedwing aircra reuire a prepared airsrip or ak
eo and landing. Given
hese characerisics, uadroors can be used in search and rescue missions, me
eorolog, peneraion
o hazardous environmens (e.g. exploraion o oher planes) and oher applic
aions suied or such an
aircra. Also, he are plaing an imporan role in research areas like con
rol engineering, where he
serve as proopes or real lie applicaions.
1.2 Quadroor operaion
Each roor in a uadroor is responsible or a cerain amoun o hr
us and orue abou is cener o
roaion, as well as or a drag orce opposie o he roorcras direcion o igh
. The uadroors propellers
are no all alike. In ac, he are divided in wo pairs, wo pusher and w
and is ighs were a success, even in orward igh. The projec ended mainl due
o he lack o demand
or he aircra.
Figure 1.3.5: Converawings Model A helicoper
5
.
3
hp://www.hingsmagazine.ne/blogimages/
4
hp://www.aviasar.org/helicopers_eng/boheza.php
5
hp://www.aviasar.org/helicopers_eng/converawings.php
1.3. HISTORICAL ROLE OF QUADROTORS 5
Recenl here has been an increasing ineres
in uadroor designs.
Bell is working on a uad
ilroor o overcome he V22 Ospra (see Figure 1.3.6), capable o carring a
large paload, achieving
high veloci and while using a shor amoun o space or Verical TakeO and
Landing (VTOL). Much
o is ssems come direcl rom he V22 excep or he number o engines. Als
o, he wing srucure on
he new design has some improvemens, i has a wider wing span on he rear roor
s. As a conseuence,
he Bell uad ilroor (Figure 1.3.7) aims or higher perormance and uel econ
om [7].
Figure 1.3.6: V22 Ospra
6
. Figure 1.3.7: Concep o Bells uad ilroor
7
.
Anoher recen and amous uadroor design is he Moller Skcar (Figure 1.3.8),
a proope or a
personal VTOL ing car. The Skcar has our duced ans allowing or a saer and
e cien operaion
a low speeds. I was a arge or much criicism because he onl demonsrai
ons o igh were hover
ess wih he Skcar ehered o a crane [8]. Is invenor, Paul Moller alrea
d ried o sell he Skcar
b aucion wihou success. Nowadas he ocuses his work on he precursor o he
Skcar, he M200G
Volanor, a ing saucersle hovercra. This laer model uses eigh ans conrol
led b a compuer and
is capable o hovering up o 3 m above he ground. This limiaion is imposed b
he onboard compuer
due o regulaions o he Federal Aviaion Adminisraions, saing ha an veh
icle ha ies above 3 m is
regulaed as an aircra [9].
Figure 1.3.8: Skcar during a es igh
8
.
Quadroors are also available o he public hrough radio conrolled os. Some
enhusiass as well as
researches have been developing heir own uadroor proopes. This is possible
due o he availabili
6
hp://zerosix.wordpress.com/2007/10/06/v22insearchandrescuemodearizonano
ira/
7
hp://sisemadearmas.sies.uol.com.br/ca/aco01il.hml
8
hp://www.smscape.com/node/622
6 CHAPTER 1. INTRODUCTION
Figure 1.4.1: Ambulance suck in a ra c jam in Lisbon, Porugal.
o cheap elecronics and lighweigh resisan maerials available o he public
. Be i or personal sais
acion, enerainmen, miliar or civilian use, uadroors have plaed an impo
ran role in he evoluion
o aircras and ma prove hemselves as imporan means o ransporaion in a
near uure.
1.4 Moivaion
In siuaions where peoples lives are a danger here is regularl he need or
ransporaion o injured or
ill paiens o a hospial or an oher medical acili capable o providing co
mpeen medical care. Oen,
when an ambulance canno rapidl reach he emergenc sie, medical aircra are
usuall deploed in
alernaive, a use which has proven o be uie valuable so ar. The rs recor
d o his kind o acion
daes back o 1870, where during he FrancoPrussian War 160 French
soldiers were ranspored b
hoair balloon o France [10]. Since hen, he concep o medical use aircra
has evolved uie a lo,
eiher in he miliar or civilian perspecives. Nowadas, medical assisance
helicopers and airplanes
are ver well euipped (e.g. some pes o euipmen he have onboard: venila
ors, elecrocardiogram
moniors, de brillaors, ec.), allowing or immediae reamen o criical cases
, avoiding in man o hem
deah beore reaching he hospial. These aircras serve grea and noble purpos
es, especiall in remoe
and isolaed inhabied areas, bu wha abou pracical uncion in developing c
iies? Surel here is a
large concenraion o medical aciliies inside a big ci, bu here are also
oher acors o consider like
ra c jams (Figure 1.4.1).
This causes a as ambulance rip o become a oal nighmare, aking hours o r
each he desinaion,
no speaking o he risk o acciden where crew and paien can be injured sever
el. We can o course
emplo helicopers, bu heir dimensions are a serious obsacle due o he high
diameer o he propeller
which can collide ino a ree or elecrici pole. Wha
i we cou
ld arrange an aircra
wih he same
characerisics o a helicoper, bu wih smaller size? A possible answer o h
is uesion is he use o a
uadroor vehicle. Here are he advanages:
No gearing necessar beween he moor and he roor;
No variable propeller pich is reuired or alering he uadroor angle o a
ack;
No roor sha iling reuired;
1.5. STATE OF THE ART 7
4 smaller moors insead o one big roor resuling in less sored
kineic energ and hus less
damage in case o accidens;
Minimal mechanical complexi,
Quadroors reuire less mainenance compared o boh helicopers and planes;
Roor mechanics simpli caion;
Paload augmenaion;
Groscopic eecs reducion.
Alhough here are he drawbacks o weigh augmenaion and high energ consump
ers. Two ears laer, in reerence [14] he analsis o he aircras aiude d
namics allowed he uning
o he mechanical design o improve disurbance rejecion and conrol sensiivi
. A linear SISO conroller
was hen implemened or aiude conrol, aiming o sabilize dominan decouple
d pich and roll modes,
while using a model o disurbance inpus o esimae he perormance o he pla
n. The model o he
plan also implemened blade apping o he propellers, having he experimenal re
suls in he X4 Fler
illusraed ha blade apping inroduces signi can sabili eecs on he vehicl
e. Resuls showed ha
he compensaor was able o conrol aiude a low roor speeds .
Figure 1.5.2: X4 Fler Mark II [1].
Mokhari e al. [15] presened a mixed robus eedback linearizaion wih linear
GH
conroller applied
o a nonlinear uadroor. Acuaor sauraion and consrain on sae
space oupu
was implemened
case scenario o
conrol law design. Also,
o analze he wors
he conrollers behavior was esed
when subjugaed o parameer uncerainies, exernal disurbances and measuremen
noise. Simulaions
revealed ha he overall ssem becomes robus when weighing uncions are cho
sen judiciousl.
Ari cial vision has also proven o be a resourceul ool when conr
olling uadroors. Tournier e
10
hp://aero.ep .ch/page57689.hml
1.5. STATE OF THE ART 9
al. [16] presened visionbased esimaion and conrol o a uadroor using a
single camera relaive o
a arge ha incorporaes moir paerns. The purpose o his research was o a
cuire six degree o
reedom esimaion, which is essenial or he operaion o vehicles in close pr
oximi o oher aircra and
landing plaorms. Tess showed he usabili o he ssem o auonomousl hove
r he aircra, given ha
he arge remains wihin he cameras eld o view.
Taking ino accoun ha curren designs oen consider onl nominal operaing c
ondiions or vehicle
conrol design, Homann e al. [2] seeks o engage in he sud o he issues
ha arise when deviaing
signi canl romhe hover igh regime. Three dieren uadroor aerodnamic eec
s were invesigaed
and heir relaion o vehicular veloci, angle o aack and airrame design. T
he rs eec sudied was he
wa how oal hrus varies no onl wih inpu power, bu wih he ree sream
veloci and angle o aack
wih respec o he ree sream. The second phenomenon was blade apping, a resul o
dieren in ow
velociies experienced b he advancing and rereaing blades, which induces rol
l and pich momens on
he roor hub as well as a de ecion o he hrus vecor. Lasl, he eec o
inererence caused b he
vehicles bod componens locaed near he slip sream o he propellers was inves
igaed because i is
responsible or unsead hrus, rendering aiude racking di cul. The ull ex
en o hese aerodnamic
eecs was uncovered wih he help o he STARMAC II uadroor (see Figure 1.5.
3), which possesses
auonomous aiude and aliude conrol. Resuls proved ha exising models an
d conrol echniues are
inadeuae or accurae rajecor racking a high speed and in unconrolled en
vironmens.
Figure 1.5.3: STARMAC II uadroor [2].
Mehme Ee [17] provided he ull dnamical model o a commerciall available u
adroor roorcra
and presened is behavior a low aliudes hrough sliding mode conrol, which
is a conrol echniue
known b is robusness agains disurbances and invariance during he sliding r
egime. The plan was
modeled as being nonlinear, wih sae variables ighl coupled. The assumpion
s made included unsa
uraed conrols, negligible weaher disurbances and reasonabl as acuaion p
eripher. The simulaions
showed ha he algorihm successull drives he ssem owards he desired ra
jecor wih bounded
conrol signals.
In he Universi o Caliornia, Berkle, Kaie Miller [18] sough o design con
rol laws or a uadroor
helicoper o rack a desired pah b implemenaion o wapoins, more speci cal
l a she used a PD
(ProporionalDerivaive) or posiioning conrol and a PID or roaion conrol
. The conrol laws were de
veloped on a model o he uadroor dnamics, which was obained b linearizing
is nonlinear euaions
o moion. The conrol laws were sudied while in he presence o modeling unc
erainies and exernal
disurbances like wind guss. The resuls showed ha while he linear conrol
laws are adeuae under
perec condiions, when uncerain is inroduced, he do no perorm well.
10 CHAPTER 1. INTRODUCTION
Suden compeiions are anoher wa o promoing uadroor developmen. AbouS
leiman e al. [3]
designed a uadroor robo or he Sixh Annual Unmanned Aerial Ssems Compei
ion
11
rom scrach
(see Figure 1.5.4). The uadroor uses GPS or wapoin racking and aliude,
onboard camera and a
dual processor capable o auonomous pah navigaion and daa exchange wih he
ground saion. Con
rol was made wih 4 PID conrollers (respecivel o conrol pich, roll, aw a
nd hrole) and heir gains
were ound experimenall. During he sar o he compeiion, immediael a
er akeo he uadroor
was hi b srong wind urbulence bu he conroller perormed exremel well an
d was able o sabilize
he ssem.
Figure 1.5.4: Oakland Universi uadroor ssem [3].
Srgio Pereira [19] has recenl presened his maser hesis where he proposed a m
odel o simulae
he ALIV (Auonomous Locomoion Individual Vehicle) uadroor UAV (Figu
re 1.5.5). He was able o
es he capabiliies o he ALIV b using a model buil in MaLab and Simulink, w
here he designed
a Kalman Filer o esimae he uadroor sae b modelling is real sensors.
Then, he implemened
re:
Overall mass no superior o 1 kg. When i comes o uadroors, he heavier
he ge, he more
expensive he are. Man uadroors used or research do no exceed his mass. S
o, aiming or a
maximum mass o 1 kg seems o be a suiable arge;
Fligh auonom beween 10 and 20 minues. There is no poin in using he uad
roor or 2 minues
and hen wai a couple o hours o recharge he baeries, wasing precious ime
;
Onboard conroller should have is separae power suppl o preven
simulaneous engine and
processor ailure in case o baer loss;
Abili o ransmi live elemer daa and receive movemen orders rom a gro
und saion wirelessl,
hereore avoiding he use o cables which could become enangled in he aircra
and cause an
acciden;
The uadroor should no ver ar rom ground saion so here is no need o
long range elemer
hardware and he exra power reuiremens associaed wih long range ransmissio
ns.
Conseuenl, he main componens should be:
4 elecric moors and 4 respecive Elecronic Speed Conrollers (ESC);
13
14 CHAPTER 2. QUADROTOR DESIGN
4 propellers;
1 onboard processing uni;
1 riaxis analog acceleromeer. Measuring acceleraion in hreedimen
sional space aboard an
aircra is a necessar ea o undersand he magniude o he orces acing on
his aircra. I ma
also be possible o use his sensor o calculae he angles o aw and pich wi
h knowledge o he
gravi acceleraion vecor;
1 elecronic compass o measure he aw angle;
2 onboard power supplies (baeries), one or he moors and anoher o he p
rocessing uni. A
his poin we will assume ha beond he need o provide elecrical power o h
e moors, we mus
assure ha he brain o he uadroor (i.e. he onboard processing uni) remai
ns working well aer
he baer o he moors has discharged;
1 airrame or supporing all he aircras componens.
2.2 Airrame
The airrame is he mechanical srucure o an aircra ha suppors
all he componens, much like a
skeleon in Human Beings. Designing an airrame rom scrach involves imporan co
nceps o phsics,
aerodnamics, maerials engineering and manuacuring echniues o achieve cer
ain perormance, re
liabili and cos crieria. The main purpose o his hesis is no airrame des
ign, so, because consrucion
ime o he uadroor is criical, i is preerable o acuire, i possible, par
s alread available or sale.
The chosen airrame or he uadroor was he Vario43 model (Figure 2.2.1
1
), wih 258g o mass
and made o GPR (GlassReinorcedPlasic), posessing a cagelike srucure in i
s cener ha will oer
exra proecion o he elecronics. This paricular deail ma prove isel ver
useul when i comes o he
es igh sage, when accidens are more likel o happen.
Figure 2.2.1: Vario43 uadroor airrame.
2.3 Propellers
The pical behavior o a propeller can be de ned b hree parameers [4]:
Thrus coe cien c
T
;
1
hp://www.mikrokoper.de/ucwiki/VarioFrame_43, Januar 2009.
2.3. PROPELLERS 15
Power coe cien c
P
;
Propeller radius r.
These parameers allow he calculaion o a propellers hrus T:
T = c
T
4
4
2
(2.1)
and o r P
P
:
P
P
= c
P
4
5
3
(2.2)
h r is th ro ll r angular s d and the density of ai.
These fomulas sho that both thust and poe incease geatly ith pope
lles diamete. If the
diamete is big enough, then it should be possible to get suf cient thust hile d
emanding lo otational
speed of the popelle. Consequently, the moto diving the popelle ill have
loe poe consumption,
giving the quadoto highe ight autonomy.
Available models of counte otating popelles ae scace in the maket of adi
o contolled aicafts.
The EPP1045 (see Figue 2.3.1), a popelle ith a diamete of 10 (25
.4 cm) and eighting 23g,
pesented itself as a possible candidate fo implementation in the quadoto.
To check its compatibility
ith the poject equisites it is necessay to calculate the espectiv
e thust and poe coef cient. In
efeence [20] e have data fom tests done on the EPP1045, fom hich e can ex
tact the mean thust
and poe coef cients by using equations (2.1) and (2.2):
c
T
= 0.1154
(2.3)
c
P
= 0.0743 (2.4)
In eality, neithe the thust no the poe coef cients ae constant values, they
ae both functions of the
advance atio J [4]:
J =
u
0
nD
P
(2.5)
hee u
0
is the aicaft ight velocity, n the popelles speed in evolutions pe second a
nd nally D
P
is the popelle diamete. Hoeve, hen obseving the chaacteistic cuves f
o both these coef cients
(see Figues 2.3.2 and 2.3.3), it is clea that hen an aicafts
i
ght velocity is vey lo (e.g. in a
constant altitude hove) the advance atio is almost zeo and the to coef cients
can be appoximated
as constants, hich is the cuent case, fo at this point thee is no inteest
in achieving high tanslation
velocity.
Assuming the quadotos maximum eight is 9.81N (1kg) and that e ha
ve fou popelles, it is
mandatoy that each popelle is able to povide at least 2.45N (
1
/4 of the quadoto eight) in ode
to achieve liftoff. Taking this data into consideation leads us to onde ab
out the minimum popelle
otational speed involved, as ell as the magnitude of the poe equied fo igh
t. Figue 2.3.4 helps us
ith some of these questions. We can see that a popelle ill have to achieve a
ppoximately 412ad.s
1
,
hich is equivalent to 3934 evolutions pe minute, to povide the minimum 2.45N
equied fo liftoff. The
espective popelle poe is 26W. Afte this shot analysis e can state that t
he EPP1045 popelles ae
16 CHAPTER 2. QUADROTOR DESIGN
suitable fo implementation in the quadoto pototype, a statement that can be
poved by expeimental
data
2
, shoing that ith the ight moto e can poduce the necessay thust fo Lift
Off.
Figue 2.3.1: EPP1045 popelles.
Figue 2.3.2: Typical popelle thust cuves as a function of advance atio J a
nd blade angle [4].
2
.adotay.com/moto_test.xls, Novembe 2008.
2.3. PROPELLERS 17
Figue 2.3.3: Typical popelle poe cuves as a function of advance atio J an
d blade angle [4].
ebuay 2009.
2.4. ELECTRONICS 19
development of small size micocontolles.
One micocontolle that has gotten special attention fom the obotics communit
y oldide is the
Aduino. This micocontolle platfom is quite inexpensive and has a Cbased
language development
envionment that is vey intuitive to use. Fom the diffeent vesions of the
Aduino, the selected one
fo this poject as the Aduino Duemilanove (Figue 2.4.3). With a mass of 35
g, the Duemilanove has
6 analog inputs ith 10 bit esolution, seial pot communication, 14 I/O pins (
of hich 6 povide PWM
output) and many othe featues
5
.
Figue 2.4.3: Aduino Duemilanove.
2.4.3 Wieless communication
Wieless communications ae alays a challenge. One has to eight impotant fa
ctos like poe con
sumption, eight, tansmission speed and eliability. Fotunately it is possib
le to use hadae ith the
Aduino that allos to satisfy all the pevious conditions, e.g. the XBee 802.
15.4 1mW (Figue 2.4.4).
This module can be apped into a seial command set (useful becaus
e the Aduino can use seial
communication), consumes only 50mA of cuent, has a maximum data ate of 250kbp
s (using adio fe
quency) and a ange of 100m (lage ange telemety is not a equiement of this
thesis)
6
. To XBee
modules ae going to be used: one fo the quadoto and anothe in the gound
station compute (that
ill handle all telemety fo system identi cation and contol puposes).
Figue 2.4.4: XBee 802.15.4 1mW ieless module.
2.4.4 Sensos
The sensos of a otocaft ae a key element of the contol loop. They ae
esponsible fo poviding
infomation like aicaft attitude, acceleation, altitude, global position, and
othe elevant data. Moden
aicaft often cay obust and expensive senso technology, hich is
often a synonymous of big and
5
http://aduino.cc/en/Main/AduinoBoadDuemilanove, Febuay 2009.
6
http://.inmotion.pt/stoe/poduct_info.php?cPath=7&poducts_id=63, Febuay 2
009.
20 CHAPTER 2. QUADROTOR DESIGN
heavy electomechanical systems. In ou quadoto pototype, the keyods ae
light and small, thus
hen it comes to small sensing e should conside MEMS (MicoElectoMechanical
Systems) tech
nology. The use of locost sensos of this type implies less ef cient data poc
essing and thus a bad
oientation data pediction in addition to a eak dift ejection, making the c
ontol pocess a lot moe
challenging. Also, and in spite of the latest advances in miniatue actuatos,
the scaling las
7
ae still
unfavoable and one has to face sometimes the poblem of actuato satuation.
Ou pototype has to sensos, a tiple axis acceleomete poviding
thee acceleations (one fo
each axis of a Catesian coodinate system) and a dual axis magneto
mete poviding the magnetic
noth diection. The acceleomete of choice is the ADXL330 (Figue 2.
4.4) and some of its main
chaacteistics ae: maximum cuent consumption of 320A, an acceleation ange
of 29.4m.s
2
and
a mass of appoximately 2g. The magnetomete is an HMC6352 (Figue 2.4.4), ith a
mass of 0.14g, a
1mA cuent consumption, update ate up to 20Hz and a selectable heading esolut
ion of eithe 0.5
o
1
.
Figue 2.4.5: ADXL330 acceleomete. Figue 2.4.6: HMC6352 compass.
2.4.5 Batteies
Poe stoage has expeienced geat advances in the last decades, mainly due to
the seach of lighte
and longlasting poe souces fo the mobile devices industy and ecently, fo
the emeging electic
ca maket. The aicaft industy hoeve as not yet had a noticeable inteest
in electic poe souces.
Inteested paties in this eld of development ae mostly elated to a fe eseac
h pojects (some of hich
make use of sola enegy as a poe souce) o even some hobby developes that a
e aleady on thei
ay to selling thei electic aiplanes (e.g. the ElectaFlyeC
8
and the Sunseeke II
9
). In the RC maket,
electic batteies have poven to be a long tem cheape solution to combustion
engines. Speci cally,
Lithiumion Polyme (o LiPo) batteies, a ecent advance in poe s
toage technology, povide high
capacity, light and obust poe souce that has a lage maket spectum of appl
ications, including RC
aicaft. Fo these easons, the quadoto pototype ill use to s
epaate LiPo batteies, one fo the
motos and anothe fo Aduino (a sepaate poe souce fo the contolle unit
assues that the contolle
has poe, even if the main moto battey eaches citical levels hich is usefu
l fo emegency situations).
The Aduino is theefoe poeed ith a 15g LiPo battey, capable of poviding 2
40mAh of cuent at
7.4V . As fo the motos, they ae poeed by a 179g battey, electic chage of
2200mAh at 11.1V (Figue
7
Scaling las ae an engineeing concept that efes to vaiables hich change d
astically depending on the scale being con
sideed. Fo example, if e ty to use a mechanical gyoscope aboad an aica
ft subject to a stong electomagnetic eld, e
may expeience diffeent eadings than hen using a small piezo electic gyosco
Aduino stop oking befoe the motos because the ESCs pevents the motos of
oking hen the
battey eaches 9V, hich is moe than enough fo ou pocessing unit).
22 CHAPTER 2. QUADROTOR DESIGN
2.5 Flight autonomy
Flight autonomy is an impotant speci cation hen designing a quadoto
. The majo facto diectly
in uencing the ight autonomy is the high poe consumption of the motos, hich inc
eases ith the
value of the popelle angula velocity. An electic DC moto does not have a
linea behavio (Figue
2.5.1). Usually, it is chaacteized by angula speed satuation, as ell as a
dead zone peceding the
minimum voltage equied fo popelle otation [19]. The satuation, pevents
to supass the maximum
speed alloable by the moto.
Figue 2.5.1: Example illustation of the angula velocity of a DC moto as a
function of poe supply
voltage.
The popety that chaacteizes ho the angula velocity of a cetain DC moto e
volves ith poe
supply voltage is the k
v
(ponounced exactly as kv). This popety simply allos to kno ho many
RPM ill the moto povide pe each Volt of supplied electicity. Thus, to calcu
late ho a motos angula
velocity changes ith voltage e can use:
v
=
2k
v
60
(2.6)
=
v
(V
m
V
0
) (2.7)
where
v
is the popelles angula speed pe Volt, V
m
is the motos poe supply voltage and V
0
is the
poe supply voltage coesponding to the motos dead zone.
At this point, it is only a matte of combining equations (2.1) (assuming that t
hust is equal to the total
eight of the vehicle) and (2.7) ith Ohms la to get the total electic cuent
consumption I
equied
to maintain a quadoto of a given mass m in the ai:
I
=
V
0
R
m
+
1
R
m
k
v
_
2
mg
4c
T
4
_
1
2
(2.8)
Fo pactical easons e can eite 2.8 into:
I
= I
0
+
2R
m
k
v
r
2
_
mg
c
T
_1
2
(2.9)
2.5. FLIGHT AUTONOMY 23
hee g is the acceleation of gavity, R
m
the motos coppe coil electic esistance and I
0
the electic
cuent consumption of the moto in noload condition. No that e can calcula
te the motos electical
consumption needs, it is time to conjugate this infomation ith cetain battey
popeties that ill allo
the calculation of the ight autonomy f
t
, given the electic chage Q
of the battey:
f
t
=
Q
I
(2.10)
The BLOutunne 282434 moto has a coil esistance of 0.3136 and is capable of
otating at 1100
RPM pe Volt. At this point it is not possible to pedict the exact mass of
the aicaft neithe to have
knoledge of the noload cuent, so let us assume a mass of 1kg a
nd a minimum electical cuent
of 3A (a consevative value fo this type of moto). Equation (2.9
) theefoe shos that each moto
consumes 3.23A. If e inset this data into equation (2.7) (not fogetting tha
t e have fou motos) e
can pedict a ight autonomy of appoximately 614s (10.2 minutes), a easonable ti
me given that e ae
uncetain about some popeties of the motos and aicaft at this stage.
24 CHAPTER 2. QUADROTOR DESIGN
Chapte 3
Quadoto modelling
The st step befoe the contol stage is the adequate modeling of the system dyna
mics. This phase ill
hopefully facilitate the contol of the aicaft as it ill povide us ith a be
tte undestanding of the oveall
system capabilities and limitations. The cuent chapte ill guide us though
the equations and tech
niques used to model ou quadoto and its sensos, poviding the mathematical b
asis fo the application
of the system dynamics in a simulation envionment.
3.1 System dynamics
Witing the equations that potay the complex dynamics of an aicaft implies s
t de ning the system
of coodinates to use. Only to efeence fames ae equied, an eath xed fame
and a mobile fame
hose dynamic behavio can be descibed elative to the xed fame. The eath xed
axis system ill
be egaded as an inetial efeence fame: one in hich the st la of Neton
1
is valid. Expeience
indicates this to be acceptable even fo supesonic aiplanes but not fo hypes
onic vehicles
2
[21]. We
shall designate this efeence fame by O
NED
(NothEastDon) because to of its axis (u
x
and u
y
) ae
aligned espectively ith the Noth and East diection, and the thid axis (u
z
) is diected don, aligned
toads the cente of the Eath (Figue 3.1.1). The mobile fame is designated
by O
ABC
, o Aicaft
BodyCenteed, and has its oigin coincident ith the quadotos cente of gavit
y (Figue 3.1.2).
1
An object that is not moving ill not move until a net foce acts upon it. An
object that is moving ill not change its velocity
(acceleate) until a net foce acts upon it.
2
The otational velocity of Eath must not be neglected fo hypesonic ight.
25
26 CHAPTER 3. QUADROTOR MODELLING
Figue 3.1.2: NED and ABC efeence fames.
Figue 3.1.1: Visualization of the NothEastDon efeence fame.
In contol theoy, knoledge about the dynamic behavio of a given system can be
acquied though
its states. Fo a quadoto, its attitude about all 3 axis of otation is knon
ith 6 states: the Eule angles
[ ] (Roll Pich Yaw as seen beore in Figure (1.2.1)) and he angular velociies a
round each axis
o he O
ABC
rame [P Q R].
Ye
anoher 6 saes are necessar: he posiion o he cener o
gravi (or COG) [X Y Z] and
respecive linear veloci componens [U V W] relaive o he xed rame. In
sum, he uadroor has
12 saes ha describe 6 degrees o reedom.
Unsurprisingl, we mus deduce he euaions describing he orienaion o he m
obile rame relaive
o he xed one, which can be achieved b using a roaion marix. This marix
resuls o he produc
beween hree oher marices (R
(), R
() and R
()), each o hem represening he roaion o he
ABC rame around each one o he O
NED
axis [22]:
3.1. SYSTEM DYNAMICS 27
R
() =
_
_
1 0 0
0 cos
0 sin
_
_ R
sin
cos
() =
_
_
cos 0 sin
0 1 0
sin 0 cos
_
_ R
() =
_
_
cos sin 0
sin cos 0
0 0 1
_
_ (3.1)
S = R
() R
() R
() (3.2)
S =
_
_
cos cos cos sin sin
sin sin cos cos sin cos cos + sinsin sin sincos
cos sin cos + sinsin sin cos sin sincos cos cos
_
_ (3.3)
where S is he roaion marix ha expresses he orienaion o he coordinae
rame O
ABC
relaive o he
reerence rame O
NED
. To mahemaicall wrie he movemen o an aircra we mus emplo Newons
second law o moion. As such, he euaions o he ne orce and momen aci
ng on he uadroors
bod (respecivel F
ne
and M
ne
) are provided:
F
ne
=
d
d
[mv]
B
+
[mv]
B
(3.4)
M
net
=
d
dt
[I
]
B
+
_
I
_
B
(3.5)
hee I is the inetia matix of the quadoto,
locities and
the vecto of
angula velocities. If the equation of Netons second la is to be as complete
as possible, e should
add exta tems such as the Coiolis, Eule and aeodynamic foces (e.g. ind)
, but to keep the model
simple, and also because the quadoto is not supposed, at this sta
ge, to go vey fa aay fom the
gound station, these foces ill not be incopoated in the modeling pocess. T
he foce of gavity (F
g
) is
too signi cative to be neglected, thus it is de ned by:
F
g
= mS
_
0 0 g
_
T
= mg
_
sin cos sin cos cos
_
T
B
(3.6)
The orce o gravi ogeher wih he oal hrus generaed b he propellers
(F
P
) have hereore o be
eual o he sum o orces acing on he uadroor:
F
g
+F
P
= F
ne
(3.7)
Combining euaions (3.4), (3.6) and (3.7) we can wrie he vecor o linear acc
eleraions acing on he
vehicles bod:
_
W
_
_ =
1
m
_
_
F
Px
F
P
F
Pz
_
_ +g
_
_
sin
cos sin
cos cos
_
_
_
_
QW RV
RU PW
PV QU
_
_ (3.8)
where [F
Px
F
P
F
Pz
] are he vecor elemens o F
P
. Assuming he aircra is in a hovered igh, in such
a scenario here are orces acing onl in he z axis o uadroor, correspondi
ng o he siuaion where
28 CHAPTER 3. QUADROTOR MODELLING
we have he engines ring o overcome he orce o gravi o keep he aircra
sable a a given aliude:
F
Pz
= (T
1
+T
2
+T
3
+T
4
) (3.9)
Noe ha he minus sign means he liing orce is acing upwards, awa rom h
e surace (noe ha he
posiive axis o he O
NED
is poin downwards).
Working now on Newons second law or roaion, he ineria marix is given b:
I =
_
_
I
11
I
12
I
13
I
21
I
22
I
23
I
31
I
32
I
33
_
_ (3.10)
Assuming he uadroor is a rigid bod wih consan mass and axis aligned wih
he principal axis o
ineria, hen he ensor I becomes a diagonal marix conaining onl he princip
al momens o ineria:
I =
_
_
I
11
0 0
0 I
22
0
0 0 I
33
_
_ (3.11)
Joining euaions 3.11 and 3.5 we ge:
M
ne
=
_
_
I
11
0 0
0 I
22
0
0 0 I
33
_
_
_
R
_
_ +
_
_
P
Q
R
_
_
_
_
I
11
0 0
0 I
22
0
0 0 I
33
_
_
_
_
P
Q
R
_
_ (3.12)
Conseuenl, we will have:
M
ne
=
_
_
I
11
P
I
22
Q
I
33
R
_
_ +
_
_
(I
33
I
22
) QR
(I
11
I
33
) RP
(I
22
I
11
) PQ
_
_ (3.13)
_
R
_
_ =
_
_
Mx
I11
M
I22
Mz
I33
_
_
_
_
(I33I22)QR
I11
(I11I33)RP
I22
(I22I11)PQ
I33
_
_ (3.14)
Inormaion on he momens acing on he aircra can be provided b:
M
x
= (T
4
T
2
) d
cg
(3.15)
M
= (T
1
T
3
) d
cg
(3.16)
M
z
= (T
1
+T
3
T
2
T
4
) K
TM
(3.17)
where d
cg
is he disance o he aircras COG (see Table 3.4) and K
TM
is a consan ha relaes momen
and hrus o a propeller (see Secion 3.4).
3.2. KINEMATIC EQUATIONS AND EULER ANGLES 29
3.2 Kinemaic euaions and Euler angles
In his secion we will sud he kinemaics o he uadroor. The rs sage o k
inemaics analsis consiss
o deriving posiion o obain veloci. Le us hen consider he posiion vec
or
r , which indicaes he
posiion o he origin o he O
ABC
rame relaive o O
NED
:
r = X
i +Y
j +Z
k (3.18)
I we derive each o he componens in
r we can obain he insananeous veloci o O
ABC
relaive o
O
NED
:
r =
X
i +
Y
j +
Z
k (3.19)
To nd ou he componens o he aircras linear veloci v
in he
_
_
U
V
W
_
_ = S
_
_ (3.20)
where we can ake ull advanage o he orhogonali o S, meaning is inverse
is eual o is ranspose:
_
Z
_
_ = S
T
_
_
U
V
W
_
_ (3.21)
_
Z
_
_ =
_
_
cos cos U + (sinsin cos cos sin) V + (cos sin cos + sinsin) W
cos sinU + (cos cos + sinsin sin) V + (sin cos sin sincos ) W
sinU + sincos V + cos cos W
_
_
(3.22)
The igh pah o he uadroor in erms o [X Y Z] can be ound b inegraion
o euaion 3.22. To per
orm his inegraion, he Euler angles , and mus be known. However, he Euler a
ngles hemselves
are uncions o ime: he Euler raes
, and
_
and [P Q R] he ollowing euali mus be sais ed:
= P
i +Q
j +R
k =
(3.23)
Noe ha alhough i ma appear ha he Euler raes are he same as angular ve
lociies, his is no he
case. I a solid objec is roaing a a consan rae, hen is angular veloci
ill be constant, hoeve
the Eule ates ill be vaying because they depend on the instantaneous angles
beteen the coodinate
fame of the body and the inetial efeence system (e.g. beteen O
ABC
and O
NED
). The Eule angle
sequence is made up of thee successive otations: Roll, Pitch and Ya. In othe
ods, the angula ate
needs hree:
= R() R() R()
_
_
0
0
_ +R() R()
_
0
_
_ +R()
_
0
0
_
_ (3.24)
30 CHAPTER 3. QUADROTOR MODELLING
Thereore:
_
_
P
Q
R
_
_ =
_
_
1 0 sin
0 cos sincos
0 sin cos cos
_
_
_
_ (3.25)
Solving or he Euler angular raes ields he desired dierenial euaions [2
3]:
_
_ = T
_
_
P
Q
R
_
_ (3.26)
T =
_
_
1 an sin an cos
0 cos sin
0 sin/ cos cos / cos
_
_ (3.27)
where T is he marix ha relaes he bod xed angular veloci vecor
and the ate of change of the
Eule angles.
3.3 Quatenion diffeential equations
A poblem ith the implementation of Eule angles , and is ha or = 90
_
cos (/2)
sin(/2) n
1
sin(/2) n
2
sin(/2) n
3
_
(3.28)
h r r r s nts a rotation about th unit v ctor [n
1
n
2
n
3
] through an angl . Th tim d rivativ of
th rotation uat rnion is also rovid d by [23]:
=
1
2
q
q +q =
1
2
_
_
0 P Q R
P 0 R Q
Q R 0 P
R Q P 0
_
_
_
_
q
0
q
1
q
2
q
3
_
_
+
_
q
0
q
1
q
2
q
3
_
_
(3.29)
= 1
_
q
2
1
+q
2
2
+q
2
3
+q
2
4
_
(3.30)
3.4. MODELLING OF A MOTORPROPELLER ASSEMBLY 31
For practical reasons, the initialization of equation (3.29) requires that we ex
press the quaternion com
ponents in terms of Euler an
les because doin
it with quaternions is not so str
ai
htforward. Therefore,
expressin
the quaternion vector elements as a function of Euler an
les yields:
_
_
q
0
= cos
_
2
_
cos
_
2
_
cos
_
2
_
+ sin
_
2
_
sin
_
2
_
sin
_
2
_
1
= cos
_
2
_
cos
_
2
_
cos
_
2
_
sin
_
2
_
sin
_
2
_
sin
_
2
_
2
= cos
_
2
_
sin
_
2
_
cos
_
2
_
+ sin
_
2
_
cos
_
2
_
sin
_
2
_
3
= sin
_
2
_
cos
_
2
_
cos
_
2
_
cos
_
2
_
sin
_
_
sin
_
2
_
(3.31)
Oherwise, i we wan o exrapolae he Euler angles rom he uaernion dier
enial euaions we can
achieve jus ha b aking inermediae seps o he roaion ensor S and rela
e hem wih he elemens
o he roaion uaernion marix, raher han calculaing hem rom he uaern
ion direcl:
S
=
_
_
2
0
+
2
1
2
2
2
3
2 (
1
2
+
0
3
) 2 (
1
3
0
2
)
2 (
1
2
0
3
)
2
0
2
1
+
2
2
2
3
2 (
2
3
+
0
1
)
2 (
1
3
+
0
2
) 2 (
2
3
0
1
)
2
0
2
1
2
2
+
2
3
_
_ (3.32)
where S
is he uaernion euivalen o he roaion marix (3.3).
can hereore be
calculaed b using:
_
_ =
_
_
arcan
_
2(23+01)
2
0
2
1
2
2
2
3
_
arcsin(2 (
1
3
0
2
))
arcan
_
2(23+01)
2
0
+
2
1
2
2
2
3
_
_
_
(3.33)
In analog o (3.21), we can also ge he absolue veloci using he uaernion
roaion ensor:
_
Z
_
_ = S
T
_
_
U
V
W
_
_ (3.34)
To emplo he uaernion mehod we mus
rs use he iniial Euler an
gles in (3.31), and use he
resuling uaernion elemens in (3.32). I is also possible o combine he prev
ious uaernion euaions
wih he dnamics euaions o compose he vehicle acceleraion on he aircra
local rame:
F
g
= mS
_
0 0 g
_
T
= mg
_
2 (
1
3
0
2
) 2 (
2
3
+
0
1
)
2
0
2
1
2
2
+
2
3
_
T
B
(3.35)
_
W
_
_ =
1
m
_
_
F
Px
F
P
F
Pz
_
_ +g
_
_
2 (
1
3
0
2
)
2 (
2
3
+
0
1
)
2
0
2
1
2
+
2
3
_
_
_
_
QW RV
RU PW
PV QU
_
_ (3.36)
3.4 Modelling o a moorpropeller assembl
The moorpropeller assemblies are essenial componens o our aircra since h
e are responsible or
he producion o he liing orce ha allows igh. This secion ocuses on m
odeling he dnamics o
32 CHAPTER 3. QUADROTOR MODELLING
hese componens.
As we have seen previousl in he uadroor design secion here are
man variables o accoun
or when choosing he righ moorpropeller assembl. This is mosl because
he hrus is a uncion
o
several properies such as he propellers diameer, hrus
coe cie
n, air densi and he moors
k
v
. Moreover, alhough we have 4 eual moors, in reali each one o hem has a d
ieren dnamic
behavior. This dissimilari reuires a more precise modelling o he moors,
a process ha oen sars
b esablishing he relaion beween he volage ed o he moor and he machi
ng speed o roaion.
A permanen magne DC moor ranser uncion is usuall described b a gain and
wo real poles
(also known as a secondorder model), respecivel associaed wih mechanical an
d elecrical ime con
sans, being he mechanical consan commonl higher han he elecrical one b
a leas one order o
magniude. In oher words, he pole associaed wih he elecrical consan is
he aser one and as a
conseuence he dnamic behavior o such a moor is dominaed b he slower pole
. Having his knowl
edge ino consideraion, a common wa o de ne he dnamic behavior o a DC moor
is b using a rs
order ranser uncion, having onl a gain and a pole associaed wih he mecha
nical ime consan.
To ideni he ranser uncion o each moor we have o conceive
a mehod o daa acuisiion,
paricularl beween he inpu and oupu o each moor. The propeller roaio
n speed (oupu) can be
acuired hrough a achomeer device. For he inpu we could use he volage p
rovided o a moor bu
or pracical reasons i is b ar simpler o use he PWM signal because i is a
variable we have direc
access o (measuring volage is also possible using he analog pins on he Ardui
T
P
(3.37)
It is im ortant to m ntion that although this m thod for m asuring th ro ll r
out ut is v ry ch a
and acc ssibl , it do s not ork v ry ll for lo s ds of rotation as as v r
i d during x rim ntal
trials, mostly du to v ry lo signal-to-nois ratios (i. . nois am litud is
so high that it g ts v ry hard
to distinguish hat is ambi nt nois from ro ll r sound). Figur 3.4.3 shos
a v ry cl ar signal form
of th ro ll r sound, h r can cl arly id ntify th sound r ssur av s o
f th assag of th
ro ll r. Proc ding ith th motor- ro ll r syst m id nti cation, m asur m n
ts took lac to nd
th r s ctiv d ad zon (Tabl 3.1) and dynamic r s ons (Tabl 3.2).
Figur 3.4.3: Pro ll r sound. Each st in am litud is a cons u nc of a ro
ll r blad assag .
34 CHAPTER 3. QUADROTOR MODELLING
Tabl 3.1: D ad zon uls idth of ach motor.
Motor i D ad zon uls idth (s)
1 1262
2 1252
3 1250
4 1252
Tabl 3.2: Data oints gath r d for und rstanding th motor- ro ll r dynamics.
In ut PWM signal (s) 1500 1700 1800 2000
Motor 1 out ut (rad.s
1
) 4268.6 5926.5 6504.8 6729.5
Motor 2 output (rad.s
1
) 4083.3 5732.8 6361.3 6645.9
Motor 3 output (rad.s
1
) 4336.5 6132.5 6746.1 6925.2
Motor 4 output (rad.s
1
) 4329.6 6064.2 6684.5 6890.2
Fi
ure 3.4.4 shows plotted data of tables 3.1 and 3.2 revealin
that the dynamic
s of the motors is
nonlinear, meanin
that direct implementation of a rstorder transfer function to
model the dynamic of
each motor is not adequate.
To solve this issue it was decided that to obtain a linear model
(to allow implementation of
rst
order transfer functions) the model should be linearized around an ad
equate operation point. Initial
experimental i
ht tests were executed and revealed that shortly beyond the dead z
one the quadrotor
took off from the
round. Takin
this observation into consideration led to th
e choice of a relatively low
operation point (pulse width) of 1300s.
Fi
ure 3.4.4: Dynamic behaviour of the motors. The
rey areas show a zoom on of
the linearization zone
in red.
(x collected data points, second order polynomial data t, linearization)
3.4. MODELLING OF A MOTORPROPELLER ASSEMBLY 35
First order transfer functions require a
ain K and a time constant :
G(s) =
K
s + 1
(3.38)
The gains are eual o he slope o he linearized dnamic response
o each moor (dashed lines o
Figure 3.4.4; slope values in Table 3.3). Inpuoupu
daa was ga
hered or moor 1 and he Open
Ssem Ideni caion Tool Graphical User Inerace in MATLAB
was used o esimae he respecive
ime consan, wih value 0.136s. A sample o he inpuoupu daa
used can be observed in Figure
3.4.5. Time consans or he our moors were assumed o be all eual o he on
e o moor 1, which also
2
(3.40)
Assuming all variabl s in s. (2.1) and (3.40) ar constant ith xc tion fo
r angular s d, ro ll r
mom nt and thrust, can r rit th s uations:
T = K
T
2
(3.41)
K
T
= c
T
4
4
2
= 0.1154
4 1.2 (5 25.4 10
3
)
4
2
= 1.46 10
5
k
.m.rad
2
(3.42)
M
T
= K
M
2
(3.43)
K
M
= c
P
4
5
3
= 0.0743
4 1.2 (5 25.4 10
3
)
5
3
= 3.8 10
7
k
.m
2
.rad
2
(3.44)
where K
M
and K
T
are constants that respectively relate a propeller moment and thrust with an
u
lar
speed. Moment and thrust can therefore also be related throu
h the constant K
TM
:
M
T
=
K
M
K
T
T = K
TM
T =
c
P
r
c
T
T (3.45)
K
TM
= 0.026m (3.46)
3.5 Mom nts of in rtia
Mass and its g om tric distribution in an aircraft is som thing of xtr m im or
tanc b caus it aff cts
th ntir dynamics of th syst m. Th n, aft r building th uadrotor, it is t
im to valuat som of its
most im ortant f atur s lik its mom nts of in rtia and mass.
W ill assum th in rtia matrix is diagonal and ositiv -d nit , ith th u
r os of sim lifying
3.5. MOMENTS OF INERTIA 37
th calculations and also du to th articular symm tric g om try of uadrotors
(s Figur 3.5.1). As
such, th calculation of in rtia mom nts ill only includ th g om try and mass
of th motors, as ll
as th ir g om tric osition on th uadrotor.
Figur 3.5.1: Distanc from th motors to th c nt r of gravity.
Each motor ights a roximat ly 0.048kg. Oth r im ortant variabl s r lat d to
th aircraft such as
th l m nts of ach motor in rtia t nsor (l
x
, l
y
and l
z
) can b consult d in tabl 3.4.
Tabl 3.4: Quadrotors mass and main g om tric variabl s.
Variabl
Valu
l
x
(m) 0.0288
l
y
(m) 0.0288
l
z
(m) 0.026
d
cg
(m) 0.29
m (kg) 0.82
It follos that th in rtia matrix l m nts of th aircraft ar :
_
I
x1
= I
x3
=
1
12
m
m
(l
2
y
+l
2
z
) = 6.0218 10
6
k
.m
2
I
x2
= I
x4
=
1
12
m
m
(l
2
y
+l
2
z
) +m
m
d
2
c
= 0.004 k
.m
2
I
11
= 2I
x1
+ 2I
x2
= 0.0081 k
.m
2
(3.47)
38 CHAPTER 3.
_
QUADROTOR MODELLING
I
y1
= I
y3
=
1
12
m
m
(l
2
x
+l
2
z
) +m
m
d
2
c
= 0.004 k
.m
2
I
y2
= I
y4
=
1
12
m
m
(l
2
x
+l
2
z
) = 6.0218 10
6
k
.m
2
I
22
= 2I
y1
+ 2I
y2
= 0.0081 k
.m
2
(3.48)
_
_
_
I
z1
= I
z2
= I
z3
= I
z4
=
1
12
m
m
(l
2
x
+l
2
y
) +m
m
d
2
c
= 0.004 k
.m
2
I
33
= 4I
z1
= 0.0162 k
.m
2
(3.49)
And the inertia matrix itself is:
I =
_
_
I
11
0 0
0 I
22
0
0 0 I
33
_
_ =
_
_
0.0081 0 0
0 0.0081 0
0 0 0.0162
_
_ k
.m
2
(3.50)
3.6 Calculation of the center of
ravity
It has been clear so far that we have assumed the quadrotor to ha
ve its center of
ravity located in
the center of the XY plane of the O
ABC
reference frame. Nonetheless it is also important to calculate
its position alon
the Z axis because we will need it to know the relative pos
software, in which some of the most heavy parts of the quadrotor were modeled (e
.
.
motors, sensors, main battery and aluminum beams) and assembled. Fi
ure 3.6.1 sh
ows the computed
position of the COG, located 6.7cm above the base of the quadrotor.
Fi
ure 3.6.1: The red dot in the center of the
ure indicates the center of
ravi
ty.
3.7 Sensors modellin
Inertial navi
ation is implemented in cases where the use of an external referen
ce to measure position
is impractical. Typical inertial navi
ation systems used in reallife applicat
ions such as aircraft or space
rockets are hi
hly advanced, and expensive, pieces of technolo
y. H
owever, inexpensive sensorin
_
T
(3.51)
where a
p
is the absolute acceleration at point p, usually the point where the accelero
meter is located.
Also, the instantaneous velocity v
p
at point p is:
v
p
= v
c
+
B
s
(3.52)
hee
s
is the position of the acceleomete elative to the quadotos cente of gavi
ty (see Table 3.5
fo data on the acceleomete location) and v
cg
is the vecto of linea velocities at the COG. Applying the
Coiolis theoem yields:
a
p
= a
cg
+
B
s
+
B
(
B
s
) + 2
B
v
cg
(3.53)
Given that:
ma
cg
= ma
B
+mS
_
0 0 g
_
T
(3.54)
e can eite equation (3.53) to povide absolute acceleation of the acceleom
ete located at point p:
a
p
=
a
B
m
+S
_
0 0 g
_
T
+
B
s
+
B
(
B
s
) (3.55)
Equation (3.55) does not contain the Coiolis acceleation. The eason fo thi
s is that the Coiolis Foce
is popotional to the velocity of the aicaft. Because e do not ant to ach
ieve exteme velocities, e
ae going to neglect this tem of the equation. As e have a tiaxis acceleo
mete, e need to ead
thee components of acceleation:
_
_
a
px
a
py
a
pz
_
_ =
1
m
_
_
F
x
F
y
F
z
_
_+g
_
_
sin
cos sin
cos cos
_
_+
_
Qr
sz
Rr
s
Rr
sx
Pr
sz
Pr
s
Qr
sx
_
_+
_
_
Q(Pr
s
Qr
sx
) R(Rr
sx
Pr
sz
)
R(Qr
sz
Rr
s
) P(Pr
s
Qr
sx
)
P(Rr
sx
Pr
sz
) Q(Qr
sz
Rr
s
)
_
_
(3.56)
40 CHAPTER 3. QUADROTOR MODELLING
Table 3.5: Relevan acceleromeer daa.
Variable Value
Number o bis used b he Arduino or A/D conversion N
b
10
Sensor measuremen range
3g
Sensiivi
0.3V.g
1
Posiion along he xaxis relaive o he cener o gravi r
sx
0.003m
Posiion along he axis relaive o he cener o gravi r
s
0m
Posiion along he zaxis relaive o he cener o gravi r
sz
0.049m
Using daa rom Table 3.5, he sensor reading resoluion o he Arduino board wi
ll be:
R
acc
=
smax
smin
2
N
b
=
3 (3)
2
10
= 0.00586g = 0.0575m.s
2
(3.57)
Oher variables such as variaion o sensor sensibili wih emperaure can be
simulaed wih he addiion
o whie Gaussian noise.
As alread noiced, we do no
have o gros onboard he uadroor.
Their absence also makes
he deerminaion o he angles o roll and pich uie rick because gros are
used or sensor usion
ogeher wih acceleromeers, providing ver sead oupus, hus maki
ng hem suiable or vibraion
inensive applicaions.
A raher less robus bu direc pah o obain hese angles is hrough he veco
r o graviaional accel
eraion provided b he acceleromeer, based on he iniial approach ha we do
no inend o move he
uadroor a high speeds (oherwise ver high acceleraions will indicae roll a
nd pich angles wih ver
high error) :
= arcan
_
a
p
a
pz
_
(3.58)
= arcan
_
a
px
a
pz
_
(3.59)
3.7.2 Compass
Compasses are scieni c insrumens used o measure he direcion o he magneic e
ld in he vicini o
he sensor. Among oher applicaions, he are used in aircra o provide a dir
ecional bearing componen
cenered on he norh or souh poles. The proope o his hesis uses a comp
ass which serves his
exac purpose. Elecronic compasses operae using he Halleec echnolog, wh
ich is o sa ha in is
inerior here are maerials which var he volage a is erminals when crosse
d b a magneic eld. Le
us assume ha he sensor can be modeled as ollows:
= N
c
(3.60)
where N
c
is he direcion o he magneic Norh Magneic Pole mapped beween and radians.
Tabl
3.6 shos th k y charact ristics of th s nsor to im rov th simulation v raci
ty.
Similarly to th acc l rom t r can add hit Gaussian nois to th com ass r
adings to simulat
th ff cts of arasit magn tic
lds.
3.8. MODEL IMPLEMENTATION IN MATLAB
AND SIMULINK
41
Tabl 3.6: R l vant com ass data
Variabl
Valu
S nsor m asur m nt rang
360
Selected esolution 1
2. Calculate the foces and moments applied on the quadoto by the popelles
using equations (3.41)
and (3.43);
3. Compute the linea and angula acceleations using equations (3.36) and (3.
14);
4. Compute the linea and quatenion angula velocities though equations (3.3
4) and (3.29);
5. Output the states vecto deivative
_
_
.
Now, when we impor he uncion ino he Simulink
environmen we mus use an inegraor block o
produce linear and angular velociies rom he linear and angular acceleraions,
and we can also use he
same logic o obain he linear posiion and uaernion angle represenaion res
pecivel rom he linear
velociies and angular veloci uaernion. O course angles in he orm o a
uaernion are no ver
inelligible, so in order o ransorm a uaernion ino Euler angles we can us
e a ransormaion block
available in Simulink
or his ask (he name o he block is Qua2Ang). One should be ver careul n
o
o misake he angles order o roaion, in our case we are working wih he ord
er ZYX (or YawPichRoll)
and hereore i we wish o obain, as an example, he roll, we should ge i in
he hird oupu o his block.
A diagram illusraing he uadss.m uncion is presened in gure 3.8.1.
42 CHAPTER 3. QUADROTOR MODELLING
Figure 3.8.1: Block diagram o uadroor dnamics.
Turning our aenion o he sensors, he uadsens.m uncion was designed o si
mulae he sensor
readings. I reads he oupus (i.e. he sae vecor) rom he uadroor dnami
4
_
(4.2)
As an example, in the case of the quadoto u can be built by nding the angula s
peed of each moto
so that the total thust poduced is equal to the foce of gavity. Implementing
the st ode Taylo seies
expansion, the lineaization of the quadotos model:
_
_
_
x = f (x, u)
y = h(x, u)
(4.3)
aound an opeating point ( x, u) is [27]:
_
_
_
x f ( x, u) +
f(x,u)
x
(x x) +
f(x,u)
u
(u u)
y h( x, u) +
h(x,u)
x
(x x) +
h(x,u)
u
(u u)
(4.4)
hee f(x, u) ae the set of equations poviding the dynamic behavio
of the system (see equations
(3.36), (3.14), (3.34) and (3.29)) and h(x, u) ae the equations tha
t simulate the senso outputs (see
equations (3.56), (3.58), (3.59) and (3.60)). Assuming that the pe
sence of vey small distubances
aound the equilibium point ( x, u), (x x) and (u u) ae both almost zeo, e ca
n veify:
_
_
_
x = 0 f ( x, u) = 0
y = 0 h( x, u) = 0
(4.5)
4.2. SYSTEM ANALYSIS 47
Theefoe:
_
_
_
x =
f(x,u)
x
(x x) +
f(x,u)
u
(u u)
y =
h(x,u)
x
(x x) +
h(x,u)
u
(u u)
(4.6)
ith:
A =
f (x, u)
x
(4.7)
B =
f (x, u)
u
(4.8)
C =
h(x, u)
x
(4.9)
D =
h(x, u)
u
(4.10)
Equation (4.6) is the geneic state space epesentation:
_
_
_
x = A x +B u
y = C x +D u
(4.11)
ith x = x x and y = y y.
4.2 System analysis
If e ish the Kalman lte to be convegent, e must analyze some popeties of d
iscete state space
systems such as eachability, contollability and obsevability. By convegence
e mean the Kalman lte
estimated states eo ties to go to zeo hile unde the in uence of noise. Th
at noise cannot push a
state vey fa and e can expect the vaiance of that state to emain bounded, a
nd this is pecisely hat
happens hen the system is stable. If the system is not stable, then the vaian
ce of a state ill explode
hen the system gets affected by noise. Hence, if e ish the Kalman lte to be
convegent, e must
analyze some popeties of discete state space systems such as eachability, co
ntollability and obsev
ability [28].
By de nition, eachability tells us if it is possible to nd a contol
sequence such that an abitay
state can be eached fom any initial state in nite time. Thee ae seveal met
hods fo detemining if a
system is eachable, depending on hethe the system matix A is o isnt singula
(i.e. if A isnt o is
invetible). This popety can be investigated by checking the deteminant of ma
tix A, hich in this case
equals zeo, meaning this matix is nonsingula. In this case e can immediate
ly say the system is not
eachable.
Contollability aims to tell us if it is possible to nd a contol sequence such t
hat the oigin (i.e. hen
a state is equal to zeo) can be eached in nite time. Fo systems in hich mat
ix A is nonsingula, e
can say the system is contollable if thee is a value fo i such that A
i
equals zeo. In this case this is
48 CHAPTER 4. KALMAN FILTER
tue fo i = 4, theefoe ou system is fully contollable.
Obsevability efes to the possibility of a given state being detemined in nite
time using only the
system outputs. This popety can be vei ed if the ank of the obsevability mat
ix O:
O =
_
_
C
CA
CA
2
.
.
.
CA
N1
_
_
(4.12)
is equal to the systems ode. The equivalent MATLAB
command is:
ank((obsv(A,C));
The esult is 6 meaning e have 6 unobsevable states (fom the total 12) in the
system.
4.3 Mathematical fomulation
To constuct a Kalman lte e must meet 3 conditions:
1. The model of the system must be linea. Vey egulaly, even in the pesenc
e of nonlineaities, the
common appoach is to engage in system lineaization aound an opeating point,
given that it is
easie to ok ith linea systems;
2. The noise is hite, hich means it has equal poe ove all fequencies.
This also simpli es the
math involved in the lte, fo hite noise is vey simila to the actual noise af
fecting the system,
ithin limited ange of fequencies;
3. The noise has Gaussian density. Typically thee ae to statistical popet
ies that ae easily asce
tainable and that chaacteize a noise signal, the mean and vaiance, to assume
that the noise is
Gaussian and simplify the pocess of lteing fom the mathematical point of vie.
Stating ith the lineaized continuoustime state space fom of the system:
_
_
_
x = Ax +Bu +G
y = Cx +Du +H+v
(4.13)
hee the stochastic vaiables v and espectively epesent the noise affectin
g the senso measue
ments and the aicaft, chaacteized by its espective noise covaiances R
k
and Q
k
:
Q
k
= E
_
T
_
(4.14)
R
k
= E
_
vv
T
_
(4.15)
4.4. KALMAN FILTER IN MATLAB
AND SIMULINK
49
The pocess noise covaiances Q
k
and measuement covaiances R
k
_
x(n) = A
k
x(n 1) +B
k
_
_
u(n)
y(n)
_
_
y(n) = C
k
x(n 1) +D
k
_
_
u(n)
y(n)
_
_
(4.16)
A
k
= I LC (4.17)
B
k
=
_
LD L
_
(4.18)
C
k
= C(I LC) (4.19)
D
k
=
_
(I CL)D CL
_
(4.20)
hee the Kalman gain L updates the estimate x(n 1) using the ne measuement y(n
):
x(n) = x(n 1) +L(y(n) C x(n 1) Du(n)) (4.21)
Notice that this last equation is the same equation used fo states estimation i
n (4.16), only eitten in
diffeent fom. The main taget of the Kalman estimato design is to minimize th
e eos:
e
x
= x x = C(x x) +v (4.22)
e
y
= y y = (ALC) (x x) +GLv (4.23)
This is accomplished by nding the lte gain L that povides the stateestimate x
that minimizes the
steadystate eo covaiance P:
P = E
_
(x x) (x x)
T
_
(4.24)
Theefoe, the gain L can be calculated by solving an algebaic Riccati simpli ed
equation [29]:
L = PC
T
_
CPC
T
+R
_
1
(4.25)
hee R is the measuement noise covaiance matix.
4.4 Kalman lte in MATLAB
and Simulink
To poduce a Kalman lte in MATLAB
e need the state space matices as ell as the covaiance
matices of the pocess and senso noises. The numeical calculus of
the state space matices as
50 CHAPTER 4. KALMAN FILTER
achieved by designing a MATLAB
function named quadss.m. This function oks ith a vey basic
algoithm:
1. Use the quadsys.m function to get matices A and B;
2. Use the quadsens.m function to calculate matices C and D.
Afte executing this code e get the folloing matices:
A =
_
_
0 0
0 0
0 0
0 0
0 0
0 0
1 0
0 1
0 0
0 0
0 0
0 0
_
_
(4.26)
B =
_
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
1
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
0
0
0
0
0
0
0 9.81 0
9.81 0 0
0.0005 0.0005
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 0
0 0 0 0
0.0132 0.0132 0.0132 0.0132
0 0.3881 0 0.3881
0.3881 0 0.3881 0
0.0174 0.0174 0.0174 0.0174
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
_
_
(4.27)
C =
_
_
0 0 0 0 0 0 0 0 0 0 19.62 0
0 0 0 0 0 0 0 0 0 19.62 0 0
0 0 0 0 0 0 0 0 0 0.001 0.001
0 0 0 0 0 0 0 0 0 2 0 0
0 0 0 0 0 0 0 0 0 0 2 0
0 0 0 0 0 0 0 0 0 0 0 1
_
_
(4.28)
4.4. KALMAN FILTER IN MATLAB
AND SIMULINK
51
D =
_
_
0 0.019 0 0.019
0.0001 0.0191 0.0001 0.019
0.0144 0.0132 0.0121 0.0132
0 0.0019 0 0.0019
0 0.0019 0 0.0019
0 0 0 0
_
_
(4.29)
Matix D shos that the Eule angles pitch and oll
inputs, a consequence of the ac
celeomete not being positioned at the COG of the quadoto.
liminate all unobsev
able o uncontollable states. The MATLAB
command fo this opeation is:
sysm=mineak(ss(A,B,C,D));
The esultant state space ealization of the system is:
A =
_
_
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 1 0 0 0 0
0 0 1 0 0 0
_
_
(4.30)
B =
_
_
0 0.3881 0 0.3881
depend on the
No e have to e
0.3881 0 0.3881 0
0.0174 0.0174 0.0174 0.0174
0 0 0 0
0 0 0 0
0 0 0 0
_
_
(4.31)
C =
_
_
0 0 0 0 19.62 0
0 0 0 19.62 0 0
0 0 0 0.001 0.001 0
0 0 0 2 0 0
0 0 0 0 2 0
0 0 0 0 0 1
_
_
(4.32)
hee matix D suffes no changes fom equation (4.29). This ealization also h
as no eachable states,
but is fully contollable and obsevable. Afte close obsevation e can also se
e that the eliminated states
ae the linea velocities and linea positions, leaving us ith the attitude of
the quadoto and angula
velocities. The linea positions ae no supise due to the absence of position
sensos. About the linea
velocities, ould be possible to extact them by integation of the acceleation
s? Well, this is something
e can not expect to do ith cuent lteing techniques because by integating th
e acceleations e ae
also doing the same to the noise associated ith it, thus the linea velocities
dift.
The state space fom of this system can be implemented in MATLAB
using the command:
52 CHAPTER 4. KALMAN FILTER
sys = ss(A,[B G],C,[D H]);
Whee the matices G and H ae de ned as
G = eye(12,6);
H = zeos(6,6);
Matix Gbasically says that e can make measuements on only 6 states (in a pef
ect situation hee all
12 states measued, G ould be an identity matix of size [12 12]). Matix H is d
e ned as null because
e ae assuming the pocess noise has no in uence in the system outputs (see equat
ion (4.13)). As
fo the covaiance matices,
e assume the noise covaiances of both
the system inputs and senso
eadings ae the same (because e do not kno its eal values):
Q = 10
4
I(6) (4.33)
R = 10
4
I(6) (4.34)
Notice that R is a [6 6] matix, because e can only measue 6 outputs (3 accele
ations and the
Eule angles ya, pitch and oll). Also, the pocess noise covaiance matix Q
has size [6 6] because
e ae oking only ith the 6 contollable and obsevable states of the system.
When designing a Kalman lte, e can also choose fomto possible types of estima
tos: continuous
time o discetetime. As e plan to use the Kalman estimato on the Aduino (a
digital system), e must
choose the discetetime option, meaning e have to use anothe vey impotant p
aamete, the sample
time t
s
. In MATLAB
the discete Kalman lte is obtained ith:
Kest = kalmd(sys,Q,R,t
s
);
hee Kest povides contains the lte matices A
k
, B
k
C
k
and D
k
.
Tying to poduce a discete Kalman lte ith the cuent state space matices an
d covaiance ma
tices esults in the folloing state space ealization of the lte:
A
k
=
_
_
1 0 0 0.5985 0 0
0 1 0 0 0.5985 0
0 0 1 0 0 0.0479
0.05 0 0 0.3384 0 0
0 0.05 0 0 0.3384 0
0 0 0.05 0 0 0.9147
_
_
(4.35)
4.4. KALMAN FILTER IN MATLAB
AND SIMULINK
53
B
k
=
_
_
0 0.0188 0 0.0188 0 0.0302 0 0.0031
0.0194 0.0006 0.0194 0.0006 0.0302 0
0.0009 0.0009 0.0009 0.0009 0 0 0 0
0 0.0002 0 0.0002 0 0.0334 0 0.0034
0.0005 0.0006 0.0005 0.0006 0.0334 0
0 0 0 0 0 0 0 0 0 0.0853
_
0.0031
0.0479
0
0
0
0
0
0.0034
_
(4.36)
C
k
=
_
_
0 0
0 0
0 0
0 0
0 0
0 0
1 0
0 1
0 0
0 0
0 0
0 0
_
0
0
0
0
0
0
0
0
1
0
0
0
0 7.2272 0
7.2272 0 0
0.0004 0.0004
0.7367 0 0
0 0.7367 0
0 0 0.917
0.5985 0 0
0 0.5985 0
0 0 0.0479
0.3684 0 0
0 0.3684 0
0 0 0.917
_
(4.37)
D
k
=
_
_
0 0.007 0 0.007 0.6251 0 0
0 0.007 0 0.007 0 0.6251 0
0.0144 0.0132 0.0121 0.0132
0 0.0007 0 0.0007 0 0.0637
0 0.0007 0 0.0007 0.0637 0
0 0 0 0 0 0 0 0 0 0
0 0.0006 0 0.0006 0 0.0302
0 0.0006 0 0.0006 0.0302 0
0 0 0 0 0 0 0 0 0 0
0 0.0006 0 0.0006 0 0.0319
0 0.0006 0 0.0006 0.0319 0
0 0 0 0 0 0 0 0 0 0
_
0 0 0.0637
0.0637 0.0637 0
0 0 0 0 0 0
0 0.0065 0.0065 0
0 0 0 0.0065
0
0
0
0
0.0031
0 0
0.0031
0.0031
0.0032
0 0
0.0032
0.0032
_
(4.38)
No that e have ou state space Kalman lte e can implement it in Simulink
using the block dispo
sition in gue 4.4.1, though a discete state space block.
54 CHAPTER 4. KALMAN FILTER
Figue 4.4.1: Kalman lte schematic.
4.5 Kalman lte fo Aduino
Implementation of a state space Kalman lte in the Aduino is quite easy, mostly
because e only need
matices A
k
and B
k
to estimate the states
_
P Q R
_
. The euaion we are going o use
or his purpose is:
x(k) = A
k
x(k 1) +B
k
u(k) (4.39)
where we esimae he saes a ime insan
k are calculaed b us
ing he saes a he ime insan
k 1 and inpus a he curren ime insan
k. Consanl ring ou dieren
sae space marices in
he Arduino can be a dilaor work. To ease his ask a MATLAB
uncion was developed o conver
an marix variable o Arduino code read or cop/pase operaions. We also n
eed o remember he
necessi o providing he iniial saes x(0) or he lers sarup calculaion.
In our case we are going o
assume he uadroor is leveled and heading norh, which makes us assume he ini
ial sae vecor as:
x(0) =
_
0 0 0 0 0 0
_
(4.40)
The esimaed saes will be used o eed a LQR conroller, which will be he ma
in subjec o he nex
chaper.
Chaper 5
Opimal Conrol The
LinearQuadraic Regulaor
In opimal conrol one endeavors on nding a conroller ha provides he bes pos
sible perormance wih
respec o some given measure o perormance (e.g. he conroller ha uses he
leas amoun o conrol
signal energ o ake he oupu o zero). In his case he measure o perorm
ance (also called opimal
crierion) would be he conrolsignal energ. In his secion we r o implem
en opimal conrol hrough
he use o a LQR conroller (LinearQuadraic Regulaor) in which he conrol si
gnal energ is measured
b a cos uncion conaining weighing acors provided b he conroller desig
ner.
Moreover, he LQR conroller is applicable o MIMO (MulipleInpu MulipleOup
u) ssems (e.g. in
he uadroor we have he speeds o 4 moors as inpus, and he sensor readings
as oupus) or which
classical designs are di cul o appl (see reerence [30]).
5.1 Mahemaical ormulaion
For a coninuousime linear ssem described b:
x = Ax +Bu (5.1)
The cos uncion J
LQR
is [29]:
J
LQR
=
0
_
x
T
Qx +u
T
Ru
_
d
(5.2)
where
R (size m m, wih
m he number o process inpus) are boh smmeric posiivede nie weighing mar
ices. Conseuenl,
he euivalen discreeime eedback conrol law (noe ha he conroller is
o be implemened in he
Arduino, which b i sel is a digial plaorm, hence i is no a coninuousi
me ssem) ha minimizes he
cos value is:
u
K
= Kx (5.3)
where u
K
is he vecor o conrol acions. The LQR gain marix K is provided b:
55
56 CHAPTER 5. OPTIMAL CONTROL THE LINEARQUADRATIC REGULATOR
K =
R
1
B
T
P (5.4)
and
P+
PA
PB
R
1
B
T
P+
Q = 0 (5.5)
The LQR design echniue presened so ar assumes he order o he conroller wi
ll be eual o he order
o he ssem. This ma no be our case. In ac, because, as we have seen beor
e, we have onl 6 saes
available o conrol rom he Kalman ler (angular velociies and Euler angles),
he order o our conroller
will be lower han he order o he ssem. This suggess ha we have wo possi
ble case sudies or he
LQR conroller: 12 saes conrol and 6 saes conrol. The rs one assumes w
e have no Kalman ler
and have ull access o all saes, as or he second we will be ineresed in
esing he perormance o
he conroller using he Kalman ler. This implies respecivel eiher we use a
[12 12] marix A and
[12 6] marix B(or 12 sae conrol) as opposie o he lower size Aand Bmarice
s when using Kalman
lering (see secion 4.4). As or marix
Q and
being diagonal:
Q
k
=
1
x
2
i,max
(5.6)
R
k
=
1
u
2
i,max
(5.7)
where x
i,max
is he highes olerable value or he sae x
i
, and u
i,max
is he highes olerable value or
he inpu u
i
. The core objecive o Brsons rule is hereore o scale he variables in
J
LQR
so ha
he highes olerable value or each erm is one. Paricularl, his is ver
mporan when x and u are
numericall ver dieren rom each oher [31].
5.2 LQR conrol in MATLAB
and Simulink
A discree LQR conroller can be produced in MATLAB
using he command:
Klr=lrd(A,B,
Q,
R,
s
);
which will reurn he gain marix K, according o he sample ime
s
. Tables 5.1 and 5.2 presen he
values chosen o building he weigh marices
Q and
he
i
rs
r.
Table 5.1: Maximum sae values or designing marix
Q.
Saes U
max
V
max
W
max
P
max
Q
max
R
max
X
max
Y
max
Z
max
max
max
max
12 0.1 0.1 0.1 0.001 0.001 0.001 0.4 0.4 0.4 0.1 0.1 0.1
1 1 1
0.1 0.1 0.1
6
5.2. LQR CONTROL IN MATLAB
AND SIMULINK
57
Table 5.2: Maximum inpu values or designing marix
R.
1 max
2 max
3 max
4 max
12 states contol 3 3 3 3
6 states contol 3 3 3 3
No all e have to do is to build matices
Q and
_
0.6649 0.0001 18.7694 0.0001 32.9579 338.8566
0.0001 0.6652 18.7694 32.9577 0.0001 338.8566
0.6652 0.0001 18.7694 0.0001 32.9577 338.8566
0.0001 0.6649 18.7694 32.9579 0.0001 338.8566
0.0803 0 3.6567 0.0057 20.6969 3.3871
0 0.0804 3.6567 20.6855 0.0057 3.3871
0.0804 0 3.6567 0.0057 20.6855 3.3871
0 0.0803 3.6567 20.6969 0.0057 3.3871
_
_
(5.8)
K
6 states
=
_
_
0 7.103 20.4163 0 18.2839 14.4659
7.103 0 20.4163 18.2839 0 14.4659
0 7.103 20.4163 0 18.2839 14.4659
7.103 0 20.4163 18.2839 0 14.4659
_
_
(5.9)
Fo simulation puposes, a LQR contolle can be implemented in Simulink
ith a simple gain block
as schematically potayed in
gue 5.2.1. When a LQR contolle is co
mbined ith a Kalman
lte,
they fom a LQG (LineaQuadaticGaussian) contolle (see Figue 5.2.2). Bot
h elements of an LQG
contolle can be designed and computed independently, as in this case.
Figue 5.2.1: LQR contol.
58 CHAPTER 5. OPTIMAL CONTROL THE LINEARQUADRATIC REGULATOR
Figue 5.2.2: LQG contol block diagam in Simulink
.
5.3 LQR contol in the Aduino
The contolle implementation in the Aduino is quite easy. We only have to de n
e the gain matix K in
the Aduino pogam envionment and multiply it by the estimated states poduced
by the Kalman lte
as in equation (4.39). We have to be caeful as the contol actions of the LQR c
ontolle ae the angula
speeds of the motos. This value has to be conveted back into a PWM signal. We
can achieve this goal
by using data in equation (3.38) to convet angula speeds to pulse idths and c
ompensating ith the
espective motos dead zone:
P =
K
+P
0
(5.10)
hee P
0
is the pulse idth of a motos dead zone, K the gain of its st ode tansfe
function, the
angula speed (fom the LQR contolle) and P is the consequent pulse idth to d
ive the moto.
Chapte 6
Implementation and esults
This chapte shos the evolution of ou attempts to contol the quadoto system
. The addessed case
studies ae:
12 states LQR contol. Stating fom the ideal case, e ill study the situa
tion hee a LQR con
tolle handles all 12 states of the system. This ill sho us the best scena
io possible ith this
kind of contolle.
LQG contol ith sensos. Although e ould like to have all 12 states of th
e quadoto system
at ou disposal, this is not alays possible. In this case, a Kalman lte is us
ed to estimate thee
angula velocities and thee Eule angles fom the sensos outputs. These six st
ates ae afteads
fed to a LQR contolle.
LQG contol ith sensos and motos dynamics. This case is the continuation of
the pevious one,
but this time the contolle obustness is put to the test hen the simulation i
ncludes the dynamics
of the motopopelle assemblies.
Pactical implementation. Taking ou contolle and Kalman lte into ou quado
to pototype is the
next logical step, hee e can test ho the LQG contolle pefoms in the eal
old.
Using gyoscopes to impove state estimation. Sometimes the intensity of sen
so noise is unde
estimated in simulation envionments. In this case study, the use of gyoscopes
(togethe ith the
tiaxis acceleomete and compass) is evaluated as a means of poviding exta i
nfomation to the
Kalman lte and obseve hat is its effect on the quality of the state estimates
(hich accuacy is
cucial fo system contol).
59
60 CHAPTER 6. IMPLEMENTATION AND RESULTS
6.1 Full 12 states contol
One of the most impotant lessons to be dan fom the pocess of simulating any
contol loop is that it
is unise to stat immediately ith the most complex situation hee e have the
system as complete as
possible. This case is often moe dif cult to contol, hich in case of the emege
Q and
R matices.
Figue 6.1.3 illustates that inceasing the value of the eighting matix
Q.
(eight matices
Q
1
>
Q
2
>
Q
3
: oll
Q
1
; oll
Q
2
; . oll
Q
3
; pitch
Q
1
; pitch
Q
2
; . pitch
Q
3
; ya
Q
1
; ya
Q
2
; . ya
Q
3
)
Figue 6.1.4: Linea positions ith inceasing eight matix
R.
(eight matices
R
1
>
R
2
>
R
3
: oll
R
1
,
R
2
and
R
3
; pitch
R
1
,
R
2
and
R
3
; ya
R
2
; ya
R
3
; . ya
R
1
)
6.2 6 states contol
6.2.1 LQG contol ith sensos
Afte ou analysis on the design of the Kalman estimato fo ou system (see Cha
pte 4) e concluded
that e can only ok ith 6 of the 12 states available fom ou system, the ang
ula velocities and and
the Eule angles. It is theefoe appopiate to investigate the behavio of ou
system in the pesence of
the LQR contolle in equation (5.9), hich is designed to contol only these 6
states.
The 6 states contol loop is displayed in Figues 6.2.1 (Simulink
) and 6.2.2, hee ou state vecto
is x =
_
P Q R
_
and our reerence is x =
_
0
_
/4
(i. . ant to k
th uadrotor l v l d and rotat d at an angl = 0.7854 radians, or 45
).
6.2. 6 STATES CONTROL 63
Figue 6.2.1: 6 states Simulink
LQR contol loop.
Figue 6.2.2: Time esponse of the LQR contol loop using 6 states.
64 CHAPTER 6. IMPLEMENTATION AND RESULTS
The Figue above shos that the system follos the efeence easonably fast (le
ss than 10 seconds).
At the same time e can see that both the linea velocities and linea positions
ae not unde the in uence
of the contolle, since they ae not contollable states. In a eal case,
if e ould like to contol the
altitude,
e ould have to do it ouselves only by contolling the poe povi
ded to the motos. This
ould not be necessay if, fo example, e had an altitude senso.
Figue 6.2.3 illustates Simulink block diagam of the next stage of simulations
, hee e include the
sensos (ith noise) in the contol loop to see ho the LQR contol pefoms (se
e Figue 6.2.4). The
contents of the LQG block of Figue 6.2.3 ae also pesented in Figue 5.2.2.
We can see (in Figue 6.2.4, hee the Eule angles ae no gaphically epesen
ted in degees to
impove eadability) that the LQR contolle continues to ende the system stab
le, ithout any need of
adjustments to the Q and R matices (note that the pitch and ya angles ae ove
lapped).
Figue 6.2.3: 6 states Simulink
LQR contol loop ith sensos.
6.2. 6 STATES CONTROL 65
Figue 6.2.4: Time esponse of the LQR contol loop using 6 states and sensos.
6.2.2 LQG contol ith sensos and motos dynamics
In this section e ill obseve hat is the effect poduced by the intoduction
of the motos dynamics in
the contol loop (see Figue 6.2.5), and check if the LQR contolle continues t
o maintain the quadoto
leveled. The efeence used in this case continues to be x =
_
0 0 0 0 0 /4
_
and th simulation
r sults ar availabl in Figur 6.2.6.
66 CHAPTER 6. IMPLEMENTATION AND RESULTS
Figur 6.2.5: 6 stat s Simulink
b ing consid r d in th
controll r d sign, continu s to b dominat d by it.
6.3 Practical im l m ntation
No that hav rov d that it is ossibl to stabiliz th uadrotor in a simu
lat d nvironm nt ith th
curr nt combination of s nsors, ill try to validat our r sults through th
im l m ntation of both th
LQR controll r and Kalman lt r in our uadrotor rototy (s Figur 6.3.1), and
s if can hav
control ov r its attitud .
Figur 6.3.1: Quadrotor rototy .
6.3.1 T l m try and joystick control softar
In this has of our ork can x ct to b n c ssary th d v lo m nt of a sof
tar modul hich
can coll ct information from th Arduino and, if n c ssary, s nd control ord rs
to it. With this obj ctiv in
mind, a small softar modul as built to assist us. This a lication is d sign
at d by Quadjoy and as
d sign d to coll ct not only ASCII t xt m ssag s s nt by th Arduino, as it can
also s nd commands
from a gam controll r that has 2 joysticks, hich tog th r hav a suf ci nt numb
r axis to control th
Eul r angl s and o r su li d to th motors (i. . n d four ax s, thr for
th Eul r angl s and 1
for motors o r). Th a lication int rfac is visibl in Figur 6.3.2 and th
control sch m in Figur
6.3.3.
68 CHAPTER 6. IMPLEMENTATION AND RESULTS
Figur 6.3.2: Quadjoy us r int rfac .
Figur 6.3.3: Quadrotor control diagram.
Th r s nt d int rfac has b n lann d to b as intuitiv as ossibl , also al
loing us to acuir
data to a l that can b load d in Matlab
, sim ly us th command:
6.3. PRACTICAL IMPLEMENTATION 69
load data.txt
6.3.2 Kalman lt r and LQR controll r
Th rst has of rototy t sts consists in v rifying th b havior of th Kalman
lt r. Th cod that as
d v lo d to im l m nt th Kalman lt r and LQR controll r in th Arduino can b f
ound in A ndix C.
This t st consists in grabbing th uadrotor ith both hands and tilting it arou
nd th roll, itch and ya
axis to nsur that it is roviding an acc tabl stimation of th 6 stat s an
t d. This x ri nc nt
fairly ll, shoing that th Kalman lt r can stimat th d sir d 6 stat . As
Figur 6.3.5: Tim r s ons of th LQR control loo using 6 stat s, s nsors and
motors dynamics ith
PWM r solution.
This n data m ans n d to mak th folloing u stion: If by mani ulation
of th uadrotor
6.3. PRACTICAL IMPLEMENTATION 71
attitud ith th motors turn d off hav control ord rs and stimat d stat s
that a ar to b fram d
ithin valu s that s m a ro riat to th stability sc nario ish to hav ,
th n hat is ha ning
h n th motors ar orking that l ads to instability? Th Arduino cod as th r
for chang d to s nd
us data dir ctly from th s nsors in a situation h r th motors r orking.
Figur 6.3.6 shos th
acc l rom t r data coll ct d, h r can s th nois a aring 4 s conds fro
m th start of data
collation, mom nt at hich th motors start d to ork, roducing vibrations acro
ss th structur of th
uadrotor.
Figur 6.3.6: S nsor nois data.
(- x-axis acc l ration; - y-axis acc l ration; - z-axis acc l ration; com ass ya
angl )
At this mom nt can only say our bigg st robl m is nois (as shon by th Fig
ur abov ). This
is articularly s rious for th acc l rom t r, hos data is us d to stimat th
itch and ya angl s.
To try to r duc th nois coming from th s nsors, a n Kalman lt r as d sign
d ith a covarianc
matrix R calculat d from th coll ct d nois data. Cons u ntly, th nois as
also introduc d in th
simulation block of s nsors in ord r to b tt r ortray r ality. Th stimation
rror rov d to b so high
that it as im ossibl to nd a functional LQR controll r for th syst m. Ev n th
addition of a lo ass
72 CHAPTER 6. IMPLEMENTATION AND RESULTS
lt r aft r th s nsors, such as a Butt rorth lt r, r nd r d th syst m im ossibl
to control.
6.4 Using gyrosco s to im rov stat stimation
W hav s n that hav too much nois for our Kalman lt r to handl . It is th
r for tim to a roach
th robl m from a diff r nt rs ctiv in ord r to achi v a stabiliz d ight.
This cha t r focus on
x loring th o tion of s nsor fusion using th curr nt s nsor con guration (th t
ri-axis acc l rom t r
and magn tic com ass) tog th r ith thr angular rat gyrosco s (on for ach
axis of rotation) to
rovid mor accurat stat stimation from noisy m asur m nts.
Angular rat s nsors, also knon as gyrosco s (or gyros), s rv th ur os of
m asuring th rat
of rotation around th s nsor axis. Th or tically, h n int grating th signal
from th gyro, on can acuir th angular chang ov r a riod of tim , but on has to consid r th rob
l m of knoing th initial
angl at tim z ro, and also th signal bias hich vari s in an un r dictabl a
y. As a cons u nc , long
t rm gyro signal int gration is v ry oor, and th calculat d angl drifts aay
from its r al valu . Hov r, gyros rovid v ry good r adings of th angular rat s, and onc g t som
knol dg on th
s nsor signal bias, can accurat ly int grat th signal ov r a short riod o
_
29.3 0 0 0 0 0 0
0 22.2 0 0 0 0 0
0 0 8 0 0 0 0
0 0 0 0.18 0 0 0
0 0 0 0 0.15 0 0
0 0 0 0 0 0.05 0
0 0 0 0 0 0 0.05
_
_
(6.1)
Notic that th matrix abov as not built ith angular gyro data, but ith data
from th acc l rom t r
and com ass. W ill also assum that th covarianc of th ya gyro r ading i
s th sam as th on
from th com ass. This a roach is not v ry accurat in th as ct that sho
uld hav gyro data to
calculat its covarianc , but b caus do not hav any at th tim , shall u
s th curr nt data as
a m ans of t sting th b havior of th Kalman lt r in th simulations. As usual
so far, shall us a
r f r nc h r th uadrotor is in stationary hov r hil rotat d 45
the system is not eachable. In othe ods, if the senso data is coupted
ith noise of such high in
intensity, it may be impossible fo the Kalman lte to estimate the desied state
s.
All the ok done so fa shos only that thee ae seveal aspects of the conto
l pocess that need
to be coected/impoved in ode to make a contolled ight fo this pototype po
ssible. In paticula,
the inclusion of 3 gyoscopes (one fo each axis of otation) should be conside
ed, hich togethe ith
the tiaxis acceleomete, fom a combination of sensos that has aleady been
tested in quadotos,
alloing fo vey good contol ove its pitch and oll angles. The idea is tha
t ith gyoscopes e can
detemine oientation, but they dift ove time (longtem eos), hich is som
ething e can coect ith
acceleometes (shottem eos, i.e. they ae vey noisy). But even then,
e ould need something
else to coect the difting ya angle fom the gyoscope, hich could be done
ith a magnetic compass.
The longtem idea is that adding a geate numbe of sensos, in addition to th
ose suggested, can only
impove the quality and quantity of states estimated by the Kalman lte.
It ould also be inteesting to develop the simulations futhe by in
cluding ne elements such as
aeodynamic foces (i.e.
ind), collisions and othe types of conto
l methods, such as Popotional
Deivative contolles. The quadoto pototype should also be taken to an outsi
de envionment to see its
pefomance tested unde moe exteme conditions.
Bibliogaphy
[1] P. Pounds, R. Mahony, J. Gesham, P. Coke, and J. Robets.
Toads DynamicallyFavouable
QuadRoto Aeial Robots. 2004.
[2] G.M. Hoffmann, H. Huang, S.L. Waslande, and C.J. Tomlin. Quadoto Helic
opte Flight Dynamics
and Contol: Theoy and Expeiment. 2007.
[3] Gjioni E. AbouSleiman R., Koff D. and Yang H. The oakland univesity un
manned aeial quadoto
system. 2008.
[4] W. Banes and W. McComick. Aeodynamics, Aeonautics and Flight Mechanics
. Ne Yok: Wiley,
2nd. edition, 1995.
[5] http://en.ikipedia.og/iki/Quadoto, Octobe 2009.
[6] J.G. Leishman. The BeguetRichet QuadRoto Helicopte of 1907. Veti ite
, 47(3):14, 2001.
[7] http://www.nationmaster.com/encyclopedia/Quadrotor, January 2009.
[8] http://en.wikipedia.or
/wiki/Moller_Skycar_M400, January 2009.
[9] http://en.wikipedia.or
/wiki/M200G_Volantor, January 2009.
[10] http://www.accessmylibrary.com/article1G183761490/airmedicaltransport
takin
.html, Septem
ber 2009.
[11] S. Bouabdallah, P. Murrieri, and R. Sie
wart. Desi
n and control of an
indoor micro quadrotor. In
Robotics and Automation, 2004. Proceedin
s. ICRA04. 2004 IEEE International Co
nference on,
volume 5.
[12] S. Bouabdallah, A. Noth, and R. Sie
wart. PID vs LQ control techniques ap
plied to an indoor micro
quadrotor. In Intelli
ent Robots and Systems, 2004.(IROS 2004). Procee
din
s. 2004 IEEE/RSJ
International Conference on, volume 3.
[13] M. Becker, S. Bouabdallah, and R. Sie
wart. Desenvolvimento de um con
trolador de desvio de
obstculos para um minihelicptero Quadrirotor autnomo 1 fase: Simulao.
[14] P. Pounds, R. Mahony, and P. Corke. Modellin
and Control of a QuadRoto
r Robot. 2006.
[15] A. Mokhtari, A. Benalle
ue, and B. Daachi. Robust feedback linearizatio
n and
h8 controller for a
quad rotor unmanned aerial vehicle. Journal of Electrical En
ineerin
, 57(1):202
7, 2006.
77
78 BIBLIOGRAPHY
[16] G.P. Tournier, M. Valenti, J.P. How, and E. Feron. Estimation and control
of a quadrotor vehicle usin
les
B.1 loadvars.m
81
82 APPENDIX B. MATLAB
FILES
%Description: This mfile initializes all necessary variables required t
o
%buid the dynamic model of a quadrotor in the statespace form.
%The motor confi
uration is as follows:
% X axis
% _|_
% / \
% | 1 |
% \___/
% |
% |
% ___ | ___
% / \ || / \
% | 4 || xZ || 2 |Y axis
% \___/ || \___/
% |
% |
% _|_
% / \
% | 3 |
% \___/
clear all;
%% Physical properties of the environment
=9.81; %Acceleration of
ravity (m)
rho=1.2; %Density of air (m3.k
1)
%% Physical properties of the quadrotor
mq=0.82; %Total mass of the quadrotor
mm=0.048; %Mass of a motor (k
). All motors have equal mass.
lx=28.8e3; %Motor len
th alon
xaxis (m). All motors have equal size
s.
ly=28.8e3; %Motor len
th alon
yaxis (m)
lz=26e3; %Motor len
th alon
zaxis (m)
dc
=0.29; %Distance from the center of
ravity to the center of a mo
tor (m).
%The quadrotor is symmetric re
ardin
the XZ and YZ planes, so
%dc
is the same for all motors.
Ix1=(1/12)*mm*(ly2+lz2); %Moment of inertia (xaxis) for motors 1 and 3
%(k
.m2).
B.1. LOADVARS.M 83
Ix2=(1/12)*mm*(ly2+lz2)+mm*dc
2; %Moment of inertia (xaxis) for motors
%2 and 4 (k
.m2).
Ixx=2*Ix1+2*Ix2; %Total moment of inertia alon
the xaxis (k
.m2)
Iy1=(1/12)*mm*(lx2+lz2)+mm*dc
2; %Moment of inertia (yaxis) for motors
%1 and 3 (k
.m2).
Iy2=(1/12)*mm*(lx2+lz2); %Moment of inertia (yaxis) for motors 2 and 4
%(k
.m2).
Iyy=2*Iy1+2*Iy2; %Total moment of inertia alon
the yaxis (k
.m2)
Iz1=(1/12)*mm*(lx2+ly2)+mm*dc
2; %Moment of inertia (zaxis) for motors
%1 and 3 (k
.m2).
Iz2=(1/12)*mm*(lx2+ly2)+mm*dc
2; %Moment of inertia (zaxis) for motors
%2 and 4 (k
.m2).
Izz=2*Iz1+2*Iz2; %Total moment of inertia alon
the zaxis (k
.m2)
I=dia
([Ixx Iyy Izz]); %Inertia matrix
cp=0.0743; %Thrust coefficient of the propeller
FILES
%function (microseconds)
accel_min = 3*
; %Accelerometer minimum possible readin
h the
%correspondin
PWM pulse of the Arduino
k4 = 1.9986; %Constant that relates Rotation speed of propeller 4 wit
h the
%correspondin
PWM pulse of the Arduino
PWMdz1 = 1256; %Dead zone of the motor 1 (PWM pulsewidth in microse
conds)
PWMdz2 = 1264; %Dead zone of the motor 2 (PWM pulsewidth in microse
conds)
PWMdz3 = 1256; %Dead zone of the motor 3 (PWM pulsewidth in microse
conds)
PWMdz4 = 1256; %Dead zone of the motor 4 (PWM pulsewidth in microse
conds)
%Calculate the PWM pulse necessary to beat the force of
ravity
syms PWM
1 PWM
2 PWM
3 PWM
4
PWM
1=solve((PWM
1PWMdz1)2(1/4)*mq*
/(Kt*(k12)),PWM
1);
PWM
1=double(PWM
1);
PWM
1=PWM
1(PWM
1>PWMdz1);
PWM
2=solve((PWM
2PWMdz2)2(1/4)*mq*
/(Kt*(k22)),PWM
2);
PWM
2=double(PWM
2);
PWM
2=PWM
2(PWM
2>PWMdz2);
PWM
3=solve((PWM
3PWMdz3)2(1/4)*mq*
/(Kt*(k32)),PWM
3);
PWM
3=double(PWM
3);
PWM
3=PWM
3(PWM
3>PWMdz3);
PWM
4=solve((PWM
4PWMdz4)2(1/4)*mq*
/((Kt*k42)),PWM
4);
PWM
4=double(PWM
4);
PWM
4=PWM
4(PWM
4>PWMdz4);
%Compute the necessary rotation speed (rad.s1) of each propeller to be
at
%the force of
ravity
wm1 = double(k1*(PWM
1PWMdz1));
wm2 = double(k2*(PWM
2PWMdz2));
wm3 = double(k3*(PWM
3PWMdz3));
wm4 = double(k4*(PWM
4PWMdz4));
w0=[wm1 wm2 wm3 wm4]; %Initial an
ular speed of motors 1 to 4 (rad.s
1)
U0=0; %Initial linear velocity alon
the xaxis (m.s1)
V0=0; %Initial linear velocity alon
the yaxis (m.s1)
W0=0; %Initial linear velocity alon
the zaxis (m.s1)
P0=0; %Initial an
ular velocity around the xaxis of the quadrotor
%(rad.s1)
Q0=0; %Initial an
ular velocity around the yaxis of the quadrotor
%(rad.s1)
R0=0; %Initial an
ular velocity around the zaxis of the quadrotor
%(rad.s1)
86 APPENDIX B. MATLAB
FILES
X0=0; %Initial quadrotor position alon
the xaxis of the inertial fra
me of
%reference (m)
Y0=0; %Initial quadrotor position alon
the yaxis of the inertial fra
me of
%reference (m)
Z0=0; %Initial 1uadrotor position alon
the zaxis of the inertial fra
me of
%reference (m)
PHI0=0; %Initial pitch an
le of the quadrotor (rad)
THETA0=0; %Initial roll an
le of the quadrotor (rad)
PSI0=0; %Initial yaw an
le of the quadrotor (rad)
q0=an
le2quat(PHI0,THETA0,PSI0); %Initial state quaternion
X0=[U0 V0 W0 P0 Q0 R0 X0 Y0 Z0 q0(1) q0(2) q0(3) q0(4)]; %Initial
state
%vector
Z0lin = 1; %Linearization point with the quadrotor stabilized (m)
PHIlin = 0; %Linearization point pitch an
le of the quadrotor (rad)
THETAlin = 0; %Linearization point roll an
le of the quadrotor (rad)
PSIlin = 0; %Linearization point yaw an
le of the quadrotor (rad)
qlin=an
le2quat(PHIlin,THETAlin,PSIlin); %Initial state quaternion
X0lin=[0 0 0 0 0 0 0 0 Z0lin PHIlin THETAlin PSIlin]; %Linearization
point
%for
eneratin
the statespace system representation of the quadrotor
[A,B,C,D]=quadss(X0lin,mq,I,w0,Kt,Ktm,dc
,daccel); %Get linearized state
%space matrices of the quadrotor
Y0ref = [0 0 0 0 0 0]; %Initial quadrotor reference
quadkalm; %Get Kalman filter
quadLQRcontrol; %Get LQR controller
%quadLQRcontrol_12states %Uncomment this line if you wish 12 states con
trol
%with pure feedback (i.e. no Kalman filter nor sensors)
damp(AB*[zeros(4,3) Klqr(:,1:3) zeros(4,3) Klqr(:,4:6)]) %Ei
en values of
%the closed loop. If an ei
envalue is not stable, the dynamics of th
is
%ei
envalue will be present in the closedloop system which therefore
will
%be unstable.
B.2. QUADSYS.M 87
B.2 quadsys.m
88 APPENDIX B. MATLAB
FILES
function [Xout] = quadsys(Xin,m,I,wm,Kt,Ktm,dc
)
%Function to simulate the quadrotor dynamics and outputs its current
%state space vector.
%The motor confi
uration is as follows:
% X axis
% _|_
% / \
% | 1 |
% \___/
% |
% |
% ___ | ___
% / \ || / \
% | 4 || xZ || 2 |Y axis
% \___/ || \___/
% |
% |
% _|_
% / \
% | 3 |
% \___/
% Inputs:
%
% Xin Initial state vector of the quadrotor:
% Xin = [U V W P Q R X Y Z q1 q2 q3 q4]
% U Linear velocity in the xaxis direction of the inertial
% reference frame (m.s1);
% V Linear velocity in the yaxis direction of the inertial
% reference frame (m.s1);
FILES
% Ktm=0.01;
% dc
=0.5;
%
% Xout = quadsys(Xin,m,I,wm,Kt,Ktm,dc
) outputs:
%
% Xout = [0 0 9.8100 0 0 0 0 0 0 0 0.0500 0 0]
U=Xin(1); %Linear velocity xaxis (m)
V=Xin(2); %Linear velocity yaxis (m)
W=Xin(3); %Linear velocity zaxis (m)
P=Xin(4); %An
ular velocity around the xaxis (rad.s1)
Q=Xin(5); %An
ular velocity around the yaxis (rad.s1)
R=Xin(6); %An
ular velocity around the zaxis (rad.s1)
X=Xin(7); %Position of the body axes frame alon
the xaxis of the i
nertial
%reference system (m)
Y=Xin(8); %Position of the body axes frame alon
the yaxis of the i
nertial
%reference system (m)
Z=Xin(9); %Position of the body axes frame alon
the zaxis of the i
nertial
%reference system (m)
q0=Xin(10); %Quaternion element 1 calculated from Euler an
le conversion
q1=Xin(11); %Quaternion element 2 calculated from Euler an
le conversion
q2=Xin(12); %Quaternion element 3 calculated from Euler an
le conversion
q3=Xin(13); %Quaternion element 4 calculated from Euler an
le conversion
Rm=[q02+q12q22q32 2*(q1*q2+q0*q2) 2*(q1*q3q0*q2);
2*(q1*q2q0*q3) q02q12+q22q32 2*(q2*q3+q0*q1);
2*(q1*q3+q0*q2) 2*(q2*q3q0*q1) q02q12q22+q32]; %rotation matrix R
%usin
quaternions
Tm(1:4)=Kt.*(wm(1:4).2); %calculate thrust produced by each propeller (N)
Fx=0; %xaxis force actin
on the quadrotor while hoverin
steady at
%constant altitude
Fy=0; %yaxis force actin
on the quadrotor while hoverin
steady at
%constant altitude
Fz=(Tm(1)+Tm(2)+Tm(3)+Tm(4)); %zaxis force actin
on the quadrotor whil
e
%hoverin
steady at constant altitude
Mx=(Tm(4)Tm(2))*dc
; %Momentum responsible for rotation around the xaxi
s
%(N.m)
My=(Tm(1)Tm(3))*dc
; %Momentum responsible for rotation around the yaxi
s
%(N.m)
Mz=Ktm*(Tm(1)+Tm(3)Tm(2)Tm(4)); %Momentum responsible for rotation around
B.2. QUADSYS.M 91
%the zaxis (N.m)
=9.81; %Acceleration of
ravity (m.s2)
Lin_a=(1/m).*[Fx Fy Fz]+Rm*[0 0
]([Q*WR*V R*UP*W P*VQ*U]); %Calc.
%the linear acceleration of the quadrotor
An
P=(Mx/I(1,1))(I(3,3)I(2,2))*Q*R/I(1,1); %An
ular acceleration around
%the xaxis of the body reference frame
An
Q=(My/I(2,2))(I(1,1)I(3,3))*R*P/I(2,2); %An
ular acceleration around
%the yaxis of the body reference frame
An
R=(Mz/I(3,3))(I(2,2)I(1,1))*P*Q/I(3,3); %An
ular acceleration around
%the zaxis of the body reference frame
Pos=Rm*[U V W]; %Calculate the position of the quadrotor
Omq=[0 P Q R; %Rotation matrix to calculate the time drivative of
the
P 0 R Q; %rotation quaternion
Q R 0 P;
R Q P 0];
epsilon=1[q0 q1 q2 q3]*[q0 q1 q2 q3];
dq=(1/2)*Omq*[q0 q1 q2 q3]+epsilon*[q0 q1 q2 q3]; %Time derivative of th
e
%rotation quaternion
Xout=[Lin_a(1) Lin_a(2) Lin_a(3) An
P An
Q An
R Pos(1) Pos(2) Pos(3)...
dq(1) dq(2) dq(3) dq(4)]; %Output state vector
end
92 APPENDIX B. MATLAB
FILES
Appendix C
Arduino source code
93
94 APPENDIX C. ARDUINO SOURCE CODE
#include <Wire.h> //Library for I2C implementation usin
analo
pins 4
(SDA) and 5 (SCL)
#include <Me
aServo.h>
#define NBR_SERVOS 4 // the maximum number of servos (up to 12 on A
rduino Diecimilia)
#define FIRST_SERVO_PIN 6 // define motor pins on the Arduino board
#define SECOND_SERVO_PIN 9
#define THIRD_SERVO_PIN 10
#define FOURTH_SERVO_PIN 11
Me
aServo Servos[NBR_SERVOS] ; // max servos
//Accelerometer variables
float ax,ay,az; //Acceleration values in m/s^2 for all three axis
const float as=0.00488; //Arduino analo
resolution = 5Volts/1024 (10bit
s)
const float z
=1.63; //Sensor zero
bias = Supply Volta
e/2 =3.3/2 (
V)
const float sR=0.3; //Sensor resolution (V/
)
//Compass variables
int compassAddress = 0x42 > > 1; // compass I2C slave adress: 0x42
// because the wire.h library only uses the 7 bits most si
nificant
bits
// a shift necessary is to
et the most si
nificant bits.
int psi = 0; // Compass headin
= yaw (direct readin
s from the com
pass in de
rees)
float psirad; // Compass headin
= yaw in radians
//Euler an
les Roll and pitch
float phi; // (rad)
float theta; // (rad)
//Kalman filter data
const float A[6][6] = {
{1.0000,0.0000,0.0000,0.5985,0.0000,0.0000},
{0.0000,1.0000,0.0000,0.0000,0.5985,0.0000},
{0.0000,0.0000,1.0000,0.0000,0.0000,0.0479},
95
{0.0500,0.0000,0.0000,0.3384,0.0000,0.0000},
{0.0000,0.0500,0.0000,0.0000,0.3384,0.0000},
{0.0000,0.0000,0.0500,0.0000,0.0000,0.9147}
};//(Kalman statespace matrix Ak)
const float B[6][10] = {
{0.0000,0.0188,0.0000,0.0188,0.0000,0.0302,0.0000,0.0031,0.0000,0.0000},
{0.0194,0.0006,0.0194,0.0006,0.0302,0.0000,0.0000,0.0000,0.0031,0.0000},
{0.0009,0.0009,0.0009,0.0009,0.0000,0.0000,0.0000,0.0000,0.0000,0.0479},
{0.0000,0.0002,0.0000,0.0002,0.0000,0.0334,0.0000,0.0034,0.0000,0.0000},
{0.0005,0.0006,0.0005,0.0006,0.0334,0.0000,0.0000,0.0000,0.0034,0.0000},
{0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,0.0853}
}; //(Kalman statespace matrix Bk)
float x[6] = {0,0,0,0,0,0}; //state vector: Roll speed, Pitch Speed,
Yaw speed, Roll an
le, Pitch an
le, Yaw an
le
float u[10] = {0,0,0,0,0,0,0,0,0,0}; //input vector: w1 (motor 1 speed
),w2,w3,w4,ax,ay,az,phi,theta,yaw
float At[6] = {0,0,0,0,0,0}; //temp variable
float Bt[6] = {0,0,0,0.01,0.04,0}; //temp variable
//LQR
ain matrix
const float K[4][6] = {
{0.0000,7.1030,20.4163,0.0000,18.2839,14.4659},
{7.1030,0.0000,20.4163,18.2839,0.0000,14.4659},
{0.0000,7.1030,20.4163,0.0000,18.2839,14.4659},
{7.1030,0.0000,20.4163,18.2839,0.0000,14.4659}
};
float w1,w2,w3,w4; //Motor inputs from LQR controller (rad.s^1)
float w0; //Throttle to the motors (from joystick)
//Motor inputs for PWM si
nal
float wc1=0;
96 APPENDIX C. ARDUINO SOURCE CODE
float wc2=0;
float wc3=0;
float wc4=0;
//PWM si
nals
int p1 = 200;
int p2 = 200;
int p3 = 200;
int p4 = 200;
//Reference
float yr[6]={0,0,0,0,0,0};
void setup()
{
Wire.be
in();
Serial.be
in(9600); // Initialize serial communications with setup
Servos[0].attach( FIRST_SERVO_PIN, 800, 2200); //Motor 1 North Cloc
kwise rotation
Servos[1].attach( SECOND_SERVO_PIN, 800, 2200); //Motor 2 East Coun
terclockwise rotation
Servos[2].attach( THIRD_SERVO_PIN, 800, 2200); //Motor 3 South Cloc
kwise rotation
Servos[3].attach( FOURTH_SERVO_PIN, 800, 2200); // Motor 4 West Co
unterclockwise rotation
//Put the motors in full stop
Servos[0].write(0);
Servos[1].write(0);
Servos[2].write(0);
Servos[3].write(0);
}
void loop()
{
//
et the most recent acceleration values for all three axis
97
ax = ((analo
Read(0)*asz
)/sR)*9.81; //acceleration xaxis (m.s^2)
ay = ((analo
Read(1)*asz
)/sR)*9.81; //acceleration yaxis (m.s^2)
az = ((analo
Read(2)*asz
)/sR)*9.81; //acceleration zaxis (m.s^2)
ax=ax; //This correction allows the accelerometer to provide positive
acceleration in the xaxis direction
// Compass task 1: connect to the HMC6352 sensor
Wire.be
inTransmission(compassAddress);
Wire.send(A); // The ascii character "A" tells the compass sensor to se
nd data
Wire.endTransmission();
// Compass task 2: wait for data processin
At[0]=A[0][0]*x[0]+A[0][1]*x[1]+A[0][2]*x[2]+A[0][3]*x[3]+A[0][4]*x[4]+A[0][5]*x
[5];
98 APPENDIX C. ARDUINO SOURCE CODE
At[1]=A[1][0]*x[0]+A[1][1]*x[1]+A[1][2]*x[2]+A[1][3]*x[3]+A[1][4]*x[4]+A[1][5]*x
[5];
At[2]=A[2][0]*x[0]+A[2][1]*x[1]+A[2][2]*x[2]+A[2][3]*x[3]+A[2][4]*x[4]+A[2][5]*x
[5];
At[3]=A[3][0]*x[0]+A[3][1]*x[1]+A[3][2]*x[2]+A[3][3]*x[3]+A[3][4]*x[4]+A[3][5]*x
[5];
At[4]=A[4][0]*x[0]+A[4][1]*x[1]+A[4][2]*x[2]+A[4][3]*x[3]+A[4][4]*x[4]+A[4][5]*x
[5];
At[5]=A[5][0]*x[0]+A[5][1]*x[1]+A[5][2]*x[2]+A[5][3]*x[3]+A[5][4]*x[4]+A[5][5]*x
[5];
//Get throttle from the joystick
if (Serial.available() > 0)
{
// read the incomin
byte:
w0 = map((float)Serial.read(), 0, 100, 0, 500);
w0=constrain(w0,0,500); //Limit maximum velocity to 600 rad/s
Serial.flush();
}
else
{
w0 = 0;
}
//place known inputs and measurements in the input vector u
u[0]=wc1w0;
u[1]=wc2w0;
u[2]=wc3w0;
u[3]=wc4w0;
u[4]=axyr[0];
u[5]=ayyr[1];
u[6]=azyr[2];
u[7]=phiyr[3];
u[8]=thetayr[4];
99
u[9]=psiradyr[5];
Bt[0]=B[0][0]*u[0]+B[0][1]*u[1]+B[0][2]*u[2]+B[0][3]*u[3]+B[0][4]*u[4];
Bt[0]=Bt[0]+B[0][5]*u[5]+B[0][6]*u[6]+B[0][7]*u[7]+B[0][8]*u[8]+B[0][9]*u[9];
Bt[1]=B[1][0]*u[0]+B[1][1]*u[1]+B[1][2]*u[2]+B[1][3]*u[3]+B[1][4]*u[4]
Bt[1]=Bt[1]+B[1][5]*u[5]+B[1][6]*u[6]+B[1][7]*u[7]+B[1][8]*u[8]+B[1][9]*u[9];
Bt[2]=B[2][0]*u[0]+B[2][1]*u[1]+B[2][2]*u[2]+B[2][3]*u[3]+B[2][4]*u[4]
Bt[2]=Bt[2]+B[2][5]*u[5]+B[2][6]*u[6]+B[2][7]*u[7]+B[2][8]*u[8]+B[2][9]*u[9];
Bt[3]=B[3][0]*u[0]+B[3][1]*u[1]+B[3][2]*u[2]+B[3][3]*u[3]+B[3][4]*u[4]
Bt[3]=Bt[3]+B[3][5]*u[5]+B[3][6]*u[6]+B[3][7]*u[7]+B[3][8]*u[8]+B[3][9]*u[9];
Bt[4]=B[4][0]*u[0]+B[4][1]*u[1]+B[4][2]*u[2]+B[4][3]*u[3]+B[4][4]*u[4]
Bt[4]=Bt[4]+B[4][5]*u[5]+B[4][6]*u[6]+B[4][7]*u[7]+B[4][8]*u[8]+B[4][9]*u[9];
Bt[5]=B[5][0]*u[0]+B[5][1]*u[1]+B[5][2]*u[2]+B[5][3]*u[3]+B[5][4]*u[4]
Bt[5]=Bt[5]+B[5][5]*u[5]+B[5][6]*u[6]+B[5][7]*u[7]+B[5][8]*u[8]+B[5][9]*u[9];
//Get estimated states x(k+1)=A.x(k)+B.u(k)
x[0]=At[0]+Bt[0];
x[1]=At[1]+Bt[1];
x[2]=At[2]+Bt[2];
x[3]=At[3]+Bt[3];
x[4]=At[4]+Bt[4];
x[5]=At[5]+Bt[5];
//Controlo LQR
w1=K[0][0]*x[0]+K[0][1]*x[1]+K[0][2]*x[2]+K[0][3]*x[3]+K[0][4]*x[4]+K[0][5]*x[5]
;
100 APPENDIX C. ARDUINO SOURCE CODE
w2=K[1][0]*x[0]+K[1][1]*x[1]+K[1][2]*x[2]+K[1][3]*x[3]+K[1][4]*x[4]+K[1][5]*x[5]
;
w3=K[2][0]*x[0]+K[2][1]*x[1]+K[2][2]*x[2]+K[2][3]*x[3]+K[2][4]*x[4]+K[2][5]*x[5]
;
w4=K[3][0]*x[0]+K[3][1]*x[1]+K[3][2]*x[2]+K[3][3]*x[3]+K[3][4]*x[4]+K[3][5]*x[5]
;
//Adjust speeds with the throttle
wc1=w0w1;
wc2=w0w2;
wc3=w0w3;
wc4=w0w4;
//Calculate and send pulse width to the motors
p1=(int)((wc1/2.0276)+1252); // (speed of motor 1 (rad/s) / transfer fu
nction
ain) + Deadzone pulse width
p2=(int)((wc2/1.8693)+1262);
p3=(int)((wc3/2.0018)+1255);
p4=(int)((wc4/1.9986)+1255);
if(w0==0) //Forar para
em
{
p1=400;
p2=400;
p3=400;
p4=400;
}
Servos[0].write(p1);
Servos[1].write(p2);
Servos[2].write(p3);
Servos[3].write(p4);
// output states
Serial.print(x[0]);
Serial.print(" ");
101
Serial.print(x[1]);
Serial.print(" ");
Serial.print(x[2]);
Serial.print(" ");
Serial.print(x[3]);
Serial.print(" ");
Serial.print(x[4]);
Serial.print(" ");
Serial.print(x[5]);
Serial.println("");
}
float de
2rad (float x) //function that receives an an
le between 0 a
nd 359 de
rees and resturns the same an
le between pi
and pi radians
{
if(x>=0 && x<=180)
{
x=x*3.14/180;
}
else
{
x = (360x)*3.14/180;
}
return x;
}