PaperlayerView exact revision on arXiv

Source-linked AI summary

Asymptotically Optimal Planning by Feasible Kinodynamic Planning in State-Cost Space

Kris Hauser, Yilun Zhou

arXiv:1505.04098v1cs.RO

TL;DR

Optimality under kinematic and differential constraints remains a challenge for sampling-based planners. The paper transforms optimal planning into feasible planning in state-cost space and builds a meta-planner around feasible planners. It proves asymptotic optimality and reports favorable comparisons in several toy scenarios.

Abstract

from arXiv · show

This paper presents an equivalence between feasible kinodynamic planning and optimal kinodynamic planning, in that any optimal planning problem can be transformed into a series of feasible planning problems in a state-cost space whose solutions approach the optimum. This transformation gives rise to a meta-algorithm that produces an asymptotically optimal planner, given any feasible kinodynamic planner as a subroutine. The meta-algorithm is proven to be asymptotically optimal, and a formula is derived relating expected running time and solution suboptimality. It is directly applicable to a wide range of optimal planning problems because it does not resort to the use of steering functions or numerical boundary-value problem solvers. On a set of benchmark problems, it is demonstrated to perform, using the EST and RRT algorithms as subroutines, at a superior or comparable level to related planners.

I. INTRODUCTION

The paper recasts optimal motion planning as feasible kinodynamic planning in an augmented state-cost space, addressing the challenge of optimality under kinematic and differential constraints. It situates this approach among sampling-based planners and introduces a meta-algorithm whose convergence is proven.

I. INTRODUCTION

  • The state-cost formulation turns optimal planning into feasible kinodynamic planning, and AO-x adapts a feasible planner into an asymptotically optimal one under relatively unrestrictive conditions.It accepts arbitrary cost functions, including non-differentiable ones, and handles the constraints supported by its underlying planner.Source: §I · p. 1
  • Augmenting each state with accumulated cost-to-come creates an expanded state-cost space for the feasible planning formulation.For a 2D path-length problem, this space is 3D and has a conical reachable set from the start; paths to the same target can reach different cost-space points.Source: §I · p. 1, Figure 1 · p. 1

II. BACKGROUND AND RELATED WORK

  • Sparse-Stable-RRT and SST* avoid steering functions, but require parameters to tune; experiments suggest AO-x planners generally outperform both.The paper also contrasts its approach with Anytime-RRT, which it reports tends not to converge to an optimum.Source: §II · p. 2

III. THEORETICAL FORMULATION

  • The paper introduces a state-cost space formulation and meta-algorithms, then establishes theoretical results on asymptotic optimality.Prior sampling-based optimal planners such as RRT* rely on steering functions, which are difficult to obtain for dynamically constrained systems.Source: §III · p. 2, §II · p. 2

A. Terminology

The terminology section defines feasible and optimal planning problems, bounded suboptimality, and three planner guarantees. It also notes how the general problem formulation accommodates kinematic, second-order, and time-varying constraints.

A. Terminology

  • A feasible kinodynamic problem asks for a trajectory and control satisfying the problem’s state, control, dynamics, and feasibility conditions.Kinematic problems can use the path derivative as control; second-order and time-varying problems can be represented by augmenting the state.Source: A. Terminology · p. 2
  • An optimal planning problem minimizes an objective over feasible trajectories, with incremental cost and terminal cost represented separately.The objective is minimized among trajectories satisfying the feasible-problem conditions.Source: A. Terminology · p. 2
  • A bounded-suboptimality problem seeks a path whose cost is C* + ϵ, where C* is the optimal-path cost and ϵ is specified and positive.Source: A. Terminology · p. 2
  • Completeness guarantees a finite-time feasible solution when one exists, probabilistic completeness approaches success probability 1 with planning time, and asymptotic optimality approaches C* with probability 1.Source: A. Terminology · p. 2

B. State-cost space equivalence

The paper shows that an optimal planning problem can be represented equivalently in state-cost space by augmenting each state with its accumulated cost. This transformation also makes bounded-suboptimality solvable as a feasible planning problem with a cost-bounded goal set.

