Fast marching tree: A fast marching sampling-based method for optimal motion planning in many dimensions

TLDR

The algorithm blends single‑query and multiple‑query sampling methods and is inspired by the Fast Marching Method for Eikonal equations. The paper introduces the Fast Marching Tree algorithm to solve complex high‑dimensional motion‑planning problems. FMT* grows a tree via lazy dynamic programming on probabilistic samples, analyzes convergence in probability to provide rate bounds, and extends asymptotic optimality to non‑uniform sampling, non‑arc‑length costs, and neighbor‑based connections. FMT* achieves asymptotic optimality with a convergence rate bound of O(n^{-1/d+ρ}) and outperforms PRM* and RRT* in high‑dimensional, collision‑heavy scenarios, as confirmed by numerical experiments.

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*. The FMT* algorithm performs a "lazy" dynamic programming recursion on a predetermined number of probabilistically-drawn samples to grow a tree of paths, which moves steadily outward in cost-to-arrive space. As such, this algorithm combines features of both single-query algorithms (chiefly RRT) and multiple-query algorithms (chiefly PRM), and is reminiscent of 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 FMT* algorithm is analyzed under the notion of convergence in probability: the extra mathematical flexibility of this approach allows for convergence rate bounds-the first in the field of optimal sampling-based motion planning. Specifically, for a certain selection of tuning parameters and configuration spaces, we obtain a convergence rate bound of order O(n-1/d+ρ), where n is the number of sampled points, d is the dimension of the configuration space, and ρ is an arbitrarily small constant. We go on to demonstrate asymptotic optimality for a number of variations on FMT*, namely when the configuration space is sampled non-uniformly, when the cost is not arc length, and when connections are made based on the number of nearest neighbors instead of a fixed connection radius. Numerical experiments over a range of dimensions and obstacle configurations confirm our the-oretical 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.