CN115451952A - 一种故障检测及抗差自适应滤波的多系统组合导航方法和装置 - Google Patents
一种故障检测及抗差自适应滤波的多系统组合导航方法和装置 Download PDFInfo
- Publication number
- CN115451952A CN115451952A CN202211039205.1A CN202211039205A CN115451952A CN 115451952 A CN115451952 A CN 115451952A CN 202211039205 A CN202211039205 A CN 202211039205A CN 115451952 A CN115451952 A CN 115451952A
- Authority
- CN
- China
- Prior art keywords
- matrix
- fault detection
- measurement
- state
- navigation
- 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.)
- Granted
Links
- 238000001914 filtration Methods 0.000 title claims abstract description 62
- 238000001514 detection method Methods 0.000 title claims abstract description 53
- 230000003044 adaptive effect Effects 0.000 title claims abstract description 42
- 238000000034 method Methods 0.000 title claims abstract description 35
- 239000011159 matrix material Substances 0.000 claims abstract description 97
- 238000005259 measurement Methods 0.000 claims abstract description 80
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 11
- 230000004927 fusion Effects 0.000 claims abstract description 7
- 230000008569 process Effects 0.000 claims abstract description 5
- 230000006870 function Effects 0.000 claims description 27
- 230000002159 abnormal effect Effects 0.000 claims description 9
- 238000004364 calculation method Methods 0.000 claims description 8
- 238000012937 correction Methods 0.000 claims description 7
- 238000009499 grossing Methods 0.000 claims description 6
- 230000007704 transition Effects 0.000 claims description 6
- 238000012545 processing Methods 0.000 claims description 4
- 238000010276 construction Methods 0.000 claims description 3
- 230000008054 signal transmission Effects 0.000 claims description 3
- 230000017105 transposition Effects 0.000 claims description 3
- 238000004590 computer program Methods 0.000 description 5
- 238000005034 decoration Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- VGMFHMLQOYWYHN-UHFFFAOYSA-N Compactin Natural products OCC1OC(OC2C(O)C(O)C(CO)OC2Oc3cc(O)c4C(=O)C(=COc4c3)c5ccc(O)c(O)c5)C(O)C(O)C1O VGMFHMLQOYWYHN-UHFFFAOYSA-N 0.000 description 1
- AJLFOPYRIVGYMJ-UHFFFAOYSA-N SJ000287055 Natural products C12C(OC(=O)C(C)CC)CCC=C2C=CC(C)C1CCC1CC(O)CC(=O)O1 AJLFOPYRIVGYMJ-UHFFFAOYSA-N 0.000 description 1
- 230000001133 acceleration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000005094 computer simulation Methods 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- AJLFOPYRIVGYMJ-INTXDZFKSA-N mevastatin Chemical compound C([C@H]1[C@@H](C)C=CC2=CCC[C@@H]([C@H]12)OC(=O)[C@@H](C)CC)C[C@@H]1C[C@@H](O)CC(=O)O1 AJLFOPYRIVGYMJ-INTXDZFKSA-N 0.000 description 1
- BOZILQFLQYBIIY-UHFFFAOYSA-N mevastatin hydroxy acid Natural products C1=CC(C)C(CCC(O)CC(O)CC(O)=O)C2C(OC(=O)C(C)CC)CCC=C21 BOZILQFLQYBIIY-UHFFFAOYSA-N 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000005295 random walk Methods 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
Images
Classifications
-
- 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
- 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- 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/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
-
- 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/23—Testing, monitoring, correcting or calibrating of receiver elements
-
- 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/393—Trajectory determination or predictive tracking, e.g. Kalman filtering
-
- 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
-
- H—ELECTRICITY
- H03—ELECTRONIC CIRCUITRY
- H03H—IMPEDANCE NETWORKS, e.g. RESONANT CIRCUITS; RESONATORS
- H03H21/00—Adaptive networks
- H03H21/0012—Digital adaptive filters
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Manufacturing & Machinery (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种故障检测及抗差自适应滤波的多系统组合导航方法和装置，属于导航技术领域，可在系统故障情况下取得较好的估计精度，提高组合导航的精度和鲁棒性。本发明方法包括：根据捷联惯性导航系统误差模型建立状态方程，建立位置速度量测方程；其次在滤波过程中实时计算每一维量测信息对应的平滑有界层宽度，构造基于平滑有界层的故障检测函数；然后基于故障检测函数值计算抗差等价权矩阵，设计抗差自适应滤波算法，重构滤波增益矩阵，对滤波器进行自适应调整后进行信息融合，得到状态估计，对导航输出的位置、速度和姿态进行校正。
Description
技术领域
本发明属于导航技术领域，尤其涉及一种故障检测及抗差自适应滤波的多系统组合导航方法和装置。
背景技术
无人机的应用日益广泛，其自主导航备受关注。GNSS/SINS(捷联惯性导航系统/全球导航卫星系统)是无人机上主流的组合导航系统。无人机常常需要在恶劣环境下执行复杂任务：如峡谷密林、电磁干扰环境等，不可避免地会遇到GNSS受扰的情况，使得其噪声特性异常，进而影响卡尔曼滤波器的性能。
针对系统建模和噪声不确定性导致的常规卡尔曼滤波精度下降的问题，Habibi团队提出了一种基于模型的鲁棒滤波方法---平滑变结构滤波，对有界的模型和噪声不确定性提供了较好的鲁棒特性。但是受原理所限，常规的平滑变结构滤波仅对量测信息直接对应的状态量有校正作用。在组合导航中，量测信息的维数一般小于状态变量维数，平滑变结构滤波的优势尚未得到完全的利用。
平滑变结构滤波算法中的平滑有界层宽度是一个包含先验状态误差方差、量测方差、量测转移矩阵、先验量测误差变量、前一时刻后验量测误差变量的函数，其与模型的不确定水平、系统噪声和量测噪声直接相关。一般认为系统模型和系统噪声具有较好的稳定性，量测噪声容易受到环境等干扰；且在实际应用中，量测噪声对滤波器影响较为显著。因此平滑有界层宽度提供了组合导航故障检测的新思路，可用来构建新的故障检测性能指标。
发明内容
本发明提出了一种故障检测及抗差自适应滤波的多系统组合导航方法和装置，基于平滑有界层的故障检测及抗差自适应滤波组合导航，提高了系统故障情况下无人机组合导航估计精度。
为达到以上目的，本发明采用以下技术方案：
一种故障检测及抗差自适应滤波的多系统组合导航方法，包括以下步骤：
步骤1：根据捷联惯性导航系统建立状态方程和位置速度量测方程；
步骤2：在滤波过程中实时计算每一维量测信息对应的平滑有界层宽度，构造基于平滑有界层的故障检测函数及指标；
步骤3，基于步骤2得到的故障检测函数计算抗差等价权矩阵，设计抗差自适应滤波算法，重构滤波增益矩阵，对滤波器进行自适应调整后进行信息融合，得到状态估计，对导航输出的位置、速度和姿态进行校正。
以上所述步骤中，步骤1中所述状态方程为：
式中，X(t)是状态向量；
是状态向量的微分；A(t)是状态转移矩阵；G(t)是系统噪声系数矩阵；W(t)是系统噪声向量；
其中，T表示转置；
φE、φN、φU分别表示捷联惯性导航系统导航坐标系东、北、天三个方向的数学平台失准角；
δvE、δvN、δvU分别表示捷联惯性导航系统导航坐标系东、北、天三个方向的速度误差；
δL、δλ、δh分别表示捷联惯性导航系统的纬度误差、经度误差和高度误差；
εx、εy、εz分别表示捷联惯性导航系统在载体坐标系右、前、上三个方向的陀螺仪漂移；
所述位置速度量测方程为：
其中，Z(t)是量测向量，LI、λI、hI分别表示捷联惯性导航系统解算的纬度、经度、高度，vIE、vIN、vIU分别表示捷联惯性导航系统解算的导航坐标系东、北、天三个方向的速度，LG、λG、hG分别表示GNSS测量的纬度、经度、高度；vGE、vGN、vGU分别表示GNSS测量的导航坐标系东、北、天三个方向的速度，H(t)为量测系数矩阵，V(t)是量测噪声向量，RM和RN分别是子午圈曲率半径和卯酉圈曲率半径，L是当地纬度，diag为构造对角矩阵的函数，03×6表示3×6维的全零矩阵；
进行离散化处理，得到：
其中，Xk是k时刻的状态向量，Xk-1是k-1时刻的状态向量，Φk/k-1是状态向量从k-1时刻转移到k时刻的转移矩阵，Γk-1是k-1时刻的系统噪声对k时刻状态向量影响的噪声系数矩阵，Wk-1是k-1时刻的系统噪声向量，Hk是k时刻量测向量Zk与k时刻的状态向量Xk间的量测系数矩阵，Vk是k时刻的量测噪声向量；
步骤2具体包括以下步骤：
步骤2-1：计算状态一步预测和预测均方误差阵：
其中Pk/k-1为状态一步预测均方误差阵；
为状态一步预测；Qk-1为系统噪声方差阵；
为状态估计，Pk-1为状态估计均方误差阵，T表示转置；
步骤2-2：计算平滑有界层：
平滑有界层宽度阵的计算公式如下：
其中，
为k-1时刻的后验量测误差变量；
为k时刻的先验量测误差变量；γ(0<γ<1)为记忆因子；
为量测误差方差阵，Rk为量测噪声均方误差阵；
可见平滑有界层宽度ψk是一个包含一步预测均方误差阵、量测噪声均方误差阵、量测系数矩阵、先验量测误差变量、前一时刻后验量测误差变量的函数，其与模型的不确定水平、系统噪声和量测噪声直接相关。一般认为系统模型和系统噪声具有较好的稳定性，量测噪声容易受到环境等干扰；且在实际应用中，量测噪声对滤波器影响较为显著，因此平滑有界层宽度提供了组合导航故障检测的新思路，可用来构建新的故障检测性能指标；
步骤2-3：构造基于平滑有界层的故障检测函数：
式中，ψkii为第i维平滑有界层宽度，即平滑有界层宽度阵ψk的第i个对角线元素；
是第i维平滑有界层宽度的平均值，σi是第i维平滑有界层宽度的标准差；
步骤2-4：基于平滑有界层的故障检测指标可表示如下：
其中阈值c为常值，一般取c＝2.5～3。通过λki与阈值c的比较，可实时判断第i维量测信息的有效性；
步骤3具体包括以下步骤：
步骤3-1：基于故障检测函数值计算基于平滑有界层的抗差等价权矩阵
αk＝diag[αk1,αk2,…,αkn]：
其中，αki为等价权矩阵αk的第i个对角线元素；
步骤3-2：设计抗差自适应滤波算法，重构滤波增益矩阵，对滤波器进行自适应调整：
基于抗差估计的思想，当量测信息中存在异常数据，实时调整增益矩阵，降低异常数据对融合结果的影响；
步骤3-3：在步骤3-2求得的滤波增益矩阵的基础上，计算状态估计和状态估计均方误差阵：
Pk＝(I-KkHk)Pk/k-1。
一种故障检测及抗差自适应滤波的多系统组合导航装置，包括：
状态方程和位置速度量测方程构建模块，用于根据捷联惯性导航系统建立状态方程和位置速度量测方程；
时间更新模块，用于计算状态一步预测和预测均方误差阵；
平滑有界层计算模块，用于实时计算每一维量测信息对应的平滑有界层宽度，构造基于平滑有界层的故障检测函数及指标；
滤波增益矩阵重构模块，用于根据故障检测函数值计算抗差等价权矩阵，设计抗差自适应滤波算法，重构滤波增益矩阵，对滤波器进行自适应调整；
量测更新模块，用于根据滤波增益矩阵计算状态估计及状态估计均方误差阵；
导航校正模块，用于根据状态估计对导航输出的位置、速度和姿态进行校正；
信号传输顺序依次为状态方程和位置速度量测方程构建模块、时间更新模块、平滑有界层计算模块、滤波增益矩阵重构模块、量测更新模块、导航校正模块。
有益效果：本发明提供了一种故障检测及抗差自适应滤波的多系统组合导航方法和装置，与现有技术相比具有以下优势：
1、本发明基于平滑有界层宽度的故障指示作用，构建故障检测函数，提出新型的基于平滑有界层的故障检测方法，通过归一化的平滑有界层宽度值，实时检测各维量测信息的有效性；
2、本发明根据基于平滑有界层的故障检测函数值，计算等价权矩阵，实时调整滤波增益矩阵，进行组合导航系统的重构和自适应调整，降低异常量测值对滤波性能的影响，实现抗差估计，提高状态估计的精度和鲁棒性。
附图说明
图1是本发明方法流程图；
图2是本发明装置中各模块之间的关系图；
图3为本发明实施例中SBLRAF和CSSHAF两种方法的速度误差曲线；
图4为本发明实施例中SBLRAF和CSSHAF两种方法的位置误差曲线。
具体实施方式
下面结合附图和具体实施例对本发明进行详细说明：
如图1所示，一种故障检测及抗差自适应滤波的多系统组合导航方法，包括以下步骤：
步骤1，根据捷联惯性导航系统建立状态方程和位置速度量测方程；
步骤2，在滤波过程中实时计算每一维量测信息对应的平滑有界层宽度，构造基于平滑有界层的故障检测函数及指标；
步骤3，基于故障检测函数值计算抗差等价权矩阵，设计抗差自适应滤波算法，重构滤波增益矩阵，对滤波器进行自适应调整后进行信息融合，得到状态估计，对导航输出的位置、速度和姿态进行校正。
如图2所示，上述实现方法采用的故障检测及抗差自适应滤波的多系统组合导航装置，包括：
状态方程和位置速度量测方程构建模块，用于根据捷联惯性导航系统建立状态方程和位置速度量测方程；
时间更新模块，用于计算状态一步预测和预测均方误差阵；
平滑有界层计算模块，用于实时计算每一维量测信息对应的平滑有界层宽度，构造基于平滑有界层的故障检测函数及指标；
滤波增益矩阵重构模块，用于根据故障检测函数值计算抗差等价权矩阵，设计抗差自适应滤波算法，重构滤波增益矩阵，对滤波器进行自适应调整；
量测更新模块，用于根据滤波增益矩阵计算状态估计及状态估计均方误差阵；
导航校正模块，用于根据状态估计对导航输出的位置、速度和姿态进行校正；
信号传输顺序依次为状态方程和位置速度量测方程构建模块、时间更新模块、平滑有界层计算模块、滤波增益矩阵重构模块、量测更新模块、导航校正模块。
在本实施例中，上述方法可以采用如下方案实现：
首先根据捷联惯性导航系统误差模型建立状态方程，基于捷联惯性导航系统解算位置、速度与GNSS测量位置、速度的差值作为量测量建立位置速度量测方程：
选取捷联惯导系统导航坐标系东-北-天三个方向的数学平台失准角φE、φN、φU、速度误差δvE、δvN、δvU，捷联惯性导航系统的纬度误差、经度误差以及高度误差δL、δλ、δh，载体坐标系右-前-上三个方向的陀螺仪漂移εx、εy、εz，载体坐标系右-前-上三个方向的加速度计零偏，作为
为组合导航的状态量：
其中，T表示向量转置；
则组合导航系统状态方程为：
式中，X(t)是状态向量；
是状态向量的微分；A(t)是状态转移矩阵；G(t)是系统噪声系数矩阵；W(t)是系统噪声向量；
捷联惯性导航系统解算的纬度、经度、高度信息为LI、λI、hI，东-北-天三个方向的速度信息为vIE、vIN、vIU；GNSS测量的纬度、经度、高度信息为LG、λG、hG，导航坐标系东-北-天三个方向的速度信息为vGE、vGN、vGU。以捷联惯性导航系统解算位置、速度与GNSS测量位置、速度的差值作为量测量，则建立位置速度量测方程为：
式中：Z(t)是量测向量，H(t)为量测系数矩阵，V(t)是量测噪声向量，RM和RN分别是子午圈曲率半径和卯酉圈曲率半径，L是当地纬度，diag为构造对角矩阵的函数，0m×n表示m×n维的全零矩阵；
进行离散化处理，得到
式中，Xk是k时刻的状态向量，Xk-1是k-1时刻的状态向量，Φk/k-1是状态向量从k-1时刻转移到k时刻的转移矩阵，Γk-1是k-1时刻的系统噪声对k时刻状态向量影响的噪声系数矩阵，Wk-1是k-1时刻的系统噪声向量、Hk是k时刻量测向量Zk与k时刻的状态向量Xk间的量测系数矩阵、Vk是k时刻的量测噪声向量。
根据k-1时刻的估计结果进行时间更新，计算状态一步预测和预测均方误差阵：
其中Pk/k-1为状态一步预测均方误差阵；
为状态一步预测；Qk-1为系统噪声方差阵；
为状态估计，Pk-1为状态估计均方误差阵，T表示矩阵转置；
根据时间更新结果计算平滑有界层宽度阵：
其中，
为k-1时刻的后验量测误差变量；
为k时刻的先验量测误差变量；γ(0<γ<1)为记忆因子；
为量测误差方差阵，Rk为量测噪声均方误差阵。
根据平滑有界层宽度阵ψk的第i个对角线元素的大小，构造基于平滑有界层的故障检测函数：
式中，ψkii为第i维平滑有界层宽度，即平滑有界层宽度阵ψk的第i个对角线元素；
是第i维平滑有界层宽度的平均值，σi是第i维平滑有界层宽度的标准差。
通过故障检测函数值λki与阈值c的比较，可实时判断第i维量测信息的有效性。基于平滑有界层的故障检测指标可表示如下：
其中阈值c为常值，一般取c＝2.5～3。
基于故障检测函数值计算基于平滑有界层的抗差等价权矩阵αk＝diag[ak1,αk2,...,αkn]：
其中，αki为等价权矩阵αk的第i个对角线元素。
基于抗差等价权矩阵设计自适应滤波算法，当量测信息中存在异常数据，实时调整滤波增益矩阵，对滤波器进行自适应调整，降低异常数据对融合结果的影响：
基于上述滤波增益矩阵，计算状态估计和状态估计均方误差阵，对导航输出的位置、速度和姿态进行校正：
Pk＝(I-KkHk)Pk/k-1。
仿真参数设置如下：
陀螺仪零偏：0.01°/h
陀螺仪角度随机游走：
加速度计零偏：100μg
加速度计速度随机游走：
GNSS速度误差：0.1m/s
GNSS位置误差：10m，10m，30m
其中，°表示度，h表示小时，g表示重力加速度，μg为10-6g，Hz表示赫兹，m表示米，s表示秒。
假设在300s～350s、500s～550s以及700s～750s时间段内，分别对GNSS注入故障，故障信息如下：
表1 GNSS故障信息
根据上述仿真条件，分别采用本专利所述基于平滑有界层的故障检测及抗差自适应滤波组合导航方法(The Fault Detection and Robust Adaptive Filter based onSmooth Bounded Layer,SBLRAF)和基于残差卡方故障检测的Sage-Husa自适应滤波组合导航方法(The Sage-Husa Adaptive Filter based on Chi-Square,CSSHAF)对GNSS/SINS组合导航进行对比分析验证。两种方法的速度误差曲线、位置误差曲线分别如图3和图4所示。
可以看出，本发明所述的故障检测及抗差自适应滤波组合导航方法，利用平滑有界层宽度对组合导航系统进行故障检测，并进一步根据平滑有界层宽度结合抗差估计原理，分别对每个量测噪声分量进行针对性的自适应调整。有效结合了卡尔曼滤波的完备性以及平滑变结构滤波的稳定性，降低异常量测信息对组合导航的影响，估计误差较小，且很快收敛，有效提高了阶跃及缓变故障情况下状态估计的精度和鲁棒性。
具体实现过程中本发明提供计算机存储介质以及对应的数据处理单元，其中，该计算机存储介质能够存储计算机程序，所述计算机程序通过数据处理单元执行时可运行本发明提供的一种故障检测及抗差自适应滤波的多系统组合导航方法的发明内容以及各实施例中的部分或全部步骤。所述的存储介质可为磁碟、光盘、只读存储记忆体(read-onlymemory，ROM)或随机存储记忆体(random access memory，RAM)等。
本领域的技术人员可以清楚地了解到本发明实施例中的技术方案可借助计算机程序以及其对应的通用硬件平台的方式来实现。基于这样的理解，本发明实施例中的技术方案本质上或者说对现有技术做出贡献的部分可以以计算机程序即软件产品的形式体现出来，该计算机程序软件产品可以存储在存储介质中，包括若干指令用以使得一台包含数据处理单元的设备(可以是个人计算机，服务器，单片机或者网络设备等)执行本发明各个实施例或者实施例的某些部分所述的方法。
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。本实施例中未明确的各组成部分均可用现有技术加以实现。
Claims (7)
1.一种故障检测及抗差自适应滤波的多系统组合导航方法，其特征在于，包括以下步骤：
步骤1：根据捷联惯性导航系统建立状态方程和位置速度量测方程；
步骤2：在滤波过程中实时计算每一维量测信息对应的平滑有界层宽度，构造基于平滑有界层的故障检测函数及指标；
步骤3，基于步骤2得到的故障检测函数计算抗差等价权矩阵，设计抗差自适应滤波算法，重构滤波增益矩阵，对滤波器进行自适应调整后进行信息融合，得到状态估计，对导航输出的位置、速度和姿态进行校正。
2.根据权利要求1所述的故障检测及抗差自适应滤波的多系统组合导航方法，其特征在于，步骤1中所述状态方程为：
式中，X(t)是状态向量；
是状态向量的微分；A(t)是状态转移矩阵；G(t)是系统噪声系数矩阵；W(t)是系统噪声向量；
所述状态向量
其中，T表示转置；
φE、φN、φU分别表示捷联惯性导航系统导航坐标系东、北、天三个方向的数学平台失准角；
δvE、δvN、δvU分别表示捷联惯性导航系统导航坐标系东、北、天三个方向的速度误差；
δL、δλ、δh分别表示捷联惯性导航系统的纬度误差、经度误差和高度误差；
εx、εy、εz分别表示捷联惯性导航系统在载体坐标系右、前、上三个方向的陀螺仪漂移；
所述位置速度量测方程为：
其中，Z(t)是量测向量，LI、λI、hI分别表示捷联惯性导航系统解算的纬度、经度、高度，vIE、vIN、vIU分别表示捷联惯性导航系统解算的导航坐标系东、北、天三个方向的速度，LG、λG、hG分别表示GNSS测量的纬度、经度、高度；vGE、vGN、vGU分别表示GNSS测量的导航坐标系东、北、天三个方向的速度，H(t)为量测系数矩阵，V(t)是量测噪声向量，RM和RN分别是子午圈曲率半径和卯酉圈曲率半径，L是当地纬度，diag为构造对角矩阵的函数，03×6表示3×6维的全零矩阵；
所述位置速度量测方程进行离散化处理得到：
其中，Xk是k时刻的状态向量，Xk-1是k-1时刻的状态向量，Φk/k-1是状态向量从k-1时刻转移到k时刻的转移矩阵，Γk-1是k-1时刻的系统噪声对k时刻状态向量影响的噪声系数矩阵，Wk-1是k-1时刻的系统噪声向量，Hk是k时刻量测向量Zk与k时刻的状态向量Xk间的量测系数矩阵，Vk是k时刻的量测噪声向量。
3.根据权利要求1所述的故障检测及抗差自适应滤波的多系统组合导航方法，其特征在于，步骤2具体包括以下步骤：
步骤2-1：计算状态一步预测和预测均方误差阵：
其中Pk/k-1为状态一步预测均方误差阵；
为状态一步预测；Qk-1为系统噪声方差阵；
为状态估计，Pk-1为状态估计均方误差阵，T表示转置；
步骤2-2：计算平滑有界层：
平滑有界层宽度阵的计算公式如下：
其中，
为k-1时刻的后验量测误差变量；
为k时刻的先验量测误差变量；γ为记忆因子；
为量测误差方差阵，Rk为量测噪声均方误差阵；
步骤2-3：构造基于平滑有界层的故障检测函数：
式中，ψkii为第i维平滑有界层宽度，即平滑有界层宽度阵ψk的第i个对角线元素；
是第i维平滑有界层宽度的平均值，σi是第i维平滑有界层宽度的标准差；
步骤2-4：基于平滑有界层的故障检测指标为：
其中阈值c为常值，通过λki与阈值c的比较，实时判断第i维量测信息的有效性。
4.根据权利要求3所述的故障检测及抗差自适应滤波的多系统组合导航方法，其特征在于，步骤2-2中记忆因子取值为：0<γ<1。
5.根据权利要求3所述的故障检测及抗差自适应滤波的多系统组合导航方法，其特征在于，步骤2-4中阈值c＝2.5～3。
6.根据权利要求1所述的故障检测及抗差自适应滤波的多系统组合导航方法，其特征在于，步骤3具体包括以下步骤：
步骤3-1：基于故障检测函数值计算基于平滑有界层的抗差等价权矩阵
αk＝diag[αk1,αk2,…,αkn]：
其中，αki为等价权矩阵αk的第i个对角线元素；
步骤3-2：设计抗差自适应滤波算法，重构滤波增益矩阵，对滤波器进行自适应调整：
基于抗差估计的思想，当量测信息中存在异常数据，实时调整增益矩阵，降低异常数据对融合结果的影响；
步骤3-3：在步骤3-2求得的滤波增益矩阵的基础上，计算状态估计和状态估计均方误差阵：
Pk＝(I-KkHk)Pk/k-1。
7.权利要求1-6任一项所述方法采用的故障检测及抗差自适应滤波的多系统组合导航装置，其特征在于，所述装置包括：
状态方程和位置速度量测方程构建模块，用于根据捷联惯性导航系统建立状态方程和位置速度量测方程；
时间更新模块，用于计算状态一步预测和预测均方误差阵；
平滑有界层计算模块，用于实时计算每一维量测信息对应的平滑有界层宽度，构造基于平滑有界层的故障检测函数及指标；
滤波增益矩阵重构模块，用于根据故障检测函数值计算抗差等价权矩阵，设计抗差自适应滤波算法，重构滤波增益矩阵，对滤波器进行自适应调整；
量测更新模块，用于根据滤波增益矩阵计算状态估计及状态估计均方误差阵；
导航校正模块，用于根据状态估计对导航输出的位置、速度和姿态进行校正；
信号传输顺序依次为状态方程和位置速度量测方程构建模块、时间更新模块、平滑有界层计算模块、滤波增益矩阵重构模块、量测更新模块、导航校正模块。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211039205.1A CN115451952B (zh) | 2022-08-29 | 2022-08-29 | 一种故障检测及抗差自适应滤波的多系统组合导航方法和装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211039205.1A CN115451952B (zh) | 2022-08-29 | 2022-08-29 | 一种故障检测及抗差自适应滤波的多系统组合导航方法和装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115451952A true CN115451952A (zh) | 2022-12-09 |
| CN115451952B CN115451952B (zh) | 2023-11-07 |
Family
ID=84300820
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211039205.1A Active CN115451952B (zh) | 2022-08-29 | 2022-08-29 | 一种故障检测及抗差自适应滤波的多系统组合导航方法和装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115451952B (zh) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116358566A (zh) * | 2023-06-01 | 2023-06-30 | 山东科技大学 | 一种基于抗差自适应因子的粗差探测组合导航方法 |
| CN116400228A (zh) * | 2023-06-09 | 2023-07-07 | 中国华能集团清洁能源技术研究院有限公司 | 基于混合滤波器的电池故障检测方法及装置 |
| CN116880171A (zh) * | 2023-06-28 | 2023-10-13 | 北京空间飞行器总体设计部 | 一种航天器轻量化自主故障诊断方法 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20170191862A1 (en) * | 2015-12-31 | 2017-07-06 | General Electric Company | System and method for identifying and recovering from a temporary sensor failure |
| CN109444841A (zh) * | 2018-12-26 | 2019-03-08 | 清华大学 | 基于修正切换函数的平滑变结构滤波方法及系统 |
| CN109945895A (zh) * | 2019-04-09 | 2019-06-28 | 扬州大学 | 基于渐消平滑变结构滤波的惯性导航初始对准方法 |
| CN112683270A (zh) * | 2020-12-07 | 2021-04-20 | 中国矿业大学 | 一种基于平滑变结构滤波的gnss/sins/磁力计的组合方法 |
| CN113137977A (zh) * | 2021-04-21 | 2021-07-20 | 扬州大学 | 一种sins/偏振光组合导航初始对准滤波方法 |
-
2022
- 2022-08-29 CN CN202211039205.1A patent/CN115451952B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20170191862A1 (en) * | 2015-12-31 | 2017-07-06 | General Electric Company | System and method for identifying and recovering from a temporary sensor failure |
| CN109444841A (zh) * | 2018-12-26 | 2019-03-08 | 清华大学 | 基于修正切换函数的平滑变结构滤波方法及系统 |
| CN109945895A (zh) * | 2019-04-09 | 2019-06-28 | 扬州大学 | 基于渐消平滑变结构滤波的惯性导航初始对准方法 |
| CN112683270A (zh) * | 2020-12-07 | 2021-04-20 | 中国矿业大学 | 一种基于平滑变结构滤波的gnss/sins/磁力计的组合方法 |
| CN113137977A (zh) * | 2021-04-21 | 2021-07-20 | 扬州大学 | 一种sins/偏振光组合导航初始对准滤波方法 |
Non-Patent Citations (1)
| Title |
|---|
| 高春雷等: "基于平滑变结构卡尔曼滤波的机载组合导航算法", 航天控制, vol. 38, no. 6, pages 9 - 13 * |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116358566A (zh) * | 2023-06-01 | 2023-06-30 | 山东科技大学 | 一种基于抗差自适应因子的粗差探测组合导航方法 |
| CN116358566B (zh) * | 2023-06-01 | 2023-08-11 | 山东科技大学 | 一种基于抗差自适应因子的粗差探测组合导航方法 |
| CN116400228A (zh) * | 2023-06-09 | 2023-07-07 | 中国华能集团清洁能源技术研究院有限公司 | 基于混合滤波器的电池故障检测方法及装置 |
| CN116880171A (zh) * | 2023-06-28 | 2023-10-13 | 北京空间飞行器总体设计部 | 一种航天器轻量化自主故障诊断方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115451952B (zh) | 2023-11-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN115451952A (zh) | 一种故障检测及抗差自适应滤波的多系统组合导航方法和装置 | |
| El-Sheimy et al. | Wavelet de-noising for IMU alignment | |
| CN109163721A (zh) | 姿态测量方法及终端设备 | |
| CN110058288B (zh) | 无人机ins/gnss组合导航系统航向误差修正方法及系统 | |
| CN113465628B (zh) | 惯性测量单元数据补偿方法及系统 | |
| CN113834483B (zh) | 一种基于可观测度的惯性/偏振/地磁容错导航方法 | |
| CN108508463B (zh) | 基于Fourier-Hermite正交多项式扩展椭球集员滤波方法 | |
| CN116147624B (zh) | 一种基于低成本mems航姿参考系统的船舶运动姿态解算方法 | |
| CN108592943A (zh) | 一种基于opreq方法的惯性系粗对准计算方法 | |
| CN112577518A (zh) | 一种惯性测量单元标定方法及装置 | |
| Bonnabel et al. | Design and prototyping of a low-cost vehicle localization system with guaranteed convergence properties | |
| CN115856922A (zh) | 一种松耦合陆地组合导航方法、装置、计算机设备和介质 | |
| Hasan et al. | Comparative study on wavelet filter and thresholding selection for GPS/INS data fusion | |
| CN115265532A (zh) | 一种用于船用组合导航中的辅助滤波方法 | |
| CN114323007A (zh) | 一种载体运动状态估计方法及装置 | |
| CN111982126B (zh) | 一种全源BeiDou/SINS弹性状态观测器模型设计方法 | |
| CN112697154A (zh) | 一种基于矢量分配的自适应多源融合导航方法 | |
| Khoder et al. | A quaternion scaled unscented kalman estimator for inertial navigation states determination using ins/gps/magnetometer fusion | |
| US11821733B2 (en) | Terrain referenced navigation system with generic terrain sensors for correcting an inertial navigation solution | |
| CN113959433A (zh) | 一种组合导航方法及装置 | |
| CN115667845A (zh) | 用于移动载体的导航辅助方法 | |
| Guo et al. | Multi-sensor Data Fusion Using Adaptive Kalman Filter | |
| CN111829511A (zh) | 一种基于m估计的auv组合导航方法及系统 | |
| Fang et al. | Integrating SINS sensors with odometer measurements for land vehicle navigation system | |
| CN115164888B (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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |