MEAM 620 Project 1

Modeling and Control of a Quad Rotor

January 29, 2018

1 Introduction
In this project you will learn about quad rotor dynamics and develop algorithms to control it. A thorough under-
standing of the theory and a clean implementation of the controller is crucial because you will use this code base in
subsequent projects. The code will be written in Matlab and first tested in a simulator. During the lab session you
will use it to fly a real, physical quad rotor with it!

ω2 ω1
body
frame
b3
inertial
a3 frame b2
b1

a2 r
ω3 ω4

a1

a3 b3 body
a’3
frame
a’’2 b2
a’1 a’1 b1

a’2

Figure 1: The CrazyFlie 2.0 robot that will be used in the lab and exercises. An Euler Z-X-Y transformation takes
the inertial frame A to the body-fixed frame B. First a rotation by ψ around the a3 axis is performed, then a roll by
φ around the (rotated!) a1 axis, and finally a pitch by θ around the (now twice rotated!) a2 axis. A translation r then
produces the coordinate system B, coinciding with the center of mass C of the robot, and aligned along the arms.
2 Modeling
2.1 Coordinate Systems and Reference frames
The coordinate systems and free body diagram for the quadrotor are shown in Fig. 1. The inertial frame, A, is
defined by the triad a1 , a2 , and a3 with a3 pointing upward. The body frame, B, is attached to the center of mass of
the quadrotor with b1 coinciding with the preferred forward direction and b3 being perpendicular to the plane of the
rotors pointing vertically up during perfect hover (see Fig. 1). These vectors arePparallel to the principal axes.
More formally, the coordinates of a vector x that is expressed in A as x = i A xi ai and in B as x = i B xi bi
P
are transformed into each other by the rotation matrix ARB and translation vector ATB :
A
x = ARB Bx + ATB . (1)

To express the rotational motion of the moving frame B, it is useful to introduce the angular velocity vector ω
that describes how the basis vectors bi move:
d
bi = ω × bi . (2)
dt
Note that this equation is coordinate free, meaning ω is not yet expressed explicitly in any particular coordinate
system. We will denote the components of angular velocity of the robot in the body frame by p, q, and r:

ω = pb1 + qb2 + rb3 . (3)

The heading (yaw) angle of the robot plays a special role since we can choose it freely without directly affecting the
robots dynamics. For this reason we use Z − X − Y Euler angles to describe the transform from A to B: first a yaw
rotation by ψ around the a3 axis is performed, then a roll by φ around the (rotated!) a1 axis, and finally a pitch by θ
around the (now twice rotated!) a2 axis 1 From the Euler angles one can compute the rotation matrix as follows:
 
cos(ψ) cos(θ) − sin(φ) sin(ψ) sin(θ) − cos(φ) sin(ψ) cos(ψ) sin(θ) + cos(θ) sin(φ) sin(ψ)
A
RB = cos(θ) sin(ψ) + cos(ψ) sin(φ) sin(θ) cos(φ) cos(ψ) sin(ψ) sin(θ) − cos(ψ) cos(θ) sin(φ) . (4)
− cos(φ) sin(θ) sin(φ) cos(φ) cos(θ)

The angular velocity and Euler angle velocities are related by:
    
p cos(θ) 0 − cos(φ) sin(θ) φ̇
q  =  0 1 sin(φ)   θ̇  (5)
r sin(θ) 0 cos(φ) cos(θ) ψ̇

2.2 Motor Model

This section describes how we model the motors. Each rotor has an angular speed ωi and produces a vertical force
Fi according to
Fi = kF ωi2 . (6)
Experimentation with a fixed rotor at steady-state shows that kF ≈ 6.11 × 10−8 N/rpm2 . The rotors also produce a
moment according to
Mi = kM ωi2 . (7)
1
In aerospace engineering, the direction of rotation for yaw and pitch are opposite to our definition, which follows the conventional
right-hand rule.

