Optimization-Based Collision Avoidance
Xiaojing Zhang∗a , Alexander Liniger∗b , Francesco Borrellia
a Model

Predictive Control Laboratory, Department of Mechanical Engineering, University of California, Berkeley, USA
Control Laboratory, Department of Information Technology and Electrical Engineering, ETH Zurich, Switzerland

arXiv:1711.03449v2 [math.OC] 8 Dec 2017

b Automatic

Abstract
This paper presents a novel method for reformulating non-differentiable collision avoidance constraints into
smooth nonlinear constraints using strong duality of convex optimization. We focus on a controlled object
whose goal is to avoid obstacles while moving in an n-dimensional space. The proposed reformulation does
not introduce approximations, and applies to general obstacles and controlled objects that can be represented
in an n-dimensional space as the finite union of convex sets. Furthermore, we connect our results with the
notion of signed distance, which is widely used in traditional trajectory generation algorithms. Our method
can be used in generic navigation and trajectory planning tasks, and the smoothness property allows the use
of general-purpose gradient- and Hessian-based optimization algorithms. Finally, in case a collision cannot
be avoided, our framework allows us to find “least-intrusive” trajectories, measured in terms of penetration.
We demonstrate the efficacy of our framework on a quadcopter navigation and automated parking problem,
and our numerical experiments suggest that the proposed methods enable real-time optimization-based
trajectory planning problems in tight environments. Source code of our implementation is provided at
https://github.com/XiaojingGeorgeZhang/OBCA.
Keywords: obstacle avoidance, collision avoidance, path planning, autonomous parking, trajectory generation, navigation in tight environments

1. Introduction
Maneuvering autonomous systems in an environment with obstacles is a challenging problem that arises
in a number of practical applications including robotic manipulators and trajectory planning for autonomous
systems such as self-driving cars and quadcopters. In almost all of those applications, a fundamental feature
is the system’s ability to avoid collision with obstacles which are, for example, humans operating in the same
area, other autonomous systems, or static objects such as walls.
Optimization-based trajectory planning algorithms such as Model Predictive Control (MPC) have received significant attention recently, ranging from (unmanned) aircraft to robots to autonomous cars [1,
✩ E-mail

DRAFT

addresses: {xiaojing.zhang, fborrelli}@berkeley.edu, liniger@control.ee.ethz.ch
December 11, 2017

2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12]. This can be attributed to the increase in computational resources, the
availability of robust numerical algorithms for solving optimization problems, as well as MPC’s ability to
systematically encode system dynamics and constraints inside its formulation. In this paper, we do not
review, or compare, the proposed optimization-based collision avoidance with alternative approaches such
as those based on dynamic programming [13], reachability analysis [14, 15, 16], graph search [17, 18, 19],
sampling based methods [20, 21, 22, 23] or interpolating curves [24]. Indeed, collision avoidance problems are
known to be NP-hard in general [25], and all practical methods constitute some sort of “heuristics”, whose
performance depends on the specific problem and configuration at hand. We refer the interested reader
to [26, 27, 28, 29, 30] for a comprehensive review on existing trajectory planning and obstacle avoidance
algorithms.
One fundamental challenge in optimization-based trajectory planning is the appropriate formulation of
collision avoidance constraints, which are known to be non-convex and computationally difficult to handle
in general. While a number of formulations have been proposed for collision avoidance constraints, they
are typically limited by one of the following features: (i) The collision avoidance constraints are approximated through linear constraint, and it is difficult to establish the approximation error [9]; (ii) Existing
formulations focus on point-mass controlled objects, and are not applicable to full-dimensional objects; (iii)
When the obstacles are polyhedral, then the collision avoidance constraints are often reformulated using
integer variables [31]. While this reformulation is attractive for linear systems with convex constraints since
in this case a mixed-integer convex optimization problem can be solved, integer variables should generally
be avoided when dealing with nonlinear systems when designing real-time controllers for robotic systems.
In this paper, we focus on a controlled object that moves in a general n-dimensional space while avoiding
obstacles, and propose a novel approach for modeling obstacle avoidance constraints that overcomes the
aforementioned limitations. Specifically, the contributions of this paper can be summarized as follows:
• We show that if the controlled object and the obstacles are described by convex sets such as polytopes
or ellipsoids (or can be decomposed into a finite union of such convex sets), then the collision avoidance constraints can be exactly and non-conservatively reformulated as a set of smooth non-convex
constraints. This is achieved by appropriately reformulating the distance-function between two convex
sets using strong duality of convex optimization.
• We provide a second formulation for collision avoidance based on the notion of signed distance, which
characterizes not only the distance between two objects but also their penetration. This reformulation
allows us to compute “least-intrusive” trajectories in case collisions cannot be avoided. Numerical
studies indicate that, compared to the first reformulation, this reformulation has the additional benefit
of increased numerical stability when it comes to solving the (non-convex) optimization problems.
• Both our formulations allows the incorporation of system dynamics and input limits in the form
2

of constraints. As a result, the trajectories generated from our obstacle avoidance algorithms are
kinodynamically feasible, and can be easily tracked by some low-level controller.
• We demonstrate the efficacy of the proposed obstacle avoidance reformulations on a quadcopter trajectory planning problem and autonomous parking application, where the controlled vehicles must
navigate in tight environments. We show that while the distance reformulation is generally computationally more efficient than the signed-distance approach, the latter is numerically more stable and
able to find solutions even in challenging circumstances. The source code for both applications is
provided at https://github.com/XiaojingGeorgeZhang/OBCA.
This paper is organized as follows: Section 2 introduces the problem setup. Section 3 presents the
collision avoidance constraint reformulation for the special case when the controlled object is a point mass.
The results are then extended to full-dimensional controlled objects in Section 4. Numerical experiments
demonstrating the efficacy of the proposed method are given in Sections 5 and 6, and conclusions are drawn
in Section 7. The Appendix contains auxiliary results needed to prove the main results of the paper.
Optimization-Based Related Work
A large body of work exists on the topic of obstacle avoidance. In the following, we review optimizationbased approaches, and refer the interested reader to [26, 27, 28, 29, 30] for a comprehensive overview of the
entire field. The main idea in optimization-based collision avoidance is to express the problem as an optimal
control problem, and then solve it using numerical optimization techniques. Earlier approaches reformulate
the collision avoidance using “artificial potential fields”, which results in an unconstrained optimization
problem. More recently, methods based on constrained optimization has attracted attention, due to its ability
to explicitly formulate collision avoidance through constraints [1, 4, 6, 9]. Broadly speaking, constrained
optimization-based collision-avoidance algorithms can be divided into two cases based on the modeling of
the controlled object: point-mass models and full-dimensional objects. Due to its conceptual simplicity, the
vast majority of literature focuses on collision avoidance for point-mass models, and consider the shape of
the controlled object by inflating the obstacles. The obstacles are generally assumed to be either polytopes
or ellipsoids. For polyhedral obstacles, disjunctive programming can be used to ensure collision avoidance,
which is often reformulated as a mixed-integer optimization problem [1, 4, 31]. In case of ellipsoidal obstacles,
the collision avoidance constraints can be formulated as a smooth non-convex constraint [32, 33], and the
resulting optimization problem can be solved using generic non-linear programming solvers.
The case of full-dimensional controlled objects has, to be best of the author’s knowledge, not been widely
studied in the context of optimization-based methods, with the exception of [9, 34]. In [34], the authors
model the controlled object through its vertices and, under the assumption that all involved object are
rectangles, ensure collision avoidance by keeping all vertices of the controlled object outside the obstacle. A
3

more general way of handling collision avoidance for full-dimensional controlled object has been proposed
in [9] by using the notion of signed distance. Unfortunately, it is generally difficult to obtain an explicit
representation of the signed distance; as a result, it is typically linearized or approximated by employing
sequential optimization techniques [9].
The approach most closely related to our formulation is probably the work of [6], where the authors
propose a smooth reformulation of the collision avoidance constraint for point-mass controlled objects and
polyhedral obstacles. However, our approach differs from [6] as (i) our approach easily generalizes to fulldimensional controlled objects, and (ii) we are, based on the notion of penetration, also able to compute
least-intrusive trajectories in case collisions cannot be avoided.
Notation
Given a proper cone K ⊂ Rl and two vectors a, b ∈ Rl , then a K b is equivalent to (b − a) ∈ K.
Furthermore, if K = Rl+ is the standard cone, then Rl+ denotes element-wise inequality, and we write ≤
instead. Moreover, k · k∗ is the dual norm of k · k, and K∗ ⊂ Rl is the dual cone of K. The “space” occupied
by the controlled object (e.g., a drone, vehicle, or robot in general) is denoted as E ⊂ Rn ; similarly, the
space occupied by the obstacles is denoted as O ⊂ Rn .
2. Problem Description
2.1. Dynamics, Objective and Constraints
We assume that the dynamics of the controlled object takes the form
xk+1 = f (xk , uk ),

(1)

where xk ∈ Rnx is the state of the controlled object at time step k given an initial state x0 = xS , uk ∈ Rnu
is the control input, and f : Rnx × Rnu → Rnx describes the dynamics of the system. In most cases, the
state xk contains information such as the position pk ∈ Rn and angles θk ∈ Rn of the controlled object, as
well the velocities ṗk and angular rates θ̇k . In this paper, we assume that no disturbance is present, and
that the system is subject to input and state constraints of the form
h(xk , uk ) ≤ 0,

(2)

where h : Rnx × Rnu → Rnh , nh is the number of constraints, and the inequality in (2) is interpreted
element-wise. Our goal is to find a control sequence, over a horizon N , which allows the controlled object to
navigate from the initial state xS to its final state xF ∈ Rnx , while optimizing some objective function J =
PN
nx
nu
→ R is a stage cost, and avoiding M ≥ 1 obstacles O(1) , O(2) , . . . , O(M) ⊂
k=0 ℓ(xk , uk ), where ℓ : R ×R

Rn . Throughout this paper, we assume that the functions f (·, ·), h(·, ·) and ℓ(·, ·) are smooth. Smoothness
is assumed for simplicity, although all forthcoming results apply equally to cases when those functions are
twice continuously differentiable.
4

2.2. Obstacle and Controlled Object Modeling
Given the state xk , we denote by E(xk ) ⊂ Rn the “space” occupied by the controlled object at time k,
which we assume is a subset of Rn . The collision avoidance constraint at time k is now given by1 .
E(xk ) ∩ O(m) = ∅,

∀m = 1, . . . , M.

(3)

Constraint (3) is non-differentiable in general, e.g., when the obstacles are polytopic [6, 9]. In this paper,
we will remodel (3) in such a way that both continuity and differentiability are preserved. To this end,
we assume that the obstacles O(m) are convex compact sets with non-empty relative interior2 , and can be
represented as
O(m) = {y ∈ Rn : A(m) y K b(m) },

(4)

where A(m) ∈ Rl×n , b(m) ∈ Rl , and K ⊂ Rl is a closed convex pointed cone with non-empty interior.
Representation (4) is entirely generic since any compact convex set admits a conic representation of the
form (4) [35, p.15]. In particular, polyhedral obstacles can be represented as (4) by choosing K = Rl+ ; in
this case K corresponds to the well-known element-wise inequality ≤. Likewise, ellipsoidal obstacles can
be represented by letting K be the second-order cone, see [36]. To simplify the upcoming exposition, the
same cone K is assumed for all obstacles; the extension to obstacle-specific cones K(m) is straight-forward.
In this paper, we will consider controlled objects E(xk ) that are modeled as point-masses as well as
full-dimensional objects. In the former case, E(xk ) simply extracts the position pk from the state xk , i.e.,
E(xk ) = pk .

(5a)

In the latter case, we will model the controlled object E(xk ) as the rotation and translation of an “initial”
convex set B ⊂ Rn , i.e.,
E(xk ) = R(xk )B + t(xk ),

B := {y : Gy K̄ g},

(5b)

where R : Rnx → Rn×n is an (orthogonal) rotation matrix and t : Rnx → Rn is the translation vector. The
matrices (G, g) ∈ Rh×n × Rh and the cone K̄, which we assume is closed, convex and pointed, define the
shape of our initial (compact) set B. Often, the rotation matrix R(·) depends on the angles θk of the
controlled object, while the translation vector t(·) depends on the position pk of the controlled object. We
assume throughout that the functions R(·) and t(·) are smooth.
1 In

this paper, we only consider collision avoidance constraints that are associated with the position and geometric shape of

the controlled object, which are typically defined by its position pk and angles θk . This is not a restriction of the theory as the
forthcoming approaches can be easily generalized to collision avoidance involving other states, but done to simplify exposition
of the material.
2 Non-convex obstacles can often be approximated/decomposed as the union of convex obstacles

5

2.3. Optimal Control Problem with Collision Avoidance
By combining (1)–(3), the constrained finite-horizon optimal control problem with collision avoidance
constraint is given by
min
x,u

N
X

ℓ(xk , uk )

k=0

s.t.

x0 = xS , xN +1 = xF


xk+1 = f (xk , uk ), 


k = 0, . . . , N,
h(xk , uk ) ≤ 0,


 m = 1, . . . , M,
E(x ) ∩ O(m) = ∅, 

(6)

k

where E(xk ) is either given by (5a) (point-mass model) or (5b) (full-dimensional set), x := [x0 , x1 , . . . , xN +1 ]
is the collection of all states, and u := [u0 , u1 , . . . , uN ] is the collection of all inputs. A key difficulty in solving
problem (6), even for linear systems with convex objective function and convex state/input constraints, is
the presence of the collision-avoidance constraints E(xk ) ∩ O(m) = ∅, which in general are non-convex and
non-differentiable [6, 9]. In the following, we present two novel approaches for modeling collision avoidance
constraints that preserve continuity and differentiability, and are amendable for use with existing off-the-shelf
gradient- and Hessian-based optimization algorithms.
2.4. Collision Avoidance
A popular way of formulating collision avoidance, which holds for generic sets E(x) ⊂ Rn and O ⊂ Rn ,
is based on the signed distance [9]
sd(E(x), O) := dist(E(x), O) − pen(E(x), O),

(7)

where dist(·, ·) and pen(·, ·) are the distance and penetration function, and are defined as
dist(E(x), O) := inf {ktk : E(x) + t ∈ O},

(8a)

pen(E(x), O) := inf {ktk : E(x) + t 6∈ O}.

(8b)

t

t

For convex sets E(x) and O, the signed distance is positive if E(x) and O do not intersect, zero if their
intersection has empty interior, and negative if their intersection has non-empty interior. Therefore, collision
avoidance can be ensured by requiring sd(E(x), O) > 0. Unfortunately, directly enforcing sd(E(x), O) > 0
inside the optimization problem (6) is generally difficult since it is non-convex and non-differentiable in
general [9]. Furthermore, for optimization algorithms to be numerically efficient, they require an explicit
representation of the functions they are dealing with, in this case sd(·, ·). This, however, is difficult to obtain
in practice since sd(·, ·) itself is the solution of the optimization problems (8a) and (8b). As a result, existing
algorithm approximate (7) through local linearization [9], for which it is difficult to establish bounds on
approximation errors.
6

