4、坐标系对齐：雷达坐标系与地图坐标系的转换关系
坐标系对齐，说白了就是让雷达看到的世界和地图上画的世界是同一个世界。我刚开始做融合定位时，觉得这步不就是个矩阵乘法嘛，结果第一次实车测试就翻车了——车明明在路中间，地图上却显示我在绿化带里飞驰。嗯，从那以后我再也不敢小看坐标系转换了。
4.1 为什么需要坐标系对齐？
毫米波雷达输出的目标位置，是在雷达自身坐标系下的。比如它告诉你「前方 50 米处有一个静止目标」，但这个「前方」是相对于车头方向的。而高精地图用的是全局坐标系，通常是经纬度或者高斯平面坐标。
你想想看，如果这两个坐标系不统一，雷达检测到的障碍物和地图上的车道线、路沿就永远对不上。我见过一个项目，就是因为坐标系转换参数标定错了，导致融合后的定位结果一直在漂移，排查了整整三天才发现是旋转矩阵写反了。
核心问题：雷达坐标系（局部）→ 车辆坐标系 → 全局坐标系（地图）。中间涉及平移、旋转、尺度缩放，一步错步步错。
4.2 三个关键坐标系
做坐标系对齐之前，先把三个坐标系搞清楚。我习惯把它们画在纸上，每次做转换前都看一眼，防止搞混。
4.2.1 雷达坐标系
- 原点：雷达几何中心
- X轴：雷达正前方
- Y轴：雷达左侧（右手系）
- Z轴：垂直向上
注意，不同雷达厂商的定义可能不一样。我遇到过某款雷达把Y轴定义为向右，结果转换矩阵里符号全反了。所以拿到雷达数据后，第一件事就是确认坐标系定义文档。
4.2.2 车辆坐标系
- 原点：车辆后轴中心（或者质心，看车型）
- X轴：车辆前进方向
- Y轴：车辆左侧
- Z轴：垂直向上
这里有个坑：有些车把原点放在前保险杠中心。我曾经因为这个偏差，导致雷达检测到的近处目标位置差了 0.3 米。0.3 米在高速上可能就是生与死的距离。
4.2.3 地图坐标系
- 常用 WGS-84 经纬度，或者高斯-克吕格投影后的平面坐标
- 高精地图一般用 ENU（东-北-天）坐标系
- 原点：通常是地图区域的某个固定参考点
4.3 转换关系与数学表达
坐标系转换本质上就是刚体变换：旋转 + 平移。公式很简单，但实际用起来全是细节。
// 雷达坐标系 → 车辆坐标系
P_vehicle = R_radar_to_vehicle * P_radar + T_radar_to_vehicle
// 车辆坐标系 → 地图坐标系
P_map = R_vehicle_to_map * P_vehicle + T_vehicle_to_map其中 R 是 3x3 旋转矩阵，T 是 3x1 平移向量。我建议用齐次坐标统一表示，这样一次矩阵乘法就能搞定，代码也干净。
我的习惯：把所有转换写成 4x4 齐次变换矩阵。调试时打印矩阵出来，一眼就能看出旋转部分是不是单位阵、平移量是不是合理。我曾经靠这个习惯，十分钟就定位到了标定参数加载错误的问题。
4.4 标定与参数获取
转换矩阵的参数怎么来？三种方式：
- 厂家提供：雷达安装位置和角度，理论上可以直接量。但实际安装总有误差，我见过标称 0 度安装角，实际偏了 2 度的案例。
- 手动标定：用已知位置的目标，反算转换参数。比如把车停在标定场地，用雷达检测几个角反射器，再和地图上的位置做匹配。
- 在线估计：利用 SLAM 或者滤波方法，在运行过程中实时优化转换参数。这个难度大，但精度高。
避坑指南：我曾经在标定时只用了 3 个点，结果解出来的旋转矩阵是奇异的。后来我改成至少用 6 个点，并且分布要均匀——不要全集中在车前方，左右两侧也要有。否则标定出来的参数在边缘区域误差会很大。
4.5 实战中的常见问题
说几个我踩过的坑，你们遇到时可以少走弯路：
- 时间戳不同步：雷达数据和地图数据的时间戳没对齐，导致用当前时刻的雷达数据去匹配上一时刻的地图位置。解决办法是加一个时间同步模块，或者用插值。
- 坐标系定义混淆：有人把雷达坐标系里的 Y 轴正方向搞反了，结果转换后目标全部镜像。我建议在代码里加一个坐标系可视化函数，把转换后的点云画出来，一眼就能看出对不对。
- 精度损失：浮点数运算会有精度损失，尤其是在经纬度转平面坐标时。我一般用 double 类型，不用 float。
4.6 知识体系图
下面这张图是我自己总结的坐标系对齐流程，每次做新项目都会拿出来对照一遍：
4.7 代码实现片段
下面是我在实际项目中用的转换函数，去掉了业务逻辑，保留了核心部分：
#include <Eigen/Dense>
// 雷达到车辆的变换矩阵（通过标定获得）
Eigen::Matrix4d T_radar_to_vehicle;
// 车辆到地图的变换矩阵（通过定位获得）
Eigen::Matrix4d T_vehicle_to_map;
Eigen::Vector3d radar_to_map(const Eigen::Vector3d& p_radar) {
// 转齐次坐标
Eigen::Vector4d p_radar_h;
p_radar_h << p_radar, 1.0;
// 链式变换
Eigen::Vector4d p_map_h = T_vehicle_to_map * T_radar_to_vehicle * p_radar_h;
// 转回三维坐标
return p_map_h.head<3>() / p_map_h(3);
}这段代码看起来简单，但我建议你在实际使用时加上矩阵合法性检查。比如检查旋转部分的行列式是不是接近 1，平移量是不是在合理范围内。我曾经因为标定文件读取错误，矩阵里全是 NaN，结果程序跑了一个小时才发现。
调试技巧：在转换前后分别打印几个已知点的坐标。比如雷达原点 (0,0,0) 转换后应该对应车辆上的雷达安装位置。如果这个点都不对，那后面的转换全白搭。
坐标系对齐是整个匹配流程的基石。我见过太多项目因为这一步没做好，后面花了几倍的时间去排查。所以我的建议是：花 80% 的精力把坐标系对齐搞扎实，后面 20% 的融合工作会顺畅很多。
可可精品资料库