Partitioning of the Free Space-Time for On-Road Navigation of
Autonomous Ground Vehicles

arXiv:1801.07961v1 [cs.SY] 24 Jan 2018

Florent Altché2,1 and Arnaud de La Fortelle1
Abstract— In this article, we consider the problem of trajectory planning and control for on-road driving of an autonomous
ground vehicle (AGV) in presence of static or moving obstacles.
We propose a systematic approach to partition the collision-free
portion of the space-time into convex sub-regions that can be
interpreted in terms of relative positions with respect to a set
of fixed or mobile obstacles. We show that this partitioning
allows decomposing the NP-hard problem of computing an
optimal collision-free trajectory, as a path-finding problem in a
well-designed graph followed by a simple (polynomial time)
optimization phase for any quadratic convex cost function.
Moreover, robustness criteria such as margin of error while
executing the trajectory can easily be taken into account at the
graph-exploration phase, thus reducing the number of paths to
explore.

I. I NTRODUCTION
In order to drive on public roads, autonomous ground
vehicles (AGVs) will be required to navigate efficiently
inside a potentially dense flow of other vehicles with uncertain behaviors. For this reason, planning safe, efficient and
dynamically feasible trajectories that can be safely followed
by a low-level controller is a particularly important problem.
One of the difficulties of “optimal” trajectory planning
for AGVs is that the presence of obstacles renders the
search space non-convex, and multiple possible maneuver
variants (for which there is at least one locally optimum
trajectory [1]) exist, such as illustrated in Figure 1. At control
level, tracking the computed trajectory may involve highly
nonlinear vehicle dynamics when nearing the handling limits
(see, e.g., [2] for a review); in less demanding (low-slip)
scenarios, simpler dynamic models can be used.
Because they allow simultaneous trajectory generation
with obstacle avoidance and control computation, model
predictive control (MPC) approaches have been very popular
for AGVs (see, e.g., [3], [4]). However, real-time constraints
usually force authors considering very precise dynamic models to choose a short (sub-second) prediction horizon, which
may in turn cause the MPC problem to become infeasible,
for instance when a new obstacle is detected with not enough
time to stop. Even with simpler dynamic models, the nonconvexity of the state-space renders continuous optimization
techniques inefficient. For this reason, hierarchical frameworks [5], [6] have been proposed, in which a medium-term
(up to a dozen seconds) planner generates a rough trajectory
which is then refined by a short-term (sub-second to a
1 MINES ParisTech, PSL Research University, Centre for robotics,
60 Bd St Michel 75006 Paris, France [florent.altche,

arnaud.de_la_fortelle] @mines-paristech.fr
2 École des Ponts ParisTech, Cité Descartes, 6-8 Av Blaise Pascal, 77455
Champs-sur-Marne, France

y
2
ev

1
x

Fig. 1. Example driving situation involving multiple maneuver choices
for an AGV (denoted ev): overtake the slower (blue, denoted 1) vehicle
before the green vehicle (2) passes or wait behind the blue vehicle, possibly
overtaking after the green vehicle has passed. Solid arrows represent the
velocity of each vehicle, dotted arrows represent possible AGV trajectories.

few seconds) controller. Mixed-integer programming (MIP)
methods are often used in medium-term trajectory planning
to encode the discrete decisions arising from multiple maneuver choices [7], [8], generalized as logical constraints in [9].
However, MIP problems are known to be NP-hard [10] and
are therefore difficult to solve in real-time.
In this article, we propose a different approach for maneuver selection, inspired by the use of graph-based coordination
of robots [11] and the decomposition of the collision-free
space presented in [12] for 2D path-planning. First, we
introduce a systematic algorithm to partition the collisionfree space-time into 3D regions with geometrical adjacency
relations; the structure of on-road driving allows to assign a
semantic interpretation to each partition subset. Using time
discretization, we further divide these regions into convex
polyhedrons, and design a transition graph in which any path
corresponds to a collision-free trajectory (that may however
be dynamically infeasible). The main advantage of our approach is to reduce the entire combinatorial decision-making
process (choosing from which side to avoid each obstacle)
to the selection of a path in a graph. Once such a path
has been selected, we show that computing a corresponding
optimal trajectory (for a quadratic convex cost function) in
an MPC fashion is widely simplified and can be performed
in polynomial time.
A second advantageous property of our decomposition
approach is to simplify the use of risk metrics, which can be
directly taken into account at the graph exploration phase;
in [13], the authors used a similar partitioning technique
to design a “space margin” metric for 2D path planning.
In this article, we introduce a complementary time margin
metric, corresponding to a temporal tolerance to execute
a particular maneuver, that can be easily computed from
our graph representation. This measure is related to the
notion of “gap acceptance”, commonly used in stochastic

