IEEE Transactions on Robotics

Efficient Constrained Dynamics Algorithms Based on an Equivalent LQR Formulation Using Gauss' Principle of Least Constraint

作者：Ajay Suresha Sathya, Herman Bruyninckx, Wilm Decré, Goele Pipeleers · 机构：Division of Robotics, Automation and Mechatronics in the Department of Mechanical Engineering, KU Leuven, Leuven, Belgium; DMMS-M Lab, Flanders Make, Leuven, Belgium; TU Eindhoven, Eindhoven, The Netherlands; Materialise NV, Leuven, Belgium

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

控制优化人形机器人

精读笔记

Problem Setting

《Efficient Constrained Dynamics Algorithms Based on an Equivalent LQR Formulation Using Gauss' Principle of Least Constraint》（IEEE Transactions on Robotics / 2024）实际处理的是刚体运动学树上的等式约束动力学求解：在给定状态、关节力矩、外力和 link-level acceleration constraints 后，求满足约束的 joint accelerations 与 Lagrange multipliers。

关键矛盾是：物理方程本身很简单，KKT 也很标准，但全局矩阵形式会破坏运动学树的局部结构。传统做法要么构造 JSIM / constraint Jacobian / OSIM 后分解，要么做 sparse factorization；这些方法工程上可靠，但在高自由度、人形/四足、多端接触时会把大量计算浪费在全局耦合矩阵上。真正困难不是“约束动力学有没有解”，而是如何只传播必要的约束信息，并避免把所有约束力都堆到一个 dense m×m 系统里。

以前方法卡在两个地方：一是 Featherstone LTL 路线虽然利用了 sparsity，但仍需要 Jacobian、JSIM/OSIM 相关结构，复杂度随树深和约束数恶化；二是 EFPA/KJR 等低阶 OSIM 算法理论复杂度好，但推导复杂、常数不一定好、社区使用少。本文的任务本质上是在“可解释结构递推”和“工程可用速度”之间重新找一个更优点。

Motivation

已有 constrained dynamics 路线的问题不是完全不能快，而是快的方法通常不可读、不可改、不可迁移。很多机器人控制/MPC/RL 研究者最终依赖 MuJoCo、Pinocchio、RBDL 这类库里的黑盒实现；一旦需要改约束模型、加正则、做梯度、做特殊拓扑优化，就很难深入。

作者的核心观察是：Gauss 最小约束原理天然就是一个二次优化问题，而运动学树上的加速度递推与离散时间 LQR 的 state dynamics 形式几乎同构。于是 constrained dynamics 不必从 Newton–Euler 或 KKT factorization 出发，也可以从 LQR/DP 的角度推导。这个视角的价值不只是教学友好，而是它暴露了“变量消元顺序”这一核心自由度：先消 primal、先消部分 dual、或把 soft constraints 吸收入 inertia，都会得到不同复杂度的算法。

关键缺口在于：PV solver 早就存在，但长期没有被主流 simulator 充分采用，也没有被系统地推广到 floating-base trees、任意 link constraints、OSIM 计算和 soft/hard 两类 O(n+m) 变体。本文主要是在这个缺口上做了结构化整理、推广和工程验证。

Core Idea

论文真正的核心思想是：把 constrained dynamics 从“求解一个全局 KKT 系统”改写为“求解一个树结构 LQR 的 saddle-point DP”。link acceleration 被看作状态，joint acceleration 被看作控制，Gauss principle 提供二次 stage cost，约束通过 Lagrange multipliers 进入 cost-to-go。这样 backward sweep 就等价于 Riccati recursion：每个节点消去该关节加速度，并向父节点传递一个压缩后的二次函数。

这个建模方式改变了信息流。传统 OSIM/KKT 路线先把全局约束耦合显式形成出来，再求解；PV 路线则在树上传播“约束力如何影响祖先/后代加速度”的局部映射，直到需要时才形成或消去 dual 系统。其 inductive bias 是运动学树的 causal / recursive structure：约束信息只沿祖先路径传播，不在无关分支之间提前 dense 化。对四足、人形这种约束集中在肢端、分支从浮动基发出的结构，这个 bias 非常强。

和 prior 的本质区别不在物理方程，而在消元顺序和中间表示。它不是更聪明地分解同一个 dense 系统，而是避免过早构造这个系统；PV-early 更进一步，把可解析的 multiplier 子空间在局部关节处提前消掉，使 constraint dimension 不随路径无限增长。

Method

1. GPLC 到 LQR/QP：解决的是“如何把动力学与约束统一成可递推优化问题”。Gauss principle 让实际加速度成为最接近 unconstrained acceleration 的质量加权最小二乘解；加入关节加速度递推后，问题具有 LQR 的 stage cost + dynamics 结构。核心变化是 constrained dynamics 的求解对象从方程组变成 cost-to-go。

