Sampling-based Optimal Motion Planning

for Non-holonomic Dynamical Systems

Sertac Karaman Emilio Frazzoli

Abstract— Sampling-based motion planning algorithms, such fact, the asymptotic computational complexity of these al-
as the Probabilistic RoadMap (PRM) and the Rapidly-exploring gorithms is the same as the most efficient existing sampling-
Random Tree (RRT), have received a large and growing amount based algorithms, such as the RRT. The enabling idea behind
of attention during the past decade. Most recently, sampling-
based algorithms, such as the PRM∗ and RRT∗ , that guarantee PRM∗ and RRT∗ algorithms is to connect samples within
asymptotic optimality, i.e., almost-sure convergence towards hyperspheres of volume log n/n, a rate that guarantees
optimal solutions, have been proposed. Despite the experimental asymptotic optimality and computational efficiency at the
success of asymptotically-optimal sampling-based algorithms, same time. More recently, the RRT∗ algorithm was demon-
their extensions to handle complex non-holonomic dynamical strated on a number of robotic platforms [10], [11].
systems remains largely an open problem. In this paper, with the
help of results from differential geometry, we extend the RRT∗ In [9], we have focused solely on holonomic dynamical
algorithm to handle a large class of non-holonomic dynamical systems (in the sense defined later in this paper). In this
systems. We demonstrate the performance of the algorithm in paper, we propose modifications to the PRM∗ and RRT∗
computational experiments involving the Dubins’ car dynamics. algorithms in order to handle a large class of non-holonomic
dynamical systems. In particular, we show that, by seeking
I. I NTRODUCTION connections within boxes that are substantially larger in
some dimensions than others, both asymptotic optimality
The motion planning problem, i.e., finding a trajectory
and computational efficiency can be ensured. The shape and
from an initial state to a goal state in a complex environ-
orientation of these boxes can be efficiently computed for
ment, is a fundamental problem of robotics [1] with many
a large class of dynamical systems. Our analysis is based
applications well outside the robotics domain [2].
on differential geometry, and it provides new insight into
Despite the discouraging computational complexity re-
sampling-based algorithms tailored for planning problems
sults [3], many practical algorithms were proposed. In partic-
involving non-holonomic dynamical systems, which may be
ular, algorithms based on sampling, such as the Probabilistic
of independent interest. In particular, our results may also
RoadMap (PRM) [4] and the Rapidly-exploring Random
lead to effective nearest neighbor search metrics for RRTs.
Tree (RRT) [5] algorithms, have achieved substantial ex-
Our work in this paper extends our previous work in [12],
perimental success (see, e.g., [6]). Often implemented using
where we had identified an RRT∗ variant for a class of
random sampling, these algorithms guarantee probabilistic
non-holonomic dynamical systems. Although this variant
completeness, i.e., the algorithm returns a feasible path, when
was asymptotically optimal, it was not as efficient as, for
one exists, with probability approaching to one as the number
example, the RRT algorithm. Extending RRT∗ to deal with
of samples approaches infinity [4], [5], [1].
non-holonomic dynamical systems have also been considered
In many applications of robotics, however, the quality
by many others (see, e.g., [13], [14]). Most of this very
of the solution returned by the algorithm is also a major
recent research focuses on developing steering functions for
concern. In fact, in the context of sampling-based motion
various classes of dynamical systems. In contrast, our work
planning, the importance of the quality of the motion,
in this paper is on ensuring computational effectiveness of
measured with respect to a cost function, has been realized
the algorithm, when the steering function is given.
early on [7], [8]. However, most existing algorithms relied
on problem-specific heuristics and failed to provide any II. P ROBLEM D EFINITION
guarantees on convergence towards optimal solutions.
Consider the following time-invariant dynamical system:
Recently, in our previous work in [9], we have studied
sampling-based algorithms in terms of their convergence to ẋ(t) = f (x(t), u(t)), x(0) = x0 (1)
optimal solutions. In particular, we have shown that, some
of the widely-used path planning algorithms, such as the where x(t) ∈ X ⊆ Rn , u(t) ∈ U ⊆ Rm , f : X × U → Rn
RRT, fail to converge to optimal solutions, with probability is Lipschitz continuous in both of its arguments.1
one. Subsequently, we have proposed two novel algorithms, The state x0 ∈ X (see Equation (1)) is called the initial
called the PRM∗ and the RRT∗ , which guarantee asymptotic state. Let Xobs ⊂ X and Xgoal ⊂ X, called the obstacle set
optimality, i.e., almost-sure convergence to globally optimal and the goal set, be open relative to X. Let c : Σ → R≥0
solutions, without sacrificing computational efficiency. In be a cost function that assigns to each non-trivial trajectory