B. State-cost space equivalence

  • Augmenting each state x with accumulated cost c yields a canonical state-cost problem that eliminates dependence on incremental cost L.Solutions in the original and transformed problems correspond one-to-one by projecting or augmenting the cost dimension.Source: B. State-cost space equivalence · p. 3
  • The transformed state-cost problem can have unreachable points even when every original state is reachable, and its goal set extends infinitely in the cost direction.Source: B. State-cost space equivalence · p. 3
  • A cost bound c̄ defines a feasible planning problem whose goal set contains terminal state-cost pairs satisfying the goal condition and cost bound.A solution corresponds to a feasible original path costing no more than c̄, and no solution exists exactly when c̄ < C*.Source: B. State-cost space equivalence · p. 3

C. Bounded-suboptimality meta-planning with a complete feasible planner

The bounded-suboptimality meta-planner repeatedly calls a complete feasible planner while lowering the cost ceiling from the best path found. It terminates in finite time with a path no more than ϵ above optimal.

C. Bounded-suboptimality meta-planning with a complete feasible planner

  • The bounded-suboptimality approach requires a complete feasible planner, a restrictive condition for practical high-dimensional problems.Source: C. Bounded-suboptimality meta-planning… · p. 3
  • The algorithm first finds an unrestricted feasible path, then repeatedly seeks a path under a ceiling lowered by ϵ from the previous best cost.It stops when the feasible planner finds no solution under the current ceiling.Source: C. Bounded-suboptimality meta-planning… · p. 3
  • With a complete feasible planner, the meta-planner terminates in finite time and returns a path costing no more than C* + ϵ.The final failed bounded-cost search establishes that the previous solution is within ϵ of optimal.Source: C. Bounded-suboptimality meta-planning… · p. 3

D. Asymptotically-optimal meta-planning with a randomized feasible planner

The algorithm relaxes the need for a complete feasible planner under a non-negligible cost-improvement condition. It repeatedly solves feasible problems with tighter cost bounds, and is proven asymptotically optimal for well-behaved randomized planners.

D. Asymptotically-optimal meta-planning with a randomized feasible planner

  • The method relaxes the requirement for a complete planner under the assumption that each found path lowers the cost bound by a non-negligible fraction.This relaxation addresses the impracticality of requiring completeness for high-dimensional problems.Source: D. Asymptotically-optimal… · p. 3
  • A well-behaved planner terminates in finite time when a feasible solution exists and makes a non-negligible expected reduction in suboptimality each iteration.The stated practical condition is a nonzero chance of avoiding the worst-possible path.Source: D. Asymptotically-optimal… · pp. 3–4
  • Starting with a feasible path, the algorithm repeatedly plans under the current path-cost bound and returns the latest path.Figure 2 describes improvement below an existing path cost as a feasible planning problem, with the reachable goal portion shrinking as the bound approaches optimality.Source: D. Asymptotically-optimal… · p. 4, Figure 2 · p. 4
  • Theorem 3 establishes that the algorithm is asymptotically optimal when its randomized planner is well-behaved.The probability that the returned path is not optimal approaches zero as the number of iterations increases.Source: D. Asymptotically-optimal… · p. 4

E. Convergence rate with respect to time

The paper analyzes convergence in total running time when each feasible-planner call may slow as the reachable goal region shrinks. Under an exponential-convergence condition, the resulting planner is asymptotically optimal in time; for EST, the bound depends on reachable goal volume and visibility.

