[go: up one dir, main page]

CN113834483B - 一种基于可观测度的惯性/偏振/地磁容错导航方法 - Google Patents

一种基于可观测度的惯性/偏振/地磁容错导航方法 Download PDF

Info

Publication number
CN113834483B
CN113834483B CN202111390144.9A CN202111390144A CN113834483B CN 113834483 B CN113834483 B CN 113834483B CN 202111390144 A CN202111390144 A CN 202111390144A CN 113834483 B CN113834483 B CN 113834483B
Authority
CN
China
Prior art keywords
filter
sub
polarization
geomagnetic
matrix
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
CN202111390144.9A
Other languages
English (en)
Other versions
CN113834483A (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.)
Beihang University
Original Assignee
Beihang 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 Beihang University filed Critical Beihang University
Priority to CN202111390144.9A priority Critical patent/CN113834483B/zh
Publication of CN113834483A publication Critical patent/CN113834483A/zh
Application granted granted Critical
Publication of CN113834483B publication Critical patent/CN113834483B/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/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/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
    • G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Environmental & Geological Engineering (AREA)
  • General Life Sciences & Earth Sciences (AREA)
  • Geology (AREA)
  • Navigation (AREA)

Abstract

本发明涉及一种基于可观测度的惯性/偏振/地磁容错导航方法，包括以下步骤：首先，基于瑞利散射模型中偏振矢量与太阳矢量的垂直关系，结合惯导模型建立偏振/惯性子滤波器量测方程；其次，基于载体系和导航系下地磁矢量转换关系，建立地磁/惯性子滤波器量测方程；再次，分别计算两个子滤波器的随机可观测度并设计信息分配因子；然后，在子滤波器的量测更新过程中，基于信息分配因子设计自适应修正因子对状态估计值进行二次修正；最后，在主滤波器进行融合，并采用里程计估计速度和位置误差，将全局估计值反馈给组合导航系统以及对子滤波器进行反馈重置；本发明针对传感器数据不可靠时组合导航系统的容错导航，具有精度高、鲁棒性强等优点。

Description