The authors are with the Department of Aeronautics and Astronautics, 1 We tacitly assume that the sets X and U are smooth manifolds (see
Massachusetts Institute of Technology. Email: sertac,[email protected] Section III-A for the definition of a smooth manifold).
a non-zero cost. Then, the motion planning problem is to M is diffeomorphic to an open set V 0 ⊂ Rd .2 An interval,
find a dynamically-feasible trajectory x : [0, T ] → X that usually denoted by I, is a convex subset of R. A smooth
(i) starts from the initial state, i.e., x(0) = x0 , (ii) avoids curve on a manifold M is a smooth function γ : I → M for
collision with obstacles, i.e., x(t) ∈/ Xobs for all t ∈ [0, T ], some interval I that includes the origin. Given a point p ∈
and (iii) reaches the goal region, i.e., x(T ) ∈ Xgoal . The M , a vector v ∈ Rn is said to be a tangent vector of M at p,
optimal motion planning problem is to find a trajectory x∗ : if there exists a smooth curve γ : R → M such that γ(0) = p
[0, T ] → X that solves the motion planning problem while and γ̇(0) = v. The tangent space of M at p is defined as
minimizing the cost function c. Tp M := {γ̇(0) | γ is a smooth curve on M and γ(0) = p}.
Example A trivial example of a manifold is the n-
III. T HE S HAPE OF S MALL - TIME ATTAINABLE S ETS dimensional Euclidean space, Rn , which is n-dimensional,
Given a state x0 ∈ X and a real number t > 0, define the since it is diffeomorphic to itself. One of the most basic
small-time attainable set, denoted by A(x0 , t), as the set of examples of a non-trivial manifold
p is the unit circle. Defined
all states reachable within time t by the dynamical system de- as S 1 := {(x1 , x2 ) ∈ Rn | x21 + x22 = 1} ⊂ R2 , it is a
scribed by Equation (1) when the system startsSfrom the state one-dimensional manifold that has important applications in
x0 . The attainable set is defined as A(x0 ) := t>0 A(x0 , t). robotics [23], [1]. In particular, the configuration space of an
The following property, also called the accessibility property n-link planar manipulator can be represented by its n joint
by D. Elliot [15], will play an important role in our analysis. angles, hence the manifold formed by the Cartesian product
of n unit circles, i.e., S 1 ×S 1 ×· · ·×S 1 . Another example is
the configuration space of a Reeds-Shepp car [24] involving
Definition 1 A system is said to be small-time locally at- its planar position and its orientation, i.e., R2 × S 1 . 
tainable (STLA) at state x0 ∈ X if A(x0 , t) has a non-empty
interior (with respect to X) for all t > 0. B. Sub-Riemannian Geometry
A smooth function Y : M → Rn is said to be a smooth
It is worth noting at this point that the small-time attain- vector field on the manifold M , if Y (p) ∈ Tp M for all
ability property is closely related to small-time local con- p ∈ M . The set of all smooth vector fields on M is denoted
trollability. According to Sussmann [15], a system described by VF(M ). Let k, l ∈ N. A set E ⊂ M × Rl is said to be a
by Equation (1) is said to be small-time locally controllable smooth vector bundle of rank k over the manifold M , if the
(STLC) at x0 ∈ X if A(x0 , t) contains a neighborhood of x0 set Ep := {v ∈ Rl | (p, v) ∈ E} is a k-dimensional linear
for all t > 0 and locally controllable at x0 if A(x0 ) contains subspace of Rl . The set Ep is also called the fiber of E
an open neighborhood of x0 . Clearly, the STLC property over p. In particular, the vector bundle T M := {(p, v) | p ∈
implies the STLA property. Hence, STLC is a stronger M, v ∈ Tp M } is called the tangent bundle of M . Note that
condition. For instance, the Dubins’ car [16] is STLA but its fiber, Tp M , is an m-dimensional linear subspace of Rn .
not STLC [17]. In Section III-C, we recall the conditions A subbundle of a vector bundle is a subset that is a vector
under which a system is STLA or STLC. bundle on its own right.
In rest of this section, a key lemma characterizing the A distribution on a manifold M is a subbundle H of
small-time attainable set for a large class of dynamical sys- the tangent bundle T M . A sub-Riemannian geometry on a
tems is provided (see Lemma 4). This key lemma constitutes manifold M is a distribution H on M together with an inner
the building block for the proposed algorithms in Section IV product h·, ·i : H × H → R≥0 . The set H is also called the
and for our analysis to follow in Section V. horizontal distribution. The fiber of H at a point p ∈ M
Before stating this result, some other useful results from is denoted by Hp . A curve γ : I → M defined on the
sub-Riemannian geometry are introduced in this section. manifold M is said to be horizontal if it is tangent to H,
Along the way, a number of examples from control theory i.e., γ̇(t) = Hγ(t) for all t ∈ I.
and robotics are provided. Our notation in this section is
fairly standard and closely follows that of Montgomery [18]. Example Suppose the configuration space of a robot is
For a detailed exposition to sub-Riemannian geometry, the represented by a manifold M . Then, the dynamics governing
reader is referred to the monographs [19], [20], [18], [21]. this robot, i.e., constraints on its velocities while moving
within its configuration space, can be represented by a dis-
tribution. Recall from the previous example that the config-
A. Fundamentals of Differential Geometry
uration space of the Reeds-Shepp car can be represented by
Let n, m ∈ N, and let V ⊆ Rn and W ⊆ Rm be open sets. the manifold M = R2 × S 1 encoding its planar position and
Recall that a function f : V → W is said to be smooth if all its orientation. To succinctly describe its dynamics, define
its partial derivatives exist and are continuous. The function the state variables (x, y, θ), where (x, y) ∈ R2 describes the
f is said to be a diffeomorphism if f is a bijection and both f position and θ ∈ [−π, π] describes orientation. Then, ẋ(t) =
and its inverse f −1 are smooth. Two open sets V, W are said
2 In general, a smooth manifold is a Hausdorff space with a countable basis
to be diffeomorphic if there exists such a diffeomorphism.
such that any point on the manifold has a neighborhood that is diffeomorphic
A set M ⊆ Rn is a smooth d-dimensional manifold, if, for to an open subset of the Euclidean space [22]. For all practical purposes,
all p ∈ M , there exists an open set V ⊂ Rn such that V ∩ we are only interested in manifolds that are subsets of an Euclidean space.
u1 (t) cos(θ(t)), ẏ(t) = u1 (t) sin(θ(t)), θ̇(t) = u1 (t) u2 (t) Theorem 2 (Chow’s Theorem [25] (see [18])) Let H be a
where |u1 (t)|, |u2 (t)| ≤ 1. The input signals u1 , u2 denote bracket generating distribution on a connected manifold M .
the forward velocity and the steering angle, respectively. Then, any two points of M is joined with a horizontal path.
Right hand side of the equation induces a distribution on
the manifold M . At the origin, i.e., at (x, y, θ) = (0, 0, 0), The condition that “the distribution is bracket generating”
this distribution is spanned by (1, 0, 0) and (0, 0, 1). These is called Chow’s condition in sub-Riemannian geometry [18],
vectors span a two dimensional plane along the coordinates the linear algebra rank condition in control theory [26], [27],
x and θ. This merely indicates that the car can move in the [28], and Hörmander’s condition in the context of PDEs [29].
longitudinal direction and it can change its orientation; but it Example We continue the previous example. Given any con-
can not move sideways, as the vector (0, 1, 0) is not included figuration (x, y, θ) of the Reeds-Shepp car, the vector fields
in the span of the distribution at the origin.  Y1 = (cos θ, sin θ, 0) and Y2 = (0, 0, 1) span the distribution
induced by its dynamics. The Lie bracket of these two vector
The length of a (smooth) R horizontal curve γ is de- fields can be calculated as [Y1 , Y2 ] = (− sin θ, cos θ, 0).
fined as Length(γ) = I
kγ̇(t)kdt, where kγ̇(t)k =
p Notice that the vector fields {Y1 , Y2 , [Y1 , Y2 ]} span the whole
hγ̇(t), γ̇(t)i. Since we are only interested in manifolds
tangent space, satisfying Chow’s condition. Then, Chow’s
that are submanifolds of an Euclidean space, we will take
theorem merely tells us that, given two configurations of
the inner product to be the usual Euclidean inner prod-
the Reeds-Shepp car, there exists a dynamically-feasible
uct. Thus, k · k denotes the usual Euclidean norm on
trajectory, i.e., a horizontal curve, that joins the two. 
Rn throughout this paper. The length function induces a
distance function on M . Given two points p, p0 ∈ M , Let H be a bracket generating distribution. Then, for any
the sub-Riemannian distance from p to p0 is defined as p ∈ H, there exists an integer r(p) such that Hp ⊂ Hp2 ⊂
d(p, p0 ) := inf{Length(γ) | γ is a smooth horizontal curve r(p)
· · · ⊂ Hp = Tp M. The smallest such integer is called the
that connects p and p0 }. By convention, d(p, p0 ) is infinite if degree of nonholonomy of the distribution H at point p [30],
there is no such curve. Define the sub-Riemannian ball of [18]. Define nk (p) := dim(Hpk ). Then, (n1 (p), n2 (p), . . . ,
radius  centered at p as B(p, ) := {p0 ∈ M | d(p, p0 ) ≤ }. nr(p) (p)) is called the growth vector of H at p. Note that,
Central to our analysis will be the size of the sub- when H is bracket generating, the dimensionality of Hp
r(p)
Riemannian ball. In what follows, we recall from the dif- equals to that of Tp M , i.e., nr(p) = n. The point p is called
ferential geometry literature an important result, called the a regular point if there exists an open neighborhood of M
ball-box theorem, which provides an asymptotic estimate for around p such that the growth vector is constant; otherwise,
the size of the sub-Riemannian ball (see Theorem 3). First, p is said to be a singular point of H.
let us introduce some preliminary definitions. For a given vector field Y , let exp tY denote its flow. That
Given two vector fields Y, Z defined on a smooth manifold is, (exp tY ) : M → M is such that (exp tY )(p) is equal to
M , their Lie bracket is a vector field defined as [Y, Z](p) := y(t) where y : [0, t] → M is a solution to the following
dZ(p)Y (p) − dY (p)Z(p), for all p ∈ M , where dY (p) : differential equation: dt d
y(τ ) = Y (y(τ )) and y(0) = p. In
Tp M → Rn denotes the derivative of the Y at point p ∈ M . other words, (exp tY )(p) denotes the point that a system
The derivative of Y at point p ∈ M in direction v ∈ Tp M , evolving on the manifold M reaches under the influence of
denoted by dY (p) v, is defined as follows. Let γ : R → M the vector field Y for t time units starting from the point p.
be a smooth curve such that γ(0) = p and γ̇(0) = v, and Suppose that p is a regular point, and denote the
define dY (p) v = dt d
t=0
Y (γ(t)) = lims→0 Y (γ(s))−Y
s
(p)
. growth vector simply as (n1 , n2 , . . . , nr ), where nr =
Let Lie(F) denote the set of all vector fields formed by n. Define the vector fields Y1 , Y2 , . . . , Yn as follows: the
F along with all iterated Lie brackets of vector fields from set {Y1 , Y2 , . . . , Yn1 } of vector fields spans H; the set
F, i.e., Lie(F) := F ∪ {[Y1 , [Y2 , . . . [Yn−1 , Yn ] . . . ] | Yi ∈ {Y1 , Y2 , . . . , Yn2 } spans H2 ; and so on. Define the joint
F, n ∈ N}. It can be shown that Lie(F), also called the Lie flow of all these vector fields as Φ(t1 , t2 , . .
. , tn ; p) :=
hull of F, is a Lie algebra. Given a manifold M and a point (exp t1 Y1 ) ◦ (exp t2 Y2 ) ◦ · · · ◦ (exp tn Yn ) (p), where
p ∈ M , define Lie(F)p := {Y (p) | Y ∈ Lie(Y )}. ◦ denotes the functional composition operator. Hence,
Given a distribution H on M , let Hp denote its fiber at the Φ(t1 , t2 , . . . , tn ; p) denotes the point reached under the influ-
point p ∈ M . Define H1 := {Y ∈ VF(M ) | Y (p) ∈ Hp }. ence of vector field Yn for tn time units, then Yn−1 for tn−1
For all k ∈ N and k > 1, define Hk+1 := Hk ∪ [H1 , Hk ], time units, and so on. Since p is a regular point, such a map
where [H1 , Hk ] = Span{[Y, Z] | Y ∈ H1 , Z ∈ Hk }. Define can be defined in some neighborhood of p. For notational
the fiber of Hk at point p ∈ M as Hpk := {Y (p) | Y S ∈ H }.
k
convenience, define the weights wi := k if Yi (p) ∈ Hpk
Then, the Lie hull of H is simply Lie(H) := k≥1 Hk . and Yi (p) ∈ / Hpk+1 for all i ∈ {1, 2, . . . , n}, and define
Its fiber over p ∈ M is Lie(H)p := {Y (p) | Y ∈ Lie(H)}. w := (w1 , w2 , . . . , wn ). Finally, define the w-weighted box
The distribution H is said to be bracket generating, if the of size  > 0 centered at p ∈ M as
Lie hull of H spans the whole tangent space of M , i.e.,
Lie(H)p = Tp M for all p ∈ M . A central result in sub- Boxw (p, ) := {Φ(t1 , t2 , . . . , tn ; p) | |tk | ≤ wk }.
Riemannian geometry is stated in the following theorem. Recall that the sub-Riemannian ball of radius  centered at
p is defined as B(p, ) := {p0 ∈ M | d(p, p0 ) ≤ }.
 The following theorem, attributed to Mitchell, Ger- Chow’s condition. Moreover, Krener’s result can be related
shkovich, and Nagel-Stein-Wainger by Gromov [20], is of to the ball-box theorem as follows.
central importance for the purposes of this paper.
Lemma 4 Let H denote the distribution (on M ) that is
Theorem 3 (Ball-Box Theorem (see [18])) Assume H sat- generated by the vector fields {f, g1 , . . . , gm }. Suppose H
isfies Chow’s condition. Then, there exists constants satisfies the Chow’s condition. Let x0 be a regular point of
0 , c, C > 0 such that for all  < 0 and for all p ∈ M , this distribution. Let w denote the weight vector at x0 . Then,
there exists constants t0 , c > 0 such that for all t < t0 there
Boxw (p, c) ⊂ B(p, ) ⊂ Boxw (p, C) . exists a point x1 ∈ M such that Boxw (x1 , c t) ⊂ A(x0 , t).
The ball-box theorem states that the set of states that can This lemma will play a central role in proving our main
be reached by a horizontal path starting from p contains a results (Theorems 5 and 6). The proof of Lemma 4 follows
weighted box of radius c  and is contained in a box of radius from the results in [33], [32]. Below, we provide an example
C . The orientation of these boxes at a particular point is that illustrates Hermann’s result and Lemma 4.
determined by the vector fields {Y1 , Y2 , . . . , Yn } evaluated
at that point. It is remarkable that the shape of the sub- Example Denote the distribution induced by the dynamics
Riemannian ball can be estimated up to a constant factor for of the Reeds-Shepp car by H. From the previous example,
an arbitrary distribution. This result will be used in the next H ⊂ H2 = Tp M , and that the vector fields Y1 =
section to estimate the shape of the small-time attainable sets, (cos θ, sin θ, 0), Y2 = (0, 0, 1), and Y3 = (− sin θ, cos θ, 0)
which in turn will be used heavily in our analysis. are such that Span{Y1 , Y2 } = H while Span{Y1 , Y2 , Y3 } =
H2 . Since dim(H) = 2 and dim(H2 ) = 3, the growth
C. Links with Control Systems vector is (2, 3) for all points on the manifold describing
It was recognized early on that Chow’s theorem has the configuration space of the Reeds-Shepp car. The weight
implications on the attainability and controllability properties vector, on the other hand, can be computed as w = (1, 2, 1).
of dynamical systems. For example, Hermann [31] has shown Suppose that the car is at the origin, i.e., (x, y, θ) = 0 :=
that, for any real analytic system that is symmetric in the (0, 0, 0). Then, Y1 (0) = (1, 0, 0), Y2 (0) = (0, 0, 1), and
sense that any dynamically-feasible trajectory run backwards Y3 (0) = (0, 1, 0). In this case, the ball-box theorem implies
is also a dynamically-feasible trajectory, the system is con- that the set of states reachable from 0 within t time units
trollable if and only if Chow’s condition holds. In fact, the contains a box of size [−c t, c t] × [−c t2 , c t2 ] × [−c t, c t] for
sufficiency of Chow’s condition is immediate from Theo- all small enough t > 0, where c > 0 is a constant.
rem 2. An example of such a system is a driftless control- That is, the car can move a distance of c t in the longi-
affine real-analytic dynamical system with the property that tudinal direction within t time units, and it can turn with
the control set U is symmetric with respect to the origin. a constant speed, both of which are apparent from the
In [30], the author studies such systems in depth using Lie equations describing its dynamics. However, the ball-box
theoretic methods. In particular, it is known that a symmetric theorem also estimates that the car can move a distance of
real-analytic system is small-time locally controllable if and c t2 towards the lateral direction by combining its maneuvers,
only if Chow’s condition holds. Moreover, the ball box which is not immediately obvious.
theorem provides distance estimates in the following sense. Let us explicitly provide a sequence of maneuvers that
Pm dynamics can be written in the following form: ẋ(t) =
The translates the car sideways for at least a distance of c t2
i=1 ui (t) gi (x(t)), where x(t) ∈ M and u(t) ∈ U . Let within time t. Consider the following four maneuvers applied
H denote the distribution generated by gi ’s. Suppose that H sequentially in this order: (i) forward and right, i.e., u1 (t) =
satisfies Chow’s condition and x0 ∈ M is a regular point of u2 (t) = −1, (ii) forward and left, i.e., u1 (t) = 1, u2 (t) = 1,
H. Denote the weight vector at x0 by w = (w1 , . . . , wn ). (iii) backward and right, i.e., u1 (t) = −1, u2 (t) = −1, and
Then, there exists t0 , c, C > 0 such that Boxw (x0 , ct) ⊂ (iv) backward and left, i.e., u1 (t) = −1, u(t) = 1, with each
A(x0 , t) ⊂ Boxw (x0 , Ct) for all t < t0 [30]. set of controls applied T /4 time units. See Figures 1.(a)-(b).
It is can be shown that a symmetric real-analytic system As seen in the figure, with each maneuver the car moves a
is STLC if and only if it is STLA. Thus, for such systems, distance of 1 = cos(α) = 1 − cos((π/2) t) along the y-axis.
2 4
Chow’s condition characterizes both the STLC and STLA Using the Taylor expansion, cos α = 1 − α2! + α4! − · · · ,
properties in terms of Lie brackets. However, many realistic we conclude that total distance traveled by the car along the
engineering systems are not symmetric. (Important examples y-axis satisfies 4 (1 − cos((π/2) t)) = c t2 + o(t2 ). Hence,
include the class of linear systems and systems with drift.) the car ends up roughly at the point (0, c t2 , 0), confirming
Fortunately, the small-time local attainability property the result of the ball-box theorem. 
is completely characterized in terms of Lie brackets by
Krener [32] (see also [33]). His result, usually called the Example An example of a dynamical system with drift
positive form of Chow’s theorem [15], implies the follow- is the Dubins’ car [16] described by the following set of
Pma real-analytic control-affine system: ẋ(t) =
ing. Consider equations: ẋ(t) = cos(θ(t)), ẏ(t) = sin(θ(t)), θ̇(t) = u(t)
f (x(t)) + i=1 ui (t) g(x(t)). This system is STLA if and where |u(t)| ≤ 1 is the steering input. In this model, the
only if the distribution generated by {f, g1 . . . , gm } satisfies Dubins car travels with constant unit speed.
 Algorithm 1: The RRT∗ Algorithm
1 V ← {zinit }; E ← ∅; i ← 0;
2 while i < N do
3 G ← (V, E);
4 zrand ← Sample(i); i ← i + 1;
5 (V, E) ← Extend(G, zrand );

Algorithm 2: The Extend Procedure

(a) (b) (c)
1 V 0 ← V ; E 0 ← E;
Fig. 1. Figure (a) shows the resulting trajectory of the vehicle when it 2 znearest ← Nearest(G, z);
moves forward while turning right. Figure (b) shows the resulting trajectory 3 (xnew , unew , Tnew ) ← Steer(znearest , z);
after four maneuvers. The x- and y-axes are also shown in the figures. In 4 znew ← xnew (Tnew );
Figure (c), the small-time reachable set of the Dubins car and the box of 5 if ObstacleFree(xnew ) then
size [−c t, c t] × [−c t2 , c t2 ] × [c t, c t] that is included in the reachable 6 V 0 := V 0 ∪ {znew };
set are illustrated. Only the projection of these sets to x- and y−axes are 7 zmin ← znearest ; cmin ← Cost(znew );
shown. The projection of the reachable set is shaded in light grey whereas
that of the box is shaded in dark grey.
8 Znearby ← NearVertices(G, znew , |V |);
9 for all znear ∈ Znearby do
By Lemma 4, the set of states reachable within 10 (xnear , unear , Tnear ) ← Steer(znear , znew );
11 if ObstacleFree(xnear ) and xnear (Tnear ) = znew
time t starting from (x, y, θ) includes a box with size
then
[−c t, c t] × [−c t2 , c t2 ] × [−c t, c t] oriented in coordinates 12 if Cost(znear ) + J(xnear ) < cmin then
(cos θ, sin θ, 0), (− sin θ, cos θ, 0), (0, 0, 1). (Figure 1.(c).)  13 cmin ← Cost(znear ) + J(xnear );
14 zmin ← znear ;
In the rest of this paper, we will focus on real analytic
control-affine dynamical
Pm systems of the following form: 15 E 0 ← E 0 ∪ {(zmin , znew )};
ẋ(t) = f (x(t)) + i=1 ui (t) gi (x(t)), x(0) = x0 where 16 for all znear ∈ Znearby \ {zmin } do
x(t) ∈ X ⊂ Rn , u(t) = (u1 (t), u2 (t), . . . , um (t)) ∈ U , 17 (xnear , unear , Tnear ) ← Steer(znew , znear );
f (·, ·) is a real analytic function, and X is a real analytic 18 if xnear (Tnear ) = znear and ObstacleFree(xnear )
and Cost(znear ) > Cost(znew ) + J(xnear ) then
manifold. Let H denote the distribution generated by the 19 zparent ← Parent(znear );
vector fields {f, g1 , . . . , gm }. In the sequel, we refer to H 20 E 0 ← E 0 \ {(zparent , znear )};
as the distribution generated by the dynamics. 21 E 0 ← E 0 ∪ {(znew , znear )};
Recall that Boxw (x, ) denotes the weighted box of size
 centered at x ∈ X, where w = (w1 , w2 , . . . , wn ) denotes 22 return G0 = (V 0 , E 0 )
the weight vector at x. We assume that the weightP vector is
constant over the whole manifold X. Define D := i=1 wi .
n d) Near Neighbors: The Near procedure returns a
The integer D is at least as large as the dimensionality n subset of V with states close to x in the following sense:
of the state space. Let us note that D coincides with the  1/D !
\
w log |V |
Hausdorff dimension of the distribution H (see [18]). Near(V, x) := V Box x, γ ,
|V |

