CN113376664A - 一种无人蜂群协同导航多故障检测方法 - Google Patents

Abstract

本发明公开了一种无人蜂群协同导航多故障检测方法，利用无人蜂群协同信息辅助进行无人飞行器导航系统多故障检测与识别；结合各无人机相对量测信息与无人机自身导航观测信息进行协同容错架构设计，利用不同量测信息的组合构造多级卡尔曼滤波器，通过不同滤波器解的差异性检测无人机导航卫星及协同量测信息的故障状态，并将故障量测隔离以实现无人机协同导航系统的容错重构。本发明考虑了无人蜂群导航系统可能出现的多故障情况，构造不同层次的滤波器实现多个故障量测信息的分离，可灵活检测协同导航系统故障量测信息的个数，无需改变层次滤波结构，具有很好的适应性；本发明适用性广，不受协同辅助信息种类和数量的限制。

Description

一种无人蜂群协同导航多故障检测方法

技术领域

本发明属于导航技术领域，具体涉及一种无人蜂群协同导航多故障检测方法。

背景技术

无人蜂群往往用于执行复杂和具有挑战性的任务，当其工作在恶劣环境下时，无人蜂群的导航系统容易出现多种类型的故障同时发生的情况，极大地影响无人蜂群的自主可靠飞行。传统故障诊断方法主要利用飞行器自身传感器信息构建故障检测模型，在复杂和不确定性干扰情况下可能出现检测灵敏度下降，甚至检测错误的现象。多无人机间的协同信息为无人机导航系统提供了多尺度的冗余量测信息，可用于监测无人机导航系统的健康状况，辅助完成多故障的检测与识别，以提高无人蜂群在复杂环境下的适应性和生存能力。

因此，针对无人蜂群协同导航对导航量测信息可靠性的要求，需研究引入协同量测信息后导航系统的容错架构设计，考虑多个故障的在线检测及隔离方法，以实现无人蜂群的容错重构。

发明内容

发明目的：为解决传统故障检测方法对多个故障检测灵敏度下降，诊断失效或错误诊断等问题，本发明提出了一种无人蜂群协同导航多故障检测方法。

发明内容：本发明提出一种无人蜂群协同导航多故障检测方法，具体包括以下步骤：

(1)当前时刻为t，无人蜂群中的成员数量为N，无人蜂群各成员根据自身的机载惯导系统进行SINS误差状态建模；

(2)无人蜂群各成员分别通过自身卫星导航接收机确定各自的可用导航卫星个数并获得各卫星量测信息；

(3)无人蜂群各成员根据其他成员的定位精度，选择定位精度较高的基准无人机作为可用协同成员，并获得与可用协同成员的相对测量信息；

(4)针对无人蜂群任意一个成员k，k＝1,2,…,N，成员k可用导航卫星量测信息和可用协同成员量测信息建立卫星-飞行器量测模型；

(5)成员k计算其可用量测量总个数为n＝x_k+y_k，其中x_k为成员k的可用导航卫星数，y_k为成员k的可用协同成员数，并将成员k的可用量测量个数n与预设的阈值q_k比较；若n-1＞q_k，则成员k的导航系统转入故障检测模式，执行(6)，否则，成员k的导航系统转入可用检测量不足告警模式，转步骤(20)；

(6)根据阈值q_k计算成员k的最大可检测故障数m＝x_k+y_k-q_k-1，并将可用卫星进行编号为i，i＝1,2,…,x_k，将所有可用协同成员进行编号为i，i＝x_k+1,…,x_k+y_k；

(7)成员k对所有可用卫星和协同成员量测信息的故障状态进行同步检测，根据x_k个卫星及y_k个协同成员量测信息和惯导信息构造主滤波器F₀，进行卡尔曼滤波后得到状态向量X₀及协方差矩阵P₀；

(8)成员k根据步骤(6)获得的最大可检测故障数m计算子滤波器个数

并构造子滤波器F_i，i＝1,2,…,a；

(9)成员k根据子滤波器F_i的量测信息和惯导信息共进行a次卡尔曼滤波，得到各子滤波器F_i的状态向量X_i、协方差矩阵P_i；

(10)根据主滤波器状态向量X₀和子滤波器F_i状态向量X_i，i＝1,2,…,a构造主滤波器与各子滤波器的解分离矢量dX_i＝X₀-X_i，得到水平位置解分离矢量为

计算子滤波器F_i，i＝1,2,…,a的故障检测统计量为

其中，|·|表示取向量的模；

(11)成员k计算子滤波器F_i，i＝1,2,…,a的检测门限T_i；

