We consider the motion planning problem under uncertainty and address it
using probabilistic inference. A collision-free motion plan with linear
stochastic dynamics is modeled by a posterior distribution. Gaussian
variational inference is an optimization over the path distributions to infer
this posterior within the scope of Gaussian distributions. We propose Gaussian
Variational Inference Motion Planner (GVI-MP) algorithm to solve this Gaussian
inference, where a natural gradient paradigm is used to iteratively update the
Gaussian distribution, and the factorized structure of the joint distribution
is leveraged. We show that the direct optimization over the state distributions
in GVI-MP is equivalent to solving a stochastic control that has a closed-form
solution. Starting from this observation, we propose our second algorithm,
Proximal Gradient Covariance Steering Motion Planner (PGCS-MP), to solve the
same inference problem in its stochastic control form with terminal
constraints. We use a proximal gradient paradigm to solve the linear stochastic
control with nonlinear collision cost, where the nonlinear cost is iteratively
approximated using quadratic functions and a closed-form solution can be
obtained by solving a linear covariance steering at each iteration. We evaluate
the effectiveness and the performance of the proposed approaches through
extensive experiments on various robot models. The code for this paper can be
found in https://github.com/hzyu17/VIMP.Comment: 19 page