Institutionen för systemteknik Department of Electrical Engineering

Institutionen för systemteknik Department of Electrical Engineering
Institutionen för systemteknik
Department of Electrical Engineering
Examensarbete
Positioning and Trajectory Control of an Autonomous
Helicopter using Real Time Kinematic GPS
Examensarbete utfört i Reglerteknik
vid Tekniska högskolan vid Linköpings universitet
av
Josef Högberg and Staffan Sjöqvist
LiTH-ISY-EX--13/4691--SE
Linköping 2013
Department of Electrical Engineering
Linköpings universitet
SE-581 83 Linköping, Sweden
Linköpings tekniska högskola
Linköpings universitet
581 83 Linköping
Positioning and Trajectory Control of an Autonomous
Helicopter using Real Time Kinematic GPS
Examensarbete utfört i Reglerteknik
vid Tekniska högskolan vid Linköpings universitet
av
Josef Högberg and Staffan Sjöqvist
LiTH-ISY-EX--13/4691--SE
Handledare:
Patrik Axelsson
isy, Linköpings universitet
Torkel Danielsson
Intuitive Aerial AB
Examinator:
David Törnqvist
isy, Linköpings universitet
Linköping, 13 juni 2013
Avdelning, Institution
Division, Department
Datum
Date
Division of Automatic Control
Department of Electrical Engineering
SE-581 83 Linköping
2013-06-13
Språk
Language
Rapporttyp
Report category
ISBN
2 Svenska/Swedish
2 Licentiatavhandling
ISRN
2 Engelska/English
2 Examensarbete
2 C-uppsats
2 D-uppsats
2
2 Övrig rapport
—
LiTH-ISY-EX--13/4691--SE
Serietitel och serienummer
Title of series, numbering
ISSN
—
2
URL för elektronisk version
http://urn.kb.se/resolve?urn=urn:nbn:se:liu:diva-XXXXX
Titel
Title
Positionering och banföljning av en autonom helikopter med hjälp av Real Time Kinematic
GPS
Positioning and Trajectory Control of an Autonomous Helicopter using Real Time Kinematic
GPS
Författare
Author
Josef Högberg and Staffan Sjöqvist
Sammanfattning
Abstract
Under examensarbetet har reglersystemet för en quadrokopter utvecklats. En quadrokopter
är en liten helikopter med fyra rotorer som styrs med hjälp av en handhållen kontroll av en
operatör positionerad på marken.
Tre olika huvudämnen behandlas i detta examensarbete. Till att börja med utvärderas
ett Real Time Kinematic Global Positioning System (RTK-GPS). Sedan definieras en mjuk
rörelsebana som farkosten i framtiden autonomt kommer att reglera efter. Till sist behandlas styrsystemet i quadrokoptern för att erhålla goda flygegenskaper i det manuella fallet
och exempelvis altitudhållning i det autonoma fallet.
Utvärderingen av RTK-GPS-systemet resulterade i att det är möjligt att uppnå centimeternoggrannhet i positionsbestämningen. I rapporten diskuteras också vad RTK-GPSsystemet kräver för att det ska fungera så bra som möjligt. En mjuk rörelsebana kan
definieras på olika sätt. Önskemålet är att banan ska ha kontinuerlig första- och andraderivata, egenskaper som kubiska spline-funktioner besitter. Reglerprestandan förbättrades genom att krafter och vinkelhastigheter transformerades mellan de två kordinatsystemen som nyttjades. På detta sätt blev styrningen av quadrokoptern mjukare.
Nyckelord
Keywords
Real Time Kinematic GPS, quadrocopter, motion path, cubic splines, automatic control
Sammanfattning
Under examensarbetet har reglersystemet för en quadrokopter utvecklats. En
quadrokopter är en liten helikopter med fyra rotorer som styrs med hjälp av en
handhållen kontroll av en operatör positionerad på marken.
Tre olika huvudämnen behandlas i detta examensarbete. Till att börja med utvärderas ett Real Time Kinematic Global Positioning System (RTK-GPS). Sedan
definieras en mjuk rörelsebana som farkosten i framtiden autonomt kommer att
reglera efter. Till sist behandlas styrsystemet i quadrokoptern för att erhålla goda
flygegenskaper i det manuella fallet och exempelvis altitudhållning i det autonoma fallet.
Utvärderingen av RTK-GPS-systemet resulterade i att det är möjligt att uppnå
centimeternoggrannhet i positionsbestämningen. I rapporten diskuteras också
vad RTK-GPS-systemet kräver för att det ska fungera så bra som möjligt. En mjuk
rörelsebana kan definieras på olika sätt. Önskemålet är att banan ska ha kontinuerlig första- och andraderivata, egenskaper som kubiska spline-funktioner
besitter. Reglerprestandan förbättrades genom att krafter och vinkelhastigheter
transformerades mellan de två kordinatsystemen som nyttjades. På detta sätt blev
styrningen av quadrokoptern mjukare.
iii
Abstract
During this master’s thesis project a control system for a quadrocopter has been
developed. A quadrocopter is a small helicopter with four rotors which is controlled by a hand held controller by an operator positioned on the ground.
This master thesis report treats three different main subjects. The first one is
about evaluating a Real Time Kinematic Global Positioning System (RTK-GPS).
The second one is about defining smooth motion paths which the aircraft autonomously shall be able to fly along. The third one is about improving the
control system in the aircraft to obtain better flight performance under manual
control and to enable autonomous features such as altitude hold.
The result is that it is possible to achieve centimetre accuracy in estimated position with the evaluated RTK-GPS system. It is also discussed how the performance of the RTK-GPS system can be maximized.
A smooth path can be defined in different ways. It is desirable that the path has
continuous first and second derivatives. Therefore cubic spline curves are used
to represent the path.
The control system was improved by transforming forces and angular velocities
between the two different coordinate systems. This resulted in improved abilities
by making the steering and control of the aircraft smoother.
v
Acknowledgments
Under examensarbetet har vi haft god hjälp av vår handledare Patrik Axelsson
som bidragit med många goda tips och kommentarer, både när det gäller denna
rapport och i reglertekniska frågor. Vi har också haft god hjälp av Mårten Svanfeldt och Torkel Danielsson på Intuitive Aerial AB som hjälpt oss lösa många
praktiska problem och kommit med värdefulla idéer och synpunkter. Till sist
vill vi tacka vår examinator David Törnqvist som hjälpt oss i reglertekniska frågor.
Tack!
Linköping, Juni 2013
Josef Högberg och Staffan Sjöqvist
vii
Contents
Notation
1 Introduction
1.1 Background .
1.2 Goals . . . . .
1.3 Previous Work
1.4 Thesis Outline
xiii
.
.
.
.
1
1
1
2
3
2 Background Mathematics Material
2.1 Matrix and Vector Notation . . . . . . . . . . . . . . . . . . . . . .
2.2 Coordinate Systems . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.2.1 Earth Fixed Coordinate System . . . . . . . . . . . . . . . .
2.2.2 Body Fixed Coordinate System . . . . . . . . . . . . . . . .
2.3 Transformations . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.3.1 Rotational Kinematics . . . . . . . . . . . . . . . . . . . . .
2.3.2 Transforming Angular Velocity . . . . . . . . . . . . . . . .
2.4 Transformation of Forces . . . . . . . . . . . . . . . . . . . . . . . .
2.4.1 Transformation of Forces from Earth frame to Body Frame
2.4.2 Translate Body Fixed Forces to the Earth Frame . . . . . . .
2.5 Translation from Latitude and Longitude to Earth Frame . . . . .
2.5.1 Translation from Latitude to Distance . . . . . . . . . . . .
2.5.2 Conversion from Longitude to Distance . . . . . . . . . . .
5
5
7
7
7
8
8
9
11
11
11
13
13
15
3 Models
3.1 Quadrocopter Model . . . . . . . . . . . . . . . . . . . . .
3.2 Motion Path - A Smooth Trajectory in 3D . . . . . . . . . .
3.2.1 Properties of Spline Curves . . . . . . . . . . . . .
3.2.2 Definition of Spline Curves . . . . . . . . . . . . .
3.2.3 Finding the Closest Point on A Spline Curve . . . .
3.3 AHRS Algoritm . . . . . . . . . . . . . . . . . . . . . . . .
3.3.1 Pros and Cons with Madgwick’s Orientation Filter
17
17
20
21
21
26
29
31
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
4 Sensors - IMU, Magnetometer, Barometer and Internal GPS
ix
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
33
x
CONTENTS
4.1 IMU . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.1.1 Gyroscopes . . . . . . . . . . . . . . . . . . .
4.1.2 Accelerometers . . . . . . . . . . . . . . . .
4.2 Magnetometer . . . . . . . . . . . . . . . . . . . . .
4.2.1 Hard Iron and Soft Iron Distortion . . . . .
4.2.2 Automatic Hard Iron Calibration Algorithm
4.3 Barometer . . . . . . . . . . . . . . . . . . . . . . .
4.4 Internal GPS . . . . . . . . . . . . . . . . . . . . . .
4.5 Sensors and AHRS Filter Performance . . . . . . .
4.5.1 Yaw Rotation . . . . . . . . . . . . . . . . . .
4.5.2 Roll, Pitch and Yaw Rotation . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
33
33
33
34
34
34
37
42
42
42
43
5 Evaluation of RTK-GPS System
5.1 Evaluation of Trimble’s RTK-GPS . . . . .
5.1.1 Experience and Usages of RTK-GPS
5.1.2 Tests . . . . . . . . . . . . . . . . .
5.2 Error Detection . . . . . . . . . . . . . . .
5.2.1 Maximum Velocity . . . . . . . . .
5.2.2 Maximum Acceleration . . . . . . .
5.2.3 Extrapolated Position . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
49
49
49
50
53
53
55
55
6 Control System
6.1 State Estimation . . . . . . . . .
6.1.1 Extended Kalman Filter
6.2 PID-Controllers . . . . . . . . .
6.2.1 Instability Issues . . . .
6.2.2 Feedback Controller . .
6.3 Controllers . . . . . . . . . . . .
6.3.1 Altitude Hold Controller
6.3.2 Yaw Hold Controller . .
6.3.3 Position Hold Controller
6.3.4 Motion Path Controller .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
57
57
57
58
59
59
59
60
63
64
64
7 Discussions and Conclusions
7.1 Models . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.1.1 Simulation Environment . . . . . . . . . . . . . . . .
7.1.2 Motion Path . . . . . . . . . . . . . . . . . . . . . . .
7.2 Sensors - IMU, Magnetometer, Barometer and Internal GPS
7.3 Evaluation of RTK-GPS System . . . . . . . . . . . . . . . .
7.3.1 Successful Tests . . . . . . . . . . . . . . . . . . . . .
7.3.2 Glitches . . . . . . . . . . . . . . . . . . . . . . . . .
7.4 Future Development . . . . . . . . . . . . . . . . . . . . . .
7.5 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.6 Experiences . . . . . . . . . . . . . . . . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
67
67
67
68
68
69
69
69
70
70
71
A Appendix
A.1 Equipment Borrowed from Trimble . . . . . . . . . . . . . . . . . .
75
75
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
CONTENTS
xi
Bibliography
77
Notation
Abbreviations
Abbreviation
dc
esc
fc
gps
ia
imu
ned
pid
rtk
Definition
Direct Current
Electronic Speed Controller
Flight Controller
Global Positioning System
Intuitive Aerial
Inertial Measurement Unit
North East Down
Proportional, Integral, Differential (controller)
Real Time Kinematic
xiii
xiv
Notation
Notations
Notation
EF
Force F expressed in the Earth Coordinate System
(Earth frame)
Force F expressed in the Body Coordinate System
(Body frame)
BF
"
Eξ
Definition
#
Eη
 
x
y 
ξ
=
 
E
 
z
 
φ
 
 θ 
Eη = 
 
ψ
 
 u 
 v 
υ
=
 
B
 
w
 
p
 q 
ω
=
 
B
 
r
R
W
KP
KI
KD
K
e(t)
u(t)
r(t)
y(t)
p (t|c; t)
f (t)
c
ti
x
x̂
State vector
Position of the aircraft in the Earth frame
Euler angles of the aircraft
Velocity of the aircraft in the Body frame
Angular velocities of the aircraft
Rotation matrix, transforming vectors from Earth
frame to Body frame
Rotation matrix, transforming angular velocities from
Earth frame to Body frame
Proportional gain
Integral gain
Derivative gain
Kalman gain
Control error
Input signal
Desired output signal, reference signal
Actual output signal
Spline segment
Spline curve
Control point in a spline curve
Knot in a spline curve
State vector
Estimated state vector
1
Introduction
This chapter contains the background and goals of this master’s thesis. The thesis
was performed at Intuitive Aerial AB.
1.1
Background
Intuitive Aerial (IA) is developing a flying camera platform for advanced film
cameras. The camera platform consists of a multi-rotor helicopter with 12 rotors
and a three-axis stabilized camera gimbal. Currently it is adapted to be operated
manually but in the future IA wants it to be able to fly along a path autonomously.
There are autopilot systems available that can fly along a path defined by discrete
waypoints, but that solution provides too little flexibility and control of the trajectory to be usable in a movie recording.
This master’s thesis is about the development of the control system and the positioning methods needed to be able to fly along a smooth motion path with high
precision. The aircraft used as a development platform is a quadrocopter, see Figure 1.1, with four rotors that are controlled by a control unit (Flight Controller)
which is mounted in the centre of the aircraft. The results of this master thesis
will be used for the further development of autonomous unmanned aerial vehicles at Intuitive Aerial.
1.2
Goals
The goals of the master’s thesis are the following
• Improve the flight performance of the aircraft
1
2
1
Introduction
Figure 1.1: The aircraft used during the project was this quadrocopter.
• Develop a method to define a smooth motion path
• Evaluate the performance of an RTK-GPS system
1.3
Previous Work
The office of Flying Machine Arena (FMA) is located at Eidgenössische Technische
Hochschule Zürich (ETH Zürich). They describe themselves as A space where flying robots live and learn. Quadrocopters are used in their research in adaption
and learning because of the agility, hover ability, mechanical simplicity and robustness of these aircraft. Their research is also about trajectory planning and
cooperation between multiple quadrocopters. For example they solve planning
problems such as constructing small structures of blocks with the help of each
other and with different responsibilities during the construction phase. They use
a high-precision motion capture system to keep track of the quadrocopters. Models over the system and other data, such as gyroscopes and accelerometers, are
used in a sensor fusion framework to predict the state of the object in the future.
For more information see [FMA].
The similarities with FMA:s work and this thesis are that quadrocopters are used
and that we are interested in the physical model of them. Both are also interested
in solving the path planning issues and the control issue. To our help both have
IMU:s with three gyroscopes and three accelerometers but there the similarities
ends. They use a high-precision motion capture system which gives them a good
overview of where their quadrocopters actually are - they know exactly in which
point in space their quadrocopters are. Thus they are limited to a confined volume of approximately 10 × 10 × 10 m3 . Instead of a motion-capture system we
use a GPS, a barometer and a magnetometer to get position in space, altitude and
heading. The quadrocopter has not been limited to a confined volume indoors
during this master’s thesis.
1.4
1.4
Thesis Outline
3
Thesis Outline
Chapter 1 gives an introduction to the master’s thesis. Chapter 2 gives the fundamental mathematics that are needed to follow the text. Chapter 3 contains a
theoretical background that gives a solid ground for the models and algorithms
used in the aircraft. After that the sensors used in the aircraft are introduced in
Chapter 4 and their pros and cons are discussed. The background and evaluation
of the RTK-GPS system from Trimble are presented in Chapter 5. In Chapter 6
the control systems are presented. Chapter 7 is dedicated to discussions and conclusions.
2
Background Mathematics Material
This chapter contains an overview of the basic mathematics that are needed to follow the work in the following chapters. The chapter starts with a section about
matrix and vector notation and continues with a section about the coordinate systems used. It ends with some transformation mathematics including rotational
matrices and conversions between the different coordinate systems.
2.1
Matrix and Vector Notation
Matrices and vectors are written with boldface letters, for example F. Left sub indices, E F, for the geometric vectors mark which coordinate system the vectors are
represented in. The elements will differ but the vector is the same independently
of which coordinate system it is described in. The left sub index E stands for the
Earth fixed frame (Earth frame) and the left sub index B stands for the Body fixed
frame (Body frame).
An example of the above described notation system is given by


