3、定位与地图模块：GNSS+IMU组合导航、高精地图格式与更新、多传感器融合定位
定位与地图模块，说白了就是自动驾驶系统的「眼睛」和「记忆」。没有它，车连自己在哪都不知道，更别提规划路径了。我这些年做过的项目里，因为定位不准出的bug，比传感器本身的问题还多。今天咱们就聊聊这个模块的核心三件套：组合导航、高精地图、多传感器融合。
3.1 GNSS+IMU组合导航：别让GPS骗了你
很多人觉得GPS定位很简单，手机导航不也能用吗？嗯，这里要注意——手机导航的精度是米级的，自动驾驶需要的是厘米级。而且城市里高楼林立，GPS信号反射、遮挡是家常便饭。
我个人习惯用松耦合的方式做GNSS+IMU融合。说白了就是：GNSS提供绝对位置，IMU提供短时高精度的相对位移。两者互补，缺一不可。
核心思路：卡尔曼滤波（EKF）是组合导航的标配。状态量包括位置、速度、姿态、陀螺仪零偏、加速度计零偏。观测量是GNSS的位置和速度。
我在项目中遇到过一个问题：IMU的零偏会随时间漂移，如果不实时估计，几分钟后位置误差就能到几十米。解决办法是在卡尔曼滤波里把零偏也作为状态量，每次GNSS更新时一起修正。
// 简化的EKF状态预测（IMU驱动）
void predictState(double dt, const ImuData& imu) {
// 姿态更新：四元数微分方程
Eigen::Quaterniond q_dot = 0.5 * q * Eigen::Quaterniond(0, imu.gyro.x(), imu.gyro.y(), imu.gyro.z());
q += q_dot * dt;
q.normalize();
// 位置更新：加速度二次积分
Eigen::Vector3d acc_world = q * imu.accel; // 转到世界坐标系
acc_world -= Eigen::Vector3d(0, 0, 9.81); // 去掉重力
vel += acc_world * dt;
pos += vel * dt + 0.5 * acc_world * dt * dt;
}避坑指南：我曾经在隧道场景里吃过亏。GNSS信号丢失后，纯靠IMU积分，误差累积得飞快。后来我加了轮速计辅助，用轮速约束纵向速度，IMU只负责姿态和横向，效果好了很多。
3.2 高精地图（HD Map）：不只是导航地图
高精地图和普通导航地图的区别，就像工程图纸和手绘草图。HD Map包含车道线、路沿、交通标志、甚至每个路灯的精确位置。精度要求是10厘米以内。
目前主流的格式有两种：
| 格式 | 特点 | 典型应用 |
|---|---|---|
| OpenDRIVE | 基于XML，描述道路拓扑和几何 | 仿真测试、路径规划 |
| NDS（NaviData Standard） | 二进制格式，压缩率高，支持增量更新 | 量产车机导航 |
| Lanelet2 | 基于图结构，适合局部路径规划 | Autoware等开源项目 |
我个人更推荐Lanelet2，因为它把车道和连接关系建模成图，做路径规划时直接跑A*算法就行，不用再转换格式。你想想看，如果每次规划都要解析一遍OpenDRIVE的XML，性能开销得多大？
3.3 高精地图更新：别指望一次建好管三年
道路是会变的。修路、施工、临时改道，这些情况在量产车上天天遇到。高精地图的更新策略，我总结为三层：
- 云端更新：每天或每周下发增量包，覆盖大范围变化
- 车端实时更新：通过感知模块检测到的变化（比如施工锥桶），临时修正局部地图
- 众包更新：多辆车上传观测数据，云端融合后生成新版本
注意：我曾经在项目里只做了云端更新，结果遇到临时施工，车直接按旧地图规划，差点撞上围挡。后来我加了车端实时更新模块，用视觉检测到的车道线变化来修正局部地图，才算解决了这个问题。
3.4 多传感器融合定位：点云配准与视觉SLAM
GNSS+IMU组合导航在开阔场景下够用，但到了地下车库、隧道、高架桥下，就得靠其他传感器了。我常用的方案是：
- 激光雷达点云配准（ICP/NICP）：精度高，但计算量大，适合低速场景
- 视觉SLAM（ORB-SLAM3/VINS-Fusion）：成本低，但受光照影响大
- 多传感器融合：用因子图优化，把GNSS、IMU、激光、视觉的观测统一建模
点云配准的核心是ICP算法。说白了就是：找两帧点云之间的旋转和平移，让它们尽可能重合。我建议用NICP（Normal ICP），它不光匹配点位置，还匹配法向量，收敛更快。
// NICP配准核心步骤
Eigen::Matrix4f nicpRegistration(
const pcl::PointCloud<pcl::PointNormal>& source,
const pcl::PointCloud<pcl::PointNormal>& target) {
// 1. 构建KD树加速最近邻搜索
pcl::KdTreeFLANN<pcl::PointNormal> kdtree;
kdtree.setInputCloud(target.makeShared());
// 2. 迭代优化：最小化点-面距离 + 法向量误差
for (int iter = 0; iter < 30; ++iter) {
// 对每个源点，找最近邻目标点
// 构建误差函数：E = sum( (R*p_i + t - q_j)^T * n_j )^2
// 用高斯-牛顿法求解R,t
}
return transformation;
}经验之谈：视觉SLAM在纹理丰富的场景下表现很好，但遇到白墙、长走廊这种弱纹理环境，很容易丢。我建议把视觉SLAM和IMU紧耦合，用IMU的短时积分来弥补视觉的不足。VINS-Fusion就是个不错的开源方案。
3.5 融合定位的工程落地要点
理论讲完了，说说实际落地时我踩过的坑：
- 时间同步：不同传感器的时钟必须统一。我习惯用PTP（精确时间协议）同步所有传感器，精度到微秒级。否则融合出来的位置是「错位」的。
- 坐标系转换：IMU坐标系、车身坐标系、世界坐标系，三者之间的转换矩阵必须标定准确。我曾经因为IMU安装角度标偏了0.5度，导致定位误差在100米外累积到1米多。
- 异常检测：GNSS可能出现跳变（多路径效应），激光雷达可能被雨雪干扰。我建议加一个异常检测模块，用卡方检验判断观测值是否可信，不可信的就直接丢弃。
- 回环检测：视觉SLAM和激光SLAM都依赖回环来消除累积误差。我建议在场景中预设一些「地标」，比如特定的交通标志或路灯，每次经过时做一次绝对位置修正。
总结一下：定位与地图模块没有银弹。GNSS+IMU是基础，高精地图提供先验信息，多传感器融合解决极端场景。我建议在量产项目中采用「松耦合+紧耦合」混合架构：正常情况下用松耦合保证实时性，遇到GNSS失效时切换到紧耦合模式，用激光/视觉做局部定位。
嗯，今天就聊到这儿。下一章咱们讲感知模块，聊聊怎么让车「看懂」周围的世界。