Position and Trajectory Control of a Quadcopter Using PID and …liu.diva-portal.org/smash/get/diva2:1129641/FULLTEXT01.pdf · 2017. 8. 4. · Axel Reizenstein. Master of Science

Master of Science Thesis in Electrical EngineeringDepartment of Electrical Engineering, Linköping University, 2017

Position and TrajectoryControl of a QuadcopterUsing PID and LQControllers

Axel Reizenstein

Master of Science Thesis in Electrical Engineering

Position and Trajectory Control of a Quadcopter Using PID and LQ Controllers

Axel Reizenstein

LiTH-ISY-EX--17/5075--SE

Supervisors: Kristoffer Bergmanisy, Linköpings universitet

Erik EkelundSAAB Dynamics

Examiner: Daniel Axehillisy, Linköpings universitet

Division of Automatic ControlDepartment of Electrical Engineering

Linköping UniversitySE-581 83 Linköping, Sweden

Copyright © 2017 Axel Reizenstein

It’s only a model.

Abstract

This thesis describes the work done to implement and develop position and tra-jectory control of a quadcopter. The quadcopter was originally equipped withsensors and software to estimate and control the quadcopter’s orientation, butdid not estimate the current position. A gps module, gps antenna and a lidarhave been added to measure the position in three dimensions. Filters have beenimplemented and developed to estimate the position, velocity and acceleration.Four controllers have been designed that use these estimates: one pid controllerand one lq controller for vertical movement, and a position controller and a tra-jectory controller for horizontal movement. The position controller maintains aconstant position, while the trajectory controller maintains a constant velocitywhile travelling along a straight line. These position and trajectory controllerscalculate the reference angles required to direct the thrust necessary to controlthe quadcopter’s movement. Additionally, an algorithm has been developed to de-crease overshoot by predicting future trajectories. These controllers have provento be successful at controlling the quadcopter’s position in all three dimensions,both in practice during outdoor flight and in simulations.

v

Acknowledgments

This thesis represents the culmination of the 5 most important and challengingyears of my life. Completing this thesis can partially be credited to my own work,but the individuals without whom success would not have been certain or evenpossible are too many to list. I will, however, attempt to summarise.

My family, for offering support and for being there when they were neededmost. Thank you for reminding me that at the end of the longest days, homeawaits.

My examiner, Daniel, and supervisors, Kristoffer and Erik, for helping me andholding me to a high standard. I could not have corrected the faults and flaws ofmy work if you had not found them.

Everyone I worked with at saab Dynamics. Thank you for offering assistanceand creating a wonderful working environment. I would especially like to thankJohn, Johan, Torbjörn, Anders and Björn.

My opponent, Petter, for his fascinating thesis and his thorough analysis ofmy own.

All my peers, for putting up with me for all these years. I hope this is notgoodbye.

Linköping, June 2017Axel Reizenstein

vii

Contents

Notation xi

1 Introduction 11.1 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.2 Problem Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . 11.3 Purpose . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21.4 Related Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21.5 Limitations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21.6 Outline of Thesis . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2 Preliminaries 52.1 Euler Angles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52.2 Modelling and Control of Quadcopter Angles . . . . . . . . . . . . 72.3 Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82.4 LQ Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

3 Platform 113.1 Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 123.2 Program Architecture . . . . . . . . . . . . . . . . . . . . . . . . . . 123.3 LIDAR Measurements . . . . . . . . . . . . . . . . . . . . . . . . . . 15

4 Modeling 174.1 LIDAR Measurements . . . . . . . . . . . . . . . . . . . . . . . . . . 174.2 Model Simplification . . . . . . . . . . . . . . . . . . . . . . . . . . 184.3 Simulation Environment . . . . . . . . . . . . . . . . . . . . . . . . 26

5 Position Estimation 275.1 Extended Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . 275.2 Height Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 275.3 Height Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . 28

5.3.1 Model Structure of Height . . . . . . . . . . . . . . . . . . . 295.3.2 Static Thrust Estimation . . . . . . . . . . . . . . . . . . . . 305.3.3 Noise Estimation . . . . . . . . . . . . . . . . . . . . . . . . 32

ix

x Contents

5.4 Magnetometer Calibration . . . . . . . . . . . . . . . . . . . . . . . 36

6 Position and Trajectory Control 396.1 Height Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

6.1.1 PID . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 406.1.2 LQR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

6.2 Position Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . 466.2.1 Assumptions . . . . . . . . . . . . . . . . . . . . . . . . . . . 466.2.2 Local Map . . . . . . . . . . . . . . . . . . . . . . . . . . . . 466.2.3 LQ Controller . . . . . . . . . . . . . . . . . . . . . . . . . . 476.2.4 Reference Angle Calculations . . . . . . . . . . . . . . . . . 48

6.3 Position Following . . . . . . . . . . . . . . . . . . . . . . . . . . . . 516.4 Trajectory Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54

6.4.1 Constant Velocity Along Line . . . . . . . . . . . . . . . . . 546.5 Missions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56

6.5.1 Constant Velocity When Turning . . . . . . . . . . . . . . . 58

7 Conclusions 657.1 Result . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 657.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66

Bibliography 67

Notation

Notations

Notation Meaning

[x y z] Inertial frame coordinates[X Y Z] Body-fixed coordinates[φ θ ψ] Euler angles roll, pitch and yaw[p q r] Angular rates in body-fixed coordinatesR Rotation matrix for vectorsT Rotation matrix for angular velocitiesux Control signal for x

[cx sx] cos(x) and sin(x)x estimated value of x

[lat lon] Latitude and Longitude angles in radiansφm Phase marginωc Cross-over frequency

Abbreviations

Abbreviation Meaning

pid Proportional, Integral, Differentiallqr Linear Quadratic Regulatorgps Global Positioning Systemimu Inertial Measurement Unitpwm Pulse Width Modulationekf Extended Kalman Filterudp User Datagram Protocollidar Light Detection And Rangingesc Electronic Speed Controllericc Inflight Compass Calibrationmpc Model Predictive Controlned North-East-Down

xi

1Introduction

This thesis details the work performed at the request of saab Dynamics to imple-ment navigational functionality on a quadcopter. This chapter will summarisethe purpose of this thesis, and the work it builds upon. A brief overview of theoutline of the thesis will also be provided.

1.1 Background

The quadcopter design is efficient in that it allows for the ability to control theorientation and movement in three dimensions using only four moving parts.The downside is that the system has no natural stabilising elements, but insteadrelies on software algorithms for stability. Combined with the fact that the systemis underactuated, this means that advanced estimation and controller algorithmsare required to control the quadcopter (Zemalache et al., 2005).

The platform used in this thesis is provided by saab Dynamics. Previously,Kugelberg (2016) used black-box modeling to implement angle controllers. Thenavigational algorithms developed in this thesis utilise these angle controllers toallow for control of the quadcopter’s position. These navigational algorithms canbe used by other students in the future to implement additional functionality.

1.2 Problem Formulation

One way to implement a trajectory controller is to control the angles in an innercontrol loop, with the trajectory controller as the outer loop (Bonna and Camino,2015). This outer loop can be implemented using lq or pid controllers. Eval-uating and implementing these controllers requires models of the quadcopter’smovement (Glad and Ljung, 2006). Accurate state estimates are required as input

1

2 1 Introduction

to these controllers, and can be generated using a Kalman filter or ekf (Gustafs-son, 2012). Implementing these filters requires models of the estimated states,noise estimation and measurement equations.

Often, nonlinear models of the quadcopter is used to generate the controllers(Bangura and Mahony, 2012). Kugelberg (2016) assumed that the quadcopterwas close to a hovering state to approximate the models used as linear, and byassuming that the quadcopter moves with a velocity low enough to neglect drag,models used for positioning can be approximated as linear as well.

1.3 Purpose

The goal of this thesis is to design and implement controllers of the quadcopter’sposition and trajectory in an outdoor environment. These controllers will gener-ate reference angles for the inner loop angle controllers. The controllers requireestimates of the quadcopter’s position in three dimensions. To estimate the po-sition new sensors will have to be integrated along with algorithms to generateposition estimates from these new sensor measurements. Using these controllers,it will be possible to program the quadcopter to perform missions, which willconsist of a set of coordinates or a path that the quadcopter will follow.

1.4 Related Work

The work done previously on the same platform by Kugelberg (2016) was focusedon estimating the dynamics of the quadcopter using black box models in orderto implement pid angle controllers. Additionally, various algorithms were im-plemented to handle control signal prioritisation, control signal saturation andintegrator wind-up.

Various research has been done on methods to control the position and trajec-tory of a quadcopter. Abdolhosseini et al. (2013) explores the usage of an mpccontroller to follow a reference trajectory. Santana et al. (2015) implements a gpsfor outdoor waypoint following, using a base station to generate control signals.

1.5 Limitations

The position and velocity of the quadcopter is estimated in a local Cartesian co-ordinate system instead of global, spherical coordinates. This results in a limitedrange before the Cartesian estimates starts to deviate from the actual position.

Vertical position is measured using a lidar, which means that the verticalposition will be estimated as height (above ground) instead of altitude (above sealevel). Additionally, the area where the quadcopter operates will be flat with littleor no inclination.

The angle controllers developed by Kugelberg (2016) assumes that the quad-copter is close to hovering. This assumption is kept in this thesis.

1.6 Outline of Thesis 3

1.6 Outline of Thesis

Each task in this thesis builds upon previous ones. The chapters are thus ar-ranged chronologically. Chapter 2 is a summary of the theory used in this thesis.Chapter 3 describes the hardware and program architecture of the platform’scurrent configuration. In Chapter 4, theoretical modeling of the quadcopter isperformed. Chapter 5 explains how the algorithms estimating the position of thequadcopter were developed. The implementation of the position controllers isdescribed in Chapter 6. Finally, Chapter 7 contains the discussion of conclusionsand future work.

2Preliminaries

The work done in this thesis involves modeling, control, state estimation andsensor fusion. This chapter will give a brief overview of the theory necessary tounderstand these methods. The notation and terminology used to describe thequadcopter will also be described.

2.1 Euler Angles

According to Nelson (1998) it is convenient to define a body-fixed coordinatesystem [X Y Z] in the aircraft. In aeronautic applications, quantities such as ac-celeration, velocities, and angular rates are often, or at least partially, measuredin relation to the aircraft. The body-fixed coordinate system can be used to relatemeasurements and estimations to the inertial system. In aeronautical applica-tions this is commonly a North-East-Down (ned) coordinate system, with the Xaxis pointing fore, the Y axis pointing starboard and the Z axis pointing to thekeel of the craft. The rotational velocity of the aircraft is measured by the angularrates p around the X axis, q around the Y axis and r around the Z axis. The air-craft’s velocities along the X, Y , and Z axes are denoted u, v and w respectively.To relate the orientation of the local coordinate system relative to a global one,the Euler angles roll (φ), pitch (θ), and yaw (ψ) can be used.

Nelson (1998) describes the three rotations needed to transform the globalcoordinate system [x y z] to the local system [X Y Z]. Each rotation results in anew coordinate system, and the two intermediate coordinate systems are denoted[x′ y′ z′] and [x′′ y′′ z′′]. [X Y Z] is reached by rotating ψ radians around z, θradians around y′ and φ radians around x′′ . The position and velocity of theaircraft in the global coordinate systems is denoted [px py pz] and [px py pz]respectively. The velocity can according to Nelson (1998) be expressed using the

5

6 2 Preliminaries

rotation matrix R according to

sφ = sin(φ), cφ = cos(φ),

sθ = sin(θ), cθ = cos(θ), tθ = tan(θ)

sψ = sin(ψ), cψ = cos(ψ),

(2.1)

R =

