PaperlayerView exact revision on arXiv

Source-linked AI summary

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

Lucas Janson, Edward Schmerling, Ashley Clark, Marco Pavone

arXiv:1306.3532v4cs.RO

TL;DR

The paper addresses the challenge of rapidly finding optimal motion-planning paths in high-dimensional configuration spaces. It introduces FMT*, a lazy dynamic-programming method over random samples, and proves asymptotic optimality with convergence-rate bounds under convergence in probability. Experiments report faster convergence and better solutions than PRM* and RRT*, particularly in high dimensions and when collision-checking is expensive.

  • Problem

    High-dimensional motion planning needs sampling-based algorithms with rapid convergence and sound performance guarantees.

  • Method

    FMT* performs a forward lazy dynamic-programming recursion over predetermined probabilistically drawn samples to grow a tree in cost-to-arrive space.

  • Results

    FMT* is asymptotically optimal, has a convergence-rate bound of O(n^-1/d+ρ) under selected conditions, and outperforms PRM* and RRT* in reported experiments.

  • Takeaways & Limitations

    FMT* is especially promising for high-dimensional planning problems and settings where obstacle collision-checking is expensive.

  • Takeaways & Limitations

    FMT* and the Fast Marching Method differ fundamentally: FMT* applies the Bellman principle over a randomized grid rather than using upwind schemes on orthogonal grids or triangulated domains.

Abstract

from arXiv · show

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.

1 Introduction

FMT* is introduced as a probabilistic motion-planning algorithm for high-dimensional configuration spaces, designed to improve convergence toward optimal solutions while reducing obstacle collision-checks. It uses a forward, lazy dynamic-programming recursion over predetermined random samples and is analyzed using convergence in probability.

  • Motivation: Probabilistic sampling-based planning avoids explicitly constructing complex configuration spaces by probing them with random samples and collision detection.These methods are particularly relevant to high-dimensional planning, including high-degree-of-freedom robots and planning under uncertainty.
  • Method: FMT* combines graph construction and search in a forward dynamic-programming recursion over a predetermined set of probabilistically drawn samples.The recursion is tailored to disk-connected graphs and grows a tree of trajectories outward in cost-to-arrive space.
  • Method: FMT* lazily skips collision-checks when evaluating local connections, although this can produce suboptimal connections that become vanishingly rare under the algorithm’s analysis.The method is designed to reduce obstacle collision-checks, particularly in cluttered, high-dimensional environments.
  • Analysis: FMT* is analyzed as asymptotically optimal under convergence in probability, a framework suited to non-sequential algorithms such as FMT* and PRM*.This differs from the almost-sure convergence notion used for sequential algorithms such as RRT*.
  • Analysis: O(n^-1/d+ρ) is the reported convergence-rate bound for FMT* and PRM* under selected tuning parameters and configuration spaces.Here, n is the number of sampled points, d is configuration-space dimension, and ρ is arbitrarily small.

2 Problem Setup

The paper formulates motion planning as finding a minimum-cost feasible path from an initial state to a regular goal region through obstacle-free configuration space. Its setup also requires clearance conditions that support near-optimal feasible paths.

  • Configuration space: The configuration space is X = [0, 1]^d with integer dimension d ≥ 2, partitioned into obstacle and obstacle-free regions.The obstacle-free space is defined from the complement of the obstacle region, with the boundary included in the obstacle set.
  • Goal region: A regular goal region has a uniformly bounded local curvature condition: every boundary point lies on the boundary of a ball contained in the goal region.This regularity ensures that feasible near-optimal solutions can approach the goal boundary through sampled points.
  • Cost: The planning cost maps paths to nonnegative real numbers and is mainly taken to be Euclidean arc length, while more general costs are considered separately.The formulation allows extensions to costs that may not satisfy the triangle inequality.
  • Optimization objective: The optimal planning problem is to find a feasible path from x_init to X_goal minimizing its cost, or report failure when no feasible path exists.The target is a path σ* whose cost equals the minimum cost among all feasible paths.
  • Clearance: Strong δ-clearance means that a collision-free path remains at least distance δ from the obstacle region throughout its trajectory.The paper defines δ-robust feasibility through a sequence of feasible paths whose costs approach the optimum while their clearance parameters decrease toward zero.

