牛顿-欧拉算法 - 机械臂动力学模型

牛顿-欧拉算法是机械臂动力学建模与计算的核心利器，理解后就能明白 Pinocchio 在底层是如何高效、实时地算出每个关节所需力矩的。

一、目标（逆动力学）

问题：如果想让机械臂按照一条精确的轨迹运动（比如快速趋近），必须实时知道每个关节电机要出多大的力（力矩），才能克服手臂自身的重量和惯性，完美跟上轨迹。

• 已知：目标轨迹上每个瞬间的关节位置 q、速度 q˙、加速度 q¨。
• 求解：每个关节所需的驱动力矩 τ。
• 牛顿-欧拉算法：就是用来解决这个逆动力学问题的标准高效算法。

二、基本原理

它的核心思想有两步：首先，把一整条机械臂，拆解成一个个独立的连杆；然后，对每个连杆应用牛顿定律（力）和欧拉方程（力矩），通过关节将它们串联起来。

对每一个单独的连杆，将它的质心视为一个质点：

• 牛顿第二定律（力）：

，连杆 i 受到的合外力，等于它的质量乘以质心加速度。

• 欧拉方程（力矩）：

，连杆 i 受到的合力矩，等于它绕质心的转动惯量乘以角加速度，再加上一个角速度带来的陀螺力矩。对于陀螺项，即使角加速度为零，只要它在旋转（ωi≠0），改变它的转轴方向就需要额外的力矩。

这两个方程给了分析单个连杆的工具，但连杆之间是靠关节连接的，关节处有约束力和约束力矩在传递作用。

三、两步递推：正向算速度，反向算力

算法通过两次计算，遍历整条手臂，完成全部计算。

第一阶段：正向递推（从基座 → 末端）

目标：在已知 q,q˙,q¨的前提下，计算出每个连杆的角速度 ω、角加速度 α 和质心线加速度 ac。

原理：就像波浪一样，运动从基座开始，通过关节一层层向外传递。

• 基座初始化：因为机械臂底座固定在地面（或移动底盘上），通过给它加一个向上的加速度来巧妙模拟重力，即设基座加速度 a0=−g（g为重力矢量），这样重力效应就被自动融入了。
• 旋转关节逐级传递：从连杆 i−1到连杆 i 的传递规则如下：
  • 角速度传递：

  

  （父连杆的角速度，加上自身关节速度带来的额外角速度）

  • 角加速度传递：

  

  （父连杆角加速度 + 自身角加速度 + 一项耦合项）

  • 线加速度传递：

  

  （父原点加速度 + 因转动产生的切向、向心加速度）

  • 质心加速度：

  

• 如果是平移关节（如SCARA升降轴），传递规则类似但更简单。

第二阶段：反向递推（从末端 → 基座）

目标：利用已知的各连杆加速度，从末端开始，逐级向基座计算每个关节所需的力矩 τi。

原理：根据作用力与反作用力原理，要把末端的外力（六维力传感器测到的接触力）和运动所需的惯性力，一层层传导回基座。

• 初始化：末端连杆 n 受到的外部作用力 f_n+1和外部作用力矩 μ_n+1，就是经过重力/惯性补偿后，六维力传感器测得的纯净接触力
• 逐级计算：对连杆 i：

1. 计算该连杆运动所需的合力和合力矩（基于正向递推算出的 aci,αi,ωi）：

2. 隔离连杆，分析受力：连杆 i通过关节 i 与父连杆 i−1连接，并通过关节 i+1与子连杆 i+1连接。

• 它从子连杆 i+1 处受到作用力 f_i+1和力矩 μ_i+1。
• 它自身运动需要力和力矩 F_i,M_i。
• 因此，为了维持平衡，父连杆 i−1必须通过关节 i 给它施加一个力 fi 和力矩 μi。

3. 建立力/力矩平衡方程：

4. 提取关节力矩：关节 i是一个有特定运动轴（如 zi）的约束机构，所需的驱动力矩 τi，就是父连杆对它的作用力/力矩在关节轴方向上的投影。

• 对旋转关节：τi=μi⋅zi
• 对平移关节：τi=fi⋅zi

5. 传递下去：现在 fi,μi 成为给父连杆 i−1 的力和力矩，开始下一轮计算，直到基座。

四、实际应用介绍

以的实际作业机械臂为例：

• 正向递推：已知末端 J5 和 J6 的角度、角速度、角加速度，算法会算出末端的质心会产生多大的切向/向心加速度。
• 反向递推：
  1. 从工具末端开始，输入六维力传感器测得的纯净接触力，作为初始的 f_n+1,μ_n+1。
  2. 计算末端（连杆6）自身加速所需的力/力矩，与接触力叠加，得到 J6 关节对它的作用力/力矩。
  3. 投影到 J6 轴线，得到 J6 电机为保持接触和克服自身惯性所需的精确力矩。
  4. 这个作用力/力矩，就是末端对 J6 所在连杆的作用力，继续向前传递。
  5. 算法同样为 J5 所在的大臂连杆计算其运动所需力/力矩，并承接来自 J6 的力。
  6. 最终投影得到 J5 电机所需的力矩。

这个过程将末端一个微小的力误差，如何层层累加到每个关节，计算得一清二楚。

总结

牛顿-欧拉算法，就是通过正向递推算加速度，反向递推算力，这两个高效步骤，为机械臂的逆动力学问题提供了工程解。 它把复杂的整臂动力学，分解成每一步都清晰明确的递推计算，不仅计算快，而且能天然地集成重力效应和末端外力，是未来实现高精度力矩前馈、全臂碰撞检测等功能的基石。