CN115236700A - 基于卫星伪距的因子图导航故障检测方法 - Google Patents

Abstract

本发明公开一种基于卫星伪距的因子图导航故障检测方法，首先基于全球卫星导航系统原始伪距数据与惯性导航系统数据，通过故障检验方法检测卫星伪距信息故障、惯性导航故障或大气、视觉等其他导航系统故障，基于卫星伪距数据的检验判断出卫星原始数据中故障存在的位置。接着根据检测的信息隔离故障传感器量测，若判定卫星伪距信息存在故障，则隔离该卫星伪距数据后重新进行解算，提高卫星定位的准确性；若惯性导航或大气、视觉等其他导航出现故障，则在因子图模型中隔离掉对应的因子图节点，防止故障信息污染融合结果。最后，在实时隔离相应的故障节点后，基于组合导航系统的因子图架构结合故障诊断信息进行自适应融合，在此基础上提升因子图导航系统融合的精度和可靠性。

Description

基于卫星伪距的因子图导航故障检测方法

技术领域

本发明属于组合导航技术领域，特别涉及了一种无人系统的因子图导航故障检测与自适应隔离方法。

背景技术

近些年，无人系统凭借其巨大的应用市场和潜在的扩展领域受到世界各国的高度重视，无人系统技术在军事领域和民用领域均获得了广泛应用。无人系统由于具有无人驾驶、远程操控的能力，在海上搜救、森林火灾救援、能源探测、商业运输等领域得到了广泛的应用。导航系统是各类无人系统安全而准确完成任务的保证，其主要目的是实时提供精确的位置、速度与姿态等相关参数或信息。由于各类干扰的存在，并且导航传感器也有一定的寿命，传感器会出现误差较大，甚至故障无法使用的情况。若是无法及时地判断和隔离，受到故障传感器量测的污染，组合导航系统无法正常工作，导航系统的故障检测技术也成为导航与定位的关键环节，及时有效的地检测出传感器的故障是国内研究的重要方向之一。

无人系统的定位主要由机载的导航系统完成，其能够实时输出无人系统的位置、速度和姿态信息，为系统提供准确的位置坐标和状态信息。在无人系统导航系统中，各传感器的导航原理不同，存在极强的互补性。在实际应用中，由于不同传感器的更新频率不同，存在时间不同步的问题，采用固定的滤波结构和方法均很难满足这种复杂多变的应用需求，因子图灵活易变的滤波结构更适合多源组合导航滤波融合方法。

通常情况下惯性导航系统(INS)不受外界环境的干扰，在故障检测系统中一般作为通用的参考系统被认为是可靠的。而全球导航卫星系统(GNSS)的测量容易受到外部干扰，包括多径效应、卫星失锁、卫星欺骗等，卫星信号可能包含较大的误差甚至故障无法使用。但INS并不是觉得可靠的，在部分使用环境下，如在受强气流干扰的复杂气流环境中，无人系统由于其气动特性，极易发生振动，耦合的惯性导航系统也可能会出现较大的解算误差或者故障。及时有效的检测出导航系统的故障，并进行故障定位和隔离，结合因子图模型下的即插即用原理，可以有效的提高导航系统的鲁棒性。

发明内容

为了解决上述背景技术提到的技术问题，本发明提出了无人系统的因子图导航故障检测与自适应隔离方法。为了实现上述技术目的，本发明的技术方案为：

基于卫星伪距的因子图导航故障检测方法，所述检测方法包括以下步骤：

S1，根据无人系统的组合导航架构、任务需求以及所处环境设计载体的运动动作和运动轨迹，并确定无人系统各导航传感器的类型，采集各传感器数据；

S2，将无人系统的位置速度值设为状态向量，采用卫星伪距数据为量测向量，构建基于卡方检验的因子图导航信息故障检测模型；

S3，基于步骤S2检测出的故障，对故障卫星节点进行隔离，基于因子节点表达式建立联合概率分布函数，并根据所述联合概率分布函数获得状态变量的估计，经过实时滤波估计和修正，完成多源导航信息的有效融合。

进一步的，S2中所述基于卡方检验的因子图导航信息故障检测模型的架构包含主滤波器和N个子滤波器，所述主滤波器用于处理所有卫星的伪距数据，在主滤波器检测到故障后，启动N个子滤波器，每个子滤波器分别处理所有伪距量测中移除一颗可见卫星伪距之后，若子滤波器检测正常，则被移除的卫星伪距存在故障，若所有子滤波器检测到故障，则在此基础上再移除一颗卫星伪距，依此类推，当所有卫星上未检测到错误时，判断惯性测量单元Inertial Measurement Unit,IMU有故障；

