Final Report:

Highly Dynamic Quadcopter Control For Drone Racing

Alvin Shek and Tom Scherlis

Abstract— Drone racing involves navigating a small quad- through a series of drone gates, such that we indirectly
copter through a series of gates at high speeds, often requiring minimize the low-level controller efforts.
highly dynamic flight and careful consideration of the con- We will be using the MIT FlightGoggles simulator [9]
straints presented by the drone gates themselves. In this work,
we present an approach to generate optimal minimum-snap ref- for testing, as it has an implementation of the quadcopter
erence trajectories given only the positions and orientations of dynamic. We use the ROS framework to develop our control
the gates. We also present a controller based on state-feedback software such that it runs in real-time with the simulator. We
linearization and MPC in order to track these trajectories in use ground-truth position for feedback.
real-time.
Video: [Link] II. P ROBLEM S TATEMENT
Code: [Link] The problem is composed of two parts. First, we must
generate a reference trajectory parameterized by time for
I. I NTRODUCTION
the controller, given a list of gate positions and orientations.
Quadcopters are highly dynamic, 6-degree-of-freedom Second, we must design a controller to track this trajectory.
nonlinear systems that often need to follow complex paths. We define two coordinate frames: the fixed world
Their prominence has grown significantly over the past frame (North-East-Down) and the quadcopter body
decade from applications such as video-making and racing. frame. A quadcopter’s state is typically made up of position,
We were inspired by the idea of designing autonomous orientation, linear velocity, and angular velocity respectively:
quadcopters to participate in these races, and possibly other
applications such as defense.
x = [x, y, z, φ, θ, ψ, ẋ, ẏ, ż, p, q, r]T
Typically, nonlinear systems can be approximated as where position and velocity are defined NED world frame.
linear and solved with fast, effective approaches such as p, q, r respectively represent rate of change of angular
Linear Quadratic Regulator(LQR) and Model Predictive velocity about x, y, z. φ, θ, ψ are euler angles about x, y, z
Control (MPC). However, linear approximations only hold respectively.
true close to their set point, and grossly miss the true values
outside this neighborhood. For quadcopters, the small angle Most quadcopters take as input desired desired mass-
approximation assumes the quadcopter will remain near the normalized thrust and angular velocities:
neutral hovering state. While this assumption works well
for applications in cinematography and surveillance, the
u = [T, p, q, r]T
model fails for use cases that require high-speed, dynamic
maneuvering such as drone-racing and target tracking. With this state and control space, our goal is to have the
controller find some optimal control trajectory to take the
In this paper, we focus on the application of drone racing, quadcopter from initial state x0 and track a trajectory xg .
where quadcopters are required to fly time-optimal paths For the other aspect of the project, our challenge was to
through gates that are highly dynamic and violate small convert a set of gate positions and orientations into a spline
angle approximations. Differential flatness is a property in order to determine xg .
that allows a nonlinear system to be separated into a
linear system and a nonlinear transformation mapping For simulation, we use MIT Flightgoggles [9]. Flightgog-
between the two systems [11]. We exploit the differential gles provides a quadcopter dynamics model simulating the
flatness property of quadcopter dynamics to apply feedback rigid body dynamics, rotor forces, and rigid body aerody-
linearization on the nonlinear dynamics, enabling us to solve namic forces.
a linear, transformed system for the optimal controls. We
apply MPC on this flat system and compare its performance III. R ELATED W ORK
to LQR. In [12], differential flatness is also used to apply a linear
controller to drone dynamics. However, rotor drag makes
Tracking typically involves following a series of waypoints up a very significant portion of the dynamics calculations in
defined by position and orientation, but in this paper, we also [12]. In our use case, however, FlightGoggles only models
present a method for generating optimal reference trajectories rigid-body aerodynamics in the simulation, which allows us
to avoid this additional complexity. where kn is a vector of coefficients:
[An , Bn , Cn , Dn , En , Fn ] and Qn is the positive
[8] Presents an approach to quadcopter trajectory genera- semidefinite hermitian matrix.
tion using minimum snap trajectories. We build on this work To define Q, we need to plug dn and tn into (2). Replacing
by adding directional constraints as a means to enforce travel the square with an outer product allows us to compute sn as
direction through an obstacle such as a drone gate, window, a linear function of kn .
or doorway. We also implemented two ways of determining Qn is derived in [13] as:
trajectory durations. We also discuss the application of these  
04×4 04×2
trajectories to the example of drone racing. Qn = (6)
02×4 Wn
IV. M ETHODOLOGY where:
24 ∗ 120 ∗ 21 ∗ d2n
 