cψcθ −sψcφ + cψsθsφ sψsφ + cψcφsθsψcθ cψcφ + sψsθsφ −cψsφ + sψcφsθ−sθ cθsφ cθcφ

, (2.2)

pxpypz

= R

uvw

. (2.3)

The derivatives of the Euler angles can be expressed using the angular rates as

T =

1 sφtθ cφtθ0 cφ −sφ0

sφcθ

cφcθ

,φθψ

= T

pqr

,(2.4)

as described by Nelson (1998). This expression for the derivatives requires thatθ , π

2 . When φ and θ are close to 0, which corresponds to the quadcopter beingclose to hover, T is approximately a unit matrix. In this case the relation betweenthe angles and angular rates can be approximated as linear according toφθ

ψ

≈pqr

. (2.5)

The body-fixed coordinate system and the angular rates of the quadcopter can beseen in Figure 2.1.

2.2 Modelling and Control of Quadcopter Angles 7

Figure 2.1: An illustration of the quadcopter’s body-fixed coordinate system(red), the angular rates (green), along with the position and direction of eachengine (blue).

2.2 Modelling and Control of Quadcopter Angles

A quadcopter has four motors and rotors. A common configuration, which isused on the quadcopter in this thesis, is to have two motors rotating clockwise,and two motors rotating counter-clockwise. By having the motors on the diag-onals rotating in the same direction it is possible to control the roll, pitch andyaw angles. The motors are numbered from 1 to 4, and each motor has its ownseparate motor signal, denoted u1, u2, u3, and u4 for each motor. The positionand the direction of each motor can be seen in Figure 2.1. Each motor signalranges from 0 to 1, where 0 denotes no power and 1 denotes max power. To con-trol the angles and throttle, the control signals uthrottle, uroll , upitch, and uyaw areused. Kugelberg (2016) implemented the relation between the motor signals andcontrol signals as

u1u2u3u4

=

1 −1 1 −11 −1 −1 11 1 −1 −11 1 1 1

uthrottleurollupitchuyaw

. (2.6)

In order to implement a fast and robust controller, the dynamics between thecontrol signals and angular rates need to be known. Previously, work was doneto estimate these relationships using black-box modelling. Time discrete mod-els were estimated for the roll, pitch and yaw channels with the approximationthat when the quadcopter is close to hovering, each channel only affects the cor-

8 2 Preliminaries

responding rate and angle. The models for each channel, under the assumptionthat the quadcopter is close to hover and that (2.5) is valid, has the form

Aφ(z)φ[k] = −A2,φ(z)p[k] + Bφ(z)uroll[k] + Cφ(z)eφ[k],

Ap(z)p[k] = −A1,p(z)φ[k] + Bp(z)uroll[k] + Cp(z)ep[k],(2.7)

Aθ(z)θ[k] = −A2,θ(z)q[k] + Bθ(z)upitch[k] + Cθ(z)eθ[k],

Aq(z)q[k] = −A1,q(z)θ[k] + Bq(z)upitch[k] + Cq(z)eq[k],(2.8)

Aψ(z)ψ[k] = −A2,ψ(z)r[k] + Bψ(z)uyaw[k] + Cψ(z)eψ[k],

Ar (z)r[k] = −A1,r (z)ψ[k] + Br (z)uyaw[k] + Cr (z)er [k],(2.9)

Ax(z) = ax,0 + ax,1z−1 + ... + ax,nz

−n,

Bx(z) = bx,1z−1 + ... + bx,nz

−n,

Cx(z) = cx,0 + cx,1z−1 + ... + cx,nz

−n,

(2.10)

where ex is noise. Equation 2.10 describes the shape of the polynomials in theblack-box models. Using these models each channel could be simulated and pidcontrollers for the φ and θ angles and the rate r were determined.

2.3 Kalman Filter

If a linear state space model of a system is available, a Kalman filter can be usedto obtain optimal estimates of the states from observations. A discrete modelwith the states x, input signals u and observations y can be written as

x[k + 1] = F[k]x[k] + Gu[k]u[k] + Gv[k]v[k],

y[k] = H[k]x[k] + D[k]u[k] + e[k],

cov(e[k]) = R[k], cov(v[k]) = Q[k],

(2.11)

where e is noise in the observations, originating from sensors, hardware and/orenvironmental disturbances among others. v is process noise, which is the mag-nitude of changes in the system that are not described by the model (Gustafsson,2012). The matrices F, Gu , Gv , H and D can change over time, but in this thesisthey are constant and will be written as such. The noise covariances R[k] andQ[k]are also assumed to be constant. Using this model, state estimates x[k] and statecovariances P [k] can be obtained. This is done in two steps, the time update thatupdates the state estimates according to the model, and the measurement updatethat updates the state estimates using observations (Gustafsson, 2012). The mea-surement update is simplified by first defining the innovation ε, the innovation

2.4 LQ Control 9

covariance S and the Kalman gain K as

ε[k] = y[k] − Hx[k|k − 1] − Du[k],

S[k] = HP [k|k − 1]HT + R,

K[k] = P [k|k − 1]HT S−1[k].

(2.12)

The measurement update can then be calculated according to

x[k|k] = x[k|k − 1] + K[k]ε[k],

P [k|k] = P [k|k − 1] − K[k]HP [k|k − 1].(2.13)

The time update can be expressed as

x[k + 1|k] = Fx[k|k] + Guu[k],

P [k + 1|k] = FP [k|k]FT + GvQGTV .

(2.14)

If the model is instead nonlinear, Gustafsson (2012) describes how an ExtendedKalman Filter (ekf) can be used.

2.4 LQ Control

When controlling a system with a known, linear structure, an lq controller canbe implemented. According to Glad and Ljung (2006) and Lewis et al. (2012), anlq controller minimises the quadratic cost J by using the control law u = −Kx.For a continuous-time system with

x = Ax + Bu, (2.15)

the cost is

J =

∞∫0

(xTQx + uT Ru)dt, (2.16)

and for a discrete-time system with

x[k + 1] = Ax[k] + Bu[k], (2.17)

the cost is

J =∞∑k=0

(xTQx + uT Ru), (2.18)

assuming infinite time horizon and no mixed terms. Q and R are weight matrices,and are commonly diagonal matrices. The feedback gain K that minimises J isfound by solving either the continuous-time algebraic Riccati equation or thediscrete-time algebraic Riccati equation. The continuous-time algebraic Riccatiequation is

AT P + P A − P BR−1BT P + Q = 0, (2.19)

10 2 Preliminaries

and the discrete-time algebraic Riccati equation is

AT P A − P AT P B(BT P B + R)−1BT P A + Q = 0. (2.20)

For larger systems, the P that solves these equations is usually found numerically.For a continuous-time system the feedback gain that minimises J is

K = R−1BT P , (2.21)

and for a discrete system it is

K = (BT P B + R)−1BT P A. (2.22)

(Glad and Ljung, 2006; Lewis et al., 2012).

3Platform

The quadcopter platform in its current configuration can be seen in Figure 3.1.The positioning and control algorithms have been implemented on a computer,and this computer uses different methods and protocols to communicate withthe remaining hardware. The hardware and the communication methods will bedescribed in this chapter and the program architecture on the computer will alsobe explained.

Figure 3.1: Image of the quadcopter’s current configuration. The lidar canbe seen in the left side of the image, and the gps antenna and receiver an-tennas can be seen on top of the quadcopter. The blue pwm board can alsobe seen behind the lidar.

11

12 3 Platform

RaspberryGPSModule

IMU

ArduinoPWMBoard

LIDAR

GPSAntenna

Receiver ESC:s

I2CAnalogue

UART PWM×4

USBCoaxial UART

Figure 3.2: Overview of the hardware on the quadcopter and the methodswith which they communicate.

3.1 Hardware

The quadcopter consists of a carbon fibre frame with 4 motors, a battery andvarious electronic boards. The total mass of the quadcopter is 0.759 kilograms.The hardware currently installed is described in Table 3.1.

The components use different methods to communicate, described further inFigure 3.2.

3.2 Program Architecture

The programs running on the Raspberry are the central function, simply calledmain, and several smaller programs called daemons. The daemons run parallelto the central function and handle communication with external components. Ta-ble 3.2 lists the current daemons and Figure 3.3 shows how all programs interact.

Communication between programs is done using User Datagram Protocol(udp). Reading from the udp buffer is blocking, which means that a programattempting to read from the udp buffer will pause until there is a message toread. This is used to set the frequency at which main runs. The imu runs at afrequency of 100 Hz, and the ekf daemon reads from it using another protocolthat is also blocking. As a result, main can only read data from the ekf daemonat a rate of 100 Hz, which is then the frequency of main.

3.2 Program Architecture 13

Table 3.1: Summary of the hardware present on the quadcopter along withdescriptions of their purpose and performance.

Raspberry PiThe central computer that performs the majority of the calculations is a RaspberryPi 3 model B. It is a microcomputer with a quad-core 1.2 GHz processor. It commu-nicates with other modules using I2C, usb and serial communication (Raspberry PiFoundation, 2017).

ESCEach motor is powered and controlled by its own Electronic Speed Controller (esc)that provides 3-phase alternating current. The esc:s keeps each motor rotating at anrpm determined by the pulse width they receive from the pwm board.

PWM boardA PCA9685 pwm controller has been configured to send pulses to each esc, withlength ranging from about one to two milliseconds. A one millisecond pulse corre-sponds to the lowest speed, and two milliseconds to maximum. The pwm controlleruses an internal 25 MHz clock to send these pulses at a frequency of about 400 Hz(NXP Semiconductors, 2015).

ReceiverTo remotely control the quadcopter, an X8R receiver module on the quadcopter re-ceives data transmitted from an X9D+ Taranis remote controller. The receiver hastwo antennas and receives data over eight 2.4 GHz channels (FrSky, 2015).

ArduinoThe data from the receiver is transmitted using a serial port. Since the serial porton the Raspberry is connected to the imu, the receiver is instead connected to anArduino Micro. This Arduino is connected to the Raspberry by I2C, acting as slave.The Arduino also handles all analogue input signals.

IMUThe imu on the quadcopter is an Xsens MTi-3. It is a board with a microcomputer andsensors, consisting of magnetometers, gyroscopes and accelerometers. These measurethe magnetic field, angular rate and acceleration respectively, in three dimensions.The microcomputer can perform various filter algorithms, bias estimation and mag-netometer calibration (Xsens, 2016).

GPSAn M8Q gpsmodule and an active gps patch antenna has been attached to the quad-copter. The antenna is attached to the M8Q module with a coaxial cable, and the M8Qis attached to the Raspberry Pi using an adapter that converts the TTL-232RG serialcommunication to usb-protocol.

LIDARAn SF30-B lidar is attached to the front of the quadcopter, pointing downwards.This allows for accurate, real time measurements of the height. The lidar is capableof outputting measurements with a resolution of three cm at 286 Hz from a micro-usb port, and even higher rates using serial communication. It also has the option foranalogue output (LightWare Optoelectronics, 2016).

14 3 Platform

Requestdatafrom

Receiver

Requestdata

from ekf

Calculatemotorsignals

Sendmotorsignals

Sendcurrentstates

Sendreceiverchannels

Readfrom

Arduino

Readfrom gpsand imu

Sendekf data

Calculatepwm

signals

Sendpwm

signalsto pwmboard

Readmotorsignals

Writeto file

Read logstruct

ReceiverMain EKF PWM Log

Figure 3.3: Flowchart describing the communication of programs on theRaspberry. Filled arrows show the progression of each program. Dashedlines show udp communication, which is blocking.

3.3 LIDAR Measurements 15

Table 3.2: A list of the programs currently running on the Raspberry, anddescriptions of their tasks.

ReceiverReads the receiver channels from the Arduino Micro. Currently also reads the lidarmea-surements.