In the following, we propose two reformulation techniques for obstacles avoidance that overcome the
issues of non-differentaibility and does not require an explicit representation of the signed distance. We
begin with the case when the point-mass model in Section 3, and treat the general case of a full-dimensional
controlled object in Section 4.

3. Collision Avoidance for Point-Mass Models
In this section, we first present a smooth reformulation for (6) when E(xk ) = pk (Section 3.1), and then
extend the approach in Section 3.2 to generate minimum-penetration trajectories in case collisions cannot
be avoided. To simplify notation, the time indices k are omitted in the remainder of this section.
3.1. Collision-Free Trajectory Generation
Proposition 1. Assume that the obstacle O and the controlled object are given as in (4) and (5a), respectively, and let dmin ≥ 0 be a desired safety margin between the controlled object and the obstacle. Then we
have:
dist(E(x), O) > dmin ⇐⇒ ∃λ K∗ 0 : (A p − b)⊤ λ > dmin , kA⊤ λk∗ ≤ 1.

(9)

Proof. It follows from (4) and (8a) that dist(E(x), O) = mint {ktk : A(E(x) + t) K b}. Following [36, p.401],

its dual problem is given by maxλ (AE(x) − b)⊤ λ : kA⊤ λk∗ ≤ 1, λ K∗ 0 , where k · k∗ is the dual norm
associated to k·k and K∗ is the dual cone of K. Since O is assumed to have non-empty relative interior, strong

duality holds, and dist(E(x), O) = maxλ (A E(x) − b)⊤ λ : kA⊤ λk∗ ≤ 1, λ K∗ 0 . Hence, for any non-

negative scalar dmin , dist(E(x), O) > dmin is satisfied if, and only if, there exists λ K∗ 0 : (A E(x) − b)⊤ λ >
dmin , kA⊤ λk∗ ≤ 1. The desired result follows from identity (5a).

Intuitively speaking, any variable λ satisfying the right-hand-side of (9) is a certificate verifying the
condition dist(E(x), O) > dmin . Since E(x) ∩ O = ∅ is equivalent to dist(E(x), O) > 0, the optimal control
problem (6) for the point-mass model (5a) is given by
min
x,u,λ

N
X

ℓ(xk , uk )

k=0

s.t.

x0 = xS , xN +1 = xF ,


xk+1 = f (xk , uk ), h(xk , uk ) ≤ 0, 


k = 0, . . . , N,
(m)
(m)
(m) ⊤
(A
pk − b ) λk > 0,

m
= 1, . . . , M,


⊤ (m)
(m)
kA(m) λ k ≤ 1, λ
 ∗0 
k

∗

k

K

(m)

where pk is the position of the controlled object at time k, λk
O

(m)

(10)

is the dual variable associated with obstacle

at time step k, and the optimization is performed over the states x, the inputs u and the dual variables
7

(1)

(m)

(1)

(m)

λ = [λ0 , . . . , λ0 , λ1 , . . . , λN ]. We emphasize that (10) is an exact reformulation of (6) and that the
optimal trajectory x∗ = [x∗0 , x∗1 , . . . , x∗N ] obtained by solving (6) is kinodynamically feasible.
Remark 1. Without further assumptions on the norm k · k and the cone K, the last two constraints in (10)
are not guaranteed to be smooth, a property that many general-purpose non-linear optimization algorithms
require3 . Fortunately, it turns out that these constraints are smooth for the practically relevant cases of
k · k being the Euclidean distance and K either the standard cone or the second-order cone, which allows us
to model polyhedral and ellipsoidal obstacles. In these case, and under the assumption that the functions
f (·, ·), h(·, ·) and ℓ(·, ·) are smooth, (10) is a smooth nonlinear optimization problem that is amendable to
general-purpose non-linear optimization algorithms such as IPOPT [37]. Without going into details, we
point out that smoothness is retained when k · k = k · kp is a general p-norm, with p ∈ (1, ∞), and K is the
cartesian product of p-order cones Kp := {(s, z) : kzkp ≤ s}, with p ∈ (1, ∞).
While reformulation (10) can be used for obstacle avoidance, it is limited to finding collision-free trajectories. In case collisions cannot be avoided, however, the above formulation is not able to find “least-intrusive”
trajectories. Intuitively speaking, this is because (10) is based on the notion of distance, and the distance
between two overlapping objects (as is in the case of collision), is always zero. From a practical point of
view, this implies that slack variables cannot be included in the constraints of (9), because the optimal
control problem is not able to distinguish between “severe” and “less severe” colliding trajectories. Finally,
we point out that in practice, it is always desirable to include slack variables since (local) infeasibilities in
non-convex optimization problem are known to cause severe numerical difficulties. In the following, we show
how this limitation can be overcome by considering the notion of penetration.
3.2. Minimum-Penetration Trajectory Generation
In this section, we consider the design of minimum-penetration trajectories for cases when collision cannot
be avoided and the goal is to find a “least-intrusive” trajectory. Following the literature [38, 39], we measure
“intrusion” in terms of penetration as defined in (8b).
Proposition 2. Assume that the obstacle O and controlled object are given as in (4) and (5a), respectively,
and let pmax ≥ 0 be a desired maximum penetration of the controlled object and the obstacle. Then we have:
pen(E(x), O) < pmax ⇐⇒ ∃λ K∗ 0 : (b − A p)⊤ λ < pmax , kA⊤ λk∗ = 1.

(11)

Proof. The proof, along with auxiliary lemmas, is given in Appendix A.
3 Strictly

speaking, these solvers often require the cost function and constraints to be twice continuously differentiable only.

Smoothness is assumed in this paper for the sake of simplicity.

8

Proposition 2 resembles Proposition 1 with the difference that the convex inequality constraint kA⊤ λk∗ ≤
1 is replaced with the non-convex equality constraint kA⊤ λk∗ = 1. In the following, we will see that
Propositions 1 and 2 can be combined to represent the signed distance as defined in (7).
Theorem 1. Assume that the obstacle O and the controlled object are given as in (4) and (5a), respectively.
Then, for any d ∈ R, we have:
sd(E(x), O) > d ⇐⇒ ∃λ K∗ 0 : (A p − b)⊤ λ > d, kA⊤ λk∗ = 1.

(12)

Proof. By definition, sd(E(x), O) = dist(E(x), O) if E(x) ∩ O = ∅, and sd(E(x), O) = −pen(E(x), O) if
E(x) ∩ O 6= ∅. Let E(x) ∩ O 6= ∅, in which case (12) follows directly from (11). If E(x) ∩ O = ∅, then
we have from (9) that sd(E(x), O) > d is equivalent to ∃λ K∗ 0 : (A p − b)⊤ λ > d, kA⊤ λk∗ ≤ 1. Due to
homogeneity with respect to λ, if the previous condition is satisfied, then there always exists another dual
multiplier λ′ K∗ 0 such that (A p − b)⊤ λ′ > d, kA⊤ λ′ k∗ = 1. This concludes the proof.
Reformulation (12) is similar to reformulation (9), with the difference that (12) holds for all d ∈ R,
while (9) only holds for d ≥ 0. The “price” we pay for this generalization is that the convex constraint
kA⊤ λk∗ ≤ 1 is turned into the non-convex equality constraint kA⊤ λk∗ = 1 which, as we will see later on,
generally results in longer compuation times. Nevertheless, Theorem 1 allows us to compute trajectories of
least penetration whenever collision cannot be avoided by solving the following soft-constrained minimumpenetration problem:

min
x,u,s,λ

N
X

k=0

s.t.

"

ℓ(xk , uk ) + κ ·

M
X

(m)
sk

m=1

#

