We present Constrained Stein Variational Trajectory Optimization (CSVTO), an algorithm for performing trajectory optimization with constraints on a set of trajectories in parallel. We frame constrained trajectory optimization as a novel form of constrained functional minimization over trajectory distributions, which avoids treating the constraints as a penalty in the objective and allows us to generate diverse sets of constraint-satisfying trajectories. Our method uses Stein Variational Gradient Descent (SVGD) to find a set of particles that approximates a distribution over low-cost trajectories while obeying constraints. CSVTO is applicable to problems with arbitrary equality and inequality constraints and includes a novel particle resampling step to escape local minima. By explicitly generating diverse sets of trajectories, CSVTO is better able to avoid poor local minima and is more robust to initialization. We demonstrate that CSVTO outperforms baselines in challenging highly-constrained tasks, such as a 7DoF wrench manipulation task, where CSVTO succeeds in 20/20 trials vs 13/20 for the closest baseline. Our results demonstrate that generating diverse constraint-satisfying trajectories improves robustness to disturbances and initialization over baselines.

翻译：我们提出了约束斯坦变分轨迹优化（CSVTO），一种可并行处理多轨迹集合并施加约束的轨迹优化算法。我们将带约束的轨迹优化问题重新定义为轨迹分布上的新型约束泛函最小化形式，该方法避免将约束视为目标函数中的惩罚项，能够生成满足约束的多样化轨迹集合。该算法利用斯坦变分梯度下降（SVGD）寻找一组近似服从低代价轨迹分布且满足约束的粒子。CSVTO适用于含任意等式与不等式约束的问题，并创新性地引入粒子重采样步骤以逃离局部极小值。通过显式生成多样化轨迹集合，CSVTO能更有效地规避不良局部极小值，且对初始化具有更强鲁棒性。实验表明，在具有高度约束的挑战性任务中（如七自由度力矩操控任务），CSVTO在20/20次试验中成功，而最佳基线方法仅13/20次成功。结果表明，生成多样化满足约束的轨迹相较基线方法能提升对扰动和初始化的鲁棒性。