An Asymptotically-Optimal Sampling-Based Algorithmfor Bi-directional Motion Planning

Joseph A. Starek∗, Edward Schmerling†, Lucas Janson‡, Marco Pavone∗

Abstract— Bi-directional search is a widely used strategy toincrease the success and convergence rates of sampling-basedmotion planning algorithms. Yet, few results are available thatmerge bi-directional search and asymptotic-optimality togetherinto existing optimal planners, such as PRM∗, RRT∗, and FMT∗.The objective of this paper is to fill this gap. Specifically, thispaper presents a bi-directional, sampling-based, asymptotically-optimal algorithm named Bi-directional FMT∗ (BFMT∗) thatextends the Fast Marching Trees (FMT∗) algorithm to bi-directional search while preserving its key properties, chieflylazy search and asymptotic optimality through convergencein probability. BFMT∗ performs a two-source, lazy dynamicprogramming recursion over a set of randomly-drawn samples,correspondingly generating two search trees: one in cost-to-come space from the initial configuration and another in cost-to-go space from the goal configuration. Multiple strategiesare discussed and analyzed for alternating tree growth andinterconnecting tree paths. Numerical experiments illustrate theadvantages of BFMT∗ over its unidirectional counterpart, aswell as a number of other state-of-the-art planners.

I. INTRODUCTION

Motion planning is a problem central to the field of roboticsinvolving the computation of a path from an initial configura-tion to a goal configuration that does not collide with nearbyobstacles and possibly optimizes an objective function. Theproblem has a long and rich history, and many algorithmictools have been developed; we refer the interested reader to [1]and references therein. Arguably, sampling-based algorithmsare among the most pervasive, widespread planners availablein robotics, including the probabilistic roadmap algorithm(PRM) [2], the expansive space trees algorithm (EST) [3],[4], and the rapidly exploring random trees algorithm (RRT)[5]. Since their development, efforts to improve the “quality”of paths led to asymptotically-optimal variants of RRT andPRM, named RRT∗ and PRM∗, respectively, whereby thecost of the returned solution converges almost surely to theoptimum as the number of samples approaches infinity [6].Recently, a conceptually different asymptotically-optimal,sampling-based motion planning algorithm, called the FastMarching Trees algorithm (FMT∗), has been presented in [7],[8]. Numerical experiments suggested that FMT∗ convergesto an optimal solution faster than PRM∗ or RRT∗, especiallyin high-dimensional configuration spaces and in scenarioswhere collision-checking is expensive.It is a well-known fact that bi-directional search can dramat-ically increase the convergence rate of planning algorithms,

∗Dept. of Aeronautics and Astronautics, Stanford University, Stanford,CA 94305, {jstarek, pavone}@stanford.edu.†Inst. for Computational & Mathematical Engineering, Stanford Univer-

sity, Stanford, CA 94305, schmrlng@stanford.edu.‡Dept. of Statistics, Stanford University, Stanford, CA 94305,

ljanson@stanford.edu.This work was supported in part by NASA ECF Grant #NNX12AQ43G.

prompting some authors [9] to advocate its use for acceler-ating essentially any motion planning query. This was firstrigorously studied in [10] and later investigated, for example,in [11], [12]. Collectively, the algorithms presented in [9]–[12] belong to the family of non-sampling-based approachesand are more or less closely related to a bi-directionalimplementation of the Dijkstra Method. More recently, andnot surprisingly in light of these performance gains, bi-directional search has been merged with the sampling-basedapproach, with RRT-Connect and SBL representing the mostnotable examples [13], [14].Though such bi-directional versions of RRT and PRM areprobabilistically complete, they do not enjoy optimalityguarantees. The next logical step in the quest for fastplanning algorithms is the design of bi-directional, sampling-based, asymptotically-optimal algorithms. To the best of ourknowledge, the only available results in this context are [15]and the unpublished work [16], both discussing bi-directionalimplementations of RRT∗. Both works, however, do notprovide a rigorous analysis of optimality. Accordingly, theobjective of this paper is to propose and rigorously analyzesuch an algorithm.Statement of Contributions: This paper proposes the Bi-directional Fast Marching Trees (BFMT∗) algorithm1 as atree-based, asymptotically-optimal bi-directional sampling-based planner. BFMT∗ extends FMT∗ to bi-directional searchand essentially performs a “lazy,” bi-directional dynamicprogramming recursion over a set of probabilistically-drawnsamples in the free configuration space. The contribution ofthis paper is threefold. First, we present the BFMT∗ algorithm,for which we propose two strategies for tree growth and twofor interconnecting tree paths (yielding four variants total).Secondly, we rigorously prove the asymptotic optimality ofBFMT∗ (under the notion of convergence in probability)and characterize its convergence rate. Finally, we performnumerical experiments across a number of planning spacesthat suggest BFMT∗ converges to an optimal solution atleast as fast as FMT∗, PRM∗, and RRT∗, and sometimessignificantly faster.Organization: This paper is structured as follows. Section IIdefines the path planning problem. Section III then presentsBFMT∗ and discusses alternatives for tree exploration andinterconnection. We then follow in Section IV with the proofof asymptotic optimality of BFMT∗, a characterization ofits convergence rate, and a discussion about extensions togeneral cost functions and non-uniform sampling strategies.Section V presents results from numerical experiments, and,finally, Section VI concludes with directions for future work.

1The asterisk ∗, pronounced “star”, is intended to represent asymptoticoptimality much like for the RRT∗ and PRM∗ algorithms.

II. PROBLEM DEFINITION

Let X be a d-dimensional configuration space, and letXobs be the obstacle region, such that X \ Xobs is anopen set (we consider ∂X ⊂ Xobs). Denote the obstacle-free space as Xfree = cl(X \ Xobs), where cl(·) denotesthe closure of a set. A path planning problem, denotedby a triplet (Xfree,xinit,xgoal), seeks to maneuver froman initial configuration xinit to a goal configuration xgoal

through Xfree. Let a continuous function of bounded variationσ : [0, 1]→ X , called a path, be collision-free if σ(τ) ∈ Xfree

for all τ ∈ [0, 1]. A path is called a feasible solution to theplanning problem (Xfree,xinit,xgoal) if it is collision-free,σ(0) = xinit, and σ(1) = xgoal.Let Σ be the set of all paths. A cost function for the planningproblem (Xfree,xinit,xgoal) is a function c : Σ→ R≥0 fromthe set of paths to the nonnegative real numbers; in this paper,we consider as a cost function c(σ) the arc length of σ withrespect to the Euclidean metric in X (the extension to generalcost functions will be briefly discussed in Section IV-C).

Optimal path planning problem: Given a pathplanning problem (Xfree,xinit,xgoal) and an arclength function c : Σ→ R≥0, find a feasible pathσ∗ such that c(σ∗) = min{c(σ) | σ is feasible}. Ifno such path exists, report failure.

Finally, we introduce some definitions concerning the clear-ance of a path, i.e., its “distance” from Xobs [8]. For a givenδ > 0, the δ-interior of Xfree is defined as the set of all pointsthat are at least a distance δ away from any point in Xobs. Acollision-free path σ is said to have strong δ-clearance if itlies entirely inside the δ-interior of Xfree. A path planningproblem with optimal path cost c∗ is called δ-robustly feasibleif there exists a strictly positive sequence δn → 0, withδn ≤ δ ∀n ∈ N, and a sequence {σn}∞n=1 of feasible pathssuch that limn→∞ c(σn) = c∗ and for all n ∈ N, σn hasstrong δn-clearance, σn(1) = xgoal, σn(τ) 6= xgoal for allτ ∈ (0, 1), and σn(0) = xinit.

III. THE BFMT∗ ALGORITHM

In this section, we present the Bi-Directional Fast March-ing Tree algorithm, BFMT∗, represented in pseudocode asAlgorithm 1. To begin, we provide a high-level descriptionof FMT∗ in Section III-A, on which BFMT∗ is based. Wefollow in Section III-B with its own high-level description,and then provide additional details in Section III-C.

A. FMT∗– High-level description

The FMT∗ algorithm, introduced in [7], [8], is a unidirec-tional algorithm that essentially performs a forward dynamicprogramming recursion over a set of sampled points andcorrespondingly generates a tree of paths that grow steadilyoutward in cost-to-come space. The recursion performedby FMT∗ is characterized by three key features: (1) It istailored to disk-connected graphs, where two samples areconsidered neighbors (hence connectable) if their distanceis below a given bound, referred to as the connectionradius; (2) It performs graph construction and graph searchconcurrently; and (3) For the evaluation of the immediate costin the dynamic programming recursion, one “lazily” ignoresthe presence of obstacles, and whenever a locally-optimal

(assuming no obstacles) connection to a new sample intersectsan obstacle, that sample is simply skipped and left for later(as opposed to looking for other locally-optimal connectionsin the neighborhood).The last feature, which makes the algorithm “lazy,” may causesuboptimal connections. A central property of FMT∗ is thatthe cases where a suboptimal connection is made becomevanishingly rare as the number of samples goes to infinity,which makes the algorithm asymptotically-optimal (AO).This manifests itself into a key computational advantage—by restricting collision detection to only locally-optimalconnections, FMT∗ (as opposed to, e.g., PRM) avoids alarge number of costly collision-check computations, at theprice of a vanishingly small “degree” of suboptimality. Werefer the reader to [7], [8] for a detailed description of thealgorithm and its advantages.

B. BFMT∗– High-level description

At its core, BFMT∗ implements a bi-directional versionof the FMT∗ algorithm by simultaneously propagating twowavefronts (henceforth, the leaves of an expanding tree willbe referred to as the wavefront of the tree) through the freeconfiguration space. BFMT∗, therefore, performs a two-sourcedynamic programming recursion over a set of sampled points,and correspondingly generates a pair of search trees: one incost-to-come space from the initial configuration and anotherin cost-to-go space from the goal configuration (see Fig. 1).Throughout the remainder of the paper, we refer to the formeras the forward tree, and to the latter as the backward tree.

(a) 0% Coverage (b) 25% Coverage (c) 50% Coverage

Fig. 1: The BFMT∗ algorithm generates a pair of search trees:one in cost-to-come space from the initial configuration (blue) andanother in cost-to-go space from the goal configuration (purple).The path found by the algorithm is in green color.

The dynamic programming recursion performed by BFMT∗is characterized by the same lazy feature of FMT∗ (seeSection III-A). However, the time it takes to run BFMT∗on a given number of samples can be substantially smallerthan for FMT∗. Indeed, for not-too-cluttered configurationspaces, the search trees grow hyperspherically, and BFMT∗only has to expand about half as far (in both trees) as FMT∗in order to return a solution. This is made clear in Fig. 1(a),in which FMT∗ would have to expand the forward tree twiceas far to find a solution. Since runtime scales approximatelywith edge number, which scales as the linear distance coveredby the tree raised to the dimension of the state space, wemay expect an approximate speed-up of a factor 2d−1 overFMT∗ in d-dimensional space (the −1 in the exponent isbecause BFMT∗ has to expand 2 trees, so it loses one factorof 2 advantage).

As for any bi-directional planner, the correctness and com-putational efficiency of BFMT∗ hinge upon two key aspects:(i) how computation is interleaved among the two trees(in other words, which wavefront at each step should bechosen for expansion), and (ii) when the algorithm shouldterminate. In this paper, we compare four possible variations:two options for tree expansions (i.e., item (i)) and two optionsfor termination (i.e., item (ii)).1) Tree expansion: As explained, BFMT∗ works by (lazily)propagating two wavefronts. Let T be the currently-expandingtree and T ′ be its companion. Each tree is associated withits own wavefront set, denoted H and H′ respectively, whichcontain the nodes on the wavefronts. Here we explore twoexpansion schemes:

1) “Alternating Trees”: (left column2, lines 15–18 in Al-gorithm 1) At each iteration, the forward and backwardtrees are “swapped;” that is, the two expanded in turns.

2) “Balanced Trees”: (right column, lines 15–18 in Algo-rithm 1) The tree with the wavefront node of smallestcost from its root is selected for expansion. If only onetree has a non-empty wavefront, that tree is selected.

The first is the simplest, and emulates the strategy adopted byRRT-Connect. The second, on the other hand, enforces moreof a “balanced” search, maintaining equal costs from the rootin each wavefront such that the two wavefronts propagateand meet roughly equidistantly in cost-to-go from their roots.2) Termination: In this paper, we also consider two termina-tion criteria, which affect solution quality and run time:

1) “First Path”: (left column, line 9 in Algorithm 1) Thealgorithm terminates once the two trees connect; namely,once a node added to tree T is also in tree T ′.

2) “Best Path”: (right column, line 9 in Algorithm 1) Thealgorithm terminates once the node z ∈ H chosen forexpansion is in the “interior” of T ′ (and hence, alreadyexpanded from the standpoint of T ′). In abbreviatedterms, the algorithm terminates if z ∈ V ′ \ H′.

Termination with the “first path” criterion returns the firstavailable path discovered, at the moment that the twowavefronts touch (which is not, in general, the lowest costpath). The “best path” condition, on the other hand, terminatesat the point where the two wavefronts have propagatedsufficiently far through each other that no better solutioncan be discovered. Intuitively, this is the first moment wherethe two trees have both selected, at the current iteration orpreviously, the node z as the minimum cost node from theirrespective roots.

C. BFMT∗– Detailed description

Let S be a set of points sampled independently and identicallyfrom the uniform distribution on Xfree, to which xinit andxgoal are added. (The extension to non-uniform samplingdistributions is addressed in Section IV-C.) Let tree T be thequadruple (V, E ,H,W), where V is the set of tree nodes, Eis the set of tree edges, and H and W are mutually exclusivesets containing the wavefront nodes in V and the unexploredsamples in S, respectively. To be precise, the unexplored

2Note that in Algorithm 1, the two partitioned columns are mutuallyexclusive (in other words, once a criterion is selected, it alone is applied).

set W stores all samples in the sample set S that have notyet been considered for addition to the tree of paths. Thewavefront set H, on the other hand, tracks in sorted order(by cost from the root) only those nodes which have alreadybeen added to the tree that are near enough to tree leaves toactually form better connections. As such, nodes H and Wplay the same role as their counterparts in FMT∗, see [7],[8]. However, in this case BFMT∗ “grows” two such trees,referred to as T = (V, E ,H,W) and T ′ = (V ′, E ′,H′,W ′).Initially, T is the tree rooted at xinit, while T ′ is the treerooted at xgoal. Note, however, that the trees are exchangedduring the execution of BFMT∗, so T in Algorithm 1 is notalways the tree that contains xinit.Before describing BFMT∗ in detail, we list briefly thebasic planning functions employed by the algorithm. LetSAMPLEFREE(n) be a function that returns a set of n ∈N points sampled independently and identically from theuniform distribution on Xfree. Let COST(x′x) be the costof the straight-line path between samples x and y. LetPATH(z, T ) return the unique path in the tree T from itsroot to node z. Also, with a slight abuse of notation, letCOST(x, T ) return the cost of the unique path in the treeT from its root to node x, and let COLLISIONFREE(x, y)be a boolean function returning true if the straight-line pathbetween samples x and y is collision free. Given a set ofsamples A, let NEAR(A, z, r) return the set of samples inA within a ball of radius r centered at sample z (i.e., the set{x ∈ A | ||x− z|| < r}). Pertaining to the tree expansionstrategies, let SWAP(T , T ′) be a function that swaps thetwo trees T and T ′. Finally, let COMPANION(T ) return thecompanion tree T ′ to T (or vice versa).The BFMT∗ algorithm is represented in Algorithm 1. First,a set of n configurations in Xfree is determined by drawingsamples uniformly. Two trees are then initialized usingINITIALIZE as shown in Algorithm 2, with a forward treerooted at xinit and a reverse tree rooted at xgoal. Oncecomplete, tree expansion begins starting with tree T rootedat xinit using the EXPAND procedure in Algorithm 3. Inthe following, the node selected for expansion will beconsistently denoted by z, while xmeet will denote the lowest-cost candidate node for tree connection (i.e., for joining thetwo trees). The EXPAND procedure requires the specificationof a connection radius parameter, rn, whose selectionwill be discussed in Section IV. EXPAND implements the“lazy” dynamic programming recursion described (at a highlevel) in Section III-B, making locally-optimal collision-freeconnections from nodes x near z unexplored by tree T (thosein set W within search radius rn of z) to wavefront nodesx′ near each x (those in set H within search radius rn of x).Any collision-free edges and newly-connected nodes are thenadded to T , the connection candidate node xmeet is updated,and z is dropped from the list of wavefront nodes. The keyfeature of the EXPAND function is that in the execution ofthe dynamic programming recursion it “lazily” ignores thepresence of obstacles (see line 6) – as discussed in Section IVthis comes at no loss of (asymptotic) optimality (see also[7], [8]). Note the EXPAND function is identical to that ofunidirectional FMT∗, with the exception here of additionallines for tracking of the connection candidate xmeet.After expansion, the termination condition is evaluated,

according to one of the two criteria outlined in Section III-B.2. If the algorithm has not terminated, the wavefrontsof both trees are checked; if both are empty, there are nofurther expansions that can be made, so failure is reported.Otherwise the algorithm proceeds with the selection of thenext node (and corresponding tree) for expansion accordingto the criteria outlined in Section III-B.1. For both criteria, ifeither wavefront H or H′ is empty (but not both), then thenext node z is taken as the lowest-cost node from the rootof the non-empty wavefront with the trees T and T ′ definedaccordingly. After selection, the entire process is iterated.

Algorithm 1 Bi-directional Fast Marching Tree Algorithm(BFMT∗)

1: S ← {xinit,xgoal} ∪ SAMPLEFREE(n)2: T ← INITIALIZE(S, xinit)3: T ′ ← INITIALIZE(S, xgoal)4:5: z← xinit, xmeet ← ∅6: success = false7: while success = false8: EXPAND(T , z, xmeet)

TerminationFirst Path Criterion

9: if xmeet 6= ∅Best Path Criterionif z ∈ (V ′ \ H′)

10: σ∗ = PATH(xmeet, T ) ∪ PATH(xmeet, T ′)11: success = true12: else13: if H′ = ∅ and H = ∅14: return Failure

Tree SelectionAlternating Trees

15: z←argminx′∈H′

{COST(x′, T ′)}

16:

17:

18: SWAP(T , T ′)

Balanced Treesz1 ← argmin

x∈H{COST(x, T )}

z2 ← argminx′∈H′

{COST(x′, T ′)}

(z,T )←argmin(z1,T ),(z2,T ′)

{COST(zi, Ti)}

T ′ = COMPANION(T )19: return σ∗

Algorithm 2 Initializes a Fast Marching Tree

1: function INITIALIZE(S, x0)2: V ← {x0}3: E ← ∅4: W ← S \ {x0}5: H ← {x0}6: return T = (V, E ,W,H)

IV. ASYMPTOTIC OPTIMALITY OF BFMT∗

In this section, we show the optimality of BFMT∗ for eachof the four versions of Algorithm 1 (one for each choiceof tree expansion and termination criteria). We begin with aresult essentially stating that any path in Xfree may be “traced”arbitrarily well by connecting randomly distributed pointsfrom a sufficiently large sample set covering the configurationspace (we refer to this property as probabilistic exhaustivity).We then provide the (asymptotic) optimality proof for BFMT∗,by showing that the algorithm recovers a solution with cost

Algorithm 3 Fast Marching Tree Expansion Step

Require: Connection radius parameter rn1: function EXPAND(T = (V, E ,H,W), z, xmeet)2: Hnew ← ∅3: Znear ← NEAR(S \ {z}, z, rn) ∩ W4: for x ∈ Znear

5: Xnear ← NEAR(S \ {x},x, rn) ∩ H6: xmin ← arg min

x′∈Xnear

{COST(x′, T )+COST(x′x)}

7: if COLLISIONFREE(xmin,x)8: V ← V ∪ {x} . Add x to tree9: E ← E ∪ {(xmin,x)} . Add edge to tree

10: Hnew ← Hnew ∪ {x} . Save x11: W ←W\{x} . Mark x explored12:13: if x ∈ V ′ and COST(x, T ) + COST(x, T ′) <

COST(xmeet, T ) + COST(xmeet, T ′)14: xmeet←x . Set xmeet as best connection15:16: H ← (H ∪ Hnew)\{z} . Add new nodes to

wavefront; drop z from wavefront17: return T = (V, E ,H,W)

no greater than that of any tracing path. In the following,let ζd denote the volume of the unit ball in d-dimensionalEuclidean space. Also, the complement of a probabilisticevent A will be denoted by Ac.

A. Probabilistic exhaustivity

Let x : [0, 1]→ X be a path. Given a set of samples (referredto as waypoints) {ym}

Mm=1 ⊂ X , we associate a path y :

[0, 1]→ X that sequentially connects the nodes y1, . . . ,yMwith line segments. We consider the waypoints {ym} to(ε, r)-trace the path x if: a)

∣∣∣∣ym − ym+1

∣∣∣∣ ≤ r for all m,b) the cost of y is bounded as c(y) ≤ (1 + ε)c(x), and c)the distance from any point of y to x is no more than r,i.e., mint∈[0, 1]||y(s)− x(t)|| ≤ r for all s ∈ [0, 1]. In thecontext of sampling-based motion planning, we may expect tofind closely-tracing {ym} as a subset of the sampled points,provided the sample size is large. This notion is formalizedin the following theorem, proved as Theorem IV.5 in [17] forthe general case of driftless control-affine control systems, aspecial case of which is path planning without differentialconstraints (as addressed in this paper).Theorem 4.1 (Probabilistic exhaustivity): Let(Xfree,xinit,xgoal) be a path planning problem andlet x : [0, 1] → Xfree be a feasible path. LetS = {xinit,xgoal} ∪ SAMPLEFREE(n), ε > 0, and for fixedn consider the event An that there exist {ym}

Mm=1 ⊂ S,

y1 = xinit, yM = xgoal which (ε, rn)-trace x, where

rn = 4√d · (1 + η)

1d

(µ(Xfree)

ζd

) 1d(

log n

n

) 1d

for a parameter η ≥ 0. Then, as n→∞, the probability thatAn does not occur is asymptotically bounded as P[Acn] =

O(n−

ηd log−

1d n)

.

B. Asymptotic optimality (AO)

We are now in a position to prove the asymptotic optimalityof BFMT∗, which represents the main result of this section.We start with an important lemma, that relates the cost ofthe path returned by BFMT∗ to that of any feasible path.Lemma 4.2 (Bi-directional FMT∗ cost comparison): Let x :[0, 1] → Xfree be a feasible path with strong δ-clearance.Consider running BFMT∗ to completion using any choiceof expansion and termination criteria with n samples and aconnection radius:

rn = 4√d (1 + η) · 2

(1

d

) 1d(µ(Xfree)

ζd

) 1d(

log(n)

n

) 1d

,

for a parameter η ≥ 0. Let cn denote the cost of the pathreturned by BFMT∗; then for fixed ε > 0:

P[cn > (1 + ε)c(x)] = O(n−

ηd log−

1d n).

