第2章 刚体运动学基础：坐标系与位姿描述
各位同学，欢迎来到第二章。上一章我们聊了逆运动学在力矩控制中的整体定位，今天开始打地基——刚体运动学。说白了，就是研究一个刚体在空间里怎么动、怎么转、怎么描述它的位置和姿态。
我刚开始做机器人控制时，觉得这玩意儿不就是高中数学里的向量嘛。直到第一次调试六轴机械臂，发现末端执行器死活对不准目标点，才意识到坐标系搞混了有多坑。嗯，咱们今天就把这个坑填上。
核心观点：逆运动学的本质，是在已知末端位姿的条件下，求解关节角度。而位姿的数学描述，就是本章要讲的内容。
2.1 坐标系与位姿——先搞清楚你在哪
想象一下，你让机器人去抓一个杯子。你得告诉它：杯子在哪儿？杯子口朝哪？这就是位姿——位置 + 姿态。
位置好理解，就是三维空间里的一个点，用 (x, y, z) 表示。但注意，这个坐标是相对于某个参考系的。你想想看，同一个点，在世界坐标系和工具坐标系下的数值完全不同。
姿态就稍微绕一点。它描述的是物体怎么「摆放」的——是正着、歪着、还是倒着？数学上，我们用旋转矩阵、欧拉角或四元数来描述。
我的经验：在实际项目中，我习惯把世界坐标系固定在地面，机器人基坐标系和世界坐标系对齐。这样调试时脑子不容易乱。如果你发现机器人动作诡异，先检查坐标系定义——八成是这里出了问题。
2.2 旋转矩阵——姿态的「硬核」表达
旋转矩阵 R 是一个 3×3 的正交矩阵，属于 SO(3) 群。它的每一列代表旋转后坐标系的基向量在原坐标系下的投影。
举个例子，绕 Z 轴旋转 θ 角：
R_z(θ) = [cosθ -sinθ 0]
[sinθ cosθ 0]
[0 0 1]绕 X 轴和 Y 轴的类似，我就不列了。记住一个性质：R 的逆等于它的转置，即 R⁻¹ = Rᵀ。这个性质在后续计算雅可比矩阵时会反复用到。
注意：旋转矩阵虽然有9个元素，但只有3个自由度。这意味着它存在冗余。直接用旋转矩阵做优化或插值，很容易产生非正交的矩阵——说白了就是「拧坏了」。所以实际控制中，我们通常用欧拉角或四元数做中间计算，最后再转回旋转矩阵。
2.3 欧拉角——直观但小心「万向锁」
欧拉角的核心思想：把一次旋转拆成绕三个轴的依次旋转。常见的约定有 ZYX（先绕Z、再绕Y、最后绕X）和 ZYZ 等。
在机器人领域，最常用的是 RPY 角（Roll-Pitch-Yaw），对应绕 X、Y、Z 轴的旋转：
- Roll（横滚）：绕 X 轴旋转，角度 φ
- Pitch（俯仰）：绕 Y 轴旋转，角度 θ
- Yaw（偏航）：绕 Z 轴旋转，角度 ψ
欧拉角的好处是直观，三个角度一看就懂。但有个致命问题——万向锁（Gimbal Lock）。
为什么会这样？当 Pitch 角接近 ±90° 时，Roll 和 Yaw 的旋转轴会重合，导致丢失一个自由度。我曾经在调试一个六轴协作机器人时，末端执行器在某个姿态附近突然「卡住」了，查了半天才发现是欧拉角表示导致的万向锁。从那以后，我只要涉及连续旋转，一律用四元数。
| 表示方法 | 优点 | 缺点 |
|---|---|---|
| 旋转矩阵 | 计算稳定，适合矩阵运算 | 9个参数冗余，不适合插值 |
| 欧拉角 | 直观，3个参数 | 存在万向锁，不唯一 |
| 四元数 | 无奇点，适合插值，4个参数 | 不够直观，需要转换 |
2.4 四元数——姿态插值的利器
四元数可以看作复数的推广：q = w + xi + yj + zk，其中 i² = j² = k² = ijk = -1。在机器人学中，我们通常用单位四元数表示旋转。
单位四元数满足 w² + x² + y² + z² = 1。它没有奇点，而且做球面线性插值（SLERP）非常平滑。如果你要做轨迹规划中的姿态平滑过渡，四元数是首选。
从旋转矩阵转四元数，或者反过来，都有现成的公式。我建议你直接写一个工具函数库，别每次都手算——容易算错。
// 四元数转旋转矩阵（C++ 示例）
Eigen::Matrix3d quatToRotMat(double w, double x, double y, double z) {
Eigen::Matrix3d R;
R(0,0) = 1 - 2*y*y - 2*z*z;
R(0,1) = 2*x*y - 2*w*z;
R(0,2) = 2*x*z + 2*w*y;
R(1,0) = 2*x*y + 2*w*z;
R(1,1) = 1 - 2*x*x - 2*z*z;
R(1,2) = 2*y*z - 2*w*x;
R(2,0) = 2*x*z - 2*w*y;
R(2,1) = 2*y*z + 2*w*x;
R(2,2) = 1 - 2*x*x - 2*y*y;
return R;
}小技巧：在嵌入式平台上，四元数乘法比旋转矩阵乘法快得多。如果你在做实时力矩控制，尽量用四元数做姿态运算，最后再转成旋转矩阵输出给底层驱动。
2.5 齐次变换矩阵——把位置和姿态打包
齐次变换矩阵 T 是一个 4×4 矩阵，把旋转和位移整合在一起：
T = [R p]
[0ᵀ 1]其中 R 是 3×3 旋转矩阵，p 是 3×1 位置向量。最后一行是 [0 0 0 1]。
为什么用齐次坐标？因为这样可以把旋转和平移统一成矩阵乘法。你想想看，如果分开算，先旋转再平移，公式写起来很啰嗦。用齐次变换矩阵，一次乘法搞定。
更重要的是，多个变换可以链式相乘：T₁₃ = T₁₂ · T₂₃。这就是机器人运动学中「链式法则」的数学基础。在逆运动学求解时，我们会反复用到这个性质。
关键点：齐次变换矩阵的逆也很好求——不用真的算 4×4 矩阵的逆，直接用公式：
T⁻¹ = [Rᵀ -Rᵀp]
[0ᵀ 1 ]
这个公式在代码里实现起来非常高效，我建议你把它写成 inline 函数。
2.6 本章小结——你该带走什么
好了，这一章的内容就这些。总结一下你必须记住的几点：
- 位姿 = 位置 + 姿态，两者缺一不可
- 旋转矩阵是姿态的数学基础，但别用它做插值
- 欧拉角直观，但小心万向锁——我吃过亏
- 四元数是姿态插值和实时控制的首选
- 齐次变换矩阵把位置和姿态打包，链式乘法是运动学核心