EKFReads data from the imu and gps, and uses an Extended Kalman Filter (ekf) to estimatethe quadcopter’s states and covariances.

PWMSends the length of the desired pwm pulses to the pwm board.

LogReceives a struct containing the current value of various states and writes to a file.

3.3 LIDAR Measurements

Currently the usb and serial port on the lidar are damaged, so the analogueoutput port is used. The max range is currently set to 8 meters. The analogue todigital converter on the Arduino Micro outputs integers ranging from 0 to 1023,corresponding to a measured voltage from 0 to 5 V (Arduino, 2017). The SF30-Blidar in turn outputs an analogue value from 0 to 2.56 V corresponding linearlyto a distance from 0 to 8 meters (LightWare Optoelectronics, 2016). Ideally, theconversion from integer to distance should then be

distance =integer · 8 · 51023 · 2.56

. (3.1)

4Modeling

The models and controllers implemented by Kugelberg (2016) can successfullyfollow reference angles. However, there are still noticeable oscillations, primar-ily in the pitch controller, that would be advantageous to eliminate. Additionally,the step response of the models of the angular rates p and q resulted in oscilla-tions towards infinity, which is not a realistic behaviour and results in unrealisticbehaviours when simulating the complete model. This chapter will describe thework done to obtain models that better describe the actual behaviour of the sys-tem, and how these models were used to implement new controllers. The methodused to determine a linear model to convert lidar measurements to distanceswill also be shown.

4.1 LIDAR Measurements

The ideal conversion from lidar measurement to distance is described in (3.1).However, there are several intermediate steps affecting the measurement receivedby the Raspberry that might affect the accuracy. To investigate these possible er-rors, the integer acquired by the analogue conversion in the Arduino was recordedat distances measured manually. The integers obtained at these distances can beseen in Figure 4.1. The distances calculated from these measurements using thetheoretical method did not correspond well to the true distances. However, themeasurements seem to be a linear function of the actual distance, so the param-eters of a linear function was estimated by minimising the sum of square errorsof

distance = k · integer + m. (4.1)

Using linear least squares results in k = 0.012686 and m = −0.093157. A new setof data was collected to validate the function, and Figure 4.2 shows the distances

17

18 4 Modeling

calculated using the theoretical conversion and the linear function acquired us-ing least squares.

0.5 1 1.5 2 2.5 3

50

100

150

200

Figure 4.1: Integers obtained by connecting the lidar analogue port to theArduino’s analogue to digital converter.

0 1 2 3

0

1

2

3

4

Figure 4.2: Distances calculated from lidar measurements using differentlinear parameters, plotted against the actual distance at which they wereobtained. There appears to be a small, constant offset at distances after 2meters.

4.2 Model Simplification

To be able to control the position of the quadcopter, fast control of the angles isrequired. The current pid controllers result in some oscillatory behaviour, so lqcontrollers have been implemented using the models for the angular rates.

4.2 Model Simplification 19

The time discrete black-box models in roll, pitch and yaw given in (2.7), (2.8) and(2.9) can be converted to continuous transfer functions on the form

φ(t) = Ap2φ(s)p(t) + Bu2φ(s)uroll(t) + Ce2φ(s)eφ(t),

p(t) = Aφ2p(s)φ(t) + Bu2p(s)uroll(t) + Ce2p(s)ep(t),(4.2)

θ(t) = Aq2θ(s)q(t) + Bu2θ(z)upitch(t) + Ce2θ(s)eθ(t),

q(t) = Aθ2q(s)θ(t) + Bu2q(s)upitch(t) + Ce2q(s)eq(t),(4.3)

ψ(t) = Ar2ψ(s)r(t) + Bu2ψ(s)uyaw(t) + Ce2ψ(s)eψ(t),

r(t) = Aψ2r (s)ψ(t) + Bu2r (s)uyaw(t) + Ce2r (s)er (t),(4.4)

where Ax2y , Bx2y and Cx2y are transfer functions from x to y, ex is noise, and uroll ,upitch and uyaw are the control signals. Kugelberg (2016) compared the black-boxmodels to theoretical transfer functions. These theoretical transfer functions aregiven by

φ = Gφ(s)uroll , p = Gp(s)uroll ,

Gφ(s) =15000

s(s + 30)(s + 1), Gp(s) =

15000(s + 30)(s + 1)

,(4.5)

θ = Gθ(s)upitch, q = Gq(s)upitch,

Gθ(s) =9000

s(s + 30)(s + 1), Gq(s) =

9000(s + 30)(s + 1)

,(4.6)

ψ = Gψ(s)uyaw, r = Gr (s)uyaw,

Gψ(s) =750

s(s + 30)(s + 0.7), Gr (s) =

750(s + 30)(s + 0.7)

.(4.7)

Figure 4.3 shows the step responses of the black-box models and the theoreticaltransfer functions for a control signal with amplitude 0.1. The black-box modelof r seems to have dynamics similar to its theoretical transfer function, while theblack-box models for p and q result in oscillations with an infinitely increasingamplitude. Since the angles can be calculated from the angular rates accordingto (2.4), models for them are not required. Thus, only models for the angularrates are necessary, and according to the theoretical models the dynamics of theangular rates should not depend on the current angle. If this dependency onthe angles is removed, and a step is done in the transfer function from controlsignal to angular rates, the response is stable and has an appearance that moreresembles the theoretical response (see Figure 4.4).

The coefficients of the transfer functions from control signal to angular rateconverted to continuous time can be seen in Table 4.1. These transfer functionshave a large number of coefficients, and would result in a state space model with

20 4 Modeling

many states that can not be measured directly. By creating simplified transferfunctions containing only the roots and poles closest to the imaginary axis, an lqcontroller can be created that controls only the dominating dynamics.

Table 4.1: The coefficients of the numerators and denominators of the con-tinuous transfer functions from the motor signals to the angular rates p, qand r.

Bu2p(s)Coefficient Numerator Denominator

s0 6.018 · 107 2.844 · 107

s1 3.309 · 108 1.435 · 106

s2 5.998 · 106 1.252 · 106

s3 4.329 · 104 2.596 · 104

s4 1.895 · 102 7.247 · 101

s5 0.000 · 100 1.000 · 100

Bu2q(s)Coefficient Numerator Denominator

s0 3.343 · 107 2.145 · 107

s1 1.587 · 108 8.789 · 105

s2 2.870 · 106 1.243 · 106

s3 2.068 · 104 2.622 · 104

s4 9.134 · 101 7.673 · 101

s5 0.000 · 100 1.000 · 100

Bu2r (s)Coefficient Numerator Denominator

s0 1.458 · 108 5.413 · 106

s1 6.201 · 106 6.269 · 106

s2 1.097 · 105 2.438 · 105

s3 9.337 · 102 1.309 · 104

s4 6.377 · 100 6.927 · 101

s5 0.000 · 100 0.000 · 100

The transfer functions for p and q have one root and a pair of poles close tothe imaginary axis, while the transfer function for r has only a pole close to theimaginary axis. For p the root and poles are -0.1824 and −0.3397 ± 4.7907i, andfor q the root and poles are -0.2114 and −0.1717 ± 4.1681i. Using the coefficientsin Table 4.1, the static gain for p, q and r can be calculated as 2.1154, 1.5584 and26.9412, respectively. A transfer function with static gain g, a root in r, and polesin p1 and p2 can be calculated according to

gr s + g

1p1 · p2

s2 + ( 1p1

+ 1p2

)s + 1. (4.8)


0 5 10

0

1

2

3

0 1 2

-1000

0

1000

2000

0 1 2

-1000

0

1000

0 2 4 6 8

0

1

2

3

0 2 4

0

10

20

0 2 4

0

10

20

30

40

Figure 4.3: The step response acquired using theoretical and black-box mod-els for p, q and r, with a step amplitude of 0.1. The only black-box modelwith a convergent step response is r.

22 4 Modeling

A continuous time transfer function on the form

y =a · s + b

c · s2 + d · s + 1u, (4.9)

can be written on state space form as[x1x2

]=

[− dc

1c

−1 0

] [x1x2

]+

[acb

]u,

y = x1.

(4.10)

This results in the state space models[pα

]=

[−0.6794 23.0661−1 0

] [pα

]+

[267.4527

2.1154

]uroll , (4.11)[

qβ

]=

[−0.3433 17.4029−1 0

] [qβ

]+

[128.2951

1.5584

]upitch, (4.12)

where α and β are states that are not measured. Using (4.11) and (4.12) α and βcan be expressed as

α =

t∫0

(2.1154up − p)dt, (4.13)

β =

t∫0

(1.5584uq − q)dt. (4.14)

The transfer function for r has a pole at -0.8929. A transfer function on the form

y =a

b · s + 1u, (4.15)

can be expressed on state space form as

x = −1bx +

abu,

y = x,(4.16)

resulting in the state space model

r = −0.8929r + 26.9412uyaw. (4.17)

Figure 4.4 shows the step responses of the transfer functions from the black-boxmodel and simplified state space models. The figure shows that they are nearlyidentical.


0 2 4 6 8 10

-2

0

2

4

0 2 4 6 8 10

-2

0

2

0 2 4 6 8 10

0

1

2

3

Figure 4.4: Response of a step with amplitude 0.1 in the motor control signalfor both the original and the simplified models of p, q and r without angledependencies. The models for r have less oscillatory dynamics and highergains than the models for p and q.

24 4 Modeling

Assuming that the quadcopter is close to hovering, states for the Euler angles areimplemented according toφp

α

=

0 1 00 −0.6794 23.06610 −1 0

φpα

+

0267.4527

2.1154

uroll , (4.18)

θqβ

=

0 1 00 −0.3433 17.40290 −1 0

θqβ

+

0128.2951

1.5584

upitch, (4.19)

[ψr

]=

[0 10 −0.8929

] [ψr

]+

[0

26.9412

]uyaw (4.20)

Various lq controllers for [φ p] and [θ q] were calculated by solving the continuous-time algebraic Riccati equation (2.19). However, none of the generated lq gainswere able to stabilise control of the angles during practical tests, and the quad-copter was unable to get airborne. Hence, the old pid controllers have been keptfor roll and pitch.

When implementing the lq controller for [ψ r], an additional state represent-ing the integration of the error,

∫ t0 ψdt = Ψ has been added, resulting in the state

space model Ψψr

=

0 1 00 0 10 0 −0.8929

Ψψr

+

00

26.9412

uyaw. (4.21)

To generate an lq gain, (2.19) and (2.21) was used with the model in (4.21). TheQ and R matrices

Q =

1 0 00 1 00 0 0.1

, R = 10, (4.22)

were tested, resulting in the lq controller

Kr =[0.3162 0.4939 0.1903

], uyaw = −Kr

Ψ − Ψ ref

ψ − ψrefr

, (4.23)

where ψref is the reference for ψ and Ψ ref =∫ t

0 ψref dt. The Bode Diagram forthis controller can be seen in Figure 4.5.

The phase margin φm is 72.258 degrees, and the cross-over frequency is 5.36rad/s. This lq controller resulted in stable control of the yaw angle, and has re-placed the previous PI controller. The measured yaw angle and reference duringa test flight can be seen in Figure 4.6.


10-2

10-1

100

101

-20

0

20

40

60

80

10-2

10-1

100

101

-180

-160

-140

-120

-100

Figure 4.5: Bode Diagram of the open loop from ψref to ψ.

150 200 250

10

15

20

Figure 4.6: Plot of the measured yaw angle when following a reference yawangle using the lq controller.

26 4 Modeling

4.3 Simulation Environment