decision-making (see, e.g., [14]). We believe that combining
a temporal margin (notably accounting for uncertainty in
predicting the future trajectory of moving obstacles) as well
as a spatial margin (accounting for perception and control
errors) is key for trajectory planning and tracking in realworld situations, for instance coupled with MPC or Linear
Quadratic Gaussian motion planning and control [15].
Our transition graph approach generalizes state-machinebased techniques [16], [17] which rely on a predefined set of
maneuvers (such as track lane or change lane) that needs to
be manually adapted to the driving situation. By contrast, our
method can be applied in many scenarios (including highway
and urban driving, for instance crossing an intersection) with
the same formalism. Although spatio-temporal graphs have
already been used for the control of AGVs [18], [19], no
existing approach provides the same desirable properties, and
notably to easily account for margins in planning.
The rest of this article is structured as follows: in Section II, we present intuitions of our main ideas using the
example scenario of Figure 1. In Section III, we formalize
these intuitions mathematically, and we present applications
of our results to planning and control for autonomous ground
vehicles in Section IV. In Section V, we present early
simulation results and data on computation time; finally,
Section VI concludes the study.
II. A GUIDING EXAMPLE
The goal of this section is to give an intuition of our main
mathematical results using the example scenario shown in
Figure 1; the formal mathematical theory is developed in the
next section. In our example, we consider an autonomous
ground vehicle (called ego-vehicle in the remainder of this
article) navigating on a road with two other vehicles (obstacles); vehicles are modeled as rectangles driving parallel
to the side of the road. Intuitively, the ego-vehicle has
three classes of maneuvers to choose from: either it can
remain behind vehicle 1, overtake it before vehicle 2 passes,
or overtake it after vehicle 2 has passed; in [1], these
maneuver choices are linked to the notion of homotopy
classes of trajectories. Assuming that the future trajectory of
the obstacles is known in advance, it is possible to compute
the obstacle set χo of (x, y, t) positions of the ego-vehicle for
which a collision exists at time t; the complement of this set
is the collision-free region of the space-time (or free spacetime), denoted by χf . Any collision-free trajectory for the
ego-vehicle corresponds to a path in χf ; Figure 2 provides
an illustration of the free space-time in our example.
Due to the complex structure of the free space-time,
notably its non-convexity, this abstraction is difficult to
use directly to compute optimal collision-free trajectories.
Inspired by the work in [11] and [12], we propose a
decomposition of χf in convex subregions with adjacency
relations. First, we partition horizontal planes (corresponding
to fixed time instants) using relative positions with respect to
each obstacle as illustrated in Figure 3. Each subset of the
partition corresponds to positions where the ego-vehicle is
either located in front (f ), to the left (l), behind (b) or to the

t

t

y

y
x

x

Fig. 2. Free space-time χf (in white) corresponding to the situation of
Figure 1. Obstacles are pictured in the color of the corresponding vehicle.
Light-gray planes represent the road extent in the y direction. The thick
curves represent possible collision-free trajectories for the ego-vehicle.

y
(lb)

(lf )

2
(br)

(f r)

1

x
(a) t = t0 : adjt0 (lf, br) = 1, adjt0 (lb, f r) = 0

y
(lb)
(br)

(lf )
(f r)

2
1

x
(b) t = t1 : adjt1 (br, lf ) = adjt1 (lb, f r) = 0

y
(lb)
(br)

(lf )
(f r)

2
1

x
(c) t = t2 : adjt2 (br, lf ) = adjt2 (lb, f r) = 0

y
(lb)
(br)

2
1

(lf )
(f r)
x

(d) t = t3 : adjt3 (lf, br) = 0, adjt3 (lb, f r) = 1
Fig. 3. Partitioning of the 2D space at different times in our example
scenario, and adjacency relations adj. In this example, adjt (lb, br) =
adjt (lf, f r) = 1 and adjt is symmetrical at all times.

right (r) of each obstacle. Using the additional information
given by road boundaries, this partitioning technique yields
four subsets denoted by (lb), (lf ), (br) and (f r), indicating
the relative position of the ego-vehicle from obstacle 1 and
2 in this order. We call these labels signature of each subset.
Additionally, for two such subsets A and B at a given time
t, we can define an adjacency relation adjt (related to that
of [12]), such that adjt (A, B) = 1 if the intersection of their
closures is not empty, i.e. Ā ∩ B̄ 6= ∅.
This partitioning method can be generalized to the threedimensional space-time by using unions of regions sharing
the same signature, as shown in Figure 4. The notion of
adjacency described above can be extended, and we let
Adj(A, B) be the set of times t such that adjt (A, B) = 1. We
call the set Adj(A, B) the validity set of the transition from
A to B, corresponding to time periods for which a collision-

fr

br

t

t

t2

t2
t1

TABLE II
T IME MARGINS OF EXAMPLE PATHS

lf

lb

t1

y

y

x

t0

TABLE I
VALIDITY SETS Adj(A, B)
br

fr

lb

lf

[t0 , +∞)
∅
[t0 , +∞)
[t0 , t1 )

∅
[t0 , +∞)
(t2 , +∞)
[t0 , +∞)

[t0 , +∞)
(t2 , +∞)
[t0 , +∞)
∅

[t0 , t1 )
[t0 , +∞)
∅
[t0 , +∞)

free trajectory from A to B exists. The validity sets in this
example are given in Table I, with initial time t0 .
Using Table I, we can build a directed graph (that we
call transition graph) representing all the possible transitions
between cells of the partition as shown in Figure 5: each
vertex of this graph corresponds to a partition cell, and we
add the edge A → B if Adj(A, B) 6= ∅. Additionally,
we associate to each edge of the graph the corresponding
validity set. A path in this graph is given as a succession
of edges and associated transition times within the validity
set of each edge, for instance ((br → lb, t1 ), (lb → f r, t2 ))
corresponding to the maneuver of waiting for the green
vehicle (2) to pass before overtaking the blue one (1).
Between these explicit transition times, the ego-vehicle is
supposed to remain inside the last reached cell.
Using this graph-based representation also allows to compute a risk metric associated to a maneuver, called time
margin. This measure is defined as the time which remains to

[t2 , +∞)

+∞
+∞
t1 − t0

t

5τ
4τ
3τ
2τ
τ

y

y
x

0