E Fx 
 F 
EF = 
E y  .
E Fz
(2.1)
The state vector is given by
"
Eξ
Eη
5
#
(2.2)
6
2
Background Mathematics Material
where
 
x
y 
Eξ = 
 
z
is the position of the body in the Earth frame and
 
φ
 θ 
Eη = 
 
ψ
(2.3)
(2.4)
are the Euler angles of the body expressed in the Earth frame. The velocity of the
aircraft expressed in the Body frame is
 
 u 
 v 
υ
=
(2.5)
  .
B
 
w
The angular velocities of the aircraft in the Body frame are
 
p
 q 
Bω = 
 
r
(2.6)
In flight mechanics motion in the B X B Z-plane, u, w and q, are called longitudinal
and v, p and r are called laterals.
Table 2.1 defines the different units that are used in this report and Table 2.2
Measure
Angle
Frequency
Force
Voltage
Table 2.1: Units used in the report.
Unit
Symbol
In base units
radian
rad
−
Hertz
Hz
s−1
Newton
N
m · kg · s−2
Volt
V
m2 · kg · s−3 · A−1
gives a summary of Equations (2.4), (2.5) and (2.6). Table 2.2 is borrowed from
Gustafsson [2012].
Table 2.2: Coordinate notation for the rotational kinematic of a body expressed in its Body fixed coordinate system with respect to the Earth fixed
coordinate system.
Axis Motion components
Rotation Euler angle Angular velocities
Longitudinal motion u Roll φ
p
BX
Lateral motion v
Pitch θ
q
BY
Longitudinal motion w Yaw ψ
r
BZ
2.2
7
Coordinate Systems
2.2
Coordinate Systems
Two coordinate systems are used in this report. Both are NED-systems where
NED is the acronym for North, East, Down. The first is fixed to a reference point
on Earth and the other is fixed to the aircraft. In Figure 2.1 the two different
coordinate systems are shown.
rd
wa
r
o
F
Roll, φ
BX
Pitch, θ
BO
BY
Yaw, ψ
BZ
Body fixed coordinate system
EO
North
EX
East
EY
Down
EZ
Earth fixed coordinate system
Figure 2.1: The Earth fixed coordinate system together with the Body fixed
coordinate system.
2.2.1
Earth Fixed Coordinate System
The Earth fixed coordinate system has its origin E O at the surface of Earth and
near the operational area of the aircraft. It may for example be the start position of the aircraft. The E X-axis points northward and the E Y -axis points eastward. The E X E Y -plane is parallel with an idealized surface segment of the Earth
around the chosen origin E O. Vectors expressed in the Earth fixed coordinate
system have left sub index E.
2.2.2
Body Fixed Coordinate System
The Body fixed coordinate system is fixed to the aircraft and rotates with the
aircraft. Its origin E O is fixed to the mass centre of the aircraft. The B X-axis
points in the forward direction and the B Y -axis points out to the right of the
aircraft. Vectors expressed in the Body fixed coordinate system have left sub
index B.
8
2
2.3
Background Mathematics Material
Transformations
In this section some useful transformations between the Earth fixed and Body
fixed coordinate systems are introduced and explained.
2.3.1
Rotational Kinematics
A vector represented in one coordinate system can be transformed to another
system by multiplying it with a rotation matrix that is defined by the Euler angles.
The Euler angles are represented by
 
φ
 θ 
(2.7)
Eη = 
 
ψ
A rotation from Earth frame to Body frame is performed in the following order:
1. Rotation ψ around the B Z-axis, see rotation matrix Rz (ψ) in (2.8)
2. Rotation θ around the B Y -axis, see rotation matrix Ry (θ) in (2.9)
3. Rotation φ around the B X-axis, see rotation matrix Rx (φ) in (2.10)
The rotation matrix Rz (ψ) is given by

 cos(ψ)

Rz (ψ) = − sin(ψ)

0
the rotation matrix Ry (θ) is given by

cos(θ)

Ry (θ) =  0

sin(θ)

sin(ψ) 0
cos(ψ) 0 ,

0
1
(2.8)

0 − sin(θ)
1
0 

0 cos(θ)
and the rotation matrix Rx (φ) is given by

0
1

Rx (φ) = 0 cos(φ)

0 − sin(φ)

0 
sin(φ)  .

cos(φ)
(2.9)
(2.10)
The above rotation matrices are the standard in flight mechanics, see Nelson
[1998]. To get the total rotation matrix R equations (2.8) to (2.10) are multiplied
together, from the left, in the order mentioned above.
R = Rx (φ)Ry (θ)Rz (ψ) =




0
0  cos(θ) 0 − sin(θ)  cos(ψ) sin(ψ) 0
1
0 cos(φ) sin(φ)   0


1
0  − sin(ψ) cos(ψ) 0 =

 




0 − sin(φ) cos(φ) sin(θ) 0 cos(θ)
0
0
1

cos(θ) cos(ψ)
cos(θ) sin(ψ)

− cos(φ) sin(ψ) + sin(φ) sin(θ) cos(ψ) cos(φ) cos(ψ) + sin(φ) sin(θ) sin(ψ)

sin(φ) sin(ψ) + cos(φ) sin(θ) cos(ψ) − sin(φ) cos(ψ) + cos(φ) sin(θ) sin(ψ)
(2.11)

− sin(θ) 
sin(φ) cos(θ) 

cos(φ) cos(θ)
2.3
9
Transformations
The transformation from the Earth fixed coordinate system E X E Y E Z to the Body
fixed frame B X B Y B Z is expressed as
BF
= RE F
(2.12)
where B F stands for a vector in the Body fixed coordinate system and E F for a
vector in the Earth fixed coordinate system. The rotation matrix R in (2.11) is
orthogonal which means that its inverse is given by its transpose
R−1 = RT ,
(2.13)
hence it is easy to transform a vector in the Earth fixed coordinate system to the
Body fixed coordinate system;
EF
= R−1 B F = RT B F
(2.14)
by using the transpose of R.
2.3.2
Transforming Angular Velocity
The transformation of angular velocities E η̇ in the Earth frame to angular velocities B ν in the Body fixed frame is given by
Bν
= W E η̇
which can be written as
  
0
p 1
 q  0 cos(φ)
  = 
r
0 − sin(φ)
 
− sin(θ)  φ̇
cos(θ) sin(φ)   θ̇ 
 
cos()φ) cos(φ) ψ̇
(2.15)
(2.16)
The inverse to W is used to transform the body’s angular velocities to inertial
angular velocities,
  1 sin(φ) tan(θ) cos(φ) tan(θ)  
 p
φ̇ 
  
  0
cos(φ)
−
sin(φ)
  q 
(2.17)
 θ̇  = 
  
  
cos(φ)
sin(φ)
 r
0
ψ̇
cos(θ)
cos(θ)
expressed in the Earth frame. Observe the singularities that appears when θ =
± π2 because the denominator cos (θ) becomes zero. With quaternion representation these singularities can be avoided.
Derivation of W
This derivation is taken from a lecture with Lars Johansson in Flight Dynamics
and is reproduced here. Assume that the angular velocities are non-zero, that is
ψ̇ , 0
(2.18)
θ̇ , 0
φ̇ , 0
(2.19)
(2.20)
10
2
Background Mathematics Material
and introduce two help coordinate systems x0 y 0 z 0 and x00 y 00 z 00 where the rotation
can take place, see Figure 2.2. The first rotation becomes
 
 0 
 0  = E W̃ ψ̇
(2.21)
x0 y 0 z 0 W̃ ψ̇ = 
 
ψ̇
where x0 y 0 z 0 rotates relative to the Earth frame E expressed in x0 y 0 z 0 components.
The second rotation is
 
 0 
θ̇  = x0 y 0 z 0 W̃ θ̇
(2.22)
x00 y 00 z 00 W̃ θ̇ = 
 
0
where x00 y 00 z 00 rotates relative to x0 y 0 z 0 expressed in x00 y 00 z 00 components. The
third and last rotation is
 
φ̇
 0  = x00 y 00 z 00 W̃ φ̇
(2.23)
B W̃ φ̇ = 
 
0
where B rotates relative to x00 y 00 z 00 expressed in B components. B is the new
orientation of the aircraft and E express the original orientation of the aircraft,
before the orientation. The aircraft follows all three rotations, (2.21) to (2.23),
and gets the angular velocity
W̃ = x0 y 0 z 0 W̃ ψ̇ + x00 y 00 z 00 W̃ θ̇ + B W̃ φ̇ .
(2.24)
To add the components in (2.24) a common coordinate system for the component
contribution needs to be chosen. With B as the common coordinate system equations (2.9) to (2.10) can be used to transform equations (2.21) to (2.23) which
gives
W = Rx (φ) Ry (θ) x0 y 0 z 0 W̃ ψ̇ + Rx (φ) x00 y 00 z 00 W̃ θ̇ + B W̃ φ̇ .
(2.25)
A summary of these steps is shown in Figure 2.2. The inverse to W can be calculated as long as θ , ±90◦ .
Rotation 1
ψ
E 0Z
z
EX
x0
Rotation 2
x00
x0
θ
EY
y0
y 00
y0
z0
z 00
Rotation 3
BX
x00
BY
y 00
φ
z 00
BZ
Figure 2.2: The help coordinate systems to transform the angular velocities
from the Earth frame to the Body frame.
2.4
11
Transformation of Forces
2.4
Transformation of Forces
In this section two examples are given of how the rotation matrix R from equation
(2.11) is used.
2.4.1
Transformation of Forces from Earth frame to Body Frame
Figure 2.3 shows an aircraft that flies in the positive E Y -direction in the Earth
fixed coordinate system. Its goal is to go from point A to point B. To obtain this
the gravitational acceleration E ag m/s2 is transformed to the Body fixed coordinate system by using B ag = RE ag . Then the force from the four rotors can be
calculated to get enough lift force E Fl and also a force E Fa in the right direction
towards B. The vector sum of E Fl and E Fa gives the force B F which is the force
needed from the propellers to achieve the right thrust.
2.4.2
Translate Body Fixed Forces to the Earth Frame
The thrust force B F can also be translated to the Earth fixed frame by the equation
EF
= R−1 B F
(2.26)
then the component of the thrust force in E Z-direction gives the lift force of the
aircraft.
Example of Rotational Kinematics in 2D
Above a general rotation matrix R was derived. In this section a rotation in 2D is
given as an example of rotational kinematics. The thrust force B F in Figure 2.3 is
transformed to the Earth fixed coordinate system with


 
0

 0  
 F   F sin(θ) 
(2.27)

EF = 
 E a  =  T

−E Fl
−FT cos(θ)
assuming that the aircraft only moves in the yz-plane. Then the accelerations in
y-direction and z-direction are calculated by
!
FT sin(θ)
E ay m
E ay m
E Fa
=
⇔
= sin(θ) ⇔ θ = arcsin
(2.28a)
E ay =
m
m
FT
FT
E az
=
E ag
− E Fl
=
mE ag − (−FT cos(θ))
m
mE az − mE ag
B FT
m
= cos(θ) ⇔ FT =
⇔
m(az − E ag )
cos(θ)
(2.28b)
12
2
Background Mathematics Material
E Fl
BF
θ
BY
A
θ
E Fa
B
EY
BX
d,
ar
w
E ag
r
Fo
BZ
in
dy
bo
North
EX
ns
se
e
East
EY
EO
Down
EZ
Figure 2.3: A model over the aircraft when moving from point A to point B.
which finally are used to find θ according to




 E ay m 
E ay cos(θ)
⇔
θ = arcsin  m( a − a )  ⇔ sin(θ) =
 E z E g 
