Institutionen för systemteknik
Department of Electrical Engineering
Examensarbete
Attitude Control of a Hexarotor
Examensarbete utfört i Reglerteknik vid Tekniska högskolan vid Linköpings universitet
av
Tobias Magnusson LiTH-ISY-EX--14/4779--SE
Linköping 2014
Department of Electrical Engineering Linköpings tekniska högskola
Linköpings universitet Linköpings universitet
Attitude Control of a Hexarotor
Examensarbete utfört i Reglerteknik
vid Tekniska högskolan vid Linköpings universitet
av
Tobias Magnusson LiTH-ISY-EX--14/4779--SE
Handledare: Niclas Evestedt
isy, Linköpings universitet
Thomas Barath
UAS Europe AB
Examinator: Daniel Axehill
isy, Linköpings universitet
Avdelning, Institution Division, Department
Department of Automatic Control Department of Electrical Engineering SE-581 83 Linköping Datum Date 2014-06-12 Språk Language Svenska/Swedish Engelska/English Rapporttyp Report category Licentiatavhandling Examensarbete C-uppsats D-uppsats Övrig rapport
URL för elektronisk version
http://urn.kb.se/resolve?urn=urn:nbn:se:liu:diva-109073
ISBN — ISRN
LiTH-ISY-EX--14/4779--SE Serietitel och serienummer
Title of series, numbering ISSN—
Titel
Title Attitydstabilisering av en HexakopterAttitude Control of a Hexarotor
Författare
Author Tobias Magnusson
Sammanfattning Abstract
This master’s thesis has been on modeling, identification and control of a hexarotor system. It has been carried out on behalf of UAS Europe in Linköping. A set of non-linear dynamic equations describing the motion of the hexarotor were derived. These equations were then implemented in Matlab/Simulink, which became a good simulation environment for further studies.
A decentralized control system using P-PD controllers was successfully implemented in both simulation and on a hexarotor platform. The non-linear simulation model and the hexarotor platform were then identified using black box identification between virtual controls and angular rates. The result from identification of the hexarotor platform was not bad at all, but left some room for improvements.
These linear models ware then used to tune the parameters of the inner PD controllers us-ing a method called placement of dominant poles. This method worked well in simulation environment but unfortunately not as well on the real platform.
Nyckelord
Abstract
This master’s thesis has been on modeling, identification and control of a hexaro-tor system. It has been carried out on behalf of UAS Europe in Linköping. A set of non-linear dynamic equations describing the motion of the hexarotor were derived. These equations were then implemented in Matlab/Simulink, which became a good simulation environment for further studies.
A decentralized control system using P-PD controllers was successfully imple-mented in both simulation and on a hexarotor platform. The non-linear simu-lation model and the hexarotor platform were then identified using black box identification between virtual controls and angular rates. The result from iden-tification of the hexarotor platform was not bad at all, but left some room for improvements.
These linear models ware then used to tune the parameters of the inner PD con-trollers using a method called placement of dominant poles. This method worked well in simulation environment but unfortunately not as well on the real plat-form.
Acknowledgments
At first, I would like to thank UAS Europe for giving me the opportunity of doing my master’s thesis. My colleagues Thomas, Thom and Patrik have not only been a very nice company during my days at the office but also helped me a lot with the project. Without them, I would not have been able to get the hexarotor flying. I also want to thank my examiner, Daniel, and my supervisor at isy, Niclas, for being supportive and helpful whenever needed.
My biggest thanks is to my parents. Without your unconditional support through out my entire life I would not even be close to where I am today. You have always been there and have given me every opportunity to become the person I am today. You have always answered my questions about big and small, lived through my never ceasing curiosity and inspired me to never stop learning.
Then I would like to give a big and special thanks to my partner Hedvig. When my thesis work was ever going slow or not going at all you were there to comfort me and cheer me up.
And lastly, to all my fellow students during my five fantastic year at Linköping University I would like to say thank you! You have made my time as a student to the best five years years of my life. I hope I will meet you all again!
Linköping, June 2013 Tobias Magnusson
Contents
Notation xi 1 Introduction 1 1.1 Background . . . 1 1.2 Problem formulation . . . 1 1.3 Previous works . . . 2 1.4 Thesis outline . . . 3 2 Hexarotor 5 2.1 Basic movement . . . 5 2.1.1 Throttle . . . 6 2.1.2 Roll . . . 6 2.1.3 Pitch . . . 7 2.1.4 Yaw . . . 7 2.2 Control loop . . . 8 2.3 Hardware . . . 82.3.1 Autopilot Easy Pilot 3.0 . . . 8
2.3.2 Propulsion system . . . 8
2.3.3 RC transmitter and receiver . . . 9
2.3.4 Sensors and filter . . . 9
3 System Modeling 11 3.1 Frames of reference . . . 11 3.2 Kinematics . . . 12 3.3 Dynamics . . . 14 3.3.1 Translational dynamics . . . 14 3.3.2 Rotational dynamics . . . 14
3.4 Applied forces and torques . . . 15
3.4.1 Gravity . . . 16
3.4.2 Thrust and torque from propellers . . . 16
3.4.3 Gyroscopic effects from propellers . . . 18
3.4.4 Air friction . . . 18
3.5 Thruster model . . . 18
viii Contents
3.5.1 Propeller model . . . 19
3.5.2 Electrical motor model . . . 19
3.6 Final model . . . 20
3.6.1 System input . . . 20
3.6.2 Total system model . . . 20
3.7 Simulator . . . 21
4 Control design 25 4.1 Choice of control design . . . 25
4.2 Control techniques . . . 26
4.2.1 Decentralization . . . 26
4.2.2 Cascade structure . . . 26
4.2.3 The general pid controller . . . 27
4.3 Implemented control . . . 28
4.3.1 Roll . . . 28
4.3.2 Pitch . . . 29
4.3.3 Yaw . . . 30
4.3.4 Throttle . . . 30
4.3.5 Scaling and rotor mapping . . . 30
4.4 Flight performance . . . 32
5 Identification 35 5.1 Identification of a multirotor system . . . 35
5.2 Black box identification in a closed loop . . . 36
5.2.1 Choice of input signal . . . 36
5.2.2 Applying the input signal . . . 37
5.3 Concept evaluation in simulation . . . 40
5.3.1 Experiment set up and data processing . . . 40
5.3.2 Roll axis identification experiment . . . 41
5.3.3 Pitch axis identification experiment . . . 42
5.3.4 Yaw axis identification experiment . . . 42
5.3.5 Validation data . . . 44
5.3.6 Choosing model structure . . . 45
5.3.7 Identification of roll dynamics model . . . 45
5.3.8 Identification of pitch dynamics model . . . 46
5.3.9 Identification of yaw dynamic model . . . 48
5.3.10 Evaluation . . . 49
5.4 Data acquired from real flight . . . 49
5.4.1 Roll axis identification experiment . . . 50
5.4.2 Pitch axis identification experiment . . . 52
5.4.3 Yaw axis identification experiment . . . 54
5.5 Models identified from flight data . . . 56
5.5.1 Roll dynamic model . . . 56
5.5.2 Pitch dynamic model . . . 59
Contents ix
6 Controller Tuning 65
6.1 Placement of dominant poles . . . 65
6.2 Tuning in simulation . . . 67
6.2.1 Quick and reactive response . . . 68
6.2.2 Smooth and damped response . . . 70
6.3 Tuning of real platform . . . 71
6.3.1 Roll rate controller . . . 72
6.3.2 Pitch rate controller . . . 75
6.3.3 Yaw rate controller . . . 76
6.3.4 Flight test . . . 77
7 Conclusions and further work 79 7.1 Conclusions . . . 79
7.2 Further work . . . 79
A Blade flapping 83
B Residual analysis 85
C Basic control theory 87
xii Notation
Notation
Notation in modeling Symbol Meaning
η Vector describing the attitude of the body frame rel-ative the earth frame, also called Euler angles using Tait-Bryan formalism
θ Pitch angle, i.e. rotation of B1around y1
B
ξ Vector describing the body frame’s position in the earth frame
ρ Air density
τi Reaction torque from rotor i
φ Roll angle, i.e. rotation of B around xB
ψ Yaw angle, i.e. rotation of B2around z2
B
ω Vector representing body angular rates Ω Angular velocity of the propeller and motor
aR Rotational air friction coefficient
aT Linear air friction coefficient
g Gravitational acceleration
l Length of air frame arm, i.e. distance between center of gravity and propeller
m Mass of the hexarotor
p Body angular rate around xB-axis (forward direction
of the body)
q Body angular rate around yB-axis (right direction of
the body)
r Body angular rate around zB-axis (down direction of
the body)
rP Propeller radius
s · , c · , t · sin( · ), cos( · ), tan( · )
u Forward speed of the hexarotor
v Sideway speed of the hexarotor
w Downward speed of the hexarotor
x, y, z The elements of the ξ-vector {xE, yE, zE} Base vectors of the earth frame
Notation xiii Notation in modeling (cont...)
Symbol Meaning
B Body fixed frame of reference
CT Propeller thrust constant
CQ Propeller torque constnat
E Earth frame, used as inertial frame of reference
F Sum of all external forces on the hexarotor
Fw Total thrust produced by propellers
FA Aerodynamic force produced by propellers
FR Force caused by air friction
I Current of the DC-motor
J Inertia matrix expressed in the body frame
JP ,zz The moment of inertia of the propeller and motor
around the propeller shaft
Jxx The hexarotor body’s moment of inertia around xB
-axis
Jyy The hexarotor body’s moment of inertia around yB
-axis
Jzz The hexarotor body’s moment of inertia around zB
-axis
Kt DC-motor torque constant
Kv DC-motor speed constant
M Sum of all external torques on the hexarotor
Mp Torque around xB axis produced by differential
pro-peller thrust
Mq Torque around yB axis produced by differential
pro-peller thrust
Mq Torque around zB axis produced by differential
reac-tion torque
MA Aerodynamic torque produced by propellers
MG Gyroscopic torque caused by the rotation of the rotors
MR Torque caused by air friction
Qi Aerodynamic torque from propeller i
RA Relation between ω and time derivative of η
RL Rotation matrix from earth frame to body frame
Ti Thrust produced by propeller i
V Vector representing linear velocity of the hexarotor in the body frame
xiv Notation
Notation in controller Symbol Meaning
δi Duty cycle of a PWM-signal
θC Pilot commanded pitch
ˆθ Estimate pitch angle ˙θD Desired pitch rate
µ Derivative filter coefficient in time discrete PID-controller
φC Pilot commanded roll
ˆ
φ Estimate roll angle ˙φD Desired roll rate
˙
ψC Pilot commanded yaw rate ˆp Estimated roll rate
ˆq Estimated pitch rate ˆr Estimated yaw rate
ui Virtual control signal to rotor i
KD Derivative parameter of a PID-controller
KI Integral parameter of a PID-controller
KP Proportional parameter of a PID-controller
Ts Sample time
TC Pilot commanded throttle
Up Virtual roll control
Uq Virtual pitch control
Ur Virtual yaw rate control
Uw Virtual throttle control
Abbreviations and Acronyms Acronym Description
ahrs Attitude and heading reference system arx Auto-regression extra input
armax Auto-regressive moving average extra input cpu Central processing unit
dc Direct current
esc Electronic speed controller gps Global positing system imu Inertial measurement unit
pid Proportional, integral, differential (regulator) psd Power spectral density
pwm Pulse width modulation rc Radio controller
uav Unmanned aerial vehicle vtol Vertical take-off and landing
1
Introduction
1.1
Background
The use and development of small unmanned aerial vehicles (uavs) have in-creased significantly over the last few decades. uavs can be used in several field and applications. People in possession of large grounds, such as farmers and forest owners, can use uavs to remotely inspect their properties. Rescue organi-sations like the coast guard can send uavs to search for capsized boats. Being able to replace human pilots with autonomous aerial vehicles is obviously a tremen-dous advantage.
The vertical take-off and landing (vtol) uavs are in particular interest because of their ability to take off and land in a limited area and hover above targets. Such air crafts make for excellent camera platforms when producing films and television.
UAS Europe AB is a company in Linköping specialized in development of uavs. Today their autopilot SkyView EasyPilot has the capability of flying fixed wing aircrafts autonomously. UAS wishes to integrate support for vtol uavs in the au-topilot and in particular multirotor vtol uavs. The specific multirotor platform which will be used during this project is a hexarotor, i.e. a small helicopter with six vertical rotors.
1.2
Problem formulation
The goal of this master’s thesis is to control the attitude of a hexarotor platform. The final field of application of the product will most likely be surveillance and reconnaissance missions, implying flying at low speed following way points. Also,
2 1 Introduction
the final controller should be applicable to other multirotor platforms than the one used in this project, why the controller should be as model-independent and easily tuned as possible, without losing too much of the flight performance. To obtain understanding of the system and to be able to evaluate different con-trol strategies a non-linear physical model will be derived and implemented in matlab/Simulink. To synthesis and tune the controller the platform will be iden-tified using decoupled black box models. But to be able to perform the flight ex-periments a novice control system is needed. This will be a simple pid-controller based upon previous works and then tested in the simulation environment. The goals of the master’s thesis are
• Derive and implement a non-linear physical model of a multirotor • Develop and implement simple pid controllers for the attitudes • Identify decoupled models of the angular rate dynamics • Synthesize and tune attitude controller from identified models
1.3
Previous works
Understanding the non-linear and unstable dynamics of a multirotor system is the core in most previous works. The basics of the model, the rotation and trans-lation of a rigid body can be found in every paper regarding modeling and control of multirotor platforms.
Identification of different parameters is a time consuming and in many cases ex-pensive processes. Works of [Elsamanty et al., 2013] show that even though it is possible, it demands a lot of equipment and multiple experiments. The param-eters will still only be valid for hovering, since recreating the condition during forward flight is very difficult. The work load may very well correspond to a master thesis of its own, why another solution must be found.
One approach that could prove useful is the one used in [Barsk, 2012, Beltramini et al., 2011]. There the authors try to identify linear decoupled models with data collected from flight experiments. The controllers are then designed using these models.
Control of a multirotor platform is a widely explored field in previous work. Lots of different model dependent control strategies have been proposed, such as back-stepping [Sanca et al., 2010, Arellano-Muro et al., 2013], linear quadratic [Bouab-dallah et al., 2004, Rinaldi et al., 2013], feedback linearization [Mokhtari et al., 2006] and model predictive control [Alexis et al., 2011, Abdolhosseini et al., 2013, Barsk, 2012].
However, the final controller should be model-independent and easily tuned. pid controllers is a well-tried concept and may very well prove enough to stabilize the
1.4 Thesis outline 3 hexarotor. Earlier work, such as [Alaimo et al., 2014, Bouabdallah et al., 2004, Ri-naldi et al., 2013], shows that pid controller can be a suitable choice of control strategy. An interesting approach is used by [Elbir et al., 2013] where they iden-tify several linear models from flight data and then tune the pid controllers using these identified models.
1.4
Thesis outline
The outline of this thesis is as follows:
• Chapter 2 explains the basic about the flying platform used. If the reader is completely unfamiliar with multirotors, he or she should start by reading this chapter.
• Chapter 3 deals with the physical modeling of the hexarotor. From Newton and Euler’s law of motion a complete set of differential equations describing the motion of the system are derived. The chapter also briefly describes how the model was implemented in matlab/Simulink.
• Chapter 4 describes the initial novice controller and its key concepts and how it was implemented in both simulation environment and on the autopi-lot.
• Chapter 5 contains the experiment design, data collection and system iden-tification performed and then presents the identified models.
• Chapter 6 deals with the synthesis and tuning of the control system and the evaluation of the flight performance after the tuning.
• Chapter 7 sums up this thesis with conclusions and some discussions of the results.
2
Hexarotor
The multirotor platform used in this thesis is a hexarotor. It consists of six arms all connected symmetrically to the central hub. At the end of each arm a pro-peller driven by an electric motor is attached. All the propro-pellers have fixed pitch blades, meaning that propellers can not be tilted (The movement of the classic helicopter is controlled by pitching the main propeller). The electronics used for communication and control are placed on the central hub together with the battery.
2.1
Basic movement
The only way of controlling the hexarotor is through its propellers. Each pro-peller produces an upward thrust by pressing air downwards. Since the source of the thrust is located outside the center of gravity, differential thrust can be used to rotate the vehicle. The rotation of the rotors (motors and propellers) also produce a reaction torque opposite of the direction of the rotation. Since half of the propellers are spinning in one direction, the net torque when all rotors have equal speed is zero.
The are four basic movements: throttle, roll, pitch and yaw. These control signals, in this thesis denoted virtual control signals, are mapped to different ways of changing the propeller speed. The speed of every individual rotor is the sum of the four control signals’ contribution to that particular rotor.
Linear movement, i.e. fly the hexarotor along the ground, is controlled through the roll and pitch angle. That is, to move forward (in some sense) the hexarotor is tilted downwards. This will produce an acceleration in the forward direction.
6 2 Hexarotor
2.1.1
Throttle
The main control of the hexarotor is the throttle. It is used to control the hexaro-tor’s movement in the body fixed vertical direction. Since the propellers are fixed pitched, the direction of the throttle is fixed. The biggest part of the throttle is used to counter act gravity. When increasing (decreasing) the throttle, the hexaro-tor will travel upwards (downwards). If the hexarohexaro-tor is tilted, part of the thrust will move the hexarotor in the tilted direction.
Figure 2.1: Throttle is produced by increasing (decreasing) the speed of all rotors equally much.
2.1.2
Roll
The roll command is used to rotate the hexarotor around its forward-axis. Rolling is performed by increasing (decreasing) the thrust produced by the propellers on the right side of the hexarotor while decreasing (increasing) the thrust produced by the propellers on left side of the hexarotor. The total thrust will thus remain unchanged and the command leads only to a rotation.
Figure 2.2: Roll movement is produced by increasing (decreasing) the speed of rotors on right side while decreasing (increasing) the left side rotors’ speed equally much.
2.1 Basic movement 7
2.1.3
Pitch
The pitch command is used to rotate the hexarotor around its right-axis. Pitching is performed by increasing (decreasing) the thrust produced by the propellers on the front of the hexarotor while decreasing (increasing) the thrust produced by the propellers on the rear of the hexarotor. The total thrust will thus remain unchanged and the command leads only to a rotation.
Figure 2.3: Pitch movement is produced by increasing (decreasing) the rear rotors’ speed while decreasing (increasing) the front rotors’ speed.
2.1.4
Yaw
The yaw command is used to rotate the hexarotor around its vertical axis. Yaw-ing is performed by increasYaw-ing (decreasYaw-ing) the thrust produced by the propellers rotating clockwise while decreasing (increasing) the thrust produced by the pro-pellers rotating counter-clockwise. The total thrust will thus remain unchanged and the command leads only to a rotation.
Figure 2.4: Yaw movement is produced by increasing (decreasing) the speed of the rotors rotating clockwise while decreasing (increasing) the rotors ro-tating counter clockwise.
8 2 Hexarotor
2.2
Control loop
The control system on the hexarotor is divided into an inner and an outer loop. The inner loop controls the attitude and height while the outer loop controls the position. The position is controlled by sending reference values to the inner controller. The outer loop could either be a pilot with a radio controller flying in stabilized mode or a control system running on board. The task of the outer loop could either be to maintain a hover position or following way points from a navigation system.
Inner controller Hexarotor Outer controller
Attitude, altitude
Position Navigation
system
Figure 2.5: A brief overview of the different blocks in the control chain
2.3
Hardware
The core of the hardware is the autopilot Easy Pilot developed by UAS Europe AB. It contains two central processing units (cpus) responsible for control, com-munication and sensor data processing. A bunch of sensors are used such as an inertial measurement unit (imu), a barometer and a gps-receiver. The system can receive flight commands through a radio controller (rc) or through a ground station control. The autopilot is also capable of sending back flight data to the ground station. A map of the different hardware components is seen in Figure 2.6.
2.3.1
Autopilot Easy Pilot 3.0
The flight board used is the Autopilot Easy Pilot 3.0 developed by UAS Europe AB. Its core are the two central processing units. The main cpu is responsible for communication and controlling the air vehicle. The secondary cpu collects data from sensors and filters that data. The board also contains a modem for communication with a ground station.
2.3.2
Propulsion system
Each propulsion system consists of an electronic speed controller (esc), a brush-less electronic motor and a propeller. The propellers used are 11 inch slow flight
2.3 Hardware 9 AHRS Main CPU IMU Magnetmetometer GPS Barometer ESC Motor ESC Motor ESC Motor ESC Motor ESC Motor ESC Motor SBUS-Receiv er Modem
Figure 2.6: An overview of the different hardware components used on the platform.
propellers. They are driven by NTM Prop Drives Series 28-30A 800kv brushless motors. They are capable of rotating up to 18400 rpm without any load. A brush-less motor cannot be driven directly by a pwm signal. It is instead controlled by an esc, the one used in this project is a Turnigy Plush 25A. The esc receives a pwmsignal from the flight board and controls the motors by rapidly turning on and off the current to the different poles in the motor.
2.3.3
RC transmitter and receiver
The transmitter is the radio controller Futaba T7C. It is used to control the hexaro-tor by sending reference values for roll, pitch, yaw and throttle. The operahexaro-tor can then control the hexarotor through its control system, in a mode called stabilized flight. The receiver for the radio controller is a Futaba R6203SB. It transfers the received commands through a serial bus to the autopilot.
2.3.4
Sensors and filter
To keep track of the attitude, heading and position of the hexarotor an array of sensors together with a software filter is needed. On the autopilot board all the needed sensors are integrated except for the external gps-receiver. On the secondary cpu a filter used to estimate heading and attitude of the hexarotor is running. A compilation of the states, sensors and data processing can be seen in
10 2 Hexarotor
Table 2.1.
The sensors used to estimate the angular velocity of the body are gyroscopes, one for each body axis. The gyro signals are low pass filtered before sent to the control loop. To estimate the attitude of the hexarotor the gyroscopes are used together with accelerometers. The estimation is made by a complementary filter. The attitude is estimated by integrating the angular velocities from the gyroscope. Since all gyros have a small drift the accelerometers are used to compensate for that drift. The estimation of heading of the body is made in a similar manner, but instead of accelerometer data a magnetometer is used.
The estimation of position, that is longitude and latitude, is made by the gps. The gpscan also estimate altitude and direction of velocity. Since a hexarotor can fly in any direction (not just forward, compared to a fixed wing aircraft) the flight direction does not have be equal to the body heading.
Finally, the two barometers measure static pressure and dynamic pressure respec-tively. From these measurement the altitude and change of altitude can be found as well as air speed.
State Sensors used Processing
Body angular velocities Gyroscope Low pass filtering Attitude (roll and pitch) Gyroscope, accelerometer Complementary filter Body heading (yaw) Gyroscope, magnetometer Complementary filter
Latitude and longitude gps
-Flight direction gps
-Flight speed gps
-Altitude Barometer Low pass filtering
Altitude change Barometer
3
System Modeling
3.1
Frames of reference
Let E = {xE, yE, zE} be an inertial frame with origin on the surface of the earth. As
convenient when describing aircraft motion, the earth fixed frame is a north-east-down-system (NED-system) with xEpointing to the north, yEpointing to the east
and zEpointing downwards [Cook, 2007].
Also, introduce B = {xB, yB, zB} as a body fixed frame with origin at the
hexaro-tor’s center of gravity. xBis equivalent with the forward direction of the hexarotor,
yB with right and zBwith down. This choice of body fixed frame has the
advan-tage that the inertia tensor is time-invariant and that the body symmetry will simplify the equations.
yE xE zE yB xB zB ξ φ θ ψ
Figure 3.1:The two frames of reference and their relation
12 3 System Modeling
3.2
Kinematics
The body fixed frame’s position in the earth fixed frame can be described by the vector ξξξ = [ x y z ]T and its orientation, attitude and heading, by the vector
ηηη = [ φ θ ψ ]T. The angles used to represent the orientation are defined
using Tait-Bryan formalism. This type of formalism differs from proper Euler angles by using three different axes when forming the rotation [Bishop, 2007]. Within aerospace literature this representation is often referred to as Euler angles, which may cause some confusion.
The adopted order of rotation is commonly used when describing aircraft motion [Cook, 2007, Merzouki et al., 2012]. To bring the body fixed frame into coinci-dence with the earth fixed frame the following rotations are considered:
• First, rotate the body-fixed frame about the xB-axis by the roll angle φ,
re-sulting in a new frame of reference called B1.
• Then, rotate the new frame B1about the new axis y1
Bby the pitch angle θ,
resulting in a new frame of reference called B2.
• Lastly, rotate the new frame B2about the new axis z2
B(which coincide with
zE) by the yaw angle ψ, resulting in a new frame aligned with the earth
fixed frame.
In order to transform any linear quantity from earth frame to body frame rotation matrices are used. In order to simplify the notation sin( · ) and cos( · ) are abbrevi-ated s · and c · respectively. The relation between B and B1after the rolling can be described by xB yB zB = 1 0 0 0 cφ sφ 0 −sφ cφ x1B y1B zB1 (3.1) where the rotation matrix is denoted R(x, φ). In similar manner, after the pitching B1is related to B2via x1B yB1 z1B = cθ 0 sθ 0 1 0 −sθ 0 cθ x2B y2B zB2 (3.2) with the rotation matrix R(y, θ). After the final yawing B2and E are related by
x2B y2B zB2 = cψ sψ 0 −sψ cψ 0 0 0 1 xE yE zE (3.3)
3.2 Kinematics 13 using the rotation matrix R(z, ψ). The total rotational matrix, to transform any quantity from earth frame to body frame, is obtained by multiplying R(x, φ),
R(y, θ) and R(z, ψ). That yields
xB yB zB = cθcψ cθsψ −sθ sφsθcψ− cφsψ sφsθsψ + cφcψ sφcθ cφsθcψ + sφsψ cφsθsψ− sφcψ cφcθ xE yE zE (3.4) and the total rotational matrix is denoted RL. This matrix is sometimes referred
to as the direction cosine matrix. One convenient feature of RLis that its inverse
is equal to the transpose, that is R−1
L = RTL, since RL∈ SO(3) [Siciliano and Khatib,
2008]. This is useful when transforming quantities in the body fixed frame to the earth fixed frame.
Denote the linear velocity of the hexarotor expressed in the body fixed frame
VVV = [ u v w ]T. Then the second time derivative of the position ξξξ is ˙ξξξ = RT
LVVV (3.5)
In order to relate the change of attitude with the body angular velocities the dif-ferent steps of the rotation have to be considered [Sidi, 1997]. First, denote the body angular velocities ωωω = [ p q r ]T, where p is rotation around the x
B-axis,
q is rotation around the yBaxis and r is rotation around the zB-axis.
The first rotation applied to the earth fixed frame, the yawing, is subject to three successive angular transformation: rotation around zE, yB1and xB. The second
ro-tation, the pitching is subject to two successive angular transformation: rotation around yB1and xB. Lastly, the rolling is only subject to one attitude
transforma-tion: rotation around xB. That gives the relation
p q r = R(x, φ)R(y, θ)R(z, ψ) 0 0 ˙ ψ + R(x, φ)R(y, θ) 0 ˙θ 0 + R(x, φ) ˙φ 0 0 = 1 0 −sθ 0 cφ sφcθ 0 −sφ cφcθ ˙φ ˙θ ˙ ψ (3.6) where the final transformation matrix is denoted RA. Then the time derivate of
the hexarotor’s attitude ηηη is
˙η˙η˙η = R−1
A ω (3.7)
and with t · short for tan( · ) the inverse can be written
R−1A = 1 sφtθ cφtθ 0 cφ −sφ 0 sφcθ cφcθ (3.8)
14 3 System Modeling
3.3
Dynamics
To begin with, the general equations of linear and angular motion of a rigid body must be derived. To do this, Newton-Euler formalism is used. In order to distin-guish between quantities expressed in different frames of reference, the following notation is used:
• A quantity expressed in the inertial earth frame is denoted XE
• A quantity expressed in the body fixed frame is denoted XB
• A quantity already defined in a frame of reference, e.g. ωωω, will not have the
notation above.
3.3.1
Translational dynamics
In the earth fixed inertial frame, Newton’s second law can be applied [Taylor, 2005], giving
F
FFE = m ¨ξ¨ξ¨ξ = maaaE (3.9) where the time derivate is with respect to the inertial frame and aaaE is the
accel-eration of the body fixed frame expressed in the inertial frame, m is the mass of the body which is constant and FFFE is the sum of all external forces applied to
the body expressed in the inertial frame. However, it would be convenient to ex-press the dynamics in the body fixed frame. By using the rotational matrix from equation (3.5) a change of basis can be obtained and the expression becomes
RLFFFB = mRLaaaB = mRL dVdtVV
!
E
(3.10) where the time derivate is still with respect to the inertial frame and VVV is the
ve-locity of the body fixed frame expressed in the body fixed frame. Computing that derivative is rather cumbersome, but using a well-known relation, sometimes called the transport theorem, yields
FFFB = m dVVV dt ! B + ωωω× VVV ! (3.11) Let the time derivative with respect to the body fixed frame be denoted by a dot, then the final equation of the translational dynamics expressed in the body fixed frame is
F F
FB = m ˙V˙V˙V + ωωω× mVVV (3.12)
3.3.2
Rotational dynamics
Again starting in the inertial frame, Euler’s second axiom is
M M
ME = ˙L˙L˙LE (3.13)
where the time derivative is with respect to the inertial frame, LLLE is the angular
3.4 Applied forces and torques 15 external torques applied to the body expressed in the earth frame. The quantities can be expressed in the body fixed frame by applying the rotational matrix for angular velocities from equation (3.7), giving
RAMMMB = RA˙L˙L˙LB = RA dJωdtωω
!
E
(3.14) where LLL = Jωωω and J is the inertia matrix expressed in the body frame. Di
fferen-tiating J with respect to the earth frame is rather difficult since it will be time-dependent. Instead, using the transport theorem again, the expression becomes
MMMB = dJωωω dt
!
B
+ ωωω× Jωωω (3.15) Since the choice of body fixed frame ensured that the inertia matrix is time in-variant, the final equation is
MMMB = J ˙ω˙ω˙ω + ωωω× Jωωω (3.16)
3.4
Applied forces and torques
The relations in equation (3.12) and (3.16) are general equations of motion for a rigid body. Now, it is time to apply them to the hexarotor system by finding the different components of the external forces FFF and torques MMM. This will be done
in similar manner as [Bresciani, 2008, Pounds et al., 2010].
The forces and torques acting on the hexacopter are gravity, air friction aerody-namic forces and torques produced by the propellers and the gyroscopic effects from the rotation of the propellers. The torque caused by the angular accelera-tion of the propeller has been neglected. All quantities are expressed in the body fixed frame unless anything else is stated.
Tk, Ωk Tj, Qj
Ωj
Qk
(a)Thrust, reaction torque and
direction of rotor rotation
T6 T1 T2 T3 T4 T5 mg
(b) Thrusts and gravity applied
to the rigid body
16 3 System Modeling
3.4.1
Gravity
First off is the gravity which is the only force or torque which is naturally ex-pressed in the earth frame. The gravitational force is acting on the hexacopter’s center of gravity according to Euler’s first axiom and is directed along the zE-axis.
In the body fixed frame the contribution of the gravitional force FFFB
Gis F F FBG= RL 0 0 mg = mg sθ sφcθ cφcθ (3.17) where g is the acceleration due to gravity.
3.4.2
Thrust and torque from propellers
The second contribution is the thrust produced by the aerodynamics of the pro-pellers and reaction torque from the rotation of the rotors. Figure 3.2 shows where the forces are applied on the air frame.
The sum of the thrust from the different propellers is total lift force and is always directed along the negative zB-axis. If the thrust from propeller i is denoted Ti,
then the lift force Fw is
Fw= T1+ T2+ T3+ T4+ T5+ T6 (3.18)
Since the sources of thrust, i.e. the propellers, are not located in the center of gravity, they will create torques around the different axes of rotation. With some basic geometry seen in Figure 3.3, it’s easy to find the produced torque. Around the xB-axis the torque Mpfrom propeller thrust is
Mp= − l 2T1− lT2− l 2T3+ l 2T4+ lT5+ l 2T6 (3.19) where l is the length of each arm. Around the yB-axis the torque Mqis
Mq= √ 3l 2 T1− √ 3l 2 T3− √ 3l 2 T4+ √ 3l 2 T6 (3.20)
The torque around the zB-axis is a result of Newton’s third law. When the
DC-motor accelerates and keeps the propeller rotating, it exerts a torque on the pro-peller shaft. The motor will be subject to an equally sized torque in opposite direction from the propeller shaft. Since the motor is mounted to the airframe, the torque will propagate to the airframe. This torque is often called reaction torque. If the reaction torque from propeller i is called τi the total torque around
the zB-axis denoted Mr is
Mr = −τ1+ τ2− τ3+ τ4− τ5+ τ6 (3.21)
The reaction torque is produced by two different sources. When the motors and propellers accelerate, they will exert a torque on the air frame. Also, pressing the propeller through the air creates friction. This friction is called aerodynamic
3.4 Applied forces and torques 17 30◦ xB yB 60◦ l
Figure 3.3:The geometry of the hexarotor seen from above
torque. The contribution from accelerating the propeller is neglected since its duration will be very quick, and thus the reaction torque will be equal to the aerodynamic torque.
The thrust and aerodynamic torque produced by a propeller can be related to the rotational speed of the propeller blades. A simplified relation of the thrust generation model used in [Bouabdallah, 2007] is used for now. If the rotational speed of propeller i is denoted Ωi, then the generated thrust Tiis
Ti = kTΩ2i (3.22)
where kT is a propeller specific constant explained more thoroughly later. In the
same way, the aerodynamic torque Qi of propeller i becomes
Qi = kQΩ2i (3.23)
where kQis a propeller specific constant.
Now, the total torque MMMAfrom the aerodynamic effects of the propeller can be
written M M MA= −2lkTΩ21− lΩ22− 2lkTΩ23+ 2lkTΩ24+ lΩ52+2lkTΩ26 √ 3l 2 kTΩ21− √ 3l 2 kTΩ23− √ 3l 2 kTΩ24+ √ 3l 2 kTΩ26 −kQΩ21+ kQΩ22− kQΩ23+ kQΩ24− kQΩ25+ kQΩ26 (3.24)
and the total force FFFAbecomes
F F FA= 0 0 −kT(Ω21+ Ω22+ Ω32+ Ω24+ Ω25+ Ω26) (3.25)
18 3 System Modeling
3.4.3
Gyroscopic effects from propellers
A propeller is a mass which rotates both around the propeller shaft, but since it is connected to the airframe, it will follow the rotations of the airframe. If the rotation around the shaft is the spin and the rotation of the airframe is the pre-cession, the gyroscopic torque produced by the propeller can be found. The spin quantity is already defined as Ωi and the precession vector as ω. The gyroscopic
torque MMMGifrom propeller i is
M MMGi = ω × JP 0 0 (−1)iΩ i (3.26) where JP is the inertia matrix of the propeller around the propeller axis and the
factor (−1)i comes from the fact that the propellers rotate in opposite directions.
Using the fact that the inertia matrix is a diagonal matrix the expression can be simplified to M MMGi = ω × 0 0 JP ,zz(−1)iΩi = qJP ,zz(−1)iΩi −pJP ,zz(−1)iΩi 0 (3.27) Summing up all propellers, the total gyroscopic torque MMMGis
M M MG= qJP ,zz(−Ω1+ Ω2− Ω3+ Ω4− Ω5+ Ω6) −pJP ,zz(−Ω1+ Ω2− Ω3+ Ω4− Ω5+ Ω6) 0 (3.28)
3.4.4
Air friction
The air frames movement through the air will cause friction. Because of the shape of the air frame, the air friction is assumed to be low and a simple model is sufficient. The model of the air friction is given by
FFFR= −AT · VVV (3.29)
MMMR= −AR· ωωω (3.30)
where AT and ARare diagonal matrices with diagonal elements aT and aR
respec-tively.
3.5
Thruster model
The thruster model can be separated into two subsystems: the propeller and the electric motor. The propeller converts rotational speed into thrust and the elec-trical motor converts voltage into rotational speed. This part of the modeling is quite often neglected, but can have a big impact of the performance. The most im-portant part is the time delay of the propulsion system which is the time between the pwm-signal is calculated and the propeller reaching the desired speed.
3.5 Thruster model 19
3.5.1
Propeller model
A proper, accurate propeller model includes but is not limited to studying of momentum theory, blade element theory, relative airflow, blade flapping and the bending and twisting of the propeller blades as in works of [Khan and Nahon, 2013]. It is a quite complicated subject, and much of the result comes out as empirical data. A basic propeller model, as found in [Pounds et al., 2010], states that the generated thrust T is
T = CTρrP4πΩ2 (3.31)
where ρ is the air density, rP is the propeller radius and CT is the thrust coefficient
of the propeller. Similarly, the aerodynamic torque Q can be expressed as
Q = CQρrP5πΩ2 (3.32)
where CQ is the propeller torque constant. Modeling CT and CQ is the
compli-cated part of the propeller model and is mostly done empirically. The constants will depend on the geometry of the propeller as well as the direction the pro-peller is traveling in. An extended model of a propro-peller taking theses factors into account can be found in [Khan and Nahon, 2013].
3.5.2
Electrical motor model
The electrical motors used on the platform are rather complex to model in detail. The esc receives a pwm-signal which it decodes to a desired rotor speed. The motor contains permanent magnets. To make the magnets rotate, a magnetic field is moved around the housing of the motor. The magnets are trying to follow that magnetic field, and a rotation is created. The speed of this rotation depends on how fast the magnetic field is moved around which is made by turning on and off the poles in the housing.
The interesting part of the dynamics of the rotor is the time constant and the pwm to rotor speed relation. Because of that, a pwm to voltage mapping and a classic model of a dc-motor is considered sufficient for the purpose of this thesis. The pwmto voltage mapping is described in Chapter 3.7. A second order dc-motor model is found in [Ljung and Glad, 1999]. By assuming that the inductance and the friction caused by the moving parts of the motor are zero the model becomes a first order model which is considered enough. The model is defined by
I = Vin− KvΩ R (3.33a) ˙Ω = Kt IP + I − τ IP (3.33b) τ = Q (3.33c)
where I is the current, Vinis the voltage input, Kvand Ktare speed and torque
constants, Ω is the motor speed, τ is the reaction torque and Q is the aerodynamic torque.
20 3 System Modeling
3.6
Final model
Combining the first derivative of the position equation (3.5), the first derivative of the orientation equation (3.7), the translational dynamics equation (3.12), the ro-tational dynamics equation (3.16), and the external forces equations (3.17), (3.24), (3.25) and (3.28) the final model becomes
˙ξ˙ξ˙ξ = RT LVVV (3.34a) m ˙V˙V˙V =−ωωω× mVVV +BFFFG+BFFFA+BFFFR (3.34b) ˙η˙η˙η = RT Aωωω (3.34c) J ˙ω˙ω˙ω =−ωωω× Jωωω +BMMMA+BMMMG+BMMMR (3.34d)
3.6.1
System input
The input to the system can be chosen as several physical quantities. To fulfill the requirement of platform independence, or at least make the model as modular as possible, one can choose the system input as total aerodynamic force and torque produced by the propellers and the gyroscopic torque as a disturbance.
To extend the model, the rotational speed of the propellers can be chosen as input to the system. Then the model can include a complex relation between rotational speed of the propellers and generated trust and reaction torque. Going even further, one can include the electrical motors in the model. These are controlled by the pwm-input to the esc, and then the duty cycle of the pwm-signals could be the input signal.
In this model, the rotational speed of the propellers will be chosen as input. To simplify notation, these will be mapped to the virtual input signals by
Fw= kT(Ω21+ Ω22+ Ω32+ Ω24+ Ω25+ Ω26) (3.35a) Mp= − l 2kTΩ21− lkTΩ22− l 2kTΩ23+ l 2kTΩ24+ lkTΩ25+ l 2kTΩ26 (3.35b) Mq= √ 3l 2 kTΩ21− √ 3l 2 kTΩ23− √ 3l 2 kTΩ24+ √ 3l 2 kTΩ26 (3.35c) Mr = −kQΩ21+ kQΩ22− kQΩ32+ kQΩ24− kQΩ25+ kQΩ26 (3.35d) WG= (−Ω1+ Ω2− Ω3+ Ω4− Ω5+ Ω6) (3.35e)
where Fw corresponds to the third component of aerodynamic force FAfrom the
propellers defined by equation (3.25) and Mp, Mqand Mr to the components in
the aerodynamic torques MA defined by equation (3.24) and where WG is the
input to the disturbance caused by gyroscopic torques.
3.6.2
Total system model
For convenience, the system model is written in component form where the in-puts have been included
3.7 Simulator 21 ˙y = cθsψu + (sφsθsψ + cφcψ)v + (cφsθsψ − sφcψ)w (3.36b) ˙z = −sθu + sφcθv + cφcθw (3.36c) ˙u = rv − qw + sθg −aT mu (3.36d) ˙v = pw − ru + sφcθg −amTv (3.36e) ˙w = qu − pv + cφcθg −m1Fw− amTw (3.36f) ˙φ = p + sφtθq + cφtθr (3.36g) ˙θ = cφq − sφr (3.36h) ˙ ψ = sφ cθq + cφ cθr (3.36i) ˙p = Jyy− Jzz Jxx qr + 1 JxxMp+ JP ,zz Jxx qWG− aR Jxxp (3.36j) ˙q = Jzz− Jxx Jyy rp + 1 JyyMq− JP ,zz Jyy pWG− aR Jyyq (3.36k) ˙r = Jxx− Jyy Jzz pq + 1 JzzMr− aR Jzzr (3.36l)
3.7
Simulator
The model derived earlier was implemented in matlab/Simulink. The aim was to make it modular, easily upgradeable and modifiable in terms of number of propellers and configuration. This led to a Simulink model divided into several subsystems. The subsystem containing the thruster could easily be altered to a quadcopter or octacopter by just changing a variable in the configuration file. The dynamic model is implemented in several subsystems according to Figure 3.4. The core is an arbitrary rigid body which is subject to external forces and torque. The rigid body contains information about its mass and moment of iner-tia. The output from the rigid body is velocity, acceleration, angular acceleration and angular speed according to equations (3.34b) and (3.34d).
The external forces are gravity, air friction and aerodynamic thrust and torque as in equations (3.34b) and (3.34d). Gravity and air friction are implemented straight forward. To transform gravity into body coordinates, the function block needs the angles of the hexarotor.
The aerodynamic thrust and torque are implemented in a more complex block which is seen in Figure 3.5. The input signal to the thruster is the duty cycle of the PWM-signal. This is mapped to a voltage according to [Khan and Nahon, 2013], that is
Vin= P (δ) = 4.99 · 10−9δ3− 2.66 · 10−5δ2+ 4.93 · 10−2δ− 27.33δ (3.37)
22 3 System Modeling Rigid body J, m V ˙ V ω ˙ω F M + g η Gravity Air friction V AT Air friction ω AR Thrusters + MA FA MG FG FR MR δi
Figure 3.4: A block diagram showing how the dynamic equations are imple-mented in Simulink. A rigid body translate the external forces generated by gravity, air friction and the thrusters into translational and rotational move-ment.
and aerodynamic torque Qi as inputs and produces an angular velocity Ωi and a
reaction torque τi. The propeller block is an implementation of equations (3.31)
and (3.32). From the angular speed of the motor it produces thrust and reaction torque. It contains information about the propeller: thrust and torque constants
CT and CQ, air density ρ and propeller radius r. The gyroscopic block realizes
equation (3.27), converting angular speed of the rotor and body angular rate of the hexarotor to gyroscopic torque MGi.
Only one thruster block is implemented. Instead of making six equal blocks, the thruster block has vectors as inputs and outputs. Every element in the vector represents a thruster. This makes it possible to simulate not just hexarotors, but
DC-motor Kt, Kv,JP ,zz, R Ω τi Vin PWM-to-voltage δ P (δ) ω Gyroscopic JP ,zz MGi Qi Propeller CT, CQ, ρ, r Ti Geometry MG FA MA l
3.7 Simulator 23 a multirotor with an arbitrary number of rotors.
In order to make the system as arbitrary as possible with respect to number of rotors, the geometry block had to be implemented. This block maps the thrust and torques produced by the arbitrary number of propellers to net forces and torque exerting on the hexarotor. It contains information about length of the arms, propeller rotation direction and the geometry seen in Figure 3.3, i.e. the position of the thruster with respect to the body-fixed frame.
The numerical parameters used in the simulation are a mixture of previous, sim-ilar projects and measurements of the the real platform. In Table 3.1 all parame-ters used in the simulator can be found.
Name Description Numerical value Source
m Mass of platform 2.1 kg Measurement
l Length of arm 0.32 m Measurement
Ixx Moment of inertia around
x-axis 3.8 · 10
−3kgm2 [Sanca et al., 2010]
Iyy Moment of inertia around
y-axis 3.8 · 10
−3kgm2 [Sanca et al., 2010]
Izz Moment of inertia around
z-axis 7.1 · 10
−3kgm2 [Sanca et al., 2010]
CT Propeller thrust constant 0.01458 [Sanca et al., 2010]
CQ Propeller torque constant 1.037 · 10−3 [Sanca et al., 2010]
r Propeller radius 0.14 m Measurement
Kv Motor constant 4.19 · 10−3Vs/rad [Sanca et al., 2010]
Kt Motor constant 4.19 · 10−3Nm/A [Sanca et al., 2010]
R Motor resistance 1 Ω [Sanca et al., 2010]
IP ,zz Motor and propeller inertia
around propeller shaft 4.3 · 10
−5kgm2 [Sanca et al., 2010]
aT Translational air friction
constant 4.8 · 10
−2Ns/m [Sanca et al., 2010]
aT Rotational air friction
con-stant 6.4 · 10
−4Ns/m [Sanca et al., 2010]
ρ Air density 1.20 kg/m3 [Nordling, 2006]
g Acceleration of gravity 9.81 m/s2 [Nordling, 2006]
4
Control design
The hexarotor is a very unstable system and not possible to control without a proper control system. In order to perform identification experiments a control system was implemented both in simulation and on the real platform. The initial control system used a decentralized approach with cascade P-PD controllers for each axis with empirically chosen parameters. The input commands to the con-trol loops were sent by a test pilot with a radio concon-troller when flying the real platform.
4.1
Choice of control design
In previous works several different control design are proposed to control multi-rotor systems. Model depended controller such as ackstepping [Sanca et al., 2010, Arellano-Muro et al., 2013], linear quadratic [Bouabdallah et al., 2004, Rinaldi et al., 2013], feedback linearization [Mokhtari et al., 2006] and model predictive control [Alexis et al., 2011, Abdolhosseini et al., 2013, Barsk, 2012] are popular in literature regarding control of multirotor systems. However, most of the result from those works are from simulations.
pidcontrollers are a well tried concept, even among multirotors. [Alaimo et al., 2014, Bouabdallah et al., 2004, Rinaldi et al., 2013] have all successfully imple-mented such controllers. Since the final users of the autopilot should be able to understand and tune the controllers without a degree in master of science, UAS Europe prefers pid controllers.
26 4 Control design
4.2
Control techniques
The initial control system used three different concepts: decentralization, cascad-ing and pid. The result was three different P-PD controllers, all easily tuned and implemented that together were capable of controlling the hexarotor.
4.2.1
Decentralization
A common approach when controlling the attitude of a multirotor system is to neglect the effects from the cross couplings and design one SISO-controller for every control signal, that is roll, pitch and yaw. The cross couplings will however exist. The gyroscopic effects from the rotors are estimated to be small since the inertia of a rotor is very small. But the gyroscopic effects from the rotating air-frame may be in the same size as the control signal when the angular rates are big. Although, previous works have shown that treating the cross couplings as a disturbance is sufficient for controlling a multirotor. This strategy can be found in works of [Bouabdallah, 2007, Bresciani, 2008] among others and has been suc-cessfully implemented in the commercial autopilot ArduCopter. [ArduPilot.com, 2012].
In Figure 4.1 a simple block diagram shows the concept of a decentralized control on a hexarotor. One control loop for every axis, roll, pitch and yaw, was used together with a throttle mapping.
Throttle mapping Pil ot commands Hexarotor Roll Control
Roll, roll rate Pitch Control
Pitch, pitch rate Yaw Control
Yaw, yaw rate
Figure 4.1: The three different loops controlling the hexarotor together with a throttle mapping.
4.2.2
Cascade structure
A cascade structure is often used when the dynamics of a system is of varying speed. The inner loop controls the fast dynamics of the angular rate while the outer controller works with the slower angle dynamics. This is a control design
4.2 Control techniques 27 which in literature is referred to as a PD2controller and which has been
success-fully implemented in the ArduCopter project.
A block diagram of a cascade control structure can be found in Figure 4.2. The pilot command is transformed into a desired angular rate by comparing it to the current angle of the hexarotor. The desired angular rate is the reference signal to the inner, faster controller. This controller produces the virtual control signal.
Angle control
Pil
ot
commands
Hexarotor
Angular rate control
Angle Angular rate
Desired angular rate V irtual con trol
Figure 4.2: The cascade structure of the control loops handling attitude. The critical part is the inner controller which controls the angular rate. This is where the stability of the hexarotor is ensured. Once able to control the angular rates, controlling the angles is just a matter of setting the angular rate. The outer controller will then be more of a design choice, the hexarotor can easily be made more or less aggressive.
4.2.3
The general
PIDcontroller
Both the angle and angular rate controller is initially implemented using pid techniques. pid controllers are very popular, easily implemented and can be tuned by common users. The ideal pid controller can be found in [Glad and Ljung, 2006] and is defined by
u(t) = KPe(t) + KI t
Z
t0
e(τ)dτ + KDde(t)dt (4.1)
where e(t) is the error between reference signal and measured value. A perfect derivation is never possible in practice and the transfer function of the controller becomes
F(s) = KP +KsI + KDµs + 1s = KP + KsI + KD 1
KDµ + 1s
(4.2) where µ is a design parameter. The implemented controller is a discrete time controller so the pid controller has to be discretized. Using Euler backward, the
28 4 Control design
derivative can be approximated as s = T1
s(1 − z
−1), where T
s is the sample time,
which yields the discrete time transfer function
F[z] = KP + KI Ts
1 − z−1+ KD 1
KDµ +1−zTs−1
(4.3) To implement the derivative part as a discrete time system, the transfer function for the derivative is written as a recurrence relation
d[k] = 1 KDµ +1−zTs−1 e[k] = (1 − z−1) KDµ(1− z−1) + Ts e[k] = KDµz−1 KDµ + Tsd[k] + 1 − z−1 KDµ + Tse[k] = KDµ KDµ + Ts d[k− 1] + 1 KDµ + Ts(e[k] − e[k − 1]) (4.4)
where d[k] is the derivative part of the output, which also is described in [Enqvist et al., 2010]. A discrete time PD controller would then be
u[k] = KPe[k] + KDd[k], d[k] = KDµ KDµ + Tsd[k− 1] + 1 KDµ + Ts(e[k] − e[k − 1]) (4.5)
4.3
Implemented control
The controllers were implemented on the autopilot and tuned empirically. The control loop ran at approximately 60 Hz. Figure 4.3 shows a block diagram de-scribing the different control loops, mapping and scaling.
4.3.1
Roll
The roll controller used a cascade structure including an outer P controller and an inner PD controller. The outer controller is described by
˙φD[k] = P
φ(φC[k] − ˆφ[k]) (4.6)
where the output ˙φD is the desired roll rate, φC is the roll command from the
pilot and ˆφ is the estimated roll angle. The inner controller is described by
Up[k] = KPe[k] + KDd[k], d[k] = KDµ KDµ + Tsd[k− 1] + 1 KDµ + Ts(e[k] − e[k − 1]) (4.7)
4.3 Implemented control 29 Mapping P +- PD +-P +- PD +-PD +-++ Mapping Scaling Hexarotor φC ˆ φ ˙φD ˆp Up θC ˆθ ˙θD ˆq Uq ˙ ψC ˆr Ur TC U w δ 1 ,2 ,..., 6 u1, 2,..., 6
Figure 4.3: A block diagram of the control loops running together with throttle mapping, virtual control to control signal mapping and hardware scaling.
where e[k] = ˙φD[k] − ˆp[k], ˆp is the measured roll rate and U
p is the virtual roll
control. Here, the assumption that p ≈ ˙φ, since the roll angle φ and the pitch angle θ always will be small, was made.
Both the input to the outer loop, the commanded roll angle, and the input to the inner loop, the desired roll rate, as well as the output Up from the cascade loop
was limited. The limit was lower than the physical limit of the propulsion system and was made to avoid far too aggressive commands.
4.3.2
Pitch
The pitch controller is implemented in the exact same manner as the roll control. The outer controller is described by
˙θD[k] = P
θ(θC[k] − ˆθ[k]) (4.8)
where the output ˙θD is the desired pitch rate, θCis the pitch command from the
pilot and ˆθ is the estimated pitch angle. The inner controller is described by
Uq[k] = KPe[k] + KDd[k], d[k] = KDµ KDµ + Tsd[k− 1] + 1 KDµ + Ts(e[k] − e[k − 1]) (4.9) where e[k] = ˙θD[k] − ˆq[k], ˆq is the measured pitch rate and U
qis the virtual pitch
control. Here, the assumption that q ≈ ˙θ was made.
30 4 Control design
the inner loop, the desired pitch rate, as well as the output Uqfrom the cascade
loop was limited. The limit was lower than the physical limit of the propulsion system and was made to avoid far too aggressive commands.
4.3.3
Yaw
The yaw loop is implemented with only the inner PD controller and the pilot is thus able to control the yaw rate directly. The yaw control is described by
Ur[k] = KPe[k] + KDd[k], d[k] = KDµ KDµ + Tsd[k− 1] + 1 KDµ + Ts(e[k] − e[k − 1]) (4.10) where e[k] = ˙ψC[k] − ˆr[k], ˙ψCis yaw rate command from the pilot, ˆr is the
mea-sured yaw rate and Uris the virtual yaw control. Here, the assumption that r ≈ ˙ψ
has been made.
The input to the controller, the commanded roll rate, and the output Ur from
the controller, was limited. The limit was lower than the physical limit of the propulsion system and was made to avoid far too aggressive commands.
4.3.4
Throttle
The throttle was mapped from pilot command to virtual throttle control with the relation
Uw = c1(ec2(RC−Rmin)/Rmax− 1) − Ulim (4.11)
where Uwis the virtual throttle control limited between ±Ulim, RCis the throttle
command from the pilot limited between Rminand Rmax. The exponential
map-ping makes the pilot’s throttle adjustment smoother at low throttle speed. To make sure the mapping is from [Rmin, Rmax] to [−Ulim, Ulim] c2 is only tunable
parameter. c1is calculated by
c1= Rmax+ Umax
ec2∗(Rmax−Rmin)/Rmax− 1 (4.12)
With this implementation the pilot was able to control the roll and pitch angle, the yaw rate and throttle which is a common set up for hobby multirotor systems.
4.3.5
Scaling and rotor mapping
The four virtual control signals had to be mapped and scaled to actual pwm signals to the rotors. This was done in two steps. First, with the help of equa-tions (3.35b) and (3.35c) the applied roll and pitch moment could be written
