IEEE Transactions on Robotics

Constrained Articulated Body Algorithms for Closed-Loop Mechanisms

作者：Ajay Suresha Sathya, Justin Carpentier · 机构：Inria, Département d'Informatique, École Normale Supérieure, PSL Research University, Paris, France

会议/期刊：IEEE Transactions on Robotics · 发布日期：2026 · 页码：819-838

控制优化人形机器人

精读笔记

Problem Setting

《Constrained Articulated Body Algorithms for Closed-Loop Mechanisms》（IEEE Transactions on Robotics / 2026）处理的是 closed-loop mechanisms 的 equality-constrained forward dynamics，尤其是 internal loops。这里的难点不是约束动力学本身，而是 internal loop 破坏了 kinematic tree 上 ABA/PV/constrainedABA 所利用的递归结构。

已有高效递归 CDA 大多擅长外部 loop，例如足端接触到 ground 的约束，因为切 ground 后仍能保留较干净的树结构；但 internal loops 会在树内部产生跨分支耦合。joint-space LTL/proxLTL 可以处理这些约束，也能借 proximal 方法处理奇异，但它们本质上还是全局稀疏因子化，复杂度和内存访问都更重。BBO 这条历史路线其实已经看到递归处理 internal loops 的可能性，但缺少奇异鲁棒性、推导不透明、实现门槛高，因此没有成为 simulator 主流。

这篇论文的关键矛盾是：closed-loop dynamics 需要全局一致性，而高频仿真/MPC/RL 又需要局部递归复杂度。论文试图证明 internal loop 并不必然意味着全局 dense solve；只要用正确的变量消元方式，loop-induced coupling 可以被限制在图拓扑的局部 fill-in 中。

Motivation

作者不是从“再做一个更快的 simulator kernel”出发，而是从一个很具体的算法缺口出发：proximal constrainedABA 已经让外部约束的递归 CDA 同时具备低复杂度和奇异鲁棒性，但 internal loops 仍没有同等级别的工具。

核心观察是，闭环刚体动力学在 GPLC 下天然是结构化 QP。树机构之所以能 ABA，是因为这个 QP 在树上可以 serial DP / Riccati-style 消元；internal loop 只是把这个 DP 的图结构从 chain/tree 推广到一般图。于是问题从“设计新的动力学方程”变成“在图上以什么顺序消元二次型变量”。

缺口在于 prior work 没有同时满足三件事：处理 internal loops、低复杂度递归、对 redundant/singular constraints 鲁棒。LTL/proxLTL 满足前两者中的稳健性但不够低复杂度；BBO 满足递归和 internal loops 但不稳健；constrainedABA/proxPV 稳健且递归但只覆盖外部 loop。本文就是把这三条线接起来。

Core Idea

论文真正的核心是把 closed-loop constrained dynamics 重新解释为图上的二次型变量消元。选择一个 spanning tree 后，tree joints 用 acceleration recurrence 直接消去，cut-joints 作为约束留下。然后在 GPLC 的 QP 上引入 proximal/ALM，使奇异约束不再导致不可逆的 Delassus 或 rank-deficient KKT。接着用 nonserial DP 从叶子向根消元，每次只维护当前 link 与其 graph neighbors 的二次耦合。

这个思想有效的直觉是：internal loop 的非局部性不是任意全局的，而是由 cut-joint 所定义的图结构决定。只要消元顺序足够好，大多数机器人中的 loop coupling 是局部的，例如四连杆、齿轮/皮带、腿部闭环、手-物体多点接触中的局部子结构。递归算法的优势来自不显式构造和分解全局 JSIM/Delassus，而是在 spatial inertia 层面做局部 Schur complement。

和 prior 的本质区别不是 proximal 本身，也不是 DP 本身，而是把二者用于 internal-loop rigid-body dynamics，并明确给出两种变量消元策略：LCABA 是 primal/ALM 消元，proxBBO 是 dual/PPA 消元。它们引入的 inductive bias 是“闭环耦合应按机构图局部传播”，而不是“先组装全局矩阵再依赖通用稀疏因子化”。

Method

LCABA：它解决 constrainedABA 无法处理 internal loop 的问题。ALM 把 cut-joint constraint residual 的二次罚项变成 link acceleration 之间的惯性耦合。消元某个 link 时，这些 coupling 被传播给 parent 和邻居，形式上就是局部 Schur complement。最重要的变化是，约束不再通过全局 Jacobian 进入求解，而是以 link-link coupling 的形式参与 ABA-like backward/forward sweeps。