一种基于可观测度的惯性/偏振/地磁容错导航方法
技术领域
本发明属于组合导航领域，涉及一种基于可观测度的惯性/偏振/地磁容错导航方法，解决在面对传感器异常数据时保证惯性/偏振/地磁组合导航的精度和鲁棒性的问题。
背景技术
组合导航技术是导航领域的一项关键技术。许多动物可以通过感知偏振光或地磁场来进行导航，动物的这种导航方式为我们提供了新思路，基于偏振光和地磁的导航方式受到广泛关注。偏振导航以大气偏振模式为理论依据，通过对天空偏振模式局部或全域的感知来解算姿态和位置信息。相比于其他导航方式，偏振导航具有自主性强、隐蔽性好、无误差累积等优点，但其对天空偏振态变化较为敏感，且受到遮挡时性能会下降等。地磁导航作为一种比较成熟的仿生导航方式，同样具有无源自主等优点，但易受到磁场干扰。为充分发挥偏振导航和地磁导航的优点，将偏振导航与地磁导航与惯性导航进行结合以构建惯性/偏振/地磁组合导航系统，对完成弱卫星环境、强干扰环境和复杂未知环境的自主导航和定位任务具有重大意义。
偏振导航和地磁导航之间的优势互补和深入融合涉及到多传感器信息融合问题。由于复杂环境下传感器数据可能会出现异常、不可靠的情况，信息质量无法得到保证，直接对传感器数据进行融合会导致异常数据污染整个组合导航系统，影响导航系统精度和可靠性。文献“高空长航无人机多源信息高精度容错自主导航研究”通过将可观测度引入导航系统反馈修正中，有效的提高了导航精度，但融合过程中并未建立可观测度和传感器信息质量的关系，无法有效应对传感器数据不可靠情况；文献“复杂城市环境下 GNSS/INS 组合导航可观测度分析及鲁棒滤波方法”和文献“基于可观测度分析和增量因子图的多源融合导航方法”虽建立了可观测度与传感器信息质量的关系，但可观测度计算方法对系统要求较高，且融合方法设计复杂，难以应用于惯性/偏振/地磁组合导航系统中；专利号为202011096081中将可观测度作为自适应因子进行混合校正，但当传感器数据不可靠时会因反馈量不足而影响组合导航系统精度。如何在传感器数据不可靠下保证组合导航的导航性能和稳定性，一直以来是一个研究的重点问题。
针对惯性/偏振/地磁组合导航系统可能出现的传感器数据不可靠、不可靠问题，本发明采用分散式联邦滤波算法，并通过随机可观测度实时反映传感器信息质量，基于随机可观测度设计信息分配因子，在子滤波器中通过信息分配因子进行二次修正，在主滤波器中通过信息分配因子进行动态反馈重置，保证惯性/偏振/地磁组合导航系统在复杂环境中传感器数据可能出现的信息质量低下、不可靠等情况下的精度和稳定性。
发明内容
本发明的技术解决问题是：克服现有方法的不足，提供一种针对惯性/偏振/地磁组合导航系统在传感器数据不可靠情况下的精度高、稳定性强的组合导航方法，该算法基于随机可观测度设计信息分配因子，在子滤波器中基于信息分配因子对估计值进行二次修正，在主滤波器中基于信息分配因子进行动态反馈重置，保证惯性/偏振/地磁组合导航系统的精度和鲁棒性。
本发明的技术解决方案为：一种基于可观测度的惯性/偏振/地磁容错导航方法，实现步骤如下：
(1)通过偏振传感器测量得到偏振方位角
Figure 826826DEST_PATH_IMAGE001
并计算得到导航系下偏振矢量p n ，通过太阳年历计算得到导航系下太阳矢量s n ，基于瑞利散射模型中偏振矢量p n 与太阳矢量s n 的垂直关系，结合惯导误差状态方程建立偏振/惯性子滤波器量测方程；
(2)通过地磁传感器测量得到载体系下地磁矢量m b ，通过国际地磁参考场计算得到导航系下三轴地磁矢量m n ，基于m b 与m n 的坐标转换关系，结合惯导误差状态方程建立地磁/惯性子滤波器量测方程；
(3)结合步骤(1)与步骤(2)建立的两个量测方程，分别计算k时刻偏振/惯性子滤波器和地磁/惯性子滤波器的随机可观测度
Figure 16498DEST_PATH_IMAGE002
，基于随机可观测度设计偏振/惯性子滤波器的信息分配因子
Figure 244217DEST_PATH_IMAGE003
和地磁/惯性子滤波器的信息分配因子
Figure 544749DEST_PATH_IMAGE004
；
(4)基于步骤(3)得到的信息分配因子
Figure 315259DEST_PATH_IMAGE005
和
Figure 800467DEST_PATH_IMAGE004
设计自适应修正因子
Figure 390848DEST_PATH_IMAGE006
，在偏振/惯性子滤波器和地磁惯性子滤波器的量测更新过程中对k时刻误差状态的估计值
Figure 495070DEST_PATH_IMAGE007
和协方差矩阵
Figure 11764DEST_PATH_IMAGE008
进行二次修正；
(5)在主滤波器中融合偏振/惯性子滤波器和地磁/惯性子滤波器的信息，得到全局滤波解，将全局滤波解反馈给组合导航系统，修正组合导航系统姿态信息，与此同时，采用里程计估计速度和位置误差并反馈给组合导航系统修正速度和位置信息，最后根据信息守恒原则，对各子滤波器进行反馈重置；
步骤(1)中，通过偏振传感器测量得到偏振方位角
Figure 543240DEST_PATH_IMAGE009
并计算得到导航系下偏振矢量p n ，通过太阳年历计算得到导航系下太阳矢量s n ，基于瑞利散射模型中偏振矢量p n 与太阳矢量s n 的垂直关系，结合惯导误差状态方程建立偏振/惯性子滤波器量测方程，具体设计过程如下：
选择三维姿态失准角
Figure 620917DEST_PATH_IMAGE010
、东向北向天向速度误差
Figure 387885DEST_PATH_IMAGE011
、经度误差
Figure 867408DEST_PATH_IMAGE012
、纬度误差
Figure 569785DEST_PATH_IMAGE013
、高度误差
Figure 259392DEST_PATH_IMAGE014
，三轴陀螺在x、y、z方向的漂移
Figure 705417DEST_PATH_IMAGE015
、加速度计在x、y、z方向的偏置
Figure 305026DEST_PATH_IMAGE016
作为惯性/偏振/地磁组合导航系统的状态矢量：
Figure 37358DEST_PATH_IMAGE017
系统状态方程为：
Figure 355207DEST_PATH_IMAGE018
其中，
Figure 604923DEST_PATH_IMAGE019
分别为k时刻和k+1时刻的状态量，
Figure 682207DEST_PATH_IMAGE020
为k时刻到k+1时刻系统一步状态转移矩阵，
Figure 460807DEST_PATH_IMAGE021
为系统噪声驱动阵，
Figure 265952DEST_PATH_IMAGE022
为白噪声，其方差记为
Figure 178413DEST_PATH_IMAGE023
；
定义导航系下的偏振矢量为p n ，太阳矢量为s n ，载体系下的偏振矢量为p b ，
Figure 752614DEST_PATH_IMAGE024
为从载体系到导航系的坐标转换矩阵；传感器测量得到偏振角
Figure 967695DEST_PATH_IMAGE009
之后，计算得到p b ：
Figure 260136DEST_PATH_IMAGE025
根据瑞利散射模型，在导航系下偏振矢量p n 垂直于太阳矢量s n ，根据坐标变换关系有
Figure 445129DEST_PATH_IMAGE026
，理论上有：
Figure 873837DEST_PATH_IMAGE027
考虑平台三维姿态失准角误差，建立实测偏振矢量与平台三维姿态失准角的关系为：
Figure 384452DEST_PATH_IMAGE028
其中，
Figure 633031DEST_PATH_IMAGE029
为从载体系到导航系的计算坐标转换矩阵，
Figure 762661DEST_PATH_IMAGE030
为实测的偏振矢量， p b 为理论偏振矢量，
Figure 937553DEST_PATH_IMAGE031
是p b 的反对称矩阵，
Figure 228857DEST_PATH_IMAGE032
是平台三维姿态失准角，v p 为偏振矢量测量误差。基于此，偏振/惯性子滤波器量测模型可表示为：
Figure 230311DEST_PATH_IMAGE033
其中，
Figure 288266DEST_PATH_IMAGE034
，
Figure 425986DEST_PATH_IMAGE035
为偏振量测噪声。
步骤(2)中，通过地磁传感器测量得到载体系下地磁矢量m b ，通过国际地磁参考场计算得到导航系下三轴地磁矢量m n ，基于m b 与m n 的坐标转换关系，结合惯导误差状态方程建立地磁/惯性子滤波器量测方程，具体设计过程如下：
根据国际地磁参考场(IGRF)以及当地地理位置和地理时间计算得到导航系下三轴地磁矢量m n ，三轴地磁传感器可以测得载体系下的地磁矢量m b ，有：
Figure 153771DEST_PATH_IMAGE036
其中，
Figure 767155DEST_PATH_IMAGE037
、
Figure 973008DEST_PATH_IMAGE038
分别表示
Figure 230814DEST_PATH_IMAGE032
和m b 的反对称矩阵。
则地磁/惯性子滤波器量测方程为：
Figure 519713DEST_PATH_IMAGE039
其中，
Figure 495759DEST_PATH_IMAGE040
，
Figure 239724DEST_PATH_IMAGE041
表示地磁量测噪声。
步骤(3)中，结合步骤(1)与步骤(2)建立的两个量测方程，分别计算k时刻偏振/惯性子滤波器和地磁/惯性子滤波器的随机可观测度
Figure 264222DEST_PATH_IMAGE042
，基于随机可观测度设计偏振/惯性子滤波器的信息分配因子
Figure 68230DEST_PATH_IMAGE043
和地磁/惯性子滤波器的信息分配因子
Figure 531573DEST_PATH_IMAGE044
，具体设计过程如下：
建立偏振/惯性子滤波器和地磁/惯性子滤波器量测方程后，计算两个子滤波器的随机可观测度，随机可观测度计算方法如下：
考虑第k时刻的量测噪声方差阵，随机可观测性矩阵
Figure 469442DEST_PATH_IMAGE045
可表示为：
Figure 701840DEST_PATH_IMAGE046
其中，
Figure 207908DEST_PATH_IMAGE047
是k-1时刻到k时刻的状态转移矩阵，H k 是对应子滤波器的量测矩阵，R k 为对应量测噪声方差阵。
定义k时刻第j个状态分量的可观测度为：
Figure 752021DEST_PATH_IMAGE048
其中，N为系统维数，
Figure 368948DEST_PATH_IMAGE049
表示随机可观测性矩阵
Figure 455852DEST_PATH_IMAGE045
的第j个对角元素。
假设第k时刻的第
Figure 132821DEST_PATH_IMAGE050
个子滤波器的可观测度表示为：
Figure 429810DEST_PATH_IMAGE051
令
Figure 584848DEST_PATH_IMAGE052
其中，
Figure 791839DEST_PATH_IMAGE053
为三轴姿态失准角的可观测度，trace{}表示矩阵的迹运算；
则k时刻第
Figure 228DEST_PATH_IMAGE050
个信息分配因子可设计为：
Figure 659880DEST_PATH_IMAGE054
此时，
Figure 618608DEST_PATH_IMAGE055
也满足信息守恒原则：
Figure 414526DEST_PATH_IMAGE056
步骤(4)中，基于步骤(3)得到的信息分配因子
Figure 557932DEST_PATH_IMAGE043
和
Figure 439300DEST_PATH_IMAGE044
设计自适应修正因子
Figure 201720DEST_PATH_IMAGE057
，在偏振/惯性子滤波器和地磁惯性子滤波器的量测更新过程中对k时刻误差状态的估计值
Figure 242357DEST_PATH_IMAGE058
和协方差矩阵
Figure 900871DEST_PATH_IMAGE059
进行二次修正，具体设计过程如下：
假设初始时刻每个子滤波器的信息分配因子为
Figure 535115DEST_PATH_IMAGE060
(
Figure 960280DEST_PATH_IMAGE050
=1,2)，首先为保证各子滤波器的初始估计协方差矩阵和过程噪声协方差矩阵互不相干，将子滤波器和主滤波器的初始估计协方差矩阵和过程噪声协方差矩阵扩大
Figure 996369DEST_PATH_IMAGE061
倍，令全局状态估计值和全局状态估计误差协方差矩阵为X g 和P g ，系统噪声方差阵为Q g ，则初始化过程表示为：
Figure 91364DEST_PATH_IMAGE062
其中，
Figure 212904DEST_PATH_IMAGE063
分别为初始时刻的全局估计值、全局估计误差协方差矩阵和第
Figure 674716DEST_PATH_IMAGE050
个子滤波器的系统噪声方差阵。
子滤波器的时间更新过程为：
Figure 299732DEST_PATH_IMAGE064
其中，
Figure 831208DEST_PATH_IMAGE065
分别为子滤波器状态估计一步预测值和状态估计均方误差一步预测值，
Figure 299098DEST_PATH_IMAGE066
分别为状态估计值和状态估计均方误差，
Figure 941432DEST_PATH_IMAGE067
和
Figure 545589DEST_PATH_IMAGE068
分别为状态转移矩阵与系统噪声驱动矩阵、
Figure 247965DEST_PATH_IMAGE069
为每个子滤波器对应的系统噪声方差阵。
增益计算过程为：
Figure 78518DEST_PATH_IMAGE070
其中，
Figure 524543DEST_PATH_IMAGE071
为滤波增益，
Figure 248785DEST_PATH_IMAGE072
为子滤波器对应的量测矩阵，
Figure 856484DEST_PATH_IMAGE073
为子滤波器的量测噪声方差阵。
按照卡尔曼滤波，估计值表示为：
Figure 174333DEST_PATH_IMAGE074
其中，
Figure 784568DEST_PATH_IMAGE075
为每个子滤波器的估计值。当传感器数据不可靠时，引入修正因子为
Figure 504263DEST_PATH_IMAGE076
（
Figure 548442DEST_PATH_IMAGE077
），对
Figure 353587DEST_PATH_IMAGE075
进行二次修正，修正值
Figure 469DEST_PATH_IMAGE078
表示为：
Figure 574670DEST_PATH_IMAGE079
此时认为根据
Figure 648805DEST_PATH_IMAGE080
求得的修正值可信度为
Figure 675667DEST_PATH_IMAGE081
；
相应的，状态协方差矩阵表示为：
Figure 267185DEST_PATH_IMAGE082
由于修正因子
Figure 695893DEST_PATH_IMAGE083
代表对根据
Figure 206508DEST_PATH_IMAGE084
求得的修正值的可信度，而修正值的可信度与传感器信息质量直接相关，因此用反映传感器信息质量的随机可观测度描述修正因子，即：
Figure 720666DEST_PATH_IMAGE085
步骤(5)中，在主滤波器中融合偏振/惯性子滤波器和地磁/惯性子滤波器的信息，得到全局滤波解，将全局滤波解反馈给组合导航系统，修正系统姿态信息，与此同时，采用里程计估计速度和位置误差并反馈给组合导航系统修正速度和位置信息，最后根据信息守恒原则，对子滤波器进行反馈重置，具体设计过程如下：
在主滤波器中融合偏振/惯性子滤波器和地磁/惯性子滤波器的信息，根据联邦滤波的最优融合定理，将各子滤波和主滤波器同一时刻的局部滤波估计值
Figure 850296DEST_PATH_IMAGE078
和协方差阵
Figure 756679DEST_PATH_IMAGE086
融合得到全局滤波解：
Figure 313562DEST_PATH_IMAGE087
其中，
Figure 315016DEST_PATH_IMAGE088
分别表示融合后的全局滤波估计值和协方差阵。
将全局滤波器反馈给组合导航系统修正组合导航系统姿态信息，假设在捷联解算过程中获得的是平台坐标系到导航坐标系的姿态转换矩阵
Figure 372971DEST_PATH_IMAGE089
，与真实载体系到导航系的姿态转换矩阵
Figure 510691DEST_PATH_IMAGE090
存在失准角
Figure 504055DEST_PATH_IMAGE091
，校正矩阵表示为：
Figure 992805DEST_PATH_IMAGE092
则姿态校正方法为
Figure 588872DEST_PATH_IMAGE093
。
在主滤波器中采用里程计修正速度和位置，里程计量测模型表示为：
Figure 581098DEST_PATH_IMAGE094
其中，
Figure 745364DEST_PATH_IMAGE095
表示3×3的零矩阵，V INS 和V OD 分别为捷联解算的速度和里程计测得的速度信息， P INS 和P OD 分别为捷联解算的位置和里程计测得的位置信息；
利用里程计估计信息校正系统速度和位置，假设校正后的载体速度为
Figure 580464DEST_PATH_IMAGE096
，速度误差估计值为
Figure 590009DEST_PATH_IMAGE097
，则速度校正过程表示为：
Figure 967900DEST_PATH_IMAGE098
假设校正后的载体位置为
Figure 663586DEST_PATH_IMAGE099
，位置误差估计值为
Figure 126929DEST_PATH_IMAGE100
，则位置校正过程表示为：
Figure 940164DEST_PATH_IMAGE101
根据信息守恒原则，利用全局滤波解对偏振/惯性子滤波器和地磁/惯性子滤波器局部滤波值和协方差阵进行重置，将主滤波器融合值
Figure 172562DEST_PATH_IMAGE102
和协方差阵P g 反馈给子滤波器：
Figure 272105DEST_PATH_IMAGE103
其中，
Figure 222744DEST_PATH_IMAGE104
。
本发明与现有技术相比的优点在于：
本发明将偏振导航和地磁导航通过联邦滤波器相结合，且使用随机可观测度来反映传感器信息质量，建立信息分配因子和随机可观测度的关系，实现基于信息质量的在线调整。本发明具有精度高、稳定性好、抗干扰能力强等优点，可用于复杂环境下轮船、无人机、地面机器人等领域的惯性/偏振/地磁组合导航解算过程。
附图说明
图1为本发明一种基于可观测度的惯性/偏振/地磁容错导航方法的设计流程图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整的描述，显然，所描述的实施例仅为本发明的一部分实施例，而不是全部的实施例，基于本发明中的实施例，本领域的普通技术人员在不付出创造性劳动的前提下所获得的所有其他实施例，都属于本发明的保护范围。
如图1所示，本发明涉及一种基于可观测度的惯性/偏振/地磁容错导航方法。第一步，通过偏振传感器测量得到偏振方位角
Figure 839670DEST_PATH_IMAGE001
并计算得到导航系下偏振矢量p n ，通过太阳年历计算得到导航系下太阳矢量s n ，基于瑞利散射模型中偏振矢量p n 与太阳矢量s n 的垂直关系，结合惯导误差状态方程建立偏振/惯性子滤波器量测方程；第二步，通过地磁传感器测量得到载体系下地磁矢量m b ，通过国际地磁参考场计算得到导航系下三轴地磁矢量m n ，基于m b 与m n 的坐标转换关系，结合惯导误差状态方程建立地磁/惯性子滤波器量测方程；第三步，结合第一步与第二步建立的两个量测方程，分别计算k时刻偏振/惯性子滤波器和地磁/惯性子滤波器的随机可观测度
Figure 785629DEST_PATH_IMAGE002
，基于随机可观测度设计偏振/惯性子滤波器的信息分配因子
Figure 462598DEST_PATH_IMAGE005
和地磁/惯性子滤波器的信息分配因子
Figure 634953DEST_PATH_IMAGE004
；第四步，基于第三步得到的信息分配因子
Figure 914625DEST_PATH_IMAGE003
和
Figure 121615DEST_PATH_IMAGE004
设计自适应修正因子
Figure 438327DEST_PATH_IMAGE006
，在偏振/惯性子滤波器和地磁惯性子滤波器的量测更新过程中对k时刻误差状态的估计值
Figure 721148DEST_PATH_IMAGE105
和协方差矩阵
Figure 679876DEST_PATH_IMAGE106
进行二次修正；第五步，在主滤波器中融合偏振/惯性子滤波器和地磁/惯性子滤波器的信息，得到全局滤波解，将全局滤波解反馈给组合导航系统，修正组合导航系统姿态信息，与此同时，采用里程计估计速度和位置误差并反馈给组合导航系统修正速度和位置信息，最后根据信息守恒原则，对子滤波器进行反馈重置；本发明能够使惯性/偏振/地磁组合导航系统面对传感器异常数据时有效做出反映，根据信息质量动态调整融合过程，具有高精度、强鲁棒的特点，适用于复杂环境下轮船、无人机、地面机器人等领域的惯性/偏振/地磁组合导航解算过程。
具体实施步骤如下：
第一步，通过偏振传感器测量得到偏振方位角
Figure 741373DEST_PATH_IMAGE009
并计算得到导航系下偏振矢量p n ，通过太阳年历计算得到导航系下太阳矢量s n ，基于瑞利散射模型中偏振矢量p n 与太阳矢量s n 的垂直关系，结合惯导误差状态方程建立偏振/惯性子滤波器量测方程，具体设计过程如下：
选择三维姿态失准角
Figure 760145DEST_PATH_IMAGE010
、东向北向天向速度误差
Figure 31726DEST_PATH_IMAGE011
、经度误差
Figure 262987DEST_PATH_IMAGE012
、纬度误差
Figure 178991DEST_PATH_IMAGE013
、高度误差
Figure 493297DEST_PATH_IMAGE014
，三轴陀螺在x、y、z方向的漂移
Figure 127541DEST_PATH_IMAGE015
、加速度计在x、y、z方向的偏置
Figure 428072DEST_PATH_IMAGE016
作为惯性/偏振/地磁组合导航系统的状态矢量：
Figure 57637DEST_PATH_IMAGE017
系统状态方程为：
Figure 418211DEST_PATH_IMAGE018
其中，
Figure 539751DEST_PATH_IMAGE019
分别为k时刻和k+1时刻的状态量，
Figure 378394DEST_PATH_IMAGE020
为k时刻到k+1时刻系统一步状态转移矩阵，
Figure 629509DEST_PATH_IMAGE021
为系统噪声驱动阵，
Figure 160984DEST_PATH_IMAGE107
为白噪声，其方差记为
Figure 504241DEST_PATH_IMAGE023
。
定义导航系下的偏振矢量为p n ，太阳矢量为s n ，载体系下的偏振矢量为p b ，
Figure 536788DEST_PATH_IMAGE024
为从载体系到导航系的坐标转换矩阵；传感器测量得到偏振角
Figure 281890DEST_PATH_IMAGE009
之后，计算得到p b ：
Figure 718688DEST_PATH_IMAGE025
根据瑞利散射模型，在导航系下偏振矢量p n 垂直于太阳矢量s n ，根据坐标变换关系有
Figure 673874DEST_PATH_IMAGE026
，理论上有：
Figure 854320DEST_PATH_IMAGE027
考虑三维姿态平台失准角误差，建立实测偏振矢量与平台三维姿态失准角的关系为：
Figure 719508DEST_PATH_IMAGE108
其中，
Figure 451840DEST_PATH_IMAGE029
为从载体系到导航系的计算坐标转换矩阵，
Figure 769689DEST_PATH_IMAGE030
为实测的偏振矢量， p b 为理论偏振矢量，
Figure 753826DEST_PATH_IMAGE109
是p b 的反对称矩阵，
Figure 473520DEST_PATH_IMAGE032
是平台失准角，v p 为偏振矢量测量误差；基于此，偏振/惯性子滤波器量测模型表示为：
Figure 140868DEST_PATH_IMAGE033
其中，
Figure 680434DEST_PATH_IMAGE034
，
Figure 468261DEST_PATH_IMAGE035
为偏振量测噪声。
第二步，通过地磁传感器测量得到载体系下地磁矢量m b ，通过国际地磁参考场计算得到导航系下三轴地磁矢量m n ，基于m b 与m n 的坐标转换关系，结合惯导误差状态方程建立地磁/惯性子滤波器量测方程，具体设计过程如下：
根据国际地磁参考场(IGRF)以及当地地理位置和地理时间可以计算得到导航系下三轴地磁矢量m n ，三轴地磁传感器测得载体系下的地磁矢量m b ，有：
Figure 167096DEST_PATH_IMAGE036
其中，
Figure 382177DEST_PATH_IMAGE037
、
Figure 409039DEST_PATH_IMAGE038
分别表示
Figure 557DEST_PATH_IMAGE032
和m b 的反对称矩阵。
则地磁/惯性子滤波器量测方程为：
Figure 553898DEST_PATH_IMAGE039
其中，
Figure 674301DEST_PATH_IMAGE040
，v mag 表示地磁量测噪声。
第三步，结合第一步与第二步建立的量测方程，分别计算k时刻偏振/惯性子滤波器和地磁/惯性子滤波器的随机可观测度
Figure 454038DEST_PATH_IMAGE002
，基于随机可观测度设计偏振/惯性子滤波器的信息分配因子
Figure 708302DEST_PATH_IMAGE043
和地磁/惯性子滤波器的信息分配因子
Figure 725936DEST_PATH_IMAGE044
，具体设计过程如下：
建立偏振/惯性子滤波器和地磁/惯性子滤波器量测方程后，计算两个子滤波器的随机可观测度，随机可观测度计算方法如下：
考虑第k时刻的量测噪声方差阵，随机可观测性矩阵
Figure 548399DEST_PATH_IMAGE110
可表示为：
Figure 175952DEST_PATH_IMAGE046
其中，
Figure 578114DEST_PATH_IMAGE047
是k-1时刻到k时刻的状态转移矩阵，H k 是对应子滤波器的量测矩阵，R k 为对应量测噪声方差阵。
定义k时刻第j个状态分量的可观测度为：
Figure 981414DEST_PATH_IMAGE048
其中，N为系统维数，
Figure 99411DEST_PATH_IMAGE049
表示随机可观测性矩阵
Figure 322582DEST_PATH_IMAGE110
的第j个对角元素。
假设第k时刻的第
Figure 59594DEST_PATH_IMAGE050
个子滤波器的可观测度表示为：
Figure 176454DEST_PATH_IMAGE051
令
Figure 75140DEST_PATH_IMAGE052
其中，
Figure 51187DEST_PATH_IMAGE053
为三轴姿态失准角的可观测度，trace{}表示矩阵的迹运算；
则动态信息分配因子可设计为：
Figure 326310DEST_PATH_IMAGE111
（
Figure 563256DEST_PATH_IMAGE112
）
此时，
Figure 898423DEST_PATH_IMAGE055
也满足信息守恒原则：
Figure 830607DEST_PATH_IMAGE113
第四步，基于第三步得到的信息分配因子
Figure 532590DEST_PATH_IMAGE043
和
Figure 764988DEST_PATH_IMAGE044
设计自适应修正因子
Figure 5477DEST_PATH_IMAGE057
，在偏振/惯性子滤波器和地磁惯性子滤波器的量测更新过程中对k时刻误差状态的估计值
Figure 815170DEST_PATH_IMAGE058
和协方差矩阵
Figure 432096DEST_PATH_IMAGE059
进行二次修正，具体设计过程如下：
假设初始时刻每个子滤波器的信息分配因子为
Figure 519001DEST_PATH_IMAGE060
(
Figure 930390DEST_PATH_IMAGE050
=1,2)，首先为保证各子滤波器的初始估计协方差矩阵和过程噪声协方差矩阵互不相干，将子滤波器和主滤波器的初始估计协方差矩阵和过程噪声协方差矩阵扩大
Figure 227380DEST_PATH_IMAGE061
倍，令全局状态估计值和全局状态估计均方误差为X g 和P g ，系统噪声方差阵为Q g ，则初始化过程可以表示为：
Figure 647997DEST_PATH_IMAGE062
其中，
Figure 323829DEST_PATH_IMAGE063
分别为初始时刻的全局估计值、全局估计误差协方差矩阵和第
Figure 30753DEST_PATH_IMAGE050
个子滤波器的系统噪声方差阵。
子滤波器的时间更新过程为：
Figure 690405DEST_PATH_IMAGE064
其中，
Figure 649134DEST_PATH_IMAGE114
分别为子滤波器状态估计一步预测值和状态估计均方误差一步预测值，
Figure 71150DEST_PATH_IMAGE115
分别为状态估计值和状态估计均方误差，
Figure 89922DEST_PATH_IMAGE116
和
Figure 971290DEST_PATH_IMAGE068
分别为状态转移矩阵与系统噪声驱动矩阵、
Figure 592764DEST_PATH_IMAGE069
为每个子滤波器对应的系统噪声方差阵。
增益计算过程为：
Figure 508768DEST_PATH_IMAGE070
其中，
Figure 557495DEST_PATH_IMAGE071
为滤波增益，
Figure 191739DEST_PATH_IMAGE117
为子滤波器对应的量测矩阵，
Figure 492270DEST_PATH_IMAGE118
为子滤波器的量测噪声方差阵。
按照卡尔曼滤波，估计值表示为：
Figure 121834DEST_PATH_IMAGE074
其中，
Figure 482409DEST_PATH_IMAGE075
为每个子滤波器的估计值。
当传感器数据不可靠时，引入修正因子为
Figure 338369DEST_PATH_IMAGE119
(
Figure 800181DEST_PATH_IMAGE120
)，对
Figure 690777DEST_PATH_IMAGE075
进行二次修正，修正值
Figure 956673DEST_PATH_IMAGE078
表示为：
Figure 424563DEST_PATH_IMAGE079
此时认为根据
Figure 66897DEST_PATH_IMAGE121
求得的修正值可信度为
Figure 811999DEST_PATH_IMAGE081
。
相应的，状态协方差矩阵表示为：
Figure 373431DEST_PATH_IMAGE082
由于修正因子
Figure 203984DEST_PATH_IMAGE081
代表对根据
Figure 650008DEST_PATH_IMAGE121
求得的修正值的可信度，而修正值的可信度与传感器信息质量直接相关，因此用反映传感器信息质量的随机可观测度描述修正因子，即：
Figure 515196DEST_PATH_IMAGE122
第五步，在主滤波器中融合偏振/惯性子滤波器和地磁/惯性子滤波器的信息，得到全局滤波解，将全局滤波解反馈给组合导航系统，修正系统姿态信息，与此同时，采用里程计估计速度和位置误差并反馈给组合导航系统修正速度和位置信息，最后根据信息守恒原则，对子滤波器进行反馈重置，具体设计过程如下：
在主滤波器中融合偏振/惯性子滤波器和地磁/惯性子滤波器的信息，根据联邦滤波的最优融合定理，将各子滤波和主滤波器同一时刻的局部滤波估计值
Figure 513108DEST_PATH_IMAGE078
和协方差阵
Figure 565378DEST_PATH_IMAGE123
融合得到全局滤波解：
Figure 549514DEST_PATH_IMAGE087
其中，
Figure 895307DEST_PATH_IMAGE088
分别表示融合后的全局滤波估计值和协方差阵；
将全局滤波器反馈给组合导航系统修正组合导航系统姿态信息，假设在捷联解算过程中获得的是平台坐标系到导航坐标系的姿态转换矩阵
Figure 939487DEST_PATH_IMAGE089
，与真实载体系到导航系的姿态转换矩阵
Figure 479052DEST_PATH_IMAGE090
存在失准角
Figure 266880DEST_PATH_IMAGE091
，校正矩阵表示为：
Figure 700135DEST_PATH_IMAGE092
则姿态校正方法为
Figure 915216DEST_PATH_IMAGE093
。
在主滤波器中采用里程计修正速度和位置，里程计量测模型表示为：
Figure 207657DEST_PATH_IMAGE094
其中，
Figure 392651DEST_PATH_IMAGE095
表示3×3的零矩阵，V INS 和V OD 分别为捷联解算的速度和里程计测得的速度信息， P INS 和P OD 分别为捷联解算的位置和里程计测得的位置信息；
利用里程计估计信息校正系统速度和位置，假设校正后的载体速度为
Figure 86937DEST_PATH_IMAGE096
，速度误差估计值为
Figure 472919DEST_PATH_IMAGE097
，则速度校正过程表示为：
Figure 846132DEST_PATH_IMAGE098
假设校正后的载体位置为
Figure 975762DEST_PATH_IMAGE099
，位置误差估计值为
Figure 524555DEST_PATH_IMAGE100
，则位置校正过程表示为：
Figure 704607DEST_PATH_IMAGE101
根据信息守恒原则，利用全局滤波解对偏振/惯性子滤波器和地磁/惯性子滤波器局部滤波值和协方差阵进行重置，将主滤波器融合值和协方差阵反馈给子滤波器：
Figure 440482DEST_PATH_IMAGE124
其中，
Figure 373803DEST_PATH_IMAGE125
。
本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。

