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 differentiable equality and inequality constraints and includes a novel particle re-sampling 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 outperforms all baselines both in success and constraint satisfaction.

翻译：本文提出约束斯坦变分轨迹优化（CSVTO）算法，用于在并行轨迹集上执行带约束的轨迹优化。我们将约束轨迹优化构建为一种新型的轨迹分布约束泛函最小化问题，该方法避免将约束作为目标函数中的惩罚项，并能生成满足约束的多样化轨迹集。本方法采用斯坦变分梯度下降（SVGD）来寻找一组近似于低成本轨迹分布的粒子，同时满足约束条件。CSVTO适用于具有可微分等式与不等式约束的问题，并包含创新的粒子重采样步骤以逃离局部极小值。通过显式生成多样化轨迹集，CSVTO能更有效地规避不良局部极小值，并对初始化具有更强鲁棒性。实验表明，在7自由度扳手操作等高约束挑战性任务中，CSVTO在成功率和约束满足度上均优于所有基线方法。