Sampling-based algorithms for optimal motion planning using closed-loop prediction

Oktay Arslan, Karl Berntorp, Panagiotis Tsiotras

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.

The dual-graph structure of CL-RRT#, showing reference trajectories (orange) and predicted state trajectories (green) navigating around obstacles (red).

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.

Evolution of the CL-RRT# algorithm on a race track simulation, showing the refinement of trajectories and convergence to an optimal path.

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

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 proposed approach on an autonomous-driving scenario.
BibTeX
@inproceedings{arslan2017sampling,
  author = {Oktay Arslan and Karl Berntorp and Panagiotis Tsiotras},
  title = {{Sampling-based algorithms for optimal motion planning using closed-loop prediction}},
  booktitle = {IEEE International Conference on Robotics and Automation},
  year = {2017},
  doi = {10.1109/icra.2017.7989581},
}
Figure