若IMU无故障，则将其输出值作为检测大气、视觉等其他导航系统的参考值，完成组合导航系统故障的有效检测。

进一步的，所述步骤S2的具体过程如下：

S2a，定义无人系统的导航系统状态模型为非线性模型，描述为如下所示：

其中X_i是状态向量，f()是状态转移矩阵，Γ_i-1是噪声系数矩阵，Z_i是量测向量，h()是量测矩阵，W_i是过程噪声，V_i是量测噪声，i为时间序列；

S2b，构建主滤波器F₀₀，选取所有卫星伪距信息为量测量，计算基于惯性测量单元的无人系统位置与卫星星历之间的估计伪距，构建卫星量测伪距与估计伪距之间的残差值，为：

其中

为基于上一时刻状态向量估计的本时刻状态向量，r_i为残差，当系统无故障时，为零均值高斯白噪声，残差的协方差阵为

其中H_i是量测方程的雅克比矩阵，为

R_i为量测噪声的协方差矩阵，P_i|i-1是一步预测均方误差方程，为

其中Φ_i,i-1状态方程的雅克比矩阵，为

Q_i为状态噪声的协方差矩阵P_i-1为均方误差矩阵；

则故障检测函数为

其中λ_i服从自由度m的χ²分布，m为量测向量Z_i的维度，写为λ_i～χ²(m)，根据λ_i的值判断是系统存在故障；

S2c，启动N个子滤波器{F_0n}_{n∈[1,2,L,N]}，每个子滤波器分别处理所有伪距量测中移除一颗可见卫星伪距之后，重复S2b的步骤，检测准则为

其中

为各滤波检测估计值，其中D_0n是一个检测门限，D_0n根据N个量测函数的虚警概率P_fa计算得到；

若子滤波器检测正常，则被移除的卫星伪距存在故障；若所有子滤波器检测到故障，则在此基础上再移除一颗卫星伪距，再次重复S2b的步骤，依此类推；当所有卫星上未检测到错误时，则判断IMU有故障；

在IMU无故障时，将IMU量测值作为参考信息检测大气、视觉等其他导航系统是否存在故障，重复步骤S2a-S2b。

更进一步的，所述根据λ_i的值判断是系统存在故障具体为：

其中T_D为检测门限，通过与检测门限的对比，判断卫星伪距或惯性导航是否存在故障。

进一步的，所述步骤S3具体为：所述导航系统状态变量X如下：

上式中，

为平台误差角，δv_E，δv_N，δv_U为东北天方向的速度误差，δL，δλ，δh为纬度、经度、高度位置误差；

无人系统导航估计的状态量是量测Z(t)发生条件下X(t)发生的最大概率，计算联合分布概率函数的最大后验概率估计值：

其中h_i(X_i)为观测方程，z_i为真实量测量，∑_i为协方差矩阵，S(r_i ^k)为权重函数，根据故障检测结果，隔离部分严重故障因子，调整融合权重；

对非线性观测方程h_i(X_i)进行一阶泰勒展开实现线性化，得到状态更新向量：

其中H_i是观测雅克比矩阵，Δ^*为估计的状态更新向量，Δ_i为i时刻的状态更新向量；求取状态更新向量后计算得出基于因子图的无人系统的速度、位置融合信息。

进一步的，所述各传感器数据，包括通过卫星接收机采集卫星原始伪距数据，查询相应时刻的卫星星历数据，解算相应时刻的卫星导航数据。

有益效果

基于卫星伪距的因子图导航故障检测与自适应隔离方法将卫星伪距信息作为量测量进行故障检测，能够判断和定位具体故障卫星伪距的位置，通过故障检测与隔离后，基于故障检测后的因子图模型进行融合，增强无人系统的检测能力，提升无人系统的导航量测精度与鲁棒性，精确隔离故障因子节点以达到修正导航状态的效果。

附图说明

图1为本发明故障检测滤波器架构示意图；

图2为本发明基于因子图的导航系统模型示意图；

图3为本发明的整体流程图。

具体实施方式

以下将结合附图，对本发明的技术方案进行详细说明。

本实施例为基于卫星伪距的因子图导航故障检测方法，所述检测方法包括以下步骤：

S1，根据无人系统的组合导航架构、任务需求以及所处环境设计载体的运动动作和运动轨迹，并确定无人系统各导航传感器的类型，采集各传感器数据；

