Optimal Sampling-Based Motion Planning under
Differential Constraints: the Driftless Case
Edward Schmerling, Lucas Janson, Marco Pavone
Abstract— Motion planning under differential constraints is
a classic problem in robotics. To date, the state of the art is
represented by sampling-based techniques, with the Rapidly-
exploring Random Tree algorithm as a leading example. Yet, the
problem is still open in many aspects, including guarantees on
the quality of the obtained solution. In this paper we provide a
thorough theoretical framework to assess optimality guarantees
of sampling-based algorithms for planning under differential
constraints. We exploit this framework to design and analyze
two novel sampling-based algorithms that are guaranteed to
converge, as the number of samples increases, to an optimal
solution (namely, the Differential Probabilistic RoadMap al-
gorithm and the Differential Fast Marching Tree algorithm).
Our focus is on driftless control-affine dynamical models, which
accurately model a large class of robotic systems. In this paper
we use the notion of convergence in probability (as opposed to
convergence almost surely): the extra mathematical flexibility
of this approach yields convergence rate bounds — a first in
the field of optimal sampling-based motion planning under
differential constraints. Numerical experiments corroborating
our theoretical results are presented and discussed.
I. INTRODUCTION
Motion planning is a fundamental problem in robotics. It
involves the computation of a sequence of actions that drives
a robot from an initial condition to a terminal condition
while avoiding obstacles, respecting kinematic/dynamical
constraints, and possibly optimizing an objective function
[1]. The basic problem, where a robot does not have any
constraints on its motion and only an obstacle-free solution is
required, is well-understood and solved for a large number of
practical scenarios [2]. On the other hand, robots do usually
have stringent kinematic/dynamical (in short, differential)
constraints on their motion, which in most settings need to be
properly taken into account. There are two main approaches
[2]: (i) a decoupling approach, in which the problem is
decomposed in steps of computing a geometric collision-free
path (neglecting the differential constraints), smoothing the
path to satisfy the motion constraints, and finally reparam-
eterizing the trajectory so that the robot can execute it, or
(ii) a direct approach, in which the differentially-constrained
motion planning problem (henceforth referred to as DMP
problem) is solved in one shot. The first approach, while
fairly common in practice, has several drawbacks, including
Edward
Schmerling
is
with
the
Institute
for
Computational
&
Mathematical
Engineering, Stanford University, Stanford, CA 94305,
schmrlng@stanford.edu.
Lucas Janson is with the Department of Statistics, Stanford University,
Stanford, CA 94305, ljanson@stanford.edu.
Marco Pavone is with the Department of Aeronautics and Astronautics,
Stanford University, Stanford, CA 94305, pavone@stanford.edu.
This work was supported by an Early Career Faculty grant from NASA’s
Space Technology Research Grants Program (Grant NNX12AQ43G).
the computation of very inefficient trajectories, failure in
finding a trajectory due to the decoupling scheme itself, and
inflated information requirements [2]. This motivates a quest
for efficient algorithms that directly solve the DMP problem.
However, directly finding a feasible, let alone optimal,
solution to the DMP problem is difficult (note that the basic
version without differential constraints is already PSPACE-
hard [3, 1], which implies NP-hard). Early work on this
topic dates back to more than two decades ago [4], but the
problem, especially when optimality is taken into account, is
still open in many aspects [5, 2], including algorithms with
practical convergence rates, guarantees on the quality of the
obtained solution, and class of dynamical systems that can
be addressed. To date, the state of the art is represented by
sampling-based techniques, where an explicit construction
of the configuration space is avoided and the configuration
space is probabilistically “probed” with a sampling scheme.
Arguably, the most successful algorithm for DMP is the
sampling-based rapidly-exploring tree algorithm (RRT) [6],
which incrementally builds a tree of trajectories by randomly
sampling points in the configuration space. Lately, several
variations of the RRT algorithm, referred to as RRT∗, stem-
ming from [7] and its kinodynamic extension [8], have been
considered to ensure that the cost of the computed trajectory
converges to the optimal cost as the number of sampled
points goes to infinity [8, 9, 10, 11, 12]. These works, while
providing strong experimental validation, only provide proof
sketches that do not fully address many of the complications
that arise in extending asymptotic optimality arguments from
the geometric case to differentially constrained paths. For
example, rewiring the RRT tree within a local volume con-
taining (in expectation) a log fraction of previous samples is
not sufficient in itself to claim optimality, as in [10, 11]. Ad-
ditional assumptions on trajectory approximability must be
stated and verified for the differential constraints in question.
Such requirements are discussed in [8], but it is not clear how
assuming the existence of forward-reachable trajectory ap-
proximations is sufficient for a “ball-to-ball” proof technique
that requires backward approximations as well. A different
approach to asymptotically optimal planning has recently
been proposed by STABLE SPARSE RRT which achieves
optimality through random control propagation instead of
connecting existing samples using a steering subroutine [13].
This paper, like the RRT∗variations, follows a steering
approach, although it may be considered less general, as it
is our view that leveraging as much knowledge as possible
of the differential constraints while planning is necessary in
order to have a hope of planning in real-time.
arXiv:1403.2483v2  [cs.RO]  2 Mar 2015
Statement of Contributions: The objective of this paper is
to provide a theoretical framework to study optimality guar-
antees of sampling-based algorithms for the DMP problem,
and to exploit this framework to design efficient sampling-
based algorithms that are guaranteed to asymptotically con-
verge to an optimal solution. The focus of this paper is on
driftless control-affine (DCA) dynamical systems of the form
˙x(t) =
m
X
i=1
gi(x(t))ui(t),
where the available motions of trajectories x(t) are linear
combinations given by input control functions ui(t) and their
corresponding actions at each point in space gi(x). Our work
is written from the perspective of optimizing trajectory arc
length, but applies also to cost metrics satisfying similar
properties, which we discuss. This model is often the result
of nonholonomic constraints that the kinematic variables of
the system must satisfy [14]. A large class of robotic systems
can be modeled as DCA, including mobile robots with
wheels that roll without slipping [1], multi-fingered robotic
hands [15], rigid bodies with zero angular momentum un-
dergoing re-orientation [16], and systems with nonholonomic
actuators [14]. The DCA model, however, rules out the pres-
ence of dynamics with drift, which in many important cases
(e.g., spacecraft control) can not be neglected. Specifically,
the contribution of this paper is threefold. First, we show that
any trajectory of a DCA system may be “traced” arbitrarily
well by connecting randomly distributed points from a suf-
ficiently large sample set covering the configuration space.
We will refer to this property as probabilistic exhaustivity,
as opposed to probabilistic completeness [1], where the
requirement is that at least one trajectory is traced with a suf-
ficiently large sample set. Probabilistic exhaustivity is a key
tool in proving asymptotic optimality, and is of independent
interest. Second, we introduce two novel sampling-based
algorithms for the solution to the DMP with DCA systems,
namely the Differential Probabilistic RoadMap algorithm
(DPRM∗) and the Differential Fast Marching Trees algorithm
(DFMT∗). Third, by leveraging the property of probabilistic
exhaustivity for DCA systems, we rigorously show that both
DPRM∗and DFMT∗are asymptotically optimal. We note
that in this paper we use the notion of convergence in
probability (as opposed to convergence almost surely, as in,
e.g., [11]): the extra mathematical flexibility of this approach
yields convergence rate bounds — a first in the field of
optimal sampling-based motion planning under differential
constraints. Our approach is inspired by [11] and [17].
Organization: This paper is structured as follows. In
Section II we provide a review of some key results in
differential geometry that will be extensively used in the
paper. In Section III we formally define the problem we wish
to solve, while in Section IV we prove the aforementioned
probabilistic exhaustivity property for DCA systems. In Sec-
tion V we present the DPRM∗and DFMT∗algorithms, and
in Section VI we prove their asymptotic optimality (together
with a convergence rate characterization). In Section VII we
provide implementation details for the proposed algorithms,
and we study them via numerical experiments. Finally, in
Section VIII, we draw some conclusions and we discuss
directions for future work.
II. BACKGROUND MATERIAL
In this section we provide some definitions and a brief
review of key results in differential geometry, on which we
will rely extensively later in the paper. Let M ⊂Rn be the
manifold defining a configuration space. Within this space let
us consider driftless control-affine (DCA) dynamical systems
of the form
˙x(t) =
m
X
i=1
gi(x(t))ui(t),
x ∈M, u ∈U,
(1)
where the available motions of trajectories x(t) are linear
combinations given by input control functions ui(t) and
their corresponding actions at each point in space gi(x).
We shall assume in this paper that g1, . . . , gm are smooth
vector fields on M, and that the control set U ⊂Rm
is closed and bounded. We also assume U is symmetric
about the origin so that the system is time-reversible and
0 is in the interior of the convex hull of U. This last
condition ensures that the local possibilities for motion at
each point appear as a linear space spanned by the gi, a fact
essential to the forthcoming controllability discussion [18].
We denote the driftless control-affine system of equation (1)
as Σ = (M, g, U). A function x : [0, T] →M is called
a dynamically feasible trajectory, alternatively path, if there
exists a corresponding control function u(t) with which
it satisfies Σ. All trajectories discussed in this paper are
dynamically feasible unless otherwise noted.
A. Arc Length and Sub-Riemannian Distance
The arc length of a path x(t) is defined as
ℓ(x) :=
Z T
0
∥˙x(t)∥dt,
where ∥˙x(t)∥= ∥˙x(t)∥2 =
p
⟨˙x(t), ˙x(t)⟩is computed using
the standard Euclidean inner product on the tangent spaces
of M. The arc length function induces a sub-Riemannian
distance d on M, defined for x1, x2 ∈M as d(x1, x2) :=
infx ℓ(x), where the infimum is taken over dynamically
feasible trajectories x(t) connecting x1 and x2. Note that
for driftless control-affine systems, time-reversibility implies
that d is symmetric and indeed a metric. The sub-Riemannian
ball may be defined in analogy to the standard Euclidean
ball (i.e., Be(x, ε) = {y ∈M : ∥x −y∥≤ε}) according to
B(x, ε) := {y ∈M : d(x, y) ≤ε}. Note that by definition
B(x, ε) ⊂Be(x, ε).
B. Controllability and Reachable Sets
We now present a series of results regarding system
controllability following the discussion in [19] and [20]. As
noted above, the vector fields g1, . . . , gm characterizing the
system Σ represent a set of possible motions for a trajectory
within M. More precisely at each point p ∈M the vectors
g1(p), . . . , gm(p) span a linear subspace of local directions
within the tangent space TpM. For a vector field Y on M
let ΦY,t : M →M denote its local flow, the function that
maps an initial state to the state obtained by following the
vector field Y for time t. That is, ΦY,t(p) := y(t) where
y : [0, t] →M is a solution to the initial value problem
˙y(τ) = Y (y(τ)), y(0) = p. Commutators of flows, akin to
control switching in (1), allow for local motions transverse
to the gi to be achieved while satisfying the differential
constraints. Given two vector fields Y , Z on M and a starting
point p ∈M, we have the approximation for small t:
p + t2[Y, Z](p) = ΦY,t ◦ΦZ,t ◦ΦY,−t ◦ΦZ,−t(p) + O(t3),
where the Lie bracket [Y, Z] is a third vector field which
may be computed with respect to a coordinate system as
[Y, Z] = JZX −JXZ, where JY and JZ are the Jacobian
matrices of Y and Z respectively. Computing Lie brackets
allows us to characterize all directions that are possible from
each p, in addition to those given by g1(p), . . . , gm(p).
Let L = L(g1, . . . , gm) be the distribution, equivalently
the set of local vector subspaces, generated by the vector
fields g1, . . . , gm. We define recursively L1 = L,
]Lk+1 = Lk + [L1, Lk]
where [L1, Lk] = Span{[Y, Z] : Y ∈L1, Z ∈Lk}. Then Lk
is the distribution generated by the iterated Lie brackets of
the gi with k terms or fewer. The Lie hull of L is Lie(L) :=
∪k≥1Lk. Let Lk(x) denote the vector space corresponding
to x ∈M in Lk.
The vector fields g1, . . . , gm are said to be bracket gener-
ating if Lie(L)(x) = TxM for all x ∈M. This requirement
is also referred to as Chow’s condition, or the linear algebra
rank condition, and means that arbitrary local motion may be
achieved by composing motions along the control directions
gi. In fact, provided that the gi are bracket generating, any
two points x1, x2 ∈M may be connected by a trajectory
satisfying Σ, that is d(x1, x2) < ∞. The remainder of this
section develops tighter bounds on d(x1, x2) that will be used
in our asymptotic optimality proofs of planning algorithms.
Chow’s condition implies that for all x ∈M, there exists
a smallest integer s = s(x) such that Ls(x) = TxM. Indeed,
we have L1(x) ⊂L2(x) ⊂· · · ⊂Ls(x)(x) = TxM. Set
nk(x) = dim Lk(x). The integer list (n1(x), . . . , ns(x)(x))
is called the growth vector of L at x. A point x is called
a regular point if there exists an open neighborhood of M
around x such that the growth vector is constant; otherwise
x is said to be a singular point.
We now further assume that every x ∈M is a regular
point, so that the growth vector (n1, . . . , ns = n) is constant
over the whole configuration manifold. Fix a base point x0 ∈
M. Using the bracket-generating assumption we select a lo-
cal orthonormal frame for Tx0M of vector fields Y1, . . . , Yn
as follows: the set {Y1 = g1, . . . , Yn1 = gm} spans L
near x0; {Y1, . . . , Yn2} spans L2 near x0; {Y1, . . . , Yn3}
spans L3 near x0; and so on. Define the weights wi = k
if Yi(x0) ∈Lk(x0) and Yi(x0) /∈Lk+1(x0). Applying
a procedure developed in [19], the coordinate system yi
corresponding to this local frame may be transformed into a
privileged coordinate system zi by a polynomial change of
coordinates of the form
z1 = y1,
z2 = y2 + poly2(y1),
z3 = y3 + poly3(y1, y2),
...
zn = yn + polyn(y1, . . . , yn−1),
(2)
where polyi(·), i = 2, . . . , n, denotes a polynomial function
that includes only terms of degree ≥2 and < wi. From
the triangular structure of (2), it is clear that the inverse
transformation y = z + poly′(z) is of the same form. Given
privileged coordinates zi, define the pseudonorm at x0 as
∥z∥x0 := max{|z1|1/w1, . . . , |zn|1/wn}.
Using this pseudonorm we define the w-weighted box of size
ε at x0 as the point set Boxw(ε) := {z ∈Rn : ∥z∥x0 ≤ε}.
We use the notation Boxw(x0, ε) for the corresponding locus
of points in M given by the coordinates zi.
Theorem II.1 (Ball-box theorem [19]). Fix a point x0 ∈
M and a system of privileged coordinates z1, . . . , zn at x0.
Then there exist positive constants a(x0), A(x0) > 0, and
σ(x0) > 0 such that for all x with d(x0, x) < σ(x0),
a(x0)∥z(x)∥x0 ≤d(x0, x) ≤A(x0)∥z(x)∥x0.
(3)
Constructing the coordinate system z thus gives structure
to how sub-Riemannian distance behaves locally; this struc-
ture may be used for steering, e.g. [21]. It can be shown
that there exists a continuously varying system of privileged
coordinates on M so that the inequality (3) holds at all
x0 for continuous positive functions a(·), A(·), and σ(·) on
M. Let us assume that the system Σ is sufficiently regular
such that there exist bounds 0 < amin ≤a(x) ≤A(x) ≤
Amax < ∞and σ(x) ≥σmin > 0 for all x ∈M. We
state a pair of lemmas (whose proofs are provided in the
Appendix) concerning how privileged coordinates relate to
the Euclidean notions of volume and distance.
Lemma II.2 (Box volume). Fix x ∈M. The volume of
Boxw(x, r) is given by µ(Boxw(x, r)) = rD where D =
Pn
i=1 wi.
Lemma II.3 (Distance comparison). Fix a point x0 ∈M
and a system of privileged coordinates z1, . . . , zn at x0. Then
there exists a positive constant θ(x0) > 0 such that for all
x with ∥x0 −x∥≤θ(x0),
∥x0 −x∥≤d(x0, x) ≤2Amax∥x0 −x∥1/s.
III. PROBLEM FORMULATION
Let M ⊂Rn be the manifold defining a configuration
space of a robotic system. Let Mobs ⊂M be the obstacle
region, such that M \ Mobs is an open set, and denote the
obstacle-free space as Mfree = cl(M \ Mobs). The starting
configuration xinit is an element of Mfree, and the goal region
Mgoal is an open subset of Mfree. The trajectory planning
problem is denoted by the tuple (Σ, Mfree, xinit, Mgoal),
where Σ, as discussed in Section II, denotes a driftless
control-affine system. A dynamically feasible trajectory x is
collision-free if x(t) ∈Mfree for all t ∈[0, T]. A trajectory
x is said to be feasible for the trajectory planning problem
(Σ, Mfree, xinit, Mgoal) if it is dynamically feasible, collision-
free, x(0) = xinit, and x(T) ∈cl(Mgoal).
Let X be the set of all feasible paths. A cost function for
(Σ, Mfree, xinit, Mgoal) is a function c : X →R≥0 from the
set of paths to the nonnegative real numbers; in this paper
we consider the cost function c(x) = ℓ(x) defined as the arc
length of x with respect to the Euclidean metric in M. The
objective is to find the feasible path with minimum associated
cost; this minimum is achieved as long as U is closed and
bounded [1]. The optimal trajectory planning problem is then
defined as follows:
Optimal motion planning for driftless sys-
tems:
Given
a
trajectory
planning
problem
(Σ, Mfree, xinit, Mgoal) and an arc length function
c : X →R≥0, find a feasible path x∗such that
c(x∗) = min{c(x) : x is feasible}. If no such path
exists, report failure.
Our analysis will rely on two key sets of assumptions,
relating, respectively, to the underlying system Σ and the
problem-specific parameters Mfree, xinit, Mgoal.
1) Assumptions on system: As in Section II, we require
from Σ that a) the vector fields g1, . . . , gm are bracket
generating, b) the configuration space M contains only
regular points, c) there exist constants amin, Amax, σmin such
that Theorem II.1 holds with these values at all x0 ∈M,
and d) there exists a bounding constant θmin such that
0 < θmin < θ(x0) for all x0 ∈M, where θ(x0) is as defined
in Lemma II.3. Assumption (a) is a basic requirement for
the system to be controllable, let alone optimally controlled,
while (b), (c), and (d) ensure that there are no extreme
regions of the configuration space which would require an
unbounded sample density to capture their geometry. These
assumptions will be collectively referred to as AΣ.
2) Assumptions on problem parameters: We require that
the goal region Mgoal has regular boundary, that is there
exists ξ > 0 such that ∀y ∈∂Mgoal, there exists z ∈Mgoal
with Be(z, ξ) ⊆Mgoal and y ∈∂Be(z, ξ). This requirement
that the boundary of the goal region has bounded curvature
ensures that a point sampling procedure may expect to select
points in the goal region near any point on its boundary.
We also make requirements on the clearance of a trajec-
tory, i.e., its “distance” from Mobs, standard for sampling-
based methods [22]. For a given δ > 0, the δ-interior of
Mfree is defined as the set of all states that are at least
a Euclidean distance δ away from any point in Mobs. A
collision-free path x is said to have strong δ-clearance if it
lies entirely inside the δ-interior of Mfree. A collision-free
path x is said to have weak δ-clearance if there exists a path
x′ that has strong δ-clearance and there exists a homotopy
ψ, with ψ(0) = x and ψ(1) = x′ that satisfies the following
three properties: (a) ψ(α) is a dynamically feasible path
for all α ∈(0, 1], (b) limα→0 c(ψ(α)) = c(x), and (c)
for all α ∈(0, 1], ψ(α) has strong δα-clearance for some
δα > 0. Properties (a) and (b) are required since pathological
obstacle sets may be constructed that squeeze all optimum-
approximating homotopies into undesirable motion. In prac-
tice, however, as long as Mfree does not contain any passages
of infinitesimal width, the fact that Σ is bracket-generating
will allow every trajectory to be weak δ-clear.
IV. PROBABILISTIC EXHAUSTIVITY OF SAMPLING
SCHEMES UNDER DRIFTLESS CONSTRAINTS
In this section we prove a key result characterizing ran-
dom sampling schemes for motion planning under drift-
less differential constraints: any feasible trajectory through
the configuration space M is “traced” arbitrarily well by
connecting randomly distributed points from a sufficiently
large sample set covering the configuration space. We will
refer to this property as probabilistic exhaustivity. Note
that this notion is much stronger than the usual notion of
probabilistic completeness in motion planning, where the
requirement is that at least one feasible trajectory is traced.
The notion of probabilistic exhaustivity, besides being a
result of independent interest, is a strong tool in proving
asymptotic optimality of sampling-based motion planning
algorithms, as will be shown in Section V (specifically, we
focus on differential variants of PRM∗[22] and FMT∗[17]).
Let x : [0, T] →M satisfy the system (1). Given a set
of waypoints {ym}M
m=1 ⊂M, we associate a dynamically
feasible trajectory y∗: [0, S] →M that connects the
nodes y1, . . . , yM in order so that each connection is locally
optimal, i.e. each path segment connecting ym to ym+1 has
length d(ym, ym+1). We consider the waypoints {ym} to
(ε, r)-trace the trajectory x if: a) d(ym, ym+1) ≤r for all
m, b) the cost of y∗is bounded as c(y∗) ≤(1 + ε)c(x),
and c) the distance from any point of y∗to x is no more
than r, i.e. mint∈[0,T ] d(y(s), x(t)) ≤r for all s ∈[0, S].
In the context of sampling-based motion planning, we may
expect to find closely tracing {ym} as a subset of the sampled
points, provided the sample size is large. We formalize this
notion in Theorem IV.5, the proof of which requires three
technical lemmas (the proofs of these lemmas are provided
in the Appendix).
Lemma IV.1. Let x be a dynamically feasible trajectory
and consider a partition of the time interval 0 = τ1 < τ2 <
· · · < τM = T. Suppose that {ym} ⊂M satisfy a) ym ∈
B(x(τm), ρ) for all m ∈{1, . . . , M}, and b) more than a
(1 −α) fraction of the ym satisfy ym ∈B(x(τm), βρ) for a
parameter β ∈(0, 1). Then the cost c(y∗) of the trajectory
y∗sequentially connecting the nodes y1, . . . , yM is upper
bounded as
c(y∗) ≤c(x) + 2Mρ(β + α −αβ).
Remark IV.2. If we further assume that y1 = x(0), then the
bound c(y∗) ≤c(x) + 2(M −1)ρ(β + α −αβ) holds.
Let SampleFree(n) denote a set of n points sampled
independently and identically from the uniform distribution
on Mfree.
Lemma IV.3. Fix n ∈N, α ∈(0, 1), and let S1, . . . , SM be
disjoint subsets of Mfree with
µ(Sm) = µ(S1) ≥
2 + log(1/α)
n

