3. 坐标系统与空间变换：世界坐标系、车身坐标系、传感器坐标系、刚体变换

做多传感器融合，第一个绕不开的坎儿就是坐标系统。说白了，你得让所有传感器在同一个“语言”下说话。

我刚开始做融合项目时，就吃过这个亏。当时激光雷达和摄像头各自输出目标位置，看起来都对，一叠加就全乱套了。查了两天，最后发现是激光雷达用的坐标系和摄像头坐标系差了90度旋转。嗯，从那以后，我养成了一个习惯：先对齐坐标系，再谈融合。

3.1 三种核心坐标系

在车载融合系统里，我们打交道最多的就是这三个坐标系：

• 世界坐标系：全局的、固定的参考系。比如GPS用的WGS-84经纬度，或者高精地图里的UTM坐标。它不随车动。
• 车身坐标系：固定在车辆上的坐标系。一般原点在车辆后轴中心或质心，X轴朝前，Y轴朝左，Z轴朝上。车辆动，它跟着动。
• 传感器坐标系：每个传感器自己的“小世界”。比如摄像头的光心、激光雷达的旋转中心、IMU的安装点。

你想想看，雷达检测到“前方5米有个障碍物”，这个“前方”是相对于雷达自己的。但决策层需要知道“这个障碍物相对于车身在哪里”，甚至“在地图上的绝对位置是多少”。所以，坐标变换就是把这些“方言”翻译成“普通话”。

3.2 刚体变换：旋转矩阵与平移向量

坐标系之间的变换，本质上是刚体变换。刚体变换包含两部分：旋转和平移。

旋转用旋转矩阵 R 表示，平移用平移向量 t 表示。一个点 P 从坐标系 A 变换到坐标系 B，公式很简单：

P_B = R * P_A + t

这里 R 是 3x3 的矩阵，t 是 3x1 的向量。

我记得有一次在标定激光雷达和摄像头时，旋转矩阵算出来总是差一点点。后来发现是旋转顺序搞反了。这里给大家提个醒：旋转矩阵不满足交换律，先绕X轴转再绕Y轴转，和先绕Y轴转再绕X轴转，结果完全不同。

3.3 四元数与欧拉角

说到旋转表示，就不得不提欧拉角和四元数。这两个东西，我建议你都要掌握。

欧拉角直观，用三个角度（滚转Roll、俯仰Pitch、偏航Yaw）描述旋转。但它的缺点也很明显：

• 存在万向锁问题
• 插值不平滑
• 不同定义顺序导致歧义

四元数用四个数表示旋转，没有奇点，插值平滑。虽然不直观，但计算稳定。我个人的经验是：内部计算全用四元数，只在人机交互时转成欧拉角。

四元数的形式：

q = w + xi + yj + zk
其中 w^2 + x^2 + y^2 + z^2 = 1

从四元数到旋转矩阵的转换，我贴一段常用的C++代码：

// 四元数转旋转矩阵
Eigen::Matrix3d quatToRotMat(double w, double x, double y, double z) {
    Eigen::Matrix3d R;
    R(0,0) = 1 - 2*y*y - 2*z*z;
    R(0,1) = 2*x*y - 2*w*z;
    R(0,2) = 2*x*z + 2*w*y;
    R(1,0) = 2*x*y + 2*w*z;
    R(1,1) = 1 - 2*x*x - 2*z*z;
    R(1,2) = 2*y*z - 2*w*x;
    R(2,0) = 2*x*z - 2*w*y;
    R(2,1) = 2*y*z + 2*w*x;
    R(2,2) = 1 - 2*x*x - 2*y*y;
    return R;
}

3.4 坐标系变换链

在实际系统中，往往不是一次变换就能搞定。比如：

• 激光雷达点云 → 传感器坐标系
• 传感器坐标系 → 车身坐标系
• 车身坐标系 → 世界坐标系

这就形成了一个变换链。每个环节都有对应的旋转和平移参数，这些参数通常通过标定获得。

我参与过一个项目，需要把多个激光雷达的数据融合到车身坐标系下。每个雷达都有自己的安装位置和朝向，变换参数稍有偏差，融合出来的点云就会出现重影。后来我们用了高精度标定板，反复迭代，才把误差控制在厘米级。

下面这张图展示了整个坐标变换的流程：

3.5 实际项目中的坐标系管理

最后，分享几个我在项目中总结的经验：

场景	推荐做法	常见坑
传感器标定	使用标定板，采集多组数据，用优化算法求解R和t	标定板不平整、光照变化导致特征点提取不准
实时变换	用四元数存储旋转，避免欧拉角累积误差	频繁的欧拉角转四元数再转回来，精度损失
多传感器融合	统一到车身坐标系，时间戳对齐后再做空间对齐	先做空间变换再做时间对齐，导致数据错位
调试与可视化	在RVIZ或类似工具中同时显示各坐标系，直观检查	只靠数值检查，坐标系偏移了看不出来

嗯，这一章的内容就到这里。坐标系这东西，说起来简单，做起来全是细节。希望我的这些经验能帮你少走一些弯路。