S2，将无人系统的位置速度值设为状态向量，采用卫星伪距数据为量测向量，构建基于卡方检验的因子图导航信息故障检测模型；

S3，基于步骤S2检测出的故障，对故障卫星节点进行隔离，基于因子节点表达式建立联合概率分布函数，并根据所述联合概率分布函数获得状态变量的估计，经过实时滤波估计和修正，完成多源导航信息的有效融合。

进一步的，S2中所述基于卡方检验的因子图导航信息故障检测模型的架构包含主滤波器和N个子滤波器，所述主滤波器用于处理所有卫星的伪距数据，在主滤波器检测到故障后，启动N个子滤波器，每个子滤波器分别处理所有伪距量测中移除一颗可见卫星伪距之后，若子滤波器检测正常，则被移除的卫星伪距存在故障，若所有子滤波器检测到故障，则在此基础上再移除一颗卫星伪距，依此类推，当所有卫星上未检测到错误时，判断惯性测量单元Inertial Measurement Unit,IMU有故障；

若IMU无故障，则将其输出值作为检测大气、视觉等其他导航系统的参考值，完成组合导航系统故障的有效检测。

进一步的，所述步骤S2的具体过程如下：

S2a，定义无人系统的导航系统状态模型为非线性模型，描述为如下所示：

其中X_i是状态向量，f()是状态转移矩阵，Γ_i-1是噪声系数矩阵，Z_i是量测向量，h()是量测矩阵，W_i是过程噪声，V_i是量测噪声，i为时间序列；

S2b，构建主滤波器F₀₀，选取所有卫星伪距信息为量测量，计算基于惯性测量单元的无人系统位置与卫星星历之间的估计伪距，构建卫星量测伪距与估计伪距之间的残差值，为：

其中

为基于上一时刻状态向量估计的本时刻状态向量，r_i为残差，当系统无故障时，为零均值高斯白噪声，残差的协方差阵为

其中H_i是量测方程的雅克比矩阵，为

R_i为量测噪声的协方差矩阵，P_i|i-1是一步预测均方误差方程，为

其中Φ_i,i-1状态方程的雅克比矩阵，为

Q_i为状态噪声的协方差矩阵P_i-1为均方误差矩阵；

则故障检测函数为

其中λ_i服从自由度m的χ²分布，m为量测向量Z_i的维度，写为λ_i～χ²(m)，根据λ_i的值判断是系统存在故障；

S2c，启动N个子滤波器{F_0n}_{n∈[1,2,L,N]}，每个子滤波器分别处理所有伪距量测中移除一颗可见卫星伪距之后，重复S2b的步骤，检测准则为

其中

为各滤波检测估计值，其中D_0n是一个检测门限，D_0n根据N个量测函数的虚警概率P_fa计算得到；

若子滤波器检测正常，则被移除的卫星伪距存在故障；若所有子滤波器检测到故障，则在此基础上再移除一颗卫星伪距，再次重复S2b的步骤，依此类推；当所有卫星上未检测到错误时，则判断IMU有故障；

在IMU无故障时，将IMU量测值作为参考信息检测大气、视觉等其他导航系统是否存在故障，重复步骤S2a-S2b。

更进一步的，所述根据λ_i的值判断是系统存在故障具体为：

其中T_D为检测门限，通过与检测门限的对比，判断卫星伪距或惯性导航是否存在故障。

进一步的，所述步骤S3具体为：所述导航系统状态变量X如下：

上式中，

为平台误差角，δv_E,δv_N,δv_U为东北天方向的速度误差，δL,δλ,δh为纬度、经度、高度位置误差；

无人系统导航估计的状态量是量测Z(t)发生条件下X(t)发生的最大概率，计算联合分布概率函数的最大后验概率估计值：

其中h_i(X_i)为观测方程，z_i为真实量测量，∑_i为协方差矩阵，S(r_i ^k)为权重函数，根据故障检测结果，隔离部分严重故障因子，调整融合权重；

对非线性观测方程h_i(X_i)进行一阶泰勒展开实现线性化，得到状态更新向量：

其中H_i是观测雅克比矩阵，Δ^*为估计的状态更新向量，Δ_i为i时刻的状态更新向量；求取状态更新向量后计算得出基于因子图的无人系统的速度、位置融合信息。

进一步的，所述各传感器数据，包括通过卫星接收机采集卫星原始伪距数据，查询相应时刻的卫星星历数据，解算相应时刻的卫星导航数据

