010｜Articulated-Body Algorithm：不形成整块逆矩阵，也能算出机器人加速度

系列首页 · 上一篇 · 下一篇

原论文：Roy Featherstone，The Calculation of Robot Dynamics Using Articulated-Body Inertias，1983，The International Journal of Robotics Research 2(1):13–30。原出版页。

补充来源：作者Spatial_v2文档。

先接住前作的问题

Newton–Euler由加速度求力矩，仿真却常已知力矩、未知加速度。本篇用铰接体惯量逐关节消元解决这一互补问题；操作空间进一步讨论末端方向的等效惯量，二者处于不同描述层次。

五个前置概念

五个前置概念是：前向动力学由力求加速度；空间惯量表达刚体响应；铰接体惯量保留内部自由度影响；Schur补通过消元减少未知量；时间积分把瞬时导数变成轨迹。

一、问题设定：给定力矩后怎样求出开放链的关节加速度

控制器给每个关节一个力矩，机器人下一瞬间怎样加速？最直接做法是构造整个惯性矩阵M，再求解$M\ddot q=\tau-h$。机械臂关节很多时，完整矩阵操作可能浪费机构本身的链式结构。

与逆动力学的区别

RNEA输入运动，输出所需力矩；ABA输入力矩，输出运动加速度。两者都利用机构结构，但未知量的方向不同，不能只把RNEA“倒着运行”就得到ABA。

前向动力学只给出瞬时导数。模拟器还需要时间积分和接触处理。即使ddq求得准确，不合适的积分步长仍会导致能量漂移或穿透，不能把所有仿真质量都归结于动力学求解器。

二、消去子机构自由度，形成铰接体惯量

铰接体惯量到底是什么

对一个普通刚体，空间惯量I把空间加速度和所需空间力联系起来。对子机构，内部关节允许部分运动，施力后能量可能分配到这些自由度，因此当前连杆感受到的是经过内部自由度消元后的关系。

用现代单自由度关节记号，设当前汇总惯量为$I^A$、关节轴为S，定义：

$$U=I^A S,\qquad d=S^\top U.$$

消去当前关节加速度后，向父连杆传递的惯量部分为：

$$\bar I^A=I^A-Ud^{-1}U^\top.$$

这个形式是一个局部Schur补。减去的项表示：当前关节可以沿自己的自由方向运动，因此父连杆不必像面对“焊死的子机构”那样承担全部惯性。还需要同步递推速度相关和已知力矩的偏置项，只有惯量式不足以完整求解。

完成向根部的消元后，再从根向叶恢复关节加速度：

$$\ddot q_i=d_i^{-1}(u_i-U_i^\top a_{parent\rightarrow i}).$$

其中u汇总驱动力矩与偏置影响，父连杆加速度要先变换到当前坐标并加入相应速度项。这是解释算法结构的简写，不是可直接省略其他项的控制代码。

三遍都是固定规模的局部运算，因此开放链中计算规模随关节数线性增长。现代实现可以推广到树和浮动基座，但应区分算法家族的后续实现与原始论文的直接范围。

用一个两自由度教学系统可以直接看到消元改变了什么。忽略速度偏置和重力，取

$$\begin{bmatrix}2&1\\1&2\end{bmatrix} \begin{bmatrix}\ddot q_1\\\ddot q_2\end{bmatrix} =\begin{bmatrix}1\\0\end{bmatrix}.$$

第二行给出 $\ddot q_2=-\ddot q_1/2$，代回第一行得到有效惯性 $2-1\cdot2^{-1}\cdot1=3/2$，因此 $\ddot q_1=2/3,\ddot q_2=-1/3$。若第二关节锁死，约束变为 $\ddot q_2=0$，此时 $\ddot q_1=1/2$，并需约束反力矩 $\tau_2=1/2$。两种响应不同源于边界条件不同，不是同一个质量被算少了。

偏置递推也不能省略。作者公开的FDab源码中，$u=\tau-S^\top p^A$，缩减后的偏置力为 $\bar p^A=p^A+\bar I^A c+Ud^{-1}u$，再以 $X^\top\bar p^A$ 传给父节点。这个 $Ud^{-1}u$ 项把已知关节力矩对上游的影响带回去；只递推惯量会在非零输入时漏项。源码是后续现代实现的直接证据，仍不能替代1983原稿的运算计数表。