x

Fig. 6. Discrete partitioning of the free space-time of Figure 2, with t0 = 0.

the ego-vehicle to perform a particular maneuver, before the
most constrained transition becomes impossible. To illustrate
this notion (which is formally defined in Section III), we
present example time margins for a selection of paths in
Table II.
Although this continuous approach is mathematically interesting, it is not necessarily suited for practical computer
implementation, which is generally based on time sampling.
For this reason, we also propose a discrete partitioning as
shown in Figure 6: for a discretization time step τ > 0, we
approximate the free space as a union of disjointed cylinders
of the form A × [t0 + kτ, t0 + (k + 1)τ ) where A is a subset
in the partition at time t0 + kτ . Using this time-discretized
partition, we can adapt the notion of adjacency to design a
time-discretized transition graph, as shown in Figure 7. In
this graph, a path can be simply given as a list of successive
vertices, thus allowing to use classic exploration algorithms.
The time margin of any edge in the graph can also be easily
computed (as shown in Section III). Note that it is also

lb5

+∞ (−4τ )
lb4

lb3

[t2 , +∞)

lb2

lb1

f r0

lb0

f r1

f r2

f r3

f r4

f r5

2τ

[t0 , t1 )
lf

br

Margin

br → br
lb → f r
br → lf

t

fr

lb

Most constr. trans.

((br → br, t0 ))
((br → lb, t1 ), (lb → f r, t3 ))
((br → lf, t0 ), (lf → f r, t1 ))

x

Fig. 4. Partitioning of the free space-time of Figure 2 into four cells. The
legend gives the signature of each cell, with blue obstacle first.

br
fr
lb
lf

Path

[t0 , t1 )
Fig. 5. Transition graph corresponding to Figure 4, with validity set of each
edge. Thinner edges shown in black have a validity set [t0 , +∞) (omitted
for readability).

br5

br4

br3

br2

br1

br0

lf0
τ

lf1

lf2

lf3

lf4

lf5

Fig. 7. Discrete-time transition graph and time margins corresponding to
the partition of Figure 6. Vertex Ak corresponds to the ego-vehicle being
in set A at time t0 + kτ .

r

rmax (s)
X
R

Fig. 8.

r

rmax

γ
Xγ
s

T1

rmin

rmin (s)

Frenet coordinates of a point on the road.

possible to perform an event-based (instead of constant-timebased) partition, which has the advantage of exactly matching
the time instants when the adjacency of two cells changes.
However, since the partitioning will ultimately be used using
the ego-vehicle’s feasible dynamics – which are not easily
integrated into an event-based framework – the constant-time
discretization is preferred.
We believe that the proposed graph-based representation
has two main advantages. First, the combinatorial part of the
trajectory planning problem, consisting in choosing a feasible
maneuver around the obstacles, is reduced to selecting a path
in a transition graph. We will show in Section IV that, once
such a path is given, computing a corresponding optimal
trajectory becomes relatively simple for a large class of
cost functions. Second, the graph approach makes it easy
to take into account safety margins by avoiding exploration
of time-constrained edges, which can be useful to handle
uncertainty in trajectory estimation. Additional metrics can
also be computed (see, e.g., [13]) for spatial constraints, in
order to account for control or positioning error.
III. M ATHEMATICAL RESULTS
A. Modeling
We now proceed to theorize and generalize the intuitions
exposed in the previous section. We consider an autonomous
ego ground vehicle, driving on a road in presence of obstacles, which can either be fixed or mobile. The ego-vehicle is
assumed to remain parallel to the local direction of the road,
as it usually is the case in normal (non-crash) situations, so
that its configuration is given by the position of its center of
mass, denoted by (x, y) in ground coordinates.
We assume that the ego-vehicle has knowledge of the road
geometry, for instance through cartography, as a C 2 reference
path γ and bounds on the lateral deviation from γ, as shown
in Figure 8; we let R ⊂ R2 be such that the ego-vehicle is
on the road if, and only if, (x, y) ∈ R. Finally, we assume
that the road curvature and width are such that, for all X =
(x, y) ∈ R, there exists a unique point Xγ ∈ γ which is
closest to X.
According to Figure 8, we define the Frenet coordinates of
X as (s(X), r(X)), where s(X) is the curvilinear position
of the corresponding point Xγ along γ, and r(X) = (X −
Xγ )·N with (T, N) the Frenet frame of γ at point Xγ . With
these notations, we let rmin and rmax be such that X ∈ R
if, and only
 if, rmin (s(X)) ≤ s(X) ≤ rmax (s(X)), and we
let Q = (s, r) ∈ R2 : rmin (s) ≤ r ≤ rmax (s) denote the
extent of the road in Frenet coordinates. In what follows, we

Q

Fig. 9.

TK
s

Decomposition of the road (in grey) in trapezes.

