Efficient Iterative Proximal Variational Inference Motion Planning

TL;DR

This paper introduces a fast, parallelized variational inference method for motion planning under uncertainty, applicable to both linear and nonlinear stochastic systems, leveraging GPU acceleration and Gaussian Belief Propagation.

Contribution

It proposes P-GVIMP, a novel parallel Gaussian variational inference framework for efficient motion planning, including an iterative approach for nonlinear systems using SLR techniques.

Findings

Significant speed improvements with GPU-based parallel computation.

Successful planning for nonlinear stochastic systems.

Open-source implementation available.

Abstract

We cast motion planning under uncertainty as a stochastic optimal control problem, where the optimal posterior distribution has an explicit form. To approximate this posterior, this work frames an optimization problem in the space of Gaussian distributions by solving a Variational Inference (VI) in the path distribution space. For linear-Gaussian stochastic dynamics, a proximal algorithm is proposed to solve for an optimal Gaussian proposal iteratively. The computational bottleneck is evaluating the gradients with respect to the proposal over a dense trajectory. To tackle this issue, the sparse planning factor graph and Gaussian Belief Propagation (GBP) are exploited, allowing for parallel computation of these gradients on Graphics Processing Units (GPUs). We term the novel paradigm the \textit{Parallel Gaussian Variational Inference Motion Planning (P-GVIMP)}. Building on the efficient algorithm for linear Gaussian systems, we then propose an iterative paradigm based on Statistical Linear Regression (SLR) techniques to solve planning problems for nonlinear stochastic systems, where the P-GVIMP serves as a sub-routine for the linearized time-varying system at each iteration. The proposed framework is validated on various robotic systems, demonstrating significant speed acceleration achieved by leveraging parallel computation and successful planning solutions for nonlinear systems under uncertainty. An open-sourced implementation is presented at \href{https://github.com/hzyu17/VIMP}{https://github.com/hzyu17/VIMP}.