CN111928846B - 一种基于联邦滤波的多源融合即插即用组合导航方法 - Google Patents
一种基于联邦滤波的多源融合即插即用组合导航方法 Download PDFInfo
- Publication number
- CN111928846B CN111928846B CN202010756570.9A CN202010756570A CN111928846B CN 111928846 B CN111928846 B CN 111928846B CN 202010756570 A CN202010756570 A CN 202010756570A CN 111928846 B CN111928846 B CN 111928846B
- Authority
- CN
- China
- Prior art keywords
- filter
- sub
- federal
- navigation system
- value
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 50
- 238000001914 filtration Methods 0.000 title claims abstract description 26
- 230000004927 fusion Effects 0.000 title claims abstract description 23
- 238000003745 diagnosis Methods 0.000 claims abstract description 8
- 238000002955 isolation Methods 0.000 claims abstract description 7
- 238000007689 inspection Methods 0.000 claims abstract description 3
- 239000011159 matrix material Substances 0.000 claims description 42
- 238000005259 measurement Methods 0.000 claims description 37
- 238000013461 design Methods 0.000 claims description 14
- 239000013598 vector Substances 0.000 claims description 14
- 230000003044 adaptive effect Effects 0.000 claims description 12
- 238000001514 detection method Methods 0.000 claims description 12
- 238000004364 calculation method Methods 0.000 claims description 5
- 230000008569 process Effects 0.000 claims description 4
- 238000012545 processing Methods 0.000 claims description 4
- 230000007704 transition Effects 0.000 claims description 4
- 238000003491 array Methods 0.000 claims description 2
- 238000006243 chemical reaction Methods 0.000 claims description 2
- 238000010606 normalization Methods 0.000 claims description 2
- 238000000638 solvent extraction Methods 0.000 claims description 2
- 230000009897 systematic effect Effects 0.000 claims description 2
- 230000009466 transformation Effects 0.000 claims description 2
- 230000004888 barrier function Effects 0.000 claims 1
- 238000004422 calculation algorithm Methods 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 2
- 230000003993 interaction Effects 0.000 description 2
- 230000007257 malfunction Effects 0.000 description 2
- 230000002159 abnormal effect Effects 0.000 description 1
- 230000000903 blocking effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000007499 fusion processing Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000010295 mobile communication Methods 0.000 description 1
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/20—Instruments for performing navigational calculations
-
- 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
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)
- Navigation (AREA)
Abstract
本发明公开了一种基于联邦滤波的多源融合即插即用组合导航方法。该方法为：采用惯性导航系统、卫星导航系统、里程计及高度计作为多源融合导航系统的导航信息源，使用捷联惯性导航系统模型作为信息融合模型的基准框架，使之与其他导航传感器进行信息融合，以捷联惯性导航系统的误差方程进行建模，建立联邦滤波组合导航模型，设计自适应联邦滤波器，实现多导航源的信息自适应分配融合，进行最优估计并将故障诊断和隔离方案加入联邦滤波器，实现各传感器模块的即插即用功能，并通过改进的残差χ2检验法，使子滤波器的故障能够更快更准确的识别并隔离。本发明提高了多源融合组合导航系统在强干扰环境下的导航定位精度，增强了系统的容错性能及鲁棒性。
Description
技术领域
本发明涉及组合导航技术领域，特别是一种基于联邦滤波的多源融合即插即用组合导航方法。
背景技术
近年来，随着GNSS系统的发展和应用，卫星导航技术不仅在战争中得到了广泛的应用，卫星导航产业也成为了信息化产业的重要支柱。个人旅游出行、交通运输、气象预测、地图测绘、救灾防洪、移动通信均离不开导航产业及其技术的支持和应用。但是GNSS系统设计的初衷只是作为一种战争环境下的导航辅助手段，因此无论是中国的BDS系统、美国的GPS系统、俄罗斯的GLONASS系统、欧洲的伽利略系统还是其他一些国家和地区小型导航系统和区域性导航系统均存在一些固有的缺点，其信号到达地面的信号功率极低，使得其接收机容易被欺骗和干扰，在室内、水下、地下以及城市峡谷等物理信号受阻环境中以及电磁干扰环境中服务性能严重下降，严重的可导致军事安全受到严重挑战。
基于以上，单一导航子系统辅助惯性导航系统的组合模式已经无法满足导航系统的高精度和高可靠性需求。为了解决上述问题，研究人员根据导航系统的环境特点和工作特性，以技术成熟和具有独特优势的惯性/卫星组合导航系统为基础，融合具有不同工作特点的导航传感器的输出信息组成基于多源信息融合组合导航系统，从而提高导航系统的精度和可靠性。然而，导航传感器在长时间运行时可能出现故障，现有的技术难以实现传感器即插即用，不能对发生故障的子系统实时地进行隔离，导致系统的稳定性和鲁棒性较差，降低了多源导航系统的抗干扰能力和可靠性。
发明内容
本发明的目的是提供一种基于联邦滤波的多源融合即插即用组合导航方法，基于联邦卡尔曼多源数据滤波的技术，并通过加入故障检测和故障隔离方案，实现传感器的即插即用，使导航系统在强干扰、都市环境下提供高精度的导航定位服务。
实现本发明目的的技术解决方案为：一种基于联邦滤波的多源融合即插即用组合导航方法，包括以下步骤：
步骤1：建立在导航系下的联邦滤波组合导航系统模型，设计自适应联邦滤波器，经过联邦滤波器的最优估计，对导航系统进行校正输出；
步骤2：通过预测残差统计量构建自适应分配因子，实现各联邦子滤波器信息因数的自适应分配，优化联邦滤波器的最优估计；
步骤3：将故障诊断与故障隔离方案加入联邦滤波器中，实现传感器的即插即用，形成自适应容错联邦滤波器，并通过改进的残差χ2检验法，识别并隔离子滤波器的故障，完成组合导航。
本发明与现有技术相比，其显著优点是：(1)在常规的GNSS/SINS组合导航算法的基础上，设计基于联邦卡尔曼滤波算法，以SINS为参考系统，GNSS/SINS、里程计/SINS、高度计/SINS三者组合融合的多源导航系统，提高了组合导航系统的导航定位精度；(2)设计了一种基于改进残差χ2检测的故障检测与故障隔离的方案，当子系统发生故障时能够实时地对子系统进行隔离，使系统具有较好的稳定性和鲁棒性，提高了多源导航系统的抗干扰能力和可靠性；(3)并且通过基于多源信息融合的容错故障诊断组合导航技术，实现传感器的即插即用，令导航系统能在不同的环境下根据需要使用各类型传感器，使导航系统能更好的长时间准确可靠地完成任务。
附图说明
图1是本发明基于联邦滤波的多源融合即插即用组合导航方法算法结构流程图。
图2是传统联邦卡尔曼滤波的结构示意图。
图3是本发明基于联邦滤波的多源融合即插即用组合导航方法中所使用的改进联邦卡尔曼滤波的结构示意图。
图4是本发明基于联邦滤波的多源融合即插即用组合导航方法中故障检测算法流程示意图。
具体实施方式
下面结合附图及具体实施例对本发明作进一步详细说明。
结合图1，本发明为一种基于联邦滤波的多源融合即插即用组合导航方法，包括以下步骤：
步骤1：建立在导航系下的联邦滤波组合导航系统模型，设计自适应联邦滤波器，经过联邦滤波器的最优估计，对导航系统进行校正输出。具体如下：
结合图2和图3，在本发明的联邦卡尔曼滤波中，使用的导航信息源有4个，所以采用GNSS/SINS、里程计/SINS、高度计/SINS子滤波器和一个用于信息融合的主滤波器，利用信息分配原则在各子滤波器和主滤波器间进行信息分配，各子滤波器处理各自的量测信息并得到局部估计，然后在主滤波器中进行信息融合得到最优估计。
(1.1)首先在导航坐标系下建立联邦滤波组合导航系统模型。惯性导航系统的误差模型选用通用的误差模型，公共误差参考系统选取惯性导航系，导航坐标系选取东北天地理坐标系，则系统的状态矢量X为
其中为东北天方向上的姿态角误差；δVE、δVN、δVU为东北天方向上的速度误差；δL、δλ、δH为纬经高方向的位置误差；εbx、εby、εbz为陀螺仪在三个轴向上的随机漂移；εrx、εry、εrz为陀螺仪在三个轴向上的一阶马尔可夫过程随机噪声；/>为加速度计在三个轴向上的常值偏差；
系统的状态方程为：
其中F(t)为系统的状态转移矩阵，G(t)为误差系数矩阵，W(t)为白噪声随机误差矢量，具体形式如下：
状态转移矩阵F(t)：
式中FN为系统误差矩阵，Fs为惯性器件的误差转换矩阵，FM为惯性器件的噪声矩阵，具体如下：
其中为姿态转换矩阵；
误差系数矩阵G(t)为：
白噪声随机误差矢量W(t)为：
W(t)＝[wgx wgy wgz wrx wry wrz wax way waz]T (8)
系统的量测方程Z为：
Z(t)＝H(t)X(t)+V(t) (9)
其中X(t)为系统的状态矢量，V(t)＝[NN NE NU]T，H(t)根据不同的子滤波器选取有所不同；
连续的系统模型在计算机中不易实现，所以需要将系统模型离散化，把状态方程式(3)和量测方程式(10)离散化，可得
式中
式中，T为迭代周期；
(1.2)设计联邦滤波器：
每个子滤波器均采用卡尔曼滤波方式，具体如下：
状态一步预测方程：
状态估计方程：
卡尔曼增益方程：
一步预测均方差方程：
均方程估计方程：
表示惯性导航系统的位置信息为
表示惯性导航系统的速度信息为
式中λI、LI、HI为惯性导航系统纬度、经度、高度的测量值，λt、Lt、Ht为车载器纬度、经度、高度的真值，δλ、δL、δH为惯性导航系统纬度、经度、高度的测量误差值；vIN、vIE、vIU为惯性导航系统在东北天方向上速度的测量值，vN、vE、vU为车载器在东北天方向上速度的真值，δvN、δvE、δvU为惯性导航系统在东北天方向上速度的测量误差值；
表示卫星导航系统的位置信息为
表示卫星导航系统的速度信息为
式中λG、LG、HG为卫星导航系统纬度、经度、高度的测量值，λt、Lt、Ht为车载器纬度、经度、高度的真值，NE、NN、NU为卫星导航系统东北天方向位置的测量误差值；vGE、vGN、vGU为卫星导航系统在东北天方向上速度的测量值，vN、vE、vU为车载器在东北天方向上速度的真值，MN、ME、MU为惯性导航系统在东北天方向上速度的测量误差值；为了与惯性导航系统的经度和纬度的量纲配合，对位置测量值N和速度测量值M进行了转换；
表示里程计的速度信息为
式中vOE、vON、vOU为里程计在东北天方向上速度的测量值，vN、vE、vU为车载器在东北天方向上速度的真值，ON、OE、OU为里程计在东北天方向上速度的测量误差值；
表示高度计的高度信息为
Ha＝Ht-A (23)
式中Ha为高度计在高度上的测量值，HU为车载器在天向上位置的真值，A为高度计在高度上的测量误差值；
(1.2.1)设计GNSS/SINS子滤波器
根据(1.1)可知，GNSS/SINS子滤波器的状态方程为
GNSS/SINS子滤波器的量测方程为
其中
VG(t)＝[NN NE NU ME MN MU]T (27)
量测噪声作为白噪声处理，其方差分别为
(1.2.2)设计里程计/SINS子滤波器
根据(1.1)可知，GNSS/SINS子滤波器的状态方程为
里程计/SINS子滤波器的量测方程为
其中
HO(t)＝[03×3 diag[1 1 1] 03×12] (30)
VG（t)＝[OE ON OU]T (31)
量测噪声作为均值为0的白噪声处理；
(1.2.3)设计高度计/SINS子滤波器
根据(1.1)可知，高度计/SINS子滤波器的状态方程为
高度计/SINS子滤波器的量测方程为
Za(t)＝hI-ha＝Ha(t)X(t)+Va(t) (33)
其中
HO(t)＝[03×6 diag[0 0 1] 03×9] (34)
VG(t)＝A (35)
量测噪声作为均值为0的白噪声处理；
(1.2.4)设计联邦滤波主滤波器
联邦滤波器是一种两级滤波结构，公共参考子系统SINS的输出Xk一方面给主滤波器，另一方面给个子滤波器作为量测值。各子系统的输出只给各自的滤波器。各子滤波器的局部估计值Xi及其协方差阵Pi送入主滤波器和主滤波器的估计值一起进行融合得到全局估计值；
由主滤波器和子滤波器合成的全局最优估计值及其相应的协方差阵Pg被放大为/>后再反馈给子滤波器，来重置子滤波器的估计值，即：/>主滤波器的协方程阵为/>βi根据信息分配原则确定；
本发明采用βm＝0,βi＝1/N，无重置结构的联邦卡尔曼滤波，该方法由于各个局部滤波器独立滤波，没有反馈重置带来的互相影响，这就提供了最高的容错性能；具体方法如下：
系统信息在主、子滤波器间的分配方法是基于信息分配原则的，即满足：
其中，βm为主滤波器信息分配系数，βi为第i个子滤波器对应的信息分配系数；
采用主滤波器无信息分配，仅对子滤波器估计进行全局融合并对子滤波器进行反馈，而系统信息则保存在速度、位置子滤波器中的方式，即如式
系统信息在子滤波器中的具体分配方法如下：
其中，为主滤波器状态估计，Pg、Qg分别为主滤波器状态误差均方差阵和系统噪声阵；/>为子滤波器状态估计，Pi、Qi分别为子滤波器状态估计对应的状态估计均方程阵和子滤波器系统噪声；
主滤波器最优信息融合：
式中为主滤波器中的协方差矩阵；/>为融合了主滤波器和子滤波器后的协方差矩阵；/>为融合后的状态估计；
步骤2：设计了一种预测残差统计量来构建自适应分配因子的方法，实现各联邦子滤波器信息因数的自适应分配，优化了联邦滤波器的最优估计。具体如下：
在多源信息融合导航系统中，导航信息源是多样的且导航信息精度不一，此外导航信息还可能存在受到干扰、发生故障以及含有测量粗差等问题，即使可以利用前面的方法进行量测值的抗差自适应调整，其估计信息仍会受到一定的影响。因此合理的信息分配是融合各个滤波器估计值的关键。传统的信息分配方法不能适应上述异常问题，通过实时调整分配因子，才可以提高系统的滤波精度和性能。本发明中采用预测残差统计量来构建自适应信息分配因子，以此来调整每个滤波器的误差协方差阵和系统噪声协方差阵。
系统残差可以表示为：
系统残差的协方差理论值可以表示为：
系统残差的协方差真实值可以表示为：
因此可以通过式(41)和式(42)构建自适应分配因子：
式中，c的范围为0.8～1，本发明中取1，其中
根据上式可进一步推导得到：
式中Ci(rk)和分别表示第i个滤波器在tk时刻预测残差的实际值和理论值；
为了确保信息分配因子遵循信息守恒原理，将上述分配因子进行归一化处理：
然后根据上述信息分配因子再进行融合，具体为：
步骤3：将故障诊断与故障隔离方案加入联邦滤波器中，实现传感器的即插即用，形成自适应容错联邦滤波器，并设计了一种改进的残差χ2检验法，使子滤波器的故障能够更快更准确的识别并隔离，提高了导航系统的自适应性。具体如下：
故障诊断与故障隔离方案加入联邦滤波器：
(3.1)用残差χ2检验对对系统的故障进行检测和隔离；由于每个子滤波器均为卡尔曼滤波器，其残差为：
其中预报值当无故障发生是卡尔曼滤波器的残差rk是零均值的白噪声，其方差为/>当系统发生故障是，残差的均值就不再为0，因此对残差的检验可以确定系统释放发生了故障；
对rk做二元假设：
无故障时：
有故障时：
可推出故障检测函数为：
式中λk是服从自由度为m的χ2分布，即λk～χ2(m)，m为量测Zk的维数；
故障判定准则为：
其中设置的门限值TD可由误警率Pf确定，误警率Pf＝α可由下面积计算公式求出：
给定Pf＝α，就可由χ2分布求出TD；
(3.2)由于残差χ2检验法难以实现对系统缓变故障有效快速地检测，本发明提出了一种改进的残差χ2检验法，在提出残差向量特征值的基础上对检测阈值进行模糊逻辑和加权处理。
对残差向量进行归一化处理：
式中rk和Ak和(3.1)中定义相同；
从当前时刻开始往前选取n个时刻的残差向量组成残差矩阵：
定义残差方差矩阵：
Q＝RTR (53)
根据控制理论知，矩阵Q的特征值最大值有界，即：
λmax≤S (54)
同时通过选取最近N个矩阵最大特征值的平均值进一步提高数据的可靠性，其具体的表达式如下：
此时，故障判断准则为：
为了减少计算量，解算过程中n和N的取值不能过大，本发明中设置为n＝3,N＝6；
(3.3)故障检测传感器即插即用功能如图4所示，当某一个子滤波器检测出故障时，系统舍弃该滤波器的信息，主滤波器的估计值舍弃故障滤波器的信息而使用其他子滤波器的值进行最优估计，组合导航系统容错设计的核心思想是根据已有的导航传感器，从组合导航整体方案的设计上来提高可靠性，实时地对导航子系统进行故障检测，当检测出子系统发生故障时，随即切断其与主滤波器间的信息交互，同时将其他正常的导航子系统重新进行数据融合处理，保证导航系统在故障条件下仍然满足一定性能要求的导航精度，因此，当某一传感器模块出现故障或者拔出时，该组合导航系统可以实时检测故障并将其隔离，从而实现传感器的即插即用功能。
Claims (1)
1.一种基于联邦滤波的多源融合即插即用组合导航方法，其特征在于，包括以下步骤：
步骤1：建立在导航系下的联邦滤波组合导航系统模型，设计自适应联邦滤波器，经过联邦滤波器的最优估计，对导航系统进行校正输出；
步骤2：通过预测残差统计量构建自适应分配因子，实现各联邦子滤波器信息因数的自适应分配，优化联邦滤波器的最优估计；
步骤3：将故障诊断与故障隔离方案加入联邦滤波器中，实现传感器的即插即用，形成自适应容错联邦滤波器，并通过改进的残差χ2检验法，识别并隔离子滤波器的故障，完成组合导航；
步骤1中所述建立在导航系下的联邦滤波组合导航系统模型，设计自适应联邦滤波器，经过联邦滤波器的最优估计，对导航系统进行校正输出，具体如下：
(1.1)首先在导航坐标系下建立联邦滤波组合导航系统模型；惯性导航系统的误差模型选用通用误差模型，公共误差参考系统选取惯性导航系，导航坐标系选取东北天地理坐标系，则系统的状态矢量X为
其中为东北天方向上的姿态角误差；δVE、δVN、δVU为东北天方向上的速度误差；δL、δλ、δH为纬经高方向的位置误差；εbx、εby、εbz为陀螺仪在三个轴向上的随机漂移；εrx、εry、εrz为陀螺仪在三个轴向上的一阶马尔可夫过程随机噪声；/>为加速度计在三个轴向上的常值偏差；
系统的状态方程为：
其中F(t)为系统的状态转移矩阵，G(t)为误差系数矩阵，W(t)为白噪声随机误差矢量，具体形式如下：
状态转移矩阵F(t)：
式中FN为系统误差矩阵，Fs为惯性器件的误差转换矩阵，FM为惯性器件的噪声矩阵，具体如下：
其中为姿态转换矩阵；
误差系数矩阵G(t)为：
白噪声随机误差矢量W(t)为：
W(t)＝[wgx wgy wgz wrx wry wrz wax way waz]T (8)
系统的量测方程Z为：
Z(t)＝H(t)X(t)+V(t) (9)
其中X(t)为系统的状态矢量，V(t)＝[NN NE NU]T，H(t)根据不同的子滤波器选取；
将系统模型离散化，把状态方程式(2)和量测方程式(9)离散化，得
式中
式中，T为迭代周期；
(1.2)设计联邦滤波器：
每个子滤波器均采用卡尔曼滤波方式，具体如下：状态一步预测方程：
状态估计方程：
卡尔曼增益方程：
一步预测均方差方程：
均方差估计方程：
表示惯性导航系统的位置信息为
表示惯性导航系统的速度信息为
式中λI、LI、HI为惯性导航系统纬度、经度、高度的测量值，λt、Lt、Ht为车载器纬度、经度、高度的真值，δλ、δL、δH为惯性导航系统纬度、经度、高度的测量误差值；vIN、vIE、vIU为惯性导航系统在东北天方向上速度的测量值，vN、vE、vU为车载器在东北天方向上速度的真值，δvN、δvE、δvU为惯性导航系统在东北天方向上速度的测量误差值；
表示卫星导航系统的位置信息为
表示卫星导航系统的速度信息为
式中λG、LG、HG为卫星导航系统纬度、经度、高度的测量值，λt、Lt、Ht为车载器纬度、经度、高度的真值，NE、NN、NU为卫星导航系统东北天方向位置的测量误差值；vGE、vGN、vGU为卫星导航系统在东北天方向上速度的测量值，vN、vE、vU为车载器在东北天方向上速度的真值，MN、ME、MU为惯性导航系统在东北天方向上速度的测量误差值；为了与惯性导航系统的经度和纬度的量纲配合，对位置测量值N和速度测量值M进行转换；
表示里程计的速度信息为
式中vOE、vON、vOU为里程计在东北天方向上速度的测量值，vN、vE、vU为车载器在东北天方向上速度的真值，ON、OE、OU为里程计在东北天方向上速度的测量误差值；
表示高度计的高度信息为
Ha＝Ht-A (23)
式中Ha为高度计在高度上的测量值，HU为车载器在天向上位置的真值，A为高度计在高度上的测量误差值；
(1.2.1)设计GNSS/SINS子滤波器
根据(1.1)知，GNSS/SINS子滤波器的状态方程为
GNSS/SINS子滤波器的量测方程为
其中
VG(t)＝[NN NE NU ME MN MU]T (27)
量测噪声作为白噪声处理，其方差分别为
(1.2.2)设计里程计/SINS子滤波器
根据(1.1)知，GNSS/SINS子滤波器的状态方程为
里程计/SINS子滤波器的量测方程为
其中
HO(t)＝[03×3 diag[1 1 1]03×12] (30)
VG(t)＝[OE ON OU]T (31)
量测噪声作为均值为0的白噪声处理；
(1.2.3)设计高度计/SINS子滤波器
根据(1.1)知，高度计/SINS子滤波器的状态方程为
高度计/SINS子滤波器的量测方程为
Za(t)＝hI-ha＝Ha(t)X(t)+Va(t) (33)
其中
HO(t)＝[03×6 diag[0 0 1]03×9] (34)
VG(t)＝A (35)
量测噪声作为均值为0的白噪声处理；
(1.2.4)设计联邦滤波主滤波器
联邦滤波器是一种两级滤波结构，公共参考子系统SINS的输出Xk一方面给主滤波器，另一方面给个子滤波器作为量测值；各子系统的输出只给各自的滤波器，各子滤波器的局部估计值Xi及其协方差阵Pi送入主滤波器和主滤波器的估计值一起进行融合得到全局估计值；
由主滤波器和子滤波器合成的全局最优估计值及其相应的协方差阵Pg被放大为后再反馈给子滤波器，来重置子滤波器的估计值，即：/>主滤波器的协方程阵为/>βi根据信息分配原则确定；
采用βm＝0,βi＝1/N，无重置结构的联邦卡尔曼滤波，各个局部滤波器独立滤波，具体方法如下：
系统信息在主、子滤波器间的分配方法是基于信息分配原则的，即满足：
其中，βm为主滤波器信息分配系数，βi为第i个子滤波器对应的信息分配系数；
采用主滤波器无信息分配，仅对子滤波器估计进行全局融合并对子滤波器进行反馈，而系统信息则保存在速度、位置子滤波器中的方式，即如式
系统信息在子滤波器中的具体分配方法如下：
其中，为主滤波器状态估计，Pg、Qg分别为主滤波器状态误差均方差阵和系统噪声阵；为子滤波器状态估计，Pi、Qi分别为子滤波器状态估计对应的状态估计均方程阵和子滤波器系统噪声；
主滤波器最优信息融合：
式中为主滤波器中的协方差矩阵；/>为融合了主滤波器和子滤波器后的协方差矩阵；/>为融合后的状态估计；
步骤2中所述通过预测残差统计量构建自适应分配因子，实现各联邦子滤波器信息因数的自适应分配，优化联邦滤波器的最优估计，具体如下：
采用预测残差统计量来构建自适应信息分配因子，以此来调整每个滤波器的误差协方差阵和系统噪声协方差阵；
系统残差表示为：
系统残差的协方差理论值表示为：
系统残差的协方差真实值表示为：
因此通过式(41)和式(42)构建自适应分配因子：
式中，c取1，其中
根据上式进一步推导得到：
式中Ci(rk)和分别表示第i个滤波器在tk时刻预测残差的实际值和理论值；
为了确保信息分配因子遵循信息守恒原理，将上述分配因子进行归一化处理：
然后根据上述信息分配因子再进行融合，具体为：
步骤3中所述将故障诊断与故障隔离方案加入联邦滤波器中，实现传感器的即插即用，形成自适应容错联邦滤波器，并通过改进的残差χ2检验法，识别并隔离子滤波器的故障，完成组合导航，具体如下：
(3.1)用残差χ2检验对对系统的故障进行检测和隔离；由于每个子滤波器均为卡尔曼滤波器，其残差为：
其中预报值当无故障发生是卡尔曼滤波器的残差rk是零均值的白噪声，其方差为/>当系统发生故障是，残差的均值就不再为0，因此对残差的检验可以确定系统是否发生了故障；
对rk做二元假设：
无故障时：
有故障时：
推出故障检测函数为：
式中λk是服从自由度为m的χ2分布，即λk～χ2(m)，m为量测Zk的维数；
故障判定准则为：
其中设置的门限值TD由误警率Pf确定，误警率Pf＝α由下面积计算公式求出：
给定Pf＝α，就由χ2分布求出TD；
(3.2)采用改进的残差χ2检验法，在提出残差向量特征值的基础上对检测阈值进行模糊逻辑和加权处理；
对残差向量进行归一化处理：
式中rk和Ak和(3.1)中定义相同；
从当前时刻开始往前选取n个时刻的残差向量组成残差矩阵：
定义残差方差矩阵：
Q＝RTR (53)
根据控制理论知，矩阵Q的特征值最大值有界，即：
λmax≤S (54)
同时通过选取最近N个矩阵最大特征值的平均值进一步提高数据的可靠性，具体的表达式如下：
此时，故障判断准则为：
为了减少计算量n和N的取为n＝3,N＝6；
(3.3)当一个子滤波器检测出故障时，系统舍弃该滤波器的信息，主滤波器的估计值舍弃故障滤波器的信息而使用其他子滤波器的值进行最优估计，实现传感器的即插即用功能。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010756570.9A CN111928846B (zh) | 2020-07-31 | 2020-07-31 | 一种基于联邦滤波的多源融合即插即用组合导航方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010756570.9A CN111928846B (zh) | 2020-07-31 | 2020-07-31 | 一种基于联邦滤波的多源融合即插即用组合导航方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111928846A CN111928846A (zh) | 2020-11-13 |
| CN111928846B true CN111928846B (zh) | 2024-04-05 |
Family
ID=73314931
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010756570.9A Active CN111928846B (zh) | 2020-07-31 | 2020-07-31 | 一种基于联邦滤波的多源融合即插即用组合导航方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111928846B (zh) |
Families Citing this family (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112363193B (zh) * | 2020-11-17 | 2023-11-24 | 西安恒图智源信息科技有限责任公司 | 基于残差建模和gnss组合惯性导航系统的高精度定位系统 |
| CN112697154B (zh) * | 2021-01-31 | 2024-04-12 | 南京理工大学 | 一种基于矢量分配的自适应多源融合导航方法 |
| CN112902967A (zh) * | 2021-01-31 | 2021-06-04 | 南京理工大学 | 一种基于残差卡方-改进序贯概率比的抗欺骗导航方法 |
| CN113063429B (zh) * | 2021-03-18 | 2023-10-24 | 苏州华米导航科技有限公司 | 一种自适应车载组合导航定位方法 |
| CN113108797B (zh) * | 2021-04-19 | 2024-04-12 | 上海商汤临港智能科技有限公司 | 车辆定位方法、装置、电子设备以及计算机可读存储介质 |
| CN113301647A (zh) * | 2021-05-25 | 2021-08-24 | 杨凯淳 | 一种基于多源信息融合的手机定位系统 |
| CN113792488A (zh) * | 2021-09-15 | 2021-12-14 | 兰州交通大学 | 一种双阈值辅助故障容错kf的组合导航系统及方法 |
| CN113984061B (zh) * | 2021-10-25 | 2023-02-14 | 哈尔滨工程大学 | 一种基于因子图优化的uuv多海域综合导航系统设计方法 |
| CN114136310B (zh) * | 2021-10-29 | 2023-10-13 | 北京自动化控制设备研究所 | 一种惯性导航系统误差自主抑制系统及方法 |
| CN113997862B (zh) * | 2021-11-19 | 2024-04-16 | 中国重汽集团济南动力有限公司 | 一种基于冗余传感器的工程车盲区监测预警系统及方法 |
| CN114252077B (zh) * | 2021-12-17 | 2024-06-18 | 南京理工大学 | 基于联邦滤波器的双gps/sins的组合导航方法及系统 |
| CN115453580A (zh) * | 2022-09-13 | 2022-12-09 | 广东汇天航空航天科技有限公司 | Gnss传感器的故障诊断方法、装置、导航系统及交通工具 |
| CN116086466B (zh) * | 2022-12-28 | 2024-03-26 | 淮阴工学院 | 一种提高ins误差精度的方法 |
| CN116399327A (zh) * | 2023-04-10 | 2023-07-07 | 烟台欣飞智能系统有限公司 | 基于多源数据融合的无人机定位系统 |
| CN116105743B (zh) * | 2023-04-17 | 2023-06-27 | 山东大学 | 一种联邦滤波系统信息因子分配方法及水下导航系统 |
| CN117804475B (zh) * | 2023-11-15 | 2024-08-13 | 诚芯智联(武汉)科技技术有限公司 | 基于惯性卫星里程计和运动学约束的车载融合导航方法 |
| CN118149802B (zh) * | 2024-03-15 | 2024-09-03 | 南京理工大学 | 一种带观测时滞的无人车多源融合卡尔曼滤波方法及系统 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103791916A (zh) * | 2014-01-28 | 2014-05-14 | 北京融智利达科技有限公司 | 一种基于mems惯导的组合车载导航系统 |
| CN106679693A (zh) * | 2016-12-14 | 2017-05-17 | 南京航空航天大学 | 一种基于故障检测的矢量信息分配自适应联邦滤波方法 |
| CN110095800A (zh) * | 2019-04-19 | 2019-08-06 | 南京理工大学 | 一种多源融合的自适应容错联邦滤波组合导航方法 |
| CN110579740A (zh) * | 2019-09-17 | 2019-12-17 | 大连海事大学 | 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法 |
| CN110823217A (zh) * | 2019-11-21 | 2020-02-21 | 山东大学 | 一种基于自适应联邦强跟踪滤波的组合导航容错方法 |
-
2020
- 2020-07-31 CN CN202010756570.9A patent/CN111928846B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103791916A (zh) * | 2014-01-28 | 2014-05-14 | 北京融智利达科技有限公司 | 一种基于mems惯导的组合车载导航系统 |
| CN106679693A (zh) * | 2016-12-14 | 2017-05-17 | 南京航空航天大学 | 一种基于故障检测的矢量信息分配自适应联邦滤波方法 |
| CN110095800A (zh) * | 2019-04-19 | 2019-08-06 | 南京理工大学 | 一种多源融合的自适应容错联邦滤波组合导航方法 |
| CN110579740A (zh) * | 2019-09-17 | 2019-12-17 | 大连海事大学 | 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法 |
| CN110823217A (zh) * | 2019-11-21 | 2020-02-21 | 山东大学 | 一种基于自适应联邦强跟踪滤波的组合导航容错方法 |
Non-Patent Citations (4)
| Title |
|---|
| 一种改进的残差χ2检验方法在联邦UKF滤波器中的应用;刘建业;仪器仪表学报;第30卷(第12期);2568-2573 * |
| 两级故障检测与隔离方法在组合导航中的应用分析;吴有龙;王晓鸣;杨玲;曹鹏;;弹道学报(04);全文 * |
| 基于联邦滤波的多源融合导航算法;谭聚豪;导航与控制;第19卷(第2期);10-18 * |
| 多源组合导航系统故障检测技术研究;李杰;陈安升;陈帅;王琮;姚晓涵;;导弹与航天运载技术(03);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111928846A (zh) | 2020-11-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111928846B (zh) | 一种基于联邦滤波的多源融合即插即用组合导航方法 | |
| CN111780755B (zh) | 一种基于因子图和可观测度分析的多源融合导航方法 | |
| CN110579740B (zh) | 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法 | |
| CN110823217B (zh) | 一种基于自适应联邦强跟踪滤波的组合导航容错方法 | |
| CN112697154B (zh) | 一种基于矢量分配的自适应多源融合导航方法 | |
| CN110095800B (zh) | 一种多源融合的自适应容错联邦滤波组合导航方法 | |
| CN109373999B (zh) | 基于故障容错卡尔曼滤波的组合导航方法 | |
| RU2434248C2 (ru) | Аэронавигационное устройство с инерционными датчиками и радионавигационными приемниками и способ аэронавигации, использующий такие элементы | |
| US6417802B1 (en) | Integrated inertial/GPS navigation system | |
| Jiang et al. | A fault-tolerant tightly coupled GNSS/INS/OVS integration vehicle navigation system based on an FDP algorithm | |
| Yuanxi | Resilient PNT concept frame | |
| CN112902967A (zh) | 一种基于残差卡方-改进序贯概率比的抗欺骗导航方法 | |
| WO2007143806A2 (en) | Vehicular navigation and positioning system | |
| CN112505737B (zh) | 一种gnss/ins组合导航方法 | |
| Zhang et al. | Improved fault detection method based on robust estimation and sliding window test for INS/GNSS integration | |
| Li et al. | An adaptive fault-tolerant multisensor navigation strategy for automated vehicles | |
| CN101629997A (zh) | 惯性辅助卫星导航完好性检测装置及检测方法 | |
| CN113792488A (zh) | 一种双阈值辅助故障容错kf的组合导航系统及方法 | |
| GB2444814A (en) | Estimating inertial acceleration bias errors | |
| CN112525188B (zh) | 一种基于联邦滤波的组合导航方法 | |
| CN114252077B (zh) | 基于联邦滤波器的双gps/sins的组合导航方法及系统 | |
| CN106716176B (zh) | 用于对导航系统进行调整的方法和系统 | |
| CN112179347B (zh) | 一种基于光谱红移误差观测方程的组合导航方法 | |
| CN117346771A (zh) | 一种面向智能驾驶组合导航精度优化方法 | |
| CN116358554A (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 |