3 The Fast Marching Tree Algorithm (FMT∗)

FMT* grows a path tree through a lazy dynamic-programming recursion over disk-connected samples, concurrently constructing and searching the graph. Its lazy collision checking reduces computation while preserving asymptotic optimality despite rare suboptimal connections.

  • High-Level Description: FMT* recursively processes a fixed sample set in increasing cost-to-arrive order to grow a tree of paths.The algorithm maintains unvisited, open, and closed sample sets while expanding the lowest-cost open node.
  • High-Level Description: FMT* connects samples within a connection radius and uses disk-graph structure for dynamic-programming shortest-path computation.Its input is a predetermined set of samples, and neighboring samples are defined by a distance bound.
  • High-Level Description: The lazy recursion selects the locally optimal obstacle-free parent, collision-checks only that connection, and defers blocked samples rather than testing every alternative.Successfully connected samples move from Vunvisited to Vopen; blocked samples remain for later consideration.
  • Basic Properties and Intuition: Lazy collision checking can create suboptimal connections, but such cases become vanishingly rare as the number of samples increases.A suboptimal connection requires four simultaneous geometric and cost conditions involving competing open nodes and an obstacle-blocked connection.
  • Conceptual Comparison with Existing AO Algorithms and Advantages of FMT*: FMT* returns the same solution or failure as PRM* without obstacles and converges to an optimal solution with obstacles as sample count grows.The obstacle-free result follows from dynamic programming over the induced disk graph; with obstacles, the lazy connections explain the possible cost difference.
  • Conceptual Comparison with Existing AO Algorithms and Advantages of FMT*: FMT* reduces costly collision checks relative to PRM* and has O(n log n) computational complexity over n samples.Its collision-check ratio relative to PRM* tends to zero as the number of samples grows.

4 Analysis of FMT∗

The analysis establishes asymptotic optimality for FMT∗ under robust-feasibility and goal-regularity assumptions, then derives convergence-rate and computational-complexity bounds. The convergence-rate result is obtained under a no-obstacle setting, while the expected computational savings concern collision checking.

  • Asymptotic optimality: FMT∗ is asymptotically optimal for δ-robustly feasible problems with ξ-regular goal regions.For every ε > 0, the probability that the returned cost exceeds (1 + ε)c∗ tends to zero as n approaches infinity.
  • Asymptotic optimality: The proof covers near-optimal paths using overlapping covering balls and shows that missing samples in the associated balls become asymptotically unlikely.The construction extends an optimal path into the goal region to ensure feasible solutions, then uses large and small balls to control path approximation.
  • Relation to PRM∗: The same asymptotic-optimality result applies to PRM∗, with a smaller connection-radius constant under this analysis.The stated reduction is expected to reduce runtime for a given number of nodes, especially in high dimensions.
  • Convergence rate: Under no obstacles, FMT∗ and PRM∗ have convergence-rate bound O(n^-1/d+ρ), where d is dimension and ρ is arbitrarily small.The bound applies for a specified tuning-parameter choice and is described as the first convergence-rate result for an optimal sampling-based motion-planning algorithm.
  • Convergence rate: The convergence rate is slow, especially in high dimensions, but decreases as a power of n rather than logarithmically.The bound’s tightness is not studied, and finite-sample terms may substantially affect or dominate the asymptotic term.
  • Computational complexity: FMT∗ requires O(n log(n)) expected operations and O(n log(n)) expected space, while making O(n) expected collision-check calls.PRM∗ makes O(n log(n)) expected edge-collision checks, so FMT∗ reduces this operation count asymptotically.

5 Extensions

