PaperlayerView exact revision on arXiv

Source-linked AI summary

Asymptotically Optimal Sampling-based Kinodynamic Planning

Yanbo Li, Zakary Littlefield, Kostas E. Bekris

arXiv:1407.2896v5cs.RO

TL;DR

Kinodynamic planning lacks practical BVP or steering-function solutions for many dynamical systems. This work introduces a framework showing that fully random selection and propagation can achieve asymptotic optimality while retaining computationally efficient sparse data structures.

  • Problem

    BVP and steering-function solutions are difficult to produce for many interesting dynamical systems, limiting roadmap and RRT∗ approaches to kinodynamic planning.

  • Method

    The work proposes a framework using fully random selection and propagation for kinodynamic systems, with pruning parameters that maintain a sparse data structure.

  • Results

    The framework achieves asymptotic optimality, while SST improves path quality over time and can perform even better than the efficient but suboptimal RRT.

  • Takeaways & Limitations

    Sparse sampling can find solutions sooner in practice without explicitly constructing specialized steering functions, supporting computationally efficient kinodynamic planning.

  • Takeaways & Limitations

    The framework assumes dynamics expressed as a nonlinear ordinary differential equation, and simulated systems may not expose the actual final propagated state.

Abstract

from arXiv · show

Sampling-based algorithms are viewed as practical solutions for high-dimensional motion planning. Recent progress has taken advantage of random geometric graph theory to show how asymptotic optimality can also be achieved with these methods. Achieving this desirable property for systems with dynamics requires solving a two-point boundary value problem (BVP) in the state space of the underlying dynamical system. It is difficult, however, if not impractical, to generate a BVP solver for a variety of important dynamical models of robots or physically simulated ones. Thus, an open challenge was whether it was even possible to achieve optimality guarantees when planning for systems without access to a BVP solver. This work resolves the above question and describes how to achieve asymptotic optimality for kinodynamic planning using incremental sampling-based planners by introducing a new rigorous framework. Two new methods, Stable Sparse-RRT (SST) and SST*, result from this analysis, which are asymptotically near-optimal and optimal, respectively. The techniques are shown to converge fast to high-quality paths, while they maintain only a sparse set of samples, which makes them computationally efficient. The good performance of the planners is confirmed by experimental results using dynamical systems benchmarks, as well as physically simulated robots.

1. Introduction

Kinodynamic planning seeks high-quality paths for systems whose dynamics make steering difficult, especially without a two-point boundary-value solver. This paper introduces a forward-propagation framework and sparse planners with asymptotic guarantees and practical convergence.

  • Motivation: Sampling-based planners evolved from probabilistic completeness toward asymptotic optimality, but kinodynamic planning remains harder because paths must respect the system dynamics.RRT* and related methods obtain asymptotic optimality through roadmap connectivity, whereas kinodynamic planning searches a higher-dimensional state space under the system flow.
  • The BVP challenge: A steering function for a dynamical system requires solving a two-point boundary value problem, which is difficult for many robots and physically simulated systems.Physics-engine settings may provide only forward propagation, so roadmap planners such as PRM* and RRT* cannot generally be used for kinodynamic planning.
  • Framework: The paper develops a rigorous analysis showing that asymptotic optimality can be achieved using only forward propagation for a broad class of kinodynamic systems.The argument uses accessibility properties and probability theory, with asymptotic optimality established for non-holonomic systems satisfying Chow’s condition.
  • Proposed planners: SST and SST* use BestNear selection and pruning to maintain sparse trees while achieving asymptotic near-optimality and optimality, respectively.The pruning operation keeps the number of stored nodes small and reduces SST’s per-iteration cost relative to suboptimal RRT.
  • Empirical evidence: The proposed methods converge to high-quality paths in practice, and experiments demonstrate their behavior on dynamical-system benchmarks and simulated robots.In a kinematic point-system example, SST reaches the optimal path’s homotopic class while maintaining a sparse data structure; pendulum experiments compare SST components without a steering function.

2. Background

