Project 1 Phase 1

Modeling and Control of a Quadrotor

1 Introduction

In this project you will put your knowledge of quadrotor dynamics to work by developing algorithms for control. A

good controller is the backbone of any quadrotor autonomy stack and thus it is crucial that you understand the theory

and implementation in this project. You will be reusing the code throughout this course; during the lab session you

will use it to fly a real, physical quadrotor! All code written in this project will be in Python. Refer to the instructions

on Piazza for more information about which version to use and getting set up.

ω ω1 2

ω3 ω4

r

b3

b1

b2

body

frame

᭶

a1

a2

a3

inertial

frame

᭵

b3

b1

b2

body

frame

a3

a’1

a’2

ᶪ

ᶪ

ᶰ

a’1

a’’2

a’3

ᶰ

ᶮ

ᶮ

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 are parallel to the principal axes.

More formally, the coordinates of a vector x that is expressed in A as x =

P

i

Axiai and in B as x =

P

i

Bxibi are

transformed into each other by the rotation matrix ARB

and translation vector ATB

:

x

A = R

A

B x

B + T

A

B . (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

dt

bi = ω × bi

. (2)

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 robot’s 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:

R

A

B =

cos(ψ) cos(θ) − sin(φ) sin(ψ) sin(θ) − cos(φ) sin(ψ) cos(ψ) sin(θ) + cos(θ) sin(φ) sin(ψ)

cos(θ) sin(ψ) + cos(ψ) sin(φ) sin(θ) cos(φ) cos(ψ) sin(ψ) sin(θ) − cos(ψ) cos(θ) sin(φ)

− cos(φ) sin(θ) sin(φ) cos(φ) cos(θ)

. (4)

The angular velocity and Euler angle velocities are related by:

p

q

r

=

cos(θ) 0 − cos(φ) sin(θ)

0 1 sin(φ)

sin(θ) 0 cos(φ) cos(θ)

φ˙

˙θ

ψ˙

(5)

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 ω

2

i

. (6)

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

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ω

2

i

. (7)

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(ω

des

i − ω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, ω

des

i

, are limited to a minimum and maximum value determined through experimentation.

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, ω

des

i

.

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

:

m¨r = Fg +

X

i

Fi

. (8)

In the coordinates of the inertial frame A this reads:

m ¨r A =

0

0

−mg

+ R

A

B

0

0

F1 + F2 + F3 + F4

. (9)

where ARB

is the rotation matrix used in Eq (1) constructed via Eq (4). If the robot is tilted, the above equation 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:

u1 =

X

4

i=1

Fi

. (10)

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:

L˙ =

X

i

BL˙

ibi +

X

i

BLib˙

i =

X

i

BL˙

ibi +

X

i

ω ×

BLibi =

X

i

BL˙

ibi + ω × L . (12)

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:

I

p˙

q˙

r˙

=

l(F2 − F4)

l(F3 − F1)

M1 − M2 + M3 − M4

−

p

q

r

× I

p

q

r

. (14)

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:

I

p˙

q˙

r˙

=

0 l 0 −l

−l 0 l 0

γ −γ γ −γ

F1

F2

F3

F4

−

p

q

r

× I

p

q

r

. (15)

where γ =

kM

kF

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:

u2 =

0 l 0 −l

−l 0 l 0

γ −γ γ −γ

F1

F2

F3

F4

. (16)

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:

m¨r =

0

0

−mg

+ R

0

0

u1

(17)

I

p˙

q˙

r˙

= u2 −

p

q

r

× I

p

q

r

, (18)

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 z

des(t) consisting of target position rT (t) and target

yaw ψT (t):

z

des(t) = ?

rT (t)

ψT (t)

?

(19)

and its first and second derivatives z˙

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 quadrotor.

3. The attitude of the quadrotor can be controlled at a much smaller time scale than the position. This “backstepping” 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 instantaneously. This further implies that it is possible to directly command the acceleration r

des. Figure 2 shows how

trajectory generator, position, and attitude controller play together.

5

trajectory

planner

position

controller

attitude

controller

motor

controller

rigid body

dynamics

des , ᵱ

ᬒ1

2

,

Რdes

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

des

i − r¨i,T ) + kd,i( ˙ri − r˙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 r˙i are provided by the state

estimation system, allowing for the commanded acceleration r¨

des

i

to be calculated:

r¨

des

i = ¨ri,T − kd,i( ˙ri − r˙i,T ) − kp,i(ri − ri,T ) . (21)

Now the attitude of the quadrotor must be controlled such that it will generate ¨r

des. For this, the linearized

version of (17) is used:

r¨

des

1 = g(θ

des cos ψT + φ

des sin ψT ) (22a)

r¨

des

2 = g(θ

des sin ψT − φ

des cos ψT ) (22b)

r¨

des

3 =

1

m

u1 − g. (22c)

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

p˙

q˙

r˙

= 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:

u2 = I

−kp,φ(φ − φ

des) − kd,φ (p − p

des)

−kp,θ(θ − θ

des) − kd,θ (q − q

des)

−kp,ψ(ψ − ψT ) − kd,ψ (r − ψ˙

T )

, (25)

6

where the desired roll and pitch velocities p

des and q

des can be computed from the equations of motion and the

specified trajectory [1], but in practice can be set to zero. 2

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 quadrotor’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 = ¨rT − Kd(r˙ − r˙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 ¨rdes. We first determine the

input u1. By rearranging Eq (17) one obtains:

m¨r

des +

0

0

mg

= u1R

0

0

1

. (27)

Note that the lhs of (27) is just the total commanded force F

des (including gravity):

F

des = m¨r

des +

0

0

mg

. (28)

On the right hand side is u1, multiplied with the quadrotor’s axis b3 = R[0, 0, 1]T

, expressed in the inertial frame.

We obtain the input u1 by projecting F

des onto b3:

u1 = b

T

3 F

des

. (29)

To compute u2, we observe that a quadrotor can only produce thrust along its b3 axis. It makes sense to align

b3 with F

des, and align b1 to match the desired yaw ψT . Please refer to Fig. 3. In the following steps we find a triad

2On the RHS of Eq. (25) there should also be the trajectory angular accelerations, analogous to r¨i,T in Eqn (20). By exploiting “Differential Flatness” [1] the trajectory angular accelerations can be computed from the equations of motion and z

des(t) and its derivatives. However,

in practice these terms can be neglected.

7

ᶪ

a3

a2

a1

aᶪ

᪶

des

b3

des

b2

des

b1

des

Figure 3: Geometry based attitude control. First b3

des is aligned along F

des. 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ψ.

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

.

As indidicated, the b3

des axis should be oriented along the desired thrust:

b3

des =

F

des

||F

des||

. (30)

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:

aψ =

cos ψT

sin ψT

0

. (31)

This can be accomplished by forming the cross product between b3

des and aψ:

b2

des =

b3

des × aψ

||b3

des × aψ||

(32)

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:

R

des = [b2

des × b3

des

, b2

des

, b3

des] . (33)

Next, we need a measure for the error in orientation RdesT

R. The following error vector (see [1])

eR =

1

2

(R

desT

R − R

T R

des)

∨

(34)

8

is obtained from the matrices by taking the ∨ operator that maps elements of so(3) to R

3

. 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(−KReR − 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 F

des 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 lowlevel 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 Python and sent to the robot via the CrazyRadio

(2.4GHz). Attitude control is performed onboard using the microcontroller, though you will control it in simulation.

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

:

[IC]

bi =

1.43 × 10−5 0 0

0 1.43 × 10−5 0

0 0 2.89 × 10−5

.

9

Note that these constants have already been incorporated into the provided Python simulator.

5 Project Work

5.1 Code Packet

We have provided you with a project packet that contains all the code you need to complete this assignment. The

flightsim directory contains the quadrotor simulator; you will not need to make any changes to the enclosed files

but you may want to poke around to see how it works under the hood. Inside the proj1 1 directory you will find

two folders: code contains files with code stubs for you to complete for this assignment and util contains a test

suite for judging the performance of your controller offline.

The file code/sandbox.py will run the simulator with your controller on the trajectory specified within and

produce plots of the state of the quadrotor over time useful for performance tuning as well as an animation of the

result. Additionally, a test script (util/test.py) and a few test cases specified as util/*.json files will help

you judge the performance of you implementation offline and gain familiarity with the testing system (more on this

later). Additionally, at the root of the packet is an important file called setup.py. This file specifies the additional

python packages necessary to run the simulator and can be used to automatically install those dependencies in your

python environment. For more of this see the piazza post on Python and Pycharm.

Before implementing your own functions, you should first try running code/sandbox.py. It should produce

a number of plots displaying useful information about the state of the quadrotor over time as well as a short animation

showing the quadrotor in action. Because you haven’t implemented your controller yet you will get an error in the

console and the animation will show your quadrotor falling out of the sky. This is because the outputs of the update

function in se3 control.py are all zeros and thus no thrust is generated.

5.2 Tasks

For this project you will complete the implementation of a controller in se3 control.py and a waypoint trajectory generator in waypoint traj.py:

1. waypoint traj.py

You will first implement a waypoint trajectory class. This class holds a set of desired waypoints for the quadrotor to visit and subsequently must prescribe a state for each instant in time that if followed by the quadrotor

will ensure it visits each point. In particular, you will populate the dict flat output containing position (x),

velocity (x dot), acceleration (x ddot), jerk (x dddot), snap (x ddddot), yaw angle (yaw), and angular

rate (yaw dot). For more information see the doc strings for each function in waypoint traj.py.

2. se3 control.py

Next you will implement a controller that drives the quadrotor towards the state specified by the waypoint

trajectory class. As mentioned before, this controller will be used throughout project 1. The controller

is implemented as a class that holds the physical parameters of the robot and produces motor speed commands (cmd motor speeds) given the current time and state of the quadrotor as well as the desired state

(flat output). For more information see the doc strings for each function in se3 control.py.

Although in this phase of the project you will only need to output motor speeds, you should also set the

corresponding commanded thrust (cmd thrust), moment (cmd moment), and attitude (cmd q). Although

they will be ignored in the simulator, they can be useful for debugging purposes and will be used when you

fly in the lab.

10

Hints:

1. sandbox.py ships with a hover trajectory generator built in; use it to first implement and tune your controller

then move on to filling out waypoint traj.py.

2. Make sure that your controller can also handle arbitrary yaw targets, and test that it works! Beware of yaw

angle wrap-around.

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

5.3 Submission

When you are finished, submit your code via Gradescope which can be accessed via the course Canvas page. You

must submit your completed waypoint traj.py and se3 control.py files along with any helper files necessary to run them; do not submit the simulator code or any other files used to test locally. Note that only the

Python modules and their dependencies specified in setup.py will be available in the gradescope autograder. Your

waypoint trajectory generator and controller will be tested against a total of six tests, three of which are identical to

the ones you are given in the project packet for offline testing.

Shortly after submitting you should receive an e-mail from [email protected] stating that your

submission has been received. Once the automated tests finish running the results will be available on the Gradescope

submission page. 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 system. 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 whether your quadrotor 1) visited each waypoint 2) came to rest at the terminal waypoint

and 3) how quickly you accomplished 1,2.

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.

11