A. Optimal Spline Generation 24 ∗ 24 ∗ dn
Wn = (7)
To generate reference trajectories, we build on the min- 24 ∗ 120 ∗ 12 ∗ d2n 120 ∗ 120 ∗ 31 ∗ d3n
imum snap trajectory generation in [8]. Snap, the fourth 24 and 120 correspond to the coefficients that arise from
derivative of position, is minimized as it indirectly minimizes differentiating eq 1. dn is the duration of the segment. This
angular velocity inputs p, q, r. We parameterize trajectories can be generalized for any order polynomial minimizing the
as fifth order piecewise polynomials parameterized by time. integral of any order derivative.
>
We build polynomials for x = ([x, y, z]) . ψ is computed Thus, to solve for the full trajectory we set
separately as a post-processing step to keep the quadcopter x = [k0 ; k1 ; ...kN ] Likewise, H = diag([Q0 , Q1 , ...QN ]).
oriented forward along the trajectory.
2) Waypoint constraints: Waypoint constraints define the
2
xn (t) = An + Bn (t − tn ) + Cn (t − tn ) + Dn (t − tn ) 3 value of a given derivative of the polynomial at time 0.
For example, a zero-order waypoint constraint defines the
+En (t − tn )4 + Fn (t − tn )5
position at the beginning of the segment. A first order
(1)
constraint defines the velocity.
Where n corresponds to the index of the spline segment We can put these constraints into matrix form as follows.
containing t, and tn is the time associated with the start of Start by letting ct (t, o) correspond to the vector correspond-
segment n. ing to the t coefficients of equation 1 at order o. For example,
To minimize snap of a segment, we minimize the integral ct (t, 0) = [t, t2 ..., t5 ]T , whereas ct (t, 4) = [0, 0, 0, 0, t, t2 ]T .
from 0 to dn of the 4th derivative of the polynomial, where Likewise, let cd (o) correspond to the polynomial coefficients
dn is the duration of segment n, equal to tn+1 − tn . To from differentiation at order o. For example, for cd (0) =
simplify things, we focus only on one dimension, x. [1, 1, 1, 1, 1, 1]T whereas cd (4) = [0, 0, 0, 0, 24, 120]T . The
overall coefficient vector c(t, o) is computed as the elemen-
2 twise product of ct and cd . Thus:
dn
d4
Z 
sn = x n (t) dt (2) dn xn
0 dt4 = c(t, n)T kn (8)
dtn
Thus, the total snap s over a given dimension is: Thus, we can easily add each of these constraints as an
N
X equality constraint:
s= sn (3)
c(t, n)T ∗ kn = b (9)
n=0

Where N is the total number of segments on the trajectory. where b is the desired waypoint value. This can be relaxed to
We therefore choose to minimize sT s to minimize snap in a pair of inequality constraints in order to enforce waypoints
all dimensions. with a nonzero acceptable radius.
1) Formulating the QP: To solve using a QP solver, we 3) Continuity Constraints: To enforce continuity along
must put the problem into QP form: the spline, we construct constraints that the values of each
derivative of interest must be equal for connecting segments.
1 In our case, we enforce 2nd order continuity. For each pair
min xT Hx + f T x
2 (4) of segments xn (t) and xn+1 (t), we enforce the following:
s.t. Lx ≤ b
xn (dn ) = xn+1 (0)
First, we frame s as a linear combination of our optimiza- x0n (dn ) = x0n+1 (0) (10)
tion variables, A, B, C, D, E, F . We also assume that tn and
dn are known ahead of time for each segment. Thus, sn for x00n (dn ) = x00n+1 (0)
a single dimension is computed as: These can be formulated as equality constraints in the form
T
sn = kn Qn ∗ kn (5) c(dn , o)T ∗ kn + c(0, o)T ∗ kn+1 = 0 (11)
Where o is the order of the continuity constraint. Linear velocity assumption: dn can be calculated based
Adding these constraints to our QP formulation is on the euclidean distance between the segment’s endpoints
as simple as adding two rows to L in the form and assuming some linear velocity. While not optimal, the
[01×6n , c(dn , o)T , c(0, o)T , 01×N −6(n+1) ], where one results were acceptable and the solution is analytical. Using
of the rows is made negative in order to enforce equality as this assumption allows very fast (≤10ms) computation of an
a pair of inequality constraints. The zero vectors are used optimal spline for around 15 gates. This allows the user to
to pad the coefficient vectors to correspond with the correct recompute the spline online as needed.
pair of k vectors.

4) Direction constraints: Finally, to enforce the direction

of flight at a given point, we introduce directional constraints.
These are useful for constraining the direction of, say, the
velocity vector at a given time without constraining it’s
magnitude. In order to construct these constraints, we need to
optimize all 3 dimensions of the parametric splines together,
since they relate x, y and z. As shown above for continuity
constraints, we can easily constrain the sum of any set
of polynomials at some time t, where each polynomial is
characterized by its kn vector.
In order to constrain direction, we constrain the motion in
all directions orthogonal to the desired direction to zero, or
some radius as desired. These directions are the basis vectors Fig 1: Trajectory Example
n1 and n2 null space of the desired direction vector:
B. Controller
The first step for designing the controller is to map the
c(t, o)T kx,n
 