E. Convergence rate with respect to time

  • Probabilistic completeness alone may be impractical: a failure probability of 1/t gives infinite expected running time.The time-convergence result therefore assumes an exponential convergence bound; the paper states this implies finite expected running time.Source: E. Convergence rate with respect to time · pp. 4–5
  • Convergence rate depends on the volume of the reachable goal region, which shrinks as the cost bound approaches the optimum.A small target region makes random sampling into it rare and slows convergence.Source: E. Convergence rate with respect to time · p. 5, Figure 3 · p. 5
  • For expansive spaces, kinodynamic EST has an exponential convergence bound whose constants depend on reachable goal volume.The result states α(g) = γ ln(1/g) and β(g) = δg for positive constants γ and δ.Source: E. Convergence rate with respect to time · p. 5
  • A probabilistically complete, exponentially convergent feasible planner makes AO-A asymptotically optimal in total running time.The analysis concerns convergence over elapsed time even though successive planner calls can take longer as the reachable goal portion shrinks.Source: E. Convergence rate with respect to time · pp. 4–5
  • The EST-based time bound is described as loose because earlier iterations are likely to finish faster than the bound assumes.The analysis bounds time using the expected time to reach the ε-suboptimal goal region.Source: E. Convergence rate with respect to time · pp. 5–6

F. Complexity Discussion

AO-x runtime depends on problem structure and on how the feasible planner performs after the cost dimension is added. Dimensionality affects runtime terms but does not directly determine AO-x's order of convergence.

F. Complexity Discussion

  • Problem visibility affects the feasible planner's running time, and planner parameters such as RRT expansion distance are problem-dependent.Source: F. Complexity Discussion · p. 6
  • Adding cost as a state dimension may increase time and space complexity and introduce drift, while leaving the control space unchanged.Many planners' performance is governed chiefly by control complexity.Source: F. Complexity Discussion · p. 6
  • Problem dimensionality does not directly determine AO-x's convergence order, but can increase the time-to-target term and affect expected cost reduction.For a locally convex-cone-shaped reachable goal region of dimension d, the expected cost reduction is O(1/(d + 1)).Source: F. Complexity Discussion · p. 6

IV. IMPLEMENTATIONS AND EXPERIMENTS

The implementation section applies AO-x with EST and RRT as feasible kinodynamic-planning subroutines, using Python implementations.

IV. IMPLEMENTATIONS AND EXPERIMENTS

  • The examples use AO-EST and AO-RRT, implemented in Python; compiled-language implementations could speed them up greatly.Source: §IV · p. 6

A. Implementations using RRT and EST

Kinodynamic EST and RRT grow trees through random state-and-control extensions, but use different sampling strategies. Their state-cost implementations also use density-based selection or a distance metric, with pruning to reduce search costs.

Planner overview

  • EST and RRT both extend a start-rooted state-space tree by sampling a node and control, then integrating the dynamics over a short horizon.EST favors extensions with terminal states spread across the reachable set; RRT biases extensions toward a random state, and both can use goal biasing.Source: A. Implementations using RRT and EST · p. 6

EST Implementation

  • EST samples extensions to approximate a uniform distribution over the tree’s reachable set, using grid-based density estimates across random projections of state-cost space.It chooses feasible extensions with probability proportional to 1/(N(x_t) + 1)^2, where N(x_t) counts nodes in the same grid cell.Source: A. Implementations using RRT and EST · pp. 6–7

RRT Implementation

  • RRT guides exploration toward randomly sampled states using a distance metric, whose choice can strongly affect performance when cost-to-go is difficult to estimate.Nearest-node selection is accelerated with a KD-tree.Source: A. Implementations using RRT and EST · p. 7

Performance considerations

  • Both planners retain trees between iterations and prune nodes whose cost exceeds the best known solution cost, reducing EST density updates and RRT nearest-neighbor queries.When an admissible heuristic is available, pruning can also remove nodes whose accumulated cost plus estimated cost-to-go exceeds the best known solution cost.Source: A. Implementations using RRT and EST · p. 7

B. Example Problems

The examples span kinematic path-length tasks, kinodynamic maneuvers, and control problems with different dynamics and costs. They include narrow passages, local minima, swing-up motion, and obstacle avoidance.

Kink

  • The Kink task minimizes path length in a unit square, with the optimal route passing through a 0.02-wide corridor with two kinks.Each tree expansion is limited to 0.15 units.Source: B. Example Problems · p. 7

