Starting from:
$30
Project 1 Phase 1 Modeling and Control of a Quadrotor
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 no-reply@gradescope.com 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
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 no-reply@gradescope.com 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
1 file (925.5KB)
