Asymptotically-Optimal Motion Planning using Lower Bounds on Cost
Oren Salzman and Dan Halperin∗
Abstract— Many path-finding algorithms on graphs such
as A* are sped up by using a heuristic function that gives
lower bounds on the cost to reach the goal. Aiming to
apply similar techniques to speed up sampling-based motion-
planning algorithms, we use effective lower bounds on the cost
between configurations to tightly estimate the cost-to-go. We
then use these estimates in an anytime asymptotically-optimal
algorithm which we call Motion Planning using Lower Bounds
(MPLB). MPLB is based on the Fast Marching Trees (FMT*)
algorithm [1] recently presented by Janson and Pavone. An
advantage of our approach is that in many cases (especially
as the number of samples grows) the weight of collision
detection in the computation is almost negligible with respect
to nearest-neighbor calls. We prove that MPLB performs no
more collision-detection calls than an anytime version of FMT*.
Additionally, we demonstrate in simulations that for certain
scenarios, the algorithmic tools presented here enable efficiently
producing low-cost paths while spending only a small fraction
of the running time on collision detection.
I. INTRODUCTION
Motion-planning algorithms aim to find a collision-free
path for a robot moving amidst obstacles. The most prevalent
approach in practice is to use sampling-based techniques [2].
These algorithms sample points in the robot’s configuration-
space (C-space) and connect close-by configurations to con-
struct a graph called a roadmap. Often, a low-cost path is
desired, where cost can be measured in terms of, for example,
path length or energy consumption along the path.
Sampling-based algorithms rely on two central primitive
operations: Collision Detection (CD) and Nearest Neighbors
(NN) search. CD determines whether a configuration is
collision-free or not and is often used to assess if a path
connecting close-by configurations is collision-free. This
latter operation is referred to as Local Planning (LP). An
NN data structure preprocesses a set of points to efficiently
answer queries such as “which are the points within ra-
dius r of a given query point?” In practice, the cost of
CD, primarily due to LP calls, often dominates the running
time of sampling-based algorithms, and is typically regarded
as the computational bottleneck for such algorithms. For a
summary of the computational complexity of NN, CD and
LP in sampling-based motion-planning algorithms see [3].
In their influential work, Karaman and Frazzoli [4] ana-
lyzed existing sampling-based algorithms (namely PRM [5]
and RRT [6]) and introduced the notion of asymptotic
optimality (AO); an algorithm is said to be AO if the cost
∗Blavatnik School of Computer Science, Tel-Aviv University, Israel
This work has been supported in part by the Israel Science Foundation
(grant no. 1102/11), by the German-Israeli Foundation (grant no. 1150-
82.6/2011), and by the Hermann Minkowski–Minerva Center for Geometry
at Tel Aviv University.
of the solution produced by it converges to the cost of the
optimal solution if the algorithm is run for sufficiently long
time. They proposed AO variants of PRM and RRT called
PRM* and RRT*, respectively. However, the AO of PRM*
and RRT* comes at the cost of increased running time and
memory consumption when compared to their non-optimal
counterparts. To reduce this cost, several improvements were
proposed which modify the sampling scheme [7], [8], the
CD [3], or relax the optimality to asymptotic near-optimality
(ANO) [9], [10], [11]. An algorithm is said to be ANO
if, given an approximation factor ε, the cost of a solution
returned by the algorithm is guaranteed to converge to within
a factor of 1 + ε of the cost of the optimal solution.
Following the introduction of PRM* and RRT*, other
AO algorithms have been suggested. RRT# [12], extends
its roadmap in a similar fashion to RRT*. However, in
contrast to RRT* which only performs local rewiring, RRT#
efficiently propagates changes to all the relevant parts of the
roadmap. Another AO algorithm, proposed by Janson and
Pavone, is the Fast Marching Trees (FMT*) [1] algorithm.
FMT*, reviewed in detail in Section II, was shown to
converge to an optimal solution faster than PRM* or RRT*.
Contribution and paper organization We show how by
looking at the roadmap induced by a set of samples, we
can compute effective lower bounds on the cost-to-go of
nodes. This is done without performing expensive LP calls
and allows to efficiently guide the search performed by the
algorithm. We call our scheme Motion Planning using Lower
Bounds or MPLB for short.
An interesting and useful implication of our approach is
that the weight of CD and LP becomes negligible when
compared to that of the NN calls1. Bialkowski et al. [3]
introduced a technique which replaces CD calls by NN
calls. Their scheme relies on additional data produced by
the CD algorithm used, namely, a bound on the clearance of
a configuration (if it is collision free) or on its penetration
depth (if it is not collision free). Alas, such a bound is not
trivial to compute for some prevalent C-spaces. In contrast,
our algorithmic framework can be paired with existing off-
the-shelf NN, CD and LP procedures. Thus, our results,
which are more general (as they are applicable to general
C-spaces), strengthen the conjecture of Bialkowski et al. [3]
that NN computation and not CD may be the bottleneck of
sampling-based motion-planning algorithms.
This work continues and expands our recent work [11]
1Throughout this paper, when we say an NN call we mean a call to find
all the nodes within radius r(n) of a given node. r(n) is a radius depending
on the number of samples used and will be formally defined in Section II.
arXiv:1403.7714v2  [cs.RO]  30 Sep 2014
where we relaxed the AO of RRT* to ANO using lower
bounds. The novel component here is that multiple nodes
are processed simultaneously while in RRT* the nodes are
processed one at a time. This allows to efficiently compute
for all nodes an estimation of the cost-to-go which in turn
can be used to speed up motion-planning algorithms.
Our framework is demonstrated for the case where dis-
tance is the cost function via the FMT* algorithm which
is reviewed in Section II. As we wish to work in an
anytime setting, we introduce in Section III a straightforward
adaptation of FMT* for anytime planning which we call
aFMT. We then proceed to present MPLB in Section IV.
We analyze aFMT* and MPLB with respect to the amount
of calls to the NN and LP procedures in Section V and
report on experimental results in Section VI. Specifically, we
demonstrate in simulations that for certain scenarios, MPLB
produces lower-cost paths faster (by a factor of between two
and three) than aFMT*. We conclude with a discussion and
suggestions for future work in Section VII.
II. TERMINOLOGY AND ALGORITHMIC BACKGROUND
We begin by formally stating the motion-planning problem
and introducing several procedures used by the algorithms
we present. We continue by reviewing the FMT* algorithm.
A. Problem definition and terminology
Let X, Xfree denote the Euclidean2 C-space and free
space, respectively, and d the dimension of the C-space. Let
(Xfree, xinit, Xgoal) be the motion-planning problem where:
xinit ∈Xfree is the initial free configuration of the robot
and Xgoal
⊆
Xfree is the goal region. We will make
use of the following procedures: sample free(n), a
procedure returning n random configurations from Xfree;
nearest neighbors(x, V, r) is a procedure that returns
all neighbors of x with distance smaller than r within the
set V ; collision free(x, y) tests whether the straight-
line segment connecting x and y is contained in Xfree;
cost(x, y) returns the cost of the straight-line path con-
necting x and y, namely, in our case, the distance. We
consider weighted graphs G = (V, E), where the weight
of an edge (x, y)
∈
E is cost(x, y). Given such a
graph G, we denote by costG(x, y) the cost of the weighted
shortest path from x to y. Let cost-to-comeG(x) be
costG(xinit, x) and cost-to-goG(x) be the minimal
costG(x, xgoal) for xgoal ∈Xgoal. Namely for every node x,
cost-to-comeG(x) is the minimal cost to reach x from
xinit and cost-to-goG(x) is the minimal cost to reach
Xgoal from x. Additionally, let BG(xinit, r), BG(Xgoal, r)
be the set of all nodes whose cost-to-come (respectively,
cost-to-go) value on G is smaller than r. Finally, we denote
by Dijkstra(G, x, c) an implementation of Dijkstra’s al-
gorithm3 running on the graph G from x until a maximal
2 Although we describe the algorithm for Euclidean spaces, by standard
techniques (see, e.g. [2, Section 3.5, Section 7.1.2] or [13]) the algorithm
can be applied to non-Euclidean spaces such as SE3. However, the AO proof
of FMT*, presented in [1], is shown only for Euclidean spaces.
3Any other algorithm that computes the shortest path from a single source
to all nodes in a graph may be used.
Algorithm 1 FMT* (xinit, Xgoal, n)
1: V ←{xinit} ∪sample free(n); E ←∅; T ←(V, E)
2: PATH ←search (T , Xgoal, 0, ∞)
// See Alg. 2
3: return PATH
cost of c has been reached. The algorithm’s implementation
updates the cost to reach each node from x and outputs the
set of nodes traversed.
Given a set of samples V , and a radius r, we denote by
G(V, r) the disk graph,4 which is the graph whose set of
vertices is V and two vertices x, y ∈V are connected by an
edge if the distance between x and y is less than r.
B. Fast Marching Trees (FMT*)
FMT*, outlined in Alg. 1, performs a “lazy” dynamic
programming recursion on a set of sampled configurations
to grow a tree rooted at xinit [14]. The algorithm samples n
collision-free nodes V (line 1). It searches for a path to Xgoal
by building a minimum-cost spanning tree growing in cost-
to-come space (line 2 and detailed in Alg. 2). As we explain
in Section IV, the algorithm may benefit from using a heuris-
tic function estimating the cost-to-go of a node and a bound
on the maximal length of the path that should be found. As
these are not part of the original formulation of FMT*, we
describe the search procedure of FMT* using a cost-to-go
estimation of zero for each node and an unbounded maximal
path length (marked in red in Alg. 1 and 2). This will allow
us to use the same pseudo-code of Alg. 2 to explain the
MPLB algorithm in Section IV.
The search-tree is built by maintaining two sets of nodes
H, W such that H is the set of nodes added to the tree
that may be expanded and W is the set of nodes that have
not yet been added to the tree (Alg. 2, line 1). It then
computes for each node the set of nearest neighbors5 of
radius r(n) (line 3). The algorithm repeats the following
process: the node z with the lowest cost-to-come value is
chosen from H (line 4 and 16). For each neighbor x of z that
is not already in H, the algorithm finds its neighbor y ∈H
such that the cost-to-come of y added to the distance between
y and x is minimal (lines 7-9). If the local path between y and
x is free, x is added to H with y as its parent (lines 10-12).
At the end of each iteration z is removed from H (line 13).
The algorithm runs until a solution is found or there are no
more nodes to process.
To ensure AO, the radius r(n) used by the algorithm is
r(n) = (1 + η) · 2
1
d
 1