FMT∗ retains asymptotic optimality across non-uniform sampling and broader cost models. These extensions modify sampling radii or neighbor searches while preserving the proof’s relevant cost structure.

  • Extension overview: The extensions cover non-uniform sampling, general cost functions, and variants using costs rather than Euclidean distance.The paper discusses the required algorithmic changes and argues that asymptotic optimality is retained.
  • Non-uniform sampling: Non-uniform sampling incorporates a density ϕ over Xfree, with the radius scaled by (1/ℓ)^1/d.Uniform sampling remains a natural choice when no outside knowledge of the optimal path is available.
  • Metric costs: Metric costs require mainly a modified Near function so nearby samples are searched using the metric dist.The asymptotic-optimality proof relies on additive costs satisfying the triangle inequality.
  • Line-integral costs: For non-metric line-integral costs, FMT∗ connects nodes using cost-optimal paths and increases the radius by a factor of fupper.The implementation also defines neighborhoods through path cost rather than Euclidean distance.
  • Approximate connections: Computing optimal paths for every line-integral connection can be costly, motivating precomputation or cost-weighted straight-line approximations.The paper studies the approximation strategy and argues that asymptotic optimality still holds.

5.3 FMT∗Using k-Nearest-Neighbors

k-nearest FMT∗ replaces a fixed cost radius with adaptive nearest-neighbor connections and retains the Bellman-recursion structure on a k-nearest-neighbor graph.

  • Variant: k-nearest FMT∗ seeks connections to k nearest neighbors instead of samples within a fixed cost radius.This makes the variant more adaptive to different obstacle spaces.
  • Implementation: The implementation uses mutual k_n-nearest neighbors for one Near operation and ordinary k_n-nearest neighbors for another.These changes apply to the corresponding Near calls in Algorithm 2.
  • Parameter choice: Choosing k_n = k_0 log(n), with k_0 > 3d e (1 + 1/d), specifies the neighbor-growth sequence used by the variant.The stated condition determines the number of neighbors as the sample count increases.
  • Dynamic programming: k-nearest FMT∗ repeatedly applies Bellman’s equation over a k-nearest-neighbor graph.The recursion is analogous to the disk-connected graph version.

6 Numerical Experiments and Discussion

The numerical study compares FMT∗ with RRT∗ and PRM∗, two state-of-the-art asymptotically optimal sampling-based planners representing tree and roadmap approaches.

  • Experimental comparison: The experiments compare FMT∗ against RRT∗ and PRM∗ using open-source, high-quality implementations.The comparison spans the principal roadmap and tree approaches within asymptotically optimal sampling-based planning.

6.1 Simulation Setup

The simulations compare FMT*, RRT*, and PRM* on standard rigid-body planning problems in SE(2), SE(3), and higher-dimensional Euclidean spaces using nearest-neighbor tuning and cost-versus-time analyses.

  • Test problems: The experiments compare FMT*, RRT*, and PRM* on standard rigid-body motion-planning problems from OMPL.app.The OMPL.app problems are posed in configuration spaces including SE(2) and SE(3).
  • Test problems: The sampled state-space dimension is three for SE(2) problems and six for SE(3) problems.
  • High-dimensional setup: Additional experiments use a robot and hyperrectangular obstacles in high-dimensional Euclidean spaces.These simulations were implemented in Julia to accommodate the studied FMT* extensions.
  • Parameterization: Nearest-neighbor counts use k_n = k_0 log(n), with algorithm-specific coefficients for RRT*, FMT*, and PRM*.The FMT* and PRM* coefficient is lower than the lower bound in the mathematical asymptotic-optimality guarantee.
  • Evaluation: Each setup reports cost versus time across simulation runs grouped by common sample counts, which represent different iteration or free-space sampling quantities by algorithm.The sample count is not necessarily the number of constructed graph nodes.

6.2 Comparison with Other AO Planning Algorithms