2. PV backward recursion：解决的是“如何避免全局 KKT”。每个 link 消去本地 joint acceleration，向父节点传递 articulated inertia H^A、resultant force f^A、constraint-force propagation K^A、desired constraint term l，以及 dual Hessian L^A。H^A/f^A 对应 ABA 的惯量/力递推；K^A/L^A 是对约束影响的新增递推。核心变化是把 OSIM 的构造嵌入 backward sweep。

3. Floating-base tree extension：解决的是“原始 PV 只适合固定基链和末端约束”。树结构下 cost-to-go 来自多个 children 的合并；浮动基处需要同时求 base acceleration 与 multipliers。核心变化是将 LQR 从线性时间链推广为 scenario-tree Riccati recursion。

4. Dual Hessian = inverse OSIM：解决的是“PV 中间量到底是什么”。论文指出 backward recursion 得到的 L_0^A 就是 JM^{-1}J^T，即 inverse OSIM。这个连接让 PV 不只是 CDA solver，也成为 OSIM solver。核心变化是把 task-space inertia computation 解释为 LQR dual curvature。

5. Soft Gauss：解决的是 MuJoCo-type soft constraints 的快速求解。软约束二次罚项可直接并入每个 link 的 H_i 和 f_i，于是算法退化为带 modified inertia/force 的 ABA。核心变化很大：不再传播 multipliers，因此复杂度 O(n+m)，但代价是约束不再精确满足。

6. Early multiplier resolution：解决的是 hard constraints 下 PV 仍需最终 factorize m×m dual Hessian。其做法是在每个关节处对 L_i^A 的可解子空间做 SVD/秩分解，提前消去已被该关节运动“解释”的 multiplier 方向，只把 nullspace 中尚未解析的约束继续上传。核心变化是从“先消所有 primal，再一次性解 dual”改成“primal-dual 交替局部消元”。

Key Insight / Why It Works

最重要的 insight 是：约束动力学的昂贵部分本质上是约束力到约束加速度的映射，也就是 inverse OSIM；而这个映射不需要通过 J 和 M^{-1} 显式构造，它可以在关节级 Riccati 消元中顺手累积出来。换言之，PV 的 dual Hessian 不是一个抽象优化量，而就是 task-space inertial coupling。

方法有效的根本原因是更好的 latent structure，而不是 scaling/data。它抓住了刚体树的局部可分解性：每个关节只引入低秩更新，约束影响沿唯一祖先路径传播；在分支机器人中，不同肢体的约束在到达浮动基之前天然块对角。Featherstone LTL 虽然也利用 sparsity，但仍更接近“全局矩阵的稀疏分解”；PV 更接近“按物理因果方向做低秩消息传递”。

最可能的核心贡献有两个：第一，dual Hessian 与 inverse OSIM 的明确等价，这让 PV-OSIM 成为一个有解释的 OSIM 算法；第二，early multiplier resolution，它把复杂度瓶颈从 dense m×m factorization 转移为局部低秩/秩揭示操作。相比之下，LQR 视角的“可读性”是重要贡献，但更多是组织和推广已有 PV 思想；PV-soft 的推导很漂亮，但本质上是 penalty absorbed into inertia，机制上并不神秘。

需要直接判断的是：很多速度增益来自算法结构而非小工程优化，但实验中的绝对 runtime 增益有一部分来自 code generation 和机器人特定表达式裁剪。论文对 operation count 的比较相对有说服力；但与 MuJoCo/Pinocchio nominal runtime 的对比不能过度解读，因为后者承担了更通用的运行时逻辑。这里没有 data leakage 或 benchmark memorization 这类问题，但存在 evaluation setting 对静态约束组合和 codegen 友好的偏置。

Relation To Prior Work

这篇论文属于 Vereshchagin / Popov–Vereshchagin / ABA / articulated-body / OSIM 递推算法谱系，而不是新的 contact solver 或新的物理建模。它与 Featherstone ABA 的关系很紧：在无约束时 PV 退化为 ABA；在有约束时多传播了 K^A、l、L^A 等 dual 相关量。

和 Featherstone LTL / LTL-OSIM 的本质差异是消元空间不同。LTL 是对 joint-space inertia 与 Jacobian sparsity 做 sparse factorization；PV 是在 spatial/link recursion 中直接传播 constraint-space curvature。前者更像矩阵算法，后者更像动态规划/消息传递。

