2. 运动学基础：刚体变换、齐次坐标、旋转矩阵、欧拉角与四元数
好，咱们正式开始啃运动学这块硬骨头。
说实话，我刚入行那会儿，觉得运动学就是一堆矩阵乘法，枯燥得很。直到有一次调试机械臂，末端执行器死活对不准目标点，折腾了两天，最后发现是旋转顺序搞反了。嗯，从那以后，我再也不敢小看这些基础概念了。
这一节，咱们把刚体变换、齐次坐标、旋转矩阵、欧拉角、四元数这几个东西彻底捋清楚。你想想看，机械臂的每一个关节都在动，末端执行器在空间里又平移又旋转，怎么描述它？怎么计算它？就是靠这些东西。
2.1 刚体变换：位置 + 姿态
一个刚体在三维空间里，有6个自由度——3个平移，3个旋转。说白了，就是「它在哪」和「它朝哪」。
我个人习惯把刚体变换拆成两步看：
第一步，把坐标系挪过去（平移）；
第二步，把坐标系转过去（旋转）。
用数学表达就是：
P' = R * P + t其中 P 是物体在自身坐标系下的坐标，R 是旋转矩阵，t 是平移向量，P' 是它在世界坐标系下的坐标。
2.2 齐次坐标：把平移和旋转统一起来
你可能会问：为什么非要用齐次坐标？直接用 P' = R*P + t 不行吗？
行，但麻烦。因为连续变换时，你要不停地加平移、乘旋转，写起来又长又容易错。齐次坐标的妙处在于——它把平移也塞进了矩阵里。
看这个：
| P' | | R t | | P |
| 1 | = | 0 1 | * | 1 |这样一来，多次变换就变成了矩阵连乘。我当年第一次看懂这个的时候，觉得这设计真漂亮——干净、统一、好算。
2.3 旋转矩阵：绕轴转的数学表达
旋转矩阵是3x3的正交矩阵，行列式为+1。绕X、Y、Z轴旋转的矩阵长这样：
绕X轴转θ：
| 1 0 0 |
| 0 cosθ -sinθ |
| 0 sinθ cosθ |
绕Y轴转θ：
| cosθ 0 sinθ |
| 0 1 0 |
| -sinθ 0 cosθ |
绕Z轴转θ：
| cosθ -sinθ 0 |
| sinθ cosθ 0 |
| 0 0 1 |我在项目中遇到过一个问题：两个旋转矩阵相乘，结果怎么都不对。后来发现，矩阵乘法不满足交换律——先绕X转再绕Y转，和先绕Y转再绕X转，结果完全不同。这个坑，踩过一次就记住了。
2.4 欧拉角：直观但容易出问题
欧拉角用三个角度表示旋转，比如滚转、俯仰、偏航。听起来很直观对吧？
但问题来了——旋转顺序怎么定？是先绕X再绕Y再绕Z？还是先绕Z再绕Y再绕X？不同的顺序，结果天差地别。
我曾经在仿真里用欧拉角控制机械臂，结果在某个角度附近，机械臂突然「抽搐」了一下。查了半天，发现是万向锁——当俯仰角接近90度时，滚转和偏航就分不清了，丢失了一个自由度。
2.5 四元数：旋转的终极方案
四元数，说白了就是一个实部加三个虚部：q = w + xi + yj + zk。它用4个参数表示旋转，没有冗余，没有万向锁，插值平滑。
绕单位向量 u = (ux, uy, uz) 转 θ 角的四元数是：
q = (cos(θ/2), ux*sin(θ/2), uy*sin(θ/2), uz*sin(θ/2))用四元数旋转一个向量 v：
v' = q * v * q_conjugate嗯，这里要注意，四元数乘法不交换，顺序很重要。
2.6 知识体系总览
下面这张图，把这一节的核心逻辑串起来了：
2.7 它们之间的转换
在实际工程中，这几个表示方法经常要互相转换。我整理了一张对照表，方便你查阅：
| 转换方向 | 方法 | 注意事项 |
|---|---|---|
| 旋转矩阵 → 欧拉角 | 根据矩阵元素反解角度 | 注意万向锁，当cosθ接近0时需特殊处理 |
| 欧拉角 → 旋转矩阵 | 按顺序乘三个基本旋转矩阵 | 必须明确旋转顺序（如ZYX、XYZ） |
| 旋转矩阵 → 四元数 | 从矩阵迹和元素计算 | 注意数值稳定性，选最大的分量做分母 |
| 四元数 → 旋转矩阵 | 用四元数元素构造矩阵 | 确保四元数是单位四元数 |
| 欧拉角 → 四元数 | 先转矩阵，再转四元数 | 或者直接用角度构造四元数乘法 |
2.8 实战建议
说了这么多，给你几条实在的建议：
- 做仿真时：用四元数做内部计算，用欧拉角做调试输出。
- 做标定时：用旋转矩阵，因为它便于做最小二乘优化。
- 做插值时：用四元数的球面线性插值（SLERP），平滑无抖动。
- 做可视化时：用欧拉角，人看着直观。
我个人习惯在代码里写一个转换工具类，把旋转矩阵、欧拉角、四元数之间的转换都封装好。这样不管外部接口用什么格式，内部统一用四元数，省心。
好了，运动学基础就聊到这儿。这些东西看着抽象，但你在仿真里跑几次机械臂，遇到几次对不准的问题，自然就理解了。下一节咱们聊正运动学——怎么从关节角度算出末端位姿。
可可精品资料库