CN114252077A - 基于联邦滤波器的双gps/sins的组合导航方法及系统 - Google Patents

基于联邦滤波器的双gps/sins的组合导航方法及系统 Download PDF

Info

Publication number
CN114252077A
CN114252077A CN202111553793.6A CN202111553793A CN114252077A CN 114252077 A CN114252077 A CN 114252077A CN 202111553793 A CN202111553793 A CN 202111553793A CN 114252077 A CN114252077 A CN 114252077A
Authority
CN
China
Prior art keywords
subsystem
sins
navigation
state
gps
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
Application number
CN202111553793.6A
Other languages
English (en)
Other versions
CN114252077B (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.)
Nanjing University of Science and Technology
Original Assignee
Nanjing University of Science and Technology
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 Nanjing University of Science and Technology filed Critical Nanjing University of Science and Technology
Priority to CN202111553793.6A priority Critical patent/CN114252077B/zh
Publication of CN114252077A publication Critical patent/CN114252077A/zh
Application granted granted Critical
Publication of CN114252077B publication Critical patent/CN114252077B/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/20—Instruments for performing navigational calculations
    • 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/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)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明公开了一种基于联邦滤波器的双GPS/SINS的高容错性、高精度的组合导航方法及系统，方法包括，对微型捷联惯性导航系统的导航的解算，建立捷联惯性导航系统与GPS导航系统的状态方程和量测方程，对联邦滤波器的结构和信息分配系数进行设计，设计导航系统出现故障时所采用的容错处理方式。与传统的GPS/SINS导航系统相比，本方法的提出提高了传统GPS/SINS组合导航系统的精度，并在容错性方面有了很大的提升，增强了系统的健壮性，能够更好的完成导航任务。

Description

