Abstract
This paper presents AO-x, a meta-algorithm designed to achieve asymptotic optimality in motion planning by leveraging existing "feasible" planners. By augmenting the robot's state space with a cost dimension, the optimal planning problem is reformulated as a sequence of feasibility problems with progressively tightening cost constraints. This approach eliminates the requirement for analytical steering functions, which are often unavailable for systems with complex kinodynamics. Experimental results across various benchmarks, including underactuated systems and non-differentiable cost landscapes, demonstrate that AO-x provides a robust and versatile framework for optimal kinodynamic planning.
Introduction
Motion planning in robotics is broadly divided into finding a valid path (feasibility) and finding the best path (optimality). While algorithms like RRT* have revolutionized optimal planning, their reliance on "steering functions"—analytical solutions to connect two specific states—limits their applicability to systems with simple dynamics. For complex kinodynamic systems, such as high-performance vehicles or robots operating in dynamic environments, deriving these functions is often intractable.
The authors propose the AO-x framework to bridge this gap. By treating the underlying feasible planner as a "black box," AO-x achieves optimality through iterative search in an augmented State-Cost Space.
Methods: State-Cost Space Augmentation
The AO-x algorithm operates by adding a cost variable $c$ to the robot's $n$-dimensional state vector. The planning process proceeds as follows:
- Initial Feasibility: A standard planner (e.g., RRT, EST) finds an initial path to the goal with cost $c_{best}$.
- Cost-Constrained Iteration: The search space is then restricted to the subset of the state-cost space where the total accumulated cost is less than $c_{best}$.
- Refinement: Each time a superior path is discovered, $c_{best}$ is updated, further pruning the search space.
This pruning mechanism ensures that the planner focuses computational resources on regions likely to contain the optimal solution, effectively turning a feasibility search into an optimization process.
Key Insights and Theoretical Convergence
A primary insight of this work is that optimal planning does not require explicit "rewiring" of the search tree if the search space is correctly constrained by cost. However, as the solution approaches the optimum, the "goal region" in the state-cost space shrinks, potentially making it harder to sample.
The authors address this "Shrinking Goal" paradox by providing a convergence proof. They show that for planners like the Expansive Space Tree (EST), the expected time to find a solution remains bounded even as the goal volume decreases.
Experimental Evaluation
The AO-x framework was tested on a variety of challenging benchmarks:
- Geometric Puzzles: In the "Kink" and "Bugtrap" environments, AO-x outperformed RRT* by avoiding the high cost of roadmap construction in narrow passages.
- Kinodynamic Systems: The algorithm successfully solved the Pendulum Swing-up and Dubins Car problems, which involve non-trivial differential constraints.
- Complex Costs: Using a "Flappy Bird" simulation, the authors showed that AO-x can handle discrete control inputs and non-differentiable cost functions that typically stymie gradient-based optimizers.
Conclusion
AO-x offers a general-purpose meta-algorithm for transforming feasible planners into optimal ones. Its primary strength lies in its independence from steering functions, making it a powerful tool for complex robotic systems. While challenges remain regarding state-space dimensionality and metric selection, the framework's ability to achieve asymptotic optimality through iterative feasibility marks a significant advancement in kinodynamic motion planning.
Summary updated