Safe Certificate-Based Maneuvers for Teams of Quadrotors Using
Differential Flatness *
Li Wang, Aaron D. Ames, and Magnus Egerstedt†
Abstract— Safety Barrier Certificates that ensure collision-free
maneuvers for teams of differential flatness-based quadrotors
are presented in this paper. Synthesized with control barrier
functions, the certificates are used to modify the nominal
trajectory in a minimally invasive way to avoid collisions.
The proposed collision avoidance strategy complements existing
flight control and planning algorithms by providing trajectory
modifications with provable safety guarantees. The effectiveness
of this strategy is supported both by the theoretical results and
experimental validation on a team of five quadrotors.
I. INTRODUCTION
Due to recent advances in the design, control, and sensing
technology, teams of quadrotors have become widely used
in aerial robotic platforms, e.g., [7], [12]. Their ability to
hover and fly agilely in three dimensional space makes
quadrotors effective tools for surveillance, delivery, precision
agriculture, search and rescue tasks, see e.g., [18], [24].
When teams of quadrotors are deployed to collaboratively
fulfil these higher level tasks, it is crucial to make sure that
they do not collide with each other. The focus of this paper
is to rectify the nominal flight trajectory, which is generated
with exisiting control and planning algorithms for teams of
quadrotors, in a minimally invasive way to avoid collisions.
Due to the under-actuated and intrinsically unstable nature
of quadrotors, it is often challenging to generate safe trajec-
tories for arbitrary tasks. An artificial potential field approach
was used in [10] to avoid inter-quadrotor collisions, which
can only be qualified to work for the linearized quadrotor
model, i.e., near the hovering state. Additionally, real-time
trajectory generation approach, utilizing the nonlinear dy-
namics together with time-optimal planning algorithm, was
proposed in [7]. However, it is computationally expensive to
accommodate collision avoidance constraints when solving
optimal control problems in real time. One remedy to this
problem is to exploit the differential flatness property of
quadrotors, as introduced in [12], [25], to simplify the
trajectory planning process, while still leveraging the non-
linear dynamics of the quadrotors. This property has been
successfully used for flight trajectory planning in cluttered
environments [11], as well as avoiding static and moving
obstacles [13].
*The work by the first and third authors was sponsored by Grant No.
1544332 from the U.S. National Science Foundation, and the work of the
second author was sponsored by Grant No. 1239055 from the U.S. National
Science Foundation.
†Li Wang and Magnus Egerstedt are with the School of Electrical and
Computer Engineering, Aaron D. Ames is with the School of Mechanical
Engineering and the School of Electrical and Computer Engineering, Geor-
gia Institute of Technology, Atlanta, GA 30332, USA. Email: {liwang,
magnus, ames}@gatech.edu
Nominal Trajectory
Safe Trajectory
Fig. 1: Safe certificate-based flight maneuvers. The nominal
trajectories generated with existing planning algorithms are
modified by the safety barrier certificates to avoid collisions.
The safe regions of quadrotors are modelled as rectangles to
avoid both collisions and air flow disturbances.
In contrast to the aforementioned methods, the goal of
this paper is to modify the trajectories in a provably safe
manner that is compatible with existing control and planning
techniques, while exploiting the nonlinear dynamics (allow-
ing significant deviation from hovering state and large Euler
angles) of teams of quadrotors. To achieve this objective,
all collision-free states of the quadrotors are encoded in a
safe set. Then, Safety Barrier Certificates are synthesized
based on the differential flatness property, and a class of
non-conservative control barrier functions [2], [23], [15]
are used to ensure the forward invariance of the safe set.
Control barrier functions were used in [21], [22] to avoid
static/moving obstacles for a single planar or 3D quadrotor.
And Safety Barrier Certificates have been applied to teams
of ground mobile robots as well for collision avoidance [19],
[20]. As such, in this paper, the certificates are extended to
more complicated multi-quadrotor systems.
The main contributions of this paper are threefold: 1)
Safety Barrier Certificates are developed that provably en-
sure the safety of differential flatness based teams of quadro-
tors; 2) a strategy is developed to modify the nominal
trajectory in a minimally invasive way to avoid collisions,
which is compatible with existing flight control and planning
algorithms; 3) the feasibility of the proposed method is
demonstrated through experimental implementation of the
arXiv:1702.01075v1  [cs.RO]  3 Feb 2017
Safety Barrier Certificates on a team of five palm-sized
quadrotors.
The rest of the paper is organized as follows: the differen-
tial flatness of quadrotor dynamics and the exponential con-
trol barrier functions are briefly revisited in Sections II and
III. The methods to synthesize the Safety Barrier Certificates
and minimally-invasively modify nominal trajectories are
presented in Section IV. In Section V, the proof of feasibility
along with a virtual vehicle parameterization method to deal
with actuator limits are provided. The experimental work is
the topic of Section VI, and the conclusions are given in
Section VII.
II. DIFFERENTIAL FLATNESS OF QUADROTOR DYNAMICS
The quadrotor is a well-modelled dynamical system with
forces and torques generated by four propellers and gravity.
Z−Y −X Euler angles conventions are used to define the roll
(φ), pitch (θ), and yaw (ψ) angles between the quadrotor
body frame and the world coordinate frame. The relevant
coordinate frames and Euler angles are illustrated in Fig. 2.
Fig. 2: Quadrotor coordinate frames. The subscripts w de-
notes the world frame Fw, b for the quadrotor body frame Fb,
and c for an intermediate frame Fc after yaw angle rotation.
ω1 to ω4 are the angular velocities of the four propellers.
The palm-sized quadrotor illustrated is a Crazyflie 2.0 [1]
used in the experiment section.
Assuming that the damping and drag-like effects are
negligible [7], the dynamics of a quadrotor is governed by
the Newton-Euler equation,
m¨r = mgzw + fzzb,
J ˙ωb = τ −ωb ×Jωb,
where r = [x,y,z]T is the position of the center of mass in
the world frame Fw, ωb = [p,q,r]T is the angular velocity
in the body frame Fb, and m and J are the mass and inertia
matrix of the quadrotor respectively. fz is the total thrust and
τ = [τx,τy,τz] is the torque generated by the motors. zw and
zb are the unit vectors in the Z-direction in the world and
body frames, respectively.
The quadrotor dynamics has been shown to be differ-
entially flat in [12], [25], i.e., the states and inputs of
the system can be written in terms of algebraic functions
of appropriately chosen flat outputs and their derivatives.
By using the differential flatness property, trajectories can
be generated by leveraging the nonlinear dynamics of the
quadrotors, rather than simply viewing the system dynamics
as constraints.
As
shown
in
[25],
the
flat
output
for
quadrotor
can be chosen as σ = [x,y,z,ψ]T. The full state ξ =
[x,y,z,vx,vy,vz,ψ,θ,φ, p,q,r]T and input µ = [ fz,τx,τy,τz]T
of the system can be represented algebraically using the
following functions
ξ
=
β(σ, ˙σ, ¨σ, ...σ),
µ
=
γ(σ, ˙σ, ¨σ, ...σ, ....
σ ),
where we refer to [25] for a detailed derivation and formula
of the so-called endogenous transformation (β,γ).
The flight trajectory can be planned in a greatly simplified
flat output space with the differential flatness property. Any
sufficiently smooth trajectory σ(t) ∈C4 in the flat output
space can then be converted analytically back into a feasible
state trajectory ξ(t) and corresponding control input µ(t)
using the endogenous transformation.
Switching from [25] to this paper, in order to generate four
times differentiable flight trajectory, a virtual control input
v ∈R3 is created for the integrator dynamics1. For simplicity
of planning, the yaw angle is always set to zero (ψ(t) = 0),
....r = v,
(1)
where r = σ1:3 = [x,y,z]T ∈R3. The integrator system can be
equivalently written as state space form
˙q =
F∈R4×4
z
}|
{


0
1
0
0
0
0
1
0
0
0
0
1
0
0
0
0

⊗I3×3 ·q
|
{z
}
f(q)
+
G∈R4
z}|{


0
0
0
1

⊗I3×3
|
{z
}
g(q)
·v,
(2)
where q = [rT, ˙rT, ¨rT,...r T]T ∈R12, ⊗is Kronecker product.
Note that since collision avoidance requires simultaneous
response of three degrees of freedom, the trajectory planning
problem here can not be simplified by decoupling three
independent degrees of freedom, as was done in [12], [7].
III. EXPONENTIAL CONTROL BARRIER FUNCTIONS
With the simplified forth-order integrator model for
quadrotors introduced in Section II, Control Barrier Func-
tions (CBF) can be used to ensure collision-free flight
maneuvers. CBFs are Lyapunov-like functions, which can
be used to provably guarantee the forward invariance of a
desired set. When collision-free maneuvers are encoded as
a safe set, CBFs can then be used to ensure that quadrotors
1Note that the trajectory generated with the integrator dynamics (1) is not
necessarily four times differentiable. In addition, the virtual control input
v ∈R3 needs to be Lipschitz continuous for the forth derivative to exist [9],
which will be shown in Section IV.
never escape from the safe set, i.e., they never collide. A
class of non-conservative CBFs [2], [23], which allow the
state to grow inside the safe set as opposed to strictly non-
increasing as shown in Fig. 3, is adopted to synthesize the
Safety Barrier Certificates. Consequently, quadrotor flight
controllers are provided with more freedom to excise desire
maneuvers while remaining safe.
Let the safe set of quadrotor states be defined as
C0 = {q ∈R12 | h(q) ≥0},
(3)
where h : R12 →R is a smooth function.
Safe Region
Unsafe 
Region
Fig. 3: Examples of non-conservative CBFs for set invari-
ance. The diamonds and curves are example initial states and
allowed state trajectories, respectively. The state is allowed
to grow or even approach the boundary from inside the
safe region. Outside the safe region, the state will converge
asymptotically to the safe region, due to CBF constraints.
Position based safety constraints, i.e., constraints defined
over r, are of particular interest for the quadrotor system (2),
so that the quadrotor does not collide with static or moving
obstacles. With a slight abuse of notation, we denote y0(r) =
h(q) as the output of the system, where h(q) only contains
the position variable r. Because the virtual control input v
is the forth derivative of the position variable r, the relative
degree of y0(r) is 4, which means that
y(4)
0 (r) = L4
f h(q)+LgL3
f h(q)v,
(4)
where the Lie derivative formulation stands for
˙h(q) = ∂h(q)
∂q ( f(q)+g(q)v) = Lf h(q)+Lgh(q)v.
Note that due to the high relative degree of y(r), CBFs
in [2], [23] can not be directly applied here. A variation
called Exponential Control Barrier Function (ECBF) [15] can
however be leveraged to ensure the forward invariance of C0.
Definition III.1: Given the dynamical system (2) and a
set C0 defined in (3), the smooth function h : C0 →R
with relative degree of 4 is an Exponential Control Barrier
Function (ECBF) if there exists a vector K ∈R1×4 such that
∀x ∈C0,
sup
u∈U
[L4
f h(q)+LgL3
f h(q)v+Kη] ≥0,
(5)
and h(q(t)) ≥CeF−GKη(q0) ≥0 when h(q0) ≥0, where η =
[h(q),Lf h(q),L2
f h(q),L3
f h(q)]T, C = [1,0,0,0].
The eligible vector K can be obtained by placing the poles
of the closed-loop matrix (F −GK) at p = −[p1, p2,..., p4]T,
where pi > 0 for i = 1,2,3,4. With these pole locations, a
family of outputs yi,i = 1,2,3,4 can be defined as
yi = ( d
dt + p1)◦( d
dt + p2)◦...◦( d
dt + pi)◦h(q),
with y0 = h(q), and the associated family of super level sets
Ci = {q ∈R12 | yi(q) ≥0}.
(6)
Theorem 3.1: Given a safe set C0 in (3) and associated
ECBF h(q) : C0 →R, with initially q0 ∈Ci,i = 0,1,2,3 for
system (2), any Lipschitz continuous controller v(q) ∈Kv(q)
renders C0 forward invariant, where
Kv(q) = {v ∈V | L4
f h(q)+LgL3
f h(q)v+Kvη ≥0},
and η = [h(q),Lf h(q),L2
f h(q),L3
f h(q)]T.
We refer to [15] for the detailed proof of general cases
of this theorem. The basic idea is to design a stabilizing
controller for the system using pole placement, then use
Comparison Lemma [9] to show recursively that Ci, i =
0,1,2,3, is forward invariant.
IV. SAFETY BARRIER CERTIFICATES FOR TEAMS OF
QUADROTORS
Two of the main tools, i.e., differential flatness property of
quadrotor and ECBF for a single quadrotor, for constructing
Safety Barrier Certificates have been revisited in sections II
and III. This section focuses on assembling Safety Barrier
Certificates for teams of quadrotors utilizing these tools.
A. Safety Region Modelled With Super-ellipsoids
Consider
a
team
of
quadrotors
indexed
by
M =
{1,2,3,...,m}, the dynamics of quadrotors are modelled as
forth-order integrators with virtual inputs vi ∈R3,
....r i = vi, i ∈M
(7)
where ri = [xi,yi,zi]T is the position of the center of mass of
quadrotor i. The full state of quadrotor i is represented by
qi = [rT
i , ˙rT
i , ¨rT
i ,...r T
i ]T ∈R12. Let r = [rT
1 ,rT
2 ,...,rT
m]T ∈R3m
and v = [vT
1 ,vT
2 ,...,vT
m]T ∈R3m denote the aggregate position
and virtual control of the team of quadrotors.
In order to ensure the safety of the team of quadrotors, all
pairwise collisions between quadrotors need to be avoided.
In addition, quadrotors can not fly directly over each other
due to the air flow disturbance generated by propellers
as illustrated in Fig. 1. During actual flights, the bottom
quadrotor generally goes unstable or even crash due to the
strong wind blowing from above.
To accommodate these safety requirements, each quadro-
tor is encapsulated with a ‘rectangle shape’ super-ellipsoid2.
2A super-ellipsoid is a solid geometry generally defined with the implicit
function [( x
a)r +( y
b )r]
n
r +( z
c )n ≤1 with r,n ∈R+[3]. r = n = 4 is selected
to approximate a ‘rectangle shape’ here.
Considering any pair of quadrotors (i, j), the pairwise safe
set is defined as
Cij
=
{(qi,qj) | hi j(qi,qj) ≥0},
(8)
hij(qi,qj)
=
(xi −x j)4 +(yi −yj)4 +(zi −zj
c
)4 −D4
s,
where Ds is the safety distance, c is the scaling factor along
the Z axis caused by air flow disturbance. In practice, c
is obtained by flying two quadrotors over each other and
identify the critical separation distance at which the bottom
quadrotor goes unstable. Two quadrotors are considered safe
when two ‘rectangle shape’ super-ellipsoids do not intersect
with each other as shown in Fig. 4.
-2
2
4
-4
-2
2
4
x2 + ( y
3)2 = 1
x4 + ( y
3)4 = 1
|x|∞+ | y
3|∞= 1
(a) XY cross section of a
super-ellipsoid
(b) Two quadrotors mod-
elled with super-ellipsoids
Fig. 4: Safe regions of quadrotors modelled with super-
ellipsoids.
Note that a ‘rectangle shape’ super-ellipsoid is chosen to
appropriately approximate the safe region, since the yaw
angle of quadrotors is trivially set to ψ = 0 and quadrotors
are flying with ‘X’ configuration3. Alternatively, a ‘cylinder
shape’ super-ellipsoid can be picked for arbitrary yaw angles,
¯hij(qi,qj) = [(xi −xj)2 +(yi −yj)2]
n
2 +(zi −z j
c
)n −Dn
s.
Since the pairwise safe set Ci j is defined in terms of
position variables ri,rj, the ECBF candidate hi j(qi,qj) has a
relative degree of 4. (qi,qj) is omitted hereafter for notation
convenience. To ensure the forward invariance of Cij, virtual
controls of quadrotor i and j need to satisfy
....
h ij +K ·[hi j, ˙hi j, ¨hi j,...
h i j]T ≥0,
(9)
where ....
h ij is affine in vi,vj. Thus, the safety barrier con-
straint (9) can be rearranged into a linear constraint on the
virtual control when qi,qj are given,
Ai j(qi,q j)·v ≤bi j(qi,qj),
(10)
where
Aij(qi,qj) = −[0,..., 1
|{z}
ith
,..., −1
|{z}
jth
,...,0] ⊗[4(xi −
xj)3,4(yi −yj)3,4 (zi−z j)3
c4
] ∈R1×3m,
bi j(qi,qj) = K · [hi j, ˙hi j, ¨hi j,...
h i j]T + (24 ˙δ 4 + 144δ ◦˙δ 2 ◦¨δ +
36¯r2 ◦¨δ 2 +48¯r2 ◦˙δ ◦...
δ )·13, δ = [xi −xj,yi −y j, zi−z j
c
],
and ◦stands for elementwise vector product.
3‘X’ configuration is when the quadrotor is configured with two propellers
facing forward as shown in Fig. 2. ‘X’ configuration is often favored due
to improved flight agility and onboard camera view clearance.
The Safety Barrier Certificates are formed by assembling
all the pairwise safety barrier constraints
Ksafe =
(11)
{v ∈R3m | Aij(qi,qj)·v ≤bij(qi,q j),∀i < j,i, j ∈M }.
As long as the virtual control v satisfies the Safety Barrier
Certificates Ksafe and corresponding initial conditions, the
team of quadrotors is guaranteed to be safe by Theorem 3.1.
B. Modifying the Nomimal Trajectory with Safety Barrier
Certificates
It is often difficult to generate provably collision-free
trajectories when planning the nominal trajectory for teams
of quadrotors. Instead, we can first plan the flight trajectory
without considering collisions, and then modify it using
Safety Barrier Certificates in a minimally invasive way to
avoid collisions. Here we consider the case when a nominal
trajectory ˆr(t) = [ˆrT
1 (t), ˆrT
2 (t),..., ˆrT
m(t)] ∈C4 is provided. For
generality, the preplanned nominal trajectory can be gen-
erated by any methods, e.g., optimal control approach [7],
vector field approach[25], or parametrized curves [12], as
long as it is sufficiently smooth, i.e., four times differentiable.
This smooth reference trajectory ˆri(t) is then tracked by a
simulated integrator model using a pole placement controller
with a simulated time step of 0.02s (simulates the 50Hz flight
controller),
ˆvi = ....r i −K ·[ˆri ˙ˆri ¨ˆri
...
ˆr i]T,
(12)
where K is picked to be the same as used for ECBFs in (9)
to trade-off tracking performance and safety enforcement.
To respect the nominal control ˆvi as much as possible,
a quadratic program is used to minimize the difference
between the actual and nominal control, which is what is
meant by minimally invasive trajectory modifications,
v∗= argmin
v
J(v) =
N
∑
i=1
∥vi −ˆvi∥2
s.t.
Aij(qi,q j)v ≤bij(qi,qj),
∀i < j,
∥vi∥∞≤αi,
∀i ∈M ,
(13)
where αi is a bound on the virtual snap control. It can be
observed that the actual controller vi will be the same as ˆvi,
if it is safe. The controller will only be rectified if it violates
the Safety Barrier Certificates, i.e., if it leads to collisions.
The dynamics of the simulated forth-order integrator
system is integrated forward using forward Euler method.
Since (12) is a feedback tracking controller for a fully
controllable integrator system, the numerical stability of the
simple forward Euler method is already very reliable. In
addition, since the nominal trajectory ˆr(t) is four times
differentiable, the nominal controller ˆvi in (12) will be
Lipschitz continuous. According to [14], the controller vi
generated by the QP (13) will be Lipschitz continuous as
well. Thus, the rectified collision-free trajectory r(t) will still
be four times differentiable. Differential flatness property of
quadrotors can still be used to execute the rectified collision-
free trajectory r(t).
The advantages of using Safety Barrier Certificates to
enforce collision-free maneuvers are as follows:
1) they can be used in conjunction with conventional tra-
jectory generation methods to provide provable safety
guarantees;
2) modifications made to ˆr(t) is minimal possible in
the least-squares sense, and the rectified collision-free
trajectory r(t) is still four times differentiable.
V. FEASIBILITY AND PARAMETERIZATION
Section IV provides a systematic approach to modify pre-
planned trajectory in a minimally-invasive and smooth way
using Safety Barrier Certificates. However, it is not clear
whether the quadratic program in (13) is always feasible
or not. In addition, the generated trajectory might require
excessive amount of control effort to execute. This section
provides theoretical guarantees and parameterization method
to address those issues.
A. Proof of Existence of Solution
The following theorem guarantees that a feasible safe
solution to the QP problem (13) always exists.
Theorem 5.1: Given a team of quadrotors indexed by
M with dynamics given in (7), the aggregate admissible
safe control space Ksafe in (11) allowed by Safety Barrier
Certificates is guaranteed to be non-empty.
Proof: If any pair of quadrotors does not collide with
each other, we have ri ̸= rj and Ai j(qi,qj) ̸= 0, ∀i < j. Thus,
hi j(qi,qj) are individually valid control barrier functions.
However, this does not imply that a common controller exists
such that all constraints are satisfied.
In order to prove that a common solution exists, let H ∈
R1×3m be a convex combination of −Ai j(qi,qj) ∈R1×3m,
H = −Σ
i<jαi jAi j(qi,qj),
(14)
where [αij] ∈D, D = {[αi j] | Σ
i<jαi j = 1,αi j ≥0}.
It can be observed that H is the gradient of a convex
function F(r) : R3m →R, where r = [rT
1 ,rT
2 ,...,rT
m]T denotes
the aggregate position of the quadrotors,
F(r) = Σ
i<jαij[(xi −xj)4 +(yi −yj)4 +(zi −zj
c
)4].
(15)
Notice that F(r) is non-negative and has a global minimum
of 0, when αij(ri −rj) = 0,∀i < j. Since all local minimums
of the convex function F(r) are global minimums [4], it can
be deduced that its gradient ∇F(r) = H = 0 if and only if
αi j(ri −rj) = 0,∀i < j.
Since ri ̸= rj (quadrotors do not collide), it can be further
inferred that H = 0 if and only if αi j = 0,∀i < j, which
violates the fact that Σ
i<jαi j = 1. Thus we have shown by
contradiction that H ̸= 0. Using this fact, it is guaranteed
that Hv+ Σ
i<jαijbij(qi,q j) > 0 always has a solution for any
convex combination of −Ai j(qi,qj), i.e.,
min
[αij]∈D sup
v∈R3m{ Σ
i<jαi j[−Ai j(qi,qj)v+bi j(qi,qj)]} > 0.
Notice that D is closed and bounded, and R3m is closed. In
this case, we can exchange min and sup by using the minmax
theorem [17], i.e.,
min
[αij]∈D sup
v∈R3m{ Σ
i<jαij[−Aij(qi,qj)v+bij(qi,qj)]}
=
sup
v∈R3m min
[αij]∈D{ Σ
i<jαij[−Aij(qi,qj)v+bij(qi,qj)]}
=
sup
v∈R3m min
i<j {−Aij(qi,qj)v+bij(qi,qj)} > 0,
which is equivalent to say that a common controller v that
satisfies all the pairwise barrier constraints (10) always exists,
i.e., Ksafe is non-empty.
The idea of control sharing barrier function used in the proof
is similar to control-sharing and merging control Lyapunov
functions introduced in [6].
B. Virtual Vehicle Parameterization
Collision avoidance maneuvers of quadrotors might some-
times lead to significant deviations from reference trajecto-
ries. After the collision hazard disappears, excessive control
effort might be required for the quadrotors to return to the
reference point along the nominal trajectory. To address this
issue, a virtual vehicle parameterization method proposed in
[5] is adopted.
The basic idea of virtual vehicle paramterization is to slow
down or speed up the virtual vehicle (reference point ˆr(t)
on the nominal trajectory) as the tracking error er = ∥r −
ˆr∥increases or decreases. In this particular application, we
use the following virtual time variable to parameterize the
reference point on the nominal trajectory
˙s = e−ks∥er∥2,
(16)
where ks is the virtual parameterization gain. Instead of ˆr(t),
ˆr(s(t)) is fed into the Safety Barrier Certificates rectifier
shown in Fig. 6. Intuitively, the virtual vehicle will slow
down (˙s < 1) when the tracking error is large; it will travel
exactly at the desire speed (˙s = 1) when the tracking error is
zero. This parameterization mechanism is intended to reduce
the amount of control effort when the quadrotor has to
deviate away from the virtual vehicle to avoid collisions.
To demonstrate the effectiveness of virtual vehicle param-
eterization, a simulation of two quadrotors flying pass each
other is presented. The trajectories of two quadrotors are
illustrated in Fig. 5.
In this example, the collision avoidance manuever requires
a maximum pitch angle of 70◦and a maximum thrust of
2.8 times hovering thrust without parameterization (ks = 0)
as shown in Fig. 7. In contrast, a maximum pitch angle of
25◦and a maximum thrust of 1.2 times hovering thrust are
needed with parameterization (ks = 100). In both cases, the
desired task is accomplished within 6s.
As proved in section V-A, the QP in (13) will always
generate a feasible solution. However, the required control
effort to avoid collisions might be excessive. In this circum-
stance, virtual vehicle parameterization method can be used
to reduces instantaneous control efforts. By increasing the
Reference
Trajectory
Generator
Safety Barrier 
Certi cates
Recti er
Di eretial Flatness
Tranformation
Reference 
trajectory
Safe 
trajectory
Feasibility
Checker
Full State Control
Virtual Vehicle
Parameterization
Quadrotor
Plant
Yes
No
Fig. 6: Flowchart of safe trajectory generation strategy.
−1.5
−1.0
−0.5
0.0
0.5
1.0
1.5
X(m)
−1.0
−0.5
0.0
0.5
1.0
Y (m)
Fig. 5: Trajectories of two quadrotors flying pass each other
plotted in X-Y plane. Control efforts for performing this task
are illustrated in Fig. 7.
virtual parameterization gain ks significantly, the quadrotors
will be granted considerably more time to perform collision
avoidance manuevers. Thus, the parameterization mechanism
will generate a feasible trajectory that satisfies given actuator
constraints.
C. Overview of Safe Trajectory Generation Strategy
An overview of the safe trajectory generation strategy is
summarized in Fig. 6. A smooth reference trajectory ˆr(t) ∈
C4 is first fed into the safety barrier certificates rectifier,
where the QP controller (13) is used to enforce collision-free
flight maneuvers. The rectified smooth safe trajectory r(t) ∈
C4 is then transformed into quadrotor states and controls
using the differential flatness property. The full states and
controls are checked to ensure that actuator limits are not
violated. Otherwise, the reference trajectory is parameterized
ˆr(s(t)) ∈C4 and fed into the safety barrier certificates rectifier
again. This process can be repeated until the virtual vehicle
parameterization strategy yields appropriate flight trajectory
that respects both safety and actuator constraints. In the end,
the generated feasible safe trajectory is sent to execute on
0
1
2
3
4
5
6
−80
−40
0
40
80
Pitch(◦)
ks = 0
ks = 100
0
1
2
3
4
5
6
0.0
0.3
0.6
Error(m)
ks = 0
ks = 100
0
1
2
3
4
5
6
t(s)
0.2
0.4
0.6
0.8
1.0
Thrust(N)
ks = 0
ks = 100
Fig. 7: Comparisons of control efforts for the quadrotor
using (ks = 100) or without using (ks = 0) virtual vehicle
parameterization.
the team of quadrotors.
VI. EXPERIMENT
The Safety Barrier Certificates are implemented on a
team of five palm-sized quadrotors (Crazyflie 2.0). All com-
munication channels between different devices and control
programs are coordinated by a ROS server. The real-time
positions and Euler angles of quadrotors are tracked by the
Optitrack motion capture system with an update rate of 50Hz.
The 50Hz quadrotor motion controller is developed based on
the ROS driver for Crazyflie 2.0 built by ACTLab at USC [8].
To ensure stable trajectory tracking behavior, Euler angles
and Euler angle rates generated with the differential flatness
property are sent to quadrotors as control commands. The
overall quadrotor control diagram is shown in Fig. 8.
A. Showcase: Flying Through a Static Formation
In the first experiment, one of the quadrotor (Q5) is
commanded to fly through a static formation consisting of
four quadrotors (Q1−Q4) as shown in Fig. 9.
ROS Server
Optitrack
System
Controller
+
Barrier Certi cates
Positions
Euler Angles
Controls
Quad States
Controls 
States
Controls 
States
Controls 
States
Controls 
States
Controls 
States
Fig. 8: Quadrotor control system diagram
Fig. 9: Snapshot from a experiment of quad Q5 flying
through a static formation consisting of four quads Q1−Q4.
The video of this experiment is available online [16].
Quadrotors (Q1−Q4) are designed to hover at four places
with reference trajectories given as
ˆr1(t) =