proxBBO：它解决 BBO 对奇异约束敏感的问题。dual PPA 在 multiplier 空间加入 proximal regularization，使需要反演的 constraint coupling block 保持正定。算法传播的是 link acceleration 与 constraint multiplier 的耦合项，并在 loop root 处 early eliminate 已经完整支持的 loop multiplier。核心变化是把 BBO 的早消元策略放进一个稳定的 proximal dual QP 框架里。

Nonserial DP：它解决 internal loops 造成递归依赖不再是单链/单树的问题。每次消元一个 leaf link，若它连接多个 loop neighbors，就在这些 neighbors 之间产生 fill-in。复杂度因此不再只由 n 决定，而由最大邻居数、每个 link 支持的 loop 数、以及 elimination order 决定。

Reduced sweeps：它解决 proximal/ALM 多次迭代的成本问题。第一次 sweep 形成 Hessian/factor-like 结构，后续迭代只传播 force/acceleration/RHS 的增量。这是工程上非常重要的效率来源，但不是新的物理 insight。

Key Insight / Why It Works

最核心的有效性来自两个层次。

第一，GPLC 的二次结构在消元下闭合。无论是 LCABA 里的 link-link inertial coupling，还是 proxBBO 里的 constraint multiplier coupling，消元一个局部变量后得到的 cost-to-go 仍是二次型。因此可以像 Riccati recursion 一样递推，只是从 serial/tree DP 变成 graph DP。这一点是理论基础。

第二，机器人机构的 loop graph 通常低 treewidth。论文的 linear practical complexity 并不是魔法，也不是最坏情况保证，而是依赖现实机构中 internal loops 多为局部结构、耦合数量有限。换句话说，速度增益本质上来自 better inductive bias / structure exploitation，而不是数值线性代数常数优化。若图的 treewidth 高，recursive 方法也会退化。

LCABA 大概率是本文最有实际价值的贡献。它比 proxBBO 更快的原因很直接：不需要在 loop root 对 constraint coupling 做额外 factorization，并且 elimination order 的自由度更高，可以用 minimum-degree 避免不必要 fill-in。proxBBO 的价值更多在理论和鲁棒性：它把 BBO 接到 proximal dual PPA 上，使其能处理 rank-deficient constraints。

proximal/ALM 处理奇异约束是必要条件，但不是主要性能贡献。性能主要来自变量消元顺序和局部结构复用。μ 的选择仍然是实际问题：proxBBO/proxLTL 对大 μ 更稳，LCABA 因 ALM penalty 在 primal Hessian 中放大，数值敏感性更明显。

所谓“state-of-the-art speed-up”应理解为 dynamics kernel 层面的结构化 scaling 胜利，而不是完整 simulator 的系统性胜利。接触检测、friction solve、time integration、stabilization、cache/layout 等端到端因素仍可能改变收益。文中未充分说明在真实 contact-rich simulator pipeline 中的整体占比。

Relation To Prior Work

这篇属于 Featherstone-style recursive rigid-body algorithms、constrained dynamics algorithms、proximal dynamics 三条谱系的交汇。

相对 LTL/proxLTL：本质差异是求解粒度。LTL/proxLTL 是 joint-space 稀疏因子化，先形成或等价处理 JSIM/Delassus；LCABA/proxBBO 则在 spatial/link 层面递归消元，避免把局部结构提升成较大的全局矩阵。proxLTL 的优势是稳健、成熟、实现清楚；本文优势是当 loop graph 低 treewidth 时更 scalable。

相对 constrainedABA/proxPV：本文真正新增的是 internal loops。constrainedABA/proxPV 的外部接触约束在图结构上较特殊，不能直接覆盖树内部 cut-joints。LCABA 可看作 constrainedABA 的 graph-DP 推广，proxBBO 可看作 proxPV/BBO 的 internal-loop 推广。

相对 BBO：proxBBO 的新意不是早消元本身，而是用 dual PPA/proximal regularization 给 BBO 一个奇异鲁棒且更可推导的形式。历史 BBO 的算法思想被保留，但其 rank assumptions 被弱化。

相对 explicit constraint / LCE：LCE 对局部 loop 很强，但依赖显式参数化或独立坐标，面对大 loop、耦合 loop、奇异 configuration 不够自然。本文坚持 implicit constraints，牺牲一部分对特定局部机构的最优性，换取更统一的约束表达。