c(t, o)T ky,n  · n1 = 0 original states and controls to the flat system. The new flat
states can be represented as follows:
c(t, o)T kz,n
(12)
c(t, o)T kx,n
 
c(t, o)T ky,n  · n2 = 0 x = [x, y, z, ẋ, ẏ, ż, ψ]T
c(t, o)T kz,n Linear position and velocity can be controlled using only
An additional constraint can be added to enforce positive roll(θ) and pitch(φ), leaving yaw(ψ) redundant. For this
dot product with the desired direction vector if necessary. reason, roll and pitch can be treated as part of control space
5) Creating trajectories for drone races: For each gate, as desired attitude and handled by the our level attitude
we add 3 constraints: controller. Angular rates are inputs into a typical low-level
quadcopter controller and so they are not needed in the state.
1) Position waypoint at the center of the gate
2) 1st order direction waypoint along the direction of the Control:
gate
3) Continuity constraints up to acceleration on the con-
u = [up T , uψ ]T (14)
necting spline segments
6) Determining Segment durations: So far, the trajectory where up is related to linear acceleration:
optimization has assumed that dn of each segment is known.
We determined two ways to solve for dn :


0
Td
Higher level optimization: A cost function can be con- up = RzT (ψ)RyT (θ)RxT (φ)  0  (15)
structed to weight the total trajectory duration against the m
−1
total snap (or jerk, acceleration, etc) along the trajectory by
reusing the H matrix from eq 4, but generalized to some
   
ẍ 0
order o: ÿ  = up + 0 (16)
N
X z̈ g
xT Hx + α dn (13)
n=0 and uψ is desired yaw rate r:
This nonlinear optimization problem can then be solved
to find the optimal durations and coefficients for some sφ cφ
r=p +r (17)
aggressiveness determined by tuning α. In practice, however, cθ cθ
this process was slow and often did not converge well, so We now have a mapping from the flat states and their
we also use a second method: derivatives to the original states and their control inputs,
which means the system is differentially flat. The dynamics We can set desired yaw ψd = 0 since it is arbitrary, or
of the flat system can be expressed as a general linear system: atan2(y 0 , x0 ) in order to fly straight forwards at all times.

With the above high-level controller providing desired

ẋ = Ax + Bu + bg (18) attitude, we tuned a low-level PD controller to output the
angular rates to achieve this target. FlightGoggles provides
where A, B are defined as:
a high-frequency(around 1kHz) PD controller converting
  angular rates to direct rotor torques. This mimics the inputs
03x3 I3x3 03x1 of most commercial quadcopters. This architecture is shown
A = 03x3 03x3 03x1  (19) below:
01x3 01x3 01x1

 
03x3 03x1
B =  I3x3 03x1  (20)
01x3 1
Fig 2. Control Architecture
and b is a vector that factors in gravity, g, into z-axis
acceleration: We tuned our PD controller by producing step responses
 T of desired attitutes that oscillated +/- some angle. With this,
b= 0 0 0 0 0 1 0 (21) we increased Kp , the proportional gain to minimize time
taken to reach a target, and increased Kd , the derivative
The above system can be fed into common algorithms
gain to eliminate any oscillations.
like LQR and MPC. Now that we have the optimal controls
of the flat system, we need to transform these back to the
We overall chose the following parameters for the MPC
original control space.
Controller:
The low-level controller built into FlightGoggles takes
as input angular rates and mass-normalized thrust. Thrust Q = diag(20, 20, 20, 0.1, 0.1, 0.1, 1) ∈ R7
is simply the norm of the acceleration vector, up , but we R = diag(0.1, 0.1, 0.1, 10) ∈ R4 (26)
perform an additional step of aligning the vector with the 7
S = Q ∗ 10 ∈ R
quadcopter’s current z-axis rather than the desired z-axis.
Given the current normal vector of the quadcopter, n: The Q matrix represents the weights we place on error
of different components of the state, and for our model, we
weighed error in state the most since position-tracking was
Td = max(0, n · up ) (22) the most important.
The R matrix represents the weights we place on control
The max operation ensures that the drone won’t attempt
effort, and for our model, we chose to minimize yaw-rate
to exert negative thrust. Because thrust is nonlinear, we
the most since we don’t want the quadcopter to spin
cannot enforce positive thrust in our controller. This can
unnecessarily.
lead to certain cases where the desired negative thrust is
The S matrix represents the weights placed on final state
unattainable.
error, and we chose to weight this more heavily to place
priority on reaching a target state.
Given our derivation of acceleration vector up from (15),
The MPC controller was run with a 10-step horizon at
we can derive desired roll and pitch:
20Hz, or overall 0.5 second horizon.

 
0 V. E XPERIMENTS
−m
z = Rz (ψ)up = RyT (θ)RxT (φ) 0 (23)
Td We initially developed an LQR controller to test our
1
system, and used acceleration from the reference trajectory
    as a feedforward. In practice, this feedforward becamse
