IEEE Transactions on Robotics

Constrained Stein Variational Trajectory Optimization

作者：Thomas Power, Dmitry Berenson · 机构：Robotics Department, University of Michigan, Ann Arbor, MI, USA

会议/期刊：IEEE Transactions on Robotics · 发布日期：2024 · 页码：3602-3619

路径规划运动规划控制优化操作

精读笔记

Problem Setting

论文实际处理的是带可微等式/不等式约束的非凸轨迹优化，尤其是可行轨迹位于低维隐式流形上的情形。关键矛盾是：硬约束要求轨迹严格贴在可行流形上，但非凸目标和障碍/操作约束又导致多个 basin；单条轨迹优化容易被初始化决定，而纯采样或 inference-MPC 又很难保证 constraint satisfaction。

以前路线各有卡点：IPOPT/TrajOpt/ALTRO 等 constrained optimization 能表达约束，但通常只返回一条局部解，online replan 时 previous solution 一旦因环境变化失效，就可能直接卡死。MPPI/SVMPC 这类 inference/control-as-inference 方法能并行探索，但约束多通过 penalty 进入 cost，结果是 feasibility 和 objective 被同一个权重 trade off，复杂约束下很容易“看起来优化了 cost，实际不可行”。本文的真正问题不是求一个最优轨迹，而是在有限时间内维护一组可行附近、低成本、多样的轨迹候选。

Motivation

作者的核心观察是：trajectory optimization as inference 本来最有价值的地方是输出 trajectory distribution，而不是单点解；但一旦约束只能靠 penalty，这个分布就不再是“可行轨迹上的低成本分布”，而是“违反约束也可能被接受的软目标分布”。这直接削弱了 inference formulation 在安全/操作约束问题中的意义。

缺口在于：已有 SVGD-MPC/entropy-regularized planning 可以用粒子表示多模态 posterior，却缺一个在 constraint manifold 上移动粒子的机制；而 constrained motion planning 有 projection/continuation 思想，却不是 cost-aware 的分布优化。CSVTO 就是把这两条线接起来：用 SVGD 的 population-based variational descent 处理多 basin，用 constrained gradient flow / tangent projection 处理可行流形。

Core Idea

CSVTO 的核心不是“又一个轨迹优化器”，而是把约束从 cost 中拿出来，放到轨迹分布的 support 上：希望 q(τ) 的概率质量只落在满足 h(τ)=0、g(τ)≤0、dynamics/bounds 的轨迹上，同时 q 偏向低 cost 且保持 entropy。这一建模变化很关键，因为它把 feasibility 从 tuning problem 变成 support constraint。

算法上，每个粒子是一条轨迹。普通 SVGD 会同时做 posterior gradient ascent 和 particle repulsion；CSVTO 将这个更新投影到约束切空间，并额外加入把粒子拉回约束面的 correction。直觉上，粒子在流形附近横向探索，而不是反复穿出流形再靠 penalty 拉回；排斥项让多个粒子覆盖不同局部解，降低单一初始化的支配性。与 prior 的本质区别在于：prior 多是在 unconstrained space 上优化一个软约束 posterior，本文是在 constrained geometry 上做 particle variational optimization。

Method

1. 约束分布优化：把 OCP 解释为 posterior inference，低 cost 轨迹有高 likelihood；再把 dynamics、equality、inequality、bounds 视为 q 的 support 约束。这解决了 penalty formulation 中 feasibility 与 cost 权重冲突的问题。

2. Tangent-space SVGD：对每条轨迹粒子构造约束 Jacobian 的 null-space projector，用 matrix-valued kernel 让 Stein 更新位于约束切空间。它的作用不是简单投影结果，而是改变整个粒子交互的几何：粒子之间的 gradient sharing 和 repulsion 都被约束流形调制。

3. Constraint restoration step：单靠切空间更新只能在已可行时保持一阶可行，实际初始化和有限步更新都会有违反。因此用 Gauss–Newton 式 correction 减少 h(τ)^T h(τ)。这部分更像 constrained gradient flow 中的 feasibility stabilizer，是工程上必要的。

4. Inequality slack：用 g(τ)+z²/2=0 把 inequality 纳入同一 equality projection 框架。优点是统一，缺点是变量数随 inequality 数量线性膨胀，并可能引入非期望 stationary point。

5. Annealing 与 resampling：annealing 让早期 repulsion 更强，先铺开可行候选，再逐步优化 cost；resampling 按 cost+violation 选择粒子，并在约束切空间加噪声，避免 online MPC 中未执行轨迹逐渐退化。这些机制主要改善有限迭代下的 robustness，不是理论核心，但对实验表现很可能很重要。

Key Insight / Why It Works

最重要的 insight 是：对高度约束问题，多样性只有在“可行流形附近”才有意义。普通 MPPI/SVGD 的多样性如果发生在不可行空间，本质上是在浪费 samples；IPOPT 的 feasibility 如果只给单个 basin，又缺乏模式切换能力。CSVTO 把二者结合：在 constraint manifold 的 tangent space 内保留 population diversity。

方法有效的主要来源有三类。第一是 better inductive bias：更新方向被约束几何过滤，减少了 penalty 方法在法向方向上反复拉扯的低效。第二是 test-time compute / population search：多粒子并行等价于有限预算下跑多个相互通信的局部优化器，能覆盖多个 homotopy / manipulation mode。第三是 memory reuse：online replanning 中保留并重采样粒子集，相比单条 warm-start 对动态变化更稳。

