Universidade de Aveiro
Departamento de
Electr´
onica, Telecomunica¸
c˜
oes e Inform´
atica,
2014
Pedro Amorim
Pais Correia
Simulador de Quadrotor
Quadrotor Simulator
Universidade de Aveiro
Departamento de
Electr´
onica, Telecomunica¸
c˜
oes e Inform´
atica,
2014
Pedro Amorim
Pais Correia
Simulador de Quadrotor
Quadrotor Simulator
Disserta¸
c˜
ao apresentada `
a Universidade de Aveiro para cumprimento dos
requisitos necess´
arios `
a obten¸
c˜
ao do grau de Mestre em Engenharia
Electr´
onica e Telecomunica¸
c˜
oes, realizada sob a orienta¸
c˜
ao cient´ıfica de
Telmo Reis Cunha, Professor do Departamento Electr´
onica,
Telecomu-nica¸
c˜
oes e Inform´
atica da Universidade de Aveiro
o j´uri / the jury
presidente / president Prof. Doutor Alexandre Manuel Moutela Nunes da Mota
Professor Associado da Universidade de Aveiro (por delega¸c˜ao da Reitora da Universidade de Aveiro)
vogais / examiners committee Prof. Doutor Telmo Reis Cunha
Professor Auxiliar da Universidade de Aveiro (orientador)
Prof. Doutor Jos´e Ant´onio Barros Vieira
agradecimentos / acknowledgements
Gostaria de agradecer aos meus pais Manuel Rui Pais Correia de Pinho e Maria Rocha Amorim por todo o apoio que me deram e pelos sacrif´ıcios que fizeram para
que eu podesse ter esta oportunidade. Agradecer tamb´em `a minha namorada,
Diana Marisa Pereira Fernandes, pelo apoio que me deu e pela paciˆencia extra
que teve de ter durante este longo periodo de tempo enquanto eu elaborava este
trabalho. Agradecer tamb´em ao meu orientador, Prof. Doutor Telmo Reis Cunha,
pela sua disponibilidade, pelo seu conhecimento e pelo tempo dispendido com este
trabalho. E por ´ultimo tamb´em gostava de agradecer a todos que me ajudaram e
Resumo Nos ´ultimos tempos tˆem surgido v´arias aplica¸c˜oes onde os dispositivos
quadro-tors (tamb´em genericamente conhecidos por ”drones”) tˆem sido utilizados, tendo
o respetivo mercado apresentado uma evolu¸c˜ao consider´avel. O projeto de um
quadrotor ´e muito desafiante pois envolve o controlo simultˆaneo de quatro ou mais
motores el´etricos para que o quadrotor siga uma determinada trajet´oria no espa¸co
tridimensional (com determinada orienta¸c˜ao). Igualmente desafiante ´e a an´alise
de desempenho de um quadrotor quando utilizado numa aplica¸c˜ao espec´ıfica, tal o
seguimento de um elemento m´ovel com uma cˆamara de v´ıdeo, consitituindo esta um
peso adicional consider´avel. Com o objetivo de permitir elaborar e analisar t´ecnicas
de controlo de dispositivos quadrotor, numa fase anterior `a sua implementa¸c˜ao, o
trabalho aqui apresentado consistiu na implementa¸c˜ao de um simulador de
dis-positivos quadrotor, tentando este ser o mais pr´oximo poss´ıvel de implementa¸c˜oes
reais. Assim, aplicando t´ecnicas avan¸cadas de simula¸c˜ao num´erica, aliadas a mode-los dinˆamicos da estrutura f´ısica do quadrotor (sendo esta configur´avel) e a modelos
de motores el´etricos e das respetivas h´elices, foi projetado e implementado um
sim-ulador em software que permite ao engenheiro de controlo testar e analisar diversas
solu¸c˜oes de controlo de quadrotors. Adicionalmente, permite tamb´em ao utilizador
do quadrotor testar o desempenho de um quadrotor espec´ıfico sob determinadas
Abstract Lately, it has emerged several applications where the quadrotor devices (generally known as drones) has been used, having the respective market shown a consider-able evolution. The project of a quadrotor is very challenging as it involves the simultaneous control of four or more electric motors so that the quadrotor follows a certain trajectory in the tridimensional space (with a certain orientation). Equally challenging is the performance analyses of a quadrotor when used in a specific application, such as following a mobile element with a video camera, having the last a considerable additional weight. With the goal of allowing the elaboration and analyses of control techniques of quadrotor devices, in a stage previous to its implementation, the work here presented consists in the implementation of a simulator of quadrotor devices, trying to make it as close as possible to the real implementations. Therefore, applying advanced numerical simulation techniques, combined with dynamic models of the quadrotor’s physical structure (being this one configurable) with the models of electric motors and the respective propellers, it has been projected and implemented a simulator in software that allows the con-trol engineer to test and analyse several quadrotors concon-trol solutions. Additionally, it also allows the quadrotor user to test the performance of a specific quadrotor under certain conditions, before submitting it to the real test.
Contents
Contents i
List of Figures iii
List of Tables v
Nomenclature 1
1 Introduction 4
1.1 Quadrotor . . . 4
1.2 Iterative Time-Step Integration Methods . . . 6
1.2.1 Euler’s method . . . 7
1.2.2 Runge Kutta 4 (RK4) . . . 7
1.2.3 Adams-Bashforth-Moulton (ABM) . . . 7
1.2.4 Dormand Prince (DOPRI) . . . 8
1.3 Structure of the Thesis . . . 10
2 Birotor 11 2.1 Dynamic Model . . . 11
2.2 Results . . . 18
3 Quadrotor 24 3.1 Dynamic Model . . . 26
3.2 Orientation of a rigid body . . . 33
3.2.1 Euler Angles . . . 33 3.2.2 Quaternions . . . 35 3.3 Motor . . . 37 3.4 Propeller . . . 38 3.5 Results . . . 39 4 Quadrotor Control 45 4.1 Motor Controller . . . 45 4.2 Controller1 . . . 46 4.3 Controller2 . . . 47 4.4 Controller3 . . . 48 4.5 Results . . . 50 5 Implementation 56 5.1 OpenGL . . . 60 5.2 Controllers . . . 60
5.3 Motor and Propeller . . . 61
5.4 Sensors . . . 62
5.6 GUI . . . 63
6 Conclusion and Future Work 65
List of Figures
1.1 Picture of Br´eguet-Richet Gyroplan from [1] . . . 4
1.2 Picture of Oehmichen No.2 from [3] . . . 5
1.3 Picture of Curtiss-Wright VZ-7 from [5] . . . 5
2.1 First approach to the Birotor’s model. . . 12
2.2 Representation of the force produced by the bar. . . 12
2.3 Representation of the decomposition of a vector. . . 13
2.4 Motion of the birotor. . . 16
2.5 Plot with the angular velocity, length of the bar and Fbar of the birotor. . . 16
2.6 Rigid body approach to the birotor’s model. . . 17
2.7 Motion of the birotor in Experiment 1. . . 20
2.8 Error between each of the implemented solver algorithms and the SIMULINK so-lution in Experiment 1. . . 21
2.9 Error between each of the implemented solver algorithms and the SIMULINK so-lution in Experiment 2 . . . 22
3.1 Quadrotor’s degrees of freedom. . . 24
3.2 Motor’s and propeller’s configuration. . . 25
3.3 Basic movements of a quadrotor: a) side movement; b) forward movement; c) rotation; d) upwards movement. . . 25
3.4 Set of referentials to represent the quadrotor’s behaviour. . . 26
3.5 Representation of a generic point with the forces acting on it. . . 29
3.6 Representation in the xOy plane. . . 30
3.7 Representation in the xOz plane. . . 31
3.8 Representation in the yOz plane. . . 32
3.9 Example of Euler angle rotations from the xyz frame into the x000y000z000 frame. . . . 34
3.10 Rotation of a referential using a quaternion. . . 35
3.11 DC Motor equivalent model. . . 37
3.12 BET subdivision. . . 38
3.13 Depiction of BET forces and vectors. . . 39
3.14 Angular velocity vs time of the DC motor. . . 40
3.15 Torque vs time of the DC motor. . . 40
3.16 Torque vs angular velocity and power vs angular velocity of the DC motor. . . 40
3.17 Angular velocity vs voltage of the DC motor. . . 41
3.18 Torque vs voltage of the DC motor. . . 41
3.19 Thrust vs time and torque vs time of the propeller. . . 41
3.20 Thrust vs angular velocity and torque vs angular velocity of the propeller. . . 41
3.21 Error between each of the implemented DOPRI algorithms and the SIMULINK solution in Experiment 3. . . 42
3.22 Error between each of the implemented solver algorithms in Experiment 4. . . 44
4.2 Diagram of controller1 . . . 46
4.3 Diagram of the global system. . . 47
4.4 Diagram of controller2. . . 47
4.5 Diagram of modified controller2. . . 48
4.6 Diagram of final version of controller2. . . 48
4.7 Performance of the motor controller(angular velocity). . . 51
4.8 Performance of the motor controller(current). . . 51
4.9 Performance of the motor controller(control signal). . . 51
4.10 Performance of controller1. . . 52
4.11 Control signals of the motors with controller1. . . 53
4.12 Performance of controller2. . . 53
4.13 Control signals of the motors with controller2. . . 54
5.1 C++ program flowchart. . . 57
5.2 MatLab program flowchart. . . .R 59 5.3 Simulator’s window. . . 61
5.4 GUI propeller appearance. . . 61
5.5 GUI motor appearance. . . 62
5.6 GUI sensor appearance. . . 62
5.7 GUI interface appearance. . . 63
5.8 GUI appearance. . . 63
List of Tables
2.1 Initial conditions and values of the parameters of the simulation with the first
birotor model approach. . . 16
2.2 Experiment 1: Initial conditions and values of the parameters. . . 19
2.3 Results of the evaluation of the implemented solver methods, for Experiment 1. . . 19
2.4 Results of the evaluation of the implemented solver methods, for Experiment 2. . . 22
3.1 Values of the parameters used for the DC motor. . . 40
3.2 Values of the parameters for the propeller. . . 41
3.3 Results of the evaluation of the implemented solver methods, for Experiment 3. . . 42
3.4 Initial conditions and values of the parameters for the Experiment 4. . . 43
3.5 Results of the evaluation of the implemented solver methods, for Experiment 4. . . 43
4.1 Saturation values and set point used in experiment 5 . . . 50
4.2 Experiment 5: Initial conditions and values of the parameters. . . 52
Nomenclature
List of Symbols
Symbol Units Description
D N s m−1 Friction coefficient
k N m−1 Elasticity constant of a spring
b N s m−1 Dynamic friction constant of a damper
r m Birotor’s arm length (half of the length of the
bar)
α rad Birotor’s orientation angle
F1 N Force generated by motor 1 propeller
F2 N Force generated by motor 2 propeller
Fb N Force produced by the birotor’s bar
Fbar N Force produced by the bar
Fspring N Force produced by the spring
Fdumper N Force produced by the damper
Ff N Friction force
m1 kg Mass of motor 1 and propeller
m2 kg Mass of motor 2 and propeller
M kg Total mass of the aircraft
g m s−2 acceleration of gravity
x m coordinate in the x-axis
y m coordinate in the y-axis
z m coordinate in the z-axis
F3 N Force generated by motor 3 propeller
F4 N Force generated by motor 4 propeller
T1 N m Torque generated by motor 1 propeller
T2 N m Torque generated by motor 2 propeller
T3 N m Torque generated by motor 3 propeller
T4 N m Torque generated by motor 4 propeller
r1 m Quadrotor’s arm 1 length
r2 m Quadrotor’s arm 2 length
r3 m Quadrotor’s arm 3 length
r4 m Quadrotor’s arm 4 length
rs m Quadrotor’s central body length
rq m Quadrotor’s arm (Symmetric)
m kg Mass of central body
m3 kg Mass of motor 3 and propeller
m4 kg Mass of motor 4 and propeller
φ rad Quadrotor’s roll angle
θ rad Quadrotor’s pitch angle
ψ rad Quadrotor’s yaw angle
Vy - y component of vector V
Vz - z component of vector V
VB - Vector V set in the body frame referential’s
coordinates
VL - Vector V set in the local referential’s
coordi-nates
VI - Vector V set in the inertial referential’s
coor-dinates
~v - Vector that identifies the position of the origin
of the body frame referential in the inertial referential coordinate system
sφ - sin(φ) cφ - cos(φ) sθ - sin(θ) cθ - cos(θ) sψ - sin(ψ) cψ - cos(ψ) q - Quaternion
qx - Quaternion’s first imaginary element
qy - Quaternion’s second imaginary element
qz - Quaternion’s third imaginary element
~
qe - Quaternion’s imaginary vector
qw - Quaternion’s angle
Ia A Armature’s current
ω rad s−1 Rotational speed of the rotor
J kg m2 Moment of inertia of the rotor
f N m s rad−1 Motor viscous friction constant
Kb V s rad−1 Back-emf constant
Kt N m A−1 Motor torque constant
Ra Ω Electric resistance
La H Electric inductance
ρ 1.225 kg m−3 air density
CL - Blade’s lift coefficient
CD - Blade’s drag coefficient
R m Length of the blade
N b - Number of Blades per propeller
θb rad Blade’s twist angle
αb rad Blade’s angle of attack
βb rad Inflow angle
c m Chord length
dr m Element of the blade
K - Feedback matrix
S - Kalman’s controllability matrix
Kp - PID’s proportinal parameter
Ki - PID’s integral parameter
Kd - PID’s derivative parameter
Φ(λ) - Characteristic polynomial of the desired
com-pensated system
∆r1 m Deviation in quadrotor’s arm 1 length
∆r2 m Deviation in quadrotor’s arm 2 length
∆r3 m Deviation in quadrotor’s arm 3 length
∆r4 m Deviation in quadrotor’s arm 4 length
∆m2 m Deviation in quadrotor’s mass 2 weight
∆m3 m Deviation in quadrotor’s mass 3 weight
∆m4 m Deviation in quadrotor’s mass 4 weight
Acronyms
UAV Unmanned Aerial Vehicles
ODE Ordinary Differential Equation
IVP Initial Value Problem
LTI Linear Time-Invariant
API Application Programming Interface
GPU Graphics Processing Unit
RK4 Runge–Kutta 4
AB4 Adams-Bashforth 4th order
AM3 Adams-Moulton 3rd order
ABM Adams-Bashforth-Moulton
DOPRI Dormand-Prince
WGN White Gaussian Noise
BET Blade Element Theory
PID Proportional-Integral-Derivative
ESC Electronic Speed Controller
Chapter 1
Introduction
1.1
Quadrotor
The quadrotor is an unmanned aerial vehicle (UAV) that belongs to the rotorcraft family. The rotorcrafts have an advantage over aeroplanes as they can manoeuvre in small spaces relatively to the size of the aircraft. They also have the ability to move in any direction, hover, do vertical take off and landing in almost any terrain. Quadrotors are robust and simple when compared with other rotorcrafts like the helicopters as they do not have complicated swashplates and complex designed linkages as in conventional rotorcrafts. Due to this mechanical simplicity they do not need as much mechanical maintenance as the others rotorcrafts being less costly over time.
A quadrotor is characterized by having two pairs of fixed propellers and motors. One pair rotates clockwise and the other one rotates counter clockwise. With this configuration it is possible to cancel the gyroscopic effects and aerodynamic torques and even control it during flight. The control of the vehicle is performed by altering the rotational speed of the motor’s rotor and consequently the torque and thrust produced by the propeller’s blades. The four rotors are usually laid up symmetrically and balanced around its centre and mounted in cross or plus configuration so that it will result in a system with good characteristics for a stable and smooth flight.
The concept of quadrotor appeared in 1907 with Louis Br´eguet when he built with his professor
Charles Richet the Br´eguet-Richet Gyroplan[1], shown in figure 1.1, which was a quadrotor that
could carry one man. This prototype was able to lift itself with the pilot with its own power but since this was the main propose of the prototype they could not control it in order to move in any direction.
Figure 1.1: Picture of Br´eguet-Richet Gyroplan from [1]
Another famous quadrotor in history was the Oehmichen No.2, shown in figure 1.2, constructed in 1920 by the french engineer Etienne Oehmichen[2], being driven by one engine. This quadrotor had 8 propellers, five in the horizontal plane, one for steering and the remaining two for forward propulsion. The prototype exhibited a considerable degree of stability and controllability for its
time and was able to remain airborne for several minutes at a time and was the first rotorcraft to complete the first one kilometer closed-circuit.
Figure 1.2: Picture of Oehmichen No.2 from [3]
In 1958, Curtiss-Wright company developed the Curtiss-Wright VZ-7[4, 5], shown in figure 1.3, for the US Army. This prototype was able to be fully controlled by changing the thrust of each of the four propellers and its main purpose was to transport heavy weights as the US Army requested a ”flying jeep” but the prototype was retired and cancelled because it didn’t met the Army’s standards.
Figure 1.3: Picture of Curtiss-Wright VZ-7 from [5]
The development of quadrotors was stalled until recently, because controlling four independent rotors has proven to be incredibly difficult and almost impossible without electronic assistance but the development of the digital technology, the evolution in the sensor’s accuracy and the de-creasing cost of modern electronics made the design and construction of completely autonomous quadcopters feasible in the present days which lead to an increasing popularity of the aircrafts. Nowadays, the quadrotor is used in a large variety of sectors and businesses as for example the surveillance of locations hard to access, transportation, search-and-rescue in hazardous environ-ments, inspection of structures with large dimensions (bridges, buildings, powerlines ,...), mobile sensor networks, high-tech toys, military as a recognition vehicle to allow a more efficient way to plan strategies and for research in scientific fields such as engineering. Today in the market there is a large variety of quadrotors with several diferent characteristics. The models of quadrotors go from big ones with over one meter of diameter to micro quadrotors that are smaller than an adult hand palm, equip with GPS and cameras and able to do autonomous flights. There are even projects where these UAVs are capable of following a person with an autonomous flight through image processing when equipped with a camera and a good processing unit. The Hexo+[6] is a good example with all this characteristics developed to film acrobatic manoeuvres in action sports
that came to replace crews with several persons that usually do this with an helicopter. This UAV also has an interesting feature that allows it to tilt the rotors to adjust the direction of the propeller’s lift without tilting the orientation of the central body and camera to get a smooth and stable shooting. Despite all of this, the technology in the present time has a limit and that is the lightweight battery’s capacity that usually gives a quadrotor around fifteen minutes of flight autonomy depending on the consumption of the hardware used.
There is when a simulator comes in hand in order to test and implement the algorithms or even test the best characteristics of a quadrotor to do a specific task, but to do this, one must first analyse the problems and needs that the simulator has, and try to get a solution to them. The simulator needs to know how the quadrotor behaves and to do so, it needs the physical-based
model that has a set of non-linear ordinary differential equations (ODEs) in the form ˙y = f (y, u, t)
where y and ˙y are the state variables of the system and their derivatives, u the variables that can be
controlled to interact with the system and t the instantaneous time. As the simulator is a software that works on a discrete device, it will need a way to estimate an approximation to discrete time instants by solving the ODEs. In numerical analysis there are several ways to produce this estimation but due to the fact that the model has non-linear equations the simulator can’t use pivoting methods and therefore must use iterative methods.
1.2
Iterative Time-Step Integration Methods
As mentioned above, an iterative method is mathematical way to generate an approximate solution for a problem. In this case the problem is presented as a set of first order non-linear ordinary differential equations with an initial state, also known as a Initial Value Problem (IVP) with the formulation presented in the set of equations (1.1).
( y(t) = f t, y(t)˙
y(t0) = y0
(1.1)
There are methods that only produce one approximation for the solution and others that use a sequence of approximations and choose the solution based on a stopping criterion like the error between consecutive approximations. A specific method is called convergent if the approximation of sequence generated by that method converges for the solution of the problem and the order of convergence of a method tells how fast the method converges to the solution with consecutive approximations. Another characteristic of a method is the order of its error that is usually related to the method order.
There is a class of methods that generates the approximation based on information from previous steps other than the present one and they are called multistep methods. The multistep methods try to gain efficiency with this kind of approximation as the information from the previous steps is already available saving the extra computation time. The other methods like those in the Runge-Kutta family divide the time interval delimited by the time step (h) into smaller ones and calculate approximations for each of the smaller ones and then use them in a weighted average
to calculate a more accurate approximation to the next step. Some methods are even more
sophisticated and use a variable time step in order to produce accurate approximations, with low computational effort. They choose the time step based on a criterion that uses the error of the approximation and the tolerance specified by the user to calculate the widest time step that satisfies the user’s tolerance. Another way to classify a method is about its time dependency. If a method uses only information from the current and previous steps to calculate the next step then the method is called an explicit method but if the method uses information from the next step in order to calculate the approximation to that next step then it is called an implicit method. Explicit methods are more direct and faster but the implicit methods generally have a better accuracy.
In the present time there are several simulators and physics engines that solve dynamic equa-tions of a class of systems. From those engines that use iterative methods some examples are the
Gauss-Seidel method, the Successive over-relaxation method, conjugate gradient method and the Newton’s method with the Krylov subspaces. Other example in simulators are the Gear method
from Pspice . MatLabR offers a wide range of solvers as for example the simple Euler’s method,R
the default Dormand Prince method (ode45), the Runge-Kutta’s method, the Heun’s method, the Adams-Bashforth-Moutlon and more. In this particular case as the simulator needs to produce an accurate solution in a very short period of time i.e. with the least computational effort to run in real time, the implicit methods will not be implemented as the explicit methods are preferred.
1.2.1
Euler’s method
The explicit Euler’s method[7] (Forward Euler) is the most basic method. It’s a first order method, which means that this method produces an estimation with an truncation error per step
of O(h2), where h is the step time between to consecutive samples, and a accumulated error of
O(h). The equations (1.2) and (1.3) describe the formulation of the explicit Euler’s method:
yn+1= yn+ hf (tn, yn) (1.2)
tn+1= tn+ h (1.3)
1.2.2
Runge Kutta 4 (RK4)
The RK4 method[7] is a more sophisticated method that computes four approximations of the differential equations and then computes the approximation for the next step based on a weighted average of the approximations. The RK4 method is a forth order method which means that
this method has a truncation error per step of O(h5) and an accumulated error of O(h4). The
formulation of the method is presented from the equation (1.4) to equation (1.9):
yn+1= yn+ h 6(k1+ 2k2+ 2k3+ k4) (1.4) k1= f (tn, yn) (1.5) k2= f (tn+ h 2, yn+ h 2k1) (1.6) k3= f (tn+ h 2, yn+ h 2k2) (1.7) k4= f (tn+ h, yn+ hk3) (1.8) tn+1= tn+ h (1.9)
1.2.3
Adams-Bashforth-Moulton (ABM)
The Adams-Bashforth-Moulton method (ABM)[8] is a multistep method that uses the informa-tion from previous steps in order to predict the next steps and it is a combinainforma-tion of two methods, the Adams-Bashforth method[8] and the Adams-Moulton method[8].
The Adams-Bashforth method (AB4) is an explicit method of forth order which produces a
truncation error per step of O(h5) and a global accumulated error of O(h4) and has the following
formulation presented in the equations (1.10) and (1.11).
yn+1= yn+
h
24(55f (tn, yn) − 59f (tn−1, yn−1) + 37f (tn−2, yn−2) − 9f (tn−3, yn−3)) (1.10)
tn+1= tn+ h (1.11)
The Adams-Moulton (AM3) method is an implicit method of forth order which produces a
(1.13) show the formulation of the method.
yn+1= yn+
h
24(9f (tn+1, yn+1) − 19f (tn, yn) − 5f (tn−1, yn−1) + f (tn−2, yn−2)) (1.12)
tn+1= tn+ h (1.13)
The ABM method uses the explicit AB4 method as the predictor (1.14) to get the first approxi-mation and then uses the implicit AM3 method as the corrector (1.15) of the first approxiapproxi-mation to produce the final approximation. The ABM method has the same order of the other two and the respective errors.
˜ yn+1= yn+ h 24(55f (tn, yn) − 59f (tn−1, yn−1) + 37f (tn−2, yn−2) − 9f (tn−3, yn−3)) (1.14) yn+1= yn+ h 24(9f (tn+1, ˜yn+1) − 19f (tn, yn) − 5f (tn−1, yn−1) + f (tn−2, yn−2)) (1.15) tn+1= tn+ h (1.16)
There are some variants of this method that add a variable time step that is controlled based on the error between the predictor and the corrector approximations.
1.2.4
Dormand Prince (DOPRI)
The Dormand Prince method[9] is an explicit method of the Runge Kutta familly. It uses seven evaluations to compute the approximation. This method also has an inner variable time step that is managed to use the widest time steps that will produce approximations with an error inferior to a tolerance set by the user, repeating this process until it fulfils the temporal window with the inner time steps and gets the final approximation. In the document A family of embedded Runge-Kutta formulae[9], J.R.Dormand and P.J.Prince present three variations of the method, the RK5(4)7M method presented from equation (1.17) to equation (1.27), the RK5(4)6M method presented from equation (1.28) to equation (1.37) and the RK5(4)7S method form presented from equation (1.38) to equation (1.48), with small differences in the stability region, convergence speed and accuracy between them.
• RK5(4)7M k1= hf (tn, yn) (1.17) k2= hf (tn+ 1 5h, yn+ 1 5k1)) (1.18) k3= hf (tn+ 3 10h, yn+ 3 40k1+ 9 40k2) (1.19) k4= hf (tn+ 4 5h, yn+ 44 45k1− 56 15k2+ 32 9 k3) (1.20) k5= hf (tn+ 8 9h, yn+ 19372 6561k1− 25360 2187k2+ 64448 6561 k3− 212 729k4) (1.21) k6= hf (tn+ h, yn+ 9017 3168k1− 355 33k2+ 46732 5247k3+ 49 176k4− 5103 18656k5) (1.22) k7= hf (tn+ h, yn+ 35 384k1+ 500 1113k3+ 125 192k4− 2187 6784k5+ 11 84k6) (1.23) ˆ yn+1= yn+ 35 384k1+ 500 1113k3+ 125 192k4− 2187 6784k5+ 11 84k6 (1.24) yn+1= yn+ 5179 57600k1+ 7571 16695k3+ 393 640k4− 92097 339200k5+ 187 2100k6+ 1 40k7 (1.25) en+1= − 71 57600k1+ 71 16695k3− 71 1920k4+ 17253 339200k5− 22 525k6+ 1 40k7 (1.26) tn+1= tn+ h (1.27) • RK5(4)6M k1= hf (tn, yn) (1.28) k2= hf (tn+ 1 5h, yn+ 1 5k1)) (1.29) k3= hf (tn+ 3 10h, yn+ 3 40k1+ 9 40k2) (1.30) k4= hf (tn+ 3 5h, yn+ 3 10k1− 9 10k2+ 6 5k3) (1.31) k5= hf (tn+ 2 3h, yn+ 226 729k1− 25 27k2+ 880 729k3+ 55 729k4) (1.32) k6= hf (tn+ h, yn− 181 270k1+ 5 2k2− 266 297k3− 91 27k4− 189 55 k5) (1.33) ˆ yn+1= yn+ 19 216k1+ 1000 2079k3− 125 216k4+ 81 88k5+ 5 56k6 (1.34) yn+1= yn+ 31 540k1+ 190 297k3− 145 108k4+ 351 220k5+ 1 20k6 (1.35) en+1= yn+1− ˆyn+1= − 33 1080k1+ 330 2079k3− 165 216k4+ 297 440k5− 11 280k6 (1.36) tn+1= tn+ h (1.37)
• RK5(4)7S k1= hf (tn, yn) (1.38) k2= hf (tn+ 2 9h, yn+ 2 9k1)) (1.39) k3= hf (tn+ 1 3h, yn+ 1 12k1+ 1 4k2) (1.40) k4= hf (tn+ 5 9h, yn+ 55 324k1− 25 108k2+ 50 81k3) (1.41) k5= hf (tn+ 2 3h, yn+ 83 330k1− 13 22k2+ 61 66k3+ 9 110k4) (1.42) k6= hf (tn+ h, yn− 19 28k1+ 9 4k2+ 1 7k3− 27 7 k4+ 22 7 k5) (1.43) k7= hf (tn+ h, yn+ 19 200k1+ 3 5k3− 243 400k4+ 33 40k5+ 7 80k6) (1.44) ˆ yn+1= yn+ 19 200k1+ 3 5k3− 243 400k4+ 33 40k5+ 7 80k6 (1.45) yn+1= yn+ 431 5000k1+ 333 500k3− 7857 10000k4+ 957 1000k5+ 193 2000k6− 1 50k7 (1.46) en+1= yn+1− ˆyn+1= − 11 1250k1+ 33 500k3− 891 5000k4+ 33 250k5+ 9 1000k6− 1 50k7 (1.47) tn+1= tn+ h (1.48)
The methods use ˆyn+1as the approximation because ˆyn+1is of order 5 and the yn+1
approxi-mation is of order 4, therefore the error between the two approxiapproxi-mations is given by the following equation:
en+1= yn+1− ˆyn+1 (1.49)
The only thing left is to know how to control the inner time step of the method and equation (1.50) is one suggested by the authors.
hn+1= 0.9hn5
s δ ||en+1||∞
(1.50) where δ is the tolerance specified by the user.
1.3
Structure of the Thesis
This document is organized in six chapters. The first chapter contains a brief introduction to the concept of quadrotor, its history and importance in the present time as well as an introduction to the concept of iterative method and presentation of some methods.
In the second chapter it is presented the physics-based model of the birotor and the results of some experiments using of the model with iterative methods from chapter one.
In the third chapter is is described the physics-based model of the quadrotor, of the DC motor and propeller and there are some results to see the behaviour of the respective models.
The forth chapter describes the controllers and the results of the controllers when faced with the physics-based models described in the third chapter.
The description of the implementation in done in chapter five as well as the important changes that were done in order to surpass some of the problems that appeared during this process.
At last, the chapter six has a brief summary of the thesis with the conclusions of this work and some suggestion for future work.
Chapter 2
Birotor
The approach considered in this work to simulate a quadrotor is to first describe its dynamics through a dynamic model in the form of a set of non-linear first order state equations, as in (1.1), which will be solved in real-time, for the given excitations, through a numerical solver algorithm based on an explicit time-step integration method. Therefore, the first step is to obtain the nonlinear dynamic model that describes the motion of the quadrotor, with respect to its properties and to the voltages applied , at every time instant, to its DC motors. However, since modeling the kinematics of the three-dimensional quadrotor is quite complicated, involving a strong modeling effort, it was decided to start this work by analyzing a simpler system, reduced to the two-dimensional space. The objective was to test, at an early stage, if the proposed modeling and simulation approach was feasible, and also to gain confidence for the more complex case of the 3D quadrotor. In this sense, the system that was initially considered was a birotor, which consists of only two dc motors mounted on the tips of a rigid bar, whose motion is restricted to the 2D space. The analyzed birotor system was assumed symmetric and balanced, with its center of mass located in the geometric center of the rigid bar (the mass of the bar is neglected). A further advantage of analyzing a simpler system is that it is easier to set testing conditions where the expected behavior is a priori known, therefore making it simpler to analyze if the solver algorithms and the system models are, or not, providing a realistic simulation of the system. In this chapter it is described the modelling strategy that was considered for this simpler test-case, and the respective simulation approach and results are presented. As will be shown, this implementation proved to be very successful, opening the way to model and simulate the full 3D quadrotor, as is described in the following chapter.
2.1
Dynamic Model
The first approach was to interpret the motors as two bodies of mass m1 and m2 where each
one produces the forces ~F1 and ~F2 respectively as illustrated in figure 2.1. The bodies are also
connected to a weightless bar with length 2r which will be, in this first characterization attempt, modelled by the parallel of a damper with high damping coefficient(b) and a spring with a very high elasticity constant(k). The respective system is represented in figure 2.1 and figure 2.2 which
represents how the bar acts on the bodies, in this particular case in the body with mass m1.
To understand how the forces interact with the system, the system must be careful analysed. From Newton’s second law of motion it is known that the acceleration of an object is directly proportional to the sum of all the forces acting on the system, and inversely proportional to the total mass of the object. It is also known that the forces that act on the system are the forces
produced by the motors ~F1 and ~F2, the force produced by the air friction ~Ff, the force of the bar
model ~Fbar and the force of gravity ~Fg. From here, the formulation of the law can be expressed
Figure 2.1: First approach to the Birotor’s model.
Figure 2.2: Representation of the force produced by the bar.
X ~F (t) = ~F1(t) + ~F2(t) + ~Ff(t) + ~Fbar(t) + ~Fg= M~a (2.1)
where ~a is the linear acceleration vector and M the total mass of the birotor. Also, it is possible to split equation (2.1) and distribute the forces acting in the overall system into a more specific set of equations (2.2) and (2.3) where the forces act on the bodies of the birotor.
~
F1(t) + ~Ff 1(t) + ~Fbar1(t) + ~Fg2= m1~a1(t) (2.2)
~
F2(t) + ~Ff 2(t) + ~Fbar2(t) + ~Fg1= m2~a2(t) (2.3)
where ~a1 and ~a2 are the linear accelerations of the bodies respectively and ~Fbar1, ~Fbar2, ~Fg1 and
~
Fg2the forces produced by the bar model and the force of gravity acting on the respective bodies.
The friction force is directly proportional to the magnitude of the linear speed of birotor, however it has the opposite direction of the linear speed vector and its formulation is presented in equation (2.4). Ff(t) = " Ff 1(t) Ff 2(t) # = D " ~ v1(t) ~ v2(t) # = D ˙ y1(t) ˙ z1(t) ˙ y2(t) ˙ z2(t) (2.4)
where D is the dynamic friction coefficient, ˙y1, ˙z1, ˙y2and ˙z2 the components of the linear velocity
vectors of the bodies respectively.
The force produced by the bar is easy calculated with the Hooke’s law for the spring and the physical equation of the damper, and is described by equation (2.5).
~
Fbar(t) = ~Fspring(t) + ~Fdamper(t) = k
2r − q y1(t) − y2(t) 2 + z1(t) − z2(t) 2 − (2.5)
−by˙2(t) cos α(t) + ˙z2(t) sin α(t) − ˙y1(t) cos α(t) − ˙z1(t) sin α(t)
where Fspringis the force produced by the spring, Fdamperis the force produced by the damper and
α is the orientation of the birotor as defined in figure 2.1. It is possible to obtain the orientation of the birotor(α) through the relation of the current positions of the bodies as expressed in equation (2.6).
α(t) = arctanz2(t) − z1(t) y2(t) − y1(t)
(2.6)
The vectors that represent the forces can be divided into their y and z components as a function of α, the orientation of the birotor, as illustrated in figure 2.3:
Figure 2.3: Representation of the decomposition of a vector.
where Fy is the y component of F and is expressed as Fy= F cos(α(t)) and Fzis the z component
of F and is expressed as Fy= F sin(α(t)).
If we split the components of the vectors in equations (2.2) and (2.3) and gather all the equations of the forces mentioned above we get the set of ordinary differential equations from (2.7) to (2.13) that describe the physical behaviour of the birotor.
m1y¨1(t) = −F1(t) sin α(t) − Fbar(t) cos α(t) − Ff y1(t) (2.7)
m1z¨1(t) = F1(t) cos α(t) − Fbar(t) sin α(t) − Ff z1(t) − gm1 (2.8)
m2y¨2(t) = −F2(t) sin α(t) + Fbar(t) cos α(t) − Ff y2(t) (2.9)
m2z¨2(t) = F2(t) cos α(t) + Fbar(t) sin α(t) − Ff z2(t) − gm2 (2.10)
Fbar(t) = Fspring(t) + Fdamper(t) = k
2r − q y1(t) − y2(t) 2 + z1(t) − z2(t) 2 − (2.11)
−by˙2(t) cos α(t) + ˙z2(t) sin α(t) − ˙y1(t) cos α(t) − ˙z1(t) sin α(t)
α(t) = arctanz2(t) − z1(t) y2(t) − y1(t) (2.12) Ff(t) = Ff y1(t) Ff z1(t) Ff y2(t) Ff z2(t) = D ˙ y1(t) ˙ z1(t) ˙ y2(t) ˙ z2(t) (2.13)
where g is the acceleration of gravity.
The simulation will use a non linear state space representation of the model, since such rep-resentation matches the formulation of the IVP (1.1) of the iterative methods. To do so, one must define the respective state vector with all the state variables and then rearrange the previous set of equations to suit the state space representation. The state variables are going to be the position and the linear velocity components of each body as represented in equation (2.14) with
the state vector ~SS and equations (2.15) and (2.16) show the formulation of the non linear state
space representation. ~ SS(t) = SS1(t) SS2(t) SS3(t) SS4(t) SS5(t) SS6(t) SS7(t) SS8(t) = y1(t) ˙ y1(t) z1(t) ˙ z1(t) y2(t) ˙ y2(t) z2(t) ˙ z2(t) (2.14) ˙ ~ SS(t) = G t, ~SS(t), u(t) (2.15) yout(t) = H t, ~SS(t), u(t) (2.16)
whereSS is the vector with the derivatives of the state space vector, u(t) the inputs of the system~˙
and yout(t) the output vector of the system. As we want the output vector to be equal to the state
vector yout(t) = ~SS(t) in order to allow the simulator user to check all the variables to assess the
performance of the model as well as the performance of the controller(s) (that will be discussed
later), and as there is no direct interaction of the input signal in the output, H t, ~SS(t), u(t) can
be expressed as H t, ~SS(t), u(t) = ~SS(t).
Rearranging the set of equations from (2.7) to (2.13) to suit the non linear state space repre-sentation, the following set of equations from (2.17) to (2.19) is obtained:
˙ SS1(t) ˙ SS2(t) ˙ SS3(t) ˙ SS4(t) ˙ SS5(t) ˙ SS6(t) ˙ SS7(t) ˙ SS8(t) = SS2(t)
−F1(t) sin α(t) − Fbar(t) cos α(t) − DSS2(t)
m1
SS4(t)
F1(t) cos α(t) − Fbar(t) sin α(t) − DSS4(t)
m1
− g SS6(t)
−F2(t) sin α(t) + Fbar(t) cos α(t) − DSS6(t)
m2
SS8(t)
F2(t) cos α(t) + Fbar(t) sin α(t) − DSS8(t)
m2 − g (2.17) Fbar(t) = k 2r − q SS1(t) − SS5(t) 2 + SS3(t) − SS7(t) 2 − (2.18)
−bSS6(t) cos α(t) + SS8(t) sin α(t) − SS2(t) cos α(t) − SS4(t) sin α(t)
α(t) = arctanSS7(t) − SS3(t)
SS5(t) − SS1(t)
(2.19)
where the term on left side of equation (2.17) is a vector that contains the derivatives of the state variables.
When this model is simulated it can be seen that it has a crucial flaw. It is not easy to tune the damping coefficient(b) and the elasticity constant of the spring(k) in order to get a good approximation of a rigid bar with the desired time step. The model may behave well with similar forces and low angular velocities, but the same does not happen with high angular velocities. The angular velocities can lead to unstable states with a continuous distension of the bar, or even with an oscillating bar. The figure 2.4 shows an example of this problem that occurred in one simulation of the current birotor model with the RK4 method. The experiment was based on an constant angular acceleration, resulting in an increasing angular velocity as then see how the model would behave in this situation. It is possible to detect in the figure 2.4 that the trails of the bodies do not have the shape of a circle but instead they have the shape of a spiral because the length of the bar is increasing. The figure 2.5 shows the plot of the angular velocity, length of the bar and the force produced by the bar throughout the simulation to help the visualization of the behaviour. The distension of the bar, in the simulation presented, can be attenuated by increasing the elasticity constant of the spring(k), but it will eventually lead to an oscillating bar due to very high elasticity constant of the spring that causes the bar to produce high forces with small distensions of the bar. The table 2.1 shows the initial conditions and the parameters used in the simulation of this behaviour.
−0.5 −0.4 −0.3 −0.2 −0.1 0 0.1 0.2 0.3 0.4 0.5 −0.5 −0.4 −0.3 −0.2 −0.1 0 0.1 0.2 0.3 0.4 0.5 y (m) z (m)
Figure 2.4: Motion of the birotor.
0 1 2 3 4 5 6 7 −20 −10 0 Angular velocity time (s) (rad/s) 0 1 2 3 4 5 6 7 0.5 0.55 0.6
Length of the bar
time (s) (m) 0 1 2 3 4 5 6 7 −100 −50 0
Force produced by the bar
time (s)
(N)
Figure 2.5: Plot with the angular velocity, length of
the bar and Fbar of the birotor.
Variables Value
time step(h) 0.05 s
friction(D) 0.05 N s m−1
acceleration of gravity(g) 0 m s−2
arm length(r) 0.25 m
elasticity constant of the spring(k) 1000 N m−1
dynamic friction constant of a damper(b) 5 N s m−1
center of mass(y,z) (0,0) m
initial orientation(α) 0 rad
force applied in body1(F1) 0.2 N
force applied in body2(F2) -0.2 N
mass of body1(m1) 1 kg
mass of body2(m2) 1 kg
initial linear velocity( ˙y1, ˙z1, ˙y2, ˙z2) (0,0,0,0) m s−1
initial angular velocity( ˙α) 0 rad s−1
Table 2.1: Initial conditions and values of the parameters of the simulation with the first birotor model approach.
Having realized that the initial modelling approach for the bar behaviour was not the most adequate, a second strategy was analysed. It consisted in analysing the system according to the physics laws of rigid bodies[10].
In this case, the model is considered to have a linear motion component and an angular motion component. The linear component considers all the mass of the birotor condensed at its center of mass, being all the forces acting in the birotor considered as applied at the center of mass. In other words, the linear motion assumes the birotor reduced to a point mass located at its center of mass. The angular component is associated with the sum of the moments of inertia of each mass around the birotor’s rotation axis (a virtual axis orthogonal to the 2D plane of the birotor movement, crossing its center of mass), and the sum of all the torques.
As before, the bar is considered as weightless and the birotor is considered symmetric and balanced. Using the Newton’s laws for linear and angular motion, it is possible to analyse the rigid body system approach illustrated in figure 2.6 and obtain the following set of equations:
Figure 2.6: Rigid body approach to the birotor’s model. X F (t) = ~F1(t) + ~F2(t) + ~Ff+ ~Fg = M~a (2.20) X ~ τ (t) = ~τ1(t) + ~τ2(t) + ~τf(t) + ~τg(t) = Icmα¨ (2.21) M = m1+ m2 (2.22)
where τ is the total torque acting on the system, Icm is the moment of inertia of the system, M
the total weight of the system and the other torques the respective torques produced by the forces
acting on the system, in other words, τf is the torque produced by the air friction force.
Knowing that the torque produced by a force applied to an arm is expressed as (2.23), where r is the length of the arm and × represents the cross-product, the equation (2.21) can be rewritten into the equation (2.24).
~
τ = ~r × ~F (2.23)
r2M ¨α = rF1(t) − rF2(t) − 2Dr2α(t) + gr(m˙ 1− m2) (2.24)
where ˙α is the angular velocity.
Since the birotor is balanced, the gravity component from equation (2.24) can be neglected. Using the approach illustrated in figure 2.3 to split the y and z components of the equation (2.20), it is possible to obtain the following set of equations from equation (2.25) to equation (2.28).
M ¨y = −F1(t) sin α(t) − F2(t) sin α(t) − D ˙y (2.25)
M ¨z = F1(t) cos α(t) + F2(t) cos α(t) − D ˙z − gM (2.26)
r2M ¨α = rF1(t) − rF2(t) − 2Dr2α(t)˙ (2.27)
M = m1+ m2 (2.28)
Once again, the model must be rearranged to suit the non linear state space representation that will be used by the simulator. The state vector used in this model contains as state variables, the components of the linear and angular position of the birotor as well as the linear and angular velocity, and is presented in equation (2.29).
~ SS(t) = SS1(t) SS2(t) SS3(t) SS4(t) SS5(t) SS6(t) = y(t) ˙ y(t) z(t) ˙ z(t) α(t) ˙ α(t) (2.29) (2.30) Rearranging the set of equations from (2.25) to (2.28) to suit the non linear state space repre-sentation, the following equations (2.31) and (2.32) is obtained:
˙ SS1(t) ˙ SS2(t) ˙ SS3(t) ˙ SS4(t) ˙ SS5(t) ˙ SS6(t) = SS2(t) −F1(t) sin SS5(t) −F2(t) sin SS5(t) −DSS2(t) M SS4(t) F1(t) cos SS5(t) +F2(t) cos SS5(t) −DSS4(t) M − g SS6(t) F1(t)−F2(t)−2DrSS6(t) rM (2.31) M = m1+ m2 (2.32)
where the term on left side of equation (2.31) is a vector that contains the derivatives of the state variables.
From this point, it is possible to begin the tests of the iterative methods to compare them and check how they perform, as it was one of the purposes of obtaining the birotor model.
2.2
Results
Since all the pieces of the simulator at this stage are gathered it is possible to test the iterative methods and see the results of the birotor’s model, but in order to do so it is needed to know a real solution for a specific set of initial values. As there was not available a real birotor system to provide the real motion trajectories to which the simulated trajectories should be compared, in order to assess the performance of the simulated model (and of the simulation algorithms), it was considered as the pseudo-real trajectory the results provided from the execution of a RK4 iterative method with a very small time step (of 0.2 milliseconds), which produced a simulation
accumulated error around 10−15. Moreover, since the birotor system is simple to analyse, and its
motion is fairly intuitive, specific tests were conceived for which the expected behaviour of the birotor was studied and previewed from the start.
In Experiment 1, all the initial conditions (position of the center of mass and orientation of the birotor), the environment parameters (acceleration of gravity and air friction coefficient), the physical characteristics of the birotor, the forces produced by the motors and the time step and
tolerance of the methods were set with the values indicated in table 2.2. The forces F1 and
F2 as well as the physical and environment parameters are considered constant throughout the
experiment. The same system model was used in MatLab ’s SIMULINK with the ode45 solverR
and compared with the results of the implemented methods to further validate the implemented methods.
Variables Value time step(h) 0.01 s friction(D) 0.05 N s m−1 acceleration of gravity(g) 0 m s−2 arm length(r) 0.25 m center of mass(y,z) (3,2) m
initial orientation(α) 5 rad
force applied in body1(F1) 1 N
force applied in body2(F2) 2 N
mass of body1(m1) 1 kg
mass of body2(m2) 1 kg
initial linear velocity( ˙y, ˙z) (1,-1) m s−1
initial angular velocity( ˙α) 2 rad s−1
simulation time 60 s
tolerance 0.001
Table 2.2: Experiment 1: Initial conditions and values of the parameters.
The set of figures 2.7 shows the motion of the birotor in Experiment 1 where figure 2.7a shows the overall motion of the birotor, figure 2.7b shows the initial part of that motion and figure 2.7c the final part of the motion. It was also added the figure 2.7d, where we can see the linear and angular velocity of the birotor, to get a better perception of the evolution of the motion throughout the experiment.
The set of figures 2.8 shows the resultant errors of the tested iterative simulation methods. The table 2.3 shows the results of the simulations where it can be seen the final accumulated error of the methods, the respective computation time and the number of computed iterations used by each method.
norm of the errors Computation time (s) Number of steps
Euler 398.20e-3 658.06e-3 6000
RK4 3.31e-6 867.10e-3 6000
ABM 236.43e-6 837.20e-3 6000
RK5(4)7M 14.09e-9 1.25 6000
RK5(4)6M 5.72e-9 1.17 6000
RK5(4)7S 15.39e-9 1.23 6000
Simulink’s ode45 14.09e-9 33.85 6000
Table 2.3: Results of the evaluation of the implemented solver methods, for Experiment 1. Since the tolerance specified for the DOPRI methods gives an internal time step greater than the time step specified in the table, the internal time step will be truncated to the value of the specified time step. The truncation of the internal time of the DOPRI methods will cause them to have the same number of total iterations as the fixed time step methods. With this experiment we can also compare the error of the methods versus the respective computation time. We should not directly compare the long time taken by the SIMULINK simulation against the execution time of the implemented methods, as SIMULINK accounts for all the complex software architecture that make it a very flexible simulation platform. However, we should directly compare the simulation time of the several implemented methods. From the information displayed in the table 2.3 and from the set of figures 2.8, the methods with the better performance are the RK4 and the variants of the DOPRI method and it can be checked that the solution presented by the RK5(4)7M is very identical to the simulink’s ode45.
0 5 10 15 20 25 30 35 −10 −8 −6 −4 −2 0 2 y(m) z(m)
(a) Overall motion of the birotor.
2 3 4 5 6 7 8 9 −1 −0.5 0 0.5 1 1.5 2 y(m) z(m)
(b) Initial motion of the birotor.
27 28 29 30 31 32 33 34 35 −10 −9.5 −9 −8.5 −8 −7.5 −7 y(m) z(m)
(c) Final motion of the birotor.
0 10 20 30 40 50 60 0 0.5 1 1.5 2 Linear Velocity time (s) (m/s) 0 10 20 30 40 50 60 0 20 40 60 80 100 Angular Velocity time (s) (rad/s)
(d) Linear and angular velocity of the birotor.
0 10 20 30 40 50 60 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4
Norm of the error
time (s) error Euler RK4 ABM RK5(4)7M RK5(4)6M RK5(4)7S ode45
(a) Overall view.
0 10 20 30 40 50 60 0
1 2
x 10−4 Norm of the error
time (s) error RK4 ABM RK5(4)7M RK5(4)6M RK5(4)7S ode45
(b) Intermediate zoomed view.
0 10 20 30 40 50 60 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6x 10
−8 Norm of the error
time (s) error RK5(4)7M RK5(4)6M RK5(4)7S ode45
(c) Further zoomed view.
Figure 2.8: Error between each of the implemented solver algorithms and the SIMULINK solution in Experiment 1.
In order to get more information about the performance of the methods, this time the time variant methods will be fed with a time step equal to the total simulation time to give them total freedom to use and adjust the internal time step with the time step control condition in order to obtain a result that respects the specified tolerance.
0 10 20 30 40 50 60 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4
Norm of the error
time (s) error Euler RK4 ABM RK5(4)7M RK5(4)6M RK5(4)7S ode45
(a) Overall view.
0 10 20 30 40 50 60 0 0.001 0.002 0.003 0.004 0.005 0.006 0.007 0.008 0.009 0.01
Norm of the error
time (s) error RK4 ABM RK5(4)7M RK5(4)6M RK5(4)7S
(b) Intermediate zoomed view.
0 10 20 30 40 50 60 0
1 2
x 10−4 Norm of the error
time (s)
error
RK4 ABM
(c) Further zoomed view.
Figure 2.9: Error between each of the implemented solver algorithms and the SIMULINK solution in Experiment 2
Once again the set of figures 2.9 shows a consecutive zoomed view of the resultant errors of the iterative methods and the table 2.4 shows the results of this experiment.
norm of the errors Computation time (s) Number of steps
Euler 398.20e-3 658.06e-3 6000
RK4 3.31e-6 867.10e-3 6000
ABM 236.42e-6 837.20e-3 6000
RK5(4)7M 20.72e-3 101.05e-3 608
RK5(4)6M 20.75e-3 151.57e-3 1072
RK5(4)7S 20.96e-3 133.71e-3 833
Simulink’s ode45 82.73e-3 3.68 670
Table 2.4: Results of the evaluation of the implemented solver methods, for Experiment 2. The results from this experiment show that even with a tolerance not very high, the DOPRI methods have a very reasonable error for the respective computational time that they require.
Since the implementation of the simulator needs to simulate the behaviour of the system for a given time instant set by the controller’s sample frequency, the experiment 1 is the experiment that is more close to this situation that is more likely to happen in practice. Due to this fact, the results from experiment 1 are the ones that should be taken more in consideration when choosing the solvers for the implementation. The RK4 and DOPRI methods were the methods that had the better performance when comparing the accumulated error and the computation time of all the methods.
Chapter 3
Quadrotor
The next step is to get and implement the quadrotor’s dynamic model but now the system becomes much more complex with the additional dimension of the system and the respective degrees of freedom that come with the third dimension. The degrees of freedom of this system are x,y and z and their derivatives to specify a position and the linear velocity vector of the quadrotor’s center of mass, and φ, θ and ψ and their derivatives to specify the orientation of the quadrotor and its angular velocity. This angular entities are defined around the x,y and z axis respectively as can be seen in figure 3.1 and can be interpreted as roll(φ), pitch(θ) and yaw(ψ).
Figure 3.1: Quadrotor’s degrees of freedom.
In the figure 3.1 the motor 1 has a different color because it will be considered as the front of the quadrotor to determine the direction that the quadrotor is facing. In this particular imple-mentation, as the main interest is the behaviour of the quadrotor during flight, the model will not accommodate the landing and take off of the quadrotor and will be interpreted as working around its hover state and will also incorporate the model of the motors and the propellers and use it to obtain the input forces of the quadrotor’s system. The quadrotor model will consider that there
is a spherical central body with mass m and radius rsand include its dynamics in the quadrotor’s
ordinary differential equations. Once again the frame will be considered as weightless but the user can pass through this consideration by distributing the frame weight by the masses in the system in order to get a more realistic approach. The quadrotor’s sensors are assumed that are all placed in the center of mass so that the readings correspond to the actual values of the quadrotor’s linear and angular motion.
The motors and propellers have two possible configurations that they can be set in order to keep the forces balanced when the quadrotor moves, and the configurations are represented in figure 3.2.
Figure 3.2: Motor’s and propeller’s configuration.
Let us consider that the quadrotor is set with the configuration on the left of figure 3.2, being shown in figure 3.3 the ways to control the basic movements of the quadrotor.
(a) (b)
(c) (d)
Figure 3.3: Basic movements of a quadrotor: a) side movement; b) forward movement; c) rotation; d) upwards movement.
Figure 3.3 shows the basic movements and the direction that the quadrotor will move:
• (a) - lowering F4and increasing F2by the same amount will cause the quadrotor to roll(φ+).
Then, if the difference in forces in undone, the quadrotor will maintain the roll angle and as
the forces are now tilted towards y−, it will cause the quadrotor to move in y− direction.
• (b) - lowering F1and increasing F3by the same amount will cause the quadrotor to pitch(θ+).
Then, if the difference in forces in undone, the quadrotor will maintain the pitch angle and
as the forces are now tilted towards x+, it will cause the quadrotor to move in x+ direction.
• (c) - lowering the pair F1,F3 and increasing the pair F2,F4 by the same amount will cause
the quadrotor to yaw(ψ+) due to Newton’s third law of motion. The action-reaction pair of
the total torque produced by the motors will be responsible for the yaw.
• (d) - increasing all the forces by the same amount will cause the quadrotor to move up in the direction of z+.
In order to move in a different way from the ones presented above, it is only needed to invert the variations caused around the hover state. It is possible to move in any direction performing combinations of the basic movements.
As the movement of the quadrotor viewed from his center of mass is different from a view from an object set in the environment, it will be needed a way to identify the perspectives and to do so it can be used the help of other referencials in order to express the movements, vectors and vertices through the different points of view.
Figure 3.4: Set of referentials to represent the quadrotor’s behaviour.
Figure 3.4 shows a representation of the points of view that will be used to describe the
quadrotor’s behaviour, where [x y z]B is the referential set by the quadrotor’s body frame i.e.
the quadrotor’s point of view, [x y z]I is the inertial referential and [x y z]L is the local reference
frame, whose axes are parallel to those of the inertial frame and its origin follows the quadrotor’s
center of mass. A point set in the local referential just needs to add the coordinates of the
orange vector ~o represented in figure 3.4 that describes the position of the local referential in the
inertial referential’s coordinate system in order to convert its coordinates from the local referential’s coordinate system to the inertial referential’s coordinate system. Relatively to the body frame referential, a point set in the body frame referential just needs to be rotated around the roll, pitch and yaw angles of the quadrotor to match the local referential coordinate system.
Since the flights of a quadrotor are usually around 15 minutes, the Coriolis effect and the centrifugal forces on the surface of the earth can be neglected as this duration along with the quadrotor’s speed will not produce significant changes. So the Inertial referential can be set as the quadrotor’s user point of view on earth instead of being placed in the fixed stars.
3.1
Dynamic Model
The quadrotor dynamic model considered in this work was referred to the referential of the body frame. This avoids the consideration of several vector rotations which would make the formulation more complex and difficult to interpret. To obtain the set of ODEs that describe the quadrotor’s behaviour, the model is based on the Newton’s laws of motion for rigid bodies and
the forces and torques acting on the system are: the forces produced by the propellers ( ~F1, ~F2,
~
propellers ( ~T1, ~T2, ~T3 and ~T4). These relations can be described in the following form: X ~F (t) = ~F1(t) + ~F2(t) + ~F3(t) + ~F4(t) + ~Fg(t) + ~Ff(t) = M~a(t) (3.1) X ~ τ (t) = ~τ1(t) + ~τ2(t) + ~τ3(t) + ~τ4(t) + ~τg(t) + ~τf(t) + ~T1(t) + ~T2(t) + ~T3(t)+ (3.2) + ~T4(t) = Icmϕ(t)~¨
where ~τ1, ~τ2, ~τ3 and ~τ4are the respective torque produced by the force ~F1, ~F2, ~F3and ~F4, ~a is the
linear acceleration vector and ~¨ϕ the angular acceleration vector. The position of the four motors
relative to the center of mass is represented in the set of equations from (3.3) to (3.6).
r1= r1x r1y r1z = rq 0 0 (3.3) r2= r2x r2y r2z = 0 rq 0 (3.4) r3= r3x r3y r3z = −rq 0 0 (3.5) r4= r4x r4y r4z = 0 −rq 0 (3.6)
where rix is the coordinate of motor i over the x axis of the body frame referential and rq the
length of the arm of the quadrotor frame.
Since the total moment of inertia[10] (Icm) is equal to the sum of the moment of inertia of
all the points of mass and the spherical central body, its formulation can be decomposed in two
moment of inertia matrices. The matrices are: Isphere representing the moment of inertia of the
spherical central body[11], and Imi representing the moment of inertia of the four point masses
with the respective formulations in equations (3.7) and (3.8). Then, the total moment of inertia matrix in easily obtained with equation (3.9).
Isphere= 2 5mr 2 s 0 0 0 25mr2 s 0 0 0 2 5mr 2 s (3.7)
where rsis the radius of the central body.
Imi= 4 X i=1 mi(riy2 + r2iz) − 4 X i=1 mirixriy − 4 X i=1 mirixriz − 4 X i=1 mirixriy 4 X i=1 mi(rix2 + r 2 iz) − 4 X i=1 riyriz − 4 X i=1 mirixriz − 4 X i=1 riyriz 4 X i=1 mi(rix2 + r 2 iy) (3.8)
where mi is the weight of point mass i.
Icm= Imi+ Isphere= (3.9) = (m4+ m2)r2q 0 0 0 (m1+ m3)r2q 0 0 0 (m1+ m2+ m3+ m4)r2q + 2 5mr 2 s 0 0 0 2 5mr 2 s 0 0 0 2 5mr 2 s = = (m4+ m2)rq2+25mr 2 s 0 0 0 (m1+ m3)r2q+ 2 5mr 2 s 0 0 0 (m1+ m2+ m3+ m4)r2q+25mr 2 s
If we analyse more carefully the equation (3.1), it is possible to see from figure 3.1 that the
forces F1, F2, F3 and F4 only act in the z component of the linear acceleration. Adding the
components of the air friction force and force of gravity the following set of equations (from (3.10) to (3.13)) is obtained:
M ¨x(t) = −D ˙x(t) + M gxB(t) (3.10)
M ¨y(t) = −D ˙y(t) + M gyB(t) (3.11)
M ¨z(t) = F1(t) + F2(t) + F3(t) + F4(t) − D ˙z(t) + M gzB(t) (3.12)
M = m1+ m2+ m3+ m4+ m (3.13)
where ˙x, ˙y and ˙z are the components of the linear velocity vector and gB is the acceleration of
gravity vector (gI = 0 0 −g
) represented in the body frame referential.
Using the figure 3.1 as a guide, the analysis of how each force interacts with the angular motion of the quadrotor can improve the equation (3.2) to the set of equations (3.14) to (3.16).
Icm11φ(t) = F¨ 2(t) − F4(t)rq− 2Dr2qφ(t)˙ (3.14)
Icm22θ(t) = F¨ 3(t) − F1(t)rq− 2Drq2θ(t)˙ (3.15)
Icm33ψ(t) = T¨ 2+ T4− T1− T3− 4Dr2qψ(t)˙ (3.16)
where ¨φ, ¨θ and ¨ψ are the components of the angular acceleration vector( ¨ϕ), ˙~ φ, ˙θ and ˙ψ are the
components of the linear velocity vector and Icm11, Icm22 and Icm33 the respective indexes of the
total moment of inertia matrix. In the previous set of equations, as the quadrotor is symmetric and balanced, the influence of the gravity was neglected because all of its components cancel each other.
Choosing the the linear and angular position components as well as the linear and angular velocity components as the state variables, the state vector can be expressed as equation (3.17). Using the state vector, it is possible to rearrange the equations from (3.10) to (3.16) to suit the state space representation and obtain the set of equations from (3.18) to (3.20).
~ SS(t) = SS1(t) SS2(t) SS3(t) SS4(t) SS5(t) SS6(t) SS7(t) SS8(t) SS9(t) SS10(t) SS11(t) SS12(t) = x(t) ˙ x(t) y(t) ˙ y(t) z(t) ˙ z(t) φ(t) ˙ φ(t) θ(t) ˙ θ(t) ψ(t) ˙ ψ(t) (3.17)
˙ SS1(t) ˙ SS2(t) ˙ SS3(t) ˙ SS4(t) ˙ SS5(t) ˙ SS6(t) ˙ SS7(t) ˙ SS8(t) ˙ SS9(t) ˙ SS10(t) ˙ SS11(t) ˙ SS12(t) = SS2(t) −DSS2(t) M + g B x(t) SS4(t) −DSS4(t) M + g B y(t) SS6(t) F1(t)+F2(t)+F3(t)+F4(t)−DSS6(t) M + g B z(t) SS8(t) F2(t)−F4(t) rq−2Drq2SS8(t) Icm11 SS10(t) F3(t)−F1(t) rq−2Dr2qSS10(t) Icm22 SS12 T2(t)+T4(t)−T1(t)−T3(t)−4Dr2qSS12(t) Icm33 (3.18) M = m1+ m2+ m3+ m4+ m (3.19) Icm= (m4+ m2)r2q+ 2 5mr 2 s 0 0 0 (m1+ m3)r2q+25mr 2 s 0 0 0 (m1+ m2+ m3+ m4)rq2+ 2 5mr 2 s (3.20) where the term on left side of equation (3.18) is a vector that contains the derivatives of the state variables.
As a matter of curiosity about the controllers’ behavior when they are faced with an unbalanced and non symmetric quadrotor, the model was improved to accommodate such characteristics. To do so, it was necessary so see how the components of a force produced at a generic point would act on the system. The figure 3.5 illustrates the generic point(P), with the representation of a
force produced by a motor at that same point( ~F ), the components of the air friction force( ~Ff)
and force of gravity ( ~Fg).
where ~P is the position vector relative to the center of mass, ~V is the linear velocity vector, ~Ff is
the air friction force and ~Fg is the force of gravity.
From the figure 3.5, it is possible to notice that as the forces acting on the system are the same. As the rigid body approach considers that the quadrotor is reduced to a point mass located at its center of mass and all the forces are applied to the center of mass, the equations that describe the linear motion of the quadrotor will not change with the consideration of the forces being applied at a generic point of the quadrotor. The same does not happen to the angular motion, and to analyse how the forces act on the system, the components of the forces in figure 3.5 will be divided into the planes xOy (yaw plane), xOz (pitch plane) and yOz (roll plane). The figure 3.6 shows the decomposition of the forces in the xOy place.
Figure 3.6: Representation in the xOy plane.
To be able to get the angular motion equations, one must get the tangential components of
each force as represented as the force ~Ft with the help of the angle 1. Let us start with the y
component of the force of gravity presented in equations (3.21) and (3.22).
cos(1) = ~ Ftgy ~ Fgy & cos(1) = ~ Px ~ Pxy ↔ ~Ftgy= ~Fgy ~ Px ~ Pxy (3.21) ~ τ = ~F × ~r = ~FtP~xy= kFtgykkPxyk = kFgyk kPxk kPxyk kPxyk = kFgykkPxk (3.22)
where Ftgy is the tangential component of Fgy to the arm Pxy. The result means that any force
parallel to the y axis, with a positive direction in the y axis and applied in a x+ position, will
cause a positive torque around the z axis (yaw). The same also happens if both, the direction and the position, are negatives.
Now, with the x component of the gravity force in the equations (3.23) and (3.24).
sin(1) = ~ Ftgx ~ Fgx & sin(1) = ~ Py ~ Pxy ↔ ~Ftgx = ~Fgx ~ Py ~ Pxy (3.23) ~ τ = ~F × ~r = ~FtP~xy= −kFtgxkkPxyk = −kFgx kPyk kPxyk kPxyk = −kFgxkkPyk (3.24)
where Ftgx is the tangential component of Fgx to the arm Pxy. The result means that any force
parallel to the x axis, with a positive direction in the x axis and applied in a y+ position, will
cause a negative torque around the z axis (yaw). The same also happens if both, the direction and the position, are negatives.
Figure 3.7 ilustrates the decomposition of the forces in the xOz place, and similar to the xOy
