简介面向水下机器人、海洋工程与多AUV协同导航研究者这份资源围绕AUV协同定位与故障检测给出MATLAB实现适用于多无人潜航器编队中的定位精度优化与异常量测识别场景。压缩包为rar格式仅含1个m脚本体量约3KB代码精简便于直接阅读和二次修改。目前已有200人学习下载。脚本由系统建模、传感器数据融合、AUV间信息交换、定位算法更新及故障诊断等模块构成可帮助初学者理解交替领航与容错定位的实现逻辑也为工程人员评估水下复杂环境下的协同导航方案提供可运行的参考。1. 水下单机定位漂移难解AUV协同定位成编队作业刚需单艘AUV水下跑一小时惯性导航累积误差就会达到航程的1%~3%DVL能压住一部分漂移但无法根除。编队作业时这个误差更要命——回收引导、目标接棒、队形保持全都依赖彼此相对位置。XT_GZJC要解决的问题就是AUV协同定位利用多艇之间水声测得的高精度相对距离通过协同定位算法把各自推算位置拉回真实几何关系同时用故障检测把坏数据挡在滤波器外面。这套方案适合水下机器人研发、多智能体系统集成和编队算法调试的工程师。文中会给出完整的算法矩阵推导、可运行的EKF协同定位代码和故障检测的卡方检验参数。2. 协同定位算法从原理到选型测距方程、状态估计与融合框架2.1 相对测距是协同定位的锚点TOA/TDOA怎么选协同定位的前提是拿到可靠的艇间距离观测。水下没有GNSS信号电磁波衰减严重声波是唯一能传几百米到几公里的物理载体声速约1500m/s测距问题于是转化为时间测量问题。TOA到达时间需要收发双方时钟严格同步测距精度取决于同步误差。时钟偏1微秒对应1.5毫米偏1毫秒就是1.5米这个比例关系决定了系统必须配备高稳晶振或者任务前统一对时。AUV编队里的普遍做法是装同源授时模块巡航期间靠高稳晶振维持可把测距精度维持在0.1~1米量级。TDOA到达时间差不要求时钟完全同步它依赖多个接收节点之间的时差解算位置更适合固定基站阵列。但AUV编队是移动的节点位置本身也在估计TDOA的未知量反而比TOA多工程上通常不作为第一选择。XT_GZJC方案以TOA测距为主双程测距询问-应答能规避时钟同步问题代价是通信时间翻倍适合测距频率不高的编队场景。声速剖面随温度盐度变化固定用1500m/s计算会在远距离上累积误差。水深超过50米时工程上会把声速剖面仪数据装进测距协议按实际平均声速修正距离解算浅水区则固定声速即可不需要额外的计算开销。2.2 状态估计选型EKF、UKF还是粒子滤波拿到测距值后需要把距离观测和自身推算位置融合。这一步是协同定位算法的核心。可选的状态估计器有三类计算量与非线性处理能力依次递增。扩展卡尔曼滤波EKF在预测点对非线性观测方程做一阶泰勒展开计算量最小状态维数不高时完全够用。无迹卡尔曼滤波UKF用sigma点近似状态分布不需要求雅可比矩阵非线性较强时精度优于EKF计算量约为EKF的3~5倍。粒子滤波PF用大量随机粒子近似后验分布适合非高斯噪声和多峰分布但计算量随粒子数线性增长在低算力嵌入式平台上跑实时更新压力很大。选型时我一般把编队规模和机动幅值作为判断条件。四艘以内的编队、水声测距频率0.2~1Hz、艇体运动近似近直线加小角度转向EKF就够。编队里有高速转弯或传感器非线性显著时建议升到UKF避免雅可比近似误差在机动段被放大。粒子滤波更多是论文对比的基准线实际系统很少直接用。状态估计器非线性处理方式相对计算量适用场景EKF一阶线性化1x近直线巡航、测距更新频率低UKFsigma点无迹变换3~5x转弯机动多、非线性观测强粒子滤波蒙特卡洛采样20x非高斯噪声、强多峰分布2.3 协同拓扑主从式与平行式的定位精度差异协同拓扑决定误差传播路径。主从式构型里有一艘领航者通常带高精度惯导和上浮GNSS校正其余从艇只测量与领航者的距离误差从领航者单向传到从艇。优点是只需要单条测距链路算法实现简单缺点是领航者一旦出现定位异常整个编队都会被带偏。平行式构型里各艇地位平等两两之间互相测距相当于完全图上的协同定位网络。它能显著提升整体定位精度但需要集中式融合或一致性分布式滤波通信压力和算法复杂度都上一个台阶。XT_GZJC的默认拓扑是主从加备用链路正常巡航走主从式领航者故障时切换到最近的健康从艇升任临时领航者。主从式构型还有个容易被忽视的点领航者的轨迹设计。领航者最好保持慢速直线或大半径转弯避免高速机动扩大从艇的观测盲区。编队进入走廊或狭窄水道时主从式测距会把相对位置约束拉近这时队形保持的冗余度明显下降需要额外依赖惯导推算撑过这段时间。3. 用EKF落地一版协同定位算法状态构建、代码实现与参数标定3.1 状态向量与观测方程的构建思路协同定位EKF的状态向量取法有很多种最稳妥的做法是把自身位置、航向和速度放进同一个状态里。以二维水平面为例取x [x, y, psi, v]其中x、y是水平面坐标psi是航向角v是前向速度。没有把加速度放进状态因为水下航行器推力变化平缓速度在短时间内可视为近似常值加速度并入过程噪声里处理。运动模型采用恒定速度偏航率模型也就是假设一个解算周期内速度恒定、转角恒定。这个模型处理AUV的直线巡航和缓慢转向时误差很小代码里用一个状态转移矩阵F描述。观测方程则是非线性的从艇到领航者的距离等于两点坐标之差的二范数。因为距离方程含平方和开根号没法写成线性矩阵HEKF要在每次更新时算出距离对位置坐标的偏导数这正是EKF与传统线性卡尔曼滤波在使用上的关键差异。3.2 最小可运行代码双AUV测距协同的EKF实现下面这段代码是完整的双AUV主从协同定位EKF。领航者在固定轨迹上航行从艇通过自身运动模型做预测再用水声测距更新位置。代码可以直接跑通也方便改造成自己的编队逻辑。import numpy as np class RangeEKF: def __init__(self, dt1.0): # 状态向量: [x, y, psi, v] # 单位: 米, 米, 弧度, 米/秒 self.dt dt self.x np.array([0.0, 0.0, 0.0, 1.0]) self.P np.eye(4) * 0.5 # 过程噪声: 位置噪声方差, 航向噪声方差, 速度噪声方差 self.Q np.diag([0.1, 0.1, 0.01, 0.01]) # 测距观测噪声方差, 单位: 平方米 self.R np.array([[2.0]]) def predict(self, u): # u (前向速度指令, 偏航角速度指令) v, omega u psi self.x[2] dt self.dt # 状态转移矩阵: 恒定速度 恒定偏航率 F np.array([ [1, 0, -v * np.sin(psi) * dt, np.cos(psi) * dt], [0, 1, v * np.cos(psi) * dt, np.sin(psi) * dt], [0, 0, 1, 0], [0, 0, 0, 1] ]) # 名义状态更新 self.x[0] v * np.cos(psi) * dt self.x[1] v * np.sin(psi) * dt self.x[2] omega * dt # 协方差预测 self.P F self.P F.T self.Q def update(self, z, anchor_pos): # z: 水声测得的相对距离, anchor_pos: 领航者坐标 dx self.x[0] - anchor_pos[0] dy self.x[1] - anchor_pos[1] pred np.sqrt(dx**2 dy**2) # 观测雅可比矩阵 H np.array([[dx / pred, dy / pred, 0, 0]]) # 新息协方差 S H self.P H.T self.R # 卡尔曼增益 K self.P H.T np.linalg.inv(S) # 状态更新 self.x self.x K.flatten() * (z - pred) # 协方差更新标准形式低维调试够用 self.P (np.eye(4) - K H) self.P # 返回新息和其协方差供故障检测模块使用 return z - pred, S代码逻辑分两部分predict阶段拿运动模型把状态和协方差向前推update阶段拿距离观测修正位置。注意F矩阵右上角三个非零元素它们表达航向和速度对位置预测的交叉影响写错会让协方差传播失真。update返回的新息r和协方差S不能丢掉故障检测全靠这两个量。高频预测、低频测距是AUV协同定位的标准节奏。惯导或DVL输出的速度可以跑高频预测比如10~20Hz水声测距链路慢得多1~5秒一次。EKF主循环应当设计成predict按高频调用、update按测距事件触发。测距数据晚到时需要维护时间戳数组做异步更新避免用过期距离修正最新状态。3.3 过程噪声与观测噪声的初始标定滤波器里的Q和R直接决定定位结果是被预测模型主导还是被测距主导。Q偏大会导致协方差膨胀快、滤波器过于相信测量值R偏小会让系统对测距噪声过度反应位置曲线出现毛刺。标定顺序建议先定R再调Q。R可以用静态实验测得把两个水声换能器固定在已知距离的水池里多次测距取样本方差。常见商用水声modem的测距标准差在0.3~0.8米取R0.5~2.0平方米作为初值都算合理。Q里三个对角线元素的物理含义位置过程噪声由模型不准引入的加速度扰动决定航向噪声对应罗经或光纤陀螺的短时漂移速度噪声对应推进器响应误差。初值参考下表Q参数典型初值范围说明位置噪声0.01~0.5平方米值越大表明认为运动模型越不可靠航向噪声0.001~0.05平方弧度对应罗经/光纤陀螺的零偏稳定性速度噪声0.001~0.02平方米/秒²对应DVL测速与螺旋桨转速换算误差4. 故障检测不靠玄学残差卡方检验与协同定位的容错设计4.1 协同定位会踩的故障类型协同定位系统里最典型的故障源有三个。第一是水声测距野值浅水区多径效应、海面反射混叠距离解算可能突然跳变几米甚至几十米新息瞬间偏离正常范围。第二是通信延迟或丢包水声链路带宽低一次握手失败就要等下一次测距周期数据到达时间比预计晚很多。第三是传感器软故障DVL输出速度持续偏高或罗经航向缓慢漂移这类故障不会让数值瞬间爆掉但会一点一点带偏状态估计。三类故障的共同特征是新息观测值减预测值偏离正常分布。只要把新息的变化监控住就抓住了故障检测的通用抓手。4.2 基于新息残差的故障检测原理EKF更新阶段计算出的新息r z - h(x̄)在健康状态下近似服从零均值高斯分布协方差等于S HPHᵀ R。有了分布参考就能用卡方检验判断当前观测是否可信。具体做法是把新息投影到S逆张成的度量空间算出马氏距离d² rᵀS⁻¹r再与卡方分布分位数阈值比较。观测只有一维距离时自由度为1显著性水平0.05对应阈值3.84同时融合距离和方位两个观测量时自由度变成2阈值变成5.99。from scipy.stats import chi2 def check_innovation(r, S, alpha0.05): # r: 新息标量或向量, S: 新息协方差矩阵 d2 float(r.T np.linalg.inv(S) r) dof len(r) threshold chi2.ppf(1 - alpha, dof) if d2 threshold: return False, d2, threshold # 判定故障 return True, d2, threshold # 判定正常这段函数接收EKF更新阶段返回的新息r和协方差S。阈值通过卡方分布分位数函数算出alpha控制误报率——取0.05意味着健康数据被误判为故障的概率不超过5%。返回的d2值还可以用于残差趋势监控观察是否有缓变型软故障正逼近阈值。4.3 检出故障后的容错切换策略判定为故障后直接丢弃量测是最保守做法但带队形的系统不能只丢弃就完事重复丢测距会导致协同更新长时间中断。XT_GZJC的容错策略分三级第一级是单次故障量测丢弃滤波器继续跑predict第二级是连续N次故障后切源把观测源从故障节点切换到备用领航者第三级是协方差限幅防止长期无量测时P矩阵发散到数值异常。切源时要同步重置观测噪声矩阵R。不同测距链路的噪声特性不同备用链路的R通常比主链路大沿用旧R会让新链路的健康观测被判为故障形成误切循环。这个细节在实际调试中最容易被忽略。软故障还需要额外的残差平滑手段。渐变的传感器漂移在前几拍根本过不了卡方阈值等它越界时位置已偏出数米。工程上的常见做法是给新息序列加滑窗累积和CUSUM把微小但持续的偏移放大成可检测的跳变。CUSUM的参数比卡方检验多一个漂移容忍量取值一般是新息标准差的四分之一到二分之一。5. 让协同定位在不理想环境中稳住仿真验证与参数调优技巧5.1 仿真场景怎么搭验证协同定位算法别直接上湖试先用仿真把参数空间扫一遍。搭一个两艇场景领航者沿直线以2m/s巡航从艇初始位置偏移(5, -8)米初始航向偏15度测距周期2秒测距噪声标准差0.5米。跑600秒每隔一段时间注入一个幅度10米的野值观察故障检测能不能在下一拍拦住它。# 仿真主循环节选 ekf RangeEKF(dt1.0) for t in range(600): ekf.predict((2.0, 0.0)) # 匀速直线运动 if t % 2 0: # 每2秒一次测距 z_true np.linalg.norm(ekf.x[:2] - anchor) z z_true np.random.normal(0, 0.5) if t 300: # 注入一次野值 z 10.0 r, S ekf.update(z, anchor) ok, d2, thr check_innovation(r, S) if not ok: print(ft{t}: 量测拒绝, d2{d2:.2f} {thr:.2f})5.2 三个最值得先调的参数第一个是Q/R比值。测距频率越低越应该把R相对调小因为每次量测机会都更宝贵反过来测距频率高时R可以调大一些避免位置曲线噪声过大。第二个是卡方检验的显著性水平alpha野值频繁时从0.05收紧到0.01防止漏检系统本身噪声偏大时则放松到0.1。第三个是连续切源计数阈值取3~5次为宜太敏感会在链路抖动时误切太迟钝会错过真正故障。5.3 参数速查表参数默认值调优方向测距噪声方差R2.0静态实测替代经验值位置过程噪声0.1运动模型不可靠时调大卡方显著性水平0.05误检多就收紧到0.01切源阈值3链路抖动多就调大到5本文还有配套的精品资源点击获取 基于EKF的AUV编队协同定位算法与故障检测技术解析
简介面向水下机器人、海洋工程与多AUV协同导航研究者这份资源围绕AUV协同定位与故障检测给出MATLAB实现适用于多无人潜航器编队中的定位精度优化与异常量测识别场景。压缩包为rar格式仅含1个m脚本体量约3KB代码精简便于直接阅读和二次修改。目前已有200人学习下载。脚本由系统建模、传感器数据融合、AUV间信息交换、定位算法更新及故障诊断等模块构成可帮助初学者理解交替领航与容错定位的实现逻辑也为工程人员评估水下复杂环境下的协同导航方案提供可运行的参考。1. 水下单机定位漂移难解AUV协同定位成编队作业刚需单艘AUV水下跑一小时惯性导航累积误差就会达到航程的1%~3%DVL能压住一部分漂移但无法根除。编队作业时这个误差更要命——回收引导、目标接棒、队形保持全都依赖彼此相对位置。XT_GZJC要解决的问题就是AUV协同定位利用多艇之间水声测得的高精度相对距离通过协同定位算法把各自推算位置拉回真实几何关系同时用故障检测把坏数据挡在滤波器外面。这套方案适合水下机器人研发、多智能体系统集成和编队算法调试的工程师。文中会给出完整的算法矩阵推导、可运行的EKF协同定位代码和故障检测的卡方检验参数。2. 协同定位算法从原理到选型测距方程、状态估计与融合框架2.1 相对测距是协同定位的锚点TOA/TDOA怎么选协同定位的前提是拿到可靠的艇间距离观测。水下没有GNSS信号电磁波衰减严重声波是唯一能传几百米到几公里的物理载体声速约1500m/s测距问题于是转化为时间测量问题。TOA到达时间需要收发双方时钟严格同步测距精度取决于同步误差。时钟偏1微秒对应1.5毫米偏1毫秒就是1.5米这个比例关系决定了系统必须配备高稳晶振或者任务前统一对时。AUV编队里的普遍做法是装同源授时模块巡航期间靠高稳晶振维持可把测距精度维持在0.1~1米量级。TDOA到达时间差不要求时钟完全同步它依赖多个接收节点之间的时差解算位置更适合固定基站阵列。但AUV编队是移动的节点位置本身也在估计TDOA的未知量反而比TOA多工程上通常不作为第一选择。XT_GZJC方案以TOA测距为主双程测距询问-应答能规避时钟同步问题代价是通信时间翻倍适合测距频率不高的编队场景。声速剖面随温度盐度变化固定用1500m/s计算会在远距离上累积误差。水深超过50米时工程上会把声速剖面仪数据装进测距协议按实际平均声速修正距离解算浅水区则固定声速即可不需要额外的计算开销。2.2 状态估计选型EKF、UKF还是粒子滤波拿到测距值后需要把距离观测和自身推算位置融合。这一步是协同定位算法的核心。可选的状态估计器有三类计算量与非线性处理能力依次递增。扩展卡尔曼滤波EKF在预测点对非线性观测方程做一阶泰勒展开计算量最小状态维数不高时完全够用。无迹卡尔曼滤波UKF用sigma点近似状态分布不需要求雅可比矩阵非线性较强时精度优于EKF计算量约为EKF的3~5倍。粒子滤波PF用大量随机粒子近似后验分布适合非高斯噪声和多峰分布但计算量随粒子数线性增长在低算力嵌入式平台上跑实时更新压力很大。选型时我一般把编队规模和机动幅值作为判断条件。四艘以内的编队、水声测距频率0.2~1Hz、艇体运动近似近直线加小角度转向EKF就够。编队里有高速转弯或传感器非线性显著时建议升到UKF避免雅可比近似误差在机动段被放大。粒子滤波更多是论文对比的基准线实际系统很少直接用。状态估计器非线性处理方式相对计算量适用场景EKF一阶线性化1x近直线巡航、测距更新频率低UKFsigma点无迹变换3~5x转弯机动多、非线性观测强粒子滤波蒙特卡洛采样20x非高斯噪声、强多峰分布2.3 协同拓扑主从式与平行式的定位精度差异协同拓扑决定误差传播路径。主从式构型里有一艘领航者通常带高精度惯导和上浮GNSS校正其余从艇只测量与领航者的距离误差从领航者单向传到从艇。优点是只需要单条测距链路算法实现简单缺点是领航者一旦出现定位异常整个编队都会被带偏。平行式构型里各艇地位平等两两之间互相测距相当于完全图上的协同定位网络。它能显著提升整体定位精度但需要集中式融合或一致性分布式滤波通信压力和算法复杂度都上一个台阶。XT_GZJC的默认拓扑是主从加备用链路正常巡航走主从式领航者故障时切换到最近的健康从艇升任临时领航者。主从式构型还有个容易被忽视的点领航者的轨迹设计。领航者最好保持慢速直线或大半径转弯避免高速机动扩大从艇的观测盲区。编队进入走廊或狭窄水道时主从式测距会把相对位置约束拉近这时队形保持的冗余度明显下降需要额外依赖惯导推算撑过这段时间。3. 用EKF落地一版协同定位算法状态构建、代码实现与参数标定3.1 状态向量与观测方程的构建思路协同定位EKF的状态向量取法有很多种最稳妥的做法是把自身位置、航向和速度放进同一个状态里。以二维水平面为例取x [x, y, psi, v]其中x、y是水平面坐标psi是航向角v是前向速度。没有把加速度放进状态因为水下航行器推力变化平缓速度在短时间内可视为近似常值加速度并入过程噪声里处理。运动模型采用恒定速度偏航率模型也就是假设一个解算周期内速度恒定、转角恒定。这个模型处理AUV的直线巡航和缓慢转向时误差很小代码里用一个状态转移矩阵F描述。观测方程则是非线性的从艇到领航者的距离等于两点坐标之差的二范数。因为距离方程含平方和开根号没法写成线性矩阵HEKF要在每次更新时算出距离对位置坐标的偏导数这正是EKF与传统线性卡尔曼滤波在使用上的关键差异。3.2 最小可运行代码双AUV测距协同的EKF实现下面这段代码是完整的双AUV主从协同定位EKF。领航者在固定轨迹上航行从艇通过自身运动模型做预测再用水声测距更新位置。代码可以直接跑通也方便改造成自己的编队逻辑。import numpy as np class RangeEKF: def __init__(self, dt1.0): # 状态向量: [x, y, psi, v] # 单位: 米, 米, 弧度, 米/秒 self.dt dt self.x np.array([0.0, 0.0, 0.0, 1.0]) self.P np.eye(4) * 0.5 # 过程噪声: 位置噪声方差, 航向噪声方差, 速度噪声方差 self.Q np.diag([0.1, 0.1, 0.01, 0.01]) # 测距观测噪声方差, 单位: 平方米 self.R np.array([[2.0]]) def predict(self, u): # u (前向速度指令, 偏航角速度指令) v, omega u psi self.x[2] dt self.dt # 状态转移矩阵: 恒定速度 恒定偏航率 F np.array([ [1, 0, -v * np.sin(psi) * dt, np.cos(psi) * dt], [0, 1, v * np.cos(psi) * dt, np.sin(psi) * dt], [0, 0, 1, 0], [0, 0, 0, 1] ]) # 名义状态更新 self.x[0] v * np.cos(psi) * dt self.x[1] v * np.sin(psi) * dt self.x[2] omega * dt # 协方差预测 self.P F self.P F.T self.Q def update(self, z, anchor_pos): # z: 水声测得的相对距离, anchor_pos: 领航者坐标 dx self.x[0] - anchor_pos[0] dy self.x[1] - anchor_pos[1] pred np.sqrt(dx**2 dy**2) # 观测雅可比矩阵 H np.array([[dx / pred, dy / pred, 0, 0]]) # 新息协方差 S H self.P H.T self.R # 卡尔曼增益 K self.P H.T np.linalg.inv(S) # 状态更新 self.x self.x K.flatten() * (z - pred) # 协方差更新标准形式低维调试够用 self.P (np.eye(4) - K H) self.P # 返回新息和其协方差供故障检测模块使用 return z - pred, S代码逻辑分两部分predict阶段拿运动模型把状态和协方差向前推update阶段拿距离观测修正位置。注意F矩阵右上角三个非零元素它们表达航向和速度对位置预测的交叉影响写错会让协方差传播失真。update返回的新息r和协方差S不能丢掉故障检测全靠这两个量。高频预测、低频测距是AUV协同定位的标准节奏。惯导或DVL输出的速度可以跑高频预测比如10~20Hz水声测距链路慢得多1~5秒一次。EKF主循环应当设计成predict按高频调用、update按测距事件触发。测距数据晚到时需要维护时间戳数组做异步更新避免用过期距离修正最新状态。3.3 过程噪声与观测噪声的初始标定滤波器里的Q和R直接决定定位结果是被预测模型主导还是被测距主导。Q偏大会导致协方差膨胀快、滤波器过于相信测量值R偏小会让系统对测距噪声过度反应位置曲线出现毛刺。标定顺序建议先定R再调Q。R可以用静态实验测得把两个水声换能器固定在已知距离的水池里多次测距取样本方差。常见商用水声modem的测距标准差在0.3~0.8米取R0.5~2.0平方米作为初值都算合理。Q里三个对角线元素的物理含义位置过程噪声由模型不准引入的加速度扰动决定航向噪声对应罗经或光纤陀螺的短时漂移速度噪声对应推进器响应误差。初值参考下表Q参数典型初值范围说明位置噪声0.01~0.5平方米值越大表明认为运动模型越不可靠航向噪声0.001~0.05平方弧度对应罗经/光纤陀螺的零偏稳定性速度噪声0.001~0.02平方米/秒²对应DVL测速与螺旋桨转速换算误差4. 故障检测不靠玄学残差卡方检验与协同定位的容错设计4.1 协同定位会踩的故障类型协同定位系统里最典型的故障源有三个。第一是水声测距野值浅水区多径效应、海面反射混叠距离解算可能突然跳变几米甚至几十米新息瞬间偏离正常范围。第二是通信延迟或丢包水声链路带宽低一次握手失败就要等下一次测距周期数据到达时间比预计晚很多。第三是传感器软故障DVL输出速度持续偏高或罗经航向缓慢漂移这类故障不会让数值瞬间爆掉但会一点一点带偏状态估计。三类故障的共同特征是新息观测值减预测值偏离正常分布。只要把新息的变化监控住就抓住了故障检测的通用抓手。4.2 基于新息残差的故障检测原理EKF更新阶段计算出的新息r z - h(x̄)在健康状态下近似服从零均值高斯分布协方差等于S HPHᵀ R。有了分布参考就能用卡方检验判断当前观测是否可信。具体做法是把新息投影到S逆张成的度量空间算出马氏距离d² rᵀS⁻¹r再与卡方分布分位数阈值比较。观测只有一维距离时自由度为1显著性水平0.05对应阈值3.84同时融合距离和方位两个观测量时自由度变成2阈值变成5.99。from scipy.stats import chi2 def check_innovation(r, S, alpha0.05): # r: 新息标量或向量, S: 新息协方差矩阵 d2 float(r.T np.linalg.inv(S) r) dof len(r) threshold chi2.ppf(1 - alpha, dof) if d2 threshold: return False, d2, threshold # 判定故障 return True, d2, threshold # 判定正常这段函数接收EKF更新阶段返回的新息r和协方差S。阈值通过卡方分布分位数函数算出alpha控制误报率——取0.05意味着健康数据被误判为故障的概率不超过5%。返回的d2值还可以用于残差趋势监控观察是否有缓变型软故障正逼近阈值。4.3 检出故障后的容错切换策略判定为故障后直接丢弃量测是最保守做法但带队形的系统不能只丢弃就完事重复丢测距会导致协同更新长时间中断。XT_GZJC的容错策略分三级第一级是单次故障量测丢弃滤波器继续跑predict第二级是连续N次故障后切源把观测源从故障节点切换到备用领航者第三级是协方差限幅防止长期无量测时P矩阵发散到数值异常。切源时要同步重置观测噪声矩阵R。不同测距链路的噪声特性不同备用链路的R通常比主链路大沿用旧R会让新链路的健康观测被判为故障形成误切循环。这个细节在实际调试中最容易被忽略。软故障还需要额外的残差平滑手段。渐变的传感器漂移在前几拍根本过不了卡方阈值等它越界时位置已偏出数米。工程上的常见做法是给新息序列加滑窗累积和CUSUM把微小但持续的偏移放大成可检测的跳变。CUSUM的参数比卡方检验多一个漂移容忍量取值一般是新息标准差的四分之一到二分之一。5. 让协同定位在不理想环境中稳住仿真验证与参数调优技巧5.1 仿真场景怎么搭验证协同定位算法别直接上湖试先用仿真把参数空间扫一遍。搭一个两艇场景领航者沿直线以2m/s巡航从艇初始位置偏移(5, -8)米初始航向偏15度测距周期2秒测距噪声标准差0.5米。跑600秒每隔一段时间注入一个幅度10米的野值观察故障检测能不能在下一拍拦住它。# 仿真主循环节选 ekf RangeEKF(dt1.0) for t in range(600): ekf.predict((2.0, 0.0)) # 匀速直线运动 if t % 2 0: # 每2秒一次测距 z_true np.linalg.norm(ekf.x[:2] - anchor) z z_true np.random.normal(0, 0.5) if t 300: # 注入一次野值 z 10.0 r, S ekf.update(z, anchor) ok, d2, thr check_innovation(r, S) if not ok: print(ft{t}: 量测拒绝, d2{d2:.2f} {thr:.2f})5.2 三个最值得先调的参数第一个是Q/R比值。测距频率越低越应该把R相对调小因为每次量测机会都更宝贵反过来测距频率高时R可以调大一些避免位置曲线噪声过大。第二个是卡方检验的显著性水平alpha野值频繁时从0.05收紧到0.01防止漏检系统本身噪声偏大时则放松到0.1。第三个是连续切源计数阈值取3~5次为宜太敏感会在链路抖动时误切太迟钝会错过真正故障。5.3 参数速查表参数默认值调优方向测距噪声方差R2.0静态实测替代经验值位置过程噪声0.1运动模型不可靠时调大卡方显著性水平0.05误检多就收紧到0.01切源阈值3链路抖动多就调大到5本文还有配套的精品资源点击获取