精读笔记

Problem Setting

这篇论文处理的不是一个新的机器人任务，而是 nonlinear MPC 求解器层面的老问题：如何在真实机器人闭环中高频求解带动力学和硬约束的非线性 OCP。真正困难点是三者同时出现：非线性动力学导致每次都要重新线性化；MPC 要求每个周期极低延迟并可 warm start；硬约束又不能像 DDP/iLQR 常用做法那样靠 penalty 权重糊过去。

以前方法的瓶颈很明确：单射击 DDP/iLQR 变量少、迭代便宜，但初值必须动态可行，难以利用任意状态轨迹 warm start，且硬约束处理不自然；多重射击 DDP/FDDP 改善了 warm start，但仍带有 DDP 社区的非标准约束处理和 rollout 设计，理论性质与 NLP/SQP 主线不完全对齐。关键矛盾是：机器人 MPC 需要 DDP 级别的时间结构利用，但又需要 SQP/NLP 级别的约束表达和收敛框架。本文的目标就是证明这两者并不冲突。

Motivation

作者的核心观察是：DDP/iLQR 的效率并不来自“DDP 作为一种特殊机器人算法”的神秘性，而主要来自它把 OCP 的时间稀疏结构利用到了极致。既然如此，标准 SQP 的 QP 子问题也有同样的时间结构，只要实现方式不是通用稀疏矩阵黑盒，就应当能达到相近复杂度。

已有路线缺的是一个 bridge：优化控制社区早就知道 SQP、interior point、active set、ADMM 等可以为 OCP 做结构化实现；机器人社区则更偏向继续扩展 DDP 家族。本文填的不是理论空白，而是实践与概念归因的空白：把现代多重射击 DDP-like 方法重新解释成 SQP 的特例/近亲，并在真机上证明 constrained nonlinear MPC 不需要绕开标准优化框架。

Core Idea

论文真正核心的方法思想是：用多重射击 SQP 作为 nonlinear MPC 的外层框架，并把每次 SQP 生成的 stagewise QP 当作一个结构化 optimal-control 问题来求解，而不是交给通用 QP solver。这样，状态和控制都作为优化变量，动态可行性不再是 warm start 的硬要求；同时，动力学等式和路径不等式都以 KKT/SQP 的标准方式进入问题。

本质区别在于信息组织方式。DDP/iLQR 通常通过 rollout 把状态从控制中隐式消去，强制沿动态可行轨迹传播；本文保留状态变量，让动态 gap 成为优化过程中被线性化约束逐步消除的量。这引入了更强的 multiple-shooting inductive bias：允许局部轨迹段先各自改善，再通过 SQP step 重新粘合。对于难问题，这通常比单一 nonlinear rollout 更稳定，也更适合 MPC warm start。

约束扩展的核心则是 ADMM splitting 的选择：动力学放进 primal 子问题 domain，使每次 ADMM primal update 都保持线性化动力学可行；不等式约束留给 projection/dual update。这不是简单“用了 OSQP”，而是重新分裂 QP，使 Riccati 递推仍然可用。这个设计让 constrained QP 的主计算仍是 LQR-like backward/forward pass，因此 horizon 上保持线性扩展。

Method

1. 多重射击 SQP：解决 warm start 和约束表达问题。显式维护状态序列后，初始轨迹可以有 dynamic defects，SQP 的 QP 子问题通过线性化动力学逐步关闭 gaps。核心变化是从“只能沿系统动力学 rollout 的控制优化”转成“状态-控制联合优化 + 动态一致性约束”。

2. SQP 子问题的 stagewise QP：解决非线性 OCP 的局部近似问题。代价二阶近似、动力学和约束一阶近似，得到标准 constrained QP。作者实际使用 Gauss-Newton/忽略约束二阶项作为实用近似，这降低计算成本并保持 QP 更容易处理。这里的代价是局部二阶信息不完整，严格二次收敛的条件被弱化；但在 MPC 中通常更看重稳健 early convergence。

