3. 坐标系与变换链

做多传感器融合，说白了就是跟坐标系打交道。我见过不少新手，一上来就调算法，结果定位飘得离谱，最后发现是坐标系定义错了。嗯，今天咱们就把这个地基打牢。

3.1 世界坐标系：一切测量的基准

世界坐标系，也叫全局坐标系。它是我们描述所有物体位置的绝对参考。你可以把它想象成一张大地图上的经纬度网格。

我个人习惯把世界坐标系的原点选在车辆启动的位置，或者某个固定的地标点。比如在停车场，我就选在入口处。为什么？因为这样算出来的坐标值不会太大，数值稳定性好。

常用的世界坐标系有几种：

• ENU坐标系：东-北-天方向，常用于GPS/IMU融合
• NED坐标系：北-东-地方向，航空领域更常见
• 自定义坐标系：比如以某个墙角为原点，X轴指向走廊方向

我在项目中遇到过一个问题：用GPS做初始化时，ENU和NED搞混了，结果车辆定位直接转了90度。那次排查花了我整整一个下午。所以，一定要在代码里明确注释你用的是哪种。

3.2 车体坐标系：车辆自身的“身体”

车体坐标系是固定在车辆上的。它的原点通常在车辆后轴中心，或者质心位置。X轴指向车头前方，Y轴指向左侧，Z轴指向上方。

你想想看，为什么不用车头最前面做原点？因为后轴中心在车辆转弯时是不动的，这样描述运动更干净。

车体坐标系的关键属性：

• 它是移动的，随着车辆运动而变化
• 它是刚体，所有传感器都固定在它上面
• 它的姿态变化就是我们说的“车辆位姿”

3.3 传感器坐标系：每个传感器的“小世界”

每个传感器都有自己的坐标系。比如：

• 激光雷达：原点在激光头中心，X轴指向扫描起始方向
• 摄像头：原点在光心，Z轴指向镜头前方（注意！不同相机模型定义不同）
• IMU：原点在MEMS芯片中心，坐标轴跟芯片封装方向有关
• 轮速计：通常以车轮中心为原点，但实际使用时我们更关心轮速脉冲

我曾经踩过一个坑：某款激光雷达的坐标系定义跟常规相反，Y轴朝右而不是朝左。结果融合出来的点云全是镜像的。从那以后，我拿到新传感器第一件事就是看它的数据手册，确认坐标系定义。

3.4 坐标变换链：把一切串起来

现在我们有三个坐标系：世界、车体、传感器。它们之间的关系就是变换链。

变换链的核心思想是：任何传感器测量到的数据，最终都要转换到世界坐标系下，才能用于定位和建图。

举个例子，激光雷达检测到一个障碍物，它在激光雷达坐标系下的坐标是 (x_l, y_l, z_l)。要把它放到地图上，需要经过两步：

1. 从激光雷达坐标系转到车体坐标系（外参标定）
2. 从车体坐标系转到世界坐标系（车辆位姿）

用数学表达就是：

P_world = T_world_vehicle * T_vehicle_lidar * P_lidar

其中 T 是 4x4 的齐次变换矩阵。这个链可以无限延伸，比如你还有GPS、IMU、摄像头，那就继续乘下去。

3.5 实战中的坐标系管理技巧

说了这么多理论，来点实际的。我在做多传感器融合时，会遵循几个原则：

• 统一中间坐标系：所有传感器数据先转到车体坐标系，再转到世界坐标系。不要搞“传感器A直接到世界，传感器B先到车体再到世界”这种混乱的路径。
• 用树状结构管理：把坐标系关系画成一棵树，根节点是世界坐标系，子节点是车体，再下面是各个传感器。这样一目了然。
• 写单元测试：每次修改外参后，跑一遍测试，看看一个已知点经过变换链后是否落在正确位置。这个习惯救过我很多次。

下面是我常用的一张坐标系关系图，你可以参考：

这张图里，世界坐标系是根，车体坐标系是中间节点，各个传感器是叶子节点。箭头方向表示“从哪到哪”，比如“世界到车体”就是车辆位姿，“车体到传感器”就是外参。

3.6 小结

坐标系和变换链是外参标定的基础。记住三点：

• 世界坐标系是绝对基准，不能动
• 车体坐标系是中间桥梁，所有传感器都通过它连接到世界
• 变换链的顺序和方向不能搞错，否则定位必飘