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

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.

Appendix

Proof

(Proof of Lemma  1 ) To start, note that $\mathbb {P}(K^{\beta }_n \ge \alpha (M_n-1)) + \mathbb {P}(A_n^c) \ge \mathbb {P}(\{K^{\beta }_n \ge \alpha (M_n-1)\} \cup A_n^c) = 1 - \mathbb {P}(\{K^{\beta }_n < \alpha (M_n-1)\} \cap A_n)$, where the first inequality follows from the union bound and the second equality follows from De Morgan’s laws. Note that the event $\{K^{\beta }_n < \alpha (M_n-1)\} \cap A_n$ is the event that each $B_{n,m}$ contains at least one node, and more than a $1-\alpha$ fraction of the $B^{\beta }_{n,m}$ balls also contains at least one node.

When two nodes $x_i$ and $x_{i+1}$, $i \in \{1,\ldots ,M_n-2\}$, are contained in adjacent balls $B_{n,i}$ and $B_{n,i+1}$, respectively, their distance apart $\Vert x_{i+1} - x_i\Vert$ can be upper bounded by,

$$ \left\{ \begin{array}{ll} \frac{\theta r_n}{2+\theta } + \frac{\beta r_n}{2+\theta } + \frac{\beta r_n}{2+\theta } &{} : \quad \text {if}\, x_i \in B^{\beta }_{n,i} \text { and } x_{i+1} \in B^{\beta }_{n,i+1} \\ \frac{\theta r_n}{2+\theta } + \frac{\beta r_n}{2+\theta } + \frac{r_n}{2+\theta } &{} : \quad \text {if}\, x_i \in B^{\beta }_{n,i} \text { or } x_{i+1} \in B^{\beta }_{n,i+1} \\ \frac{\theta r_n}{2+\theta } + \frac{r_n}{2+\theta } + \frac{r_n}{2+\theta } &{} : \quad \text {otherwise,} \end{array}\right. $$

where the three bounds have been suggestively divided into a term for the distance between ball centers and a term each for the radii of the two balls containing the nodes. This bound also holds for $\Vert x_{M_n}-x_{M_n-1}\Vert$, although necessarily in one of the latter two bounds, since $B^{\beta }_{n,M_n}$ being undefined precludes the possibility of the first bound. Thus we can rewrite the above bound, for $i \in \{1,\ldots , M_n-1\}$, as $\Vert x_{i+1} - x_i\Vert \le \bar{c}(x_i) + \bar{c}(x_{i+1})$, where

$$\begin{aligned} \bar{c}(x_k) := \left\{ \begin{array}{ll} \frac{\theta r_n}{2(2+\theta )} + \frac{\beta r_n}{2+\theta } &{} : \, x_k \in B^{\beta }_{n,k}, \\ \frac{\theta r_n}{2(2+\theta )} + \frac{r_n}{2+\theta } &{} : \, x_k \notin B^{\beta }_{n,k}. \\ \end{array} \right. \end{aligned}$$

(3)

Again, $\bar{c}(x_{M_n})$ is still well-defined, but always takes the second value in Eq. (3) above. Let $L_{n,\alpha ,\beta }$ be the length of a path that sequentially connects a set of nodes $\{x_1 = x_{\text {init}}, x_2, \dots , x_{M_n}\}$, such that $x_m \in B_{n,m} \; \forall m \in \{1,\dots ,M_n\}$, and more than a $(1-\alpha )$ fraction of the nodes $x_1,\dots ,x_{M_n-1}$ are also contained in their respective $B^{\beta }_{n,m}$ balls. The length $L_{n,\alpha ,\beta }$ can then be upper bounded as follows

$$\begin{aligned} L_{n,\alpha ,\beta }&= \sum ^{M_n-1}_{k = 1} \Vert x_{k+1} - x_k\Vert \le \sum ^{M_n-1}_{k = 1} 2\bar{c}(x_k) - \bar{c}(x_1) + \bar{c}(x_{M_n})\nonumber \\&\le (M_n - 1)\frac{\theta r_n}{2+\theta } + \lceil (1-\alpha )(M_n-1) \rceil \frac{2\beta r_n}{2+\theta } + \lfloor \alpha (M_n-1) \rfloor \frac{2 r_n}{2+\theta } + \frac{(1-\beta ) r_n}{2+\theta }\nonumber \\&\le (M_n - 1) \, r_n \, \frac{\theta + 2\alpha + 2(1-\alpha )\beta }{2+\theta } + \frac{(1-\beta )r_n}{2+\theta } \le M_n \,r_n \frac{\theta + 2\alpha + 2\beta }{2+\theta } + \frac{r_n}{2+\theta }. \end{aligned}$$

(4)

In Eq. (4), $\lceil x \rceil$ denotes the smallest integer not less than x, while $\lfloor x \rfloor$ denotes the largest integer not greater than x. Furthermore, we can upper bound $M_n$ as follows,

$$\begin{aligned} c(\sigma _n')&\ge \! \!\sum _{k = 1}^{M_n-2} \Vert \sigma _n(\tau _{k+1}) \!- \!\sigma _n(\tau _k)\Vert +\Vert \sigma _n'(1) \!-\! \sigma _n(\tau _{M_n-1})\Vert \ge (M_n\! -\! 2) \frac{\theta r_n}{2+\theta } + \frac{r_n}{2(2+\theta )}\nonumber \\&= M_n \frac{\theta r_n}{2+\theta } + \left( \frac{1}{2} - 2\theta \right) \frac{r_n}{2+\theta } \ge M_n \frac{\theta r_n}{2+\theta }, \end{aligned}$$

(5)

where the last inequality follows from the assumption that $\theta < 1/4$. Combining Eqs. (4) and (5) gives

$$\begin{aligned} \begin{aligned} L_{n,\alpha ,\beta }&\le c(\sigma _n')\, \left( 1+\frac{2\alpha + 2\beta }{\theta } \right) + \frac{r_n}{2+\theta } = \kappa (\alpha ,\beta ,\theta )\, c(\sigma _n') + \frac{r_n}{2+\theta }. \end{aligned} \end{aligned}$$

(6)

We will now show that when $A_n$ occurs, $c_n$ is no more than the length of the path connecting any sequence of $M_n$ vertices tracing through the balls $B_{n,1},\dots ,B_{n,M_n}$ (this of course also implies $c_n < \infty$). Coupling this fact with Eq. (6), one can then conclude that the event $\{K^{\beta }_n < \alpha (M_n-1)\} \cap A_n$ implies that $c_n \le \kappa (\alpha ,\beta ,\theta )\, c(\sigma _n') + \frac{r_n}{2+\theta }$, which, in turn, would prove the lemma.

Let $x_1 = x_{\text {init}}$, $x_2 \in B_{n,2}$, $\dots$, $x_{M_n} \in B_{n,M_n} \subseteq \mathscr {X}_{\text {goal}}$. Note that the $x_i$’s need not all be distinct. The following property holds for all $m \in \{2, \dots , M_n-1\}$: $\Vert x_m - x_{m-1}\Vert \ \le \ \Vert x_m - \sigma _n(\tau _m)\Vert + \Vert \sigma _n(\tau _m) - \sigma _n(\tau _{m-1})\Vert + \Vert \sigma _n(\tau _{m-1}) - x_{m-1}\Vert \ \le _{ {O}} \ \frac{r_n}{2+\theta } + \frac{\theta r_n}{2+\theta } + \frac{r_n}{2+\theta } = r_n$. Similarly, one can write $\Vert x_{M_n} - x_{M_n-1}\Vert \ \le \ \frac{r_n}{2+\theta } + \frac{(\theta + 1/2)r_n}{2+\theta } + \frac{r_n}{2(2+\theta )} = r_n$. Furthermore, we can lower bound the distance to the nearest obstacle for $m \in \{2, \dots , M_n-1\}$ by $\inf _{w \in X_{\text {obs}}}\Vert x_m - w\Vert \ge \inf _{w \in X_{\text {obs}}}\Vert \sigma _n(\tau _m) - w\Vert - \Vert x_m - \sigma _n(\tau _m)\Vert \ge \frac{3+\theta }{2+\theta }r_n - \frac{r_n}{2+\theta } = r_n$, where the second inequality follows from the assumed $\delta _n$-clearance of the path $\sigma _n$. Again, similarly, one can write $\inf _{w \in X_{\text {obs}}}\Vert x_{M_n} - w\Vert \ge \inf _{w \in X_{\text {obs}}}||x_m - \sigma _n(1)\Vert - \Vert \sigma _n(1) - w\Vert \ge \frac{3+\theta }{2+\theta }r_n - \frac{r_n}{2+\theta } = r_n$. Together, these two properties imply that, for $m \in \{2,\dots , M_n\}$, when a connection is attempted for $x_m$, $x_{m-1}$ will be in the search radius and there will be no obstacles in that search radius. In particular, this implies that either the algorithm will return a feasible path before considering $x_{M_n}$, or it will consider $x_{M_n}$ and connect it. Therefore, $\text {FMT}^*$ is guaranteed to return a feasible solution when the event $A_n$ occurs. Since the remainder of this proof assumes that $A_n$ occurs, we will also assume $c_n < \infty$.

Finally, assuming $x_m$ is contained in an edge, let $c(x_m)$ denote the (unique) cost-to-come of $x_m$ in the graph generated by $\text {FMT}^*$ at the end of the algorithm, just before the path is returned. If $x_m$ is not contained in an edge, we set $c(x_m) = \infty$. Note that $c(\cdot )$ is well-defined, since if $x_m$ is contained in any edge, it must be connected through a unique path to $x_{\text {init}}$. We claim that for all $m \in \{2,\dots , M_n\}$, either $c_n \le \sum _{k = 1}^{m-1} \Vert x_{k+1} - x_k\Vert$, or $c(x_m) \le \sum _{k = 1}^{m-1} \Vert x_{k+1} - x_k\Vert$. In particular, taking $m = M_n$, this would imply that $c_n \le \min \{c(x_{M_n}),\sum _{k = 1}^{M_n-1} \Vert x_{k+1} - x_k\Vert \} \le \sum _{k = 1}^{M_n-1} \Vert x_{k+1} - x_k\Vert$, which, as argued before, would imply the claim.

The claim is proved by induction on m. The case of $m = 1$ is trivial, since the first step in the $\text {FMT}^*$ algorithm is to make every collision-free connection between $x_{\text {init}} = x_1$ and the nodes contained in $B(x_{\text {init}}; r_n)$, which will include $x_2$ and, thus, $c(x_2) = \Vert x_2 - x_1\Vert$. Now suppose the claim is true for $m-1$. There are four exhaustive cases to consider:

• 1. $c_n \le \sum _{k = 1}^{m-2}\Vert x_{k+1} - x_k\Vert$,

• 2. $c(x_{m-1}) \le \sum _{k = 1}^{m-2}\Vert x_{k+1} - x_k\Vert$ and $\text {FMT}^*$ ends before considering $x_m$,

• 3. $c(x_{m-1}) \le \sum _{k = 1}^{m-2}\Vert x_{k+1} - x_k\Vert$ and $x_{m-1} \in H$ when $x_m$ is first considered,

• 4. $c(x_{m-1}) \le \sum _{k = 1}^{m-2}\Vert x_{k+1} - x_k\Vert$ and $x_{m-1} \notin H$ when $x_m$ is first considered.

Case 1: $c_n \le \sum _{k = 1}^{m-2}\Vert x_{k+1} - x_k\Vert \le \sum _{k = 1}^{m-1}\Vert x_{k+1} - x_k\Vert$, thus the claim is true for m. Without loss of generality, for cases 2–4 we assume that case 1 does not occur.

Case 2: $c(x_{m-1}) < \infty$ implies that $x_{m-1}$ enters H at some point during $\text {FMT}^*$. However, if $x_{m-1}$ were ever the minimum-cost element of H, $x_m$ would have been considered, and thus $\text {FMT}^*$ must have returned a feasible solution before $x_{m-1}$ was ever the minimum-cost element of H. Since the end-node of the solution returned must have been the minimum-cost element of H, $c_n \le c(x_{m-1}) \le \sum _{k = 1}^{m-2}\Vert x_{k+1} - x_k\Vert \le \sum _{k = 1}^{m-1}\Vert x_{k+1} - x_k\Vert$, thus the claim is true for m.

Case 3: $x_{m-1} \in H$ when $x_m$ is first considered, $\Vert x_m - x_{m-1}\Vert \le r_n$, and there are no obstacles in $B(x_m; r_n)$. Therefore, $x_m$ must be connected to some parent when it is first considered, and $c(x_m) \le c(x_{m-1}) + \Vert x_m - x_{m-1}\Vert \le \sum _{k = 1}^{m-1} \Vert x_{k+1} - x_k\Vert$, thus the claim is true for m.

Case 4: When $x_m$ is first considered, there must exist $z \in B(x_m; r_n)$ such that z is the minimum-cost element of H, while $x_{m-1}$ has not even entered H yet. Note that again, since $B(x_m; r_n)$ intersects no obstacles and contains at least one node in H, $x_m$ must be connected to some parent when it is first considered. Since $c(x_{m-1}) < \infty$, there is a well-defined path $\mathscr {P} = \{v_1, \dots , v_q\}$ from $x_{\text {init}} = v_1$ to $x_{m-1} = v_q$ for some $q \in \mathbb {N}$. Let $w = v_j$, where $j = \max _{i \in \{1,\dots ,q\}} \{i : v_i \in H \text { when } x_m \text { is first considered}\}$. Then there are two subcases, either $w \in B(x_m; r_n)$ or $w \notin B(x_m; r_n)$. If $w \in B(x_m; r_n)$, then, $c(x_m) \ \le \ c(w) + \Vert x_m - w\Vert \ \le \ c(w) + \Vert x_{m-1} - w\Vert + \Vert x_m - x_{m-1}\Vert \ \le \ c(x_{m-1}) + \Vert x_m - x_{m-1}\Vert \le \sum _{k = 1}^{m-1} \Vert x_{k+1} - x_k\Vert$, thus the claim is true for m (the second and third inequalities follow from the triangle inequality). If $w \notin B(x_m; r_n)$, then, $c(x_m) \le c(z) + \Vert x_m - z\Vert \ \le \ c(w) + r_n \ \le \ c(x_{m-1}) + \Vert x_m - x_{m-1}\Vert \ \le \ \sum _{k = 1}^{m-1} \Vert x_{k+1} - x_k\Vert$, where the third inequality follows from the fact that $w \notin B(x_m, r_n)$, which means that any path through w to $x_m$, in particular the path $\mathscr {P} \cup {x_m}$, must traverse a distance of at least $r_n$ between w and $x_m$. Thus, in the final subcase of the final case, the claim is true for m. Hence, we can conclude that $c_n \le \sum _{k = 1}^{M_n-1} \Vert x_{k+1} - x_k\Vert$. As argued before, coupling this fact with Eq. (6), one can conclude that the event $\{K^{\beta }_n < \alpha (M_n-1)\} \cap A_n$ implies that $c_n \le \kappa (\alpha ,\beta ,\theta )\, c(\sigma _n') + \frac{r_n}{2+\theta }$, and the claim follows.   $\square$