IEEE Transactions on Robotics

ProxDDP: Proximal Constrained Trajectory Optimization

作者：Wilson Jallet, Antoine Bambade, Etienne Arlaud, Sarah El-Kazdadi, Nicolas Mansard, Justin Carpentier · 机构：Inria—Département d'Informatique de l'École normale supérieure, PSL Research University, Paris, France; LAAS-CNRS, Toulouse, France

会议/期刊：IEEE Transactions on Robotics · 发布日期：2025 · 页码：2605-2624

运动规划控制优化仿生机器人

精读笔记

Problem Setting

ProxDDP 处理的是带硬路径约束的离散时间轨迹优化，尤其是面向机器人 MPC 的 shooting/multiple-shooting 风格 OCP。难点不是约束形式本身，而是三者冲突：硬约束需要 KKT 级别满足；MPC 需要 warm-start 和毫秒级迭代；复杂机器人需要利用时序结构，否则通用 NLP solver 太重。

传统 DDP/iLQR 的强项是 Riccati 递推和 nonlinear rollout，但它天然假设无硬约束或只处理很弱的约束形式。把约束软化成 penalty/barrier 会把问题推给权重调参和数值 conditioning；用 interior-point 或通用 sparse NLP 可以更稳，但 warm-start 和实时性通常不理想。关键矛盾是：怎样在不牺牲 DDP 结构化速度的情况下，把硬约束作为 KKT 对象而不是 cost hack 来处理。

Motivation

已有 constrained DDP 路线缺的是一个同时满足高精度约束、warm-start 友好、数值条件可控、且保留 Riccati 结构的统一机制。等式约束 DDP/nullspace Riccati 对 active set 和 reduced Hessian 有假设；control bounds projection 类方法覆盖面窄；barrier/IPM 在约束贴边和 MPC warm-start 下不自然；传统 AL-DDP 虽然 warm-start 友好，但 primal AL Hessian 在 penalty 变大时会病态，常常只能产生粗解。

作者的出发点很明确：AL 本身不是问题，问题是用 primal-only 方式求 AL 子问题。优化文献里已有结论显示，把 AL Newton 方向改写为 primal-dual proximal KKT 系统可以改善 conditioning。缺口在于：如何把这个 generic NLP insight 嵌入 DDP 的 backward/forward pass，而不是退回通用 sparse linear solver。

Core Idea

论文真正核心是：把 constrained trajectory optimization 视为一个 proximal augmented Lagrangian saddle/KKT 问题，并在 DDP 的动态规划递推中求解 primal-dual semismooth Newton 方向。约束不再只是附加到 stage cost 的大 penalty，而是通过 multiplier estimates、proximal-KKT residual 和 active inequality projection 进入每个 stage 的线性系统。

这个重组改变了信息流：standard DDP 只传播 value gradient/Hessian 对未来 cost 的影响；ProxDDP 还传播 dynamics multiplier、path-constraint multiplier 及其 sensitivity。于是 backward pass 不只是解 unconstrained local LQ 子问题，而是在解一个具有 proximal regularization 的 local constrained KKT 子问题。理论直觉上有效，是因为 proximal dual block 给 KKT 系统提供了可控 regularization，避免 Schur complement 后 primal Hessian 中出现巨大 constraint Jacobian penalty 项导致病态。

和 prior 的本质差异不是“用了 AL”，而是“用了 primal-dual AL，并把它结构化成 DDP”。这使它比普通 AL-DDP 更可能达到高精度，比 IPM 更适合 warm-start，比通用 NLP 更能利用 OCP 时序结构。

Method

1）Proximal AL formulation：用 PHR augmented Lagrangian 把等式、动力学约束和不等式路径约束统一成 proximal dual update。它解决的是硬约束与 warm-start 的兼容问题：multipliers 可以从上一 MPC 问题继承，约束通过 first-order multiplier estimates 推进，而不是靠无限大 penalty。