In order to test algorithms and controllers, a complete model of the quadcopterhas been created in Simulink. It contains all models for the angular rates anddetermines the Euler angles using integration and the rotation matrix for angularrates according to (2.4). Using a quadratic model of drag from Ljung and Glad(2004) and Newtons second law, the position p, velocity v and acceleration a inthe global coordinates [x y z] are simulated according to

ax =Tx − αv2

x

m, vx =

t∫0

axdt, px =

t∫0

vxdt,

ay =Ty − αv2

y

m, vy =

t∫0

aydt, py =

t∫0

vydt,

az =Tz + mg − αv2

z

m, vz =

t∫0

azdt, pz =

t∫0

vzdt,

(4.24)

where α describes the drag and m is the mass of the quadcopter. The thrust Tx,Ty and Tz are the amounts of the quadcopter’s thrust parallel to the different co-ordinate vectors. The angular velocity of the motors has a rise time due to inertiaand drag. This rise time has been estimated as approximately 33 ms by saabDynamics and is included in the simulation. The angular rates p, q and r arecalculated using the previously determined black-box models. The Euler anglesare not calculated using black-box models, but are instead calculated as the in-tegration of the rotated rates according to (2.4). Using this Simulink structure,different quadcopter models and controllers can be tested and evaluated.

5Position Estimation

Estimation of the quadcopter’s states requires the implementation of sensor fu-sion algorithms. Horizontal position estimation is done using an ekf programthat can access the imu and gps to estimate the quadcopter’s states. However,this ekf does not support the use of a lidar, so height estimation using lidarmeasurements has been implemented. Additionally, the magnetometers on theimu have been calibrated to compensate for external disturbances.

5.1 Extended Kalman Filter

The ekf available is a program written in C that uses measurements from an imuand optionally a gps to estimate Euler angles and angular rates along with posi-tion, speed, and acceleration in three dimensions. The position format is latitudeand longitude in radians along with altitude. The output angles are Euler angles,and the angular rates, velocities and accelerations are given in the quadcopter’sbody-fixed coordinate system.

5.2 Height Estimation

Measurements of the quadcopter’s height can be done using the accelerometersand the lidar. Both these measurements requires knowledge of the current ori-entation of the quadcopter to acquire the vertical components of the measuredacceleration and distance. Assuming that vertical velocities are small enoughthat drag can be neglected, all dynamics are linear and a Kalman filter can beused to estimate vertical velocity and smooth the height measurements. ThisKalman filter uses the lidarmeasurements and the ekf output. This means thatthe acceleration has already been subject to filtering, and if the ekf is not fast

27

28 5 Position Estimation

enough the time delays could result in an unstable system.In Figure 5.1, the raw acceleration is compared to the difference between raw

and filtered acceleration, and the cross-covariance of the raw and filtered accelera-tion can be seen in Figure 5.2. The raw accelerometer data is not free accelerationsince it includes the gravitational acceleration. To obtain comparable signals, thex-component of the rotated gravitational acceleration has been subtracted fromthe raw measurements.

60 70 80 90

-10

-5

0

60 70 80 90

-0.15

-0.1

-0.05

0

0.05

0.1

0.15

0.2

Figure 5.1: The plot to the left shows the raw acceleration measured during atest flight, and the plot to the right shows the difference between the raw andfiltered acceleration. As the difference is orders of magnitude smaller thanthe measurements, the raw and estimated acceleration are almost identical.

As the difference between raw and filtered acceleration is small, and the cross-covariance has a dominating peak at 0 lags, it seems that time delays caused bythe filter are shorter than the sample time of the controller. Hence, it is safe to usethis filtered signal. Additionally, the ekf estimates the bias for all accelerometerswhich makes the measurements more accurate.

5.3 Height Kalman Filter

In order to acquire an estimate of the vertical velocity v, a smoothed estimate ofthe height h and an estimate of the thrust required to hover u0, a Kalman filterwas implemented using measurements from the accelerometers and the lidar.A state space model has been derived, and a noise analysis has been done toestimate the noise covariance.

5.3 Height Kalman Filter 29

-50 0 50

0

1000

2000

3000

4000

Figure 5.2: The covariance between filtered and raw acceleration, showing aclear peak at 0 lags.

5.3.1 Model Structure of Height

The relation between the acceleration a, velocity v and height h is linear. Thechange in acceleration is considered to be process noise. Measurements for accel-eration and height are available. A continuous model can be written as

x =[h v a

]T,

x(t) =

0 1 00 0 10 0 0

x(t) +

001

v(t),

y(t) =[1 0 00 0 1

]x(t) +

[e1(t)e2(t)

],

(5.1)

where e1(t) is measurement noise in the lidar, e2(t) is measurement noise inthe accelerometers and v(t) is process noise. Transforming this model to discretetime results in

x =[h v a

]T,

x[k + 1] =

1 T T 2

20 1 T0 0 1

x[k] +

T 3

6T 2

2T

v[k],

y[k] =[1 0 00 0 1

]x[k] +

[e1[k]e2[k]

],

(5.2)

where e1[k] is measurement noise in the lidar, e2[k] is measurement noise in theaccelerometers and v[k] is process noise (Gustafsson, 2012). The distance andacceleration measurements are in the quadcopter’s local coordinates, and are ro-tated into the global coordinate system. Hence, the noise of the measurementsdepend on the current orientation and will vary with time. For the sake of sim-


plicity, it is assumed that the influence of the angles is negligible and that thenoise can be approximated as Gaussian.

The lidar is located 0.122 meters in front of the centre of the imu. Thisdistance and the current orientation of the quadcopter needs to be consideredwhen calculating the height. Using the rotation matrix R defined in (2.2), theunit vectors of the quadcopter’s body-fixed coordinate system [XQ YQ ZQ] andthe unit vectors of the global coordinate system [xg yg zg ] can be expressed as

xg =[1 0 0

]T, yg =

[0 1 0

]T, zg =

[0 0 1

]T,

XQ = Rxg , YQ = Ryg , ZQ = Rzg .(5.3)

The height h can be calculated from the lidar distance d as

h = d(ZQ • zg ) + 0.122(XQ • zg ), (5.4)

where the scalar product is used to determine the cosine of the angle between thevectors. The vertical acceleration a in global coordinates is similarly acquired byrotating the measurements from the quadcopter according to

aQ =[aX aY aZ

]T,

a = −RaQ • zg ,(5.5)

where aX , aY and aZ are accelerometer measurements in the quadcopter. Thesign is inverted since quadcopter acceleration is measured in an ned system andthe height is measured upwards from the ground.

5.3.2 Static Thrust Estimation

The LiPo-battery currently used can power the quadcopter for approximately tenminutes during flight. It was discovered that as the battery is depleted, the thrustsignal necessary for constant height, and thus the point around which the heightcontroller should operate, increases. This static thrust is also different for differ-ent batteries, and will change if weight is added or removed from the quadcopter.While an integrating action in the controller avoids stationary height errors, itis more efficient to constantly be aware of the current operating point. Hence,a state has been included in the Kalman filter that estimates the current staticthrust.

There are various methods to estimate the thrust generated from the motors.The thrust can be expressed as proportional to the square of the motor angularvelocity, or as a function of various factors such as relative air velocity (Luukko-nen, 2011; Spakovszky, 2007). Previous models of the quadcopter platform byKugelberg (2016) assumed that the thrust was proportional to the motor signal,implying that the square of the rpm is proportional to the motor signal. Thisassumption will be used in this thesis as well. At motor signal 0 the total thrustis also 0, and at motor signal u0 it is mg. Thus the relation between the throttle


control channel uthrottle and the force from the motors FT hrust can be written as

FT hrust =uthrottlemg

uo. (5.6)

The vertical acceleration a in global coordinates can be expressed as

a =mg − FT hrustcφcθ

m= g(1 −

uthrottlecφcθu0

) (5.7)

and u0 can be expressed as

u0 =uthrottlecφcθ

1 − ag

. (5.8)

This expression will only be useful as long as there is some amount of thrustdirected upwards. If this is not the case, software fail-safes have been included toprevent the update of the static thrust state in the measurement update. However,being in a free fall is not a situation that should occur during normal operation.The progression of this state during a test flight, initialised with value 0.75 anduncertainty 0.001, can be seen in Figure 5.3.

320 325 330 335 340 345

0.5

0.6

0.7

0.8

0.9

1

Figure 5.3: Measurements of static thrust signal compared to the filteredestimate during a test flight.


Using the model in (5.2) and the measurements in (5.4), (5.5) and (5.8), the com-plete model used in the Kalman filter is

x =[h v a u0

]T,

y =

d(ZQ • zg ) + 0.122(XQ • zg )

−zg • RaQuT hrustcφcθ

1− ag

,

x[k + 1] =

1 T T 2

2 00 1 T 00 0 1 00 0 0 1

x[k] +

T 3

6 0T 2

2 0T 00 T

[v1[k]v2[k]

],

y[k] =

1 0 0 00 0 1 00 0 0 1

x[k] +

e1[k]e2[k]e3[k]

.

(5.9)

The update of the static thrust is conditional. Close to the ground the quadcopteris subject to what is called the ground effect, which increases the lift (Danjun et al.,2015). Hence, the first condition is that the quadcopter needs to be more than0.3 meters above the ground. The second condition, to avoid division by 0 is thata , g. Finally, the quadcopter needs to be close to hovering, so the last conditionis that the angles and angular rates in pitch and roll are within certain intervals.These intervals are

∣∣∣φ∣∣∣ < 0.2, |θ| < 0.2, |p| < 0.5, |q| < 0.5. If these conditions arenot met, the measurement of u0 and the final row in H is set to 0.

5.3.3 Noise Estimation

To acquire the covariance matrix R, the covariance of the measurements were es-timated individually. When the quadcopter is in flight, the variations in heightmeans that the quadcopter does not maintain a height constant enough to esti-mate the noise covariance. Hence, the noise estimation in height measurementswas done while the quadcopter was stationary on the ground. The data used toestimate the measurement noise in acceleration and static thrust was howeverdone in flight. The static thrust can only be calculated while the quadcopter isin flight, and vibrations from the motors will result in noise that is only presentduring flight.

A segment of data from when the quadcopter remained stable for about 14seconds was used to estimate noise covariance in acceleration and static thrust,and 50 seconds of recorded data was used to estimate the noise covariance inthe height measurement. The autocovariance of these segments was estimatedto determine if the noise was coloured or white. Histograms of the segmentswere used to determine whether the noise is Gaussian. The data can be seenin Figure 5.4, the autocovariance of the data can be seen in Figure 5.5, and thehistograms can be seen in Figure 5.6.


1000 2000 3000 4000 5000

0.13

0.14

0.15

0.16

200 400 600 800 1000 1200 1400

-1

0

1

200 400 600 800 1000 1200 1400

0.7

0.75

0.8

0.85

0.9

Figure 5.4: The data used to estimate measurement noise covariances. Someoscillatory elements are present in the acceleration and u0 measurements.


-200 -150 -100 -50 0 50 100 150 200

0

0.05

0.1

0.15

-200 -150 -100 -50 0 50 100 150 200

0

100

200

300

-200 -150 -100 -50 0 50 100 150 200

0

0.5

1

1.5

2

Figure 5.5: The autocovariance of the data used to estimate measurementnoise covariances.


0.12 0.13 0.14 0.15 0.16 0.17

0

100

200

-2 -1 0 1 2

0

20

40

60

0.65 0.7 0.75 0.8 0.85 0.9 0.95

0

20

40

60

Figure 5.6: Histograms of the measurements and bell curves calculated us-ing the estimated variance and mean of the measurements. The bell curveshave been scaled in order to be comparable to the histograms.

