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

Oktay Arslan, Karl Berntorp, Panagiotis Tsiotras

TL;DR

CL-RRT# is a new motion planning algorithm that combines the robustness of closed-loop prediction with the asymptotic optimality of the RRT# framework. By sampling in the output space and using a controller to generate feasible trajectories, it effectively handles complex, unstable robot dynamics without needing to solve computationally expensive "steering" problems.

What Problem Does This Paper Address?

In robotics, kinodynamic motion planning—finding a path that respects both obstacles and the robot's physical limits (like momentum and turning radii)—is notoriously difficult. Traditional sampling-based methods like Rapidly-exploring Random Trees (RRT) often struggle in two ways:

  1. Unstable Dynamics: If you sample random control inputs (like "steer left at 5 degrees") for an unstable system, the robot will likely spin out or crash before it explores anything useful.
  2. The Steering Problem: To connect two specific points in space, you usually have to solve a "two-point boundary value problem" to find the exact controls needed. For complex robots, this is mathematically grueling and slow.
  3. Suboptimality: Standard RRT finds a solution, but not necessarily the best (lowest cost) one.

The authors aim to create an algorithm that is both asymptotically optimal (it finds the best path given enough time) and computationally efficient for robots with complex, unstable differential constraints.

Methods

The authors introduce CL-RRT#, a hybrid approach that leverages three main pillars:

  • Closed-Loop Prediction (from CL-RRT): Instead of sampling in the "control space," the algorithm samples in the "output space" (the actual coordinates where the robot should go). It then uses a low-level tracking controller to simulate how the robot would actually move toward that point. This ensures that every trajectory in the tree is physically feasible by construction.
  • Graph Rewiring (from RRT#): Unlike standard RRT, which only grows a tree, CL-RRT# maintains a graph. It constantly "rewires" the connections between nodes as it finds shorter, more efficient paths. It uses a Bellman-style update to propagate cost improvements throughout the graph.
  • Dual-Graph Structure: The algorithm manages two interconnected graphs:
  • A graph of reference trajectories in the output space.
  • A graph of state trajectories representing the actual predicted motion of the closed-loop system.

By combining these, the algorithm avoids the "steering" problem (it just simulates forward) while still guaranteeing that the solution will eventually converge to the global optimum.

Datasets

The paper evaluates the algorithm using numerical simulations of a nonholonomic wheeled vehicle (a unicycle model) on a race track.

  • Scenario 1: Point-to-Point Navigation. The robot must navigate a 100m x 100m race track from a starting state to a goal region while minimizing the Euclidean path length.
  • Scenario 2: Sequential Waypoint Navigation. The robot is tasked with following a series of waypoints provided by a high-level navigator, mimicking a real-world autonomous driving task.

No external datasets were used; the focus was on the performance of the algorithm within these simulated environments.

Key Insights and Ideas

  • Output Space is Key: By sampling where the robot should be rather than what the motors should do, the search becomes much more intuitive and efficient, especially for systems that are hard to control in open-loop.
  • Feasibility by Construction: Because the algorithm uses the robot's actual controller during the planning phase (the "prediction" part), it never suggests a path that the robot can't actually follow.
  • Decoupling Planning and Control: CL-RRT# allows researchers to use any stable tracking controller. The planner focuses on the high-level path, while the controller handles the "messy" dynamics.
  • Asymptotic Optimality without Steering: This is the "aha" moment. Usually, optimality requires a steering function to connect nodes perfectly. CL-RRT# achieves optimality by searching over alternative reference trajectories and updating the graph as better paths emerge.

Key Results

  • Convergence: In the race track simulation, CL-RRT# successfully found a feasible path and continued to refine it over 1,500 iterations, converging to a smooth, low-cost trajectory (with a final cost of 127.2).
  • Handling Constraints: The algorithm successfully navigated the nonholonomic constraints of the unicycle (which cannot move sideways) and avoided the track boundaries.
  • Dynamic Feasibility: The state trajectories computed during the search phase were shown to be directly trackable by the low-level controller, proving the "closed-loop" benefit.
  • Efficiency: By leveraging the RRT# search technique, the algorithm improved solution quality much faster than a standard kinodynamic RRT would.

Limitations and Future Work

  • Computational Overhead: Simulating the system dynamics forward for every potential edge in the graph is more computationally expensive than simple geometric checks.
  • Heuristic Dependency: The efficiency of the search relies on having a good "admissible heuristic" (a way to estimate the remaining cost to the goal). In very complex environments, finding a good heuristic can be difficult.
  • Controller Sensitivity: The quality of the final plan is inherently tied to the quality of the tracking controller used for prediction. If the controller is poorly tuned, the planner may struggle to find efficient paths.
  • Future Work: The authors suggest that further optimizations in the replanning phase and testing on higher-dimensional systems (like aerial vehicles) would be valuable next steps.

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},
}

Loading related content...

Figure