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

中文翻译：

---

快速行进树：一种基于快速行进采样的多维度最优运动规划方法

在本文中，我们提出了一种新的基于概率采样的运动规划算法，称为快速行进树算法 (FMT*)。该算法专门针对解决高维配置空间中的复杂运动规划问题。该算法被证明是渐近最优的，并且比其最先进的算法（主要是 PRM* 和 RRT*）收敛到最优解的速度更快。FMT* 算法对预定数量的概率抽取样本执行“惰性”动态规划递归，以生成路径树，该路径树在到达成本空间中稳定向外移动。因此，该算法结合了单查询算法（主要是 RRT）和多查询算法（主要是 PRM）的特点，让人联想到求解 Eikonal 方程的 Fast Marching 方法。与之前基于几乎肯定收敛概念的分析方法不同，FMT* 算法是在概率收敛的概念下进行分析的：这种方法的额外数学灵活性允许收敛速度界限——该领域中的第一个基于最优采样的运动规划。具体来说，对于一定的调整参数和配置空间的选择，我们得到一个 O(n −1/d+ρ ) 阶收敛速度界限，其中 n 是采样点的数量，d 是配置空间的维度， ρ 是一个任意小的常数。我们继续证明 FMT* 的许多变体的渐近最优性，即当配置空间被非均匀采样时，当成本不是弧长时，当基于最近邻居的数量而不是固定的连接半径进行连接时。在一系列维度和障碍物配置上的数值实验证实了我们的理论和启发式论点，表明 FMT* 在给定的执行时间内返回比 PRM* 或 RRT* 更好的解决方案，尤其是在高维配置空间和场景中碰撞检查是昂贵的。