An asymptotically-optimal sampling-based algorithm for Bi-directional motion planning

Joseph A. Starek, Javier V. Gómez, Edward Schmerling, Lucas Janson, Luis Moreno, Marco Pavone

TL;DR

This paper introduces Bi-directional Fast Marching Tree (BFMT), a novel algorithm that combines the efficiency of bi-directional search with the asymptotic optimality guarantees of advanced motion planners. BFMT significantly speeds up the process of finding optimal paths in complex environments compared to its unidirectional counterparts.

What Problem Does This Paper Address?

Motion planning is the fundamental problem of finding a collision-free path for a robot from a starting configuration to a goal configuration, often while optimizing for path length or time. Sampling-based motion planning algorithms, like RRT and PRM, are popular because they can handle high-dimensional spaces. However, these algorithms can be slow to converge to the optimal path. Bi-directional search, which simultaneously explores from both the start and goal, is known to accelerate planning. The challenge this paper addresses is how to combine the benefits of bi-directional search with asymptotic optimality – the property that the found path's cost converges to the true minimum as more samples are considered. Existing bi-directional planners often sacrifice optimality for speed, and existing optimal planners are typically unidirectional. This paper aims to bridge this gap by creating a bi-directional, asymptotically-optimal planner.

Methods

The core innovation is the Bi-directional Fast Marching Tree (BFMT*) algorithm. It extends the Fast Marching Tree (FMT) algorithm, which uses a "lazy" dynamic programming approach to efficiently find optimal paths. BFMT adapts FMT* to a bi-directional search by:

  • Two-Source Dynamic Programming: Instead of a single search tree growing from the start, BFMT* maintains two trees: one growing from the initial configuration (cost-to-come) and another from the goal configuration (cost-to-go).
  • Lazy Search: Similar to FMT, BFMT postpones collision checking for connections until they are locally optimal, significantly reducing the computational burden of collision detection. This "laziness" is crucial for maintaining asymptotic optimality.
  • Wavefront Propagation: Both trees expand their "wavefronts" (the frontier of nodes being considered for expansion) through the configuration space. The algorithm aims to have these wavefronts meet.
  • Asymptotic Optimality Guarantee: The paper provides a rigorous proof that BFMT* converges to the optimal path with probability 1 as the number of samples increases, even in the presence of obstacles. This is a key theoretical contribution.

Datasets

The paper does not introduce new datasets. Instead, it uses standard benchmarks from the Open Motion Planning Library (OMPL) for evaluation. These include:

  • SE(2) Bug Trap: A 2D environment with a convex polyhedral robot.
  • SE(2) Maze: Another 2D environment with a convex polyhedral robot.
  • SE(3) a-puzzle: A challenging 3D problem involving untangling non-convex loops.
  • Cluttered Unit Hypercubes: 5D and 10D environments with point-mass robots and randomly placed obstacles, used to test performance in higher dimensions.

Key Insights and Ideas

  • Bi-directional search is a powerful accelerator for optimal planners: The paper demonstrates that merging bi-directional search with an already efficient optimal planner like FMT* yields significant speedups without sacrificing optimality.
  • "Lazy" collision checking is key to efficiency and optimality: By delaying collision checks, BFMT* avoids many expensive computations while still guaranteeing convergence to the optimal solution. This is a critical trade-off that makes the algorithm practical.
  • Higher dimensions benefit most from bi-directionality: In high-dimensional spaces, the advantage of BFMT over its unidirectional counterpart (FMT) becomes exponentially more pronounced. This is because the bi-directional approach prevents one tree from growing excessively large, which is a major bottleneck in high dimensions.
  • Homotopy classes can temporarily affect average performance: In complex problems like the a-puzzle, the algorithm might initially find paths belonging to less efficient "homotopy classes" (different topological paths), leading to a temporary increase in average cost as sample counts grow. However, the asymptotic optimality ensures this is resolved in the limit.

Key Results

  • Significant speedup: BFMT consistently outperforms its unidirectional counterpart, FMT, in terms of execution time for a given number of samples across various scenarios. For example, in 5D and 10D hypercube problems, BFMT achieved convergence in roughly 0.5 and 1.4 seconds (5D), and 5 and 20 seconds (10D) respectively, while FMT took longer.
  • Comparable or better performance than RRT and PRM: BFMT generally achieves higher success rates and finds lower-cost solutions faster than RRT and PRM*, especially in challenging environments like the a-puzzle.
  • Rigorous proof of asymptotic optimality: The paper provides a formal mathematical proof that BFMT* converges to the optimal path in probability, generalizing previous results to include obstacle-filled spaces.
  • 100% success rates: In the tested scenarios, BFMT* achieved 100% success rates, indicating its robustness.

Limitations and Future Work

  • Limitations:
    • The paper focuses on static environments and arc-length cost functions.
  • While the "lazy" approach is efficient, it can lead to temporary suboptimal path choices in certain complex scenarios (e.g., the a-puzzle).
  • Future Work:
    • Extending BFMT* to handle dynamic environments by leveraging its tree structure for efficient re-planning.
  • Investigating more advanced sampling strategies, such as adaptive sampling or sample biasing, to further improve performance, especially in areas with narrow passages or traps.
  • Exploring parallelization of BFMT*, where each tree could be grown on a separate CPU, to further enhance convergence speed.

Summary updated

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, asymptotically-optimal algorithm named Bi-directional FMT* (BFMT*) that extends the Fast Marching Tree (FMT*) algorithm to bi-directional 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-to-come space from the initial configuration and another in cost-to-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.
BibTeX
@inproceedings{starek2015asymptotically,
  author = {Joseph A. Starek and Javier V. Gómez and Edward Schmerling and Lucas Janson and Luis Moreno and Marco Pavone},
  title = {{An asymptotically-optimal sampling-based algorithm for Bi-directional motion planning}},
  booktitle = {IEEE/RSJ International Conference on Intelligent Robots and Systems},
  year = {2015},
  url = {https://openreview.net/forum?id=hU7l6SgKDFU},
}

Loading related content...

Figure