  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+\rho})$, where
$n$ is the number of sampled points, $d$ is the dimension of the configuration
space, and $\rho$ 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.
