Technical Summary: PRISM - Efficient and Locally Optimal Probabilistic Planning with Reachability Guarantees

Problem Statement

The paper addresses the fundamental challenge of belief-space planning for robotic systems operating under motion uncertainty, state constraints, and control constraints. Specifically, the problem involves steering a system from an initial Gaussian distribution $I = \mathcal{N}(\mu_I, \Sigma_I)$ to a goal Gaussian distribution $G = \mathcal{N}(\mu_G, \Sigma_G)$ while satisfying probabilistic (chance) constraints on states and controls.

Existing approaches face significant limitations:

• Sampling-based methods (e.g., FIRM, CS-BRM, RRT*) often struggle to achieve high coverage in high-dimensional belief spaces without requiring an impractically large number of nodes. They frequently lack finite-time completeness guarantees and may produce trajectories that violate constraints or are suboptimal due to the reliance on robust control techniques that inflate costs.
• Set-based methods (e.g., Graphs of Convex Sets) offer completeness guarantees for deterministic planning but face difficulties in establishing reachability within belief space convex sets.
• Covariance steering methods typically jointly optimize mean and covariance trajectories, making it difficult to analyze reachability separately and often resulting in high computational costs for multi-query planning.

The core difficulty lies in establishing reachability guarantees in constrained belief spaces, particularly determining if a state covariance can be shrunk or steered to a target while respecting actuator limits and obstacle avoidance.

Methodology: PRISM Algorithm

The authors propose PRISM (Precomputed Regions for Inflation-based Stochastic Motion planning), a multi-query planning algorithm that decomposes the belief-space planning problem into two distinct phases: covariance shrinking and deterministic mean steering.

1. Theoretical Foundation: Reachability of State Covariance

A key theoretical contribution is the derivation of new sufficient conditions for the reachability of state covariance under obstacle and actuator constraints.

• Shrinkability: The paper introduces the concept of "$\delta$-strictly shrinkable" covariance. It proves that if an initial covariance $\Sigma_1$ lies within specific bounds defined by a parameter $\delta$ and a time horizon $N$, it can be shrunk to a smaller isotropic covariance $cI$ in finite time while satisfying all constraints.
• Decoupling: These conditions allow PRISM to separate the planning problem. It first shrinks the system's uncertainty to a precomputed, constant, isotropic covariance ($c^{(N)}_{1,\delta}I$). Once the covariance is reduced to this "safe" level, the problem transforms into a deterministic mean steering problem with tightened constraints and inflated obstacles.

2. Offline Graph Construction

PRISM constructs a roadmap offline in the deterministic space:

• Constraint Tightening: Using the precomputed covariance trajectory, the algorithm tightens state and control constraints and inflates obstacles to ensure that any trajectory satisfying the deterministic constraints will satisfy the original probabilistic chance constraints.
• Space Decomposition: The safe state space (with tightened constraints) is decomposed into a union of overlapping convex polytopes.
• Graph Generation: Nodes are placed at the centroids of these polytopes. Edges are computed between intersecting polytopes by solving a deterministic mean steering problem (Problem V.3) to find feasible paths between centroids.
• Lifting: These deterministic edges are "lifted" back into belief space by combining the mean trajectory with the precomputed covariance trajectory.

3. Online Planning and Local Optimization

• Online Phase: Given a specific start and goal distribution, PRISM first attempts to steer the system to a stationary mean within a safe polytope where the covariance can be shrunk (if the initial state does not already satisfy shrinkability conditions). It then connects the start and goal distributions to the precomputed graph and performs a graph search to find a feasible path.
• Local Optimization: Once a feasible path is found, PRISM employs a novel online local optimization module. This module alternates between:
  1. Edge Shortening: Reducing the time allocation for individual edges.
  2. Shortcutting: Finding direct connections between non-adjacent nodes to eliminate unnecessary edges.
  3. Convex Optimization: Solving a convex semidefinite program (SDP) to optimize the mean and covariance trajectories for fixed time allocations and edge sequences.
     This process is guaranteed to be anytime (always returning a feasible solution) and improves the cost significantly.

Key Contributions

1. New Reachability Conditions: The paper establishes new sufficient conditions for the reachability of state covariance under obstacle and actuator constraints, enabling the separation of mean and covariance planning.
2. PRISM Algorithm: A multi-query planner that leverages these conditions to achieve high coverage without sampling the belief space directly, avoiding the inefficiencies of high-dimensional sampling.
3. Local Optimization Submodule: An online method that co-optimizes mean and covariance trajectories, demonstrating performance improvements of over $2.5\times$ compared to state-of-the-art methods.
4. Theoretical Guarantees: Proofs of soundness (all trajectories satisfy chance constraints) and completeness (guaranteed to find a path in finite time) under mild assumptions on the start and goal distributions (e.g., stationary means, bounded covariances).
5. Empirical Validation: Extensive simulations demonstrating superior coverage and lower cost compared to FIRM, CS-BRM, MAX-COVAR, MAX-COV-BALL, and RRT* variants.

Experimental Results

The authors evaluated PRISM on a 6-DoF quadrotor in 2D environments with varying corridor widths and cluttered obstacle fields.

• Coverage:
  • In confined office environments with narrow passages, PRISM achieved 100% coverage across all corridor widths, whereas sampling-based methods (RRT, RRT*, MAX-COV-BALL) saw coverage drop significantly as passages narrowed.
  • In cluttered environments with varying actuator noise, PRISM maintained 97–100% coverage even in "hard" scenarios (non-stationary means, large initial covariance) where all other methods achieved less than 45% coverage.
• Cost:
  • After local optimization, PRISM produced trajectories with substantially lower mean cost and cost variance.
  • In the cluttered environment, PRISM's median path cost was 97.9, compared to 110.8 for CS-BRM and 114.5 for RRT* (after their own local optimization).
  • PRISM's local optimization converged faster (median 5.93s) than other methods, largely because its global planner produces paths with fewer edges.
• Computation Time:
  • PRISM's offline graph construction was highly efficient (<1 minute for office environments, ~4-5 minutes for cluttered environments), significantly outperforming CS-BRM which required hours due to collision checking of thousands of edges.
  • Online path search times were comparable to the fastest baselines (~0.65s average).

Significance and Claims

The paper claims that PRISM represents a significant advance in constrained belief-space planning by addressing the trade-off between coverage and cost that plagues existing methods. By decoupling covariance shrinking from mean steering, PRISM avoids the "curse of history" and the inefficiency of high-dimensional sampling.

The authors emphasize that PRISM provides finite-time completeness guarantees for a large subset of the belief space, a property often lacking in sampling-based approaches. Furthermore, the inclusion of an efficient local optimization module ensures that the resulting trajectories are not only feasible but also locally optimal in terms of control cost. The results suggest that PRISM is particularly well-suited for complex, constrained environments where traditional sampling methods fail to find feasible paths or produce excessively costly trajectories.