Proof: We address the case of the “first path” terminationcriterion, in conjunction with any expansion criterion (that is,the two employed in this paper and otherwise). It is clear thatthe “best path” termination criterion returns a path at least asgood as the first path termination criterion. This is becauseit returns the best path among a number of choices, one ofwhich is the path of the first path criterion. See Remark 4.5following this proof for further discussion.If xinit = xgoal, then BFMT∗ immediately terminates withcn = 0, trivially satisfying the claim. Thus we assume thatxinit 6= xgoal. Consider n sufficiently large so that rn ≤min

{δ / 2 , ε ||xinit − xgoal||

/2}

, and apply Theorem 4.1 toproduce, with probability at least 1 − O

(n−

ηd log−

1d n)

, asequence of waypoints {ym}Mm=1 ⊂ S, y1 = xinit, yM =xgoal which (ε / 2 , rn)-trace x. We claim that in the event thatsuch {ym} exists, the BFMT∗ algorithm returns a path withcost upper bounded as cn ≤ c(y) + rn ≤ (1 + ε / 2)c(x) +(ε / 2)c(x) = (1 + ε)c(x). It is clear that the desired resultfollows from this claim.Assume the existence of an (ε / 2 , rn)-tracing {ym}. LetB(x, r) represent a ball of radius r centered at a samplex. Note that our upper bound on rn implies that B(ym, rn)intersects no obstacles. This follows from our choice of rnand the distance bound

infa∈Xobs

||ym − a|| ≥ infa∈Xobs

||xm − a|| − ||ym − xm||

≥ 2rn − rn ≥ rn.This fact, along with

∣∣∣∣ym − ym+1

∣∣∣∣ ≤ rn for all m, impliesthat when a connection is attempted for ym, both ym−1 andym+1 will be in the search radius without obstacles withinthat search radius. Running BFMT∗ to completion generatesone cost-to-come tree Ti(Vi, Ei,Hi,Wi) and one cost-to-gotree Tg(Vg, Eg,Hg,Wg) rooted at xinit and xgoal, respectively(the subscripts i and g are used to identify the root of a treewithout ambiguity). The above discussion ensures that thetrees will meet and the algorithm will return a feasible pathwhen it terminates – the path outlined by the waypoints {ym}disallows the possibility of failure.For each sample point z ∈ S, let ci(z) := COST(z, Ti)denote the cost-to-come of z from xinit in Ti, and letcg(z) := COST(z, Tg) denote the cost-to-go from z to xgoal

in Tg. If z is not contained in a tree Tk, k = {i, g}, weset ck(z) = ∞. When the algorithm terminates, we knowthere exists a sample point zmeet ∈ Vi ∩ Vg where thetwo trees meet; indeed we select the particular meetingpoint zmeet = arg minz∈Vi∩Vg ci(z) + cg(z). Then cn =ci(zmeet) + cg(zmeet). We now note a lemma bounding thecosts-to-come of the {ym}, the proof of which may be foundas an inductive hypothesis (Eq. 5) in Theorem VI.1 [17].Lemma 4.3: Let m ∈ {1, . . . ,M}. If ci(ym) < ∞,then ci(ym) ≤

∑m−1k=1

∣∣∣∣yk − yk+1

∣∣∣∣. Otherwise if ym /∈Vi, then ci(zmeet) ≤

∑m−1k=1

∣∣∣∣yk − yk+1

∣∣∣∣. Similarly ifcg(ym) < ∞, then cg(ym) ≤

∑M−1k=m

∣∣∣∣yk − yk+1

∣∣∣∣; oth-erwise cg(zmeet) ≤

∑M−1k=m ||yk − yk+1||.

To bound the performance cn of BFMT∗, there are two casesto consider. Note in either case we find that cn ≤ c(y) + rn,thus completing the proof.

Case 1: There exists some ym ∈ Vi ∩ Vg .In this case, cn = ci(zmeet) + cg(zmeet) ≤ci(ym) + cg(ym) < ∞ by our choice of zmeet.Then applying Lemma 4.3 we see that cn ≤ci(ym) + cg(ym) ≤

∑M−1k=1

∣∣∣∣yk − yk+1

∣∣∣∣ = c(y).

Case 2: (Vi ∩ Vg) ∩ {ym} = ∅.Consider m = max{m | ci(ym) <∞}. Thenym ∈ Vi and ym can not have been the minimumcost element of Hi at any point during algorithmexecution or else we would have connected ym+1 ∈Vi. Let z denote the minimum cost element of Hiwhen zmeet was added to Vi. We have the bound:

ci(zmeet) ≤ ci(z) + rn≤ci(ym) + rn

≤m−1∑k=1

∣∣∣∣yk−yk+1

∣∣∣∣ + rn. (1)

By our assumption for this case, ym /∈ Vg.Then by Lemma 4.3 we know that cg(zmeet) ≤∑M−1

k=m

∣∣∣∣yk − yk+1

∣∣∣∣. Combining with the previ-ous inequality yields cn = ci(zmeet)+cg(zmeet) ≤∑M−1k=1

∣∣∣∣yk − yk+1

∣∣∣∣ + rn = c(y) + rn.

Remark 4.4 (Tightened bound for connection radius): Asdiscussed in [17], for the sake of clarity the spatial constant4√d in the expression for rn is greater than is necessary for

Theorem 4.1 to hold. A more careful argument along thelines of the original FMT∗ AO proof [7] would suffice to

show that rn = (1 + η) · 2(1d

)1/d(µ(Xfree)ζd

)1/d(log(n)n

)1/dsatisfies the theorem as well.Remark 4.5 (Alternative termination criteria): If we ana-lyze the ”best path” criterion instead of the ”first path”criterion (i.e., if BFMT∗ continues until an element of Vi∩Vgis selected as the minimum cost element of Hi or Hg), thenthe rn term may be omitted from inequality (1). In that casewe need only consider n sufficiently large so that rn ≤ δ / 2 .We now have everything we need to show that BFMT∗is asymptotically-optimal. The next theorem defines thisformally.Theorem 4.6 (BFMT∗ asymptotic optimality): Assume a δ-

robustly feasible path planning problem as defined in Sec-tion II with optimal path σ∗ of cost c∗. Then BFMT∗converges in probability to σ∗ as the number of samplesn→∞. Specifically, for any ε > 0,

limn→∞

P[cn > (1 + ε)c∗] = 0

Proof: The proof follows as a corollary to Lemma 4.2.By our δ-robustly feasible assumption, we can find a strongδ-clearance feasible path x : [0, 1]→ Xfree that approximatesσ∗ with cost c(x) < (1 + ε / 3)c∗ (i.e., less than factor ε / 3from c∗), for any ε > 0. By Lemma 4.2, we can choosen sufficiently large such that BFMT∗ returns an ε / 3 costapproximation to the approximant:

P[cn > (1 + ε / 3)

2c∗]< P[cn > (1 + ε / 3)c(x)]

= O(n−

ηd log−

1d n)

To approach the optimal path, let the number of samplesn→∞. It follows that, for any η ≥ 0:

limn→∞

P[cn > (1 + ε / 3)

2c∗]< limn→∞

O(n−

ηd log−

1d n)

= 0

Now we relate this to the original claim. First suppose thatε ≤ 3. From (1 + ε / 3)

2 ≤ 1+ε, the event {cn > (1 + ε)c∗}is a subset of the event

{cn > (1 + ε / 3)

2c∗}

, hence:

limn→∞

P[cn > (1 + ε)c∗] ≤ limn→∞

P[cn > (1 + ε / 3)

2c∗]

=0.

Because the probability is monotone-decreasing in ε as εincreases, the statement holds for all ε > 3 as well (to see this,apply Lemma 4.2 again for m sufficiently large to handle ε =3; then by similar argument as above P[cm > (1 + ε)c∗] <

P[cm > (1 + 3)c∗] = O(m−

ηd log−

1d m

)and take the limit

as m → ∞). Hence limn→∞ P[cn > (1 + ε)c∗] = 0 holdsfor arbitrary ε, and we see that BFMT∗ converges inprobability to the optimal path, as claimed.Remark 4.7 (Convergence rate): Note that we can also trans-late the convergence rate from Lemma 4.2 to the setup ofTheorem 4.6, which doesn’t require strong δ-clearance. Forany ε > 0, the optimal path can be approximated by a strong-δ-clear path with cost less than (1 + ε)c(x) and we can focuson approximating that path to high-enough precision to stillapproximate the optimal path to within (1 + ε). Since theconvergence rate in Lemma 4.2 only contains ε in the rate’sconstant, the big-O convergence rate remains the same.

