写在前面

我的其他两篇博客，分别介绍了外推法和内推法，本文将二者汇总起来，形成可复用的算法

参考自craig的《机器人学导论》

算法汇总 (Craig 改进DH版)

为了方便编程实现，以下是完整的算法流程（假设均为旋转关节，重力由 ${\dot{v}}_0$ 引入）：

初始化
$$\begin{gathered} {}^0{\omega }_0=0,{}^0{\dot{\omega }}_0=0, \\ {}^0{\dot{v}}_0=\mathbf{g}\text{ (例如 }[0,0,9.81{]}^T\text{ 或 }[0,0,-9.81{]}^T\text{ 取决于定义)} \end{gathered}$$
$${}^{n+1}{f}_{n+1}=0,{}^{n+1}{n}_{n+1}=0\text{ (若末端无外力)}$$

步骤 1: 外推 ($i=1\dots n$)

因为公式和外推的博客中说的不完全一致，故再解释一下拆解后的公式

这一步我们只关心运动，不关心力。对于每一个 $i$，我们需要用到上一步算出的 $i-1$ 的数据：

1. 坐标变换： 计算 ${}_{i-1}^{i}R$（将上一帧的数据转到当前帧）。
2. 角速度/角加速度： 利用 ${}^{i-1}{\omega }_{i-1}$ 算出 ${}^i{\omega }_i$ 和 ${}^i{\dot{\omega }}_i$。
3. 线加速度： 利用 ${}^{i-1}{\dot{v}}_{i-1}$ 算出 ${}^i{\dot{v}}_i$（原点加速度）。
4. 质心加速度： 利用 ${}^i{\dot{v}}_i$ 算出 ${}^i{\dot{v}}_{c_i}$。
5. 惯性力/力矩（中间产物）： 既然已经有了 ${}^i{\dot{v}}_{c_i}$ 和 ${}^i{\dot{\omega }}_i$，立即算出该连杆所需的惯性力 ${}^i{F}_i$ 和惯性力矩 ${}^i{N}_i$
   • ${}^i{F}_i=m_i{}^i{\dot{v}}_{c_i}$
   • ${}^i{N}_i={}^{c_i}{I}_i{}^i{\dot{\omega }}_i+{}^i{\omega }_i\times ({}^{c_i}{I}_i{}^i{\omega }_i)$

需要明确的是，前四步都是第五步的前置步骤，核心是第五步，内推法需要的也只是第五步的结果
第五步算出的 ${}^i{F}_i$ 和 ${}^i{N}_i$ 被称为达朗贝尔惯性力/力矩（D’Alembert Inertial Forces/Torques），它们是系统运动产生的“负载”

具体公式如下：

1. 计算坐标系变换 ${}_{i-1}^{i}R$ 和 ${}^{i-1}{P}_i$，它们取决于机械结构和当前姿态
2. ${}^i{\omega }_i={}_{i-1}^{i}R{}^{i-1}{\omega }_{i-1}+{\dot{\theta }}_i{\hat{Z}}_i$
   ${}^i{\dot{\omega }}_i={}_{i-1}^{i}R{}^{i-1}{\dot{\omega }}_{i-1}+{}_{i-1}^{i}R{}^{i-1}{\omega }_{i-1}\times {\dot{\theta }}_i{\hat{Z}}_i+{\ddot{\theta }}_i{\hat{Z}}_i$
3. ${}^i{\dot{v}}_i={}_{i-1}^{i}R({}^{i-1}{\dot{v}}_{i-1}+{}^{i-1}{\dot{\omega }}_{i-1}\times {}^{i-1}{P}_i+{}^{i-1}{\omega }_{i-1}\times ({}^{i-1}{\omega }_{i-1}\times {}^{i-1}{P}_i))$
4. ${}^i{\dot{v}}_{c_i}={}^i{\dot{v}}_i+{}^i{\dot{\omega }}_i\times {}^i{P}_{c_i}+{}^i{\omega }_i\times ({}^i{\omega }_i\times {}^i{P}_{c_i})$
5. ${}^i{F}_i=m_i{}^i{\dot{v}}_{c_i}$
   ${}^i{N}_i={}^{c_i}{I}_i{}^i{\dot{\omega }}_i+{}^i{\omega }_i\times ({}^{c_i}{I}_i{}^i{\omega }_i)$

步骤 2: 内推 ($i=n\dots 1$)

这里没啥好说的，公式和内推博客中的完全一致

1. ${}^i{f}_i={}_{i+1}^{i}R{}^{i+1}{f}_{i+1}+{}^i{F}_i$
2. ${}^i{n}_i={}^i{N}_i+{}_{i+1}^{i}R{}^{i+1}{n}_{i+1}+{}^i{P}_{c_i}\times {}^i{F}_i+{}^i{P}_{i+1}\times ({}_{i+1}^{i}R{}^{i+1}{f}_{i+1})$
3. ${\tau }_i={}^i{n}_i^T{\hat{Z}}_i$

关键点

1. 重力处理： 通过设定基座向上加速 ${\dot{v}}_0=-g$ (或者根据坐标系方向设定)，可以自动在所有连杆上产生重力项，无需在 $F_i$ 中显式加 $m_{i}g$。
2. 坐标系一致性： 所有运算（加法、叉乘）必须在同一个坐标系下进行。上述公式中，所有量左上标均为 $i$，表示都在当前连杆坐标系 { i } \{i\} {i} 下运算。
3. Craig MDH 特性： 最核心的区别在于关节 $i$ 的速度 ${\dot{\theta }}_i$ 直接加在坐标系 $i$ 的 $Z$ 轴上，而在标准DH中，关节 $i$ 通常关联坐标系 $i-1$ 的 $Z$ 轴。