2. 数学基础（上）：三维空间中的坐标系定义

做多传感器融合，第一件事不是调算法，而是把坐标系理清楚。我见过太多项目，标定参数算得再准，最后因为坐标系定义不一致，点云投影出来全是歪的。说白了，坐标系就是所有传感器的「共同语言」。你想想看，激光雷达说「前方 10 米」，IMU 说「加速度朝东」，如果它们不在同一个坐标系里说话，那融合出来的结果就是一团浆糊。

这一节，我们先把三维空间里的几个核心坐标系讲透。包括世界系、车身系、传感器系，以及贯穿始终的右手定则。嗯，这里要注意，每个坐标系的选择都有它的物理意义和工程习惯，不是随便定的。

2.1 右手定则：三维空间的「交通规则」

三维坐标系的方向怎么定？右手定则就是答案。伸出你的右手，拇指、食指、中指互相垂直——拇指指向 X 轴正方向，食指指向 Y 轴正方向，中指指向 Z 轴正方向。这就是右手坐标系。

为什么一定要用右手系？因为旋转方向也是由右手定则定义的：四指弯曲的方向就是正旋转方向。如果团队里有人用了左手系，有人用了右手系，那旋转矩阵算出来符号全是反的。我曾经在一个项目里接手过别人的代码，发现点云绕 Z 轴旋转时方向总是不对，查了两天才发现是坐标系习惯不一致——从那以后，我要求团队所有传感器驱动里必须显式声明坐标系手性。

旋转正方向也遵循右手定则：

• 绕 X 轴正旋转：从 Y 轴转向 Z 轴（右手拇指指向 X 正方向）
• 绕 Y 轴正旋转：从 Z 轴转向 X 轴
• 绕 Z 轴正旋转：从 X 轴转向 Y 轴

这个规则在后续章节计算旋转矩阵、四元数、欧拉角时都会反复用到。记牢它，能省掉一半的调试时间。

2.2 世界坐标系：一切测量的「参考」

世界坐标系，也叫全局坐标系，是描述车辆和传感器在真实世界中位置的基准。它固定在大地上，不随车辆移动而改变。

在自动驾驶中，世界坐标系通常这样定义：

• X 轴：指向东（或指向北，取决于具体约定）
• Y 轴：指向北（或指向东，与 X 轴垂直）
• Z 轴：指向天（垂直于地面向上）

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

我个人习惯在量产项目里统一使用 ENU 坐标系。为什么？因为 ROS 的 map 和 odom 帧默认就是 ENU 约定，和 GPS 输出的经纬高转换过来也最直接。如果你用 NED，那 Z 轴朝下，激光雷达点云的高度值全是负数，看着就别扭。

2.3 车身坐标系：车辆的「本体感知」

车身坐标系固定在车辆上，随车移动。它描述的是「车自己怎么看世界」。定义方式如下：

• 原点：通常在后轴中心（或车辆质心）
• X 轴：指向车辆前进方向（车头方向）
• Y 轴：指向车辆左侧（符合右手系，Z 轴朝上时 Y 轴指向左）
• Z 轴：指向车顶（垂直于地面向上）

这里有个容易搞混的地方：有些车厂把 Y 轴定义为指向右侧。如果你用右手定则，Z 轴朝上时 Y 轴指向左才是正方向。指向右的话，就变成左手系了。我记得有一次集成某供应商的车辆动力学模型，他们输出的侧向速度符号总是反的，最后发现是车身坐标系 Y 轴方向定义不同。从那以后，我要求所有外协模块必须提供坐标系定义文档。

车身坐标系在以下场景中直接使用：

• 车辆动力学控制（速度、加速度、横摆角速度）
• 底盘 CAN 信号解析（方向盘转角、轮速）
• IMU 原始数据（加速度、角速度）
• 障碍物相对位置（前向碰撞预警、盲区监测）

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

每个传感器都有自己的坐标系。激光雷达、摄像头、毫米波雷达、IMU，它们的坐标系定义各不相同。做融合的第一步，就是把它们统一到车身坐标系下。

常见的传感器坐标系定义：

摄像头比较特殊。它的原始数据是像素坐标 (u, v)，需要先转换到相机坐标系（3D 空间），再转到车身坐标系。相机坐标系通常定义 Z 轴指向拍摄方向，X 轴向右，Y 轴向下——这和右手系不太一样，转换时要特别注意。

2.5 坐标系关系图：从传感器到世界

下面这张图展示了四个坐标系之间的层级关系。理解这个层级，是后续做坐标变换的基础。

从图中可以看出，所有传感器数据最终都要变换到世界坐标系下，才能进行融合和决策。中间经过车身坐标系作为「」。这样做的好处是：每个传感器的外参标定只针对车身坐标系，不直接与世界坐标系耦合。如果车辆移动了，只需要更新车身到世界的变换，传感器到车身的变换保持不变。

2.6 坐标系变换的数学表示

坐标系之间的变换，用齐次变换矩阵 T 表示。它是一个 4×4 矩阵：

T = [ R   t ]
    [ 0   1 ]

其中：
R：3×3 旋转矩阵
t：3×1 平移向量
0：1×3 零向量

举个例子，把激光雷达坐标系下的点 p_lidar 变换到车身坐标系：

p_body = T_body_to_lidar * p_lidar

这里的 T_body_to_lidar 就是激光雷达的外参矩阵。它描述了激光雷达坐标系相对于车身坐标系的位姿。标定的过程，就是求解这个矩阵的过程。

2.7 坐标系定义检查清单

在实际项目中，我总结了一份检查清单。每次接入新传感器或新算法模块时，我都会逐条核对：

1. 手性确认：所有坐标系都是右手系吗？有没有混入左手系？
2. 轴向定义：每个坐标系的 X、Y、Z 正方向是否明确？有没有文档？
3. 原点位置：传感器坐标系的原点在传感器物理中心的哪个位置？是光学中心还是安装面？
4. 旋转正方向：绕各轴的正旋转方向是否遵循右手定则？
5. 单位统一：所有坐标系使用米（m）还是毫米（mm）？角度用弧度（rad）还是度（deg）？
6. 时间戳对齐：坐标系变换时，对应的数据时间戳是否一致？

嗯，这六条看起来简单，但每一条都踩过坑。尤其是第 5 条，单位问题——我见过有人把毫米当成米传进来，结果障碍物距离直接差了 1000 倍。还好是在仿真阶段发现的，不然实车测试就危险了。