Globalized Constrained Stein Variational Inference for Diverse Feasible Robot Motion Planning

2026-07-14

AI Summary

This work addresses the challenge of generating diverse, dynamically feasible robot trajectories under hard constraints such as collision avoidance, joint limits, contact conditions, and dynamics—constraints that traditional planners often satisfy only by producing a single solution. The authors propose SteinSQP, the first method to deeply integrate constrained optimization with Stein variational inference. By formulating sequential quadratic programming (SQP) subproblems in a reproducing kernel Hilbert space, SteinSQP leverages an interacting particle ensemble to evolve fully feasible and diverse trajectory samples. A GPU-friendly, matrix-free primal-dual algorithm efficiently solves the resulting constrained Stein–Newton subproblems, while an ensemble-level value function balances optimality, constraint satisfaction, and diversity. Experiments across five constrained motion planning tasks demonstrate that SteinSQP significantly outperforms first-order constrained baselines and multi-start nonlinear programming approaches in convergence speed, particle feasibility, and batch-solving efficiency.

Abstract

Robot motion planning is inherently multimodal, yet classical planners typically return only a single solution. Probabilistic formulations address this limitation by maintaining a distribution over motions, allowing the planner to reason over multiple low-cost alternatives. In robotics, however, motion samples must also satisfy strict constraints, including collision avoidance, joint limits, contact conditions, and dynamics consistency. These hard requirements make motion sampling substantially more challenging: within a limited planning budget, the ensemble must cover diverse low-cost motions while ensuring that every sample remains feasible under the relevant constraints. We propose SteinSQP (Stein Variational Sequential Quadratic Programming), a constrained Stein variational inference method for diverse feasible robot motion sampling. SteinSQP evolves an interacting particle ensemble, as in Stein variational methods, while embedding constraints directly into a kernel-space SQP subproblem. We solve the resulting constrained Stein-Newton subproblem with a GPU-friendly matrix-free primal-dual algorithm, enabling efficient batched ensemble updates. To globalize the method, we introduce an ensemble-level merit function that jointly balances objective value, constraint violation, and particle diversity. Across five constrained motion-planning tasks, SteinSQP returns fully feasible ensembles while preserving diverse motion alternatives. Compared with first-order constrained Stein baselines and serial multistart nonlinear programming, SteinSQP shows faster and more robust ensemble convergence in terms of iterations, improves particle-wise feasibility, and achieves faster batched time-to-solution on challenging robot-scale tasks.

Problem

• robot motion planning
• constraint satisfaction
• diverse feasible trajectories
• multimodal planning
• constrained sampling

Innovation

• Constrained Stein Variational Inference
• Sequential Quadratic Programming
• Diverse Motion Planning
• GPU-friendly Optimization
• Ensemble-level Merit Function