IEEE Transactions on Robotics

On Second-Order Derivatives of Rigid-Body Dynamics: Theory and Implementation

作者：Shubham Singh, Ryan P. Russell, Patrick M. Wensing · 机构：Department of Aerospace Engineering, University of Texas at Austin, Austin, TX, USA; Department of Aerospace and Mechanical Engineering, University of Notre Dame, Notre Dame, IN, USA

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

运动规划控制优化人形机器人

精读笔记

Problem Setting

《On Second-Order Derivatives of Rigid-Body Dynamics: Theory and Implementation》（IEEE Transactions on Robotics / 2024）处理的是刚体动力学二阶导数的可计算性问题，而不是提出新的控制器。目标对象是开放链机器人，包括固定基、浮动基和多自由度关节；输出是逆动力学 ID 和正动力学 FD 对 q、qdot、qddot/tau 的二阶偏导。

关键矛盾是：优化控制需要更高阶局部模型，但 RBD 的二阶导数一旦展开就是高维张量；如果把 RNEA/ABA 当黑盒，有限差分不准、complex-step 太慢、AD/CodeGen 代码膨胀、逐行 chain-rule 又不利用物理结构。以前方法卡住的点不是“不能求导”，而是求导后完全失去刚体树结构、子树复用和 sparsity，导致二阶 DDP 在机器人系统里长期被一阶近似替代。

Motivation

已有路线的缺口很明确：一阶 RBD 导数已经有较成熟的解析算法，但二阶导数缺少可维护、可实现、支持多自由度关节的统一表达。Lee et al. 类 chain-rule 方法能得到二阶量，但更像对程序逐行展开，计算图层面的冗余很大；Nganga/Wensing 的方向导数避免完整张量，但牺牲了并行性和通用 SQP/DDP 使用场景；AD 能省推导但在高 DoF 模型上会变成代码生成和内存问题。

作者的观察是：二阶导数复杂，很大程度是 notation 和 algebra 没有跟上。空间向量代数本来就把刚体动力学结构压缩得很好；若能把 SVA 扩展到空间矩阵和张量，并系统描述多自由度关节上的 Lie derivative，那么二阶导数可以像一阶导数一样沿树递归，而不是靠通用 AD 暴力展开。

Core Idea

论文的真正核心是结构化重写二阶导数的信息流。它不是对 RNEA/ABA 做更快的 AD，而是把二阶导数表达成 composite inertia、composite force、body-Coriolis、motion subspace 及其导数的组合，并沿祖先路径填充非零张量块。这样，动力学耦合只在同一祖先链/子树关系中出现，分支树上的大量导数天然为零。

引入的 inductive bias 是刚体树的物理结构：子树锁定惯量、速度相关项、力传播方向、关节 motion subspace 的几何扰动。prior 的通用微分工具不知道这些对象的物理语义，只能沿计算图传播；本文把“哪些量会影响哪些量”显式编码到算法中，因此在 branched robot 和 humanoid 上更 scalable。对 FD，作者没有硬推 ABA 的二阶递归，而是利用 ID-FD 的隐式关系：FD 二阶导数 = ID 二阶导数 + dM/dq + FD 一阶导数 + M^{-1}。这是一种问题重分解，把难导的 ABA 换成更好结构化的 ID 导数和线性代数。

Method

1. Lie derivative 作为配置导数定义：解决 SO(3)/SE(3) 上“对 q 求导”的坐标依赖问题。对每个关节自由模式选取生成元，扰动是 T exp(epsilon E)。这让球关节、浮动基等不需要依赖 Euler angle，也解释了为什么多自由度关节下二阶 q-q 导数可能不交换。

2. Tensorial SVA：把空间矩阵的列视为多个空间运动方向，定义扩展叉乘、扩展力叉乘、tensor transpose 和矩阵-张量乘法。它的作用是把多自由度关节的整块导数一次性表达出来。机制上，它保留了空间向量代数的反对称和伴随关系，使大量项可以化简。

