Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

REVISE: Robust Probabilistic Motion Planning in a Gaussian Random Field

Alex Rose, Naman Aggarwal, Christopher Jewison, Jonathan P. How

Abstract

This paper presents Robust samplE-based coVarIance StEering (REVISE), a multi-query algorithm that generates robust belief roadmaps for dynamic systems navigating through spatially dependent disturbances modeled as a Gaussian random field. Our proposed method develops a novel robust sample-based covariance steering edge controller to safely steer a robot between state distributions, satisfying state constraints along the trajectory. Our proposed approach also incorporates an edge rewiring step into the belief roadmap construction process, which provably improves the coverage of the belief roadmap. When compared to state-of-the-art methods, REVISE improves median plan accuracy (as measured by Wasserstein distance between the actual and planned final state distribution) by 10x in multi-query planning and reduces median plan cost (as measured by the largest eigenvalue of the planned state covariance at the goal) by 2.5x in single-query planning for a 6DoF system. We will release our code at https://acl.mit.edu/REVISE/.

REVISE: Robust Probabilistic Motion Planning in a Gaussian Random Field

Abstract

This paper presents Robust samplE-based coVarIance StEering (REVISE), a multi-query algorithm that generates robust belief roadmaps for dynamic systems navigating through spatially dependent disturbances modeled as a Gaussian random field. Our proposed method develops a novel robust sample-based covariance steering edge controller to safely steer a robot between state distributions, satisfying state constraints along the trajectory. Our proposed approach also incorporates an edge rewiring step into the belief roadmap construction process, which provably improves the coverage of the belief roadmap. When compared to state-of-the-art methods, REVISE improves median plan accuracy (as measured by Wasserstein distance between the actual and planned final state distribution) by 10x in multi-query planning and reduces median plan cost (as measured by the largest eigenvalue of the planned state covariance at the goal) by 2.5x in single-query planning for a 6DoF system. We will release our code at https://acl.mit.edu/REVISE/.

Paper Structure

This paper contains 10 sections, 3 theorems, 17 equations, 3 figures, 2 tables, 2 algorithms.

Table of Contents

  1. INTRODUCTION
  2. PROBLEM STATEMENT
  3. PRELIMINARIES: COVARIANCE STEERING IN A GAUSSIAN RANDOM FIELD
  4. ROBUST SIGMA POINT METHOD FOR COVARIANCE STEERING
  5. BELIEF ROADMAP CONSTRUCTION
  6. EXPERIMENTS
  7. Multi-query Experiment
  8. Single-query Experiment
  9. CONCLUSION
  10. Proof Sketch of Theorem \ref{['thm: min_cov_coverage']}

Key Result

Theorem V.1

Consider two belief roadmaps $\mathcal{T}(\mathcal{I}, N, n_{\text{nodes}})$ and $\mathcal{T}^*(\mathcal{I}, N, n_{\text{nodes}})$, such that $\mathcal{T}$ is generated by Algorithm alg: no_rewiring and $\mathcal{T}^*$ is generated by Algorithm alg: edge_rewiring. Suppose both roadmaps are construct

Figures (3)

  • Figure 2: (a-b) Baseline covariance steering steers between Gaussian distributions ridderhof2022chanceaggarwal2024sdpzheng2024cs. (c-d) REVISE samples points on a Gaussian distribution, then steers a mixture of Gaussian distributions to a Gaussian distribution.
  • Figure 3: Distribution of Wasserstein distance between the planned and actual final distribution for 100 different goals reachable from the multi-query roadmap. Trials with $W_2$(plan, goal) $> 1$ are not shown.
  • Figure 4: Trajectories and final state distribution for the single query experiment. Trial 1 (with random seed 0) plotted for each method. Top left: Baseline trajectories, top right: REVISE trajectories, bottom left: baseline final state distribution, bottom right: REVISE final state distribution.

Theorems & Definitions (3)

  • Theorem V.1
  • Lemma VII.1
  • Lemma VII.2