PaperlayerView exact revision on arXiv

Source-linked AI summary

Incremental Sampling-based Algorithms for Optimal Motion Planning

Sertac Karaman, Emilio Frazzoli

arXiv:1005.0416v1cs.RO

TL;DR

Incremental sampling-based planners had feasibility guarantees but lacked theoretical guarantees on solution quality. This paper analyzes RRT and introduces RRG and RRT* using random-geometric-graph connections. It proves that RRT is almost surely suboptimal, while RRG and RRT* are asymptotically optimal with comparable asymptotic computational complexity.

  • Problem

    Incremental sampling-based planners had theoretical feasibility guarantees, but the quality of their returned solutions lacked established theoretical bounds.

  • Method

    The paper analyzes RRT and RRG and introduces RRT*, which rewires tree connections to inherit RRG’s optimality while retaining a tree structure.

  • Results

    RRT converges almost surely to a suboptimal solution, whereas RRG and RRT* converge almost surely to an optimum with asymptotic computational complexity essentially matching RRT.

  • Takeaways & Limitations

    RRG and RRT* provide asymptotically optimal incremental planning while preserving the graph or tree structures useful for online planning.

Abstract

from arXiv · show

During the last decade, incremental sampling-based motion planning algorithms, such as the Rapidly-exploring Random Trees (RRTs) have been shown to work well in practice and to possess theoretical guarantees such as probabilistic completeness. However, no theoretical bounds on the quality of the solution obtained by these algorithms have been established so far. The first contribution of this paper is a negative result: it is proven that, under mild technical conditions, the cost of the best path in the RRT converges almost surely to a non-optimal value. Second, a new algorithm is considered, called the Rapidly-exploring Random Graph (RRG), and it is shown that the cost of the best path in the RRG converges to the optimum almost surely. Third, a tree version of RRG is introduced, called the RRT$^*$ algorithm, which preserves the asymptotic optimality of RRG while maintaining a tree structure like RRT. The analysis of the new algorithms hinges on novel connections between sampling-based motion planning algorithms and the theory of random geometric graphs. In terms of computational complexity, it is shown that the number of simple operations required by both the RRG and RRT$^*$ algorithms is asymptotically within a constant factor of that required by RRT.

I. INTRODUCTION

Motion planning seeks feasible, collision-free robot trajectories, but complete methods can be computationally impractical. Incremental sampling-based algorithms address online planning efficiently, motivating this paper’s analysis of solution quality and asymptotic optimality.

  • Motivation: Motion planning finds control inputs that drive a robot from an initial state to a goal while obeying environmental constraints.A complete algorithm returns a solution when one exists and failure otherwise.
  • Motivation: Complete planners faced severe computational complexity, including doubly exponential time for one algebraic approach and PSPACE-hardness for the piano movers problem.These results discouraged practical use of complete planning methods.
  • Related approaches: Resolution-complete and other practical planners improved performance but were mostly limited to state spaces with up to five dimensions.Sampling-based methods subsequently attracted substantial attention for complex planning problems.
  • Sampling-Based Algorithms: Incremental sampling-based planners support online applications because they can terminate after finding a solution and reuse or improve trajectories after environmental changes.These properties provide computational savings that enable online real-time implementations.
  • Research gap: RRTs had guarantees for feasibility, including probabilistic completeness and exponential failure-probability decay, but the quality of their returned solutions lacked theoretical analysis.The paper addresses this gap by analyzing RRT, RRG, and RRT* optimality.
  • Contributions: The paper connects incremental sampling-based planning with random geometric graphs to obtain asymptotically optimal algorithms whose computational complexity closely matches nearest-neighbor lower bounds.RRT* preserves a tree structure while inheriting RRG’s asymptotic optimality.

B. Problem Formulation

The paper formalizes feasible and optimal path planning in bounded obstacle-free state spaces, then defines incremental sampling procedures that grow RRT and RRG graphs through collision-free extensions.

  • B. Problem Formulation: The feasibility problem asks for a collision-free path from xinit to Xgoal, reporting failure when no such path exists.The state space is a bounded connected open subset of R^d with d ≥ 2.
  • B. Problem Formulation: The optimality problem asks for a feasible path with minimum value under a cost function defined on collision-free paths.The optimal path is denoted σ* and may lie in the closure of Xfree.
  • III. ALGORITHMS: RRT and RRG are incremental sampling-based algorithms that begin at xinit and repeatedly sample states in Xfree before extending the maintained graph.Each sampling-and-extension step is one iteration.
  • III. ALGORITHMS: Steer moves toward a target while limiting the extension length to a prescribed η, and Nearest selects the closest graph vertex using Euclidean distance.These procedures provide the basic extension operations for both algorithms.
  • III. ALGORITHMS: Near returns a collection of vertices close to a queried point, generalizing Nearest; its neighborhood radius uses a logarithmic sample-size term and a constant γ.The supplied passage specifies the ball-volume expression as min{γ log n / n, ζd ηd}.
  • III. ALGORITHMS: ObstacleFree accepts an extension exactly when the line segment between two states remains inside Xfree.Only collision-free extensions are added as edges, with terminal points added as vertices.
  • III. ALGORITHMS: RRT extends only the nearest vertex, whereas RRG additionally extends vertices returned by Near after a successful nearest-vertex extension.Both algorithms share the main loop, but RRT maintains a tree while RRG can maintain vertices with multiple incoming neighbors.
  • III. ALGORITHMS: The steering procedure is widely used and the analysis also extends to Rapidly-exploring Random Dense Trees, while deterministic sampling is not addressed.The latter scope boundary is explicitly deferred to cited prior work.