e2µ(Mfree),
for each m. Let V = SampleFree(n) and define
Kn := #{m ∈{1, . . . , M} : Sm ∩V = ∅}.
Then P (Kn ≥αM) ≤e−αM
1−e−n .
Lemma IV.4. Fix n ∈N and let T1, . . . , TM be subsets of
Mfree, possibly overlapping, with
µ(Tm) = µ(T1) ≥κ (log n/n) µ(Mfree)
for each m and some constant κ
>
0. Let V
=
SampleFree(n) and denote by Em the event that Tm∩V =
∅for each m. Then
P
WM
m=1 Em

≤Mn−κ.
Before stating the theorem and proof in full, we sketch
our approach for proving probabilistic exhaustivity. Given
a path to be traced with waypoints from a sample set, we
tile the span of the path with two sequences of concentric
sub-Riemannian balls – a sequence of “small” balls and a
sequence of “large” balls. With high probability, all but a
tiny α fraction of the small balls will contain a point from
the sample set (Lemma IV.3), and for any small balls that
don’t contain such a point we ensure that the concentric large
ball does (Lemma IV.4). We take these points as a sequence
of waypoints which tightly follows the reference path with
few exceptions, and never has a gap over any section of
the reference path when it deviates. We then use the metric
inequality for d to bound the total cost of the waypoint
trajectory (Lemma IV.1).
Theorem IV.5 (Probabilistic exhaustivity). Let Σ be a
DCA system satisfying the assumptions AΣ and suppose
x : [0, T] →Mfree is a dynamically feasible trajectory
with strong δ-clearance, δ
>
0. Let V
=
{xinit} ∪
SampleFree(n), ε > 0, and for fixed n consider the event
An that there exist {ym}M
m=1 ⊂V which (ε, rn)-trace x,
where
rn = 4Amax(1 + η)1/D
µ(Mfree)
D
1/D log n
n
1/D
for a parameter η ≥0. Then, as n →∞, the probability that
An does not occur is asymptotically bounded as P (Ac
n) =
O(n−η/D log−1/D n).
Proof. Note that in the case c(x) = 0 we may pick y1 = x(0)
to be the only waypoint and the result is trivial. Therefore
assume c(x) > 0. Fix n sufficiently large so that rn/2 ≤
σmin. Take x(τn,m) to be points spaced along x at sub-
Riemannian distances rn/2; more precisely let τ1 = 0, and
for m = 2, 3, . . . consider
τn,m =min ({τ ∈(τn,m−1, 1) : d(x(τ), x(τn,m−1)) ≥rn/2}) .
Let Mn be the first m for which the set is empty; take
τn,Mn = T. Note that since the distance d is an infimum
over all feasible trajectories, one of which is the segment
of x between τn,m and τn,m+1, we have the bound Mn ≤
⌈2c(x)/rn⌉.
We now make the identification α = β = ε/2 anticipating
the application of Lemma IV.1. Take ρn = rn/4 and define a
sequence of sub-Riemannian balls Bn,1, . . . , Bn,Mn centered
along the trajectory x by Bn,m = B(xm, ρn), where xm =
x(τn,m) for m ∈{1, . . . , Mn}. Within these balls define
a concentric sequence of smaller, disjoint balls Bβ
n,m =
B(xm, βρn) for each m ∈{1, . . . , Mn}. Note that since
Boxw(xm, ρn/Amax) ⊂Bn,m, we have the volume lower
bound µ(Bn,m) ≥(ρn/Amax)D and similarly µ(Bβ
n,m) ≥
(βρn/Amax)D . Denote Kβ
n := #{m ∈{1, . . . , Mn} :
Bβ
n,m ∩V = ∅}. We consider the event ˜An that every large
ball Bn,m, as well as at least a (1 −α) fraction of the small
balls Bβ
n,m, contains at least one point of V :
˜An =

