Safe Trajectory Synthesis for Autonomous Driving in Unforeseen
Environments
Shreyas Kousik1, Sean Vaskov1, Matthew Johnson-Roberson2, Ram Vasudevan1
Abstract— Path planning for autonomous vehicles in arbi-
trary environments requires a guarantee of safety, but this
can be impractical to ensure in real-time when the vehicle is
described with a high-fidelity model. To address this problem,
this paper develops a method to perform trajectory design
by considering a low-fidelity model that accounts for model
mismatch. The presented method begins by computing a
conservative Forward Reachable Set (FRS) of a high-fidelity
model’s trajectories produced when tracking trajectories of a
low-fidelity model over a finite time horizon. At runtime, the
vehicle intersects this FRS with obstacles in the environment
to eliminate trajectories that can lead to a collision, then
selects an optimal plan from the remaining safe set. By
bounding the time for this set intersection and subsequent path
selection, this paper proves a lower bound for the FRS time
horizon and sensing horizon to guarantee safety. This method
is demonstrated in simulation using a kinematic Dubin’s car
as the low-fidelity model and a dynamic unicycle as the high-
fidelity model.
I. INTRODUCTION
Safe control of autonomous vehicles for use on public
roads is a challenging problem that directly impacts both
public safety and development costs for auto manufacturers.
To be safe, autonomous vehicles must be able to account
for errors in their sensors and dynamic models. Moreover,
since it is impractical to preplan an obstacle avoidance
trajectory for every plausible environment and configuration
of obstacles, controllers need to be constructed, evaluated for
correctness, and selected in real-time.
Autonomous vehicles typically apply a hierarchical control
design structure with three levels [6], [3], [8]. The highest-
layer in this architecture is responsible for route planning
using Dijkstra’s algorithm or its variants. The mid-level
controller generates trajectories to perform actions such as
lane-keeping or obstacle avoidance. The low-level controller
follows these generated trajectories by applying throttle,
brake, and steering inputs. For computational efficiency, the
mid-level controller often uses a lower degree of freedom
model than the low-level controller. As a result, there is
typically a non-trivial gap between the trajectories generated
by the mid-level controller and what a vehicle is capable
of executing. This paper develops a real-time optimization
based scheme for the mid-level controller that can provably
design trajectories which can be safely followed by the
low-level controller in arbitrary environments with static
obstacles.
1. University of Michigan - Ann Arbor, Department of Mechanical
Engineering. Contact {skousik, skvaskov, ramv}@umich.edu.
2. University of Michigan - Ann Arbor, Department of Naval Architecture
and Marine Engineering. Contact mattjr@umich.edu.
1
2
1
2
Fig. 1: An illustration of the Forward Reachable Set in the
physical world (right) and the trajectory parameter space (left).
Each trajectory parameter on the left corresponds to a trajectory
in the physical world. The FRS (the funnel beginning from the
blue car) is intersected with obstacles (the road boundaries and the
red car) at run-time to identify trajectory parameters that lead to
collisions (orange and red sets in left picture). In this sketch, the
parameters labeled 1 produce a guaranteed-safe trajectory, whereas
the parameters labeled 2 result in a collision. Optimal parameters
can be selected from the safe set, Ksafe.
Various methods exist in the literature for mid-level con-
troller design, which rely upon either temporal or spatial
discretization, or both. Rapidly Random Exploring Trees,
for example, compute safe trajectories by sampling from a
vehicle’s input space and numerically integrating trajectories
forward using a dynamic model [13]. These methods can
handle nonlinear dynamics and non-convex constraints and
can even be made asymptotically optimal [11]. Partially Ob-
servable Markov Decision Processes are also able to handle
nonlinear dynamics, non-convex constraints, and bounded
uncertainty, while planning trajectories in a spatially and
temporally discretized space[2]. Though each of these mid-
level control design methods is powerful, they struggle to
account for the uncertainty introduced by discretization,
which can result in trajectories that are impossible to execute.
For this reason, others have focused solely on the real-time
verification of the safety of a generated mid-level controller
using approaches grounded in zonotopes [1].
Several approaches have recently been pursued to perform
control design with safety guarantees. Robust Model Pre-
dictive Control (MPC), for example, has been demonstrated
to successfully account for uncertainty in the dynamics and
can be used for safe control design for nonlinear vehicle
models in the presence of non-convex constraints [7], [23].
This requires linearizing the dynamics about a pre-specified,
spatially discretized trajectory to synthesize a controller
that is able to safely follow this trajectory while avoiding
obstacles. Thus, this approach requires solving a scenario-
specific nonlinear program, which can be challenging to
solve at run-time and, as a result, has few associated safety
arXiv:1705.00091v1 [cs.SY] 28 Apr 2017
guarantees. To try to address the more general safe control
design problem, others have employed the Hamilton-Jacobi-
Bellman Equation, which is typically solved using state-
space discretization, to compute the set of safe feedback
controllers in the presence of bounded disturbance [5]. These
approaches rely upon a complete characterization of obstacle
locations, which can make real-time applications challenging
due to the computational expense associated with state-
space discretization. Despite this limitation, extensions of
this approach have been successfully employed to perform
cooperative control for connected vehicles [4]. Similarly, the
viability kernel, which is also computed using state-space
discretization, has been employed to evaluate the set of
possible trajectories for autonomous vehicles on pre-defined
scenarios [14].
To avoid this curse of dimensionality, others have em-
ployed barrier functions using Lyapunov theory to devise
safety-preserving controllers for adaptive cruise control and
lane-keeping [20], [25], [24]. These approaches can quickly
solve a quadratic program at run-time to guarantee safety, but
the barrier function, which is computed off-line, is scenario
specific. To overcome this situation-specific limitation, an
approach relying upon funnel libraries was recently proposed
to perform real-time control design in the presence of uncer-
tainty [17]. This approach relies upon pre-computing a rich-
enough finite family of trajectories, which is then searched
at run-time to ensure safety.
This paper presents an approach for mid-level control
design that simultaneously accounts for nonlinear dynam-
ics, non-convex constraints, and uncertainty without pre-
specifying the scenario, pre-computing a finite family of
trajectories, or relying on spatial or temporal discretization.
The proposed method, which is depicted in Figure 1, begins
by computing the set of trajectories that can be reached by a
high-fidelity vehicle model under a parameterized controller.
This Forward Reachable Set (FRS), which is computed off-
line, is then intersected with obstacles at run-time using
convex optimization to exclude controllers that could cause
collisions. Finally, the remaining safe set of controllers
is optimized over to minimize an arbitrary cost function,
such as following a nominal trajectory, minimizing total
acceleration, etc.
This paper makes the following pair of contributions:
first, a convex optimization based method to synthesize
trajectories which can be safely tracked by a high fidelity
vehicle model; and second, a method to select braking,
sensing, and planning time horizons to guarantee that a
safety preserving trajectory always exists in an environment
with static obstacles. The rest of the paper is structured as
follows: Section II introduces the notation and assumptions
utilized throughout the paper. Section III outlines methods to
determine the FRS in the presence of bounded uncertainty, to
compute the set of safe trajectory parameters in the presence
of obstacles, and to compute an optimal trajectory from
this set. Section IV describes the implementation of the
methods formulated in Section III. Section V illustrates the
performance of our method on an example.
II. PRELIMINARIES
This section introduces necessary notation for this paper,
and formalizes the dynamics and assumptions using to de-
scribe the problem of interest. This paper makes substantial
use of semidefinite programming theory and sums of squares
programming [12].
A. Notation
The following notation is adopted throughout the remain-
der of the paper: Sets are italicized and capitalized. The
boundary of a set K is ∂K. Finite truncations of the set of
natural numbers are Nn := {1,...,n}. The set of continuous
functions on a compact set K are C(K). The Lebesgue
measure on K is denoted by λK. We let yK denote the
Lebesgue moments of λK. The i-th component of a vector
v ∈Rn is denoted by vi. The ring of polynomials in x is
R[x], and the degree of a polynomial is the degree of its
largest multinomial; the degree of the multinomial xα, α ∈Nn
is |α| = ∥α∥1; and Rd[x] is the set of polynomials in x with
degree d. Let vec(p) denote the vector of coefficients of
p ∈R[x].
B. Low and High-Fidelity Vehicle Models
Let an autonomous vehicle by described by:
˙x(t) = f(t, x(t),u(t))
(1)
where f : [0,T] × X × U →Rn, T > 0, X ⊂Rn and U ⊂Rm.
The objective of this paper is to design safe trajectories for
this autonomous vehicle which may be high dimensional and
challenging to optimize over in real-time. Our approach relies
upon generating parameterized trajectories using a second,
lower-dimensional model for this autonomous vehicle that
shares a common set of states, Xs ⊂Rns with Xs ⊂X. This
model is described as:
"˙xs(t)
˙k(t)
#
=
" fs(t, xs(t),k(t))
0
#
(2)
where the parameters, k(t), do not evolve after initialization
and are drawn from a set K ⊂Rp.
Example 1: Consider a dynamic unicycle model:
˙x(t)
˙y(t)
˙θ(t)
¨θ(t)
˙v(t)
=
v(t)cos(θ(t))
v(t)sin(θ(t))
˙θ(t)
u1(t)
u2(t)
(3)
where x and y represent position; θ and ˙θ represent heading
and yaw rate, both relative to the initial condition of the
vehicle; and v represents the longitudinal speed. The inputs
are u1 for steering and u2 for throttle or braking. Consider
a low-fidelity, trajectory-producing model with shared states
h
x
y
θ
iT such that:
˙x(t)
˙y(t)
˙θ(t)
=
k2 cos(θ(t))
k2 sin(θ(t))
k1
(4)
with the trajectory parameters that are steering rate k1 and
speed k2. The unicycle model “adds” mass to the Dubins car.
For the sake of convenience, the dynamics of the shared
states, XS , in the high dimensional model are assumed to
be described by the first ns coordinates of f throughout this
paper. Let the controlled vehicle be represented as a compact
set X0 ⊂Xs, typically a bounding box around the vehicle. We
make the following assumptions to ensure the well-posedness
of the subsequent problem formulation.
Assumption 2: The sets U and X are compact and suppose
K,X0, and Xs have the following representation:
K =
n
k ∈Rp | hKi(k) ≥0, ∀i ∈{1,...,nK}
o
,
(5)
X0 =
n
xs ∈Rns | h0i(xs) ≥0, ∀i ∈{1,...,n0}
o
,
(6)
Xs =
n
xs ∈Rns | hXi(xs) ≥0, ∀i ∈{1,...,nXs}
o
.
(7)
The objective of this paper is to optimize over trajectories
using the lower complexity dynamical model which are then
followed by the high dimensional vehicle model. Unfortu-
nately this tracking cannot be done perfectly. The higher
fidelity model can select a controller, uk : [0,T] × X →Rm,
to follow the generated trajectory which is parameterized by
k ∈K (e.g. using a PID control loop); however there may
still be a gap between the generated pair of trajectories.
To describe this error as a function of time, we make the
following assumption:
Assumption 3: There exists a bounded function g : [0,T]×
Xs →Xs such that:
max
x∈Axs
fi(t, x,uk(t, x)−fs,i(t, xs,k)
≤gi(t, xs),
(8)
for all xs ∈Xs, t ∈[0,T], k ∈K, and i ∈{1,...,ns}, where
Axs := {x ∈X | xi = xs,i∀i ∈{1,...,ns}}.
In other words, the error in the shared states is bounded by
g. Though this paper does not describe a formal method to
construct such a g, in the instance of polynomial, rational, or
trigonometric dynamics, such a g can be found numerically
by applying Sums-of-Squares programming as we describe
in Section IV.
Example 4: For the unicycle model in Example 1, uk can
be a proportional velocity controller for the yaw rate and
speed:
¨θ(t) = u1(t) = 20·(˙θ(t)−˙θdes)
(9)
˙v(t) = u2(t) = 10·(v(t)−vdes)
(10)
where (˙θdes,vdes) map to a k1 and k2 in the parameter space
for the Dubins car model described in Equation (4). Given
this uk one can generate a g that overapproximates the error
when transitioning from top speed to a complete stop in the
v dimension; and when switching from turning full-left to
turning full-right (or vice-versa) in the ˙θ dimension. Such a
g, illustrated in Figure 2, could be:
g(t, x,y,θ) =
verr(t)·(1−1
2θ2)
verr(t)·(θ −1
6θ3)
˙θerr
(11)
where verr(t) = (t −1)2 and ˙θerr(t) = (t −1)4.
-1.5
-1
-0.5
0
0.5
1
1.5
x
-0.2
0
0.2
0.4
y
0
0.5
1
1.5
2
t
0
0.5
1
v
0
0.5
1
1.5
2
t
-0.5
0
0.5
d /dt
Fig. 2: An example of the worst-case error dynamics. The top
subplot shows the (x,y) trajectories of a Dubins car (grey) and
unicycle model (blue), beginning at (−0.75,0) and ending at the
corresponding stars. Both trajectories begin at full speed (1 [m/s])
and then emergency brake at t = 1. The resulting velocity profile,
with the same color scheme, is shown in the middle subplot. In
addition, the Dubins car begins its trajectory with ˙θ = +0.5 [rad/s],
whereas the unicycle begins at 0 [rad/s] and must catch up. At t = 1,
the Dubins car switches instantaneously to turning at −0.5 [rad/s],
as shown in the bottom subplot. The red dashed line in the middle
and bottom subplots is the error function g that overapproximates
the worst-case possible error between the Dubins car and unicycle
with a polynomial.
As a result of this assumption, the dynamics can be
rewritten in a form amenable to computing the flow of the
system fs while subject to error dynamics:
˙xs(t) = fs(t, xs(t),k)+g(t, xs(t),k) d(t)
(12)
where d(t) ∈[−1,1] for each t ∈[0,T] can be chosen to
describe the worst-case error behavior. This use of d is
discussed further in Section III-A. To understand the gap
between lower and higher dimensional models, we rely upon
a pair of linear operators. First, the linear operator L fs :
C1�[0,T] × Xs × K →C�[0,T] × Xs × K on a test function
v as:
L fsv(t, xs,k) = ∂v
∂t (t, xs,k)+
ns
X
i=1
∂v
∂xi,s
(t, xs,k) fi,s(t, xs,k). (13)
Next, define the linear operator Lg : C1�[0,T] × Xs × K →
C�[0,T]× Xs × K as:
Lgv(t, xs,k) =
ns
X
i=1
∂v
∂xi,s
(t, xs,k)gi(t, xs).
(14)
Note that these linear operators are useful in summarizing
the evolution of a system as we describe in Section III-A.2.
C. Sensing, Planning, and Braking Time Horizons
Next, we define a sensing, planning, and braking time hori-
zon whose relationships are formalized in the next section
to ensure the safety of the autonomous vehicle. To construct
these definitions, we begin by formalizing obstacles:
Definition 5: An obstacle is any portion of Xs that must be
avoided by the vehicle. In general, obstacles are represented
as closed subsets of Xs. Obstacles are assumed to be static
in the system’s local reference frame and are defined as:
Xobs =
n
xs ∈Rns | hobsi(xs) ≥0,∀i ∈{1,...,nobs}
o
.
(15)
Obstacles can be other vehicles, the edges of the road,
pedestrians, etc. A trajectory is safe if it does not intersect
with an obstacle. These obstacles are assumed to enter the
scene as follows:
Assumption 6: During operation, obstacles always enter
Xs from outside, i.e., any obstacle’s trajectory relative to the
system must begin outside of, and may pass through, ∂Xs.
In addition, let the system be limited to a scalar top speed,
denoted ˙xs,max. Then, this assumption requires that obstacles
are sensed at a minimum sensor horizon Dsense := ˙xs,max ·
Tsense.
Tsense is a sensor time horizon that is to be determined, as a
safety condition that we describe in the next section.
Next, we assume that the time to process sensor informa-
tion and plan a trajectory is bounded and known:
Assumption 7: The time required to process sensor data
and perform trajectory planning has a finite upper bound
τplan.
Specific sensor processing to detect obstacles is outside
of the scope of this paper, but in practice most modern
obstacle detectors, working from camera, lidar, or radar,
have a bounded processing time [10], [15]. Though we do
not prove that the trajectory planning time is bounded, the
convex optimization based implementation that we utilize
has a processing time that takes several seconds, at most, in
practice as we describe in Section IV.
Remarks 8: To understand how this τplan can be used,
suppose the vehicle is planning as quickly as possible and
executing a trajectory beginning at t = 0 . While executing
this trajectory, the vehicle can plan its next trajectory which
is applied beginning at t = τplan. The initial condition for
this next trajectory is computed by the vehicle by forward
integrating f over the time interval [0,τplan]. This process
can be repeated recursively to replan trajectories.
Finally, in certain instances it may be impossible to reach a
desired position safely. For these scenarios, we must assume
that there exists a braking maneuver which can stop the car:
Assumption 9: There exists a family of braking trajectory
parameters Kb ⊆K such that, without deviating from the
spatial component of a pre-planned trajectory, ˙xs = 0 in
a finite amount of time τb(xs) ∈R+ for each xs ∈Xs. In
addition, this stopped state can be held indefinitely.
In other words, the vehicle is able to brake and stop along
any trajectory it is following. It follows from Assumption 9
that there is a maximum braking time for the system, i.e.
there exists τstop ∈R+ such that τb(xs) ≤τstop for all xs ∈Xs.
This braking time is used to construct planning and sensing
time horizons to guarantee safety.
III. PROBLEM FORMULATION
This section outlines a method for trajectory design and
presents a theorem relating the braking, planning, and sens-
ing time horizons to ensure the safety of the trajectory design
procedure. The approach is broken into the following steps:
1) Precompute a Forward Reachable Set (FRS) that cap-
tures all possible trajectories and associated parame-
ters, subject to error function g, over a time interval
[0,T] (Section III-A).
2) During operation, perform set intersection of the FRS
with obstacles in the vehicle’s local environment to
identify trajectory parameters, Ksafe ⊂K, which could
cause collisions using a convex optimization technique
(Section III-B).
3) During operation, perform trajectory optimization over
Ksafe of a user-specified cost function to drive the
system towards an objective while guaranteeing safety
(Section III-C).
These steps are described next.
A. Forward Reachable Set
This subsection describes our approach to compute the
FRS, which contains all points that can be reached from
X0 under the dynamics of f. The dynamics of f may be
high-dimensional in general as a result, which can make
computing the FRS for f impractical. To overcome this
challenge, we compute the FRS of the lower-dimensional
model described in Equation (12).
XFRS =
n
(xs,k) ∈Xs × K | ∃x0 ∈X0,τ ∈[0,T], and
d : [0,T] →[−1,1] s.t. x(τ) = xs, where
˙x(t) = fs(t, x(t),k)+g(t, x(t),k) d(t)
a.e. t ∈[0,T] and x(0) = x0
o
(16)
Notice that in this definition the error function g along with
d(t) ∈[−1,1] accounts for the difference between a trajectory,
parameterized by k ∈K, of the lower dimensional model and
the higher-dimensional model which follows this trajectory
using a control uk.
1) Selecting T to Ensure Safety: Identifying the time
horizon T for the FRS is the critical first step. If T is too
short, then safe behavior cannot be guaranteed; if it is too
long, then computing the FRS may be impractical. Before
describing how to compute XFRS, we first describe how to
select the time horizon T to ensure safety while planning:
Theorem 10: Let Tsense, τplan, and τstop be as in Assump-
tions 6,7, and 9, respectively. Suppose the vehicle plans a
trajectory every τplan and there exists a T ≥0 such that:
τplan +τstop ≤T
(17)
T +τplan ≤Tsense,
(18)
and a safe trajectory over the time horizon [0,T]. Then, there
exists a safe trajectory for all t > T.
Proof: This theorem follows directly from the assump-
tions, which we can use to construct the worst-case scenario.
At time t = 0, let the vehicle be following a trajectory that has
been previously identified as safe for the time interval [0,T].
Suppose trajectories are planned as described in Remark 8.
Obstacles that could cause collisions fall into one of three
cases. It is sufficient to show that none of these cases can
cause a collision.
Case 1: Any obstacle that can be reached within time T.
These obstacles will already be avoided since the trajectory
starting at time t = 0 is safe for the entire duration [0,T] by
definition, even if the vehicle must come to a stop.
Case 2: Any obstacle that can be reached at a time t ∈
(T,Tsense]. These obstacles are sensed according to As-
sumption 6. The worst-case scenario, in this instance, is
an obstacle that can be reachable infinitesimally after t =
T. Recall that the vehicle is replanning while traveling a
duration of τplan, so the plan beginning from time t = τplan
incorporates the positions of all sensed obstacles by adjusting
their relative position using the initial condition for the
upcoming trajectory. In addition, this plan incorporates the
error that accumulates over the trajectory before τplan because
the FRS uses Assumption 3. Therefore, at time t = τplan, no
obstacle is reachable within a time horizon less than T −
τplan, which is greater than or equal to τstop. Consequently,
while traversing the first time interval [0,τplan], the vehicle
determines whether or not it must begin stopping, or if a
non-braking trajectory exists, at time t = τplan. As a result, at
t = τplan, the obstacle falls into Case 1.
Case 3: Any obstacle that can be reached at a time t > Tsense.
We assume that the vehicle is detecting obstacles continually,
as per Assumption 6. Therefore, by similar logic to Case 2,
the obstacle is no closer than Tsense −τplan at time t = τplan.
The obstacle thus falls into Case 2 for the trajectory starting
at t = τplan.
2) FRS Computation: To compute the FRS one can solve
the following linear program over the space of functions,
which is adapted from [18, Section 3.3, Program (D)]:
inf
v,w,q
Z
Xs×K
w(xs,k) dλXs×K
(D)
s.t. L fsv(t, xs,k)+q(t, xs,k) ≤0,
on [0,T]× Xs × K
Lgv(t, xs,k)+q(t, xs,k) ≥0,
on [0,T]× Xs × K
−Lgv(t, xs,k)+q(t, xs,k) ≥0,
on [0,T]× Xs × K
q(t, xs,k) ≥0,
on [0,T]× Xs × K
−v(0, xs,k) ≥0,
on X0 × K
w(xs,k) ≥0,
on Xs × K
w(xs,k)+v(t, xs,k)−1 ≥0,
on [0,T]× Xs × K
The given data in this problem are f,g,Xs,X0,K and T and
the infimum is taken over (v,w,q) ∈C1 ([0,T]× Xs)×C(Xs)×
C([0,T]× Xs).
Notice first that the 1-superlevel set of feasible w’s in (D)
are outer approximations of XFRS:
Lemma 11: Let (v,w,q) be feasible functions to (D), then
XFRS ⊂(xs,k) ∈Xs × K | w(xs,k) ≥1 .
Proof:
Suppose (xs,k) ∈XFRS, then ∃τ ∈[0,T],
d : [0,T] 7→[−1,1] and x : [0,T] 7→Xs such that ˙x(t) =
fs(t, x(t),k) + g(t, x(t),k)d(t) for a.e. t ∈[0,T] with x(τ) = xs
and x(0) ∈X0. Observe that:
v(τ, x(τ),k) = v(0, x(0),k)+
τ
Z
0
L fsv(t, x(t),k)+Lgv(t, x(t),k)d(t)
dt
≥v(0, x(0),k)+
Z τ
0
L fsv(t, x(t),k)+q(t, x(t),k)
dt
≥v(0, x(0),k),
where the first equality follows from the Fundamental Theo-
rem of Calculus, the second inequality follows from noticing
that supd(t)∈[−1,1]|Lgv(t, x(t),k)d(t)| = |Lgv(t, x(t),k)| and that
|Lgv(t, x(t),k)| ≥q(t, x(t),k) due to the constraints in (D), and
the final inequality follows from the constraints in (D). No-
tice the desired result follows by noticing that v(0, x(0),k) ≥0
for x(0) ∈X0 and applying the last constraint in (D).
The result of this lemma is that the 1-superlevel set of any
feasible w contains all possible trajectories of the high fidelity
vehicle model f beginning from X0. In fact, one can prove
that the solution to this infinite dimensional linear program
allows one to compute XFRS:
Theorem 12: [18, Theorem 3.5] There is a sequence of
feasible solutions to (D) whose w-component converges from
above to an indicator function on XFRS in the L1 norm and
almost uniformly.
In Section IV-A we describe how to generate feasible solu-
tions to (D) using semidefinite programming over polynomial
representations of continuous functions. Once this computa-
tion is completed once offline, the result can be used online
by performing real-time set intersection as described next.
B. Set Intersection
Any outer-approximation to XFRS can be intersected with
obstacles in the state space to return the trajectory parameters
that could result in a collision. The complement of the set
returned by this intersection are then parameters which can
be safely followed by the high fidelity vehicle model. To
describe this process, consider a w generated as a solution
to (D). One can construct a closed inner approximation to the
set of safe gains, denoted Ksafe ⊂K by solving the following
optimization problem:
sup
h
Z
K
h(k)dλK
(19)
s.t. 1−w(xs,k)−h(k) ≥0,
on Xobs × K
h(k) ≤1,
on K
where w is the given data and the supremum is taken over
h ∈C(K).
To appreciate how his optimization problem generates
an inner approximation to the set of safe gains, consider
a trajectory parameterization, k, that generates a trajectory
which intersects an obstacle as illustrated in Figure 3. In this
instance, there exists some xs in Xobs such that w(xs,k) ≥1. In
that instance the first constraint in the previous optimization
-1
0
1
x
-1
-0.5
0
0.5
1
y
0
0.5
1
speed
-0.5
0
0.5
steering rate
Fig. 3: An example of set intersection, with Xobs chosen as three
points in Xs. On the right is the (x,y) subspace of Xs with each
point obstacle shown, and the vehicle plotted in blue. On the left is
the trajectory parameter space K, with three dashed-line contours
containing an outer approximation of the trajectory parameters that
would cause a collision with each point shown on the right (the
colors match between the points and contours). The set intersection
step returns the subset of K shown by the green contour, which
outer approximates all trajectory parameters that could result in
collisions with any of the three points in Xobs. Therefore, Ksafe is
inner-approximated.
problem requires that h(k) be less than zero. This observation
is formalized in the next lemma:
Lemma 13: Let h be a feasible function to Equation (19).
Then {k ∈K | h(k) ≥0} ⊂Ksafe.
By exploiting the polynomial outer approximation of the
XFRS which is generated in Section IV-A one can formulate
a convex optimization method to compute a closed, inner ap-
proximation to Ksafe. This optimization method is described
in Section IV-B. Note that if T is chosen as in Theorem 10,
then as a result of Assumption 9, Ksafe , ∅.
C. Trajectory Optimization
Once Ksafe is determined for an obstacle, one can optimize
the original dynamics directly over the set. Specifically, if a
user specifies a continuously differentiable cost function, J :
K →R then one can optimize this cost function with a con-
straint that enforces that k ∈Ksafe. Since each k corresponds
to a uk one can optimize directly over safe trajectories of the
high fidelity model that travel to some waypoint, minimize
total acceleration along a trajectory, match a desired system
speed, etc.
This constrained nonlinear optimal control problem can
be solved in a variety of different ways via collocation,
solving a variational equation, sampling, or using convex
relaxations [26]. If this optimization program is unable to
conclude within τplan, then one can always apply a braking
maneuver which always exists as described in Section III-B.
IV. IMPLEMENTATION
This section describes the specific implementations of the
FRS Computation, the set intersection, and the trajectory
optimization described in Section III. For implementation,
we require the following assumption:
Assumption 14: The functions
fs is a polynomial in
R[t, xs,k], and the error function g is in R[t, xs]. In addition,
the sets K, X0, Xs, and Xobs are semialgebraic, defined by
polynomials in their respective spaces as in Equations (5) -
(7) and (15).
A. FRS Computation
To solve (D), we construct a sequence of convex sums-
of-squares programs by relaxing the continuous function in
(D) to polynomial functions with degree truncated to 2l.
The inequality constraints in (D) then transform into SOS
constraints which then transforms (D) into a semidefinite
program[21] To formulate this problem, let hτ = t(T −t) and
define Q2l(hτ,hX1,...,hXnXs ,hK1,...,hKnK ) ⊂R2l[t, xs,k] to be
the set of polynomials q ∈R2l[t, xs,k] (i.e. of total degree less
than 2l) expressible as:
q = s0 + s1hτ +
nXs
X
i=1
si+2hXi +
nK
X
i=1
si+nXS +2hKi,
(20)
for
some
polynomials
{si}nXs+nK+1
i=0
⊂R2k[t, x,k]
that
are sums of squares of other polynomials, where we
have dropped the dependence on t, x, and k in each
polynomial for the sake of convenience. Note that every
such polynomial is non-negative on [0,T] × Xs × K [12,
Theorem
2.14].
Define
Q2l(h01,...,h0n0,hK1,...,hKnK ) ⊂
R2l[xs,k],
Q2l(hX1,...,hXnXs ,hK1,...,hKnK )
⊂
R2l[xs,k],
Q2l(hobs1,...,hobsnobs,hK1,...,hKnK )
⊂
R2l[xs,k],
and
Q2l(hK1,...,hKnK ) ⊂R2l[k] similarly.
Employing this notation, the l-th relaxed semidefinite
programming representation of (D), denoted (Dl), is defined
as:
inf
v,w,q yT
Xs×Kvec(w)
(Dl)
s.t.
−Lfsv−q ∈Q2l(hτ,hX1,...,hXnXs ,hK1,...,hKnK )
Lgv+q ∈Q2l(hτ,hX1,...,hXnXs ,hK1,...,hKnK )
−Lgv+q ∈Q2l(hτ,hX1,...,hXnXs ,hK1,...,hKnK )
q ∈Q2l(hτ,hX1,...,hXnXs ,hK1,...,hKnK )
−v(0,·) ∈Q2l(h01,...,h0n0,hK1,...,hKnK )
w ∈Q2l(hX1,...,hXnXs ,hK1,...,hKnK )
w+v−1 ∈Q2l(hτ,hX1,...,hXnXs ,hK1,...,hKnK )
where the infimum is taken over the vector of polynomials
(v,w,q) ∈Rl[t, xs,k]×Rl[xs,k]×Rl[t, xs,k].
If wl denotes the w-component of the solution to (Dl), one
can prove that wl converges from above to an indicator func-
tion on XFRS in the L1 norm [18, Theorem 6]. Importantly
since each such wl is feasible with respect to the constraints
in (D), one can apply the result of Lemma 11 to prove that
the 1-superlevel set of wl is an outer approximation to XFRS.
B. Set Intersection
In this section, we present a semidefinite program to
generate a closed inner-approximation to Ksafe and comment
on computing τplan. Using the notation developed in Section
IV-A, consider the following semidefinite formulation of
Equation (19):
sup
h
yT
Kvec(h)
(21)
s.t. 1−w−h ∈Q2l(hobs1,...,hobsnobs,hK1,...,hKnK )
1−h ∈Q2l(hK1,...,hKnK )
where the given data is any feasible w ∈Rl[xs,k] to (Dl)
and the supremum is taken over polynomials h ∈Rl[k]. Note
again that since the constraints in this optimization problem
are sufficient to ensure positivity on the required sets in
Equation (19), one can apply the result of Lemma 13 to prove
that the 0-superlevel set of hl is an inner approximation to
Ksafe.
Solving such an SDP, while possible in real time, is still
computationally expensive. Compared to the trajectory opti-
mization step, which takes milliseconds, the set intersection
step can take several tenths of a second per obstacle, and
is thus the primary contributor to τplan. The planning time
increases with the number of dimensions of Xobs and with
the number of obstacles. We find τplan by precomputing an
FRS over an arbitrary, short time horizon T, then empiri-
cally determining τplan by performing set intersection with
representative obstacles and environments.
C. Trajectory Optimization
We implement trajectory optimization by directly passing
hl from solving (21) as a nonlinear constraint to an off-the-
shelf gradient-descent solver, such as MATLAB’s fmincon.
V. SIMULATION RESULTS
This section presents simulation results of the proposed
method implemented on the unicycle model example. Each
semidefinite program was prepared using a custom software
toolbox and the modeling tool YALMIP [16]. The programs
are run with commercial solver MOSEK on a machine with
1 TB availabe memory.
A. FRS Computation
We computed the FRS for a 3rd order Taylor-expanded
Dubins car as the low-fidelity model fs. Trajectories pro-
duced by this model were tracked by the unicycle model
from Equation (1) as the high-fidelity model f. The vehi-
cle’s representation as an initial distribution X0 ⊂Xs, was a
rectangle of length 0.2 [m] in x and width 0.1 [m] in y, at 0◦
initial heading, and centered at x = −0.75 and y = 0. This is
the same vehicle representation shown in all previous figures.
We chose τstop = τplan = 0.5 [s], so T = 1 [s]. The stopping
time can be seen in Figure 2. The FRS computation took 79
hours and used a maximum of 150 GB of memory
B. Set Intersection and Trajectory Planning
We used the precomputed FRS for safe trajectory planning
in 1000 simulated trials in MATLAB on the aforementioned
machine. For each trial, the vehicle began at the same
initial location and heading, surrounded by 1−10 randomized
obstacles and a randomly-located goal to reach. The vehicle’s
-1
-0.5
0
0.5
1
1.5
2
2.5
x [m]
-0.5
0
0.5
y [m]
-1
-0.5
0
0.5
x [m]
-0.5
0
0.5
y [m]
-1
-0.5
0
0.5
x [m]
-0.5
0
0.5
Fig. 4: The top subplot shows an example result out of the 1000 tri-
als. This trial used eight randomly-generated obstacles. The vehicle
begins on the left at x = −0.75 and reaches a randomly-generated
goal near (2.5,0.5), plotted as a blue circle. Every τplan = 0.5[s],
the vehicle replans its trajectory, shown by an asterisk plotted on
the global trajectory in blue. The bounding box of the vehicle at
each planning step is shown as a grey rectangle. In the bottom-
left subplot, an obstacle was constructed between the vehicle and
the goal, forcing an emergency braking maneuver. In the bottom-
right subplot, an obstacle was constructed with a hole that would
allow the vehicle to pass, but the set intersection result is overly
conservative, resulting in a braking maneuver.
initial speed, and the desired speed to maintain for the
duration of the trial, were randomly chosen between 0.25
and 0.75 [m/s].
Obstacles were represented as line segments between 0.1
and 0.2[m] in length, with random location and orientation.
The obstacles were always placed between the vehicle and
the goal. We checked for crashes conservatively for each
trial, by inspecting if any obstacle was within a circle circum-
scribing the rectangular vehicle at any point of the vehicle’s
trajectory. Using this method, no crashes were detected in
any trial. Out of all the trials, 82% reached the goal, and
15% performed an emergency braking maneuver (by setting
vdes = 0). The remaining 3% hit a simulation iteration limit.
Examples of the vehicle’s path from a randomly-generated
trial and from two constructed emergency braking cases are
shown in Figure 4.
Currently, our implementation cannot consistently achieve
τplan = 0.5 [s]. Consequently, instead of replanning and
driving simultaneously, we pause time every 0.5 [s] of the
simulation to guarantee that the vehicle can finish replanning.
In a physical implementation, if τplan is exceeded, then the
vehicle must emergency brake; recall that a safe braking
trajectory is always available. As shown in Figure 5, τplan
scales linearly with the number of obstacles.
0
2
4
6
8
10
0
1
2
3
set int mean [s]
0
2
4
6
8
10
Number of Obstacles
0.04
0.06
0.08
0.1
traj opt mean [s]
Fig. 5: The mean set intersection time (top) and trajectory opti-
mization time (bottom) versus the number of obstacles. Over the
1000 trials, each number of obstacles from 1 to 10 was used for
100 trials. Notice that set intersection takes up to 3[s], and scales
with the number of obstacles. On the other hand, the trajectory
optimization takes around 80 [ms] and has low correlation with
number of obstacles.
VI. CONCLUSION
This paper presents a method to plan safe trajectories with
obstacle avoidance for autonomous vehicles. This approach
is able to guarantee safety in arbitrary environments for
multiple, static obstacles. The method begins with computing
the forward reachable set (FRS) of parameterized trajectories
that a vehicle can realize. This set is computed in continuous
space and time, and is robust to model uncertainty between
the dynamics of the vehicle’s mid- and low-level controllers.
As an example, we use a kinematic Dubin’s car and
dynamic unicycle model as low- and high-fidelity models. At
runtime, the FRS is intersected with obstacles to eliminate
unsafe trajectories, and an optimal trajectory is chosen from
the remaining, safe trajectories. This method is proven to
ensure vehicle safety for present and future static obstacles
by considering the time required for path planning, the time
required to stop the vehicle, and the error between the low-
and high-fidelity models. One thousand simulations with
randomly-located obstacles were run to show the effective-
ness of this method.
The next step in applying this method is to expand the
error function g beyond modeling uncertainty. To reduce the
conservativeness of the approach, we plan to explore ex-
tension to the FRS computation that incorporate confidence
level sets [19], [9]. The error function g can also be improved
by considering time variation of trajectory parameters across
planning steps by posing the FRS computation as a hybrid
problem [22]. Finally, we plan to use convex optimization to
find a global solution to the nonlinear trajectory optimization
problem at each planning step [26], [27].
References
[1] Matthias Althoffand John M Dolan. Online verification of automated
road vehicles using reachability analysis.
IEEE Transactions on
Robotics, 30(4):903–918, 2014.
[2] Sebastian Brechtel, Tobias Gindele, and R¨udiger Dillmann. Probabilis-
tic decision-making under uncertainty for autonomous driving using
continuous pomdps.
In Intelligent Transportation Systems (ITSC),
2014 IEEE 17th International Conference on, pages 392–399. IEEE,
2014.
[3] Martin Buehler, Karl Iagnemma, and Sanjiv Singh. The DARPA urban
challenge: autonomous vehicles in city traffic, volume 56. springer,
2009.
[4] Aparna Dhinakaran, Mo Chen, Glen Chou, Jennifer C Shih, and
Claire J Tomlin.
A hybrid framework for multi-vehicle collision
avoidance. arXiv preprint arXiv:1703.07375, 2017.
[5] Jerry Ding, Eugene Li, Haomiao Huang, and Claire J Tomlin.
Reachability-based synthesis of feedback policies for motion planning
under bounded disturbances.
In Robotics and Automation (ICRA),
2011 IEEE International Conference on, pages 2160–2165. IEEE,
2011.
[6] Paolo Falcone, Francesco Borrelli, Jahan Asgari, Hongtei Eric Tseng,
and Davor Hrovat. Predictive active steering control for autonomous
vehicle systems. IEEE Transactions on control systems technology,
15(3):566–580, 2007.
[7] Yiqi Gao, Andrew Gray, H Eric Tseng, and Francesco Borrelli. A tube-
based robust nonlinear predictive control approach to semiautonomous
ground vehicles. Vehicle System Dynamics, 52(6):802–823, 2014.
[8] Andrew Gray, Yiqi Gao, Theresa Lin, J Karl Hedrick, H Eric Tseng,
and Francesco Borrelli. Predictive control for agile semi-autonomous
ground vehicles using motion primitives. In American Control Con-
ference (ACC), 2012, pages 4239–4244. IEEE, 2012.
[9] Patrick Holmes, Shreyas Kousik, Shankar Mohan, and Ram Vasude-
van. Convex estimation of the α-confidence reachable set for systems
with parametric uncertainty. In Decision and Control (CDC), 2016
IEEE 55th Conference on, pages 4097–4103. IEEE, 2016.
[10] Matthew
Johnson-Roberson,
Charles
Barto,
Rounak
Mehta,
Sharath Nittur Sridhar, and Ram Vasudevan.
Driving in the
matrix: Can virtual worlds replace human-generated annotations for
real world tasks? arXiv preprint arXiv:1610.01983, 2016.
[11] Sertac Karaman and Emilio Frazzoli.
Sampling-based algorithms
for optimal motion planning. The international journal of robotics
research, 30(7):846–894, 2011.
[12] Jean Bernard Lasserre.
Moments, positive polynomials and their
applications, volume 1. World Scientific, 2009.
[13] Steven M LaValle and James J Kuffner Jr. Randomized kinodynamic
planning. The international journal of robotics research, 20(5):378–
400, 2001.
[14] Alexander Liniger and John Lygeros.
Real-time control for
autonomous racing based on viability theory.
arXiv preprint
arXiv:1701.08735, 2017.
[15] Wei Liu, Dragomir Anguelov, Dumitru Erhan, Christian Szegedy, Scott
Reed, Cheng-Yang Fu, and Alexander C Berg.
Ssd: Single shot
multibox detector. In European Conference on Computer Vision, pages
21–37. Springer, 2016.
[16] Johan Lofberg.
Yalmip: A toolbox for modeling and optimization
in matlab. In Computer Aided Control Systems Design, 2004 IEEE
International Symposium on, pages 284–289. IEEE, 2004.
[17] Anirudha Majumdar and Russ Tedrake. Funnel libraries for real-time
robust feedback motion planning. arXiv preprint arXiv:1601.04037,
2016.
[18] Anirudha Majumdar, Ram Vasudevan, Mark M Tobenkin, and Russ
Tedrake. Convex optimization of nonlinear feedback controllers via
occupation measures. The International Journal of Robotics Research,
33(9):1209–1230, 2014.
[19] Shankar Mohan and Ram Vasudevan.
Convex computation of the
reachable set for hybrid systems with parametric uncertainty.
In
American Control Conference (ACC), 2016, pages 5141–5147. IEEE,
2016.
[20] Petter Nilsson, Omar Hussien, Ayca Balkan, Yuxiao Chen, Aaron D
Ames, Jessy W Grizzle, Necmiye Ozay, Huei Peng, and Paulo
Tabuada.
Correct-by-construction adaptive cruise control: Two ap-
proaches.
IEEE Transactions on Control Systems Technology,
24(4):1294–1307, 2016.
[21] Pablo A Parrilo. Structured semidefinite programs and semialgebraic
geometry methods in robustness and optimization. PhD thesis, Cali-
fornia Institute of Technology, 2000.
[22] Victor Shia, Ram Vasudevan, Ruzena Bajcsy, and Russ Tedrake.
Convex computation of the reachable set for controlled polynomial
hybrid systems.
In Decision and Control (CDC), 2014 IEEE 53rd
Annual Conference on, pages 1499–1506. IEEE, 2014.
[23] Victor A Shia, Yiqi Gao, Ramanarayan Vasudevan, Katherine Driggs
Campbell, Theresa Lin, Francesco Borrelli, and Ruzena Bajcsy. Semi-
autonomous vehicular control using driver modeling. IEEE Transac-
tions on Intelligent Transportation Systems, 15(6):2696–2709, 2014.
[24] Li Wang, Aaron D Ames, and Magnus Egerstedt.
Safety barrier
certificates for collisions-free multirobot systems. IEEE Transactions
on Robotics, 2017.
[25] Xiangru Xu, Jessy W Grizzle, Paulo Tabuada, and Aaron D Ames.
Correctness guarantees for the composition of lane keeping and
adaptive cruise control. arXiv preprint arXiv:1609.06807, 2016.
[26] Pengcheng Zhao, Shankar Mohan, and Ram Vasudevan.
Control
synthesis for nonlinear optimal control via convex relaxations. arXiv
preprint arXiv:1610.00394, 2016.
[27] Pengcheng Zhao, Shankar Mohan, and Ram Vasudevan.
Optimal
control for nonlinear hybrid systems via convex relaxations. arXiv
preprint arXiv:1702.04310, 2017.