刚体运动学基础：坐标系与变换、旋转矩阵、欧拉角、四元数
各位同学，今天我们来啃一块硬骨头——刚体运动学。说白了，就是研究一个不会变形的物体，在空间里怎么转、怎么动。你想想看，人形机器人走路、挥手、弯腰，本质上都是各个刚体（大腿、小腿、躯干）之间的相对运动。搞不懂这个，后面做模型辨识和运动控制就是空中楼阁。
我个人习惯，学任何东西先搭框架。刚体运动学就三件事：怎么描述位置、怎么描述姿态、怎么在坐标系之间来回倒腾。今天我们就把这三点彻底讲透。
1. 坐标系与变换——你得先有个“锚点”
做机器人控制，第一件事就是定义坐标系。没有坐标系，你连“往前走”都说不清楚。我一般把坐标系分成两类：世界坐标系和机体坐标系。
- 世界坐标系：固定在地面，不动的。比如房间的墙角、实验室的基准点。机器人要走到哪里，都是相对于世界坐标系说的。
- 机体坐标系：绑在机器人身上，跟着动的。比如脚底的坐标系、腰部的坐标系。传感器测到的数据，默认都是机体坐标系下的。
那问题来了：机体坐标系下的数据，怎么换算到世界坐标系？这就是坐标变换要干的事。一个完整的变换包含两部分：旋转（方向变了）和平移（位置变了）。
p_world 和 p_body，一眼就能看出这个向量是在哪个坐标系下。这习惯救过我很多次，尤其是在调试IMU数据融合的时候。
2. 旋转矩阵——最“笨”但最可靠的方法
旋转矩阵，说白了就是一个3x3的矩阵，用来描述一个坐标系相对于另一个坐标系的朝向。它的每一列，就是原坐标系基向量在新坐标系下的投影。
举个例子，绕Z轴转θ角，旋转矩阵长这样：
R_z(θ) = [cosθ -sinθ 0]
[sinθ cosθ 0]
[0 0 1]旋转矩阵有个重要性质：它是正交矩阵，也就是说 R^T = R^{-1}。这意味着求逆特别简单——转置一下就行。我在做实时控制时，经常利用这个性质来减少计算量。
3. 欧拉角——直观但暗藏“杀机”
欧拉角用三个角度来描述姿态：偏航（Yaw）、俯仰（Pitch）、滚转（Roll）。你想想看，飞机转弯、点头、侧翻，就是这三个动作。人形机器人也是一样，躯干有偏航角，大腿有俯仰角。
但欧拉角有个著名的坑——万向锁。当俯仰角达到±90°时，偏航和滚转的旋转轴会重合，丢失一个自由度。我记得有一次调试双足机器人的上身姿态，俯仰角一过85°，姿态解算就乱跳。查了半天，就是万向锁在作怪。
| 表示方法 | 优点 | 缺点 | 适用场景 |
|---|---|---|---|
| 旋转矩阵 | 无奇点、易组合 | 冗余、需正交化 | 理论推导、离线计算 |
| 欧拉角 | 直观、参数少 | 万向锁、插值困难 | 人机交互、遥控 |
| 四元数 | 无奇点、易插值 | 不够直观 | 实时控制、滤波 |
4. 四元数——工程师的“瑞士军刀”
四元数，听起来玄乎，其实就是一个四维向量 q = [w, x, y, z]，满足 w² + x² + y² + z² = 1。它用四个数表示三维旋转，没有奇点，没有冗余，插值平滑。
我个人习惯，在实时控制代码里全部用四元数。为什么？因为做姿态插值时，四元数的球面线性插值（SLERP）效果远好于欧拉角的线性插值。你想想看，让机器人从抬头45°转到低头30°，用欧拉角插值可能会绕远路，但四元数走的是最短路径。
四元数乘法也简单：
q1 * q2 = [w1*w2 - x1*x2 - y1*y2 - z1*z2,
w1*x2 + x1*w2 + y1*z2 - z1*y2,
w1*y2 - x1*z2 + y1*w2 + z1*x2,
w1*z2 + x1*y2 - y1*x2 + z1*w2]5. 复合变换——把“旋转”和“平移”串起来
单个刚体的姿态搞定了，那多个刚体呢？比如大腿相对于髋关节，小腿相对于大腿。这就需要用齐次变换矩阵把旋转和平移打包在一起：
T = [R p]
[0 1]其中R是3x3旋转矩阵，p是3x1平移向量。这样，一个4x4矩阵就能完整描述一个坐标系到另一个坐标系的变换。多个变换连起来就是矩阵乘法：T_13 = T_12 * T_23。
嗯，这里要注意：矩阵乘法不满足交换律。先旋转再平移，和先平移再旋转，结果完全不同。我在做正运动学时，经常要反复检查变换顺序，一个不小心就串错了。
好了，刚体运动学的基础就这些。坐标系是骨架，旋转矩阵、欧拉角、四元数是三种描述姿态的语言。你不需要全部精通，但至少要能熟练切换。后面做模型辨识时，你会发现这些概念无处不在——从IMU数据解算到关节角度映射，每一步都离不开它们。