1. 课程导入
在自动驾驶、移动机器人、无人机以及高精度测绘装备中，定位系统是整个感知—决策—控制链路的“地基”。一旦定位输出发生跳变、漂移、冻结或完全丢失，上层规划与控制就会在错误的位置假设下运行，轻则轨迹抖动、任务失败，重则引发碰撞与安全事故。本课程围绕定位失效检测与冗余切换两条主线，讲解如何让系统在定位异常时“看得见、判得准、切得快、稳得住”。
一、核心概念：什么是“定位失效”
定位失效并不等同于“定位模块断电”。在工程实践中，它更多表现为输出仍然存在但不可信。常见形态包括：位置缓慢漂移、姿态角突变、协方差被错误放大或缩小、时间戳停滞、坐标系错配、以及多传感器融合后出现的“隐性发散”。因此，失效检测的核心不是判断“有没有数据”，而是判断“数据是否与真实运动状态一致”。
与之对应，冗余切换也不是简单地“主备切换”。它包含三个层次：冗余源（GNSS、IMU、轮速计、激光雷达、视觉、UWB等）、冗余路径（不同融合算法或不同观测组合）、冗余决策（仲裁器、健康管理模块）。只有三者协同，才能在单点故障时保持定位连续可用。
二、整体知识框架
本课程的知识框架可以概括为“一条主线、四个环节、两类指标”。一条主线是从异常感知到安全切换的闭环；四个环节分别是：失效建模、检测判据、冗余仲裁、切换验证；两类指标是检测实时性与切换平顺性。下图展示了该框架中各模块之间的数据流与信号关系。
三、方法流程：从异常到切换的闭环
工程上可落地的流程通常分为五步。第一步是建立基准：通过标定与离线回放，得到各传感器在正常工况下的残差分布与协方差范围。第二步是在线监测：对每一帧观测计算新息、卡方统计量、多源位置差与时间戳连续性。第三步是故障判定：结合阈值、滑窗计数与状态机，区分瞬时噪声与持续失效。第四步是仲裁切换：按优先级与健康度选择融合源，必要时隔离故障源并重新初始化。第五步是验证与复盘：记录切换前后轨迹、协方差与标志位，用于回归测试与参数调优。
# 典型健康监测伪代码
for each frame:
innovation = z_gnss - h(x_pred)
chi2 = innovation.T @ S.inverse() @ innovation
if chi2 > chi2_threshold:
fault_counter += 1
else:
fault_counter = max(0, fault_counter - 1)
if fault_counter > N_confirm:
health[gnss] = FAULT
trigger_arbitration()
四、案例与场景：城市峡谷中的GNSS跳变
某园区配送机器人行驶至楼宇密集区时，GNSS出现约12米的位置跳变，但数据并未中断。若直接信任GNSS，机器人会误判自身进入非机动车道。引入失效检测后，系统发现GNSS新息卡方值连续5帧超限，同时与激光雷达里程计的位置差超过阈值，于是将GNSS健康度降级，切换至“激光雷达+IMU+轮速计”的短时航位推算，并在驶出遮挡区后重新引入GNSS。整个过程定位输出未发生明显跳变，任务得以继续。
| 失效模式 | 典型表现 | 检测手段 | 切换策略 |
|---|---|---|---|
| 缓慢漂移 | 位置逐渐偏离，协方差偏小 | 多源一致性、残差趋势 | 降权、切换至里程计 |
| 突变跳变 | 单帧位置大幅偏移 | 新息卡方、速度一致性 | 隔离该源、保持航位推算 |
| 数据冻结 | 时间戳停滞、输出不变 | 时间戳监测、心跳检测 | 立即切换备用源 |
| 坐标系错配 | 整体偏移或旋转 | 外参校验、地图匹配 | 重新标定、降级运行 |
五、常见误区与复盘要点
复盘时应重点关注三个问题：失效是否被及时检出？切换是否平顺且可解释？系统是否具备降级与恢复能力？只有把检测、仲裁、切换、验证串成闭环，定位系统才能在真实复杂环境中保持鲁棒。本课程后续将围绕这些环节逐一展开，帮助你把“定位失效检测与冗余切换”从概念变成可落地的工程能力。