0.25
0
−0.8

, ˆr2(t) =


0
0.25
−0.8

,
ˆr3(t) =


−0.25
0
−0.8

, ˆr4(t) =


0
−0.25
−0.8

.
Another quadrotor (Q5) is designed to go from p0 =
[0.6,−0.6,−0.8]T to p1 = [−0.6,0.6,−0.8]T. The nominal
trajectory can be generated as
ˆr5(t) = BezierInterp(p0, p1),
where the BezierInterp function stands for the Bezier curve
interpolation algorithm between two waypoints.
The safety distance between quadrotors is specified as
Ds = 25cm to account for the tracking frames and controller
tracking errors. Intuitively, quadrotors will collide with each
other if nominal trajectories are directly executed. During
the experiment, the Safety Barrier Certificates are applied
to modify the nominal trajectories in a minimally invasive
way to avoid collisions. As demonstrated in Fig. 10, Q5
successfully navigated through the static formation of four
quadrotors within 3.4s.
−0.8
−0.4
0.0
0.4
0.8
−0.8
−0.4
0.0
0.4
0.8
Q1
Q2
Q3
Q4
Q5
(a) Agents at 8.8s
−0.8
−0.4
0.0
0.4
0.8
−0.8
−0.4
0.0
0.4
0.8
Q1
Q2
Q3
Q4
Q5
(b) Agents at 9.6s
−0.8
−0.4
0.0
0.4
0.8
−0.8
−0.4
0.0
0.4
0.8
Q1
Q2
Q3
Q4
Q5
(c) Agents at 11.0s
−0.8
−0.4
0.0
0.4
0.8
−0.8
−0.4
0.0
0.4
0.8
Q1
Q2
Q3
Q4
Q5
(d) Agents at 12.2s
Fig. 10: Experimental data of the team of quadrotors plotted
in the X-Y plane. The tail of each quadrotor illustrates its
trajectory in the past 0.8s.
B. Showcase: Flying Through a Spinning Formation
Similar to the previous experiment, the quadrotor Q5 is
commanded to fly through a spinning formation as illustrated
in Fig. 11.
Fig. 11: Snapshot from a experiment of quad Q5 flying
through a spinning formation consisting of four quads Q1−
Q4. The video of this experiment is available online [16].
The reference trajectories of quadrotors are designed as
ˆr1(t) =