2）Primal-dual semismooth Newton inner loop：不直接解 AL Hessian 系统，而解等价的 primal-dual proximal-KKT 系统。它解决的是 conditioning 问题。primal-only 形式会出现 A^T A / mu 这类项，mu 小时病态；primal-dual 形式把约束 Jacobian 作为 KKT coupling，dual block 中的 -mu I 起 regularization 作用。

3）DDP-style backward pass：把每个 stage 的 proximal-KKT operator 线性化，解关于 control、next state、dynamics multiplier、inequality multiplier 的 sensitivity system，并向前一时刻传播 value gradient/Hessian。它解决的是 scalability：避免把整个 horizon 的 NLP KKT 系统一次性交给通用 sparse solver。

4）Active inequality handling：通过 [h + mu nu]_+ 定义 active set，并用 semismooth generalized Hessian / projection matrix 处理不等式。它解决的是 complementarity 的局部建模问题，但本质仍是 active-set 局部 Newton，active set 快速变化时仍可能不稳。

5）Forward pass and globalization：linear rollout 对应 SQP step；nonlinear rollout 更接近 DDP，利用真实动力学改善远离线性化点时的轨迹质量。linesearch/merit function 保证基本 globalization。这里更多是使算法实用的工程稳定化，不是核心理论创新。

Key Insight / Why It Works

最重要的 insight 是：constrained DDP 的瓶颈不是能不能写出约束二阶展开，而是如何避免 AL/penalty 在高精度约束下把 Riccati 子问题搞病态。ProxDDP 的有效性主要来自 primal-dual reformulation 对 conditioning 的改善，而不是来自更复杂的 rollout 或更多 heuristics。

从机制上看，它把约束满足从“cost landscape 中的陡峭峡谷”变成“proximal KKT 系统中的 saddle coupling”。这非常关键：前者会让 Hessian 随 penalty 爆炸，后者通过 dual variables 显式吸收约束力，并用 -mu I 控制 dual regularization。对机器人 OCP 来说，这恰好匹配物理直觉：动力学和接触/路径约束对应的乘子类似 forces/costates，显式传播它们比把一切压进 state-control cost 更自然。

最可能的核心贡献是 stage-wise primal-dual Riccati 化，而不是 AL 本身。AL、BCL schedule、semismooth Newton、nonmonotone linesearch 都是优化文献已有组件；论文的价值在于把这些组件组织成一个 DDP-compatible solver，并证明这个组织在机器人 MPC 中足够快。

哪些可能只是辅助：nonlinear rollout 对 DDP 收敛域有帮助，但不是 constrained handling 的本质；dynamics penalty scaling、inertia correction、默认参数等属于 solver engineering；aligator/proxsuite-nlp 的实现质量也很可能贡献了大量 wall-time 优势。benchmark 中 ProxDDP 对 mu、linesearch moving average、rollout 选择敏感，说明部分增益来自调参和实现，而不是完全由理论机制保证。

它不是 scaling/data/retrieval 类工作；本质是 better inductive bias + test-time structured compute：把 OCP 的 temporal sparsity 和 KKT saddle structure 同时编码进求解器。这个 inductive bias 是可迁移的：凡是有链式时序结构且约束乘子有局部物理意义的问题，都可能受益。

Relation To Prior Work

它最接近三条线：AL-based constrained DDP/ALTRO，primal-dual AL/semismooth Newton NLP/QP solver，以及 Riccati/SQP/IPM 型 OCP solver。看似新的是 ProxDDP 这个算法名；真正新增的是把 primal-dual proximal AL Newton 系统嵌入 DDP backward pass，而不是只在全局 NLP 层面使用。

相对 soft penalty/barrier：ProxDDP 的区别在于约束是 KKT 对象，有 multiplier 和 complementarity residual，不是单纯 cost shaping。相对 IPM：它不沿 central path 解 barrier relaxation，因此 warm-start 更自然，约束可以贴边；但 IPM 的成熟 globalization 和 robustness 仍更强。相对传统 AL-DDP/ALTRO：关键差别是 primal-dual 子问题方向，而不是只对 AL cost 做 primal minimization；这解释了为什么它更能追求紧 KKT tolerance。相对通用 ipopt：ProxDDP 放弃一部分通用性和成熟 heuristics，换取对 OCP 时序结构的强利用。