Claims (1)

1.一种基于可观测度的惯性/偏振/地磁容错导航方法，其特征在于，实现步骤如下：
(1)通过偏振传感器测量得到偏振方位角
Figure 433084DEST_PATH_IMAGE001
并计算得到导航系下偏振矢量p n ，通过太阳年历计算得到导航系下太阳矢量s n ，基于瑞利散射模型中偏振矢量p n 与太阳矢量s n 的垂直关系，结合惯导误差状态方程建立偏振/惯性子滤波器量测方程；
(2)通过地磁传感器测量得到载体系下地磁矢量m b ，通过国际地磁参考场计算得到导航系下三轴地磁矢量m n ，基于m b 与m n 的坐标转换关系，结合惯导误差状态方程建立地磁/惯性子滤波器量测方程；
(3)结合步骤(1)与步骤(2)建立的两个量测方程，基于随机可观测度理论，分别计算k时刻偏振/惯性子滤波器和地磁/惯性子滤波器的随机可观测度
Figure 893585DEST_PATH_IMAGE002
，基于随机可观测度设计偏振/惯性子滤波器的信息分配因子
Figure 410017DEST_PATH_IMAGE003
和地磁/惯性子滤波器的信息分配因子
Figure 454196DEST_PATH_IMAGE004
；
(4)基于步骤(3)得到的信息分配因子
Figure 665866DEST_PATH_IMAGE005
和
Figure 516010DEST_PATH_IMAGE006
设计自适应修正因子
Figure 11582DEST_PATH_IMAGE007
，在偏振/惯性子滤波器和地磁/惯性子滤波器的量测更新过程中对k时刻误差状态的估计值
Figure 23401DEST_PATH_IMAGE008
和协方差矩阵
Figure 315842DEST_PATH_IMAGE009
进行二次修正；
(5)在主滤波器中融合偏振/惯性子滤波器和地磁/惯性子滤波器的信息，得到全局滤波解，将全局滤波解反馈给组合导航系统，修正组合导航系统姿态信息，与此同时，采用里程计估计速度和位置误差并反馈给组合导航系统修正速度和位置信息，最后根据信息守恒原则，对各子滤波器进行反馈重置；
所述步骤(1)中，通过偏振传感器测量得到偏振方位角
Figure 313885DEST_PATH_IMAGE010
并计算得到导航系下偏振矢量p n ，通过太阳年历计算得到导航系下太阳矢量s n ，基于瑞利散射模型中偏振矢量p n 与太阳矢量s n 的垂直关系，结合惯导误差状态方程建立偏振/惯性子滤波器量测方程，具体设计过程如下：
选择三维姿态失准角
Figure 70488DEST_PATH_IMAGE011
、东向北向天向速度误差
Figure 879306DEST_PATH_IMAGE012
、经度误差
Figure 190202DEST_PATH_IMAGE013
、纬度误差
Figure 319832DEST_PATH_IMAGE014
、高度误差
Figure 806308DEST_PATH_IMAGE015
，三轴陀螺在x、y、z方向的漂移
Figure 159929DEST_PATH_IMAGE016
、加速度计在x、y、z方向的偏置
Figure 82755DEST_PATH_IMAGE017
作为惯性/偏振/地磁组合导航系统的状态矢量：
Figure 16076DEST_PATH_IMAGE019
系统状态方程为：
Figure 481692DEST_PATH_IMAGE020
其中，
Figure 881580DEST_PATH_IMAGE021
分别为k时刻和k+1时刻的状态量，
Figure 167068DEST_PATH_IMAGE022
为k时刻到k+1时刻系统一步状态转移矩阵，
Figure 904080DEST_PATH_IMAGE023
为系统噪声驱动阵，
Figure 581793DEST_PATH_IMAGE024
为白噪声，其方差记为
Figure 277216DEST_PATH_IMAGE025
；
定义导航系下的偏振矢量为p n ，太阳矢量为s n ，载体系下的偏振矢量为p b ，
Figure 190946DEST_PATH_IMAGE026
为从载体系到导航系的坐标转换矩阵；偏振传感器测量得到偏振角
Figure 262807DEST_PATH_IMAGE010
之后，计算得到
Figure 375119DEST_PATH_IMAGE027
：
Figure 631657DEST_PATH_IMAGE028
根据瑞利散射模型，在导航系下偏振矢量p n 垂直于太阳矢量s n ，根据坐标变换关系有
Figure 891737DEST_PATH_IMAGE029
，理论上有：
Figure 642656DEST_PATH_IMAGE030
考虑平台三维姿态失准角误差，建立实测偏振矢量与平台三维姿态失准角的关系为：
Figure 609475DEST_PATH_IMAGE031
其中，
Figure 912280DEST_PATH_IMAGE032
为从载体系到导航系的计算坐标转换矩阵，
Figure 285755DEST_PATH_IMAGE033
为实测的偏振矢量， p b 为理论偏振矢量，
Figure 699419DEST_PATH_IMAGE034
是p b 的反对称矩阵，
Figure 786323DEST_PATH_IMAGE035
是平台三维姿态失准角，v p 为偏振矢量测量误差，基于此，偏振/惯性子滤波器量测模型表示为：
Figure 135396DEST_PATH_IMAGE036
其中，
Figure 370068DEST_PATH_IMAGE037
，
Figure 712057DEST_PATH_IMAGE038
为偏振量测噪声；
所述步骤(2)中，通过地磁传感器测量得到载体系下地磁矢量m b ，通过国际地磁参考场计算得到导航系下三轴地磁矢量m n ，基于m b 与m n 的坐标转换关系，结合惯导误差状态方程建立地磁/惯性子滤波器量测方程，具体设计过程如下：
根据国际地磁参考场以及当地地理位置和地理时间计算得到导航系下三轴地磁矢量m n ，三轴地磁传感器测得载体系下的地磁矢量m b ，有：
Figure 653468DEST_PATH_IMAGE039
其中，
Figure 298076DEST_PATH_IMAGE040
、
Figure 895411DEST_PATH_IMAGE041
分别表示
Figure 650877DEST_PATH_IMAGE035
和m b 的反对称矩阵；
则地磁/惯性子滤波器量测方程为：
Figure 712374DEST_PATH_IMAGE042
其中，
Figure 891332DEST_PATH_IMAGE043
，
Figure 100597DEST_PATH_IMAGE044
表示地磁量测噪声；所述步骤(3)中，结合步骤(1)与步骤(2)建立的量测方程，分别计算k时刻偏振/惯性子滤波器和地磁/惯性子滤波器的随机可观测度
Figure 535120DEST_PATH_IMAGE002
，基于随机可观测度设计偏振/惯性子滤波器的信息分配因子
Figure 247861DEST_PATH_IMAGE045
和地磁/惯性子滤波器的信息分配因子
Figure 437534DEST_PATH_IMAGE046
，具体设计过程如下：
建立偏振/惯性子滤波器和地磁/惯性子滤波器量测方程后，计算两个子滤波器的随机可观测度，随机可观测度计算方法如下：
考虑第k时刻的量测噪声方差阵，随机可观测性矩阵
Figure 727570DEST_PATH_IMAGE047
表示为：
Figure 90418DEST_PATH_IMAGE048
其中，
Figure 798611DEST_PATH_IMAGE049
是k-1时刻到k时刻的状态转移矩阵，H k 是对应子滤波器的量测矩阵，R k 为对应量测噪声方差阵；
定义k时刻第j个状态分量的可观测度为：
Figure 159186DEST_PATH_IMAGE050
其中，N为系统维数，
Figure 811884DEST_PATH_IMAGE051
表示随机可观测性矩阵
Figure 338942DEST_PATH_IMAGE052
的第j个对角元素；
假设第k时刻的第
Figure 291855DEST_PATH_IMAGE053
个子滤波器的可观测度表示为：
Figure 495434DEST_PATH_IMAGE054
令
Figure 635428DEST_PATH_IMAGE055
其中，
Figure 277762DEST_PATH_IMAGE056
为三轴姿态失准角的可观测度，trace{}表示矩阵的迹运算；
则k时刻第
Figure 944236DEST_PATH_IMAGE057
个信息分配因子设计为：
Figure 443350DEST_PATH_IMAGE058
此时，
Figure 211586DEST_PATH_IMAGE059
也满足信息守恒原则：
Figure 454349DEST_PATH_IMAGE060
；所述步骤(4)中，基于步骤(3)得到的信息分配因子
Figure 473864DEST_PATH_IMAGE061
和
Figure 143880DEST_PATH_IMAGE062
设计自适应修正因子
Figure 461729DEST_PATH_IMAGE063
，在偏振/惯性子滤波器和地磁惯性子滤波器的量测更新过程中对k时刻误差状态的估计值
Figure 383548DEST_PATH_IMAGE064
和协方差矩阵
Figure 634401DEST_PATH_IMAGE065
进行二次修正，具体设计过程如下：
假设初始时刻每个子滤波器的信息分配因子为
Figure 865531DEST_PATH_IMAGE066
，
Figure 467414DEST_PATH_IMAGE057
=1,2，首先为保证各子滤波器的初始估计协方差矩阵和过程噪声协方差矩阵互不相干，将子滤波器和主滤波器的初始估计协方差矩阵和过程噪声协方差矩阵扩大
Figure 927345DEST_PATH_IMAGE067
倍，令全局状态估计值和全局状态估计均方误差为X g 和P g ，系统噪声方差阵为Q g ，则初始化过程表示为：
Figure 501546DEST_PATH_IMAGE068
其中，
Figure 513364DEST_PATH_IMAGE069
；
其中，
Figure 494221DEST_PATH_IMAGE070
分别为初始时刻的全局估计值、全局估计误差协方差矩阵和第
Figure 616898DEST_PATH_IMAGE057
个子滤波器的系统噪声方差阵；
子滤波器的时间更新过程为：
Figure 45605DEST_PATH_IMAGE071
其中，
Figure 369270DEST_PATH_IMAGE072
分别为子滤波器状态估计一步预测值和状态估计均方误差一步预测值，
Figure 680166DEST_PATH_IMAGE073
分别为状态估计值和状态估计均方误差，
Figure 996746DEST_PATH_IMAGE074
和
Figure 342277DEST_PATH_IMAGE075
分别为状态转移矩阵与系统噪声驱动矩阵、
Figure 571264DEST_PATH_IMAGE076
为每个子滤波器对应的系统噪声方差阵；
增益计算过程为：
Figure 572718DEST_PATH_IMAGE077
其中，
Figure 302777DEST_PATH_IMAGE078
为滤波增益，
Figure 143561DEST_PATH_IMAGE079
为子滤波器对应的量测矩阵，
Figure 933663DEST_PATH_IMAGE080
为子滤波器的量测噪声方差阵；
按照卡尔曼滤波，估计值表示为：
Figure 94517DEST_PATH_IMAGE081
其中，
Figure 565949DEST_PATH_IMAGE082
为每个子滤波器的估计值；当传感器数据不可靠时，引入修正因子为
Figure 620493DEST_PATH_IMAGE083
，对
Figure 706129DEST_PATH_IMAGE084
进行二次修正，修正值
Figure 744493DEST_PATH_IMAGE085
表示为：
Figure 426141DEST_PATH_IMAGE086
此时认为根据
Figure 600770DEST_PATH_IMAGE087
求得的修正值可信度为
Figure 670357DEST_PATH_IMAGE088
；
相应的，状态协方差矩阵表示为：
Figure 556536DEST_PATH_IMAGE089
由于修正因子
Figure 432088DEST_PATH_IMAGE090
代表对根据
Figure 336590DEST_PATH_IMAGE091
求得的修正值的可信度，而修正值的可信度与传感器信息质量直接相关，因此用反映传感器信息质量的随机可观测度描述修正因子，即：
Figure 639396DEST_PATH_IMAGE092
；
所述步骤(5)中，在主滤波器中融合偏振/惯性子滤波器和地磁/惯性子滤波器的信息，得到全局滤波解，将全局滤波解反馈给组合导航系统，修正系统姿态信息，与此同时，采用里程计估计速度和位置误差并反馈给组合导航系统修正速度和位置信息，最后根据信息守恒原则，对子滤波器进行反馈重置，具体设计过程如下：
在主滤波器中融合偏振/惯性子滤波器和地磁/惯性子滤波器的信息，根据联邦滤波的最优融合定理，将各子滤波和主滤波器同一时刻的局部滤波估计值
Figure 324455DEST_PATH_IMAGE093
和协方差阵
Figure 862752DEST_PATH_IMAGE094
融合得到全局滤波解：
Figure 11974DEST_PATH_IMAGE095
其中，
Figure 361047DEST_PATH_IMAGE096
分别表示融合后的全局滤波估计值和协方差阵；
将全局滤波器反馈给组合导航系统修正组合导航系统姿态信息，假设在捷联解算过程中获得的是平台坐标系到导航坐标系的姿态转换矩阵
Figure 330140DEST_PATH_IMAGE097
，与真实载体系到导航系的姿态转换矩阵
Figure 485178DEST_PATH_IMAGE098
存在失准角，校正矩阵表示为：
Figure 112075DEST_PATH_IMAGE099
则姿态校正方法为
Figure 756683DEST_PATH_IMAGE100
；
在主滤波器中采用里程计修正速度和位置，里程计量测模型表示为：
Figure 354017DEST_PATH_IMAGE101
其中，
Figure 109484DEST_PATH_IMAGE102
表示3×3的零矩 阵，V INS 和V OD 分别为捷联解算的速度和里程计测得的速度信息， P INS 和P OD 分别为捷联解算 的位置和里程计测得的位置信息；
利用里程计估计信息校正系统速度和位置，假设校正后的载体速度为
Figure 826773DEST_PATH_IMAGE103
，速度误差估计值为
Figure 642282DEST_PATH_IMAGE104
，则速度校正过程表示为：
Figure 461334DEST_PATH_IMAGE105
假设校正后的载体位置为
Figure 223753DEST_PATH_IMAGE106
，位置误差估计值为
Figure 936494DEST_PATH_IMAGE107
，则位置校正过程表示为：
Figure 549003DEST_PATH_IMAGE108
根据信息守恒原则，利用全局滤波解对偏振/惯性子滤波器和地磁/惯性子滤波器局部滤波值和协方差阵进行重置，将主滤波器融合值
Figure 979985DEST_PATH_IMAGE109
和协方差阵
Figure 280516DEST_PATH_IMAGE110
反馈给子滤波器：
Figure 988709DEST_PATH_IMAGE111
其中，
Figure 880442DEST_PATH_IMAGE112
，
Figure 188932DEST_PATH_IMAGE113
。
CN202111390144.9A 2021-11-23 2021-11-23 一种基于可观测度的惯性/偏振/地磁容错导航方法 Active CN113834483B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111390144.9A CN113834483B (zh) 2021-11-23 2021-11-23 一种基于可观测度的惯性/偏振/地磁容错导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111390144.9A CN113834483B (zh) 2021-11-23 2021-11-23 一种基于可观测度的惯性/偏振/地磁容错导航方法

