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能更好地避免不良局部极小值，并对初始化具有更强的鲁棒性。我们证明，在具有挑战性的高约束任务中（如7自由度扳手操作任务），CSVTO的性能优于基线方法——在该任务中CSVTO在20/20次试验中成功，而最优基线方法仅成功13/20次。实验结果表明，生成多样化的满足约束轨迹能提升对干扰和初始化的鲁棒性。