An Efficient Motion Planning Algorithm for Stochastic Dynamic Systems with Constraints on Probability of Failure

Masahiro Ono, Brian C. WilliamsAAAI 2008

Introduction

In real-world robotics, such as an Autonomous Underwater Vehicle (AUV) mapping a rugged seafloor, operators face a fundamental trade-off between mission performance and safety. To capture high-resolution data, an AUV must navigate close to the terrain; however, stochastic disturbances like unpredictable currents significantly increase the risk of collision. This paper introduces Bi-stage Robust Motion Planning (BRMP), an efficient algorithm designed to manage this trade-off by treating "risk" as a limited resource to be intelligently allocated across a mission.

The Challenge of Chance Constraints

Traditional approaches to safety often rely on chance constraints, which require that the total probability of failure over a mission remains below a specified threshold (e.g., < 0.1%). Solving these constraints is computationally intensive. Existing methods typically fall into two categories:

  1. Conservative Methods: These spread risk uniformly, leading to excessively large safety margins that degrade performance (e.g., Ellipsoidal relaxation).
  2. Sample-based Methods: These use simulations (Particle Control) that are computationally expensive and lack strict safety guarantees for finite sample sizes.

BRMP provides a middle ground: a fast, real-time algorithm that ensures strict safety guarantees without being overly conservative.

The BRMP Framework

The authors propose a two-stage optimization architecture that separates the management of risk from the physical pathfinding process.

The BRMP architecture separates risk allocation from deterministic path planning.

Two-Stage Architecture

  • The Upper Stage (Risk Allocation): This stage manages the "risk budget" using a novel Iterative Risk Allocation (IRA) algorithm. Instead of a uniform distribution, IRA decides how much probability of failure to "spend" at each time step. It intelligently shifts risk from "easy" segments of the path to "hard" segments, such as narrow canyons or sharp corners, where tighter navigation is required.
  • The Lower Stage (Deterministic Planning): Once the risk budget for a specific time step is fixed, the problem is transformed into a standard deterministic motion planning problem. The algorithm calculates a physical "safety margin" based on the allocated risk and plans a path that remains outside that margin using tools like Mixed-Integer Linear Programming (MILP).
Visualization of the nominal path and safety margins generated by the BRMP algorithm.

Key Insights and Methodology

  • Risk as a Currency: By treating the probability of failure as a resource, the IRA algorithm allows the robot to take calculated risks where they provide the most benefit to the objective function.
  • Decomposition via Union Bound: The paper utilizes Boole’s inequality (the union bound) to decompose a complex global mission constraint into simpler, individual constraints for each time step.
  • Monotonic Improvement: The IRA algorithm is mathematically guaranteed to improve (or maintain) mission performance with every iteration, ensuring a reliable convergence to a solution.

Experimental Results

The BRMP algorithm was evaluated using simulations of a Dorado-class AUV mission using real bathymetric data from the Monterey Bay Aquarium Research Institute (MBARI). Compared against Ellipsoid Relaxation (ER) and Particle Control (PC) over 50 seafloor segments, the results were conclusive:

  • Optimality: BRMP significantly outperformed ER. While ER showed suboptimality up to 57%, BRMP reduced this gap to at most 14%.
  • Computational Efficiency: BRMP is approximately 1,000 times faster than Particle Control. While PC required over 15 minutes for a 100-second horizon, BRMP completed the task in roughly 1.36 seconds.
  • Safety Compliance: Unlike Particle Control, which occasionally violated failure limits in testing, BRMP strictly satisfied all chance constraints in every test case.

Conclusion

BRMP represents a significant step forward in robust motion planning. By decoupling risk allocation from pathfinding, it enables robots to operate at the edge of their safety envelopes with high computational efficiency. Future work may focus on extending this framework to non-linear dynamics and non-Gaussian uncertainty distributions.

Summary updated

BibTeX
@inproceedings{ono2008efficient,
  author = {Masahiro Ono and Brian C. Williams},
  title = {{An Efficient Motion Planning Algorithm for Stochastic Dynamic Systems with Constraints on Probability of Failure}},
  booktitle = {AAAI Conference on Artificial Intelligence},
  year = {2008},
}
Figure