IV. A LGORITHM where |V | denotes the cardinality of V , D is the Hausdorff

dimension of the distribution generated by the dynamics, and
Before presenting the RRT∗ algorithm for non-holonomic γ is a constant that is independent of |V |. Note at this point
dynamical systems, let us note the following primitive pro- that the volume of the weighted box in the right hand side
cedures. Recall that k · k denotes the usual Euclidean norm. of the definition above is equal to γ D (log |V |/|V |).
a) Sampling: The Sample procedure returns uniformly e) Collision Checking: Given a trajectory x : [0, T ] →
random samples from the free space on the maximal integral X, the ObstacleFree(x) procedure returns true if x avoids
manifold of the dynamics through x0 , i.e., Nx0 \ Xobs . collision with obstacles, i.e., x(t) ∈
/ Xobs for all t ∈ [0, T ],
b) Local Steering: Given two states x1 , x2 ∈ X, and returns false otherwise.
the Steer procedure returns a terminal time T and a Let us remark that the sampling, local steering, and
dynamically-feasible trajectory connecting x1 to x2 . This near neighbor procedures differ from their counterparts in
trajectory is denoted by Steer(x1 , x2 ) : [0, T ] → X. We RRT∗ for holonomic systems [9] in a number of ways.
assume that the local steering procedure satisfies the topolog- Firstly, the sampling procedure is assumed to return random
ical property (see [17]), i.e., for any  > 0 there exists some samples from the maximal integral manifold through the
real number η > 0 such that, for any two states x1 , x2 ∈ X initial state.3 Second, the steering procedure must have the
with kx1 − x2 k < η, we have kx1 − Steer(x1 , x2 )(t)k <  topological property4 To guarantee optimality, in addition,
for all t ∈ [0, T ]. Clearly, η ≤ . Apart from this property, 3 If the Lie hull of the distribution generated by the dynamics spans the
we also assume η / converges to one as  approaches zero. tangent space of the state space at every point of X, then the maximal
c) Nearest Neighbor: Given a state x and a finite set V integral manifold is X itself. Hence, in that case, the sampling procedure
returns samples from the free space X \ Xobs .
of states, the Nearest procedure returns the state in V that 4 This property is often required even when one is concerned only with
is closest to x, i.e., Nearest(V, x) := arg minx0 ∈V kx0 −xk. finding a feasible trajectory and optimality is not a concern [17].
it requires that η / converges to one as  approaches zero. Theorem 6 (Computational Complexity) The RRT∗ and
Roughly speaking, this implies that as  approaches zero, the RRT algorithms have the same per-iteration asymptotic com-
trajectories returned by the local steering algorithm resemble putational complexity, i.e., there exists a finite c such that
∗
time-optimal trajectories. Third, and most important, the near limn→∞ E[MnRRT ]/E[MnRRT ] ≤ c.
neighbors are not computed considering an Euclidean ball,
but within a weighted Euclidean box the shape of which The proofs of these theorems are omitted for the sake
resembles the shape of the sub-Riemannian ball. of brevity. The proof of Theorem 5 is the same as the
The RRT∗ algorithm is given in Algorithm 1. The algo- corresponding result in our previous work in [9] with one
rithm is the same as that presented in [9] on all accounts exception: the balls with volume (log n/n) are replaced with
except the primitive procedures. If the Hausdorff dimension scaled boxes (as defined in the presentation of the Near
of the distribution is equal to that of the state space, i.e., procedure), which also have volume log n/n, where n is the
D = n, that is, when the system at hand is holonomic, then number of vertices in the graph. The proof of Theorem 6 is
Algorithm 1 reduces to the RRT∗ algorithm presented in [9]. also similar to the corresponding result in [9].