铰接体惯量已经消去了子机构能够自行运动的响应；恢复关节加速度时不能把它当焊死后的惯量和。自由关节与锁死关节的对照会检查这个区别。

三、执行递推恢复，用自由与锁死关节作对照

输入：机构、q、dq、tau、惯性、重力和外力
第一遍，从根向叶：计算关节变换、空间速度与偏置加速度
初始化每个连杆的铰接惯量与偏置力
第二遍，从叶向根：
    计算U、d及有效关节驱动力u
    消去该关节未知加速度
    把缩减惯量和偏置力变换、累加到父连杆
第三遍，从根向叶：
    根据父连杆加速度恢复当前关节ddq
    更新当前空间加速度，供后续子连杆使用
输出ddq

教学例子：自由关节与锁死关节

想象两根杆通过一个关节连接。末端关节锁死时，上游驱动必须使两根杆像一个整体运动；关节自由且没有驱动力矩时，后杆可以相对转动，上游看到的瞬时动力学响应不同。ABA在消元中保留这种可运动性，而不是把两杆合并为永久刚体。

这并不意味着自由关节消除了质量或违反动量守恒。变化的是可允许运动及其耦合关系。若把关节锁死，需要在模型里移除相应自由度或施加约束，不能仍使用“自由关节”算法结果解释锁死系统。

一个数值一致性试验是先随机选择q、dq和ddq，用逆动力学求tau，再把tau输入前向动力学，检查是否恢复原ddq。在良态模型下应接近；如果失败，可检查重力、外力符号、关节轴和空间变换。此试验验证实现互相一致，不验证真实机器人的惯性参数。

四、读复杂度论证并区分原文与现代教学实现

读图：左右不是两种近似精度，而是自由与锁死两种不同边界条件。Schur 补消去可运动的内部自由度；锁死后则必须保留约束反力，不能沿用自由关节的加速度。

1983 年原版运算表与图仍不可核查；后续 Spatial_v2 的 FDab、FDcrb、ID 有助于区分接口，不能据此倒推原版已经具备后来的全部功能。

复杂度是随规模增长的规律，不是固定规模的速度排行榜。一个渐近更好的算法可能有较大常数；具体机械臂自由度少时，另一种方法可能更快。原文运算比较表的详细范围尚未完全核实，因此不填入看似精确的交叉点数字。

现代复现可将ABA结果与“显式构造M再线性求解”比较，但应保持同一模型、同一外力和相同浮点精度。测试还要避免通过同一段错误代码生成两边的参考项，否则一致性可能只是共享错误。

五、检查关节约束、模型拓扑与数值退化

闭链或与地面刚性接触时，未知约束力把不同分支连接起来，单纯开放链递推不够。可以在ABA基础上构造约束求解器，也可以采用其他稀疏方法，但需要额外方程和一致性条件。

质量、惯量或关节模型病态会让d接近零，使局部求解不可靠。随意加入一个小常数可以避免除零，却改变物理模型；应先判断是否存在零质量连杆、错误惯量或冗余坐标。

在模型预测控制中，快速动力学有助于更频繁地模拟候选动作，但控制效果仍受模型误差、优化收敛和传感延迟影响。计算效率改进不能直接等同为真实机器人更稳定。

六、追问：不形成完整惯性矩阵，能否计算它的逆对一个向量的作用？

七、把开放链动力学接到任务与接触

• 教学联系：操作空间：直接设计夹爪的动力学行为。比较末端任务惯量与连杆子树等效惯量，明确两者描述层次不同。
• 模块组合：Stewart–Trinkle：不要追逐无限大的接触力，直接计算冲量。在刚体动力学外增加冲量与接触约束，不能直接把开放链递推当完整接触求解器。

八、自测与参考答案

问题1：铰接体惯量为什么不等于把后面所有连杆惯量相加？

参考答案：后续关节允许相对运动，影响当前连杆受到的动力学反作用。只有把相对运动锁死时，才可按复合刚体方式处理；ABA需要先消去内部自由度。

问题2：O(n)为什么不保证六关节机器人上一定最快？

参考答案：复杂度只描述规模增长，实际耗时还取决于固定常数、缓存、向量化和实现。应在相同模型和硬件上测量，而非由大O符号推断全部性能。

问题3：ABA求得加速度正确，仿真却越来越增能，可能哪里出问题？

参考答案：积分方法或步长可能不适合当前动力学，接触处理也可能注入非物理能量。应分别检查动力学残差、积分收敛与接触模型，不能只反复修改ABA。