Robust-RRT: Probabilistically-Complete Motion Planning for Uncertain Nonlinear Systems

Albert Wu, Thomas Lew, Kiril Solovey, Edward Schmerling, Marco Pavone

Introduction

Motion planning for robotic systems is often complicated by the gap between mathematical models and the physical world. Robots face two primary types of uncertainty: epistemic uncertainty, such as unknown mass or friction parameters, and aleatoric uncertainty, such as stochastic external disturbances like wind. While traditional algorithms like the Rapidly-exploring Random Tree (RRT) are effective in deterministic settings, they often fail to guarantee safety when these uncertainties are present.

To address this, researchers have developed Robust-RRT, a motion planning framework that integrates reachability analysis directly into the RRT process. This approach provides the first proof of probabilistic completeness for robust planning in nonlinear systems, ensuring that if a safe path exists, the algorithm will eventually find it.

The Limitations of Heuristic Padding

A common engineering workaround for uncertainty is to "pad" obstacles, effectively making them larger to create a safety buffer. However, this method is often either too risky or too conservative. Insufficient padding leads to collisions, while excessive padding can make a feasible environment appear impassable, causing the planner to fail. Furthermore, many existing robust planners struggle with unstable nonlinear systems that require active feedback to remain on course.

The Robust-RRT Framework

Robust-RRT moves beyond planning for a single "nominal" trajectory. Instead, it plans using reachable sets—volumes in state space that encompass all possible positions the robot could occupy at a given time, given the range of possible uncertainties.

Dual-Tree Architecture

Calculating distances between sets (e.g., Hausdorff distances) is computationally expensive. To maintain the efficiency of the RRT's nearest-neighbor search, Robust-RRT employs a dual-tree structure:

  1. Nominal Tree: A tree of points used for efficient spatial indexing and exploration guidance.
  2. Reachable Set Tree: A parallel structure that tracks the actual uncertainty "funnels" to ensure every branch is safety-guaranteed.

This "Nominal Guidance" insight allows the algorithm to retain the exploration bias of standard RRT while the reachable sets handle the rigorous safety checks.

RandUP-RRT: A Practical Implementation

Since exact reachable sets are often impossible to compute for complex nonlinear dynamics, the authors propose RandUP-RRT. This variant uses Randomized Uncertainty Propagation, approximating the reachable set as the convex hull of multiple random simulations (particles). This makes the algorithm compatible with black-box physics engines and complex contact dynamics.

Theoretical Foundations: The Growing Funnel

The core theoretical contribution is the proof of probabilistic completeness (PC). By demonstrating that the algorithm constructs a sequence of "growing funnels," the authors show that as long as the system's dynamics are Lipschitz continuous, a sufficiently dense sampling of the control space will eventually contain a "tube" of safe trajectories that reaches the goal.

Experimental Evaluation

The algorithm was tested across three challenging simulated environments:

  • Nonlinear Quadrotor: Navigating a cluttered space with unknown drag coefficients.
  • Planar Pusher: A box-pushing task in the PyBullet physics engine involving complex contact physics and random force disturbances.
  • Hybrid Jumping Robot: A system with uncertain timing in transitions between flight and contact modes.

Key Results

In the quadrotor benchmark, Robust-RRT achieved 100% validity (zero crashes) across 10,000 test runs. In contrast, a standard RRT with 0.3m of padding only achieved a 50% success rate. In the Planar Pusher task, the algorithm demonstrated "Safety without Conservatism." While a standard planner might attempt a risky shortcut through a narrow gap, Robust-RRT correctly identified the risk and opted for a longer, safer route.

While Robust-RRT is computationally more intensive—ranging from 2x to 22x slower than standard RRT due to the multiple simulations required per node—this is a necessary trade-off for a safety guarantee. Furthermore, the particle-based simulations in RandUP-RRT are highly parallelizable, offering a path toward improved performance.

Conclusion and Future Directions

Robust-RRT represents a significant step forward in bringing theoretical safety guarantees to complex, nonlinear robotic systems. By replacing point-based nodes with reachable sets and utilizing a dual-tree structure, it balances exploration efficiency with robust safety.

Future work may address the current limitation of the convex hull assumption in RandUP-RRT, which might struggle with highly non-convex distributions. Additionally, extending the framework to account for sensor noise (belief space planning) would further bridge the gap between simulation and real-world deployment.

Summary updated

BibTeX
@inproceedings{wu2022robust,
  author = {Albert Wu and Thomas Lew and Kiril Solovey and Edward Schmerling and Marco Pavone},
  title = {{Robust-RRT: Probabilistically-Complete Motion Planning for Uncertain Nonlinear Systems}},
  booktitle = {ISRR},
  year = {2022},
  url = {https://openreview.net/forum?id=RM6_uF-OZK9},
}
Figure