Kinodynamic planning methods range from search-based and optimization approaches to sampling-based planners, but high-quality sampling-based methods generally require a steering or BVP solver. Prior work established asymptotic optimality for some sampling-based planners, while this paper develops formal guarantees without a steering function through SST and SST*.

  • Kinodynamic planning: Kinodynamic planning must account for system dynamics, while highly-dynamic problems remain challenging.
  • Prior approaches: Search-based methods operate on state-space discretizations but depend exponentially on resolution.
  • Sampling-based planners: Sampling-based planners construct graphs by sampling states and connecting them with local paths, with RRT-Extend and EST propagating dynamics forward in time without steering functions.
  • Sampling-based planners: RRT is effective at returning solutions quickly but converges to a sub-optimal solution.
  • Asymptotic optimality: PRM* and RRT* achieve asymptotic optimality using random geometric graph theory, but require steering functions and logarithmically many neighbor-connection tests.
  • Paper contribution: High-quality prior methods require a BVP solver, whereas this work provides formal guarantees without a steering function through SST and SST*.The proposed methods are described as asymptotically near-optimal and optimal, respectively, and improved computational performance over a shooting variant is reported.

3. Problem Setup

The paper formulates kinodynamic planning for dynamical systems under robustness, regularity, and piecewise-constant-control assumptions. Its analysis defines clearance-dependent near-optimality and shows that satisfying the property for decreasing clearances yields traditional probabilistic completeness and asymptotic optimality.

  • System model: The system evolves in a state space X under controls U, with collision-free subset Xf and trajectories generated by forward integration.
  • Control representation: The analysis primarily uses piecewise constant control functions, while the methods could potentially extend to continuous controls such as splines or basis functions.
  • System assumptions: STLA systems can reach full-dimensional neighborhoods locally, and the assumptions also require bounded second derivatives and Lipschitz continuity.
  • Robust trajectories: Chow’s condition implies that δ-similar trajectories exist, and δ-robust trajectories require both obstacle clearance and dynamic clearance greater than δ.
  • Planning problem: The planning problem seeks a trajectory from initial state x0 to the goal region XG, assuming a δ-robust solution generated by piecewise constant controls exists.
  • Optimality criterion: Asymptotic δ-robust near-optimality bounds returned cost by h(c*, δ), with the analysis giving h(c*, δ) = (1+α·δ)·c* for α ≥0.
  • Optimality criterion: If the relevant completeness and near-optimality properties hold for all decreasing δ, the algorithm satisfies probabilistic completeness and asymptotic optimality.

4. Algorithms

The paper develops sampling-based kinodynamic planners that avoid requiring a steering function or BVP solver while providing asymptotic near-optimality or optimality. Its framework combines random propagation, best-first selection, and pruning to improve convergence and computational efficiency.

  • The framework addresses kinodynamic planning without access to a BVP solver or steering function.
  • SST and SST*: The framework produces computationally efficient planners that achieve asymptotic near-optimality and can be extended to asymptotic optimality.
  • Asymptotic Optimality from Random Primitives: Random forward propagation can provide asymptotic optimality, although the resulting method has poor convergence to optimal solutions and is computationally impractical.
  • Improving Convergence Rate: Best-first selection combined with random propagation yields an asymptotically δ-robust near-optimal solution with good convergence per iteration.
  • Sparsity and Efficiency: The pruning operation preserves the convergence properties of the selection strategy while reducing stored nodes and per-iteration computational cost.

5. Analysis

The analysis establishes that forward-propagation planners can achieve completeness and asymptotic near-optimality or optimality without a steering function. It develops this result through MonteCarlo-Prop, covering-ball arguments, increasingly efficient node selection, and sparse pruning.

  • Covering-ball analysis: Covering balls partition a robust trajectory into overlapping regions whose centers are separated by a fixed cost increment C∆.The resulting sequence provides the structure for proving that generated paths can remain δ-similar to a robust optimal path.
  • MonteCarlo-Prop: MonteCarlo-Prop provides a forward-propagation primitive with properties sufficient for asymptotic-optimality analysis without a steering function.The analysis uses its positive probability of generating trajectories similar to a target trajectory.
  • Naive algorithm: NAIVE_RANDOM_TREE is probabilistically complete and asymptotically optimal, but its convergence can be intractable because required iterations grow exponentially with the reciprocal of propagation success probability.The algorithm eventually propagates from each node indefinitely, yielding the guarantee but poor practical convergence.
  • SST: SST preserves asymptotic δ-robust near-optimality through pruning while reducing per-iteration complexity and maintaining a sparse active data structure.The stability argument ensures that near-optimal active states remain available, and the convergence rate is nearly identical to RRT-BestNear.
  • SST*: SST* is probabilistically complete and asymptotically optimal, extending the sparse-planning framework from near-optimality to optimality.The analysis also considers trajectories with smaller clearance, closer to the true optimum.

