3、漂移检测：基于残差分析、速度突变检测、以及NDT匹配分数的漂移触发机制
漂移检测这件事，说白了就是回答一个问题：“现在这个位姿，我到底该不该信？”
我做了这么多年定位，见过太多“看起来很美”的轨迹。明明已经飘到姥姥家了，算法还在那儿自信满满地输出位姿。嗯，这其实不怪算法，怪我们没给它装一个“警报器”。
今天我们就来聊聊这个警报器怎么装。我个人习惯用三种手段联合判断：残差分析、速度突变检测、以及NDT匹配分数。三管齐下，基本能覆盖90%以上的漂移场景。
3.1 残差分析：最朴素的“直觉”
残差是什么？就是你当前观测和地图之间的“不一致程度”。
举个例子。你明明看到前方有一堵墙，但根据你的位姿推算，那堵墙应该在身后5米处。这就叫残差大。
我在项目中遇到过一种情况：激光雷达扫到一面玻璃墙，点云反射率极低，导致匹配残差突然飙升。但位姿其实没飘，只是传感器数据质量下降了。所以残差分析不能只看绝对值，还得看变化趋势。
具体实现上，我一般这样写：
// 伪代码：残差分析
float computeResidual(const PointCloud& scan, const Pose& pose, const Map& map) {
float total_error = 0.0f;
int valid_points = 0;
for (auto& pt : scan.points) {
Point3D transformed = pose * pt;
Point3D nearest = map.findNearest(transformed);
float dist = (transformed - nearest).norm();
// 过滤掉太远的点（可能是动态物体）
if (dist < 5.0f) {
total_error += dist;
valid_points++;
}
}
return total_error / valid_points;
}
3.2 速度突变检测：抓住“不自然”的瞬间
你想想看，一个正常行驶的机器人，速度变化应该是平滑的。突然从0.5m/s跳到2.0m/s，然后又跌回0.3m/s——这合理吗？
不合理。这大概率是位姿跳变导致的“伪速度”。
我曾经在一个AGV项目上吃过这个亏。机器人明明在直行，定位结果却显示它在原地转圈。后来一查，是轮子打滑导致里程计积分出错，位姿瞬间跳了半米。速度突变检测就是用来抓这种“瞬间异常”的。
具体做法很简单：
- 计算瞬时速度： 当前帧位姿与上一帧位姿的位移 / 时间差
- 与历史速度比较： 用卡尔曼滤波或滑动平均估计“期望速度”
- 判断突变： 如果瞬时速度与期望速度的偏差超过3倍标准差，触发警告
3.3 NDT匹配分数：最直接的“信心指数”
NDT匹配分数，说白了就是当前帧点云和地图“长得像不像”。
分数越高，说明匹配越好。分数突然掉下来，说明要么环境变了，要么位姿飘了。
我习惯用NDT的似然概率作为匹配分数。这个值通常在0到1之间，0.8以上算优秀，0.5以下就要警惕了。
| 匹配分数范围 | 状态判断 | 建议操作 |
|---|---|---|
| 0.8 ~ 1.0 | 优秀 | 正常定位，无需干预 |
| 0.5 ~ 0.8 | 一般 | 记录日志，持续观察 |
| 0.3 ~ 0.5 | 较差 | 触发漂移检测，准备重对齐 |
| < 0.3 | 极差 | 立即触发重对齐，停止运动 |
3.4 三合一触发机制：怎么联合决策？
单独用任何一种方法都有缺陷。我的做法是加权投票：
- 残差分析：权重0.3
- 速度突变：权重0.3
- NDT分数：权重0.4（因为最直接）
当综合得分超过阈值时，触发漂移重对齐流程。
下面是我画的一张流程图，帮你理清整个逻辑：
嗯，这套机制我在多个项目里验证过。最让我印象深刻的是在某个地下车库场景——GPS信号全无，全靠激光雷达。有一次机器人经过一片全是镜面柱子的区域，NDT分数直接掉到0.2，但残差分析和速度突变都正常。最后综合判断下来，是环境特征丢失，不是真漂移。我们选择不触发重对齐，机器人顺利通过了那片区域。
你看，这就是三合一的好处——互相验证，减少误报。