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. I NTRODUCTION  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 calls 1 . 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]  1 Throughout 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. T ERMINOLOGY 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   ,   X free   denote the Euclidean 2   C-space and free space, respectively, and   d   the dimension of the C-space. Let  ( X free , x init ,   X goal )   be the motion-planning problem where:  x init   ∈ X free   is the initial free configuration of the robot and   X goal   ⊆   X free   is   the   goal   region.   We   will   make use   of   the   following   procedures:   sample free ( n ) ,   a procedure returning   n   random configurations from   X free ;  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   X free ;  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   cost G   ( x, y )   the cost of the weighted shortest path from   x   to   y . Let   cost-to-come G   ( x )   be  cost G   ( x init , x )   and   cost-to-go G   ( x )   be   the   minimal  cost G   ( x, x goal )   for   x goal   ∈ X goal . Namely for every node   x ,  cost-to-come G   ( x )   is the minimal cost to reach   x   from  x init   and   cost-to-go G   ( x )   is the minimal cost to reach  X goal   from   x .   Additionally,   let   B G   ( x init , r ) ,   B G   ( X goal , 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- gorithm 3   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.  3 Any other algorithm that computes the shortest path from a single source to all nodes in a graph may be used.  Algorithm 1   FMT*   ( x init ,   X goal , n )  1:   V   ← { x init } ∪   sample free ( n ) ;   E   ← ∅ ;   T ←   ( V, E )  2:   PATH   ←   search   ( T   ,   X goal ,   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   x init   [14]. The algorithm samples   n  collision-free nodes   V   (line 1). It searches for a path to   X goal  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 neighbors 5   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   (   μ ( X free )  ζ 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.  4 The disk graph is sometimes referred to as the the   neighborhood graph .  5 The 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   ,   X goal ,   cost to go , c max )  1:   W   ←   V   \ { x init } ;   H   ← { x init }  2:   for all   v   ∈   V   do  3:   N v   ←   nearest neighbors ( V   \ { v } , v, r ( n ))  4:   z   ←   x init  5:   while   z / ∈ X Goal   do  6:   H new   ← ∅ ;   X near   ←   W   ∩   N z  7:   for   x   ∈   X near   do  8:   Y near   ←   H   ∩   N x  9:   y min   ←   arg min y ∈ Y near   { cost T   ( y ) +   dist ( y, x ) }  10:   if   collision free ( y min , x )   then  11:   T   . parent ( x )   ←   y min  12:   H new   ←   H new   ∪ { x } ;   W   ←   W   \ { x }  13:   H   ←   ( H   ∪   H new )   \ { z }  14:   if   H   =   ∅   then  15:   return   FAILURE  16:   z   ←   arg min y ∈ H   { cost T   ( y ) +   cost to go ( y ) }  17:   if   cost T   ( z ) +   cost to go ( z )   ≥   c max   then  18:   return   FAILURE  19:   return   PATH III. A NYTIME   FMT* ( A FMT*) 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   =   n 0   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. A LGORITHMIC 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   =   s 1 , s 2   . . .   denote by   V i ( S )   the set of the first   2 i  elements of   S . Let   G i ( S ) =   G ( V i ( S ) , r ( | V i ( S ) | ))   and let  H i ( S )   ⊆ G i ( 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   V i ( S ) ,   G i ( S )   and  H i ( 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   ∈   V i ,  cost G i   ( x, y )   ≤   cost H i   ( x, y ) . Thus for any node   x   ∈   V i ,  cost-to-go G i   ( x )   ≤   cost-to-go H i   ( x ) .   Namely, the cost-to-go computed using the disk graph   G i   is a lower bound on the cost-to-go that may be obtained using   H i . We call this the   lower bound property . For an illustration, see Fig. 1. X goal  x init  (a) X goal  x init   (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   G i   that are contained and not contained in   H i   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   V i   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   c i ( ALG )   denote the cost of the solution obtained by an algorithm ALG using   V i   as the set of samples (set  c 0 ( 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   V i   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   ∈   V i   is   promising   (at iteration   i ) if  cost-to-come H i   ( x )+ cost-to-go H i   ( x )   < c i − 1 ( MPLB ) .  In the preprocessing phase, MPLB will traverse   G i   (and not   H i ) to collect a set of nodes that contains all promising  Algorithm 3   MPLB   ( x init ,   X goal , n 0 )  1:   V   ← { x init } ;   n   ←   n 0 ;   c prev   ← ∞  2:   while   time permits()   do  3:   V   ←   V   ∪ sample free ( n ) ;   E   ← ∅ ;   T ←   ( V, E )  4:   estimate cost to go ( V, x init ,   X goal , c prev   )  5:   PATH   ←   search   ( T   ,   X goal ,   cost to go , c prev   )  6:   n   ←   2 n ;   c prev   =   cost (PATH)  7:   return   PATH  Algorithm 4   estimate cost to go   ( V, x init ,   X goal , c )  1:   V preproc   ←   Dijkstra ( G ( V, r ( | V   | )) , x init ,   c  2   )  2:   V preproc   ←   V preproc   ∪   Dijkstra ( G ( V, r ( | V   | )) ,   X goal ,   c  2   )  3:   for   x   ∈   V   \   V preproc   do  4:   cost to go ( x )   ← ∞  5:   Dijkstra ( G ( V preproc , r ( | V preproc | )) ,   X goal , 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   ∈   V i . Specifically, the only nodes we wish to consider are   promising nodes . This is done by col- lecting the set of nodes   V preproc   =   B G i  (  x init ,   c i − 1   ( MPLB ) 2  )  ∪  B G i  (  X goal ,   c i − 1 ( MPLB ) 2  )  . Namely, by performing one traver- sal from   x init   (line 1) and one traversal from   X goal   (line 2), all nodes such that   cost-to-come G i   ≤   c i − 1   ( MPLB ) 2   or  cost-to-go G i   ≤   c i − 1 ( MPLB ) 2   are found. Clearly, any node  not   in either set is not promising (lines 3-4). After collecting all nodes in   V preproc , MPLB computes the distance of every such node from   X goal   (line 5). This is done by running a shortest paths algorithm on the graph   G i  restricted to the nodes in   V preproc . 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-come H i   ( x )   in our case) and  h   is a lower bound on the cost-to-go of   x   to the goal ( cost-to-go G i   ( 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-come H i   ( x ) +  cost-to-go G i   ( x )   ≥   c i − 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   V i   while discarding nodes allows to focus the search only on nodes that may potentially improve the existing solution. V. C OMPARATIVE ANALYSIS AND   D ISCUSSION  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   V i ( ALG )   ⊆   V i   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   c i ( aFMT ∗ ) . Thus, the size of the search-tree of aFMT* is:   | V i ( aFMT* ) |   =  |{ x   ∈   V i   |   cost-to-come H i   ( x )   ≤   c i ( aFMT ∗ ) }| .  Similar   to   aFMT*,   each   node   x   traversed   by   MPLB in   the   searching   phase   has   cost-to-come H i   ( x ) +  cost-to-go G i   ( x )   ≤   c i ( aFMT ∗ ) .   Additionally,   due   to node discarding (see Section IV),   cost-to-come H i   ( x ) +  cost-to-go G i   ( x )   ≤   c i − 1 ( MPLB ) . Thus, the size of the search-tree of MPLB is:   | V i ( MPLB ) |   =  |{ x   ∈   V i   |   cost-to-come H i   ( x ) +   cost-to-go G i   ( x )  ≤   min { c i − 1 ( MPLB ) , c i ( aFMT ∗ ) }}| .  Namely,   | V i ( MPLB ) | ≤ | V i ( 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 ∗ )   ≥ | V i ( aFMT* ) | .  Observation 2:   The number of NN calls performed by MPLB is:   # NN ,i ( MPLB ) = ∣ ∣ ∣ {  x   ∈   V i | x   ∈   B G i  (  x init ,   c i − 1 ( MPLB ) 2  )  ∪ B G i  (  X goal ,   c i − 1 ( MPLB ) 2  )}∣ ∣ ∣   .  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   G i   (namely their distance is less than   r ( | V i | ) ), (ii)   cost-to-come H i   ( x )   <   cost-to-come H i   ( 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-come H i   ( z ) . Using this observation and that  V preproc   ⊆   V i   (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. E VALUATION  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   k RRG   NN for   k RRG ( n )   ≥  2 e   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. C ONCLUSION 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
x init  x goal (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. A CKNOWLEDGEMENTS  We wish to thank Marco Pavone and his co-workers for their advice and support regarding the FMT* algorithm. R EFERENCES [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.