E az − E ag
(2.29a)
cos(θ)
sin (θ)
E ay
E ay
=
⇔ tan(θ) =
⇔
cos(θ)
E az − E ag
E az − E ag
!
E ay
arctan
E az − E ag
(2.29b)
(2.29c)
In this 2D-case only one Euler angle is unknown, θ, in the 3D-case all three Euler
angles are unknown and the rotation matrix R is needed to make the transformation possible. See Luukkonen [2012] for more information about transformations
between different coordinate systems.
2.5
Translation from Latitude and Longitude to Earth Frame
13
Table 2.3: Constants used in the transformation from latitudes and longitudes to meters.
Constant Value
Meaning
requator
6 378 137 [m]
Earth radius at the equator
rnorth pole
6 356 752 [m]
Earth radius at the north pole
1
f
[m]
Flattening constant
298.257223563
2.5
Translation from Latitude and Longitude to Earth
Frame
The GPS provides the position in latitude and longitude in the WGS84 datum,
see Stanaway [2007]. WGS84 is a global standard reference coordinate frame
for the Earth that defines a spheroidal surface for altitude and a gravitational
equipotential surface for the sea level. The latitude measures the angle from the
equator to north and south, see Figure 2.5. The north pole lies on the geodetic
latitude of 90◦ north thus all lines of longitude converge at the north pole and at
the south pole. The equator lies on latitude 0◦ and the south pole on −90◦ .
The longitude measures the angle from Greenwich in eastward and westward
direction. It values goes from −180◦ to 180◦ or equivalent from 180◦ west to
180◦ east.
2.5.1
Translation from Latitude to Distance
To translate from latitude to meters an origin is needed, in Figure 2.4 the test area
is displayed and the origin is marked with a red cross. The constants used in the
translation are collected in Table 2.3. At different latitudes a slice of the Earth,
parallel to the equator, will have different radii, see Figure 2.5. At the north and
the south pole the slice will only cut out a dot with zero meters in radius. The
Earth is not a perfect sphere and it is a bit flattened. From the centre of Earth to
the equator the mean radius is 6 378 137 m and from the centre to the north pole
the radius is 6 356 752 m. The angle between the current latitude φbody and the
latitude of the above chosen origin φorigin is labelled ∆φ,
∆φ = φbody − φorigin .
(2.30)
The tests were performed in the area nearby latitude Φ origin = 58.3934◦ and
longitude Λorigin = 15.5617◦ which expressed in radians are φorigin = 1.0192 rad
and λorigin = 0.2716 rad.
The distance in meter between the aircraft’s position and the origin, in the north-
14
2
Background Mathematics Material
Figure 2.4: The test area is displayed above. Origin for the transformation from latitudes/longitudes to meters is located at the edge on the wall,
marked with a red cross.
N orthpole
90◦
E Xx
f
= requator 1 − φorigin π/2 ∆φ
r
EZ
∆φ
φorigin
a
0◦
Equator
Figure 2.5: The blue dotted circle is on latitude 58◦ . The distance x in E Xdirection is given above.
2.5
15
Translation from Latitude and Longitude to Earth Frame
ward direction, is
x = requator
|
!
f
1 − φorigin
∆φ
π/2
{z
}
(2.31)
r
where the flatting constant f is used to account for the Earth’s different radii
at the equator and at the north pole. Furthermore, r gives the Earth’s radius at
latitude φorigin and ∆φ is the difference in latitude between the origin and the
aircraft. The distance in meters, following the circle path between the two points,
is then the product between the Earth radius at the current latitude and the angle
difference.
EY
∆λ
N orthpole
r3
λorigin
y
Greenwich
Figure 2.6: The Earth seen from above the north pole.
2.5.2
Conversion from Longitude to Distance
Two different methods are used to calculate the orthogonal radius from the rotational axis of the Earth to the origin. The first one uses cosine and the radius at
the equator,
r1 = cos φorigin · requator
(2.32)
whereas the second one uses sine and the radius at the north pole,
π
r2 = sin
− φorigin · rnorth pole
2
(2.33)
16
2
Background Mathematics Material
to calculate the radius r from the centre of Earth to the origin. The resulting radii
from (2.32) and (2.33) are weighted together. The weight for the equator radius
was set to
90 − Φ origin
wa =
(2.34)
90
and the weight for the north pole radius was set to
wb =
Φ origin
(2.35)
90
With the above mentioned origin r2 get higher weight because that radius is calculated with the help of the radius at the north pole, which is closer to the above
mentioned origin than the equator.
In Figure 2.6 an outline of Earth seen from above the the north pole is shown.
The radius r3 is displayed where
r3 = wa · r1 + wb · r2
(2.36)
and the difference in angle is calculated according to
∆λ = λbody − λorigin
(2.37)
Finally the distance between the longitude of the origin and the position of the
aircraft is calculated as
y = r3 · ∆λ
(2.38)
which gives how far away the aircraft is in the eastward direction from the origin,
compare with Figure 2.6.
With (2.31) and (2.38) the latitude and longitude values can be translated to meters.
3
Models
This chapter describes the dynamic model of the aircraft that has been used during the project.
3.1
Quadrocopter Model
In Figure 3.1 the forces from the propellers and the gravitational force are shown.
The gravitational acceleration E ag is expressed in the Earth fixed coordinate system and the force B Ftot from the propellers are expressed in the body fixed coordinate system. To understand how the gravitational acceleration acts on the body,
in the Body fixed frame, it needs to be transformed into the body fixed coordinate
system. This is done by using rotational kinematics from Section 2.3.1. The Euler
Angles are represented by
 
φ
 θ 
(3.1)
Eη = 
 
ψ
where roll φ, pitch θ and yaw ψ are the rotations around the B X-, B Y - B Z-axis
respectively. In a +-configured model forward is along one of the booms which is
shown in Figure 3.1.
The thrust produced by the propeller is proportional to the square of the angular
velocity. The revolutions result in a particular torque with respect to the propeller
dimension and shape. The motors are voltage controlled and the control system
use this to control the force from the rotors. More expensive electronic speed
controllers (ESC) are controlled with respect to the angular velocity.
A model for the quadrocopter and the direct current motor is presented below.
17
18
3
B F4
B F1
Models
rd
wa
For
Roll, φ
BX
B F3
EO
BO
North
EX
East
EY
Pitch, θ
B F2
Y
B
Yaw, ψ
BZ
Body fixed coordinate system
E ag
Down
EZ
Earth fixed coordinate system
Figure 3.1: Earth fixed coordinate system according to the North, East,
Down-model. In the Body fixed coordinate system roll (φ) is around the
B X-axis, pitch (θ) around the B Y -axis and yaw (ψ) around the B Z-axis.
The forces from the four rotors are summed together,
= B F1 + B F2 + B F3 + B F4
B Ftot
(3.2)
where B Ftot is the total force from the four rotors and B F1...4 are the individual
forces from each rotor. The angular acceleration calculated around the B X-axis is
B ω̇x
=
(B F4 − B F2 )l
Ix
(3.3a)
B ω̇y
=
(B F1 − B F3 )l
Iy
(3.3b)
and around the B Y -axis
and finally around the B Z-axis
B ω̇z
=
B L2
+ B L4 − B L1 − B L3
Iz
(3.3c)
where l is the distance from the centre to the motor axis. The parameters B ω̇x,y,z
has the SI-units rad/s2 , Ix,y,z has the SI-units kgm2 and B L1...4 has the SI-units
Nm, see Table 3.1. Each of the propellers gives a thrust force upward
B Fn
2
= kt B ωrotor
(3.4)
2
= kd B ωrotor
(3.5)
and a torque around the motor axis
B Ln
3.1
19
Quadrocopter Model
Table 3.1: Variables used in the dynamic model for the aircraft.
Variable SI-unit
Description
kgm2
Angular momentum
B τ Mi
h rad i
2
kgm
Moment of inertia around one rotor axis
BI M
kgm2
b
Drag constant
h radi
rad
Angular velocity
Bω
s i
h kgm
k
Lift constant
2
i
h rad
kgm
Lift force for motor i
B fi
s2
l
Length from the centre of the aircraft to the rotor axis
[m]
where B ωrotor stands for the angular velocity around the rotor axis for each motor
n. Both (3.4) and (3.5) are proportional to the angular velocity squared. The
parameter kt has the SI-unit kgm and kd has the SI-unit kgm2 . For an electric
motor, the two differential equations
V = I R + ke B ω + B L
dI
dt
M B ω̇z = kT I − v B ω − τ
(3.6)
(3.7)
are useful. In (3.6) V stands for the voltage applied to the motor, I the current,
R the resistance and L for the inductance of the motor. In (3.7) M stands for
the rotor’s moment of inertia and τ is the torque applied to the rotor. The constants ke , kT and v are the motor’s back electro magnetic force constant, torque
constant and viscous friction constant, respectively. Usually the dynamics depending on the inductance of the motor are fast enough to be neglected, but the
motors still have a certain time constant because of the motor and propeller inertia, see Movellan [2010].
The inertia of the aircraft is assumed to be symmetric, the same assumption as in
Luukkonen [2012]. That means that Ixx is equal to Iyy and that the inertia matrix
is


0 
Ixx 0
 0 I
0  ,
(3.8)

yy
BI = 


0
0 Izz
which is a diagonal matrix. The angular momentum B τ Mi around the rotor axis
is given by
B τ Mi
= b B ωi2 + B I M B ω̇i
(3.9)
where the used variables are explained in Table 3.1.
The angular momentum around each axis in the body is labelled τφ , τθ and τψ
20
3
Models
and is given by

 2
2
  lk −B ω2 + B ω4 

τφ  
 τ  lk −B ω12 + B ω32 

Bτ = 
 θ  = 
4

P

τψ

τ
 −
B Mi
(3.10)
i=1
where k is the lift constant.
3.2
Motion Path - A Smooth Trajectory in 3D
Here, the term motion path is used to describe a path through 3D space that
defines the motion of the aircraft. It also has a time stamp related to different
sections of the path. This means that at any given time, the system shall be at a
certain point along the path. One way to get a 3D-path is to take an earlier flight.
In Figure 3.2 such a path is displayed. But this approach lacks the possibility
to evaluate it before flight. Smooth motion paths can also be defined using for
example 3D Bezier curves or splines which are time coded, see Lyche and Morken
[2008].
A fundamental problem is how the path should be defined and what properties
it should have to make it suitable as a camera trajectory. Desirable properties for
a motion path are:
• Possibility to define smooth paths of any shape
• Ability to control the velocity and/or flight time along the path
• Intuitive for an operator to define and modify the path
• There should be an efficient way of calculating the closest point on the path
from the current position and to calculate the tangent vector at any point
on the path
A smooth path in this sense is a path that does not contain any sudden changes
in direction or velocity.
There are several ways to define a path through the air:
• Discrete waypoints
• Straight lines connected by circle segments of constant radii
• Spline curves
Using discrete waypoints provides a simple and intuitive way of defining the
path of the aircraft. Straight lines connected by circle segments provide more
flexibility than discrete waypoints as smooth turns can be defined. The first two
approaches are not good enough because discrete waypoints results in a discontinuous motion and circle segments with constant radii limits the possibilities in
making the path. Spline curves are defined by polynomial interpolation between
3.2
21
Motion Path - A Smooth Trajectory in 3D
−45
−50
North [m]
−55
−60
−65
−70
−50
−80
20
Down [m]
−75
25
30
35
40
East [m]
45
50
55
(a) An example of a path that would
be possible to fly after.
−60
77
76.5
76
25
30
−70
35
40
45
50
North [m]
East [m]
(b) Same path but seen from the
side. The path does not cross itself,
although it is difficult to see that from
this view.
Figure 3.2: An earlier flight log can be used as parameters for an autonomous flight.
a set of control points. Curves of any shape can be constructed by using spline
curves.
3.2.1
Properties of Spline Curves
A spline curve is a polynomial interpolation of a set of control points. Splines can
be defined as linear splines, quadratic splines, cubic splines and so forth. The
simplest case, linear splines, connect points with straight lines, like waypoints.
A quadratic spline connects the control points with polynomials of degree two,
and have a continous first derivative. In the same way a cubic spline have both
continuous first and second derivatives. In Figure 3.3 and Figure 3.4 a quadratic
and cubic spline, respectively, are shown.
3.2.2
Definition of Spline Curves
Splines are defined by interpolation between a set of control points. The simplest
form of spline is the linear spline, which connect a set of control points by straight
lines. A point p on a linear spline segment is defined by
t −t
t − t1
c1 +
c
(3.11)
p (t|c1 , c2 ; t1 , t2 ) = 2
t2 − t1
t2 − t1 2
where ci are the control points, ti are the knots, and t ∈ [t1 , t2 ]. The knots ti give
the value of the parameter t at the corresponding control points. Equation (3.11)
computes a point on the line connecting c1 and c2 . The computation is called a
convex combination of points c1 and c2 . Convex combinations have the property
of being insensitive to round off errors in computers, see page 3 in Lyche and
22
3
2
C2
Models
Control points
Interpolated points
Spline
1.5
P2
1
y
P
P1
0.5
C3
0
C1
0
0.5
1
1.5
2
x
Figure 3.3: A non-uniform quadratic spline in two dimensions with control
points (0, 0), (1, 2), (2, 0.5) and knots (0, 0, 1, 1). The points P 1, P 2 and P are
evaluated for t = 0.4.
Morken [2008].
Quadratic Spline Curves
To compose a quadratic spline curve three control points c1 , c2 and c3 are needed
together with four knots t1 , t2 , t3 and t4 where t1 6 t2 6 t3 6 t4 . A quadratic
spline can be constructed by forming a convex combination of two linear splines
according to
t −t
p (t|c1 , c2 ; t1 , t3 )
p (t|c1 , c2 , c3 ; t1 , t2 , t3 , t4 ) = 3
t3 − t2
t − t2
+
p (t|c2 , c3 ; t2 , t4 )
(3.12)
t3 − t2
where t ∈ [t2 , t3 ). The line connecting c1 and c2 is denoted p (t|c1 , c2 ; t1 , t3 ) and
the line connecting c2 and c3 is denoted p (t|c2 , c3 ; t2 , t4 ).
In the case with n control points (ci )ni=1 and n + 1 knots (ti )n+2
i=2 , where the knots
are nondecreasing and the two first and two last knots are t2 = t3 and tn+1 = tn+2 ,
3.2
23
Motion Path - A Smooth Trajectory in 3D
2.5
Control points
Interpolated points
Spline
2
C2
C3
P2
P5
1.5
P
y
P4
P3
1
P1
0.5
0
C1
0
C4
1
2
x
3
4
Figure 3.4: A non-uniform cubic spline in two dimensions with control
points (0, 0), (1, 2), (2, 2), (3, 0) and knots (0, 0, 0, 0, 1, 1, 1).
a quadratic spline curve f (t) can be denoted


pd+1,d (t)
t ∈ [td+1 , td+2 ]




(t)
p
t ∈ [td+2 , td+3 ]

 d+2,d
f (t) = 
.
.
 .
..


.


 p (t)