6. Experimental Evaluation

Experiments across kinematic, rigid-body, pendulum, aerial, and other dynamical benchmarks show that SST improves path quality over time while remaining computationally efficient. Its sparse representation reduces memory and often improves running time, with trade-offs involving path diversity and parameter choices.

  • Experimental setup: SST is evaluated against RRT, RRT*, and shooting-based alternatives across multiple dynamical benchmarks, including a 12-dimensional quadrotor and fixed-wing aircraft.Each benchmark experiment averages results over 50 runs of each algorithm.
  • Path quality and runtime: SST consistently improves path quality with more iterations and achieves running times comparable to or better than RRT while maintaining fewer samples.This pattern is reported across the evaluated systems.
  • Path quality and runtime: SST’s average path costs improve globally over time, whereas RRT’s average increases because it continues propagating suboptimal nodes.The comparison concerns the average quality of paths associated with all tree nodes.
  • Baseline comparisons: Shooting-based planning has varying success, including failure on most fixed-wing runs, while SST avoids the same issue on nonlinear pendulum-like systems with random propagation.The authors attribute shooting’s poor performance potentially to the distance function or shooting inaccuracies.
  • Computational efficiency: SST’s sparse data structure can improve running time after initial growth and may offer additional efficiency when collision checking dominates nearest-neighbor queries.The latter strategy was not used in the reported experiments.
  • Space efficiency: SST stores significantly fewer nodes than competing algorithms, including when its witness set is counted, thereby reducing memory requirements.The sparse structure produces substantial pruning despite a trade-off between sparsity and path diversity.

7. Discussion and Conclusion

The paper develops a sampling-based framework for kinodynamic planning without specialized steering functions, combining best-first selection with pruning. SST and SST* provide path-quality improvement and sparse data structures, while extensions and assumptions define their practical scope.

  • Core contribution: The framework achieves asymptotic near-optimality and optimality for kinodynamic planning without requiring specialized steering functions.It uses fully random selection and forward propagation under reasonable assumptions.
  • Core contribution: Best-first selection and pruning enable fast convergence to high-quality solutions while maintaining a sparse data structure.These design choices distinguish the approach from previous sampling-based analyses.
  • Efficiency and convergence: SST has lower running-time and space requirements than related implementations while retaining path-quality improvement over time.The reported efficiency advantage is associated with SST's sparse representation and pruning process.
  • Parameter selection: The pruning radius δs should not be too large, while δBN should exceed δs but remain balanced with state-space size.Large δs can miss narrow passages, whereas large δBN can repeatedly select nodes near the root and reduce exploration.
  • Extensions: SST can extend to planning under uncertainty because forward propagation updates beliefs, although suitable distance metrics and higher dimensionality remain challenges.The discussion identifies belief-space steering as difficult and suggests Earth Mover's distance as one possible metric.
  • Scope and limitations: The analysis covers nominal trajectories under smooth dynamical models, not feedback policies or systems with contact-induced discontinuities and stratified state spaces.Rigid-body dynamics, stick-slip friction, and contact constraints are identified as outside the current analysis.

Appendix A

Appendix A establishes the local geometric basis for approximating trajectories under Chow’s condition. It shows that nearby regions along a trajectory overlap sufficiently to support δ-similar trajectory construction.

  • The construction relates to prior notions including linking sequences, attraction sequences, and homotopy in the δ-interior of Xfree.
  • Lemma 6 introduces a positive dynamic clearance δ0 under which trajectories can be approximated by δ-similar trajectories.The result applies to systems satisfying Eq. 1 and Chow’s condition.
  • Chow’s condition yields Ball Box structure and local accessibility for the relevant dynamical manifold.The proof sketch connects Chow’s condition to the Ball Box theorem and small-time local accessibility.
  • Weighted boxes centered at sufficiently close states intersect, allowing a positive-radius hyper-ball to fit inside the intersection.The construction uses smoothly varying bases oriented by Lie-bracket vector fields.
  • Concatenating overlapping hyper-ball regions along a trajectory generates δ-similar trajectories between successive regions.The connecting horizontal curves have length bounded by the sub-Riemannian-ball radius tϵ.

Appendix B