Kβ
n < αMn
	
∧
Mn
^
m=1
{Bn,m ∩V ̸= ∅}.
We claim that ˜An implies the event An that there exist
(ε, rn)-tracing {ym} ⊂V . If
˜An holds, then we select
waypoints {ym}Mn
m=1 ⊂V such that ym ∈Bn,m for every
point, as well as ym ∈Bβ
n,m for at least a (1 −α) fraction
of the points. In particular let us select y1 = x(0).
First note that ym ∈Bn,m for each m implies
d(ym, ym+1) ≤d(ym, xm) + d(xm, xm+1)
+ d(xm+1, ym+1)
≤rn/4 + rn/2 + rn/4 = rn.
Next, applying Remark IV.2 we have
c(y∗) ≤c(x) + 2(Mn −1)ρn(β + α −αβ)
≤c(x) + 2(2c(x)/rn)(rn/4)ε
≤(1 + ε)c(x).
Finally we check that any point y∗(t) is within distance rn
from a point on x. To see this suppose that y∗(t) lies on the
shortest path connecting ym to ym+1. Note that
d(ym, y∗(t)) + d(y∗(t), ym+1) = d(ym, ym+1) ≤rn.
Then we may write
d(xm, y∗(t)) + d(xm+1, y∗(t)) ≤d(xm, ym)
+ d(xm+1, ym+1) + rn
≤(3/2)rn,
so that min{d(xm, y∗(t)), d(xm+1, y∗(t))} ≤(3/4)rn < rn.
Thus the waypoints {ym} (ε, rn)-trace x.
Now making the identifications Sm = Bβ
n,m and Tm =
Bn,m in Lemmas IV.3 and IV.4 respectively, we see that
by considering n ≥N1 sufficiently large so that log N1 ≥
Dβ−D(2 + log(1/α))e2 (satisfying the volume assumption
of Lemma IV.3), we compute the union bound
P (Ac
n) ≤P