Publications (2)

Publication Number Publication Date
CN113834483A CN113834483A (zh) 2021-12-24
CN113834483B true CN113834483B (zh) 2022-03-25

Family

ID=78971576

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111390144.9A Active CN113834483B (zh) 2021-11-23 2021-11-23 一种基于可观测度的惯性/偏振/地磁容错导航方法

Country Status (1)

Country Link
CN (1) CN113834483B (zh)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115014347A (zh) * 2022-05-16 2022-09-06 上海交通大学 一种快速可观测度分析及其指导的多传感器信息融合方法
CN115014321B (zh) * 2022-05-23 2024-03-19 北京航空航天大学杭州创新研究院 一种基于自适应鲁棒滤波的仿生偏振多源融合定向方法
CN115164871B (zh) * 2022-06-16 2024-04-09 北京航空航天大学 一种基于偏振光场时间差分的两步自主定位方法
CN116429096B (zh) * 2023-02-22 2024-09-17 中国科学院国家空间科学中心 一种地磁惯性融合导航方法及系统
CN116182855B (zh) * 2023-04-28 2023-07-07 北京航空航天大学 一种弱光强环境下仿复眼偏振视觉无人机组合导航方法
CN116255988B (zh) * 2023-05-11 2023-07-21 北京航空航天大学 一种基于舰船动力学组合导航复合抗干扰自适应滤波方法

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105758401A (zh) * 2016-05-14 2016-07-13 中卫物联成都科技有限公司 一种基于多源信息融合的组合导航方法和设备
CN107576327A (zh) * 2017-08-07 2018-01-12 西南技术物理研究所 基于可观测度分析的可变结构综合导航系统设计方法
CN110631574A (zh) * 2018-06-22 2019-12-31 北京自动化控制设备研究所 一种惯性/里程计/rtk多信息融合方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9810549B2 (en) * 2011-01-06 2017-11-07 University Of Utah Research Foundation Systems, methods, and apparatus for calibration of and three-dimensional tracking of intermittent motion with an inertial measurement unit

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105758401A (zh) * 2016-05-14 2016-07-13 中卫物联成都科技有限公司 一种基于多源信息融合的组合导航方法和设备
CN107576327A (zh) * 2017-08-07 2018-01-12 西南技术物理研究所 基于可观测度分析的可变结构综合导航系统设计方法
CN110631574A (zh) * 2018-06-22 2019-12-31 北京自动化控制设备研究所 一种惯性/里程计/rtk多信息融合方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
一种基于可观测度分析的SINS/GPS空中对准新方法;刘百奇等;《系统仿真学报》;20080820(第16期);全文 *