V. A NALYSIS VI. C OMPUTATIONAL E XPERIMENTS

In this section, we analyze the proposed algorithm in terms The algorithm proposed in Section IV was implemented
of asymptotic optimality and computational complexity. using the C++ programming language . The implementation
First, let us recall some definitions from [9] modified for was executed on a computational platform powered by a
the setting in this paper. Let δ be a positive real number. A processor with 2.66 GHz clock speed and 8GB of RAM.
state x is said to be in the δ-interior of the free space, if the The algorithm was run in an empty environment of length
closed ball of radius δ lies entirely inside Xfree = X \ Xobs . 20 and width 20 centered at the origin. The cost function
The collection of all states that are in the δ-interior of Xfree , was chosen as the time to reach the goal region, a square
denoted by intδ (Xfree ), is called the δ-interior of Xfree . region centered at (7, 7) of size 2 in both directions.
A dynamically-feasible trajectory x : [0, T ] → X is said The results are compared with a naive algorithm that
to have strong δ-clearance if it lies entirely in the δ-interior is asymptotically optimal but not computationally efficient.
of the free space, i.e., x(t) ∈ intδ (Xfree ) for all t ∈ [0, T ]. A This algorithm runs exactly as the RRT∗ , except the Near
dynamically feasible trajectory is said to be robustly feasible procedure is defined as follows: Near(V, x) := {x0 ∈ X :
it has strong δ-clearance, for some δ > 0, and solves kx0 − xk∞ ≤ (log n/n)1/D }, where k · k∞ denotes the
Let ΣT denote the set of all Lebesgue measurable func- L∞ norm. Hence, this algorithm seeks connections within a
tions from [0, T ] to Rn . Given x, x0 ∈ Σ, define kx0 −xk∞ := hypercube, rather than a scaled box. It can be shown that this
maxt∈[0,T ] kx0 (t) − x(t)k, where k · k is the usual Euclidean hypercube is large enough to ensure asymptotic optimality. In
norm on Rn . A continuous function ψ : [0, 1] → Σ is called fact, it includes the scaled box in the earlier definition of the
a homotopy between two trajectories x, x0 : [0, T ] → Xfree , Near procedure. However, it makes too many connections.
if ψ(0) = x and ψ(1) = x0 and ψ(τ ) is collision-free for all In the experimental study, the proposed algorithm is com-
τ ∈ [0, 1]. The trajectories x and x0 are said to be homotopic, pared with the naive algorithm in Monte-Carlo simulations
if there exists a homotopy between them. A dynamically- where each algorithm is run 50 times independently. The
feasible trajectory x : [0, T ] → X is said to have weak results are shown in Figure 2. In Figure 2.(a), we compare
δ-clearance if there exists a path x0 : [0, T ] → X that has the number of connections made by each algorithm. More
strong δ-clearance, and there exists a homotopy ψ between x precisely, the average number of connections per iterations is
and x0 such that ψ(τ ) has strong δτ -clearance for some δτ > divided by log(n), where n is the number of iterations. From
0 for all τ ∈ (0, 1). The reader is referred to [9] for examples Theorem 6, we expect that this quantity is a constant for the
of trajectories with weak δ-clearance. A dynamically-feasible proposed algorithm, since the number of connections should
trajectory x∗ : [0, T ] → X that solves the optimal motion be order log n, which can be seen in the plot. However, in the
planning problem is said to be robustly optimal if it has weak naive algorithm, the same quantity is clearly an increasing
δ-clearance and any sequence {xn }n∈N of trajectories that function of n. In fact, the excess number of connections
converge to x∗ satisfies limn→∞ c(xn ) = c(x∗ ). lead to substantial computational overhead in the planning
A sampling-based algorithm is said to be asymptotically process as seen in Figure 2.(b), where we plot the average
optimal if, for any instance of the motion planning problem computation time to reach a particular iteration for both
that admits a robustly optimal solution with finite cost c∗ , algorithms. Yet, most of the excess connection attempts
P(lim supn→∞ Yn = c∗ ) = 1, where Yn is the cost of the are indeed unsuccessful. As seen from Figure 2.(c), the
best path after invoking the sampling procedure n times. difference in the quality of the solution provided by the two
algorithms is negligible, despite the substantial difference in
Theorem 5 (Asymptotic Optimality) The RRT∗ algorithm computational costs.
(Algorithm 1) is asymptotically optimal. ACKNOWLEDGEMENTS
Let MnRRT denote the number of simple operations per- This work was supported in part by the Army Research
formed the by the RRT algorithm in iteration n. The random Office (ARO MURI award W911NF-11-1-0046) and the
∗
variable MnRRT is defined similarly. National Science Foundation (grant CNS-1016213).
 140