3. Riccati/Thomas 递推解释：解决 QP 求解复杂度问题。无约束 QP 的 KKT 系统是块三对角，Riccati backward pass 可视为该系统的结构化 Gaussian elimination。这个机制把 DDP 和 SQP 统一起来：DDP backward pass 不是魔法，而是特定 KKT 结构的线性代数求解器。

4. Stagewise ADMM for constrained QP：解决硬不等式约束下仍保持实时的问题。动力学约束不放入一般 affine constraint，而放入 ADMM primal subproblem 的 domain；这样 primal step 仍能用 Riccati 解，约束通过 projection 和 dual 更新处理。核心变化是让 dynamics feasibility 在 ADMM 内部被优先维护，而不是等通用 OSQP 收敛后才满足。

5. Filter line search 与 KKT termination：解决 SQP globalization 和停止准则问题。filter 避免 merit function 权重调参，把 cost、dynamic gap、constraint violation 分开比较。它可能是实验增益的重要工程因素之一，而不只是附属细节。

Key Insight / Why It Works

最重要的 insight 是：optimal-control solver 的关键效率来源是时间诱导稀疏结构，而不是 DDP 这个算法标签。只要 SQP 的 QP 子问题按 stagewise 结构求解，它和 DDP 在计算核心上共享同一类 Riccati 消元。因此，SQP 可以继承 DDP 的线性 horizon complexity，同时获得标准 NLP 框架对约束、multipliers、line-search、KKT termination 的自然支持。

无约束情形下，本文的优势很可能主要来自 multiple shooting + linear rollout + filter globalization 的组合。作者倾向认为 FDDP 处于 single/multiple shooting 之间，一旦 gaps 关闭就不再重新打开，因而在困难问题上表现更像 DDP；而 SQP 保持完整多重射击自由度，允许不可行动力学轨迹作为中间迭代。这一解释是可信的：在强非线性或远初值问题中，强制 nonlinear rollout 可能过早把搜索限制在某个动态流形附近，而 SQP 的 linearized defects 给了更宽的局部修正空间。

约束情形下，核心贡献不是“ADMM 能解 QP”，而是 ADMM splitting 与 OCP 结构对齐。通用 OSQP 把动力学作为普通等式约束处理，动态可行性通常到收敛才出现；本文的 OSQP_OCP 每个 primal step 都解一个 dynamics-feasible LQR-like 子问题，因此早停时也保留更有控制意义的解。这对 MPC 很关键，因为真实部署中常常不会等 QP 完全收敛。

哪些可能只是辅助？Gauss-Newton Hessian、rho schedule、filter size、具体 tolerances、矩阵库选择都明显有 engineering 成分。benchmark 中 SQP 优于 FDDP 的增益归因不完全干净：line-search 的改变本身已经显著改善 FDDP，说明一部分收益来自优化工程而不是 SQP 框架本身。QP solver timing 中 HPIPM_OCP 与 OSQP_OCP 接近，也说明核心不是 ADMM 特有，而是 stagewise sparsity exploitation。换言之，最可迁移的贡献是“结构化实现标准优化器”，不是某个特定 solver 配方。

Relation To Prior Work

这篇论文最接近的谱系其实不是 robotics DDP variants，而是 optimization-based control 中的 multiple shooting SQP、structured QP、Riccati-based interior point/active-set/ADMM。作者做的是把这条老谱系重新带回机器人 nonlinear MPC，并用现代开源机器人动力学栈和真机实验把它跑通。

和 DDP/iLQR/FDDP 的本质差异不是 backward pass 形式，而是变量消元和可行性策略。DDP/iLQR 通常控制变量主导、状态由 rollout 隐式决定；FDDP 引入 gaps 但仍带有 nonlinear rollout 逻辑。SQP 则保留完整状态-控制变量，通过线性化等式约束和 QP multipliers 管理动态一致性。这让约束进入方式更标准，也让 warm start 更自由。

和 acados/HPIPM/optimization-control 工具链相比，本文的新意较少在理论，而在机器人社区语境下的重新归因和实证。它把 GNMS 明确解释为 Gauss-Newton SQP，把 Riccati pass 解释为 KKT 三对角消元，把 constrained MPC 展示为“只需要结构化 QP solver”。实质创新是 stagewise OSQP_OCP 的分裂实现和真机 constrained nonlinear torque MPC 展示；概念创新是打破“DDP 才能高频机器人 MPC”的默认叙事。