IV. ANALYSIS

The feasibility analysis shows that RRG inherits RRT’s probabilistic completeness and exponential failure decay. The algorithms are modeled over random sample sequences and compared through their discovered paths.

  • Feasibility: RRG inherits RRT’s probabilistic completeness and exponential decay of failure probability as the number of samples increases.Thus, RRT and RRG have the same asymptotic performance on feasibility.
  • Probabilistic model: The analysis assumes random sampling, models the sample space as Xfree^∞, and represents graph vertices and edges as functions of infinite sample sequences.The deterministic-sampling case is explicitly excluded.
  • Feasibility: The maintained RRT paths are essentially a subset of the paths discovered by RRG at the same iteration.This relationship supports transferring feasibility guarantees from RRT to RRG.
  • Feasibility: Probabilistic completeness means finding a feasible path with probability approaching one as the iteration count approaches infinity.The RRT’s connected tree always contains a collision-free path from xinit to every tree vertex.
  • Feasibility: Under an attraction-sequence assumption, the probability that RRT fails despite an existing solution decays exponentially fast.The rate depends on the attraction sequence and its minimum-volume set.
  • Feasibility: Faster convergence is associated with shorter attraction sequences and larger minimum-volume sets, corresponding to environments without narrow passages and with good visibility.These are environmental conditions supporting the stated convergence behavior.
  • Feasibility: Theorem 6 combines the path-subset lemma with RRT feasibility theorems to establish corresponding limiting probability-one results for RRG.The supplied theorem fragments state both feasibility convergence and exponential failure decay.

B. Asymptotic Optimality

The RRT converges almost surely to a suboptimal solution under mild technical assumptions, whereas the RRG converges almost surely to the optimum.

  • Almost Sure Suboptimality of the RRT: The RRT’s best-path cost converges to a random variable, but not to the optimal cost, under Assumptions 7–9.These assumptions concern zero-measure optimal paths, absolutely continuous sampling, and cost monotonicity.
  • Almost Sure Suboptimality of the RRT: The RRT converges to a suboptimal solution with probability one.The result follows because the limiting best-path cost is strictly greater than c∗ almost surely.
  • Implications: Running multiple RRT instances amounts to drawing multiple samples of the RRT’s limiting cost random variable.This provides the paper’s stated explanation for the effectiveness of multiple-RRT approaches.

2) Almost Sure Optimality of the RRG:

The RRG achieves asymptotic optimality while adding only logarithmic-scale overhead in key primitive operations, leaving its expected simple-operation complexity within a constant factor of RRT.

  • Almost Sure Optimality of the RRG: The RRG’s minimum-cost path converges to the optimum almost surely under its stated cost, obstacle-spacing, and sampling assumptions.The connection parameter must satisfy γ > γL when d ≥ 2.
  • Primitive Procedures: Sample, Steer, and Nearest are called equally often by both algorithms, whereas Near and ObstacleFree call counts differ.RRG calls Near at most once and ObstacleFree at least once per iteration; RRT does not call Near.
  • Computational Complexity: The RRG incurs order log n additional ObstacleFree calls and one Near call, while otherwise matching RRT primitive-call scaling.Here n denotes the number of vertices in the RRG.
  • Computational Complexity: The expected number of simple operations performed by RRG is asymptotically within a constant factor of RRT.The comparison counts elementary operations such as comparisons, additions, and multiplications rather than absolute runtime.
  • Computational Complexity: RRT requires at least order log(N_i) expected steps per iteration, while RRG requires at most order log(N_i) expected steps.These bounds assume the specified approximate-nearest-neighbor implementation in fixed dimensions.

D. On the efficient construction of Probabilistic RoadMaps

The paper’s complexity results suggest a locally connected PRM variant that retains probabilistic completeness while reducing expected construction cost from quadratic to O(n log n).

  • Standard PRM: Standard PRM construction requires O(n^2) calls to Steer and ObstacleFree when checking every pair of samples.PRM connects samples when the Steer path between them lies in obstacle-free space.
  • Modified PRM: Connecting each vertex only to vertices within a ball of volume γL_n could achieve probabilistic completeness with O(n log n) expected computational cost.The paper leaves pursuing this modified PRM direction to future work.

V. A TREE VERSION OF THE RRG ALGORITHM