d µ(Xfree)
ζd
 1
d log n
n
 1
d
,
(1)
where η > 0 is some small constant, µ(·) denotes the d-
dimensional Lebesgue measure and ζd is the volume of the
unit ball in the d-dimensional Euclidean space.
4The disk graph is sometimes referred to as the the neighborhood graph.
5The nearest-neighbor computation can be delayed and performed only
when a node is processed but we present the batched mode of computation
to simplify the exposition.
Algorithm 2 search (T , Xgoal, cost to go, cmax)
1: W ←V \ {xinit};
H ←{xinit}
2: for all v ∈V do
3:
Nv ←nearest neighbors(V \ {v}, v, r(n))
4: z ←xinit
5: while z /∈XGoal do
6:
Hnew ←∅;
Xnear ←W ∩Nz
7:
for x ∈Xnear do
8:
Ynear ←H ∩Nx
9:
ymin ←arg miny∈Ynear{costT (y) + dist(y, x)}
10:
if collision free(ymin, x) then
11:
T .parent(x) ←ymin
12:
Hnew ←Hnew ∪{x};
W ←W \ {x}
13:
H ←(H ∪Hnew) \ {z}
14:
if H = ∅then
15:
return FAILURE
16:
z ←arg miny∈H{costT (y) + cost to go(y)}
17:
if costT (z) + cost to go(z) ≥cmax then
18:
return FAILURE
19: return PATH
III. ANYTIME FMT* (AFMT*)
An algorithm is said to be anytime if it yields meaningful
results even after a short time and it improves the quality
of the solution as more computation time is available. We
outline a straightforward enhancement to FMT* to make it
anytime. As noted in previous work (see, e.g., [15]) one can
turn a batch algorithm into an anytime one by the following
general approach: choose an initial small number of samples
n = n0 and apply the algorithm. As long as time permits,
double n and repeat the process. The total running time is
less than twice that of the running time for the largest n.
Note that as FMT* is AO, aFMT* is also AO.
We can further speed up this method by reusing both
existing samples and connections from previous iterations.
Due to lack of space, we omit these details and refer the
interested reader to the extended version of our paper [16].
IV. ALGORITHMIC FRAMEWORK
We are now ready to present our approach to exploiting
lower bounds on cost in order to speed up sampling-based
motion-planning algorithms.
Given a random infinite sequence of collision-free samples
S = s1, s2 . . . denote by Vi(S) the set of the first 2i
elements of S. Let Gi(S) = G(Vi(S), r(|Vi(S)|)) and let
Hi(S) ⊆Gi(S) be the subgraph containing collision-free
edges only (here r(n) is the radius defined in Eq. 1). For
brevity, we omit S when referring to Vi(S), Gi(S) and
Hi(S). Moreover, when we compare our algorithm to the
aFMT* algorithm, we do so for runs on the same random
infinite sequence S. Clearly, for any two nodes x, y ∈Vi,
costGi(x, y) ≤costHi(x, y). Thus for any node x ∈Vi,
cost-to-goGi(x) ≤cost-to-goHi(x). Namely, the
cost-to-go computed using the disk graph Gi is a lower bound
on the cost-to-go that may be obtained using Hi. We call this
the lower bound property. For an illustration, see Fig. 1.
Xgoal
xinit
(a)
Xgoal
xinit
(b)
Fig. 1: This figure demonstrates that the part of the tree expanded
when searching in cost-to-come space (shaded blue region, Fig. (a))
is larger than the one expanded when searching in cost-to-come+cost-
to-go space (shaded green region, Fig. (b)). Obstacles in the C-space
are depicted in red, start location and goal region are depicted by a
purple circle and a turquoise region, respectively. Edges of the disk
graph Gi that are contained and not contained in Hi are depicted in
black and dashed red, respectively. The figure is best viewed in color.
We present Motion Planning using Lower Bounds, or
MPLB (outlined in Alg. 3). Similar to aFMT*, the algorithm
runs in iterations and at the i’th iteration, uses Vi as its
set of samples. Unlike aFMT*, each iteration consists of a
preprocessing phase (line 4) of computing a lower bound on
the cost-to-go values and a searching phase (line 5) where
a modified version of FMT* is used.
Let ci(ALG) denote the cost of the solution obtained
by an algorithm ALG using Vi as the set of samples (set
c0(ALG) ←∞). We now show that only a subset of the
nodes sampled in each iteration need to be considered. We
then proceed to describe the two phases of MPLB.
A. Promising nodes
We use the lower bound property to consider only a subset
of Vi that will be used in the i’th iteration. Intuitively, we
only wish to consider nodes that may produce a solution that
is better than the solution obtained in previous iterations. This
leads us to the definition of promising nodes:
Definition 1: A node x∈Vi is promising (at iteration i) if
cost-to-comeHi(x)+cost-to-goHi(x)<ci−1(MPLB).
In the preprocessing phase, MPLB will traverse Gi (and
not Hi) to collect a set of nodes that contains all promising
Algorithm 3 MPLB (xinit, Xgoal, n0)
1: V ←{xinit};
n ←n0;
cprev ←∞
2: while time permits() do
3:
V ←V ∪sample free(n);
E ←∅;
T ←(V, E)
4:
estimate cost to go(V, xinit, Xgoal, cprev)
5:
PATH ←search (T , Xgoal, cost to go, cprev)
6:
n ←2n;
cprev = cost(PATH)
7: return PATH
Algorithm 4 estimate cost to go (V, xinit, Xgoal, c)
1: Vpreproc ←Dijkstra(G(V, r(|V |)), xinit, c
2)
2: Vpreproc ←Vpreproc ∪Dijkstra(G(V, r(|V |)), Xgoal, c
2)
3: for x ∈V \ Vpreproc do
4:
cost to go(x) ←∞
5: Dijkstra(G(Vpreproc, r(|Vpreproc|)), Xgoal, c)
nodes (and possibly other nodes), compute a lower bound on
their cost-to-go and use this set in the searching phase.
B. Preprocessing phase: Estimating the cost-to-go
Recall that in the preprocessing phase, outlined in Alg. 4,
we wish to compute a lower bound on the cost-to-go for
(a subset of) nodes x ∈Vi. Specifically, the only nodes we
wish to consider are promising nodes. This is done by col-
lecting the set of nodes Vpreproc = BGi