以无人系统机载导航系统为例，首先结合任务需求，基于载体中各导航系统得到传感器量测信息，其中通过全球卫星导航系统获取的是原始伪距数据，接着基于卫星接收机原始伪距信息、惯导信息及大气、视觉等其他导航系统信息进行综合完好性分析，检测并定位故障的位置，可以检测出IMU故障、具体卫星伪距故障、大气导航、视觉导航等其他导航系统故障等，在因子图模型下隔离并去除相应因子节点后，在因子图滤波器中，对导航的结果进行修正，获得更高精度的导航信息。下文将对本发明的整个过程进行详细说明。

1、在无人系统飞行环境下，根据实际情况、任务需求以及所处环境设计无人系统飞行航迹、传感器信号、运动状态等。

此处以无人系统为例，对无人机航迹的设计进行具体的说明。根据任务目标，要设计符合无人系统任务需求、载体所处的复杂环境以及任务特点的飞行轨迹。对于不同任务的需求，例如：军事攻击、抢险救援、线路巡检、快递运送、飞行表演等，其工作环境、飞行动作、任务需求也有很大的不同，飞行构型和飞行航迹也不一样。对于无人系统而言，典型的飞行构型可以分为如下几种：

①三角形；②菱形；③S形；④8字形；⑤梯形；⑥圆形；⑦直线形等组合。

要完成不同的飞行任务，无人系统可以在上述构型中选择一种或多种进行组合。根据上述要求，设计一个符合实际情况的构型和各自的航迹，使其能满足任务要求和实际需要。

2、由于无人系统的任务需求不同，同时考虑成本与无人系统的机载载重的因素，需要以此确定可用的传感器类型。

可用于因子图架构的导航传感器类型众多，根据任务需求及实际情况，对无人系统的导航系统方案进行设计，选用惯性导航传感器、卫星导航传感器、视觉相机、大气数据系统、磁力计作为具体实施传感器选型方案，并通过卫星接收机获得无人系统原始伪距数据，查询相关星历信息。

3、基于卫星伪距的因子图导航故障检测模型建立

对于无人系统因子图导航故障检测模型，将其无人系统的位置、速度值为状态向量，将卫星接收机接收到的伪距值作为量测向量，构建基于卡方故障检验的因子图导航信息故障检测算法模型。

如图1所示，此模型中，首先需要一个主滤波器处理所有卫星的伪距数据，与根据无人系统估计的伪距值之间的差值作为残差对比门限判断。在图中，主滤波器为F₀₀，在主滤波器检测到故障后，启动N个依次移除每一颗可见卫星伪距的子滤波器F₀₁-F_0N，重复检测步骤，判断是否某一颗星伪距出现问题，若无故障再次减掉一颗星F₁₂-F_N,N-1，依此类推，当所有子滤波器上未检测到错误时，判断IMU有故障。若无IMU故障后将其作为参考检测大气、视觉等其他导航系统，完成组合导航系统故障的有效检测。

如图2所示，本发明中，无人系统采用因子图架构作为信息融合方案，在隔离掉相应因子后，进行因子图融合。图中，空心圆圈代表状态变量节点，实心圆圈代表因子节点，X代表系统的导航状态，在位置因子图、速度因子图和姿态因子图中分别为位置状态量、速度状态量和姿态状态量，f代表各传感器量测信息，f_Prior表示先前的量测信息，f_IMU表示自IMU的量测信息，与k时刻和k+1时刻的导航状态相关，f_GNSS、f_ADS、f_EOS也分别是其他导航系统的量测信息。在因子图模型中，建立滤波估计方程，经过实时滤波估计和修正，从而完成多源传感器信息的有效融合。

在图3中，基于卫星伪距的因子图导航故障检测架构将卫星伪距信息作为量测量进行故障检测，能够判断和定位具体故障卫星伪距的位置，通过故障检测与隔离后，基于故障检测后的因子图模型进行融合，增强无人系统的检测能力，提升无人系统的导航量测精度与鲁棒性，精确隔离故障因子节点以达到修正导航状态的效果。

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

Claims (6)

1.基于卫星伪距的因子图导航故障检测方法，其特征在于，所述检测方法包括以下步骤：

S1，根据无人系统的组合导航架构、任务需求以及所处环境设计载体的运动动作和运动轨迹，并确定无人系统各导航传感器的类型，采集各传感器数据；

S2，将无人系统的位置速度值设为状态向量，采用卫星伪距数据为量测向量，构建基于卡方检验的因子图导航信息故障检测模型；

S3，基于步骤S2检测出的故障，对故障卫星节点进行隔离，基于因子节点表达式建立联合概率分布函数，并根据所述联合概率分布函数获得状态变量的估计，经过实时滤波估计和修正，完成多源导航信息的有效融合。