According to Gustafsson et al. (2001), the autocovariance of white noise shouldbe 0 except for a peak at 0 lags. The autocovariance of the height measurementsis noise with low variance except for a peak at 0 lags, and thus the noise of theheight measurements is said to be white. In the autocovariances of the acceler-ation and u0 measurements however there are recurring peaks, suggesting thatthe noise is coloured. However, as the peaks at 0 lags dominate over the recurringpeaks, the noise in these measurements is still approximated as white. The noise


covariances achieved from the estimations are

cov(e1) = 3.7268 · 10−5, cov(e2) = 2.8153 · 10−1, cov(e3) = 1.6978 · 10−3. (5.10)

Comparing the histograms with bell curves calculated using the covariances andmeans of the signals shows that the noise in height measurements closely resem-bles Gaussian noise, and the noise in acceleration and u0 is approximately Gaus-sian. Thus, the noise in height measurements is expected to be possible to fairlyaccurately be approximated as white Gaussian noise.

The calculated covariances are used as the diagonal elements in the R matrixof the Kalman filter, described in (2.11). The covariance of the process noise wasnot determined through measurements, but was instead selected as values thatresulted in good state estimates during post processing of measurements. Settingthe diagonal values of the Q matrix in the Kalman filter in (2.11) to [10 0.00001]results in satisfactory estimations.

5.4 Magnetometer Calibration

To obtain an estimate of the yaw angle that does not drift over time, the mag-netometers on the imu are used. These are however sensitive to disturbancesfrom magnetic alloys and magnetic fields from wires or electromagnets (Konva-lin, 2009). The magnetometers can be calibrated to compensate for these distur-bances, as long as the disturbances remain stationary relative to the magnetome-ters. The magnetometers will still be affected by disturbances not located on thequadcopter, but these effects are negligible when flying in an open field.

To calibrate the magnetometers, a program on the imu called Inflight Com-pass Calibration (icc) is used (Xsens, 2016). The ekf program has been modifiedin order to activate this function when the gps is not active. The icc gathers datafrom the magnetometers for 100 seconds. According to the Xsens KnowledgeBase (2017), the magnetometers should be moved through as many orientationsas possible during this time for accurate calibration. After 100 seconds the iccwill calculate calibration parameters which are then saved on the imu.

Currently the calibration is performed by a user holding the quadcopter byhand, in order to reach more extreme angles. The motors generate a noticeablemagnetic field while running, so the calibration procedure is done while the mo-tors are running at an rpm close to what is required to hover. However, the mo-tors require less power and generate a weaker magnetic field without propellersattached. Therefore, four plastic bars with shape and weight similar to the pro-pellers are attached during calibration as a safer alternative than using the actualpropellers (see Figure 5.7). With these mock-up propellers and the motors run-ning, the conditions during calibration are similar to the conditions during flight.

5.4 Magnetometer Calibration 37

Figure 5.7: A comparison of an actual propeller and the substitute used dur-ing calibration.

To analyse the result of a calibration, the magnetic norm can be studied. Ideally,the norm of the magnetic measurements should always be 1 regardless of orienta-tion (Xsens Knowledge Base, 2017). If the magnetometers are correctly calibratedthe norm should not deviate substantially from 1 when moving the quadcopterthrough various orientations. The norm of the magnetometers while varying thequadcopter orientation before and after calibration can be seen in Figure 5.8.

20 40 60 80 100 120

0.4

0.6

0.8

1

1.2

Figure 5.8: The magnetic norm while changing the quadcopter orientationbefore and after calibration. The calibration takes effect at around 108.47seconds.

The figure shows the magnetometer norm while the motors are running, both be-fore and after calibration. It can be seen that there is an offset in the magnetome-ter norm before and after calibration. However, after calibration the magnitudeof this offset is reduced, and the variation of the norm when altering the orienta-tion has been significantly reduced. A side effect is that the magnetometers are


improperly calibrated when the motors are off and may result in incorrect head-ing estimation when the motors are inactive. However, as long as the headingis estimated accurately while the quadcopter is in flight the performance shouldnot be affected.

6Position and Trajectory Control

The control of the position has been divided into two parts, horizontal and ver-tical. To control the vertical position, a pid controller and an lq controller havebeen implemented. For horizontal movement, a position controller that main-tains constant position and a trajectory controller that maintains constant veloc-ity have been implemented. The vertical controllers generate a control signal forthrust, and the horizontal controllers generate reference angles used by the anglecontrollers. The estimates used by the controllers and the control signals theygenerate can be seen in Figure 6.1.

6.1 Height Control

To maintain a height reference, two different controllers have been implemented:one lq controller and one pid controller. Both controllers use two feed-forwardcomponents: the static thrust and the angle of the quadcopter. Feed-forwardingthe static thrust means that the controllers do not have to compensate for thegravitational acceleration and only control deviations from the reference height.A larger roll and/or pitch angle will require more thrust to stay airborne, andfeed-forwarding this angle means the controllers will not have to compensate asmuch for this.

The signal generated by the controllers, ucontroller , is added to the signal thatmaintains constant acceleration, u0. The throttle signal uthrottle is then generatedby compensating for the current roll and pitch angles according to

uthrottle =u0 + ucontroller

zg • ZQ, (6.1)

where zg and ZQ are described by (5.3).

39

40 6 Position and Trajectory Control

HightKalman

filter

Heightcontroller

EKFPosition

controller

Anglecontroller

Quadcopter

pz

vz

az

θ φ

px, py

vx, vy

p, q, r

φ, θ, ψ

φref θref ψref

uthrottle

uroll

upitch

uyaw

Reference height

Trajectory

LIDAR

Acceleration

IMU

GPS

Figure 6.1: Flowchart showing how the measurements are used to gener-ate state estimates, which in turn are used to generate reference angles andcontrol signals.

6.1.1 PID

By using the height and vertical velocity estimated by the Kalman filter given by(5.9), a pid controller for the height has been implemented according to

e = href − h, (6.2)

uP ID = KP e + KI

t∫0

(e)dt − KDv. (6.3)

To prevent integrator wind-up, conditional integration is used. The condition forintegration is that uthrottle must be in the interval u0 ± 0.25. This interval waschosen since u0 ≈ 0.75 and uthrottle is limited to [0, 1], and the interval whereintegration is allowed should be approximately symmetrical around u0 in orderto handle positive and negative height deviations equally. To determine the sta-

6.1 Height Control 41

bility of the system, the relation between uthrottle and the height h has been deter-mined. The following calculations are done assuming that φ and θ are small, andthat the angle feed-forward will handle angular deviations. Assuming that thethrust from the motors is a linear function of the control signal, the thrust forceis Fthrottle = Ktuthrottle, for some parameter Kt . When hovering, uthrottle = u0,and Fthrottle = mg. Hence,

Ktu0 = mg =⇒ Kt =mg

u0. (6.4)

Also, using (6.1) and F = ma,

a =Fm

=Ktuthrottle −mg

m=Ktu0 + KtuP ID −mg

m=KtuP IDm

=uP IDg

u0. (6.5)

Using uP ID described in (6.3), a differential equation can be written as

a = h =g

u0(KP (href − h) +

t∫0

(href − h)dt − KD h) =⇒

u0

gh + KP h + KI

t∫0

hdt + KD h = KP href + KI

t∫0

href dt.

(6.6)

This can be rewritten into a transfer function according to

(u0

gs3 + KD s

2 + KP s + KI )h = (KP s + KI )href =⇒

h =KP s + KI

u0g s

3 + KD s2 + KP s + KIhref .

(6.7)

u0 is not constant but is commonly in the interval of 0.7-0.8. The transfer func-tion in (6.7) can be used to determine the roots and poles of the closed loop sys-tem, as well as simulating a step response for various values of u0. Various pa-rameters were tested, and the simulated step response with the values KP = 0.2,KI = 0.05 and KD = 0.2 resulted in a step response that was deemed sufficientlyfast with acceptable overshoot. Step responses for different values of u0 can beseen in Figure 6.3, and Bode Diagrams can be seen in Figure 6.2.


Figure 6.2: The Bode Diagram of the pid controller for different values of u0.As u0 increases the phase is constant but the magnitude decreases, decreas-ing the phase margin.

0 5 10 15 20

0

0.5

1

Figure 6.3: The simulated step response after a step in the reference heightusing a pid controller for different values of u0. As u0 increases, the settlingtime decreases but the overshoot increases.

The phase margin φm and cross-over frequency ωc for different values of u0 canbe seen in Table 6.1. While the phase margin decreases as u0 increases, the size


of the phase margin still allows for sufficient tolerance against time delays (Gladand Ljung, 2006).

Table 6.1: The phase margins and cross-over frequencies for different valuesof u0. As u0 increases, φm and ωc decreases.

u0 φm [deg] ωc [Rad/s]0.6 72.99 3.34

0.75 69.06 2.710.9 65.31 2.29

The result of a test flight using these parameters can be seen in Figure 6.4.

230 240 250 260

0

0.2

0.4

0.6

0.8

Figure 6.4: The measured height while following a reference using a pidcontroller. The angles were controlled manually to maintain a constant po-sition.

The pid controller successfully follows a reference height, although it suffersfrom a large overshoot at takeoff. This may be caused by the ground effect result-ing in unexpectedly powerful lift before the quadcopter has cleared the ground.

6.1.2 LQR

To create an lq controller of the height, a state space model is required. In themodel, drag is neglected and the input is a force instead of a control signal. Thereason for this is that since u0 is not constant, the relation between the statesand control signal is not linear. However, the relation between the states and thegenerated force is linear. The model is given byIhh

v

=

0 1 00 0 10 0 0

Ihhv

+

001m

Fd , (6.8)


where Fd is the deviation from the hovering thrust and Ih is the integral of h. Anlqr feedback gain KFd can be calculated after selecting suitableQ and Rmatricesaccording to Section 2.4. Using (6.4), uLQR can be calculated as

Fd =uLQRmg

u0=⇒ uLQR =

Fdu0

mg, (6.9)

which gives that

uLQR = −KLQR

Ihhv

= − u0

mgKFd

Ihhv

. (6.10)

To find a suitable gain matrix, the step response for different matrices R and Qwas simulated. Since the control signal is generated using feedback from the ve-locity, height and integration of the height, the resulting LQ controller is basicallya PID controller with KP = KLQR(2), KI = KLQR(1) and KD = KLQR(3). Therefore,the transfer function given by (6.7) can be used to simulate the step response.Since KLQR is proportional to u0, the step response will be the same regardless ofthe value of u0.

Using Q with diagonal elements [5 100 10] and R = 5 resulted in the gain ma-trix KFd = [1 5.1239 3.1270]T , and the step response in Figure 6.5 was acquired.The accompanying Bode Diagram can be seen in Figure 6.6. The phase marginφm was 68.99 degrees and and the cross-over frequency ωc was 4.34 rad/s, show-ing a resistance against time delays that does not vary with u0.

0 5 10 15 20

0

0.5

1

Figure 6.5: The simulated step response after a step in the reference heightusing an lq controller.


10-2

10-1

100

101

0

50

100

10-2

10-1

100

101

-250

-200

-150

-100

Figure 6.6: The Bode Diagram for the lq controller. The Bode Diagram andphase margin does not depend on u0.

305 310 315 320 325 330 335

0

0.2

0.4

0.6

Figure 6.7: The measured height while following a reference using an lqcontroller. The angles were controlled manually to maintain a constant po-sition.

Comparing the simulated step responses from the lq and pid controller, the lqcontroller is faster and has a smaller overshoot. Although the lq controller hasa longer settling time, it is not affected by variations in u0. Depending on u0,


the pid controller has a higher or lower phase margin than the lq controller.The measured height and reference height during a test flight can be seen inFigure 6.7. Compared to the pid controller, the lq controller seems to have lessovershoot at takeoff, and after takeoff appears to be faster without overshooting.