2
The constant, kM , is determined to be about 1.5 × 10−9 Nm/rpm2 by matching the performance of the simulation to
the real system.
Data obtain from system identification experiments suggest that the rotor speed is related to the commanded
speed by a first-order differential equation
ω̇i = km (ωides − ωi ).
This motor gain, km , is found to be about 20 s−1 by matching the performance of the simulation to the real system.
The desired angular velocities, ωides , are limited to a minimum and maximum value determined through experimen-
tation.
However, as a first approximation, we can assume the motor controllers to be perfect and the time constant km
associated with the motor response to be arbitrarily small. In other words, we can assume the actual motor velocities
ωi are equal to the commanded motor velocities, ωides .
Notes:

1. In simulation, you will be able to directly command torque and thrust. The thrust limit is available as a
parameter inside the simulator, although you don’t need it for this project.

2. In the lab, we will provide the code to translate torque and thrust to motor commands. You will however
encounter saturation of your motors and may have to tweak your gains.

2.3 Rigid Body Dynamics: Newton’s Equations of Motion for the Center of Mass
We will describe the motion of the center of mass (CoM) in the inertial (“world”) coordinate frame A. This makes
sense because we will want to specify our waypoints (where the robot should fly), trajectories and controller targets
(where the robot should be at this moment) in the inertial frame.
Newton’s equation of motion for the robot’s CoM r is determined by the robot’s mass m, the gravitational force
Fg = mg, and the sum of the motor’s individual forces Fi :
X
mr̈ = Fg + Fi . (8)
i

In the coordinates of the inertial frame A this reads:

   
0 0
m Ar̈ =  0  + ARB  0 . (9)
−mg F1 + F2 + F3 + F4

where ARB is the rotation matrix from Eq (1). If the robot is tilted, ARB (cf Eqn (4)), will mix the propeller forces
into the x and y plane and so generate horizontal acceleration of the robot. In other words, we can accomplish
movement in all three directions by manipulating the attitude of the vehicle and its thrust.
Equation (9) further suggests that rather than using the forces Fi as control inputs, we should define our first
input u1 as the sum
X 4
u1 = Fi . (10)
i=1

3
2.4 Euler’s Equations of Motion for the Attitude
Since from Eq (9) we know that attitude control is necessary to generate horizontal motion, in this section we will
examine how the motors affect the orientation of the vehicle.
In the inertial frame, the rate at which the robot’s angular momentum L = Iω changes is determined by the total
moment M generated by the propellers:
L̇ = M . (11)
While this equation looks clean and simple in the inertial frame, it is actually hard to work with because the inertial
tensor I changes with the attitude of the robot, and is thus time dependent and non diagonal! For control purposes,
this equation is best expressed in the body frame that is aligned with the principal axis, where the inertia tensor I
is constant, and diagonal. However, since now the basis vectors bi are time-dependent, the equation for the time
derivative of the angular momentum in the body frame becomes:
X X X X X
B B B
L̇ = L̇i bi + Li ḃi = L̇i bi + ω × B Li bi = B
L̇i bi + ω × L . (12)
i i i i i

Combining (11) and (12), and using the fact that I is constant in B yield’s Euler’s Equation:
I Bω̇ = BM − Bω × I Bω , (13)
where depending on context I means alternatingly a tensor or a matrix expressed in the body frame.
How do the motors come into play? First, by generating a force Fi that is a distance l away from the CoM, each
motor can exert a torque that is in the b1 , b2 plane. In addition, each rotor produces a moment Mi perpendicular to
the plane of rotation of the blade. Rotors 1 and 3 rotate clockwise in the −b3 direction while 2 and 4 rotate counter
clockwise in the b3 direction. Since the moment produced on the quadrotor is opposite to the direction of rotation
of the blades, M1 and M3 act in the b3 direction while M2 and M4 act in the −b3 direction. In contrast to this, the
forces Fi are all in the positive b3 direction due to the fact that the pitch is reversed on two of the propellers, see Fig
1.
Using this geometric intuition, and expressing the angular velocity in the body frame by p, q, and r as in Eq (3),
the Euler equation (13) becomes:
       
