Fast Marching Tree: a Fast Marching Sampling-Based Method for Optimal Motion Planning in Many Dimensions

Lucas Janson, Edward Schmerling, Ashley Clark, Marco Pavone

Seminal June 2013

TL;DR FMT* grows a tree of paths outward in cost-to-arrive over a fixed set of samples, collision-checking only the locally best edge, and is proven asymptotically optimal with the first convergence-rate bound for optimal sampling-based planners. On SE(2) benchmarks it reaches a given solution quality about 2x faster than RRT* and 10x faster than PRM*.

What they do

FMT* is an asymptotically optimal sampling-based motion planner for high-dimensional configuration spaces. It runs a “lazy” dynamic programming recursion over a fixed set of samples, growing a tree of paths steadily outward in cost-to-arrive, like the Fast Marching Method. The aim is far fewer collision checks than PRM* and RRT*.

How it works

Lazy one-step connections. Pop the lowest-cost open node from a heap. Each unvisited neighbor picks the open node that gives it the lowest cost-to-arrive, ignoring obstacles, and only that edge is collision-checked. If it collides, the sample waits for a later iteration.
Rare suboptimality. This misses a better parent only near obstacles, and the fraction of such samples goes to zero as n grows.
Analysis in probability. Proving optimality as convergence in probability yields a rate bound of order n^(-1/d+ρ) without obstacles. FMT* needs O(n log n) time but only O(n) collision checks in expectation, versus O(n log n) for PRM*.
Extensions that stay optimal: non-uniform sampling, general costs and k-nearest neighbors.

Results

SE(2) bug trap: similar solution quality in about half the time of RRT* and a tenth that of PRM. FMT nears 100% success in about 1 s, RRT* in about 5 s, while PRM* is at 80% after 14 s.
SE(3) maze: in under 5 s FMT* reaches a mean cost below what RRT* and PRM* reach in about 20 s and 70 s.
7D recursive maze: RRT* never found a solution in the allotted time; FMT* reaches low cost in about half the time of PRM*.
With ADAPTIVE-SHORTCUT smoothing, FMT* gets within 10% of optimal with fewer than 1000 samples in a 5D maze.

Limitations

FMT* runs on a fixed sample count, so it is not anytime. In the Alpha puzzle’s narrow corridors it stops expanding when no sample lands inside, and reaches full success more slowly than RRT*. The neighbor scale and sample count need tuning per problem. The rate bound assumes no obstacles, and all experiments are simulated without differential constraints.

Figures

From Figure 1: FMT* grows its tree steadily outward in cost-to-arrive space, shown after the first 2,500 edges in a 2... — From Figure 1: FMT* grows its tree steadily outward in cost-to-arrive space, shown after the first 2,500 edges in a 2D environment.

From Figure 3: a case where FMT* makes a suboptimal connection: x picks u2, whose edge is blocked by an obstacle, and... — From Figure 3: a case where FMT* makes a suboptimal connection: x picks u2, whose edge is blocked by an obstacle, and never reconsiders its better parent u1. Such cases become vanishingly rare as the number of samples grows.

From Figure 5: the SE(3) maze benchmark from OMPL.app, with the rigid body at its start and goal poses.

From Figure 8: solution cost versus execution time in the SE(3) maze for FMT*, PRM* and RRT*.

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 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 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.

Summaries are written automatically from each paper's text. Figures and videos come from the paper and its project page; all credit goes to the authors.

Matt Strong