6.2 Position Controller

Control of the quadcopter’s horizontal position is performed by calculating thereference angles necessary to direct the thrust required to steer the quadcoptertowards the desired position. The measurements used are the quadcopter’s ro-tated velocity along with longitude and latitude measurements. The controllersinteraction with the remaining system can be seen in Figure 6.1.

6.2.1 Assumptions

When developing the models and expressions for the quadcopter’s translationaldynamics, a number of simplifications and assumptions have been made.

It is assumed that the quadcopter always is close enough to its starting loca-tion that the small-angle approximation can be used to approximate longitudeand latitude angles as Cartesian coordinates. It is also assumed that the quad-copter is moving slowly enough that drag can be neglected. To make calculationsregarding the direction of the quadcopter’s thrust simpler, it is assumed that allthrust is directed along the quadcopter’s negative Z axis and that the magnitudeof the thrust along the global negative z axis is mg, where m is the mass of thequadcopter.

6.2.2 Local Map

Latitude and longitude measurements are received from the gps. The latitudeis the angle from the equator to the current position, and longitude is the anglefrom a latitude line passing through Greenwich, England to the current position(Ordnance Survey, 2016). The circle parallel to the equator at the current latitudeis called a parallel. The circumference of the parallels decrease closer to the poles.While the latitude and longitude measurements are spherical coordinates, in asmall enough area they can be approximated as Cartesian coordinates using thesmall-angle approximation. However, two things need to be taken into consider-ation when doing this approximation: the radius at the starting position and therelation between longitude and latitude.

The current radius is calculated using the starting latitude and longitude latsand lons. By assuming that the Earth is an ellipsoid with a polar radius rpo andequatorial radius req of approximately 6356.8 km and 6378.1 km respectively, thecurrent radius is calculated as

r =√

(req cos(lats))2 + (rpo sin(lats))2. (6.11)

6.2 Position Controller 47

The conversion from longitude to position depends on the current latitude. Athigher latitudes an increase or decrease in longitude corresponds to a shorter dis-tance than on low latitudes, since the parallel has a smaller circumference. Bydefining a coordinate system with origin at the quadcopter’s starting position,with the x axis pointing north and the y axis pointing east, the small-angle ap-proximation can be used to convert the difference between the current latitudelat and lats as the x position, and the difference between the current longitudelon and lons as the y position. The radius of the parallel that lonmoves alongis r cos(lat), so the quadcopter’s position can be expressed as

px = r(lat − lats), (6.12)

py = r cos(lat)(lon − lons), (6.13)

where lat and lon are the current latitude and longitude measurements.

6.2.3 LQ Controller

The control of the position is done in two steps. First, lq controllers are usedto calculate the thrust in the x and y directions of the local map to maintain aposition. Then, using the assumption that there is always a thrust force withmagnitude mg directed upwards in the local map, angles to direct the desiredamount of thrust in the x and y directions can be calculated.

The state space models for horizontal motion, neglecting the effect of drag,are [

pxvx

]=

[0 10 0

] [pxvx

]+

[0g

]ux,[

pyvy

]=

[0 10 0

] [pyvy

]+

[0g

]uy ,

(6.14)

where ux and uy are thrust forces in the x and y direction respectively, normalisedwith the lift force so that u = 1 corresponds to a thrust force mg. Various lqfeedback gains were implemented according to (2.19) and (2.21), using the modelin (6.14) and different R and Q matrices. After evaluating the performance ofthese controller it was determined that using a Q matrix with diagonal elements[1 0.5] and R = 20 resulted in a controller with desirable behaviour. With thesematrices, the lq controllers are

ux = −[0.2236 0.2657

] [pxvx

],

uy = −[0.2236 0.2657

] [pyvy

],

(6.15)


6.2.4 Reference Angle Calculations

Using the definitions in (2.1) and (2.2), rotating the quadcopter’s negative Z-axis,which is the direction of the thrust, into the global coordinate system yields

R

00−1

=

−sψsφ − cψcφsθcψsφ − sψcφsθ−cθcφ

=⇒

R

00− 1cθcφ

=

−sψ sφ−cψcφsθ

cθcφcψ sφ−sψcφsθ

cθcφ−1

.(6.16)

This means that if the normalised thrust of the quadcopter is 1cθcφ

, the normalised

thrust in the global coordinates [x y z] is given by (6.16). Since the signals fromthe controller in (6.15) represent desired normalised thrust in x and y, the anglesφref and θref need to be calculated so that

ux =sψsφ + cψcφsθ

cθcφ, (6.17)

uy =−cψsφ + sψcφsθ

cθcφ. (6.18)

The reference angle for ψ is controlled by a user with a remote controller, andcurrent measurements of ψ are used to calculate φref and θref .

The quadcopter has limited motor power available, so the assumption thatthere is always a force mg directed upwards in global coordinates is only validwithin a certain interval. To ensure that the quadcopter does not attempt to keepitself at an angle where it can not maintain its height, K is defined as

K = min(Kmax, u2x + u2

y ), (6.19)

where Kmax is a constant that will be determined later. To preserve the length anddirection of the desired thrust, the following two conditions must be satisfied:

uxuy

=sψsφ + cψcφsθ−cψsφ + sψcφsθ

, (6.20)(sψsφ + cψcφsθ

cθcφ

)2

+(−cψsφ + sψcφsθ

cθcφ

)2

= K. (6.21)

It can be shown that(sψsφ + cψcφsθ

)2+

(−cψsφ + sψcφsθ

)2=

s2ψs2φ + 2sψsφsθcψcφ + c2

ψc2φc

2θ + c2

ψs2φ − 2sψsφsθcψcφ + s2ψc

2φs

2θ =

(s2ψ + c2ψ)s2φ + (s2ψ + c2

ψ)c2φs

2θ = s2φ + c2

φs2θ ,

(6.22)

6.2 Position Controller 49

so (6.21) can be rewritten into

s2φ + c2φs

2θ = Kc2

θc2φ = K

(1 − s2θ

)c2φ = K

(c2φ − s

2θc

2φ

), (6.23)

s2θc2φ(1 + K) = K

(1 − s2φ

)− s2φ =⇒ s2θc

2φ =

K1 + K

− s2φ. (6.24)

Equation 6.20 can be rewritten as

cφsθ(uxsψ − uycψ

)= sφ

(uy sψ + uxcψ

), (6.25)

and by squaring the left and right hand sides, (6.24) can be substituted into theleft hand side, which yields( K

1 + K− s2φ

) (uxsψ − uycψ

)2= s2φ

(uy sψ + uxcψ

)2, (6.26)

which can be simplified and rewritten into

K1 + K

(uxsψ − uycψ

)2= s2φ

(u2x + u2

y

). (6.27)

With this, the reference angle φref can be calculated as

φref = sin−1

√

K

(1 + K)(u2x + u2

y

) (uxsψ − uycψ

) , (6.28)

unless u2x + u2

y = 0, in which case φref is set to 0. Equation 6.23 can be rewrittenas

s2φ + c2φ

(1 − c2

θ

)= 1 − c2

φc2θ = Kc2

φc2θ =⇒ c2

φc2θ =

11 + K

. (6.29)

The angle θref can be calculated by using the calculated value of φref accordingto

s = sign(uy sψ + uxcψ

),

θref = s cos−1

1

cφ√

1 + K

. (6.30)

To determine the value of Kmax, the angle between the global z axis and the quad-copter’s Z axis is defined as γ , and the max angle the position controller willoutput is defined as γmax. The normalised thrust vector is assumed to have mag-nitude 1 along the z-axis. When the angles are saturated, the normalised thrustvector will have absolute length Tmax, and the length of the normalised thrust vec-tor in the [x y] plane will be Tmax sin (γmax). From Pythagoras theorem it followsthat

T 2max = 12 + T 2

max sin2 (γmax) =⇒ T 2max =

1

1 − sin2 (γmax), (6.31)


meaning that Kmax should be

Kmax =sin2 (γmax)

1 − sin2 (γmax). (6.32)

An illustration of the forces can be seen in Figure 6.8.

max

1

√Kmax

Tmax

Figure 6.8: Illustration of the normalised forces at the maximum allowedangle γmax.

As mentioned in Section 6.1.1, the static thrust signal u0 is usually between 0.7and 0.8. This means that the quadcopter needs about 70 to 80% of maximumthrust to maintain height. The maximum angle where this amount of thrust canbe directed upwards is cos−1 (0.8) ≈ 36.86◦. The maximum angle allowed is cur-rently set to 10◦ to allow for large safety margins in case of overshoot in the anglecontrollers, and to add robustness towards external influences on the angles.

The performance of the position controller during a step in reference positioncan be seen in Figure 6.9 and Figure 6.10. These figures show that using theposition controller, the quadcopter can travel to the new position while avoidingovershoot. However, the lack of integral action makes the position controllerssusceptible to stationary errors if external forces such as wind are present. As canbe seen in Figure 6.10 the quadcopter does not reach the desired y coordinates,suggesting that there is more wind in the y direction than the x direction.

6.3 Position Following 51

110 120 130 140 150 160

4

5

6

7

Figure 6.9: The measured x position during a step in reference position usingthe position controller. The initial x position is 3.2536 and the step height is3.5482.

110 120 130 140 150 160

-6

-5

-4

Figure 6.10: The measured y position during a step in reference positionusing the position controller. The initial y position is -3.1468 and the stepheight is -3.5325.

6.3 Position Following

In order to allow an operator to control the position of the quadcopter, the lqcontrollers have been modified such that they follow a reference point instead of


maintaining position [0 0], according to

ux = −[0.2236 0.2657

] [px − rxvx

], (6.33)

uy = −[0.2236 0.2657

] [py − ryvy

]. (6.34)

(6.35)

The reference position [rx ry] is changed by using the left stick on the remotecontroller. The vertical position of the stick controls the speed of rx, and thehorizontal position of the stick controls the speed of ry . The maximum speedof rx and ry is 1 m/s. The Bode Diagram of the reference following positioncontroller can be seen in Figure 6.11.

10-2

10-1

100

101

-20

0

20

40

60

80

10-2

10-1

100

101

-180

-160

-140

-120

-100

Figure 6.11: Bode diagram of the the position controller with reference fol-lowing. The phase margin is 72.868 degrees and the cross-over frequency is2.73 rad/s.

The Bode Diagram shows that the phase margin is 72.87 degrees and the cross-over frequency is 2.73 rad/s.

The position controller can also follow a path by defining a list of coordinates.The quadcopter will move to each coordinate by setting it as the reference posi-tion, starting with the first coordinate in the list. When the quadcopter is within acertain distance of the current reference position the next coordinate in the list is

6.3 Position Following 53

set as the reference position. To illustrate this, a path in a figure eight shape wascreated. The distance between coordinates was approximately 0.1 meters, andthe distance where the reference position is updated was set to 1 meter. The mea-sured position and velocity while following this path can be seen in Figure 6.12and Figure 6.13, respectively.

-10 -5 0 5 10

-10

-8

-6

-4

-2

0

2

4

6

8

Figure 6.12: The measured position while following reference points spacedapproximately 0.1 meter apart, roughly shaped like a figure eight. The sta-tionary errors suggest the presence of wind from the northwest.

The quadcopter manages to maintain a velocity that piecewise seems to be within0.1 m/s of some constant velocity. The velocity can be increased or decreased byincreasing or decreasing the threshold, as the lq controller will generate a greatercontrol signal when the destination is further away. The quadcopter mostly man-ages to stay within 0.5 meters of the path. Since there is no integral action, exter-nal forces such as wind will result in stationary errors.


60 80 100 120 140 160 180 200

0.5

1

1.5

Figure 6.13: The measured absolute horizontal velocity while flying along afigure eight path.