x0 = x(0), xN +1 = xF ,
xk+1 = f (xk , uk ), h(xk , uk ) ≤ 0,
(m)

(A(m) pk − b(m) )⊤ λk
⊤ (m)

(m)

kA(m) λk k∗ = 1, sk

(m)

> −sk ,
(m)

≥ 0, λk

object O

(m)

(13)
k = 0, . . . , N,



 m = 1, . . . , M
K∗ 0 

(m)

where pk is the position of the controlled object at time k, sk







∈ R+ is the slack variable associated to the

at time step k, and κ ≥ 0 is a weight factor that keeps the slack variable as close to zero as

possible. Without going into details, we point out that the weight κ should be chosen “big enough” such that
the slack variables only become active when the original problem is infeasible, i.e., when obstacle avoidance
is not possible [40]. Notice that a positive slack variable implies a colliding trajectory, where the penetration
(m)

depth is given by sk . We close this section by pointing out that if, a priori, it is known that a collision-free
trajectory can be generated, then formulation (10) should be given preference over formulation (13) because
⊤ (m)

the former has fewer decision variables, and because the constraint kA(m) λk k∗ ≤ 1 is convex, which
generally leads to improved solution times. Smoothness of (13) is ensured if k · k is the Euclidean distance,
and K is either the standard cone or the second-order cone, see Remark 1 for details.
9

4. Collision Avoidance for Full-Dimension Controlled Objects
In the previous section, we provided a framework for generating collision-free and minimum-penetration
trajectories for controlled objects that are described by point-mass models. While such models can be used
to generate trajectories for “ball-shaped” controlled objects, done by setting the minimum distance dmin
equal to the radius of the controlled object (see Section 5 for such an example), it can be restrictive in other
cases. For example, modeling a car in a parking lot as a Euclidean ball is generally a crude approximation,
and might prevent the car from finding a parking spot. To alleviate this issue, we show in this section how
the results of Section 3 can be extended to full-dimensional controlled objects.
4.1. Collision-Free Trajectory Generation
Similar to Section 3, we begin by first reformulating the distance function, which will allow us to generate
collision-free trajectories:
Proposition 3. Assume that the controlled object and the obstacle are given as in (5b) and (4), respectively,
and let dmin ≥ 0 be a desired safety margin. Then we have:
dist(E(x), O) > dmin
⇐⇒ ∃λ K∗ 0, µ K̄∗ 0 : − g ⊤ µ + (At(x) − b)⊤ λ > dmin , G⊤ µ + R(x)⊤ A⊤ λ = 0, kA⊤ λk∗ ≤ 1.

(14)

Proof. Recall that dist(E(x), O) = mine,o {ke − ok : Ao K b, e ∈ E(x)} = mine′ ,o {kR(x)e′ + t(x)− ok : Ao K
b, Ge′ K̄ g}, where the last equality follows from (5b). The dual of this minimization problem is given
by maxλ,µ {−g ⊤ µ + (At(x) − b)⊤ λ : G⊤ µ + R(x)⊤ A⊤ λ = 0, kA⊤ λk∗ ≤ 1, λ K∗ 0, µ K̄⋆ 0}, see e.g.,
[36, Section 8.2] for the derivation, where k · k∗ is the dual norm, and K∗ and K̄⋆ are the dual cones of
K and K̄, respectively. Since O and B are assumed to have non-empty relative interior, strong duality
holds, and dist(E(x), O) > dmin ⇔ maxλ,µ {−g ⊤ µ + (At(x) − b)⊤ λ : G⊤ µ + R(x)⊤ A⊤ λ = 0, kA⊤ λk∗ ≤
1, λ K∗ 0, µ K̄⋆ 0} > dmin ⇔ ∃λ K∗ 0, µ K̄∗ 0 : − g ⊤ µ + (At(x) − b)⊤ λ > dmin , G⊤ µ + R(x)⊤ A⊤ λ =
0, kA⊤ λk∗ ≤ 1.
Compared to Proposition 1, we see that full-dimensional controlled objects require the introduction of
the additional dual variables µ(m) , one for each obstacle O(m) . By setting dmin = 0, we obtain now the
following reformulation of (6) for full-dimensional objects:
min
x,u,λ,µ

N
X

ℓ(xk , uk )

k=0

s.t.

x0 = x(0), xN +1 = xF ,
xk+1 = f (xk , uk ), h(xk , uk ) ≤ 0,
(m)
(m)
−g ⊤ µk + (A(m) t(xk ) − b(m) )⊤ λk > 0,
⊤ (m)
(m)
G⊤ µk + R(xk )⊤ A(m) λk = 0,
⊤ (m)
(m)
(m)
kA(m) λk k∗ ≤ 1, λk K∗ 0, µk K̄∗

10










k = 0, . . . , N,

m
= 1, . . . , M,





0, 

(15)

(m)

where λk

(m)

and µk

(m)

collection of all λk

are the dual variables associated with the obstacle O(m) at step k, λ and µ are the
(m)

and µk , respectively, and the optimization is performed over (x, u, λ, µ). Notice that

(15) is an exact reformulation of (6). Smoothness of (15) is ensured if k · k is the Euclidean distance, and
K and K̄ are either the standard cone or the second-order cone, see also Remark 1.
Similar to the point-mass case in Section 3.1, the optimal control problem (15) is able to generate
collision-free trajectories, but unable to find “least-intrusive” trajectories in case collision-free trajectories
cannot be found. This is addressed next.
4.2. Minimum-Penetration Trajectory Generation
We overcome the above limitation by considering once more the notion of penetration. We begin with
the following result:
Proposition 4. Assume that the obstacles and controlled object are given as in (4) and (5b), respectively,
and let pmax ≥ 0 be a maximal penetration depth. Then we have:
pen(E(x), O) < pmax
⇐⇒ ∃λ K∗ 0, µ K̄∗ 0 : g ⊤ µ + (b − A t(x))⊤ λ < pmax , G⊤ µ + R(x)⊤ A⊤ λ = 0, kA⊤ λk∗ = 1.

(16)

Proof. Notice that pen(E(x), O) = pen(0, O − E(x)), where O − E(x) := {o − e : o ∈ O, e ∈ E(x)} is
the Minkowski difference [38]. Furthermore, it follows from the proof of Proposition 2 that pen(0, O −
E(x)) = inf {z : kzk∗ =1} {maxy∈O−E(x) {y ⊤ z}}. Using strong duality of convex optimization, we can dualize
the inner maximization problem as maxo∈O,e∈E(x) {z ⊤(o − e)} = maxo∈O,e′ ∈B {z ⊤(o − R(x)e′ − t(x))} =
minλ,µ {b⊤ λ + g ⊤ µ − z ⊤ t(x) : A⊤ λ = z, G⊤ µ = −R⊤ z : λ K ∗ 0, µ K̄∗ 0}. Hence, pen(0, O − E(x)) =
inf z,λ,µ {b⊤ λ + g ⊤ µ − z ⊤ t(x) : A⊤ λ = z, G⊤ µ = −R⊤ z, kzk∗ = 1}. Eliminating the z-variable using the
first equality constraint and following the steps of the proof of Proposition 2 gives the desired result.
Similar to the point-mass case, the reformulation of penetration differs from the reformulation of distance
in that the (convex) inequality constraint kA⊤ λk∗ ≤ 1 becomes a non-convex equality constraint kA⊤ λk∗ =
1. The following theorem shows that Propositions 3 and 4 can be combined to represent the signed distance
function.
Theorem 2. Assume that the obstacles and controlled object are given as in (4) and (5b), respectively.
Then, for any d ∈ R, we have:
sd(E(x), O) > d
⇐⇒ ∃λ K∗ 0, µ K̄∗ 0 : − g ⊤ µ + (At(x) − b)⊤ λ > d, G⊤ µ + R(x)⊤ A⊤ λ = 0, kA⊤ λk∗ = 1.