˜Ac
n

≤P
Kβ
n ≥αMn


+ P
WMn
m=1{Bn,m ∩V = ∅}

≤e−αMn
1 −e−n + Mnn−(1+η)/D.
Now, c(x) > 0 and rn = Θ((log n/n)1/D) together imply
that Mn = Θ((n/ log n)1/D). The second term in the bound
dominates as n →∞, and P (Ac
n) = O(n−η/D log−1/D n).
V. OPTIMAL SAMPLING-BASED ALGORITHMS FOR
DRIFTLESS CONTROL-AFFINE SYSTEMS
In this section we present two algorithms for the mo-
tion planning problem with driftless control-affine sys-
tems. The first algorithm, named the Differential Probabilis-
tic RoadMap algorithm (DPRM∗), is a derivation of the
PRM∗algorithm presented in [22], while the second algo-
rithm, named the Differential Fast Marching Tree algorithm
(DFMT∗), is a derivation of the FMT∗algorithm presented in
[17]. This section provides a description of both algorithms,
while the next section focuses on their theoretical character-
ization (chiefly, their asymptotic optimality property).
As in Section IV, let SampleFree(k) be a function that
returns a set of k ∈N states sampled independently and
identically from the uniform distribution on Mfree. These
sampled states are connected as vertices in a graph from
which a solution trajectory will be computed. Given two ver-
tices x1 and x2, we denote with the edge (x1, x2) an optimal
cost trajectory from x1 to x2 neglecting obstacle constraints.
Let CollisionFree(x1, x2) denote the boolean function
which returns true if and only if the edge (x1, x2) does not
intersect an obstacle. Given a set of vertices V , a state x ∈
M, and a threshold r > 0, let Near(V, x, r) be a function
that returns the set of states {v ∈V : ∥v∥x < r/amin}.
Given a graph G = (V, E), where V is the vertex set and
E is the edge set, and a vertex x ∈V , let Cost(x, G) be
the function that returns the cost of the shortest path in the
graph G between the vertices xinit and x. Let Path(x, G)
be the function that returns the path achieving that cost.
The DPRM∗algorithm is given in Algorithm 1, while the
DFMT∗algorithm is given in Algorithm 2. DPRM∗works
by sampling a set of points within Mfree and connecting
each state to every state in a local neighborhood around it,
provided that the connection is collision free. The resulting
graph spans Mfree, with the local connections combining
to yield a global “roadmap” for traveling between any two
states, not just xinit and the goal. The least cost path in the
graph between xinit and Mgoal, computed, e.g., using the
Dijkstra’s algorithm, is output by DPRM∗.
The DFMT∗algorithm essentially implements a stream-
lined version of DPRM∗by performing a “lazy” dynamic
programming recursion, this time during the state connection
phase instead of as a last step, to grow a tree of trajectories
Algorithm 1 Differential Probabilistic RoadMap (DPRM∗)
1 V ←{xinit} ∪SampleFree(n); E ←∅
2 for each v ∈V do
3
for each u ∈Near(V \{v}, v, rn) do
4
if CollisionFree(u, v) then
5
E ←E ∪{{u, v}}
6
end if
7
end for
8 end for
9 V ∗←argminv∈V ∩Mgoal Cost(v, G = (V, E))
10 if V ∗̸= ∅then
11
v∗←random vertex in V ∗
12
return Path(v∗, G = (V, E))
13 else
14
return Failure
15 end if
which moves steadily outward in cost-to-come space. In
the Algorithm 2 outline, the set W consists of all of the
nodes that have not yet been added into the tree, while H
is comprised only of nodes that are in the tree. In particular,
while H keeps track of nodes which have already been added
to the tree, nodes are removed from H if they are not near
enough to the edge of the expanding tree to actually have any
new connections made with W (see [17] for further details).
At each iteration, DFMT∗examines the neighborhood of a
state in H and only considers locally-optimal (assuming no
obstacles) connections for potential inclusion in the tree (see
line (10)). By only checking for collision on the locally-
optimal (assuming no obstacles) connection, as opposed
to every possible connection (essentially what is done in
DPRM∗), DFMT∗saves a large (indeed unbounded as the
number of vertices increases) number of collision-check
computations. A more detailed explanation of the meaning
of the dual sets H and W and of the philosophy behind
DFMT∗can be found in [17].
The main differences of these algorithms with respect to
their geometric counterparts (i.e., PRM∗and FMT∗) are that
(i) near vertices lie within the privileged coordinate box
Boxw(x, r/amin) rather than within the Euclidean ball, and
(ii) the edges connecting vertices are optimal trajectories
rather than straight lines. Indeed, attempting connections
within the sub-Riemannian ball B(x, r) would be the best
analogy to the geometric planning case, and is preferable
for efficiency if possible. The structure of this set is difficult
to compute exactly, however, which motivates the choice
Boxw(x, r/amin) as a tractable approximation (as also sug-
gested in [11]). Checking connections within this superset
of B(x, r) induces an extra run time factor of at most
(Amax/amin)D, a bound on the volume ratio between the
two sets.
VI. ASYMPTOTIC OPTIMALITY OF DFMT∗AND DPRM∗
In this section, we prove the asymptotic optimality of
DFMT∗and DPRM∗(obtained as a simple corollary). We
conclude the section by providing a discussion of the results.
Algorithm 2 Differential Fast Marching Tree (DFMT∗)
1 V ←{xinit} ∪SampleFree(n); E ←∅
2 W ←V \{xinit}; H ←{xinit}
3 z ←xinit
4 while z /∈Mgoal do
5
Hnew ←∅
6
Xnear = Near(V \{z}, z, rn) ∩W
7
for x ∈Xnear do
8
Ynear ←Near(V \{x}, x, rn) ∩H
9
ymin ←arg miny∈Ynear{Cost(y, T
= (V, E)) +
d(y, x)}
10
if CollisionFree(ymin, x) then
11
E ←E ∪{{ymin, x}}
12
Hnew ←Hnew ∪{x}
13
W ←W\{x}
14
end if
15
end for
16
H ←(H ∪Hnew)\{z}
17
if H = ∅then
18
return Failure
19
end if
20
z ←arg miny∈H{Cost(y, T = (V, E))}
21 end while
22 return Path(z, T = (V, E))
A. Asymptotic Optimality of DFMT∗and DPRM∗
The following theorem presents a result comparing the
output cost of DFMT∗to the cost of any feasible trajectory.
Theorem
VI.1
(DFMT∗
cost
comparison).
Let
(Σ, Mfree, xinit, Mgoal) be a trajectory planning problem
satisfying the assumptions AΣ and suppose x : [0, T] →M
is a feasible path with strong δ-clearance, δ > 0. Assume
further that x extends into the interior of Mgoal, i.e. there
exists γ > 0 such that B(x(T), γ) ⊂Mgoal. Let cn denote
the cost of the path returned by DFMT∗with n vertices
using a radius
rn = 4Amax(1 + η)1/D
µ(Mfree)
D
1/D log n
n
1/D
for a parameter η ≥0. Then for fixed ε > 0
P (cn > (1 + ε)c(x)) = O(n−η/D log−1/D n).
Proof. The proof of this theorem is conceptually similar to
the one of Theorem 1 in [17]. The details are provided in
the Appendix.
We are now in a position to state the optimality result for
DFMT∗(note that the result also provides a convergence rate
bound). The optimality of DPRM∗will follow as a corollary.
Theorem
VI.2
(DFMT∗
asymptotic
optimality).
Let
(Σ, Mfree, xinit, Mgoal) be a trajectory planning problem,
satisfying the assumptions AΣ and with Mgoal ξ-regular,
such that there exists an optimal path x∗: [0, T ∗] →M
with weak δ-clearance for some δ > 0. Let c∗denote the arc
length of x∗, and let cn denote the cost of the path returned
by DFMT∗with n vertices using the radius
rn = 4Amax(1 + η)1/D
µ(Mfree)
D
1/D log n
n
1/D
for a parameter η ≥0. Then for fixed ε > 0
P (cn > (1 + ε)c∗) = O(n−η/D log−1/D n).
Proof. Note that c∗= 0 implies xinit ∈cl(Mgoal), and
DFMT∗will terminate immediately and optimally. In this
case the result is trivial, therefore assume c∗> 0. Let
ψ denote the homotopy given by the weak δ-clearance
definition for x∗. Using the fact that limα→0 c(ψ(α)) =
c∗, we pick an α0
> 0 such that c(ψ(α0)) ≤(1 +
ε/4)c∗. Denote x := ψ(α0). We now extend x into the
interior of Mgoal so that we may apply Theorem VI.1.
Since x∗is optimal, x(T) = x∗(T ∗) ∈∂Mgoal and
the ξ-regularity of Mgoal means there exists z ∈Mgoal
with Be(z, ξ)
⊆
Mgoal and y
∈
∂Be(x(T), ξ). Pick
ρ = min
n
ξ, δα0
2 ,
1
2Amax

