3. 坐标系统与空间变换：世界坐标系、传感器坐标系、刚体变换、欧拉角与四元数
各位同学，大家好。今天我们来聊聊多传感器融合里一个绕不开的话题——坐标系统与空间变换。
说实话，我刚开始做融合的时候，觉得这玩意儿不就是几个坐标系来回倒腾嘛，有啥难的？结果第一次做IMU和摄像头联合标定，数据死活对不上，折腾了三天才发现是欧拉角万向锁的问题。从那以后，我再也不敢小看这部分内容了。
你想想看，一个激光雷达扫出来的点云，一个摄像头拍到的图像，一个IMU测出来的加速度，它们各自生活在自己的小世界里。想让它们协同工作，就得先搞清楚它们之间的“亲戚关系”。
3.1 世界坐标系与传感器坐标系
先说说世界坐标系。说白了，这就是我们定义的一个“绝对参考系”。
比如你在一栋大楼里做定位，你可以把大楼的东北角设为原点，正东为X轴，正北为Y轴，垂直向上为Z轴。这个坐标系一旦定下来，所有传感器的数据最终都要转换到这个坐标系下。
传感器坐标系呢？每个传感器都有自己的“小算盘”。
- IMU坐标系：通常以芯片中心为原点，X轴指向芯片的某个方向，Y轴和Z轴按右手定则确定。我见过不少新手直接把IMU的数据当成世界坐标系下的数据用，结果可想而知。
- 摄像头坐标系：原点在光心，Z轴指向镜头前方，X轴向右，Y轴向下。嗯，这里要注意，摄像头坐标系的Y轴是向下的，和通常的右手系不太一样。
- 激光雷达坐标系：原点在旋转中心，通常Z轴向上，X轴指向正前方。
核心要点：所有传感器数据最终都要统一到同一个坐标系下，通常是世界坐标系或者车体/机器人本体坐标系。
3.2 刚体变换：旋转 + 平移
从一个坐标系到另一个坐标系，本质上就是一个刚体变换。刚体变换包含两部分：旋转和平移。
数学上可以写成：
P_w = R * P_s + T其中P_w是世界坐标系下的点，P_s是传感器坐标系下的点，R是旋转矩阵，T是平移向量。
我个人习惯把R和T合起来写成一个4x4的齐次变换矩阵：
| R T |
| 0 1 |这样写的好处是，多个变换可以连乘，非常方便。比如从传感器A到传感器B，再到世界坐标系，直接三个矩阵乘起来就行。
小技巧：我在做多传感器标定时，习惯把所有传感器的变换关系画成一个树状图。根节点是世界坐标系，叶子节点是各个传感器。这样每次做变换时，沿着树往上找路径就行，不容易出错。
3.3 欧拉角：直观但危险
欧拉角可能是最直观的旋转表示方式了。用三个角度来描述旋转：绕X轴转（roll）、绕Y轴转（pitch）、绕Z轴转（yaw）。
但是，欧拉角有个致命的问题——万向锁。
我曾经在一个无人机项目里吃过这个亏。当时用欧拉角做姿态控制，飞机做大角度机动时，突然就失控了。查了半天，发现是pitch角接近90度时，roll和yaw变得无法区分，这就是典型的万向锁。
警告：欧拉角在pitch接近±90度时会出现奇异性。如果你的系统可能涉及大角度运动，请谨慎使用欧拉角。
另外，欧拉角的旋转顺序也很重要。常见的顺序有ZYX、ZYZ等。不同顺序得到的角度值完全不同。我建议在项目一开始就明确约定好旋转顺序，并在代码注释里写清楚。
3.4 四元数：稳定可靠的选择
四元数是我个人最喜欢的旋转表示方式。它没有万向锁问题，插值平滑，计算效率也高。
一个四元数可以写成：
q = w + xi + yj + zk其中w是实部，x、y、z是虚部。它本质上是一个单位四元数，模长为1。
用四元数做旋转时，公式是：
P' = q * P * q^(-1)其中P是纯四元数（实部为0），q^(-1)是q的共轭。
嗯，这里要注意，四元数的乘法不满足交换律，顺序搞反了结果就完全不对了。
为什么推荐四元数？
- 无奇异性，适用于全姿态
- 插值平滑（球面线性插值SLERP）
- 计算量小，适合嵌入式平台
3.5 实战：IMU姿态解算中的坐标变换
我们来看一个实际例子。假设你有一个IMU，它输出的是传感器坐标系下的角速度和加速度。你想得到世界坐标系下的姿态。
基本流程是这样的：
- 读取IMU原始数据（角速度、加速度）
- 用四元数表示当前姿态
- 用角速度更新四元数（姿态预测）
- 用加速度和地磁修正四元数（姿态校正）
代码示例（简化版）：
// 四元数更新
void updateQuaternion(Quaternion &q, float gx, float gy, float gz, float dt) {
// 角速度转四元数增量
float half_dt = 0.5f * dt;
float dq_w = 1.0f;
float dq_x = gx * half_dt;
float dq_y = gy * half_dt;
float dq_z = gz * half_dt;
// 四元数乘法
Quaternion delta_q = {dq_w, dq_x, dq_y, dq_z};
q = q * delta_q;
// 归一化
float norm = sqrt(q.w*q.w + q.x*q.x + q.y*q.y + q.z*q.z);
q.w /= norm;
q.x /= norm;
q.y /= norm;
q.z /= norm;
}这段代码看起来简单，但实际项目中要考虑的问题很多。比如陀螺仪的零偏、加速度计的噪声、地磁的干扰等等。我建议在正式用之前，先做一下传感器校准。
3.6 知识体系总览
下面这张图是我自己画的，把本章的核心逻辑串起来了：
从这张图可以看出，世界坐标系和传感器坐标系之间通过刚体变换连接，而刚体变换的核心就是旋转。旋转可以用旋转矩阵、欧拉角或四元数来表示。我个人建议，在算法内部统一用四元数，只在人机交互时转成欧拉角。
避坑指南：我曾经在一个项目里混用了不同顺序的欧拉角，结果姿态解算完全乱套。后来我定了个规矩：所有接口统一用四元数，只在显示时转成欧拉角。这个习惯一直用到现在。
好了，关于坐标系统和空间变换，我们就聊到这里。记住，坐标系是融合的基础，搞不清楚坐标系，后面的融合算法再漂亮也是白搭。
公众号:蓝海资料掘金营，微信deep3321