3. IDSVA-SO：算法分为运动学前向、动力学 composite 后向、导数后向。前两步计算 v、a、Sdot、Psi dot/ddot、I^C、B^C、f^C；第三步按 i-j-k 祖先三重循环填充二阶张量。重要点是循环拓扑由树结构决定，而不是对所有 n^3 项暴力枚举。

4. FD 二阶导数的隐式组装：使用 dFD/du = -M^{-1} dID/du，以及二阶扩展式。对于 tau 相关项，dFD/dtau = M^{-1}，二阶 tau-tau 和 tau-qdot 为零，tau-q 由 dM^{-1}/dq 给出。这样 FD 的高阶导数复用 ID-SO 和一阶 FD，而不是重新微分 ABA。

5. 工程选择：作者尝试 AZA/IDFOZA 来避免直接大张量乘法，但发现实际 cross-over 与硬件、编译器优化有关。最终并不是所有理论低复杂度路线都赢；这部分说明论文的实现结论很务实，也意味着部分性能增益来自精细工程。

Key Insight / Why It Works

最核心贡献是：把二阶 RBD 导数的“张量爆炸”变成“树结构上的局部张量块填充”。这不是单纯 scaling，也不是 AD 工具替换，而是 better inductive bias：刚体系统的耦合结构先验被显式注入导数算法。对于串联链，最坏情况下仍接近高阶复杂度；但对真实 humanoid/quadruped，树分支和浮动基结构带来的 sparsity 会显著减少有效计算。

为什么有效：RBD 的许多导数不是任意函数 Hessian，而是由空间变换、惯量、速度叉乘和力传播组成。空间叉乘的反对称性、composite inertia 的子树可加性、motion subspace 的祖先依赖，共同决定了大量项可以共享或为零。AD 看不到这些高层语义，只能在低层算子图上做链式法则；本文直接在物理对象层面求导，所以减少了冗余中间量。

最可能的核心贡献排序：第一是 tensorial SVA + 多自由度关节 Lie derivative 约定，它让公式可写且不依赖坐标奇异参数；第二是 IDSVA-SO 的树递归和索引重排；第三是 FD 通过 ID 二阶导数组装。FD 部分理论上漂亮，但性能仍受张量乘法和 M^{-1} 乘法支配，更多是工程可用性贡献。

哪些可能只是 engineering：CodeGen 对比、Eigen tensor slicing、AVX/O3/march=native、Pinocchio 版本优化等明显属于实现层；它们影响最终数字，但不是方法成立的原因。论文里 AD+CodeGen 在某些小模型上接近甚至优于普通解析实现，也说明所谓解析优势并非无条件，必须结合结构化实现才成立。

需要直接指出的是：这篇没有证明“用这些二阶导数后控制效果更好”。它证明的是导数本身可高效准确计算。二阶 DDP 是否比 iLQR 在接触-rich、人形 MPC 中值得，仍是另一个问题。

Relation To Prior Work

最接近的路线有三类：一是 RBD 一阶解析导数算法，如 IDSVA/FDSVA、Garofalo 等质量矩阵导数；二是 Lee et al. 的二阶 chain-rule/RNEA 递归表达；三是 CasADi/ADOL-C/CppAD/RobCoGen/Drake 等 AD 或 CodeGen 路线。

本质差异在抽象层级。AD 路线在程序算子层面传播导数，优点是通用，缺点是生成图巨大且不理解刚体树；chain-rule 路线比 AD 更显式，但仍偏逐行微分，缺少足够代数化简；本文在物理结构层面定义导数对象，把 composite inertia、body-Coriolis、motion subspace 的导数作为基本构件。因此它不是“已有公式的代码优化”，而是把二阶 RBD 导数放回 SVA 的结构体系里。