ṗ l(F2 − F4 ) p p
I q̇  =  l(F3 − F1 )  − q  × I q  . (14)
ṙ M 1 − M2 + M3 − M4 r r
Note that the total net torque along the yaw (b3 ) axis of the robot is simply the signed sum of the motor’s torques
Mi (why does the distance l to the center of the robot play no role?).
We can rewrite this as:
 
    F1    
ṗ 0 l 0 −l   p p
F2   
I q̇  = −l 0 l 0   F3  − q × I q  . (15)
ṙ γ −γ γ −γ r r
F4
where γ = kkM F
is the relationship between lift and drag given by Equations (6-7). Accordingly, we will define our
second set of inputs to be the vector of moments u2 given by:
 
  F1
0 l 0 −l  
F2 
u2 = −l 0 l 0   F3  . (16)
γ −γ γ −γ
F4

4
Dropping the reference frame superscripts for brevity, we can now write the equations of motion for center of mass
and orientation in compact form:    
0 0
mr̈ =  0  +R 0
 (17)
−mg u1
     
ṗ p p
I q̇  = u2 − q  × I q  , (18)
ṙ r r
where (17) is in inertial coordinates, and (18) is in body coordinates. The inputs u1 and u2 are related to the motor
forces Fi via the linear system of equations formed by (10) and (16). We are thus facing a system governed by two
coupled second order differential equations which we will exploit in section 3 to design controllers.

3 Robot Controllers
3.1 Trajectory Generation
Our ultimate goal is to make the robot follow a trajectory. For the time let’s assume we are given a trajectory
generator that for any given time t produces a trajectory target zdes (t) consisting of target position rT (t) and target
yaw ψT (t):  
des rT (t)
z (t) = (19)
ψT (t)
and its first and second derivatives żdes and z̈des . To hover for example, the trajectory generator would produce a
constant r(t) = r0 and e.g. a fixed ψ(t) = ψ0 , with all derivatives being zero.
The controller’s job will then be to produce the correct torque u2 and thrust u1 to bring the robot to the desired
state specified by the trajectory generator.

3.2 Linear Backstepping Controller

For this controller we will make the following assumptions:

1. The robot is near the hover points, meaning the roll angle φ and pitch angle θ are small enough to allow
linearization of trigonometric functions, i.e. cos(φ) = 1, sin(φ) = φ, cos(θ) = 1, sin(θ) = θ. This will allow
us to linearize the rotation matrix in Eq (17).

2. The robot’s angular velocity is small enough for the cross product term between angular momentum and
velocity in (18) to be negligible. This is usually a good assumption for almost any quad rotor.

3. The attitude of the quad rotor can be controlled at a much smaller time scale than the position. This “back-
stepping” approach to controller design allows a decoupling of position and attitude control loops. In practice
this is generally warranted since the attitude controller usually runs almost an order of magnitude faster than
the position controller.

Making the backstepping approximation is equivalent to assuming that R in Eq (17) can be commanded instan-
taneously. This further implies that it is possible to directly command the acceleration rdes . Figure 2 shows how
trajectory generator, position, and attitude controller play together.

5
 des
trajectory position 1 motor rigid body
planner controller controller dynamics
attitude
des controller 2
, ,

Figure 2: The position and attitude control loops.

Define the position error in terms of components by:

ei = (ri − ri,T ).

In order to guarantee that this error goes exponentially to zero, we require

(r̈ides − r̈i,T ) + kd,i (ṙi − ṙi,T ) + kp,i (ri − ri,T ) = 0 . (20)

In Eq (20), ri,T and its derivatives are given by the trajectory generator, and ri and ṙi are provided by the state
estimation system, allowing for the commanded acceleration r̈ides to be calculated:

r̈ides = r̈i,T − kd,i (ṙi − ṙi,T ) − kp,i (ri − ri,T ) . (21)

Now the attitude of the quad rotor must be controlled such that it will generate r̈des . For this, the linearized
version of (17) is used:

r̈1des = g(θdes cos ψT + φdes sin ψT ) (22a)

