Focused Bidirectional Search Trees: A Bidirectional Optimal Fast Matching Method for Robot Path Planning

Robot path planning in high-dimensional continuous space is of great significance in robotics. The conventional bidirectional fast marching method utilizes brute-force search to explore the space, which is computationally expensive. Though improving the planning efficiency, heuristic-based bidirectional fast marching search results in a suboptimal path under a predetermined number of random samples. To address these issues, we propose Focused Bidirectional Fast Marching Trees (FBFMT*), developing a novel vertex exploring sequence to constrain the forward and backward searches within the brute-force searched regions. It enables the search trees to meet in the middle of the solution path, which guarantees the optimality under the current set of samples and searches faster than the bidirectional brute-force strategy. Extensive simulations in different scenarios validate the superiority of the proposed method, showing that FBFMT* can speed up the planning process and find a better solution. We also extend the proposed method to general conditions so that the forward and backward searches of FBFMT* can meet at any portion of the path. FBFMT* reveals that the heuristic-based bidirectional fast marching search strategy can be a general and efficient technique for robot path planning in high-dimensional continuous spaces.