C. Discussion

It is worth mentioning a few variations on the vanilla versionof BFMT∗. First, if one has prior information about areasthat the optimal path is unlikely to pass through, one mayconsider a non-uniform sampling strategy that downsamplesthese regions. As long as the sampling density is lower-bounded by a positive number over the configuration space,BFMT∗ can be slightly altered (by just increasing rn bya constant factor) to ensure it stays AO. The argument isanalogous to that made in [8], and essentially proceeds bymaking the search radius wide enough to balance out thedetrimental effect of the lower sampling density (in someareas). Another common problem variation is when the costis not arc-length, but a different metric or a line integralcost. In either case, BFMT∗ need only consider cost balls

instead of Euclidean balls when making connections. Detailsfor adjusting the algorithm and why the AO proof still holdscan be derived from [8]. The argument basically shows thatthe triangle inequality either holds exactly (for metric costs)or approximately, and that this approximation goes away inthe limit as n→∞. Finally, note that BFMT∗, like FMT∗,is not an anytime algorithm. However, the algorithm can beadjusted analogously to FMT∗ as in [18] in order to make itanytime. Essentially, batches of samples are repeatedly addedto the configuration space and connected to the tree untiltime runs out, with previous computations carefully reused.

V. SIMULATIONS

In this section, we provide numerical experiments thatcompare the performance of BFMT∗ with other sampling-based, asymptotically-optimal planning algorithms (namely,FMT∗, RRT∗, and PRM∗) and existing sampling-based, bi-directional algorithms (namely, RRT-Connect and SBL). Totest them, we would like to observe the quality of the solutionreturned as a function of the execution time allotted tothe algorithm. As a basis for quality comparison betweenincremental or ”anytime” planners (such as RRT∗) andnon-incremental planners (such as BFMT∗, which generatesolutions via sample batches), we use as our independentvariable the number of samples drawn by the planners duringthe planning process (which in essence serves as a proxy toexecution time). Note sample count has a different connotationdepending on the planner that will not necessarily be thenumber of nodes stored in the constructed solution graph –for RRT∗ (with one sample drawn per iteration), this is thenumber of iterations, while for FMT∗, PRM∗, and BFMT∗,this is the number of free space samples taken.In the first experiments, we investigate the relative benefitsof the four versions of BFMT∗. Then, in the second set,we compare the version of BFMT∗ that appears to performbest (namely, BFMT∗ with the “alternating trees” criterionfor expansion and “best path” criterion for termination)with the other planners. To generate simulation data fora given experiment, we queried the planning algorithmsonce each for a series of sample counts, recording thecost of the solution returned, the planner execution time3,and whether the planner succeeded or not, then repeatedthis process over 50 trials. To ensure a fair comparison,each planning algorithm was tested using the Open MotionPlanning Library (OMPL) v0.15.0 [19], which provides high-quality implementations of many state-of-the-art planners anda common framework for executing motion plans. In this way,we could ensure that all algorithms employed the exact sameprimitive routines (e.g., nearest-neighbor search, collision-checking, data handling, etc.), and measure their performancesfairly. Regarding implementation, BFMT∗, FMT∗, RRT∗, andPRM∗ used η = 0 from Lemma 4.2 for the nearest-neighborradius rn in order to satisfy the theoretical bounds providedin Section IV and [6]. For RRT-Connect, SBL, and RRT∗, weused the default OMPL settings; namely, a steering parameterequal to 20% of the maximum extent of the configurationspace, and for RRT∗ a 5% goal bias.

3Code for all experiments was written in C++. Corresponding programswere compiled and run on Linux-operated PC, clocked at 2.4 GHz andequipped with 7.5 GB of RAM.

Before proceeding, note that each marker shown on the plotsthroughout this section represents a single simulation at a fixedsample count. The points on the curves, however, representthe mean cost/time of successful algorithm runs only for aparticular sample count, with error bars corresponding to onestandard deviation of the 50 run sample mean.4

A. Comparison amongst BFMT∗ versions

We first test the four versions of BFMT∗ against a standardpath planning problem: a point mass robot moving in a clut-tered unit hypercube.5 The variants permute the terminationcriterion (whether at the first or best path possible) as well asthe expansion criterion (whether swapping trees every time orbalancing tree reachable volumes) as outlined in Section III.

(a) 5D, 50% Coverage (b) 10D, 50% Coverage

Fig. 2: Cost vs. time plots for the four versions of BFMT∗. BFMT∗

with “best path” termination and “alternating trees” expansion seemsto represent the best trade-off in exploration vs. exploitation.

Collectively, Fig. 2 shows four facts: (i) all variants ofBFMT∗ tend to perform fairly similarly, (ii) the maindiscriminant in performance is the termination criterion (ascurves corresponding to the same expansion criterion arealmost overlapping), (iii) the “best path” termination criterionappears to be the best choice fairly uniformly, and (iv)for a given termination criterion, the swapping expansioncriterion appears to yield slightly better performance. Fact(iv) exemplifies a classic trade-off in exploration versusexploitation. To clarify, note that ”balanced” growth hereis analogous to two-source breadth-first search in that thewavefront node of lowest-cost within either tree is chosenfor expansion at the next iteration, effectively requiring thatboth wavefronts fully explore the current cost-curve levelset before progressing. These wavefronts, then, spread outequally (which, in the Euclidean distance case, means radiallyfrom their sources) at identical rates, with one tree waitingfor the other to finish its level set before continuing. This canbe a hindrance in cases where one tree has less surroundingfree-space to cover than the other – exactly as we see heregiven a goal located in a corner rather than near the hypercubecenter. Conversely, when iteratively swapping between trees,wavefronts now explore the level sets of the cost-to-come andcost-to-go curves independently and at “asynchronous” rates;if one tree finishes a level set sooner, it continues on to thenext without waiting (a particularly useful quality in clutteredspaces). In light of this and the results in Fig. 2, we selected

4Standard deviation of the mean indicates where we expect with 1-σconfidence the mean to lie if we ran another set of 50 runs, and is relatedto the standard deviation of the distribution by σµ = σ/

√50.

5We populated the space to 50% obstacle coverage with randomly-sized,axis-oriented hyperrectangles. xinit was set to the center at [0.5, . . . , 0.5],with the goal xgoal at the one-vector (i.e., [1, . . . , 1]). Collision-checkingcalled upon a two-phase tiling-based hashing scheme.

BFMT∗ with “best path” termination and “alternating trees”for the subsequent experiments.

B. Comparison with the State-of-the-Art

Here we present benchmarking results (average solution costvs. average execution times and success rates) comparing thebest of the four BFMT∗ variants, herein simply denoted asBFMT∗, to other state-of-the-art sampling-based planners.Three benchmarking test scenarios were considered: (1) a2D “bug trap” and (2) a 2D “maze” problem for a convexpolyhedral robot in the SE(2) configuration space, as well as(3) a more challenging 3D problem called the “α-puzzle” inwhich we seek to untangle two loops of metal (non-convex)in the SE(3) configuration space. All problems were drawndirectly from OMPL’s bank of tests, and are illustrated inFig. 3. In each case, collision-checks called upon OMPL’sbuilt-in collision-checking library, FCL.

(a) SE(2) bug trap (b) SE(2) maze (c) SE(3) α-puzzle

Fig. 3: Depictions of the three OMPL rigid-body planning problems