εc∗
4
s
, θmin
o
, and take z′ on the
straight line segment between x(T) and z with ∥x(T)−z′∥≤
ρ. Then B(z′, ρ) ⊂Be(z′, ρ) ⊂Be(z, ξ) ⊂Mgoal. The last
two terms in the minimum above ensure, from Lemma II.3,
that d(x(T), z′) ≤(ε/4)c∗. Consider the extension x′ of
x constructed by concatenating x with the shortest sub-
Riemannian path between x(T) and z′. The cost of x′ is
bounded as
c(x′) ≤c(x′) + (ε/4)c∗≤(1 + ε/2)c∗.
The strong (δα0/2)-clearance of x′ is established by noting
for any p along the path between x(T) and z′:
inf
a∈Mobs ∥p −a∥≥
inf
a∈Mobs ∥x(T) −a∥−∥p −x(T)∥
≥δα0 −δα0/2 = δα0/2.
Then by Theorem VI.1, with the approximation factor ε/4,
we have for ε ∈(0, 1) that
P (cn > (1 + ε)c∗) ≥P (cn > (1 + ε/4)(1 + ε/2)c∗)
= O(n−η/D log−1/D n).
For ε ≥1 the desired result follows from the monotonicity
in ε of the above probability.
Remark VI.3. There is an implicit dependence on the
problem parameters and approximation factor ε in the con-
vergence rate bound given above in Theorem VI.2; these fixed
parameters influence the threshold n > N after which the
stated asymptotic bound holds.
We conclude this section with the asymptotic optimality
result for DPRM∗.
Corollary
VI.4
(DPRM∗
asymptotic
optimality).
Let
(Σ, Mfree, xinit, Mgoal) be a trajectory planning problem,
satisfying the assumptions AΣ and with Mgoal ξ-regular,
such that there exists an optimal path x∗: [0, T ∗] →M
with weak δ-clearance for some δ > 0. Let c∗denote the arc
length of x∗, and let cn denote the cost of the path returned
by DPRM∗with n vertices using the radius
rn = 4Amax(1 + η)1/D
µ(Mfree)
D
1/D log n
n
1/D
for a parameter η ≥0. Then for fixed ε > 0
P (cn > (1 + ε)c∗) = O(n−η/D log−1/D n).
Proof. Given the same sample set V , the trajectory tree
constructed by DFMT∗is a subgraph of the roadmap graph
constructed by DPRM∗. Hence the cost of the path returned
by DPRM∗is upper bounded by that of DFMT∗. The desired
result then follows immediately from Theorem VI.2.
B. Discussion
We note that there is room for improvement in the choice
of constants for the theorems and lemmas above (particularly
in the connection radius rn), which for the sake of clarity
were not pushed as tight as possible. As a consequence,
the asymptotic rate bound O(n−η/D log−1/D n) may be
achieved with a connection radius smaller by a constant fac-
tor. The convergence rate bounds for DFMT∗and DPRM∗,
stated in terms of the sample size, are also tunable to specific
implementations and planning problems. A greater local
connection radius achieves a faster asymptotic convergence
rate, but such a choice may ultimately result in greater
algorithm execution time as more connections are checked.
The essential property of the system Σ underpinning the
asymptotic optimality results above is the ball-box Theo-
rem II.1. Indeed, although the results we present nominally
apply only to arc length, the proofs hold for any system with
a cost metric d on M satisfying the ball-box inequality (3).
For example, if the ball-box inequality holds for arc length
on a subset of dimensions of M, DFMT∗and DPRM∗
asymptotic optimality hold with respect to that subspace
metric, a fact we make use of in our simulations. The
constant amin ensures that the sub-Riemannian ball B(x, r)
is covered by the connection neighborhood of x (i.e. the
algorithms search far enough in each direction), while the
constant Amax provides a lower bound on µ(B(x, r)) that
ensures the sample set will intersect the neighborhood with
high probability (i.e. the algorithms examine enough volume
along an optimal path). If checking state membership in balls
B(x, r) is computationally efficient, then algorithmic perfor-
mance gains may be achieved by using Near(V, x, r) = V ∩
B(x, r), while maintaining the guarantees of Theorems VI.1
and VI.2. Another consequence of this ball-box analysis
foundation is the possibility of accommodating approximate
steering techniques (as in [21]) for computing local, obstacle-
free state connections (one of the bottlenecks for DPRM∗and
DFMT∗). As long as DFMT∗and DPRM∗have access to a
local planner that can connect two states exactly, but possibly
with some approximation factor to the optimal distance, then
the global optimality theorems hold up to that same factor.
ç
ç
ç
ç
ç
ç
ç
ç
ç
çç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
çç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
çç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
çç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
çç
ç
ç
ç
ç
ç
ç
ç
çç
ç
ç
ç
ç
ç
ç
ç
çç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
çç ç
ç
ç
ç
ç
ç
çç
ç
ç
ç
ç
ç
ç
çç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
çç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç
ç ç
ç
ç
ç
ç
ç
á
á
á
á
á
á
á
á
á
áá
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
áá
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
áá
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
áá
á
á
á
á
á
á
á
áá
á
á
á
á
á
á
á
áá
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
áá á
á
á
á
á
á
áá
á
á
á
á
á
á
áá
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
áá
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á
á á
á
á
á
á
á
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ó
ç
DFMT*
á
DFMT* Hno precomputationL
ó
DPRM*
0
5
10
15
20
1.2
1.4
1.6
1.8
2.0
Execution Time HsL
Solution Cost
Reeds-Shepp Car
ç
ççç ç
ç
ç
ç
ç
á
ááá
á
á
á
á
á
ó
ó
ó
ó
ó
ç
DFMT*
á
DFMT* Hno precomputationL
ó
DPRM*
0
5
10
15
20
0.0
0.2
0.4
0.6
0.8
1.0
Execution Time HsL
Success Rate
Reeds-Shepp Car
DFMT ∗ Tree, N = 1000 Samples
Fig. 1.
Top: Simulation results for the Reeds-Shepp car system with a
maze obstacle set. The error bars in each axis represent plus and minus
one standard error of the mean for a fixed sample size n. Bottom: Example
DFMT∗tree for n = 1000. The feasible path returned is highlighted in
blue with car direction at each sample state denoted by an arrow.
VII. IMPLEMENTATION AND NUMERICAL EXPERIMENTS
The DFMT∗and DPRM∗algorithms were implemented
in Julia and run using a Unix operating system with a
2.0 GHz processor and 8 GB of RAM. Near neighbor sets
were precomputed and cached at algorithm initialization after
the sample set was selected. Note that for batch-processing
(as opposed to “anytime”) algorithms such as DFMT∗and
DPRM∗, one can precompute both near neighbor sets and
sub-Riemannian distance ahead of time, as they do not
depend on the obstacle configuration — the price to pay
is a moderate increase in memory requirements.
We tested DFMT∗and DPRM∗on the Reeds-Shepp car
system [23], a well-known example of a regular driftless
control-affine system. Within the configuration manifold
M = R2 × S1, trajectories x(t) = (x1(t), x2(t), θ(t)) are
subject to equation (1) with g1(x) = (cos θ, sin θ, 0) and
g2(x) = (0, 0, 1). With the car constrained to move at
unit speed with turning radius R, the control set is U =
{−1, 1}×[−1/R, 1/R]. The privileged coordinate system at
each point x is defined by the vectors g1(x), g2(x), and
[g1(x), g2(x)] = (sin θ, −cos θ, 0) with associated weight
vector (1, 1, 2). We aim to optimize planar arc length in the
first two dimensions of M, in which case the constants for
the ball-box Theorem II.1 are computed as amin =
√
2R and
Amax = 2
√
2R for σmin = R.
An analytic solution to the Reeds-Shepp optimal steering
problem in the absence of obstacles is known [23], and
consists of checking forty-six candidate curves for the one of
minimum length. We precomputed and cached the optimal
curve type between a state at the origin and any general
state (using spatial symmetry this gives the optimal steering
curve type between any two states in Reeds-Shepp space);
the time for this operation was not included in our accounting
of algorithm execution time. We used this data to implement
the variants of DFMT∗and DPRM∗discussed in Section VI-
B where the exact sub-Riemannian ball is used for the
Near function instead of the privileged coordinate box.
Collisions with obstacles were detected by approximating
trajectories by piecewise linear interpolations, for which
polytope intersection is simple to compute.
The simulation results are summarized in Figure 1. A
maze was used for Mobs, and our DFMT∗and DPRM∗
implementations were run 50 times each on sample sizes up
to n = 4000 in the case of DFMT∗and n = 1000 in the case
of DPRM∗. We plot results for DFMT∗run both with and
without the precomputed neighbor sets and distance cache;
the solutions returned are identical and differ only in their
timing data. For the Reeds-Shepp system, local connection
computation and collision checking takes the majority of the
computation time, and the speed increase from the cache is
modest. For a greater speedup at the cost of more memory
usage, all local collision-free connections could be cached
ahead of time as well, leaving only collision checking with
Mobs (the only detail specific to the problem instance) to be
computed online.
VIII. CONCLUSIONS
In this paper we presented a thorough theoretical frame-
work to assess optimality guarantees of sampling-based
algorithms for planning with driftless control-affine sys-
tems. We exploited this framework to design and analyze
two novel sampling-based algorithms that are guaranteed
to converge, as the number of samples increases, to an
optimal solution. Our theoretical framework, which relies
on the key notion of probabilistic exhaustivity, separates the
probabilistic analysis of random sampling from the discrete
analysis of combinatorial optimization algorithms, the two
main elements in determining the performance of batch-
processing motion planners (such as DFMT∗and DPRM∗).
We hope that this analysis framework can prove useful to
analyze other types of algorithms for this class of problems.
We note that our analysis approach yields convergence rate
guarantees, which shed additional light on the behavior
of sampling-based algorithms for differentially-constrained
motion planning problems.
This paper leaves numerous important extensions open for
further research. One extension that appears quite tractable
is generalizing the cost function from arc length to functions
that may depend on state-space position and/or control effort.
For example, the techniques established in this paper may
be used, with a few modifications, to prove DFMT∗and
DPRM∗variants for positionally weighted arc length costs,
in which certain spatial regions incur higher or lower costs
for traversal. A second extension of great interest is the
design and analysis of algorithms that address systems with
drift (of critical importance, for example, for spacecraft
motion planning). The specific analysis techniques used in
this paper rely heavily on the fact that driftless control-
affine systems can be viewed as metric spaces, but asym-
metric adaptations may be imagined within the same proof
framework. Third, we plan to study bidirectional versions
of DFMT∗and DPRM∗. Finally, determining how algorithm
tuning parameters affect asymptotic convergence rates and,
ultimately, run time merits more extensive theoretical and
experimental analysis, especially for DCA systems beyond
the basic proof-of-concept Reeds-Shepp example presented
in this paper.
REFERENCES
[1] S. Lavalle, Planning Algorithms.
Cambridge University Press, 2006.
[2] S. M. LaValle, “Motion planning,” IEEE Robotics Automation Maga-
zine, vol. 18, no. 2, pp. 108–118, 2011.
[3] J. H. Reif, “Complexity of the mover’s problem and generalizations,”
in 20th Annual Symposium on Foundations of Computer Science, 1979,
pp. 421 –427.
[4] B. Donald, P. Xavier, J. Canny, and J. Reif, “Kinodynamic motion
planning,” Journal of the Association for Computing Machinery,
vol. 40, no. 5, pp. 1048–1066, 1993.
[5] K. I. Tsianos, I. A. Sucan, and L. E. Kavraki, “Sampling-based robot
motion planning: Towards realistic applications,” Computer Science
Review, vol. 1, no. 1, pp. 2 – 11, 2007.
[6] S. M. LaValle and J. J. Kuffner, “Randomized kinodynamic planning,”
International Journal of Robotics Research, vol. 20, no. 5, pp. 378–
400, 2001.
[7] S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimal
motion planning,” International Journal of Robotics Research, vol. 30,
no. 7, pp. 846–894, 2011.
[8] ——, “Optimal kinodynamic motion planning using incremental
sampling-based methods,” in Proc. IEEE Conf. on Decision and
Control, 2010, pp. 7681–7687.
[9] A. Perez, R. Platt, G. Konidaris, L. Kaelbling, and T. Lozano-
Perez, “LQR-RRT∗: Optimal sampling-based motion planning with
automatically derived extension heuristics,” in Proc. IEEE Conf. on
Robotics and Automation, 2012, pp. 2537–2542.
[10] G. Goretkin, A. Perez, r. Platt Jr., and G. Konidaris, “Optimal
sampling-based planning for linear-quadratic kinodynamic systems,”
in Proc. IEEE Conf. on Robotics and Automation, 2013.
[11] S. Karaman and E. Frazzoli, “Sampling-based optimal motion planning
for non-holonomic dynamical systems,” in Proc. IEEE Conf. on
Robotics and Automation, 2013.
[12] D. J. Webb and J. van den Berg, “Kinodynamic RRT*: Optimal motion
planning for systems with linear differential constraints,” in Proc.
IEEE Conf. on Robotics and Automation, 2013.
[13] Y. Li, Z. Littlefield, and K. Bekris, “Sparse methods for efficient
asymptotically optimal kinodynamic planning,” Workshop on Algo-
rithmic Foundations of Robotics, 2014, available at http://arxiv.org/
pdf/1407.2896v2.pdf.
[14] R. T. M’Closkey III, “Exponential stabilization of driftless nonlinear
control systems,” Ph.D. dissertation, 1995.
[15] R. M. Murray and S. Sastry, A mathematical introduction to robotic
manipulation.
CRC press, 1994.
[16] N. E. Leonard and P. Krishnaprasad, “Averaging for attitude control
and motion planning,” in Proc. IEEE Conf. on Decision and Control,
1993, pp. 3098–3104.
[17] L. Janson, E. Schmerling, A. Clark, and M. Pavone, “Fast marching
tree: a fast marching sampling-based method for optimal motion
planning in many dimensions,” International Journal of Robotics
Research, 2015.
[18] F. Bullo and A. Lewis, Geometric control of mechanical systems, ser.
Texts in Applied Mathematics.
Springer, 2005, vol. 49.
[19] A. Bella¨ıche, “The tangent space in sub-riemannian geometry,” in Sub-
Riemannian Geometry, A. Bella¨ıche and J. Risler, Eds.
Birkhauser,
1996, pp. 1–78.
[20] R. Montgomery, A tour of subriemannian geometries, their geodesics,
and applications, ser. Mathematical Surveys and Monographs. Amer-
ican Mathematical Society, 2002, vol. 91.
[21] F. Jean, G. Oriolo, and M. Vendittelli, “A globally convergent steering
algorithm for regular nonholonomic systems,” in Proc. IEEE Conf. on
Decision and Control, 2005, pp. 7514–7519.
[22] S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimal
motion planning,” International Journal of Robotics Research, vol. 30,
no. 7, pp. 846–894, 2011.
[23] J. Reeds and R. Shepp, “Optimal paths for a car that goes both
forwards and backwards,” Pacific Journal of Mathematics, vol. 145,
no. 2, 1990.
[24] M. Penrose, Random Geometric Graphs.
Oxford University Press,
2003.
APPENDIX
Lemma II.2 (Box volume). Fix x ∈M. The volume of
Boxw(x, r) is given by µ(Boxw(x, r)) = rD where D =
Pn
i=1 wi.
Proof. From the formula (2) we see that the Jacobian
matrix JZ for the change of coordinates between the lo-
cal frame y1, ..., yn and privileged coordinates z1, . . . , zn
is lower-triangular with all diagonal elements equal to 1;
thus the Jacobian determinant |JZ| is identically 1. Since
Y1(x), . . . , Yn(x) are orthonormal, the change of coordi-
nates from the Euclidean standard basis to the coordinates
y1, . . . , yn, with Jacobian JY , satisfies |JY | = 1. Then
µ(Boxw(x, r)) =
R
∥z∥x≤r |J−1
Z ||J−1
Y |dz =
R
∥z∥x≤r 1 dz =
r
Pn
i=1 wi.
Lemma II.3 (Distance comparison). Fix a point x0 ∈M
and a system of privileged coordinates z1, . . . , zn at x0. Then
there exists a positive constant θ(x0) > 0 such that for all
x with ∥x0 −x∥≤θ(x0),
∥x0 −x∥≤d(x0, x) ≤2Amax∥x0 −x∥1/s.
Proof. The left-hand inequality is a consequence of the
definition of d. For the right-hand inequality, we note for each
coordinate that |zi(x)1/wi| = |yi(x)|1/wi+O(∥y(x)∥2/wi) ≤
∥x0 −x∥1/wi + O(∥x0 −x∥2/wi), so then d(x0, x) ≤
Amax∥z(x)∥x0 ≤2Amax∥x0−x∥1/s for small ∥x0−x∥.
Lemma IV.1. Let x be a dynamically feasible trajectory
and consider a partition of the time interval 0 = τ1 < τ2 <
· · · < τM = T. Suppose that {ym} ⊂M satisfy a) ym ∈
B(x(τm), ρ) for all m ∈{1, . . . , M}, and b) more than a
(1 −α) fraction of the ym satisfy ym ∈B(x(τm), βρ) for a
parameter β ∈(0, 1). Then the cost c(y∗) of the trajectory
y∗sequentially connecting the nodes y1, . . . , yM is upper
bounded as
c(y∗) ≤c(x) + 2Mρ(β + α −αβ).
Proof. Using the triangle inequality for the metric d we
compare:
c(y∗) =
M−1
X
m=1
d(ym, ym+1) ≤
M−1
X
m=1

