Safely: Safe Stochastic Motion Planning Under Constrained Sensing via Duality

Michael Hibbard, Abraham P. Vinod, Jesse Quattrociocchi, Ufuk Topcu

Abstract

Robots navigating real-world environments must contend with moving obstacles whose future positions are uncertain. Traditional approaches often lead to overly conservative behavior or fail under sensing constraints. This paper introduces Safely, a motion planning framework that leverages duality theory to identify "trajectory-relevant" obstacles. By prioritizing limited sensing resources on obstacles that most impact the optimal path, Safely ensures safety while maintaining operational efficiency.

Introduction

In dynamic environments, predicting the trajectories of pedestrians or other vehicles is inherently stochastic. These predictions typically involve "uncertainty bubbles"—ellipsoidal regions where an obstacle is likely to be. To guarantee safety, a robot must treat these bubbles as keep-out zones. However, large uncertainty leads to large keep-out zones, forcing the robot into inefficient, circuitous paths.

While sensing can reduce this uncertainty, physical robots face constrained sensing: limited battery, CPU cycles, or sensor fields-of-view prevent tracking every obstacle simultaneously. The Safely framework addresses the critical question: Which obstacles should the robot sense to maximize performance?

Methodology

Safely frames motion planning as a constrained optimization problem solved via Sequential Quadratic Programming (SQP).

Probabilistic Safety Constraints

The framework converts probabilistic collision constraints into ellipsoidal keep-out sets. By ensuring the robot remains outside these ellipsoids, the probability of collision is kept below a user-defined threshold $\alpha$.

Duality and Sensitivity Analysis

The core innovation of Safely lies in its use of dual variables (Lagrange multipliers) generated during the optimization process. These variables represent the sensitivity of the path's cost to each obstacle constraint. Obstacles with high dual variables are deemed "relevant" because reducing their uncertainty would significantly improve the robot's trajectory.

Figure 4: Multi-obstacle scenario where the robot $\mathcal{R}$ must navigate toward a goal while managing uncertainty in obstacles $\mathcal{O}_1, \mathcal{O}_2,$ and $\mathcal{O}_3$. Duality helps identify which obstacle is most relevant to the current plan.

Sensor Scheduling and Control

The robot identifies the top $K$ relevant obstacles, schedules measurements to update its belief via a Kalman Filter, and re-plans its trajectory. This process repeats in a Receding Horizon Control loop. In nonlinear cases, the robot may even deviate slightly from the shortest path to bring a critical obstacle into its camera's field of view.

Figure 6: Illustration of how a measurement reduces the uncertainty ellipsoid. The "Before measurement" state (red) represents a larger keep-out zone compared to the "After measurement" state (blue), allowing for a more flexible path.

Evaluation and Results

The authors evaluated Safely through numerical simulations, nonlinear car-like models, and hardware experiments using Turtlebot 3 robots.

  • Relevance-Driven Efficiency: Safely reached targets significantly faster than non-sensing baselines by shrinking only the obstacles that blocked the optimal path.
  • Robustness: Unlike standard Belief-Space Planning (BSP), which may try to reduce uncertainty globally, Safely focuses on obstacles that actually impede progress, avoiding "pinched" scenarios where a robot gets stuck between large uncertainty bubbles.
  • Real-Time Performance: The algorithm is computationally lightweight. In hardware tests, the average computation time was 0.139 seconds, well within the requirements for real-time control at 4Hz.

Conclusion

Safely demonstrates that "relevance" is a more powerful metric for sensor allocation than raw uncertainty. By using duality as a "free" heuristic from the optimization solver, the framework enables robots to navigate complex, uncertain environments safely and efficiently under strict hardware constraints. Future work will explore extending this to non-Gaussian obstacle dynamics and more complex geometric shapes.

Summary updated

BibTeX
@article{hibbard2023safely,
  author = {Michael Hibbard and Abraham P. Vinod and Jesse Quattrociocchi and Ufuk Topcu},
  title = {{Safely: Safe Stochastic Motion Planning Under Constrained Sensing via Duality}},
  journal = {IEEE Trans. Robotics},
  year = {2023},
  doi = {10.1109/tro.2023.3297018},
}
Figure