运动学基础（一）：刚体运动描述、旋转矩阵、欧拉角与四元数

各位同学，欢迎来到《人形机器人动态轨迹平衡术》的第二讲。

今天我们要聊的，是运动学里最基础、也最绕不开的一块硬骨头——刚体运动描述。说白了，就是怎么用数学语言告诉机器人：「你的脑袋现在朝哪边歪？你的手臂转了多少度？」

我个人习惯把这一讲称为「坐标系的语言课」。你想想看，人形机器人全身几十个关节，每个关节都有自己的小脾气（坐标系）。如果连怎么描述它们之间的相对关系都搞不清楚，后面谈轨迹规划、动态平衡，那都是空中楼阁。

核心认知：刚体在三维空间中的运动 = 平移 + 旋转。平移很简单，就是三个坐标值。真正让人头疼的，是旋转。

旋转矩阵，我刚开始学的时候觉得它很友好。3x3的矩阵，每一列代表一个坐标轴在参考系里的投影。你用它左乘一个向量，就能把向量转到新的坐标系里。

举个例子，绕Z轴转θ角，旋转矩阵长这样：

R_z(θ) = [cosθ  -sinθ  0]
         [sinθ   cosθ  0]
         [0      0     1]

嗯，看起来挺规整的。但我在项目中遇到过一个问题：旋转矩阵有9个元素，但自由度只有3个。这意味着什么？意味着你每次更新矩阵，都得小心翼翼地保证它满足正交性（R^T R = I）和行列式为+1。否则，你的机器人模型会「扭曲」，仿真直接崩掉。

我的小技巧：在实际代码里，我很少直接操作旋转矩阵的9个元素。要么用库函数（比如Eigen、numpy的scipy.spatial.transform），要么用其他参数化方式。手动维护9个数的正交性，太容易出bug了。

欧拉角就亲民多了。三个角度：偏航（Yaw）、俯仰（Pitch）、滚转（Roll）。你想想看，描述一架飞机的姿态，说「机头偏左30度，抬头15度，机身倾斜10度」，多直观。

但这里有个大坑——万向锁。

为什么会这样？因为欧拉角的旋转顺序是固定的（比如ZYX顺序）。当中间的那个角度（俯仰）达到±90度时，第一个和第三个旋转轴会重合，导致丢失一个自由度。这时候，你无论怎么调偏航和滚转，效果都一样。

我曾经在一个机械臂的逆解项目里踩过这个坑。当时用欧拉角做插值，结果在某个奇异点附近，机械臂突然「抽搐」了一下，差点撞到旁边的设备。从那以后，我只要涉及连续旋转的插值，一律不用欧拉角。

避坑指南：欧拉角适合做人机交互的输入（比如操作员说「偏航5度」），也适合做姿态的直观显示。但千万别用它做内部连续旋转的计算，尤其是插值和微分。

四元数，说实话我第一次看到它时觉得挺玄乎的。一个标量加上三个虚部，q = w + xi + yj + zk，这玩意儿能描述旋转？

但用久了你会发现，它简直是工程界的福音。

我记得有一次做双足机器人的步态规划，需要在两个关键帧之间插值出20个中间姿态。用欧拉角插出来，脚掌的轨迹歪歪扭扭的；换成四元数SLERP，轨迹平滑得像丝绸一样。嗯，从那以后我就成了四元数的忠实用户。

四元数旋转的公式也很简洁：

v' = q * v * q_conj

其中v是三维向量（写成纯四元数形式），q是单位四元数，q_conj是它的共轭。就这么简单。

重要提醒：四元数一定要归一化！数值误差会让它慢慢偏离单位长度，导致旋转变形。我习惯在每个控制周期末尾都做一次归一化，养成习惯。

下面这张图，是我自己梳理的本章知识脉络。你可以把它当作一张地图，随时回来对照。

说了这么多，到底该用哪个？我个人的经验是：

我的习惯：在代码内部，所有旋转计算都用四元数。只在输入输出接口处，转换成欧拉角或旋转矩阵。这样既保证了计算的鲁棒性，又兼顾了可读性。

好了，这一讲的内容就到这里。刚体运动的描述是后续所有运动学、动力学分析的基础。你想想看，如果连「机器人脚底相对于地面的姿态」都描述不清楚，那平衡控制根本无从谈起。嗯，把这些基础打牢，后面我们会越来越有意思。