坐标系与变换：世界坐标系、机体坐标系、传感器坐标系，欧拉角与四元数基础
做传感器融合，说白了就是跟坐标系打交道。我见过不少新手，算法公式背得滚瓜烂熟，结果一上手数据全乱套——十有八九是坐标系没搞明白。今天咱们就把这摊子事捋清楚。
1. 三个核心坐标系
先说说最常用的三个坐标系。你想想看，一个机器人身上挂着好几个传感器，每个传感器都有自己的小算盘，得有个统一的世界观才能把数据对齐。
世界坐标系（World Frame）
这是绝对参考系，通常用 W 表示。我习惯把机器人启动时的位置设为原点，X轴指向正东，Y轴指向正北，Z轴朝天。当然，你也可以用GPS经纬度作为基准，看具体场景。
机体坐标系（Body Frame）
也叫载体坐标系，用 B 表示。原点在机器人质心，X轴朝前，Y轴朝左，Z轴朝上。注意，这里有个坑——不同行业定义不一样。无人机领域常用“前-左-上”，但汽车领域可能是“前-右-下”。
我在做AGV小车项目时，就吃过这个亏。供应商给的IMU数据按“前-右-下”定义，我按“前-左-上”解算，结果航向角差了180度。嗯，后来我学乖了，拿到传感器第一件事就是确认坐标系定义。
传感器坐标系（Sensor Frame）
每个传感器都有自己的坐标系。比如IMU的加速度计和陀螺仪，它们的敏感轴方向可能跟机体坐标系不完全重合。这就涉及到标定问题了。
| 坐标系 | 原点 | 典型用途 |
|---|---|---|
| 世界坐标系 W | 固定点（如起点） | 全局定位、路径规划 |
| 机体坐标系 B | 机器人质心 | 控制指令、姿态描述 |
| 传感器坐标系 S | 传感器中心 | 原始数据输出 |
2. 坐标系变换：旋转矩阵
坐标系之间怎么转换？核心就是旋转矩阵。一个点在世界坐标系下的坐标 PW，转换到机体坐标系下就是 PB = RWB · PW。
旋转矩阵 R 是3x3的正交矩阵，行列式为1。说白了，它只改变方向，不改变长度。我刚开始学的时候，总觉得旋转矩阵太抽象，后来发现把它看成三个列向量就简单了——每一列就是目标坐标系的基向量在原坐标系下的投影。
// 绕Z轴旋转θ角的旋转矩阵
Rz(θ) = [
cosθ, -sinθ, 0,
sinθ, cosθ, 0,
0, 0, 1
]3. 欧拉角：直观但危险
欧拉角用三个角度描述姿态：横滚角（Roll）、俯仰角（Pitch）、偏航角（Yaw）。直观吧？但我要说，欧拉角是个美丽的陷阱。
为什么？因为万向锁（Gimbal Lock）。当俯仰角达到±90°时，横滚和偏航就分不清了。我在做四轴飞行器时遇到过这问题——飞机垂直爬升时，姿态解算突然跳变，差点炸机。
欧拉角的另一个问题是旋转顺序。ZYX顺序和XYZ顺序结果完全不同。我建议你统一用ZYX顺序（先偏航，再俯仰，最后横滚），这是航空领域的惯例。
4. 四元数：工程师的救星
四元数 q = w + xi + yj + zk，其中 w 是实部，x、y、z 是虚部。别被数学公式吓到，你只需要记住三点：
- 四元数没有万向锁问题
- 四元数插值平滑（SLERP）
- 四元数乘法比旋转矩阵快
四元数表示绕单位向量 (x, y, z) 旋转 θ 角度：
q = [cos(θ/2), x*sin(θ/2), y*sin(θ/2), z*sin(θ/2)]注意，四元数必须归一化。我见过有人忘了归一化，结果姿态越算越飘。嗯，这属于低级错误，但确实容易犯。
5. 欧拉角与四元数互转
实际项目中，我们经常需要在两者之间切换。比如从IMU读到的四元数，要转成欧拉角显示给用户看。
// 四元数转欧拉角（ZYX顺序）
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²))6. 知识体系总览
下面这张图是我自己整理的坐标系与变换知识结构，你可以照着这个思路去搭建自己的标定流程。
7. 实战建议
最后，给你几条实在的建议：
- 统一约定：项目一开始就定好坐标系定义和旋转顺序，写在文档里。我吃过亏，所以现在每个项目第一件事就是建个坐标系定义表。
- 可视化验证：写个简单的3D可视化程序，把坐标系画出来。数据对不对，看一眼就知道。
- 单元测试：写几个已知的旋转案例做测试。比如绕X轴转90°，看看结果对不对。
好了，坐标系这块就聊到这儿。记住一句话：坐标系定义清楚，融合就成功了一半。