第二章 运动学基础：刚体在空间中的位姿描述、旋转矩阵与齐次变换矩阵、欧拉角与四元数
各位同学，欢迎来到运动学基础这一章。说实话，很多初学者一上来就被各种矩阵和角度绕晕了。我当年刚入行时也是这样，总觉得这些东西太抽象。直到有一次在调试六轴机器人时，因为位姿算错导致末端撞上了夹具……嗯，从那以后我再也不敢轻视这些基础了。
这一章，咱们就聊聊刚体在空间里到底怎么「待着」的。说白了，就是搞清楚两个问题：它在哪？它朝哪？
2.1 刚体在空间中的位姿描述
先说说「位姿」这个词。位，就是位置（Position）；姿，就是姿态（Orientation）。一个刚体在三维空间里，有6个自由度——3个平移自由度（x, y, z）和3个旋转自由度（绕x、y、z轴转）。
我在项目中遇到过这样一个场景：要抓取一个放在传送带上的工件。你得先知道工件中心在哪（位置），还得知道它朝哪个方向（姿态），否则夹爪一伸过去就偏了。这就是位姿描述的实际意义。
核心概念：
- 位置：用三维向量 p = [x, y, z]^T 表示
- 姿态：用旋转矩阵 R 或欧拉角或四元数表示
- 位姿：位置 + 姿态，共6个独立参数
2.2 旋转矩阵
旋转矩阵，说白了就是一个3×3的方阵，用来描述一个坐标系相对于另一个坐标系的旋转关系。它有三个重要性质：
- 每列都是单位向量，且两两正交
- 行列式为 +1（右手系）
- 逆矩阵等于转置矩阵：R^{-1} = R^T
你想想看，为什么逆等于转置？因为旋转不改变向量的长度，只改变方向。这个性质在后续计算中非常有用，我经常用它来快速求逆。
举个绕z轴旋转θ角的例子：
R_z(θ) = [cosθ -sinθ 0]
[sinθ cosθ 0]
[0 0 1]绕x轴和y轴的类似，我就不一一写了。记住一个规律：绕哪个轴转，哪个轴对应的行和列就不变。
我的小技巧： 写旋转矩阵时，先确定旋转轴，再确定旋转方向。右手法则——大拇指指向旋转轴正方向，四指弯曲方向就是正旋转方向。
2.3 齐次变换矩阵
旋转矩阵只能描述姿态，不能描述位置。那如果我想同时描述位置和姿态呢？齐次变换矩阵就派上用场了。
齐次变换矩阵是一个4×4的矩阵：
T = [R p]
[0 1]其中R是3×3旋转矩阵，p是3×1位置向量。最后一行是[0 0 0 1]。
为什么要加这一行？说白了，就是为了把旋转和平移统一成一个矩阵乘法。这样，连续变换就可以通过矩阵乘法串联起来，非常方便。
我记得有一次做机器人正运动学，需要把6个关节的变换矩阵连乘。如果不用齐次变换，你得先算旋转再算平移，代码写起来又长又容易出错。用了齐次变换，一行代码搞定。
注意： 齐次变换矩阵的乘法顺序很重要！T1 * T2 表示先做T1变换，再做T2变换。顺序搞反了，结果完全不一样。我见过有人因为这个bug调试了一整天。
2.4 欧拉角
旋转矩阵虽然直观，但9个参数太多了。能不能用更少的参数描述姿态？欧拉角就是答案——只用3个角度。
常见的欧拉角约定有：
| 约定名称 | 旋转顺序 | 常见应用 |
|---|---|---|
| ZYX（RPY） | 绕Z→绕Y→绕X | 机器人末端姿态 |
| ZYZ | 绕Z→绕Y→绕Z | 工业机器人 |
| XYZ | 绕X→绕Y→绕Z | 航空航天 |
欧拉角有个著名的缺陷——万向锁（Gimbal Lock）。当第二个旋转角为±90°时，第一个和第三个旋转轴会重合，导致丢失一个自由度。我曾经在调一个六轴机器人的姿态规划时，就遇到了万向锁问题，末端突然乱转。后来改用四元数才解决。
避坑指南： 如果你的应用需要全姿态运动（比如机器人做球面运动），尽量避免用欧拉角。用四元数或旋转矩阵更安全。
2.5 四元数
四元数，听起来很玄乎，其实可以理解为复数的扩展。一个四元数由四个数组成：
q = w + xi + yj + zk其中w是实部，x、y、z是虚部。约束条件：w² + x² + y² + z² = 1（单位四元数）。
四元数最大的优点：没有万向锁。而且插值平滑，计算效率高。在机器人运动规划、计算机图形学里用得非常多。
我个人的习惯是：
- 人机交互时用欧拉角（直观）
- 内部计算时用四元数（稳定）
- 需要矩阵运算时用旋转矩阵（方便）
三者之间可以互相转换。比如四元数转旋转矩阵的公式：
R = [1-2y²-2z² 2xy-2wz 2xz+2wy]
[2xy+2wz 1-2x²-2z² 2yz-2wx]
[2xz-2wy 2yz+2wx 1-2x²-2y²]看着复杂，但写代码时直接套公式就行。我一般会封装成函数，用的时候直接调用。
经验之谈： 做四元数插值时，记得用球面线性插值（SLERP），而不是普通线性插值。普通线性插值会导致旋转速度不均匀，SLERP能保证角速度恒定。
2.6 三种表示方法的对比
| 方法 | 参数数量 | 优点 | 缺点 |
|---|---|---|---|
| 旋转矩阵 | 9 | 直观，易组合 | 冗余参数，需正交化 |
| 欧拉角 | 3 | 参数少，直观 | 万向锁，插值困难 |
| 四元数 | 4 | 无万向锁，易插值 | 不够直观 |
嗯，这一章的内容就到这里。记住，位姿描述是运动学的基础，后面的正运动学、逆运动学、轨迹规划全都依赖它。把基础打牢，后面才能走得远。
公众号:蓝海资料掘金营，微信deep3321