(17)

Proof. By definition, sd(E(x), O) = dist(E(x), O) if E(x) ∩ O = ∅, and sd(E(x), O) = −pen(E(x), O) if
E(x) ∩ O 6= ∅. Consider now E(x) ∩ O 6= ∅, in which case (17) follows directly from (16). Assume now
11

that E(x) ∩ O = ∅; then we have from (14) that sd(E(x), O) > d is equivalent to ∃λ K∗ 0, µ K̄∗
0 : − g ⊤ µ + (At(x) − b)⊤ λ > d, G⊤ µ + R(x)⊤ A⊤ λ = 0, kA⊤ λk∗ ≤ 1. Due to homogeneity with respect
to λ and µ, if the previous condition is satisfied, then there also exists a λ′ K∗ 0 and µ′ K̄ ∗ 0 such that
−g ⊤ µ + (At(x) − b)⊤ λ > d, G⊤ µ + R(x)⊤ A⊤ λ = 0, kA⊤ λk∗ = 1. This concludes the proof.
Theorem 2 now allows us to formulate the following soft-constrained minimum-penetration optimal
control problem
min
x,u,s,λ,µ

N
X

k=0

s.t.

"

ℓ(xk , uk ) + κ ·

M
X

(m)
sk

m=1

#

x0 = xS , xN +1 = xF ,

xk+1 = f (xk , uk ), h(xk , uk ) ≤ 0,
(m)
(m)
(m)
−g ⊤ µk + (A(m) t(xk ) − b(m) )⊤ λk > −sk ,
⊤ (m)
(m)
G⊤ µk + R(xk )⊤ A(m) λk = 0,
⊤ (m)
(m)
(m)
(m)
kA(m) λk k∗ = 1, sk ≥ 0, λk K∗ 0, µk
(m)

where sk

K̄∗








 k = 0, . . . , N,


m = 1, . . . , M.




0 

(18)

∈ R+ is the slack variables associated to the object O(m) at time step k, and κ ≥ 0 is a weight

factor that keeps the slack variable as small as possible. Smoothness of (18) is ensured if k·k is the Euclidean
distance, and K and K̄ are either the standard cone or the second-order cone, see also Remark 1.
In the following sections, we illustrate our obstacle avoidance formulation on two applications: a quadcopter path planning problem where the point-mass formulation is used (Section 5), and an automated
parking problem where the full-dimensional obstacle avoidance problem formulation is used (Section 6).

5. Example 1: Quadcopter Path Planning
In this section, we illustrate reformulations (10) and (13) on a quadcopter navigation problem, where
the quadcopter must find a path from one end of the room to the other end, while avoiding a low-hanging
wall and passing through a small window hole, see Figure 1 for an illustration of the setup.
5.1. Environment and Obstacle Modeling
The size of the room is 10.5 × 10.5 × 5.5m, and we see from Figure 1 that the direct path between the
start and end position is blocked by two obstacles. The first obstacle, a low-hanging wall, blocks the entire
upper part of the room, and can only be passed from below. The second obstacle, another wall, blocks the
entire room, but has a small window through which the quadcopter must pass. We approximate the shape
of the quadcopter by a (Euclidean) sphere of radius 0.25m. In the framework of (10) and (13), the shape of
the quadcopter can be taken into account by requiring a safety distance of dmin = 0.25m.
The first wall, which can only be passed from below, is placed at X = 2m, and the passage below is
0.85m high. The second wall is placed at X = 7m, and the window is of size 1 × 1m and is placed in the
12

Figure 1:

Setup of quadcopter example together with a (locally optimal) point-to-point trajectory.

The start position is behind the blue wall at X = 1, and the end position in front of red wall at
X = 9. The quadcopter needs to fly below the blue wall and pass through a window in the red wall,
see https://youtu.be/7WLNJhaHcoQ for an animation. The black circles illustrate the safety distance
dmin = 0.25m which is used to model the shape of the quadcopter.
middle of the room at a height of Z = 2.5m. Finally the depth of both walls is 0.5m. This obstacle formation
can be formally formulated using five axis-aligned rectangles, where the first obstacle is represented by one
such rectangle and the window can be modeled as the union of four rectangles, see Figure 1. The collision
avoidance constraints with respect to the four outer walls of the room are achieved by appropriately upperand lower-bounding the (X, Y, Z)−coordinates of the quadcopter.
5.2. Quadcopter Model
We consider the standard quadcopter model as used in [41], which is derived by finding the equation of
motion of the center of gravity (CoG). In this model, X, Y, Z denote the position of the CoG in the world
frame, and we use the Z-X-Y Euler angles do describe the rotation of the quadcopter, where φ is the pitch
angle, θ is the roll angle, and ψ is the yaw angle. The rotation matrix that translates from the world to the
body frame, which is defined with respect to the CoG, is hence given by


cψ cθ − sφ sψ sθ −cφ sψ cψ sθ + cφ sψ sθ




RW B = sψ cθ + sφ cψ sθ cφ cψ sψ sθ − sφ cψ cθ  ,


−cφ sθ
sφ
cφ cθ

where sφ := sin(φ) and cφ := cos(φ). The accelerations of the CoG can be derived by considering the sum

of the forces produced by the four rotors Fi which point in positive z-direction in the body frame, and the
13

gravity force which acts on the negative z-direction in the world frame, resulting in the following equation
of motion,

  


Ẍ
0
0

  



  


m  Ÿ  =  0  + RW B 
,
0
P
  


4
F
Z̈
−mg
i
i=1

(19a)

where m is the mass of the quadcopter and Ẍ, Ÿ and Z̈ are the second time derivatives of X, Y and Z,
respectively. The attitude dynamics of the quadcopter is derived in the body rates p, q, and r which are
related to the Euler angles through the following rotation matrix,
 
  
p
cθ
0
sθ
φ̇
 
  
 
  
 θ̇  =  sθ tφ
1 −cθ tφ  q  .
 
  
r
−sθ /cφ 0 cθ /cφ
ψ̇

(19b)

Fi , as well as the corresponding moments Mi and has the following form,
  
  
 
ṗ
L(F2 − F4 )
p
p
  
  
 
  
  
 
I q̇  = 
 − q  × I q  ,
L(F3 − F1 )
  
  
 
ṙ
M1 − M2 + M3 − M4
r
r

(19c)

The body rates itself are given by the following equation of motion, which is driven by the four rotor forces

where I is the inertia matrix which in our case is diagonal and L is the distance from the CoG to the rotor.
The rotor forces and moments depend quadratically on the motor speed ωi , which are the control inputs
and are defined as follows,
Fi = kF ωi2 ,

Mi = kM ωi2 ,

(19d)

where, kF and kM are constants depending on the rotor blades. Hence, the state of the quadcopter is
x = [X, Y, Z, φ, θ, ψ, Ẋ, Ẏ , Ż, p, q, r] and the inputs are the four rotor speeds u = [ω1 , ω2 , ω3 , ω4 ].
The parameters of the model, as well as the bounds on the inputs, are taken from [42], which corresponds
to a quadcopter which weighs 0.5kg and is approximately half a meter in diameter. Furthermore, the angles,
velocities and body rates are all limited within bounds, and the dynamics can be brought into the form (1)
using a (forward) Euler discretization, such that xk+1 = xk + Toptf˜(xk , uk ), where Topt is the sampling time,
and f˜(·, ·) is the continuous-time dynamics that can be obtained from (19a)–(19d).
5.3. Cost function
Our control objective is to fly the quadcopter as fast as possible, while avoiding excessive control inputs.
We combine these competing goals as a weighted sum of the form
J = qτF +