d(ym, x(τm))
+ d(x(τm), x(τm+1)) + d(x(τm+1), ym+1)

=
M−1
X
m=1
d(x(τm), x(τm+1)) + 2
M
X
m=1
d(x(τm), ym)
−d(x1, y1) −d(xM, yM)
≤c(x)+2 (Mβρ + αM(1−β)ρ) ≤c(x)+2Mρ(β + α −αβ).
Lemma IV.3. Fix n ∈N, α ∈(0, 1), and let S1, . . . , SM be
disjoint subsets of Mfree with
µ(Sm) = µ(S1) ≥
2 + log(1/α)
n

e2µ(Mfree),
for each m. Let V = SampleFree(n) and define
Kn := #{m ∈{1, . . . , M} : Sm ∩V = ∅}.
Then P (Kn ≥αM) ≤e−αM
1−e−n .
Proof. The proof relies on a Poissonization argument. Let
˜n be a random variable drawn from a Poisson distribution
with parameter n/e2 (denoted as Poisson(n/e2)). Consider
the set of nodes eV := SampleFree(˜n). Let eKn be the
Poissonized analogue of Kn, namely
eKn := #
n
m ∈{1, . . . , M} : Sm ∩eV = ∅
o
.
From the definition of eV , we can see that P (Kn ≥αM)) =
P

