简介一份以MATLAB脚本呈现的AUV协同定位与故障检测算法实现面向水下机器人定位算法研究者及海洋探测开发工程师用于解决多AUV协同导航中的定位误差累积和异常量测识别问题。压缩包仅含1个m文件大小约3KB脚本涵盖系统动态建模、传感器数据融合、相互定位更新及故障诊断等核心逻辑可实现交替领航模式下的集体定位优化。目前已有200人学习下载适合快速上手理解和验证算法。通过阅读该脚本读者能掌握协同定位算法的流程与关键参数设置理解如何利用统计或滤波方法对异常量测进行检测为后续在复杂水下环境中的定位方案设计提供实用参考。1. AUV 协同定位为什么绕不开故障检测一个故障节点能拖垮整片编队多台 AUV 在水下编队执行海底测绘或目标搜索时惯导误差会随时间持续累积GPS 信号又完全不可用最常见的补救办法是让编队成员通过水声测距互相校准位置这就是标题里的 AUV 协同定位。这套代号为 XT_GZJC 的方案把故障检测和协同定位算法放在同一层而不是当成事后救火的补丁。原因很直接水下传感器的工作环境远比陆地恶劣一旦某台 AUV 的测距声呐或深度计故障它向编队广播的是错误量测融合滤波器没有分辨能力会把错误当真实观测吸收进去最终整个编队的定位误差从几米扩散到几十米。这个方向真正要解决的问题就是让协同定位系统在成员带病工作时依然维持可信定位。适合读者是正在做多水下机器人协同导航、水下组网定位的工程师和研究生。2. 协同定位算法的骨架相对量测模型与三种主流融合结构2.1 相对量测模型测距、方位角与速度一致性哪种信息最可靠协同定位能工作靠的是编队成员之间有可交换的相对观测。水下可用的相对观测分三类水声测距、水声方位角、相对速度或速度一致性信息。工程上用得最多、也最值得信任的是测距。测距的数学模型很直观第 i 台 AUV 和第 j 台 AUV 之间的测距量测写成z_ij || p_i - p_j || v_ij其中 p_i、p_j 是两台 AUV 的位置向量v_ij 是量测噪声通常建模为零均值高斯分布。这里有个容易被新手忽略的点模型里用的是两台 AUV 的位置差而位置来自各自的惯导推算推算误差本身又相关。所以测距残差里既有量测噪声也有双方位置估计误差在径向上的投影这直接影响后面故障检测门限的设计。方位角量测理论上能提供比测距更强的约束一个测距只约束距离方位角还能约束方向。但实际水下场景里方位角来自声学基阵安装角误差、声线弯曲、多径效应都会让方位角带明显系统偏差可靠性远不如测距。速度一致性信息只提供弱约束一般用来做辅助校验比如判断某台 AUV 是否真的在运动而不是直接参与定位更新。我的建议是以测距为主量测、以方位角为辅、用速度信息做运动一致性校验。这套搭配在 XT_GZJC 这类协同定位算法里基本是标配。选测距还有一个工程理由水声测距设备技术成熟误差模型相对干净故障检测更容易从残差里看出异常。测距的更新周期通常在 210 秒这个节奏也刚好匹配故障检测滑动窗口的时间尺度。2.2 集中式、分布式、主从式三种结构在故障场景下的差异确定了量测类型下一步是决定数据怎么汇聚、在哪里融合。常见架构有三种集中式、分布式、主从式。集中式是所有测距量测通过水声通信发到某台中心节点由中心统一的滤波器完成全部融合再把结果广播回去。优点是只有一个滤波器状态一致性好故障检测逻辑集中最容易实现。缺点是通信瓶颈水声信道带宽极小编队大了以后数据排队导致延迟另外中心节点自身是单点它出故障整个系统失去融合能力。分布式是每台 AUV 只跟能通信的邻居交换量测各自本地维护一个滤波器。优点是鲁棒性好单台掉线不影响别人缺点是各节点由于量测到达时间不同对同一台 AUV 的状态估计会有差异故障检测的判定结果也可能不一致——这个问题放在后面的避坑章详细说。主从式介于两者之间一台主 AUV 携带更精密的设备若干从 AUV 以主节点播报的位置为基准做跟随修正。从节点实现简单但主节点的位置质量直接决定全队质量主节点故障对全队是灾难性的。因此主从式方案里故障检测必须优先布置在主节点侧。架构融合位置故障影响范围状态一致性适用规模集中式中心节点中心故障全停好35 台分布式各节点本地单点故障影响局部弱10 台以上主从式从节点跟随修正主故障全队降级中5 台左右选择上编队规模在 35 台且通信拓扑固定我一般选集中式方便把故障检测做完整编队规模大、任务周期长、允许成员临时掉线就用分布式故障检测做成本地检测加邻居投票的形式。XT_GZJC 把协同定位和故障检测并列实际上无论选哪种架构检测模块的位置都要在设计之初确定集中式放在融合器入口分布式放在每个节点本地滤波器入口主从式在主节点入口再加一道独立校验。2.3 从 EKF 到因子图协同定位算法的精度与计算量取舍结构定下来之后融合算法本身有得选。最常用的是扩展卡尔曼滤波EKF。EKF 把状态预测和量测更新拆成两个清晰的阶段残差、新息协方差全都现成故障检测需要的卡方检验量可以直接从滤波器内部拿这是它最大的工程优势。缺点是测距方程非线性线性化误差在近距离、强机动时比较明显。粒子滤波不依赖线性化能处理强非线性精度高但粒子数量一上来计算量和功耗在水下平台上非常敏感。AUV 的嵌入式板卡资源有限、电池有限长时任务下粒子滤波往往撑不住。当前多 AUV 协同导航里更受认可的做法是因子图加非线性优化。把每个时刻的位置当成变量节点把测距、惯导推算、深度计约束当成因子节点用滑窗内的历史约束一起优化。精度比 EKF 高也方便加入异步量测但计算量和内存占用是三者里最大的通常只在中心节点或主节点上运行。选型建议起步用 EKF把协同定位和故障检测的闭环跑通等精度不够、要处理异步水声量测时再迁到因子图框架。不要一上来就上因子图故障检测的阈值标定、误检调参在 EKF 里链路更短调起来更快。这个顺序和很多做水下导航团队的真实路线一致先用简单模型验证明白再逐步加复杂度。3. 把故障检测嵌进协同定位残差卡方、一致性检验与量测门的实现路径3.1 残差卡方检验检测量测异常的第一道闸故障检测在协同定位里最经典的入口是融合滤波器的新息。在正常 EKF 流程里每条外部量测进入更新步之前都会计算新息r_k z_k - h(x_pred)在模型准确、噪声高斯的前提下r_k 服从零均值高斯分布协方差就是新息协方差 S_k H P_pred H^T R。把马氏距离 m r^T S^-1 r 作为统计量它服从自由度等于量测维数的卡方分布。检测逻辑就是给定置信度 alpha算一个门限超过门限就认为这条量测异常。import numpy as np from scipy.stats import chi2 def chi2_gating(innovation, innovation_cov, dof, alpha0.01): 卡方门限检验返回是否通过、马氏距离、门限值。 innovation: 新息向量 (n,) innovation_cov: 新息协方差 (n,n) dof: 自由度标量测距取 1 alpha: 显著性水平越小门限越宽 m2 innovation.T np.linalg.inv(innovation_cov) innovation gate chi2.ppf(1 - alpha, dfdof) return m2 gate, m2, gate这段代码是故障检测的第一道闸。注意三个参数dof 取 1因为纯测距是标量量测alpha 取 0.01 而不是 0.05因为水下测距的误差模型并不干净门限太紧会把正常量测误杀工程上宁松勿紧innovation_cov 里 R 的取值要和实际设备噪声一致R 设小了门限自动收窄误检率立即飙升。函数返回的 m2 要做日志记录后面滑动窗口统计要用。3.2 一致性检验与滑动窗口区分瞬时野值和持续故障单次卡方门限只能挡住野值。水声通信里瞬时干扰很常见一次超限不代表声呐坏了持续故障的判定要靠滑动窗口。常见做法是维护长度 N 的窗口记录每条量测是否通过门限窗口内累计超限比例越过阈值时才把对应节点标记为疑似故障。from collections import deque class FaultDetector: def __init__(self, window20, fail_ratio0.6): self.window window self.fail_ratio fail_ratio self.history deque(maxlenwindow) def push(self, passed): 每条量测通过门限时 push(True)超限时 push(False) self.history.append(passed) if len(self.history) self.window: return False fail_count self.window - sum(self.history) return (fail_count / self.window) self.fail_ratio参数上window 取 20、fail_ratio 取 0.6 是常用的起始点。水声测距更新周期通常是 210 秒一次20 个样本对应约 40 秒到 200 秒的观察窗口。窗口太短会把一次通信丢包误判成故障太长会让故障持续污染定位几十秒才被隔离编队可能已经偏出去很远。除了单节点自身的窗口统计编队里还能做闭合差校验三条测距两两构成三角形三边应满足几何闭合闭合差超过阈值就说明至少有一条测距有问题再结合哪条边残差大来定位故障源。3.3 故障隔离与权重降级检测到之后怎么办检测只是第一步确诊之后动作要分级。我一般分三级第一次超限只丢弃当前量测不修改任何状态滑动窗口标记疑似后把该节点所有量测的 R 矩阵放大 10 倍让融合贡献大幅下降但不完全切断连续若干窗口仍判定故障才把它从通信拓扑里摘除。def apply_fault_mitigation(detector, node_id, chi2_ok, meas_noise_R, topo): # 三级处置: 瞬时超限丢弃; 疑似降权; 确诊摘除 hit detector.push(chi2_ok) if not hit: return drop if detector.probable(node_id): return deweight, meas_noise_R * 10 if detector.confirmed(node_id): topo.remove(node_id) return isolate return normal这段代码只为说明处置逻辑实际工程里 isolate 不只是从拓扑表移除还要把该节点之前的量测从滑窗里清掉并保留一份历史状态估计避免重新接入时滤波器状态跳变。这里最容易犯的错误是直接清零该节点的位置估计导致它重新入网时和编队其他节点的位置差巨大第一次测距就被门限拒绝形成进不了网的死循环。正确做法是把它最后可信的位置作为初始值并在重新接入的首个量测周期把 R 设大一些。4. 协同定位与故障检测的融合实现一个最小可跑的 Python 示例4.1 系统状态与协同观测数据流把前面几章的内容串起来我写了一个最小可复现的示例。场景设定三台 AUV 在平面内做匀速直线运动每台 AUV 自带 DVL 测速和 AHRS 航向能推算自身位置但会漂移编队之间通过水声测距协同修正测距更新周期 5 秒其中第二台 AUV 在某一时刻之后测距声呐开始输出带 15 米偏置的故障量测。仿真先离线生成轨迹和量测。import numpy as np rng np.random.default_rng(42) dt_step 1.0 # 惯导推算步长 1s range_period 5 # 测距更新周期 5s N 200 # 总时长 200s true_pos { auv1: np.zeros((N, 2)), auv2: np.zeros((N, 2)), auv3: np.zeros((N, 2)), } # 三条航迹: auv1 沿 x 轴, auv2 斜向, auv3 偏向另一侧 for name, vel in [(auv1, [2.0, 0.0]), (auv2, [1.5, 1.2]), (auv3, [1.0, 1.8])]: for k in range(1, N): true_pos[name][k] true_pos[name][k-1] np.array(vel) * dt_step轨迹生成后惯导推算位置就是给真值叠加随机游走。水声测距在每个测距周期取出两台 AUV 的真实距离加均值为零、标准差 0.3 米的噪声故障脚本在 t80 秒之后给第二台 AUV 相关的所有测距加 15 米固定偏置。这一步的目的不是模拟精细声学而是让融合滤波器和故障检测模块有可重复的输入。4.2 带故障检测的融合滤波核心代码核心融合部分用常速模型 EKF状态向量是位置和速度x, y, vx, vy量测是编队成员之间的距离。代码里把卡方门限与滑动窗口直接嵌进更新步让读者能看到检测和融合的先后顺序。from scipy.stats import chi2 class SimpleEKF: def __init__(self, init_state, init_cov, process_noise0.05, meas_noise0.3): self.x init_state.copy() # [x, y, vx, vy] self.P np.eye(4) * init_cov self.Q np.eye(4) * process_noise # 过程噪声协方差 self.R meas_noise ** 2 # 测距噪声方差 def predict(self, dt): F np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) self.x F self.x self.P F self.P F.T self.Q def update(self, z, other_pos, rejectTrue): dx, dy self.x[0] - other_pos[0], self.x[1] - other_pos[1] dist np.hypot(dx, dy) H np.array([[dx/dist, dy/dist, 0, 0]]) # 测距对状态的雅可比 y z - dist S H self.P H.T self.R m2 y * (1.0 / S) * y if reject and m2 chi2.ppf(0.99, df1): return False, m2 # 卡方门限拒绝跳过更新 K self.P H.T / S self.x K * y self.P (np.eye(4) - K H) self.P return True, m2这里有几个设计点。第一H 矩阵是测距对状态的雅可比dx/dist 和 dy/dist 是视线方向的单位向量分量测距只约束沿视线方向的位置垂直方向的信息要靠其他 AUV 的测距或自身推算来补所以编队几何构型对精度影响非常大。第二reject 参数控制是否做门限检验方便做对照实验关掉 reject 就是不带故障检测的纯协同定位。第三S 用标量除法因为这里是单一测距量测自由度是 1。外层循环把三台 AUV 的滤波器和故障检测器组织起来每 5 秒做一次两两测距更新并把被拒量测送进滑动窗口统计。from collections import deque filter_init { auv1: SimpleEKF(np.array([0, 0, 2.0, 0.0]), 0.1), auv2: SimpleEKF(np.array([0, 0, 1.5, 1.2]), 0.1), auv3: SimpleEKF(np.array([0, 0, 1.0, 1.8]), 0.1), } windows {name: deque(maxlen20) for name in filter_init} # 简化的测距生成器: 真实距离 噪声, t80 后 auv2 相关量测加入 15m 偏置 def gen_range(a, b, t): r np.linalg.norm(true_pos[a][t] - true_pos[b][t]) noise rng.normal(0, 0.3) bias 15.0 if (auv2 in (a, b) and t 80) else 0.0 return r noise bias for t in range(N): for ekf in filter_init.values(): ekf.predict(dt_step) if t % range_period 0 and t 0: for a, b in [(auv1, auv2), (auv2, auv3), (auv1, auv3)]: z gen_range(a, b, t) # 用对方滤波器当前估计位置工程中就是本地持有的邻居状态 obs_pos filter_init[b].x[:2] passed, _ filter_init[a].update(z, obs_pos, rejectTrue) windows[a].append(passed)跑完之后可以把每台 AUV 的位置误差曲线画出来。你会看到没有故障检测的版本里第二台 AUV 的故障量测会把三台的误差同时推高加上卡方门限和滑动窗口后故障量测被拒绝第二台 AUV 因为自身量测被弃置而回到纯惯导漂移状态编队整体误差保持可控。这就是故障检测在协同定位里的核心价值牺牲单点精度保住编队全局。4.3 参数表阈值、窗口、噪声协方差的设置建议参数设置是整个方案里最需要经验的部分不同设备、不同海况下最优参数差异很大。我把常用参数和调节方向整理成表作为起步基准。参数作用对象推荐初值调节方向测距噪声 R融合滤波器0.3 m按设备标定偏小会误检偏大会漏检过程噪声 Q融合滤波器0.05机动强时加大卡方置信度 alpha卡方门限0.01误检多就调大漏检多就调小滑动窗口长度故障判定20 个测距样本通信周期长就缩短故障判定比例故障判定0.6想快隔离就调低到 0.5降权倍数疑似故障处置10 倍 R隔离效果不够就加大调节顺序我一般是固定的先标 R 和 Q让正常场景下马氏距离的分布基本落在 15 以内再调 alpha原则是正常场景误检率低于 1%最后调窗口和判定比例目标是故障发生后 35 个测距周期内能隔离。不要同时动多个参数水下定位的误差链很长同时改两个参数出了问题都说不清是哪一个引起的。5. 避坑清单水声延迟、异步量测与故障误检的五个常见翻车点5.1 现象通信延迟把正常测距变成超限野值现象仿真里一切正常一到带延迟的水声信道环境卡方门限拒绝率暴涨健康量测大量被丢弃定位精度反而下降。原因水声传播速度约 1500 米/秒3 公里距离就有约 2 秒延迟。滤波器用当前时刻的状态去算预测距离但量测对应的是 2 秒前的位置新息里混入了运动导致的差量看起来就像野值。解决量测进来时先根据时间戳把状态序列回放到量测对应时刻再做残差检验。我一般会在滤波器里维护一个短的状态历史缓冲长度覆盖最大单程通信延迟更新用延迟对齐后的状态。另一个土办法是把测距更新周期故意拉长5 秒的量测配上 2 秒延迟误差仍可接受但编队高速机动时这个方法就不够用了。5.2 现象AUV 正常转弯机动被误判为故障现象编队直线航行时误检率为零一旦编队转向或某台 AUV 做规避机动残差立刻超限几分钟内该节点被隔离。原因滤波器用的是匀速模型转弯时的法向加速度不在模型里残差增大是模型失配不是传感器坏了。这是典型的模型把账算到量测头上。解决检测窗口里加入运动一致性先验。AUV 自身有航向和角速度信息可以在预测步就把转弯造成的位移变化写进过程噪声或者按当前运动模式动态放宽门限。我会采集一段正常机动的残差分布把门限余量定在正常机动也能通过的位置再靠滑动窗口去抓真正的持续故障。5.3 现象缓慢漂移故障漏检现象某台 AUV 的深度计或测距声呐出现缓慢偏置漂移残差始终在门限内卡方检验完全失效直到编队定位误差大到肉眼可见才发现。原因卡方检验本质是检验残差相对协方差的大小。缓慢漂移让残差缓慢增大滤波器在更新时也在放大协方差两边同步增长马氏距离就稳定在门限以内。解决对测距量测的相邻周期做差分统计。正常测距相邻差取决于 AUV 相对运动有界漂移故障会让差分带单调趋势。用累计和检验CUSUM或斜率估计来抓这类渐变故障。另一个辅助手段是定期让编队做测距闭合差校验闭合差对公共偏置非常敏感。5.4 现象分布式模式下两台 AUV 对同一节点的故障判定不一致现象A 节点判定 B 故障并把 B 的测距全部丢弃C 节点却判定 B 正常继续使用编队里出现两套不一致的定位结果。原因分布式各节点滤波器状态不同B 的测距到达各节点的时间也不同同一个量测在不同节点的残差不一样门限判定自然可能相反。解决故障判定改成邻居投票。每个节点只上报我观测到某节点的量测是否超限各节点汇总票数超过一定比例的节点同意才判定故障判定结果通过通信广播。在拓扑动态变化时还要给投票设最低票数下限避免编队只剩两台 AUV 时单票误判。5.5 现象隔离故障节点后编队定位精度反而下降现象故障节点被摘除后编队剩余成员的定位误差不降反升编队规模越小越明显。原因协同定位的精度依赖编队几何构型。被隔离的节点虽然量测坏了但它还在编队中占据一个几何位置提供视线方向的约束。摘除后构型变差定位几何精度因子恶化剩余节点误差变大。解决不要硬摘除采用保留位置、丢弃量测的软隔离。用故障节点最后的可信状态继续保持它在拓扑中的锚点作用只是它的测距不再参与任何更新同时把它的位置预测误差协方差调大避免把不确定的锚点信息反向传给健康节点。等它重新通过量测门限再恢复完整参与。这个处理方式也解释了为什么故障检测模块必须和协同定位算法一起设计——只检测不处置或者处置方式过于粗暴都会让定位系统更脆弱。6. 验证进阶用故障注入把检测算法跑出可量化的可信度6.1 设计三类故障注入场景要把故障检测算法拿出可信度光靠一次性仿真不够。我一般会在仿真框架里做三类标准故障注入突发野值单次 20 米大偏置、固定偏置15 米持续偏差、缓慢漂移0.1 米/秒线性增长的偏差。每一类都对应真实设备的一种失效模式分别统计指标后才有说服力。6.2 两个关键指标检出率与误检率评价协同定位故障检测核心指标就两个检出率以及检测延迟故障发生后多少测距周期内能正确标记误检率健康量测被误标记的比例。每类场景跑 20 次蒙特卡洛记录平均检测延迟和误检次数。def evaluate_scenario(scenario_func, detector_factory, runs20): delays, false_rates [], [] for _ in range(runs): det detector_factory() delay, false_rate scenario_func(det) delays.append(delay) false_rates.append(false_rate) return np.mean(delays), np.max(false_rates)用最大误检率而不是均值是因为故障检测必须保证健康节点不被误伤一次误检在真实任务里可能引起连锁反应。均值好看但掩盖最坏情况。6.3 蒙特卡洛与参数敏感性给阈值留出余量阈值参数不要取让当前场景最优的点而要在三类场景里都表现一致。我的习惯是把 alpha 和窗口长度做小规模网格搜索选出在检出率和误检率之间折中的区域再在边界留 20% 余量。比如 alpha 最优在 0.01窗口在 20最终交付就取 alpha0.015、window24。跑完蒙特卡洛看最大误差包络而不是单次轨迹。这个习惯救过我一次某次海试前我用单次仿真调好的参数做半物理测试误检率到了 8%后来换成三类场景的网格搜索才发现原来参数恰好落在敏感边界上。希望帮到你。本文还有配套的精品资源点击获取