Across benchmark environments from SE(2) to 7D, FMT* generally reaches high-quality solutions faster than RRT* and PRM*, especially as dimension and obstacle complexity increase. Its main exception is the narrow-corridor Alpha puzzle, where performance depends on execution time and sampling.

  • SE(2) bug trap: FMT* reaches comparable-quality solutions about twice as fast as RRT* and ten times as fast as PRM* in the SE(2) bug trap.It also reaches nearly 100% success in about one second, compared with roughly five seconds for RRT* and 80% success for PRM* after 14 seconds.
  • SE(2) bug trap: FMT* and PRM* achieve nearly identical solution quality for a given sample count, while RRT* performs worse because of its greedy expansion.FMT* uses O(1) collision-checks per sample, compared with O(log(n)) for RRT* in this experiment.
  • SE(2) maze: FMT* reaches given solution qualities about twice as fast as RRT* and ten times as fast as PRM* in the SE(2) maze.All algorithms reach 100% success quickly, but FMT* remains the fastest.
  • SE(3) maze: In the SE(3) maze, FMT* reaches an average solution quality in under five seconds that RRT* and PRM* reach in about 20 and 70 seconds, respectively.FMT* also appears to improve faster than RRT* at 20 seconds, while both reach 100% completion in about three seconds.
  • Recursive mazes: In recursive mazes, FMT* reaches lower-cost solutions faster than RRT*, with the improvement increasing with dimension.RRT* finds no solution within the allotted time in 7D, whereas FMT* reaches 100% success in under one second in 5D.
  • Recursive mazes: FMT* remains substantially faster than PRM* in recursive mazes, but its relative advantage decreases with dimension to about a factor of two in 7D.Both algorithms achieve 100% success in approximately the same amount of time.
  • Alpha puzzle: The Alpha puzzle is the only tested problem where FMT* does not consistently outperform RRT*, although FMT* outperformed PRM* in all numerical tests.RRT* performs better at short execution times, while FMT* tends to find a higher-quality solution in the complementary, longer-time regime.
  • Alpha puzzle: Extremely narrow corridors can stop FMT* when no sample lies inside them, motivating adaptive sampling around tree leaves as a possible improvement.The proposed modification would sample new points when Vopen becomes empty.

6.3 In-Depth Study of FMT∗

Section 6.3 examines practical variants, tuning choices, heuristics, and cost settings for FMT*, showing how performance depends on environment structure and available computation.

  • FMT* and k-nearest FMT*: k-nearest FMT* can attempt connections to farther samples near obstacles, increasing collision checks but preserving connection opportunities where local density is low.This creates both a potential advantage near obstacles and a collision-checking disadvantage when distant neighbors lie across walls.
  • Tuning k-nearest FMT*: Small neighbor-scaling values quickly reach a fixed solution quality, whereas larger values improve more slowly but continue reducing cost for longer.The curves cross, so the preferred scaling factor depends on available time and sample count.
  • Simple heuristics: FMT* solutions in a complicated 5D recursive maze came within 10% of the optimum using fewer than 1000 samples after ADAPTIVE-SHORTCUT smoothing.The heuristic barely increases collision checks in this experiment.
  • General costs: k-nearest FMT* correctly selected a shorter route through a high-cost region rather than a longer route around it under a general cost function.The experiment used two-dimensional environments with non-constant cost density.
  • FMT* variants: k-nearest FMT* avoids the loss of available radial connections near obstacles by attempting a fixed number of neighbor connections.Radial FMT* may still be worth testing in predominantly open spaces.
  • Practical use: For general costs, FMT* is a standalone method that provably converges to the optimum; for metric costs, smoothing is recommended to address within-homotopy convergence.The paper characterizes FMT* as a fast homotopy finder whose solutions may plateau without smoothing.

7 Conclusions

