3. 机器人运动学基础：齐次变换矩阵、正运动学与逆运动学、雅可比矩阵与力映射
各位同学，大家好。我是你们这门课的老朋友。今天咱们来啃一块硬骨头——机器人运动学基础。
说实话，我刚开始做机器人控制那会儿，觉得运动学就是一堆矩阵乘法，枯燥得很。直到有一次，我调试一个六轴协作臂的装配程序，发现末端执行器死活对不准螺丝孔。折腾了两天，最后发现是正运动学算错了。嗯，从那以后，我再也不敢小看这些基础了。
这一章，咱们不讲虚的。直接上干货：齐次变换矩阵、正逆运动学、还有雅可比矩阵。你想想看，这些就是机器人的“骨架”和“神经”。搞懂了它们，柔顺控制才有根。
3.1 齐次变换矩阵：机器人的“坐标系语言”
说白了，齐次变换矩阵就是用来描述两个坐标系之间关系的。一个旋转，一个平移，合在一起就是4x4的矩阵。
我个人习惯把它拆成三块看：
- 旋转部分（左上角3x3）：描述姿态，比如手腕怎么扭。
- 平移部分（右上角3x1）：描述位置，比如手臂伸多远。
- 最后一行（[0 0 0 1]）：固定格式，别动它。
举个例子，一个简单的平移变换：
// 沿X轴移动100mm的齐次变换矩阵
T = [1, 0, 0, 100;
0, 1, 0, 0;
0, 0, 1, 0;
0, 0, 0, 1]我在项目中遇到过一个问题：两个工程师用了不同的坐标系定义，结果机器人撞了夹具。后来我们统一用齐次变换矩阵做中间层，再也没出过事。记住，坐标系一致性是命根子。
3.2 正运动学：从关节角度到末端位姿
正运动学，就是已知每个关节的角度，求末端执行器在空间中的位置和姿态。说白了，就是“我动了一下关节，手会跑到哪里？”
标准做法是用DH参数法。我建议你记住这个流程：
- 建立连杆坐标系（每个关节一个坐标系）
- 写出相邻连杆的齐次变换矩阵
- 把所有矩阵乘起来，得到末端相对于基座的变换矩阵
为什么正运动学重要？因为它是所有控制算法的基础。你想想看，如果连手在哪都不知道，怎么谈柔顺？
我曾经带过一个实习生，他写正运动学代码时，把矩阵乘法顺序搞反了。结果机器人末端位置差了十万八千里。所以，矩阵乘法顺序：左乘是相对基座，右乘是相对当前关节。这个千万别搞混。
3.3 逆运动学：从末端位姿到关节角度
逆运动学正好反过来：已知末端要到达某个位置和姿态，求每个关节该转多少度。这是控制中最头疼的部分。
为什么头疼？因为：
- 解可能不存在（目标点太远）
- 解可能不唯一（多组关节角度都能到同一个点）
- 解可能不连续（微小移动导致关节角度突变）
我常用的解法有两种：
| 方法 | 适用场景 | 缺点 |
|---|---|---|
| 解析法（封闭解） | 六轴机器人，且满足Pieper准则 | 推导复杂，特定构型才适用 |
| 数值法（迭代法） | 任意构型，冗余机器人 | 计算量大，可能不收敛 |
我个人习惯：能用解析法就用解析法。速度快，稳定性好。数值法我一般只用在冗余自由度机器人上。
3.4 雅可比矩阵与力映射
雅可比矩阵，说白了就是关节速度到末端速度的映射关系。但它的用处远不止于此。
在柔顺控制中，我们更关心它的力映射功能：
τ = J^T * F其中，τ是关节力矩，F是末端力，J^T是雅可比矩阵的转置。
这个公式太重要了。它告诉我们：
- 末端感受到的力，如何折算到每个关节上
- 我们给关节施加力矩，末端会产生多大的力
我记得有一次做力控装配，机器人末端力总是超差。查了半天，发现是雅可比矩阵在奇异位形附近数值不稳定。后来加了阻尼最小二乘法（DLS），问题就解决了。
3.5 本章知识体系
为了让你更直观地理解这些概念之间的关系，我画了一张图：
从这张图你可以看到，齐次变换矩阵是地基，正/逆运动学是骨架，雅可比矩阵是神经。它们层层递进，缺一不可。
好了，这一章的内容就到这里。记住，运动学是柔顺控制的前提。如果你在项目中遇到装配对不准、力控不稳定，先回头检查一下运动学算对了没有。很多时候，问题就出在这些最基础的地方。