AORRTC: Almost-Surely Asymptotically Optimal Planning with RRT-Connect (2505.10542v2)

Abstract: Finding high-quality solutions quickly is an important objective in motion planning. This is especially true for high-degree-of-freedom robots. Satisficing planners have traditionally found feasible solutions quickly but provide no guarantees on their optimality, while almost-surely asymptotically optimal (a.s.a.o.) planners have probabilistic guarantees on their convergence towards an optimal solution but are more computationally expensive. This paper uses the AO-x meta-algorithm to extend the satisficing RRT-Connect planner to optimal planning. The resulting Asymptotically Optimal RRT-Connect (AORRTC) finds initial solutions in similar times as RRT-Connect and uses any additional planning time to converge towards the optimal solution in an anytime manner. It is proven to be probabilistically complete and a.s.a.o. AORRTC was tested with the Panda (7 DoF) and Fetch (8 DoF) robotic arms on the MotionBenchMaker dataset. These experiments show that AORRTC finds initial solutions as fast as RRT-Connect and faster than the tested state-of-the-art a.s.a.o. algorithms while converging to better solutions faster. AORRTC finds solutions to difficult high-DoF planning problems in milliseconds where the other a.s.a.o. planners could not consistently find solutions in seconds. This performance was demonstrated both with and without single instruction/multiple data (SIMD) acceleration.

• The paper introduces AORRTC, which extends RRT-Connect to achieve almost-sure asymptotically optimal motion planning while preserving fast initial feasibility.
• It employs an augmented (n+1)-dimensional search space and iterative cost resampling to progressively refine solution quality.
• Experimental results show AORRTC outperforms standard planners by converging much faster to near-optimal solutions in high-dimensional, challenging environments.

AORRTC (Almost-Surely Asymptotically Optimal RRT-Connect) is a motion planning algorithm that extends the popular RRT-Connect planner to be almost-surely asymptotically optimal (ASAO) while maintaining its characteristic speed in finding initial feasible solutions. This is achieved by applying the AOX meta-algorithm framework to RRT-Connect.

The core idea of AORRTC is to perform motion planning not just in the robot's configuration space ($n$ dimensions), but in an augmented search space that includes an additional dimension representing the cost-to-come to a given configuration. This augmented space is $(n+1)$-dimensional. AORRTC iteratively calls a modified RRT-Connect planner on this augmented space.

Here's how it works in practice:

1. Initial Solution: AORRTC first runs a standard RRT-Connect search (conceptually, on the augmented space with an infinite cost bound). Once a feasible path is found, it is simplified using techniques like randomized shortcutting and B-spline smoothing. The cost of this simplified path becomes the initial upper bound ($c_\text{min}$) on solution cost.
2. Iterative Refinement: AORRTC then enters an anytime loop. In each iteration, it calls a modified RRT-Connect planner. This modified RRT-Connect searches the augmented space, but it is constrained by the current best-found solution cost, $c_\text{min}$. When sampling a new vertex in the augmented space, it draws a configuration uniformly from the free space and samples a cost value. This sampled cost effectively acts as an upper limit for the cost-to-come that a connection to this sample can have to be considered useful towards a better solution. The nearest neighbor search in the augmented space considers both the configuration distance and the difference in the cost-to-come dimension, incorporating the sampled cost limit.
3. Cost Resampling: A practical improvement in the implementation involves resampling the cost-to-come for newly added vertices. After a new vertex is connected to a tree based on the initial nearest neighbor search in the augmented space, its cost-to-come is calculated. The algorithm then attempts to find a better parent for this new vertex by searching for neighbors with potentially lower costs, effectively resampling the cost connection within a lower range. This helps reduce path costs locally.
4. Updating the Bound: If the RRT-Connect call finds a new feasible path whose cost is lower than the current $c_\text{min}$, this new path replaces the best solution found so far, and $c_\text{min}$ is updated to the cost of the new simplified path. The next iteration of RRT-Connect will then search within this tighter cost bound.
5. Anytime Property: This process continues iteratively. Given more planning time, AORRTC continues to find paths with decreasing costs, converging probabilistically to the optimal solution.

The theoretical guarantees of AORRTC (probabilistic completeness and ASAO) stem from the properties of the AOX meta-algorithm and the probabilistic completeness of the underlying RRT-Connect search. By restricting the search space iteratively based on improving cost bounds, the algorithm guarantees that, with enough samples, it will find a path if one exists and that the path cost will converge to the optimal cost.

For practical implementation, the paper highlights several considerations:

• Using informed sampling techniques to bias sampling towards regions likely to contain optimal solutions (though the core AOX concept provides ASAO even without informed sampling, it improves performance).
• Employing a balanced bidirectional search strategy for the RRT-Connect component to effectively grow trees from both start and goal.
• Leveraging efficient nearest neighbor data structures for searching in the augmented space.
• Applying path simplification (like shortcutting and B-spline smoothing) to the found paths to quickly improve their quality before updating the cost bound.

Experiments using the MotionBenchMaker dataset with 7 DoF Panda and 8 DoF Fetch robot arms demonstrate AORRTC's effectiveness. It was tested against standard OMPL implementations (RRT-Connect, RRT*, BIT*, AIT*) and VAMP implementations (RRT-Connect, RRT*, BIT*, FCIT*), which leverage SIMD acceleration for faster collision checking.

The results show that AORRTC (both OMPL and VAMP versions) achieves initial solution times comparable to RRT-Connect, significantly faster than other tested ASAO planners. It consistently solves more problems, especially difficult high-dimensional ones, than other ASAO methods within the given time limits. Furthermore, AORRTC converges to better solutions much faster than other ASAO planners, reliably finding near-optimal solutions within milliseconds, even for challenging environments. The VAMP implementation benefits from SIMD acceleration, finding solutions and converging faster overall, but the relative performance advantage of AORRTC over other planners holds across both implementations.

In summary, AORRTC provides a practical approach to obtaining ASAO guarantees without sacrificing the rapid initial solution discovery that makes RRT-Connect popular. By iteratively applying a cost-bounded bidirectional search in an augmented space, it effectively balances exploration and exploitation, making it a strong candidate for real-world applications requiring both fast initial feasibility and eventual optimality.