The paper concludes that FMT* is asymptotically optimal and often converges faster than RRT* and PRM*, while retaining asymptotic optimality across several extensions.

  • Conclusion: FMT* is asymptotically optimal and appears to converge significantly faster than RRT* and PRM* across many challenging problem instances.The experiments cover varied dimensions and obstacle configurations, with especially prominent speedups in higher dimensions and expensive collision-checking settings.
  • Conclusion: The analysis uses convergence in probability to derive convergence-rate bounds, rather than relying only on almost-sure convergence.The paper presents this as a mathematical flexibility of its analysis.
  • Extensions: Non-uniform sampling, general costs, and k-nearest-neighbor connections are presented as extensions that retain asymptotic optimality.These variations broaden the algorithm's supported planning settings.

Appendix A: Proofs for Lemmas 4.2–4.4

The appendix proves feasibility and cost bounds for FMT* by constructing chains of sampled points along a robust path and analyzing their probabilistic coverage.

  • Feasibility: When the coverage event occurs, every relevant ball contains a sample and FMT* is guaranteed to return a feasible solution.The argument uses obstacle clearance and the search radius to ensure collision-free connections.
  • Path construction: The proof constructs points inside successive balls along a robust path and bounds distances between adjacent points by the connection radius.The construction separates center distances and ball radii to establish the required local connections.
  • Cost bound: The appendix bounds the returned cost by the length of a constructed sampled path multiplied by a factor κ(α, β, θ), plus a radius-dependent term.This connects the geometric path construction to the cost guarantee through equation (10).
  • Inductive argument: The cost argument proceeds inductively over the sampled path, considering whether predecessor nodes are open, connected, or already associated with finite cost-to-arrive.The four cases establish the inductive claim for each successive sample.
  • Probability bounds: Poissonization replaces the fixed sample count with a Poisson-distributed count, enabling independent occupancy arguments for disjoint balls.The proof then uses binomial and Poisson tail bounds to show the relevant failure probability vanishes.

Appendix B: Proof of Convergence Rate Bound

The appendix derives a convergence-rate bound by simplifying probabilistic inequalities and selecting proof parameters so the dominant asymptotic terms yield the final exponent.

  • Parameter choice: The convergence-rate bound differs slightly from the conference version because of a corrected typographical error.This is stated as an appendix note accompanying the proof.
  • Bound simplification: The asymptotic comparison is governed by the relative growth of the remaining terms involving n and n log(n).The proof uses (1−x/n)^n ≤ e^-x and observes that the first term becomes negligible as n and n log(n) diverge.
  • Asymptotic bound: The resulting upper bound concerns the probability that the returned cost exceeds (1 + ε)c∗.The proof explicitly states the asymptotic upper bound for P(c_n > (1 + ε)c∗).
  • Parameter choice: If η > 2(2d−1)^1/d −1, θ can be chosen sufficiently small for the required inequality to hold and improve the asymptotic rate.The proof then replaces θ with an arbitrarily small additive exponent parameter ρ.

Appendix C: Proof of Computational Complexity

The appendix bounds FMT*’s computational work by analyzing edge-cost computations, node considerations, collision checks, neighbor queries, and open-set operations.

  • Operation counts: The proof first establishes bounds on edge-cost computations and node considerations, which control the main repeated operations in Algorithm 2.The appendix introduces separate lemmas for edge-cost computations and node considerations before proving total complexity.
  • Operation counts: Nodes without nearby obstacles contribute at most n considerations, while obstacle-near nodes contribute O(n) considerations in expectation.Obstacle-near counts use the obstacle surface area and the radius scaling r_n ∈ O((log(n)/n)^1/d).
  • Total complexity: FMT* makes O(n log(n)) calls to Cost and O(n) calls to CollisionFree in expectation.CollisionFree is called exactly when a node is under consideration.
  • Total complexity: Neighbor sets are computed at most once per node, so substantive Near computations are bounded by n.The algorithm saves neighbor sets rather than recomputing them.
  • Total complexity: A binary min heap supports V_open additions and minimum deletions in O(n log(n)) total time.There are at most n operations of each type, with each operation costing O(log(card V_open)).

Appendix D: AO of FMT∗with Non-Uniform Sampling

