第二章：坐标系与刚体运动基础
各位同学，欢迎来到《传感器融合与轨迹优化实战》的第二讲。
今天我们要聊的，是整个课程的基石——坐标系与刚体运动。说白了，就是搞清楚“我在哪”以及“我怎么动”这两个问题。你想想看，无论是无人机、机器人还是自动驾驶汽车，所有运动规划的前提，都是对自身姿态和位置的精确描述。如果这一步搞错了，后面再牛的算法也是白搭。
2.1 世界坐标系与机体坐标系
我们先从最直观的概念入手。
世界坐标系，也叫全局坐标系。它是一个固定不动的参考系。通常我们定义东北天（ENU）或者北东地（NED）作为世界坐标系。在室内场景，我习惯把机器人启动的位置设为原点，这样调试起来方便。
机体坐标系，是固定在运动物体上的坐标系。比如四旋翼无人机，原点在质心，X轴指向机头，Y轴指向右侧，Z轴垂直向下（或向上，取决于定义）。
为什么要区分这两个坐标系？
因为传感器测量的数据，都是在各自的机体坐标系下得到的。比如IMU测的加速度和角速度，就是机体坐标系下的值。而我们要做轨迹规划，最终是要在世界坐标系下描述路径。所以，坐标变换就成了绕不开的核心问题。
核心要点：所有传感器数据最终都要统一到世界坐标系下处理。这是传感器融合的第一步，也是最容易出错的一步。
2.2 旋转矩阵：最直观的旋转表示
旋转矩阵，是描述刚体旋转最直接的方式。它是一个3x3的正交矩阵，行列式为+1。
假设有一个向量 v 在机体坐标系下的坐标为 v_b，在世界坐标系下的坐标为 v_w，那么：
v_w = R * v_b这里的 R 就是旋转矩阵。它把机体坐标系下的向量，映射到了世界坐标系下。
我在项目中遇到过一个问题：用旋转矩阵做连续旋转时，由于浮点数精度问题，矩阵会慢慢失去正交性。嗯，这里要注意，长时间迭代后必须对旋转矩阵做正交化处理，否则误差会累积。
个人小技巧：每次更新旋转矩阵后，用 R = (R * R^T)^(-1/2) * R 做一次修正。虽然计算量稍大，但能保证长期稳定性。
2.3 欧拉角：直观但小心万向锁
欧拉角用三个角度来描述旋转：横滚角（roll）、俯仰角（pitch）、偏航角（yaw）。
它的优点是直观。你一看 roll=10°，pitch=5°，yaw=30°，脑子里就能大概想象出姿态。
但欧拉角有个致命缺陷——万向锁。当 pitch 接近 ±90° 时，roll 和 yaw 的旋转轴会重合，丢失一个自由度。我曾经在调试一个机械臂时，就因为欧拉角插值导致末端执行器突然“抽搐”，后来查了半天才发现是万向锁在作怪。
| 表示方法 | 优点 | 缺点 |
|---|---|---|
| 旋转矩阵 | 无奇异性，可组合 | 9个参数，冗余，需正交化 |
| 欧拉角 | 直观，3个参数 | 万向锁，插值困难 |
| 四元数 | 无奇异性，可插值 | 不够直观，需归一化 |
2.4 四元数：工程中的首选
四元数，说白了就是一个超复数。形式是 q = w + xi + yj + zk，其中 w 是实部，x、y、z 是虚部。
为什么工程中偏爱四元数？
- 无奇异性：不会出现万向锁
- 便于插值：球面线性插值（SLERP）可以生成平滑的旋转过渡
- 计算高效：比旋转矩阵少一个维度
我个人习惯用四元数做姿态估计。在卡尔曼滤波中，状态量通常就是四元数加角速度偏置。
避坑指南：我曾经因为忘记归一化四元数，导致滤波器发散。记住，四元数必须保持单位模长，即 w² + x² + y² + z² = 1。每次更新后都要做归一化。
2.5 相互转换：你必须要会的公式
在实际工程中，这三种表示方法经常需要来回切换。下面给出最常用的转换公式。
2.5.1 欧拉角 → 旋转矩阵
假设旋转顺序为 Z-Y-X（偏航-俯仰-横滚）：
R = Rz(yaw) * Ry(pitch) * Rx(roll)
其中：
Rx(roll) = [[1, 0, 0],
[0, cos(roll), -sin(roll)],
[0, sin(roll), cos(roll)]]
Ry(pitch) = [[cos(pitch), 0, sin(pitch)],
[0, 1, 0],
[-sin(pitch), 0, cos(pitch)]]
Rz(yaw) = [[cos(yaw), -sin(yaw), 0],
[sin(yaw), cos(yaw), 0],
[0, 0, 1]]2.5.2 旋转矩阵 → 四元数
从旋转矩阵 R 的元素提取四元数：
w = sqrt(1 + R[0][0] + R[1][1] + R[2][2]) / 2
x = (R[2][1] - R[1][2]) / (4 * w)
y = (R[0][2] - R[2][0]) / (4 * w)
z = (R[1][0] - R[0][1]) / (4 * w)注意：当 w 接近 0 时，需要用其他公式避免数值不稳定。
2.5.3 四元数 → 欧拉角
roll = atan2(2*(w*x + y*z), 1 - 2*(x² + y²))
pitch = asin(2*(w*y - z*x))
yaw = atan2(2*(w*z + x*y), 1 - 2*(y² + z²))这里 atan2 是四象限反正切函数，能正确处理符号。
实战建议：在代码中，我建议统一使用四元数做内部运算，只在需要显示或人机交互时转成欧拉角。这样可以避免万向锁，也减少转换误差。
2.6 知识体系总览
下面这张图，是我自己总结的本章知识结构。你可以把它当作一个快速索引。
这张图把本章的核心逻辑串起来了。从世界坐标系和机体坐标系出发，引出三种旋转表示方法，最后强调它们之间的相互转换。你可以在后续学习中反复回来对照这张图。
2.7 本章小结
好了，这一章的内容就到这里。
我们讲了世界坐标系和机体坐标系的概念，深入分析了旋转矩阵、欧拉角、四元数这三种旋转表示方法，并给出了它们之间的转换公式。
记住，没有哪一种表示方法是万能的。旋转矩阵直观但冗余，欧拉角易懂但有万向锁，四元数高效但不够直观。工程中要根据场景灵活选择。
下一章，我们会把这些知识用到实际的传感器数据融合中。到时候你会发现，今天打下的基础有多重要。
课后练习：写一个小程序，输入欧拉角 (30°, 45°, 60°)，分别计算对应的旋转矩阵和四元数，再转回欧拉角，看看是否一致。这个小练习能帮你巩固今天的知识。