CN111780755A - 一种基于因子图和可观测度分析的多源融合导航方法 - Google Patents

一种基于因子图和可观测度分析的多源融合导航方法 Download PDF

Info

Publication number
CN111780755A
CN111780755A CN202010610024.4A CN202010610024A CN111780755A CN 111780755 A CN111780755 A CN 111780755A CN 202010610024 A CN202010610024 A CN 202010610024A CN 111780755 A CN111780755 A CN 111780755A
Authority
CN
China
Prior art keywords
navigation
factor
fusion
sub
source
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
CN202010610024.4A
Other languages
English (en)
Other versions
CN111780755B (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 CN202010610024.4A priority Critical patent/CN111780755B/zh
Publication of CN111780755A publication Critical patent/CN111780755A/zh
Application granted granted Critical
Publication of CN111780755B publication Critical patent/CN111780755B/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/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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled

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

本发明公开了一种基于因子图和可观测度分析的多源融合导航方法。该方法包括以下步骤：构建基于惯导/辅助传感器组合导航模型的多源融合导航系统，得到以惯导为核心，卫星、视觉、里程计中两种或者两种以上为辅助传感器的组合导航鲁棒卡尔曼子滤波器；基于各组合导航鲁棒卡尔曼子滤波器的导航解算结果，度量各各组合导航鲁棒卡尔曼子滤波器状态变量的可观测度；采用增量式因子图架构，根据多源组合导航因子的可信度评价，在线选择最佳因子参与融合，并自动调节信息分配的权重，从而实现跨场景的多源融合导航。本发明能够实现多传感器的自适应融合与安全可靠导航定位，提高了惯性/卫星/视觉等多源导航信息融合的精度和可靠性。

Description

一种基于因子图和可观测度分析的多源融合导航方法
技术领域
本发明涉及组合导航与多源信息融合技术领域，特别是一种基于因子图和可观测度分析的多源融合导航方法。
技术背景
当前，多源融合导航已成为学术界和工业界关注的焦点和研究热点，从自动驾驶到深空探测等领域都具有广泛的应用前景，推动着智能交通、应急救援、智能安防、国防军事等领域发展。为了保证多源融合导航系统导航定位的高精度、强鲁棒和可靠性，有必要综合利用惯性导航系统(Inertial Navigation System,INS)和全球卫星导航系统(GlobalNavigation Satellite System,GNSS)接收机、里程计、相机等辅助传感器，实现多源异构导航信息融合和实时精准定位。目前一般通过加权融合方法、联邦卡尔曼滤波方法、交互式多模型融合等算法对多传感器信息进行融合。
由于复杂环境中的诸多不确定因素(例如雨雪天气、城市峡谷、隧道、地下或室内停车场等)，传感器出现短时间或长期失效的问题，严重影响惯性基组合导航系统的精度和可靠性。由于自主无人系统所处环境随时间不断改变，客观上要求其组合导航模式也应随之变化。因此，需要一个评价机制能在线评估各个传感器的工作状态，实现运行过程中传感器的选择和多传感器管理。
同时，不同种类的传感器通常具有不同的工作频率，融合框架需要对不同频率、不同线性特征的传感器数据进行融合处理。而对于基于传统Kalman滤波的多源融合方法，当导航系统中新加入一种导航传感器时，滤波器的框架需要进行一定的修改。如联邦滤波器的主滤波器中全局估计的协方差阵需要重新计算，这使得联邦滤波器需要一段时间来进行结构的调整以适应新的导航传感器量测信息输入。因此，为主动适应复杂动态场景变化，针对跨场景条件下即插即用的导航需求，迫切需要设计高精度、低延时、结合异步异构传感器的多源融合导航系统。
发明内容
本发明的目的在于提供一种基于因子图和可观测度分析的多源融合导航方法，保证惯性/卫星/视觉等多源导航信息融合的精度和可靠性。
实现本发明目的的技术解决方案为：本发明基于因子图和可观测度分析的多源融合导航方法，包括以下步骤：
步骤1、构建基于惯导/辅助传感器组合导航模型的多源融合导航系统，得到以惯导为核心，卫星、视觉、里程计中两种或者两种以上为辅助传感器的组合导航鲁棒卡尔曼子滤波器；
步骤2、基于各组合导航鲁棒卡尔曼子滤波器的导航解算结果，度量各各组合导航鲁棒卡尔曼子滤波器状态变量的可观测度；
步骤3、采用增量式因子图架构，根据多源组合导航因子的可信度评价，在线选择最佳因子参与融合，并自动调节信息分配的权重，从而实现跨场景的多源融合导航。
本发明与现有技术相比，其显著优点为：(1)通过对惯性/卫星、惯性/视觉等组合导航模式的可观测度分析，在线评价多传感器不同组合方案的可信程度，从而确定适合于当前场景的多传感器优选组合方案；(2)采用因子图架构，基于可观测度分析，构建多源可信组合导航因子，根据传感器感知能力灵活配置和使用各传感器，实现多传感器的自适应融合与安全可靠导航定位。
附图说明
图1为本发明基于因子图和可观测度分析的多源导航信息自适应融合系统结构图。
图2为基于因子图和可观测度分析的多源导航信息自适应融合方法流程图。
图3为增量式因子图的模型示意图。
图4为基于可观测度分析的因子自适应优选示意图。
具体实施方式
为了解决城市峡谷、隧道、地下或室内停车场等复杂场景下卫星信号弱甚至丢失等引起的导航性能下降的问题，本发明提供一种基于因子图和可观测度分析的多源融合导航方法，以保证惯性/卫星/视觉等多源导航信息融合的精度和可靠性。
本发明基于因子图和可观测度分析的多源融合导航方法，包括以下步骤：
步骤1、构建基于惯导/辅助传感器组合导航模型的多源融合导航系统，得到以惯导为核心，卫星、视觉、里程计中两种或者两种以上为辅助传感器的组合导航鲁棒卡尔曼子滤波器；
步骤2、基于各组合导航鲁棒卡尔曼子滤波器的导航解算结果，度量各各组合导航鲁棒卡尔曼子滤波器状态变量的可观测度；
步骤3、采用增量式因子图架构，根据多源组合导航因子的可信度评价，在线选择最佳因子参与融合，并自动调节信息分配的权重，从而实现跨场景的多源融合导航。
进一步地，步骤1所述构建基于惯导/辅助传感器组合导航模型的多源融合导航系统，得到以惯导为核心，卫星、视觉、里程计中两种或者两种以上为辅助传感器的组合导航鲁棒卡尔曼子滤波器，具体如下：
选取东北天即E-N-Up地理坐标系作为多源融合导航解算的坐标系，构建惯导/辅助传感器组合导航模型，确定多源融合导航系统的状态方程和量测方程，其中状态方程为：
xk+1＝Φk+1/kxk+Γkwk (1)
式中，状态变量xk＝[δLk δVk φk εk ▽k]T，δLk、δVk、φk、εk、▽k分别为位置、速度、姿态、陀螺仪漂移和加速度计零偏的误差；wk为系统过程噪声；Γk为过程噪声分布矩阵；Φk+1/k为tk至tk+1时刻的系统状态转移矩阵，下标k表示k时刻；
量测方程为：
zk+1＝Hk+1xk+1+vk+1 (2)
式中zk+1为测量矢量，Hk+1为量测矩阵，vk+1为测量噪声矢量，vk～N(0,Qk)；
卡尔曼滤波算法如下：
Figure BDA0002561744130000031
Qk是系统过程噪声方差矩阵；Rk是系统观测噪声矩阵；Pk+1|k为误差估计的先验协方差矩阵，Pk+1为误差估计的后验协方差矩阵，
Figure BDA0002561744130000032
是xk+1的最优估计，Kk+1是卡尔曼滤波增益；
针对载体机动的不确定性和外界环境动态干扰的随机性，基于比较新息协方差矩阵的递推值和理论计算值，定义自适应因子αk：
Figure BDA0002561744130000033
式中，
Figure BDA0002561744130000034
和
Figure BDA0002561744130000035
为对角矩阵，
Figure BDA0002561744130000036
表示k时刻真实新息协方差的理论值的对角矩阵，
Figure BDA0002561744130000037
表示k时刻理论上计算的新息协方差的对角矩阵，d为k时刻量测值的维数；tr(·)表示矩阵的迹；
随着外界环境的变化，基于式(4)所示的自适应因子，在线调整估计误差协方差Pk以及卡尔曼滤波增益Kk。
进一步地，步骤2所述基于各组合导航鲁棒卡尔曼子滤波器的导航解算结果，度量各各组合导航鲁棒卡尔曼子滤波器状态变量的可观测度，具体如下：
基于线性时变系统的可观测度分析，采用式(5)计算各组合导航子滤波器状态变量的可观测度；
系统状态误差变量的第k时刻的可观测度
Figure BDA0002561744130000041
为：
Figure BDA0002561744130000042
式中，xi是系统的第i个状态变量，yi是系统的第i个状态变量的观测值，观测噪声的方差
Figure BDA0002561744130000043
Figure BDA0002561744130000044
为系统可观测性伪逆矩阵
Figure BDA0002561744130000045
的第i行的元素，j＝1,…,n，n为状态变量总个数；
参照公式(5)，计算位置、速度、姿态误差、陀螺仪漂移和加速度计零偏的误差。
进一步地，步骤3所述采用增量式因子图架构，根据多源组合导航因子的可信度评价，在线选择最佳因子参与融合，并自动调节信息分配的权重，从而实现跨场景的多源融合导航，具体如下：
在计算出每个组合导航子滤波器状态变量的可观测度后，根据信息守恒原则并通过归一化处理，计算出第ζ个子滤波器中的第i个状态变量的信息分配权重
Figure BDA0002561744130000046
Figure BDA0002561744130000047
自适应信息分配因子为
Figure BDA0002561744130000048
采用分段处理策略，设置信息分配权重的阈值，动态调整多源融合过程中随机接入的多源组合导航因子的可信度自调节函数
Figure BDA0002561744130000049
Figure BDA00025617441300000410
为可信度权值；
最优系统状态估计
Figure BDA00025617441300000411
基于最大后验概率准则得到：
Figure BDA00025617441300000412
其中，p(xk|zk)为后验概率，
Figure BDA00025617441300000413
为全局函数因式分解后的局部函数，
Figure BDA00025617441300000414
为多源组合导航状态变量的最优估计；
采用增量平滑的因子图融合算法，对于第一次检测，定义先验因子fPrior；
构建可信度先验因子结点fpζ,k：
Figure BDA0002561744130000051
对于式子(7)(8)(9)所定义的因子结点，假设为高斯噪声模型，则代价函数定义为
Figure BDA0002561744130000052
其中
Figure BDA0002561744130000053
为平方马氏距离，Σ表示协方差矩阵；对于式(7)，可信度权值
Figure BDA0002561744130000054
Figure BDA0002561744130000055
为可信度初始值，Ξk为可信度权值服从的高斯分布协方差矩阵，
Figure BDA0002561744130000056
为可信度权值及其初始值的误差函数；
考虑可信度权值
Figure BDA0002561744130000057
对接入的多源组合导航因子的影响，定义fSm/INS为传感器Sm与惯导组合时的因子结点：
Figure BDA0002561744130000058
式中，
Figure BDA0002561744130000059
为多源组合导航变量因子及量测信息的误差函数；观测变量
Figure BDA00025617441300000510
满足零均值的高斯分布，表示为
Figure BDA00025617441300000511
利用两相邻时刻变量结点之间的约束，定义k及k+1时刻间的二元状态转移因子
Figure BDA00025617441300000512
Figure BDA00025617441300000513
其中
Figure BDA00025617441300000514
为状态变量
Figure BDA00025617441300000515
及
Figure BDA00025617441300000516
间的漂移，
Figure BDA00025617441300000517
为漂移协方差，
Figure BDA00025617441300000518
为状态变量
Figure BDA00025617441300000519
及
Figure BDA00025617441300000520
间的漂移，
Figure BDA00025617441300000521
为两时刻漂移的误差函数；
此时，系统状态的最优估计为：
Figure BDA00025617441300000522
通过高斯牛顿法求解非线性最小二乘问题，经过QR分解使用增量平滑，将最优解更新迭代直到收敛。
下面结合附图及具体实施例对本发明做进一步详细说明。
实施例
结合图1，本发明设计适用于跨场景的多源融合导航系统，以主动适应复杂动态场景变化。首先，构建惯性/卫星、惯性/视觉等组合导航的卡尔曼子滤波器，通过设计自适应鲁棒的卡尔曼滤波算法，优化滤波算法并减少不确定因素的影响；然后，基于动态时变系统的可观测度分析，在线度量惯性/卫星、惯性/视觉等组合导航因子的可信度；最后，采用增量式因子图集中融合架构，构建各惯性基组合导航因子(即子系统)的可信因子，以主动优化因子构建，实现多传感器自适应融合导航与实时精准定位。
结合图2，首先获取传感器量测信息，以惯性导航元件为基础，与辅助导航元件两两之间组合，进行自适应调节协方差及卡尔曼增益的鲁棒卡尔曼滤波；通过鲁棒卡尔曼子滤波器输出的状态估计进行可观测度的计算，构建标量函数计算可信度；将子滤波融合数据送入因子图集中融合模块，首先判断是否为第一次检测，如是则构建先验因子，否则根据因子结点及变量节点组合构建因子图模型，通过可信度描述插入的传感器组合数据估计的不确定程度，动态调整插入的传感器组合；最后使用增量平滑及高斯牛顿算法，判断阈值后循环迭代或输出最优解。
本发明公开的基于因子图和可观测度分析的多源导航信息自适应融合方法，包括如下步骤：
步骤一：构建以惯导为核心，卫星、视觉、里程计等辅助传感器中两种或者两种以上的组合导航子滤波器。
步骤一的具体实现方法为：
选取东北天(E-N-Up)地理坐标系作为多源融合导航解算的坐标系，构建惯导/辅助传感器组合导航模型，给出系统状态方程和量测方程。多源融合导航系统状态方程为
xk+1＝Φk+1/kxk+Γkwk (1)
式中，xk＝[δLk δVk φk εk ▽k]T，包括位置、速度、姿态、陀螺仪漂移和加速度计零偏等误差；wk为系统过程噪声；Γk为过程噪声分布矩阵；Φk+1/k为系统状态转移矩阵，其中包含位置、速度、姿态和传感器误差模型。
多源融合导航系统量测方程为
zk+1＝Hk+1xk+1+vk+1 (2)
式中zk+1为测量矢量，Hk+1为量测(或观测)矩阵，vk+1为测量噪声矢量，vk～N(0,Qk)；
卡尔曼滤波(Kalman Filter,KF)算法如下：
Figure BDA0002561744130000071
Qk是系统过程噪声方差矩阵；Rk是系统观测噪声矩阵；Pk+1|k为误差估计的先验协方差矩阵，Pk+1为误差估计的后验协方差矩阵，
Figure BDA0002561744130000072
是xk+1的最优估计，Kk+1是卡尔曼滤波增益。
针对载体机动的不确定性和外界环境动态干扰的随机性，基于比较新息协方差矩阵的递推值和理论计算值，定义自适应因子αk：
Figure BDA0002561744130000073
式中，
Figure BDA0002561744130000074
和
Figure BDA0002561744130000075
为对角矩阵，
Figure BDA0002561744130000076
表示k时刻真实新息协方差的理论值的对角矩阵，
Figure BDA0002561744130000077
表示k时刻理论上计算的新息协方差的对角矩阵，m为k时刻量测值的维数；tr(·)表示矩阵的迹。
随着外界复杂环境的变化，基于式(4)所示的自适应因子，在线调整估计误差协方差Pk以及卡尔曼滤波增益Kk。
步骤二：基于各子滤波器导航解算结果，在线度量惯性/卫星、惯性/视觉等组合导航因子的可信程度。
步骤二的具体实现方法为：
基于线性时变系统的可观测度分析，采用式(5)，计算各组合导航子滤波器状态变量的可观测度；
系统状态误差变量的第k时刻的可观测度
Figure BDA0002561744130000078
为：
Figure BDA0002561744130000079
式中，xi是系统的第i个状态变量，yi是系统的第i个状态变量的观测值，观测噪声的方差
Figure BDA0002561744130000081
Figure BDA0002561744130000082
为系统可观测性伪逆矩阵
Figure BDA0002561744130000083
的第i行的元素，j＝1,…,n，n为状态变量总个数；
参照公式(5)，计算位置、速度、姿态误差、陀螺仪漂移和加速度计零偏的误差。
步骤三：采用增量式因子图架构，根据多源组合导航因子(即子系统)的可信度评价，在线选择最佳因子参与融合，并自动调节信息分配的权重，从而实现跨场景的多源融合导航。
步骤三的具体实现方法为：
在计算出每个组合导航子滤波器状态变量的可观测度后，根据信息守恒原则并通过归一化处理，计算出第ζ个子滤波器中的第i个状态变量的信息分配权重
Figure BDA0002561744130000084
Figure BDA0002561744130000085
自适应信息分配因子为
Figure BDA0002561744130000086
采用分段处理策略，设置信息分配权重的阈值，动态调整多源融合过程中随机接入的多源组合导航因子的可信度自调节函数
Figure BDA0002561744130000087
Figure BDA0002561744130000088
为可信度权值；
最优系统状态估计
Figure BDA0002561744130000089
基于最大后验概率准则得到：
Figure BDA00025617441300000810
其中，p(xk|zk)为后验概率，
Figure BDA00025617441300000811
为全局函数因式分解后的局部函数，
Figure BDA00025617441300000812
为多源组合导航状态变量的最优估计。
采用增量平滑的因子图融合算法，对于第一次检测，定义先验因子fPrior；
构建可信度先验因子结点fpζ,k：
Figure BDA00025617441300000813
对于式子(7)(8)(9)所定义的因子结点，假设为高斯噪声模型，则代价函数通常可以定义为
Figure BDA00025617441300000814
其中
Figure BDA00025617441300000815
为平方马氏距离，Σ表示协方差矩阵。对于式(7)，可信度权值
Figure BDA00025617441300000816
Figure BDA00025617441300000817
为可信度初始值，Ξk为可信度权值服从的高斯分布协方差矩阵，
Figure BDA00025617441300000818
为可信度权值及其初始值的误差函数；
考虑可信度权值
Figure BDA0002561744130000091
对接入的多源组合导航因子的影响，定义fSm/INS为传感器Sm与惯导组合时的因子结点：
Figure BDA0002561744130000092
式中，
Figure BDA0002561744130000093
为多源组合导航变量因子及量测信息的误差函数；观测变量
Figure BDA0002561744130000094
满足零均值的高斯分布，表示为
Figure BDA0002561744130000095
利用两相邻时刻变量结点之间的约束，定义k及k+1时刻间的二元状态转移因子
Figure BDA0002561744130000096
Figure BDA0002561744130000097
其中
Figure BDA0002561744130000098
为状态变量
Figure BDA0002561744130000099
及
Figure BDA00025617441300000910
间的漂移，
Figure BDA00025617441300000911
为漂移协方差，
Figure BDA00025617441300000912
状态变量
Figure BDA00025617441300000913
及
Figure BDA00025617441300000914
间的漂移，
Figure BDA00025617441300000915
为两时刻漂移的误差函数；
此时，系统状态的最优估计为：
Figure BDA00025617441300000916
通过高斯牛顿法求解非线性最小二乘问题，经过QR分解使用增量平滑，将最优解更新迭代直到收敛。
图3中大空心圆圈表示变量结点，小实心圆圈表示因子结点，xi表示第i时刻的状态变量，每个状态向量连接m组传感器的自适应因子fSm/INS，通过先验信息构建初始时刻的因子结点fPrior，状态转移因子fst[0,1]连接0到1时刻的状态向量，自适应分配因子
Figure BDA00025617441300000917
动态调整fSm/INS的变换函数，初始值由fpζ,k确定，后续时刻以此类推。
图4中惯导联合卫星、里程计、视觉传感器进行位置估计。位置i的多源融合过程用浅色实线表示，位置i+1的多源融合过程用深色实线表示，两状态之间约束用虚线表示。当场景变换导致传感器效果变差，例如卫星的信号丢失，本发明的算法将按照可信的程度削减卫星信号对定位的影响(虚线)。
本发明通过对惯性/卫星、惯性/视觉等组合导航模式的可观测度分析，在线评价多传感器不同组合方案的可信程度，从而确定适合于当前场景的多传感器优选组合方案；采用因子图架构，基于可观测度分析，构建多源可信组合导航因子，根据传感器感知能力灵活配置和使用各传感器，实现了多传感器的自适应融合与安全可靠导航定位。

Claims (4)

1.一种基于因子图和可观测度分析的多源融合导航方法，其特征在于，包括以下步骤：
步骤1、构建基于惯导/辅助传感器组合导航模型的多源融合导航系统，得到以惯导为核心，卫星、视觉、里程计中两种或者两种以上为辅助传感器的组合导航鲁棒卡尔曼子滤波器；
步骤2、基于各组合导航鲁棒卡尔曼子滤波器的导航解算结果，度量各各组合导航鲁棒卡尔曼子滤波器状态变量的可观测度；
步骤3、采用增量式因子图架构，根据多源组合导航因子的可信度评价，在线选择最佳因子参与融合，并自动调节信息分配的权重，从而实现跨场景的多源融合导航。
2.根据权利要求1所述的基于因子图和可观测度分析的多源融合导航方法，其特征在于，步骤1所述构建基于惯导/辅助传感器组合导航模型的多源融合导航系统，得到以惯导为核心，卫星、视觉、里程计中两种或者两种以上为辅助传感器的组合导航鲁棒卡尔曼子滤波器，具体如下：
选取东北天即E-N-Up地理坐标系作为多源融合导航解算的坐标系，构建惯导/辅助传感器组合导航模型，确定多源融合导航系统的状态方程和量测方程，其中状态方程为：
xk+1＝Φk+1/kxk+Γkwk (1)
式中，状态变量
Figure FDA0002561744120000011
δLk、δVk、φk、εk、
Figure FDA0002561744120000012
分别为位置、速度、姿态、陀螺仪漂移和加速度计零偏的误差；wk为系统过程噪声；Γk为过程噪声分布矩阵；Φk+1/k为tk至tk+1时刻的系统状态转移矩阵，下标k表示k时刻；
量测方程为：
zk+1＝Hk+1xk+1+vk+1 (2)
式中zk+1为测量矢量，Hk+1为量测矩阵，vk+1为测量噪声矢量，vk～N(0,Qk)；
卡尔曼滤波算法如下：
Figure FDA0002561744120000013
Qk是系统过程噪声方差矩阵；Rk是系统观测噪声矩阵；Pk+1|k为误差估计的先验协方差矩阵，Pk+1为误差估计的后验协方差矩阵，
Figure FDA0002561744120000021
是xk+1的最优估计，Kk+1是卡尔曼滤波增益；
针对载体机动的不确定性和外界环境动态干扰的随机性，基于比较新息协方差矩阵的递推值和理论计算值，定义自适应因子αk：
Figure FDA0002561744120000022
式中，
Figure FDA0002561744120000023
和
Figure FDA0002561744120000024
为对角矩阵，
Figure FDA0002561744120000025
表示k时刻真实新息协方差的理论值的对角矩阵，
Figure FDA0002561744120000026
表示k时刻理论上计算的新息协方差的对角矩阵，d为k时刻量测值的维数；tr(·)表示矩阵的迹；
随着外界环境的变化，基于式(4)所示的自适应因子，在线调整估计误差协方差Pk以及卡尔曼滤波增益Kk。
3.根据权利要求2所述的基于因子图和可观测度分析的多源融合导航方法，其特征在于，步骤2所述基于各组合导航鲁棒卡尔曼子滤波器的导航解算结果，度量各各组合导航鲁棒卡尔曼子滤波器状态变量的可观测度，具体如下：
基于线性时变系统的可观测度分析，采用式(5)计算各组合导航子滤波器状态变量的可观测度；
系统状态误差变量的第k时刻的可观测度
Figure FDA0002561744120000027
为：
Figure FDA0002561744120000028
式中，xi是系统的第i个状态变量，yi是系统的第i个状态变量的观测值，观测噪声的方差
Figure FDA0002561744120000029
Figure FDA00025617441200000210
为系统可观测性伪逆矩阵
Figure FDA00025617441200000211
的第i行的元素，j＝1,…,n，n为状态变量总个数；
参照公式(5)，计算位置、速度、姿态误差、陀螺仪漂移和加速度计零偏的误差。
4.根据权利要求3所述的基于因子图和可观测度分析的多源融合导航方法，其特征在于，步骤3所述采用增量式因子图架构，根据多源组合导航因子的可信度评价，在线选择最佳因子参与融合，并自动调节信息分配的权重，从而实现跨场景的多源融合导航，具体如下：
在计算出每个组合导航子滤波器状态变量的可观测度后，根据信息守恒原则并通过归一化处理，计算出第ζ个子滤波器中的第i个状态变量的信息分配权重
Figure FDA0002561744120000031
Figure FDA0002561744120000032
自适应信息分配因子为
Figure FDA0002561744120000033
采用分段处理策略，设置信息分配权重的阈值，动态调整多源融合过程中随机接入的多源组合导航因子的可信度自调节函数
Figure FDA0002561744120000034
Figure FDA0002561744120000035
为可信度权值；
最优系统状态估计
Figure FDA0002561744120000036
基于最大后验概率准则得到：
Figure FDA0002561744120000037
其中，p(xk|zk)为后验概率，
Figure FDA0002561744120000038
为全局函数因式分解后的局部函数，
Figure FDA0002561744120000039
为多源组合导航状态变量的最优估计；
采用增量平滑的因子图融合算法，对于第一次检测，定义先验因子fPrior；
构建可信度先验因子结点fpζ,k：
Figure FDA00025617441200000310
对于式子(7)(8)(9)所定义的因子结点，假设为高斯噪声模型，则代价函数定义为
Figure FDA00025617441200000311
其中
Figure FDA00025617441200000312
为平方马氏距离，Σ表示协方差矩阵；对于式(7)，可信度权值
Figure FDA00025617441200000313
Figure FDA00025617441200000314
为可信度初始值，Ξk为可信度权值服从的高斯分布协方差矩阵，
Figure FDA00025617441200000315
为可信度权值及其初始值的误差函数；
考虑可信度权值
Figure FDA00025617441200000316
对接入的多源组合导航因子的影响，定义fSm/INS为传感器Sm与惯导组合时的因子结点：
Figure FDA00025617441200000317
式中，
Figure FDA00025617441200000318
为多源组合导航变量因子及量测信息的误差函数；观测变量
Figure FDA00025617441200000319
满足零均值的高斯分布，表示为
Figure FDA00025617441200000320
利用两相邻时刻变量结点之间的约束，定义k及k+1时刻间的二元状态转移因子
Figure FDA0002561744120000041
Figure FDA0002561744120000042
其中
Figure FDA0002561744120000043
为状态变量
Figure FDA0002561744120000044
及
Figure FDA0002561744120000045
间的漂移，
Figure FDA0002561744120000046
为漂移协方差，
Figure FDA0002561744120000047
为状态变量
Figure FDA0002561744120000048
及
Figure FDA0002561744120000049
间的漂移，
Figure FDA00025617441200000410
为两时刻漂移的误差函数；
此时，系统状态的最优估计为：
Figure FDA00025617441200000411
通过高斯牛顿法求解非线性最小二乘问题，经过QR分解使用增量平滑，将最优解更新迭代直到收敛。
CN202010610024.4A 2020-06-30 2020-06-30 一种基于因子图和可观测度分析的多源融合导航方法 Active CN111780755B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010610024.4A CN111780755B (zh) 2020-06-30 2020-06-30 一种基于因子图和可观测度分析的多源融合导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010610024.4A CN111780755B (zh) 2020-06-30 2020-06-30 一种基于因子图和可观测度分析的多源融合导航方法

Publications (2)

Publication Number Publication Date
CN111780755A true CN111780755A (zh) 2020-10-16
CN111780755B CN111780755B (zh) 2023-05-05

Family

ID=72760820

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010610024.4A Active CN111780755B (zh) 2020-06-30 2020-06-30 一种基于因子图和可观测度分析的多源融合导航方法

Country Status (1)

Country Link
CN (1) CN111780755B (zh)

Cited By (30)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112379395A (zh) * 2020-11-24 2021-02-19 中国人民解放军海军工程大学 一种定位导航授时系统
CN112560974A (zh) * 2020-12-22 2021-03-26 清华大学 一种信息融合、车辆信息获取方法及装置
CN112556690A (zh) * 2020-11-09 2021-03-26 北京全路通信信号研究设计院集团有限公司 一种多源传感器融合定位方法及装置
CN112577496A (zh) * 2020-11-25 2021-03-30 哈尔滨工程大学 一种基于自适应选权的多源融合定位方法
CN112672299A (zh) * 2020-12-09 2021-04-16 电子科技大学 一种基于多源异构信息融合的传感器数据可信度评估方法
CN112880659A (zh) * 2021-01-09 2021-06-01 西北工业大学 一种基于信息概率的融合定位方法
CN112904388A (zh) * 2020-12-05 2021-06-04 哈尔滨工程大学 基于领航者策略的融合定位跟踪控制方法
CN112925000A (zh) * 2021-01-25 2021-06-08 东南大学 基于可见光通信和惯性导航的隧道环境下车辆定位方法
CN113034903A (zh) * 2021-03-05 2021-06-25 交通运输部公路科学研究所 基于多源信息融合的交通状态估计方法和装置
CN113053106A (zh) * 2021-03-05 2021-06-29 交通运输部公路科学研究所 基于多传感器信息融合的交通状态估计方法和装置
CN113063429A (zh) * 2021-03-18 2021-07-02 苏州华米导航科技有限公司 一种自适应车载组合导航定位方法
CN113076826A (zh) * 2021-03-19 2021-07-06 广州小鹏自动驾驶科技有限公司 一种传感器的滤波方法和装置
CN113155124A (zh) * 2021-04-27 2021-07-23 涵涡智航科技(玉溪)有限公司 一种多源辅助导航方法及装置
CN113175933A (zh) * 2021-04-28 2021-07-27 南京航空航天大学 一种基于高精度惯性预积分的因子图组合导航方法
CN113219506A (zh) * 2021-05-07 2021-08-06 江苏俱为科技有限公司 一种多模融合无缝切换的定位方法
CN113237482A (zh) * 2021-05-13 2021-08-10 东南大学 一种基于因子图的城市峡谷环境下车辆鲁棒定位方法
CN113433553A (zh) * 2021-06-23 2021-09-24 哈尔滨工程大学 一种水下机器人多源声学信息融合精确导航方法
CN113984061A (zh) * 2021-10-25 2022-01-28 哈尔滨工程大学 一种基于因子图优化的uuv多海域综合导航系统设计方法
CN114252090A (zh) * 2021-12-15 2022-03-29 哈尔滨工业大学 一种多源导航传感器可信性评估方法
CN114485723A (zh) * 2021-02-08 2022-05-13 北京理工大学 一种自适应鲁棒矩阵卡尔曼滤波的高旋体空中对准方法
CN114562992A (zh) * 2022-02-09 2022-05-31 南京航空航天大学 一种基于因子图及场景约束的多径环境组合导航方法
CN115856974A (zh) * 2022-11-18 2023-03-28 苏州华米导航科技有限公司 一种基于不变滤波的gnss、ins和视觉紧组合导航定位方法
CN115932913A (zh) * 2022-11-24 2023-04-07 国网思极位置服务有限公司 一种卫星定位伪距修正方法及装置
CN116045970A (zh) * 2023-03-06 2023-05-02 北京航空航天大学 一种基于外部条件约束的多平台信息协同导航增强方法
CN116105743A (zh) * 2023-04-17 2023-05-12 山东大学 一种联邦滤波系统信息因子分配方法及水下导航系统
CN116540286A (zh) * 2023-07-06 2023-08-04 中国科学院空天信息创新研究院 基于非完整性约束的多源鲁棒融合定位方法
WO2023198090A1 (en) * 2022-04-14 2023-10-19 The Hong Kong Polytechnic University 3d vision aided gnss real-time kinematic positioning for autonomous systems in urban canyons
CN117076824A (zh) * 2023-09-26 2023-11-17 腾讯科技（深圳）有限公司 移动终端的状态预测方法、装置、电子设备和存储介质
CN117268381A (zh) * 2023-11-17 2023-12-22 北京开运联合信息技术集团股份有限公司 一种航天器状态的判断方法
CN117451043A (zh) * 2023-12-25 2024-01-26 武汉大学 一种数-模混合估计的多源融合定位方法及系统

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102937438A (zh) * 2012-08-03 2013-02-20 南京理工大学 基于最优化方法的红外弱小目标距离探测方法
CN106197408A (zh) * 2016-06-23 2016-12-07 南京航空航天大学 一种基于因子图的多源导航信息融合方法
CN109883426A (zh) * 2019-03-08 2019-06-14 哈尔滨工程大学 基于因子图的动态分配与校正多源信息融合方法
CN110579740A (zh) * 2019-09-17 2019-12-17 大连海事大学 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法
CN111337020A (zh) * 2020-03-06 2020-06-26 兰州交通大学 引入抗差估计的因子图融合定位方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102937438A (zh) * 2012-08-03 2013-02-20 南京理工大学 基于最优化方法的红外弱小目标距离探测方法
CN106197408A (zh) * 2016-06-23 2016-12-07 南京航空航天大学 一种基于因子图的多源导航信息融合方法
CN109883426A (zh) * 2019-03-08 2019-06-14 哈尔滨工程大学 基于因子图的动态分配与校正多源信息融合方法
CN110579740A (zh) * 2019-09-17 2019-12-17 大连海事大学 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法
CN111337020A (zh) * 2020-03-06 2020-06-26 兰州交通大学 引入抗差估计的因子图融合定位方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
VADIM INDELMAN等: "Information fusion in navigation systems via factor graph based incremental smoothing" *
夏琳琳;肖建磊;徐迅;李鑫颖;: "一种基于降维对偶四元数的多源导航系统信息融合方法" *
马晓爽等: "基于因子图的AUV 多传感器组合导航算法" *
高军强等: "基于因子图的车载ＩＮＳ／ＧＮＳＳ／ＯＤ组合导航算法" *

Cited By (48)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112556690A (zh) * 2020-11-09 2021-03-26 北京全路通信信号研究设计院集团有限公司 一种多源传感器融合定位方法及装置
CN112556690B (zh) * 2020-11-09 2023-10-27 北京全路通信信号研究设计院集团有限公司 一种多源传感器融合定位方法及装置
CN112379395A (zh) * 2020-11-24 2021-02-19 中国人民解放军海军工程大学 一种定位导航授时系统
CN112379395B (zh) * 2020-11-24 2023-09-05 中国人民解放军海军工程大学 一种定位导航授时系统
CN112577496B (zh) * 2020-11-25 2024-03-26 哈尔滨工程大学 一种基于自适应选权的多源融合定位方法
CN112577496A (zh) * 2020-11-25 2021-03-30 哈尔滨工程大学 一种基于自适应选权的多源融合定位方法
CN112904388A (zh) * 2020-12-05 2021-06-04 哈尔滨工程大学 基于领航者策略的融合定位跟踪控制方法
CN112672299A (zh) * 2020-12-09 2021-04-16 电子科技大学 一种基于多源异构信息融合的传感器数据可信度评估方法
CN112672299B (zh) * 2020-12-09 2022-05-03 电子科技大学 一种基于多源异构信息融合的传感器数据可信度评估方法
CN112560974A (zh) * 2020-12-22 2021-03-26 清华大学 一种信息融合、车辆信息获取方法及装置
CN112880659A (zh) * 2021-01-09 2021-06-01 西北工业大学 一种基于信息概率的融合定位方法
CN112925000A (zh) * 2021-01-25 2021-06-08 东南大学 基于可见光通信和惯性导航的隧道环境下车辆定位方法
CN114485723A (zh) * 2021-02-08 2022-05-13 北京理工大学 一种自适应鲁棒矩阵卡尔曼滤波的高旋体空中对准方法
CN114485723B (zh) * 2021-02-08 2024-02-27 北京理工大学 一种自适应鲁棒矩阵卡尔曼滤波的高旋体空中对准方法
CN113034903A (zh) * 2021-03-05 2021-06-25 交通运输部公路科学研究所 基于多源信息融合的交通状态估计方法和装置
CN113034903B (zh) * 2021-03-05 2021-11-16 交通运输部公路科学研究所 基于多源信息融合的交通状态估计方法和装置
CN113053106A (zh) * 2021-03-05 2021-06-29 交通运输部公路科学研究所 基于多传感器信息融合的交通状态估计方法和装置
CN113063429B (zh) * 2021-03-18 2023-10-24 苏州华米导航科技有限公司 一种自适应车载组合导航定位方法
CN113063429A (zh) * 2021-03-18 2021-07-02 苏州华米导航科技有限公司 一种自适应车载组合导航定位方法
CN113076826B (zh) * 2021-03-19 2024-04-05 广州小鹏汽车科技有限公司 一种传感器的滤波方法和装置
CN113076826A (zh) * 2021-03-19 2021-07-06 广州小鹏自动驾驶科技有限公司 一种传感器的滤波方法和装置
CN113155124A (zh) * 2021-04-27 2021-07-23 涵涡智航科技(玉溪)有限公司 一种多源辅助导航方法及装置
CN113175933A (zh) * 2021-04-28 2021-07-27 南京航空航天大学 一种基于高精度惯性预积分的因子图组合导航方法
CN113175933B (zh) * 2021-04-28 2024-03-12 南京航空航天大学 一种基于高精度惯性预积分的因子图组合导航方法
CN113219506A (zh) * 2021-05-07 2021-08-06 江苏俱为科技有限公司 一种多模融合无缝切换的定位方法
CN113237482B (zh) * 2021-05-13 2022-05-13 东南大学 一种基于因子图的城市峡谷环境下车辆鲁棒定位方法
CN113237482A (zh) * 2021-05-13 2021-08-10 东南大学 一种基于因子图的城市峡谷环境下车辆鲁棒定位方法
CN113433553A (zh) * 2021-06-23 2021-09-24 哈尔滨工程大学 一种水下机器人多源声学信息融合精确导航方法
CN113984061B (zh) * 2021-10-25 2023-02-14 哈尔滨工程大学 一种基于因子图优化的uuv多海域综合导航系统设计方法
CN113984061A (zh) * 2021-10-25 2022-01-28 哈尔滨工程大学 一种基于因子图优化的uuv多海域综合导航系统设计方法
CN114252090A (zh) * 2021-12-15 2022-03-29 哈尔滨工业大学 一种多源导航传感器可信性评估方法
CN114562992A (zh) * 2022-02-09 2022-05-31 南京航空航天大学 一种基于因子图及场景约束的多径环境组合导航方法
WO2023198090A1 (en) * 2022-04-14 2023-10-19 The Hong Kong Polytechnic University 3d vision aided gnss real-time kinematic positioning for autonomous systems in urban canyons
CN115856974A (zh) * 2022-11-18 2023-03-28 苏州华米导航科技有限公司 一种基于不变滤波的gnss、ins和视觉紧组合导航定位方法
CN115856974B (zh) * 2022-11-18 2024-04-05 苏州华米导航科技有限公司 一种基于不变滤波的gnss、ins和视觉紧组合导航定位方法
CN115932913A (zh) * 2022-11-24 2023-04-07 国网思极位置服务有限公司 一种卫星定位伪距修正方法及装置
CN115932913B (zh) * 2022-11-24 2024-03-08 国网思极位置服务有限公司 一种卫星定位伪距修正方法及装置
CN116045970A (zh) * 2023-03-06 2023-05-02 北京航空航天大学 一种基于外部条件约束的多平台信息协同导航增强方法
CN116045970B (zh) * 2023-03-06 2023-06-16 北京航空航天大学 一种基于外部条件约束的多平台信息协同导航增强方法
CN116105743A (zh) * 2023-04-17 2023-05-12 山东大学 一种联邦滤波系统信息因子分配方法及水下导航系统
CN116540286B (zh) * 2023-07-06 2023-08-29 中国科学院空天信息创新研究院 基于非完整性约束的多源鲁棒融合定位方法
CN116540286A (zh) * 2023-07-06 2023-08-04 中国科学院空天信息创新研究院 基于非完整性约束的多源鲁棒融合定位方法
CN117076824A (zh) * 2023-09-26 2023-11-17 腾讯科技（深圳）有限公司 移动终端的状态预测方法、装置、电子设备和存储介质
CN117076824B (zh) * 2023-09-26 2024-01-12 腾讯科技（深圳）有限公司 移动终端的状态预测方法、装置、电子设备和存储介质
CN117268381A (zh) * 2023-11-17 2023-12-22 北京开运联合信息技术集团股份有限公司 一种航天器状态的判断方法
CN117268381B (zh) * 2023-11-17 2024-02-02 北京开运联合信息技术集团股份有限公司 一种航天器状态的判断方法
CN117451043B (zh) * 2023-12-25 2024-03-15 武汉大学 一种数-模混合估计的多源融合定位方法及系统
CN117451043A (zh) * 2023-12-25 2024-01-26 武汉大学 一种数-模混合估计的多源融合定位方法及系统

Also Published As

Publication number Publication date
CN111780755B (zh) 2023-05-05

Similar Documents

Publication Publication Date Title
CN111780755A (zh) 一种基于因子图和可观测度分析的多源融合导航方法
CN108519615B (zh) 基于组合导航和特征点匹配的移动机器人自主导航方法
CN109459040B (zh) 基于rbf神经网络辅助容积卡尔曼滤波的多auv协同定位方法
CN110823217B (zh) 一种基于自适应联邦强跟踪滤波的组合导航容错方法
CN110057354B (zh) 一种基于磁偏角修正的地磁匹配导航方法
CN111880207B (zh) 一种基于小波神经网络的视觉惯性卫星紧耦合定位方法
CN111337020A (zh) 引入抗差估计的因子图融合定位方法
CN109141413B (zh) 具有数据缺失uwb行人定位的efir滤波算法及系统
CN108983271B (zh) 基于rtk-gps/ins列车组合定位方法
CN103776453A (zh) 一种多模型水下航行器组合导航滤波方法
Xu et al. Cooperative localization in harsh underwater environment based on the MC-ANFIS
CN112697138B (zh) 一种基于因子图优化的仿生偏振同步定位与构图的方法
CN106680765A (zh) 基于分布式组合滤波ins/uwb行人导航系统及方法
CN109141412B (zh) 面向有数据缺失ins/uwb组合行人导航的ufir滤波算法及系统
CN110954132A (zh) Grnn辅助自适应卡尔曼滤波进行导航故障识别的方法
CN113175931B (zh) 基于约束卡尔曼滤波的集群组网协同导航方法及系统
Srinivas et al. Overview of architecture for GPS-INS integration
Fan et al. Cooperative positioning for multi-AUVs based on factor graph and maximum correntropy
CN111750865A (zh) 一种用于双功能深海无人潜器导航系统的自适应滤波导航方法
CN112946711A (zh) 一种gnss/ins组合导航系统的导航方法
El-Wakeel et al. Robust positioning for road information services in challenging environments
CN116086445A (zh) 一种基于因子图优化的多源信息时延融合导航方法
CN115930977A (zh) 特征退化场景的定位方法、系统、电子设备和可读存介质
CN114915913A (zh) 一种基于滑窗因子图的uwb-imu组合室内定位方法
Chen et al. An optimal selection of sensors in multi-sensor fusion navigation with factor graph

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
EE01 Entry into force of recordation of patent licensing contract

Application publication date: 20201016

Assignee: Chengdu Yichuangxin Cloud Intelligent Technology Co.,Ltd.

Assignor: NANJING University OF SCIENCE AND TECHNOLOGY

Contract record no.: X2024980000293

Denomination of invention: A multi-source fusion navigation method based on factor graph and observability analysis

Granted publication date: 20230505

License type: Common License

Record date: 20240109

EE01 Entry into force of recordation of patent licensing contract