http://ijr.sagepub.com/
Robotics Research
The International Journal of
http://ijr.sagepub.com/content/30/7/846
The online version of this article can be found at:
DOI: 10.1177/0278364911406761
2011 30: 846The International Journal of Robotics Research
Sertac Karaman and Emilio Frazzoli
Sampling-based algorithms for optimal motion planning
Published by:
http://www.sagepublications.com
On behalf of:
Multimedia Archives
can be found at:The International Journal of Robotics ResearchAdditional services and information for
http://ijr.sagepub.com/cgi/alertsEmail Alerts:
http://ijr.sagepub.com/subscriptionsSubscriptions:
http://www.sagepub.com/journalsReprints.navReprints:
http://www.sagepub.com/journalsPermissions.navPermissions:
http://ijr.sagepub.com/content/30/7/846.refs.htmlCitations:
What is This?
- Jun 22, 2011Version of Record >>
at UCSF LIBRARY & CKM on December 8, 2014ijr.sagepub.comDownloaded from at UCSF LIBRARY & CKM on December 8, 2014ijr.sagepub.comDownloaded from
Sampling-based algorithms for optimal
motion planning
The International Journal of
Robotics Research
30(7) 846–894
© The Author(s) 2011
Reprints and permission:
sagepub.co.uk/journalsPermissions.nav
DOI: 10.1177/0278364911406761
ijr.sagepub.com
Sertac Karaman and Emilio Frazzoli
Abstract
During the last decade, s ampling-based path planning algorithms, such as probabilistic roadmaps (PRM) and rapidly
exploring random trees (RRT), have been shown to work well in practice and possess theoretical guarantees such as
probabilistic completeness. However, little effort has been devoted to the formal analysis of the quality of the solution
returned by such algorithms, e.g. as a function of the number of samples. The purpose of this paper is to fill this gap, by
rigorously analyzing the asymptotic behavior of the cost of the solution returned by stochastic sampling-based algorithms
as the number of samples increases. A number of negative results are provided, characterizing existing algorithms,
e.g. showing that, under mild technical conditions, the cost of the solution returned by broadly used sampling-based
algorithms converges almost surely to a non-optimal value. The main contribution of the paper is the introduction of
new algorithms, namely, PRM
∗
and RRT
∗
, which are provably asymptotically optimal, i.e. such that the cost of the
returned solution converges almost surely to the optimum. Moreover, it is shown that the computational complexity of
the new algorithms is within a constant factor of that of their probabilistically complete (but not asymptotically optimal)
counterparts. The analysis in this paper hinges on novel connections between stochastic sampling-based path planning
algorithms and the theory of random geometric graphs.
Keywords
Motion planning, optimal path planning,
sampling-based
algorithms, random geometric graphs
1. Introduction
The robotic motion planning problem has received a consid-
erable amount of attention, especially over the last decade,
as robots started becoming a vital part of modern indus-
try as well as our daily life (Latombe 1991; Choset et al.
2005; LaValle 2006). Even though modern robots may
possess significant differences in sensing, actuation, size,
workspace, application, etc., the problem of navigating
through a complex environment is embedded and essential
in almost all robotics applications. Moreover, this problem
is relevant to other disciplines such as verification, compu-
tational biology, and computer animation (Finn and Kavraki
1999; Latombe 1999; Liu and Badler 2003; Bhatia and
Frazzoli 2004; Branicky et al. 2006; Cortes et al. 2007).
Informally speaking, given a robot with a description of
its dynamics, a description of the environment, an initial
state, and a set of goal states, the motion planning problem
is to find a sequence of control inputs so as the drive the
robot from its initial state to one of the goal states while
obeying the r ules of the environment, e.g. not colliding
with the surrounding obstacles. An algorithm to address
this problem is said to be complete if it terminates in finite
time, returning a valid solution if one exists, and failure
otherwise.
Unfortunately, the problem is known to be very hard
from the computational point of view. For example, a basic
version of the motion planning problem, called the gener-
alized piano movers problem, is PSPACE-hard (Reif 1979).
In fact, while complete planning algorithms exist (see, e.g.,
Lozano-Perez and Wesley 1979; Schwartz and Sharir 1983;
Canny 1988), their complexity makes them unsuitable for
practical applications.
Practical planners came around with the development
of cell decomposition methods (Brooks and Lozano-Perez
1983) and potential fields (Khatib 1986). These approaches,
Laboratory for Information and Decision Systems, Massachusetts Institute
of Technology, Cambridge, MA, USA
Corresponding author:
Sertac Karaman, Laboratory for Information and Decision Systems, Mas-
sachusetts Institute of Technology, Cambridge, MA 02139, USA
Email: sertac@mit.edu
at UCSF LIBRARY & CKM on December 8, 2014ijr.sagepub.comDownloaded from
Karaman and Frazzoli 847
if properly implemented, relaxed the completeness require-
ment to, for instance, resolution completeness, i.e. the abil-
ity to return a valid solution, if one exists, if the resolu-
tion parameter of the algorithm is s et fine enough. These
planners demonstrated remarkable performance in accom-
plishing various tasks in complex environments within rea-
sonable time bounds (Ge and Cui 2002). However, their
practical applications were mostly limited to state spaces
with up to five dimensions, since decomposition-based
methods suffered from large number of cells, and poten-
tial field methods from local minima (Koren and Borenstein
1991). Important contributions towards broader applicabil-
ity of these methods include navigation functions (Rimon
and Koditschek 1992) and randomization (Barraquand and
Latombe 1993).
The above methods rely on an explicit representation
of the obstacles in
the
configuration space, which is used
directly to construct a solution. This may result in an
excessive computational burden in high dimensions, and
in environments described by a large number of obstacles.
Avoiding such a representation is the main underlying idea
leading to the development of sampling-based algorithms
(Kavraki and Latombe 1994; Kavraki et al. 1996; LaValle
and Kuffner 2001). See Lindemann and LaValle (2005) for
a historical perspective. These algorithms proved to be very
effective for motion planning in high-dimensional spaces,
and attracted significant attention over the last decade,
including very recent work (see, e.g., Stilman et al. 2007;
Berenson et al. 2008; Yershova and LaValle 2008; Prentice
and Roy 2009; Koyuncu et al. 2010; Luders et al. 2010;
Tedrake et al. 2011). Instead of using an explicit representa-
tion of the environment, sampling-based algorithms rely on
a collision-checking module, providing information about
the feasibility of candidate trajectories, and connect a set
of points sampled from the obstacle-free space in order
to build a graph (roadmap) of feasible trajectories. The
roadmap is then used to construct the solution to the original
motion-planning problem.
Informally speaking, sampling-based methods provide
large amounts of computational savings by avoiding explicit
construction of obstacles in the state space, as opposed to
most complete motion planning algorithms. Even though
these algorithms are not complete, they provide probabilis-
tic completeness guarantees in the sense that the probability
that the planner fails to return a solution, if one exists,
decays to zero as the number of samples approaches infin-
ity (Barraquand et al. 1997). (See also Hsu et al. 1997;
Kavraki et al. 1998; Ladd and Kavraki 2004.) Moreover,
the rate of decay of the probability of failure is exponential,
under the assumption that the environment has good ‘visi-
bility’ properties (Barraquand et al. 1997). More recently,
the empirical success of sampling-based algorithms was
argued to be strongly tied to the hypothesis that most practi-
cal robotic applications, even though involving robots with
many degrees of freedom, feature environments with such
good visibility properties (Hsu et al. 2006).
1.1. Sampling-based algorithms
Arguably, the most influential sampling-based motion plan-
ning algorithms to date include probabilistic roadmaps
(PRMs) (Kavraki et al. 1996, 1998) and rapidly-exploring
random trees (RRTs) (Kuffner and LaValle 2000; LaValle
and Kuffner 2001; LaValle 2006). Even though the idea of
connecting points sampled randomly from the state space is
essential in both approaches, these two algorithms differ in
the way that they construct a graph connecting these points.
The PRM algorithm and its variants are multiple-query
methods that first construct a graph (the roadmap), which
represents a rich set of collision-free trajectories, and then
answer
queries
by computing a shortest path that connects
the initial state with a final state through the roadmap. The
PRM algorithm has been reported to perform well in high-
dimensional state spaces (Kavraki et al. 1996). Further-
more, the PRM algorithm is probabilistically complete, and
such that the probability of failure decays to zero exponen-
tially with the number of samples used in the construction
of the roadmap (Kavraki et al. 1998). During the last two
decades, the PRM algorithm has been a focus of robotics
research: several improvements were suggested by many
authors and the reasons to why it performs well in many
practical cases were better understood (see, e.g., Branicky
et al. 2001; Ladd and Kavraki 2004; Hsu et al. 2006, for
some examples).
Even though multiple-query methods are valuable in
highly structured environments, such as f actory floors, most
online planning problems do not require multiple queries,
since, for instance, the robot moves from one environment
to another, or the environment is not known apriori. More-
over, in some applications, computing a roadmap apriori
may be computationally challenging or even infeasible. Tai-
lored mainly for these applications, incremental sampling-
based planning algorithms such as RRTs have emerged
as an online, single-query counterpart to PRMs (see, e.g.,
Kuffner and LaValle 2000; Hsu et al. 2002). The incre-
mental nature of these algorithms avoids the necessity to
set the number of samples apriori, and returns a solution
as soon as the set of trajectories built by the algorithm is
rich enough, enabling on-line implementations. Moreover,
tree-based planners do not require connecting two states
exactly and more easily handle systems with differential
constraints. The RRT algorithm has been shown to be prob-
abilistically complete (Kuffner and LaValle 2000), with an
exponential rate of decay for the probability of failure (Fraz-
zoli et al. 2002). The basic version of the RRT algorithm
has been extended in several directions, and found many
applications in the robotics domain and elsewhere (see, for
instance, Frazzoli et al. 2002; Bhatia and Frazzoli 2004;
Branicky et al. 2003, 2006; Cortes et al. 2007; Zucker et al.
2007). In particular, RRTs have been shown to work effec-
tively for systems with differential constraints and nonlinear
dynamics (LaValle and Kuffner 2001; Frazzoli et al. 2002)
as well as purely discrete or hybrid systems (Branicky et al.
2003). Moreover, the RRT algorithm was demonstrated in
at UCSF LIBRARY & CKM on December 8, 2014ijr.sagepub.comDownloaded from
848 The International Journal of Robotics Research 30(7)
major robotics events on various experimental robotic plat-
forms (Kuffner et al. 2002; Bruce and Veloso 2003; Kuwata
et al. 2009; Teller et al. 2010; Shkolnik et al. 2011).
Other sampling-based planners of note include expansive
space trees (EST) (Hsu et al. 1997, 1999) and sampling-
based roadmap of trees (SRT) (Plaku et al. 2005). The latter
combines the main features of multiple-query algorithms
such as PRM with those of single-query algorithms such as
RRT and EST.
1.2. Optimal motion planning
In most applications, the quality of the solution returned
by a motion planning algorithm is important. For example,
one may be interested in solution paths of minimum cost,
with respect to a given cost functional, such as the length
of a path, or the time required to execute it. The problem of
computing optimal motion plans has been proven by Canny
and Reif (1987) to be very challenging even in basic cases.
In the context of sampling-based motion planning algo-
rithms, the importance of computing optimal solutions has
been pointed out in early seminal papers (LaValle and
Kuffner 2001). However, optimality properties of sampling-
based motion planning algorithms have not been investi-
gated systematically, and most of the relevant work relies
on heuristics. For example, in many field implementations
of sampling-based planning algorithms (see, e.g., Kuwata
et al. 2009), it is often the case that since a feasible path
is found quickly, additional available computation time is
devoted to improving the solution with heuristics until the
solution is executed. Urmson and Simmons (2003) pro-
posed heuristics to bias the tree growth in RRT towards
those regions that result in low-cost solutions. They have
also shown experimental results evaluating the performance
of different heuristics in terms of the quality of the s olution
returned. Ferguson and Stentz (2006) considered running
the RRT algorithm multiple times in order to progressively
improve the quality of the solution. They showed that each
run of the algorithm results in a path with smaller cost, even
though the procedure is not guaranteed to converge to an
optimal solution. Criteria for restarting multiple RRT runs,
in a different context, were also proposed by Wedge and
Branicky (2008). A more recent approach is the transition-
based RRT (T-RRT) designed to combine rapid exploration
properties of the RRT with stochastic global optimization
methods (Jaillet et al. 2010; Berenson et al. 2011).
A different approach that also offers optimality guar-
antees is based on graph search algorithms, such as A
∗
,
applied over a finite discretization (based, e.g., on a grid,
or a cell decomposition of the configuration space) that
is generated offline. Recently, these algorithms received a
large amount of attention. In particular, they were extended
to run in an anytime fashion (Likhachev et al. 2004,
2008), deal with dynamic environments (Stentz 1995;
Likhachev et al. 2008), and handle systems with differen-
tial constraints (Likhachev and Ferguson 2009). These have
also been successfully demonstrated on various robotic
platforms (Dolgov et al. 2009; Likhachev and Ferguson
2009). However, optimality guarantees of these algorithms
are only ensured up to the grid resolution. Moreover, since
the number of grid points grows exponentially with the
dimensionality of the state space, so does the (worst-case)
running time of these algorithms.
1.3. Statement of contributions
To the best of the authors’ knowledge, this paper provides
the first systematic and thorough analysis of optimality and
complexity properties of the major paradigms for sampling-
based path planning algorithms, for multiple- or single-
query applications, and introduces the first algorithms that
are both asymptotically optimal and computationally effi-
cient, with respect to other algorithms in this class. A sum-
mary of the contributions can be found below, and is shown
in Table 1.
As a first set of results, it is proven that the standard PRM
and RRT algorithms are not asymptotically optimal, and
that the ‘simplified’ PRM algorithm is asymptotically opti-
mal, but computationally expensive. Moreover, it is shown
that the k-nearest variant of the (simplified) PRM algo-
rithm is not necessarily probabilistically complete (e.g. it
is not probabilistically complete for k = 1), and is not
asymptotically optimal for any fixed k.
In order to address the limitations of sampling-based
path planning algorithms available in the literature, new
algorithms are proposed, i.e. PRM
∗
, RRG, and RRT
∗
,
and proven to be probabilistically complete, asymptotically
optimal, and computationally efficient. Of these, PRM
∗
is
a batch variable-radius PRM, applicable to multiple-query
problems, in which the radius is scaled with the number
of samples in a way that provably ensures both asymp-
totic optimality and computational efficiency. RRG is an
incremental algorithm that builds a connected roadmap,
providing similar performance to PRM
∗
in a single-query
setting, and in an anytime fashion (i.e. a first solution
is provided quickly, and monotonically improved if more
computation time is available). The RRT
∗
algorithm is
a variant of RRG that incrementally builds a tree, pro-
viding anytime solutions, provably converging to an opti-
mal solution, with minimal computational and memory
requirements.
In this paper, the problem of planning a path through
a connected bounded subset of a d-dimensional Euclidean
space is considered. As in the early seminal papers on incre-
mental sampling-based motion planning algorithms such as
Kuffner and LaValle (2000), no differential constraints are
considered (i.e. the focus of the paper is on path planning
problems), but our methods can be easily extended to plan-
ning in configuration spaces and applied to several practical
problems of interest. The extension to systems with differ-
ential constraints is deferred to future work (see Karaman
and Frazzoli (2010c) for preliminary results).
at UCSF LIBRARY & CKM on December 8, 2014ijr.sagepub.comDownloaded from
Karaman and Frazzoli 849
Table 1. Summary of results. Time and space complexity are expressed as a function of the number of samples n,forafixed
environment.
Probabilistic Asymptotic Monotone Time complexity Space
Algorithm completeness optimality convergence Processing Query complexity
Existing PRM Yes no Yes O( n log n) O( n log n) O( n)
algorithms sPRM Yes Yes Yes O( n
2
) O( n
2
) O( n
2
)
k-sPRM Conditional No No O( n log n) O( n log n) O( n)
RRT Yes No Yes O( n log n) O( n) O( n)
Proposed PRM
∗
Yes Yes No O( n log n) O( n log n) O( n log n)
algorithms k-PRM
∗
RRG
Ye s Ye s Ye s O( n log n) O( n log n) O( n log n)
k-RRG
RRT
∗
Ye s Ye s Ye s O( n log n) O( n) O( n)
k-RRT
∗
Finally, the results presented in this article, and the tech-
niques used in the analysis of the algorithms, hinge on
novel connections established between sampling-based path
planning algorithms in robotics and the theory of random
geometric graphs, which may be of independent interest.
A preliminary version of this article has appeared in
Karaman and Frazzoli (2010b). Since then a variety of new
algorithms based on the ideas behind PRM
∗
, RRG, and
RRT
∗
have been proposed in the literature. For instance, a
probabilistically complete and probabilistically sound algo-
rithm for solving a class of differential games has appeared
in Karaman and Frazzoli (2010a). Algorithms based on the
RRG were used to solve belief-space planning problems in
Bry and Roy (2011). The RRT
∗
algorithm was used for any-
time motion planning in Karaman et al. (2011), where it
was also demonstrated experimentally on a full-size robotic
fork truck. In Alterovitz et al. (2011), the analysis given
in Karaman and Frazzoli (2010b) was used to guarantee
computational efficiency and asymptotic optimality of a
new algorithm that can trade off between exploration and
optimality during planning.
A software library implementing the new algorithms
introduced in this paper has been released as open-source
software by the authors, and is currently available at
http://ares.lids.mit.edu/software/
1.4. Paper organization
This paper is organized as follows. Section 2 lays the
ground in terms of notation and problem formulation. Sec-
tion 3 is devoted to the discussion of the algorithms that
are considered in the paper: first, the main paradigms for
sampling-based motion planning algorithms available in
the literature are presented, together with their main vari-
ants. Then, the new proposed algorithms are presented and
motivated. In Section 4 the properties of these algorithms
are rigorously analyzed, formally establishing their proba-
bilistic completeness and asymptotically optimality (or lack
thereof), as well as their computational complexity as a
function of the number of samples and of the number of
obstacles in the environment. Experimental results are pre-
sented in Section 5, to illustrate and validate the theoretical
findings. Finally, Section 6 contains conclusions and per-
spectives for future work. In order not to excessively disrupt
the flow of the presentation, a summary of notation used
throughout the paper, as well as lengthy proofs of important
results, are presented in the appendices.
2. Preliminary material
This section contains some preliminary material that will
be necessary for the discussion in the remainder of the
paper. Namely, the problems of feasible and optimal
motion planning are introduced, and some important results
from the theory of random geometric graphs are summa-
rized. The notation used in the paper is summarized in
Appendix A.
2.1. Problem formulation
In this section, the feasible and optimal path planning
problems are formalized.
Let X =(0, 1)
d
be the configuration space, where d ∈
N, d ≥ 2. Let X
obs
be the obstacle region, such that X \
X
obs
is an open set, and denote the obstacle-free space as
X
free
= cl(X \ X
obs
), where cl(·) denotes the closure of a
set. The initial condition x
init
is an element of X
free
, and the
goal region X
goal
is an open subset of X
free
. A path planning
problem is defined by a triplet (X
free
, x
init
, X
goal
). Let σ :
[0, 1] → R
d
;thetotal variation of σ is defined as
TV(σ ) = sup
{
n∈N,0=τ
0
<τ
1
<···<τ
n
=s
}
n
i=1
|σ (τ
i
) −σ (τ
i−1
) |.
A function σ with TV(σ ) < ∞ is said to have bounded
variation.
Definition 1 (Path). A function σ : [0, 1] → R
d
of bounded
variation is called a
at UCSF LIBRARY & CKM on December 8, 2014ijr.sagepub.comDownloaded from
评论0