Figure 4 shows the results for each BFMT∗, FMT∗, RRT∗,and PRM∗ (average costs for RRT-Connect and SBL wereroughly 2-4x greater, which occluded the details of othercurves; they were thus omitted for clarity). The plots revealin each case that BFMT∗ tends to an optimal cost at least asfast as the other planners, and sometimes much faster. BFMT∗noticeably outperforms RRT∗ (despite its goal biasing) as wellas PRM∗. Its performance relative to FMT∗ seems somewhatmore complex, however.To compare them in detail, we test their performance as afunction of configuration space dimension using the (scalable)unit hypercube test case. Results for 5D and 10D areshown in Fig. 5. Here BFMT∗ substantially outperformsFMT∗, particularly as dimension increases. This suggests thatreachable volumes play a significant role in their executiontime. The relatively small volume of reachable configurationsaround the goal at the corner implies that the reverse treeof BFMT∗ expands its wavefront through many fewer statesthan the forward tree of FMT∗ (which in fact needlesslyexpands towards the zero-vector); tree interconnection in thebi-directional case prevents its forward tree from growing toolarge compared to unidirectional search. This is pronouncedexponentially as the dimension increases. In trap or maze-likescenarios, however, bi-directionality does not seem to changesignificantly the number of states explored by the marchingtrees, leading to comparable performance for the SE(2) bug-trap and maze. Fortunately, it does offer slight improvementsto success rate over FMT∗, and often the other planners aswell. Note we expect a greater contrast in execution times asthe cost of collision-checking increases, such as with manynon-convex obstacles or in time-varying environments.

Importantly, we note that BFMT∗ misses out on one of theusual advantages of bi-directionality: the improved ability toescape bug traps via two-sided approach. By the nature ofBFMT∗’s growth, if a forward tree cannot escape the trap,then adding a backward tree will not help. A future area ofstudy is to consider possible remedies to this; for instance, byadaptively-sampling and regrowing near failing wavefronts, itmay be possible to quickly salvage a failed run. In this case,a bi-directional approach would be quite advantageous.

ççç

ç

ç

ç

ç

çç

ç

ç

çç

ç

ç

ç

ç

ç

ç

çç

ç

ç

ç

ç

ç

ç

ç

çç

ç

ççç

ç

ç

çç

çç

ç

ç

ç

ç

ç

ç

ç

ç

çç

ç

ç

ç

ççç

çç

ç

ç

ç

ç

çç

ç

ç

ç

çç

ç

ç

ç

ç

ç

ççç

ç

ç

ç

ç

ç

çç

ç

ç

ç

çç

ç

ç

ç

çç

ç

ç

ç

ç

ç

çç

çç

ç

çç

ç

ç

ççç

ç

ç

ç

ç

ç

ç

ç

ç

ç

çç

ç

ç

ç

ç

çç

ç

ç

çç

ç

çç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

çç

ç

ç

ç

ç

ç

ç

çççç

ç

ç

çç

ç

çç

ç

ç

ç

ç

ç

ç

ç

ç

ç

çç

ççç

ç

çççç

ç

ç

ç

ç

ç

ç

çç

çç

ç

ç

çç

ç

ç

ç

çç

çç

ç

ç

ç

çç

ç

çç

ç

ç

ç

ç

çç

ç

ç

ç

ç

ç

ç

ç

ççç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ççç

ç

ç

ç

ç

ç

çç

ç

çç

çç

ç

ç

ç

çç

ç

ç

çç

ç

ç

ç

ççç

ç

ç

çç

ç

ç

ç

ç

ç

çççç

ç

ç

ç

çç

ç

çç

ç

ççç

ç

çç

ç

ç

ççççç

çç

ç

çç

ç

ç

ç

ç

ç

ç

ç

çç

çç

ççç

ç

ç

ç

ç

çç

ççç

ç

ç

ç

çç

ç

ç

ç

çççç

ç

ççç

ç

ç

ç

ççç

çç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

çç

ç

ç

ç

ç

ççç

ççç

ç

çç

ç

ç

ç

çççç

çççç

ç

çç

ç

ç

ç

çç

çç

ççç

ç

ç

çççç

ççç

ççç

ç

ç

ç

ç

çççççç

ç

ç

ç

çç

ç

ççç

ççç

ç

ç

ç

ç

çç

ç

ç

ç

ç

ç

ç

ç

çç

ç

ç

ç

ç

á

á

á

á

ááá

á

á

á

á

áá

áá

á

á

áá

á

áá

á

áá

áá

á

á

á

á

á

á

áá

á

á

á

áá

á

áá

áá

á

á

á

á

á

áá

áá

á

ááá

á

á

ááá

á

á

á

á

á

á

á

áá

á

á

á

á

á

á

á

á

á

á

á

á

áá

áá

á

á

á

á

á

á

á

á

á

á

áá

á

áá

á

á

á

á

á

á

áá

á

á

á

á

áá

á

á

á

á

á

á

áá

á

áááá

áá

á

á

á

á

á

áá

á

áá

á

á

ááá

á

á

á

á

á

á

á

á

á

áá

á

á

á

á

á

áá

áá

á

áá

á

á

áá

á

áááá

á

áá

á

á

ááá

áá

ááááá

áááá

ááá

á

á

á

á

áááááá

á

áá

áá

á

á

áá

ááá

á

áá

áá

áá

á

á

ááááá

á

á

á

á

á

á

á

á

á

áá

áááá

á

á

á

á

á

áá

á

á

áá

áá

áá

áá

á

á

á

áá

á

á

ááá

á

á

á

á

á

á

á

á

á

á

áá

á

ááá

á

ááá

á

á

á

ááá

á

á

á

á

á

á

á

á

áá

á

á

áá

ááá

áá

á

áá

áááá

á

á

á

á

ááááá

á

á

á

áá

áá

á

á

á

áá

á

á

á

á

á

á

áá

áá

á

ááá

ááá

á

áá

ááá

á

áááá

á

áá

á

á

á

ááá

á

á

áá

á

á

á

áá

ááá

áá

á

ááá

ááá

á

á

á

á

á

ááá

ááá

á

ááá

ááá

ááá

á

á

áá

á

á

á

á

á

á

á

ááá

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

óó

ó

ó

ó

ó

ó

ó

ó

ó

óó

ó

ó

ó

ó

ó

ó ó

ó

ó

ó

ó

ó

ó

óó

ó

ó

ó ó

ó

ó

ó

óó

ó

ó

ó

ó

óó

ó

ó

ó

ó

ó

ó

ó

óó

ó

óó

ó

ó

ó

óó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

óó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

óóóó

ó

ó

ó

óó ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

óó

óó

óó

óó

ó

óó

ó

ó

ó ó

ó

ó

ó

óó

ó

ó

ó

óó

ó ó

ó

ó

óó

ó

ó

óó

ó

óó

ó

óó

ó

óó

óó

ó

ó

ó

ó

óó ó

ó

óó

ó

ó

ó

óó

ó

ó

ó

ó

ó

ó

óó

ó

ó

óó

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õõ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õõõ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õõõ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õõ

õõ

õ

õ

õ

õ

õ

õ

õõõ

õõ

õ

õ

õ

ç BFMT*

á FMT*

ó RRT*

õ PRM*

0 1 2 3 4 5 6 7120

130

140

150

160

Execution Time HsL

So

lutio

nC

ost

SEH2L Bug Trap HOMPL.appL

(a)

ç

ç

ç

ç

çç

ç

ç

çç

çç ç ç

áá

á

á

áá

á

áá

áá

á á á

óóó

ó

ó

ó

ó

ó

ó

ó

ó

ó

õ

õ

õ

õ

õ

õ

ç BFMT*

á FMT*

ó RRT*

õ PRM*

0 1 2 3 4 5 6 7

0.0

0.2

0.4

0.6

0.8

1.0

Execution Time HsL

Su

cce

ss

Ra

te

SEH2L Bug Trap HOMPL.appL

(b)

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

çç

