Informed anytime fast marching tree for asymptotically-optimal motion planning

In many applications, it is necessary for motion planning planners to get high-quality solutions in high-dimensional complex problems. In this paper, we propose an anytime asymptotically-optimal sampling-based algorithm, namely Informed Anytime Fast Marching Tree (IAFMT*), designed for solving motion planning problems. Employing a hybrid incremental search and a dynamic optimal search, the IAFMT* fast finds a feasible solution, if time permits, it can efficiently improve the solution toward the optimal solution. This paper also presents the theoretical analysis of probabilistic completeness, asymptotic optimality, and computational complexity on the proposed algorithm. Its ability to converge to a high-quality solution with the efficiency, stability, and self-adaptability has been tested by challenging simulations and a humanoid mobile robot.