和 EFPA/KJR 的关系更微妙。它们都在算 inverse OSIM，且都利用 force propagation；PV-OSIM 的差异在于直接在 constraint space 累积 dual Hessian，少了一次 forward sweep 和空间逆惯量传播，因此常数更好。但从渐近复杂度看，EFPA 在深树、多约束的极端结构上可能更优，所以 PV-OSIM 不是无条件支配。

和闭链/loop constraint solver、Otter 等 early elimination 方法也很接近。论文的 PV solver 本身并非完全全新；实质新增在于 LQR/Gauss 统一推导、floating-base 任意 link constraints 的系统化扩展、dual Hessian=OSIM 的连接、PV-soft 和 PV-early 两个 O(n+m) 变体，以及与主流算法的系统 benchmark。换句话说，这篇更像“重新组织并推进一条被忽视的经典算法路线”，而不是从零发明 constrained dynamics。

Dataset / Evaluation

评估覆盖了典型机器人动力学场景：固定基机械臂、四足、人形、长链和合成 ladder 机制；约束包括手/脚的 3D/6D equality constraints，也包括 soft Gauss 情况。任务覆盖足以验证论文的核心 claim：结构化递推在高自由度、多分支、多约束系统上比常用 sparse factorization 更有优势。

它不是真实世界/真机评估，也不需要，因为论文关心的是算法复杂度和数值求解成本，不是控制策略性能。更关键的问题是 benchmark 是否验证了“更 scalable”：合成长链和 ladder 机制确实展示了复杂度差异，尤其 PV-early/PV-soft 的线性 scaling；OSIM 部分也通过长 stem 多 branch 机制说明 PV-OSIM 与 EFPA 的 crossover 条件，这一点比较诚实。

主要 evaluation limitation 是实现条件不完全对称。作者用 CasADi 生成机器人特定 C code，能裁剪大量静态表达式；而通用 simulator 的 nominal execution 需要处理更多运行时情况。作者也意识到这一点，因此更有价值的是 operation count 和自实现 LTL/EFPA 对比，而不是直接宣称比 MuJoCo/Pinocchio runtime 快多少。另一个限制是评估集中在 equality constraints；真实接触仿真中的 unilateral/frictional/contact switching 没有被验证。

Limitation

1. 约束模型上限：本文核心算法针对等式加速度约束。真实机器人接触通常是 unilateral + friction cone + complementarity + impact；这些才是 simulator 最难的部分。论文未来工作提到扩展，但当前方法没有解决这一层。

2. 满秩/可行性假设强：主推导默认 J 满行秩、L^A 可逆或子问题可解析。过约束、冲突约束、奇异构型需要 regularization、pseudo-inverse、proximal-point 或优先级处理。文中讨论了但未充分说明在真实接触密集场景下的稳定策略。

3. PV-fast 的结构适用性有限：MIL 加速依赖 floating-base 处 branching 明显且 L_b^A invertible。对约束集中在靠近 base 的高维 link、奇异姿态或非典型拓扑，增益可能下降甚至不可用。

4. PV-early 的 O(n+m) 不是免费的：它依赖每个关节局部能消去足够多 multiplier，使传播的 K^A 行数保持常数。单自由度关节可用 rank-1 Householder trick；多自由度关节需要 SVD 或拆成虚拟单自由度链，后者有表示奇异和非物理速度问题。这里的工程鲁棒性仍待验证。

5. Soft Gauss 的速度来自放松问题：PV-soft 很快，但它求的是 penalty-relaxed dynamics，不是 hard constraint dynamics。高 penalty 会改善约束残差，但也带来数值病态，并可能污染 constraint nullspace 下的加速度。它适合 simulation engineering，不应被等同于精确约束求解。

6. 部分增益归因不完全干净：算法结构优势明确，但绝对速度受 code generation、静态接触集合、局部坐标实现、矩阵操作裁剪影响。对运行时接触组合未知的通用 simulator，收益可能需要 hybrid codegen/cache 策略才能兑现。

Takeaway

  • 1. 最值得迁移的不是具体公式，而是“用优化问题的消元顺序重新设计动力学算法”。
  • 很多机器人算法的瓶颈不是模型，而是过早形成了错误的全局表示。
  • 2. Dual Hessian = inverse OSIM 是一个很有用的桥：它把 task-space inertia、constraint solving、LQR Riccati curvature 统一起来。
  • 这个视角可能对 constrained inverse dynamics、DDP/MPC gradients、contact-aware trajectory optimization 都有价值。

一句话总结

这篇论文把被低估的 PV 约束动力学路线重新表述为 Gauss principle 下的 LQR/Riccati 消元，并通过 OSIM 等价、soft constraint 吸收和 early dual elimination 证明：许多 constrained dynamics 的速度瓶颈本质上是错误的全局表示与消元顺序。