6.4 Trajectory Control

While the position controller is capable of moving between predefined points itdoes so either with varying speed or requiring a large amount of predefined posi-tions. To allow for greater control over how a mission is performed, a trajectorycontroller has been implemented. This trajectory controller moves between userdefined destinations while maintaining a set reference speed.

6.4.1 Constant Velocity Along Line

The basic principle of the trajectory controller is similar to the position controller,with two lq controllers determining the thrust in orthogonal directions. Unlikethe position controller, these forces are not north-south and east-west, but insteadparallel and perpendicular to a reference line. The first lq controller in the trajec-tory controller maintains a constant velocity along the line, and the other controlsthe distance to this current line. The line is drawn between the previous and thecurrent destination. The height controller still maintains the reference heightof the current destination, while the trajectory controller follows the line in thehorizontal directions. The continuous-time state space model for the movementalong a trajectory can be expressed asp⊥(t)

v⊥(t)v‖(t)

=

0 1 00 0 00 0 0

p⊥(t)v⊥(t)v‖(t)

+

0 0g 00 g

[u⊥(t)u‖(t)

], (6.36)

where p⊥(t) and v⊥(t) is the distance and velocity perpendicular to the line tobe followed while v‖(t) is the velocity parallel to the line. The corresponding

6.4 Trajectory Control 55

discrete-time model isp⊥[k + 1]v⊥[k + 1]v‖[k + 1]

=

1 T 00 1 00 0 1

p⊥[k]v⊥[k]v‖[k]

+

gT 2

2 0gT 00 gT

[u⊥[k]u‖[k]

], (6.37)

where p⊥[k] and v⊥[k] is the distance and velocity perpendicular to the line to befollowed while v‖[k] is the velocity parallel to the line and T is the sample time.Either of these models can be used to determine an lq controller. To avoid imple-menting what is essentially a P-controller for the parallel velocity, measurementsof the parallel acceleration a‖ is also used. The model in (6.37) was rewritten as

p⊥[k + 1]v⊥[k + 1]v‖[k + 1]a‖[k + 1]

=

1 T 0 00 1 0 00 0 1 T

20 0 0 0

p⊥[k]v⊥[k]v‖[k]a‖[k]

+

gT 2

2 0gT 00 gT

20 g

[u⊥[k]u‖[k]

]. (6.38)

This means that the update of v‖[k] is determined by the mean of the expectedacceleration from u‖[k] and the previous value of the acceleration a‖[k], whichcan be measured.

Each destination is determined by coordinates in the global coordinate system.The line to be followed is defined by the previous destination [dk−1,x dk−1,y] andthe current destination [dk,x dk,y]. Using linear algebra, the unit vectors l‖ and l⊥are calculated. l‖ describes the direction of the line in global coordinates, and l⊥is the tangent of the line. They are calculated according to

l‖ =1

||dk − dk−1||

[dk,x − dk−1,xdk,y − dk−1,y

], (6.39)

l⊥ =1

||dk − dk−1||

[−dk,y + dk−1,ydk,x − dk−1,x

]. (6.40)

If dk = dk−1 there is no horizontal line to follow, and the position controller isused to maintain the [x y] coordinates of the destination while the target heightis reached with the height controller.

The acceleration and velocity parallel and perpendicular to the line can becalculated using these unit vectors and scalar products according to

a‖ = l‖ •[axay

], (6.41)

v‖ = l‖ •[vxvy

], (6.42)

v⊥ = l⊥ •[vxvy

], (6.43)

where the quadcopter’s velocity and acceleration in global coordinates is expressedwith vx, vy and ax, ay . The distance from the line p⊥ and the distance to the desti-


nation p‖ can be calculated as

p⊥ = l⊥ •[px − dk−1,xpy − dk−1,y

], (6.44)

p‖ = l‖ •[dk,x − pxdk,y − py

]. (6.45)

Using the model in (6.38), a Q matrix with diagonal elements [1 0.5 5 1] and anR matrix with diagonal elements [10 20], the lq gain Kline was calculated as

Kline =[0.2207 0.2634 0 0

0 0 0.1301 0.065

], (6.46)

resulting in the control signals

[u⊥u‖

]= −Kline

p⊥v⊥

v‖ − vrefa‖

, (6.47)

where u⊥[k] and u‖[k] are the control signals perpendicular and parallel to theline, and vref is the reference velocity. These are expressed in the global coordi-nate system as [

uxuy

]= l⊥u⊥ + l‖u‖. (6.48)

References for φ and θ can then be calculated using (6.28) and (6.30). If the lineis horizontal the reference for ψ is set such that the heading of the quadcopter isparallel to the current line, according to

ψ = atan2(l‖,y , l‖,x), (6.49)

where l‖,x and l‖,y are the x and y components of l‖, and atan2 is the four quadrantinverse tangent that returns an angle in the interval [−π π].

6.5 Missions

Each mission is defined by a matrix with four columns, and with one row for eachdestination the quadcopter shall visit. For each destination, these columns rep-resent the [x y z] coordinates of the destination and the reference velocity whenmoving to the destination. The quadcopter will move to all these coordinatesat the reference speed given. When p‖ ≤ 0 the destination is reached, and thecurrent destination is updated. The matrix describing the mission for the quad-copter to perform is stored as a text file, which the quadcopter will load whenbooting. A test flight was performed using the mission described by Table 6.2.

6.5 Missions 57

Table 6.2: The coordinates and reference velocities when testing the trajec-tory controller.

x y z v6 0 1 16 6 1 1-6 6 1 1-6 -6 1 16 -6 1 16 6 1.5 2-6 6 1 2-6 -6 1 26 -6 1 36 6 1 30 0 1 10 0 0 1

60 80 100 120 140 160

0.5

1

1.5

2

2.5

3

Figure 6.14: Reference and measured velocity during the test flight. There isa significant and sudden decrease in velocity when the quadcopter reachesthe current waypoint and performs a turn.

The measured position during the test flight can be seen in Figure 6.15 and themeasured velocity can be seen in Figure 6.14. These figures show that the con-troller is capable of following a reference trajectory at a reference velocity. Thereis some position overshoot at breakpoints, which increases at higher velocities.There is a sharp decrease in speed when the quadcopter reaches its current desti-nation.


-6 -4 -2 0 2 4 6 8

-6

-4

-2

0

2

4

6

Figure 6.15: Measured position during a test flight. The quadcopter startedat [0 0], flew in a square twice and then returned to [0 0]. The deviationfrom the first segment of the trajectory may be a result of reduced control attakeoff.

6.5.1 Constant Velocity When Turning

In order to avoid overshoot at breakpoints, a predictive transition has been imple-mented. This predictive transition calculates the upcoming heading, and beginstransitioning before the current destination has been reached.

At the distance dturn from the breakpoint a turn that takes tturn seconds tocomplete is initiated. During the turn, two sets of control signals [ux uy] arecalculated, ucurrent and unext . Both are computed using (6.47) and (6.48), withucurrent using the current line, and unext using the upcoming line. The weighted

6.5 Missions 59

control signal is

uweighted = (1 −W )ucurrent + Wunext , (6.50)

ψweighted = (1 −W )ψcurrent + Wψnext , (6.51)

W =t − tstarttturn

, (6.52)

where t is the current time and tstart is the time the turn was started. W is definedin [0 1], and when t − tstart = tturn the turn is completed and the current line tofollow is updated.

For flexibility, the distance and time to turn depends on the size of the turnand the current velocity. The distance when the turn must be started dependson how much of the current velocity must be eliminated when the breakpoint isreached. The baseline for these values is the constant t90, which is the preferredtime it should take to perform a 90 degree turn.

When performing a 90 degree turn with an initial reference velocity vref , start-

ing the turn atvreft90

meters from the breakpoint should be enough to eliminate thevelocity perpendicular to the new line when the breakpoint is reached. However,a smaller turn means that not all of the current velocity needs to be eliminated,and the time to turn is shorter. tturn is approximated as being proportional to theangle of the turn ψturn, hence

tturn =|t90ψturn|

π2

, (6.53)

dturn =vreftturn

. (6.54)

The value of t90 has been selected as one second. Using the controller in (6.47)and the model in (6.14), a flight with and without this algorithm could be simu-lated. t90 was set to one second, and vref was set to 2 m/s for all segments. Thesimulated paths can be seen in Figure 6.16, and the simulated velocities can beseen in Figure 6.17.


-8 -6 -4 -2 0 2 4 6 8

-8

-6

-4

-2

0

2

4

6

8

1 2

34

5

6

7

Figure 6.16: Simulated flight path with vref = 2 and t90 = 1, which showsthe effect of using predictive turning.

In this simulation, predictive turning completely eliminates overshoot. The veloc-ity clearly diverges from the reference velocity at breakpoints, but this divergenceis smaller and slower with predictive turning. As a result, the simulated missionis completed faster when using predictive turning, however the waypoints are notreached completely. Whether predictive or normal turning is preferred dependson if reaching the destination completely is more important than minimising de-viations from the reference velocity.

6.5 Missions 61

0 10 20 30

0

0.5

1

1.5

Figure 6.17: Simulated absolute velocity while flying along a path withvref = 2 and t90 = 1, with and without predictive turning. The velocitydeviation is decreased with predictive turning, resulting in faster missioncompletion.

Table 6.3: The coordinates and reference velocities when testing the predic-tive turning algorithm.

x y z v0 0 1 26 0 1 26 6 1 2-6 6 1 2-6 -6 1 26 -6 1 26 6 1.5 2

Using t90 = 1, a test flight was performed using the predictive turning algorithmand the mission in Table 6.3. The resulting trajectory can be seen in Figure 6.18,the velocity can be seen in Figure 6.19, and the yaw angle can be seen in Fig-ure 6.20.

Figure 6.18 shows that the quadcopter starts turning when it is 2 metersfrom the next breakpoint, which is correct since t90 = 1, vref = 2 and all turnsare 90 degrees. When the velocity exceeds the reference velocity there is someovershoot, but otherwise the overshoot is mostly or completely eliminated. Fig-ure 6.19 shows that turning still results in velocity variations, and there seems tobe some relation between the amplitude of the velocity variation and the size ofthe overshoot.

Figure 6.20 shows that the reference yaw angle is followed relatively well ex-cept during the last turn, although there is significant overshoot.


-8 -6 -4 -2 0 2 4 6 8

-8

-6

-4

-2

0

2

4

6

8

Figure 6.18: Trajectory of the quadcopter when testing the predictive turn-ing algorithm. The trajectory is marked red when the predictive turningalgorithm is active.

Comparing the simulated results and the test flights with and without predictiveturning shows that predictive turning has less overshoot and velocity variationsthan ordinary turning, but more than the simulated results. When the overshootis properly eliminated, the resulting trajectory and velocity variations are simi-lar to the simulation, with the velocity decreasing with approximately 0.5 m/s.When the overshoot is not eliminated the amplitude of the overshoot seems to atleast be reduced compared to normal turning.

It should be noted that during this test, the predictive turning algorithmappears to more efficiently reduce overshoot along the y axis than the x axis.Whether this is due to external influences or the implementation requires furtherinvestigation.

6.5 Missions 63

50 60 70 80 90 100

0

0.5

1

1.5

2

2.5

Figure 6.19: Velocity of the quadcopter when testing the predictive turningalgorithm. At some breakpoints the velocity variations have been signifi-cantly reduced.

60 70 80 90

0

2

4

6

8

Figure 6.20: Yaw angle of the quadcopter when testing the predictive turn-ing algorithm, showing how the transition at breakpoints is now a ramp in-stead of a step.

7Conclusions

The implemented controllers have been tested both in simulations and in practi-cal tests using the position estimations. This chapter will comment on the resultof the algorithms, and the possibilities for future development.