这篇论文属于“结构化 constrained second-order OCP solver”的谱系，而不是新的控制理论或新的规划范式。它的实质创新偏 solver architecture：把已有优化思想按 DDP 的信息流重新编译。

Dataset / Evaluation

评估覆盖面不错：从低维 toy 到机械臂、quadrotor、四足跳跃和真机 whole-body MPC，约束类型包括 terminal constraints、actuator limits、obstacle avoidance、velocity/position limits、contact phase constraints。真实世界部分是强加分项，因为它验证了 warm-start 和单步 MPC update 的实用性，而不仅是离线 solve。

benchmark 基本能支持“比 altro 更可靠、比 ipopt 更实时友好、在许多机器人 OCP 上 competitive”这个 claim。但它没有证明 ProxDDP 全面比 ipopt robust；事实上 UR5 带速度限制等难例里 ipopt L-BFGS 更强。评估也显示 solver 参数对表现影响很大，尤其 mu、linesearch、rollout 模式。文中对失败 case 的系统分析不够，尤其是 nonlinear rollout 在 Solo-12 yoga benchmark 上会发散这一点，暴露出稳定性边界。

真机 MPC 展示更多是 feasibility/performance demonstration，而不是严格 ablation。增益来源不完全清楚：算法机制、C++ 实现、Pinocchio derivatives、problem formulation、low-level Riccati feedback 都混在一起。作为工程系统证据足够强，作为算法归因证据还不够干净。

Limitation

1）非凸性没有被解决。ProxDDP 仍是局部二阶方法，依赖初始化、局部模型和 active set 识别。所谓 hard constraint satisfaction 是收敛后的 KKT 层面，不代表全局可行性搜索能力。

2）参数敏感是实质问题。初始 mu、penalty schedule、linesearch、rollout 选择会显著改变表现。作者承认成熟 NLP solver 有大量 heuristics，而 ProxDDP 还比较少。这意味着部署时很可能需要 expert tuning。

3）implicit dynamics 下 nonlinear rollout 需要解 root-finding，等于把一部分难度转移到 forward pass。对于 stiff DAE、复杂 contact dynamics，这可能成为主要瓶颈。

4）active-set semismooth 处理在约束边界频繁切换时的稳定性没有充分说明。尤其对接触、碰撞、摩擦、多障碍密集场景，active set 可能高度不连续，局部 Newton 方向未必可靠。

5）scalability 上限仍取决于每个 stage primal-dual 系统的维度和 factorization。对 humanoid、dexterous hand、dense inequality constraints，stage KKT 规模会膨胀；论文提出未来做并行 Riccati，说明当前版本仍有扩展压力。

6）泛化不是这里的主题。它不学习分布，也不从数据泛化；跨任务表现来自问题结构相似和 solver robustness。不要把多机器人实验解读成泛化能力。

Takeaway

  • 1）constrained DDP 的关键演化方向不是继续堆 penalty/barrier，而是把 KKT/primal-dual 结构显式放进 Riccati 递推。
  • 2）AL 在 MPC 中仍然很有生命力，前提是不要用 naive primal penalty Hessian；primal-dual proximal reformulation 是更正确的打开方式。
  • 3）这篇推动的是 solver architecture：把通用优化里的 semismooth proximal-KKT 思想编译到时序最优控制结构中。
  • 这个 insight 可迁移到其他链式结构优化问题，例如 smoothing、estimation、contact-rich planning。

一句话总结

ProxDDP 是把 primal-dual proximal augmented Lagrangian 机制 Riccati 化的一类 constrained DDP solver，真正贡献在于用结构化 KKT 信息流替代 penalty/barrier hack，使硬约束机器人 MPC 更接近实时且可 warm-start。