真正核心贡献应是 tangent-space SVGD formulation for trajectory particles；resampling 和 annealing 更像让这个框架在有限迭代下能工作的辅助机制。实验增益并不完全来自理论上的 variational optimality，更可能来自 constrained population optimization + heuristic resampling 的组合。文中虽然给了 stationary point 满足一阶条件的分析，但这并不等于全局性或有限步 feasibility guarantee；实践成功主要还是靠多粒子覆盖和约束校正。

是否是 scaling？部分是。粒子数、GPU 并行、automatic differentiation、在线重复优化都在贡献性能。但它不是单纯 scaling，因为把 SVGD 的 repulsion 放进约束切空间确实改变了搜索几何。是否是 hidden supervision / retrieval？不是数据驱动方法，不依赖 dataset 覆盖；这里的“泛化”主要是优化器对不同初始/目标/障碍状态的适应，不是 learned policy generalization。

Relation To Prior Work

最接近的是三条谱系：constrained NLP / trajectory optimization、planning/control as inference、O-SVGD / constrained gradient flows。

相对 IPOPT/TrajOpt/ALTRO，CSVTO 的本质差异不是约束表达能力，而是从单解局部优化转为多粒子分布优化。它牺牲了成熟 NLP solver 的收敛/数值优势，换来多 basin 覆盖和在线扰动下的模式切换能力。

相对 MPPI/SVMPC/entropy-regularized motion planning，CSVTO 的关键新增信息是约束不再通过 penalty 编码到 log posterior，而是通过 support/tangent geometry 进入更新。这是实质差异，因为 penalty-based inference 在 equality manifold 上天然尴尬：可行集测度为零，软惩罚很难同时保证探索和可行。

相对 O-SVGD，本文的创新是把 constrained SVGD 推到 trajectory optimization：多等式约束、不等式 slack、dynamics/bounds、trajectory kernel、online resampling。严格说，投影切空间和 slack variables 都是已有思想重组；实质创新在于将它们组织成一个能处理高维轨迹粒子集的 constrained inference optimizer。

Dataset / Evaluation

评估覆盖了三类任务：欠驱动 quadrotor、桌面约束操作、wrench manipulation。它们都符合论文想证明的核心场景：非凸、高约束、初始化敏感、需要 online replanning。尤其 wrench 任务和动态障碍 quadrotor 能较好展示“多样可行轨迹集”优于单解 warm-start 的 claim。

不过 benchmark 范围仍偏窄：horizon 不长，约束和动力学基本可微，任务结构由作者手工设计，未展示大规模 contact-rich manipulation 或复杂多阶段任务。真实世界实验是有价值的，但更像 proof-of-concept，而不是系统性 real-world validation。

实验确实支持“相对 penalty-based inference，CSVTO 约束满足更好；相对 IPOPT，CSVTO 对初始化/动态变化更稳”的 claim。但消融不足：annealing、resampling、粒子数、kernel decomposition、slack treatment 各自贡献没有被充分隔离。因此性能归因仍不够干净，增益来源不清的部分主要在工程策略而非核心 constrained SVGD。

Limitation

第一，核心假设很硬：cost、dynamics、constraints 至少需要一阶可微，很多地方还希望二阶信息。真实机器人中最难的接触、碰撞切换、摩擦模式变化通常非光滑，CSVTO 在这些问题上可能只能靠平滑近似，约束意义会变弱。

第二，不等式约束通过 squared slack 转等式会显著增加维度，并可能产生伪驻点。作者承认 regularity assumption，但实际长 horizon、多障碍、多接触场景中该假设很脆弱。这里有把问题从“调 penalty”转移到“管理 slack 增维和数值稳定”的嫌疑。

第三，scalability 上限明显：SVGD 粒子交互、trajectory kernel、constraint Jacobian/projection、SVD/pseudoinverse 都随粒子数、轨迹维度、约束数增长。论文中的实时性并不强，online solve time 对高频控制仍偏慢。所谓 online replanning 更接近低频 MPC，不是 reactive control。

第四，理论保证较弱。stationary point 分析只说明在理想条件和等式约束下满足一阶 KKT-like 条件；有限迭代、粒子近似、非凸约束、不等式 slack、数值伪逆都会削弱这个结论。实际 feasibility 仍是 approximate。

第五，kernel 选择仍是开放工程问题。RBF + sliding window 是合理 heuristic，但没有真正利用约束流形拓扑、动力学可达性或任务 homotopy。对于高维长 horizon，kernel 退化和粒子 collapse 仍可能发生。

Takeaway

  • 1. 对 constrained trajectory optimization，真正有价值的分布式优化不是在 unconstrained space 里撒粒子，而是在 constraint manifold 上维护多样粒子；这是本文最值得迁移的思想。
  • 2. CSVTO 展示了一条介于 NLP solver 和 sampling/inference planner 之间的路线：用局部约束几何保证 feasibility，用 particle repulsion 获得多 basin 覆盖。
  • 这类方法适合中等维度、强约束、需要 online fallback 的机器人任务。
  • 3. 未来更值得做的不是继续调 SVGD 超参数，而是解决 active inequality、非光滑接触、任务相关 kernel、以及更强的有限步 feasibility/control-rate guarantee。

一句话总结

Constrained Stein Variational Trajectory Optimization 是把 SVGD 式多粒子 inference planning 搬到约束流形上的一次实质推进，核心贡献在于用 constrained population optimization 替代 penalty-based 单目标搜索，从而在高约束非凸轨迹优化中同时获得可行性和多模式鲁棒性。