2. 定位基础：坐标系与坐标变换
做定位，说白了就是回答三个问题：我在哪？我要去哪？我怎么去？ 而回答这些问题的前提，是先把「坐标系」这个度量衡搞明白。我见过不少新手，算法推得头头是道，结果一跑实车，定位飘得离谱——最后发现是坐标系搞反了。
嗯，这节课我们就来把坐标系和坐标变换彻底理清楚。
2.1 三大核心坐标系
自动驾驶里，坐标系多到你可能会晕。但别怕，核心就三个：世界坐标系、车体坐标系、传感器坐标系。它们之间的关系，就像你拿着手机导航：世界坐标系是地图，车体坐标系是你自己，传感器坐标系是你的眼睛。
核心原则：所有传感器数据最终都要统一到车体坐标系，再与世界坐标系对齐。这是定位模块的「宪法」。
2.1.1 世界坐标系（World Frame）
世界坐标系是全局的「绝对参考系」。它固定在地球上，不随车辆移动而改变。常用的有：
- ENU（东-北-天）：X 轴指向东，Y 轴指向北，Z 轴指向天。我在项目中习惯用这个，因为直观，符合人的直觉。
- NED（北-东-地）：X 轴指向北，Y 轴指向东，Z 轴指向地。航空领域常用，但在地面自动驾驶里容易搞混「天」和「地」。
- UTM（通用横轴墨卡托）：把地球投影到平面上，单位是米。高精地图和定位模块的标配。
| 坐标系 | X 轴 | Y 轴 | Z 轴 | 适用场景 |
|---|---|---|---|---|
| ENU | 东 | 北 | 天 | 地面车辆、机器人 |
| NED | 北 | 东 | 地 | 航空、无人机 |
| UTM | 东向（投影） | 北向（投影） | 高程 | 高精地图、全局规划 |
我的习惯：项目里统一用 ENU + UTM 投影。GPS 原始数据是经纬度，先转 UTM 得到米制坐标，再按 ENU 定义轴方向。这样后续所有计算都在米级单位下进行，省去很多麻烦。
2.1.2 车体坐标系（Body Frame / Vehicle Frame）
车体坐标系固定在车辆上。你想想看，车辆在运动，但传感器是装在车上的，所以需要一个「随车移动」的参考系。
标准定义（ISO 8855）：
- X 轴：指向车辆前进方向（车头）
- Y 轴：指向车辆左侧（左为正）
- Z 轴：指向车辆上方（天向）
- 原点：通常在后轴中心，或者车辆质心
注意：不同车厂可能定义不同。有的用「前轴中心」，有的用「后轴中心」。我曾经在一个项目里，因为车体坐标系原点定义不一致，导致融合模块输出的位置偏差了 30 厘米——查了三天才找到原因。所以，一定要在项目初期就把坐标系定义写进接口文档。
2.1.3 传感器坐标系（Sensor Frame）
每个传感器都有自己的「小世界」：
- 相机：Z 轴指向镜头前方，X 轴向右，Y 轴向下（图像坐标系）
- 激光雷达：X 轴指向车头，Y 轴向左，Z 轴向上（通常与车体坐标系对齐，但安装位置有偏移）
- IMU：X 轴指向车头，Y 轴向左，Z 轴向上（但安装角度可能有旋转）
- GPS 天线：通常以天线相位中心为原点，方向与车体一致
你想想看，如果每个传感器都用自己的坐标系，那定位模块根本没法工作。所以，我们需要一个统一的「坐标系变换链」。
2.2 坐标变换：从传感器到世界
坐标变换的核心就两个操作：平移 和 旋转。组合起来就是 刚体变换。
2.2.1 变换矩阵（Transformation Matrix）
在三维空间中，一个刚体变换可以表示为 4x4 的齐次变换矩阵：
| R t |
| 0 1 |
其中：
R：3x3 旋转矩阵
t：3x1 平移向量
举个例子，把激光雷达点云从传感器坐标系变换到车体坐标系：
P_vehicle = T_sensor_to_vehicle * P_sensor
T_sensor_to_vehicle =
| R_sv t_sv |
| 0 1 |
这里的 T_sensor_to_vehicle 就是「外参」。它描述了传感器在车上的安装位置和朝向。
关键点：变换矩阵的「方向」很重要。T_a_to_b 表示「从 a 坐标系到 b 坐标系的变换」。我习惯用 T_source_to_target 的命名方式，这样代码里一眼就能看出变换方向。
2.2.2 变换链（Transformation Chain）
从传感器到世界，需要经过多级变换：
P_world = T_vehicle_to_world * T_sensor_to_vehicle * P_sensor
其中：
T_sensor_to_vehicle：外参（固定值，标定得到）T_vehicle_to_world：定位结果（随时间变化）
这就是定位模块的核心工作：实时估计 T_vehicle_to_world。
2.3 坐标系变换的常见表示方法
旋转矩阵虽然精确，但参数多（9 个），而且有正交约束，优化起来不方便。实际工程中，我们常用更紧凑的表示：
| 表示方法 | 参数数量 | 优点 | 缺点 |
|---|---|---|---|
| 旋转矩阵 | 9 | 直观，易于组合 | 冗余，有约束 |
| 欧拉角 | 3 | 直观，参数少 | 万向锁，不连续 |
| 四元数 | 4 | 无万向锁，可插值 | 不够直观 |
| 轴角 | 3 | 紧凑，几何意义明确 | 插值困难 |
我的建议：内部计算统一用四元数。滤波、优化、插值都很方便。只在可视化或调试时转成欧拉角给人看。我曾经在 EKF 里用欧拉角做状态量，结果车辆大角度转弯时直接炸了——万向锁的锅。
2.4 实战：坐标系变换的代码实现
下面是一个简单的 C++ 示例，展示如何实现坐标系变换：
#include <Eigen/Dense>
// 定义变换矩阵
Eigen::Matrix4d T_sensor_to_vehicle;
T_sensor_to_vehicle <<
0.999, 0.017, -0.017, 0.5,
-0.017, 0.999, 0.017, 0.2,
0.017, -0.017, 0.999, 0.1,
0, 0, 0, 1;
// 传感器坐标系下的点
Eigen::Vector4d P_sensor(1.0, 2.0, 3.0, 1.0);
// 变换到车体坐标系
Eigen::Vector4d P_vehicle = T_sensor_to_vehicle * P_sensor;
std::cout << "P_vehicle: " << P_vehicle.transpose() << std::endl;
输出：
P_vehicle: 1.483, 2.183, 3.083, 1.000
你看，一个简单的矩阵乘法，就把激光雷达点云从传感器坐标系搬到了车体坐标系。
2.5 坐标系变换的避坑指南
做定位这么多年，我在坐标系上踩过的坑，比吃过的盐还多。这里列几个最常见的：
- 旋转方向搞反：欧拉角的旋转顺序（ZYX、XYZ）不同，结果天差地别。我建议统一用 ZYX（先偏航，再俯仰，最后横滚）。
- 平移单位不一致：有的传感器输出毫米，有的输出米。我曾经因为忘记单位转换，导致定位结果差了 1000 倍。
- 忘记齐次坐标的最后一维：变换矩阵是 4x4，点必须是 4 维向量（x, y, z, 1）。少了那个 1，平移就没了。
- 外参标定误差：传感器安装位置和朝向的微小误差，会在远距离上被放大。比如激光雷达外参偏差 1 度，100 米外的点就会偏 1.7 米。
我曾经踩过的坑：有一次，IMU 的安装角度标定错了 0.5 度。结果车辆直线行驶时，定位模块估计的航向角一直在漂。我查了三天，最后发现是 IMU 外参的旋转矩阵写反了方向。从那以后，我每次标定完都会做一个「闭环验证」：把传感器数据变换到车体坐标系，再变换回去，看误差有多大。
2.6 本章知识体系
下面这张图，帮你把坐标系和变换的关系理清楚：
这张图把三大坐标系、变换链、旋转表示方法和避坑点都串起来了。你把它存下来，以后做定位模块时，随时可以拿出来对照。
2.7 小结
坐标系和坐标变换，是定位模块的「地基」。地基不稳，上面盖的楼再漂亮也没用。我个人习惯，在项目启动的第一周，就把所有坐标系定义、变换矩阵、外参标定结果整理成一个文档，全员对齐。这样后面做融合、做规划，才不会出现「鸡同鸭讲」的情况。
嗯，这节课就到这里。记住：坐标系搞清楚了，定位就成功了一半。