CN111189441B - 一种多源自适应容错联邦滤波组合导航***及导航方法 - Google Patents

一种多源自适应容错联邦滤波组合导航***及导航方法 Download PDF

Info

Publication number
CN111189441B
CN111189441B CN202010027513.7A CN202010027513A CN111189441B CN 111189441 B CN111189441 B CN 111189441B CN 202010027513 A CN202010027513 A CN 202010027513A CN 111189441 B CN111189441 B CN 111189441B
Authority
CN
China
Prior art keywords
sub
filter
representing
sins
information
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
Application number
CN202010027513.7A
Other languages
English (en)
Other versions
CN111189441A (zh
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.)
Shandong University
Original Assignee
Shandong 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 Shandong University filed Critical Shandong University
Priority to CN202010027513.7A priority Critical patent/CN111189441B/zh
Publication of CN111189441A publication Critical patent/CN111189441A/zh
Application granted granted Critical
Publication of CN111189441B publication Critical patent/CN111189441B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/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
    • 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
    • G06N3/045—Combinations of networks
    • 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
    • G06N3/048—Activation functions
    • 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
    • G06N3/084—Backpropagation, e.g. using gradient descent

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Computing Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Biophysics (AREA)
  • Computational Linguistics (AREA)
  • Data Mining & Analysis (AREA)
  • Evolutionary Computation (AREA)
  • General Health & Medical Sciences (AREA)
  • Molecular Biology (AREA)
  • Artificial Intelligence (AREA)
  • Biomedical Technology (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Mathematical Physics (AREA)
  • Software Systems (AREA)
  • Health & Medical Sciences (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)
  • Filters That Use Time-Delay Elements (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明公开了一种多源自适应容错联邦滤波组合导航***及导航方法，该***包括捷联惯导***、卫星导航***、多普勒测速***、天文导航***，以及分别与上述***进行信息连接的主滤波器和三个子滤波器；上述三个子滤波器均与捷联惯导***进行信息连接，并且通过故障检测与隔离模块与主滤波器连接，所述捷联惯导***通过状态传播器与故障检测与隔离模块连接，所述故障检测与隔离模块的输出结果经过信息共享因子计算模块后，将计算结果输入到主滤波器；所述主滤波器将融合后的信息输出，同时将融合结果同步于三个子滤波器和状态传播器。本发明所公开的***及方法可以更加准确地跟踪各子滤波器的状态，获得更精确的融合结果。

Description

一种多源自适应容错联邦滤波组合导航***及导航方法
技术领域
本发明涉及导航通信技术领域，特别涉及一种多源自适应容错联邦滤波组合导航***及导航方法。
背景技术
随着高精度、实时、无缝导航和定位需求的不断提高，传统的单传感器导航***不能满足实际需要。多传感器集成导航技术和一些其他新兴的混合导航技术已成为研究的热点。全球导航卫星***(GNSS)是基于卫星的无线电导航***，能够在各种情况下长期提供可靠的位置信息。但在一些信号闭塞的环境中，如森林、峡谷、隧道和城市地区，其性能会急剧下降。捷联惯导***(SINS)是一种自主导航***，不受外界环境的干扰。它可以提供导航信息而不依赖于外部传感器。但是，SINS导航误差将随着时间的推移而累积并且在长的持续时间之后发散。同时，在使用SINS之前，需要较长的初始对准时间。多普勒测速***(DVL)基于多普勒效应设计，是一种高精度、易于使用的理想速度传感器，但DVL的成本相对较高。天文导航***(CNS)可以以星作为信标，提供车辆的姿态信息。它具有较高的导航精度和无累积误差，但容易受到大气环境的干扰。因此，使用单个传感器不能实现精确的实时导航和定位。为了获得理想的导航定位结果，近年来，综合导航***已成为一个热门的研究领域，许多研究人员提出了多种组合方法来提高导航精度。
同时，为了提高综合导航***的稳定性，必须建立适当的自主故障检测、隔离和恢复(FDIR)***。FDIR***由故障检测、故障隔离和故障恢复三部分组成。
在组合导航中，正确的滤波估计算法也是必不可少的。经典卡尔曼滤波器(KF)在集成***中得到了广泛的应用，但需要严格的***模型和噪声类型。然而，在实践中，这些要求往往不能得到满足。因此，需要找到一些更适合实际环境的滤波估计算法来完成***的估计。
近年来，分散滤波技术在多传感器***中得到了越来越广泛的应用，并提出了各种分散滤波器。联邦滤波器(FF)是一种特殊的分散滤波器，它由与集中式滤波器相反的分布式滤波器结构组成。联邦滤波器采用了局部滤波器和主滤波器的信息共享原理，利用上界技术消除了局部估计之间的相关性。联邦滤波器的优点是具有较高的精度和容错性.联邦滤波器的容错性和精度直接受到信息共享原理的影响。然而，传统的信息共享系数不能充分反映各子滤波器各状态变量的差异，无法跟踪它们的变化。因此自适应信息共享因子(ISF)的研究迫在眉睫。
发明内容
为解决上述技术问题，本发明提供了一种多源自适应容错联邦滤波组合导航***及导航方法，以达到可以更加准确地跟踪各子滤波器的状态，获得更精确的融合结果的目的。
为达到上述目的，本发明的技术方案如下：
一种多源自适应容错联邦滤波组合导航***，包括捷联惯导***、卫星导航***、多普勒测速***、天文导航***，以及分别与上述***进行信息连接的主滤波器、子滤波器一、子滤波器二和子滤波器三；上述三个子滤波器均与捷联惯导***进行信息连接，并且通过故障检测与隔离模块与主滤波器连接，所述捷联惯导***通过状态传播器与故障检测与隔离模块连接，所述故障检测与隔离模块的输出结果经过信息共享因子计算模块后，将计算结果输入到主滤波器；所述主滤波器将融合后的信息输出，同时将融合结果同步于三个子滤波器和状态传播器。
上述方案中，所述故障检测与隔离模块采用BP神经网络作为故障检测、隔离与恢复算法。
上述方案中，所述主滤波器为无重置联邦滤波器，三个子滤波器为强跟踪滤波器。
一种多源自适应容错联邦滤波组合导航方法，采用上述的一种多源自适应容错联邦滤波组合导航***，包括如下过程：
步骤一，设置于运动车辆上的传感器进行运动车辆的数据采集，并将数据传输给三个子滤波器；
步骤二，三个子滤波器分别对数据进行滤波处理，并将处理后的数据传输给故障检测与隔离模块；
步骤三，故障检测与隔离模块对数据进行计算，判断各个子滤波器是否发生故障，若发生故障，则将此子滤波器进行隔离，其信息不能进入主滤波器；若没有故障发生，则将各个子滤波器的输出结果输入到主滤波器中；同时，故障检测与隔离模块将计算结果发送到信息共享因子计算模块；
步骤四，信息共享因子计算模块对信息共享因子进行计算后，将结果输入到主滤波器，并反馈给三个子滤波器；
步骤五，主滤波器对接收到的数据进行信息分配和信息融合，并将结果输出，同时，将结果同步于三个子滤波器和状态传播器，对全局进行调控。
进一步的技术方案中，所述步骤一具体如下：
基于捷联惯导***，通过轨迹发生器，设定运动车辆的运动时长、运动参数以及运动环境参数信息，生成运动车辆的运动轨迹信息，产生捷联惯导***的原始数据；
通过轨迹发生器，利用运动车辆的运动轨迹信息，加入卫星导航***的噪声，生成卫星导航***对运动车辆的位置数据；
通过轨迹发生器，利用运动车辆的运动轨迹信息，加入多普勒测速仪的噪声，生成多普勒测速仪对运动车辆的速度数据；
通过轨迹发生器，利用运动车辆的运动轨迹信息，加入天文导航***的噪声，产生天文导航***对运动车辆的姿态数据。
进一步的技术方案中，所述步骤二具体如下：
(1)建立导航坐标系下的联邦滤波组合导航***模型：
以SINS作为参考***，通过摄动法得到SINS误差模型，线性化的***状态方程描述为：
Figure BDA0002363001030000031
其中，x表示***状态向量，
Figure BDA0002363001030000032
表示下一时刻的***状态向量，F表示状态转移函数，w表示状态噪声；
为了实现滤波算法，首先将状态方程离散化，得到***状态向量x从第k-1时刻传播到第k时刻的离散时间状态方程：
xk＝Fk,k-1xk-1+wk
其中，Fk,k-1表示***过渡矩阵，xk表示k时刻的***状态向量，xk-1表示k-1时刻的***状态向量，wk表示过程噪声，且其满足以下统计特性：
E[wk]＝0
Figure BDA0002363001030000033
其中，E[·]表示求一个矩阵的均值，T表示对一个矩阵求转置，δkj表示一个克罗内克函数，Qk是过程噪声的协方差矩阵，并且***状态向量x定义为
Figure BDA0002363001030000041
其中，δφEδφNδφU表示在东、北、上的姿态误差，δvEδvNδvU表示东、北、上的速度误差；δLδλδh表示纬度、经度和高度的位置误差；εrxεryεrz由陀螺漂移造成的误差；
Figure BDA0002363001030000042
由加速度计偏差造成的误差；
(2)三个子滤波器分别对数据进行滤波处理：
(2.1)子滤波器一关于SINS/GNSS的量测方程：
在与GNSS相连接的局部滤波器中，将SINS的位置输出与GNSS之间的差值作为SINS/GNSS测量方程的测量信息，测量方程表示为：
Figure BDA0002363001030000043
其中，LSINS、λSINS、hSINS分别表示SINS***测量的纬度、经度和高度，LGNSS、λGNSS、hGNSS分别表示GNSS***测量的纬度、经度和高度，δL、δλ、δh分别表示SINS***在纬度、经度和高度上与真实位置的误差，v11、v12、v13分别表示GNSS***在纬度、经度和高度上与真实位置的误差，它们是独立的零均值高斯白噪声过程，v1表示由v11、v12、v13组成的矩阵；
H1表示为：
H1＝[03×6diag[111]03×6]
其中，0表示全零矩阵，diag[]表示对角矩阵；
(2.2)子滤波器二关于SINS/DVL的量测方程：
在与DVL相连接的子滤波器二中，将SINS的速度输出与DVL之间的差值作为SINS/DVL测量方程的测量信息，测量方程表示为：
Figure BDA0002363001030000044
其中，vE,SINS、vN,SINS、vU,SINS分别表示SINS***测量的东、北和向上的速度；vE,DVL、vN,DVL、vU,DVL分别表示DVL***测量的东、北和向上的速度；δvE、δvN、δvU分别表示SINS***在东、北和向上速度上与真实速度的误差；v21、v22、v23分别表示DVL***在东、北和向上速度上与真实速度的误差，它们是独立的零均值高斯白噪声过程，v2表示由v21、v22、v23组成的矩阵；
H2表示为：
H2＝[03×3diag[111]03×9]
其中，0表示全零矩阵，diag[]表示对角矩阵；
(2.3)子滤波器三关于SINS/CNS的量测方程：
在与CNS相连接的子滤波器三中，将SINS的姿态输出与CNS之间的差值作为SINS/DVL测量方程的测量信息，测量方程表示为：
Figure BDA0002363001030000051
其中，φE,SINS、φN,SINS、φU,SINS分别表示SINS***测量的东、北和向上的姿态角度；φE,CNS、φN,CNS、φU,CNS分别表示CNS***测量的东、北和向上的姿态角度；δvE、δvN、δvU分别表示SINS***在东、北和向上姿态角度上与真实姿态角度的误差；v21、v22、v23分别表示CNS***在东、北和向上姿态角度上与真实姿态角度的误差；它们是独立的零均值高斯白噪声过程，v2表示由v21、v22、v23组成的矩阵；
H3示为：
H3＝[diag[111]03×12]
其中，0表示全零矩阵，diag[]表示对角矩阵。
进一步的技术方案中，所述步骤三具体如下：
故障检测与隔离模块采用BP神经网络作为故障检测、隔离与恢复算法，BP神经网络由输入层、隐层和输出层组成，隐层有一层或多层；
首先，通过故障信息与正常信息对BP神经网络进行训练，网络选用S型传递函数
Figure BDA0002363001030000061
通过反传误差函数
Figure BDA0002363001030000062
不断调节网络权值和阈值使误差函数F达到极小，其中，ti为期望输出、Oi为网络的计算输出；
然后通过输入状态传播器与各子滤波器的差值，检测子滤波器是否发生故障，在本BP神经网络中，采用Sigmoid函数作为网络的激励函数，当激励函数输出结果大于0.5时，该子滤波器发生故障，当激励函数输出结果小于0.5时，该子滤波器正常运行；
如果子滤波器发生故障，那么便对这一子滤波器进行隔离，并在下一时刻将主滤波器的输出更新到该子滤波器。
进一步的技术方案中，所述步骤四中信息共享因子的计算方法如下：
Figure BDA0002363001030000063
Figure BDA0002363001030000064
Figure BDA0002363001030000065
其中，β1、β2、β3别是子滤波器一、子滤波器二、子滤波器三的信息共享因子；y1、y2、y3分别是子滤波器一、子滤波器二、子滤波器三的BP人工神经网络激励函数的输出。
进一步的技术方案中，所述步骤五的具体方法如下：
信息分配过程如下所示：
Figure BDA0002363001030000066
Figure BDA0002363001030000067
Figure BDA0002363001030000068
其中，
Figure BDA0002363001030000071
表示k时刻第i个子滤波器的过程噪声协方差，Qk表示k时刻主滤波器的过程噪声协方差，
Figure BDA0002363001030000072
表示k时刻第i个子滤波器的估计误差协方差阵，Pk|k表示k时刻主滤波器的估计误差协方差阵，
Figure BDA0002363001030000073
表示k时刻第i个子滤波器的状态估计，
Figure BDA0002363001030000074
表示k时刻主滤波器的状态估计，βi是第i个子滤波器的信息共享因子，并且满足：
Figure BDA0002363001030000075
其中，I表示单位矩阵；
信息融合过程如下所示：
Figure BDA0002363001030000076
Figure BDA0002363001030000077
其中，Pg表示主滤波器的估计误差协方差阵，Pi表示k时刻第i个子滤波器的估计误差协方差阵，
Figure BDA0002363001030000078
表示主滤波器的状态估计，
Figure BDA0002363001030000079
表示k时刻第i个子滤波器的状态估计。
通过上述技术方案，本发明提供的一种多源自适应容错联邦滤波组合导航***及导航方法与现有技术相比，其显著优点在于：
(1)采用联邦滤波算法，以惯性导航***SINS作为公共参考***，实现多普勒测速***、天文导航***以及全球导航卫星***与惯性导航***SINS的组合，各个子滤波器自动判断***的当前工作状态，自适应更新信息分配因子得到局部最优解，实现组合模式的灵活选择，最终合成公共参考***SINS误差状态的全局最优估计，提高了多源融合组合导航***的定位精度；
(2)主滤波器选用无重置联邦滤波器，采用自适应联邦信息分配因子，在某一子信息源出现故障时，能够对故障进行隔离，不影响滤波器的正常滤波，使***保持较好的稳定性和鲁棒性，提高了多源融合组合导航***的可靠性和抗干扰能力。
(3)子滤波器采用强跟踪滤波器，可以获得更加精准的估计结果，从而提高了多源融合组合导航***的可靠性和抗干扰能力。
(4)BP神经网络具有对外界刺激和输入信息进行联想记忆的能力，同时对外界输入样本有很强的识别与分类能力，另外还具有优化计算能力；将BP神经网络作为故障检测、隔离与恢复算法，更好地提高了***的抗干扰能力；同时，将误差检测结果引入自适应信息分配因子，将非线性更好地引入，可以更加准确地跟踪各子滤波器的状态，获得更精确的融合结果。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍。
图1是本发明多源自适应容错联邦滤波组合导航***的结构示意图；
图2是本发明多源自适应容错联邦滤波组合导航方法的流程示意图；
图3是BP神经网络的结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。
如图1所示，一种多源自适应容错联邦滤波组合导航***，包括捷联惯导***、卫星导航***、多普勒测速***、天文导航***，以及分别与上述***进行信息连接的主滤波器、子滤波器一、子滤波器二和子滤波器三；上述三个子滤波器均与捷联惯导***进行信息连接，并且通过故障检测与隔离模块与主滤波器连接，捷联惯导***通过状态传播器与故障检测与隔离模块连接，故障检测与隔离模块的输出结果经过信息共享因子计算模块后，将计算结果输入到主滤波器；主滤波器将融合后的信息输出，同时将融合结果同步于三个子滤波器和状态传播器。
故障检测与隔离模块采用BP神经网络作为故障检测、隔离与恢复算法。
本实施例中，主滤波器为无重置联邦滤波器，三个子滤波器为强跟踪滤波器。
如图2所示，一种多源自适应容错联邦滤波组合导航方法，包括如下过程：
步骤一，设置于运动车辆上的传感器进行运动车辆的数据采集，并将数据传输给三个子滤波器；
基于捷联惯导***，通过轨迹发生器，设定运动车辆的运动时长、运动参数以及运动环境参数信息，生成运动车辆的运动轨迹信息，产生捷联惯导***的原始数据，具体如下：
根据运动车辆运动时的物理模型，设置各阶段的运动参数及运动时长，生成运动车辆的运动轨迹包括直线前进、后退、转弯、加速和减速等，并且生成相应的SINS数据。
通过轨迹发生器，利用运动车辆的运动轨迹信息，加入卫星导航***的噪声，生成卫星导航***对运动车辆的位置数据，具体如下：
根据运动车辆的运动轨迹信息，利用轨迹发生器产生运动车辆的三维位置信息，根据卫星导航***误差产生原因，加入噪声，得到运动车辆的位置信息；
通过轨迹发生器，利用运动车辆的运动轨迹信息，加入多普勒测速仪的噪声，生成多普勒测速仪对运动车辆的速度数据，具体如下：
根据运动车辆的运动轨迹信息，利用轨迹发生器产生运动车辆的三维速度信息，根据多普勒测速仪误差产生原因，加入噪声，得到运动车辆的速度信息；
通过轨迹发生器，利用运动车辆的运动轨迹信息，加入天文导航***的噪声，产生天文导航***对运动车辆的姿态数据，具体如下：
根据运动车辆的运动轨迹信息，利用轨迹发生器产生运动车辆的三维姿态信息，根据天文导航***误差产生原因，加入噪声，得到运动车辆的姿态信息。
步骤二，三个子滤波器分别对数据进行滤波处理，并将处理后的数据传输给故障检测与隔离模块；
(1)建立导航坐标系下的联邦滤波组合导航***模型：
以SINS作为参考***，通过摄动法得到SINS误差模型，线性化的***状态方程描述为：
Figure BDA0002363001030000091
其中，x表示***状态向量，
Figure BDA0002363001030000092
表示下一时刻的***状态向量，F表示状态转移函数，w表示状态噪声；
为了实现滤波算法，首先将状态方程离散化，得到***状态向量x从第k-1时刻传播到第k时刻的离散时间状态方程：
xk＝Fk,k-1xk-1+wk
其中，Fk,k-1表示***过渡矩阵，xk表示k时刻的***状态向量，xk-1表示k-1时刻的***状态向量，wk表示过程噪声，且其满足以下统计特性：
E[wk]＝0
Figure BDA0002363001030000101
其中，E[·]表示求一个矩阵的均值，T表示对一个矩阵求转置，δkj表示一个克罗内克函数，Qk是过程噪声的协方差矩阵，并且***状态向量x定义为
Figure BDA0002363001030000102
其中，δφEδφNδφU表示在东、北、上的姿态误差，δvEδvNδvU表示东、北、上的速度误差；δLδλδh表示纬度、经度和高度的位置误差；εrxεryεrz由陀螺漂移造成的误差；
Figure BDA0002363001030000103
由加速度计偏差造成的误差；
(2)三个子滤波器分别对数据进行滤波处理：
(2.1)子滤波器一关于SINS/GNSS的量测方程：
在与GNSS相连接的局部滤波器中，将SINS的位置输出与GNSS之间的差值作为SINS/GNSS测量方程的测量信息，测量方程表示为：
Figure BDA0002363001030000104
其中，LSINS、λSINS、hSINS分别表示SINS***测量的纬度、经度和高度，LGNSS、λGNSS、hGNSS分别表示GNSS***测量的纬度、经度和高度，δL、δλ、δh分别表示SINS***在纬度、经度和高度上与真实位置的误差，v11、v12、v13分别表示GNSS***在纬度、经度和高度上与真实位置的误差，它们是独立的零均值高斯白噪声过程，v1表示由v11、v12、v13组成的矩阵；
H1表示为：
H1＝[03×6diag[111]03×6]
其中，0表示全零矩阵，diag[]表示对角矩阵；
(2.2)子滤波器二关于SINS/DVL的量测方程：
在与DVL相连接的子滤波器二中，将SINS的速度输出与DVL之间的差值作为SINS/DVL测量方程的测量信息，测量方程表示为：
Figure BDA0002363001030000111
其中，vE,SINS、vN,SINS、vU,SINS分别表示SINS***测量的东、北和向上的速度；vE,DVL、vN,DVL、vU,DVL分别表示DVL***测量的东、北和向上的速度；δvE、δvN、δvU分别表示SINS***在东、北和向上速度上与真实速度的误差；v21、v22、v23分别表示DVL***在东、北和向上速度上与真实速度的误差，它们是独立的零均值高斯白噪声过程，v2表示由v21、v22、v23组成的矩阵；
H2表示为：
H2＝[03×3diag[111]03×9]
其中，0表示全零矩阵，diag[]表示对角矩阵；
(2.3)子滤波器三关于SINS/CNS的量测方程：
在与CNS相连接的子滤波器三中，将SINS的姿态输出与CNS之间的差值作为SINS/DVL测量方程的测量信息，测量方程表示为：
Figure BDA0002363001030000112
其中，φE,SINS、φN,SINS、φU,SINS分别表示SINS***测量的东、北和向上的姿态角度；φE,CNS、φN,CNS、φU,CNS分别表示CNS***测量的东、北和向上的姿态角度；δvE、δvN、δvU分别表示SINS***在东、北和向上姿态角度上与真实姿态角度的误差；v21、v22、v23分别表示CNS***在东、北和向上姿态角度上与真实姿态角度的误差；它们是独立的零均值高斯白噪声过程，v2表示由v21、v22、v23组成的矩阵；
H3示为：
H3＝[diag[111]03×12]
其中，0表示全零矩阵，diag[]表示对角矩阵。
步骤三，故障检测与隔离模块对数据进行计算，判断各个子滤波器是否发生故障，若发生故障，则将此子滤波器进行隔离，其信息不能进入主滤波器；若没有故障发生，则将各个子滤波器的输出结果输入到主滤波器中；同时，故障检测与隔离模块将计算结果发送到信息共享因子计算模块；
故障检测与隔离模块采用BP神经网络作为故障检测、隔离与恢复算法，如图3所示，BP神经网络由输入层、隐层和输出层组成，隐层有一层或多层；
首先，通过故障信息与正常信息对BP神经网络进行训练，网络选用S型传递函数
Figure BDA0002363001030000121
通过反传误差函数
Figure BDA0002363001030000122
不断调节网络权值和阈值使误差函数F达到极小，其中，ti为期望输出、Oi为网络的计算输出；
然后通过输入状态传播器与各子滤波器的差值，检测子滤波器是否发生故障，在本BP神经网络中，采用Sigmoid函数作为网络的激励函数，当激励函数输出结果大于0.5时，该子滤波器发生故障，当激励函数输出结果小于0.5时，该子滤波器正常运行；
如果子滤波器发生故障，那么便对这一子滤波器进行隔离，并在下一时刻将主滤波器的输出更新到该子滤波器。
步骤四，信息共享因子计算模块对信息共享因子进行计算后，将结果输入到主滤波器，并反馈给三个子滤波器；
信息共享因子的计算方法如下：
Figure BDA0002363001030000123
Figure BDA0002363001030000124
Figure BDA0002363001030000131
其中，β1、β2、β3别是子滤波器一、子滤波器二、子滤波器三的信息共享因子；y1、y2、y3分别是子滤波器一、子滤波器二、子滤波器三的BP人工神经网络激励函数的输出。
步骤五，主滤波器对接收到的数据进行信息分配和信息融合，并将结果输出，同时，将结果同步于三个子滤波器和状态传播器，对全局进行调控。
信息分配过程如下所示：
Figure BDA0002363001030000132
Figure BDA0002363001030000133
Figure BDA0002363001030000134
其中，
Figure BDA0002363001030000135
表示k时刻第i个子滤波器的过程噪声协方差，Qk表示k时刻主滤波器的过程噪声协方差，
Figure BDA0002363001030000136
表示k时刻第i个子滤波器的估计误差协方差阵，Pk|k表示k时刻主滤波器的估计误差协方差阵，
Figure BDA0002363001030000137
表示k时刻第i个子滤波器的状态估计，
Figure BDA0002363001030000138
表示k时刻主滤波器的状态估计，βi是第i个子滤波器的信息共享因子，并且满足：
Figure BDA0002363001030000139
其中，I表示单位矩阵；
信息融合过程如下所示：
Figure BDA00023630010300001310
Figure BDA00023630010300001311
其中，Pg表示主滤波器的估计误差协方差阵，Pi表示k时刻第i个子滤波器的估计误差协方差阵，
Figure BDA00023630010300001312
表示主滤波器的状态估计，
Figure BDA00023630010300001313
表示k时刻第i个子滤波器的状态估计。
对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (6)

1.一种多源自适应容错联邦滤波组合导航方法，采用一种多源自适应容错联邦滤波组合导航***，其特征在于，组合导航***包括捷联惯导***、卫星导航***、多普勒测速***、天文导航***，以及分别与捷联惯导***、卫星导航***、多普勒测速***、天文导航***进行信息连接的主滤波器、子滤波器一、子滤波器二和子滤波器三；三个子滤波器均与捷联惯导***进行信息连接，并且通过故障检测与隔离模块与主滤波器连接，所述捷联惯导***通过状态传播器与故障检测与隔离模块连接，所述故障检测与隔离模块的输出结果经过信息共享因子计算模块后，将计算结果输入到主滤波器；所述主滤波器将融合后的信息输出，同时将融合结果同步于三个子滤波器和状态传播器；
组合导航方法包括如下过程：
步骤一，设置于运动车辆上的传感器进行运动车辆的数据采集，并将数据传输给三个子滤波器；
步骤二，三个子滤波器分别对数据进行滤波处理，并将处理后的数据传输给故障检测与隔离模块；
步骤三，故障检测与隔离模块对数据进行计算，判断各个子滤波器是否发生故障，若发生故障，则将此子滤波器进行隔离，其信息不能进入主滤波器；若没有故障发生，则将各个子滤波器的输出结果输入到主滤波器中；同时，故障检测与隔离模块将计算结果发送到信息共享因子计算模块；
步骤四，信息共享因子计算模块对信息共享因子进行计算后，将结果输入到主滤波器，并反馈给三个子滤波器；
步骤五，主滤波器对接收到的数据进行信息分配和信息融合，并将结果输出，同时，将结果同步于三个子滤波器和状态传播器，对全局进行调控；
所述步骤三具体如下：
故障检测与隔离模块采用BP神经网络作为故障检测、隔离与恢复算法，BP神经网络由输入层、隐层和输出层组成，隐层有一层或多层；
首先，通过故障信息与正常信息对BP神经网络进行训练，网络选用S型传递函数
Figure FDA0004159701390000011
通过反传误差函数
Figure FDA0004159701390000012
不断调节网络权值和阈值使误差函数F达到极小，其中，ti为期望输出、Oi为网络的计算输出；
然后通过输入状态传播器与各子滤波器的差值，检测子滤波器是否发生故障，在本BP神经网络中，采用Sigmoid函数作为网络的激励函数，当激励函数输出结果大于0.5时，该子滤波器发生故障，当激励函数输出结果小于0.5时，该子滤波器正常运行；
如果子滤波器发生故障，那么便对这一子滤波器进行隔离，并在下一时刻将主滤波器的输出更新到该子滤波器；
所述步骤四中信息共享因子的计算方法如下：
Figure FDA0004159701390000021
Figure FDA0004159701390000022
Figure FDA0004159701390000023
其中，β1、β2、β3别是子滤波器一、子滤波器二、子滤波器三的信息共享因子；y1、y2、y3分别是子滤波器一、子滤波器二、子滤波器三的BP人工神经网络激励函数的输出。
2.根据权利要求1所述的一种多源自适应容错联邦滤波组合导航方法，其特征在于，所述故障检测与隔离模块采用BP神经网络作为故障检测、隔离与恢复算法。
3.根据权利要求1或2所述的一种多源自适应容错联邦滤波组合导航方法，其特征在于，所述主滤波器为无重置联邦滤波器，三个子滤波器为强跟踪滤波器。
4.根据权利要求1所述的一种多源自适应容错联邦滤波组合导航方法，其特征在于，所述步骤一具体如下：
基于捷联惯导***，通过轨迹发生器，设定运动车辆的运动时长、运动参数以及运动环境参数信息，生成运动车辆的运动轨迹信息，产生捷联惯导***的原始数据；
通过轨迹发生器，利用运动车辆的运动轨迹信息，加入卫星导航***的噪声，生成卫星导航***对运动车辆的位置数据；
通过轨迹发生器，利用运动车辆的运动轨迹信息，加入多普勒测速仪的噪声，生成多普勒测速仪对运动车辆的速度数据；
通过轨迹发生器，利用运动车辆的运动轨迹信息，加入天文导航***的噪声，产生天文导航***对运动车辆的姿态数据。
5.根据权利要求1所述的一种多源自适应容错联邦滤波组合导航方法，其特征在于，所述步骤二具体如下：
(1)建立导航坐标系下的联邦滤波组合导航***模型：
以SINS作为参考***，通过摄动法得到SINS误差模型，线性化的***状态方程描述为：
Figure FDA0004159701390000031
其中，x表示***状态向量，
Figure FDA0004159701390000032
表示下一时刻的***状态向量，F表示状态转移函数，w表示状态噪声；
为了实现滤波算法，首先将状态方程离散化，得到***状态向量x从第k-1时刻传播到第k时刻的离散时间状态方程：
xk＝Fk,k-1xk-1+wk
其中，Fk,k-1表示***过渡矩阵，xk表示k时刻的***状态向量，xk-1表示k-1时刻的***状态向量，wk表示过程噪声，且其满足以下统计特性：
E[wk]＝0
Figure FDA0004159701390000033
其中，E[·]表示求一个矩阵的均值，T表示对一个矩阵求转置，δkj表示一个克罗内克函数，Qk是过程噪声的协方差矩阵，并且***状态向量x定义为
Figure FDA0004159701390000034
其中，δφEδφNδφU表示在东、北、上的姿态误差，δvEδvNδvU表示东、北、上的速度误差；δLδλδh表示纬度、经度和高度的位置误差；εrxεryεrz由陀螺漂移造成的误差；
Figure FDA0004159701390000035
由加速度计偏差造成的误差；
(2)三个子滤波器分别对数据进行滤波处理：
(2.1)子滤波器一关于SINS/GNSS的量测方程：
在与GNSS相连接的局部滤波器中，将SINS的位置输出与GNSS之间的差值作为SINS/GNSS测量方程的测量信息，测量方程表示为：
Figure FDA0004159701390000041
其中，LSINS、λSINS、hSINS分别表示SINS***测量的纬度、经度和高度，LGNSS、λGNSS、hGNSS分别表示GNSS***测量的纬度、经度和高度，δL、δλ、δh分别表示SINS***在纬度、经度和高度上与真实位置的误差，v11、v12、v13分别表示GNSS***在纬度、经度和高度上与真实位置的误差，它们是独立的零均值高斯白噪声过程，v1表示由v11、v12、v13组成的矩阵；
H1表示为：
H1＝[03×6 diag[1 1 1] 03×6]
其中，0表示全零矩阵，diag[]表示对角矩阵；
(2.2)子滤波器二关于SINS/DVL的量测方程：
在与DVL相连接的子滤波器二中，将SINS的速度输出与DVL之间的差值作为SINS/DVL测量方程的测量信息，测量方程表示为：
Figure FDA0004159701390000042
其中，vE,SINS、vN,SINS、vU,SINS分别表示SINS***测量的东、北和向上的速度；vE,DVL、vN,DVL、vU,DVL分别表示DVL***测量的东、北和向上的速度；δvE、δvN、δvU分别表示SINS***在东、北和向上速度上与真实速度的误差；v21、v22、v23分别表示DVL***在东、北和向上速度上与真实速度的误差，它们是独立的零均值高斯白噪声过程，v2表示由v21、v22、v23组成的矩阵；
H2表示为：
H2＝[03×3 diag[1 1 1] 03×9]
其中，0表示全零矩阵，diag[]表示对角矩阵；
(2.3)子滤波器三关于SINS/CNS的量测方程：
在与CNS相连接的子滤波器三中，将SINS的姿态输出与CNS之间的差值作为SINS/DVL测量方程的测量信息，测量方程表示为：
Figure FDA0004159701390000051
其中，φE,SINS、φN,SINS、φU,SINS分别表示SINS***测量的东、北和向上的姿态角度；φE,CNS、φN,CNS、φU,CNS分别表示CNS***测量的东、北和向上的姿态角度；δφE、δφN、δφU分别表示SINS***在东、北和向上姿态角度上与真实姿态角度的误差；v31、v32、v33分别表示CNS***在东、北和向上姿态角度上与真实姿态角度的误差；它们是独立的零均值高斯白噪声过程，v3表示由v31、v32、v33组成的矩阵；
H3示为：
H3＝[diag[111]03×12]
其中，0表示全零矩阵，diag[]表示对角矩阵。
6.根据权利要求1所述的一种多源自适应容错联邦滤波组合导航方法，其特征在于，所述步骤五的具体方法如下：
信息分配过程如下所示：
Figure FDA0004159701390000052
Figure FDA0004159701390000053
Figure FDA0004159701390000054
其中，
Figure FDA0004159701390000055
表示k时刻第i个子滤波器的过程噪声协方差，Qk表示k时刻主滤波器的过程噪声协方差，
Figure FDA0004159701390000056
表示k时刻第i个子滤波器的估计误差协方差阵，
Figure FDA0004159701390000057
表示k时刻主滤波器的估计误差协方差阵，
Figure FDA0004159701390000058
表示k时刻第i个子滤波器的状态估计，
Figure FDA0004159701390000059
表示k时刻主滤波器的状态估计，βi是第i个子滤波器的信息共享因子，并且满足：
Figure FDA00041597013900000510
其中，I表示单位矩阵；
信息融合过程如下所示：
Figure FDA0004159701390000061
Figure FDA0004159701390000062
其中，Pg表示主滤波器的估计误差协方差阵，Pi表示k时刻第i个子滤波器的估计误差协方差阵，
Figure FDA0004159701390000063
表示主滤波器的状态估计，
Figure FDA0004159701390000064
表示k时刻第i个子滤波器的状态估计。
CN202010027513.7A 2020-01-10 2020-01-10 一种多源自适应容错联邦滤波组合导航***及导航方法 Active CN111189441B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010027513.7A CN111189441B (zh) 2020-01-10 2020-01-10 一种多源自适应容错联邦滤波组合导航***及导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010027513.7A CN111189441B (zh) 2020-01-10 2020-01-10 一种多源自适应容错联邦滤波组合导航***及导航方法

Publications (2)

Publication Number Publication Date
CN111189441A CN111189441A (zh) 2020-05-22
CN111189441B true CN111189441B (zh) 2023-05-12

Family

ID=70708774

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010027513.7A Active CN111189441B (zh) 2020-01-10 2020-01-10 一种多源自适应容错联邦滤波组合导航***及导航方法

Country Status (1)

Country Link
CN (1) CN111189441B (zh)

Families Citing this family (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111811512B (zh) * 2020-06-02 2023-08-01 北京航空航天大学 基于联邦平滑的mpos离线组合估计方法和装置
CN111854742B (zh) * 2020-07-15 2022-06-21 中南大学 基于多源信息融合的移动物体的测速定位方法和***
CN111829508B (zh) * 2020-07-24 2022-02-08 中国人民解放***箭军工程大学 一种基于新息的容错联邦滤波方法及***
CN114076959A (zh) * 2020-08-20 2022-02-22 华为技术有限公司 故障检测方法、装置及***
CN112180361B (zh) * 2020-09-30 2023-09-26 南京航空航天大学 一种基于动态联邦滤波的车载雷达目标跟踪方法
CN112577485B (zh) * 2020-11-26 2023-11-21 江西省科学院应用物理研究所 一种基于惯性导航的定位***及方法
CN112711055B (zh) * 2020-12-08 2024-03-19 重庆邮电大学 一种基于边缘计算的室内外无缝定位***及方法
CN112525188B (zh) * 2020-12-15 2022-08-05 上海交通大学 一种基于联邦滤波的组合导航方法
CN113375666B (zh) * 2021-06-23 2022-12-27 北京超星未来科技有限公司 一种传感器融合定位***及方法
CN113819911A (zh) * 2021-09-13 2021-12-21 北京理工大学 Gnss失锁下基于自适应容错滤波的导航方法
CN113776527B (zh) * 2021-09-13 2023-04-07 中国民用航空飞行学院 一种民航飞机全时空的组合导航***和导航方法
CN114370870B (zh) * 2022-01-05 2024-04-12 中国兵器工业计算机应用技术研究所 适用于位姿测量卡尔曼滤波的滤波器更新信息筛选方法
CN116105743B (zh) * 2023-04-17 2023-06-27 山东大学 一种联邦滤波***信息因子分配方法及水下导航***
CN117031521B (zh) * 2023-10-08 2024-01-30 山东大学 一种室内外无缝环境下的弹性融合定位方法及***
CN117452448B (zh) * 2023-12-25 2024-04-09 合众新能源汽车股份有限公司 高精定位跳变联合故障诊断方法和车辆定位控制***

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106197408A (zh) * 2016-06-23 2016-12-07 南京航空航天大学 一种基于因子图的多源导航信息融合方法
CN110579740A (zh) * 2019-09-17 2019-12-17 大连海事大学 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法
WO2019242336A1 (zh) * 2018-06-22 2019-12-26 东南大学 一种水下滑翔器导航定位***及上浮精度校正方法

Family Cites Families (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP1570289A4 (en) * 2002-11-15 2008-01-23 Lockheed Corp PRECISION GUIDANCE AND NAVIGATION SYSTEM FOR ALL WEATHER POSITIONS
US20060074558A1 (en) * 2003-11-26 2006-04-06 Williamson Walton R Fault-tolerant system, apparatus and method
CN103323007B (zh) * 2013-06-17 2015-08-19 南京航空航天大学 一种基于时变量测噪声的鲁棒联邦滤波方法
CN103697894B (zh) * 2013-12-27 2016-05-25 南京航空航天大学 基于滤波器方差阵修正的多源信息非等间隔联邦滤波方法
CN104913781A (zh) * 2015-06-04 2015-09-16 南京航空航天大学 一种基于动态信息分配的非等间隔联邦滤波方法
CN105253330B (zh) * 2015-10-30 2017-04-05 中国空间技术研究院 一种基于优化的信息融合geo卫星控制***菜单式设计方法
CN106679693A (zh) * 2016-12-14 2017-05-17 南京航空航天大学 一种基于故障检测的矢量信息分配自适应联邦滤波方法
CN109900300B (zh) * 2019-03-27 2020-12-04 北京航空航天大学 一种用于无人机的组合导航完好性监测***
CN110095800B (zh) * 2019-04-19 2024-01-09 南京理工大学 一种多源融合的自适应容错联邦滤波组合导航方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106197408A (zh) * 2016-06-23 2016-12-07 南京航空航天大学 一种基于因子图的多源导航信息融合方法
WO2019242336A1 (zh) * 2018-06-22 2019-12-26 东南大学 一种水下滑翔器导航定位***及上浮精度校正方法
CN110579740A (zh) * 2019-09-17 2019-12-17 大连海事大学 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法

Non-Patent Citations (9)

* Cited by examiner, † Cited by third party
Title
A fault-tolerant integrated navigation for UAV systems using time-variant vector distribution coefficients;Yueqian Liang et al.;2014 American Control Conference;全文 *
A Robust Single GPS Navigation and Positioning Algorithm Based on Strong Tracking Filtering;Hailiang Xiong et al.;IEEE Sensors Journal;第18卷(第1期);全文 *
一种认知超宽带无线通信***的设计与实现;熊海良等;***工程理论与实践;第34卷(第7期);全文 *
动态捷联惯导/多卫星组合导航自适应联邦滤波算法研究;王春霞;赵伟;刘瑞华;刘建业;;信息与控制(第04期);全文 *
基于BDS/SINS的组合导航***研究;蒋文;中国优秀硕士学位论文全文数据库 信息科技辑(第9期);全文 *
基于神经网络的SINS/GPS/TAN容错组合导航***设计;陆建新,赵大炜,缪栋;上海航天(第02期);全文 *
基于联邦滤波的无人机组合导航***设计;高尚;祝小平;;科学技术与工程(第12期);全文 *
改进故障隔离的容错联邦滤波;熊智;邵慧;华冰;方峥;;航空学报(第03期);全文 *
水下无源组合导航***智能容错方法设计;李佩娟;徐晓苏;王立辉;李瑶;;中国惯性技术学报(第02期);全文 *

Also Published As

Publication number Publication date
CN111189441A (zh) 2020-05-22

Similar Documents

Publication Publication Date Title
CN111189441B (zh) 一种多源自适应容错联邦滤波组合导航***及导航方法
Xiong et al. Fault-tolerant GNSS/SINS/DVL/CNS integrated navigation and positioning mechanism based on adaptive information sharing factors
Shen et al. Dual-optimization for a MEMS-INS/GPS system during GPS outages based on the cubature Kalman filter and neural networks
CN109459040B (zh) 基于rbf神经网络辅助容积卡尔曼滤波的多auv协同定位方法
CN109521454B (zh) 一种基于自学习容积卡尔曼滤波的gps/ins组合导航方法
WO2021208190A1 (zh) 导航板卡、用于导航板卡的多源数据融合方法以及运载体
CN110837854B (zh) 一种基于因子图的auv多源信息融合方法和设备
EP1745261B1 (en) System and method for aligning multiple navigation components
CN110823217A (zh) 一种基于自适应联邦强跟踪滤波的组合导航容错方法
CN103323007B (zh) 一种基于时变量测噪声的鲁棒联邦滤波方法
CN112505737B (zh) 一种gnss/ins组合导航方法
CN109471146B (zh) 一种基于ls-svm的自适应容错gps/ins组合导航方法
Williamson et al. An instrumentation system applied to formation flight
CN111366156A (zh) 基于神经网络辅助的变电站巡检机器人导航方法及***
Jingsen et al. Integrating extreme learning machine with Kalman filter to bridge GPS outages
CN113008229B (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
Sun et al. Fine resolution position estimation using Kalman filtering
CN112697154A (zh) 一种基于矢量分配的自适应多源融合导航方法
CN114136323B (zh) 基于运动补偿下tdoa量测的auv协同导航方法
Yuan et al. Reaearch on underwater integrated navigation system based on SINS/DVL/magnetometer/depth-sensor
Liu et al. Finite-horizon URTSS-based position estimation for urban vehicle localization
Wang et al. Neural network aided Kalman filtering for integrated GPS/INS geo-referencing platform
Wang et al. The aiding of MEMS INS/GPS integration using artificial intelligence for land vehicle navigation
Zhang et al. A integrated navigation algorithm based on distributed Kalman filter
Zhang et al. A suboptimal Kalman filter with fading factors for DGPS/MEMS-IMU/magnetic compass integrated navigation

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