Efficient Iterative Proximal Variational Inference Motion Planning

Abstract: Motion planning under uncertainty can be cast 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, we propose a proximal algorithm to solve for an optimal Gaussian proposal iteratively. The computational bottleneck is evaluating the gradients with respect to the proposal over a dense trajectory. We exploit the sparse motion planning factor graph and Gaussian Belief Propagation (GBP), allowing for parallel computing of these gradients on Graphics Processing Units (GPUs). We term the novel paradigm as the 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 motion planning for nonlinear stochastic systems, where the P-GVIMP serves as a sub-routine for the linearized time-varying system. We validate the proposed framework 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 https://github.com/hzyu17/VIMP.