r̈2des des
= g(θ sin ψT − φ des
cos ψT ) (22b)
1
r̈3des = u1 − g. (22c)
m
From the third equation we can directly read off the thrust control u1 . The first two equations can be solved for θdes
and φdes , since ψ des = ψT is given directly by the trajectory generator.
Linearizing (18), we get:  
ṗ
I q̇  = u2 , (23)
ṙ
and by further exploiting that via Eq (5) Euler angle velocities are to linear approximation equal to angular velocities,
it becomes evident that we can directly command the acceleration of the Euler angles:
 
φ̈
I θ̈  = u2 .
 (24)
ψ̈

Thus the “inner loop” attitude control can also be done with a simple PD controller:

−kp,φ (φ − φdes ) − kd,φ (p − pdes )

 

u2 = I  −kp,θ (θ − θdes ) − kd,θ (q − q des )  , (25)

−kp,ψ (ψ − ψT ) − kd,ψ (r − ψ̇T )
6
where the desired roll and pitch velocities pdes and q des can be computed from the equations of motion and the
specified trajectory, but in practice can be set to zero.
In summary, the controller then works as follows:

1. Use Eq (21) to compute the commanded acceleration r̈des .

2. Use Eq (22c) to compute u1 and the desired angles θdes and φdes from (22a) and (22b).

3. Use Eq (25) to compute u2 .

3.3 A Geometric Nonlinear Controller

Nonlinear controllers are generally built based on geometric intuition, i.e. the quad rotor’s b3 axis is tilted to point
in the desired direction, and thrust is applied. Such controllers are suitable for very aggressive maneuvers and will
allow for faster flight and sharper turns, in particular in the simulator. Note that we have considerable freedom to
choose a control algorithm so long as it results in a stable system.
The following section is based on the controller developed in [1], which in turn is a simplified version of the
controller in [2]. For a thorough treatment and stability analysis please refer to [2].
It turns out that the basic layout of the controller remains as shown in Figure 2, i.e. there is an attitude and a
position controller, although we no longer make the backstepping assumption.
We start out from the PD controller in Eq (21), but now write it in more compact vector form:

r̈des = r̈T − Kd (ṙ − ṙT ) − Kp (r − rT ) , (26)

where Kd and Kp are diagonal, positive definite gain matrices.

We are again faced with the question how to compute the inputs u1 , u2 to generate r̈des . We first determine the
input u1 . By rearranging Eq (17) one obtains:
   
0 0
mr̈des +  0  = u1 R 0 . (27)
mg 1

Note that the lhs of (27) is just the total commanded force Fdes (including gravity):
 
0
Fdes = mr̈des +  0  . (28)
mg

On the right hand side is u1 , multiplied with the quad rotors axis b3 = R[0, 0, 1]T , expressed in the inertial frame.
We obtain the input u1 by projecting Fdes onto b3 :

u1 = bT3 Fdes . (29)

To compute u2 , we observe that a quad rotor can only produce thrust along its b3 axis. It makes sense to align
b3 with Fdes , and align b1 to match the desired yaw ψT . Please refer to Fig. 3. In the following steps we find a triad
Rdes = [b1 des , b2 des , b3 des ] that has the proper alignment, then proceed to develop a control input u2 that produces
the corresponding torque to drive the attitude towards Rdes .

7
 a3 des

b3des
b2des a2

b1des a
a1

Figure 3: Geometry based attitude control. First b3 des is aligned along Fdes . Then b2 des is chosen to be perpendicular
to the plane spanned by the desired yaw heading vector aψ and b3 des . This ensures that b1 des and b3 des are in the
plane containing aψ .

As indidicated, the b3 des axis should be oriented along the desired thrust:

Fdes
b3 des = . (30)
||Fdes ||

Next, we want the b2 des axis to be perpendicular to both b1 des and the vector aψ that defines the yaw direction
in the plane (a1 , a2 ) plane:  
cos ψT
aψ =  sin ψT  . (31)
0
This can be accomplished by forming the cross product between b3 des and aψ :

