企业官网建设流程全解析

简介：一份以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 这类协同定位算法里基本是标配。选测距还有一个工程理由：水声测距设备技术成熟，误差模型相对干净，故障检测更容易从残差里看出异常。测距的更新周期通常在 2～10 秒，这个节奏也刚好匹配故障检测滑动窗口的时间尺度。

2.2 集中式、分布式、主从式：三种结构在故障场景下的差异

确定了量测类型，下一步是决定数据怎么汇聚、在哪里融合。常见架构有三种：集中式、分布式、主从式。

集中式是所有测距量测通过水声通信发到某台中心节点，由中心统一的滤波器完成全部融合，再把结果广播回去。优点是只有一个滤波器，状态一致性好，故障检测逻辑集中，最容易实现。缺点是通信瓶颈，水声信道带宽极小，编队大了以后数据排队导致延迟；另外中心节点自身是单点，它出故障整个系统失去融合能力。

分布式是每台 AUV 只跟能通信的邻居交换量测，各自本地维护一个滤波器。优点是鲁棒性好，单台掉线不影响别人；缺点是各节点由于量测到达时间不同，对同一台 AUV 的状态估计会有差异，故障检测的判定结果也可能不一致——这个问题放在后面的避坑章详细说。

主从式介于两者之间，一台主 AUV 携带更精密的设备，若干从 AUV 以主节点播报的位置为基准做跟随修正。从节点实现简单，但主节点的位置质量直接决定全队质量，主节点故障对全队是灾难性的。因此主从式方案里，故障检测必须优先布置在主节点侧。

选择上，编队规模在 3～5 台且通信拓扑固定，我一般选集中式，方便把故障检测做完整；编队规模大、任务周期长、允许成员临时掉线，就用分布式，故障检测做成"本地检测加邻居投票"的形式。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, alpha=0.01): """卡方门限检验：返回是否通过、马氏距离、门限值。 innovation: 新息向量 (n,) innovation_cov: 新息协方差 (n,n) dof: 自由度，标量测距取 1 alpha: 显著性水平，越小门限越宽 """ m2 = innovation.T @ np.linalg.inv(innovation_cov) @ innovation gate = chi2.ppf(1 - alpha, df=dof) 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, window=20, fail_ratio=0.6): self.window = window self.fail_ratio = fail_ratio self.history = deque(maxlen=window) 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 是常用的起始点。水声测距更新周期通常是 2～10 秒一次，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 米的噪声；故障脚本在 t=80 秒之后给第二台 AUV 相关的所有测距加 15 米固定偏置。这一步的目的不是模拟精细声学，而是让融合滤波器和故障检测模块有可重复的输入。

4.2 带故障检测的融合滤波核心代码

核心融合部分用常速模型 EKF，状态向量是位置和速度（x, y, vx, vy），量测是编队成员之间的距离。代码里把卡方门限与滑动窗口直接嵌进更新步，让读者能看到检测和融合的先后顺序。

from scipy.stats import chi2 class SimpleEKF: def __init__(self, init_state, init_cov, process_noise=0.05, meas_noise=0.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, reject=True): 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, df=1): 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(maxlen=20) for name in filter_init} # 简化的测距生成器: 真实距离 + 噪声, t>80 后 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, reject=True) windows[a].append(passed)

跑完之后可以把每台 AUV 的位置误差曲线画出来。你会看到：没有故障检测的版本里，第二台 AUV 的故障量测会把三台的误差同时推高；加上卡方门限和滑动窗口后，故障量测被拒绝，第二台 AUV 因为自身量测被弃置而回到纯惯导漂移状态，编队整体误差保持可控。这就是故障检测在协同定位里的核心价值：牺牲单点精度，保住编队全局。

4.3 参数表：阈值、窗口、噪声协方差的设置建议

参数设置是整个方案里最需要经验的部分，不同设备、不同海况下最优参数差异很大。我把常用参数和调节方向整理成表，作为起步基准。

调节顺序我一般是固定的：先标 R 和 Q，让正常场景下马氏距离的分布基本落在 1～5 以内；再调 alpha，原则是正常场景误检率低于 1%；最后调窗口和判定比例，目标是故障发生后 3～5 个测距周期内能隔离。不要同时动多个参数，水下定位的误差链很长，同时改两个参数出了问题都说不清是哪一个引起的。

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, runs=20): 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，最终交付就取 alpha=0.015、window=24。跑完蒙特卡洛看最大误差包络而不是单次轨迹。这个习惯救过我一次：某次海试前我用单次仿真调好的参数做半物理测试，误检率到了 8%，后来换成三类场景的网格搜索，才发现原来参数恰好落在敏感边界上。希望帮到你。

本文还有配套的精品资源，点击获取