Dataset / Evaluation

评估覆盖了无约束随机 OCP benchmark、KUKA 真机无约束 tracking、约束 quadruped 仿真、KUKA 真机 constrained MPC，以及 QP solver timing。任务类型上包含固定基座机械臂、quadrotor、double pendulum、humanoid、quadruped standing/contact force constraints，覆盖面足以支持“结构化 SQP 在典型机器人 OCP 中有效”的 claim。

最有说服力的是 constrained KUKA 真机实验：关节硬约束、末端半空间约束、末端等式约束和外部扰动下的在线重规划，确实验证了 SQP 框架处理硬约束的实用性。尤其是无需把约束改写成 cost penalty，这一点对机器人部署有实际价值。

但 evaluation 也有边界。第一，真机 constrained 实验集中在 7-DoF 固定基座机械臂，horizon 短、约束 mostly stagewise，不能直接外推到复杂接触切换或长时域全身 MPC。第二，无约束 benchmark 虽然显示 SQP 迭代数更少，但对 rollout、line-search、multiple shooting 自由度的消融还不够彻底，增益来源仍有混合。第三，QP solver 对比说明 stagewise implementation 关键，但 OSQP_OCP vs HPIPM_OCP 的差异很大程度受线性代数 backend 影响，不能得出 ADMM 本身普遍优于 interior point 的结论。

Limitation

最大的隐含前提是约束和代价具有 stagewise 局部结构。只要出现强跨时间耦合约束、全局碰撞约束、长时域资源约束或复杂 contact schedule，块三对角结构会被破坏或需要额外变量重构，本文的线性复杂度优势会被削弱。

第二，方法依赖高质量模型和导数。Pinocchio/Crocoddyl 提供解析刚体动力学导数，这对结果至关重要；如果动力学来自 learned model 或包含不可微接触/摩擦切换，SQP 的局部线性化质量会成为瓶颈。文中未充分说明在 model mismatch、nonsmooth contact、state estimation delay 下的鲁棒性边界。

第三，硬约束满足在实时 MPC 中仍依赖 QP early termination 的质量。ADMM 早停能给出“reasonable solution”，但这不等于严格满足原非线性约束；闭环中约束 violation 如何随 SQP/QP 迭代预算变化，文中没有系统理论保证。所谓 hard constraints 在部署层面更准确地说是“优化问题中硬编码，实际由有限迭代近似满足”。

第四，constrained nonlinear MPC 的上限没有被真正压测。机械臂实验频率降到 100 Hz，SQP 迭代和 QP 迭代都有限，说明约束处理带来的计算负担仍显著。复杂 humanoid 真机、动态接触、障碍密集环境下是否还能保持实时，文中未充分说明。

第五，部分收益可能来自 engineering。filter line-search、warm start、rho 不重置、矩阵后端、tolerance 选择都会影响结果。论文的核心信息是对的，但若把性能提升全部归因于 SQP 而非结构化实现 + 优化工程，会过度解读。

Takeaway

• 1. DDP/iLQR 的核心资产是时间结构，不是算法范式；把标准 SQP/QP 做成 stagewise solver，基本可以拿回同样的计算优势。
• 2. 对机器人 constrained MPC，更值得投资的是结构化 QP/SQP 实现，而不是继续给 DDP 家族打补丁式加入约束处理。
• 硬约束、multipliers、KKT residual、globalization 都是 SQP 的自然语言。
• 3. ADMM 的价值在 MPC 中不只是最终收敛速度，而是 early termination 下仍能给出可用解；如果 splitting 设计得让 dynamics feasibility 始终被优先维护，它比通用 QP 早停更适合闭环控制。

一句话总结

这篇论文把机器人 nonlinear MPC 从 DDP-centric 叙事拉回结构化 SQP/NLP 主线，真正贡献是证明“标准优化器 + 时间稀疏实现”足以实现实时硬约束真机 MPC，并且很多所谓 DDP 优势本质上只是 Riccati 化稀疏线性代数。