b3 des × aψ
b2 des = (32)
||b3 des × aψ ||

which guarantees that the plane formed by b3 des and the axis b1 des representing the head of the robot contains the
aψ . Finally, b1 des is obtained by cross product and the desired rotation matrix is:

Rdes = [b2 des × b3 des , b2 des , b3 des ] . (33)

T
Next, we need a measure for the error in orientation Rdes R. The following error vector (see [1])
1 T
eR = (Rdes R − RT Rdes )∨ (34)
2

8
is obtained from the matrices by taking the ∨ operator that maps elements of so(3) to R3 . This error vector is zero
when Rdes = R. It is the rotation vector that generates the error in orientation. Consequently, by applying a torque
along its direction it can be decreased, suggesting the control input:
u2 = I(−KR eR − Kω eω ) , (35)
where eω = ω − ω des is the error in angular velocities and KR and Kω are diagonal gains matrices. The desired
angular velocities ω des can be computed from the output of the trajectory generator zT and its derivatives, but setting
them to zero will work for the purpose of this project.
To summarize, the steps to implement the controller are:
1. calculate Fdes from Eq (28), (27), and (26).
2. compute u1 from Eq (29)
3. determine Rdes from Eq (33) and the definitions of bi des .
4. find the error orientation error vector eR from Eq (34) and substitute ω for eω .
5. compute u2 from Eq (35).

4 System Description
4.1 Quadrotor Platform
For this project we will be using the CrazyFlie 2.0 platform made by Bitcraze, shown in Fig. 1. The CrazyFlie has a
motor to motor (diagonal) distance of 92 mm, and a mass of 30 g, including a battery. A microcontroller allows low-
level control and estimation to be done onboard the robot. An onboard IMU provides feedback of angular velocities
and accelerations.

4.2 Software and Integration

Position control and other high level commands are computed in Matlab at 100 Hz and sent to the robot via the
CrazyRadio (2.4GHz). Attitude control is performed onboard using the microcontroller

4.3 Inertial Properties

Since bi are principal axes, the inertia matrix referenced to the center of mass along the bi reference triad, I, is
a diagonal matrix. In practice, the three moments of inertia can be estimated by weighing individual components
of the quadrotor and building a physically accurate model in SolidWorks. The key parameters for the rigid body
dynamics for the CrazyFlie platform are as follows:

(a) mass: m = 0.030 kg;

(b) the distance from the center of mass to the axis of a motor: l = 0.046 m; and
(c) the components of the inertia dyadic using bi in kg m2 :
1.43 × 10−5
 
0 0
[IC ]bi =  0 1.43 × 10−5 0 .
0 0 2.89 × 10−5

9
 Note that these constants will be made available to you as a matlab structure for both simulation and lab session.

5 Project Work
5.1 Simulator
The quadrotor simulator can be found under “projects” on the MEAM620 web page and comes with skeleton code
that has interface documentation. Before implementing your own functions, you should first try running runsim.m
in your matlab. If you see a quadrotor falling from position (0, 0, 0) then the simulator works on your computer and
you may continue with other tasks. This is because the outputs of controller.m are all zeros, thus no thrust is
generated.
When you have the basic version of your trajectory generator and controller done (see below), you will be able
to see the quadrotor flying in the space with proper roll, pitch, and yaw,leaving trails of desired and actual position
behind. The desired position is color-coded blue and the actual position is red. After the simulation finishes, two
plots of position and velocity will be generated to give you an overview of how well your trajectory generator and
controller are doing. Note that you will not be graded on these plots but by the automatic grader.

5.2 Tasks
You will simulate the quadrotor dynamics and control using the matlab simulator. The simulator relies on the
numerical solver ode45, details about this solver can be easily found online. You don’t need to be an expert in
numerical methods, but it would be beneficial if you know the basics of how ode solvers work. Your tasks include:

Figure 4: Helix trajectory (circle.m) Figure 5: Diamond trajectory (diamond.m)