ç

ç

çç

ç

ç

ç

ç

çç

ç

çç

ç

ç

ç

ç

ç

ç

çç

ç

ç

ç

çç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ççç

ç

ç

çç

ç

ç

ç

ç

ç

çç

ç

ç

ç

ç

çççç

ç

ç

ç

ç

ç

ç

ç

ç

ççç

ç

çç

çç

çç

ç

ç

ç

ç

çç

ç

çç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ççç

ç

ç

ç

ç

ç

ççç

çççç

ç

ç

çç

çççç

ç

ççç

ç

çç

ççççççççççççççççç

ç

ç

ç

ç

ç

çççç

ç

ççç

ç

çççç

çç

çççççç

ç

ç

ç

ççççç

ç

ççççç

çç

çç

ç

çç

ç

ç

ççç

ç

ççççç

çççç

çççççç

çççç

çç

ç

çç

çççççç

çç

ç

ç

çç

ç

ççççç

ç

ççççç

ç

ççççç

çççç

ç

ç

ç

çççççççççççç

çç

ççç

çç

ç

çç

çççççç

çççççççç

ççççç

ç

ççççç

ççççççç

çççççç

çççççççç

ççççççç

ççççççççç

ç

ç

çç

ççççç

ççç

ç

çç

ç

ççççççç

çç

ççççççççççç

ççççç

çççççççç

ç

çççç

çççç

ç

ççççççç

ççççççççç

ççç

ççç

çççççççççççç

ççççççç

ççç

ç

çççç

çççç

ççç

çççççç

çççç

ççççççççç

çç

ççççç

çç

çççççççç

ççç

ç

ççç ç

ççç

çç

çççç

çççççççç

ççççççççççç

ççççç

çççççççççç

çççççç çç

çççççç

ççççççççç

ççççç

çççççç

çç

çççççççççççççççç

çççç

á

á

á

á

á

á

á

á

á

á

á

á

á

á

á

á

á

áá

á

á

á

á

á

á

á

á

á

á

á

á

áá

áá

á

á

á

á

á

áá

á

áá

á

á

á

áá

áá

á

á

áá

á

á

á

á

á

á

á

á

á

á

á

á

áá

á

ááá

ááá

á

á

ááááá

á

ááááá

á

ááááááá

áá

á

áá

á

á

á

á

áá

á

á

áá

á

á

á

áá

á

á

á

áá

áááá

á

áááá

á

ááááááá

á

ááá

áá

á

á

ááá

áááá

á

ááááá

á

á

áá

á

ááá

á

á

ááá

á

ááááááá

á

ááá

áááá

áááááááá

á

áá

ááááááá

á

áá

á

áááááá

á

á

á

áá

ááááá

ááá

ááá

ááá

á

áá

á

áááá

ááá

á

ááááááááááá

áááááá

ááááááá

áááá

ááá

ááááá

áááááááááá

áá

á

ááááááááááá

áááááááá

áááá

áááá

áá

ááá

áá

áááá

ááááááá

ááá

ááá

ááááááááááá

áááá

áááá

áááá

áá á

ááááá

áááááááááá

áááááá

áááááá

ááá

á

áá

áá

áá

á

áááááá

ááááááááá áá

á

áááááááá

áá

ááááááááá

á

áá

áááá

áá

ááááááááá

áá

áááááááá á

áááááá

áááááááááá

áá

áááá

ááá

áá

ááá

á

á

áááá

á

áááááááááá

áá

áááááá áááááááá

áááá á

áááááá

áá áááááááááá

á áááááá

áááááá áááááá á

á á ááá

áááá

áá

ááááá áááááááá á

áááááá

ááá

áá

ááááá

áá

ó

ó

ó

ó

ó

ó

ó

óó

ó

óó

ó

ó

óó

ó

ó óóó

ó

ó

ó

ó

ó

óó

ó

ó

ó

ó

ó

ó

ó

ó

ó

óó

óó

ó

óó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

óó

ó

ó

óó

óó

ó

ó

ó

ó

ó

óó

óó

óóó

ó

ó

ó

ó

ó

ó

ó

óó

óó

óó óóó

ó óó

ó ó

ó

ó

ó

óóóó

ó

óó

óó

ó

ó

ó óó óó

ó

ó

óó

ó

óóó ó

óóó

óó ó óóó óó

óóó

óóó

óóó

ó

óó ó

óó

ó

óó

ó

ó

óóó óó

ó óóó

óó ó

óó

ó

óó

ó

óó

óó

óó

ó ó ó

óóó

ó

ó óóó

óó

ó

ó

óóó

óó

óóó

óó

óó

óó

ó

óó ó

ó

ó ó

ó

ó óóó

óó óóó

ó

óó

ó

óóóóó

óó

óóó óó

óóóó ó

óóóóó

óó óóó

óó ó

ó

ó óó

óó ó

óó ó

óóó

óó

óó

óó ó

óó ó óóóó

óó

óóóóó

ó

ó

óó

óóó

ó óóóó

óóó

ó

óó ó ó ó

óó

óó óó ó

óó

ó ó

ó

óóóóó

óóó

ó óóóóóóó

óóó

ó

ó óó

óóó

óó óó

óóó ó

ó

õ

õ

õ

õõ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õ

õõ

õõ

õ

õõ

õ

õ

õ

õõ õõ

õ

õõõ

õõ

õ

õ

õõõ

õ õõ

õ

õõ

õ

õõ

õ

õ

õ

õõ

õ

õ

õ

õ

õõõõõõ

õõ õ

õõ

õ

õ

õ

õ

õõ

õ

õõõõ

õ

õõ

õ

õ

õ

õ

õõõ

õõõ

õ

õõ

õ

õ

õ

õ

õ

õõõõ

õõõõ

õõ õõ

õ

õ õõ

õõ

õõõ

õõ

õõ

õõõ

õ

õ õõ õõõ

õ õõ

õõ õõ

õõõ

õ

õõõ

õõ

õõ

õ

õõõ

õõ

õ

õõõ õõ

õ

ç BFMT*

á FMT*

ó RRT*

õ PRM*

0 2 4 6 860

80

100

120

140

160

Execution Time HsL

So

lutio

nC

ost

SEH2L Maze HOMPL.appL

(c)

ç

ç

ççççççççç ç ç ç

á

á

áááááááá á á á á

óó

ó

ó

ó

ó

ó

ó ó ó ó ó

õ

õ

õ õ õ

ç BFMT*

á FMT*

ó RRT*

õ PRM*

0 2 4 6 8

0.0

0.2

0.4

0.6

0.8

1.0

Execution Time HsL

Su

cce

ss

Ra

te

SEH2L Maze HOMPL.appL

(d)

çç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

çç

ç

ç

ç

ç

ç

ç

çç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

çç

ç

ç

ç

ç

ç

ç

ç

ç

çç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

çç

ç

ç

ç

ç

ç

çççç

ç

ç

çç

ç

ç

ç ç

ç

ç

ç

ç

çç

ç

ç

ç

çç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

çç

çç

ç

ç

ç

ç

ç ç

ç

ç

ç

ç

çççç

çç

ç ç

ç

ç

ç

ç

çç

ç

ç

ç

ç

çç

ç

ç

ç

ç

çç

ç

ç

çç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

ç

çç

ç

á

á

á

á

á

á

á

á

á

á

á

á

á

á

á

áá

á

áá

á

ááá

á

á

á

á

á

á

á

á

áá

á

á

á

á

á

á

á

á

á

á

á

á

á

áá

á

á

á

á

á

áá

áá

á

á

á

áá

á

á

á

á

áá

á á

áá

á

áááá

á

á

á

á

á

á

á

á

á

á

á

áá

á

á

á

áá

á

á

á

á

á

á

á á

á

á

áá

á

áá

á

á

ááá

á

á

áá

á

á

á

á

á

á

á

á

á

á

á

á

á

á

áá á

á

áá

á