The appendix extends asymptotic optimality to non-uniform sampling by reserving a uniform component of the sampling distribution and adjusting the connection radius.

  • Mixture sampling: A sampling density ϕ is decomposed into a uniform component of probability ℓ and a second distribution with probability 1−ℓ.The construction retains approximately nℓ uniformly sampled nodes.
  • Mixture sampling: Running FMT* on the approximately nℓ uniform samples preserves the asymptotic-optimality proof after increasing r_n by (1/ℓ)^1/d.The radius adjustment compensates for using only the uniform subset.
  • Measure-theoretic qualification: The density conditions are understood up to arbitrary sets of Lebesgue measure 0.The appendix notes that probability density functions are defined only up to such sets.

Appendix E: AO of FMT∗for General Costs

The appendix establishes asymptotic optimality for general costs under cost-ball and piecewise-Lipschitz conditions, while controlling approximation and triangle-inequality errors.

  • Metric costs: For metric costs, asymptotic optimality requires the unit cost-ball measure ζ_d to lie in (0, ∞).The proof also embeds the cost-ball within a Euclidean ball of radius r_outer.
  • Metric costs: The proof replaces length-based quantities with cost-based quantities while preserving conditions that ensure samples occur in the goal region.The required radius conditions are adjusted using r_outer and the goal-region parameter ξ.
  • Piecewise-Lipschitz costs: The general-cost argument uses approximating paths whose neighborhoods remain inside finitely many Lipschitz regions away from region boundaries.These conditions cover broad piecewise-defined cost functions.
  • Triangle-inequality control: Without a triangle inequality, the proof requires either local exactness or an approximation whose accumulated error over search balls vanishes as n increases.The argument uses an approximate triangle inequality within Lipschitz portions of the configuration space.
  • Straight-line connections: The straight-line variant incurs total cost penalty O(r_n), so it is asymptotically no worse than the optimal-path variant and is therefore asymptotically optimal.The penalty tends to zero because r_n → 0.

Appendix F: AO of k-nearest FMT∗

The appendix establishes asymptotic optimality for k-nearest FMT* by combining mutual-nearest-neighbor graph approximation, FMT* path-cost domination, and probabilistic neighbor bounds. The proof modifies the PRM* argument with an enlarged geometric construction and supports mutual-nearest-neighbor connections.

  • Proof structure: Three facts establish asymptotic optimality: mutual-nearest PRM* approximates feasible paths, FMT* matches their costs, and long edges become sufficiently short.The neighbor count uses k_n = k_0 log(n), with k_0 > 3de(1 + 1/d).
  • Geometric construction: The geometric construction triples the outer-ball radius, increasing its volume by 3^d and requiring the corresponding kPRM lower-bound adjustment.This enlargement ensures that samples in successive smaller balls become k-nearest neighbors despite points outside the enlarged ball.
  • Geometric construction: The original narrow-ball construction can fail because points in successive balls may be farther apart than nearby points just outside the outer ball.For arbitrarily small θ_1, the appendix explains why a sample in B_n,m+1 need not be among a sample in B_n,m’s k nearest neighbors.
  • Graph approximation: Mutual-k_n-nearest PRM* preserves the PRM* proof because samples in neighboring balls are mutual neighbors under the relevant event.The appendix states that the connectedness argument applies equally to mutual-k_n-nearest PRM* and k_n-nearest PRM*.
  • Cost domination: k_n-nearest FMT* returns a path no worse than any feasible mutual-nearest PRM* path whose nodes have obstacle-clear nearest-neighbor connections.The proof considers FMT* cost-to-arrive recursively along the path and uses collision-free connections plus the triangle inequality.
  • Probabilistic bound: The proof controls the probability that any sample’s ε_n-ball contains too few neighbors using a union bound and binomial neighbor counts.For ε_n < Υ, the neighbor count is binomial with parameters n − 1 and ε_n^d.
Loading 1306.3532v4…