1. Trajectory Generator
You will first implement three trajectory generators to produce a hover, a circle, and a diamond trajectory.
In order to specify the trajectory in the simulator, you will need to change the variable trajhandle in
runsim.m. The handle trajhandle is the name of a function that takes in current time t and current
quadrotor number qn and returns a struct of desired state as a function of time. The struct desired state
has 5 fields, which are pos, vel, acc, yaw and yawdot. For a proper trajectory, you need to specify pos,
vel and acc, whereas yaw and yawdot can be left as 0.

10
 (a) hover.m
Hover at whatever altitude you wish. This is just a test trajectory for you to debug your controller and
will not be used by the auto grader.
(b) circle.m
A helix in the xy plane of radius 5 m centered about the point (0, 0, 0) starting at the point (5, 0, 0). The
z coordinate should start at 0 and end at 2.5. The helix should go counter-clockwise. See Fig. 4.
(c) diamond.m √ √ √ √ √
A ”diamond helix” with corners at (0, 0, 0), (0, 2, 2), (0, 0, 2 2), and (0, − 2, 2) when projected
into the yz plane, and an x coordinate starting at 0 and ending at 1. The quadrotor should start at (0, 0, 0)
and end at (1, 0, 0). See Fig. 5.

2. Controller
You will then implement a controller in file controller.m that makes sure that your quadrotor follows the
desired trajectory. This controller will be used again in the following phases. The controller takes in a cell
struct qd, current time t, current quadrotor number qn and quadrotor parameters params and outputs thrust
F, moments M, desired thrust, roll, pitch and yaw trpy and derivatives of desired roll, pitch and yaw drpy.
Although in this phase of the project you will only need to output thrust F and moments M, you should also
set trpy and drpy. Although they will be ignored in the simulator, you will need to specify them when
you later fly in the lab. The cell array qd contains structs for each quadrotor. This means qd{qn} has all the
information (position, velocity, euler angles and etc.) about quadrotor number qn. Those information can be
accessed via qd{qn}.pos for example.

3. Simulation
Lastly, you need to fill in the appropriate variables for trajhandle and controlhandle in the script
runsim.m for simulating your result.

Hints:

1. Make sure that your controller can also handle arbitrary yaw targets, and test that it works! Beware of yaw
angle wrap-around, consider using Matlab’s angdiff() function.

2. Tune attitude and position gains separately. Which ones should you do first?

5.3 Submission
When you are finished you may submit your code via turnin. The project name for this assignment is titled
“proj1phase1” so the command to submit should be

# turnin -c meam620 -p proj1phase1 -v *

Your turnin submission should contain:

1. A README file detailing anything we should be aware of.

2. Files crazyflie.m, controller.m, diamond.m, circle.m, as well as any other Matlab files you
need to run your code. (That being said, we recommend you to only submit those 4 files)

11
 Shortly after submitting you should receive an e-mail from meam620@[Link] stating that your
submission has been received. You can check on the status of your submission at [Link]
[Link]/˜meam620/monitor/. Once the automated tests finish running, you should receive another e-mail
containing your test results. This report will only tell you whether you passed or failed tests; it will not tell you what
the test inputs were or why your code failed. Your code will be given at most 5 minutes to complete all the tests.
There is no limit on the number of times you can submit your code.
Please do not attempt to determine what the automated tests are or otherwise try to game the automated test sys-
tem. Any attempt to do so will be considered cheating, resulting in a 0 on this assignment and possible disciplinary
action.
You will be graded on successful completion of the code and how quickly and accurately your quadrotor follows
the circle and diamond paths and two other trajectories which will not be released.

References
[1] D. Mellinger and V. Kumar, “Minimum snap trajectory generation and control for quadrotors,” in Proc. of the
IEEE Int. Conf. on Robotics and Automation, Shanghai, China, May 2011.

[2] T. Lee, M. Leok, and N. McClamroch, “Geometric tracking control of a quadrotor uav on SE(3),” in Proc. of the
IEEE Conf. on Decision and Control, 2010.

12