继续读实现，而不只记流程： 这篇保留为 ABA 入门笔记；新建的 Pinocchio 浮动基与源码专题 按三步展开：
01：Model、Data 与 SE(3) 状态 →
02：RNEA、CRBA 与 ABA 源码 →
03：接口调用与数值自检。

特别注意：带四元数浮动基时，配置向量 q 与切空间速度 v 维数不同；下文用 v、a 表示动力学的广义速度和加速度，不能直接当成配置数组的一阶、二阶逐元素导数。

• Articulated-Body Algorithm (ABA) 笔记
  • 简介
  • 动力学背景
  • 算法流程
    • Step 1: 前向遍历 (Forward pass)
    • Step 2: 反向遍历 (Backward pass)
    • Step 3: 前向求解 (Forward pass)
  • 算法示意图
    • 三关节机械臂的递归传播
  • 应用
  • 伪代码示例
• 参考

Articulated-Body Algorithm (ABA) 笔记

简介

Articulated-Body Algorithm（简称 ABA）是 Featherstone 提出的机器人动力学高效算法，主要用于 前向动力学（Forward Dynamics） 计算：

输入：

• 系统配置 q
• 广义速度 v
• 关节力矩 τ

输出：

• 广义加速度 a（与 v 同维）

算法优势：
对树形刚体系统、每个关节自由度有界且不包含接触约束求解时，ABA 通过递归求前向动力学，复杂度为 O(n)。通用稠密矩阵分解通常为 O(n³)，但稀疏结构会改变比较；工程上即使使用质量矩阵，也应解线性方程，而不是显式求逆。

动力学背景

动力学方程

$$M(q)a + h(q, v) = \tau$$

符号说明：

• M(q)：质量矩阵
• h(q, v)：科氏力、离心力、重力等非线性项
• τ：广义力矩

传统求解方法

直接解需要：

$$a = M^{-1}(\tau - h)$$

问题：上式用于表达数学关系；实现应使用分解后的 solve，或直接调用 ABA，不要照抄为 inverse()。

ABA 解决方案：通过递归传播替代矩阵求逆，避免显式计算 M⁻¹

算法流程

ABA 采用三步递归策略：

Step 1: 前向遍历（Forward Pass）

方向：从基座 → 末端

计算内容：

• vᵢ：每个关节的空间速度
• cᵢ：由关节运动与速度传播产生的偏加速度；重力需按算法约定另行引入，不应直接混写进每个 cᵢ。

Step 2: 反向遍历（Backward Pass）

方向：从末端 → 基座

计算内容：

• Iᵢ*：有效惯性（articulated-body inertia）
• pᵢ*：偏广义力

关键操作：聚合子关节的信息，逐层往上传递

Step 3: 前向求解（Forward Pass）

方向：从基座 → 末端

计算内容：

• 解出每个关节切空间的加速度 aᵢ
• 结合 τ 和上一步结果，传播最终加速度

算法示意图

三关节机械臂的递归传播

图例说明：

• 蓝色箭头：速度/偏加速度从基座传到末端
• 红色箭头：力和惯性从末端传回基座
• 绿色箭头：加速度从基座传到末端

应用场景

下面列出的是刚体动力学的相关应用与工具，不代表每个仿真引擎在所有模式下都默认调用同一种 ABA 实现；接触、闭环与约束求解还需要额外算法。

机器人仿真引擎

• Gazebo
• MuJoCo
• Isaac Sim

动力学库

• RBDL
• Pinocchio
• Drake

控制算法

• 模型预测控制（MPC）
• 强化学习（RL）
• 其他需要实时动力学计算的场景

伪代码示例

function ABA(q, v, tau):
    # Step 1: Forward pass - 计算速度和偏加速度
    for link in tree(root → leaves):
        compute spatial velocity v[link]
        compute bias acceleration c[link]

    # Step 2: Backward pass - 计算有效惯性和偏广义力
    for link in tree(leaves → root):
        compute articulated inertia I_star[link]
        compute bias force p_star[link]
        propagate to parent

    # Step 3: Forward pass - 计算关节加速度
    for link in tree(root → leaves):
        compute joint tangent acceleration a[link]
        propagate to child

    return a

参考文献

1. R. Featherstone, The Calculation of Robot Dynamics Using Articulated-Body Inertias, International Journal of Robotics Research, 1983.

2. R. Featherstone, Rigid Body Dynamics Algorithms, Springer, 2008.

3. Pinocchio v3.8.0 的 ABA 源码，更详细的变量与调用关系见本专题第 02 篇。