eKn ≥αM | ˜n = n

. Thus, we have
P

e
Kn ≥αM

=
∞
X
j=0
P

e
Kn ≥αM | ˜n = j

P (˜n = j)
≥
n
X
j=0
P

e
Kn ≥αM | ˜n = j

P (˜n = j)
≥
n
X
j=0
P

e
Kn ≥αM | ˜n = n

P (˜n = j)
= P

e
Kn ≥αM | ˜n = n

P (˜n ≤n)
= P (Kn ≥αM) P (˜n ≤n)
≥(1 −e−n)P (Kn ≥αM),
(4)
The third line follows from the fact that P( eKn ≥αM|˜n =
j) is nonincreasing in j, and the last line follows from a tail
approximation of the Poisson distribution [24, p. 17] and the
fact that E[˜n] = n/e2.
The locations of the nodes in eV are distributed as a
spatial Poisson process with intensity n/(e2µ(Mfree)). This
means that for a Lebesgue-measurable region R ⊆Mfree,
the number of nodes in R is distributed as a Poisson random
variable with distribution Poisson

(n/e2)µ(R)/µ(Mfree)

,
independent of the number of nodes in any region disjoint
with R [22, Lemma 11]. Since by assumption the Sm are all
disjoint, we get that for m ∈{1, . . . , M},
P