基于联邦滤波器的双GPS/SINS的组合导航方法及系统
技术领域
本发明属于容错式组合导航领域，特别是一种应用于无人机的高容错、高精度组合导航方法及系统。
背景技术
容错组合导航系统的研究主要包括两个方面的内容，第一是组合导航，第二是容错技术。其中组合导航是指将各种传感器的量测数据，通过多源信息融合技术将这些数据有机的结合在一起，最终给出载体的姿态、速度、位置等导航信息，容错技术是指能够及时地对系统工作时产生的故障进行检测，实时地将故障子系统进行隔离，并且完成对剩余子系统的重组，使整个导航系统正常的运行。前者主要的工作是提高导航系统的导航精度，而后者是保证系统的健壮性，保证系统能够正常、稳定地运行。
目前，无人机的室外导航定位技术主要有卫星导航、惯性导航、多普勒导航、无线电导航、天文导航等。每种单一的导航系统都有各自的优点和不足之处，随着各种辅助导航手段和现代估计技术的发展，多传感器组合导航系统在近年来获得了广泛的应用。在众多的组合导航方式里，GPS和惯性导航系统的组合被称为黄金组合，两者之间很好的克服了彼此的缺点，取长补短，使组合后的导航精度高于两个系统单独工作的精度。对于GPS/INS的组合导航方式，主要有松组合、紧组合和深组合。针对小型无人机，其导航计算机运算量较小，使得紧组合和深组合的导航方式很难在小型无人机的导航中使用。
同时，组合导航系统通常需要在高动态的环境下进行长时间的工作，这必然会增大传感器发生故障的概率。组合导航系统所采用的故障检测方法有残差检验和状态卡方检验、广义似然比、累积和和序贯概率比检验等。在组合导航系统中，状态卡方检测由于其在计算复杂度和正确性之间的平衡而被广泛采用。状态卡方检测是一种统计假设检验方法，用于检验随机向量是否假设均值和和协方差，它用于测试滤波器和状态传播器状态估计的一致性。状态卡方检测方法对突变故障的检测具有较高的灵敏度，但是针对缓变故障，其刚开始故障量较小，状态卡方检测方法不能及时检测出故障。因此，在发生缓变故障前期，可能会对整个导航系统的导航性能造成不利的影响。
因此，针对小型无人机，改进组合导航的方式，以及提升整个导航系统的健壮性是有必要的。
发明内容
本发明的目的在于针对目前小型无人机长时间导航精度不足，导航系统对于缓变性故障检测不及时，导致精度下降的问题，提供一种基于联邦滤波器的双GPS/SINS的组合导航方法及系统。
实现本发明目的的技术解决方案为：一种基于联邦滤波器的双GPS/SINS的组合导航方法，所述方法包括以下步骤：
步骤1，基于上一时刻捷联惯性导航系统SINS的载体的位置、速度和姿态角，以及当前SINS采集的比力和角速度进行解算，将SINS中的惯性传感器采集的信息转化成导航系统所需要输出的导航参数，即当前时刻载体的姿态、位置和速度；
步骤2，利用步骤1解算出的参数与GPS接收机自身的导航参数，构建每个GPS/SINS组合子系统所需的量测量；
步骤3，将子系统的状态量和量测量传入子滤波器中，对子系统的状态进行滤波估计；
步骤4，根据子系统滤波后的状态量，判断该子系统是否出现故障，并对故障子系统进行隔离；
步骤5，将无故障子系统的输出量传入到主滤波器中，进行全局最优融合，输出双GPS/SINS组合系统的全局最优估计值；
步骤6，利用全局最优估计值，根据极大似然准则，对故障子系统进行估计、修正，并将全局最优估计值反馈给SINS，对SINS的导航参数进行校正，然后输出校正后的导航参数。
一种基于联邦滤波器的双GPS/SINS的组合导航系统，所述系统包括依次执行的：
捷联惯导解算模块，用于基于上一时刻捷联惯性导航系统SINS的载体的位置、速度和姿态角，以及当前SINS采集的比力和角速度进行解算，将SINS中的惯性传感器采集的信息转化成导航系统所需要输出的导航参数，即当前时刻载体的姿态、位置和速度；
量测量构建模块，用于利用捷联惯导解算模块解算出的参数与GPS接收机自身的导航参数，构建每个GPS/SINS组合子系统所需的量测量；
滤波估计模块，用于将子系统的状态量和量测量传入子滤波器中，对子系统的状态进行滤波估计；
故障检测模块，用于根据子系统滤波后的状态量，判断该子系统是否出现故障，并对故障子系统进行隔离；
融合模块，用于将无故障子系统的输出量传入到主滤波器中，进行全局最优融合，输出双GPS/SINS组合系统的全局最优估计值；
校正模块，用于利用全局最优估计值，根据极大似然准则，对故障子系统进行估计、修正，并将全局最优估计值反馈给SINS，对SINS的导航参数进行校正，然后输出校正后的导航参数。
一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行计算机程序时实现以下步骤：
步骤1，基于上一时刻捷联惯性导航系统SINS的载体的位置、速度和姿态角，以及当前SINS采集的比力和角速度进行解算，将SINS中的惯性传感器采集的信息转化成导航系统所需要输出的导航参数，即当前时刻载体的姿态、位置和速度；
步骤2，利用步骤1解算出的参数与GPS接收机自身的导航参数，构建每个GPS/SINS组合子系统所需的量测量；
步骤3，将子系统的状态量和量测量传入子滤波器中，对子系统的状态进行滤波估计；
步骤4，根据子系统滤波后的状态量，判断该子系统是否出现故障，并对故障子系统进行隔离；
步骤5，将无故障子系统的输出量传入到主滤波器中，进行全局最优融合，输出双GPS/SINS组合系统的全局最优估计值；
步骤6，利用全局最优估计值，根据极大似然准则，对故障子系统进行估计、修正，并将全局最优估计值反馈给SINS，对SINS的导航参数进行校正，然后输出校正后的导航参数。
一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现以下步骤：
步骤1，基于上一时刻捷联惯性导航系统SINS的载体的位置、速度和姿态角，以及当前SINS采集的比力和角速度进行解算，将SINS中的惯性传感器采集的信息转化成导航系统所需要输出的导航参数，即当前时刻载体的姿态、位置和速度；
步骤2，利用步骤1解算出的参数与GPS接收机自身的导航参数，构建每个GPS/SINS组合子系统所需的量测量；
步骤3，将子系统的状态量和量测量传入子滤波器中，对子系统的状态进行滤波估计；
步骤4，根据子系统滤波后的状态量，判断该子系统是否出现故障，并对故障子系统进行隔离；
步骤5，将无故障子系统的输出量传入到主滤波器中，进行全局最优融合，输出双GPS/SINS组合系统的全局最优估计值；
步骤6，利用全局最优估计值，根据极大似然准则，对故障子系统进行估计、修正，并将全局最优估计值反馈给SINS，对SINS的导航参数进行校正，然后输出校正后的导航参数。
本发明与现有技术相比，其显著优点为：1)本发明采用的是联邦滤波器的方式实现分散式滤波，使各个子系统独立运行，互不干扰，减少了单个子系统的计算量；2)通过联邦滤波器将导航系统的滤波和融合分开，降低了系统间的通信负担；3)各个子系统均采用位置、速度松组合模式，在保证导航精度的同时，降低了系统的计算量；4)采用一种极大似然估计手段，通过对量测噪声进行估计，来抑制缓变故障，利用卡方检测对突变故障进行检测隔离，增强了系统的容错能力，提高了系统的健壮性。
下面结合附图对本发明作进一步详细描述。
附图说明
图1为一个实施例中基于联邦滤波器的双GPS/SINS的组合导航方法的示意图。
图2为一个实施例中容错导航局部示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。基于本发明的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明的保护范围。
在一个实施例中，本发明提供了一种基于联邦滤波器的双GPS/SINS的高容错、高精度组合导航方法，根据极大似然准则，利用真实残差对子系统的量测噪声阵进行实时的更新，是该算法的重要优化策略。本发明对它的优化可以对导航系统中出现的缓变性故障进行有效的抑制，减小缓变故障对系统性能的影响，从而提高系统的整体性能。如图1和图2所示，本发明所述方法包括以下步骤：
步骤1，基于上一时刻捷联惯性导航系统SINS的载体的位置、速度和姿态角，以及当前SINS采集的比力和角速度进行解算，将SINS中的惯性传感器采集的信息转化成导航系统所需要输出的导航参数，即当前时刻载体的姿态、位置和速度；
步骤2，利用步骤1解算出的参数与GPS接收机自身的导航参数，构建每个GPS/SINS组合子系统所需的量测量；
步骤3，将子系统的状态量和量测量传入子滤波器中，对子系统的状态进行滤波估计；
步骤4，根据子系统滤波后的状态量，判断该子系统是否出现故障，并对故障子系统进行隔离；
步骤5，将无故障子系统的输出量传入到主滤波器中，进行全局最优融合，输出双GPS/SINS组合系统的全局最优估计值；
步骤6，利用全局最优估计值，根据极大似然准则，对故障子系统进行估计、修正，并将全局最优估计值反馈给SINS，对SINS的导航参数进行校正，然后输出校正后的导航参数。
进一步地，在其中一个实施例中，步骤2所述利用步骤1解算出的参数与GPS接收机自身的导航参数，构建每个GPS/SINS组合子系统所需的量测量，具体过程包括：
将步骤1中捷联惯导解算出的位置信息(λI,LI,hI)和速度信息(VxI,VyI,VzI)与GPS采集的定位信息(LG,λG,hG)以及速度信息(VxG,VyG,VzG)相减，构造组合导航子系统的位置量测方程ZP(t)和速度量测方程ZV(t)：
ZP(t)＝HP(t)X(t)+VP(t)
Zv(t)＝Hv(t)X(t)+Vv(t)
式中，下标为I的量表示SINS系统的输出量，下标为G的量表示GPS的输出量，下标x,y,z分别表示导航坐标系的x方向、y方向和z方向，λ表示经度，L表示纬度，h表示高度，V表示速度，位置量测矩阵和速度量测矩阵分别是
Figure BDA0003417961780000051
位置量测噪声VP＝[NN NE NU]T和速度量测噪声Vv＝[MN ME MU]T均为白噪声，其中，R表示地球半径；L是SINS输出的纬度值；O表示零矩阵，其下标表示零矩阵的维度；NN,NE,NU分别表示GPS接收机沿东北天方向的位置误差；MN,ME,MU为GPS接收机在东北天三个方向的测速误差。
进一步地，在其中一个实施例中，步骤3所述将子系统的状态量和量测量传入子滤波器中，对子系统的状态进行滤波估计，具体过程包括：
步骤3-1，利用捷联惯导的位置误差、速度误差、姿态角误差、加速度计零偏误差以及陀螺仪漂移误差，构建子系统的状态量，并将量测的初始值Z0作为状态量XI,0的初始值：
XI,0＝Z0
步骤3-2，对子系统的状态量构建状态方程：
Figure BDA0003417961780000061
式中，X表示状态矢量，W表示白噪声随机误差矢量，状态转移矩阵FI(t)表示为：
Figure BDA0003417961780000062
其中，Fw为SINS中三个方向的位置误差、速度误差、姿态角误差对应的状态转移矩阵，Fs表示为：
Figure BDA0003417961780000063
式中，b表示载体坐标系，t表示导航坐标系，
Figure BDA0003417961780000064
表示载体坐标系到导航坐标系的变换矩阵；
步骤3-3，对所述状态方程进行离散化，得到：
Figure BDA0003417961780000065
式中，
Figure BDA0003417961780000066
Figure BDA0003417961780000067
其中，T为滤波器的迭代周期；
步骤3-4，对子系统的状态量和状态误差协方差阵进行一步预测：
Figure BDA0003417961780000071
Figure BDA0003417961780000072
式中，
Figure BDA0003417961780000073
表示子系统状态量
Figure BDA0003417961780000074
的一步预测值，Pk|k-1表示一步预测状态误差协方差阵；
步骤3-5，利用一步预测状态误差协方差阵Pk|k-1对子系统的滤波增益Kk进行估计：
Figure BDA0003417961780000075
步骤3-6，使用计算出的滤波增益Kk对一步预测状态量进行补偿，得出k时刻的状态估计值
Figure BDA0003417961780000076
并对状态的误差协方差阵Pk进行更新：
Figure BDA0003417961780000077
进一步地，在其中一个实施例中，步骤4根据子系统滤波后的状态量，判断该子系统是否出现故障，具体过程包括：
步骤4-1，利用系统的量测值和状态一步预测值构造子系统的残差序列γk：
Figure BDA0003417961780000078
式中，
Figure BDA0003417961780000079
是步骤3中状态量
Figure BDA00034179617800000710
的一步预测值；
系统残差的方差阵Ak表示为：
Figure BDA00034179617800000711
式中，Pk|k-1是步骤3中一步预测协方差阵，Rk是量测噪声方差阵；
步骤4-2，利用残差序列和残差的方差阵构造卡方故障检测函数κk：
Figure BDA00034179617800000712
故障检测函数是服从自由度为m的卡方分布，其中m是系统状态量的维数；
步骤4-3，设置预警率TD对子系统的运行状态做出判断，其故障判别函数表示如下：
Figure BDA0003417961780000081
进一步地，在其中一个实施例中，步骤5所述将无故障子系统的输出量传入到主滤波器中，进行全局最优融合，输出双GPS/SINS组合系统的全局最优估计量，具体过程包括：
将无故障的子系统的系统状态的滤波值
Figure BDA0003417961780000082
和状态协方差阵Pi,k传入主滤波器，与SINS状态量的一步预测值
Figure BDA0003417961780000083
和状态协方差的预测值Pm,k|k-1进行最优估计：
Figure BDA0003417961780000084
式中，
Figure BDA0003417961780000085
为主滤波器输出的全局最优估计值，Pg,k是其对应的协方差矩阵。
进一步地，在其中一个实施例中，步骤6所述利用全局最优估计量，根据极大似然准则，对故障子系统进行估计、修正，并将全局最优估计量反馈给SINS，对SINS的导航参数进行校正，然后输出校正后的导航参数，该步骤具体对小故障子系统进行估计和修正，该步骤的过程包括：
步骤6-1，利用反馈到子系统的全局最优估计值
Figure BDA0003417961780000086
进行一步预测，采用该预测值构建子系统的真实残差序列εk：
Figure BDA0003417961780000087
Figure BDA0003417961780000088
步骤6-2，计算子系统的残差的协方差Ak为：
Figure BDA0003417961780000089
步骤6-3，根据极大似然准则，采用当前时刻的前m个时刻，对子系统的量测噪声方差阵Rk进行实时修正：
Figure BDA00034179617800000810
步骤6-4，将全局最优估计值反馈给SINS系统，对捷联惯导中的误差进行校正，将校正后的导航参数输出，完成导航任务。
进一步地，在其中一个实施例中，步骤3中的子滤波器、步骤5中的主滤波器均采用卡尔曼滤波。
在一个实施例中，提供了一种基于联邦滤波器的双GPS/SINS的组合导航系统，所述系统包括依次执行的：
捷联惯导解算模块，用于基于上一时刻捷联惯性导航系统SINS的载体的位置、速度和姿态角，以及当前SINS采集的比力和角速度进行解算，将SINS中的惯性传感器采集的信息转化成导航系统所需要输出的导航参数，即当前时刻载体的姿态、位置和速度；
量测量构建模块，用于利用捷联惯导解算模块解算出的参数与GPS接收机自身的导航参数，构建每个GPS/SINS组合子系统所需的量测量；
滤波估计模块，用于将子系统的状态量和量测量传入子滤波器中，对子系统的状态进行滤波估计；
故障检测模块，用于根据子系统滤波后的状态量，判断该子系统是否出现故障，并对故障子系统进行隔离；
融合模块，用于将无故障子系统的输出量传入到主滤波器中，进行全局最优融合，输出双GPS/SINS组合系统的全局最优估计值；
校正模块，用于利用全局最优估计值，根据极大似然准则，对故障子系统进行估计、修正，并将全局最优估计值反馈给SINS，对SINS的导航参数进行校正，然后输出校正后的导航参数。
关于基于联邦滤波器的双GPS/SINS的组合导航系统的具体限定可以参见上文中对于基于联邦滤波器的双GPS/SINS的组合导航方法的限定，在此不再赘述。上述基于联邦滤波器的双GPS/SINS的组合导航系统中的各个模块可全部或部分通过软件实现，也可以借助软硬件结合的形式加以实现。基于这样的理解，上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。本发明未尽事宜为公知技术。
在一个实施例中，提供了一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行计算机程序时实现以下步骤：
步骤1，基于上一时刻捷联惯性导航系统SINS的载体的位置、速度和姿态角，以及当前SINS采集的比力和角速度进行解算，将SINS中的惯性传感器采集的信息转化成导航系统所需要输出的导航参数，即当前时刻载体的姿态、位置和速度；
步骤2，利用步骤1解算出的参数与GPS接收机自身的导航参数，构建每个GPS/SINS组合子系统所需的量测量；
步骤3，将子系统的状态量和量测量传入子滤波器中，对子系统的状态进行滤波估计；
步骤4，根据子系统滤波后的状态量，判断该子系统是否出现故障，并对故障子系统进行隔离；
步骤5，将无故障子系统的输出量传入到主滤波器中，进行全局最优融合，输出双GPS/SINS组合系统的全局最优估计值；
步骤6，利用全局最优估计值，根据极大似然准则，对故障子系统进行估计、修正，并将全局最优估计值反馈给SINS，对SINS的导航参数进行校正，然后输出校正后的导航参数。
关于每一步的具体限定可以参见上文中对于基于联邦滤波器的双GPS/SINS的组合导航方法的限定，在此不再赘述。
在一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现以下步骤：
步骤1，基于上一时刻捷联惯性导航系统SINS的载体的位置、速度和姿态角，以及当前SINS采集的比力和角速度进行解算，将SINS中的惯性传感器采集的信息转化成导航系统所需要输出的导航参数，即当前时刻载体的姿态、位置和速度；
步骤2，利用步骤1解算出的参数与GPS接收机自身的导航参数，构建每个GPS/SINS组合子系统所需的量测量；
步骤3，将子系统的状态量和量测量传入子滤波器中，对子系统的状态进行滤波估计；
步骤4，根据子系统滤波后的状态量，判断该子系统是否出现故障，并对故障子系统进行隔离；
步骤5，将无故障子系统的输出量传入到主滤波器中，进行全局最优融合，输出双GPS/SINS组合系统的全局最优估计值；
步骤6，利用全局最优估计值，根据极大似然准则，对故障子系统进行估计、修正，并将全局最优估计值反馈给SINS，对SINS的导航参数进行校正，然后输出校正后的导航参数。
关于每一步的具体限定可以参见上文中对于基于联邦滤波器的双GPS/SINS的组合导航方法的限定，在此不再赘述。
与传统方法相比，本发明提出的基于联邦滤波器的双GPS/SINS的高容错、高精度导航方法，在提高导航精度的同时，减小了系统的计算量和通信负担，通过对缓变性故障的处理，增强了系统的鲁棒性。
以上显示和描述了本发明的基本原理、主要特征及优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (10)

