PRISM: Efficient and Locally Optimal Probabilistic Planning with Reachability Guarantees

Belief-space planning under motion uncertainty and state and control constraints remains a fundamental challenge, largely due to the difficulty of establishing reachability guarantees in constrained belief spaces. Existing constrained belief-space planners rely on sampling to construct multi-query belief roadmaps and explicitly find feasible trajectories between sampled nodes to establish reachability. These methods often struggle to cover the belief space or use robust control techniques that improve coverage at the cost of indirect, high-cost trajectories; they also lack finite-time or finite-memory completeness guarantees. We propose PRISM, a multi-query motion planning algorithm for belief spaces with state and control constraints that targets both high coverage and low cost. We present a new result on controllability of the state covariance under constraints, which is used by PRISM to decompose belief-space planning into deterministic mean planning and covariance shrinking. PRISM further includes an online local optimization method that reduces the cost of feasible belief-space trajectories. Under mild assumptions on the start and goal distributions, we prove that PRISM guarantees full coverage (i.e. completeness) despite actuator and obstacle constraints. In challenging simulated scenarios, PRISM achieves substantially higher roadmap coverage than state-of-the-art belief-space planning methods while producing trajectories with lower mean cost and cost variance. For example, PRISM achieves 100% coverage in easy and medium-difficulty scenarios, and, in the hardest scenario, which violates PRISM's coverage assumptions, it still achieves 97-100% coverage, while all other methods achieve less than 45%.