Fast Marching Trees: A Fast Marching Sampling-Based Method for Optimal Motion Planning in Many Dimensions

Lucas Janson, Marco Pavone

Abstract: In this paper we present a novel probabilistic sampling-based motion planning algorithm called the Fast Marching Tree algorithm (FMT $$^*$$ ). The algorithm is specifically aimed at solving complex motion planning problems in high-dimensional configuration spaces. This algorithm is proven to be asymptotically optimal and is shown to converge to an optimal solution faster than its state-of-the-art counterparts, chiefly PRM $$^*$$ and RRT $$^*$$ . An additional advantage of $$\text {FMT}^*$$ is that it builds and maintains paths in a tree-like structure (especially useful for planning under differential constraints). The $$\text {FMT}^*$$ algorithm essentially performs a “lazy” dynamic programming recursion on a set of probabilistically-drawn samples to grow a tree of paths, which moves steadily outward in cost-to-come space. As such, this algorithm combines features of both single-query algorithms (chiefly RRT) and multiple-query algorithms (chiefly PRM), and is conceptually related to the Fast Marching Method for the solution of eikonal equations. As a departure from previous analysis approaches that are based on the notion of almost sure convergence, the $$\text {FMT}^*$$ algorithm is analyzed under the notion of convergence in probability: the extra mathematical flexibility of this approach allows for significant algorithmic advantages and provides convergence rate bounds—a first in the field of optimal sampling-based motion planning. Numerical experiments over a range of dimensions and obstacle configurations confirm our theoretical and heuristic arguments by showing that FMT $$^*$$ , for a given execution time, returns substantially better solutions than either PRM $$^*$$ or RRT $$^*$$ , especially in high-dimensional configuration spaces and in scenarios where collision checking is expensive.