A batch informed sampling-based algorithm for fast anytime asymptotically-optimal motion planning in cluttered environments

Practical applications favor anytime asymptotically-optimal algorithms that find and improve an initial solution toward the optimal solution as quickly as possible due to the algorithms may be terminated at any time. We present Batch-to-batch Informed Fast Marching Tree (BBI-FMT*), an anytime asymptotically-optimal sampling-based algorithm that is designed for solving complex motion planning problems. The proposed algorithm has the ability to fast find an initial low-cost solution by the batch sampling-based incremental search and the “lazy” optimal search, then it employs the batch informed sampling-based incremental search and the anytime optimal search to quickly improve the tree and achieve the optimal solution. The proposed anytime optimal search strategy integrates the “lazy” and “non-lazy” optimal search to efficiently improve the tree to the minimum-cost spanning tree in cluttered environments. This paper theoretically analyzes the proposed algorithm in depth and evaluates it by numerical experiments under a few challenging scenarios. The experimental results show that BBI-FMT* outperforms the state-of-the-art algorithms in the self-adaptability, robustness, convergence rate, and success rate of the planning. The proposed algorithm can be widely applied to intelligent robots with expert systems to improve the efficiency and stability of the motion planning and navigation modules which are the core modules in the expert systems.