3. DVL失锁时的导航状态降级策略验证

降级策略验证的仿真配置与判据

本例在仿真环境中复现DVL底跟踪失锁，验证导航系统从组合导航（INS/DVL）降级为纯惯导（INS-only）或航位推算（DR）后的误差增长特性，并据此判断是否触发任务中断阈值。核心思路是：先建立可复现的失锁注入点，再配置降级切换逻辑，最后用位置误差增长速率与阈值比较得出中断结论。

仿真参数设定如下（示例假设值，非实测）：AUV 巡航速度 1.5 m/s，DVL 正常输出频率 1 Hz，底跟踪有效高度上限 120 m；INS 陀螺零偏 0.02 °/h，加速度计零偏 50 µg；组合导航位置误差约 2 m（1σ）。任务中断阈值设为水平位置误差 50 m 或推算航程超过 300 s 未恢复。失锁注入时刻 t=600 s，持续至仿真结束。

失锁注入与降级切换的实现要点

仿真中需要把“DVL 是否有效”做成一个可切换的布尔量，并让导航滤波器在无效时停止速度观测更新。下面给出一个最小可运行的 Python 仿真片段，用一维航迹近似演示误差增长，便于快速验证降级逻辑与阈值判断（示例参数，结果由给定输入直接推出，非实测）。

import math

# 示例参数（假设值）
dt = 1.0                 # 仿真步长 s
T = 900.0                # 总时长 s
v = 1.5                  # 巡航速度 m/s
t_lock = 600.0           # DVL 失锁时刻 s
err_comb = 2.0           # 组合导航位置误差 m
gyro_bias = 0.02/3600    # 陀螺零偏 rad/s
acc_bias = 50e-6*9.81    # 加速度计零偏 m/s^2
thr_pos = 50.0           # 位置误差阈值 m
thr_time = 300.0         # 失锁持续阈值 s

def run(mode):
    t, pos_err, v_err = 0.0, err_comb, 0.0
    t_unlock = None
    while t < T:
        dvl_ok = t < t_lock
        if not dvl_ok and t_unlock is None:
            t_unlock = t
        if dvl_ok:
            pos_err = err_comb
            v_err = 0.0
        else:
            # 降级：误差按惯性/推算模型增长
            if mode == "INS":
                v_err += acc_bias*dt
                pos_err += v_err*dt + 0.5*acc_bias*dt*dt
                pos_err += v*gyro_bias*dt*10  # 航向误差耦合的简化项
            else:  # DR
                pos_err += v*gyro_bias*dt*20
        if pos_err >= thr_pos:
            return t, pos_err, t_unlock
        t += dt
    return t, pos_err, t_unlock

for m in ("INS", "DR"):
    t_hit, e, tu = run(m)
    dur = t_hit - tu if tu is not None else 0
    print(f"{m}: 达阈值时刻={t_hit:.0f}s, 误差={e:.1f}m, 失锁持续={dur:.0f}s, "
          f"中断={'是' if (e>=thr_pos or dur>=thr_time) else '否'}")

运行后应观察到：INS-only 因加速度计零偏二次积分，误差增长更快；DR 依赖航向精度，误差近似线性增长。若任一模式在失锁后 300 s 内位置误差达到 50 m，或失锁持续超过 300 s 仍未恢复，则判定触发任务中断。实际工程中还需把 DVL 有效高度、回波质量、波束一致性作为失锁判据，而不是仅看单帧丢失。

验证记录表（可直接填写）