Abstract-Bi-directional search is a widely used strategy to increase the success and convergence rates of sampling-based motion planning algorithms. Yet, few results are available that merge both bi-directional search and asymptotic optimality into existing optimal planners, such as PRM * , RRT * , and FMT * . The objective of this paper is to fill this gap. Specifically, this paper presents a bi-directional, sampling-based, asymptoticallyoptimal algorithm named Bi-directional FMT * (BFMT * ) that extends the Fast Marching Tree (FMT * ) algorithm to bidirectional search while preserving its key properties, chiefly lazy search and asymptotic optimality through convergence in probability. BFMT * performs a two-source, lazy dynamic programming recursion over a set of randomly-drawn samples, correspondingly generating two search trees: one in cost-tocome space from the initial configuration and another in costto-go space from the goal configuration. Numerical experiments illustrate the advantages of BFMT * over its unidirectional counterpart, as well as a number of other state-of-the-art planners. Motion planning is the computation of paths that guide systems from an initial configuration to a set of goal configuration (s) around nearby obstacles, while possibly optimizing an objective function. The problem has a long and rich history in the field of robotics, and many algorithmic tools have been developed; we refer the interested reader to [1] and references therein. Arguably, sampling-based algorithms are among the most pervasive, widespread planners available in robotics, including the Probabilistic Roadmap algorithm (PRM) [2], the Expansive Space Trees algorithm (EST) [3] , [4] , and the Rapidly-Exploring Random Tree algorithm (RRT) [5] . Since their development, efforts to improve the "quality" of paths led to asymptotically-optimal (AO) variants of RRT and PRM, named RRT * and PRM * , respectively, whereby the cost of the returned solution converges almost surely to the optimum as the number of samples approaches infinity [6] Though such bi-directional versions of RRT and PRM are probabilistically complete, they do not enjoy optimality guarantees. The next logical step in the quest for fast planning algorithms is the design of bi-directional, samplingbased, asymptotically-optimal algorithms. To the best of our knowledge, the only available results in this context are [18] and the unpublished work [19] , both of which discuss bidirectional implementations of RRT * . Neither work, however, provides a mathematically-rigorous proof of asymptotic optimality starting from first principles. Accordingly, the objective of this paper is to propose and rigorously analyze such an algorithm. Statement of Contributions: This paper introduces the Bidirectional Fast Marching Tree (BFMT * ) algorithm. 1 To the best of the authors' knowledge, this is the first treebased, asymptotically-optimal bi-directional sampling-based planner. BFMT * extends FMT * to bi-directional search and essentially performs a "lazy," bi-directional dynamic programming recursion over a set of probabilistically-drawn samples in the free configuration space. The contribution of this paper is threefold. First, we present the BFMT * algorithm in Section III. Second, we rigorously prove the asymptotic optimality of BFMT * (under the notion of convergence in probability) and characterize its convergence rate in Section IV. We note that the convergence rate of FMT * in [11] is proved only for obstacle-free configuration spaces, while we generalize that result to allow for the presence of obstacles. Finally, we perform numerical experiments in Section V across a number of planning spaces that suggest BFMT * converges to an optimal solution at least as fast as FMT * , PRM * , and RRT * , and sometimes significantly faster. Let X be a d-dimensional configuration space, and let X obs be the obstacle region, such that X \ X obs is an 1 The asterisk * , pronounced "star", is intended to represent asymptotic optimality much like for the RRT * and PRM * algorithms.
BFMT Ã , which is a bidirectional version of FMT Ã , was introduced by REF .
An Asymptotically-Optimal Sampling-Based Algorithm for Bi-directional Motion Planning
Proceedings of the ... IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE/RSJ International Conference on Intelligent Robots and Systems
Proceedings of the ... IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE/RSJ International Conference on Intelligent Robots and Systems
Computer Science Medicine