CN113792488A - 一种双阈值辅助故障容错kf的组合导航系统及方法 - Google Patents

一种双阈值辅助故障容错kf的组合导航系统及方法 Download PDF

Info

Publication number
CN113792488A
CN113792488A CN202111078295.0A CN202111078295A CN113792488A CN 113792488 A CN113792488 A CN 113792488A CN 202111078295 A CN202111078295 A CN 202111078295A CN 113792488 A CN113792488 A CN 113792488A
Authority
CN
China
Prior art keywords
matrix
fault
threshold
error
observation
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202111078295.0A
Other languages
English (en)
Inventor
陈光武
袁祎
司涌波
周鑫
李朋朋
邢东峰
石建强
李鹏
刘洋
杨菊花
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Lanzhou Jiaotong University
Original Assignee
Lanzhou Jiaotong University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Lanzhou Jiaotong University filed Critical Lanzhou Jiaotong University
Priority to CN202111078295.0A priority Critical patent/CN113792488A/zh
Publication of CN113792488A publication Critical patent/CN113792488A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation
    • G06F30/27—Design optimisation, verification or simulation using machine learning, e.g. artificial intelligence, neural networks, support vector machines [SVM] or training a model
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/13—Receivers
    • G01S19/24—Acquisition or tracking or demodulation of signals transmitted by the system
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/40—Correcting position, velocity or attitude
    • G01S19/41—Differential correction, e.g. DGPS [differential GPS]
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/02—Neural networks
    • G06N3/04—Architecture, e.g. interconnection topology
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/02—Neural networks
    • G06N3/08—Learning methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F2119/00—Details relating to the type or aim of the analysis or the optimisation
    • G06F2119/10—Noise analysis or noise optimisation

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Theoretical Computer Science (AREA)
  • Mathematical Physics (AREA)
  • Data Mining & Analysis (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Evolutionary Computation (AREA)
  • Pure & Applied Mathematics (AREA)
  • Computing Systems (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Artificial Intelligence (AREA)
  • Computational Linguistics (AREA)
  • Algebra (AREA)
  • General Health & Medical Sciences (AREA)
  • Biophysics (AREA)
  • Biomedical Technology (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Health & Medical Sciences (AREA)
  • Molecular Biology (AREA)
  • Databases & Information Systems (AREA)
  • Geometry (AREA)
  • Operations Research (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Medical Informatics (AREA)
  • Computer Hardware Design (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)

Abstract

本发明是一种双阈值辅助故障容错KF的组合导航系统及方法，含有存储器、处理器、数据输入端口，步骤是：通过KF滤波器构造组合导航系统观测量残差，并设定双阈值进行故障检测；当观测量残差小于双阈值最小值时，其无故障发生；当观测量残差超过最大设定阈值时，组合导航系统发生突变野值干扰，利用RBF神经网络预测数据进行组合；当观测量残差处于两阈值中间时，利用最小二乘支持向量机进行拟合预测，然后通过输出值计算实际新息，以实际新息对观测噪声协方差阵进行准确估计，然后减小增益矩阵，削弱故障观测量对状态估计修正过程中所占的权重。有益效果：在一定程度上提高组合导航系统故障容错能力，保证导航结果的准确性及可靠性。

Description

一种双阈值辅助故障容错KF的组合导航系统及方法
技术领域
本发明的属于导航技术领域，涉及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的组合导航方法，步骤一：建立组合导航系统的状态方程和量测方程如下：
Figure BDA0003262789950000011
式(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，
Figure BDA0003262789950000021
E[Vk]＝0，
Figure BDA0003262789950000022
其中，Wk是系统误差向量，Qk为k时刻的系统过程噪声矩阵，δkj为克朗尼克δ函数，Vk是观测误差向量；Rk为k时刻的观测噪声协方差矩阵；
Figure BDA0003262789950000023
均为线性代数上的矩阵转置(字母右上角带“T”的符号表示对应符号的矩阵转置，以下类似情况不再一一说明)；
步骤二：构建组合导航系统故障检测函数；
由传统卡尔曼滤波过程，可得K时刻系统状态的递推值为：
Figure BDA0003262789950000024
式(2)中，
Figure BDA0003262789950000025
为k时刻系统状态递推值；
Figure BDA0003262789950000026
为一步预测状态估计；
Figure BDA0003262789950000027
为k时刻增益矩阵；Zk是系统观测向量；Hk是观测误差系数矩阵；
且K时刻系统观测的预测值为：
Figure BDA0003262789950000028
式(3)中，Hk为系数矩阵；
Figure BDA0003262789950000029
为一步预测状态估计；
量测与估计之差为新息，即残差序列：
Figure BDA00032627899500000210
式(4)中，rk为残差序列；Zk为观测向量；
Figure BDA0003262789950000031
为预测观测值；
新息也服从零均值高斯白噪声过程，协方差矩阵为：
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：
Figure BDA0003262789950000032
式(6)中，rk为残差序列；Sk为协方差矩阵；
步骤三：设定双阈值门限进行故障判别：
Figure BDA0003262789950000033
其中，TD1和TD2分别为故障检测的上下门限；
步骤四：当观测量残差小于双阈值最小值TD1，系统无故障发生；当观测量残差超过双阈值最大值TD2时，系统发生突变故障或野值，对故障信息源进行隔离，并通过构造新的观测值进行融合；当观测量残差处于两阈值中间(大于TD1，且小于TD2)时，利用训练好的RBF网络进行预测，通过输出值计算实际新息，以实际新息对观测噪声协方差阵进行准确估计，然后减小增益矩阵，从而削弱故障观测量对状态估计修正过程中所占的权重，提高诊断期间组合导航系统的滤波精度。
所述步骤三中，TD1和TD2故障检测的上、门限，其计算方式为：
由残差的统计特性知，残差序列rk服从自由度为m的χ2分布，取rk大于某一门限TD的概率为
Figure BDA0003262789950000041
式中：α为允许的虚警率，
Figure BDA0003262789950000042
给定的警概率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回归拟合得到的预测值
Figure BDA0003262789950000043
为输出代替滤波新息，则此时的检验统计量为：
Figure BDA0003262789950000044
以此改进传统残差检验法，避免软故障跟踪造成滤波污染。
所述步骤四中，在软故障判别期间，以实际新息对观测噪声协方差阵进行准确估计，然后减小增益矩阵，从而削弱故障观测量对状态估计修正过程中所占的权重，解决由于故障误差跟踪，造成估计精度下降的问题，其中，
此时残差向量(新息)的理论协方差矩阵为：
Figure BDA0003262789950000045
式(11)中，
Figure BDA0003262789950000046
为残差向量的理论协方差矩阵；Hk为系数矩阵；Pk/k-1为从k-1时刻到k时刻的状态一步预测协方差矩阵；Rk为k时刻的观测噪声协方差矩阵；
Figure BDA0003262789950000051
式(12)中，Φk，k-1为从k-1时刻到k时刻的状态转移矩阵；Γk,k-1为噪声分布矩阵；Pk/k-1为从k-1时刻到k时刻的状态一步预测协方差矩阵；Pk-1为k-1时刻协方差矩阵；Qk-1为系统过程噪声矩阵；
利用预测值进行滤波增益矩阵的计算：
Figure BDA0003262789950000052
用所得增益更新误差状态及协方差矩阵：
Figure BDA0003262789950000053
Figure BDA0003262789950000054
式(14)、(15)中，
Figure BDA0003262789950000055
为从k-1时刻到k时刻的状态转移矩阵；
Figure BDA0003262789950000056
为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的组合导航方法，步骤一：建立组合导航系统的状态方程和量测方程如下：
Figure BDA0003262789950000061
式(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，
Figure BDA0003262789950000071
E[Vk]＝0，
Figure BDA0003262789950000072
其中，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时刻系统状态的递推值为：
Figure BDA0003262789950000073
式(2)中，
Figure BDA0003262789950000074
为k时刻系统状态递推值；
Figure BDA0003262789950000075
为一步预测状态估计；
Figure BDA0003262789950000076
为k时刻增益矩阵；Zk是系统观测向量；Hk是观测误差系数矩阵；
且K时刻系统观测的预测值为：
Figure BDA0003262789950000077
式(3)中，Hk为系数矩阵；
Figure BDA0003262789950000078
为一步预测状态估计；
量测与估计之差为新息，即残差序列:
Figure BDA0003262789950000079
式(4)中，rk为残差序列；Zk为观测向量；
Figure BDA00032627899500000710
为预测观测值；
新息也服从零均值高斯白噪声过程，协方差矩阵为：
Sk＝HkPk,k-1Hk T+Rk (5)
其中，在组合导航模型和噪声假设均准确的情况下，残差rk服从N(0,Sk)；
构建故障检测函数为：
Figure BDA0003262789950000081
rk为残差序列；Sk为协方差矩阵；
步骤三：设定双阈值门限进行故障判别：
Figure BDA0003262789950000082
其中，TD1和TD2分别为故障检测的上下门限；
步骤四：当观测量残差小于双阈值最小值TD1，系统无故障发生；当观测量残差超过双阈值最大值TD2时，系统发生突变故障或野值，对故障信息源进行隔离，并通过构造新的观测值进行融合；当观测量残差处于两阈值中间时，利用训练好的RBF网络进行预测，然后通过输出值计算实际新息，以实际新息对观测噪声协方差阵进行准确估计，然后减小增益矩阵，从而削弱故障观测量对状态估计修正过程中所占的权重。利用RBF神经网络训练正常情况下的量测信息，当观测量残差超过设定双阈值门限最大值TD2时，表明系统发生突变故障或存在野值，使用训练好的RBF输出预测数据进行组合。
步骤三中，TD1和TD2故障检测的上、门限，其计算方式为：
由残差的统计特性知，rk服从自由度为m的χ2分布，取rk大于某一门限TD的概率为
Figure BDA0003262789950000083
式中：α为允许的虚警率，
Figure BDA0003262789950000084
给定的警概率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回归拟合得到的预测值
Figure BDA0003262789950000091
为输出代替滤波新息，则此时的检验统计量为：
Figure BDA0003262789950000092
以此改进传统残差检验法，避免软故障跟踪造成滤波污染。
步骤四中，在软故障判别期间，以实际新息对观测噪声协方差阵进行准确估计，然后减小增益矩阵，从而削弱故障观测量对状态估计修正过程中所占的权重，解决由于故障误差跟踪，造成估计精度下降的问题，其中，
此时残差向量的理论协方差矩阵为：
Figure BDA0003262789950000093
式(11)中，
Figure BDA0003262789950000094
为残差向量的理论协方差矩阵；Hk为系数矩阵；Pk/k-1为从k-1时刻到k时刻的状态一步预测协方差矩阵；Rk为k时刻的观测噪声协方差矩阵；
Figure BDA0003262789950000095
利用预测值进行滤波增益矩阵的计算：
Figure BDA0003262789950000096
用所得增益更新误差状态及协方差矩阵：
Figure BDA0003262789950000101
Figure BDA0003262789950000102
式(14)、(15)中，
Figure BDA0003262789950000103
为从k-1时刻到k时刻的状态转移矩阵；
Figure BDA0003262789950000104
为k时刻增益矩阵；Zk为观测向量；Hk为误差系数矩阵；Rk为k时刻的观测噪声协方差矩阵；I为单位矩阵。
一种双阈值辅助故障容错KF的组合导航系统，含有存储器、处理器、数据输入端口，所述数据输入端口用以输入要处理的数据，所述存储器用以存储计算机程序，所述处理器用于处理数据，处理器处理的步骤是如图1所示：首先，通过KF滤波器构造组合导航系统观测量残差，并设定双阈值进行故障检测；当观测量残差小于双阈值最小值时，其无故障发生；当观测量残差超过最大设定阈值时，组合导航系统发生突变野值干扰，利用RBF神经网络预测数据进行组合，提高组合导航结果的精度及稳定性；当观测量残差处于两阈值中间时，利用最小二乘支持向量机进行拟合预测，然后通过输出值计算实际新息，以实际新息对观测噪声协方差阵进行准确估计，然后减小增益矩阵，从而削弱故障观测量对状态估计修正过程中所占的权重，由此抑制软故障跟踪及相关野值对滤波的不利影响，提高诊断期间组合导航系统的滤波精度。具体处理器处理步骤如导航方法中所述。
如图2所示，本发明涉及的残差检验的缓变故障检测方法以滤波器正常组合情况下的观测量[ΔvE,ΔvN,ΔvU,ΔpE,ΔpN,ΔpU]为输入，残差ek为输出，对LS-SVM模型进行训练，并达到一定精度；判断当观测量残差处于两阈值门限中间时，利用LS-SVM回归拟合得到的预测值
Figure BDA0003262789950000105
为输出代替滤波信息，构建故障检测量，再与设定阈值进行比较，保证提前感知故障信息，进行及时处理。
如图3-4所示，本发明涉及的基于RBF神经网络的结构及训练、预测过程。图3中RBF神经网络结构是前向型神经网络中一类特殊的三层神经网络，输入层单元只传递输入信号到隐含层，其隐含层特性函数采用非线性径向基函数，以对输入层激励产生局部化响应，输出层单元则对隐单元基函数的输出进行线性组合，本发明中以观测量[ΔvE,ΔvN,ΔvU,ΔpE,ΔpN,ΔpU]为输入，隐含层通过经验选取参数，输出值为与输入对应的位置及速度信息；图4利用正常组合模式下的卫星数据进行训练，在故障检验量超过最大阈值，判别存在突变故障及野值时，输出预测值进行组合，提高导航结果的精度及可靠性。

Claims (8)

1.一种双阈值辅助故障容错KF的组合导航方法，其特征在于，步骤一：建立组合导航系统的状态方程和量测方程如下：
Figure FDA0003262789940000011
式(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，
Figure FDA0003262789940000012
E[Vk]＝0，
Figure FDA0003262789940000013
其中，Wk是系统误差向量，Qk为k时刻的系统过程噪声矩阵，δkj为克朗尼克δ函数，Vk是观测误差向量；Rk为k时刻的观测噪声协方差矩阵；
步骤二：构建组合导航系统故障检测函数；
由传统卡尔曼滤波过程，可得K时刻系统状态的递推值为：
Figure FDA0003262789940000014
式(2)中，
Figure FDA0003262789940000015
为k时刻系统状态递推值；
Figure FDA0003262789940000016
为一步预测状态估计；
Figure FDA0003262789940000017
为k时刻增益矩阵；Zk是系统观测向量；Hk是观测误差系数矩阵；
且K时刻系统观测的预测值为：
Figure FDA0003262789940000018
式(3)中，Hk为系数矩阵；
Figure FDA0003262789940000021
为一步预测状态估计；
量测与估计之差为新息，即残差序列：
Figure FDA0003262789940000022
式(4)中，rk为残差序列；Zk为观测向量；
Figure FDA0003262789940000023
为预测观测值；
新息也服从零均值高斯白噪声过程，协方差矩阵为：
Sk＝HkPk,k-1Hk T+Rk (5)
式(5)中，Sk为协方差矩阵；Hk为系数矩阵；Pk,k-1为从k-1时刻到k时刻的状态一步预测协方差矩阵；Rk为k时刻的观测噪声协方差矩阵；Hk T为Hk矩阵的数学转置；
其中，在组合导航模型和噪声假设均准确的情况下，残差rk服从N(0,Sk)；
构建故障检测函数为ek：
Figure FDA0003262789940000024
式(6)中，rk为残差序列；Sk为协方差矩阵；
步骤三：设定双阈值门限进行故障判别：
Figure FDA0003262789940000025
其中，TD1和TD2分别为故障检测的上下门限；
步骤四：当观测量残差小于双阈值最小值TD1，系统无故障发生；当观测量残差超过双阈值最大值TD2时，系统发生突变故障或野值，对故障信息源进行隔离，并通过构造新的观测值进行融合；当观测量残差处于两阈值中间时，利用训练好的RBF网络进行预测，然后通过输出值计算实际新息，以实际新息对观测噪声协方差阵进行准确估计，然后减小增益矩阵，从而削弱故障观测量对状态估计修正过程中所占的权重。
2.根据权利要求1所述的双阈值辅助故障容错KF的组合导航方法，其特征在于，所述步骤三中，TD1和TD2故障检测的上、门限，其计算方式为：
由残差的统计特性知，残差序列rk服从自由度为m的χ2分布，取rk大于某一门限TD的概率为
Figure FDA0003262789940000031
式中：α为允许的虚警率，
Figure FDA0003262789940000032
给定的警概率Pf＝α，α＝10-4，由χ2分布求出上门限阈值TD1；给定的警概率Pf＝α，α＝10-2，求出下门限阈值TD2。
3.根据权利要求1所述的双阈值辅助故障容错KF的组合导航方法，其特征在于，所述步骤四中，利用RBF神经网络训练正常情况下的量测新息，当观测量残差超过设定双阈值门限最大值TD2时，表明系统发生突变故障或存在野值，使用训练好的RBF输出预测数据进行组合。
4.根据权利要求1所述的双阈值辅助故障容错KF的组合导航方法，其特征在于，所述步骤四中，滤波器正常组合情况下的观测量[ΔvE,ΔvN,ΔvU,ΔpE,ΔpN,ΔpU]为输入，ΔvE,ΔvN,ΔvU为三位速度误差向量；ΔpE,ΔpN,ΔpU为三维位置误差向量，残差ek为输出，对LS-SVM模型进行训练，并达到一定精度；判断当观测量残差处于两阈值门限中间时，利用LS-SVM回归拟合得到的预测值
Figure FDA0003262789940000033
为输出代替滤波新息，则此时的检验统计量为：
Figure FDA0003262789940000034
以此改进传统残差检验法，避免软故障跟踪造成滤波污染。
5.根据权利要求1所述的双阈值辅助故障容错KF的组合导航方法，其特征在于，所述步骤四中，在软故障判别期间，以实际信息对观测噪声协方差阵进行准确估计，然后减小增益矩阵，从而削弱故障观测量对状态估计修正过程中所占的权重，解决由于故障误差跟踪，造成估计精度下降的问题，其中，
此时残差向量的理论协方差矩阵为：
Figure FDA0003262789940000041
式(11)中，
Figure FDA0003262789940000042
为残差向量的理论协方差矩阵；Hk为系数矩阵；Pk/k-1为从k-1时刻到k时刻的状态一步预测协方差矩阵；Rk为k时刻的观测噪声协方差矩阵；
Figure FDA0003262789940000043
式(12)中，Φk，k-1为从k-1时刻到k时刻的状态转移矩阵；Γk,k-1为噪声分布矩阵；Pk/k-1为从k-1时刻到k时刻的状态一步预测协方差矩阵；Pk-1为k-1时刻协方差矩阵；Qk-1为系统过程噪声矩阵；
利用预测值进行滤波增益矩阵的计算：
Figure FDA0003262789940000044
用所得增益更新误差状态及协方差矩阵：
Figure FDA0003262789940000045
Figure FDA0003262789940000046
式(14)、(15)中，
Figure FDA0003262789940000047
为从k-1时刻到k时刻的状态转移矩阵；
Figure FDA0003262789940000048
为k时刻增益矩阵；Zk为观测向量；Hk为误差系数矩阵；Rk为k时刻的观测噪声协方差矩阵；I为单位矩阵。
6.根据权利要求1所述的双阈值辅助故障容错KF的组合导航方法，其特征在于，步骤一选取6维量测向量，取SINS和GNSS输出的位置和速度之差作为量测量，具体包括三维速度误差向量和三维位置误差向量，为Z＝[ΔvE，ΔvN，ΔvU，ΔpE，ΔpN，ΔpU]T；H为m×n维量测矩阵，ω(k)为动态模型噪声，Vk为观测模型噪声，A(k)为状态转移矩阵。
7.一种双阈值辅助故障容错KF的组合导航系统，其特征是：含有存储器、处理器、数据输入端口，所述数据输入端口用以输入要处理的数据，所述存储器用以存储计算机程序，所述处理器用于处理数据，处理器处理的步骤是：首先，通过KF滤波器构造组合导航系统观测量残差，并设定双阈值进行故障检测；当观测量残差小于双阈值最小值时，其无故障发生；当观测量残差超过最大设定阈值时，组合导航系统发生突变野值干扰，利用RBF神经网络预测数据进行组合，提高组合导航结果的精度及稳定性；当观测量残差处于两阈值中间时，利用最小二乘支持向量机进行拟合预测，然后通过输出值计算实际新息，以实际新息对观测噪声协方差阵进行准确估计，然后减小增益矩阵，从而削弱故障观测量对状态估计修正过程中所占的权重。
8.如权利要求7所述的一种双阈值辅助故障容错KF的组合导航系统，其特征是：处理器处理的步骤进一步具体为：步骤一：建立组合导航系统的状态方程和量测方程如下：
Figure FDA0003262789940000051
式(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，
Figure FDA0003262789940000052
E[Vk]＝0，
Figure FDA0003262789940000053
其中，Wk是系统误差向量，Qk为k时刻的系统过程噪声矩阵，δkj为克朗尼克δ函数，Vk是观测误差向量；Rk为k时刻的观测噪声协方差矩阵；
步骤二：构建组合导航系统故障检测函数；
由传统卡尔曼滤波过程，可得K时刻系统状态的递推值为：
Figure FDA0003262789940000061
式(2)中，
Figure FDA0003262789940000062
为k时刻系统状态递推值；
Figure FDA0003262789940000063
为一预测状态估计；
Figure FDA0003262789940000069
为k时刻增益矩阵；Zk是系统观测向量；Hk是观测误差系数矩阵；
且K时刻系统观测的预测值为：
Figure FDA0003262789940000064
式(3)中，Hk为系数矩阵；
Figure FDA0003262789940000065
为一步预测状态估计；
量测与估计之差为新息，即残差序列：
Figure FDA0003262789940000066
式(4)中，rk为残差序列；Zk为观测向量；
Figure FDA0003262789940000067
为预测观测值；
新息也服从零均值高斯白噪声过程，协方差矩阵为：
Sk＝HkPk,k-1Hk T+Rk (5)
式(5)中，Sk为协方差矩阵；Hk为系数矩阵；Pk,k-1为从k-1时刻到k时刻的状态一步预测协方差矩阵；Rk为k时刻的观测噪声协方差矩阵；
其中，在组合导航模型和噪声假设均准确的情况下，残差rk服从N(0,Sk)；
构建故障检测函数为ek：
Figure FDA0003262789940000068
式(6)中，rk为残差序列；Sk为协方差矩阵；
步骤三：设定双阈值门限进行故障判别：
Figure FDA0003262789940000071
其中，TD1和TD2分别为故障检测的上下门限；
步骤四：当观测量残差小于双阈值最小值TD1，系统无故障发生；当观测量残差超过双阈值最大值TD2时，系统发生突变故障或野值，对故障信息源进行隔离，并通过构造新的观测值进行融合；当观测量残差处于两阈值中间时，利用训练好的RBF网络进行预测，然后通过输出值计算实际新息，以实际新息对观测噪声协方差阵进行准确估计，然后减小增益矩阵，从而削弱故障观测量对状态估计修正过程中所占的权重。
CN202111078295.0A 2021-09-15 2021-09-15 一种双阈值辅助故障容错kf的组合导航系统及方法 Pending CN113792488A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111078295.0A CN113792488A (zh) 2021-09-15 2021-09-15 一种双阈值辅助故障容错kf的组合导航系统及方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111078295.0A CN113792488A (zh) 2021-09-15 2021-09-15 一种双阈值辅助故障容错kf的组合导航系统及方法

Publications (1)

Publication Number Publication Date
CN113792488A true CN113792488A (zh) 2021-12-14

Family

ID=79183354

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111078295.0A Pending CN113792488A (zh) 2021-09-15 2021-09-15 一种双阈值辅助故障容错kf的组合导航系统及方法

Country Status (1)

Country Link
CN (1) CN113792488A (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114563804A (zh) * 2021-12-27 2022-05-31 中国人民解放军空军工程大学 一种gnss/ins紧组合导航系统的自适应容错方法
CN114966762A (zh) * 2022-05-18 2022-08-30 中国人民解放军空军工程大学 一种gnss/ins紧组合导航系统的故障检测方法
CN115358319A (zh) * 2022-08-23 2022-11-18 天津大学 基于双阈值检测的自适应容错滤波方法及系统
CN116086466A (zh) * 2022-12-28 2023-05-09 淮阴工学院 一种提高ins误差精度的方法
CN117452448A (zh) * 2023-12-25 2024-01-26 合众新能源汽车股份有限公司 高精定位跳变联合故障诊断方法和车辆定位控制系统

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20060074558A1 (en) * 2003-11-26 2006-04-06 Williamson Walton R Fault-tolerant system, apparatus and method
CN103134491A (zh) * 2011-11-30 2013-06-05 上海宇航系统工程研究所 Geo轨道转移飞行器sins/cns/gnss组合导航系统
CN111928846A (zh) * 2020-07-31 2020-11-13 南京理工大学 一种基于联邦滤波的多源融合即插即用组合导航方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20060074558A1 (en) * 2003-11-26 2006-04-06 Williamson Walton R Fault-tolerant system, apparatus and method
CN103134491A (zh) * 2011-11-30 2013-06-05 上海宇航系统工程研究所 Geo轨道转移飞行器sins/cns/gnss组合导航系统
CN111928846A (zh) * 2020-07-31 2020-11-13 南京理工大学 一种基于联邦滤波的多源融合即插即用组合导航方法

Non-Patent Citations (8)

* Cited by examiner, † Cited by third party
Title
傅军;韩洪祥;戴海发;: "基于改进新息序列的组合导航系统缓变故障检测方法", 农业机械学报, no. 03 *
孔雪博;许建新;熊智;唐攀飞;刘建业;: "惯性辅助的北斗导航故障自适应方法研究", 航空计算技术, no. 03 *
张源;詹金林;韩冰;陈伟;: "基于自适应迭代扩展卡尔曼滤波算法的INS/BDS组合导航系统", 科技视界, no. 04 *
张闯，赵修斌，庞春雷，冯波，高超: "LS-SVM辅助的小幅值及缓变故障检测与容错方法", 中国惯性技术学报, vol. 27, no. 3, pages 415 - 420 *
李胜男: "无人机多源信息融合容错组合导航研究", 南京航空航天大学 *
王鼎杰;孟德利;李朝阳;董毅;吴杰;: "抗野值自适应卫星/微惯性组合导航方法", 仪器仪表学报, no. 12 *
赵修斌，高超，庞春雷，张闯，王勇: "BP神经网络辅助的缓变故障双阈值检测法", BP神经网络辅助的缓变故障双阈值检测法, vol. 35, no. 6, pages 1384 - 1390 *
高运广;王仕成;刘志国;赵欣;: "一种基于LS-SVM的联邦滤波故障检测方法", 控制与决策, no. 09 *

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114563804A (zh) * 2021-12-27 2022-05-31 中国人民解放军空军工程大学 一种gnss/ins紧组合导航系统的自适应容错方法
CN114563804B (zh) * 2021-12-27 2024-05-24 中国人民解放军空军工程大学 一种gnss/ins紧组合导航系统的自适应容错方法
CN114966762A (zh) * 2022-05-18 2022-08-30 中国人民解放军空军工程大学 一种gnss/ins紧组合导航系统的故障检测方法
CN114966762B (zh) * 2022-05-18 2024-05-28 中国人民解放军空军工程大学 一种gnss/ins紧组合导航系统的故障检测方法
CN115358319A (zh) * 2022-08-23 2022-11-18 天津大学 基于双阈值检测的自适应容错滤波方法及系统
CN116086466A (zh) * 2022-12-28 2023-05-09 淮阴工学院 一种提高ins误差精度的方法
CN116086466B (zh) * 2022-12-28 2024-03-26 淮阴工学院 一种提高ins误差精度的方法
CN117452448A (zh) * 2023-12-25 2024-01-26 合众新能源汽车股份有限公司 高精定位跳变联合故障诊断方法和车辆定位控制系统
CN117452448B (zh) * 2023-12-25 2024-04-09 合众新能源汽车股份有限公司 高精定位跳变联合故障诊断方法和车辆定位控制系统

Similar Documents

Publication Publication Date Title
CN113792488A (zh) 一种双阈值辅助故障容错kf的组合导航系统及方法
CN110823217B (zh) 一种基于自适应联邦强跟踪滤波的组合导航容错方法
CN109373999B (zh) 基于故障容错卡尔曼滤波的组合导航方法
CA2664994C (en) Hybrid ins/gnss system with integrity monitoring and method for integrity monitoring
CN103235328B (zh) 一种gnss与mems组合导航的方法
CN111928846A (zh) 一种基于联邦滤波的多源融合即插即用组合导航方法
CN112507281B (zh) 一种基于双状态多因子抗差估计sins/dvl紧组合系统的方法
CN110567454B (zh) 一种复杂环境下sins/dvl紧组合导航方法
CN109471146B (zh) 一种基于ls-svm的自适应容错gps/ins组合导航方法
Jiang et al. An effective integrity monitoring scheme for GNSS/INS/vision integration based on error state EKF model
CN110196049A (zh) 一种动态环境下四陀螺冗余式捷联惯导系统硬故障检测与隔离方法
WO2021202004A2 (en) System and method for reconverging gnss position estimates
CN111024124B (zh) 一种多传感器信息融合的组合导航故障诊断方法
CN105823503B (zh) 基于改进灰色预测gm（1,1）模型自主水下航行器传感器故障诊断方法
CN112902967A (zh) 一种基于残差卡方-改进序贯概率比的抗欺骗导航方法
CN112697154B (zh) 一种基于矢量分配的自适应多源融合导航方法
GB2444814A (en) Estimating inertial acceleration bias errors
Yi-ting et al. A fast gradual fault detection method for underwater integrated navigation systems
CN114252077B (zh) 基于联邦滤波器的双gps/sins的组合导航方法及系统
CN116147624A (zh) 一种基于低成本mems航姿参考系统的船舶运动姿态解算方法
US8566055B1 (en) Gyro indexing compensation method and system
CN112179347A (zh) 一种基于光谱红移误差观测方程的组合导航方法
CN116358554A (zh) 具有异常观测处理能力的水下航行器因子图融合导航方法
CN110779549A (zh) 一种水下组合导航系统突变型故障诊断方法
CN111649744B (zh) 一种基于动力学模型辅助的组合导航定位方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination