坐标系基础：世界坐标系、车辆坐标系、传感器坐标系

做感知融合，说白了就是跟坐标系打交道。我刚开始接触这个领域时，觉得坐标系不就是几个箭头嘛，有什么好讲的？结果第一次做真车路测，数据对齐就出了大问题——嗯，从那以后我再也不敢小看坐标系了。

今天咱们就把这三个坐标系彻底聊透。你想想看，自动驾驶车上那么多传感器，激光雷达、摄像头、毫米波雷达，每个都有自己的"视角"。如果不把它们统一到一个坐标系下，那感知结果就是一团乱麻。

世界坐标系，也叫全局坐标系。它是个固定不动的参考系，用来描述车辆在整个环境中的位置。

我个人习惯用东北天坐标系（ENU）作为世界坐标系。X轴指向东，Y轴指向北，Z轴指向天。为什么选这个？因为直观啊！你开车时说的"往东走"、"向北拐"，跟这个坐标系天然对应。

  关键点：世界坐标系的原点通常是固定的，比如地图上的某个GPS基准点。所有车辆的轨迹、障碍物的绝对位置，最终都要映射到这个坐标系里。

我在项目中遇到过一个问题：高精地图用的是WGS84经纬度坐标，但感知算法需要的是米制坐标。这就得做投影转换，把经纬度转成平面坐标。常用的有UTM投影，但要注意不同UTM带之间的切换——我曾经因为这个吃过亏，数据拼接时出现了几米的偏差。

车辆坐标系是跟着车走的。它的原点通常在车辆后轴中心（或者质心），X轴指向车头前方，Y轴指向车身左侧，Z轴指向上方。

为什么用后轴中心？因为车辆的动力学模型是基于后轴建立的。你想想看，车辆转弯时，后轴的轨迹更稳定，前轴会有侧偏角。所以做运动预测、轨迹规划时，用后轴中心更合理。

小技巧：车辆坐标系下的障碍物位置，通常用距离和角度表示。比如"前方20米，左侧3米处有个行人"。这种表示法在决策规划时特别方便。

每个传感器都有自己的坐标系。激光雷达的原点在激光发射中心，摄像头的光心在镜头后方，毫米波雷达的原点在雷达天线中心。

这里有个坑：不同传感器的坐标系定义可能不一样。比如有的激光雷达把Z轴朝上，有的朝下。摄像头坐标系更是五花八门——有的用右手系，有的用左手系。

避坑指南：我曾经在项目里遇到过一个bug，原因是激光雷达的坐标系是"前-左-上"，而摄像头是"右-下-前"。两个坐标系没有统一，导致融合后的目标位置直接飞到了天上。后来我养成了一个习惯：拿到新传感器，第一件事就是确认它的坐标系定义。

做感知融合，核心工作之一就是坐标系转换。说白了，就是把所有传感器的数据都转到同一个坐标系下，通常是车辆坐标系。

转换公式其实不复杂，就是旋转加平移：

// 传感器坐标系 -> 车辆坐标系
// 假设传感器安装在车辆上，有固定的安装位置和朝向
// 旋转矩阵 R 和平移向量 T 通过标定得到

P_vehicle = R * P_sensor + T

这里的R是3x3的旋转矩阵，T是3x1的平移向量。标定过程就是确定R和T的值。我建议用外参标定工具来做，手动测量误差太大。

咱们来看一个实际例子。假设激光雷达检测到一个障碍物，在雷达坐标系下的位置是(10, 2, 0)米。现在要把它转到车辆坐标系：

你想想看，如果这一步做错了，那后面的目标关联、跟踪、预测全都会乱套。所以坐标系转换是感知融合的基石，一点都马虎不得。

  核心要点：
  世界坐标系：固定不动，用于全局定位和地图匹配
车辆坐标系：随车移动，用于局部感知和决策
传感器坐标系：每个传感器独立，需要统一转换
转换的核心：旋转矩阵R + 平移向量T

嗯，坐标系这部分就讲到这里。记住一句话：坐标系不乱，融合就成功了一半。下一节咱们聊聊目标关联的具体算法，到时候你会看到坐标系转换是怎么实际应用的。

个人经验：我建议在代码里把坐标系转换封装成独立的模块，每次调用都明确标注输入和输出坐标系。这样调试时一目了然，不会搞混。

好了，坐标系基础就聊这么多。记住：坐标系是感知融合的"语言"，语言不通，啥都干不成。下一节咱们用实际代码来演示坐标系转换的具体实现。