4、正运动学解算（Forward Kinematics）：DH参数法详解、2自由度机械臂正解推导、代码实现与验证

正运动学解算，说白了就是：给你每个关节的角度，算出末端执行器在空间里的位置和姿态。

我刚开始做机械臂控制的时候，总觉得这步是纯数学推导，跟硬件关系不大。直到有一次在产线上调试，发现电机转到了目标角度，但末端就是抓不到工件——查了半天，原来是DH参数表里一个连杆长度写错了。嗯，从那以后我再也不敢轻视正解这一步了。

这一章，我们就从DH参数法讲起，手推一个2自由度机械臂的正解，最后用C代码在STM32上跑通验证。

4.1 DH参数法：建立连杆坐标系的“通用语言”

Denavit-Hartenberg参数法，是机器人学里最经典的建模方法。它用四个参数来描述相邻两个连杆之间的变换关系：

有了这四个参数，相邻连杆的变换矩阵就可以写成标准形式：

T = Rot_x(α) · Trans_x(a) · Rot_z(θ) · Trans_z(d)

展开成4×4矩阵就是：

| cosθ    -sinθ    0     a |
| sinθ·cosα  cosθ·cosα  -sinα  -d·sinα |
| sinθ·sinα  cosθ·sinα   cosα   d·cosα |
| 0        0        0     1 |

这个矩阵，就是我们从连杆i-1变换到连杆i的“通行证”。

4.2 2自由度机械臂正解推导

我们用一个最简单的平面2R机械臂来演示。它有两个旋转关节，都在同一个平面内运动。

先建立DH参数表。假设连杆1长度L1，连杆2长度L2：

代入变换矩阵公式，得到T₁和T₂。把两个矩阵乘起来，就得到末端相对于基座的位姿：

T_0_2 = T_0_1 · T_1_2

乘出来的结果，末端位置坐标是：

x = L1·cos(θ1) + L2·cos(θ1+θ2)
y = L1·sin(θ1) + L2·sin(θ1+θ2)

这就是2R机械臂的正运动学方程。你想想看，是不是很直观？就是两个连杆在平面上的矢量叠加。

下面这张图展示了整个正运动学解算的流程：

4.3 代码实现：在STM32上跑通正解

理论推导完了，接下来上代码。我个人习惯在STM32上用定点数或单精度浮点实现，因为双精度在Cortex-M4上太慢了。

先定义数据结构：

typedef struct {
    float a;    // 连杆长度
    float alpha; // 连杆扭角
    float d;    // 连杆偏距
    float theta; // 关节角
} DHParam_t;

typedef struct {
    float x;
    float y;
    float phi;  // 末端姿态角
} EndEffectorPose_t;

然后实现单个变换矩阵的计算：

void dh_transform(DHParam_t *dh, float T[4][4]) {
    float ct = cosf(dh->theta);
    float st = sinf(dh->theta);
    float ca = cosf(dh->alpha);
    float sa = sinf(dh->alpha);
    
    T[0][0] = ct;   T[0][1] = -st;  T[0][2] = 0;    T[0][3] = dh->a;
    T[1][0] = st*ca; T[1][1] = ct*ca; T[1][2] = -sa; T[1][3] = -dh->d*sa;
    T[2][0] = st*sa; T[2][1] = ct*sa; T[2][2] = ca;  T[2][3] = dh->d*ca;
    T[3][0] = 0;    T[3][1] = 0;    T[3][2] = 0;    T[3][3] = 1;
}

最后是2R机械臂的正解函数：

EndEffectorPose_t forward_kinematics_2R(float theta1, float theta2, float L1, float L2) {
    EndEffectorPose_t pose;
    
    // 直接使用推导出的公式，避免矩阵乘法开销
    float c1 = cosf(theta1);
    float s1 = sinf(theta1);
    float c12 = cosf(theta1 + theta2);
    float s12 = sinf(theta1 + theta2);
    
    pose.x = L1 * c1 + L2 * c12;
    pose.y = L1 * s1 + L2 * s12;
    pose.phi = theta1 + theta2;  // 末端姿态角
    
    return pose;
}

4.4 验证：用实际数据说话

代码写完了，怎么验证对不对？我的做法是：

1. 边界测试：所有关节角为0时，末端应该在( L1+L2, 0 )位置。
2. 特殊角度：θ1=90°, θ2=0°时，末端应该在( 0, L1+L2 )。
3. 随机角度：用上位机生成一组随机角度，同时用MATLAB的机器人工具箱算一遍，对比结果。

下面是我在调试时用的一组验证数据：

正运动学解算是整个运动控制的基础。你想想看，如果连正解都算不准，后面的逆解、轨迹规划、闭环控制就全乱套了。所以这一章的内容，值得你花时间亲手推一遍公式、写一遍代码、跑一遍验证。