Informed anytime Bi-directional Fast Marching Tree for optimal motion planning in complex cluttered environments

Abstract

Practical applications favor anytime asymptotically-optimal algorithms that find a feasible solution and improve it toward the optimal solution within limited time. This paper proposed Informed Anytime Bi-directional Fast Marching Tree (IABFMT*), an anytime asymptotically-optimal sampling-based algorithm that combines the strength of BFMT* (bi-directional variant of FMT*) and IAFMT* (an anytime variant of FMT*). IABFMT* performs a bi-directional “lazy” search to efficiently find a feasible solution and quickly improve it towards optimal. In addition, we implemented a graph pruning method to simplify the planning problem and heuristic cost evaluation to reject samples, which avoided a lot of unnecessary computations and improved the convergence rate. Different simulations in OMPL have been carried out to demonstrate the superior efficiency of IABFMT* in comparison with other state-of-the-art algorithms in complex cluttered environments.