4. 惯性导航数据跳变的容错滤波处置

故障现象与判据

ROV在定深定向作业时，惯性导航组合（INS/DVL）输出的位置突然出现阶跃式跳变。典型表现为：DVL对底速度仍连续，但INS推算位置在1~2个控制周期内偏移数米，且与超短基线（USBL）声学定位、深度计、罗经航向出现明显不一致。此时不能直接采信INS位置，也不能立即中止作业，而应先做冗余交叉验证，判断是INS自身漂移、DVL失锁、USBL野值，还是推进器/机械臂动作引起的短时扰动。

判据可量化为：位置跳变量 Δp 超过阈值（例如 2.0 m/周期），同时检查三路冗余源的一致性残差。若DVL速度与INS速度残差小、USBL与INS位置残差大，则倾向INS积分异常；若DVL速度本身出现离群点，则优先怀疑DVL；若USBL残差大但DVL与INS一致，则可能是USBL多径或声学野值。

容错滤波与备用组合切换

识别出故障源后，处置核心是“冻结—隔离—重构—复核”。首先冻结INS位置通道的更新，保留其姿态与角速度输出，避免跳变继续污染控制回路；随后根据判定结果隔离故障源。若判定为INS积分漂移，则用DVL对底速度积分配合USBL绝对位置做松组合，深度计与罗经提供约束；若判定为DVL失锁，则临时以USBL+INS姿态航位推算为主，并降低水平推进器增益；若为USBL野值，则对USBL做滑窗中值滤波并剔除离群点。

滤波层面可采用简化的一维卡尔曼或滑动中值+限幅组合：对位置残差设置门限，超限时用预测值替代观测值，避免阶跃直接进入控制器。下面给出一个可运行的位置跳变检测与备用组合切换的Python示例，输入为教学示例数据，输出为每周期采用的导航源与滤波后位置。

import numpy as np

# 教学示例数据：每周期 [INS位置x, INS位置y, DVL速度x, DVL速度y, USBL位置x, USBL位置y]
# 单位：m, m/s；dt = 0.5 s
dt = 0.5
data = np.array([
    [0.0, 0.0, 0.10, 0.02, 0.05, -0.01],
    [0.05, 0.01, 0.10, 0.02, 0.10, 0.00],
    [0.10, 0.02, 0.10, 0.02, 0.15, 0.01],
    [5.20, 0.03, 0.10, 0.02, 0.20, 0.02],   # INS 跳变
    [5.25, 0.04, 0.10, 0.02, 0.25, 0.03],
    [5.30, 0.05, 0.10, 0.02, 0.30, 0.04],
])

JUMP_TH = 2.0      # 位置跳变阈值 m/周期
RES_TH  = 0.8      # 残差一致性阈值 m

def detect_and_switch(data, dt):
    pos_est = data[0, 0:2].copy()
    prev_ins = data[0, 0:2].copy()
    results = []
    for k in range(1, len(data)):
        ins = data[k, 0:2]
        dvl_v = data[k, 2:4]
        usbl = data[k, 4:6]

        dp = np.linalg.norm(ins - prev_ins)
        pred = pos_est + dvl_v * dt
        r_ins_usbl = np.linalg.norm(ins - usbl)
        r_pred_usbl = np.linalg.norm(pred - usbl)

        if dp > JUMP_TH and r_ins_usbl > RES_TH and r_pred_usbl < RES_TH:
            source = "INS跳变→备用组合"
            pos_est = pred + 0.3 * (usbl - pred)   # 松组合修正
        elif dp > JUMP_TH:
            source = "跳变待复核→限幅"
            pos_est = pred
        else:
            source = "INS主用"
            pos_est = ins

        prev_ins = ins.copy()
        results.append((k, round(dp, 2), source, np.round(pos_est, 2)))
    return results

for r in detect_and_switch(data, dt):
    print(f"周期{r[0]} 跳变量={r[1]}m 采用={r[2]} 滤波位置={r[3]}")

示例输出（由给定输入直接推出）：周期3跳变量约5.1 m，超过阈值且INS-USBL残差大、DVL预测-USBL残差小，判定为INS跳变，切换至备用组合；周期4、5继续用DVL速度积分并以USBL做松组合修正，位置平滑过渡，不再出现阶跃。实际作业中应结合ROV控制周期与USBL更新率调整门限，避免把正常机动误判为跳变。