Informed Anytime Fast Marching Tree for Asymptotically Optimal Motion Planning

Abstract

In many applications, it is necessary for motion planning planners to get high-quality solutions in high-dimensional complex problems. In this article, we propose an anytime asymptotically optimal sampling-based algorithm, namely, informed anytime fast marching tree (IAFMT <sup xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">*</sup> ), designed for solving motion planning problems. Employing a hybrid incremental search and a dynamic optimal search, the IAFMT <sup xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">*</sup> fast finds a feasible solution; if time permits, it can efficiently improve the solution toward the optimal solution. This article 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 efficiency, stability, and self-adaptability has been tested by challenging simulations and a humanoid mobile robot.