坐标系基础：世界坐标系、车辆坐标系、传感器坐标系定义与转换

各位同学好，我是你们的老朋友。今天我们来聊聊坐标系，这玩意儿看着基础，但说实话，我见过太多项目栽在坐标系搞混上。我自己刚入行那会儿，就因为搞反了传感器坐标系，导致融合出来的目标位置偏了整整两米，排查了三天才找到问题。嗯，从那以后，我对坐标系就格外上心。

说白了，多传感器融合的核心，就是要把不同传感器看到的东西，统一到同一个坐标系下说话。你想想看，激光雷达说「前方5米有障碍物」，摄像头说「图像中心偏右100像素有个目标」，毫米波雷达说「径向速度20m/s」。这三个信息如果不统一到同一个坐标系，根本没法融合。

1. 世界坐标系：我们生活的真实世界

世界坐标系，也叫全局坐标系。它描述的是物体在真实物理世界中的位置。我习惯把它理解成「上帝视角」——一个固定不动的大地坐标系。

在自动驾驶中，世界坐标系通常采用东北天（ENU）坐标系：

• X轴：指向东（East）
• Y轴：指向北（North）
• Z轴：指向天顶（Up），垂直于地面向上

为什么用ENU？因为符合人的直觉。你开车往北走，Y轴方向就是前进方向。不过也有用北东地（NED）的，Z轴向下，航空领域用得比较多。我个人建议做自动驾驶用ENU，更直观。

2. 车辆坐标系：车身的「自我认知」

车辆坐标系是固定在车身上的坐标系。它随着车辆移动而移动，描述的是「相对于车本身」的位置关系。

标准定义（ISO 8855）：

• X轴：指向车辆前进方向（车头方向）
• Y轴：指向车辆左侧（注意！是左侧，不是右侧）
• Z轴：指向车顶上方

原点通常放在车辆后轴中心，或者车辆质心。我个人习惯放在后轴中心，因为车辆运动学模型用这个点算起来最方便。

我曾经遇到过一个项目，团队里有人把车辆原点放在前保险杠中心。结果做路径规划时，算出来的转弯半径总是偏大，排查了好久才发现是坐标系原点不一致导致的。所以，原点选择一定要统一，最好写在团队的标定规范文档里。

3. 传感器坐标系：每个传感器的「小世界」

每个传感器都有自己的坐标系。这是最容易出问题的地方。

激光雷达坐标系：

• 通常以激光雷达的旋转中心为原点
• X轴指向激光雷达正前方
• Y轴指向左侧
• Z轴指向上方
• 点云数据就是在这个坐标系下采集的

摄像头坐标系：

• 原点在摄像头的光学中心（光心）
• Z轴指向摄像头正前方（沿着光轴方向）
• X轴指向右侧
• Y轴指向下方（注意！图像坐标系中Y轴是向下的）

这里有个坑——摄像头坐标系和图像坐标系的关系。图像坐标系是二维的（u, v），而摄像头坐标系是三维的（Xc, Yc, Zc）。两者通过内参矩阵关联。我刚开始做视觉时，经常把这两个搞混，后来养成了一个习惯：每次写代码前，先画个坐标系示意图贴在显示器上。

毫米波雷达坐标系：

• 原点在雷达天线中心
• X轴指向雷达正前方
• Y轴指向左侧
• Z轴指向上方
• 注意：毫米波雷达通常只提供距离、方位角和径向速度，不提供高度信息

4. 坐标系转换：把「小世界」统一到「大世界」

坐标系转换说白了就是两步：旋转 + 平移。用数学语言说，就是刚体变换。

公式很简单：

P_world = R * P_sensor + T

其中：

• P_world：物体在世界坐标系下的坐标
• P_sensor：物体在传感器坐标系下的坐标
• R：旋转矩阵（3x3）
• T：平移向量（3x1）

旋转矩阵R有3个自由度（绕X、Y、Z轴的旋转角），平移向量T也有3个自由度。所以外参标定要标的就是这6个参数。

实际工程中，我们通常用欧拉角或者四元数来表示旋转。我个人更推荐四元数，因为它没有万向锁问题。不过欧拉角更直观，调试时方便看。

// 传感器到车辆坐标系的转换
P_vehicle = R_sensor_to_vehicle * P_sensor + T_sensor_to_vehicle

// 车辆到世界坐标系的转换
P_world = R_vehicle_to_world * P_vehicle + T_vehicle_to_world

// 合并：传感器到世界坐标系
P_world = R_vehicle_to_world * (R_sensor_to_vehicle * P_sensor + T_sensor_to_vehicle) + T_vehicle_to_world

5. 转换链：从传感器到世界的完整路径

在实际系统中，数据流是这样的：

1. 传感器采集原始数据（在传感器坐标系下）
2. 通过外参转换到车辆坐标系
3. 结合车辆定位信息（GPS/IMU/里程计），转换到世界坐标系
4. 所有传感器的数据都在世界坐标系下，进行融合

这里有个关键点：车辆坐标系是中间桥梁。所有传感器先统一到车辆坐标系，再统一到世界坐标系。这样做的好处是，如果某个传感器的外参变了（比如摄像头被碰歪了），只需要重新标定该传感器到车辆的外参，不影响其他传感器。

下面我用一张图来展示整个坐标系转换的流程：

从这张图可以看得很清楚：每个传感器先通过外参标定转换到车辆坐标系，然后车辆再通过定位信息转换到世界坐标系。这样层层递进，最终所有数据都在同一个坐标系下。

6. 实际工程中的注意事项

做坐标系转换时，有几个坑我踩过，分享给大家：

• 旋转方向要统一：顺时针还是逆时针？建议统一用右手定则。我见过团队里有人用左手定则，结果旋转方向全反了。
• 单位要一致：平移向量的单位是米还是毫米？角度是弧度还是度？这些细节一定要在代码里写清楚注释。
• 坐标系顺序不能乱：先旋转再平移，还是先平移再旋转？标准做法是先旋转再平移。顺序搞反了，结果会差很多。
• 验证转换是否正确：我每次写完转换代码，都会找一个已知点做验证。比如把一个传感器坐标系下的原点（0,0,0）转换到车辆坐标系，看看是不是对应传感器的安装位置。

好了，坐标系基础就讲到这里。记住一句话：坐标系转换是传感器融合的基石，基础不牢，地动山摇。下一节我们会深入讲旋转矩阵和四元数的具体计算方法，到时候会带大家手写转换代码。