z1 −sθ cφ ineffective when error grew, which only caused error to
z = z2  =  sφ  (24) grow further. We decided to also develop an MPC controller
z3 cθ cφ due to it’s ability to track full trajectories rather than just
stabilize the system. Overall, the MPC controller massively
φd = sin−1 (−z2 ) decreased tracking error and improved the cornering
z1 (25) performance significantly. The below figure shows compares
θd = tan−1 ( ) performance of both controllers in tracking error of the
z3
quadcopter while moving at approximately 25mph: are part of the entire pre-generated spline. The red line
shows the reference trajectory for the same time horizon.
The figure shows how applying feedforward input given a
future time horizon helps the drone handle sharp turns
well. The LQR controller here would simply attempt to
follow the current immediate reference pose using feedback
error from the previous timestep. In the above scenario, the
LQR controller causes the drone to undershoot the turn
completely.
VI. C ONCLUSION
Overall, we first implemented a method for generating
optimal reference trajectories through a series of drone gates,
such that we indirectly minimize the low-level controller
efforts. The generator takes as input a series of drone
gate positions and orientations and a hyperparameter of
aggressiveness α that defines how fast to move through the
Fig 3. LQR Tracking Error
trajectory. Second, we implemented feedback linearization
on the differentially flat quadcopter dynamics and compared
the performance of two controllers, MPC and LQR, on this
system.
Through the optimal spline generation, we learned much
about polynomial splines and how to formulate complex
constraints such that they can be solved with quadratic
programming. We also solidified our understanding of two
popular controllers, LQR and MPC so we can comfortably
apply these in industry. We learned the tradeoffs between
horizon, time discretization granularity, and computation
time in order to run our MPC controller in real time. We
also explored the idea of feedback linearization to apply
Fig 4. MPC Tracking Error effective, but linear-based controllers to a nonlinear system.
The above two figures compare absolute tracking error of Possible future improvements could include the use of a
linear position and velocity of both controllers. The rotor drag model in our simulation and differential flatness
individual plots are ordered in row-major order: x, y, z, model, as well as learning-based techniques which we orig-
ẋ, ẏ, ż. Comparing corresponding axes scales, LQR inally intended to use to improve disturbance rejection and
controller has nearly double the error of MPC. The overall performance. In practice, most drone races involve
oscillating absolute error indicates that both controllers multiple laps, so ILC could be used to modify our reference
oscillate above and below the desired reference trajectories. trajectories in order to reduce repetive tracking errors. The
low-level controller stack could be replaced with adaptive
The below figure demonstrates the MPC controller during controllers to improve performance and reduce the sensitivity
a test run: to tuning. Overall, much of our work focused on the soft-
ware implementation and tuning of our trajectory generation
and control systems. Going forward, we plan to reuse this
work and specifically apply it to planning and control for
an Autonomous Underwater Vehicle (AUV) built by the
undergraduate robotics club.
R EFERENCES
[1] A Review of Control Algorithms for Autonomous Quadrotors:
[Link]
[2] Dynamics modelling and linear control of quadcopter:
[Link]
[3] Quadrotor control: modeling, nonlinear control design, and simulation:
[Link]
%20Francesco%[Link]
[4] Teaching UAVs to Race: End-to-End Regression of Agile Controls in
Fig 5. MPC on a sample track Simulation: [Link]
[5] Quadcopter Dynamics, Simulation, and Control:
In the figure above, the blue arrows show show MPC [Link]
predicted trajectory over the time horizon. The green lines
 [6] OIL: Observational Imitation Learning:
[Link]
[7] Autonomous Driving Motion Planning With Con-
strained Iterative LQR: [Link]
[Link]/stamp/[Link]?tp=&arnumber=8671755
[8] Minimum Snap Trajectory Generation and Control for Quadrotors:
[Link]
[9] FlightGoggles: Photorealistic Sensor Simulation for Perception-
driven Robotics using Photogrammetry and Virtual Reality:
[Link]
[10] A Reduction of Imitation Learning and Structured Prediction to No-
Regret Online Learning: [Link]
[11] Differential Flatness of Mechanical Control Systems: A Catalog
of Prototype Systems [Link] files/2011/4/Ross-
[Link]
[12] Differential Flatness of Quadrotor Dynamics Subject to Ro-
tor Drag for Accurate Tracking of High-Speed Trajectories
[Link]
[13] Minimum snap trajectory planning in MATLAB
[Link] snap trajectory generation