(12)将子滤波器F_i的故障检验统计量d_i与检测门限T_i比较，若存在d_i＞T_i，则成员k的所有量测量中存在故障，转入故障识别模式，执行步骤(13)，否则，转步骤(20)；

(13)成员k根据子滤波器F_i，i＝1,2,…,a，计算各子滤波器F_i下属子-子滤波器个数为b＝n-m，并构造子滤波器F_i下属子-子滤波器F_i,j，j＝1,2,…,b；

(14)成员k根据步骤((13)子滤波器F_i，i＝1,2,…,a下属子-子滤波器F_i,j，j＝1,2,…,b的量测信息和惯导信息共进行a×b次卡尔曼滤波，得到各子-子滤波器F_i,j的状态向量X_i,j、协方差矩阵P_i,j；

(15)根据子滤波器F_i状态向量X_i和子-子滤波器F_i,j状态向量X_i,j构造各子滤波器与其下属子-子滤波器的解分离矢量dX_i,j＝X_i-X_i,j，得到水平位置解分离矢量为

计算子-子滤波器F_i，_j的故障检测统计量为

其中，|·|表示取向量的模；

(16)成员k计算子滤波器F_i，i＝1,2,…,a下属子-子滤波F_i,j，j＝1,2,…,b的检测门限T_i,j；

(17)将子滤波器F_i，i＝1,2,…,a下属子-子滤波器F_i,j，j＝1,2,…,b的故障检验统计量d_i,j与检测门限T_i,j比较，得到子滤波器F_i下属子-子滤波器的检验统计量d_i,j均小于检测门限T_i,j的子滤波个数c，令其为正常子滤波器G_i，i＝1,2,…,c，并得到正常子滤波器弃用的量测信息编号集合V_k，集合V_k中第1个元素为正常子滤波器G₁弃用的量测信息编号，第i个元素为正常子滤波器G_i弃用的量测信息编号，第c个元素为正常子滤波器G_c弃用的量测信息编号；

(18)根据步骤(17)获得的正常子滤波器G_i，i＝1,2,…,c判断成员k的故障量测信息个数z和故障量测信息编号，若c＝1，则z＝m，正常子滤波器G₁弃用的量测信息编号为故障量测信息编号，并存入故障量测信息编号集合W_k；若c＞1，则计算集合V_k中所有元素中均出现的量测信息编号的个数e，令z＝e，集合V_k中所有元素中均出现的量测信息编号为故障量测信息编号，并存入故障量测信息编号集合W_k；

(19)成员k根据故障量测信息编号集合W_k判断所有可用量测量中的故障卫星及故障协同成员，将z个故障量测信息排除后利用剩余n-z个量测信息和惯导信息重构卡尔曼滤波，并进行滤波修正，输出组合导航解算结果，转步骤(21)；

(20)成员k利用n个量测信息和惯导信息进行卡尔曼滤波，并进行滤波修正，输出组合导航解算结果；

(21)判断无人飞行器是否着陆，若是，则停止计算；否则，t+1，且返回步骤(1)，整个无人蜂群协同导航系统进行下一时刻的故障检测及排除。

进一步地，步骤(1)所述的SINS误差状态建模是以东北天地理坐标系为导航坐标系，建立系统状态方程为：

其中，状态向量X(t)为：

其中，

为东向、北向、天向数字平台误差角，δv_e,δv_n,δv_u为东向、北向、天向速度误差，δL,δλ,δh为东向、北向、天向位置误差，ε_bx,ε_by,ε_bz为东向、北向、天向陀螺常值漂移误差，ε_rx,ε_ry,ε_rz为陀螺东向、北向、天向一阶马尔可夫漂移误差，

为东向、北向、天向加速度计零偏误差；Φ(t)为系统一步状态转移矩阵；Γ(t)为系统误差矩阵；W(t)为系统误差白噪声矢量。

进一步地，所述步骤(4)包括以下步骤：

(4.1)基于导航卫星量测信息建立卫星量测方程为：

Z_s(t)＝H_s(t)X(t)+V_s(t)

其中，Z_s(t)为各卫星伪距与惯导计算伪距差值，H_s(t)为各卫星量测系数矩阵，X(t)为18维惯导系统状态向量，V_s(t)为卫星量测噪声阵，设其为均值为0，方差为

的白噪声；

(4.2)基于协同成员量测信息得到飞行器距离差方程为：

δd_i＝d_Ii-d_ci＝e_i1δx+e_i2δy+e_i3δz+υ_ci

其中，d_ci为成员k与其可用协同成员i，i＝1,2,…,y_k的测量距离，y_k为可用协同成员个数，d_Ii为成员k与其协同成员i的计算距离，

(x_I,y_I,z_I)为成员k惯导系统相对于ECEF坐标系的位置，(x_ci,y_ci,z_ci)为协同成员i的位置，(δx,δy,δz)为惯导系统在ECEF坐标系下的三维位置误差，υ_ci为成员k与可用协同成员i的等效测距误差，设其为均值为0，方差为

的白噪声；

其中，

(x,y,z)为成员k的位置真值，(x_ci,y_ci,z_ci)为协同成员i的位置；

(4.3)建立飞行器量测模型为Z_c(t)＝H_c(t)X(t)+V_c(t)，其中，

λ,L,h分别为惯导输出的经纬高；

(4.4)将卫星和飞行器量测方程结合，建立卫星-飞行器量测模型量测模型为：

其中，V(t)为系统量测噪声阵，其协方差矩阵R为

进一步地，步骤(5)所述q_k为预设好的保证组合导航系统卡尔曼滤波器工作稳定的最小观测量的个数。

进一步地，步骤(8)所述子滤波器F_i为弃用m个量测信息后利用n-m个量测信息和惯导信息进行卡尔曼滤波，并得到各子滤波器F_i弃用的量测信息编号集合为U_k，U_k各元素为{1,2,…,m},{1,3，…,m+1},…,{n-m+1,…,n-1,n}。

进一步地，所述步骤(11)包括以下步骤：

(11.1)根据解分离矢量dX_i计算其协方差矩阵dP_i为：

其中，P₀为主滤波器F₀状态向量X₀的协方差矩阵，P_i为子滤波器F_i状态向量X_i的协方差矩阵，

为状态向量X₀与状态向量X_i的协方差矩阵；

(11.2)计算水平位置协方差矩阵

并计算

的各特征值，令其最大特征值为

(11.3)计算子滤波器F_i的检测门限为

其中P_FA为系统设定的误警概率。

进一步地，步骤(13)所述子-子滤波器F_i,j为除弃用子滤波器F_i弃用的m个量测信息外，还弃用另外1个量测信息后利用n-m-1个量测信息和惯导信息进行卡尔曼滤波。

进一步地，所述步骤(16)包括以下步骤：

(16.1)根据解分离矢量dX_i,j计算其协方差矩阵dP_i,j为：

其中，P_i为子滤波器F_i状态向量X_i的协方差矩阵，P_i,j为子-子滤波器F_i,j状态向量X_i,j的协方差矩阵，

为状态向量X_i与状态向量X_i,j的协方差矩阵；

(16.2)计算水平位置协方差矩阵

并计算

的各特征值，令其最大特征值为

(16.3)计算子-子滤波F_i,_j的检测门限为

其中P_FA为系统设定的误警概率。

有益效果：与现有技术相比，本发明的有益效果：本发明考虑了无人蜂群导航系统可能出现的多故障情况，构造不同层次的滤波器实现多个故障量测信息的分离，可灵活检测协同导航系统故障量测信息的个数，无需改变层次滤波结构，具有很好的适应性；本发明将无人机蜂群间的协同信息与无人机自身导航系统量测信息融合，建立无人蜂群协同容错体系架构，除检测导航卫星故障，也同步进行协同量测信息的故障诊断，以保证所有可用量测信息的可靠性，实现无人蜂群导航系统的容错重构；本发明适用性广，不受协同辅助信息种类和数量的限制。

附图说明

图1是本发明的流程图；

图2是无人蜂群协同导航多故障检测系统的结构示意图；

图3是蜂群无人飞行器飞行航迹图；

图4是未采本发明时无人机蜂群惯导RMSE位置误差图；

图5是无人飞行器1所有可用量测信息编号示意图；

图6是子滤波器F₁下属子-子滤波器F_1,1、F_1,2和F_1,3的检测函数曲线图；

图7是子滤波器F₃下属子-子滤波器F_3,1、F_3,2和F_3,3的检测函数曲线图；

图8是子滤波器F₅下属子-子滤波器F_5,1、F_5,2和F_5,3的检测函数曲线图；

图9是子滤波器F₆下属子-子滤波器F_6,1、F_6,2和F_6,3的检测函数曲线图；

图10是子滤波器F₇下属子-子滤波器F_7,1、F_7,2和F_7,3的检测函数曲线图；

图11是采用本发明后无人飞行器1的故障检测结果图；

图12是未采用本发明和采用本发明后的无人飞行器1导航误差曲线对比图。

具体实施方式

下面结合附图对本发明作进一步详细说明。

本发明提供一种无人蜂群协同导航多故障检测方法，利用无人蜂群协同信息辅助进行无人飞行器导航系统多故障检测与识别；结合各无人机相对量测信息与无人机自身导航观测信息进行协同容错架构设计，利用不同量测信息的组合构造多级卡尔曼滤波器，通过不同滤波器解的差异性检测无人机导航卫星及协同量测信息的故障状态，并将故障量测隔离以实现无人机协同导航系统的容错重构。如图1所示，具体步骤如下所示：

步骤(1)：令当前时刻为t，设无人蜂群中的成员数量为N，无人蜂群各成员根据自身的机载惯导系统进行SINS误差状态建模。

以东北天地理坐标系为导航坐标系，建立系统状态方程为：

其中，状态向量X(t)为：

其中，

为东向、北向、天向数字平台误差角，δv_e,δv_n,δv_u为东向、北向、天向速度误差，δL,δλ,δh为东向、北向、天向位置误差，ε_bx,ε_by,ε_bz为东向、北向、天向陀螺常值漂移误差，ε_rx,ε_ry,ε_rz为陀螺东向、北向、天向一阶马尔可夫漂移误差，

为东向、北向、天向加速度计零偏误差；Φ(t)为系统一步状态转移矩阵；Γ(t)为系统误差矩阵；W(t)为系统误差白噪声矢量。

步骤(2)：无人蜂群各成员分别通过自身卫星导航接收机确定各自的可用导航卫星个数并获得各卫星量测信息。

步骤(3)：无人蜂群各成员根据其他成员的定位精度，选择定位精度较高的基准无人机作为可用协同成员，并获得与可用协同成员的相对测量信息。

步骤(4)：针对无人蜂群任意一个成员k，k＝1,2,…,N，成员k根据步骤(2)获得可用导航卫星量测信息和步骤(3)获得的可用协同成员量测信息建立卫星/飞行器量测模型，包括以下子步骤：

(4.1)根据步骤(2)获得可用导航卫星伪距量测信息建立卫星量测方程为：

Z_s(t)＝H_s(t)X(t)+V_s(t)

其中，Z_s(t)为各卫星伪距与惯导计算伪距差值，H_s(t)为各卫星量测系数矩阵，X(t)为18维惯导系统状态向量，V_s(t)为卫星量测噪声阵，设其为均值为0，方差为

的白噪声。

(4.2)根据步骤(3)获得的可用协同成员量测信息得到飞行器距离差方程为：

δd_i＝d_Ii-d_ci＝e_i1δx+e_i2δy+e_i3δz+υ_ci

其中，d_ci为成员k与其可用协同成员i，i＝1,2,…,y_k的测量距离，y_k为可用协同成员个数，d_Ii为成员k与其协同成员i的计算距离，

(x_I,y_I,z_I)为成员k惯导系统相对于ECEF坐标系的位置，(x_ci,y_ci,z_ci)为协同成员i的位置，(δx,δy,δz)为惯导系统在ECEF坐标系下的三维位置误差，υ_ci为成员k与可用协同成员i的等效测距误差，设其为均值为0，方差为

的白噪声；

其中，

(x,y,z)为成员k的位置真值，(x_ci,y_ci,z_ci)为协同成员i的位置。

(4.3)建立飞行器量测模型为Z_c(t)＝H_c(t)X(t)+V_c(t)，

其中，

其中，λ,L,h分别为惯导输出的经纬高。

(4.4)将卫星和飞行器量测方程结合，建立卫星/飞行器量测模型量测模型为

其中，V(t)为系统量测噪声阵，其协方差矩阵R为

步骤(5)：成员k计算其可用量测量总个数为n＝x_k+y_k，其中x_k为成员k的可用导航卫星数，y_k为成员k的可用协同成员数，并将成员k的可用量测量个数n与预设的阈值q_k比较；若n-1＞q_k，则成员k的导航系统转入故障检测模式，执行步骤(6)，否则，成员k的导航系统转入可用检测量不足告警模式，转步骤(20)；所述q_k为预设好的保证组合导航系统卡尔曼滤波器工作稳定的最小观测量的个数，本实施例中x_k＝4，y_k＝2，q_k＝2。

步骤(6)：根据步骤(5)所述的阈值q_k计算成员k的最大可检测故障数m＝x_k+y_k-q_k-1，并将所有可用卫星进行编号为i，i＝1,2,…,x_k，将所有可用协同成员进行编号为i，i＝x_k+1,…,x_k+y_k。

步骤(7)：成员k对所有可用卫星和协同成员量测信息的故障状态进行同步检测，根据x_k个卫星及y_k个协同成员量测信息和惯导信息构造主滤波器F₀，进行卡尔曼滤波后得到状态向量X₀及协方差矩阵P₀。

步骤(8)：成员k根据步骤(6)获得的最大可检测故障数m计算子滤波器个数

并构造子滤波器F_i，i＝1,2,…,a，所述子滤波器F_i为弃用m个量测信息后利用n-m个量测信息和惯导信息进行卡尔曼滤波，如图2所示，得到各子滤波器F_i弃用的量测信息编号集合为U_k，U_k各元素为{1,2,…,m},{1,3，…,m+1},…,{n-m+1,…,n-1,n}。

步骤(9)：成员k根据步骤(8)子滤波器F_i，i＝1,2,…,a的量测信息和惯导信息共进行a次卡尔曼滤波，得到各子滤波器F_i的状态向量X_i、协方差矩阵P_i。

步骤(10)：根据步骤(7)获得的主滤波器状态向量X₀和步骤(9)获得的子滤波器F_i状态向量X_i，i＝1,2,…,a构造主滤波器与各子滤波器的解分离矢量dX_i＝X₀-X_i，得到水平位置解分离矢量为

计算子滤波器F_i的故障检测统计量为

其中，|·|表示取向量的模。

步骤(11)：成员k计算子滤波器F_i，i＝1,2,…,a的检测门限T_i，包括以下子步骤：

(11.1)根据步骤(10)获得的解分离矢量dX_i计算其协方差矩阵dP_i为

其中，P₀为主滤波器F₀状态向量X₀的协方差矩阵，P_i为子滤波器F_i状态向量X_i的协方差矩阵，

为状态向量X₀与状态向量X_i的协方差矩阵。

(11.2)计算水平位置协方差矩阵

并计算

的各特征值，令其最大特征值为

(11.3)计算子滤波器F_i的检测门限为

其中P_FA为系统设定的误警概率。

步骤(12)：将子滤波器F_i的故障检验统计量d_i与检测门限T_i比较，若存在d_i＞T_i，则成员k的所有量测量中存在故障，转入故障识别模式，执行步骤(13)，否则，转步骤(20)。

步骤(13)：成员k根据步骤(8)获得的子滤波器F_i，i＝1,2,…,a，计算每个子滤波器F_i下属子-子滤波器个数为b＝n-m，并构造子滤波器F_i下属子-子滤波器F_i,j，j＝1,2,…,b，所述子-子滤波器F_i,j为除弃用子滤波器F_i弃用的m个量测信息外，还弃用另外1个量测信息后利用n-m-1个量测信息和惯导信息进行卡尔曼滤波。

步骤(14)：成员k根据步骤(13)子滤波器F_i，i＝1,2,…,a下属子-子滤波器F_i,j，j＝1,2,…,b的量测信息和惯导信息共进行a×b次卡尔曼滤波，得到各子-子滤波器F_i,j的状态向量X_i,j、协方差矩阵P_i,j。

步骤(15)：根据步骤(9)获得的子滤波器状态向量X_i和步骤(14)获得的子-子滤波器状态向量X_i,j构造各子滤波器与其下属子-子滤波器的解分离矢量dX_i,j＝X_i-X_i,j，则水平位置解分离矢量为

计算子-子滤波器F_i，_j的故障检测统计量为

其中，|·|表示取向量的模。

步骤(16)：成员k计算子滤波器F_i，i＝1,2,…,a下属子-子滤波F_i,j，j＝1,2,…,b的检测门限T_i,j，包括以下子步骤：

(16.1)根据步骤(15)获得的解分离矢量dX_i,j计算其协方差矩阵dP_i,j为

其中，P_i为子滤波器F_i状态向量X_i的协方差矩阵，P_i,j为子-子滤波器F_i,j状态向量X_i,j的协方差矩阵，

为状态向量X_i与状态向量X_i,j的协方差矩阵。

(16.2)计算水平位置协方差矩阵

并计算

的各特征值，令其最大特征值为

(16.3)计算子-子滤波器F_i,j的检测门限为

其中P_FA为系统设定的误警概率。

步骤(17)：将子滤波器F_i，i＝1,2,…,a下属子-子滤波器F_i,j，j＝1,2,…,b的故障检验统计量d_i,j与检测门限T_i,j比较，得到子滤波器F_i下属子-子滤波器的检验统计量d_i,j均小于检测门限T_i,j的子滤波个数c，令其为正常子滤波器G_i，i＝1,2,…,c，并得到正常子滤波器弃用的量测信息编号集合V_k，集合V_k中第1个元素为正常子滤波器G₁弃用的量测信息编号，第i个元素为正常子滤波器G_i弃用的量测信息编号，第c个元素为正常子滤波器G_c弃用的量测信息编号。

步骤(18)：根据步骤(17)获得的正常子滤波器G_i，i＝1,2,…,c判断成员k的故障量测信息的个数z和故障量测信息编号，若c＝1，则z＝m，正常子滤波器G₁弃用的量测信息编号为故障量测信息编号，并存入故障量测信息编号集合W_k；若c＞1，则计算集合V_k中所有元素中均出现的量测信息编号个数e，令z＝e，集合V_k中所有元素中均出现的量测信息编号为故障量测信息编号，并存入故障量测信息编号集合W_k。

步骤(19)：成员k根据故障量测信息编号集合W_k判断所有可用量测量中的故障卫星及故障协同成员，将z个故障量测信息排除后利用剩余n-z个量测信息和惯导信息重构卡尔曼滤波，并进行滤波修正，输出组合导航解算结果，转步骤(21)。

步骤(20)：成员k利用n个量测信息和惯导信息进行卡尔曼滤波，并进行滤波修正，输出组合导航解算结果。

步骤(21)：判断无人飞行器是否着陆，若是，则停止计算；否则，t+1，且返回步骤(1)，整个无人蜂群协同导航系统进行下一时刻的故障检测及排除。

为了验证本发明所提出的无人蜂群协同导航多故障检测方法的有效性，进行数字仿真分析。仿真中采用的无人机蜂群中无人飞行器数量为8架，飞行器间相对距离测量精度为5米。图3是蜂群无人飞行器飞行航迹；图4是未采用多故障检测方法时无人机蜂群RMSE位置误差图。以无人飞行器1为待检测飞行器为例，定位精度较高的基准无人飞行器为飞行器3和飞行器6，因此其可用协同成员个数为2。导航过程可见导航卫星个数为4。将无人飞行器1所有可用量测分别编号为1～4号卫星，5～6号协同成员，如图5。在仿真飞行的第800s-1000s时间段1号卫星和2号卫星具有40米的伪距故障，5号协同成员具有30米的伪距故障；第1800-2000s时间段1号卫星和3号卫星具有25米的伪距故障。仿真时，将检测统计量和检测限值由度转换为以米为单位。图6是子滤波器F₁下属子-子滤波器F_1,1、F_1,2和F_1,3的检测函数曲线；图7是子滤波器F₃下属子-子滤波器F_3,1、F₃,₂和F_3,3的检测函数曲线；图8是子滤波器F₅下属子-子滤波器F_5,1、F_5,2和F_5,3的检测函数曲线；图9是子滤波器F₆下属子-子滤波器F_6,1、F_6,2和F_6,3的检测函数曲线；图10是子滤波器F₇下属子-子滤波器F_7,1、F_7,2和F_7,3的检测函数曲线；图11是采用本发明方法后无人飞行器1的故障检测结果；图12是未采用本发明方法和采用本发明方法后的无人飞行器1导航误差曲线对比图。由图6-图10可以看出，在第800-1000s时间段，只有子滤波器3下属子-子滤波器的检测统计量均小于检测门限，该子滤波器弃用1号卫星、2号卫星和5号协同成员为故障量测信息；在第1800-2000s时间段，子滤波器1、子滤波器5、子滤波器6和子滤波器7下属子-子滤波器的检测统计量均小于检测门限，由子滤波器1弃用1号、2号和3号量测信息，子滤波器5弃用1号、3号和4号量测信息，子滤波器6弃用1号、3号和5号量测信息，子滤波器7弃用1号、3号和6号量测信息，可得1号卫星和3号卫星为故障量测信息；由图11可以看出，采用本发明方法后，无人飞行器1在飞行过程能检测不同的故障量测个数和编号，具有很好的适应性；由图12可以看出，采用本实施例所提出的一种无人蜂群协同导航多故障检测方法后，无人飞行器1的位置误差保持较小，无人机蜂群系统具有较好的容错能力。

以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (8)

1.一种无人蜂群协同导航多故障检测方法，其特征在于，包括如下步骤：

(1)当前时刻为t，无人蜂群中的成员数量为N，无人蜂群各成员根据自身的机载惯导系统进行SINS误差状态建模；

(2)无人蜂群各成员分别通过自身卫星导航接收机确定各自的可用导航卫星个数并获得各卫星量测信息；

(3)无人蜂群各成员根据其他成员的定位精度，选择定位精度较高的基准无人机作为可用协同成员，并获得与可用协同成员的相对测量信息；

(4)针对无人蜂群任意一个成员k，k＝1,2,…,N，成员k可用导航卫星量测信息和可用协同成员量测信息建立卫星-飞行器量测模型；

(5)成员k计算其可用量测量总个数为n＝x_k+y_k，其中x_k为成员k的可用导航卫星数，y_k为成员k的可用协同成员数，并将成员k的可用量测量个数n与预设的阈值q_k比较；若n-1＞q_k，则成员k的导航系统转入故障检测模式，执行(6)，否则，成员k的导航系统转入可用检测量不足告警模式，转步骤(20)；

(6)根据阈值q_k计算成员k的最大可检测故障数m＝x_k+y_k-q_k-1，并将可用卫星进行编号为i，i＝1,2,…,x_k，将所有可用协同成员进行编号为i，i＝x_k+1,…,x_k+y_k；

(7)成员k对所有可用卫星和协同成员量测信息的故障状态进行同步检测，根据x_k个卫星及y_k个协同成员量测信息和惯导信息构造主滤波器F₀，进行卡尔曼滤波后得到状态向量X₀及协方差矩阵P₀；

(8)成员k根据步骤(6)获得的最大可检测故障数m计算子滤波器个数

并构造子滤波器F_i，i＝1,2,…,a；

(9)成员k根据子滤波器F_i的量测信息和惯导信息共进行a次卡尔曼滤波，得到各子滤波器F_i的状态向量X_i、协方差矩阵P_i；

(10)根据主滤波器状态向量X₀和子滤波器F_i状态向量X_i，i＝1,2,…,a构造主滤波器与各子滤波器的解分离矢量dX_i＝X₀-X_i，得到水平位置解分离矢量为

计算子滤波器F_i，i＝1,2,…,a的故障检测统计量为

其中，|·|表示取向量的模；

(11)成员k计算子滤波器F_i，i＝1,2,…,a的检测门限T_i；

(12)将子滤波器F_i的故障检验统计量d_i与检测门限T_i比较，若存在d_i＞T_i，则成员k的所有量测量中存在故障，转入故障识别模式，执行步骤(13)，否则，转步骤(20)；

(13)成员k根据子滤波器F_i，i＝1,2,…,a，计算各子滤波器F_i下属子-子滤波器个数为b＝n-m，并构造子滤波器F_i下属子-子滤波器F_i,j，j＝1,2,…,b；

(14)成员k根据步骤((13)子滤波器F_i，i＝1,2,…,a下属子-子滤波器F_i,j，j＝1,2,…,b的量测信息和惯导信息共进行a×b次卡尔曼滤波，得到各子-子滤波器F_i,j的状态向量X_i,j、协方差矩阵P_i,j；

(15)根据子滤波器F_i状态向量X_i和子-子滤波器F_i,j状态向量X_i,j构造各子滤波器与其下属子-子滤波器的解分离矢量dX_i,j＝X_i-X_i,j，得到水平位置解分离矢量为

计算子-子滤波器F_i，j的故障检测统计量为

其中，|·|表示取向量的模；

(16)成员k计算子滤波器F_i，i＝1,2,…,a下属子-子滤波F_i,_j，j＝1,2,…,b的检测门限T_i,j；

(17)将子滤波器F_i，i＝1,2,…,a下属子-子滤波器F_i,j，j＝1,2,…,b的故障检验统计量d_i,j与检测门限T_i,j比较，得到子滤波器F_i下属子-子滤波器的检验统计量d_i,j均小于检测门限T_i,j的子滤波个数c，令其为正常子滤波器G_i，i＝1,2,…,c，并得到正常子滤波器弃用的量测信息编号集合V_k，集合V_k中第1个元素为正常子滤波器G₁弃用的量测信息编号，第i个元素为正常子滤波器G_i弃用的量测信息编号，第c个元素为正常子滤波器G_c弃用的量测信息编号；

(18)根据步骤(17)获得的正常子滤波器G_i，i＝1,2,…,c判断成员k的故障量测信息个数z和故障量测信息编号，若c＝1，则z＝m，正常子滤波器G₁弃用的量测信息编号为故障量测信息编号，并存入故障量测信息编号集合W_k；若c＞1，则计算集合V_k中所有元素中均出现的量测信息编号的个数e，令z＝e，集合V_k中所有元素中均出现的量测信息编号为故障量测信息编号，并存入故障量测信息编号集合W_k；

(19)成员k根据故障量测信息编号集合W_k判断所有可用量测量中的故障卫星及故障协同成员，将z个故障量测信息排除后利用剩余n-z个量测信息和惯导信息重构卡尔曼滤波，并进行滤波修正，输出组合导航解算结果，转步骤(21)；

(20)成员k利用n个量测信息和惯导信息进行卡尔曼滤波，并进行滤波修正，输出组合导航解算结果；

(21)判断无人飞行器是否着陆，若是，则停止计算；否则，t+1，且返回步骤(1)，整个无人蜂群协同导航系统进行下一时刻的故障检测及排除。

2.根据权利要求1所述的无人蜂群协同导航多故障检测方法，其特征在于，步骤(1)所述的SINS误差状态建模是以东北天地理坐标系为导航坐标系，建立系统状态方程为：

其中，状态向量X(t)为：

其中，

为东向、北向、天向数字平台误差角，δv_e,δv_n,δv_u为东向、北向、天向速度误差，δL,δλ,δh为东向、北向、天向位置误差，ε_bx,ε_by,ε_bz为东向、北向、天向陀螺常值漂移误差，ε_rx,ε_ry,ε_rz为陀螺东向、北向、天向一阶马尔可夫漂移误差，

为东向、北向、天向加速度计零偏误差；Φ(t)为系统一步状态转移矩阵；Γ(t)为系统误差矩阵；W(t)为系统误差白噪声矢量。

3.根据权利要求1所述的无人蜂群协同导航多故障检测方法，其特征在于，所述步骤(4)包括以下步骤：

(4.1)基于导航卫星量测信息建立卫星量测方程为：

Z_s(t)＝H_s(t)X(t)+V_s(t)

其中，Z_s(t)为各卫星伪距与惯导计算伪距差值，H_s(t)为各卫星量测系数矩阵，X(t)为18维惯导系统状态向量，V_s(t)为卫星量测噪声阵，设其为均值为0，方差为

的白噪声；

(4.2)基于协同成员量测信息得到飞行器距离差方程为：

δd_i＝d_Ii-d_ci＝e_i1δx+e_i2δy+e_i3δz+υ_ci

其中，d_ci为成员k与其可用协同成员i，i＝1,2,…,y_k的测量距离，y_k为可用协同成员个数，d_Ii为成员k与其协同成员i的计算距离，

(x_I,y_I,z_I)为成员k惯导系统相对于ECEF坐标系的位置，(x_ci,y_ci,z_ci)为协同成员i的位置，(δx,δy,δz)为惯导系统在ECEF坐标系下的三维位置误差，υ_ci为成员k与可用协同成员i的等效测距误差，设其为均值为0，方差为

的白噪声；

其中，

(x,y,z)为成员k的位置真值，(x_ci,y_ci,z_ci)为协同成员i的位置；

(4.3)建立飞行器量测模型为Z_c(t)＝H_c(t)X(t)+V_c(t)，

其中，

λ,L,h分别为惯导输出的经纬高；

(4.4)将卫星和飞行器量测方程结合，建立卫星-飞行器量测模型量测模型为：

其中，V(t)为系统量测噪声阵，其协方差矩阵R为

4.根据权利要求1所述的无人蜂群协同导航多故障检测方法，其特征在于，步骤(5)所述q_k为预设好的保证组合导航系统卡尔曼滤波器工作稳定的最小观测量的个数。

5.根据权利要求1所述的无人蜂群协同导航多故障检测方法，其特征在于，步骤(8)所述子滤波器F_i为弃用m个量测信息后利用n-m个量测信息和惯导信息进行卡尔曼滤波，并得到各子滤波器F_i弃用的量测信息编号集合为U_k，U_k各元素为{1,2,…,m},{1,3，...,m+1},…,{n-m+1,…,n-1,n}。

6.根据权利要求1所述的无人蜂群协同导航多故障检测方法，其特征在于，所述步骤(11)包括以下步骤：

(11.1)根据解分离矢量dX_i计算其协方差矩阵dP_i为：

其中，P₀为主滤波器F₀状态向量X₀的协方差矩阵，P_i为子滤波器F_i状态向量X_i的协方差矩阵，P_i ^cross为状态向量X₀与状态向量X_i的协方差矩阵；

(11.2)计算水平位置协方差矩阵dP_i ^hpos＝dP_i[7:8,7:8]，并计算dP_i ^hpos的各特征值，令其最大特征值为λ_i ^max；

(11.3)计算子滤波器F_i的检测门限为

其中P_FA为系统设定的误警概率。

7.根据权利要求1所述的无人蜂群协同导航多故障检测方法，其特征在于，步骤(13)所述子-子滤波器F_i,j为除弃用子滤波器F_i弃用的m个量测信息外，还弃用另外1个量测信息后利用n-m-1个量测信息和惯导信息进行卡尔曼滤波。

8.根据权利要求1所述的无人蜂群协同导航多故障检测方法，其特征在于，所述步骤(16)包括以下步骤：

(16.1)根据解分离矢量dX_i,j计算其协方差矩阵dP_i,j为：

其中，P_i为子滤波器F_i状态向量X_i的协方差矩阵，P_i,j为子-子滤波器F_i,j状态向量X_i,j的协方差矩阵，

为状态向量X_i与状态向量X_i,j的协方差矩阵；

(16.2)计算水平位置协方差矩阵

并计算

的各特征值，令其最大特征值为

(16.3)计算子-子滤波F_i,j的检测门限为

其中P_FA为系统设定的误警概率。

Images