only consider the Frenet coordinates of the ego-vehicle, and
we drop the dependence of s and r in X. We assume that
the ego-vehicle only moves forward along the road, in the
direction of increasing s.
We denote by O the set of obstacles (considered as open
sets) existing on the road around the ego-vehicle, and by
N = |O| the number of obstacles. At a given time t0 , we
consider a time horizon T and we assume that an estimation
of the trajectory of each obstacle o ∈ O is available over
[t0 , t0 + T ]. This estimation could come, for instance, be
performed through machine learning techniques [20]. We let
χ = Q × [t0 , t0 + T ] the set of space-time points for which
the vehicle is on the road, and we define the free portions
of the space (respectively, of the space-time) as follows:
Definition 1 (Free space): The S
(collision-)free space at
time t is the set Qtf = Q \ o∈O o. The (collision)free space-time over [to0 , t0 + T ] is the χf
=
n
t
Qf × t : t ∈ [t0 , t0 + T ] .
We call obstacle space-time χo the complement of χf in χ.
Note that the free space-time is similar to the notion of configuration space(-time), which is widely used in robotics [21],
and can be computed efficiently [22] provided that each
obstacle’s trajectory is known in advance. In this article, we
suppose perfect knowledge of these future trajectories over
[t0 , t0 + T ]; however, probabilistic trajectory estimates can
also be taken into account, for instance by defining χpf as
the set of points of χ which are free with probability p.
To simplify the rest of the presentation, we consider that
for all t1 ∈ [t0 , t0 + T ], the intersection of the obstacle
space-time χo is a union of (potentially rotated) rectangles;
due to the roughly rectangular shape of classical vehicles,
this assumption does not excessively sacrifice precision.
Moreover, we assume that the road boundary functions
rmin and rmax are piecewise-linear and continuous. In the
following subections, we present our approach to partition
χf into semantically meaningful subsets using a two-step
algorithm: first, we partition planes corresponding to a fixed
time t1 ∈ [t0 , t0 + T ] in Section III-B; second, we deduce a
partition of χf in Sections III-C and III-D.
B. Semantic free-space partitioning
First, note that we can use trapeze decomposition to
partition the road in convex regions using rmin and rmax as
shown in Figure 9; since the road profile does not depend on
time, this decomposition allows to fully partition χ by using
cylinders with trapezoidal base. Each trapeze Tk (with k ∈

Algorithm 1 Partitioning of Qtf1

r
Co2
Co3

Coobs
Co4

Co1
s

Fig. 10. Partitioning of the 2D space around a single obstacle (Coobs ) into
four collision-free regions Coi .

{1 . . K}) can be defined by a set of linear constraints1 , in the
form Ak X ≤ bk with Ak a 4-by-2 matrix, X = [s, r]T and
bk a vector of R4 . This approach allows modeling varying
roadway width and curvature, but may require an important
number of trapezes to correctly handle sharp bends.
For a single rectangular obstacle o at time t1 , we define
four regions Coi ⊂ R2 (i ∈ {1 . . 4}) as illustrated in
Figure 10; as in Section II, these regions can be identified as
positions where the ego-vehicle is located in front, to the left,
behind or to the right of the obstacle. Similarly, we let Coobs
be the obstacle region corresponding to o. Since all obstacles
are assumed rectangular, each of the Coi regions is defined by
a set of linear constraints1 in the form Aio X ≤ bio , with Aio
a two-column matrix, X = [s, r]T and bio a vector having
the same number of lines as Aio . The partition of the free
space Qtf1 can be built recursively according to Algorithm 1;
Theorem 1 ensures the validity of this algorithm; we let P t1
be the partition of Qtf1 obtained by Algorithm 1.

P0 ← {Tk }k=1..K
. Initialize P0 as a partition of Q
N ← |O|
for n = 1 . . N do
. Loop over all obstacles on
Pn ← {}
for all C ∈ Pn−1 do
. Loop over cells C in Pn−1
for j = 1 . . 4 do
if Cojn ∩ C 6= ∅ then
. Partition C \ Coobs
n
j
Pn ← Pn ∪ {Con ∩ C}
end if
end for
end for
end for
P t1 ← PN

From the previous proof, we deduce that our partitioning
of Qtf1 bijectively corresponds to relative positions from all
N obstacles in the free space at time t1 . Thus, each element
in the partition can be uniquely defined by a signature, as
stated in Corollary 1 and Definition 2:

Definition 2 (Signature): We call σt1 (e) ∈ Σ from Corollary 1 the signature of subset e.
Moreover, there is a finite number of elements in the
partition which is bounded by K4N for K trapezes and
N obstacles. Additionally, all elements e ∈ P t1 also are
convex polygons (or cells), which can be fully described
using a single (matrix, vector) pair that can easily be stored in
computer memory. Figures 11 and 12 illustrate our partitioning in a more complex scenario2 with 3 vehicles; note that,
for clarity purposes, we respectively used f, l, b, r instead
of 1, 2, 3, 4 as defined in Definition 2. Also remark that,
although Figure 11 is shown in world coordinates (x, y),
Figure 12 uses Frenet coordinates (s, r). In order to encode
the relation between elements of the partition, we introduce
the notion of adjacency as follows:
Definition 3 (Adjacency): For e1 , e2 ∈ P t1 , we say that
e1 and e2 are adjacent if, and only if the intersection of their
closures is not empty, i.e. e1 ∩ e2 6= ∅. For σ, σ 0 ∈ Σ, we let
adjt1 (σ, σ 0 ) = 1 if σt−1
(σ) and σt−1
(σ 0 ) are adjacent, and 0
1
1
otherwise.
Note that this definition considers cells whose closures
intersect at single point as adjacent; this situation could
happen, e.g., in the case shown in Figure 3b between (br)
and (lf). From a theoretical standpoint, the hypothesis that
obstacles are open sets allows to overcome this issue since,
in this case, the vehicle can actually perform the maneuver;
in practice, the use of safety margins around the obstacles,
and constraints on time margin (see Section III-D) prevent
this question from becoming an issue. The main reason for
choosing this slightly looser criterion compared, e.g., to that
of [12] – requiring the intersection to be a non-singleton
segment – is that it can be verified in polynomial time using
the matrix inequality representation and linear programming.