Also Published As

Publication number Publication date
CN113834483A (zh) 2021-12-24

Similar Documents

Publication Publication Date Title
CN113834483B (zh) 一种基于可观测度的惯性/偏振/地磁容错导航方法
CN110567454B (zh) 一种复杂环境下sins/dvl紧组合导航方法
CN106767752B (zh) 一种基于偏振信息的组合导航方法
CN109916395B (zh) 一种姿态自主冗余组合导航算法
CN104655152B (zh) 一种基于联邦滤波的机载分布式pos实时传递对准方法
CN111982106A (zh) 导航方法、装置、存储介质及电子装置
CN107797125B (zh) 一种减小深海探测型auv导航定位误差的方法
CN101261130B (zh) 一种船用光纤捷联惯导系统传递对准精度评估方法
CN114018242B (zh) 一种基于偏振/太阳/惯性信息智能匹配的自主定姿方法
CN102829785A (zh) 基于序列图像和基准图匹配的飞行器全参数导航方法
CN104374388A (zh) 一种基于偏振光传感器的航姿测定方法
CN109506660B (zh) 一种用于仿生导航的姿态最优化解算方法
WO2021253488A1 (zh) 一种水下重力无源导航系统
CN105371844A (zh) 一种基于惯性/天文互助的惯性导航系统初始化方法
CN106405592B (zh) 车载北斗载波相位周跳检测与修复方法及系统
CN114964221B (zh) 基于rtk/uwb/imu多单元协同室内外定位方法
CN104697520A (zh) 一体化无陀螺捷联惯导系统与gps系统组合导航方法
CN115388884B (zh) 一种智能体位姿估计器联合初始化方法
CN116222551A (zh) 一种融合多种数据的水下导航方法及装置
CN107677292A (zh) 基于重力场模型的垂线偏差补偿方法
CN107907898A (zh) 基于格网框架的极区sins/gps组合导航算法
CN110887472A (zh) 一种偏振-地磁信息深度融合全自主姿态解算方法
CN113008229A (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN116448145A (zh) 一种基于偏振矢量空间差分的航姿确定方法
CN111207773A (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