一种双阈值辅助故障容错kf的组合导航系统及方法
阅读说明：本技术 一种双阈值辅助故障容错kf的组合导航系统及方法 (Combined navigation system and method of double-threshold auxiliary fault-tolerant KF ) 是由 陈光武 袁祎 司涌波 周鑫 李朋朋 邢东峰 石建强 李鹏 刘洋 杨菊花 于 2021-09-15 设计创作，主要内容包括：本发明是一种双阈值辅助故障容错KF的组合导航系统及方法,含有存储器、处理器、数据输入端口,步骤是：通过KF滤波器构造组合导航系统观测量残差,并设定双阈值进行故障检测；当观测量残差小于双阈值最小值时,其无故障发生；当观测量残差超过最大设定阈值时,组合导航系统发生突变野值干扰,利用RBF神经网络预测数据进行组合；当观测量残差处于两阈值中间时,利用最小二乘支持向量机进行拟合预测,然后通过输出值计算实际新息,以实际新息对观测噪声协方差阵进行准确估计,然后减小增益矩阵,削弱故障观测量对状态估计修正过程中所占的权重。有益效果：在一定程度上提高组合导航系统故障容错能力,保证导航结果的准确性及可靠性。(The invention relates to a double-threshold auxiliary fault-tolerant KF integrated navigation system and method, comprising a memory, a processor and a data input port, wherein the steps are as follows: constructing an observation measurement residual error of the integrated navigation system through a KF filter, and setting double thresholds for fault detection; when the observed quantity residual error is smaller than the minimum value of the double thresholds, no fault occurs; when the observed quantity residual exceeds the maximum set threshold, the combined navigation system generates abrupt wild value interference, and the RBF neural network prediction data is used for combination; when the observed quantity residual error is between two thresholds, fitting prediction is carried out by using a least square support vector machine, then actual innovation is calculated through output values, the observation noise covariance matrix is accurately estimated by the actual innovation, then a gain matrix is reduced, and the weight occupied by the fault observed quantity in the state estimation correction process is weakened. Has the advantages that: the fault tolerance capability of the integrated navigation system is improved to a certain extent, and the accuracy and reliability of the navigation result are ensured.)
技术领域
本发明的属于导航技术领域，涉及GNSS/INS组合导航系统。
背景技术
由我国北斗卫星导航、欧洲ALILEO、美国GPS、俄罗斯GLONASS组成的全球导航卫星系统GNSS(Globe Navigation Satellites System)以全天时、全天候探测的优势，在面向位置服务的各个领域中得到了广泛的应用。而惯性导航系统(INS)则以其全天候工作、不易受外界环境干扰等优势称为自主导航的首选。随着导航领域“天地一体”发展思路的提出，组合导航技术的作用也愈加突出，主要通过对两者组合进行优势互补，为运动载体提供高精度、实时并且连续的位置、速度及姿态信息，用以满足众多基于位置服务行业的实际运营需求，以带来极大的便利。通常，GNSS/INS组合导航系统的组合方式根据信息结合的节点不同分为3种，包括松组合、紧组合和深组合方式，其中松组合模式的特点是将GPS和惯导作为两套独立的位置和姿态参数输入到组合导航框架中，组合导航框架与GPS和惯导之间存在反馈回路，最终产生单一的组合导航解。紧组合方式指的GPS和IMU不作为独立的系统，而仅仅作为一个测量传感器；利用IMU输出到位置和速度信息来估计GPS的伪距和伪距率，与GPS输出值相比较，用差值构造观测方程，经卡尔曼滤波，获得精确的导航解。
其中松组合方式便于扩展，得到了广泛的应用。而对于导航信息的融合，则普遍采用卡尔曼滤波算法，但是组合导航系统卫星信号在传播过程中容易受到干扰导致卫星导航观测值出现故障或野值，而卡尔曼滤波方法不具备故障容错能力，这会导致GNSS/INS组合导航系统滤波性能恶化并发散，从而影响整体导航精度。因此，对于组合导航系统具有容错性的要求越来越高，容错技术以故障检测为基础，旨在能够及时辨别故障，将其误差源隔离以提高系统可靠性。而对于故障的种类则存在软故障与硬故障两种，硬故障通过机内自检方法进行检测，而软故障存在载体机动、温度变化或陀螺漂移而引起加速度计漂移，以及由于模型不准确引起的滤波器发散等故障难以进行确定并隔离。目前最常用的是状态卡方检验法和残差检验法，残差检验多用于突变故障，而状态卡方检验适用于缓变故障的检验，有将两者结合起来使用，取得了一定的效果。
发明内容
本发明的目的是提出一种双阈值辅助故障容错KF的组合导航系统，同时提出一种双阈值辅助故障容错KF的组合导航方法，旨在提高组合导航系统故障容错能力，保证导航结果的准确性及可靠性。
本方面的技术方案：一种双阈值辅助故障容错KF的组合导航方法，步骤一：建立组合导航系统的状态方程和量测方程如下：
式(1)中，Xk为状态向量，Φk，k-1为从k-1时刻到k时刻的状态转移矩阵，Γk，k-1为噪声分布矩阵，Wk-1为过程噪声，Zk为观测向量，Vk是观测误差向量，Hk为系数矩阵；
n维系统状态量为
X(t)＝[δλ δL δh δve δvn δvu φe φn φu εbx εby εbz Δax Δay Δaz]，其中，δλ,δL,δh为运动载体经纬度及高度误差；δve,δvn,δvu为载体东、北、天向速度误差；φe,φn,φu为载体三向失准角误差；εbx,εby,εbz为陀螺仪在三轴的随机漂移；Δax,Δay,Δaz为加速度计在三轴的漂移，且E[Wk]＝0，E[Vk]＝0，其中，Wk是系统误差向量，Qk为k时刻的系统过程噪声矩阵，δkj为克朗尼克δ函数，Vk是观测误差向量；Rk为k时刻的观测噪声协方差矩阵；均为线性代数上的矩阵转置(字母右上角带“T”的符号表示对应符号的矩阵转置，以下类似情况不再一一说明)；
步骤二：构建组合导航系统故障检测函数；
由传统卡尔曼滤波过程，可得K时刻系统状态的递推值为：
式(2)中，为k时刻系统状态递推值；为一步预测状态估计；
为k时刻增益矩阵；Zk是系统观测向量；Hk是观测误差系数矩阵；
且K时刻系统观测的预测值为：
式(3)中，Hk为系数矩阵；为一步预测状态估计；
量测与估计之差为新息，即残差序列：
式(4)中，rk为残差序列；Zk为观测向量；为预测观测值；
新息也服从零均值高斯白噪声过程，协方差矩阵为：
Sk＝HkPk,k-1Hk T+Rk (5)
式(5)中，Sk为协方差矩阵；Hk为系数矩阵；Pk,k-1为从k-1时刻到k时刻的状态一步预测协方差矩阵；Rk为k时刻的观测噪声协方差矩阵；Hk T为数学上的转置矩阵；
其中，在组合导航模型和噪声假设均准确的情况下，残差rk服从N(0,Sk)；
构建故障检测函数为ek：
式(6)中，rk为残差序列；Sk为协方差矩阵；
步骤三：设定双阈值门限进行故障判别：
其中，TD1和TD2分别为故障检测的上下门限；
步骤四：当观测量残差小于双阈值最小值TD1，系统无故障发生；当观测量残差超过双阈值最大值TD2时，系统发生突变故障或野值，对故障信息源进行隔离，并通过构造新的观测值进行融合；当观测量残差处于两阈值中间(大于TD1，且小于TD2)时，利用训练好的RBF网络进行预测，通过输出值计算实际新息，以实际新息对观测噪声协方差阵进行准确估计，然后减小增益矩阵，从而削弱故障观测量对状态估计修正过程中所占的权重，提高诊断期间组合导航系统的滤波精度。
所述步骤三中，TD1和TD2故障检测的上、门限，其计算方式为：
由残差的统计特性知，残差序列rk服从自由度为m的χ2分布，取rk大于某一门限TD的概率为
式中：α为允许的虚警率，
给定的警概率Pf＝α，α＝10-4，由χ2分布求出上门限阈值TD1；给定的警概率Pf＝α，α＝10-2，求出下门限阈值TD2。
所述步骤四中，利用RBF神经网络训练正常情况下的量测信息，当观测量残差超过设定双阈值门限最大值TD2时，表明系统发生突变故障或存在野值，使用训练好的RBF输出预测数据进行组合。
所述步骤四中，滤波器正常组合情况下的观测量
[ΔvE,ΔvN,ΔvU,ΔpE,ΔpN,ΔpU]为输入，ΔvE,ΔvN,ΔvU为三位速度误差向量；ΔpE,ΔpN,ΔpU为三维位置误差向量，残差ek为输出，对LS-SVM模型进行训练，并达到一定精度；判断当观测量残差处于两阈值门限中间时，利用LS-SVM回归拟合得到的预测值为输出代替滤波新息，则此时的检验统计量为：
以此改进传统残差检验法，避免软故障跟踪造成滤波污染。
所述步骤四中，在软故障判别期间，以实际新息对观测噪声协方差阵进行准确估计，然后减小增益矩阵，从而削弱故障观测量对状态估计修正过程中所占的权重，解决由于故障误差跟踪，造成估计精度下降的问题，其中，
此时残差向量(新息)的理论协方差矩阵为：
式(11)中，为残差向量的理论协方差矩阵；Hk为系数矩阵；Pk/k-1为从k-1时刻到k时刻的状态一步预测协方差矩阵；Rk为k时刻的观测噪声协方差矩阵；
式(12)中，Φk，k-1为从k-1时刻到k时刻的状态转移矩阵；Γk,k-1为噪声分布矩阵；Pk/k-1为从k-1时刻到k时刻的状态一步预测协方差矩阵；Pk-1为k-1时刻协方差矩阵；Qk-1为系统过程噪声矩阵；
利用预测值进行滤波增益矩阵的计算：
用所得增益更新误差状态及协方差矩阵：
式(14)、(15)中，为从k-1时刻到k时刻的状态转移矩阵；为k时刻增益矩阵；Zk为观测向量；Hk为误差系数矩阵；Rk为k时刻的观测噪声协方差矩阵；I为单位矩阵。
所述步骤一选取6维量测向量，取SINS和GNSS输出的位置和速度之差作为量测量，具体包括三维速度误差向量和三维位置误差向量，为Z＝[ΔvE,ΔvN,ΔvU,ΔpE,ΔpN,ΔpU]T；其中ΔvE,ΔvN,ΔvU为三维速度误差向量；ΔpE,ΔpN,ΔpU为三维位置误差向量，H为m×n维量测矩阵，ω(k)为动态模型噪声，Vk为观测模型噪声，A(k)为状态转移矩阵。
一种双阈值辅助故障容错KF的组合导航系统，含有存储器、处理器、数据输入端口，所述数据输入端口用以输入要处理的数据，所述存储器用以存储计算机程序，所述处理器用于处理数据，处理器处理的步骤是：首先，通过KF滤波器构造组合导航系统观测量残差，并设定双阈值进行故障检测；当观测量残差小于双阈值最小值时，其无故障发生；当观测量残差超过最大设定阈值时，组合导航系统发生突变野值干扰，利用RBF神经网络预测数据进行组合，提高组合导航结果的精度及稳定性；当观测量残差处于两阈值中间时，利用最小二乘支持向量机(LS-SVM)进行拟合预测，然后通过输出值计算实际新息，以实际新息对观测噪声协方差阵进行准确估计，然后减小增益矩阵，从而削弱故障观测量对状态估计修正过程中所占的权重。处理器处理的步骤按上述步骤一到步骤四的过程进行。
本发明的有益效果：1.本发明提出的一种双阈值辅助故障容错KF的组合导航方法在一定程度上提高组合导航系统故障容错能力，保证导航结果的准确性及可靠性。
2.本发明提出的一种双阈值辅助故障容错KF的组合导航方法在故障检测方面对传统状态检测方法进行改进，设定双阈值门限对突变故障或野值信息、软故障或偏差信息进行有效区分，保证组合数据的可靠性。
3.本发明提出的一种双阈值辅助故障容错KF的组合导航方法，组合导航系统当发生突变野值干扰时，利用RBF神经网络预测数据进行组合，提高组合导航结果的精度及稳定性。
4.本发明提出的一种双阈值辅助故障容错KF的组合导航方法改进传统残差检验法对缓变故障信息不敏感的情况。在软故障跟踪期间，利用最小二乘支持向量机进行拟合预测输出的新息值对观测噪声协方差阵进行准确估计，能提前感知误差影响，及时识别误差信息；同时减小增益矩阵，从而削弱故障观测量对状态估计修正过程中所占的权重能够有效抑制误差跟踪造成滤波器估计值污染的情况。
附图说明
图1是本发明双阈值辅助故障容错KF的组合导航方法流程图；
图2是本发明涉及的改进残差检验的缓变故障检测方法示意图；
图3是本发明涉及的RBF神经网络结构示意图
图4是本发明涉及的基于RBF故障容错的组合导航系统结构示意图
具体实施方式
下面结合附图1-3对本发明的一种双阈值辅助故障容错KF的组合导航方法做详细说明。
一种双阈值辅助故障容错KF的组合导航方法，步骤一：建立组合导航系统的状态方程和量测方程如下：
式(1)中，Xk为状态向量，Φk，k-1为从k-1时刻到k时刻的状态转移矩阵，Γk，k-1为噪声分布矩阵，Wk-1为过程噪声，Zk为观测向量，Vk是观测误差向量，Hk为系数矩阵；
n维系统状态量为
X(t)＝[φe φn φu δve δvn δvu δx δy δz εbx εby εbz Δax Δay Δaz]，其中，δλ,δL,δh为运动载体经纬度及高度误差；δve,δvn,δvu为载体东、北、天向速度误差；φe,φn,φu为载体三向失准角误差；εbx,εby,εbz为陀螺仪在三轴的随机漂移；Δax,Δay,Δaz为加速度计在三轴的漂移，且E[Wk]＝0，E[Vk]＝0，其中，Wk是系统误差向量，Qk为k时刻的系统过程噪声矩阵，δkj为克朗尼克δ函数，Vk是观测误差向量；Rk为k时刻的观测噪声协方差矩阵；
选取6维量测向量，取SINS和GNSS输出的位置和速度之差作为量测量，具体包括三维速度误差向量和三维位置误差向量，为Z＝[ΔvE,ΔvN,ΔvU,ΔpE,ΔpN,ΔpU]T；H为m×n维量测矩阵，ω(k)为动态模型噪声，Vk为观测模型噪声，A(k)为状态转移矩阵。
步骤二：构建组合导航系统故障检测函数；
由传统卡尔曼滤波过程，可得K时刻系统状态的递推值为：
式(2)中，为k时刻系统状态递推值；为一步预测状态估计；
为k时刻增益矩阵；Zk是系统观测向量；Hk是观测误差系数矩阵；
且K时刻系统观测的预测值为：
式(3)中，Hk为系数矩阵；为一步预测状态估计；
量测与估计之差为新息，即残差序列:
式(4)中，rk为残差序列；Zk为观测向量；为预测观测值；
新息也服从零均值高斯白噪声过程，协方差矩阵为：
Sk＝HkPk,k-1Hk T+Rk (5)
其中，在组合导航模型和噪声假设均准确的情况下，残差rk服从N(0,Sk)；
构建故障检测函数为：
rk为残差序列；Sk为协方差矩阵；
步骤三：设定双阈值门限进行故障判别：
其中，TD1和TD2分别为故障检测的上下门限；
步骤四：当观测量残差小于双阈值最小值TD1，系统无故障发生；当观测量残差超过双阈值最大值TD2时，系统发生突变故障或野值，对故障信息源进行隔离，并通过构造新的观测值进行融合；当观测量残差处于两阈值中间时，利用训练好的RBF网络进行预测，然后通过输出值计算实际新息，以实际新息对观测噪声协方差阵进行准确估计，然后减小增益矩阵，从而削弱故障观测量对状态估计修正过程中所占的权重。利用RBF神经网络训练正常情况下的量测信息，当观测量残差超过设定双阈值门限最大值TD2时，表明系统发生突变故障或存在野值，使用训练好的RBF输出预测数据进行组合。
步骤三中，TD1和TD2故障检测的上、门限，其计算方式为：
由残差的统计特性知，rk服从自由度为m的χ2分布，取rk大于某一门限TD的概率为
式中：α为允许的虚警率，
给定的警概率Pf＝α，α＝10-4，由χ2分布求出上门限阈值TD1；给定的警概率Pf＝α，α＝10-2，求出下门限阈值TD2。
步骤四中，滤波器正常组合情况下的观测量
[ΔvE,ΔvN,ΔvU,ΔpE,ΔpN,ΔpU]为输入，ΔvE,ΔvN,ΔvU为三位速度误差向量；ΔpE,ΔpN,ΔpU为三维位置误差向量，残差ek为输出，对LS-SVM模型进行训练，并达到一定精度；判断当观测量残差处于两阈值门限中间时，利用LS-SVM回归拟合得到的预测值为输出代替滤波新息，则此时的检验统计量为：
以此改进传统残差检验法，避免软故障跟踪造成滤波污染。
步骤四中，在软故障判别期间，以实际新息对观测噪声协方差阵进行准确估计，然后减小增益矩阵，从而削弱故障观测量对状态估计修正过程中所占的权重，解决由于故障误差跟踪，造成估计精度下降的问题，其中，
此时残差向量的理论协方差矩阵为：
式(11)中，为残差向量的理论协方差矩阵；Hk为系数矩阵；Pk/k-1为从k-1时刻到k时刻的状态一步预测协方差矩阵；Rk为k时刻的观测噪声协方差矩阵；
利用预测值进行滤波增益矩阵的计算：
用所得增益更新误差状态及协方差矩阵：
式(14)、(15)中，为从k-1时刻到k时刻的状态转移矩阵；为k时刻增益矩阵；Zk为观测向量；Hk为误差系数矩阵；Rk为k时刻的观测噪声协方差矩阵；I为单位矩阵。
一种双阈值辅助故障容错KF的组合导航系统，含有存储器、处理器、数据输入端口，所述数据输入端口用以输入要处理的数据，所述存储器用以存储计算机程序，所述处理器用于处理数据，处理器处理的步骤是如图1所示：首先，通过KF滤波器构造组合导航系统观测量残差，并设定双阈值进行故障检测；当观测量残差小于双阈值最小值时，其无故障发生；当观测量残差超过最大设定阈值时，组合导航系统发生突变野值干扰，利用RBF神经网络预测数据进行组合，提高组合导航结果的精度及稳定性；当观测量残差处于两阈值中间时，利用最小二乘支持向量机进行拟合预测，然后通过输出值计算实际新息，以实际新息对观测噪声协方差阵进行准确估计，然后减小增益矩阵，从而削弱故障观测量对状态估计修正过程中所占的权重，由此抑制软故障跟踪及相关野值对滤波的不利影响，提高诊断期间组合导航系统的滤波精度。具体处理器处理步骤如导航方法中所述。
如图2所示，本发明涉及的残差检验的缓变故障检测方法以滤波器正常组合情况下的观测量[ΔvE,ΔvN,ΔvU,ΔpE,ΔpN,ΔpU]为输入，残差ek为输出，对LS-SVM模型进行训练，并达到一定精度；判断当观测量残差处于两阈值门限中间时，利用LS-SVM回归拟合得到的预测值为输出代替滤波信息，构建故障检测量，再与设定阈值进行比较，保证提前感知故障信息，进行及时处理。
如图3-4所示，本发明涉及的基于RBF神经网络的结构及训练、预测过程。图3中RBF神经网络结构是前向型神经网络中一类特殊的三层神经网络，输入层单元只传递输入信号到隐含层，其隐含层特性函数采用非线性径向基函数，以对输入层激励产生局部化响应，输出层单元则对隐单元基函数的输出进行线性组合，本发明中以观测量[ΔvE,ΔvN,ΔvU,ΔpE,ΔpN,ΔpU]为输入，隐含层通过经验选取参数，输出值为与输入对应的位置及速度信息；图4利用正常组合模式下的卫星数据进行训练，在故障检验量超过最大阈值，判别存在突变故障及野值时，输出预测值进行组合，提高导航结果的精度及可靠性。