1.一种基于联邦滤波器的双GPS/SINS的组合导航方法，其特征在于，所述方法包括以下步骤：
步骤1，基于上一时刻捷联惯性导航系统SINS的载体的位置、速度和姿态角，以及当前SINS采集的比力和角速度进行解算，将SINS中的惯性传感器采集的信息转化成导航系统所需要输出的导航参数，即当前时刻载体的姿态、位置和速度；
步骤2，利用步骤1解算出的参数与GPS接收机自身的导航参数，构建每个GPS/SINS组合子系统所需的量测量；
步骤3，将子系统的状态量和量测量传入子滤波器中，对子系统的状态进行滤波估计；
步骤4，根据子系统滤波后的状态量，判断该子系统是否出现故障，并对故障子系统进行隔离；
步骤5，将无故障子系统的输出量传入到主滤波器中，进行全局最优融合，输出双GPS/SINS组合系统的全局最优估计值；
步骤6，利用全局最优估计值，根据极大似然准则，对故障子系统进行估计、修正，并将全局最优估计值反馈给SINS，对SINS的导航参数进行校正，然后输出校正后的导航参数。
2.根据权利要求1所述的基于联邦滤波器的双GPS/SINS的组合导航方法，其特征在于，步骤2所述利用步骤1解算出的参数与GPS接收机自身的导航参数，构建每个GPS/SINS组合子系统所需的量测量，具体过程包括：
将步骤1中捷联惯导解算出的位置信息(λI,LI,hI)和速度信息(VxI,VyI,VzI)与GPS采集的定位信息(LG,λG,hG)以及速度信息(VxG,VyG,VzG)相减，构造组合导航子系统的位置量测方程ZP(t)和速度量测方程ZV(t)：
ZP(t)＝HP(t)X(t)+VP(t)
Zv(t)＝Hv(t)X(t)+Vv(t)
式中，下标为I的量表示SINS系统的输出量，下标为G的量表示GPS的输出量，下标x,y,z分别表示导航坐标系的x方向、y方向和z方向，λ表示经度，L表示纬度，h表示高度，V表示速度，位置量测矩阵和速度量测矩阵分别是
Figure FDA0003417961770000026
位置量测噪声VP＝[NN NE NU]T和速度量测噪声Vv＝[MN ME MU]T均为白噪声，其中，R表示地球半径；L是SINS输出的纬度值；O表示零矩阵，其下标表示零矩阵的维度；NN,NE,NU分别表示GPS接收机沿东北天方向的位置误差；MN,ME,MU为GPS接收机在东北天三个方向的测速误差。
3.根据权利要求1或2所述的基于联邦滤波器的双GPS/SINS的组合导航方法，其特征在于，步骤3所述将子系统的状态量和量测量传入子滤波器中，对子系统的状态进行滤波估计，具体过程包括：
步骤3-1，利用捷联惯导的位置误差、速度误差、姿态角误差、加速度计零偏误差以及陀螺仪漂移误差，构建子系统的状态量，并将量测的初始值Z0作为状态量XI,0的初始值：
XI,0＝Z0
步骤3-2，对子系统的状态量构建状态方程：
Figure FDA0003417961770000021
式中，X表示状态矢量，W表示白噪声随机误差矢量，状态转移矩阵FI(t)表示为：
Figure FDA0003417961770000022
其中，Fw为SINS中三个方向的位置误差、速度误差、姿态角误差对应的状态转移矩阵，Fs表示为：
Figure FDA0003417961770000023
式中，b表示载体坐标系，t表示导航坐标系，
Figure FDA0003417961770000024
表示载体坐标系到导航坐标系的变换矩阵；
步骤3-3，对所述状态方程进行离散化，得到：
Figure FDA0003417961770000025
式中，
Figure FDA0003417961770000031
Figure FDA0003417961770000032
其中，T为滤波器的迭代周期；
步骤3-4，对子系统的状态量和状态误差协方差阵进行一步预测：
Figure FDA0003417961770000033
Figure FDA0003417961770000034
式中，
Figure FDA0003417961770000035
表示子系统状态量
Figure FDA0003417961770000036
的一步预测值，Pk|k-1表示一步预测状态误差协方差阵；
步骤3-5，利用一步预测状态误差协方差阵Pk|k-1对子系统的滤波增益Kk进行估计：
Figure FDA0003417961770000037
步骤3-6，使用计算出的滤波增益Kk对一步预测状态量进行补偿，得出k时刻的状态估计值
Figure FDA0003417961770000038
并对状态的误差协方差阵Pk进行更新：
Figure FDA0003417961770000039
4.根据权利要求3所述的基于联邦滤波器的双GPS/SINS的组合导航方法，其特征在于，步骤4根据子系统滤波后的状态量，判断该子系统是否出现故障，具体过程包括：
步骤4-1，利用系统的量测值和状态一步预测值构造子系统的残差序列γk：
Figure FDA00034179617700000310
式中，
Figure FDA00034179617700000311
是步骤3中状态量
Figure FDA00034179617700000312
的一步预测值；
系统残差的方差阵Ak表示为：
Figure FDA00034179617700000313
式中，Pk|k-1是步骤3中一步预测协方差阵，Rk是量测噪声方差阵；
步骤4-2，利用残差序列和残差的方差阵构造卡方故障检测函数κk：
Figure FDA0003417961770000041
故障检测函数是服从自由度为m的卡方分布，其中m是系统状态量的维数；
步骤4-3，设置预警率TD对子系统的运行状态做出判断，其故障判别函数表示如下：
Figure FDA0003417961770000042
5.根据权利要求4所述的基于联邦滤波器的双GPS/SINS的组合导航方法，其特征在于，步骤5所述将无故障子系统的输出量传入到主滤波器中，进行全局最优融合，输出双GPS/SINS组合系统的全局最优估计量，具体过程包括：
将无故障的子系统的系统状态的滤波值
Figure FDA0003417961770000043
和状态协方差阵Pi,k传入主滤波器，与SINS状态量的一步预测值
Figure FDA0003417961770000044
和状态协方差的预测值Pm,k|k-1进行最优估计：
Figure FDA0003417961770000045
式中，
Figure FDA0003417961770000046
为主滤波器输出的全局最优估计值，Pg,k是其对应的协方差矩阵。
6.根据权利要求5所述的基于联邦滤波器的双GPS/SINS的组合导航方法，其特征在于，步骤6所述利用全局最优估计量，根据极大似然准则，对故障子系统进行估计、修正，并将全局最优估计量反馈给SINS，对SINS的导航参数进行校正，然后输出校正后的导航参数，该步骤具体对小故障子系统进行估计和修正，该步骤的过程包括：
步骤6-1，利用反馈到子系统的全局最优估计值
Figure FDA0003417961770000047
进行一步预测，采用该预测值构建子系统的真实残差序列εk：
Figure FDA0003417961770000048
Figure FDA0003417961770000049
步骤6-2，计算子系统的残差的协方差Ak为：
Figure FDA00034179617700000410
步骤6-3，根据极大似然准则，采用当前时刻的前m个时刻，对子系统的量测噪声方差阵Rk进行实时修正：
Figure FDA0003417961770000051
步骤6-4，将全局最优估计值反馈给SINS系统，对捷联惯导中的误差进行校正，将校正后的导航参数输出，完成导航任务。
7.根据权利要求1所述的基于联邦滤波器的双GPS/SINS的组合导航方法，其特征在于，步骤3中的子滤波器、步骤5中的主滤波器均采用卡尔曼滤波器。
8.一种基于联邦滤波器的双GPS/SINS的组合导航系统，其特征在于，所述系统包括依次执行的：
捷联惯导解算模块，用于基于上一时刻捷联惯性导航系统SINS的载体的位置、速度和姿态角，以及当前SINS采集的比力和角速度进行解算，将SINS中的惯性传感器采集的信息转化成导航系统所需要输出的导航参数，即当前时刻载体的姿态、位置和速度；
量测量构建模块，用于利用捷联惯导解算模块解算出的参数与GPS接收机自身的导航参数，构建每个GPS/SINS组合子系统所需的量测量；
滤波估计模块，用于将子系统的状态量和量测量传入子滤波器中，对子系统的状态进行滤波估计；
故障检测模块，用于根据子系统滤波后的状态量，判断该子系统是否出现故障，并对故障子系统进行隔离；
融合模块，用于将无故障子系统的输出量传入到主滤波器中，进行全局最优融合，输出双GPS/SINS组合系统的全局最优估计值；
校正模块，用于利用全局最优估计值，根据极大似然准则，对故障子系统进行估计、修正，并将全局最优估计值反馈给SINS，对SINS的导航参数进行校正，然后输出校正后的导航参数。
9.一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至7中任一项所述方法的步骤。
10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至7中任一项所述方法的步骤。
CN202111553793.6A 2021-12-17 2021-12-17 基于联邦滤波器的双gps/sins的组合导航方法及系统 Active CN114252077B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111553793.6A CN114252077B (zh) 2021-12-17 2021-12-17 基于联邦滤波器的双gps/sins的组合导航方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111553793.6A CN114252077B (zh) 2021-12-17 2021-12-17 基于联邦滤波器的双gps/sins的组合导航方法及系统