2.根据权利要求1所述的基于卫星伪距的因子图导航故障检测方法，其特征在于，S2中所述基于卡方检验的因子图导航信息故障检测模型的架构包含主滤波器和N个子滤波器，所述主滤波器用于处理所有卫星的伪距数据，在主滤波器检测到故障后，启动N个子滤波器，每个子滤波器分别处理所有伪距量测中移除一颗可见卫星伪距之后，若子滤波器检测正常，则被移除的卫星伪距存在故障，若所有子滤波器检测到故障，则在此基础上再移除一颗卫星伪距，依此类推，当所有卫星上未检测到错误时，判断惯性测量单元InertialMeasurement Unit,IMU有故障；

若IMU无故障，则将其输出值作为检测大气、视觉等其他导航系统的参考值，完成组合导航系统故障的有效检测。

3.根据权利要求1所述的基于卫星伪距的因子图导航故障检测方法，其特征在于，

所述步骤S2的具体过程如下：

S2a，定义无人系统的导航系统状态模型为非线性模型，描述为如下所示：

其中X_i是状态向量，f()是状态转移矩阵，Γ_i-1是噪声系数矩阵，Z_i是量测向量，h()是量测矩阵，W_i是过程噪声，V_i是量测噪声，i为时间序列；

S2b，构建主滤波器F₀₀，选取所有卫星伪距信息为量测量，计算基于惯性测量单元的无人系统位置与卫星星历之间的估计伪距，构建卫星量测伪距与估计伪距之间的残差值，为：

其中

为基于上一时刻状态向量估计的本时刻状态向量，r_i为残差，当系统无故障时，为零均值高斯白噪声，残差的协方差阵为

其中H_i是量测方程的雅克比矩阵，为

R_i为量测噪声的协方差矩阵，P_i|i-1是一步预测均方误差方程，为

其中Φ_i,i-1状态方程的雅克比矩阵，为

Q_i为状态噪声的协方差矩阵P_i-1为均方误差矩阵；

则故障检测函数为

其中λ_i服从自由度m的χ²分布，m为量测向量Z_i的维度，写为λ_i～χ²(m)，根据λ_i的值判断是系统存在故障；

S2c，启动N个子滤波器{F_0n}_{n∈[1,2,L,N]}，每个子滤波器分别处理所有伪距量测中移除一颗可见卫星伪距之后，重复S2b的步骤，检测准则为

其中

为各滤波检测估计值，其中D_0n是一个检测门限，D_0n根据N个量测函数的虚警概率P_fa计算得到；

若子滤波器检测正常，则被移除的卫星伪距存在故障；若所有子滤波器检测到故障，则在此基础上再移除一颗卫星伪距，再次重复S2b的步骤，依此类推；当所有卫星上未检测到错误时，则判断IMU有故障；

在IMU无故障时，将IMU量测值作为参考信息检测大气、视觉等其他导航系统是否存在故障，重复步骤S2a-S2b。

4.根据权利要求3所述的基于卫星伪距的因子图导航故障检测方法，其特征在于，所述根据λ_i的值判断是系统存在故障具体为：

其中T_D为检测门限，通过与检测门限的对比，判断卫星伪距或惯性导航是否存在故障。

5.根据权利要求1所述的基于卫星伪距的因子图导航故障检测方法，其特征在于，所述步骤S3具体为：所述导航系统状态变量X如下：

上式中，

为平台误差角，δv_E,δv_N,δv_U为东北天方向的速度误差，δL,δλ,δh为纬度、经度、高度位置误差；

无人系统导航估计的状态量是量测Z(t)发生条件下X(t)发生的最大概率，计算联合分布概率函数的最大后验概率估计值：

其中h_i(X_i)为观测方程，z_i为真实量测量，∑_i为协方差矩阵，

为权重函数，根据故障检测结果，隔离部分严重故障因子，调整融合权重；

对非线性观测方程h_i(X_i)进行一阶泰勒展开实现线性化，得到状态更新向量：

其中H_i是观测雅克比矩阵，Δ^*为估计的状态更新向量，Δ_i为i时刻的状态更新向量；求取状态更新向量后计算得出基于因子图的无人系统的速度、位置融合信息。

6.根据权利要求1至5任一项所述的基于卫星伪距的因子图导航故障检测方法，其特征在于，所述各传感器数据，包括通过卫星接收机采集卫星原始伪距数据，查询相应时刻的卫星星历数据，解算相应时刻的卫星导航数据。

Images