3. 正运动学求解：齐次变换矩阵与末端位姿

正运动学，说白了就是：给定关节角度，算出末端在哪儿、朝哪儿。这是机器人学里最基础、也最绕不开的一步。我刚开始做机械臂项目时，总觉得这玩意儿就是矩阵乘法，没啥好琢磨的。直到有一次，我手算的末端位置跟实际差了 5 厘米……嗯，从那以后，我再也不敢轻视齐次变换矩阵的推导了。

3.1 齐次变换矩阵：从关节到末端的“翻译官”

每个关节，都可以用一个 4×4 的齐次变换矩阵来描述。它把上一关节的坐标系，变换到当前关节的坐标系。你想想看，从基座到末端，中间经过 n 个关节，那就是 n 个矩阵连乘。

每个 T 矩阵长这样：

| R(3×3)   P(3×1) |
| 0 0 0      1     |

R 是旋转矩阵，描述姿态；P 是位置向量，描述坐标。我个人习惯把 DH 参数表先列出来，再逐个生成矩阵，这样不容易乱。

3.2 DH 参数法：标准流程

DH 参数有四个：θ（关节角）、d（连杆偏距）、a（连杆长度）、α（连杆扭角）。对于旋转关节，θ 是变量；对于移动关节，d 是变量。

标准 DH 变换矩阵公式：

T_i = 
| cosθ_i   -sinθ_i·cosα_i    sinθ_i·sinα_i    a_i·cosθ_i |
| sinθ_i    cosθ_i·cosα_i   -cosθ_i·sinα_i    a_i·sinθ_i |
|    0          sinα_i           cosα_i           d_i      |
|    0            0                0               1       |

这个公式我建议你手推一遍。我在项目中遇到过，直接抄公式结果符号搞反了，调试了一整天。自己推一遍，印象深得多。

3.3 举个栗子：两连杆机械臂

假设一个平面两连杆机械臂：

• 连杆1：长度 L1 = 100 mm
• 连杆2：长度 L2 = 80 mm
• 关节角：θ1、θ2

DH 参数表：

代入公式，得到 T_0_1 和 T_1_2，然后相乘：

T_0_2 = T_0_1 × T_1_2

结果：
Px = L1·cosθ1 + L2·cos(θ1+θ2)
Py = L1·sinθ1 + L2·sin(θ1+θ2)
Pz = 0

姿态角（绕 Z 轴）：φ = θ1 + θ2

这就是末端的位置和姿态。简单吧？但实际六轴机械臂，推导起来就复杂多了。我建议你从两连杆开始，用 MATLAB 验证，再逐步增加关节数。

3.4 MATLAB 实现：快速验证

我个人习惯先用 MATLAB 把公式跑通，再移植到 STM32。这样能快速发现推导错误。

% 两连杆正运动学
L1 = 100; L2 = 80;
theta1 = 30 * pi/180;
theta2 = 45 * pi/180;

% 齐次变换矩阵
T01 = dh_transform(theta1, 0, L1, 0);
T12 = dh_transform(theta2, 0, L2, 0);
T02 = T01 * T12;

% 提取位置
px = T02(1,4);
py = T02(2,4);
pz = T02(3,4);

% 提取姿态（绕Z轴）
phi = atan2(T02(2,1), T02(1,1));

fprintf('末端位置: (%.2f, %.2f, %.2f)\n', px, py, pz);
fprintf('末端姿态: %.2f rad\n', phi);

function T = dh_transform(theta, d, a, alpha)
    T = [cos(theta), -sin(theta)*cos(alpha),  sin(theta)*sin(alpha), a*cos(theta);
         sin(theta),  cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta);
         0,           sin(alpha),             cos(alpha),            d;
         0,           0,                      0,                     1];
end

3.5 STM32 实现：定点数 vs 浮点数

STM32 上跑正运动学，有两个选择：

• 浮点数： 直接用 float，简单直观，但速度稍慢
• 定点数： 用 Q15 或 Q31 格式，速度快，但精度有限

我的建议是：先用浮点数把功能跑通，再根据性能需求决定是否优化。STM32F4 以上有 FPU，浮点运算并不慢。

// STM32 正运动学计算（浮点版本）
typedef struct {
    float x;
    float y;
    float z;
    float roll;
    float pitch;
    float yaw;
} Pose;

Pose forward_kinematics(float theta1, float theta2) {
    float L1 = 100.0f;
    float L2 = 80.0f;
    
    float c1 = cosf(theta1);
    float s1 = sinf(theta1);
    float c12 = cosf(theta1 + theta2);
    float s12 = sinf(theta1 + theta2);
    
    Pose end;
    end.x = L1 * c1 + L2 * c12;
    end.y = L1 * s1 + L2 * s12;
    end.z = 0.0f;
    end.yaw = theta1 + theta2;
    end.pitch = 0.0f;
    end.roll = 0.0f;
    
    return end;
}

3.6 知识结构图：正运动学核心流程

3.7 常见坑与避坑指南

• 坐标系方向搞反： 我曾经把 Z 轴方向定义反了，结果末端位置全反。建议画个坐标系草图，标清楚每个轴的方向。
• 角度单位混淆： MATLAB 用弧度，STM32 里也建议用弧度。我见过有人混用度跟弧度，结果位置差了十万八千里。
• 矩阵乘法顺序： 左乘还是右乘？标准 DH 是左乘。顺序错了，姿态就全错了。
• 浮点精度问题： 长时间运行后，累积误差会变大。建议定期用外部传感器做校准。