SDP Synthesis of Distributionally Robust Backward Reachable Trees for Probabilistic Planning

TL;DR

This work addresses multi-query stochastic planning under control-input constraints by constructing a backward reachable tree of ambiguity sets, instead of relying on a traditional distribution roadmap. It introduces MAXELLIPSOID BRT and distributionally robust edge controllers (MAXELLIOID and MAXCOVARELL), solved via a nonlinear program with a convex SDP relaxation to enable efficient, robust steering between ambiguity sets. A maximum coverage theorem shows that MAXCOVAR-style expansions achieve the largest possible backward reachability, ensuring broad applicability across initial conditions. Experiments on a 6-DoF quadrotor model demonstrate that MAXELLIPSOID yields dramatically smaller, faster-to-construct roadmaps with strong real-time planning performance when concatenating offline edge controllers.

Abstract

The paper presents Maximal Ellipsoid Backward Reachable Trees MAXELLIPSOID BRT, which is a multi-query algorithm for planning of dynamic systems under stochastic motion uncertainty and constraints on the control input. In contrast to existing probabilistic planning methods that grow a roadmap of distributions, our proposed method introduces a framework to construct a roadmap of ambiguity sets of distributions such that each edge in our proposed roadmap provides a feasible control sequence for a family of distributions at once leading to efficient multi-query planning. Specifically, we construct a backward reachable tree of maximal size ambiguity sets and the corresponding distributionally robust edge controllers. Experiments show that the computation of these sets of distributions, in a backwards fashion from the goal, leads to efficient planning at a fraction of the size of the roadmap required for state-of-the-art methods. The computation of these maximal ambiguity sets and edges is carried out via a convex semidefinite relaxation to a novel nonlinear program. We also formally prove a theorem on maximum coverage for a technique proposed in our prior work.

Figures (3)

• Figure 1: Recursive feasibility through sequential composition (refer to Lemma \ref{['lemma:sequential_composition']} for a formalization). An illustration of the satisfaction of constraints (goal reaching constraint, and chance constraints on the state and control input) for a $2N$-step trajectory initialized at the $(\mu_{i}, \Sigma_{i})$ distribution driven through a sequential composition of two $N$-step controllers, $\mathscr{C}_{i,j}$ designed for the $(\mathscr{R}_{i}, \Sigma_{i}) \rightarrow (\mathscr{R}^{-}_{j}, \Sigma^{-}_{j})$ maneuver, and $\mathscr{C}_{j,k}$ designed for the $(\mathscr{R}_{j}, \Sigma_{j}) \rightarrow (\mathscr{R}_{k}, \Sigma_{k})$ maneuver where $\mathscr{R}^{-}_{j} \subset \mathscr{R}_{j}$ and $\Sigma^{-}_{j} \prec \Sigma_{j}$. The distribution $(\mu_{i}, \Sigma_{i})$ is steered to the goal $(\mathscr{R}_{k}, \Sigma_{k})$ in $2N$-steps under the concatenated controller such that all chance constraints on the state and control input are satisfied for the $2N$-step trajectory.
• Figure 2: The above figure show the three backward reachable trees that were used for our planning experiment: $\operatorname{MAXELLIPSOID}$ (15 nodes added in 15 iterations, less than 1 min to compute), $\operatorname{MAXCOVAR}$ (382 nodes added in 1000 iterations, around 16 mins to compute), and $\operatorname{RANDCOVAR}$ (255 nodes added in 3500 iterations, more than 1 hour to compute). For all the three subplots, each blue dot on the plot represents the first two dimensions of the node mean for $\operatorname{MAXCOVAR}$ and $\operatorname{RANDCOVAR}$ trees, and the center of the ellipsoidal mean ambiguity set for the $\operatorname{MAXELLIPSOID}$ tree. Additionally for the $\operatorname{MAXELLIPSOID}$ subplot, Fig. \ref{['fig:three_trees']} shows the 2D projections of the 6D ellipsoidal regions corresponding to the mean ambiguity sets stored at each node. The directed edges between the nodes in each of the subplots represent the pre-computed $N$-step control sequences. Planning experiment: Query means were randomly sampled from the blue annulus and a query covariance of $0.2\mathbf{I}_{6}$ was used to attempt connects to the trees. The proposed $\operatorname{MAXELLIPSOID}$ shows superior performance as compared to the other two methods as summarized in Table \ref{['tab:exp1']}with a fraction of the number of nodes (15 nodes) and at a fraction of the time spent computing the tree (less than 1 minute) as compared to the other two methods.
• Figure 3: Real-time planning to the goal (origin) through the precomputed $\operatorname{MAXCOVAR}$ and $\operatorname{MAXELLIPSOID}$ BRTs respectively for a randomly sampled query distribution (red ellipse corresponds to the 3-$\sigma$ confidence interval of the sampled query distribution). The only real-time computation was the steering control sequence from the query distribution to the first node on the branch of the tree corresponding to the discovered feasible path. Black lines represent Monte-Carlo trajectories for the system evolution under the concatenated control sequence.

Theorems & Definitions (16)

• Proposition 3.1
• Theorem 3.1
• proof
• Remark 3.1
• Definition 3.1
• Definition 3.2
• Definition 3.3
• Lemma 4.1
• proof
• Lemma 5.1