0.45sin( π
2t −π
2 )
0.45cos( π
2t −π
2 )
−0.8

, ˆr2(t) =


0.45cos( π
2t)
0.45sin( π
2t)
−0.8

,
ˆr3(t) =


0.45cos( π
2t + π
2 )
0.45sin( π
2t + π
2 )
−0.8

, ˆr4(t) =


0.45cos( π
2t +π)
0.45sin( π
2t +π)
−0.8

,
ˆr5(t) = BezierInterp(


−0.9
−0.9
−0.8

,


0.9
0.9
−0.8

).
As shown in Fig. 12, Q5 successfully navigated through
the spinning formation with minimal impact on the other
four quadrotors by applying the Safety Barrier Certificates.
−0.8
−0.4
0.0
0.4
0.8
−0.8
−0.4
0.0
0.4
0.8
Q1
Q2
Q3
Q4
Q5
(a) Agents at 24.3s
−0.8
−0.4
0.0
0.4
0.8
−0.8
−0.4
0.0
0.4
0.8
Q1
Q2
Q3
Q4
Q5
(b) Agents at 25.3s
s
−0.8
−0.4
0.0
0.4
0.8
−0.8
−0.4
0.0
0.4
0.8
Q1
Q2
Q3
Q4
Q5
(c) Agents at 26.7s
−0.8
−0.4
0.0
0.4
0.8
−0.8
−0.4
0.0
0.4
0.8
Q1
Q2
Q3
Q4
Q5
(d) Agents at 27.5s
Fig. 12: Experimental data of the team of quadrotors plotted
in the X-Y plane. The tail of each quadrotor illustrates its
trajectory in the past 0.6s.
Experimental results provided in this section demonstrate
that the Safety Barrier Certificates can save flight plan-
ners the hassle of considering collision avoidance when
designing the higher level multi-robot coordination algo-
rithm. This strategy can be easily used in conjunction with
other complicated motion planning strategies, e.g., optimal
control algorithms, temporal/spatial assignment algorithms,
to provide desired safety guarantees. The minimally invasive
enforcement of the Safety Barrier Certificates ensures that
desired controller will not be rectified unless truly necessary.
VII. CONCLUSIONS
A flight trajectory modification strategy is presented in this
paper to ensure collision-free manuevers for teams of differ-
ential flatness based quadrotors. To do this, nominal flight
trajectories, which are generated with existing control and
planning algorithms, are modified in a minimally invasive
way using the Safety Barrier Certificates to avoid collisions.
The proof of existence of feasible controller and virtual
vehicle parameterization method to accommodate actuator
limits are presented. In the end, experimental implementation
of the Safety Barrier Certificates on a team of five quadrotors
validates the effectiveness of the proposed strategy. To for-
mally prove that the virtural vehicle parameterization method
results in feasible flight trajectories subject to actuator limits
will be an interesting future direction.
REFERENCES
[1] Bitcraze AB. https://www.bitcraze.io/.
[2] A. D. Ames, J. W. Grizzle, and P. Tabuada. Control Barrier Func-
tion Based Quadratic Programs with Application to Adaptive Cruise
Control. In Decision and Control (CDC), 2014 IEEE 53rd Annual
Conference on, pages 6271–6278, Dec 2014.
[3] A. H. Barr. Superquadrics and angle-preserving transformations. IEEE
Computer graphics and Applications, 1(1):11–23, 1981.
[4] S. Boyd and L. Vandenberghe.
Convex Optimization.
Cambridge
University Press, 2004.
[5] M. Egerstedt, X. Hu, and A. Stotsky. Control of mobile platforms
using a virtual vehicle approach.
IEEE Transactions on Automatic
Control, 46(11):1777–1782, 2001.
[6] S. Grammatico, F. Blanchini, and A. Caiti.
Control-sharing and
merging control lyapunov functions. IEEE Transactions on Automatic
Control, 59(1):107–119, 2014.
[7] M. Hehn and R. DAndrea.
Real-time trajectory generation for
quadrocopters. IEEE Transactions on Robotics, 31(4):877–892, 2015.
[8] W. Hoenig, C. Milanes, L. Scaria, T. Phan, M. Bolas, and N. Ayanian.
Mixed reality for robotics. In IEEE/RSJ Intl Conf. Intelligent Robots
and Systems, pages 5382 – 5387, Hamburg, Germany, Sept 2015.
[9] H. K. Khalil. Nonlinear systems. Prentice hall, third edition, 2002.
[10] Y. Kuriki and T. Namerikawa. Consensus-based cooperative formation
control with collision avoidance for a multi-uav system.
In 2014
American Control Conference, pages 2077–2082. IEEE, 2014.
[11] B. Landry. Planning and control for quadrotor flight through cluttered
environments. Master’s thesis, Massachusetts Institute of Technology,
2015.
[12] D. Mellinger and V. Kumar. Minimum snap trajectory generation and
control for quadrotors.
In Robotics and Automation (ICRA), 2011
IEEE International Conference on, pages 2520–2525. IEEE, 2011.
[13] D. Mellinger, A. Kushleyev, and V. Kumar. Mixed-integer quadratic
program trajectory generation for heterogeneous quadrotor teams. In
Robotics and Automation (ICRA), 2012 IEEE International Conference
on, pages 477–483. IEEE, 2012.
[14] B. Morris, M. J. Powell, and A. D. Ames. Sufficient conditions for the
lipschitz continuity of qp-based multi-objective control of humanoid
robots.
In 52nd IEEE Conference on Decision and Control, pages
2920–2926. IEEE, 2013.
[15] Q. Nguyen and K. Sreenath.
Exponential control barrier functions
for enforcing high relative-degree safety-critical constraints. In 2016
American Control Conference (ACC), pages 322–328. IEEE, 2016.
[16] Online. Barrier certificates for safe quad swarm. https://www.
youtube.com/watch?v=rK9oyqccMJw, 2016.
[17] R. T. Rockafellar. Convex analysis. Princeton university press, 2015.
[18] M. Valenti, D. Dale, J. How, and J. Vian. Mission health management
for 24/7 persistent surveillance operations. In AIAA Guidance, Control
and Navigation Conference, Myrtle Beach, SC, 2007.
[19] L. Wang, A. D. Ames, and M. Egerstedt. Safety Barrier Certificates
for Heterogeneous Multi-robot Systems. In 2016 American Control
Conference (ACC), pages 5213–5218, July 2016.
[20] L. Wang, A. D. Ames, and M. Egerstedt. Multi-objective compositions
for collision-free connectivity maintenance in teams of mobile robots.
In Decisions and Control Conference, 2016, to appear.
[21] G. Wu and K. Sreenath. Safety-critical control of a planar quadrotor. In
2016 American Control Conference (ACC), pages 2252–2258. IEEE,
2016.
[22] G. Wu and K. Sreenath.
Safety-critical control of a 3d quadrotor
with range-limited sensing. In ASME Dynamics Systems and Control
Conference (DSCC), to appear, 2016.
[23] X. Xu, P. Tabuada, J. W. Grizzle, and A. D. Ames. Robustness of
control barrier functions for safety critical control. In Analysis and
Design of Hybrid Systems, 2015 IFAC Conference on. IEEE, Oct 2015.
[24] C. Zhang and J. M. Kovacs. The application of small unmanned aerial
systems for precision agriculture: a review.
Precision agriculture,
13(6):693–712, 2012.
[25] D. Zhou and M. Schwager.
Vector field following for quadrotors
using differential flatness. In 2014 IEEE International Conference on
Robotics and Automation (ICRA), pages 6567–6572. IEEE, 2014.