看似新的部分中，FD 二阶导数通过 ID 导数和 M^{-1} 隐式关系组装，本质上延续了一阶 FD 导数利用 ID 导数的思想；真正新增的是把它系统扩展到二阶张量并讨论实现路径。Tensorial SVA 和多自由度关节下二阶张量约定是更实质的创新，因为它解决了先前文献中 revolute/prismatic 单自由度假设过强的问题。

Dataset / Evaluation

评估不是数据集意义上的 benchmark，而是数值算法评测。覆盖了合成串联链、二叉树，以及多个真实机器人模型，包括固定基机械臂、四足、ATLAS/Talos 等浮动基人形。这个覆盖范围足以验证算法在不同树深度、分支稀疏性、多自由度基座上的 runtime/accuracy 行为。

实验支持了核心数值 claim：解析 IDSVA-SO/FDSVA-SO 比 finite difference、bicomplex、朴素 chain-rule 和 AD+CodeGen 更快；误差相对 bicomplex 或 AD 保持在可接受水平；AD+CodeGen 会出现代码规模膨胀和生成不可行问题。尤其是串联链 vs 二叉树对比很好地说明了 branch-induced sparsity 的作用。

但 evaluation 没有覆盖真实控制任务闭环表现，没有展示 DDP/SQP 收敛速度、MPC 频率、接触任务鲁棒性，也没有比较“全二阶导数带来的优化收益是否抵消计算开销”。因此它验证的是 derivative engine，而不是验证 full second-order control pipeline 的最终价值。

Limitation

主要前提是开放链刚体树、无外力/接触的基础动力学。文中提到外力可由扩展覆盖，但核心推导和评估并未把接触、多点约束、闭链机构、摩擦互补这些真实腿式机器人优化中的难点纳入主线。因此方法解决的是 RBD smooth dynamics 的二阶导数，不是整个 contact-rich optimal control Hessian。

scalability 上，ID-SO 的 O(N d^2) 对分支树友好，但串联链 d=N 时仍会接近 O(N^3)。FD-SO 更明显：直接张量-矩阵操作有 O(N^4) 成分，虽然实际机器人规模下可接受，但算法上没有完全消除高阶张量成本。对非常高 DoF 系统或需要大量 shooting nodes 的 MPC，内存带宽和张量写入可能成为瓶颈。

多自由度关节的非交换 Lie derivative 使 Hessian 块不再具备普通欧氏坐标下的对称直觉；下游优化器若假设 Hessian 对称，可能需要非常小心的接口约定。文中对此有说明，但没有深入讨论对 DDP/SQP 实现的影响。

增益归因也不是完全纯粹。解析公式、树稀疏、手工循环、Pinocchio/Eigen 优化、编译器向量化、CodeGen 策略都混在最终 runtime 中。可以判断主要增益来自结构化解析与 sparsity，但具体到 3.2x/3.8x 这类数字，工程因素占比不小。

最后，论文没有证明二阶动力学导数在实际控制中一定值得。许多机器人优化器使用 Gauss-Newton、iLQR 或忽略动力学 Hessian，并不只是因为算不出，而是因为二阶项可能带来非凸性、正则化负担和数值不稳定。这个应用层 trade-off 文中未充分说明。

Takeaway

  • 1. 二阶 RBD 导数不是只能交给 AD；只要把导数对象提升到空间向量/张量代数层，刚体树结构可以继续被利用。
  • 这是本文最值得迁移的 insight。
  • 2. 对复杂机器人，真正的 scalability 常来自物理 sparsity 和 composite 量复用，而不是通用微分工具的模式选择。
  • AD 在小模型上方便，但在高 DoF、高阶导数、CodeGen 场景下会迅速暴露代码规模问题。

一句话总结

这篇论文把刚体动力学二阶导数从通用 AD/有限差分问题重构为基于 tensorial SVA 和刚体树稀疏性的结构化递归计算，是二阶优化控制基础设施层面的实质推进，而不是新的控制算法。