Bugtrap

  • Bugtrap minimizes path length in a unit square and challenges RRT planners because they can become trapped in a local minimum.Each tree expansion is limited to 0.15 units.Source: B. Example Problems · p. 7

Dubins

  • The Dubins-car task seeks a sideways maneuver with minimal execution time, equivalent here to minimum path length.The car’s orientation should remain relatively fixed, and the goal is a 0.4-unit sideways move within a 0.1-unit state-space tolerance.Source: B. Example Problems · p. 7

Double Integrator

  • The double-integrator task moves a point with bounded velocities and accelerations from near the left edge to a target near the right edge.The goal tolerance is 0.2 units in state space, and time steps are sampled from [0, 0.05] seconds.Source: B. Example Problems · p. 7

Pendulum

  • The pendulum task minimizes swing-up time, requiring the planner to reach a state within 10° of inverted with angular velocity below 0.5 rad/s.Because the available torque cannot complete a full rotation, the pendulum must swing back and forth to accumulate angular momentum; AO-RRT’s first five solutions shorten from 8.46 to 5.51 seconds.Source: B. Example Problems · p. 7, Figure 6 · p. 8

Flappy

  • Flappy models a bird moving horizontally at constant speed while gravity and binary upward thrust govern its vertical motion toward a goal region.The example solution travels from the left to the green goal region while avoiding gray rectangular obstacles.Source: B. Example Problems · pp. 7–8, Figure 7 · p. 8

C. Experiments

Experiments show that AO-EST is consistently strong across benchmarks, while AO-RRT is more variable; planner performance and useful cost-space weighting depend on the problem.

Comparing AO-EST and AO-RRT

  • On the Car example, AO-RRT finds an initial path sooner, but AO-EST converges faster because each EST iteration is cheaper.The experiments report no clear winner between the planners across the other tests.Source: C. Experiments · p. 8

Benchmarking against comparable planners

  • AO-EST consistently outperforms M-EST and M-EST-Prune, while AO-RRT ranges from better to roughly equal against its M-RRT variants; AO-EST is best or near-best on most problems.Anytime-RRT and SS-RRT usually perform no better than M-RRT, with reported exceptions on Flappy and Bugtrap.Source: C. Experiments · pp. 9–10, Figure 8 · p. 9

RRT distance metric

  • For the Pendulum example, higher cost weights have a minor effect on solution cost but increase running time per iteration.The tested weights were 0.1, 0.3, 1, 3, and 10, with ten 60-second runs per setting.Source: C. Experiments · p. 10, Figure 9 · p. 10
  • On the planar kinematic problem, cost weight 1 matches cost-to-go and experiments show faster convergence toward the optimum.The paper calls this the “ideal” weight for that problem.Source: C. Experiments · p. 10, Figure 10 · p. 10

Adaptation to different costs

  • Under path-length cost, Flappy’s illustrated path cost falls from 1866 px to 1234 px; under low-altitude penalty, it falls from 1330 to 321.The experiments use AO-RRT to adapt to both cost metrics, including the non-differentiable low-altitude penalty.Source: Figure 11 · p. 10, C. Experiments · p. 10

V. CONCLUSION

The conclusion presents state-cost space as a transformation from optimal to feasible kinodynamic planning. It frames the resulting sampling-based meta-planner as broadly applicable and points to possible improvements and future research.

V. CONCLUSION

  • The state-cost space transformation gives an equivalence between optimal motion planning, whether kinematic or kinodynamic, and feasible kinodynamic planning.The resulting meta-planner takes a sampling-based kinodynamic feasible planner as input and uses control-based sampling without a steering function.Source: §V · p. 10
  • The authors suggest local optimization of each trajectory as a possible way to improve convergence, citing prior success for kinematic optimal path planning.They also report curious results about state-space versus cost-space weighting in the RRT distance metric.Source: §V · p. 10
  • The authors identify sampling strategies for state-cost space planning, including appropriate biasing, as a future research direction.Source: §V · p. 11

Related papers

Loading 1505.04098v1…