Safely: Safe Stochastic Motion Planning Under Constrained Sensing via Duality

TL;DR

This paper presents Safely, a receding-horizon motion planning framework that jointly optimizes robot trajectories and sensor strategies to ensure safety in environments with uncertain, dynamic obstacles under sensing constraints.

Contribution

It introduces a novel joint trajectory and sensor selection approach using duality in convex optimization, guaranteeing safety despite sensing limitations.

Findings

Successfully plans safe trajectories with probabilistic collision guarantees.

Demonstrates effectiveness through software and hardware experiments.

Optimizes sensor usage to focus on relevant obstacles.

Abstract

Consider a robot operating in an uncertain environment with stochastic, dynamic obstacles. Despite the clear benefits for trajectory optimization, it is often hard to keep track of each obstacle at every time step due to sensing and hardware limitations. We introduce the Safely motion planner, a receding-horizon control framework, that simultaneously synthesizes both a trajectory for the robot to follow as well as a sensor selection strategy that prescribes trajectory-relevant obstacles to measure at each time step while respecting the sensing constraints of the robot. We perform the motion planning using sequential quadratic programming, and prescribe obstacles to sense based on the duality information associated with the convex subproblems. We guarantee safety by ensuring that the probability of the robot colliding with any of the obstacles is below a prescribed threshold at every time step of the planned robot trajectory. We demonstrate the efficacy of the Safely motion planner through software and hardware experiments.