第2章：刚体运动学基础：坐标系与变换

各位同学，欢迎来到第二章。说实话，刚体运动学是整个四足机器人运动学建模的基石。你想想看，机器人要走路，本质上就是它的各个刚体（大腿、小腿、躯干）在空间中不断变换位置和姿态。如果连怎么描述一个刚体在空间中的位姿都搞不清楚，后面的正逆运动学、步态规划根本无从谈起。

我个人习惯把这一章叫做“数学翻译器”——把物理世界的运动，翻译成计算机能理解的数字。咱们今天就把这个翻译器彻底搞明白。

2.1 坐标系与变换：先定个“老家”

做机器人仿真，第一件事就是定坐标系。没有坐标系，你连“往前走”都说不清楚。我见过不少新手，上来就写代码，结果机器人跑起来完全乱套——一查，原来是坐标系搞反了。

咱们在四足机器人里，主要用两种坐标系：

• 世界坐标系（World Frame）：固定在大地上的“参考系”。机器人往哪走、走了多远，都是相对于世界坐标系说的。
• 机体坐标系（Body Frame）：固定在机器人躯干上的“相对参考系”。通常原点在躯干质心，x轴朝前，y轴朝左，z轴朝上。

为什么要区分？举个例子。你让机器人“抬左前腿”，这个指令是在机体坐标系下说的。但如果要计算足端是否碰到地面，就得转换到世界坐标系下去判断。

2.2 旋转矩阵：最直观，但别滥用

旋转矩阵，说白了就是一个3x3的矩阵，用来描述一个坐标系相对于另一个坐标系的朝向。它的每一列，就是被旋转坐标系的基向量在原坐标系下的投影。

数学上长这样：

R = [r11 r12 r13]
    [r21 r22 r23]
    [r31 r32 r33]

旋转矩阵有个重要性质：它是正交矩阵，逆等于转置。这意味着 R⁻¹ = Rᵀ，求逆运算瞬间变转置，计算量小很多。

但说实话，旋转矩阵在实际工程中并不常用——因为它有9个参数，却只描述3个自由度，冗余太大。而且你想想看，9个数里稍微带点误差，矩阵就不再正交了，数值稳定性很差。

2.3 欧拉角：直观，但有“万向锁”

欧拉角用三个角度来描述旋转：横滚（Roll）、俯仰（Pitch）、偏航（Yaw）。对应到四足机器人：

欧拉角的好处是直观——你一看“横滚10度”，脑子里就能想象出机器人往右歪了10度。但它的致命问题是万向锁（Gimbal Lock）。

为什么会这样？当俯仰角接近±90度时，横滚和偏航的旋转轴会重合，丢失一个自由度。这时候你无论怎么调横滚和偏航，机器人都只能在一个平面内转。

2.4 四元数：工程师的“真香”选择

四元数，听起来很玄乎，其实就是一个四维向量 q = [w, x, y, z]，其中w是实部，x、y、z是虚部。它用四个参数描述三维旋转，没有冗余，没有万向锁。

我刚开始学四元数时也觉得头大，但用顺手之后，真香。为什么？

• 无奇点：随便你怎么转，不会卡死
• 插值平滑：做姿态插值时，用四元数球面线性插值（Slerp）非常平滑
• 计算高效：组合旋转只需要一次乘法，比矩阵快

四元数乘法表示旋转组合：

q_result = q2 * q1  // 先应用q1旋转，再应用q2旋转

注意顺序！四元数乘法不满足交换律，这和矩阵乘法一样。

2.5 齐次变换矩阵：位置+姿态，一网打尽

前面我们分别说了位置（3个坐标）和姿态（旋转矩阵/四元数）。但在实际工程中，我们经常需要同时描述一个刚体的位置和朝向——也就是位姿。

齐次变换矩阵就是干这个的。它是一个4x4矩阵：

T = [R    p]
    [0ᵀ   1]

其中：
R: 3x3 旋转矩阵
p: 3x1 位置向量
0ᵀ: 1x3 零向量

举个例子，从机体坐标系到世界坐标系的变换：

T_wb = [R_wb    p_wb]
       [0       1   ]

其中 p_wb 是机体在世界坐标系中的位置，R_wb 是机体的朝向。有了这个矩阵，机体坐标系下的任意点 p_b 都可以转换到世界坐标系：

p_w = T_wb * p_b

齐次变换矩阵的好处是：你可以把多个变换“串”起来。比如从世界到机体，再到大腿，再到小腿，最后到足端——一连串的矩阵乘法，一次搞定。

2.6 四种表示方法对比