7.1 Result

The goal of this thesis was to allow for a position controller to allow the quad-copter to perform missions. These missions are defined by a sequence of coor-dinates that the quadcopter will attempt to reach and velocities to follow whenmoving to the destinations. The implementation of the trajectory controller wassuccessful, with the quadcopter being capable of maintaining the reference veloc-ities and travelling to the defined coordinates. An algorithm to decrease positionovershoot and improve velocity reference tracking was developed and showedpromising results during simulations and practical tests. Both height controllerscould follow a reference and the position controller could follow a step in thereference position and maintain it, but with a minor stationary error.

One problem when verifying the performance of the quadcopter is that thereare no external measurements of the quadcopter’s position available. If the quad-copter’s position estimates drift over time while the quadcopter is in flight, thequadcopter will compensate for the resulting position errors by adjusting its po-sition accordingly. As such, even though the quadcopter will move in the realworld, the logs will show that it maintained its position, and was unaffected bythe drifting errors. While data can be logged while the quadcopter is stationary,and an operator can visually detect abnormal position compensations from thequadcopter, drifts that occur during trajectory following can be difficult to detectwithout an external measurement to compare with.

Using the models estimated by Kugelberg (2016) it was possible to implement

65

66 7 Conclusions

an lq controller for ψ, but attempts to do the same for φ or θ were unsuccessful.The reason for this may be that while data could be gathered for all angles ofψ, the angles of φ and θ needed to be in a small interval to maintain stability.This limitation on the data collection may have resulted in models that did notdescribe the dynamics accurately enough to create an lq controller. It is also pos-sible that the assumptions and simplifications made in this thesis are not valid,and that it is possible to implement a controller using a complete model.

7.2 Future Work

The ability to estimate and control the position of the quadcopter can be builtupon further and be used in future experiments. New black-box models for an-gles and angular rates could be estimated that included the effect of the quad-copter’s horizontal velocities.

The positioning and trajectory controllers can be combined with different po-sition estimation methods. An alternative for the future is to evaluate the perfor-mance indoors, using external cameras and image processors to generate positionand orientation measurements.

Horizontal and vertical positioning estimation is done by separate programs.In the future it could be advantageous to integrate the lidarmeasurements intothe ekf for better estimates.

Since the quadcopter no longer requires a user with a remote controller, im-plementing controllers for the angular rates instead of angles could result in im-proved navigational capabilities. This will however require fast controllers or theintegration of the angular rate errors will result in significant angle deviations.

The current position controllers assume that there is always a force mg di-rected upwards. While this is true in most cases, large changes in uthrottle couldaffect the performance of the position controllers. Feedforwarding the currentthrottle signal could improve the precision of the position controllers. Addition-ally, the height controllers could be improved by determining a more accuratemodel of how the thrust depends on uthrottle.

An algorithm could be developed that, given a set of coordinates, determinesthe set of trajectories that results in, for example, the shortest path or fastest routerequired to visit all coordinates.

There is significant room for improvement in the predictive turning algo-rithm. By implementing an optimal control algorithm, with finite time horizonand final conditions on velocity and distance to the next line, the overshoot andvelocity variations could be further decreased.

Bibliography

M. Abdolhosseini, Y. M. Zhang, and C. A. Rabbath. An efficient modelpredictive control scheme for an unmanned quadrotor helicopter. Jour-nal of Intelligent & Robotic Systems, 70(1):27–38, 2013. ISSN 1573-0409.doi: 10.1007/s10846-012-9724-3. URL http://dx.doi.org/10.1007/s10846-012-9724-3. Cited on page 2.

Arduino. Arduino analog read, 2017. URL https://www.arduino.cc/en/Reference/AnalogRead. Accessed: 2017-05-23. Cited on page 15.

Moses Bangura and Robert Mahony. Nonlinear dynamic modelingfor high performance control of a quadrotor. In Proceedings ofAustralasian Conference on Robotics and Automation, December 3-5, 2012. URL https://pdfs.semanticscholar.org/e899/0d6f6144a34e3ea31ff55094607bcf5b118d.pdf. Cited on page 2.

R. Bonna and J. F. Camino. Trajectory tracking control of a quadrotor usingfeedback linearization. In Proceedings of the XVII International Symposiumon Dynamic Problems of Mechanics, February 22-27, 2015. URL http://abcm.org.br/anais/diname/2015/PDFS/DIN-2015-0122.PDF. Citedon page 1.

L. Danjun, Z. Yan, S. Zongying, and L. Geng. Autonomous landing of quadrotorbased on ground effect modelling. In 2015 34th Chinese Control Conference(CCC), pages 5647–5652, July 2015. doi: 10.1109/ChiCC.2015.7260521. Citedon page 32.

FrSky. FrSky 2.4GHz ACCST X8R Manual. FrSky Electronic Co., Ltd, May 21,2015. Cited on page 13.

Torkel Glad and Lennart Ljung. Reglerteknik : grundläggande teori. Studentlit-teratur AB, fourth edition, 2006. ISBN 9144022751. Cited on pages 1, 9, 10,and 43.

Fredrik Gustafsson. Statistical Sensor Fusion. Studentlitteratur AB, second edi-tion, 2012. ISBN 9789144077321. Cited on pages 2, 8, 9, and 29.

67

http://dx.doi.org/10.1007/s10846-012-9724-3

http://dx.doi.org/10.1007/s10846-012-9724-3

https://www.arduino.cc/en/Reference/AnalogRead

https://www.arduino.cc/en/Reference/AnalogRead

https://pdfs.semanticscholar.org/e899/0d6f6144a34e3ea31ff55094607bcf5b118d.pdf

https://pdfs.semanticscholar.org/e899/0d6f6144a34e3ea31ff55094607bcf5b118d.pdf

http://abcm.org.br/anais/diname/2015/PDFS/DIN-2015-0122.PDF

http://abcm.org.br/anais/diname/2015/PDFS/DIN-2015-0122.PDF

68 Bibliography

Fredrik Gustafsson, Lennart Ljung, and Mille Millnert. Signalbehandling. Lund: Studentlitteratur, 2001 ;, 2001. ISBN 914401709X. Cited on page 35.

Christopher Konvalin. Compensating for tilt, hard-iron,and soft-iron effects. Sensors Magazine, December 1,2009. URL http://www.sensorsmag.com/components/compensating-for-tilt-hard-iron-and-soft-iron-effects.Cited on page 36.

Ingrid Kugelberg. Black-box modeling and attitude control of a quadcopter. Mas-ter’s thesis, Linköping University, 2016. URL https://liu.diva-portal.org/smash/get/diva2:908582/FULLTEXT02.pdf. Cited on pages 1, 2, 7,17, 19, 30, and 65.

Frank L. Lewis, Draguna L., Vrabie, and Vassilis L. Syrmos. Optimal Control.John Wiley & Sons, third edition, 2012. Cited on pages 9 and 10.

LightWare Optoelectronics. SF30 Accelerated laser rangefinder - Product Manual- Revision 7, September 13, 2016. Cited on pages 13 and 15.

Lennart Ljung and Torkel Glad. Modellbygge och simulering. Lund: Studentlitteratur, 2004, 2004. ISBN 9144024436. URL https://login.e.bibl.liu.se/login?url=https://search.ebscohost.com/login.aspx?direct=true&AuthType=ip,uid&db=cat00115a&AN=lkp.380250&lang=sv&site=eds-live&scope=site. Cited on page26.

T Luukkonen. Modelling and control of quadcopter, August 22, 2011. URLhttp://sal.aalto.fi/publications/pdf-files/eluu11_public.pdf. Cited on page 30.

R.C Nelson. Flight Stabiliy and Automatic Control. McGraw-Hill, second edition,1998. Cited on pages 5 and 6.

NXP Semiconductors. PCA9685 16-channel, 12-bit PWM Fm+ I2C-bus LED con-troller, Rev. 4, April 16, 2015. URL https://cdn-shop.adafruit.com/datasheets/PCA9685.pdf. Cited on page 13.

Ordnance Survey. A guide to coordinate systems in great britain, v3.0, August2016. URL https://www.ordnancesurvey.co.uk/docs/support/guide-coordinate-systems-great-britain.pdf. Cited on page 46.

Raspberry Pi Foundation. Raspberry pi 3 model b, 2017. URL https://www.raspberrypi.org/products/raspberry-pi-3-model-b/. Accessed:2017-05-28. Cited on page 13.

L. V. Santana, A. S. Brandão, and M. Sarcinelli-Filho. Outdoor waypoint navi-gation with the ar.drone quadrotor. In 2015 International Conference on Un-manned Aircraft Systems (ICUAS), pages 303–311, June 2015. doi: 10.1109/ICUAS.2015.7152304. Cited on page 2.

http://www.sensorsmag.com/components/compensating-for-tilt-hard-iron-and-soft-iron-effects

http://www.sensorsmag.com/components/compensating-for-tilt-hard-iron-and-soft-iron-effects

https://liu.diva-portal.org/smash/get/diva2:908582/FULLTEXT02.pdf

https://liu.diva-portal.org/smash/get/diva2:908582/FULLTEXT02.pdf

https://login.e.bibl.liu.se/login?url=https://search.ebscohost.com/login.aspx?direct=true&AuthType=ip,uid&db=cat00115a&AN=lkp.380250&lang=sv&site=eds-live&scope=site




http://sal.aalto.fi/publications/pdf-files/eluu11_public.pdf

http://sal.aalto.fi/publications/pdf-files/eluu11_public.pdf

https://cdn-shop.adafruit.com/datasheets/PCA9685.pdf

https://cdn-shop.adafruit.com/datasheets/PCA9685.pdf

https://www.ordnancesurvey.co.uk/docs/support/guide-coordinate-systems-great-britain.pdf

https://www.ordnancesurvey.co.uk/docs/support/guide-coordinate-systems-great-britain.pdf

https://www.raspberrypi.org/products/raspberry-pi-3-model-b/

https://www.raspberrypi.org/products/raspberry-pi-3-model-b/

Bibliography 69

Z. S. Spakovszky. Unified: Thermodynamics and Propulsion, 2007. URLhttp://web.mit.edu/16.unified/www/FALL/thermodynamics/notes/notes.html. Accessed: 2017-05-26. Cited on page 30.

Xsens. Data sheet MTi 1-series Revision D, December 5, 2016. URLhttps://www.xsens.com/download/pdf/documentation/mti-1/mti-1-series_datasheet.pdf. Cited on pages 13 and 36.

Xsens Knowledge Base. In-run Compass Calibration (ICC) and RepresentativeMotion, 2017. URL https://base.xsens.com/hc/en-us/articles/213588029. Accessed: 2017-05-23. Cited on pages 36 and 37.

K. M. Zemalache, L. Beji, and H. Marref. Control of an under-actuated system:application a four rotors rotorcraft. In 2005 IEEE International Conferenceon Robotics and Biomimetics - ROBIO, pages 404–409, 2005. doi: 10.1109/ROBIO.2005.246301. Cited on page 1.

http://web.mit.edu/16.unified/www/FALL/thermodynamics/notes/notes.html

http://web.mit.edu/16.unified/www/FALL/thermodynamics/notes/notes.html

https://www.xsens.com/download/pdf/documentation/mti-1/mti-1-series_datasheet.pdf

https://www.xsens.com/download/pdf/documentation/mti-1/mti-1-series_datasheet.pdf

https://base.xsens.com/hc/en-us/articles/213588029

https://base.xsens.com/hc/en-us/articles/213588029

Documents

Position and Trajectory Control of a Quadcopter Using PID and …liu.diva-portal.org/smash/get/diva2:1129641/FULLTEXT01.pdf · 2017. 8. 4. · Axel Reizenstein. Master of Science