🤖 AI Summary
This work addresses motion planning under uncertainty by modeling the optimal trajectory as a posterior distribution and proposing Gaussian Variational Inference for Motion Planning (GVI-MP), an efficient variational approximation framework. Methodologically, it establishes, for the first time, a theoretical duality between motion planning and variational inference. Building upon this insight, it introduces a dual-algorithm architecture: Natural Gradient-based Factor Graph Optimization (NGFGO) and Proximity-Constrained Steering with terminal constraints and covariance guidance (PCS-MP). The approach integrates sparse factor graph modeling, covariance-guided control, and quadratic approximations of nonlinear costs. Extensive evaluation across diverse robotic platforms demonstrates robustness and real-time performance—solutions are computed in milliseconds. An open-source implementation is provided to facilitate scalable deployment.
📝 Abstract
We present a novel formulation for motion planning under uncertainties based on variational inference where the optimal motion plan is modeled as a posterior distribution. We propose a Gaussian variational inference-based framework, termed Gaussian Variational Inference Motion Planning (GVI-MP), to approximate this posterior by a Gaussian distribution over the trajectories. We show that the GVI-MP framework is dual to a special class of stochastic control problems and brings robustness into the decision-making in motion planning. We develop two algorithms to numerically solve this variational inference and the equivalent control formulations for motion planning. The first algorithm uses a natural gradient paradigm to iteratively update a Gaussian proposal distribution on the sparse motion planning factor graph. We propose a second algorithm, the Proximal Covariance Steering Motion Planner (PCS-MP), to solve the same inference problem in its stochastic control form with an additional terminal constraint. We leverage a proximal gradient paradigm where, at each iteration, we quadratically approximate nonlinear state costs and solve a linear covariance steering problem in closed form. The efficacy of the proposed algorithms is demonstrated through extensive experiments on various robot models. An implementation is provided in https://github.com/hzyu17/VIMP.