t ∈ [tn tn+1 ]
n,d
(3.13)
Another way to express f (t) is with a sum of all segments,
f (t) =
n
X
pi,d (t) Bi,0 (t)
(3.14)
i=d+1
where Bi,0 (t) are piecewise constant functions defined by
(
1,
ti ≤ t ≤ ti+1
Bi,0 (t) =
.
0,
otherwise
(3.15)
The difference between two adjacent values in the knot vector can be seen as the
time difference between two control points, although the spline generally does
not pass through the control points.
Cubic Splines
The calculation of points on linear and quadratic splines are easily generalized
to higher polynomial degrees by using more averages. Calculating a point on an
arbitrary spline segment of polynomial degree d requires n = d + 1 control points
24
3
Models
c1 , c2 . . . cd , cd+1 and 2d knots (ti )2d
i=1 . A spline segment of degree d is then given
as the average of two spline segments of degree d − 1
p t|ci−d , ci−(d−1) , . . . , ci ; ti−(d−1) , . . . , ti , ti+1 . . . ti+(d−1) , ti+d
t −t = i+1
p t|ci−d , ci−(d−1) , . . . , ci−1 ; ti−(d−1) , . . . ti+(d−2) , ti+(d−1)
ti+1 − ti
t − ti
+
p t|ci−(d−1) , ci−(d−2) , . . . , ci ; ti−(d−2) , . . . , ti , ti+(d−1) , ti+d (3.16)
ti+1 − ti
where i starts at d + 1 for the first segment of the spline curve. The spline curve
of degree d and with n control points (ci )ni=1 and 2d knots (ti )2d
i=2 is given by
(3.13). The spline segment in equation (3.16) can be shortened to psi,d where
i stands for the ingoing control points and knots, d stands for the polynomial
degree and s gives the gap between the knots in the computation of the weight
(t − ti ) / (ti+s − ti ) which is explained at page 24 in Lyche and Morken [2008].
A cubic spline segment requires four control points (n = 4) c1 , c2 , c3 and c4 and
six knots (ti )n+2
i=2 . The cubic spline segment is given by
p (t|ci−3 , ci−2 , ci−1 , ci ; ti−2 , ti−1 , ti , ti+1 , ti+2 , ti+3 )
t −t
p (t|ci−3 , ci−2 , ci−1 ; ti−2 , ti−1 , ti+1 , ti+2 )
= i+1
ti+1 − ti
t − ti
+
p (t|ci−2 , ci−1 , ci ; ti−1 , ti , ti+2 , ti+3 )
ti+1 − ti
(3.17)
which is a combination of quadratic splines.
Matrix Representation of Cubic Spline
In order to simplify the evaluation of points on a spline and enable simple calculation of the derivatives of the spline curve the function can be represented on
matrix form. As shown in Lyche and Morken [2008][p. 46], a cubic spline can be
written on the form
h
iT
f (t) = R1 (t)R2 (t)R3 (t) cµ−3 cµ−2 cµ−1 cµ ,
(3.18)
where ci = µ − 3, ..., µ are the control points that are active on segment µ and Ri
are defined by the knots t i according to
t−tµ
tµ+1 −t
R1 (t) = tµ+1 −tµ tµ+1 −tµ = A1 (T 1 t + B 1 ),
(3.19)
 tµ+1 −t

t−tµ−1
 t −t
0 
 µ+1 µ−1 tµ+1 −tµ−1
 = A2 (T 2 t + B 2 ),
R2 (t) = 
(3.20)
tµ+2 −t
t−tµ−1 

 0
tµ+2 −tµ
tµ+2 −tµ
 tµ+1 −t

t−tµ−2
 t −t

0
0
t
−t
µ+1 µ−2

 µ+1 µ−2
t
−t
t−t
µ+2
µ−1


0
R3 (t) =  0
 = A3 (T 3 t + B 3 ). (3.21)
tµ+2 −tµ−1
tµ+2 −tµ−1


tµ+3 −t
t−tµ 


0
0
tµ+3 −tµ
tµ+3 −tµ
3.2
25
Motion Path - A Smooth Trajectory in 3D
Ri (t) can be rewritten as the matrix product
Ri = Ai (T i t + B i )
(3.22)
where
1
tµ+1 − tµ


1
0 
 tµ+1 −t
µ−1

= 

1
0
tµ+2 −tµ

1
0
 tµ+1 −t
µ−2

1
=  0
tµ+2 −tµ−1

 0
0
h
i
= tµ+1 −tµ
#
"
tµ+1 −tµ−1
0
=
0
tµ+2 tµ−1

0
tµ+1 −tµ−2

tµ+2 −tµ−1
=  0
 0
0
tµ+3
h
i
= −1 1
#
"
−1 1 0
=
0 −1 1


0 0
−1 1


=  0 −1 1 0


0
0 −1 1
A1 =
(3.23a)
A2
(3.23b)
A3
B1
B2
B3
T1
T2
T3
0
0
1
tµ+3 −tµ







(3.23c)
(3.23d)
(3.23e)

0 

0 
t 
(3.23f)
µ
(3.23g)
(3.23h)
(3.23i)
Using the relationship (3.22), The multiplication of the R-matrices can be written
as
R1 (t)R2 (t)R3 (t) = (A1 (T 1 t + B 1 ))(A2 (T 2 t + B 2 ))(A3 (T 3 t + B 3 ))
= (A1 T 1 t + A1 B 1 )(A2 T 2 t + A2 B 2 )(A3 T 3 t + A3 B 3 )
= t 3 A1 T 1 A2 T 2 A3 T 3
+t 2 (A1 T 1 A2 T 2 A3 B 3 + A1 T 1 A2 B 2 A3 T 3 + A1 B 1 A2 T 2 A3 T 3 )
+t(A1 T 1 A2 B 2 A3 B 3 + A1 B 1 A2 T 2 A3 B 3 + A1 B 1 A2 B 2 A3 T 3 )
+(A1 B 1 A2 B 2 A3 B 3 )
h
i
= t 3 t 2 t 1 R̃
(3.24)
where


A1 T 1 A2 T 2 A3 T 3


A T A T A B + A T A B A T + A B A T A T 
 1 1 2 2 3 3
1 1 2 2 3 3
1 1 2 2 3 3

R̃ = 
 A1 T 1 A2 B 2 A3 B 3 + A1 B 1 A2 T 2 A3 B 3 + A1 B 1 A2 B 2 A3 T 3 


A1 B 1 A2 B 2 A3 B 3
(3.25a)
26
3
Models
This allows to separate the parameter t from the matrix R̃ (called the blend matrix) that depends only on the knots tµ−2 . . . tµ+3 that are active on segment µ.
By doing this, the blend matrix can be pre-calculated for every segment and the
function value at t can be efficiently calculated as
h
i h
iT
f (t) = t 3 t 2 t 1 R̃ cµ−3 cµ−2 cµ−1 cµ
(3.26)
This also makes it easy to calculate the first and second derivatives (velocity and
acceleration) at any point on the curve as
h
i h
iT
f 0 (t) = 3t 2 2t 1 0 R̃ cµ−3 cµ−2 cµ−1 cµ
(3.27)
and
h
f 00 (t) = 6t
2
0
i h
0 R̃ cµ−3
cµ−2
cµ−1
cµ
iT
(3.28)
respectively by simply differentiating f (t) with respect to t.
Continuous Derivatives
3.1 Theorem. Suppose that the number ti+1 occurs m times among the knots
(tj )m+d
j=i−d with m some integer bounded by 1 ≤ m ≤ d + 1, that is
ti ≤ ti+1 = · · · = ti+m ≤ ti+m+1
(3.29)
Then the spline function f (t) = pi,d,1 (t) Bi,0 (t) + pi+m,d,1 (t) Bi+m,0 (t) has continuous derivatives up to order d − m at the joint ti+1 .
For a more detailed derivation of spline curves see page 27 in Lyche and Morken
[2008] where this theorem is taken from.
3.2.3
Finding the Closest Point on A Spline Curve
The latest known visited position at the curve gives an allowed section of the
curve that it is okay to control towards. Together with the wish to follow the path
as good as possible it would be sound to control towards the nearest point on the
curve but also weighting in to control towards point p(t).
In Wang et al. [2003] the problem of finding the closest point on a spline curve
is treated. They suggest a combination of Quadratic Minimization Method and
Newton’s Method, see Algorithm 1 and Algorithm 2, respectively. The strength
in Wang et al. [2003] is that they use the result of the Quadratic Minimization
Method as an initial guess for Newton’s Method. The problem is not convex and
there are often local minima. Due to the local minima it is important that the
initial guesses are in the right region of the spline curve, otherwise the Quadratic
Minimization Method will find a point that is suboptimal. After four iterations
with Quadratic Minimization Method the result is often very near the optimal
solution but continue searching after the optimal solution with Quadratic Minimization Method will converge at super linear rate. Super linear rate means
roughly that it often takes very few iterations before the difference is little enough.
On the other hand Newton’s Method converges quadratically, which is faster than
3.2
Motion Path - A Smooth Trajectory in 3D
27
super linear, but can get problems in the beginning if the initial guess is poor.
Hence it is a good idea to combine these two methods and utilize the advantages
of them.
To find the point on the curve that lies nearest the aircraft the square distance
between the aircraft’s position pa and an optional point along the spline curve,
p(t) is calculated. The squared distance is
D(t) = (E x(t) − E xa )2 + (E y(t) − E ya )2 + (E z(t) − E za )2
(3.30)
t?
The goal is to find the value
that minimizes D(t) and hence gives the nearest
h
iT
?
?
?
point p(t ) = E x(t ), E y(t ), E z(t ? ) on the curve.
Algorithm 1 Quadratic Minimization Method
1. Initial estimates, guess three values of t ?
t̃1 , t̃2 and t̃3
(3.31)
2. Calculate a fourth t value
1 q23 D(t̃1 ) + q31 D(t̃2 ) + q12 D(t̃3 )
, k = 1, 2, 3, . . .
2 t23 D(t̃1 ) + t31 D(t̃2 ) + t12 D(t̃3 )
tij = t̃i − t̃j , i, j ∈ {1, 2, 3}
t ?,k =
qij =
t̃i2
−
t̃j2 ,
i, j ∈ {1, 2, 3}
(3.32a)
(3.32b)
(3.32c)
3. Calculate a quadratic polynomial P (t) that interpolates D(t) at t̃1 , t̃2 and t̃3
and then minimize it
P (t) =
(t − t̃2 ) (t − t̃3 )
D(t̃1 )
(t̃1 − t̃2 ) (t̃1 − t̃3 )
(t − t̃1 ) (t − t̃3 )
D(t̃2 )
+
(t̃2 − t̃1 ) (t̃2 − t̃3 )
(t − t̃1 ) (t − t̃2 )
+
D(t̃3 )
(t̃3 − t̃1 ) (t̃3 − t̃2 )
(3.33)
4. Pick the three values from t̃1 , t̃2 , t̃3 and t ?,k that gives the smallest value of
P (t) and restart at 3 with these estimates and repeat until the difference
between the current calculated P (t) and the one before is small enough.
The three initial estimates of t ? in step 1 in Algorithm 1 can be initialized in
different ways. One choice is to use the previous visited point on the curve, the
point that the aircraft should be at according to its velocity and the point it should
be at according to the time. Another way is to utilize that the spline curve is
divided into segments and the three initial guesses can be chosen as ti , ti +t2i+1
28
3
Models
1.6
1.4
1.2
y
1
0.8
0.6
P1
P5
0.4
P2
P3 P4
Spline Curve
Aircraft
Closest Point
Iterations
0.2
0
1.2
1.4
1.6
1.8
2
x
2.2
2.4
2.6
Figure 3.5: A spline curve and the iterations of the Quadratic Minimization
Method. In this case nine iterations were needed (only the first five and the
last one is shown) to find the closest point on the curve. The start guesses of
t were 0, 0.5 and 3.75.
1
0.8
0.6
0.4
0.2
P3
P5
P4
03
2
3
P1
2
1
P2
00
1
Figure 3.6: Motion path expressed as a cubic spline.
3.3
29
AHRS Algoritm
and ti+1 where i denotes the last segment the aircraft was in and i + 1 the next
segment that the aircraft will enter.
Figure 3.5 shows a graph over a motion path in the E X E Y -plane and the functionality of the Quadratic Minimization Method. The iterations are marked with
red stars, the closest point is marked with a circle and the aircraft’s position is
marked with a plus sign. The Quadratic Minimization Method was used to find
the closest point on the curve. During this test the t-values were chosen in two
different segments of the curve, 0, 0.5 and 3.75. This choice of t-values was made
to show the functionality of the algorithm and it took nine iterations to find the
closest point on the curve, with respect to the allowed error. With t-values chosen with less difference the iterations were fewer and often the iterated points
ended very near each other which resulted in a poor plot for the purpose of illustrating the principal of the algorithm. In Figure 3.6 a graph over a motion path
Algorithm 2 Newton’s Method
1. Initial estimate
2. Iteration
t ?,m+1 = t ?,m −
t ?,0
D 0 t ?,m
D 00 (t ?,m )
(3.34a)
, m = 0, 1, 2, . . .
(3.35a)
created with a cubic spline is displayed. The closest point to the aircraft and the
iterations toward it are also displayed. Newton’s Method uses the tangent of the
curve to find a better value. In Algorithm 2, the notation D 0 and D 00 stand for the
derivative and second derivative, respectively, of the squared distance D in (3.30)
between the aircraft and a point on the curve.
3.3
AHRS Algoritm
AHRS stands for Attitude and Heading Reference System and provides an orientation of the aircraft relative to the direction of gravity and the Earth’s magnetic field. In the orientation filter developed by Madgwick [2010] the tri-axis
gyroscopes and accelerometers in the IMU are used together with the tri-axis
magnetometer to calculate the orientation of the aircraft. This so called MARG
implementation makes it possible to compensate for magnetic distortion and gyroscopic bias drift. MARG stands for Magnetic, Angular Rate and Gravity and is
a combination of an IMU and a tri-axis magnetometer. MARG is thus the same
as AHRS.
The orientation filter uses a quaternion representation to avoid the risk of gimbal
lock, and uses an optimized gradient-descent algorithm to compute the direction of the measurement error in the gyroscope data. Madgwick’s orientation
filter is computationally cheap and effective at low rates down to 10 Hz. Algorithm 3 gives a brief overview of the algorithm, a thorough derivation can be
found in Madgwick [2010].
30
3
Models
Algorithm 3 AHRS Filter
1. Integrate angular velocity measurements
If a measurement of the angular velocity in the body frame is expressed on
the form
h
i
s ω = 0 ωx ωy ωz ,
(3.36)
the quaternion derivative that represents the same rotation can be calculated as
1
q̇ω,k = q̂k−1 ⊗ s ω ,
(3.37)
2
where ⊗ denotes quaternion multiplication and q̂k−1 is the previous quaternion estimate. The new estimated quaternion q̂ω,k is then calculated by
integrating according to
q̂ω,k = q̂ω,k−1 + q̇ω,k ∆t,
(3.38)
where ∆t is the sampling time.
2. Calculate correction based on vector measurements
An estimate of the current quaternion can be calculated using vector measurements such as
gravity and
magnetic field according to
f q̂k−1 , E d, B s = q̂∗k−1 ⊗ E d ⊗ q̂k−1 − B s
(3.39)
E
B
∇f q̂k−1 , d, s
q̂∇,k−1 = q̂k−1 − µ
(3.40)
||∇f q̂k−1 , E d, B s ||
where E d is the reference vector in the earth frame and B s is the measurement in the body frame. The function ∇f is the gradient of f with respect
to the quaternion. Equations (3.39) and (3.40) describe a gradient descent
algorithm that uses the previous quaternion estimate as a starting point
and makes one step of length µ in the direction of the gradient.
3. Combine the two estimates The two estimates are then weighed together
to give the final estimate according to
q̂k = γ q̂∇,k + (1 − γ) q̂ω,k , 0 ≤ γ ≤ 1
(3.41)
where γ controls the influence of the vector measurements.
4. Normalize quaternion
Only quaternions of magnitude 1 represent orientations, therefore the
quaternion need to be renormalized every iteration according to
q̂ω,k
.
(3.42)
q̂ω,k =
||q̂ω,k ||
3.3
31
AHRS Algoritm
3.3.1
Pros and Cons with Madgwick’s Orientation Filter
One advantage of Madgwick’s orientation filter is for example a single adjustable
parameter. Other advantages of the orientation filter are:
• Good performance at low sampling rates
• A single adjustable parameter
• Gyroscope bias drift compensation
• The use of quaternions avoid the problems with the singularities associated
with the Euler angle representation
• Simple to implement and tune
The quaternions used in the filter need to be transformed to Euler angle representation to be able to interpret the orientation of the aircraft. This is done by
!
2q2 q3 − 2q1 q4
ψ=arctan2
(3.43)
2q12 + 2q22 − 1
θ = − arcsin (2q2 q4 + 2q1 q3 )
(3.44)
!
2q3 q4 − 2q1 q2
(3.45)
φ=arctan2
2q12 + 2q42 − 1
where
arctan2
y
=Principal value arg (x + iy)
x
(3.46)
which can be written as











y 

arctan2
=

x 









arctan
arctan
y
yx yx x>0
+π
arctan x − π
+ π2
− π2
undefined
and the results are given in the interval (−π, π].
y > 0, x < 0
y
y
y
y
< 0, x < 0
> 0, x = 0
< 0, x = 0
= 0, x = 0
(3.47)
4
Sensors - IMU, Magnetometer,
Barometer and Internal GPS
This chapter describes the different sensors that are used in the flight controller.
Their abilities and limitations are discussed under each section. The aircraft uses
gyroscopes, accelerometers and magnetometers to measure its orientation and
angular velocity relative to the Earth. None of the sensors alone can provide
a complete representation of the attitude of the aircraft as they all suffer from
different limitations.
4.1
IMU
The IMU consists of gyroscopes and accelerometers which are discussed next.
4.1.1
Gyroscopes
The aircraft is equipped with three gyroscopes that each measure the angular velocity along one of the axes in the Body fixed coordinate system. The gyroscope
suffers from two main drawbacks, bias and noise. If it were not for these drawbacks, the measured angular velocities alone could be integrated to provide a
complete representation of the attitude of the aircraft if only the initial orientation would be known.
4.1.2
Accelerometers
The three accelerometers in the IMU are used to measure the direction of the
gravitational acceleration, which is assumed to always point down (along the positive z-axis) in the Earth frame. They are mounted in three mutually orthogonal
directions to be able to measure in three dimensions. One problem with the accelerometers is that they also measure the linear acceleration of the aircraft and
33
34
4
Sensors - IMU, Magnetometer, Barometer and Internal GPS
can therefore not be used alone for attitude determination of a non stationary
system.
4.2
Magnetometer
A three axis magnetometer is used to provide an absolute reference of heading,
something that cannot be achieved with only accelerometers and rate gyroscopes.
The magnetometer measures the magnetic field in the Body fixed frame.
4.2.1
Hard Iron and Soft Iron Distortion
The magnetic measurements are distorted by magnetic objects in the vicinity of
the sensor. The distortion can be divided into two categories, hard iron and soft
iron distortion. Hard iron distortion can be described as an offset that is constant
in the Body frame and not affected by the orientation of the sensor with respect
to the Earth’s magnetic field.
The hard iron offset is caused by permanent magnets or electric currents close to
the sensor. Soft iron distortion is caused by the interaction between the external
magnetic field and magnetic materials in the Body frame. The soft iron offset
is therefore dependent on the orientation of the sensor relative to the external
magnetic field. Hard iron distortion can be compensated for by subtracting the
offset from the measurements directly in the Body frame, while compensating for
soft iron effects is more complicated, see Vasconcelos et al. [2011]
4.2.2
Automatic Hard Iron Calibration Algorithm
The problem of magnetometer bias (hard iron) is addressed by an automatic algorithm that continuously estimates the bias of all three axes during flight. The
algorithm is based on the work by Premerlani [2011].
A measurement of the magnetic field is described by
B b − B b0
= RE b
(4.1)
where B b is the measurement in the Body frame, B b0 is the measurement bias,
R is the rotation matrix from the Earth frame to the Body frame and E b is the
Earth’s magnetic field. The square of the magnitude of both sides of (4.1) is given
by
|B b − B b0 |2 = |RE b|2
(4.2)
Because taking the squared magnitude of a vector is equivalent to taking the dot
product of the vector with itself and because R is a rotation matrix and thus
preserves vector magnitudes, (4.2) can be rewritten as
(B b − B b0 ) · (B b − B b0 ) = |E b|2 ,
(4.3)
where the left side expands to
|B b|2 − 2B b · B b0 + |B b0 |2 = |E b|2 .
(4.4)
4.2
35
Magnetometer
Taking two different measurements, labeled E b1 and E b2 , inserting them in (4.4)
and subtracting two instances of the equation gives
|B b1 |2 − |B b2 |2 − 2(B b1 − B b2 ) · B b0 = 0,
(4.5)
which defines a plane in three dimensions with the vector B b1 − B b2 as the normal.
The unknown bias B b0 cannot be explicitly calculated from (4.5), but we know
that it is located somewhere in the plane. A reasonable approach to estimate the
bias vector is to take a small step in the direction of the normal towards the origin
every iteration. If the length of the step is scaled by the difference in length of the
two measurements and a configurable gain parameter the bias can be estimated
by the recursive formula
B b̂0,k
= B b̂0,k−1 + KM
B b1 − B b2
(|B b1 | − |B b2 |) ,
|B b1 − B b2 |
(4.6)
where B b̂0,k is the new estimate, B b̂0,k−1 is the previous estimate and KM is the
gain of the algorithm. The factor (|B b1 | − |B b2 |) scales the step size so that it decreases as the bias converges. The estimated biases are then subtracted from every
measurement. Tuning the gain is a trade off between the time it takes to converge
to the correct bias estimate and sensitivity to noise. The risk of division by zero
is handled by checking every iteration that the difference of the vectors and only
running the algorithm if it is larger than a certain threshold. This also reduces
the sensitivity to noise by only updating the bias if a large enough rotation has
occurred between the iterations.
Part of the magnetometer offset is caused by magnetic fields induced by currents
flowing to the motors of the aircraft. Because of this, the offset has been discovered to differ significantly when the motors are on compared to when they are off.
This problem was solved by saving two sets of biases, one for when the motors are
off and one for when the motors are on. The ARHS filter uses the correctly compensated magnetometer measurements to converge to the correct heading while
the aircraft is still on the ground with motors off, and when the motors are turned
on the correct offset for that case is used. In Figure 4.2 the magnetometer’s sensitivity noise level at two different motor speeds is tested. Low speed resulted in
less noise and high speed resulted in more noise.
Figure 4.3 shows the estimated bias during a test flight when the initial value
of the bias was set to zero. Figure 4.1 shows raw and calibrated magnetometer
data from a test flight where the aircraft was tilted as much as possible in the
roll and pitch axes and rotated a few turns around the yaw axis. This provides
more information to the calibration algorithm that estimates the bias because the
change of the measured magnetic field is bigger between the samples when the
magnetic field is measured from many different orientations. In Figure 4.4 the
same data as in Figure 4.1 is plotted versus time.
36
4
Sensors - IMU, Magnetometer, Barometer and Internal GPS
(a) Raw magnetometer data.
(b) Calibrated magnetometer data.
(c) Raw magnetometer data, only xand y-components.
(d) Calibrated magnetometer data,
only x- and y-components.
Figure 4.1: Raw and calibrated magnetometer data.
4.3
37
Barometer
0.6
Magnetic Field [G]
0.4
0.2
0
−0.2
−0.4
BX
BY
BZ
20
25
30
Time [s]
35
40
Figure 4.2: The magnetometer noise at low and at high motor speed, respectively.
4.3
Barometer
The barometer that is used is BMP085 which is a digital pressure sensor made by
Bosch. Its pressure range is 3000 − 110000 Pa and its altitude range is +9000 m −500 m above sea level, see Sensortec.
Figure 4.5 shows an approximately 13 min long test where the air pressure, sensor temperature and the sensor’s height above its start position were measured.
The test was performed indoor and the sensor was fixed at a constant altitude
during the entire test. Ideally the altitude should have been 0 m during the entire test. The temperature increased slightly during the test due to the heat from
the electronics in the confined space of the flight controller. The red line in the
middle of the measured altitude data in the third subplot shows the result of the
altitude after that a first order filter has worked on the raw data. The first order
filter looks as
E An
= α E an + (1 − α) E An−1
(4.7)
where n is the actual sample and n − 1 the preceding sample. The unfiltred altitude is labelled an and the filtred altitude is labelled An . The parameter α is used
to tune the filter, in this case α was choosed to 0.35. On page 403 in Söderkvist
[2007] there is an introduction to low pass filters.
The digital pressure sensor has four different modes; ultra low power, standard,
38
4
0.3
BX
BY
BZ
0.25
Estimated Magnetometer bias [G]
Sensors - IMU, Magnetometer, Barometer and Internal GPS
0.2
0.15
0.1
0.05
0
−0.05
0
50
100
150
200
250
Time [s]
Figure 4.3: Magnetometer bias converging during a test flight with a calibration algorithm gain of 0.02.
high resolution and ultra high resolution. To decide which mode gives the best
estimated altitude the four different modes were tested. The test was performed
indoor under similar conditions. In the performed test the sensor stands still on
a table during the entire test. Only the first 60 s after the altitude was calculated
for the first time was used to compare the different modes. It is important to
isolate every factor that can affect the result to be able to compare the different
tests. One factor is how long time a test proceeded - a longer test often leads to a
bigger standard deviation caused by the natural drift in the sensor and the change
of the environment over time. The temperature changed quite fast because the
sensor got hot. The standard deviation was used to compare the four different
modes. The results were quite similar for the four different modes but according
to Table 4.1 the mode with 2 internal samples was best when consideration was
taken both to the time it took to update the measured signal and to the standard
deviation of the altitude. The standard deviation for altitude did not change
much with more internal samples. In Figure 4.6 the four tests are displayed.
The standard deviation for the two measured values pressure and temperature
and for the calculated value altitude for the four different modes of the digital
pressure sensor are shown in Table 4.1. A low σ is better in this case, that would
mean that few values are far away from the mean value, which should be zero in
this case because the aircraft was fixed to a constant altitude. The low pass filter
variable α was here chosen as α = 0.35 to catch the fast changes and be able to
follow these with a control signal.
4.3
Barometer
39
Table 4.1: Standard deviation for pressure, temperature and altitude calculated for the four different modes that the barometer have. According to the
data sheet, see the BMP085 data sheet by Sensortec, a low number of internal
samples results in lower power consumption and a high number of internal
samples results in high resolution.
Internal number
Standard deviation
σT [◦ C]
σA [m]
of samples
σP [Pa]
1
6.3598
3.4932
0.4869
2
4.2577
0.3306
0.3836
4
4.2746
0.2536
0.3867
8
4.0348
0.2609
0.3658
Table 4.2: Variables and constants used in the barometric formula (4.8)
Variable Value
SI-unit
Description
P
−
Pressure at actual time and position
[Pa]
Pb
−
Pressure at start position
[Pa]
h
i
g0
9.80665
m/s2
Gravitational acceleration
?
R
8.31432
[Nm/molK] Universal gas constant for air
h
−
Height above start position
[m]
h0
0
Height at start position
[m]
M
0.0289644
Mole mass for air
[kg/mol]
Tb
293.15
Temperature at start position
[K]
40
4
1
BX
BY
BZ
0.8
Estimated Magnetometer bias [G]
Sensors - IMU, Magnetometer, Barometer and Internal GPS
0.6
0.4
0.2
0
−0.2
−0.4
0
20
40
Time [s]
60
80
Figure 4.4: Calibrated magnetometer data.
To calculate the altitude from the measured pressure and temperature the barometric formula was used. The barometric formula is
P = Pb e
−g0 M(h−h0 )
R? Tb
(4.8)
where the used variables are explained in Table 4.2, see Berberan-Santos et al.
[1997]. From (4.8), the height above start position is given by
R? Tb ln PP
b
h = h0 −
(4.9)
g0 M
Figure 4.7 shows a test outdoor where the aircraft initially stands on the field
for approximately the first 25 seconds and then hover to 1 m altitude. After
that the aircraft rises to approximately 10 m altitude quite slowly. Then slowly
down to approximately 1 m altitude. Then a fast climb to 10 m followed by a
fast drop down to 1 m altitude. These rises and drops have been isolated to the
z-direction. The last rise is a fast one which has an inclination of approximately
45◦ against the E X E Y -plane. Although no ground truth data were available the
result in Figure 4.7 is quite satisfying since the altitude measurement caught the
actions described below. During the flight a test protocol was followed which
for each 10 s had an action. The window of 10 s was not quite long enough for
all moments since the time it took to rise and drop was longer than estimated.
During the first 25 seconds the aircraft is in a start-up mode when calibrations
are done. It is important that the aircraft stands still and remains horizontal
4.3
41
Barometer
1.0037
1.0036
1.0035
1.0034
1.0033
0
Altitude [m]
Temperature [C]
Pressure [Pa]
5
x 10
38
36
34
32
30
28
0
100
200
300
400
Time [s]
500
600
700
100
200
300
400
Time [s]
500
600
700
100
200
300
400
Time [s]
500
600
700
1
0
−1
0
Figure 4.5: Air pressure, sensor temperature and the unit’s height above the
start position is shown in the figure. The body was placed indoor on a table
and stood still during the entire test. The low pass filter coefficient α was
chosen as 0.35.
42
4
Sensors - IMU, Magnetometer, Barometer and Internal GPS
1
0
−1
0
10
20
30
40
50
60
10
20
30
40
50
60
10
20
30
40
50
60
10
20
30
Time [s]
40
50
60
Altitude [m]
1
0
−1
0
1
0
−1
0
1
0
−1
0
Figure 4.6: 1, 2, 4 respective 8 internal samples in each altitude calculation.
during this time.
4.4
Internal GPS
The model name of the internal GPS device is LEA-6N, see AG. It is a small
device, only a couple of centimetres in width, and much lighter than the RTKGPS-system from Trimble. It is mounted on the flight controller and has a small
antenna that is mounted on one of the booms. One drawback with the internal
GPS-receiver is its precision which is around 50 centimetres instead of a couple
of centimetres. In Figure 4.8 the internal GPS receiver’s performance is shown.
4.5
Sensors and AHRS Filter Performance
In this section some performance tests of the sensors and AHRS algorithm are
presented.
4.5.1
Yaw Rotation
Figure 4.9 shows a plot over an indoor test. This test especially shows the magnetometers contribution to the estimated orientation of the aircraft. The aircraft
was rotated only around the E Z-axis in order to test the estimated yaw angle. The
test was performed indoors, with the motors turned off, by manually rotating the
4.5
43
Sensors and AHRS Filter Performance
Pressure [Pa]
5
x 10
1.007
1.006
10
20
30
40
Time [s]
50
60
10
20
30
40
Time [s]
50
60
10
20
30
40
Time [s]
50
60
22.6
22.5
22.4
0
Altitude [m]
Temperature [C]
0
10
5
0
0
Figure 4.7: This shows a test flight outdoor where the aircraft was moved
up and down in a specific pattern. The low pass filter variable α was here
chosen as α = 0.35 to catch the fast changes and be able to follow these.
The low pass filter is shown in green and it eliminates fast changes in the
measured altitude.
aircraft in approximately 90◦ increments, first clock-wise and then counter-clockwise back to the original orientation. Figure 4.9 clearly shows how the magnetometer measurements are affected by the rotation.
4.5.2
Roll, Pitch and Yaw Rotation
During this test rotation and estimation of Euler angles around all axes were performed and the result is shown in Figure 4.10. The test was performed indoor,
with the motors turned off, by manually rotating the aircraft 45◦ , in positive direction, around every axes and then back again.
1. The first rotation was performed with a positive 45◦ rotation around the
E Y -axis, that is pitch, and then back to the original position
2. The second rotation was performed with a positive 45◦ rotation around the
E X-axis, that is roll, and then back to the original position
3. The third rotation was performed with a positive 45◦ rotation around the
E Z-axis, that is yaw, and then back to the original position
44
4
Sensors - IMU, Magnetometer, Barometer and Internal GPS
−20
−30
North [m]
−40
−50
−60
−70
−80
−90
20
30
40
50
60
East [m]
70
80
90
Figure 4.8: The internal GPS in the aircraft has approximately half metre
accuracy. Here it was moved along the edge of a pathway and should ideally,
especially at the ends, be three overlapping curves instead of three parallel.
In Figure 4.11 the same test as the previous one was performed, but with the
motors turned on. The reason for this is to see how the induced magnetic field in
the wires affect the orientation estimation.
4.5
45
Sensors and AHRS Filter Performance
Angular Rate [◦ /s]
Gyroscope
BX
BY
BZ
100
0
−100
0
20
40
60
80
100
120
140
100
120
140
100
120
140
100
120
140
Acceleration [g]
Accelerometer
BX
BY
BZ
1
0
−1
0
20
40
60
80
Time [s]
(a) Gyroscope and accelerometer measurements.
Magnetic Field B [G]
Magnetometer
BX
BY
BZ
0.2
0
−0.2
0
40
60
80
Orientation
200
Orientation [◦ ]
20
BX
BY
BZ
0
−200
0
20
40
60
80
Time [s]
(b) Magnetometer and estimated orientation with help of the AHRS filter.
Figure 4.9: During this test the aircraft was rotated only around the E Z-axis.
It was rotated 90◦ clockwise to 90◦ and then an additional 90◦ to 180◦ . After
that the aircraft was rotated 90◦ counter clockwise back to 90◦ and then an
additional 90◦ back to 0◦ .
46
4
Sensors - IMU, Magnetometer, Barometer and Internal GPS
Angular Rate [◦ /s]
Gyroscope
BX
BY
BZ
50
0
−50
0
20
40
60
80
60
80
Acceleration [g]
Accelerometer
BX
BY
BZ
1
0
−1
0
20
40
Time [s]
(a) Gyroscope and accelerometer measurements.
Magnetic Field B [G]
Magnetometer
0.5
BX
BY
BZ
0
−0.5
0
20
40
60
80
60
80
Orientation
Orientation [◦ ]
100
BX
BY
BZ
0
−100
0
20
40
Time [s]
(b) Magnetometer and estimated orientation with help of the AHRS filter.
Figure 4.10: During this test the aircraft was rotated around all Body fixed
axes with the motors turned off.
4.5
47
Sensors and AHRS Filter Performance
Angular Rate [◦ /s]
Gyroscope
BX
BY
BZ
50
0
−50
0
20
40
60
80
60
80
Acceleration [g]
Accelerometer
BX
BY
BZ
1
0
−1
0
20
40
Time [s]
(a) Gyroscope and accelerometer measurements.
Magnetic Field B [G]
Magnetometer
0.5
BX
BY
BZ
0
−0.5
0
20
40
60
80
60
80
Orientation
Orientation [◦ ]
100
BX
BY
BZ
0
−100
0
20
40
Time [s]
(b) Magnetometer and estimated orientation with help of the AHRS filter.
Figure 4.11: During this test the aircraft was rotated around all Body fixed
axes with the motors turned on.
5
Evaluation of RTK-GPS System
This chapter deals with Trimble’s RTK-GPS, the tests and evaluation of different
placements of the GPS-receiver and antennas.
5.1
Evaluation of Trimble’s RTK-GPS
During the first two months of the master’s thesis a RTK-GPS system from Trimble was evaluated. The equipment was a base station unit (BX960) and a rover
unit (BD920). One big antenna (Antenna Zephyr Model 2) for the base station
and two smaller antennas (Antenna AV34) for the rover and the heading units
were used.
5.1.1
Experience and Usages of RTK-GPS
This section explains some of the experiences gathered and issues encountered
from the evaluation of the GPS system. Below are some of the lessons learned
during the evaluation of the RTK-GPS:
• The base station should be in an unobstructed environment with a clear
view of the sky in every direction
• The base station antenna should be on a distance from the ground of at least
one to two meters
• The base station unit’s antenna should be located under an open sky. Trees
above and around the antenna will disturb the precision and can cause multipath issues.
Trees or other obstacles above the rover unit antenna can disturb the contact with
the GPS satellites which can lead to lost tracking of one or more satellites. When
49
50
5
Evaluation of RTK-GPS System
−102.4
−102.5
−102.5
−102.6
−102.52
−102.54
−102.8
North [m]
North [m]
−102.7
−102.9
−103
−102.56
−102.58
−103.1
−102.6
−103.2
−102.62
−103.3
−103.4
88.1
88.2
88.3
88.4
East [m]
88.5
88.6
88.7
(a) During the first 30 seconds RTKGPS was turned off.
88.15
88.2
East [m]
88.25
(b) With RTK on the quality is very
high. During a couple of minutes the
drift is less than 10 centimetres, with
every value counted!
Figure 5.1: This first test shows the improvement with RTK compared with
traditional GPS. The GPS-receiver is lying still on the ground during the
entire test that lasted 7 minutes and 34 seconds.
one or more GPS satellites leave the calculation of the position the precision can
be reduced. In some cases the estimated position jumped around a half meter
from one sample to the next. This often happened when the rover antenna was
under trees.
When the base station was close to the trees the tree trunks caused multipath
issues which gave bias to the rover’s RTK position. Bias in the measurement
domain would be difficult to determine in the position domain at the rover when
it is in an open sky environment. Moving the base station antenna away from the
trees reduced the problem of suddenly jumping position estimates.
5.1.2
Tests
Different tests were made to evaluate Trimble’s RTK-GPS-system with respect to
precision, repeatability and performance near buildings and obstacles. In the
figures the time is colour coded with blue corresponding to the beginning of the
test and red to the end.
Still on the Ground
In the first test the base station was placed under an open sky and the rover
unit with the GPS-receiver on it was placed approximately 70 m away. The rover
unit was placed still on one location. In the beginning of the test no RTK was
activated and in Figure 5.1 it is possible to see the GPS drift over time. But after
approximately 30 seconds RTK is enabled and the precision is very high during
the rest of the test which lasted for 7 minutes and 34 seconds.
5.1
51
Evaluation of Trimble’s RTK-GPS
0
−5
−15.06
−10
−15.08
−20
North [m]
North [m]
−15
−25
−15.1
−15.12
−30
−35
−15.14
−40
−45
−10
−5
0
5
10
East [m]
15
20
(a) An overview over the path.
25
−15.16
22.8
22.82
22.84
East [m]
22.86
(b) Zoom in of one of the stand still
spots on the path.
Figure 5.2: During this test the GPS-receiver was moved four laps clockwise
in the displayed path. At two positions along each lap the GPS-receiver stood
still a couple of seconds on exactly the same spot.
Repeatability
In the second test the GPS-receiver was carried several laps along a path. The test
data is shown in Figure 5.2. During each lap around the path the GPS-receiver
was placed at two distinct spots, which was well-marked with two circles with
a diameter of approximately 15 cm. In Figure 5.2b it is possible to see that the
error is less than the marked position where to stop. Our conclusions are that
this is good enough to be flying after.
During the second test, which tested the repeatability, the base station was located under some trees and very near the path so that the antenna was covered
by the trees. In some of the tests this lead to loss of consistency and accuracy.
In Figure 5.3 the discontinuities in the path due to multipath issues are shown.
Trimble explained that the Base station should be under an open sky environment and around 2 meters above the ground to work well. In this test both of
these requirements were unfullfilled. The Base station was placed on a bench
of approximately 0.5 meters height above the ground and surrounded by some
trees.
Near Buildings
In the third test the base station was still placed on the bench under the trees as
above and the rover unit with the GPS-receiver on it was moved along the wall
of a building. The resulting path was not as good as during the repeatability test
above which was mostly under open sky. Figure 5.4 shows that the RTK-GPS
system performs quite good even when it is near high buildings with complex
shapes.
52
5
Evaluation of RTK-GPS System
0
−28
−5
−29
−10
−30
−31
North [m]
North [m]
−15
−20
−25
−32
−33
−34
−30
−35
−35
−36
−40
−37
−45
−10
−5
0
5
10
East [m]
15
20
6
25
(a) During this test the signal was
lost after 1.5 lap and before that the
system got problems when the rover
passed under the trees and near the
Base station.
8
10
East [m]
12
14
(b) Multipath issues under the trees
lead to a discontinuous path which
would have been very difficult to fly
after.
Figure 5.3: This test shows that Trimble’s equipment does not always provide satisfactory results.
−35
−40
−45
North [m]
−50
−55
−60
−65
−70
−75
−80
−50
(a) This map is over the area with the
building. The picture is taken from
Eniro.
−40
−30
−20
East [m]
−10
0
(b) As suspected the estimated path
was not as good as under the open sky
environment. But it was surprising
that the path glitches over the open
sky part of the path, the red part of
the path is between the buildings and
not near them.
Figure 5.4: Trimble’s RTK-GPS system handled the buildings quite well in
this test.
5.2
53
Error Detection
Repeatability Under an Open Sky
During this fourth test the location was totally under an open sky environment
and the path was between two lamp posts and a marking on the ground. The
rover unit was moved between these markings and at each corner of the triangle
it was stopped and kept still for a couple of seconds. The result is impressive, during the five laps all the measurements data, when stopped at the northernmost
lamp post, lies inside a square of 4 · 4 cm2 , see Figure 5.5c. The base station was
still on the bench under the trees but this time it worked well.
5.2
Error Detection
To become more sure of how the aircraft moves in the air and estimate the accuracy of the GPS-data three suggestions of motion models are given. The purpose
of these models are to make the system more robust to incorrect values from
the GPS-system. The first one uses only a maximum velocity limit, the second
one uses a maximum acceleration limit and the third one uses earlier estimated
positions.
5.2.1
Maximum Velocity
The maximum velocity is denoted B vmax and the sample time is denoted ∆t, with
these variables the maximum distance moved during one sample (E smax ) is calculated as
E smax
= |B vmax | · ∆t.
(5.1)
With a maximum velocity of 15 m/s the aircraft can maximally move 0.75 m between each sample when the sample time is 5 ms. This means that if the Euclidean distance between the coordinates of two samples exceeds 0.75 meters the
control system shall be limited to its maximal control output. To calculate the
Euclidean distance between two points in space the latitude value and longitude
value given from the GPS-system is transformed into an Earth fixed coordinate
system with help of (2.31) and (2.38). Then the Euclidean distance between the
actual location denoted


E xτ 
 y 
(5.2)
E ξτ = 
E τ 
z
E τ
and the one before, denoted E ξ τ−1 , is calculated according to
∆E ξ = kE ξ τ − E ξ τ−1 k
q
= (E xτ − E xτ−1 )2 + (E yτ − E yτ−1 )2 + (E zτ − E zτ−1 )2
(5.3a)
Equation (5.3a) makes it possible to determine if the actual RTK-GPS-estimated
position is probable or not. If the current estimated position has low probability
it will affect the following 3D-path in such a way that the system becomes unsure
where it has actually been. The reason for this is that it is difficult to decide if the
54
5
Evaluation of RTK-GPS System
−40
−42
−42.5
−43
−50
North [m]
North [m]
−45
−55
−43.5
−44
−44.5
−60
−45
−65
35
40
45
East [m]
50
55
(a) An overview over the path.
47
48
East [m]
48.5
49
(b) Zoom in of one of the stand still
spots on the path.
−41.92
−41.93
−41.93
−41.935
−41.94
−41.94
−41.95
−41.945
−41.96
North [m]
North [m]
47.5
−41.97
−41.95
−41.955
−41.98
−41.96
−41.99
−42
−41.965
−42.01
−41.97
−42.02
48.28
48.3
48.32
48.34
East [m]
48.36
48.38
(c) Even more zoom than in 5.5b
which reveals the high accuracy. Notice the axis values.
48.31
48.32
48.33 48.34
East [m]
48.35
48.36
(d) All the measured data was inside
a square which measured 4 × 4 cm2 .
Figure 5.5: This test was done under an open sky environment. The base
station was still on the bench under the trees but this time it worked well.
5.2
55
Error Detection
aircraft has visited a place when it is unsure where it is.
5.2.2
Maximum Acceleration
The second motion model uses a maximum acceleration approach. The maximum acceleration is denoted amax and the acceleration is calculated using the
two preceding samples. In (5.4) and (5.5) the two latest velocities for the aircraft
are calculated
sτ − sτ−1
(5.4)
B vτ =
∆t
sτ−1 − sτ−2
∆t
and the current acceleration is calculated as
v − vτ−1
=
aτ = τ
∆t
−sτ−2
sτ −sτ−1
− sτ−1∆t
= ∆t
=
∆t
s − 2sτ−1 + sτ−2
= τ
(∆t)2
B vτ−1
=
(5.5)
(5.6)
The acceleration can be allowed to be, for example, at most 12 m/s2 which is comparable to the gravitation at the Earth’s surface which is approximately 9.82 m/s2 .
5.2.3
Extrapolated Position
The third motion model uses the mean value of the two preceding samples to
estimate how far away the aircraft should be at sample τ compared with sample
τ − 1. The mean velocity is given by
v
+ vτ−2
vmean = τ−1
,
(5.7)
2
which gives
∆Xest = vmean · ∆t,
(5.8)
where ∆Xest is an estimation of the displacement since the last sample. If ∆Xest
differs significantly from the displacement given by the positioning system the
control system can detect that something went wrong between the two samples.
The magnitude of the difference is given by
r=
k∆Xest − ∆Xk2
.
k∆Xest k2
(5.9)
If r is larger than a certain threshold, it is interpreted as though the positioning
system is providing erroneous values. The threshold for r can be tuned depending on the performance of the positioning system.
6
Control System
To control the attitude of the aircraft one PID controller for each of the axes is
used.
6.1
State Estimation
A state, for example altitude of the aircraft, can be estimated from measurements,
for example measured altitude and measured acceleration. This is called state
estimation and is done with for example sensor fusion.
6.1.1
Extended Kalman Filter
The Extended Kalman Filter (EKF) is used to estimate the state of a nonlinear
system. The EKF is explained in detail by Gustafsson [2012]. The nonlinear
system with added noise can be written
ẋ(t) = f (x(t), u(t)) + v(t)
(6.1a)
y(t) = h(x(t), u(t)) + e(t)
(6.1b)
where v(t) and e(t) are process noise and measurement noise with covariance
matrices Q and R, respectively. f describes the dynamics of the system and h
is the measurement equation that relates the measurements y(t) to the state x(t)
and the input u(t). In discrete time the model is
x k+1 = f (x k , uk ) + v k
y k = h (x k , uk ) + ek
(6.2a)
(6.2b)
where x k+1 is the state vector in the next sample and y k is the current measurement vector. The recursion algorithm for the EKF is divided into three parts, one
57
58
6
Control System
initialization part, one measurement update and one time update. The initialization part initializes the estimated state vector x̂0|0 and the covariance matrix P 0|0
for the estimated state vector. The measurement update gives the estimated state
vector x̂ k|k evaluated at time k given the measurements up to and including time
k and the covariance matrix P k|k . The time update predicts the next value of the
state vector x̂ k+1|k and updates the covariance matrix P k+1|k .
Algorithm 4 The Extended Kalman Filter (EKF)
1. Initialization
x̂0|0 = x0
(6.3a)
P 0|0 = P 0
(6.3b)
2. Time update
x̂ k|k−1 = f x̂ k−1|k−1 , uk
∂f x̂ k−1|k−1 , uk
Fk =
∂x̂ k−1|k−1
(6.4a)
(6.4b)
P k|k−1 = F k P k−1|k−1 F Tk + Qk
3. Measurement update
∂h x̂ k|k−1 , uk
Hk =
∂x̂ k|k−1
−1
K k = P k|k−1 H Tk H k P k|k−1 H Tk + Rk
(6.5a)
(6.5b)
x̂ k|k = x̂ k|k−1 + K k (y k − h(x̂ k|k−1 , uk ))
−1
H k P k|k−1
P k|k = P k|k−1 − P k|k−1 H Tk H k P k|k−1 H Tk + Rk
6.2
(6.4c)
(6.5c)
(6.5d)
PID-Controllers
A PID controller combines proportional gain, integral gain and derivative gain
in one controller. The input signal u (t) is based on the difference between the
desired and actual output signal e (t) = r (t) − y (t) according to
Zt
u (t) = KP e (t) + KI
e (τ) dτ + KD
d
e (t)
dt
(6.6)
0
where KP , KI and KD are the gains for the proportional, integral and derivative
parts. A large value of KP gives a faster response to errors but can lead to instability if it is too large. The integral part KI controls the ability to eliminate constant
errors in the output signal. The derivative part KD can increase the stability of
the system but differentiating noisy measurements can lead to large variations in
the control signal. More information about PID controllers can be found in Glad
and Ljung [2006].
6.3
59
Controllers
6.2.1
Instability Issues
The cause of instability in a system due to wrong KP and/or KI values is that
the feedback controller trusts old information too much. In dynamic systems it
takes a certain time before the input signal has an impact on the output signal.
This time delay between a change of input signal to the whole impact in the
output signal often leads to overcompensation in the feedback controller. When
the effect from the input signal finally appears in the output signal the system
may have an even bigger error, but with the opposite sign. This will lead to
oscillations with increasing amplitude.
6.2.2
Feedback Controller
Figure 6.1 shows a block diagram over a feedback system where G is the transfer
function of the system and H is the controller. The feedback system gives the
r
P e
H
u
G
y
Figure 6.1: A feedback system.
following advantages:
• Disturbances that affect the output signal are taken into consideration even
though they are not measurable by themselves
• The system does not need to be fully known
• An unstable system can be stabilized
and the disadvantage is:
• The system can become unstable with poor feedback
In the flight controller on the aircraft the feedback control is used specifically to
take care of wind disturbances.
6.3
Controllers
In Figure 6.2 the control loop for the aircraft is displayed. The Navigation controller sends out which acceleration the aircraft needs for the different axes. In
the Angle controller these accelerations are transformed to pitch, roll, yaw angles and thrust that the aircraft should have to achieve the desired accelerations.
Depending on which control system that are active the pitch angle θ is translated
to an angular velocity ω or a velocity u which the aircraft tries to achieve. The
controller that is used is a PID regulator implemented on differential form
∆u = Kp (ek − ek−1 ) + Ki ek + Kd (ek − 2ek−1 + ek−2 )
uk = uk−1 + ∆u
(6.7)
(6.8)
60
6
Control System
Thrust
Ψref
+
Σ
-
eΨ
PID
ωref + eω
TransΣ
formation
-
PID
τ
Mixer
u
Ψ
ω
Figure 6.2: The attitude control loop.
and it is the basis for the following controllers that are used in the aircraft, see
Glad and Ljung [2006]. In (6.8) e stands for the error between measured value
and the setpoint value. Kp is the proportional gain, Ki is the integral gain and Kd
is the differential gain. The time is noted with k for the current sample and k − 1
for the preceding sample.
6.3.1
Altitude Hold Controller
The altitude hold controller controls exclusively the aircraft’s altitude. When it
is turned on, from the hand held controller, the aircraft holds the current height
and the operator is able to change its position in the horizontal plane with help
of pitch and roll.
In Figure 6.3 the measurements from a manual flight are shown when the aircraft
was hoovering at approximately 3 m altitude during the test. Due to discrete
motor input from the hand held controller, which is seen in the figure, the aircraft varied in altitude with approximately ±0.20 m around the chosen hovering
height. The barometer measurements show much higher variation though, over
±1.00 m around the hovering height. This behaviour in measured sensor input
makes it very hard to regulate the aircraft.
One attempt to solve this issue was to use a Kalman Filter. In the Kalman Filter
the barometer and acceleration are fused together to get a better estimation of
the altitude. The state vector x used in the Kalman Filter is defined as
" #
z
x=
(6.9)
ż
where z is the altitude and ż is the velocity in E Z direction. The constant velocity
model in continuous time
"
#
" #
" #
0 1
0
0
ẋ(t) =
x (t) +
a (t) +
w (t)
(6.10a)
0 0
1
1
h
i
y (t) = 1 0 x (t)
(6.10b)
and discrete time
#
" 2#
" 2#
Ts
Ts
1 Ts
2
x k+1 =
xk +
ak + 2 wk
0 1
Ts
Ts
h
i
y k = 1 0 xk
"
(6.11a)
(6.11b)
6.3
61
Controllers
with the sampling time Ts , see Gustafsson [2012]. The Kalman Filter is used on
the discrete time model (6.11a) and (6.11b). The result of the Kalman Filter is
shown in Figure 6.3 as the blue dotted line, which varies approximately ±0.80 m
around the hoovering height. The measured motor input data from Figure 6.3
25
Measured Altitude [m]
20
Motor Input [-]
Estimated Altitude [m]
Value
15
Estimated Velocity [m/s]
10
5
0
−5
−10
70
0
Time [s]
Figure 6.3: Estimated altitude and vertical velocity with Kalman Filter. Between the two peaks the aircraft was hoovering at approximately the same
height, approximately ±0.20 m around 3 m altitude. The solid cyan line are
the measurements from the barometer which is more than ±1.00 m around
the hoovering level. The green dashed line shows the estimated velocity, the
black line the motor input and the blue dotted line the estimated altitude.
varies with approximately 1 Hz and with the amplitude 0.4 motor input units.
The resulting altitude controller follows the measurements but due to the poor
measurements the altitude controller becomes poor. With better data, from example RTK-GPS, it would probably work better.
Another solution could be to use the motor input in the state vector x because the
motor input is quite stable and affects the height directly. It would consequently
be a good idea to also estimate it with the Kalman Filter. Then the state vector x
becomes
 
 z 
 
x =  ż 
(6.12)
 
K
62
6
Control System
g−z̈
where K = u
and umotor is the motor input. K has the unit m/s2 because
motor
both g and z̈ are expressed in m/s2 while umotor is dimensionless. The non-linear
model is


x2




ẋ (t) = g − x3 umotor 
(6.13)


0
"
#
x1
y (t) =
(6.14)
−x3 umotor
where the measurements are barometer and accelerometer. By discretizing (6.13)
and (6.14) with Euler sampling the continuous time model becomes


x2,k


g − x u

x k+1 = x k + Ts 
(6.15)
3,k motor,k 


0
#
"
1 0
0
x
(6.16)
yk =
0 0 −umotor k
The result after applying the Extended Kalman Filter on (6.15) and (6.16) is
shown in Figure 6.4. The use of EKF improved the altitude estimation, compare
Figure 6.3 (KF) with Figure 6.4 (EKF), the variance of the estimates are visibly
less with the EKF.
25
Measured Altitude [m]
20
Motor Input [-]
Estimated Altitude [m]
15
Estimated Velocity [m/s]
Value
Estimated K [m/s2 ]
10
5
0
−5
−10
0
10
20
30
40
Time [s]
50
60
70
Figure 6.4: Estimated altitude and vertical velocity with Extended Kalman
Filter. The input data is the same as in Figure 6.3.
6.3
63
Controllers
A model was also made in Simulink in MatLab to try to catch the dynamic behaviour of the system. In Figure 6.5 this model can be seen. White band limited
Process noise
Control signal
1
PID(s)
Step
PID Controller
0.05s+1
Saturation
Transfer Fcn
-K-
a
1
s
v
Integrator
Gain
1
s
z
Integrator1
Altitude
9.82
Gravity
2
200 Hz sampling
Gain1
Velocity estimation delay
Measurement noise
50 Hz sampling
Barometer delay
Barometer noise
Figure 6.5: Simulink model over the aircraft and the altitude controller.
noise together with a delay were added to the model to try to catch as much as
possible of the reality.
The experience from this implementation is that you need to know the sensors
limits and construct your control system thereafter.
6.3.2
Yaw Hold Controller
The yaw hold controller keep the aircraft facing the same direction as it was facing when the operator turned this function on. The operator can change the altitude with the thrust lever on the controller and also pitch and roll of the aircraft.
Achieving the Correct Euler Angles
Given a force E F in the Earth fixed frame and a desired yaw angle it is desirable
to translate this into corresponding Euler angles φ, θ and ψ and a body fixed
thrust force B F . This can be done with
T = k E F k2
φ = arcsin
θ = arctan
(6.17)
−E F x sin(ψ) + E F y cos(ψ)
T
!
F
cos(ψ)
+
E x
E F y sin(ψ)
EFz
!
(6.18)
(6.19)
where
 
 0 
 0 
F
=
 
B
 
−T
(6.20)
64
6
Control System
The assumption that the body only has a force in z-direction is made with respect
to that the rotors are aligned with the z-axis. From the rotation matrix R and the
relation between Earth fixed and Body fixed forces
EF
= R−1 B F


 sin(φ) sin(ψ) + cos(φ) sin(θ) cos(ψ) 


= −T − sin(φ) cos(ψ) + cos(φ) sin(θ) sin(ψ)


cos(φ) cos(θ)
(6.21)
(6.22)
the equations (6.17) to (6.19) can be derived, see Zuo [2009]. In the control loop
equations (6.17) to (6.19) are used to translate the calculated force E F to Euler
angles.
6.3.3
Position Hold Controller
The position hold controller keeps the aircraft hoovering on a specific spot in
space. The operator can only change the aircraft’s yaw angle.
6.3.4
Motion Path Controller
There are different solutions to how the aircraft can fly along a smooth motion
path. One solution is to let the forward direction of the aircraft always point in
the direction of the path tangent. This approach is intuitive for many people, the
vehicle goes in the direction that is ahead of it. But a quadrocopter, which has
been studied during this master’s thesis, is quite symmetric and does not have
any clear front from the beginning which opens for other solutions. A second
solution is that a certain boom of the aircraft always points against one specific
point, for example the pilot. This method makes it easier for some helicopter
pilots to keep track of roll and pitch controls when they take over the control of
the aircraft after a successful autonomous flight.
Follow a Spline Curve
To be able to follow a smooth motion path defined by a spline curve it can be
good to know the closest point on the curve given the aircraft’s position. The
parametric cubic spline is expressed as


E x(t)
 y(t)
(6.23)
p(t) = E
 , 0 ≤ t ≤ L


E z(t)
where E x(t), E y(t) and E z(t) are the components of the point p(t) along the spline
curve evaluated at t and expressed in the Earth frame. The aircraft’s position
is pa = [E xa , E ya , E za ]T . In general these two positions are not the same. One
possibility for the control system is to navigate along the shortest path between
the current location pa and the desired location p(t) which results in the direction

 

E x(t) E xa 

 

ha = p(t) − pa = E y(t) − E ya 
(6.24)

 

E z(t)
E za
6.3
Controllers
65
where ha denotes the desired heading of the velocity of the aircraft. Due to obstacles in the real world environment this control strategy can lead to catastrophic
consequences. It is not desirable to take a shortcut through for example a lamp
post or a building.
A better control strategy would be to try to follow the motion path, the spline
curve, as good as possible and relax the time constraint.
7
Discussions and Conclusions
In this chapter the results and conclusions from previous chapters are discussed.
This chapter is divided in sections corresponding to the previous chapters and
ends with Section 7.6 where the experiences from this work are collected and
commented.
7.1
Models
The quadrocopter model in Chapter 3.1 has not been used during the project
however the flight dynamics in Chapter 2 have been used. Especially the rotation
matrices R and W have improved the aircraft’s abilities by making the steering
and control of the aircraft smoother.
7.1.1
Simulation Environment
One experience from this project is that it would have been much easier to achieve
good results faster if a good simulation environment had been available. It would
maybe have been possible to build an own simulation environment with the
model from Chapter 3.1 and physical data about the studied aircraft. This would
probably have taken much time to get it good enough to be useful, maybe it would
be enough material to do a master’s thesis about simulating a quadrocopter. At
last a simple model for the altitude controller was made in Simulink in MatLab,
see Figure 6.5. This made it possible to at least see the relative relationship between the control parameters and how these affected the system.
The tests were otherwise performed outdoors and one or two of the control parameters were tuned manually during the flight from the hand held controller.
This approach was time consuming but had the advantage that the evaluation of
67
68
7
Discussions and Conclusions
the controller was made in the real environment. A recommendation to similar
projects is to use a simulation environment where it is easy to change control
parameters and evaluate them.
7.1.2
Motion Path
The smooth motion path was defined with a cubic spline curve. The property of
such spline curves is that they are continuous with respect both to their first and
second derivatives. That is the derivative of their position and velocity. A thorough derivation of the making of spline curves is given in Chapter 3.2. From linear spline curves, which are the easiest type, to quadratic curves, which provide
continuous velocity, up to cubic spline curves are derived. It is also mentioned
how to make spline curves of higher degree.
With the introduction of matrix representation of splines it becomes easy to
evaluate the spline curves in, for example, MatLab or C. The two algorithms,
Quadratic Minimization Method and Newton’s Method make the finding of the
closest point, relative to the aircraft’s position, on a segment of the curve fast and
computationally efficient. The tangential vector in each point of the spline curve
can also be displayed together with the acceleration vector.
In a further motion path control system splines will provide smooth curves which
are defined by a small amount of control points c and knots t. For each segment
of the curve only the actual control points and knots for that segment needs to be
known together with the blending matrix R̃. These variables are all that needs to
be known to calculate points along a curve segment, which make the spline curve
representation computationally cheap.
Regardless of the advantages of the spline curves, creating the path can be a bit
unintuitive the first times. The spline curve does not, in the representation presented, go through the control points and a change of one control point or knot
changes the curve.
7.2
Sensors - IMU, Magnetometer, Barometer and
Internal GPS
The sensors used during the project had different precision. For example the
barometer had quite poor precision, see Figure 4.5, while Trimbles RTK-GPS was
on the other end of the scale with centimetre precision, see Figure 5.1a. It is
unclear if the barometers poor precision is due to the specific sensor used or if it is
a common problem for all barometer sensors. Maybe the tested barometer sensor
is as good as a barometer sensor possible can be in the different environments that
have been tested. It is difficult to say when no other barometers have been tested
and evaluated. One thing for sure is that the measurements from the barometer
sensor where very difficult for a PID-control system to handle due to the high
level of noise.
7.3
Evaluation of RTK-GPS System
69
Good sensor measurements are fundamental for getting good performance from
a control system.
One experience from the implementation of the internal GPS is that it is important what data type is used to represent latitude and longitude. In the programming language C it is possible to decide how numbers shall be interpreted. For
example int, float and double. One problem that was encountered when implementing the GPS in the Flight Controller code was that the precision was low
when float was used for the entire coordinate. This was solved by using two
floats, the first one holds the minutes part and the second one the fraction.
7.3
Evaluation of RTK-GPS System
In this section the results from the evaluation of the RTK-GPS system are discussed and analysed.
7.3.1
Successful Tests
The evaluation of Trimble’s RTK-GPS resulted in plots with very high position
accuracy. These position data would be possible to use as input to a control algorithm for autonomous flight. For example see Figure 5.5 which shows the very
high accuracy under an open sky environment. Important in this figure is that a
particular spot was revisited five times with approximately 90 seconds space in
between. With a normal GPS-receiver the change of satellite constellation should
have given a much higher uncertainty compared with the case with RTK. An ordinary GPS-system should have had meter accuracy in position compared with
the achieved centimetre accuracy with Trimble’s system. Compare for example
Figure 4.8 which shows the internal GPS with Figure 5.5a which shows the RTKGPS system. In both these figures the aircraft or test bench with the GPS receiver
equipment is hand held and moved along two different paths multiple times.
The conclusion of this is that a flying aircraft with Trimble’s RTK-GPS system
would be able to return to exactly the same spot in the world time after time and
it would rather be the control system that sets the limits for the success of a flight.
Another constraint is the flight time which for IA:s big hexacopter (six booms) is
approximately five minutes.
7.3.2
Glitches
During some of the tests Trimble’s RTK-GPS-system turned off and restarted.
And some other times it got discontinuance in the calculated positions - the estimated position jumped approximately one meter between two samples. The
time between two samples was five milliseconds. This means that the velocity,
for the hand-held GPS-receiver based on these data was around 20 m/s between
two samples, which corresponds to 72 km/h, which was not the case during the
actual test. This can be detected with help of the discussed error detection in
Section 5.2. The best way to avoid glitches is to operate the aircraft under an
70
7
Discussions and Conclusions
open sky environment and to place the base station away from trees, buildings
and similar obstacles. In Figure 5.3 some glitches are shown.
Handle Glitches
In a control system glitches can be handled with a dynamic model of the aircraft
that says that the aircraft’s maximal velocity is 15 m/s which means that the
aircraft can maximally move 0.75 m between two time samples. An even more
accurate estimation of the aircraft’s position in the next time sample would be
to extrapolate, say, two samples back just to get an approximation of how far
the aircraft can reach. A third alternative is to use the maximal acceleration and
calculate how far away the aircraft can come.
7.4
Future Development
This project has big opportunities to be further developed. The lessons from this
project can be used to plan a future project better with more time deposited for
hardware issues. It would also be good to find a simulation environment in which
the ideas can be tested and future developed. When the motionpath system for
the aircraft works it would be a good idea to add a similar system for the camera
system steering the gimbal in a specific path coupled to the aircraft’s path. It
would also be interesting to study commercialising of the system, how shall the
user create the paths for example?
7.5
Results
The goals for the master’s thesis were the following
• Improve the abilities of the aircraft
• Develop a motion path system
• Evaluate a RTK-GPS system
• Implement more sensors in the aircraft
• Define a smooth motion path
which was mentioned in Chapter 1.2. Of these goals have four out of five been
achieved.
The abilities of the aircraft have been improved. At the start of the project the
most of the code of the flight controller was implemented but it was not tuned
for the aircraft used during the project.
Develop a motion path system ended with an altitude holder due to lack of time.
It took a lot longer time to tune the parameters than calculated. A better altitude
estimation together with a better test environment would have shortened this
time.
7.6
Experiences
71
The evaluation of RTK-GPS also took a lot of time. There was a lot of hardware
issues during the first weeks. After the hardware issues were solved a lot of test
material were collected and analysed.
During the project sensor reading for magnetometer and two different barometers have been implemented in the flight controller. This made it possible to get
altitude and heading estimates of the aircraft.
A smooth motion path has been defined with help of splines. This was implemented in MatLab. The advantages with using splines are that a smooth path is
achieved and with splines of dimension three the velocity in each point of the
spline is continuous.
7.6
Experiences
During this project a lot of lessons have been learned. The most important one
is that hardware issues takes a lot more time than planned, at least 5 times the
amount of time scheduled. Another lesson is that it is important to have a developed test environment to be able to evaluate control systems faster. It is also
good to know the pros and cons of the sensors in an early stage of the project and
to know how they can be improved and fused together.
Personal experiences during the project are a better understanding of hardware,
quadrocopter pilot experience and an improved English and improved knowledge in report writing.
Appendix
A
Appendix
A.1
Equipment Borrowed from Trimble
Figure A.1 displays the equipment that Trimble lent the project.
Figure A.1: Trimble lent this equipment to evaluate.
75
76
A
Appendix
Bibliography
Ublox AG. LEA-6 u-blox 6 GPS Modules Data Sheet. http://www.u-blox.
com/en/gps-modules/pvt-modules/lea-6-family.html. Cited on
page 42.
Mário N. Berberan-Santos, Evgeny N. Bodunov, and Lionello Pogliani. On the
Barometric Formula. American Association of Physics Teacheers, 65(5):404–
412, 1997. Cited on page 40.
Flying Machine Arena (FMA). Flying Machine Arena Publications. http://
www.flyingmachinearena.org/publications/. Cited on page 2.
Torkel Glad and Lennart Ljung. Reglerteknik Grundläggande teori. Studentlitteratur, 2006. Cited on pages 58 and 60.
Fredrik Gustafsson. Statistical Sensor Fusion. Studentlitteratur, second edition,
2012. Cited on pages 6, 57, and 61.
Lars Johansson. Flygmekanik för Y. Cited on page 9.
Teppo Luukkonen. Modelling and Control of Quadcopter. Technical Report Mat2.4108, Aalto University - School of Science, Espoo, August 2012. Cited on
pages 12 and 19.
Tom Lyche and Knut Morken. Spline Methods Draft. Department of Informatics
Centre of Mathematics for Applications University of Oslo, 2008. Cited on
pages 20, 21, 24, and 26.
Sebastian O.H. Madgwick. An Efficient Orientation Filter for Inertial and Inertial/Magnetic Sensor Arrays. April 2010. Cited on page 29.
Javier R. Movellan. DC Motors. March 2010. URL http://mplab.ucsd.edu/
wordpress/tutorials/DC.pdf. Cited on page 19.
R. C Nelson. Flight Stability and Automatic Control. McGraw-Hill, second edition, 1998. Cited on page 8.
77
78
Bibliography
William Premerlani.
Magnetometer Offset Cancellation: Theory and Implementation revisited. https://gentlenav.googlecode.com/files/
MagnetometerOffsetNullingRevisited.pdf, 2011. Cited on page 34.
Sune Söderkvist. Från insignal till utsignal.
Linköping, Sweden, 2007. Cited on page 37.
Tryckeriet Erik Larsson AB,
Bosch Sensortec. BMP085 Digital Pressure Sensor Data sheet, 1.2 edition. Cited
on pages 37 and 39.
Richard Stanaway. GDA94, ITRF, WGS84: What’s the Difference? Working with
Dynamic Datums. Spatial Sciences Institute - Biennial Conference, Hobart,
May 2007. Cited on page 13.
J.F. Vasconcelos, G. Elkaim, C. Silvestre, P. Oliveira, and B. Cardeira. Geometric Approach to Strapdown Magnetometer Calibration in Sensor Frame.
Aerospace and Electronic Systems, IEEE Transactions on, 47(2):1293–1306,
2011. doi: 10.1109/TAES.2011.5751259. Cited on page 34.
Hongling Wang, Joseph Kearney, and Kendall Atkinson. Robust and Efficient
Computation of the Closest Point on a Spline Curve. Curve and Surface Design:
Saint-Malo 2002, 2003. Cited on page 26.
Z. Zuo. Trajectory Tracking Control Design with Command-Filtered Compensation for a Quadrotor. IET Control Theory and Applications, 4(11):2343–2355,
2009. Cited on page 64.
Upphovsrätt
Detta dokument hålls tillgängligt på Internet — eller dess framtida ersättare —
under 25 år från publiceringsdatum under förutsättning att inga extraordinära
omständigheter uppstår.
Tillgång till dokumentet innebär tillstånd för var och en att läsa, ladda ner,
skriva ut enstaka kopior för enskilt bruk och att använda det oförändrat för ickekommersiell forskning och för undervisning. Överföring av upphovsrätten vid
en senare tidpunkt kan inte upphäva detta tillstånd. All annan användning av
dokumentet kräver upphovsmannens medgivande. För att garantera äktheten,
säkerheten och tillgängligheten finns det lösningar av teknisk och administrativ
art.
Upphovsmannens ideella rätt innefattar rätt att bli nämnd som upphovsman
i den omfattning som god sed kräver vid användning av dokumentet på ovan
beskrivna sätt samt skydd mot att dokumentet ändras eller presenteras i sådan
form eller i sådant sammanhang som är kränkande för upphovsmannens litterära
eller konstnärliga anseende eller egenart.
För ytterligare information om Linköping University Electronic Press se förlagets hemsida http://www.ep.liu.se/
Copyright
The publishers will keep this document online on the Internet — or its possible replacement — for a period of 25 years from the date of publication barring
exceptional circumstances.
The online availability of the document implies a permanent permission for
anyone to read, to download, to print out single copies for his/her own use and
to use it unchanged for any non-commercial research and educational purpose.
Subsequent transfers of copyright cannot revoke this permission. All other uses
of the document are conditional on the consent of the copyright owner. The
publisher has taken technical and administrative measures to assure authenticity,
security and accessibility.
According to intellectual property law the author has the right to be mentioned when his/her work is accessed as described above and to be protected
against infringement.
For additional information about the Linköping University Electronic Press
and its procedures for publication and for assurance of document integrity, please
refer to its www home page: http://www.ep.liu.se/
© Josef Högberg and Staffan Sjöqvist
Was this manual useful for you? yes no
Thank you for your participation!

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

Related manuals

Download PDF

advertisement