of a Quadrotor
Juan Pablo Daza Echeverry
Submitted to the Department of Mechanical Engineering in partial fulfillment of the Requirements for the Degree of
Mechanical Engineer
Department of Mechanical Engineering Universidad de Los Andes
Adviser: Carlos Francisco Rodr´ıguez Herrera
In this work we study a quadcopter theoretically and experimentally. First we develop a mathematical model of a quadcoter, composed of a dynamical model and an aerodynamical model, both parts are deduced in detail. The dynamical model will be verified computationally.
With the previous results several transfer functions are developed for future use in the design of a Flight Controller, we start with the nonlinear state space representation, it is linearized in order to obtain the transfer functions.
Then we proceed to measure the properties of a true system, this includes the inertial properties and the most important of all, the behavior of the propellers of the quadcopter. This experiments were designed with care in order to be in accordance with the model.
After obtaining the properties of a real quadcopter, the mathematical model and transfer functions are employed to develop and tune a flight controller, that is imple-mented on a real system.
Ante todo quiero agradecer a mi asesor Carlos Francisco Rodriguez Herrera por
su apoyo en este proyecto, por su paciencia y ayuda, de no ser por el no habr´ıa podido hacer un proyecto de grado que me diera tanta felicidad.
Tambi´en quiero agradecer a mis padres, que me han entregado toda su ayuda y
amor con tan grande generosidad, como una copa que nunca se vac´ıa, a ellos dedico
este trabajo por que sin su apoyo dudo que estuviese en este lugar.
Finalmente quiero agradecer a Alexandra Pinto, mi novia, quien me acompa˜no
en cada etapa de este proyecto, quien vio mi frustraci´on con las cosas quemadas y los
choques, pero quien tambi´en comparti´o la emoci´on de poder hacer volar mi proyecto de grado.
Mil gracias a todos, espero hacer valer el tiempo que de su vida me han entregado.
Juan Pablo Daza Echeverry
And to all who seek a life guided by their will, a mind nourished by independence, free of all others,
Abstract . . . iii
Acknowledgements . . . iv
1 Introduction 3 2 Quadcopter Model 4 2.1 Introduction . . . 4
2.2 Dynamical Model . . . 5
2.2.1 Reference Frames and Variables . . . 5
2.2.2 Kinematical Variables . . . 9
2.2.3 Newton-Euler Equations . . . 10
2.2.4 Preliminary Simulation . . . 13
2.3 Aerodynamical Model . . . 21
2.3.1 Aerodynamical Force of the Propeller . . . 21
2.3.2 Aerodynamical Torque of the Propeller . . . 22
2.4 Transfer Functions . . . 24
2.4.1 Control Inputs and Outputs . . . 24
2.4.2 Linearizing the State Vector . . . 25
3 Prototype 30 3.1 Inertial Properties CAD . . . 31
3.2 Inertial Measurment Unit . . . 33
3.3 Propeller and Motor Characterization . . . 33
3.3.1 Experimental Setup. . . 33
3.4 Control . . . 39
3.5 Implementation on the Real System . . . 41
A.2 Arduino Code for reading the Inertial Measurement Unit . . . 46
B Additional Theoretical Aspects 51
B.0.1 Alternative Definition of Reference Frames and Variables . . . 51
2.1 Open loop model of a Quadcopter. . . 5
2.2 Frame A is the Earth, FrameB has all unit vectors in common with Frame A, however this frame moves with the center of mass of the Quadcopter. . . 6
2.3 Roll, Pitch and Yaw rotations. . . 7
2.4 Forces and torques that act on the system. . . 11
2.5 Steady height of the Quadcopter, stationary position. . . 16
2.6 Quadcopter being accelerated in the vertical direction, due to higher force of the propellers. . . 17
2.7 Stationary rotation about the vertical axis by inducing a total torque on the cuadcopter. . . 18
2.8 Front heading north, displacement to the east (negative y direction) due to a roll angle of 3 degrees. . . 19
2.9 Front heading northwest, displacement to the northwest due to a pitch angle of 2 degrees. . . 20
2.10 Simulink Model Solver. . . 20
2.11 Diagram of the propeller. . . 21
3.1 Selected quadcopter. . . 30
3.2 CAD of the Quadcopter. . . 31
3.3 CAD of the Propeller and case of the motor. . . 32
3.4 Support designed to hold the motor . . . 34
3.5 Left, experimental setup for measurements. Right, tachometer. . . . 35
3.6 Scheme for filtering data. . . 36
3.7 Behavior of force with respect to ω. . . 37
3.8 Moment as function of ω. . . 38
IMU. . . 41
3.11 Succesful Flight of the Quadcopter . . . 42
B.1 Frame A is the Earth, FrameB has all unit vectors in common with Frame A, however this frame moves with the center of mass of the Quadcopter. . . 52
B.2 Frame C is the result of a simple rotation of frame B, frame D is a simple rotation of C and E a simple rotation ofD. . . 53
Introduction
The following work pretends to shed light on the dynamical, electronic and practical
workings of a quadcopter. The main objective of this work is to develop a precise
mathematical model of a quadcopter, and to use this model to develop a controller
for a real system.
The specific goals are:
• Develop a mathematical model that approximates the behavior of a quadcopter.
• Develop a computer program to verify the model.
• Develop a controller for the stabilization and displacement of the quadcopter.
Quadcopter Model
2.1
Introduction
The purpose of this chapter is to develop a precise model of a quadrotor helicopter.
We will consider in detail the model for an electrical quadrotor, and as with the
model of any machine the main part is a dynamical model, however there are some
additional considerations that must be taken.
First of all, the dynamical model has as an input several forces and moments
that will result on an output displacement, in our case these forces are the result of
gravity, inertial forces and the most important, aerodynamical forces and moments
not considering drag. This fact takes us to the next component, the aerodynamical
model, which could be simple regarding that we only need to study the model for
one propeller.
As is evident the aerodynamical forces are the result of the propeller connected to the motor, as a consequence there is the need to model the behavior of the motor
with respect to the input current. The input current is an alternating current which
is originated in an element called Electronic Speed Controller or ESC, the last part
needed to be considered in our model.
The ESC is connected directly to the battery and to the flight controller, the
latter transmits a Pulse Width Modulation (PWM) signal to the ESC, as a result the
Figure 2.1: Open loop model of a Quadcopter.
necessary to drive the motor. The ESC’s input PWM signals are the variables that
we intend to control, through a potential difference or serial port command that the microprocessor will convert to PWM, because these allow us to control de forces and
moments applied on the system.
In summary we have control of four input signals that change the amplitude of
the same number alternating currents, which in turn moderate the speed of motors
that drive four propellers responsible of generating the forces and moments that allow
us to control the quadcopter. Figure2.1 resumes the different parts of the open loop
model that emulates the quadrotor.
2.2
Dynamical Model
2.2.1
Reference Frames and Variables
To define the different variables that describe the quadcopter it is necessary, in our
case, to choose one variable for each degree of freedom that the system has. A
quad-copter has ten degrees of freedom, three of them associated with its linear movement,
another three with its rotations and four additional degrees of freedom that are
related to the rotation of its propellers. The generalized coordinates employed to
Figure 2.2: Frame A is the Earth, Frame B has all unit vectors in common with Frame A, however this frame moves with the center of mass of the Quadcopter.
orderly manner.
First we define a frame of reference fixed to the earth, frame A, it contains three
orthogonal unit vectors a1,a2 and a3, see figure 2.2. The vector a1 points in the direction of the magnetic north of the earth,a2 points towards the west and a3 goes upwards to the heaven.
We proceed to define a frame fixed in the center of mass of the Quadcopter, frame
B, this frame has all unit vectors in common with frame A, that isa1 =b1,a2 =b2 and a3 =b3, see figure 2.2. This frame does not rotate.
Three variables are needed to describe the relation between frame A and frame
• q8: Displacement of the center of mass to the north, in the direction of a1 .
• q9: Displacement of the center of mass to the west, in the direction of a2.
• q10: Displacement of the center of mass upwards, in the direction of a3.
Next we describe the rotations of the quadcopter with respect to its center of
mass. For this we define one additional reference frame, the frame E with unit
vectors e1, e2 and e3. Frame E rotates with respect to frame B and this rotation is described by the Roll, Pitch and Yaw representation. It must be noted that the
center of mass of the quadcopter Q∗ is a common point between frame E and frame
B, that means . See figure 2.3.
In our case, the Roll is associated to a rotation with respect to the vectora1as axis of rotation, with the positive a1 direction as the direction of rotation following the right hand rule, we denote this angle with the q5 generalized coordinate. If we were
sitting on the quadcopter facing north, we would feel that it tilts towards the right.
Assuming that the motors are working and the quadcopter is hovering at a stable
height, if we were to set the roll angleq5 to a moderately small and positive angle the
quadcopter would tilt towards the east, and would move eastwards as the thrust force
would have a component in the horizontal direction. There are two coupled degrees of freedom, a rotation tilts the thrust force which in turn causes a linear displacement.
The Pitch angle somehow describes the inclination of the front of the vehicle, it is
described by variableq6 and has been defined in such a way that, the axis of rotation
is the vectora2 which points towards the west, and the rotation is in the positive a2 direction. As before if we set the pitch to a small positive angle, the quadcopter will
tilt and move towards the north, as in this case the rotation is coupled with a linear
displacement, this fact highlights that we are dealing with an underactuated system.
Last but not least, the Yaw angle describes the rotation of the quadcopter when it
is contained in a plane parallel to the ground, we assign variableq7 to the Yaw angle,
the axis of rotation is the vectora3 and the direction of rotation is in the positivea3 direction. If the quadcopter is hovering and we increase the Yaw angle at a constant
rate, it will keep rotating parallel to the ground and no linear displacement will be
caused, in contrast to our previous rotations the Yaw angle is independent of any
linear displacements.
The rotation matrix relating relating the vectors from frameAto the vectors from frame E is:
A
RE =
CψCθ CψSθSφ−SψCφ CψSθCφ+SψSφ
SψCθ SψSθSφ+CψCφ SψSθCφ−CψSφ
−Sθ CθSφ CθCφ
(2.1)
Where the usual notation φ, θ and ψ for Roll, Pitch and Yaw respectively has been used, which implies φ = q5, θ = q6 and ψ = q7. The explicit relation between
the ground unit vectors{ai} and the quadcopter unit vectors {ei} is:
ei =AREai (2.2)
{a1,a2,a3}= 1 0 0 , 0 1 0 , 0 0 1 (2.3)
It is worth noting that the selection of the variables that describe rotations, has
been done with respect to axes that are fixed to the ground, in some cases this is
not the best approach. An alternative definition of the rotational variables can be
found in appendix B, in that case with respect to moving axes. The model with the
moving axes had been simulated, however the model did not converged fast and in key cases it did not converged at all. The main reason being the highly coupled differential
equations that had to be solved, the discussion of that definition of reference frames is
included to show a correct process which led to an almost unsolvable set of equations,
however its discussion is a good example of an approach which might be useful in
another cases.
2.2.2
Kinematical Variables
Having defined the variables that describe the degrees of freedom of the cuadcopter,
we will proceed to define the kinematical variables that explain how the quadcopter
moves. This variables are of utmost importance for all the methods that will be used
to solve the dynamics of the system.
First of all we are going to define the position of the center of mass, and although
the center of mass lies on frame B it also lies on frame E, as the center of mass was
a common vertex in the rotations q5, q6 and q7. If point O is the origin of frame A
then:
rQ∗/O =q8a1 +q9a2+q10a3 (2.4)
Is defined as the position of the center of massQ∗, with respect to pointO, as the center of mass of the quadcopter is a common vertex of the rotations from frame B
to frame E its position is not altered by those rotations, for this reason the velocity measured from the ground can be defined as:
AvQ∗ =
Ad
dt r
Q∗/O
And the acceleration with respect to frame A is:
AaQ∗ = Ad2
dt2 r
Q∗/O
=
Ad
dt
AvE∗
= ¨q8a1+ ¨q9a2+ ¨q10a3 (2.6)
The next important kinematical variables we need to define are the angular
ve-locities between frames, that is AωB the angular velocity of frame B with respect to
frame A, BωE the angular velocity of frame E as seen from frameB.
AωB = 0 (2.7)
BωE = ˙q
5a1+ ˙q6a2+ ˙q7a3 (2.8)
Additionally to these variables we need to define the location of the propellers,
or at least the location of the axes around which the propellers rotate, as those axes
are assumed to be the line of action of the forces generated by the propellers. The
vectors defining these locations will be constant on frame E, so they will be defined
using the vectors on this frame. An important experimental fact that usually is the
case, is that the frame is perfectly symmetrical and in our case we are using an X
configuration of the quadcopter, which is a configuration in which the front of the
quadcopter has two motors, see figure2.2, with this information we define the vectors
locating the axes as:
rH1∗/O =rE∗/O+ √L
2[e1+e2] +he3 (2.9)
rH2∗/O =rE ∗/O
+ √L
2[−e1+e2] +he3 (2.10)
rH3∗/O =rE ∗/O
+ √L
2[e1−e2] +he3 (2.11)
rH4∗/O =rE∗/O+ √L
2[−e1−e2] +he3 (2.12)
2.2.3
Newton-Euler Equations
Now that we have defined the necessary kinematical quantities that describe the
movement of the quadcopter, we proceed to find the dynamics of the system. We
Figure 2.4: Forces and torques that act on the system.
the vehicle and they are the result of the thrust force of the motors. Additionally
to these forces there are four torques that act on the vehicle, which are the result of
the aerodynamical drag originated when the propeller is rotating and has the same
value as the torque generated by the motors. The diagram showing these forces can
be seen in figure 2.4.
A careful inspection of figure 2.4, shows that the torques applied on the
quad-copter are balanced M1 and M4 are counterclockwise, M2 and M3 are clockwise.
This prevents the quadcopter from rotating incontrollably an allows the control of the Yaw angle.
In order to have a reasonable dynamical model, the first simplifying assumption
is that the moment of inertia of the propellers behave as cylinders, having the same
moment of inertia about the axis of rotation of the motor. In order to consider the
complete tensor of inertia of the propeller, we need the exact location of the blades
and this information is not available to us. Additionally the propellers rotate at
about 100 Hz and the dynamical movement of importance will be of the order of
1Hz, plus the mass of the propellers in our case is very small. The simplification is well justified, the only additional consideration that we will have is that of ignoring
drag.
Having simplified the propellers, it must be noted that if we look at the quad-copter with cylindrical propellers, it will look as a solid geometry which does not
change configuration, the center of mass stays in place an the total tensor of inertia
does not change.
The only forces acting on the quadcopter are the weight and the thrust force of
the propellers, the equation of forces is:
(F1 +F2+F3+F4)e3−M ga3 =MAaQ ∗
(2.13)
For the equation of moments it must be highlighted that there exist a angular
momentum due to the rotation of the propellers, we split the equation in the applied momentsMa and the inertial responseMi, the applied moments with respect to the
origin are:
MaO= (M1−M2−M3+M4)e3−M grQ ∗/O
×a3+ 4
X
i=1
FirH
∗
i/O×e
3 (2.14)
And the inertial part is:
MiO=IQ/Q∗· AαE + AωE × IQ/Q∗· AωE+rQ∗/O×(MAaQ∗) (2.15) + AωE ×a3
(−q1 +q2+q3 −q4)Ipropelleraxial (2.16)
Where Iaxial
propeller is the rotation inertia of the propeller about the axis of the motor, it must be noted that the last term is very small because the angular speeds of
the propellers are very close, however we will no drop this term in the model. The
moments equation is:
The Dynamical part of the model is now ready, what is left to determine is the
inertial properties of the Quadcopter that will be employed, this will be done in the
next chapter. #####
2.2.4
Preliminary Simulation
With equations 2.14 and 2.17 we are now ready to solve the model numerically in
Matlab, for this we arrange a state vectorX:
X = [q8, q9, q10, q5, q6, q7,q˙8,q˙9,q˙10,q˙5,q˙6,q˙7]T (2.18)
= [x, y, z, φ, θ, ψ, Vx, Vy, Vz, ωφ, ωθ, ωψ]T (2.19)
Where {x, y, z} describe the position, {φ, θ, ψ} describe the angular location, {Vx, Vy, Vz} the velocity and {ωφ, ωθ, ωψ} the angular speed of the Quadcopter. The
derivative of the state vectorX˙ is:
˙
X = [ ˙q8,q˙9,q˙10,q˙5,q˙6,q˙7,q¨8,q¨9,q¨10,q¨5,q¨6,q¨7]T (2.20)
= [Vx, Vy, Vz,φ,˙ θ,˙ ψ, a˙ x, ay, az, αφ, αθ, αψ]T (2.21)
Where {ax, ay, az} describe acceleration and {αφ, αθ, αψ} describe de angular
acceleration. The vector X˙ is then used for integration in Matlab, which gives us the vector X at every time interval of the simulation, and thus the solution to our
model. The full simplified expression forX˙ is not practical to obtain for a numerical solution, in the in code 2.1 for solving X˙ is shown step by step how this vector of derivatives is created.
% # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #
% M A T H E M A T I C A L M O D E L OF A Q U A D C O P T E R
% # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #
f u n c t i o n [X _ d o t] = M a t h M o d e l Q u a d(t,X)
% S t a t e V e c t o r P o s i t i o n s
x=X(1) ;y=X(2) ;z=X(3) ;
% S t a t e V e c t o r S p e e d s
Vx=X(7) ;Vy=X(8) ;Vz=X(9) ;
W p h i=X( 1 0 ) ;W t h e t a=X( 1 1 ) ;W p s i=X( 1 2 ) ;
% % D E F I N I T I O N OF C O N S T A N T S AND P R O P E R T I E S
I=[ 0 . 0 0 0 7 8 4 2 6 7 9 0 2 , 0 , 0 ; . . .
0 , 0 . 0 0 0 8 0 0 9 7 4 1 1 1 , 0 ; . . .
0 , 0 , 0 . 0 0 1 4 3 0 3 1 0 4 6 7 ] ; % I n e r t i a T e n s o r in Kg * m ^2
Ip= 0 . 0 0 0 0 0 2 3 3 0 0 3 9 ;% A x i a l I n e r t i a of the P r o p e l l e r
Mt= 0 . 2 4 0 ; % T o t a l M a s s in Kg
g= 9 . 8 ; % G r a v i t y m / s ^2
L= . 2 5 / 2 ; % Arm of the Q u a d c o p t e r
% % R E F E R E N C E F R A M E S
% E a r t h R e f e r e n c e F r a m e
a1=[1 ,0 ,0] ’;a2=[0 ,1 ,0] ’;a3=[0 ,0 ,1] ’;
% R o t a t i o n M a t r i x f r o m E a r t h to the Q u a d c o p t e r
% - N e c e s a r y T e r m s
C p h i=cos(phi) ;S p h i=sin(phi) ;
C t h e=cos(t h e t a) ;S t h e=sin(t h e t a) ;
C p s i=cos(psi) ;S p s i=sin(psi) ;
% - R o t a t i o n M a t r i x
R=[C p s i*Cthe,C p s i*S t h e*Sphi-S p s i*Cphi,C p s i*S t h e*C p h i+S p s i*S p h i; . . .
S p s i*Cthe,S p s i*S t h e*S p h i+C p s i*Cphi,S p s i*S t h e*Cphi-C p s i*S p h i; . . .
-Sthe, C t h e*Sphi, C t h e*C p h i];
% Q u a d c o p t e r R e f e r e n c e F r a m e s
e1=R*a1; % P o i n t s t o w a r d s the n o r t h
e2=R*a2; % P o i n t s t o w a r d s the w e s t
e3=R*a3; % P o i n t s to the Sky
% % D E F I N I T I O N OF A N G U L A R SPEEDS , A P P L I E D F O R C E S AND M O M E N T S
w1= 3 0 0 ; % T h i s d e f i n i t i o n s are p r e l i m i n a r y and no d e f i n i t e
w2= 2 0 0 ; % as an a e r o d y n a m i c a l m o d e l of the p r o p e l l e r is n e e d e d
w3= 2 0 0 ; % H o w e v e r t h e s e a n g u l a r s p e e d are g o o d e s t i m a t e s
w4= 3 0 0 ;
Fw=(Mt*g) / ( 4 ) *dot(a3,e3) ; % F o r c e to h o l d h o v e r i n g in N
F2=Fw* 2 1 / 2 2 ; % i n c r e a s e in the yaw a n g l e .
F3=Fw* 2 1 / 2 2 ;
F4=Fw* 2 3 / 2 2 ;
Kfm= 0 . 1 8 ; % B e c a u s e the b e h a v i o r of the T o r q u e is the s a m e as the
% as the Force , c u a d r a t i c , j u s t one c o n s t a n t r e l a t e s
% F o r c e and T o r q u e
M1=F1*Kfm; % N m
M2=F2*Kfm;
M3=F3*Kfm;
M4=F4*Kfm;
% % D E F I N I T I O N OF K I N E M A T I C A L Q U A N T I T I E S
rQO=x*a1+y*a2+z*a3; % L o c a t i o n of the c e n t e r of m a s s
r H 1 O=rQO+(L/s q r t(2) ) *(e1+e2) ; % L o c a t i o n of the p r o p e l l e r
r H 2 O=rQO+(L/s q r t(2) ) *( -e1+e2) ;
r H 3 O=rQO+(L/s q r t(2) ) *(e1-e2) ;
r H 4 O=rQO+(L/s q r t(2) ) *( -e1-e2) ;
AwE=W p h i*a1+W t h e t a*a2+W p s i*a3; % A n g u l a r s p e e d f r o m f r a m e A to E
% % D Y N A M I C A L E Q U A T I O N S
AaQ=((F1+F2+F3+F4) *e3) /Mt-g*a3; % A c c e l e r a t i o n f r o m f o r c e e q u a t i o n
% M O M E N T E Q U A T I O N
% - A p p l i e d T o r q u e s S i d e
Ta=(M1-M2-M3+M4) *e3-Mt*g*c r o s s(rQO,a3) + . . .
F1*c r o s s(rH1O,e3) +F2*c r o s s(rH2O,e3) +F3*c r o s s(rH3O,e3) + . . .
F4*c r o s s(rH4O,e3) ;
% - D y n a m i c s s i d e w i t h o u t I * A a l p h a E so as to s i m p l i f y l a t e r
Td=c r o s s(AwE,I*AwE) +Mt*c r o s s(rQO,AaQ) + . . .
( -w1+w2+w3-w4) *Ip*c r o s s(AwE,e3) ;
% - A n g u l a r A c c e l e r a t i o n
A a p l h a E=I\(Ta-Td) ;
X _ d o t=[Vx;Vy;Vz;W p h i;W t h e t a;W p s i; . . .
AaQ(1) ;AaQ(2) ;AaQ(3) ; . . .
A a p l h a E(1) ;A a p l h a E(2) ;A a p l h a E(3) ];
end
Code 2.1: Matlab code for solving the Dynamical Model of the Quadcopter
In the code 2.1, the inertial properties are defined first, followed by the inertial
reference frames and matrix of rotation from one frame to another. Then applied
forces and Torques are defined to test the dynamical model (this will be replaced by
a Aerodynamical model of the propeller), later the kinematical variables are defined
in order to be used in the equations of motion. Finally the derivative of the state
vector X˙ is constructed and given as result.
It is important to note that the input to X˙ is time t and X, this means that in order to start the simulation we need a vector of initial conditions X0.
0 1 2 3 4 5 −2 −1 0 1 2 Tiempo [s] Posicion [m] x y z
0 1 2 3 4 5 −2 −1 0 1 2 Tiempo [s] Velocidad [m/s] V x V y Vz
0 1 2 3 4 5 0 10 20 30 Tiempo [s] Angulo [°] φ θ ψ
0 1 2 3 4 5 −10 −5 0 5 10 Tiempo [s]
Velocidad Angular [°/s]
ωφ ωθ ωψ
Figure 2.5: Steady height of the Quadcopter, stationary position.
We now proceed to test our model given a vector X0, of initial conditions, several
acceleration, the forces are balanced in such a way that the Quadcopter keeps itself
at constant height. This is done by keeping the following restriction:
[(F1+F2+F3+F4)e3]·a3 =M g (2.22)
The stationary position can be seen in figure 2.5, the initial state vector was
X0 = [0,0,1,0,0,0,0,0,0,0,0,0], that is z = 1 m, and all other variables were zero,
the forces were balanced to keep the quadcopter stationary, Fi = M g/4. As can be
seen in the figure, the quadcopter hovers a 1m above the ground devoid of any linear
or angular displacement.
0 1 2 3 4 5 0 2 4 6 8 10 Tiempo [s] Posicion [m] x y z
0 1 2 3 4 5 0 1 2 3 4 5 Tiempo [s] Velocidad [m/s] V x Vy Vz
0 1 2 3 4 5 −5 0 5 Tiempo [s] Angulo [°] φ θ ψ
0 1 2 3 4 5 −5
0 5
Tiempo [s]
Velocidad Angular [°/s]
ωφ ωθ ωψ
Figure 2.6: Quadcopter being accelerated in the vertical direction, due to higher force of the propellers.
The next test of the model consisted uniquely of vertical acceleration, for this the
initial state vector was X0 = ~0 and the forces of the propellers were all equal and 10% greater than the weight. This caused an accelerated vertical displacement that
can be seen in figure2.6, by looking a the velocity plot it is clear that the acceleration
is constant as expected.
After considering only linear displacement, we now test exclusively angular
displacement which was the simulation in the code 2.1, in this case, the force on the
0 1 2 3 4 5 −5 0 5 Tiempo [s] Posicion [m] x y z
0 1 2 3 4 5 −5 0 5 Tiempo [s] Velocidad [m/s] V x V y V z
0 1 2 3 4 5 −100 0 100 200 300 Tiempo [s] Angulo [°] φ θ ψ
0 1 2 3 4 5 −50
0 50 100
Tiempo [s]
Velocidad Angular [°/s]
ωφ ωθ ωψ
Figure 2.7: Stationary rotation about the vertical axis by inducing a total torque on the cuadcopter.
counter clockwise, keeping the restriction on equation 2.22. As a result the forces
keep the cuadcopter hovering, but a counterclockwise net torque is applied on the
system which causes the yaw angle to increase. By looking a the angular speed plot
of figure 2.7, it is clear that ψ increases at constant angular acceleration, once again
the expected result.
We now proceed to test a linear displacement caused by an inclination of the
aircraft, as we mention earlier a quadcopter is an under actuated system for this
reason some angular displacements cause linear displacements. In this case our initial state vector is X0 = [0,0,0,3,0,0,0,0,0,0,0,0], that is ψ0 = 3◦, which means that the quadcopter is initially tilted towards the negative y direction, this causes an
accelerated linear displacement, which can be seen in figure 2.8.
Because the restriction2.22is kept the cuadcopter has the vertical forces balanced
and only moves in the negative y, however the height is slightly diminished, but the
overall behavior is as expected.
We now finish by setting θ and ψ different than zero. The initial state vector is
0 1 2 3 4 5 −8 −6 −4 −2 0 2 Tiempo [s] Posicion [m] x y z
0 1 2 3 4 5 −3 −2 −1 0 1 Tiempo [s] Velocidad [m/s] V x V y V z
0 1 2 3 4 5 −1 0 1 2 3 Tiempo [s] Angulo [°] φ θ ψ
0 1 2 3 4 5 −5
0 5
Tiempo [s]
Velocidad Angular [°/s]
ωφ ωθ ωψ
Figure 2.8: Front heading north, displacement to the east (negative y direction) due to a roll angle of 3 degrees.
case the quadcopter is pointing northwest by setting the Yaw angle, and the front is
tilted towards the northwest by setting the pitch angle. As can be seen in the figure
2.9, the cuadcopter moves in the positive x and ydirection, by looking a the velocity
plot, it is clear the quadcopter moves faster in the x direction, this happens because
ψ is 40◦ which means that the front points more towards the north than towards the east. Again the behavior is a expected.
Additionally a 3D interface was implemented with the help of Simulink in
order to see the model clearly, the Simulink block diagram can be seen in figure
2.10. The integrator block solves the model, the selector blocks select the angular
and linear positions separately. Because the angular positions are in the Euler
angles representation, they need to be converted first to a Direction Cosine Matrix
representation and later to the Axis-Angle representation which is the representation
managed by the Virtual Realty block.
The virtual reality block needs as input the angular position in Axis-Angle
representation, the position vector and a .wrl file which contains the 3D geometry
0 1 2 3 4 5 −1 0 1 2 3 4 Tiempo [s] Posicion [m] x y z
0 1 2 3 4 5 −0.5 0 0.5 1 1.5 Tiempo [s] Velocidad [m/s] V x V y V z
0 1 2 3 4 5 −20 0 20 40 60 Tiempo [s] Angulo [°] φ θ ψ
0 1 2 3 4 5 −5
0 5
Tiempo [s]
Velocidad Angular [°/s]
ωφ ωθ ωψ
Figure 2.9: Front heading northwest, displacement to the northwest due to a pitch angle of 2 degrees.
wishes to work in this project.
2.3
Aerodynamical Model
The aerodynamical behavior of the quadcopter is composed of two parts, the drag due
to the geometry of the aircraft, which dampens the linear displacement and the
an-gular displacements, and the propeller aerodynamics which relates de anan-gular speed
ωof the propeller with the torques and forces that the motors apply on the quadrotor.
Because it will not be intended that the quadcopter will move at considerable
speeds, in part because experiments will be carried indoors, the drag on the aircraft
will be ignored and only the aerodynamical model of the propeller will be considered.
2.3.1
Aerodynamical Force of the Propeller
To obtain the main behavior of the force with respect to the angular velocity of the
propeller, we will make some theoretical simplifications, first of all we will assume that the wing of the propeller is rectangular (a×b), which ignores any effect of lift that the propeller could provide, second we will assume that the air is pushed downwards
and that the speed of the air is ωr, which would be the case if the inclination of the
wing were 45◦, this implies that the air is not pushed evenly by the propeller but still at at constant rate. The summary of the simplifications can be seen in figure2.11.
b
Ro
Δθ
Δ
r
a
r
Based on figure 2.11 we assume that the force generated is the contribution of
the rate of change and final speed of the mass leaving the propeller, that is.
dP
dt =
Z
(b dr r dθ ρ) ω 2π
ωr (2.23)
Where the first term in parenthesis is the mass displaced given a rotation dθ, the
second term is the rate at which the mass leaves the wing and the last term the speed
at which the air leaves the propeller. This equation considers the rate of change of momentum in the air rather than the force, but the results are the same and is
not rigorous mathematically but it provides a good aproximation. Summing all the
contributions we obtain.
dP
dt =F(ω) =ω
2bρ 2π
Z R0+a
R0
r2dr
Z 2π
0
dθ (2.24)
All terms are known or can be measured, so in summary the behavior of the force
generated by the propeller as a function of ω can be modeled as:
F(ω) = Kfω2 (2.25)
The factor Kf can be obtained experimentally.
2.3.2
Aerodynamical Torque of the Propeller
We now find the behavior of the torque given that we know the behavior of the force,
based on figure 2.11 we assume we can simplify the force as an evenly distributed
pressure on the wing of the propeller and that we can ignore the air drag on the
propeller. P=F(ω)/A where A = a·b, as the torque depends on the distance from the axis of the propeller we have to add the contribution to the torque at each distance
r from the axis of the propeller.
T =
Z R0+a
R0
F(ω) cosα
A br dr (2.26)
Where α is the angle of incidence of the force with respect to the wing of the
T =F(ω)[Ro+a/2] cosα (2.27)
Noting that ([Ro+a/2] cosα) and using the results of equation2.25, we can rewrite
the behavior of the propeller as:
T(ω) = Ktω2 (2.28)
Where Kt =Kf[Ro+a/2] cosα.
The behavior of the force and the torque due to the motors behaves similarly as
a function of ω, this result will be of utmost importance as the dynamics depends
on forces and toques applied, and to have both related by a constant simplifies the
2.4
Transfer Functions
The control solution that will be implemented on the system will be a PID controller,
for this purpose we will obtain the transfer functions for the movements we wish to
control. In order to achieve this the nonlinear state vector X˙ must be linearized first and the on has to obtain the transfer function from the state space representation.
We will assume that the equilibrium of the system is the state in which it is
hovering a constant height above the ground, the one described in figure 2.5. In
which the state vector is X0 =~0.
2.4.1
Control Inputs and Outputs
Before the linearization of the state vector, it is necessary to define the control inputs
of the system, one can consider the input as being the angular speed of the motors, however this approach introduces a nonlinearity and does not relate the inputs with
the outputs correctly. First we define the output of the system as:
yout = z φ θ ψ (2.29)
With this in mind, and knowing that the relation of the angular speed of the
motors with the force and torque is quadratic the following control input is proposed.
u= uroll upitch uyaw uthrottle =
w12+w22−w32−w42
−w2
1 +w22−w32+w24
w12−w22−w32+w42
w2
1+w22+w32+w24
=
1 1 −1 −1 −1 1 −1 1
1 −1 −1 1
1 1 1 1
w12
w2 2
w32
w2 4 = u1 u2 u3 u4 (2.30)
2.4.2
Linearizing the State Vector
Before being capable of getting the desired transfer functions one must linearize the
state vector, the first 6 components of the nonlinear state vector are:
˙
X1 =Vx X˙2 =Vy X˙3 =Vz (2.31)
˙
X4 = ˙φ X˙5 = ˙θ X˙6 = ˙ψ (2.32)
In the components 7,8 and 9 of X˙ one can appreciate how the thrust of the propellers are responsible for the accelerations in the x, y and z, highly coupled with angular orientation.
˙
X7 =
Kf(SφSψ+CφCψSθ)(w21 +w22+w32+w42)
M (2.33)
˙
X8 =−
Kf(CψSφ−CφSψSθ)(w12+w22+w23+w42)
M (2.34)
˙
X9 =
KfCφCθ(w21 +w22+w23+w42)
M −g (2.35)
Finally the last three components of X˙ are strongly coupled with angular dis-placements as they describe moments.
˙
X10= (2Ixx)−1
h
2Iyyψ˙θ˙−2Izzψ˙θ˙+ 2Ktw21SφSψ −2Ktw22SφSψ−2Ktw23SφSψ
+ 2Ktw24SφSψ + 2Ktw12CφCψSθ−2Ktw21CφCψSθ−2Ktw22CφCψSθ
−2Ktw32CφCψSθ+ 2Ktw24CφCψSθ+ 2
1
2KfLw2
1CψCθ+ 2
1
2KfLw2 2CψCθ
−212K
fLw32CψCθ−2
1 2K
fLw42CψCθ+ 2
1 2K
fLw12CφSψ−2
1 2K
fLw22CφSψ
+ 212KfLw2
3CφSψ−2
1
2KfLw2
4CφSψ −2
1
2KfLw2
1CψSφSθ+ 2
1
2KfLw2
2CψSφSθ
−212KfLw2
3CψSφSθ+ 2 1
2KfLw2
4CψSφSθ
i
˙
X11=−(2Iyy)−1
h
2Ixxφ˙ψ˙ −2Izzφ˙ψ˙+ 2Ktw12CψSφ−2Ktw22CψSφ−2Ktw23CψSφ
+ 2Ktw42CψSφ−2Ktw21CφSψSθ−2Ktw21CφSψSθ+ 2Ktw22CφSψSθ
+ 2Ktw32CφSψSθ−2Ktw24CφSψSθ+ 2
1
2KfLw2
1CφCψ−2
1
2KfLw2 2CφCψ
+ 212K
fLw23CφCψ−2
1 2K
fLw42CφCψ −2
1 2K
fLw21CθSψ−2
1 2K
fLw22CθSψ
+ 212KfLw2
3CθSψ+ 2
1
2KfLw2
4CθSψ+ 2
1
2KfLw2
1SφSψSθ−2
1
2KfLw2
2SφSψSθ
+ 212KfLw2
3SφSψSθ−2 1
2KfLw2
4SφSψSθ
i
(2.37)
˙
X12 = (2Izz)−1
h
2Ixxφ˙θ˙−2Iyyφ˙θ˙+ 2Ktω12CφCθ−2Ktω22CφCθ−2Ktω32CφCθ
+ 2Ktω42CφCθ−2
1
2KfLω2 1Sθ−2
1
2KfLω2 2Sθ+ 2
1
2KfLω2 3Sθ+ 2
1
2KfLω2 4Sθ
−212KfLω2
1CθSφ+ 2
1
2KfLω2
2CθSφ−2
1
2KfLω2
3CθSφ+ 2
1
2KfLω2 4CθSφ
i
(2.38)
Equations 2.31 through 2.38 show how nonlinear an coupled is the model of the
quadcopter, for this reason we linearize the model as follows:
˙
X =AX+Bu (2.39)
yout =CX+Du
Where yout is the output described in equation 2.29, D =0 because there is not a trace of the input on the output, and A and B are the linearization of X˙ with respect to the state vector X and the input u respectively, about the equilibrium
point:
X0 =~0 u0 =
0 0 0 u4 (2.40)
Note that the equilibrium of the system was described as the quadcopter hovering
statically at constant height, for this reason the input u4 6= 0, as this input must be equivalent to gravity. Rewriting equation 2.39 as the linearization of vector X˙ we have:
˙
X =X˙(X0,u0) +∇XX˙(X,u)
X
0,u0
(X−X0) +∇uX˙(X,u)
X
0,u0
(u−u0)
=∇XX˙(X,u)
X0,u0
X +∇uX˙(X,u)
X0,u0
u (2.41)
Where A = ∇XX˙(X,u)
X0,u0
a 12×12 matrix representing the linearization
of the system with respect to the state vector and B = ∇uX˙(X,u)
X
0,u0
a 12×4
matrix representing the lienarization of the system with respect to the input. It must be noted that in order to linearize adequately the next substitution must be made:
w21
w2 2
w2 3
w24
u1−u2+u3+u4 4
u1+u2−u3+u4 4
−u1+u2+u3−u4 4
−u1−u2−u3−u4 4 (2.42)
In order to expressX˙ as a function of u. the matrix Ais:
A=
0 0 0 0 0 0 1 0 0 0 0 0
0 0 0 0 0 0 0 1 0 0 0 0
0 0 0 0 0 0 0 0 1 0 0 0
0 0 0 0 0 0 0 0 0 1 0 0
0 0 0 0 0 0 0 0 0 0 1 0
0 0 0 0 0 0 0 0 0 0 0 1
0 0 0 0 (Kfu4)/M 0 0 0 0 0 0 0
0 0 0 −(Kfu4)/M 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0
(2.43)
And the matrix B is: B =
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 Kf/M
[(√2KfL)/(2Ixx) 0 0 0
0 (√2KfL)/(2Iyy) 0 0
0 0 Kt/Izz 0
(2.44)
The Matrix D = 0 and the only components of matrix C different than zero are
C3,3 = C4,4 = C5,5 = C6,6 = 1. With this information we now proceed to get the
vector containing the transfer functions:
Y(s)= [C(sI12×12−A)−1B+D]U(s) (2.45) Substituting the expressions for A, B, C, D and:
U(s)=L(u) = [U1(s), U2(s), U3(s), U4(s)]T (2.46)
We get Y(s) by components as:
Y1−2(s) =
"
0
0
#
Y3−6(s) =
(KfU4(s))/(M s2)
(p(2)KfLU1(s))/(2Ixxs2)
(p(2)KfLU2(s))/(2Iyys2)
(KtU3(s))/(Izzs2)
Y7−12(s) =
0 0 0 0 0 0 (2.47)
Z(s)
U4(s)
= Kf
M s2 (2.48)
For the Roll angle we have:
Φ(s)
U1(s) =
√ 2KfL
2Ixxs2 (2.49)
For the Pitch angle:
Θ(s)
U2(s) =
√ 2KfL
2Iyys2
(2.50)
And finally for the Yaw angle we have:
Ψ(s)
U3(s)
= Kt
Izzs2
(2.51)
We could have arrived at this same equations by writing the differential equations
of each actionui, assuming no coupling, linearizing and taking the Laplace Transform.
By having arrived at the transfer functions of the movements we intend to control
Prototype
The quadcopter selected was a small microquadcopter, with a distance from motor
to motor of 25 cm, this selection was adequate to budget and for working indoors.
The system can be seen in figure 3.1
Figure 3.1: Selected quadcopter.
The characteristics of the system are:
• 4 Motors,ωmax =25000 rpm
• 4 ESCs, 6A.
• 2 CCW Propellers
• 2 CW Propellers
• 4 Propeller Adpaters
• 950mAh (25kJ) Battery
3.1
Inertial Properties CAD
In order to estimate the inertial properties of the quadcopter, the frame was taken
apart and each piece up to screws and nuts were weighted in a precision scale, then a CAD of each part was created and later an assembly of the quadcopter was done,
this assembly can be seen in figure3.2. With the weight and geometrical information
the software Autodesk Inventor 2013 estimated the inertial properties.
Figure 3.2: CAD of the Quadcopter.
Mass = M = 238g (3.1)
Inertia Tensor =I = 106
0.78 0 0
0 .80 0
0 0 1.43
[g mm
2] (3.2)
There were non diagonal elements the inertia tensor, however this were small
compared to the diagonal elements, and were not taken into account.
Finally as the propeller and case of the motor rotate a CAD of this part was
assembled to estimate the value Ip required by the model, the CAD can be seen in
figure 3.3.
Figure 3.3: CAD of the Propeller and case of the motor.
3.2
Inertial Measurment Unit
In order to obtain dynamical measurements of the quadcopter, the inertial
measure-ment unit IMU selected was the Sparkfun 9DOF Sensor Stick, it an accelerometer,
magnetometer and gyroscope. The characteristics of these components are listed
below.
ADXL345 Accelerometer ITG2300 Gyroscope HMC5883L Magnetometer
Max Acceleration ±16g Max ω ±2000◦/s Max B~ ±8 gauss
Resolution ±4mg Resolution ±2◦/s Resolution: 0.73 mili-gauss
Supply: 2.0-3.6V Supply:2.1-3.6V Supply:2.16-3.6V
Table 3.1: IMU components description
One has access to data by the I2C protocol, the accelerometer and magnetometer
were calibrated with the Gauss-Newton algorithm. In the gyroscope only the bias was taken into account.
In order to get the heading of the quadcopter{φ, θ, ψ}the Direction Cosine Matrix was employed, the data from the sensors were given different weights when estimating
the heading.
3.3
Propeller and Motor Characterization
The last experimental result to obtain a complete model of a quadcopter, is the
behavior of the propeller. As was highlighted in section 2.3, the behavior of the
propeller is quadratic with respect to angular speed, in this section we try to verify
the model an obtain the constant Kf and Kt.
3.3.1
Experimental Setup
In order to test the propeller a special experimental setup must developed, in our
case a complete base and support for the motor was constructed, first of all, one must
locate the propeller at a considerable distance from the ground, because the ground
can alter the aerodynamic behavior considerably. Second, the base in which the
to oppose the minimal resistance to the flow of air and must have the appropriate
holes to match the base of the motor and the measuring device.
Figure 3.4: Support designed to hold the motor .
A support for the motor was developed, in figure3.4 one can see the design steps,
first its aerodynamic behavior was modeled to ensure that no pressure gradients are
present, later a precise CAD of the part was created, it consisted of two parts, the
bottom part and the top part, this with the purpose of locating the Electronic Speed
an ATI MINI 40 6DOF Force-Torque Sensor.
The CAD was later introduced in a fast prototyping machine to create the desired form, this can be seen in figure 3.4.
Finally a metal base was designed to provide support for the Force-Torque sensor,
and to isolate the propeller from the ground. It has 4 screw legs which allow leveling
of the base and the ground, a metal plate in which a pipe of 60 cm is welded, this
to provide isolation to the propeller. A machined top in which the Force-Torque
sensor can be supported. Additionally, care was taken to hide the cables that drive
the motor, this cables go through the pipe down to the floor where the battery and
microprocessor that runs the motor are located. The complete experimental setup can be seen in figure3.5.
The experiment was carried out for 6 propellers, an average of 18 different angular
speeds where performed on each for a total of 612 measurements. The measurements
were, {Fx, Fy, Fz}, {Tx, Ty, Tz} with the ATI MINI 40 Force-Torque sensor, ω with a tachometer, and an integral number between 0 and 1023 that the microprocessor
interprets in order to set the angular speed.
The measurements were full of noise as seen in figure 3.6, a FTT was performed
to see where the noise must be reduced, as a constant variable was being measured
a low bandpass Butterworth filter was employed to filter data, the filter had a cutoff
frequency of 15 Hz and was of order 5, it can be seen in figure3.6. Finally the filtered
signal was averaged with respect to time.
0 5 10 15 20 −0.2 0 0.2 0.4 0.6 0.8 Tiempo [s] F ue rz a [N ]
0 500 1000 1500 −100 −80 −60 −40 −20 Frecuencia [Hz] E sp ec tr o de P ot en ci a (W /H z)
0 10 20 30 0 0.2 0.4 0.6 0.8 1 Frequency [Hz] |S ig na l|
0 5 10 15 20 0 0.1 0.2 0.3 0.4 Tiempo [s] F ue rz a [N ]
Figure 3.6: Scheme for filtering data.
For each propeller a measurement at ω = 0 was performed, in order to set the
offset of the measurement. And as only one force and one moment was expected to be
generated, a vector sum was performed to find the total force and torque generated
by the propeller.
F =
q
(Fx−Fx0)2+ (Fy −Fy0)2+ (Fz−Fz0)2 (3.3)
T =
q
As a result 102 points were obtained for Force and another set for Torque. This
results were later processed to find the relation of force and torque with angular
speed. The results can be seen in figures 3.7 and 3.8.
The force is the described as:
F(ω) = 7.58×10−7ω2 [N] (3.5)
0 500 1000 1500
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6
ω [rad/s]
Force [N]
F(ω) = 7.58×10−7ω2
H
1
H
2
H
3
H4
H
5
H6
0 500 1000 1500 0
0.01 0.02 0.03 0.04 0.05 0.06
ω [rad/s]
Torque [Nm]
T(ω) = 2.5×10−8ω2
H
1
H
2
H
3
H
4
H
5
H
6
Figure 3.8: Moment as function of ω.
And torque:
3.4
Control
With the experimental data, the transfer functions can now be used to tune the PID
controllers that will move the quadcopter. For practical reasons only the Roll and
Pitch angle where possible to control. Because the Microcontroller (Arduino) did not
have enough memory to support the Magnetometer with a PID that would control
the Yaw angle. Additionally the lack of a distance sensor did not permit to control the
vertical direction, however all the transfer functions are presented for future work.For
the z displacement:
Z(s)
U4(s)
= 3.185×10
−6
s2 (3.7)
For the Roll angle we have:
Φ(s)
U1(s)
= 8.543×10
−5
s2 (3.8)
For the Pitch angle:
Θ(s)
U2(s)
= 8.365×10
−5
s2 (3.9)
And finally for the Yaw angle we have:
Ψ(s)
U3(s) =
1.748×10−5
s2 (3.10)
With the transfer functions ready, we proceed to use the Matlab auto PID tuning
tool to tune the PID controllers, then apply the controllers to the nonlinear model
and tune again. The results for the Roll and Pitch angle controller are the same, the
controller is:
P = 47 I =27 D=−103 (3.11)
It must be noted that care was taken with the change of variables of equation
2.42, during the PID tuning. The behavior of the controller in the nonlinear model
0 1 2 3 4 5 6 7 8 9 10 0
1 2 3 4 5 6 7 8
Time [s]
Angle [rad]
Step
PID Roll Response PID Pitch Response
3.5
Implementation on the Real System
The controller then needed to be uploaded into something in order to control the
quadcopter. The selected hardware was an Arduino FIO, it has 32KB of memory
2KB of RAM and an XBee port, this micro-controller was selected for future use in
sending wireless control signals and to gather wireless data, this with the purpose of
developing different scheme controls in the future.
A complex assembly was needed in order to connect the motors, receptor and
IMU, this assembly can be seen in figure3.10, the receptor signal need to be covered
from 5V to 3.3V to prevent any damage to the Arduino FIO, so a bidirectional level converter from Sparkfun was employed.
Figure 3.10: Controller Assembly, left - bidirectional level converter and ports for the
Initially the software that would go into the Arduino FIO was going to be
devel-oped, however due to the complexity of the technical details, for example anti-windup
schemes, an open-source software developed by aeroquad.com was employed, this software employed a PID controller for the Roll and Pitch motion, so the PID found
by simulation was successfully tested in flight. The final flight of the quadcopter can
be seen in figure 3.11.
Conclusions and Future Work
Throughout this work all of the proposed objectives were reached, first of all, a
precise aerodynamical model of a quadcopter was created. When flying the real
quadcopter, the specific traits of the model could be appreciated.
Thanks to a precise model, good transfer functions were obtained and this saved
precious time in the tuning of the PID controllers, additionally the nonlinear model,
helped considerably when tuning the PID constants as the transfer functions are an
oversimplification of the original model.
Perhaps the most important aspect of this project lied in the preflight
experi-ments, getting an accurate estimation of the inertial properties was very important
as the closer the model is to reality, the better the constants of the controller.
The most important experiment of this project was the propeller characterization,
for this reason extreme care was taken in the design and the results were greater
than expected, they proved that approximate theoretical models bring a decent
approximation of reality, plus the information obtained in this experiment was of key importance in the model and thus in the design of the controller.
Finally, to work both in simulation and in a real system has been a great learning
experience, one cannot grasp reality if not in a real system, but the physical intuition
real system.
The following recommendation are for anyone who wishes to proceed with this work:
• Develop a new controller, a State Space controller is a great solution.
• Implement a distance sensor, they are affordable and will allow the vertical control of the system.
• Develop a wireless communication between the computer and the quadcopter.
• Use a camera to provide another input for an AI controller, or a higher hierarchy controller.
• Enjoy every moment, burnt circuits, crashes etc. Those errors give an incredible learning experience, do not fear to fail.
Code
A.1
Arduino Code for controlling the Motor
// Using t h e s e r v o l i b r a r y
#include <S e r v o . h>
// D e f i n e PWM v a r i a b l e
S e r v o e s c ;
// Analog i n p u t
i n t t h r o t t l e P i n = 1 ;
// S e t ESC on p i n 9
void s e t u p ( )
{
e s c . a t t a c h ( 9 ) ;
}
// Read a n a l o g s i g n a l and w r i t e PWM s i g n a l f o r t h e ESC
void l o o p ( )
{
i n t t h r o t t l e = analogRead ( t h r o t t l e P i n ) ;
t h r o t t l e = map( t h r o t t l e , 0 , 1 0 2 3 , 0 , 1 7 9 ) ;
e s c . w r i t e ( t h r o t t l e ) ;
A.2
Arduino Code for reading the Inertial
Mea-surement Unit
//============================================================================== // F i r s t Arduino version d e s i g n e d t o r e a d de S p a r k f u n 9DOF s e n s o r s t i c k SEN 1 0 7 2 4 // h t t p : / / c h i o n o p h i l o u s . w o r d p r e s s . com / 2 0 1 2 / 0 2 / 1 0 / c o n n e c t i n g to s p a r k f u n s 9 d o f / / s e n s o r s t i c k i 2 c a c c e s s to a d x l 3 4 5 i t g 3 2 0 0 and hmc5843 /
// A c c e l e r o m e t e r ADXL345 // Magnetometer HMC5883L // Gyro ITG 3 2 0 0 MEMS
// The boa rd u s e s t h e I2C i n t e r f a c e //
// The Wire l i b r a r y t o u s e I2C #i n c l u d e <Wire . h>
//============================================================================== // DEFINITIONS
//==============================================================================
/ / ACCELEROMETER
#d e f i n e ADXL345 ADDRESS ( 0 xA6 >> 1 ) //ADRESS
// There a r e 6 d a t a r e g i s t e r s , t h e y a r e s e q u e n t i a l s t a r t i n g // w i t h t h e LSB o f X . We’ l l r e a d a l l 6 i n a b u r s t and won ’ t // a d d r e s s them i n d i v i d u a l l y
#d e f i n e ADXL345 REGISTER XLSB ( 0 x32 )
// Need t o s e t power c o n t r o l b i t t o wake up t h e a d x l 3 4 5 #d e f i n e ADXL REGISTER PWRCTL ( 0 x2D )
#d e f i n e ADXL PWRCTL MEASURE ( 1 << 3 ) / /
/ / GYRO
#d e f i n e ITG3200 ADDRESS ( 0 xD0 >> 1 ) //ADRESS // r e q u e s t b u r s t o f 6 b y t e s from t h i s a d d r e s s #d e f i n e ITG3200 REGISTER XMSB ( 0 x1D )
#d e f i n e ITG3200 REGISTER DLPF FS ( 0 x16 ) #d e f i n e ITG3200 FULLSCALE ( 0 x03 << 3 ) #d e f i n e ITG3200 42HZ ( 0 x03 ) / /FILTER
/ /
/ / MAGNETOMETER
#d e f i n e HMC5843 ADDRESS ( 0 x3C >> 1 ) / /ADRESS
// F i r s t d a t a a d d r e s s o f 6 i s XMSB. A l s o need t o s e t a c o n f i g u r a t i o n r e g i s t e r f o r // c o n t i n u o u s measurement
#d e f i n e HMC5843 REGISTER XMSB ( 0 x03 ) #d e f i n e HMC5843 REGISTER MEASMODE ( 0 x02 ) #d e f i n e HMC5843 MEASMODE CONT ( 0 x00 ) / /
//
//============================================================================== // GENERAL INITIALIZATION
//============================================================================== v o i d s e t u p ( ) {
Wire . b e g i n ( ) ; / / I n i t i a l i z e s w i r e l i b r a r y t o work S e r i a l . b e g i n ( 9 6 0 0 ) ; / / I n i t i a l i z e s s e r i a l p o r t // I n i t i a l i z e s IMU d a t a
f o r( i n t i = 0 ; i < 3 ; ++i ) {
a c c e l e r o m e t e r d a t a [ i ] = m a g n e t o m e t e r d a t a [ i ] = g y r o d a t a [ i ] = 0 ;
}
// I n i t i a l i z e s e a c h component o f t h e IMU, c a l l i n g t h e i n i t i a l i z a t i o n f u n t i o n s i n i t a d x l 3 4 5 ( ) ;
i n i t h m c 5 8 4 3 ( ) ; i n i t i t g 3 2 0 0 ( ) ;
}
/ / END OF GENERAL INITIALIZATION //
//============================================================================== // GENERAL LOOP
//============================================================================== v o i d l o o p ( ) {
//ACCELEROMETER DATA READ r e a d a d x l 3 4 5 ( ) ;
S e r i a l .print( ”ACCEL: ” ) ;
S e r i a l .print( a c c e l e r o m e t e r d a t a [ 0 ] ) ; S e r i a l .print( ”\t ” ) ;
S e r i a l .print( a c c e l e r o m e t e r d a t a [ 1 ] ) ; S e r i a l .print( ”\t ” ) ;
S e r i a l .print( a c c e l e r o m e t e r d a t a [ 2 ] ) ; S e r i a l .print( ”\n ” ) ;
//GYRO DATA READ r e a d i t g 3 2 0 0 ( ) ;
S e r i a l .print( ”GYRO: ” ) ; S e r i a l .print( g y r o d a t a [ 0 ] ) ; S e r i a l .print( ”\t ” ) ;
S e r i a l .print( g y r o d a t a [ 1 ] ) ; S e r i a l .print( ”\t ” ) ;
S e r i a l .print( g y r o d a t a [ 2 ] ) ; S e r i a l .print( ”\n ” ) ;
//MAGNETOMETER DATA READ r e a d h m c 5 8 4 3 ( ) ;
S e r i a l .print( ”MAG: ” ) ;
S e r i a l .print( m a g n e t o m e t e r d a t a [ 0 ] ) ; S e r i a l .print( ” , ” ) ;
S e r i a l .print( m a g n e t o m e t e r d a t a [ 1 ] ) ; S e r i a l .print( ” , ” ) ;
S e r i a l .print( ”\n ” ) ; // Sample a t 10Hz d e l a y ( 1 0 0 ) ; }
/ / END OF GENERAL LOOP //
//============================================================================== // FUNCTIONS THAT INITIALIZE AND READ ACCELEROMETER DATA
//============================================================================== // INITIALIZE
v o i d i n i t a d x l 3 4 5 ( ) {
b y t e d a t a = 0 ;
i 2 c w r i t e (ADXL345 ADDRESS , ADXL REGISTER PWRCTL, ADXL PWRCTL MEASURE ) ; // Check t o s e e i f i t worked !
i 2 c r e a d (ADXL345 ADDRESS , ADXL REGISTER PWRCTL, 1 , &d a t a ) ; S e r i a l . p r i n t l n ( ( u n s i g n e d i n t ) d a t a ) ;
}
//READ DATA
i n t a c c e l e r o m e t e r d a t a [ 3 ] ; v o i d r e a d a d x l 3 4 5 ( ) {
b y t e b y t e s [ 6 ] ; memset ( b y t e s , 0 , 6 ) ;
// r e a d 6 b y t e s from t h e ADXL345
i 2 c r e a d (ADXL345 ADDRESS , ADXL345 REGISTER XLSB , 6 , b y t e s ) ; //now unpack t h e b y t e s
f o r ( i n t i =0; i<3;++ i ) {
a c c e l e r o m e t e r d a t a [ i ] = ( i n t ) b y t e s [ 2∗i ] + ( ( ( i n t ) b y t e s [ 2∗i + 1 ] ) << 8 ) ;
} }
/ / END FUNCTIONS THAT INITIALIZE AND READ ACCELEROMETER DATA //
//============================================================================== // FUNCTIONS THAT INITIALIZE AND READ GYRO DATA
//============================================================================== // INITIALIZE
v o i d i n i t i t g 3 2 0 0 ( ) {
b y t e d a t a = 0 ;
// S e t DLPF t o 42 Hz ( c h a n g e i t i f you want ) and //s e t t h e s c a l e t o ” F u l l S c a l e ”
i 2 c w r i t e ( ITG3200 ADDRESS , ITG3200 REGISTER DLPF FS , ITG3200 FULLSCALE|ITG3200 42HZ ) ; // S a n i t y c h e c k ! Make s u r e t h e r e g i s t e r v a l u e i s c o r r e c t .
i 2 c r e a d ( ITG3200 ADDRESS , ITG3200 REGISTER DLPF FS , 1 , &d a t a ) ; S e r i a l . p r i n t l n ( ( u n s i g n e d i n t ) d a t a ) ;
}
//READ
i n t g y r o d a t a [ 3 ] ; v o i d r e a d i t g 3 2 0 0 ( ) {
b y t e b y t e s [ 6 ] ; memset ( b y t e s , 0 , 6 ) ;
// r e a d 6 b y t e s from t h e ITG3200
i 2 c r e a d ( ITG3200 ADDRESS , ITG3200 REGISTER XMSB , 6 , b y t e s ) ; //now unpack t h e b y t e s f o r ( i n t i =0; i<3;++ i ) {
g y r o d a t a [ i ] = ( i n t ) b y t e s [ 2∗i + 1 ] + ( ( ( i n t ) b y t e s [ 2∗i ] ) << 8 ) ;
} }
/ / END FUNCTIONS THAT INITIALIZE AND READ GYRO DATA //
//============================================================================== // FUNCTIONS THAT INITIALIZE AND READ MAGNETOMETER DATA
//============================================================================== // INITIALIZE
v o i d i n i t h m c 5 8 4 3 ( ) {
b y t e d a t a = 0 ;
//s e t up c o n t i n u o u s measurement
i 2 c w r i t e (HMC5843 ADDRESS , HMC5843 REGISTER MEASMODE, HMC5843 MEASMODE CONT ) ; // S a n i t y ch ec k , make s u r e t h e r e g i s t e r v a l u e i s c o r r e c t .
i 2 c r e a d (HMC5843 ADDRESS , HMC5843 REGISTER MEASMODE, 1 , &d a t a ) ; S e r i a l . p r i n t l n ( ( u n s i g n e d i n t ) d a t a ) ;
}
//READ
i n t m a g n e t o m e t e r d a t a [ 3 ] ; v o i d r e a d i t g 3 2 0 0 ( ) {
b y t e b y t e s [ 6 ] ; memset ( b y t e s , 0 , 6 ) ;
// r e a d 6 b y t e s from t h e HMC5843
i 2 c r e a d (HMC5843 ADDRESS , HMC5843 REGISTER XMSB , 6 , b y t e s ) ; //now unpack t h e b y t e s
f o r ( i n t i =0; i<3;++ i ) {
m a g n e t o m e t e r d a t a [ i ] = ( i n t ) b y t e s [ 2∗i + 1 ] + ( ( ( i n t ) b y t e s [ 2∗i ] ) << 8 ) ;
} }
/ / END FUNCTIONS THAT INITIALIZE AND READ MAGNETOMETER DATA //
//============================================================================== // I2C FUNCTIONS TO READ AND WRITE USING I2C
//============================================================================== //WRITE
v o i d i 2 c w r i t e ( i n t a d d r e s s , b y t e r e g , b y t e d a t a ) {
// Send o u t p u t r e g i s t e r a d d r e s s Wire . b e g i n T r a n s m i s s i o n ( a d d r e s s ) ; Wire . s e n d ( r e g ) ;
// Connect t o d e v i c e and s e n d b y t e Wire . s e n d ( d a t a ) ; // low b y t e Wire . e n d T r a n s m i s s i o n ( ) ;
}
//READ
i n t i = 0 ;
// Send input r e g i s t e r a d d r e s s Wire . b e g i n T r a n s m i s s i o n ( a d d r e s s ) ; Wire . s e n d ( r e g ) ;
Wire . e n d T r a n s m i s s i o n ( ) ;
// Connect t o d e v i c e and r e q u e s t b y t e s Wire . b e g i n T r a n s m i s s i o n ( a d d r e s s ) ; Wire . r e q u e s t F r o m ( a d d r e s s , c o u n t ) ;
while( Wire . a v a i l a b l e ( ) ) // s l a v e may s e n d l e s s than r e q u e s t e d {
c h a r c = Wire . r e c e i v e ( ) ; // r e c e i v e a b y t e a s c h a r a c t e r d a t a [ i ] = c ;
i ++;
}
Wire . e n d T r a n s m i s s i o n ( ) ;
}
// END OF I2C FUNCTIONS TO READ AND WRITE USING I2C //
Additional Theoretical Aspects
B.0.1
Alternative Definition of Reference Frames and
Vari-ables
To define the different variables that describe the quadcopter it is necessary, in our
case, to choose one variable for each degree of freedom that the system has. A quad-copter has ten degrees of freedom, three of them associated with its linear movement,
another three with its rotations and four additional degrees of freedom that are
related to the rotation of its propellers. The generalized coordinates employed to
describe each degree of freedom, are numbered conveniently but not presented in an
orderly manner.
First we define a frame of reference fixed to the earth, frame A, it contains three
orthogonal unit vectors a1,a2 and a3, see figure B.1. The vector a1 points in the direction of the magnetic north of the earth,a2 points towards the west and a3 goes upwards to the heaven.
We proceed to define a frame fixed in the center of mass of the Quadcopter, frame
B, this frame has all unit vectors in common with frame A, that isa1 =b1,a2 =b2 and a3 =b3, see figure B.1. This frame does not rotate.
Three variables are needed to describe the relation between frame A and frame
B, this variables are defined as follows with respect to frame A:
Figure B.1: Frame A is the Earth, Frame B has all unit vectors in common with Frame A, however this frame moves with the center of mass of the Quadcopter.
• q9: Displacement of the center of mass to the west, in the direction of a2.
• q10: Displacement of the center of mass upwards, in the direction of a3.
Next we describe the rotations of the quadcopter with respect to its center of
mass. For this we define three additional reference frames, the first of them being
frame C with unit vectors c1, c2 and c3. Frame C rotates with respect to frame
B, having as axis of rotation the unit vector b3 which is equal to c3, see figure B.2, this rotation is another degree of freedom described by the variable q5, the table
B.1 describes the relation of the unit vectors of B with the unit vectors of C, it is a
rotation matrix condensed in the form of a table, where one can read the relations
directly. The angle q5 is positive in the counterclockwise direction withb3 as axis of rotation.
Figure B.2: FrameC is the result of a simple rotation of frameB, frameDis a simple rotation of C and E a simple rotation of D.
q5 b1 b2 b3
c1 cos sin 0
c2 −sin cos 0
c3 0 0 1
Table B.1: A simple rotation from frame B to frameC.
To contextualize the effect of this rotation assume that the quadcopter were static
in the air, the front part of it were pointing to the north and the motors providing the right support, if a positive value of q5 was set, the quadcopter would rotate
counterclockwise parallel to the ground, it would not have a linear displacement but
a rotation fixed in place.
For the next rotation we define frame D with unit vectorsd1, d2 and d3, which is a frame that rotates with respect to frame C about an axis parallel to c2, as it is