不少“新”其实是已有思想的重组：ALM-dual PPA 等价、Schur complement 消元、minimum-degree heuristic、Riccati/DP 类比都不是新概念。实质创新在于把这些放进 closed-loop rigid-body CDA，并给出可实现的递归算法与 Pinocchio 级 benchmark。

Dataset / Evaluation

evaluation 覆盖了真实机器人拓扑和合成拓扑两类。真实拓扑包括手抓物体、双手协作、humanoid with hands、Digit 的腿部闭环与脚接触、双 Digit 协作等，基本覆盖了作者 claim 中最重要的 internal-loop 场景。合成拓扑则用于展示 single loop、chain of loops、worst-case coupled loops、LCABA-friendly branching topology 下的 scaling 行为。

这组实验足以支持一个中等强度的 claim：在低 loop coupling、高 DoF、内部闭环较局部的机器人上，LCABA/proxBBO 比 proxLTL 更有 scaling 优势；在 redundant constraints 下 proximal 方法能收敛且避免奇异失败。尤其对 humanoid + internal loops 的 kernel benchmark，证据比较充分。

但 evaluation 没有证明端到端仿真或控制系统一定获得同等收益。所有结果主要是 CDA kernel timing，且是 equality constraints。没有 frictional contact complementarity、contact mode switching、solver outer loop、implicit integration、controller closed-loop stability 或真机部署。benchmark 对 claim“作为 MPC/RL/large-scale simulation 基础”只是间接支持。

另外，proxLTL 已是高度优化实现，比较相对公平；但 LCABA/proxBBO 作为新实现，cache layout、vectorization、memory allocation 等工程因素可能仍影响结果。文中对这部分归因没有完全拆开，部分增益可能来自 algorithmic complexity，部分可能来自 implementation path 和拓扑特化。

Limitation

第一，复杂度上限受 graph fill-in 控制。论文强调 practical linear complexity，但这依赖 loop 局部、coupling 稀疏、elimination order 好。若所有 loop 强耦合，LCABA/proxBBO 都会接近全局 cubic 行为。这里不是泛化性问题，而是图 treewidth 的硬上限。

第二，spanning tree 是输入且影响巨大。作者承认自动选择 spanning tree / elimination order 是离散优化问题且未解决。实际部署到动态接触图时，哪些 contact edge 入树、哪些作为 cut constraint，会显著影响性能。这个问题被转移给用户或上层系统。

第三，LCABA 的数值稳定性依赖 μ。高 μ 加速 ALM 收敛但恶化 conditioning，论文实验也显示 LCABA 相对 proxBBO/proxLTL 对大 μ 更敏感。推荐 μ 范围更像经验 recipe，不是强理论选择。

第四，目前只处理 equality motion constraints。现实 contact-rich simulation 的主要困难往往是不等式接触、摩擦锥、stick-slip、contact mode changes。本文算法更像这些方法的 inner equality solver，而不是完整 contact solver。

第五，integration 处理偏基础。semi-implicit Euler + Baumgarte 可以跑 benchmark，但对高刚度闭环、长期能量行为、隐式 DAE 稳定性没有充分覆盖。若要进入高保真 simulator，可能需要非平凡改造。

第六，proxBBO 对 CoM/momentum 等涉及多 link 的 global constraints 可能更合适，但文中没有系统验证。LCABA 对 multilink/global constraints 会因 neighbor count quadratic 增长而不推荐，这说明方法并不是通用 sparse QP solver，而是针对机构图局部性很强的 CDA。

Takeaway

  • 1. 这篇真正推动的是：internal closed-loop CDA 可以重新回到递归算法路线，而不必默认走 joint-space LTL/proxLTL。
  • 2. 最值得迁移的 insight 是“把 dynamics kernel 看成结构化二次图模型，然后选择变量消元顺序”。
  • 这不仅适用于刚体动力学，也可能迁移到 graph-structured optimal control、factor graph inference、periodic/scenario-tree Riccati recursion。
  • 3. LCABA 是更可能被 simulator/MPC 实际采用的算法：快、结构接近 ABA/constrainedABA、对局部 loops 友好。

一句话总结

这篇论文把 closed-loop constrained dynamics 从 joint-space 全局因子化重新组织为 proximal 图消元式递归 ABA，是一次针对 internal loops 的结构化 scaling 推进，而非新的物理模型。