xinit, ci−1(MPLB)
2

∪
BGi

Xgoal, ci−1(MPLB)
2

. Namely, by performing one traver-
sal from xinit (line 1) and one traversal from Xgoal (line 2),
all nodes such that cost-to-comeGi ≤
ci−1(MPLB)
2
or
cost-to-goGi ≤ci−1(MPLB)
2
are found. Clearly, any node
not in either set is not promising (lines 3-4).
After collecting all nodes in Vpreproc, MPLB computes the
distance of every such node from Xgoal (line 5). This is
done by running a shortest paths algorithm on the graph Gi
restricted to the nodes in Vpreproc. This distance is stored for
each node and will be used as a lower bound on the cost-to-
go. We note that this preprocessing phase only uses NN calls
and does not use any CD calls (as there are no LP calls).
C. Searching phase: Using cost-to-go estimations
The lower bounds computed in the preprocessing phase
allow for two algorithmic enhancements to the searching
phase when compared to aFMT*: (i) incorporating the cost-
to-go estimation in the ordering scheme of the nodes and
(ii) discarding nodes that are found to be not promising.
Node ordering: Recall that in aFMT*, H is the set of
nodes added to the tree that may be expanded and that
these nodes are ordered according to their cost-to-come value
(Alg. 2, line 16). Instead, we suggest using the cost-to-
come added to the cost-to-go estimation to order the nodes
in H. This follows exactly the formulation of A* [17]
which performs a Dijkstra-like search on a set of nodes.
The nodes that were encountered but not processed yet (H
in our setting) are ordered according to a cost function
f() = g() + h(). Here, g(x) is the (computed) cost-to-
come value of x (cost-to-comeHi(x) in our case) and
h is a lower bound on the cost-to-go of x to the goal
(cost-to-goGi(x) in our case). aFMT* essentially uses
the trivial heuristic h = 0. Instead, we suggest to use a much
sharper bound to speed up the search towards the goal.
Discarding nodes: In the preprocessing stage MPLB com-
putes a set of nodes that may be promising, though for each
such node, the cost-to-come value was not computed. In the
searching phase, once a node is added to the tree, its cost-to-
come value will not change in the current iteration. Thus, ev-
ery node x added to the tree with cost-to-comeHi(x) +
cost-to-goGi(x) ≥ci−1(MPLB) is discarded as it cannot
be promising. This implies that MPLB will terminate an
iteration when it is evident that the previous iteration’s
solution cannot be improved (see Alg. 2, lines 17-18).
In Section VI we demonstrate through various simulations
that using lower bounds has a significant effect on the
running time of the algorithm in practice. Ordering the nodes
using a heuristic that tightly estimates the cost-to-go allows
MPLB to expand a smaller portion of the nodes Vi while
discarding nodes allows to focus the search only on nodes
that may potentially improve the existing solution.
V. COMPARATIVE ANALYSIS AND DISCUSSION
We compare aFMT* and MPLB with respect to the size of
the tree constructed in the searching phase and with respect
to the primitive procedures, namely NN and LP. This is done
by quantifying the number of NN and LP calls performed
by both algorithms and allows us to discuss the fundamental
differences between the two algorithms.
Let #NN,i(ALG), #LP,i(ALG) denote the number of NN
and LP calls performed by an algorithm ALG in iteration i,
respectively for a fixed sequence of samples S. Recall that
when comparing the two algorithms, it is done for the same
sequence S.
A. Search-tree size
Let Vi(ALG) ⊆Vi denote the set of nodes in the tree in
the i’th iteration of an algorithm ALG.
Lemma 1: At every iteration, the set of nodes traversed in
MPLB’s searching phase is not larger than that of aFMT*.
Proof:
Every node x in the tree of aFMT* has cost-
to-come not larger than ci(aFMT∗). Thus, the size of the
search-tree of aFMT* is: |Vi(aFMT*)| =
|{x ∈Vi | cost-to-comeHi(x) ≤ci(aFMT∗)}|.
Similar to aFMT*, each node x traversed by MPLB
in
the
searching
phase
has
cost-to-comeHi(x) +
cost-to-goGi(x) ≤ci(aFMT∗). Additionally, due to
node discarding (see Section IV), cost-to-comeHi(x) +
cost-to-goGi(x) ≤ci−1(MPLB). Thus, the size of the
search-tree of MPLB is: |Vi(MPLB)| =
|{x ∈Vi | cost-to-comeHi(x) + cost-to-goGi(x)
≤min{ci−1(MPLB), ci(aFMT∗)}}|.
Namely, |Vi(MPLB)| ≤|Vi(aFMT*)|.
B. Nearest neighbor calls (NN)
To quantify the number of NN queries performed by each
algorithm we note the following observations (explained in
detail in the extended version of this paper [16] due to lack
of space):
Observation 1: The
number
of
NN
calls
performed
by aFMT* can be bounded from below as follows:
#NN,i(aFMT∗) ≥|Vi(aFMT*)|.
Observation 2: The number of NN calls performed by
MPLB is: #NN,i(MPLB) =

n
x ∈Vi|x ∈BGi

xinit, ci−1(MPLB)
2

∪BGi

Xgoal, ci−1(MPLB)
2
o .
Thus, MPLB may perform more NN queries than FMT*.
C. Local planning calls (LP)
The LP will be called whenever either algorithm (aFMT*
or MPLB) attempts to insert a node to the search-tree (line 10
in Alg. 2). Thus we can state the following lemma:
Lemma 2: If MPLB performs an LP call for the edge
(x, y) in the i’th iteration then aFMT* will perform an LP
call for the edge (x, y) as well.
Proof:
The LP procedure will be called for every
pair of nodes x, y in the search tree such that: (i) x, y are
neighbors in Gi (namely their distance is less than r(|Vi|)),
(ii)
cost-to-comeHi(x)
<
cost-to-comeHi(y)
(namely x is inserted to the tree before y), and (iii) the edge
(z, y) is not collision-free for all other neighbors z of y in
the tree that could potentially lead to smaller cost-to-come
values of y.
If MPLB performs an LP call for the edge (x, y) then
conditions (i),(ii) and (iii) hold for the samples x, y in
MPLB. To prove the lemma we show that they hold for
the samples x, y in aFMT*. Condition (i) holds trivially
as it is a property of the samples. Note that the cost-to-
come of any node z computed by both algorithms equals
to cost-to-comeHi(z). Using this observation and that
Vpreproc ⊆Vi (namely the nodes used by MPLB is a subset
those used by aFMT*), conditions (ii) and (iii) hold as well.
D. Discussion
From the above analysis we conclude that MPLB will
perform no more LP calls than aFMT*. It may perform
more NN calls than aFMT*. As we demonstrate empirically
in the Evaluation section, the number of NN calls that
MPLB performs may actually be smaller than that of aFMT*.
Moreover, as the number of iterations increases, MPLB
performs only a tiny fraction of the number of LP calls
performed by aFMT*.
VI. EVALUATION
We present simulations evaluating the performance of
MPLB as an anytime algorithm on 2, 3 and 6 dimensional
C-spaces. All experiments were run on a 2.8GHz Intel
Core i7 processor with 8GB of memory. The MPLB and
aFMT* implementations are based on the FMT* imple-
mentation provided by Pavone’s research group using the
Open Motion Planning Library (OMPL 0.10.2) [18]. Each
result is averaged over one hundred different runs. Scenarios
and additional material are available at http://acg.cs.
tau.ac.il/projects/MPLB.
The AO proof of FMT* (and thus of aFMT* and MPLB)
relies on the fact that the C-space is Euclidean. Thus, we start
by studying the motion of robots translating in the plane and
in space (Fig. 2a and 2b). Next, we continue to examine
the behavior of the algorithms in SE(3) (Fig. 2c). Here the
radius provided for FMT* (Eq. 1) is irrelevant due to the
differences in the rotational and translational components of
the C-space. Hence, for both aFMT* and MPLB, we chose
to connect each node to its k NN, where k(n) = 9 log n:
Karaman and Frazzoli [4] proposed a variant of RRG where
each node is connected to its kRRG NN for kRRG(n) ≥
2e log n. Although this variant was analyzed for Euclidean
spaces only, applying it to non-Euclidean spaces works well
in practice (see, e.g. [11]).
Fig. 4: Percentage of time spent for each of the main components in
each iteration for both algorithms for the Grids Scenario. Each iteration
is represented by the number of samples used. The left (right) bars
of each iteration represent the result of aFMT* (MPLB, respectively).
Note that the time of each iteration for each algorithm is different.
A. Fast convergence to high-quality solutions
We start by comparing the cost of a solution obtained
by aFMT* and MPLB as a function of time (Fig 3). In
all scenarios MPLB typically finds a solution of given cost
between two to three times faster than aFMT*. In the
Corridors scenario (Fig. 2a) the convergence rate can be
sped up by using an approximation factor (see suggestion for
future work in Section VII). Interestingly, as we will show,
the speed-up achieved by MPLB is done while spending a
smaller proportion of the time on LP compared to aFMT*.
B. Nearest Neighbors and Local Planning calls
We profiled aFMT* and MPLB and collected the total
time spent on CD for point sampling, LP for edges, NN
calls and cost computations. Results for the Grids scenario
are presented in Fig. 4 (similar behavior was observed for
the other scenarios as well). Clearly, CD computation time
(due to sampling, not LP) is negligible for both algorithms
and cost calculation plays a larger (but still small) role for
MPLB. CD calls due to LP calls are the main bottleneck for
aFMT* (starting at around 65% and gradually decreasing to
45%). For MPLB they start as a main time consumer but as
samples are added their percentage of the overall iteration
time becomes quite small (around 2% for the last iteration).
NN calls play an almost complementary role to the LP and
for the last iteration take 40% of the total running time for
the MPLB algorithm while taking less than 20% for aFMT*.
n
the ratio
the ratio
#NN(MPLB)
#NN(aFMT∗)
#LP(MPLB)
#LP(aFMT∗)
1.6K
0.71
0.38
3.2K
0.53
0.31
6.4K
0.68
0.33
12.8K
0.68
0.19
25.6K
0.69
0.20
51.2K
0.99
0.05
The
table
to
the
right reports on the ra-
tio of NN and LP calls
performed by MPLB
and aFMT* for the
Grids
scenario.
The
number of NN calls
performed by MPLB
is
lower
than
those
performed by aFMT*. As expected, MPLB performs sig-
nificantly less LP calls than aFMT*.
VII. CONCLUSION AND OUTLOOK
In this work we show that by using effective lower bounds
and with no compromise on the cost of paths produced by
the algorithm, the weight of CD (via LP calls) may become
almost negligible with respect to NN calls. This follows the
xinit
xgoal
(a) Corridors
(b) Grids
(c) Home
Fig. 2: Scenarios used for the evaluation. (a) Two dimensional setting for a point robot. A low-cost path is easy to find yet in order to find a
high-quality path, the robot needs to pass through two narrow passages. (b) Three-dimensional C-space for a translating robot in space. To find
the shortest path the robot needs to pass through a three-dimensional grid. (c) Six-dimensional C-space for an L-shaped robot translating and
rotating in space. Finding a path is relatively easy yet much time is needed to converge to the optimal path. Start and target configurations for
(b) and (c) are depicted by green and red robots, respectively, The Home scenario is provided by the OMPL [18] distribution.
(a) Corridors
(b) Grids
(c) Home
Fig. 3: Average cost vs. time. Cost values are normalized such that a cost of one represents the cost of an optimal path. Low and high error bars
denote the twentieth and eightieth percentile, respectively.
ideas presented by Bialkowski et al. [3] but uses different,
more general, methods. Looking into NN computation, one
can notice that AO algorithms such as sPRM* [4], FMT* and
MPLB rely on a specific type of NN computation: given a
set P of n points, either compute for each point all its k
nearest neighbors, or all neighbors within distance r from
the point. In both cases, P is known in advance and k (or r)
are parameters that do not change throughout the algorithm
or throughout a single iteration of the algorithm.
This calls for using application-specific NN algorithms and
not general purpose ones. For example, the recent work on
randomly shifted grids by Aiger et al. [19] may be used.
Indeed, we show that using this data structure allows to
significantly speed up motion-planning algorithms [20].
A different possibility to enhance MPLB is to relax AO to
ANO: Asymptotically-optimal motion-planning algorithms
such as MPLB often, from a certain stage of their execution,
invest huge computational resources at only slightly improv-
ing the cost of the current best existing solution. Similar to
the approach presented by the authors in a previous work [11]
one can construct a variant such that given an approximation
factor ε, the cost of the solution obtained is within a factor
of 1 + ε from the solution that MPLB would obtain for the
same set of samples. We expand on this idea in the extended
version of our paper [16]. Preliminary results, presented in
Fig. 3a show the potential benefit of this approach.
VIII. ACKNOWLEDGEMENTS
We wish to thank Marco Pavone and his co-workers for
their advice and support regarding the FMT* algorithm.
REFERENCES
[1] L. Janson and M. Pavone, “Fast marching trees: a fast marching
sampling-based method for optimal motion planning in many dimen-
sions,” CoRR, vol. abs/1306.3532, 2013.
[2] H. Choset, K. M. Lynch, S. Hutchinson, G. Kantor, W. Burgard,
L. E. Kavraki, and S. Thrun, Principles of Robot Motion: Theory,
Algorithms, and Implementation.
MIT Press, June 2005.
[3] J. Bialkowski, S. Karaman, M. Otte, and E. Frazzoli, “Efficient
collision checking in sampling-based motion planning,” in WAFR,
2012, pp. 365–380.
[4] S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimal
motion planning,” I. J. Robotic Res., vol. 30, no. 7, pp. 846–894, 2011.
[5] L. E. Kavraki, P. Svestka, J.-C. Latombe, and M. H. Overmars, “Prob-
abilistic roadmaps for path planning in high dimensional configuration
spaces,” IEEE Trans. Robot., vol. 12, no. 4, pp. 566–580, 1996.
[6] J. J. Kuffner and S. M. LaValle, “RRT-Connect: An efficient approach
to single-query path planning,” in ICRA, 2000, pp. 995–1001.
[7] B. Akgun and M. Stilman, “Sampling heuristics for optimal motion
planning in high dimensions,” in IROS, 2011, pp. 2640–2645.
[8] J. D. Gammell, S. S. Srinivasa, and T. D. Barfoot, “Informed RRT*:
Optimal incremental path planning focused through an admissible
ellipsoidal heuristic,” in IROS, 2014, to appear.
[9] A. Dobson and K. E. Bekris, “Sparse roadmap spanners for asymptot-
ically near-optimal motion planning,” I. J. Robotic Res., vol. 33, no. 1,
pp. 18–47, 2014.
[10] Z. Littlefield, Y. Li, and K. E. Bekris, “Efficient sampling-based mo-
tion planning with asymptotic near-optimality guarantees for systems
with dynamics,” in IROS, 2013, pp. 1779–1785.
[11] O. Salzman and D. Halperin, “Asymptotically near-optimal RRT for
fast, high-quality, motion planning,” in ICRA, 2014, pp. 4680–4685.
[12] O. Arslan and P. Tsiotras, “Use of relaxation methods in sampling-
based algorithms for optimal motion planning,” in ICRA, 2013, pp.
2421–2428.
[13] J. J. Kuffner, “Effective sampling and distance metrics for 3d rigid
body path planning,” in ICRA, 2004, pp. 3993–3998.
[14] L. Jaillet and J. M. Porta, “Asymptotically-optimal path planning on
manifolds,” in RSS, 2012.
[15] W. Wang, D. Balkcom, and A. Chakrabarti, “A fast streaming spanner
algorithm for incrementally constructing sparse roadmaps,” IROS, pp.
1257–1263, 2013.
[16] O. Salzman and D. Halperin, “Asymptotically-optimal motion plan-
ning using lower bounds on cost,” CoRR, vol. abs/1403.7714, 2014.
[17] J. Pearl, Heuristics: Intelligent Search Strategies for Computer Prob-
lem Solving.
Addison-Wesley, 1984.
[18] I. A. S¸ucan, M. Moll, and L. E. Kavraki, “The Open Motion Planning
Library,” IEEE Robot. Automat. Mag., vol. 19, no. 4, pp. 72–82, 2012.
[19] D. Aiger, H. Kaplan, and M. Sharir, “Reporting neighbors in high-
dimensional euclidean space,” SIAM J. Comput., vol. 43, no. 4, pp.
1363–1395, 2014.
[20] M. Kleinbort, O. Salzman, and D. Halperin, “Efficient high-quality
motion planning by fast all-pairs r-nearest-neighbors,” CoRR, vol.
abs/1409.8112, 2014.