Appendix B shows that piece-wise constant controls can generate trajectories close to one another when their control inputs are close. The argument uses Euler discretization and Lipschitz continuity of the dynamics.

  • Theorem 15 states that piece-wise constant control sampling can generate a trajectory δ-similar to an optimal trajectory.
  • Euler’s method divides [0, T] into n equal segments and approximates the trajectory using states x0 through xn and sampled controls ui.
  • The trajectory discrepancy is bounded using control and state Lipschitz constants, Ku and Kx.The bound compares trajectories sharing the same initial state and depends on Δu, the supremum control difference.
  • Repeatedly applying the one-step inequalities propagates the control-induced error across the full time horizon T.
  • Under Assumption 5, Euler’s method converges to the solution of the initial-value problem.

Appendix C

Appendix C proves that NAIVE_RANDOM_TREE eventually generates trajectories δ-similar to any robustly feasible optimal trajectory. This establishes probabilistic completeness under positive robust clearance.

  • NAIVE_RANDOM_TREE is probabilistically complete.
  • Theorem 18 states that NAIVE_RANDOM_TREE eventually generates a δ-similar trajectory to any optimal trajectory with robust clearance δ > 0.
  • The proof covers an optimal trajectory with a sequence of δ-balls and analyzes the probability of failing to generate trajectories through successive balls.
  • The induction argument shows that repeated random propagation eventually reaches each successive neighborhood along the optimal trajectory.
  • For any robust clearance 0 < δ′ ≤ δ, NAIVE_RANDOM_TREE eventually generates a δ-similar trajectory to an optimal trajectory.

Appendix D

Appendix D connects δ-similar trajectory generation to cost guarantees. It concludes that NAIVE_RANDOM_TREE is asymptotically δ-robust near-optimal for every δ > 0.

  • Theorem 20 states that NAIVE_RANDOM_TREE is asymptotically optimal.
  • A δ-similar trajectory to an optimal trajectory is discovered almost surely under the theorem’s assumptions.
  • The cost difference between a trajectory and the optimal trajectory is bounded by Kc · δ.
  • The resulting planner is asymptotically δ-robust near-optimal for any δ > 0.The step cost CΔ is determined by the ball sequence and does not shrink.

Appendix E

Theorem 21 states that, in the worst case, the kth trajectory segment returned by NAIVE_RANDOM_TREE converges. Its proof analyzes the worst case of Equation 13 and uses monotonicity properties.

  • Theorem 21 states convergence of the kth trajectory segment returned by NAIVE_RANDOM_TREE in the worst case.
  • The proof considers the worst case described by Equation 13.
  • The argument invokes the monotonicity of P(E(n)k−1).

Appendix F

Appendix F establishes that every added state is selected for MonteCarlo-Prop infinitely often as iterations continue. The proof relies on uniform random vertex selection and divergence of a harmonic-series expression.

  • Theorem 22 states that any state xi added to V at iteration i is selected for MonteCarlo-Prop infinitely often.
  • The relevant event probability is formulated from selecting xi for MonteCarlo-Prop at later iterations.
  • The summation separates into a harmonic series and the i-th harmonic number.
  • The resulting expression diverges as n →∞, implying infinitely many selections almost surely.
  • Uniform random selection of existing vertices makes selections of xi independent across different iterations after its extension.

Appendix G

Appendix G proves that RRT-BestNear eventually generates δ-similar trajectories to any optimal trajectory and is probabilistically δ-robustly complete.

  • Theorem 24 states that RRT-BestNear eventually generates a δ-similar trajectory to any optimal trajectory.
  • The proof analyzes failure probabilities through a sequence of Ak events and their complements.
  • The induction begins from x0 being in BδBN(x0), establishing the base case.
  • The inductive probability calculation concludes with P(E(j)k+1) = 1 − 0 = 1.
  • Corollary 40 states that RRT-BestNear is probabilistically δ-robustly complete.

Appendix H

Appendix H establishes asymptotic δ-robust near-optimality for RRT-BestNear and develops the probability and continuity arguments supporting that result. It also relates covering-ball radii to trajectory-extension probabilities.

  • Theorem 25 states that RRT-BestNear is asymptotically δ-robustly near-optimal.
  • MonteCarlo-Prop supplies a lower bound on the probability of generating each required trajectory segment.
  • BestNear may return xi or another state with smaller or equal cost.
  • The cost induction accumulates a Kc · δ term per trajectory segment.
  • The proof uses Theorem 24 to obtain δ-similar trajectory segments almost surely.
  • Lemma 36 relates extension probabilities for covering balls with radii δ and δ′, where δ′/δ = α.
  • The appendix derives the radius relation using bounded system dynamics and the distance between states on the same trajectory.
Loading 1407.2896v5…