1. 机器人运动学基础：刚体变换与齐次坐标、DH参数建模法、正运动学与逆运动学求解

各位同学，欢迎来到《STM32机器人运动学标定与参数优化性能调优实战》的第一章。

说实话，做机器人这么多年，我见过太多人一上来就调PID、写控制算法，结果机械臂动起来歪歪扭扭，定位误差大得离谱。为什么？因为运动学基础没打牢。你想想看，连机器人每个关节转到哪、末端执行器在什么位置都算不准，后面的优化全是空中楼阁。

这一章，我们就从最根本的东西讲起。我会结合自己在STM32上做六轴机械臂控制的实际经验，把刚体变换、齐次坐标、DH参数、正逆运动学这些概念，掰开了揉碎了讲清楚。

1.1 刚体变换与齐次坐标——机器人的“空间语言”

机器人运动学，说白了就是研究“谁在哪儿、怎么到那儿去”的问题。刚体变换就是描述这个“谁”和“哪儿”的数学工具。

一个刚体在三维空间里有6个自由度：3个平移（x, y, z）和3个旋转（绕x、y、z轴的角度）。我们用一个4×4的齐次变换矩阵，就能把这6个自由度全部装进去。

我个人习惯把齐次变换矩阵写成这样：

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

左上角的3×3是旋转矩阵，右上角的3×1是平移向量。最后一行固定是[0 0 0 1]，这是齐次坐标的“身份证”。

为什么要用4×4矩阵而不是3×4？嗯，这里有个关键点：只有4×4矩阵才能通过矩阵乘法把旋转和平移“打包”在一起运算。我在做STM32的矩阵运算库时，专门为4×4矩阵做了硬件加速优化，效果非常明显。

1.2 DH参数建模法——串联机器人的“身份证”

DH参数法，是Denavit和Hartenberg两位老先生在1955年提出的。到现在快70年了，依然是机器人运动学建模的黄金标准。

每个关节用4个参数描述：

参数	符号	物理含义	我的理解
连杆长度	ai-1	沿xi-1轴，从zi-1到zi的距离	说白了就是两个关节轴之间的“硬距离”
连杆扭角	αi-1	绕xi-1轴，从zi-1到zi的角度	两个关节轴之间的“扭转程度”
关节距离	di	沿zi轴，从xi-1到xi的距离	相邻连杆沿关节轴的“偏移量”
关节角度	θi	绕zi轴，从xi-1到xi的角度	这就是我们实际控制的电机角度

我曾经在一个六轴协作机器人项目里，因为DH参数表里一个扭角符号写反了，导致正运动学算出来的末端位置差了5厘米。查了整整两天才找到问题。从那以后，我每次建DH模型都会画一张坐标系图，一个一个参数对着检查。

下面这张图是我当年做项目时画的DH坐标系建立流程，现在拿出来给大家参考：

1.3 正运动学求解——从关节空间到笛卡尔空间

正运动学，就是已知每个关节的角度，求末端执行器在空间中的位置和姿态。说白了，就是“我让电机转到这个角度，手应该在哪里”。

对于n自由度的串联机器人，正运动学就是n个齐次变换矩阵的连乘：

T_0n = T_01 × T_12 × T_23 × ... × T_(n-1)n

其中每个T_(i-1)i都是根据DH参数构建的4×4矩阵。

我在STM32上实现正运动学时，最常用的方法是写一个通用的DH变换函数：

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

正运动学计算量是固定的，n个关节就需要n次4×4矩阵乘法。对于六轴机械臂，在STM32F4上跑一次正运动学大约需要15-20微秒，完全满足1kHz控制频率的要求。

1.4 逆运动学求解——从笛卡尔空间回到关节空间

逆运动学就比正运动学麻烦多了。已知末端位姿，求每个关节的角度。说白了就是“手要到这里，电机该怎么转”。

逆运动学有几个特点，我当年踩过不少坑：

• 多解性： 同一个末端位姿，可能有多种关节角度组合。比如六轴机械臂通常有8组逆解。
• 奇异性： 在某些位姿下，关节速度会趋于无穷大。比如肘关节伸直时，肩关节的微小转动就会导致末端大幅移动。
• 无解性： 目标位姿超出了机器人的工作空间。

对于六轴机械臂，逆运动学通常有两种解法：

方法	适用场景	计算量	我的评价
解析法（封闭解）	满足Pieper准则的机器人	小，微秒级	首选，稳定可靠
数值法（迭代法）	任意构型机器人	大，毫秒级	备选，注意收敛性

我个人强烈建议：能用解析法就别用数值法。解析法速度快、精度高、没有收敛问题。大多数工业机器人和协作机器人都满足Pieper准则（三个相邻关节轴交于一点），可以用解析法求解。

解析法求解逆运动学的一般步骤：

1. 根据末端位姿矩阵，分离出位置向量和姿态矩阵
2. 从腕部位置反推前三个关节的角度（肩、肘）
3. 从姿态矩阵反推后三个关节的角度（腕）
4. 根据关节限位和避障需求，选择最优解

在STM32上实现时，我建议把逆解函数设计成可配置的，允许用户指定“偏好解”的选择策略。比如“肘部朝上还是朝下”、“腕部翻转还是不翻转”。

1.5 本章小结

运动学是机器人控制的基石。这一章我们覆盖了：

• 刚体变换与齐次坐标——描述空间位姿的数学语言
• DH参数建模——串联机器人的标准化建模方法
• 正运动学——从关节到末端的正向计算
• 逆运动学——从末端到关节的反向求解

这些内容看起来是纯理论，但每一行代码、每一个公式，最终都会跑在你的STM32上，控制真实的电机转动。我建议你学完这一章后，立刻动手写一个正运动学函数，用你手头的机器人模型验证一下。纸上得来终觉浅，绝知此事要躬行。