005｜递归Newton–Euler：沿机械臂来回走一遍，计算关节力矩

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

原论文：J. Y. S. Luh、M. W. Walker、R. P. C. Paul，On-Line Computational Scheme for Mechanical Manipulators，1980，Journal of Dynamic Systems, Measurement, and Control 102(2):69–76。原出版记录。

先接住前作的问题

D-H和任务速度控制分别给出几何位置与协调速度，仍没有回答执行这条轨迹需要多少力矩。这里是模型层的组合关系：逆动力学增加质量、惯量与外力，并利用链式依赖组织在线计算。

五个前置概念

五个前置概念：逆动力学由运动求力；空间向量组合转动和平移；惯性描述加速度代价；力的对偶变换维护功率一致；递推利用机构拓扑避免重复计算。

一、问题设定：给定关节运动后怎样在线求出驱动力矩

规划器给出一条关节轨迹，电机却需要力矩指令。即使两个关节各自匀速，旋转连杆之间仍有惯性耦合，载荷和重力也会改变所需力矩。直接展开完整动力学表达式容易变得庞大，而且每个控制周期都要重新求值。

逆动力学究竟解什么

机械臂动力学常写成：

$$M(q)\ddot q+h(q,\dot q)=\tau+J(q)^\top f_{ext}.$$

M是惯性矩阵，h包含重力、科里奥利和离心等项，$f_{ext}$是外部施加到机器人的力旋量。给定运动求$\tau$叫逆动力学；给定力矩求$\ddot q$叫前向动力学。两者不能因为都含M而混为一谈。

如果载荷未知，计算得到的是所设载荷模型下的力矩。算法再精确也不会自动知道夹爪刚拿起一个更重的箱子。实际控制通常将模型前馈与反馈结合，处理这种偏差。

二、向外传播运动，再向内回传力与力矩

两次递推：运动向外，受力向内

为了理解结构，使用六维空间速度v与空间加速度a。每根连杆的空间惯性I把加速度与力关联，X负责把父连杆上的量变换到当前连杆坐标，S描述关节允许运动的轴。

前向递推的现代示意形式为：

$$v_i=X_i v_{p(i)}+S_i\dot q_i,$$ $$a_i=X_i a_{p(i)}+S_i\ddot q_i+v_i\times(S_i\dot q_i).$$

最后一项表示转动坐标与速度组合产生的加速度项。不能在多个坐标系之间直接加数字；变换在这里是物理必要步骤。

连杆所需空间力为$I_i a_i+v_i\times^*I_i v_i-f_i^{ext}$。然后累加子连杆传来的反作用，最后投影到关节允许的运动方向：$\tau_i=S_i^\top f_i$。空间力与空间速度的交叉运算不同，所以使用$\times^*$区分对偶运算。

为什么回传用转置而不是把运动变换照抄一次？若子坐标中的速度满足 $v_i=X_iv_p$，同一相互作用的功率必须满足 $f_i^\top v_i=f_p^\top v_p$，于是 $f_p=X_i^\top f_i$。这给出回传式

$$f_i\leftarrow f_i+\sum_{j\in\operatorname{children}(i)}X_j^\top f_j, \qquad \tau_i=S_i^\top f_i.$$

转置不仅旋转力，还把力臂产生的力矩带回父节点。漏掉这个力矩，会使每个连杆自身的Newton–Euler方程看起来正确，整条链的关节力矩却不守恒。该推导采用现代空间向量记号；可与Featherstone作者的ID实现说明交叉检查，不把后来的软件当作1980年程序。

原创静态回传例：两根1米、各1千克的均匀杆水平伸直，取 $g=10\,\mathrm{m/s^2}$。第二关节承受第二杆重力，故 $\tau_2=10\times0.5=5\,\mathrm{N\,m}$；第一关节还承担两个质心，故 $\tau_1=10\times0.5+10\times1.5=20\,\mathrm{N\,m}$。末端增加1千克载荷后，变为 $(\tau_1,\tau_2)=(40,15)$。这个两节例子比单杆检查更能抓出“只在末关节加负载”的错误。

为什么不必显式形成完整惯性矩阵

逆动力学只需要给定一次运动时的力矩，并不总需要把M的每个元素存下来。递推直接计算这次运动造成的受力，避免先展开所有关节对之间的完整符号表达。

每根连杆只执行固定规模的局部计算，遍历次数固定，因此对于相应链结构，运算量随连杆数线性增长。这个复杂度描述不意味着所有基于Lagrange方程的方法都天然低效：同一个动力学也可以通过不同算法有效计算。应比较具体算法与实现，而不是把物理定律当作性能标签。

两次递推的方向来自运动依赖与受力平衡，代码中还要把重力和外力放进正确符号。静止水平杆能先暴露漏掉重力的实现，再看历史计算效率证据。

三、实现两遍递推，用水平静杆检查重力项