Corollary 1 (Semantization): For all e ∈ P t1 , there exists
a unique tuple σt1 (e) = (k, jT
1 , . . . , jN ) ∈ {1 . . K} ×
{1 . . 4}N such that e = Tk ∩ n=1..N Cojnn . Using Σ =
{1 . . K} × {1 . . 4}N , σt1 is a bijection from Σ to P t1 ∪ ∅.

C. Continuous free space-time partitioning
We now proceed to generalize these results to the free
space-time χf ; to define a partition of this space we use the
union (over time) of cells sharing the same signature:

1 To ensure the sets are disjoint, some of the inequalities should be strict.
In practice, we use non-strict inequalities with a small tolerance ε.

2 Obstacle regions 1, 2, 3 in Figure 12 are computed for a point-mass
ego-vehicle, in order to match the vehicle shapes shown in Figure 11.

Theorem 1 (Partition): P t1 is a partition of Qtf1 .

Proof: We will proveSthat, for all 0 ≤ n ≤ N , Pn is
n
a partition of Qn = Q \ i=1 Coobs
. First, this property is
i
verified for P0 which is a partition of Q. Second, the loop
preserves the following invariants for all n ≥ 1 and e ∈ Pn :
0
0
• e 6= ∅ and ∃e ∈ Pn−1 such that e ⊂ e ;
j
• for all 1 ≤ i ≤ n, ∃j ∈ {1 . . 4} such that e ⊂ Coi .

Thus, Pn = e ∩ Cojn e ∈ Pn−1 , j ∈ {1 . . 4}, e ∩ Cojn 6= ∅ .
and
Since the sets Cojn j=1..4 define a partition of R2 \ Coobs
n
since all e ∈ P0 is a subset of Q, we deduce by induction
that all elements of Pn are nonempty subsets of Qn .
Reciprocally, for all any q ∈ Qn = Qn−1 \ Coobs
there
n
exists j ∈ {1 . . 4} such that q ∈ Qn−1 ∩ Cojn . Since
P
S 0 is
a partition of Q, inductive reasoning yields Qn ⊂ e∈Pn e.

path, we can now define the corresponding time margin as
the time left for the vehicle to perform the most constrained
transition before it becomes impossible:
Definition 8 (Time margin along a path): Consider
a
path in G c given as π c = (σ1 , . . , σm+1 ), (t1 , . . , tm ) .
c
The time margin
along π c
is V (π
=
n
o)
mini=1...m sup t − ti [ti , t) ⊂ Adj(σi , σi+1 ) .

2
y
ev

1
x

3
Fig. 11.

A more complicated example with 3 obstacles.

2

r
lrl
brl

D. Discrete-time partitioning

1

lf l
f rl f f l

lll llf llr
f ll f lf llr

s

3
Fig. 12. Partitioning in the example of Figure 11, with subsets signature;
for instance, brl means that the ego-vehicle is behind the first vehicle, to the
right of the second and left of the third. The thick black lines correspond to
the unique trapeze encoding road boundaries; its index is omitted for clarity.

(Space-time cell): For σ ∈ Σ, we let Eσ =
S Definition 4 −1
σ
t1 ∈[t0 ,t0 +T ] t1 (σ) × {t1 } be the space-time cell corresponding to σ, i.e. the set of all points in the free space-time
sharing this signature.
Since the set of non-empty elements of σt−1 (Σ) defines a
partition of Qtf , the set P = {Eσ | σ ∈ Σ, Eσ 6= ∅} defines a
partition of χf (see Figure 4). The notion of adjacency can
then be generalized as follows:
Definition 5 (Validity set): For σ, σ 0 ∈ Σ, we define the
validity set Adj(σ, σ 0 ) = {t ∈ [t0 , t0 + T ] | adjt (σ, σ 0 ) = 1}.
Using this notion, we can now define a transition graph
(see Figure 5) as follows:
Definition 6 (Continuous transition graph): The continuous transition graph is the directed graph G c = (V c , E c , Adj)
with
vertex set V c = {σ ∈ Σ | Eσ 6= ∅}, edges set E c =

(σ1 , σ2 ) ∈ Σ2 Adj(Eσ1 , Eσ2 ) 6= ∅ and associated validity set Adj.
The motivation for introducing this graph is that any
collision-free maneuver corresponds to a unique path in G c .
To account for the temporal aspect of this graph, such a path
is defined as follows:
Definition 7 (Path in G c ): A path in G c is given by a list
of vertices (σ1 , . . . , σm+1 ) ∈ V c so that for all i ≤ m,
(σi , σi+1 ) ∈ E c , and a list of strictly increasing transition
times (t1 , . . . , tm ) such that for all i ∈ {1 . . m}, ti ∈
Adj(σi , σi+1 ) and [ti , ti+1 ) ⊂ Adj(σi , σi ).

In other words, a path in G c is a sequence of cells and
time instants corresponding to the transition time between
two successive cells; between two consecutive transitions, the
ego-vehicle can remain in the cell it occupies last. For a given