RRT* is introduced as a tree version of RRG that preserves asymptotic optimality and computational efficiency while retaining a tree structure. Its Extend procedure selects lower-cost connections and rewires nearby vertices, and the analysis establishes probabilistic completeness with exponentially decreasing failure probability.

  • Motivation and contribution: RRT* preserves RRG’s asymptotic optimality and computational efficiency while maintaining a tree structure.The tree structure is advantageous for differential constraints and modeling errors.
  • Algorithm: RRT* differs from RRT and RRG primarily through its handling of the Extend procedure.The algorithm tracks the accumulated cost of each vertex from xinit.
  • Algorithm: RRT* connects each new vertex through the nearby vertex with minimum accumulated cost, then rewires nearby vertices reachable at lower cost.Rewiring is performed when routing through xnew reduces a neighbor’s cost.
  • Theoretical guarantees: The analysis asserts that RRT* is probabilistically complete and that its failure probability decays exponentially.These properties follow from Lemma 20 and Theorem 6 according to the cited theorem statement.

C. Asymptotic Optimality

Theoretical and experimental results show that RRT converges to suboptimal solutions, while RRT* converges to optimal solutions almost surely. RRT* preserves a tree structure and has asymptotic computational complexity within a constant factor of RRT.

  • Theory: Theorem 22 establishes that the minimum-cost path in RRT* converges to the optimal cost c* under the stated assumptions.
  • Experiments: In obstacle-free experiments, RRT converges to a cost close to twice the optimum, whereas RRT* converges to the optimum.
  • Experiments: In cluttered environments, all RRT* runs converge to the optimum, while RRT reaches about 1.5 of the optimal solution on average.
  • Experiments: RRT* improves paths over time and can discover a different homotopy class that considerably reduces the cost of reaching the target.
  • Computational complexity: The running-time ratio of RRT* to RRT converges to a constant value as iterations increase.
  • Theory: RRT* preserves asymptotic optimality while maintaining a tree structure rather than a graph.

Proof of Theorem 10

The proof establishes that RRT cannot converge asymptotically to the optimum under the paper’s assumptions: it almost surely never constructs an optimal path at finite time and fails a necessary condition for convergence.

  • RRT constructs an optimal path at any finite iteration with probability zero.
  • Asymptotic convergence to the optimum would require the initial state to be selected for extension infinitely often, with probability one.
  • Therefore, the RRT’s best-path cost converges almost surely to a value strictly greater than the optimum.The theorem follows by combining the necessary-condition lemma with the zero-probability result for infinitely many root extensions.
  • The proof shows that RRT extends its root node infinitely many times with probability zero.
  • For each cone covering a neighborhood of the initial state, the probabilities of repeated root extensions are summable when d ≥ 2.The argument bounds the relevant regions by balls whose volume scales as z^d, then applies the Borel-Cantelli lemma.

Outline of the proof:

The RRG optimality proof approximates an optimal path by increasingly obstacle-clear paths, covers each approximation with overlapping balls, and shows that RRG eventually connects samples through those balls.

  • The proof constructs paths σ_i inside expanding subsets X_i that remain at least λ_i from obstacles and converge to the optimal path.The clearance sequence satisfies λ_i = αr_i, while X_i converges to X_free; the paths are assembled by replacing near-obstacle segments.
  • Each σ_i is covered by overlapping equal-radius balls chosen small enough to lie entirely in the obstacle-free region.The ball radius is q_i := λ_i(1+θ_1), and consecutive centers are separated by θ_1q_i.
  • With probability one, RRG places a node in every ball for infinitely many i, producing paths whose costs converge to c∗.The chosen q_i also ensures that consecutive sampled points are joined by RRG edges.
  • The obstacle assumptions ensure collision-free approximating paths, while the cost assumptions support the convergence arguments.
  • The approximating path costs converge to the optimal cost: lim_i→∞ c(σ_i) = c(σ∗).The cost difference is bounded by the number of replaced segments times κλ_i, with λ_i tending to zero.
  • Points selected from consecutive balls are within the RRG connection radius r_i, so their joined path is constructable by RRG.The construction uses the triangle inequality and the definitions of λ_i and q_i to establish the required distance bound.

Proof of Lemma 16

The lemma’s proof models sampled vertices and RRG connections through random geometric graphs, then uses bounds on isolated vertices and edge counts to establish the required asymptotic complexity property.

  • The proof invokes established random geometric graph results to analyze the sampled-vertex structure.
  • A random geometric graph connects independently sampled vertices whenever their distance is at most r_n.The vertex samples come from X_free according to a continuous density function.
  • When the connection radius is parameterized by γ log(N_i), the relevant series is finite whenever γ > 1.
  • The RRG graph can be related to a random geometric graph by separating isolated sampled vertices from the remaining vertices and their incident edges.The proof identifies corresponding vertex and edge sets and establishes the relation between the graphs.
  • The number of obstacle-freeness checks at iteration i equals the number of edges created there and is bounded by the corresponding edge-set size.
  • The expected edge-count terms are controlled asymptotically: one term tends to zero and another remains finite, yielding the lemma.
Loading 1005.0416v1…