SDP Synthesis of Maximum Coverage Trees for Probabilistic Planning under Control Constraints

TL;DR

This work describes formally the notion of coverage of a roadmap in this stochastic domain via introduction of the h-BRS (Backward Reachable Set of Distributions) of a tree of distributions under control constraints, and supports the method with extensive simulations on a 6 DoF model.

Abstract

The paper presents Maximal Covariance Backward Reachable Trees (MAXCOVAR BRT), which is a multi-query algorithm for planning of dynamic systems under stochastic motion uncertainty and constraints on the control input with explicit coverage guarantees. In contrast to existing roadmap-based probabilistic planning methods that sample belief nodes randomly and draw edges between them \cite{csbrm_tro2024}, under control constraints, the reachability of belief nodes needs to be explicitly established and is determined by checking the feasibility of a non-convex program. Moreover, there is no explicit consideration of coverage of the roadmap while adding nodes and edges during the construction procedure for the existing methods. Our contribution is a novel optimization formulation to add nodes and construct the corresponding edge controllers such that the generated roadmap results in provably maximal coverage under control constraints as compared to any other method of adding nodes and edges. We characterize formally the notion of coverage of a roadmap in this stochastic domain via introduction of the h-$\operatorname{BRS}$ (Backward Reachable Set of Distributions) of a tree of distributions under control constraints, and also support our method with extensive simulations on a 6 DoF model.

Figures (4)

• Figure 1: An illustration of the recursive satisfaction of constraints (goal distribution reaching constraint, and chance constraints on the state and control input) for the overall trajectory initialized at the $(\mu_{i}, \Sigma_{i})$ distribution through sequential composition of two $N$-step controllers, $\mathscr{C}_{i,j}$ (designed for the $i \rightarrow j$ maneuver) and $\mathscr{C}_{j,k}$ (designed for the $j_{\mathrm{max}} \rightarrow k$ maneuver).
• Figure 2: Coverage experiment setup: MAXCOVAR and RANDCOVAR BRTs
• Figure 3: Coverage experiment metrics
• Figure 4: Real-time planning through the pre-computed MAXCOVAR BRT

Theorems & Definitions (9)

• Remark 1
• Lemma 1
• proof
• Lemma 2
• proof
• Theorem 1: Maximum Coverage
• proof
• proof : Proof of Lemma \ref{['lemma:sigma_brs_lemma']}
• proof : Proof of Theorem \ref{['theorem:coverage']}