Due to the potentially complex trajectories followed by the
obstacles, there is no guarantee regarding the topology of the
subsets Eσ , which can for instance have multiple connected
components; similarly, Adj(σ1 , σ2 ) is in general a union of
disjointed intervals. To make practical applications easier,
we also propose a temporal discretization of the free spacetime with a time step duration τ (with T = P τ ). Note that
the value of τ depends on a trade-off between acceptable
computation time, length of the planning horizon and required precision in vehicle dynamics; Section V provides
some performance reports on the influence of this parameter.
We approximate Eσ as a union of box-shaped cells:
Definition 9 (Discrete space-time cell): For σ ∈ Σ and
p ∈ {0 . . . P }, we let Eσp = σθ−1
(σ) × [θp , θp+1 ) be the
p
discrete space-time cell corresponding to σ at step p, with
θp = t0 + pτ .
In the rest of this article, we assume3 that Eσp ⊂ χf for
all σ ∈ Σ and p ∈ {0 . . P }. Since σθ−1
(σ) is a convex (or
p
empty) set, Eσp is either empty or convex, and fully defined
by a set of linear inequalities in the form A[X, t]T ≤ b
(the comments of footnote 1 also apply here). Finally, we
define a partition of the free space-time χf in convex boxshaped cells as P τ = {Eσp |p ∈ {0 . . P }, σ ∈ Σ, Eσp 6= ∅}
(see Figure 6), and we associate a discrete transition graph:
Definition 10 (Discrete transition graph): The
discrete transition graph is the directed graph
G d =n(V d , E d ) with vertex set V d = P τ and edges oset
∈ V d , adjθp (σ1 , σ2 ) = 1 .
) Eσp1 , Eσp+1
E d = (Eσp1 , Eσp+1
2
2

Therefore, each vertex of G corresponds to a certain cell
of the partition P τ at a given time θp , and the edge v1 → v2
exists if v1 and v2 represent two adjacent cells (possibly
twice the same) at two consecutive time steps. Paths in G d
comply with the usual definitions of graph theory and can
be given as a set of vertices.
Finally, we define the time margin for paths in G d as:
Definition 11 (Time margin in the discrete graph): For a
path π d = Eσ00 , . . . , Eσm+1
in G d , the time margin is
m+1
d
v(π ) = min max ṽ(i, p) with ṽ(i, p) = τ (p − i +
i=0...m p=i...m

1) ∀q ∈ {i . . p}, adjθq (σi , σi+1 ) = 1 .
Note
that ṽ(i, p) is the discrete-time equivalent of the set

t − ti [ti , t) ⊂ Adj(σi , σi+1 ) from Definition 8, and this
discrete time margin is analogous to that of Definition 8.
3 This assumption requires taking slight safety margins, the size of which
being proportional to the distance traveled by the obstacles during the
duration of a discretization time step τ .

IV. A PPLICATION TO PLANNING AND CONTROL
Before presenting the applications of our approach to
planning and control, let us formalize the link between paths
in the transition graph and trajectories for the ego-vehicle:
Definition 12 (Interpretation of transition
 graph paths):
Let π0c = (σ1 , . . . , σm+1 ), (t1 , . . . , tm ) be a path in G c
and x(t) be a collision-free trajectory for the ego-vehicle.
We say that
 π0 corresponds to x if, for all i ∈ {1 . . m},
x [ti−1 , ti ) ⊂ Eσi and x [tm , T ) ⊂ Eσm+1 .

From this definition, we deduce that for a given collisionfree trajectory x(t) there exists a unique corresponding path
in G c denoted by π(x). Reciprocally, for a given path π0 in
G c , there exists a set of corresponding trajectories denoted
by π −1 (π0 ). We obtain the following theorem:

x0 realizing

min

(xp )∈π −1 (π0 ),up ∈U

J(x, u) can be computed in

polynomial time in the number of obstacles and time steps.
Proof: We will show that this problem is an instance
of convex quadratic programming (QP), which has a complexity O(n3 ) where n is the number of constraints [23].
First, the cost function J is quadratic and convex. Second,
vehicle dynamics and control bounds can be encoded as
linear constraints. Moreover, the condition (xp ) ∈ π −1 (π0 )
corresponds to a set of O(P N ) linear constraints,
leading to

a QP problem with complexity O (P N )3 for N obstacles
over P time steps, thus proving the announced result.

An interesting feature of this decomposition of the trajectory planning problem is that we effectively separate the
discrete choice of a maneuver variant, and the search for
an optimal control corresponding to this maneuver as was
obtained in [12] for path-planning. This second problem
can be solved efficiently under certain assumptions on the
vehicle dynamics. We consider an AGV with linear discrete
dynamics xp+1 = Axp + Bup for a state xp and a control
up ∈ U (with U a convex polyhedron) with a discretization
time step τ , where A and B have constant coefficient.
For a positive semi-definite matrix Q, a line vector L and
defining Xp = [xTp , uTp ]T , we consider a generic quadratic
cost function J(x, u) = 12 XpT QXp + LT Xp . In this case, an
optimal solution can be computed in polynomial time:

V. S IMULATION RESULTS
To showcase the advantages of our approach and the
axes for improvement, we present preliminary simulation
results in the scenario of Figure 1. For illustration purposes,
we consider a very simple second-order
dynamics
for the


ego-vehicle as: Xp+1 = 00 I02 Xp + I02 up with Xp =
[s, r, ṡ, ṙ]Tp , up = [alon , alat ]Tp and I2 the R2 identity. To
account for the nonholonomic constraints, we bound the
lateral velocity as |ṙp | ≤ αṡp with α > 0 a parameter, and
we also require alon and alat to be bounded.
objective
2 The
P
+ ṙp2 + rp2 , and
function is chosen as J = p ṡp − sref
p
we use a planning horizon of 10 s with a time step τ = 1 s,
and a minimum time margin of 1 s.
One difficulty in the proposed approach is the need to
ensure that the selected optimal path in the transition graph
is dynamically feasible. In this early implementation, we
use a naive approach consisting in computing the optimal
trajectory corresponding to each explored branch; as stated
in Theorem 3, this computation can be performed relatively
fast. Additionally, we use a branch-and-bound technique to
prune dynamically infeasible or poor quality branches. The
trajectories from both algorithms are shown in Figure 13;
Table III reports the computation time for all steps of our algorithm using a standard desktop computer. For comparison
purposes, we added an implementation of the same problem
using a pure mixed-integer quadratic programming (MIQP)
method from [9]. We use an implementation of Seidel’s linear
programming algorithm [24] to perform the partitioning, and
Gurobi [25] in version 7.5 to solve quadratic programming
(trajectory optimization) and MIQP (from [9]) problems.
Although the MIQP is faster overall, it is not able to take
notions such as time margin into account; this results in the
ego-vehicle trying to overtake the blue vehicle (1) before
the green one (2), which is a higher-risk maneuver that is
excluded by our algorithm as shown in Figure 13; when
removing the time margin constraints, both solvers converge
to the same optimum. A likely explanation for the better
performance of the pure MIQP method is that it can directly
use vehicle dynamics to guide the exploration; future work
will focus on designing an hybrid algorithm between these
methods, to benefit from the advantages of both approaches.

Theorem 3 (Polynomial time computability): Let π0 be a
path in G d and x0 an initial AGV state. The optimal trajectory (and associated control sequence) (Xp ) starting from

VI. C ONCLUSION
This article generalized the divide-and-conquer approach
used in [12] in two dimensions, to the 3D space-time for on-

Theorem 2 (Motion-planning equivalence): Let J(x) be
a cost function for a given trajectory x(t), X the set of
collision-free trajectories, and Π the set of paths in G c . Then:


min J(x) = min
min
J(x)
(1)
−1
x∈X

π0 ∈Π

x∈π

(π0 )

Proof:
From the previous definition, for any
collision-free trajectory x ∈ X there exists π0 ∈
Π such that π(x) = π0 . Therefore, minx∈X J(x) ≤
minπ0 ∈Π minx∈π−1 (π0 ) J(x) . Reciprocally, for all π0 ∈ Π,
any x ∈ π −1 (π0 ) is guaranteed to be collision-free, leading
to the reciprocal inequality.
In other words, it is equivalent to find an optimal trajectory
for the ego-vehicle, and to find an optimal path in the transition graph G c and then the optimal trajectory corresponding
to this path. These results can be extended to paths in the
discrete graph G d ; due to space limitations, details are not
presented here and we only provide the following definition:
Definition 13 (Interpretation of discrete graph paths):
Let τ > 0 be a discretization time step, π0d =
) be a path in G d and x(t) be a collision(Eσ00 , . . . , Eσm+1
m+1
free trajectory for the ego-vehicle. We say that π0d
corresponds to x if, for all p ∈ {0 . . m}, x(θp ) ∈ Eσpp and
x([θp , θp+1 )) ⊂ Eσpp ∪ Eσpp+1 .

t = 0 s:
t = 2 s:

[6]

t = 3 s:
t = 4 s:

[7]

t = 5 s:
[8]

t = 6 s:
Fig. 13. Trajectories computed by both algorithms (MIQP in red, our graphbased approach in thicker cyan) for a time margin of 1 s. The rectangles
correspond to the vehicles of Figure 1; the circles to the ego-vehicle position.

[9]

TABLE III
S UMMARY OF COMPUTATION TIME ( AVERAGE OVER 100 ITERATIONS )
[10]

Algorithm

Comp. time

Objective val.

Partitioning
Graph exploration (1 s margin)
Optimal path computation

2.5 ms
27.6 ms
< 1 ms

13.89
-

[11]

MIQP [9]
Our graph exploration (no margin)

14.0 ms
28.4 ms

13.58
13.58

[12]

road navigation of autonomous ground vehicles in presence
of moving obstacles. We described a systematic method to
partition the collision-free space-time in the presence of fixed
or moving obstacles, and we provided a graph representation
of all possible collision-free trajectories. This approach allows to treat the combinatorial problem of optimal trajectory
planning in two steps: first, a path-finding problem in a graph,
and then a simple optimization that can be performed in
polynomial time in the number of obstacles for any quadratic
cost function. Moreover, we introduced a notion of time
margin and showed that our graph-based approach can easily
take into account margin of error in the execution for a
particular maneuver. Coupled with additional similar metrics,
we believe that our approach can have useful applications for
planning under prediction and control uncertainty, notably in
the frame of stochastic decision-making. Future work will
also focus on improving our graph exploration algorithm to
allow faster computation.

[13]

[14]

[15]

[16]
[17]

[18]

[19]

R EFERENCES
[1] P. Bender, O. S. Tas, J. Ziegler, and C. Stiller, “The Combinatorial
Aspect of Motion Planning: Maneuver Variants in Structured
Environments,” pp. 1386–1392, 2015.
[2] J. Kong, M. Pfeiffer, G. Schildbach, and F. Borrelli, “Kinematic and
dynamic vehicle models for autonomous driving control design,” in
2015 IEEE Intelligent Vehicles Symposium (IV). IEEE, jun 2015,
pp. 1094–1099.
[3] P. Falcone, F. Borrelli, J. Asgari, H. E. Tseng, and D. Hrovat,
“A model predictive control approach for combined braking and
steering in autonomous vehicles,” in 2007 Mediterranean Conference
on Control & Automation. IEEE, jun 2007, pp. 1–6.
[4] M. A. Abbas, R. Milman, and J. M. Eklund, “Obstacle avoidance in
real time with Nonlinear Model Predictive Control of autonomous
vehicles,” in 2014 IEEE 27th Canadian Conference on Electrical and
Computer Engineering (CCECE). IEEE, may 2014, pp. 1–6.
[5] R. V. Cowlagi and P. Tsiotras, “Hierarchical motion planning with
kinodynamic feasibility guarantees: Local trajectory planning via

[20]
[21]
[22]
[23]
[24]
[25]

model predictive control,” in 2012 IEEE International Conference on
Robotics and Automation. IEEE, may 2012, pp. 4003–4008.
X. Qian, A. De La Fortelle, and F. Moutarde, “A hierarchical model
predictive control framework for on-road formation control of autonomous vehicles,” in Intelligent Vehicles Symposium (IV), 2016
IEEE. IEEE, 2016, pp. 376–381.
T. Schouwenaars, É. Féron, and J. How, “Safe receding horizon
path planning for autonomous vehicles,” Proceedings of the Annual
Allerton Conference on Communication Control and Computing,
vol. 40, no. 1, pp. 295–304, 2002.
S. J. Anderson, S. C. Peters, T. E. Pilutti, and K. Iagnemma, “Design
and development of an optimal-control-based framework for trajectory
planning, threat assessment, and semi-autonomous control of passenger vehicles in hazard avoidance scenarios,” Springer Tracts in
Advanced Robotics, vol. 70, no. STAR, pp. 39–54, 2011.
X. Qian, F. Altché, P. Bender, C. Stiller, and A. de La Fortelle,
“Optimal trajectory planning for autonomous driving integrating
logical constraints: An MIQP perspective,” in 2016 IEEE 19th
International Conference on Intelligent Transportation Systems
(ITSC). IEEE, nov 2016, pp. 205–210.
R. M. Karp, “Reducibility among combinatorial problems,” in Complexity of computer computations. Springer, 1972, pp. 85–103.
J. Gregoire, S. Bonnabel, and A. de La Fortelle, “Prioritybased intersection management with kinodynamic constraints,” in
2014 European Control Conference (ECC). IEEE, jun 2014, pp.
2902–2907.
J. Park, S. Karumanchi, and K. Iagnemma, “Homotopy-Based Divideand-Conquer Strategy for Optimal Trajectory Planning via MixedInteger Programming,” IEEE Transactions on Robotics, vol. 31, no. 5,
pp. 1101–1115, 2015.
A. Constantin, J. Park, and K. Iagnemma, “A margin-based approach
to threat assessment for autonomous highway navigation,” in 2014
IEEE Intelligent Vehicles Symposium Proceedings, vol. 12, no. 4.
IEEE, jun 2014, pp. 234–239.
S. Lefevre, C. Laugier, and J. Ibanez-Guzman, “Evaluating risk at road
intersections by detecting conflicting intentions,” in 2012 IEEE/RSJ
International Conference on Intelligent Robots and Systems. IEEE,
oct 2012, pp. 4841–4846.
J. van den Berg, P. Abbeel, and K. Goldberg, “LQG-MP: Optimized
path planning for robots with motion uncertainty and imperfect
state information,” The International Journal of Robotics Research,
vol. 30, no. 7, pp. 895–913, jun 2011.
Q. Wang, T. Weiskircher, and B. Ayalew, “Hierarchical Hybrid
Predictive Control of an Autonomous Road Vehicle,” in 2015
Dynamic Systems and Control Conference. ASME, oct 2015.
Q. Wang, B. Ayalew, and T. Weiskircher, “Optimal assigner decisions
in a hybrid predictive control of an autonomous vehicle in public
traffic,” Proceedings of the American Control Conference, vol. 2016July, pp. 3468–3473, 2016.
M. Ono, G. Droge, H. Grip, O. Toupet, C. Scrapper, and A. Rahmani,
“Road-following formation control of autonomous ground vehicles,”
in 2015 54th IEEE Conference on Decision and Control (CDC), no.
Cdc. IEEE, dec 2015, pp. 4714–4721.
T. Gu, J. Atwood, C. Dong, J. M. Dolan, and Jin-Woo Lee, “Tunable
and stable real-time trajectory planning for urban autonomous
driving,” in 2015 IEEE/RSJ International Conference on Intelligent
Robots and Systems (IROS). IEEE, sep 2015, pp. 250–256.
F. Altché and A. de La Fortelle, “An LSTM network for highway
trajectory prediction,” in Intelligent Transportation Systems (ITSC),
2017 IEEE 19th International Conference on. IEEE, 2017.
M. Erdmann and T. Lozano-Perez, “On multiple moving objects,”
in Robotics and Automation. Proceedings. 1986 IEEE International
Conference on, vol. 3. IEEE, 1986, pp. 1419–1424.
S. Kockara, T. Halic, K. Iqbal, C. Bayrak, and R. Rowe, “Collision
detection: A survey,” in Systems, Man and Cybernetics, 2007. ISIC.
IEEE International Conference on. IEEE, 2007, pp. 4046–4051.
S. A. Vavasis, Complexity theory: quadratic programming Complexity
Theory: Quadratic Programming. Boston, MA: Springer US, 2009,
pp. 451–454.
R. Seidel, “Small-dimensional linear programming and convex hulls
made easy,” Discrete & Computational Geometry, vol. 6, no. 1, pp.
423–434, 1991.
Gurobi Optimization, Inc., “Gurobi optimizer reference manual,”
2017.

