Constrained Gaussian Process Motion Planning via Stein Variational Newton Inference

TL;DR

This paper tackles constrained motion planning where traditional GPMP struggles with hard nonlinear constraints and full Bayesian mixing. It introduces Constrained Stein Variational GPMP (cSGPMP), which uses a nonlinear GPMP prior via Hilbert-space GP (HSGP) and equality-constrained Stein Variational Newton (SVN) updates within a Sequential Quadratic Programming framework to enforce hard constraints efficiently. Key contributions include a general nonlinear GP prior with analytic joint distributions for position and velocity, and a constrained SVGD/SVN machinery with a KKT-based solution that scales via block-diagonal approximations. Empirical results on a nonholonomic unicycle task and the Panda MBM benchmark show near 98% success and significant speedups over baselines, demonstrating robust, multimodal trajectory reasoning under tight compute budgets for real-world robotics.

Abstract

Gaussian Process Motion Planning (GPMP) is a widely used framework for generating smooth trajectories within a limited compute time--an essential requirement in many robotic applications. However, traditional GPMP approaches often struggle with enforcing hard nonlinear constraints and rely on Maximum a Posteriori (MAP) solutions that disregard the full Bayesian posterior. This limits planning diversity and ultimately hampers decision-making. Recent efforts to integrate Stein Variational Gradient Descent (SVGD) into motion planning have shown promise in handling complex constraints. Nonetheless, these methods still face persistent challenges, such as difficulties in strictly enforcing constraints and inefficiencies when the probabilistic inference problem is poorly conditioned. To address these issues, we propose a novel constrained Stein Variational Gaussian Process Motion Planning (cSGPMP) framework, incorporating a GPMP prior specifically designed for trajectory optimization under hard constraints. Our approach improves the efficiency of particle-based inference while explicitly handling nonlinear constraints. This advancement significantly broadens the applicability of GPMP to motion planning scenarios demanding robust Bayesian inference, strict constraint adherence, and computational efficiency within a limited time. We validate our method on standard benchmarks, achieving an average success rate of 98.57% across 350 planning tasks, significantly outperforming competitive baselines. This demonstrates the ability of our method to discover and use diverse trajectory modes, enhancing flexibility and adaptability in complex environments, and delivering significant improvements over standard baselines without incurring major computational costs.

Figures (8)

• Figure 1: The figure illustrates a real-world demonstration of the robot executing two distinct trajectory modes identified by our planner to achieve the same task. The solid line represents the motion of the Panda hand.
• Figure 2: We set $x_0$ to be $p(x_0) \sim \mathcal{N}(2, 10^{-4})$. The kernel for velocity is Matérn 3/2. The GP prior represents the distribution without any observations, while the GP posterior is computed using asynchronous observations of position and velocity. The velocity noise $\sigma_n^2$ is also set to $10^{-4}$. This process is regarded as a nonlinear continuous-time diffusion starting from $x(0) = 2$. With observations, the diffusion uncertainty is reduced.
• Figure 3: Particle trajectories for cSVGD and cSVN. The red ellipses represent an equality constraint, while the underlying distribution is a correlated Gaussian, shown in the background. The particles are initialized with the same random seed, and a small update rate of $3e^{-5}$ is used to smooth the trajectories.
• Figure 4: The left plot illustrates the convergence of the objective function, while the right plot depicts the convergence of the constraints. The constraint violation and iteration number are plotted on a logarithmic scale.
• Figure 5: The unicycle problem involves maneuvering the unicycle from the starting pose, represented by the green triangle, to the target pose, represented by the yellow triangle. Pink circles indicate obstacles. Trajectories with high log-likelihood are shown in dark blue, while those with low log-likelihood appear in light blue. The best trajectory is highlighted in red. Both methods yield a similar distribution of particles that satisfy the non-holonomic constraints. However, cSVN identifies a trajectory with a higher log-likelihood than cSVGD.