Publications (2)

Publication Number Publication Date
CN114252077A true CN114252077A (zh) 2022-03-29
CN114252077B CN114252077B (zh) 2024-06-18

Family

ID=80792844

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111553793.6A Active CN114252077B (zh) 2021-12-17 2021-12-17 基于联邦滤波器的双gps/sins的组合导航方法及系统

Country Status (1)

Country Link
CN (1) CN114252077B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115453580A (zh) * 2022-09-13 2022-12-09 广东汇天航空航天科技有限公司 Gnss传感器的故障诊断方法、装置、导航系统及交通工具
CN117451034A (zh) * 2023-12-25 2024-01-26 天津云圣智能科技有限责任公司 一种自主导航的方法、装置、存储介质及电子设备

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102095424A (zh) * 2010-12-06 2011-06-15 国营红峰机械厂 一种适合车载光纤航姿系统的姿态测量方法
CN110017837A (zh) * 2019-04-26 2019-07-16 沈阳航空航天大学 一种姿态抗磁干扰的组合导航方法
CN110579740A (zh) * 2019-09-17 2019-12-17 大连海事大学 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法
AU2020101525A4 (en) * 2020-07-28 2020-09-03 Xi`an University of Architecture and Technology Inertial basis combined navigation method for indirect grid frame in polar zone
CN111928846A (zh) * 2020-07-31 2020-11-13 南京理工大学 一种基于联邦滤波的多源融合即插即用组合导航方法
AU2020103939A4 (en) * 2020-12-08 2021-02-11 Xi'an University Of Architecture And Technology Polar Integrated Navigation Algorithm of SINS / GPS Based on Grid Framework

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102095424A (zh) * 2010-12-06 2011-06-15 国营红峰机械厂 一种适合车载光纤航姿系统的姿态测量方法
CN110017837A (zh) * 2019-04-26 2019-07-16 沈阳航空航天大学 一种姿态抗磁干扰的组合导航方法
CN110579740A (zh) * 2019-09-17 2019-12-17 大连海事大学 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法
AU2020101525A4 (en) * 2020-07-28 2020-09-03 Xi`an University of Architecture and Technology Inertial basis combined navigation method for indirect grid frame in polar zone
CN111928846A (zh) * 2020-07-31 2020-11-13 南京理工大学 一种基于联邦滤波的多源融合即插即用组合导航方法
AU2020103939A4 (en) * 2020-12-08 2021-02-11 Xi'an University Of Architecture And Technology Polar Integrated Navigation Algorithm of SINS / GPS Based on Grid Framework

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115453580A (zh) * 2022-09-13 2022-12-09 广东汇天航空航天科技有限公司 Gnss传感器的故障诊断方法、装置、导航系统及交通工具
CN117451034A (zh) * 2023-12-25 2024-01-26 天津云圣智能科技有限责任公司 一种自主导航的方法、装置、存储介质及电子设备
CN117451034B (zh) * 2023-12-25 2024-04-02 天津云圣智能科技有限责任公司 一种自主导航的方法、装置、存储介质及电子设备