N
−1
X
k=0

14

uTk Ruk ,

(20)

where τF is the final time and R = R⊤ ∈ Rnu ×nu  0, and q ∈ R ≥ 0 are weighting factors. In (20), uTk Ruk
model the minimum input objective, while τF represents the minimum time objective. Motivated by [43],
we do not directly optimize over τF ; instead, observing that τF = N Topt , we will treat the discretization
time Topt as a decision variable. This motivates the use of the slightly modified cost function J = qTopt +
PN −1 T
k=0 uk Ruk , which will be used in the numerical simulations later on.

Treating Topt as an optimization variable has the additional benefit that the duration of the maneuver

does not need to be fixed a priori, allowing us to avoid feasibility issues caused by a too short maneuver
length. We point out that this comes at the cost of an additional decision variable Topt , which renders the
dynamics “more” non-linear, which can be seen when looking at the Euler discretization in the previous
section.
5.4. Choice of Initial Guess
It is well-known that non-convex optimization problems are computationally difficult to solve in general,
and that one must content oneself with a locally optimal solution, since most NLP-solvers are operate locally.
Often times, the solution quality can be influenced by the choice of the initial guess. Ideally this initial
guess satisfies all constraints including the obstacle avoidance and system constraints. For the quadcopter
application we have found that a (X, Y, Z)-path that avoids all obstacles (but not necessarily satisfies the
system dynamics) results in a good performance, while the other variables are initialized with zero. A feasible
(X, Y, Z)-path is constructed using ordered waypoints which are connected with lines and are chosen such
that the lines do not intersect with the obstacles. In our set-up this results in a path consisting of five
straight segments connecting the starting and finishing point while avoiding all the obstacles, see Figure 2.
5.5. Simulation Results
To verify the performance and robustness of our approach, we considered 36 path planning scenarios, each
starting and ending in a hovering points. The starting point is always located at X = 1, Y = 1, Z = 3m, and
the finishing point is always located behind the wall with the window at X = 9m, but with varying Y and Z
coordinates. The final positions are generated by gridding the (Y, Z) space with nine points in the Y direction
and four points in the Z direction as shown in Figure 3. We tested both the distance formulation (10) as
well as the signed distance formulation with soft constraints (13). We use a horizon of N = 80 and limit
the sampling time Topt to lie between 0.125s and 0.375s. The optimization problems are implemented with
the modeling toolbox JuMP in the programming language Julia [44], and solved using the general purpose
nonlinear solver IPOPT [37]. The problems are solved on a MacBook Pro with an i7 processor clocked at
2.4 GHz. A Julia-based example code can be found at https://github.com/XiaojingGeorgeZhang/OBCA.
Figure 3 shows the computation time as a function of the finishing position, where a circle implies that
a (locally optimal) solution was found successfully, see Figure 1 for such a trajectory. Despite having more
15

Figure 2: The chosen warm start trajectory for the quadcopter is shown. Notice that this trajectory avoids
obstacles but does not satisfy the dynamic constraints of the quadcopter, which will be corrected by the
optimization algorithm. The black tubes is the safety distance dmin which is used to model the shape of the
quadcopter.
decision variables, we observe from Figure 3 that the signed-distance approach (right) is solved not only
slightly faster overall, but also has fewer outliers in terms of computation time. Furthermore, notice that both
approaches are able to compute all paths but one. Interestingly, we observe that the unsuccessful scenarios,
as well as computation times of the two approaches, are not correlated; in other words, a “difficult” scenario
for the distance approach might be “easy” for the signed distance approach, and vice versa. In practice,
this implies that to obtain feasible trajectories as fast as possible, the navigation problem should be solved
with both obstacle avoidance formulations, and the first solution should be taken. For our problem setup,
this would result in a worst case computation time of 22.0s.

50s
40s
30s
20s
10s

4
2
0
0

5

Z [m]

Z [m]

6

6

40s

4

30s

2

20s

0

10s

0

10

5

10

Y [m]

Y [m]

Figure 3: Solution time for quadcopter trajectory planning with distance approach (left) and signed-distance
approach (right). Median computation time for distance approach is 12.2s, while median computation time
for signed-distance is 10.4s. One scenario in each case could not be obtained.
16

6. Example 2: Autonomous Parking
As a second application for our collision avoidance formulation, we consider the autonomous parking
problem of self-driving cars. In contrast to the quadcopter case, modeling a car as a point-mass and then
conservatively approximating its shape through a ball can be very conservative and prevent a car finding
a feasible trajectory, especially in tight environments. In this section, we model the car as a rectangle,
and then employ the full-dimensional approach of Section 4 to the parking problem. Two scenarios are
considered: backward parking (Figure 4) and parallel parking (Figure 5).
12
10

Y [m]

8
6
4
2
0

-10

-5

0

5

10

X [m]

Figure 4: Backward parking maneuver. The controlled vehicle is shown in green at every time step. Vehicle
starts facing to the right, and ends facing upwards, see https://youtu.be/2NcfVE52q1U for an animation.

12
10

Y [m]

8
6
4
2
0
-10

-5

0

5

10

X [m]

Figure 5: Parallel parking maneuver. The controlled vehicle is shown in green at every time step. Vehicle
starts facing to the right, and ends facing to the right, see https://youtu.be/igDkznOzPGw for an animation.

6.1. Environment and Obstacle Modeling
For the backwards parking scenario, the parking spot is assumed 2.6m wide and 5.2m long. The width
of the road, where the car can maneuver in, is 5m, see Figure 4 for an illustration. For the parallel parking
17

scenario, the parking spot is 2.5m deep and 6m long, and the space to maneuver is 7m wide (Figure 5).
Note that both parking situations can be described by 3 axis aligned boxes serving as obstacles. Finally, in
both cases, the controlled vehicle is modeled as a rectangle of size 4.7 × 2m, whose orientation is determined
by the car’s yaw angle.
6.2. System Dynamics and Cost Function
The car is described by the classical kinematic bicycle model, which is well-suited for velocities used in
typical parking scenarios. The states (X, Y ) correspond to the center of the rear axes, while ϕ is the yaw
angle with respect to the X-axis, and v is the velocity with respect to the rear axes. The inputs are the
steering angle δ and the acceleration a. Hence, the continuous-time dynamics of the car is given by
Ẋ = v cos(ϕ) ,
Ẏ = v sin(ϕ) ,
ϕ̇ =

v tan(δ)
,
L

v̇ = a ,
where L = 2.7m is the wheel base of the car. The steering angle is limited between ±0.6rad (approximately
34 degrees) and acceleration between ±1m/s2 . We limit the car’s velocity to lie between −1 and 2m/s.
Similar as in the quadcopter case, the continuous-time dynamics are discretized using a forward Euler
scheme. Finally, the same cost function as in the quadcopter is used, i.e., a weighted sum between the
discretization-time and control effort is considered.
6.3. Initial Guess
In contrast to the quadcopter case, finding meaningful way points as good initial guesses is more challenging because the yaw angle of the car needs to be considered when constructing an initial guess. Therefore, we
propose the use of an alternative approach, where we first solve a point-to-point trajectory planning problem
without any obstacles, and then use this trajectory as a initial guess to warm start the full optimization
problem, see Figures 6 and 7 for the warm start trajectories associated to the trajectories in Figures 4 and
5. While this often results in a reasonable initial guess, it does not guarantee success at all times as we will
see next.
6.4. Simulation Results
To evaluate the performance of the collision avoidance formulations (15) and (18), we study the backward
and parallel trajectory planning problem. For both cases, we consider different starting positions but one
fixed end position at X = 0, and investigate the performance and success rate of each method. The starting
18

12
10

Y [m]

8
6
4
2
0
-10

-5

0

5

10

X [m]

Figure 6: Initial guess for the backward parking maneuver, with the controlled vehicle shown in green at
every time step.
12
10

Y [m]

8
6
4
2
0
-10

-5

0

5

10

X [m]

Figure 7: Initial guess 2 for the parallel parking maneuver, with the controlled vehicle shown in green at
every time step.

positions are generated by gridding the maneuvering space within X ∈ [−10, 10] and Y ∈ [5, 10], with eleven
points in the X direction and 5 points in Y direction, see Figure 8. The orientation for all the starting points
is ϕ = 0, resulting in a total of 105 starting points. For the specific implementation, we use a horizon of
N = 80 and a variable sampling time Topt ∈ [0.15, 0.6]. The optimization problems are again implemented
with the modeling toolbox JuMP in the programming Julia [44], and IPOPT [37] is used as the NLP solver.
The problems are solved on a MacBook Pro with a i7 processor clocked at 2.4 GHz. A Julia-based example
code can be found at https://github.com/XiaojingGeorgeZhang/OBCA.
We begin by considering the backward parking case, where one specific maneuver is illustrated in Figure 4.
The computation times for the distance and the signed distance approaches are shown in Figure 8, for all
105 initial conditions. In contrast to the quadcopter example, the distance approach is faster than the
signed-distance approach, with a median computation time of 2.1s compared to 7.0s. Furthermore, notice
19

that both approaches are able to find trajectories for all considered initial conditions. Interestingly, we see

14s

10

10s

5

6s

0

2s

-10

0

Y [m]

Y [m]

that there are no obvious connections between starting positions and solution times.

10

15s

5

10s

0

10

5s

-10

0

10

X [m]

X [m]

Figure 8: Solution time for backwards parking with distance approach (left) and signed-distance approach
(right). It can be seen that the signed-distance approach is computationally more demanding than the
distance approach.
For the parallel parking case, Figure 5 shows one parking instance, while Figure 9 depicts the computation
time for different initial conditions. Compared to the backward parking case, we see that neither approach
(i.e., distance and signed-distance) has a 100% success rate. Specifically, we see that starting points with
X ∈ [−10, −5] are difficult for the distance approach, where the computation times are high and success
rates are low. We believe that this effect can be attributed to the choice of our initial guess, since the optimal
path with and without obstacles can be dramatically different for these starting points, see Figure 10 for
such a scenario. Finally, we point out that while the signed-distance approach is computationally more
challenging on average, it shows better performance than the distance approach in terms of success rates, a

80s
60s
40s
20s

10
5
0

Y [m]

Y [m]

phenomena that requires further investigation.

10

20s

5

10s

0
-10

0

10

-10

0

10

X [m]

X [m]

Figure 9: Solution time for parallel parking with distance approach (left) and signed-distance approach
(right). It can be seen that, overall, the signed-distance approach is more robust and computationally
demanding than the distance approach. Median computation time is 6.4s for distance approach, and 9.7s
for signed-distance approach.

20

12
10

Y [m]

8
6
4
2
0
-10

-5

0

5

10

5

10

X [m]
12
10

Y [m]

8
6
4
2
0
-10

-5

0

X [m]

Figure 10: A parallel parking scenario where the warm-start trajectory (top) significantly deviates from the
final trajectory (bottom). The controlled vehicle shown in green at every time step. An animation of the
collision-free trajectory can be found at https://youtu.be/t5HOGLQiAu8.

7. Conclusion
In this paper, we presented smooth reformulations for collision avoidance constraints for problems where
the controlled object and the obstacle can be represented as the finite union of convex sets. We have
shown that non-differentiable polytopic obstacle constraints can be dealt with via dualization techniques
to preserve differentiability, allowing the use of gradient- and Hessian-based optimization methods. The
presented reformulation techniques are exact and non-conservative, and apply equally to point-mass and
full-dimensional controlled vehicles. Furthermore, in case collision-free trajectories cannot be generated, our
framework allows us to find least-intrusive trajectories, measured in terms of penetration.
Our numerical studies, performed on a quadcopter trajectory planning and car parking example, indicate
that the signed-distance reformulation is numerically more stable than the simpler distance reformulation,
although the latter can yield lower computation times. Furthermore, we have seen that the performance of
the numerical algorithms to solve the non-convex optimization problems depends on the choice of the initial
guess. Current research focuses on the appropriate choice of initial guesses, as well as methods for speeding
21

up computation time.

Appendix A. Proof of Proposition 2
We need the following auxiliary result:
Lemma 1. Let C ⊂ Rn be a compact convex set and let H(z) := {x : z ⊤ x ≤ maxy∈C y ⊤ z}. Then ∂H(z) is
a supporting hyperplane of C, and
C=

\

H(z),

(A.1)

z : kzk=1

for any norm k · k.
Proof of Proposition 2
It is easy to see that pen(E(x), O) = dist(E(x), O∁ ), where C∁ ⊂ Rn denotes the complement of the set C.
Using this definition and recalling that E(x) is a point-mass, it follows from Lemma 1 that pen(E(x), O) =
inf {z : kzk∗ =1} dist(E(x), ∂H(z)), where we exploited the fact that (A.1) holds for any norm, and hence also
the dual norm k · k∗ . Furthermore, it can be shown that dist(E(x), ∂H(z)) = maxy∈C {y ⊤ z} − z ⊤E(x), which

allows us to express the penetration function as pen(E(x), O) = inf {z : kzk∗ =1} maxy∈C {y ⊤ z} − z ⊤ E(x) . So

see that the min-max problem implies (11), we use strong duality of convex optimization to reformulate the
inner maximization problem as maxy∈C {y ⊤ z} = minλ {b⊤ λ : A⊤ λ = z, λ K∗ 0}. Hence, pen(E(x), O) =

inf z,λ {b⊤ λ − z ⊤ E(x) : kzk∗ = 1, A⊤ λ = z, λ K∗ 0} = inf λ {(b − AE(x))⊤ λ : kA⊤ λk∗ = 1, λ K∗ 0}.
Finally, we have pen(E(x), O) < pmax ⇔ inf λ {(b − AE(x))⊤ λ : kA⊤ λk∗ = 1, λ K∗ 0} < pmax ⇔ λ K∗
0 : (b − A E(x))⊤ λ < pmax , which concludes the proof.

References
[1] A. Richards and J. P. How. Aircraft trajectory planning with collision avoidance using mixed integer linear programming.
In American Control Conference, pages 1936–1941, May 2002.
[2] F. Borrelli, T. Keviczky, and G. J. Balas. Collision-free UAV formation flight using decentralized optimization and invariant
sets. In IEEE Conference on Decision and Control, pages 1099–1104 Vol.1, Dec 2004.
[3] M. Diehl, H.G. Bock, H. Diedam, and P.-B. Wieber. Fast Direct Multiple Shooting Algorithms for Optimal Robot Control,
pages 65–93. Springer Berlin Heidelberg, Berlin, Heidelberg, 2006.
[4] L. Blackmore and B. Williams. Optimal manipulator path planning with obstacles using disjunctive programming. In
American Control Conference, June 2006.
[5] Y. Gao, T. Lin, F. Borrelli, E. Tseng, and D. Hrovat. Predictive control of autonomous ground vehicles with obstacle
avoidance on slippery roads. In ASME Dynamic Systems and Control Conference, 2010.
[6] R. Patel and P. J. Goulart. Trajectory generation for aircraft avoidance maneuvers using online optimization. AIAA
Journal of Guidance, Control and Dynamics, 34(1):218–230, 2011.
[7] L. Blackmore, M. Ono, and B. C. Williams. Chance-constrained optimal path planning with obstacles. IEEE Transactions
on Robotics, 27(6):1080–1094, Dec 2011.

22

[8] S. Sutrisno, E. Joelianto, A. Budiyono, I. E. Wijayanti, and N. Y. Megawati. Model predictive control for obstacle avoidance
as hybrid systems of small scale helicopter. In International Conference on Instrumentation Control and Automation,
pages 127–132, Aug 2013.
[9] J. Schulman, Y. Duan, J. Ho, A. Lee, I. Awwal, H. Bradlow, J. Pan, S. Patil, K. Goldberg, and P. Abbeel. Motion planning
with sequential convex optimization and convex collision checking. The International Journal of Robotics Research,
33(9):1251–1270, 2014.
[10] A. Liniger, A. Domahidi, and M. Morari. Optimization-based autonomous racing of 1:43 scale RC cars. Optimal Control
Applications and Methods, 36(5):628–647, 2015.
[11] U. Rosolia, A. Carvalho, and F. Borrelli. Autonomous racing using learning model predictive control. In American Control
Conference (ACC), 2017. IEEE, 2017.
[12] J. Funke, M. Brown, S. M. Erlien, and J. C. Gerdes. Collision avoidance and stabilization for autonomous vehicles in
emergency scenarios. IEEE Transactions on Control Systems Technology, 25(4):1204–1216, July 2017.
[13] G. Schildbach and F. Borrelli. A dynamic programming approach for nonholonomic vehicle maneuvering in tight environments. In IEEE Intelligent Vehicles Symposium (IV), pages 151–156, June 2016.
[14] C. Tomlin, G. J. Pappas, and S. Sastry. Conflict resolution for air traffic management: a study in multiagent hybrid
systems. IEEE Transactions on Automatic Control, 43(4):509–521, Apr 1998.
[15] K. Margellos and J. Lygeros. Hamilton-Jacobi Formulation for Reach-Avoid Differential Games. IEEE Transactions on
Automatic Control, 56(8):1849–1861, Aug 2011.
[16] K. Margellos and J. Lygeros. Toward 4-D Trajectory Management in Air Traffic Control: A Study Based on Monte Carlo
Simulation and Reachability Analysis. IEEE Transactions on Control Systems Technology, 21(5):1820–1833, Sept 2013.
[17] M. Likhachev and D. Ferguson. Planning long dynamically feasible maneuvers for autonomous vehicles. The International
Journal of Robotics Research, 28(8):933–945, 2009.
[18] D. Dolgov, S. Thrun, M. Montemerlo, and J. Diebel. Path Planning for Autonomous Vehicles in Unknown Semi-structured
Environments. The International Journal of Robotics Research, 29(5):485–501, 2010.
[19] S. Aine, S. Swaminathan, V. Narayanan, V. Hwang, and M. Likhachev. Multi-Heuristic A*. The International Journal
of Robotics Research, 35(1-3):224–243, 2016.
[20] S. M. LaValle and J. J. Kuffner. Randomized Kinodynamic Planning. The International Journal of Robotics Research,
20(5):378–400, 2001.
[21] J. Ziegler and M. Werling. Navigating car-like robots in unstructured environments using an obstacle sensitive cost
function. In IEEE Intelligent Vehicles Symposium, pages 787–791, June 2008.
[22] S. Karaman and E. Frazzoli. Sampling-based algorithms for optimal motion planning. The International Journal of
Robotics Research, 30(7):846–894, 2011.
[23] H. Banzhaf, L. Palmieri, D. Nienhuser, T. Schamm, S. Knopp, and J.M. Zollner. Hybrid curvature steer: A novel extend
function for sampling-based nonholonomic motion planning in tight environments. In IEEE International Conference on
Intelligent Transportation Systems, pages 2239–2246, 2017.
[24] H. Vorobieva, S. Glaser, N. Minoiu-Enache, and S. Mammar. Automatic parallel parking with geometric continuouscurvature path planning. In IEEE Intelligent Vehicles Symposium Proceedings, pages 465–471, June 2014.
[25] J. Canny. The complexity of robot motion planning. MIT press, 1988.
[26] S. M. LaValle. Planning Algorithms. Cambridge University Press, 2006.
[27] M. Campbell, M. Egerstedt, How J, and R. Murray. Autonomous driving in urban environments: approaches, lessons
and challenges. Philosophical Transactions of the Royal Society of London A: Mathematical, Physical and Engineering
Sciences, 368(1928):4649–4672, 2010.
[28] B. Paden, M. Cáp, S. Yong, D. S. Yershov, and E0 Frazzoli. A Survey of Motion Planning and Control Techniques for

23

Self-Driving Urban Vehicles. IEEE Transactions on Intelligent Vehicles, 1:33–55, 2016.
[29] D. Gonzlez, J. Prez, V. Milans, and F. Nashashibi. A review of motion planning techniques for automated vehicles. IEEE
Transactions on Intelligent Transportation Systems, 17(4):1135–1145, 2016.
[30] C. Katrakazas, M. Quddus, W. Chen, and L0 Deka. Real-time motion planning methods for autonomous on-road driving:
State-of-the-art and future research directions. Transportation Research Part C: Emerging Technologies, 60:416 – 442,
2015.
[31] I. E. Grossmann. Review of Nonlinear Mixed-Integer and Disjunctive Programming Techniques. Optimization and Engineering, 3(3):227–252, 2002.
[32] U. Rosolia, S. DeBruyne, and A. Alleyne. Autonomous vehicle control: A nonconvex approach for obstacle avoidance.
IEEE Transactions on Control Systems Technology, 25(2):469–484, 2017.
[33] T. Nägeli, J. Alonso-Mora, A. Domahidi, D. Rus, and O. Hilliges. Real-Time Motion Planning for Aerial Videography
With Dynamic Obstacle Avoidance and Viewpoint Optimization. IEEE Robotics and Automation Letters, 2(3):1696–1703,
2017.
[34] B. Li and Z. Shao. A unified motion planning method for parking an autonomous vehicle in the presence of irregularly
placed obstacles. Knowledge-Based Systems, 86:11 – 20, 2015.
[35] T.R. Rockafellar. Convex Analysis. Princeton University Press, 1970.
[36] S. Boyd and L. Vandenberghe. Convex Optimization. Cambridge University Press, 2004.
[37] A. Wächter and L. Biegler. On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear
programming. Mathematical Programming, 106(1):25–57, Mar 2006.
[38] S. Cameron and R. Culley. Determining the minimum translational distance between two convex polyhedra. In International Conference on Robotics and Automation, volume 3, pages 591–596, Apr 1986.
[39] D. Dobkin, J. Hershberger, D. Kirkpatrick, and S. Suri. Computing the intersection-depth of polyhedra. Algorithmica,
9(6):518–533, Jun 1993.
[40] F. Borrelli, A. Bemporad, and M. Morari. Predictive control for linear and hybrid systems. Cambridge University Press,
2017.
[41] D. Mellinger, N. Michael, and V. Kumar. Trajectory generation and control for precise aggressive maneuvers with
quadrotors. The International Journal of Robotics Research, 31(5):664–674, 2012.
[42] D. Mellinger. Trajectory Generation and Control for Quadrotors. PhD thesis, 2012.
[43] F. Fahroo and I. Ross. Direct trajectory optimization by a Chebyshev pseudospectral method. In American Control
Conference, volume 6, pages 3860–3864, 2000.
[44] I. Dunning, J. Joey, and M. Lubin. Jump: A modeling language for mathematical optimization. SIAM Review, 59(2):295–
320, 2017.

24