Sm ∩eV = ∅

= e−(n/e2)µ(S1)/µ(Mfree).
Therefore, eKn is distributed according to a binomial distri-
bution, in particular according to a distribution Binomial(M,
e−(n/e2)µ(S1)/µ(Mfree)). Noting that e−(n/e2)µ(S1)/µ(Mfree) ≤
α/e2 by assumption, we have that E[ eKn] ≤αM/e2. So from
a tail approximation to the Binomial distribution [24, p. 16],
P( eKn ≥αM) ≤e−αM and thus plugging into inequality
(4)
P(Kn ≥αM) ≤
e−αM
1 −e−n .
Lemma IV.4. Fix n ∈N and let T1, . . . , TM be subsets of
Mfree, possibly overlapping, with
µ(Tm) = µ(T1) ≥κ (log n/n) µ(Mfree)
for each m and some constant κ
>
0. Let V
=
SampleFree(n) and denote by Em the event that Tm∩V =
∅for each m. Then
P
WM
m=1 Em

≤Mn−κ.
Proof. We union bound the probability of any Em occurring:
P
WM
m=1 Em

≤
M
X
m=1
P (Tm ∩V = ∅)
=
M
X
m=1

1 −
µ(Tm)
µ(Mfree)
n
≤Me
−

µ(Tm)
µ(Mfree)

n ≤Mn−κ.
Theorem
VI.1
(DFMT∗
cost
comparison).
Let
(Σ, Mfree, xinit, Mgoal) be a trajectory planning problem
satisfying the assumptions AΣ and suppose x : [0, T] →M
is a feasible path with strong δ-clearance, δ > 0. Assume
further that x extends into the interior of Mgoal, i.e. there
exists γ > 0 such that B(x(T), γ) ⊂Mgoal. Let cn denote
the cost of the path returned by DFMT∗with n vertices
using a radius
rn = 4Amax(1 + η)1/D
µ(Mfree)
D
1/D log n
n
1/D
for a parameter η ≥0. Then for fixed ε > 0
P (cn > (1 + ε)c(x)) = O(n−η/D log−1/D n).
Proof. Consider n so that rn ≤min{γ, δamin/2Amax}, and
apply Theorem IV.5 to produce, with probability at least 1−
O(n−η/D log−1/D n), a sequence of waypoints {ym}M
m=1 ⊂
V which (ε, rn)-trace x. We claim that in the event that
such {ym} exist, the DFMT∗algorithm will return a path
with cost upper bounded as cn ≤c(y∗) ≤(1 + ε)c(x). It is
clear that the desired result follows from this claim.
Assume the existence of (ε, rn)-tracing {ym}. Note that
our upper bound on rn implies that Boxw(ym, rn/amin)
intersects
no
obstacles.
This
follows
from
the
inclu-
sion
Boxw(ym, rn/amin)
⊂
B(ym, Amaxrn/amin)
⊂
Be(ym, Amaxrn/amin), and the Euclidean distance bound
inf
a∈Mobs ∥ym −a∥≥
inf
a∈Mobs ∥xm −a∥−∥ym −xm∥
≥2
Amax
amin

rn −rn ≥
Amax
amin

rn.
Consider running DFMT∗to completion and for each ym,
let c(ym) denote the cost-to-come of ym in the generated
tree. If ym is not contained in any edge of the tree, we set
c(ym) = ∞. We show by induction that
min(c(ym), cn) ≤
m−1
X
k=1
d(yk, yk+1),
(5)
for all m ∈{2, . . . , M}.
The base case m = 2 is trivial, since the first step
in the DFMT∗algorithm is to make every collision-free
connection between xinit = y1 and the nodes contained in
Boxw(x, rn/amin), which will include y2 and, thus, c(y2) =
d(y1, y2). Now suppose (5) holds for m−1; that means that
one of the following four statements must hold.
1. cn ≤Pm−2
k=1 d(yk, yk+1),
2. c(ym−1) ≤Pm−2
k=1 d(yk, yk+1) and DFMT∗ends before
considering ym,
3. c(ym−1) ≤Pm−2
k=1 d(yk, yk+1) and ym−1 ∈H when
ym is first considered,
4. c(ym−1) ≤Pm−2
k=1 d(yk, yk+1) and ym−1 /∈H when
ym is first considered.
Case 1: cn ≤Pm−2
k=1 d(yk, yk+1) ≤Pm−1
k=1 d(yk, yk+1).
Case 2: c(ym−1) < ∞implies that ym−1 enters H at
some point during DFMT∗. The case that ym goes uncon-
sidered means that the algorithm terminates before xm−1
is ever minimum-cost element of H. Since the end-node
of the solution returned must have been the minimum-cost
element of H, cn ≤c(ym−1) ≤Pm−2
k=1 d(yk, yk+1) ≤
Pm−1
k=1 d(yk, yk+1).
Case 3: Since Boxw(ym, rn/amin) intersects no obstacles,
ym must be connected to some parent when it is first
considered. ym−1 is a candidate, so c(ym) ≤c(ym−1) +
d(ym−1, ym) ≤Pm−1
k=1 d(yk, yk+1).
Case 4: When ym is first considered, it is because there
exists z ∈Boxw(ym, rn/amin) such that z is the minimum-
cost element of H. Again since Boxw(ym, rn/amin) inter-
sects no obstacles and contains at least one node in H,
ym must be connected to some parent when it is first
considered. Since c(ym−1) < ∞, there is a well-defined
path P = {v1 = xinit, v2, . . . , vq = ym−1} giving its cost-to-
come in the DFMT∗tree at termination. Let w = vj, where
j = maxi∈{1,...,q}{i : vi ∈H when ym is first considered}.
Then there are two subcases, either w ∈B(ym, rn) or
w /∈B(ym, rn). If w ∈B(ym, rn), then,
c(ym) ≤c(w) + d(w, ym)
≤c(w) + d(w, ym−1) + d(ym−1, ym)
≤c(ym−1) + d(ym−1, ym) ≤
m−1
X
k=1
d(yk, yk+1).
If w /∈B(ym, rn), then rn ≤d(w, ym) ≤d(w, ym−1) +
d(ym−1, ym), so we have
c(ym) ≤c(z) + d(z, ym) ≤c(w) + rn
≤c(w) + d(w, ym−1) + d(ym−1, ym)
≤c(ym−1) + d(ym−1, ym) ≤
m−1
X
k=1
d(yk, yk+1).
Thus the inductive step holds in all cases and therefore
(5) holds for all m. In particular taking m = M, and noting
that rn ≤γ implies that yM ∈Mgoal, this means that cn ≤
c(yM) ≤PM−1
k=1 d(yk, yk+1) = c(y∗) as desired.