输入：运动q、dq、ddq，机构树，空间惯性和外力
设置基座速度与加速度；按约定加入重力
从根向叶遍历：
    计算父到子的运动变换X和关节轴S
    递推当前连杆速度v、加速度a
    计算惯性力与速度相关力，减去已知外力
从叶向根遍历：
    把子连杆力变换并累加到当前连杆
    tau_i = S_i转置 * f_i
输出所有关节驱动力tau

这段教学伪代码表达RNEA家族的结构，不把现代树状扩展或空间向量库接口归给原文。原论文主要讨论机械臂在线计算；处理闭环约束需要额外约束力求解。

教学例子：一根静止水平杆

一根长度L、质量m的均匀杆，在重力下水平静止。关节速度和加速度为零，但维持姿态所需重力力矩幅值仍为$mgL/2$。若程序输出零，通常是重力没有进入基座加速度或外力计算。

如果杆末端再挂一个质量M的物体，附加力矩幅值为$MgL$。这部分受力沿链条向基座传递，多连杆情况下每个上游关节都会受到影响。不能只给最后一个关节增加负载补偿。

再让杆旋转，加速度项与速度相关项会出现。速度为零并不代表加速度为零，某个轨迹转折点可能瞬时停止，却仍需要较大加速力矩。这是区分姿态、速度与加速度输入的一个简单测试。

四、结合原版勘误阅读递推证据与教学图

读图：箭头标出各重力的真实作用点。新增末端载荷对两个关节的力臂分别为 2 m 和 1 m，因此需要增加 20 与 10 N·m，不能只在第二关节补力矩。

原版勘误与复现版本

原论文有正式勘误：Erratum: On-Line Computational Scheme for Mechanical Manipulators，Journal of Dynamic Systems, Measurement, and Control 102，1980 年 9 月，第 173 页。出版原页镜像。勘误原页已视觉核对：第 74 页第二栏漏掉 Step 6，应按式（45）计算关节作用量；Step 7 应在 i=1 时停止，否则取 i'=i−1 回到 Step 5；第 73 页式（38）还修正了相邻连杆为移动关节时的第三项。

式（38）的细小上标在镜像中辨认有限，因此这里不手工转录该长式。实现原论文逐式算法时需同时采用正式勘误。下面的现代空间向量教学递推说明计算结构，不宣称逐字复刻存在勘误的初版步骤。

原文证据与现代检验

1980 年原版详细运算表与实验图仍不可核查；现代空间向量递推解释算法结构，不等同于当年实现逐式复刻，历史硬件计时也不能预测现代平台速度。

理论上精确是相对于刚体模型而言。摩擦、弹性、执行器滞后和参数误差会使真实系统偏离模型。原文摘要中历史计算机的毫秒数，不应脱离浮点实现、机构自由度和测量方式直接比较今天的控制库。

适当的现代复现实验可以对同一组q、dq、ddq，比较递推结果与由能量法构造的独立实现；再改变连杆数观察耗时趋势。两个实现都使用同一错误惯性参数时，彼此一致也不说明真实模型正确，因此还需要真实力矩或运动测量。

五、核对外力符号、惯性坐标与刚体假设

实际需要注意外力符号：传感器读到的“工具作用于环境的力”与方程中“环境作用于工具的力”方向相反。惯性参数也必须在正确坐标系表达，质心惯量与连杆原点惯量不能混用。

另一边界是执行器能力。算出力矩只是需求分析；若超出电机限值，实际运动无法遵从规划加速度。规划、时间参数化和控制器必须据此协调，而不是把超限数值直接裁剪后仍声称跟踪原轨迹。

六、追问：只用静止水平杆测试通过，为什么仍不能确认速度相关项正确？

七、反求加速度，或在线修正未知参数

• 平行路线：Articulated-Body Algorithm：不形成整块逆矩阵，也能算出机器人加速度。在已知力矩时递推求加速度，是与逆动力学互补的计算方向。
• 模块组合：Slotine–Li 自适应控制：一边跟踪，一边修正动力学参数。将参数更新加入跟踪控制，处理固定负载模型失准。

八、自测与参考答案

问题1：为什么先算运动，再从末端回传力？

参考答案：子连杆运动依赖上游运动，所以向外计算；上游需要承受所有下游连杆的惯性和外部负载，所以受力向内累加。两个方向分别对应两类依赖。

问题2：静止时所有关节力矩是否都应为零？

参考答案：不应。零速度与零加速度没有消除重力或外部载荷。只有在相应力矩恰好为零、或由其他支撑承担时，某个关节才可能无需驱动力。

问题3：RNEA快，是否意味着可以直接用它计算任意接触系统的加速度？

参考答案：不能。它主要解决给定运动的逆动力学；接触系统还要确定未知约束力和满足接触条件的加速度。前向动力学及约束求解是额外问题。