Also Published As

Publication number Publication date
CN114252077B (zh) 2024-06-18

Similar Documents

Publication Publication Date Title
CN109373999B (zh) 基于故障容错卡尔曼滤波的组合导航方法
CN110823217B (zh) 一种基于自适应联邦强跟踪滤波的组合导航容错方法
CN111780755B (zh) 一种基于因子图和可观测度分析的多源融合导航方法
CN110579740B (zh) 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法
US11821729B2 (en) Elman neural network assisting tight-integrated navigation method without GNSS signals
CN109781099B (zh) 一种自适应ukf算法的导航方法及系统
CN111928846B (zh) 一种基于联邦滤波的多源融合即插即用组合导航方法
CN109471146B (zh) 一种基于ls-svm的自适应容错gps/ins组合导航方法
US8145419B2 (en) Mobile object position estimation apparatus and method
CN114252077B (zh) 基于联邦滤波器的双gps/sins的组合导航方法及系统
US20070050138A1 (en) Enhanced inertial system performance
EP2927640B1 (en) Global positioning system (gps) self-calibrating lever arm function
CN113465628B (zh) 惯性测量单元数据补偿方法及系统
CN113792488A (zh) 一种双阈值辅助故障容错kf的组合导航系统及方法
CN114689047A (zh) 基于深度学习的组合导航方法、装置、系统及存储介质
CN115451952A (zh) 一种故障检测及抗差自适应滤波的多系统组合导航方法和装置
CN112697154B (zh) 一种基于矢量分配的自适应多源融合导航方法
CN114019954B (zh) 航向安装角标定方法、装置、计算机设备和存储介质
CN112484731B (zh) 一种高精度实时飞行导航计算方法
CN115014321A (zh) 一种基于自适应鲁棒滤波的仿生偏振多源融合定向方法
CN114088104A (zh) 一种自动驾驶场景下的地图生成方法
CN111854728B (zh) 一种基于广义相对熵的容错滤波方法
CN113566849B (zh) 惯性测量单元的安装角度标定方法、装置和计算机设备
CN116086466B (zh) 一种提高ins误差精度的方法
Yu et al. AOA-redundant INS integrated navigation algorithm based on federated Kalman filter

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