á

á

á

á

á

ááá á

á

áá

ááá

á

á

áá

áá

á

á

á

á

á

á

áá

á

á

á

á

á

á

á

áááá

á

á

á

áá

á

áá

á

á

áá

á

áá

á

ááá

á

áá á

á

á

á

á

á

á

á

á

á

áá

áá

áááá

á

á

á

á

á

á

á áá á

áá

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

óó

ó

óó

ó

ó

ó

ó

ó

ó

óó

ó

ó

ó

ó

ó

óó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

óó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

óó

ó

ó

ó

ó

ó

ó

ó

óó

ó

ó

óó

ó

ó

ó

ó

ó

ó

ó

ó

ó

óó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

óó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

óóóó

ó

ó

ó

ó

óó

óó

ó

ó

ó

ó

óó ó

ó

óó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

óó

óó

ó

ó

ó

óó

ó

ó

ó

ó

ó

ó

ó

ó

óó

ó

ó

ó

ó

ó

óó

ó

ó

ó

ó

óó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

óóó

ó

ó

ó

ó

óó

ó

ó

ó

ó

ó

ó

ó

ó

ó

óó

ó

ó

ó

óó

ó

ó

ó

ó

ó

óóó

ó

óóó

ó

ó

ó

ó

ó

ó

ó

óóó

ó

ó

ó

ó

ó

óó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

óó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

ó

óó

ó ó

ó ó

ó

ó

ó

ó

ó

ó

óó

ó

ó

ó

ó

ó

ó ó

ó

ó

ó

ó

ó

ç BFMT*

á FMT*

ó RRT*

0 20 40 60 80 100200

300

400

500

600

700

Execution Time HsL

So

lutio

nC

ost

SEH3L Alpha Puzzle HOMPL.appL

(e)

ç

ç

ç

ç

ç

ç

ç

á

á

á

á

á

áá

ó

ó

ó ó

óó

ó

ç BFMT*

á FMT*

ó RRT*

0 20 40 60 80 100

0.0

0.2

0.4

0.6

0.8

1.0

Execution Time HsL

Su

cce

ss

Ra

te

SEH3L Alpha Puzzle HOMPL.appL

(f)

Fig. 4: Simulation results for the three OMPL scenarios. The jump in(e) is from two homotopies for the α-puzzle; note that BFMT∗ findsthe cheaper homotopy faster and more often than other planners.

ç

ç

ç

ç

ç çç ç

á

á

áá á

á

á á

ç BFMT*

á FMT*

0.0 0.5 1.0 1.5 2.0

1.4

1.6

1.8

2.0

2.2

Execution Time HsL

So

lutio

nC

ost

5D Point Robot, 50% Obstacle Coverage

(a)

BFMT

FMT

0 10 20 30 40

2.0

2.2

2.4

2.6

2.8

3.0

Execution Time s

SolutionCost

10D Point Robot, 50 Obstacle Coverage

(b)

Fig. 5: FMT∗ and BFMT∗ comparison for 5D and 10D clutteredhypercubes (50% coverage; all success rates were near 100%).

VI. CONCLUSION

In this paper, we presented a bi-directional, sampling-based,asymptotically-optimal motion planning algorithm namedBFMT∗, for which we rigorously proved its optimality andcharacterized its convergence rate – arguably a first in thefield of bidirectional sampling-based planning. Numericalexperiments in Rd, SE(2), and SE(3) revealed that BFMT∗tends to an optimal solution at least as fast as its state-of-the-art counterparts, and in some cases significantly faster.

Future efforts will study extension of BFMT∗ to dynamicenvironments through lazy re-evaluation (leveraging its tree-like forward and reverse path structures) in a way that reusesprevious results as much as possible. Maintaining bounds onrun-time performance and solution quality in time-varyingenvironments will remain the greatest challenges. Ultimately,we hope that BFMT∗ will enable fast, easy-to-implementre-planning with proven performance guarantees, analogousto planning in static environments as we have shown here.

REFERENCES

[1] S. Lavalle, Planning Algorithms. Cambridge University Press, 2006.

[2] L. Kavraki, P. Svestka, J.-C. Latombe, and M. Overmars, “Probabilisticroadmaps for path planning in high-dimensional configuration spaces,”IEEE Transactions on Robotics and Automation, vol. 12, no. 4, pp.566 –580, 1996.

[3] D. Hsu, J. C. Latombe, and R. Motwani R, “Path planning in expan-sive configuration spaces,” International Journal of ComputationalGeometry & Applications, vol. 9, pp. 495–512, 1999.

[4] J. M. Phillips, N. Bedrossian, and L. E. Kavraki, “Guided expansivespaces trees: A search strategy for motion- and cost-constrained statespaces,” in Proc. IEEE Conf. on Robotics and Automation, vol. 4, 2004,pp. 3968–3973.

[5] S. M. LaValle and J. J. Kuffner, “Randomized Kinodynamic Planning,”International Journal of Robotics Research, vol. 20, no. 5, pp. 378–400,2001.

[6] S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimalmotion planning,” International Journal of Robotics Research, vol. 30,no. 7, pp. 846–894, 2011.

[7] L. Janson and M. Pavone, “Fast Marching Trees: a fast marchingsampling-based method for optimal motion planning in many dimen-sions,” in International Symposium on Robotics Research, 2013.

[8] L. Janson, E. Schmerling, A. Clark, and M. Pavone, “Fast marching tree:a fast marching sampling-based method for optimal motion planning inmany dimensions,” International Journal of Robotics Research, 2014,Provisionally Accepted. Available at http://web.stanford.edu/∼pavone/papers/Janson.Pavone.IJRR14.pdf.

[9] Y. K. Hwang and N. Ahuja, “Gross Motion Planning – a Survey,” ACMComput. Surv., vol. 24, no. 3, pp. 219–291, Sep. 1992.

[10] I. Pohl, Bi-directional and heuristic search in path problems. Depart-ment of Computer Science, Stanford University., 1969, no. 104.

[11] M. Luby and P. Ragde, “A bidirectional shortest-path algorithm withgood average-case behavior,” Algorithmica, vol. 4, no. 1-4, pp. 551–567,1989.

[12] A. Goldberg, H. Kaplan, and R. Werneck, Reach for A*: EfficientPoint-to-Point Shortest Path Algorithms, 2006, ch. 12, pp. 129–143.

[13] J. J. Kuffner and S. M. LaValle, “RRT-Connect: An efficient approachto single-query path planning,” in Proc. IEEE Conf. on Robotics andAutomation, San Francisco, CA, Apr. 2000, pp. 995–1001.

[14] G. Sanchez and J.-C. Latombe, “A single-query bi-directional proba-bilistic roadmap planner with lazy collision checking,” in InternationalJournal of Robotics Research. Springer, 2003, pp. 403–417.

[15] B. Akgun and M. Stilman, “Sampling heuristics for optimal motionplanning in high dimensions,” in IEEE/RSJ Int. Conf. on IntelligentRobots & Systems. IEEE, 2011, pp. 2640–2645.

[16] M. Jordan and A. Perez, “Optimal Bidirectional Rapidly-ExploringRandom Trees,” Aug. 2013, http://people.csail.mit.edu/aperez/obirrt/csailtech.pdf.

[17] E. Schmerling, L. Janson, and M. Pavone, “Optimal sampling-based motion planning under differential constraints: thedriftless case,” 2014, submitted to ICRA ’15. Available athttp://arxiv.org/abs/1403.2483/.

[18] O. Salzman and D. Halperin, “Asymptotically Near-Optimal MotionPlanning using Lower Bounds on Cost,” http://arxiv.org/abs/1403.7714,2014.

[19] I. A. Sucan, M. Moll, and L. E. Kavraki, “The Open Motion PlanningLibrary,” IEEE Robotics and Automation Magazine, vol. 19, no. 4, pp.72–82, Dec. 2012, http://ompl.kavrakilab.org.