• 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
• 关节速度 q̇
• 关节力矩 τ

输出：

• 关节加速度 q̈

算法优势：
与传统方法相比，ABA 避免了质量矩阵 M(q) 的显式求逆，将复杂度从 O(n³) 降到 O(n)（n 为关节数）。

动力学背景

动力学方程

$$M(q)\ddot{q} + h(q, \dot{q}) = \tau$$

符号说明：

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

传统求解方法

直接解需要：

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

问题：矩阵求逆计算代价高（O(n³) 复杂度）

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

算法流程

ABA 采用三步递归策略：

Step 1: 前向遍历（Forward Pass）

方向：从基座 → 末端

计算内容：

• vᵢ：每个关节的空间速度
• cᵢ：偏加速度（Coriolis/重力项）

Step 2: 反向遍历（Backward Pass）

方向：从末端 → 基座

计算内容：

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

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

Step 3: 前向求解（Forward Pass）

方向：从基座 → 末端

计算内容：

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

算法示意图

三关节机械臂的递归传播

图例说明：

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

应用场景

机器人仿真引擎

• Gazebo
• MuJoCo
• Isaac Sim

动力学库

• RBDL
• Pinocchio
• Drake

控制算法

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

伪代码示例

function ABA(q, qd, 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 acceleration qdd[link]
        propagate to child

    return qdd

参考文献

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 / RBDL 源码实现