Risk Density RRT$^*$: Planning in the Space of Probability

Lorenzo Paiola, Giorgio Grioli, Antonio BicchiarXiv 2026 · DOI · arXiv

TL;DR

RD-RRT* is a novel motion planning algorithm that treats collision probability as the primary optimization objective rather than a secondary constraint. By leveraging a computationally efficient "Risk Density" functional, it finds paths that are significantly safer than traditional geometric planners while remaining fast enough for high-level robotics applications.

What Problem Does This Paper Address?

In robotics, planning under uncertainty—specifically when obstacle positions are known only probabilistically (e.g., a human's predicted path)—is usually handled via Chance Constraints (CCs). These methods seek the shortest path where the probability of collision stays below a certain threshold.

However, this approach has two major flaws:

  1. Conservatism/Inaccuracy: Summing probabilities along a path often overestimates risk, leading to "frozen robot" scenarios.
  2. Myopia: Local constraints don't necessarily lead to globally safe paths. A series of "safe enough" steps can lead a robot into a high-risk "trap" that a global risk-minimizing objective would have avoided.

The authors propose moving from "planning with constraints" to "planning in the space of probability," where the goal is to directly minimize the total risk accumulated over the entire trajectory.

Methods

The core of the paper is the Risk Density RRT (RD-RRT). It extends the classic RRT algorithm by replacing Euclidean distance with a cost derived from the Risk Density functional*.

Key Innovations:

  • Risk Density Functional: The authors derive a first-order Taylor approximation of collision probability. For a robot and obstacles modeled as disks with Gaussian uncertainty, they provide a closed-form solution for the risk of a straight-line path segment. This allows the planner to evaluate thousands of potential edges per second.
  • Interdependence Hypotheses: The paper explores how risk accumulates. It compares three hypotheses: H1 (complete independence), H2 (Markov property on swept area), and H3 (stopped process/static uncertainty). They demonstrate that for small objects, H2 and H3 converge to the same functional, providing a robust theoretical foundation.
Figure 1. Visual representation of how considered assumptions work on distinct events. Left: The events {ω ∈Ω| wd(ω, s0) ∈A} and {ω ∈Ω| wd(ω, s1) ∈B}, where the sets A and B are shown as red and blue disks. P0(·) and P1(·) stand respectively for the probability measure of the first and the second random variable to be inside a set. Right: Graphical illustration of the collision events considered respectively in (H1) Eq. (9), (H2) Eq. (12) and (H3) Eq. (13).
  • Hybrid & Severity Variants: The framework supports a Risk-Based variant (where different obstacles have different "severities," like a hospital vs. a trash can) and a Hybrid variant that adds a small path-length penalty to regularize exploration in low-risk regions.

Datasets

The authors evaluated their algorithm using several custom-designed benchmarks:

  • Default Scenario: A standard environment to validate core performance.
  • Local Minimum Scenario: Specifically designed to create a conflict between geometric distance and probabilistic safety.
  • Risk Trap Scenario: A "funnel" that leads myopic planners into a high-risk narrow passage.
  • Complex Random Scenarios: Procedurally generated environments with up to 30 uncertain obstacles to test scalability.
  • MuJoCo Simulation: A physics-based validation using a model of the Stanford TidyBot.

Key Insights and Ideas

  • The "Self-Reinforcing" Accuracy: The Risk Density approximation is most accurate in the "tails" of the probability distributions (far from the center of obstacles). Since the planner's objective is to find low-risk paths, it naturally gravitates toward regions where its own mathematical approximation is most valid.
  • Risk is a Semimetric: Unlike Euclidean distance, the risk cost violates the triangle inequality. A direct line through a high-uncertainty zone can "cost" more than a longer, winding detour through clear space.
  • Regularization is Mandatory: In regions very far from obstacles, the risk gradient becomes nearly zero (flat). Without the small path-length penalty in the Hybrid variant, the planner can experience "rewiring catastrophes" where it stalls trying to optimize negligible cost differences.
  • Simple Exploration Wins: Surprisingly, simple uniform sampling outperformed more complex, risk-aware sampling strategies. The authors suggest that the quality of the cost function itself is more important than the sophistication of the sampling strategy.

Key Results

  • Superior Safety: In the "Risk Trap" scenario, RD-RRT found paths with a ~7% collision probability, whereas standard RRT (using obstacle inflation) was drawn into a trap with ~24% risk.
  • High Fidelity: Monte Carlo simulations (10,000 trials) confirmed that the Risk Density approximation is highly accurate, showing a strong linear correlation with ground-truth collision frequencies.
  • Scalability: The algorithm exhibits near-linear time complexity relative to the number of uncertain obstacles, making it viable for complex, real-world environments.
  • Physics Validation: In MuJoCo rollouts, the predicted risk closely matched the empirical collision rates of a simulated physical robot, despite the planner using simplified geometric abstractions.

Limitations and Future Work

  • Asymptotic Optimality: While RRT is traditionally asymptotically optimal, the authors do not formally establish this for RD-RRT due to the mismatch between the non-metric cost space and the Euclidean neighbor search used for efficiency.
  • Robot Dynamics: The current model assumes a kinematic robot and does not yet account for how the robot's own control uncertainty propagates over time.
  • Task Space: The implementation is currently limited to Euclidean spaces. Extending this to non-Euclidean manifolds (like high-DOF robot arms) is a key next step.
  • Dynamic Obstacles: While the math handles uncertain positions, it doesn't yet explicitly model time-varying uncertainty or moving obstacles in a time-extended way.

Summary updated

Abstract

Planning under uncertainty is a fundamental problem in robotics. While much of the literature seeks minimum-cost paths subject to probabilistic constraints, this paper instead considers collision probability, or risk classically defined as probability times severity, directly as the planning objective. We propose Risk Density RRT$^*$ (RD-RRT$^*$), an extension of RRT$^*$ for high-level planning in environments with deterministic and uncertain obstacles. RD-RRT$^*$ builds on Risk Density, a functional that evaluates a continuous path as a whole and provides a tractable approximation of its collision probability. The framework also supports severity-weighted risk and a hybrid variant that adds a path-length penalty, balancing risk and geometric efficiency while regularizing exploration in low-density regions. We validate the method across a wide range of scenarios. The results show that RD-RRT$^*$ consistently finds paths with lower estimated collision probability by optimizing a global risk-based objective. We further demonstrate the fidelity of the underlying approximation through Monte Carlo simulations. Finally, we discuss how the topological structure induced by Risk Density may provide a useful representation of the environment and offer insights into risk-aware planning.
BibTeX
@misc{paiola2026risk,
  author = {Lorenzo Paiola and Giorgio Grioli and Antonio Bicchi},
  title = {{Risk Density RRT$^*$: Planning in the Space of Probability}},
  year = {2026},
  eprint = {2611.22654},
  archivePrefix = {arXiv},
  url = {https://arxiv.org/abs/2611.22654},
}
Figure