150

120

Average number of connections / log (number of iterations)

9.15

9.1
100

9.05
100

Computation time (seconds)

80
9

Average cost
8.95
60

8.9

40 50
8.85

8.8
20

8.75

0
0 1 2 3 4 5 6 7 8 9 10
4
8.7
x 10 0
0 1 2 3 4 5 6 7 8 9 10 1 2 3 4 5 6 7 8 9 10
Number of iterations Number of iterations 4 Number of iterations 4
x 10
x 10

(a) Average number of connections over (b) Average time to reach a particular (c) Average cost versus the number of
log of # of iterations vs. # of iterations. iteration. iterations.
Fig. 2. The two algorithms are compared in terms of the average number of connections (Fig a), the average time to reach a particular iteration (Fig c),
and the average cost at a particular iteration (Fig b). The proposed approach is shown in black, and the naive algorithm is shown in grey.

VII. C ONCLUSION IEEE/RSJ International Conference on Intelligent Robots and Systems

(IROS), 2011.
We have proposed an extension of the RRT∗ that can han- [12] S. Karaman and E. Frazzoli. Optimal Kinodynamic Motion Planning
dle a large class of non-holonomic dynamical systems. The using Incremental Sampling-based Methods. In IEEE Conference on
enabling idea behind this extension is to seek connections Decision and Control (CDC), Atlanta, GA, December 2010.
[13] A. Perez, R. Platt, G. Konidaris, L. Kaelbling, and T. Lozano-
within a scaled box, the dimensions and the orientation of Perez. LQR-RRT*: Optimal sampling-based motion planning with
which is dictated by the ball-box theorem of sub-Riemannian automatically derived extension heuristics*: Optimal sampling-based
geometry. We have shown that the proposed algorithm guar- motion planning with automatically derived extension heuristics. In
IEEE International Conference on Robotics and Automation, 2012.
antees asymptotic optimality, without sacrificing computa- [14] D. J. Webb and J. van den Berg. Kinodynamic RRT∗ : Optimal motion
tional efficiency. We have evaluated the proposed algorithm, planning for systems with linear differential constraints. arXiv, May
in Monte-Carlo computational experiments, when compared 2012.
[15] H J Sussmann. A General Theorem on Local Controllability. SIAM
to a naive algorithm that seeks connections in a hypercube Journal of Control and Optimization, 25(1):158–194, 1987.
large enough to guarantee asymptotic optimality. The results [16] L E Dubins. On Curves of Minimal Length with a Constraint on
indicate that the proposed approach provides significant Average Curvature, and with Prescribed Initial and Terminal Positions
and Tangents. American Journal of Mathematics, 79(3):497–516,
savings in computation time with almost no sacrifice in the 1957.
quality of the solution, when compared to the naive solution. [17] J P Laumond, S Sekhavat, and F Lamiraux. Guidelines in Nonholo-
nomic Motion Planning for Mobile Robots. In Robot Motion Planning
R EFERENCES and Control, pages 171–253. Springer, 1998.
[18] R Montgomery. A Tour of Subriemannian Geometries, Their
[1] Steven Michael LaValle. Planning algorithms. Cambridge Univ Pr, Geodesics, and Applications. American Mathematical Society, 2002.
May 2006. [19] A Bellaiche. The Tangent Space in Sub-Riemannian Geometry. In
[2] J. Latombe. Motion Planning: A Journey of Molecules, Digital Actors, A Bellaiche and J Risler, editors, Sub-Reimannian Geometry, pages
and Other Artifacts. International Journal of Robotics Research, 1–78. Birkhauser, 1996.
18(11):1119–1128, 1999. [20] M Gromov. Carnot-Caratheodory spaces seen from within. In Sub-
[3] J. Reif. Complexity of the Generalized Mover’s Problem. In Riemannian Geometry, pages 79–323. Birkhauser, 1996.
J Schwartz, J Hopcroft, and M. Sharir, editors, Planning, Geometry, [21] R S Strichartz. Sub-Riemannian Geometry. Journal of Differential
and Complexity of Robot Motion, pages 267–281. Ablex Publishing Geometry, 24:221–263, 1986.
Corp., 1987. [22] J R Munkres. Topology. Prentice Hall, 2nd edition, 2000.
[4] L. Kavraki, P. Svestka, J. Latombe, and M. H. Overmars. Probabilistic [23] Jean-Claude Latombe. Robot motion planning. Springer, 1991.
roadmaps for path planning in high-dimensional configuration spaces. [24] J A Reeds and L A Shepp. Optimal paths for a car that goes both
IEEE Transactions on Robotics and Automation, January 1996. forwards and backwards. Pacific Journal of Mathematics, 145(2):367–
[5] S M LaValle and J Kuffner. Randomized kinodynamic planning. The 393, 1990.
International Journal of Robotics Research, 20(5):378–400, 2001. [25] W L Chow. Uber Systeme von Linearen Partiellen Differential-
[6] Y. Kuwata, J. Teo, G. Fiore, S. Karaman, E. Frazzoli, and J. P. How. glaichungen erster Ordnung. Math. Ann., 117:98–105, 1970.
Real-Time Motion Planning with Applications to Autonomous Urban [26] F Bullo and A D Lewis. Geometric Control of Mechanical Systems.
Driving. IEEE Transactions on Control Systems, 17(5):1105–1118, Springer, 2004.
2009. [27] V Jurdevic. Geometric Control Theory. 1997.
[7] C. Urmson and R. Simmons. Approaches for heuristically biasing RRT [28] A Isidori. Nonlinear Control Systems. Springer, 3rd edition, 1994.
growth. In Proceedings of the IEEE/RSJ International Conference on [29] L Hormander. Hypoelliptic second order differential equations. Acta
Intelligent Robots and Systems, 2003. Mathematica, 119(1):147–171, 1967.
[8] D. Furgeson and A. Stentz. Anytime RRTs. In Proceedings of the [30] A Bellaiche, F Jean, and J J Risler. Geometry of Nonholonomic
IEEE/RSJ International Conference on Intelligent Robots and Systems, Systems. In J P Laumond, editor, Robot Motion Planning and Control,
2006. pages 55–91. Springer, 1998.
[9] S Karaman and E Frazzoli. Sampling-based Algorithms for Opti- [31] R Hermann. On the accessibility problem in control theory. In
mal Motion Planning. International Journal of Robotics Research, International Symposium on Nonlinear Differential Equations and
30(7):846–894, 2011. Nonlinear Mechanics, 1963.
[10] S. Karaman, M. Walter, A. Perez, E. Frazzoli, and S. Teller. Anytime [32] A J Krener. A Generalization of Chow’s Theorem and The Bang-Bang
Motion Planning using the RRT*. In IEEE International Conference Theorem To Nonlinear Control Problems. SIAM Journal on Control,
on Robotics and Automation (ICRA), 2011. 12(1):43–52, 1974.
[11] A. Perez, S. Karaman, M. Walter, A. Shkolnik, E. Frazzoli, and [33] H Hermes. Lie Algebras of Vector Fields and Local Approximation
S. Teller. Asymptotically-optimal Path Planning for Manipulation of Attainable Sets. SIAM Journal on Control and Optimization,
using Incremental Sampling-based Algorithms. In Proceedings of the 16(5):715–727, 1978.