4、定位模块参数调优：NDT匹配参数详解、ICP参数配置、EKF融合参数调整、定位精度评估方法
定位模块，说白了就是自动驾驶系统的「眼睛」和「大脑」的结合体。它得知道自己到底在哪儿，才能决定下一步怎么走。我做了这么多年自动驾驶，最头疼的就是定位出问题——车一偏，后面所有决策全乱套。
今天咱们重点聊聊NDT、ICP、EKF这三个核心模块。嗯，每个都有不少坑，我一个个说。
4.1 NDT匹配参数详解
NDT（正态分布变换）是目前用得最多的点云匹配方法。为什么？因为它快，而且对初值不敏感。我刚开始用的时候，觉得它就是个黑盒子，后来踩了坑才明白——参数调不好，再好的算法也白搭。
4.1.1 核心参数：体素分辨率
体素分辨率是NDT最重要的参数。它决定了你把点云空间切成多大的小格子。
| 参数名 | 推荐值 | 说明 |
|---|---|---|
| ndt_resolution | 1.0 - 2.0（米） | 体素边长，越小精度越高，但计算量越大 |
| ndt_step_size | 0.1 - 0.5（米） | 牛顿法迭代步长，影响收敛速度 |
| ndt_max_iterations | 30 - 50 | 最大迭代次数，防止死循环 |
我个人习惯把分辨率设在1.5米左右。为什么？太细了（比如0.5米），计算量翻好几倍，而且容易陷入局部最优。太粗了（比如3米），匹配精度不够，车跑起来会「抖」。
4.1.2 关键参数：输出概率阈值
这个参数很多人忽略，但它其实很关键。它决定了NDT认为「匹配成功」的最低置信度。
# Autoware NDT参数配置示例
ndt:
resolution: 1.5
step_size: 0.1
max_iterations: 30
epsilon: 0.01
output_prob_threshold: 0.7 # 低于此值认为匹配失败
你想想看，如果阈值设得太低（比如0.3），哪怕匹配得很差，系统也认为成功了。结果就是定位漂移，车越跑越偏。我曾经吃过这个亏——有一次在隧道里，NDT匹配得分只有0.4，但系统没报警，结果定位直接偏了2米多。
4.2 ICP参数配置
ICP（迭代最近点）是另一种经典的点云匹配方法。它比NDT慢，但在某些场景下精度更高。我一般把ICP作为NDT的补充——NDT做粗匹配，ICP做精匹配。
4.2.1 核心参数：最大对应距离
这个参数决定了两个点云中，什么样的点才算「对应点」。
| 参数名 | 推荐值 | 说明 |
|---|---|---|
| icp_max_correspondence_distance | 0.5 - 2.0（米） | 对应点最大距离，越小匹配越精确 |
| icp_transformation_epsilon | 1e-6 - 1e-8 | 变换矩阵收敛阈值 |
| icp_euclidean_fitness_epsilon | 0.01 - 0.1 | 欧氏距离收敛阈值 |
说白了，这个值设得越小，匹配越「挑剔」。但太挑剔了也不行——点云本身就有噪声，你非要找完全对应的点，那基本找不到。
4.2.2 迭代次数与收敛条件
ICP的迭代次数不是越多越好。我见过有人设100次迭代，结果每帧处理时间超过200ms，车都跑出去好几米了还没算完。
# ICP参数配置示例
icp:
max_correspondence_distance: 1.0
max_iterations: 30
transformation_epsilon: 1e-6
euclidean_fitness_epsilon: 0.05
use_reciprocal_correspondence: false # 是否使用双向对应
嗯，这里要注意：use_reciprocal_correspondence这个参数。如果设为true，匹配会更精确，但计算量翻倍。我一般只在低速场景（比如泊车）才开它。
4.3 EKF融合参数调整
EKF（扩展卡尔曼滤波）负责把NDT/ICP的定位结果、IMU、轮速计、GPS等传感器数据融合在一起。说白了，就是「大家投票，谁靠谱谁说了算」。
4.3.1 噪声协方差矩阵
这是EKF最核心的参数。它决定了每个传感器的「话语权」。
| 参数名 | 推荐值 | 说明 |
|---|---|---|
| process_noise_covariance | diag(0.01, 0.01, 0.01, 0.001) | 过程噪声，越大表示模型越不可信 |
| measurement_noise_covariance | diag(0.1, 0.1, 0.05) | 观测噪声，越大表示传感器越不可信 |
| initial_covariance | diag(1.0, 1.0, 0.1) | 初始协方差，表示初始不确定度 |
我个人习惯这样调：先让IMU和轮速计做短时预测（过程噪声设小一点），然后让NDT/ICP做长时修正（观测噪声设大一点）。为什么？因为IMU短时准，长时漂；NDT长时准，短时可能跳变。
4.3.2 融合频率与延迟补偿
不同传感器的频率不一样。IMU可能200Hz，NDT只有10Hz。怎么融合？
# EKF融合参数示例
ekf:
frequency: 50 # EKF更新频率
predict_only_on_input: false
use_differential: true
delay_compensation: 0.05 # 延迟补偿时间（秒）
你想想看，如果NDT处理耗时100ms，那它输出的位置其实是100ms前的。如果不做延迟补偿，EKF融合的就是「过时」的数据。我建议把delay_compensation设成NDT平均处理时间的一半左右。
4.4 定位精度评估方法
调了半天参数，怎么知道效果好不好？不能光靠「感觉」。我总结了一套评估方法，分享给你。
4.4.1 绝对精度评估
用高精度RTK-GPS作为真值，对比定位模块的输出。
- 均方根误差（RMSE）： 最常用的指标，反映整体偏差
- 最大误差： 看最差情况，比如过隧道时
- 95%分位误差： 排除极端情况后的典型误差
我一般要求：城区场景RMSE小于0.3米，高速场景小于0.5米。如果超过这个范围，说明参数还有问题。
4.4.2 相对精度评估
绝对精度需要真值，但很多时候没有。这时候看相对精度——相邻两帧之间的位移误差。
# 相对精度评估脚本示例
def evaluate_relative_accuracy(estimated_poses, ground_truth_poses):
errors = []
for i in range(1, len(estimated_poses)):
est_delta = estimated_poses[i] - estimated_poses[i-1]
gt_delta = ground_truth_poses[i] - ground_truth_poses[i-1]
error = np.linalg.norm(est_delta - gt_delta)
errors.append(error)
return np.mean(errors), np.std(errors)
说白了，就是看你的定位模块在短时间内的「自洽性」。如果相邻两帧的位移误差很大，说明定位在「跳变」。
4.4.3 可视化评估
除了数值指标，可视化也很重要。我习惯把定位轨迹叠加到地图上，看看有没有「穿墙」或「漂移」。
- 轨迹叠加： 看定位轨迹是否平滑，有没有突变
- 点云匹配可视化： 看NDT/ICP匹配后的点云是否对齐
- 协方差椭圆： 看EKF输出的不确定度是否合理
嗯，这里有个小技巧——如果协方差椭圆突然变大，说明EKF对当前定位没信心。这时候可以触发重定位或者降级策略。
好了，定位模块的参数调优就聊到这儿。记住，参数是死的，场景是活的。多跑数据，多分析，慢慢就能找到感觉。