Abstract
Motion planning under differential constraints is one of the canonical problems in robotics. State-of-the-art methods evolve around kinodynamic variants of popular sampling-based algorithms, such as Rapidly-exploring Random Trees (RRTs). However, there are still challenges remaining, for example, how to include complex dynamics while guaranteeing optimality. If the open-loop dynamics are unstable, exploration by random sampling in control space becomes inefficient. We describe CL-RRT#, which leverages ideas from the RRT# algorithm and a variant of the RRT algorithm, which generates trajectories using closed-loop prediction. Planning with closed-loop prediction allows us to handle complex unstable dynamics and avoids the need to find computationally hard steering procedures. The search technique presented in the RRT# algorithm allows us to improve the solution quality by searching over alternative reference trajectories. We show the benefits of the provided approach on an autonomous-driving scenario.
Introduction
Kinodynamic motion planning involves finding a path that respects both environmental obstacles and the robot's physical constraints, such as momentum and turning radii. Traditional sampling-based methods like Rapidly-exploring Random Trees (RRT) face significant hurdles in this domain. First, for unstable systems, sampling random control inputs often leads to immediate failure or inefficient exploration. Second, connecting two specific points requires solving complex two-point boundary value problems (the "steering problem"). Finally, standard RRTs typically find feasible but suboptimal solutions.
The CL-RRT# Algorithm
To address these challenges, the authors propose CL-RRT#, an algorithm that combines the robustness of closed-loop prediction with the asymptotic optimality of the RRT# framework.
Closed-Loop Prediction and Dual-Graph Structure
Instead of sampling directly in the control space, CL-RRT# samples in the output space (e.g., target coordinates). A low-level tracking controller then simulates the robot's movement toward these points. This "closed-loop" approach ensures that every generated trajectory is physically feasible by construction, effectively bypassing the need for complex steering functions.
The algorithm maintains two interconnected graphs: one for reference trajectories in the output space and another for the actual predicted state trajectories of the closed-loop system. By leveraging the rewiring logic of RRT#, the algorithm continuously updates these graphs as more efficient paths are discovered.
Experimental Results
The performance of CL-RRT# was evaluated using a nonholonomic wheeled vehicle (unicycle model) on a complex race track. The primary objective was to navigate from a start state to a goal region while minimizing path length.
The algorithm demonstrated strong convergence properties. Over 1,500 iterations, it successfully refined an initial feasible path into a smooth, low-cost trajectory. By using the robot's actual controller during the planning phase, the resulting state trajectories were guaranteed to be trackable, proving the practical utility of the closed-loop prediction approach.
Conclusion
CL-RRT# offers a powerful framework for optimal motion planning with complex dynamics. By decoupling high-level planning from low-level control and utilizing closed-loop prediction, it achieves asymptotic optimality without the computational burden of traditional steering methods. Future work will focus on reducing the computational overhead of forward simulations and extending the approach to higher-dimensional systems like aerial vehicles.
Summary updated