CN115014321A - 一种基于自适应鲁棒滤波的仿生偏振多源融合定向方法 - Google Patents

一种基于自适应鲁棒滤波的仿生偏振多源融合定向方法 Download PDF

Info

Publication number
CN115014321A
CN115014321A CN202210561134.5A CN202210561134A CN115014321A CN 115014321 A CN115014321 A CN 115014321A CN 202210561134 A CN202210561134 A CN 202210561134A CN 115014321 A CN115014321 A CN 115014321A
Authority
CN
China
Prior art keywords
measurement
vector
bionic
filtering
method based
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
CN202210561134.5A
Other languages
English (en)
Other versions
CN115014321B (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.)
Hangzhou Innovation Research Institute of Beihang University
Original Assignee
Hangzhou Innovation Research Institute of 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 Hangzhou Innovation Research Institute of Beihang University filed Critical Hangzhou Innovation Research Institute of Beihang University
Priority to CN202210561134.5A priority Critical patent/CN115014321B/zh
Publication of CN115014321A publication Critical patent/CN115014321A/zh
Application granted granted Critical
Publication of CN115014321B publication Critical patent/CN115014321B/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
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F2119/00—Details relating to the type or aim of the analysis or the optimisation
    • G06F2119/10—Noise analysis or noise optimisation
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Abstract

本发明涉及一种基于自适应鲁棒滤波的仿生偏振多源融合定向方法，针对含有建模不准确、参数未知、噪声统计特性未知的仿生偏振多传感器融合的姿态估计问题，首先，建立仿生多传感器组合导航系统模型和量测模型；其次，基于鲁棒代价函数设计抑制异常值的鲁棒滤波方法；再次，设计Rauch‑Tung‑Striebel平滑预处理方法；最后，设计基于变分贝叶斯的未知参数估计器，完成基于自适应鲁棒滤波的仿生偏振多源融合定向方法。本发明实现基于仿生偏振多传感器融合的低动态飞行器姿态估计，具有较快的收敛速率、较强的鲁棒性、较高的估计精度的特点，适用于需要进行多传感器融合的动态环境下飞行器姿态估计系统。

Description

一种基于自适应鲁棒滤波的仿生偏振多源融合定向方法
技术领域
本发明属于仿生组合导航领域，具体涉及一种基于自适应鲁棒滤波的仿生偏振多源融合定向方法。
背景技术
无人机作为无人系统技术重要实践平台，在空中作业、搜索和救援、包裹递送、通信等方面得到了广泛应用，无人机将在发展智慧生态系统中发挥重要的作用。导航系统好比无人机的“眼睛”，向无人机提供姿态、位置、速度。安装在无人机上的惯性测量单元一般包含陀螺仪和加速度组件。随时间推移，惯性测量单元误差随时间累积。此外，无人机飞行过程中的抖动、外界环境干扰都会引起惯性测量单元较大的测量误差。GNSS在受到遮挡或者信号干扰时会导致接收信号减弱，从而导致导航精度降低。而长天线GNSS装备也不利于无人机在复杂场景下灵活作业。因此，惯导/GPS组合导航模式显然无法达到如动物般灵活、智能的导航技巧。基于对生物导航机理的探索，已经发现使用偏振光定向可以提高载体的航向精度。偏振光定向在无人机上的应用要考虑载体倾斜、三维定姿、无GNSS时测速困难及环境干扰的问题。在基于仿生偏振的导航应用中，非线性滤波是重要的导航技术，是组合导航系统中信息融合的关键环节。针对复杂环境的应用场景，系统参数包括模型参数及噪声的统计参数具有不确定性，例如噪声统计特性服从非高斯分布。然而，传统的扩展卡尔曼滤波处理实际数据时收敛较慢甚至有发散的情况。这些问题限制了无人机的导航性能，制约了无人机向智能体的发展。因此，结合现有技术，设计基于仿生偏振多源融合的飞行器高精度姿态估计方法很重要，具有广泛的应用前景。
目前，针对仿生偏振多源融合定向的研究主要集中在信息融合层面，对仿生组合导航受到的多源干扰问题研究较少，文献《一种偏振光/双目视觉仿生组合导航方法》，设计了图优化紧耦合的组合导航算法，实现了偏振光/双目视觉传感器的数据融合，提高了组合导航系统的航向精度。中国发明专利CN201911252069.2中提出了一种基于偏振/天文信息融合的航向与姿态确定方法，针对天文导航在大气层内工作时受天气的影响，利用仿复眼偏振传感器获得的信息解算单位太阳矢量，结合天文敏感器获取的信息解算姿态信息。中国发明专利CN201810129372.2中提出了一种基于扩展卡尔曼滤波的仿生偏振传感器多源误差标定方法，该方法对多源误差进行了分析，估计了安装误差、尺度因子和偏振方位角，但未考虑外部干扰的影响，尤其是动态环境下的多源干扰。中国发明专利CN202111390144.9提出了一种基于可观测度的惯性/偏振/地磁容错导航方法，针对组合导航系统中传感器数据不可靠的问题，设计了基于可观测度的信息分配因子，提高了导航系统的精度和鲁棒性。但是扩展卡尔曼滤波存在不一致性问题，因而可观测度分析也有其局限性。
综上所述，现有方法在多源干扰导致的建模不准确、参数未知、噪声统计特性未知的情形下，缺乏抑制多源干扰的高精度姿态估计方法，亟需攻克基于鲁棒滤波技术的多传感器融合姿态估计方法。
发明内容
针对含有建模不准确、参数未知、噪声统计特性未知的仿生偏振多传感器融合的姿态估计问题，为克服现有技术的不足，本发明提供一种基于自适应鲁棒滤波的仿生偏振多源融合定向方法，实现了对多源干扰的抑制以及对未知统计特性的噪声的近似推断与估计，采用一套基于鲁棒代价函数和自适应因子的滤波算法抑制干扰的影响，同时采用基于变分贝叶斯的推断方法估计噪声的统计特性，提高基于多传感器融合的飞行器导航定向的精度、鲁棒性以及抗干扰能力。
为达到上述目的，本发明的技术解决方案为：
一种基于自适应鲁棒滤波的仿生偏振多源融合定向方法，包括以下步骤：
第一步，建立仿生多传感器组合导航系统模型和量测模型；系统状态量包含姿态误差角、速度误差矢量、位置误差矢量、陀螺零偏和加速度零偏，量测模型是GNSS与偏振传感器测量模型；
第二步，针对第一步中的模型，进行时间更新，并基于鲁棒代价函数设计抑制异常值的鲁棒滤波方法；
第三步，在第二步基础上进行RTS平滑预处理；
第四步，在第三步基础上设计基于变分贝叶斯的未知参数估计器，完成基于自适应鲁棒滤波的仿生偏振多源融合定向方法。
进一步地，所述第一步中，
系统模型为：
Figure BDA0003656630260000021
Figure BDA0003656630260000022
量测模型为：
Figure BDA0003656630260000031
Figure BDA0003656630260000032
其中，x表示n维的状态矢量，φ,υ,p,ε,
Figure BDA00036566302600000319
分别表示姿态误差角、速度矢量、位置矢量、陀螺漂移和加速度零偏，δυ与δp分别表示速度误差与位置误差，
Figure BDA0003656630260000033
为状态矢量的导数，f(x)为状态方程函数，w为过程噪声；zp,zg分别表示偏振传感器矢量和GNSS量测矢量，sn为地理系下的太阳矢量，
Figure BDA0003656630260000034
为从载体系到地理系的姿态转换的计算值，
Figure BDA0003656630260000035
为实际测得的偏振矢量，
Figure BDA0003656630260000036
和
Figure BDA0003656630260000037
分别为惯导和GNSS测得的导航系下的速度，
Figure BDA0003656630260000038
和
Figure BDA0003656630260000039
分别为惯导和GNSS测得的导航系下的位置，hp(φ)和hg(x)分别为偏振传感器函数和GNSS量测函数，vp和vg分别为偏振传感器噪声和GNSS量测噪声。
进一步地，所述第二步中，根据强跟踪滤波理论，预测误差协方差Pk|k-1校正，具体步骤如下：
Figure BDA00036566302600000310
其中，
Figure BDA00036566302600000311
Λk,Qk-1分别表示校正后的预测误差协方差、校正对角阵和过程噪声协方差，基于正交原理引入的对角阵计算如下：
Figure BDA00036566302600000312
其中，
Figure BDA00036566302600000313
Hk为量测函数的雅可比矩阵，
Figure BDA00036566302600000314
表示求广义逆，Rk为量测噪声协方差，
Figure BDA00036566302600000315
Figure BDA00036566302600000316
η,b均为柔化因子，Gk(j,j)代表取矩阵Gk的第j个对角元素，Jk(j,j)代表矩阵Jk的第j个对角元素，k表示时刻，zk代表k时刻的量测，
Figure BDA00036566302600000317
代表量测预测值；
量测噪声协方差校正为
Figure BDA00036566302600000318
其中ψR,k＝diag[ρ′(ei,k)/ei,k]，ρ表示损失函数，ei,k为误差k时刻的第i个分量；
ρ的具体形式如下：
Figure BDA0003656630260000041
其中，σ为核的大小，
Figure BDA0003656630260000042
ei,k表示第k时刻e的第i个分量；
将以上计算的
Figure BDA0003656630260000043
与
Figure BDA0003656630260000044
代入扩展卡尔曼滤波的量测更新过程，滤波增益Kk与状态估计值
Figure BDA0003656630260000045
计算如下：
Figure BDA0003656630260000046
Figure BDA0003656630260000047
在滤波过程中对量测更新进行迭代，每次迭代中
Figure BDA0003656630260000048
与
Figure BDA0003656630260000049
会根据上面的校正方程进行修正，迭代终止的条件为
Figure BDA00036566302600000410
进一步地，所述第三步中，在第二步基础上进行RTS平滑预处理：
Figure BDA00036566302600000411
Figure BDA00036566302600000412
Figure BDA00036566302600000413
其中，Fk-1表示状态函数f(xk)在k-1时刻的雅可比矩阵。
进一步地，所述第四步，设计基于变分贝叶斯的未知参数估计方法，具体包括：
Figure BDA00036566302600000414
Figure BDA00036566302600000415
Figure BDA00036566302600000416
其中，
Figure BDA00036566302600000417
Figure BDA00036566302600000418
Figure BDA00036566302600000419
Figure BDA00036566302600000420
Figure BDA00036566302600000421
Figure BDA0003656630260000051
Figure BDA0003656630260000052
Figure BDA0003656630260000053
Figure BDA0003656630260000054
Figure BDA0003656630260000055
Figure BDA0003656630260000056
其中，n与m分别是状态矢量与量测矢量的维数；
Figure BDA0003656630260000057
ξ1，ξ2，
Figure BDA0003656630260000058
为缩放调整参数；
Figure BDA0003656630260000059
的初值为
Figure BDA00036566302600000510
参数右上标i表示迭代数；
Figure BDA00036566302600000511
属于递推参数，满足上述递推关系，它们的初值也就是k＝0时刻的值则预先设定；
Figure BDA00036566302600000512
为量测值，与具体的传感器相对应；上述
Figure BDA00036566302600000513
描述的分别是k时刻的滤波增益、估计的状态量、误差协方差，(i+1)表示迭代次数；
在第四步的具体滤波过程设计完成后，进行迭代处理，设置迭代次数N次，输出
Figure BDA00036566302600000514
Figure BDA00036566302600000515
本发明与现有技术相比的优点在于：
本发明针对现有方法在含有建模不准确、参数未知、噪声统计特性未知的情况下，缺乏抑制多源干扰的高精度姿态估计能力的缺点，设计了基于鲁棒代价函数抑制异常值的鲁棒滤波方法，在滤波进程中使下一步骤的平滑预处理的估计精度更高，鲁棒滤波对异常值的有效处理使得后续近似推断噪声分布时受模型误差影响程度被尽可能减小，在此基础上进行平滑预处理，得到更高精度的滤波参数估计，提升未知参数估计的效率，并设计了基于变分贝叶斯的未知参数估计方法，构建了一套完整的多源融合估计方法，鲁棒滤波、平滑滤波、近似推断在每时刻滤波进程中依次减小各种误差对后面滤波步骤的影响，在完整的运动的轨迹中，提升后一时刻的预测精度，实现了对多源干扰的抑制以及对未知统计特性的噪声的准确估计，具有导航定向精度高、鲁棒性强以及强抗干扰能力的特点，适用于仿生偏振多传感器融合的飞行器姿态估计系统中。
附图说明
图1为本发明一种基于自适应鲁棒滤波的仿生偏振多源融合定向方法的设计流程图；
图2为本发明一种基于自适应鲁棒滤波的仿生偏振多源融合定向方法的滤波结构图。
具体实施方式
下面结合附图及实施例对本发明进行详细说明。
如图1所示，本发明涉及一种基于自适应鲁棒滤波的仿生偏振多源融合定向方法。第一步，建立仿生偏振多传感器组合导航系统模型和量测模型，其中系统状态量包含姿态误差角、速度误差矢量、位置误差矢量、陀螺零偏和加速度零偏，量测模型是GNSS(全球卫星导航系统)与偏振传感器测量模型；第二步，在第一步的基础上，基于鲁棒代价函数设计抑制异常值的鲁棒滤波算法；第三步，在第二步基础上进行RTS(Rauch-Tung-Striebel)平滑预处理；第四步，在第三步基础上设计基于变分贝叶斯的未知参数估计器，完成基于自适应鲁棒滤波的仿生偏振多源融合定向方法。本发明采用基于鲁棒代价函数和自适应因子的滤波算法抑制干扰的影响，同时采用基于变分贝叶斯的推断方法估计噪声的统计特性，能够在存在多源干扰的情形下实现仿生偏振多传感器组合导航系统姿态估计，具有高精度、强鲁棒的特点，适用于仿生偏振多传感器融合的飞行器姿态估计系统中。
具体实施步骤如下：
第一步，建立仿生偏振多传感器组合导航系统模型和量测模型，其中系统状态量包含姿态误差角、速度误差矢量、位置误差矢量、陀螺零偏和加速度零偏，量测模型是GNSS与偏振传感器测量模型。
系统模型为：
Figure BDA0003656630260000061
Figure BDA0003656630260000062
量测模型为：
Figure BDA0003656630260000063
Figure BDA0003656630260000064
其中，x表示n维的状态矢量，初始值为15×1的零矩阵，φ,υ,p,ε,
Figure BDA0003656630260000065
分别表示姿态误差角、速度矢量、位置矢量、陀螺漂移和加速度零偏，δυ与δp分别表示速度误差与位置误差，
Figure BDA0003656630260000066
为状态矢量的导数，f(x)为状态方程函数，w为过程噪声。zp,zg分别表示偏振传感器矢量和GNSS量测矢量，sn为地理系下的太阳矢量，
Figure BDA0003656630260000067
为从载体系到地理系的姿态转换的计算值，
Figure BDA0003656630260000068
为实际测得的偏振矢量，
Figure BDA0003656630260000069
和
Figure BDA00036566302600000610
分别为惯导和GNSS测得的导航系下的速度，
Figure BDA00036566302600000715
和
Figure BDA0003656630260000071
分别为惯导和GNSS测得的导航系下的位置，hp(φ)和hg(x)分别为偏振传感器和GNSS量测函数，vp和vg分别为偏振传感器噪声和GNSS量测噪声。
第二步，针对第一步中的模型，进行时间更新，并基于鲁棒代价函数设计抑制异常值的鲁棒滤波方法。
时间更新：
Figure BDA0003656630260000072
Figure BDA0003656630260000073
其中，Pk|k-1,Fk|k-1,Qk-1分别表示k-1时刻的预测误差协方差、状态方程的雅可比矩阵和过程噪声协方差。
鲁棒滤波方法：
根据强跟踪滤波理论，预测误差协方差可以校正如下：
Figure BDA0003656630260000074
其中，
Figure BDA0003656630260000075
Λk,Qk-1分别表示校正后的预测误差协方差、校正对角阵和过程噪声协方差，基于正交原理引入的对角阵计算如下：
Figure BDA0003656630260000076
其中，Qk-1为过程噪声协方差，
Figure BDA0003656630260000077
Hk为量测函数的雅可比矩阵，
Figure BDA00036566302600000716
表示求广义逆，Rk为量测噪声协方差，
Figure BDA0003656630260000078
Figure BDA0003656630260000079
η＝0.01,b＝0.95均为柔化因子，Gk(j,j)代表取矩阵Gk的第j个对角元素，Jk(j,j)代表取矩阵Jk的第j个对角元素，k表示时刻，zk代表k时刻的量测，
Figure BDA00036566302600000710
代表量测预测值，zk代表k时刻的量测，
Figure BDA00036566302600000711
代表量测预测值。
如图2所示，根据具体的量测传感器响应情况进行测量更新，量测噪声协方差校正为
Figure BDA00036566302600000712
其中ψR,k＝diag[ρ′(ei,k)/ei,k]，ρ表示损失函数，ei,k为误差k时刻的第i个分量。ρ的具体形式如下：
Figure BDA00036566302600000713
其中，σ＝0.85为核的大小，
Figure BDA00036566302600000714
ei,k表示第k时刻e的第i个分量。
将以上计算的
Figure BDA0003656630260000081
与
Figure BDA0003656630260000082
代入扩展卡尔曼滤波的量测更新过程，滤波增益Kk与状态估计值
Figure BDA0003656630260000083
计算如下：
Figure BDA0003656630260000084
Figure BDA0003656630260000085
在滤波过程中对量测更新进行迭代，每次迭代中
Figure BDA0003656630260000086
与
Figure BDA0003656630260000087
会根据上面的校正方程进行修正，迭代终止的条件为
Figure BDA0003656630260000088
第三步，在第二步基础上进行RTS平滑预处理：
Figure BDA0003656630260000089
Figure BDA00036566302600000810
Figure BDA00036566302600000811
其中，Fk-1表示状态函数f(xk)在k-1时刻的雅可比矩阵。
第四步，在第三步基础上设计基于变分贝叶斯的未知参数估计方法：
Figure BDA00036566302600000812
Figure BDA00036566302600000813
Figure BDA00036566302600000814
其中，
Figure BDA00036566302600000815
Figure BDA00036566302600000816
Figure BDA00036566302600000817
Figure BDA00036566302600000818
Figure BDA00036566302600000819
Figure BDA00036566302600000820
Figure BDA00036566302600000821
Figure BDA00036566302600000822
Figure BDA00036566302600000823
Figure BDA0003656630260000091
Figure BDA0003656630260000092
其中，n与m分别是状态矢量与量测矢量的维数；
Figure BDA0003656630260000093
ξ1，ξ2，
Figure BDA0003656630260000094
为缩放调整参数；
Figure BDA0003656630260000095
的初值为
Figure BDA0003656630260000096
参数右上标i表示迭代数；
Figure BDA0003656630260000097
属于递推参数，满足上述递推关系，它们的初值也就是k＝0时刻的值则预先设定；
Figure BDA0003656630260000098
为量测值，与具体的传感器相对应；上述
Figure BDA0003656630260000099
描述的分别是k时刻的滤波增益、估计的状态量、误差协方差，(i+1)表示迭代次数。另外第四步实施在偏振传感器时的参数具体为ξ1＝0.75,ξ2＝0.6,
Figure BDA00036566302600000910
第四步实施在GNSS上时的参数取值为ξ1＝0.5,ξ2＝diag{1-exp(-2.6),1-exp(-2.6),1-exp(-3),1-exp(-2.6),1-exp(-2.6),1-exp(-2.6)}，
Figure BDA00036566302600000911
在第四步的具体滤波过程设计完成后，进行迭代处理，设置迭代次数N＝5次，输出
Figure BDA00036566302600000912
Figure BDA00036566302600000913
采用本发明方法进行仿生偏振多传感器融合的飞行器姿态估计，可在低动态飞行状态下使用一套自适应鲁棒滤波算法，能够抑制多源干扰的影响，保证滤波快速收敛，航向估计的均方根误差能在0.3度左右，达到了高精度与强鲁棒性效果。
本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (5)

1.一种基于自适应鲁棒滤波的仿生偏振多源融合定向方法，其特征在于，包括以下步骤：
第一步，建立仿生多传感器组合导航系统模型和量测模型；系统状态量包含姿态误差角、速度误差矢量、位置误差矢量、陀螺零偏和加速度零偏，量测模型是GNSS与偏振传感器测量模型；
第二步，针对第一步中的模型，进行时间更新，并基于鲁棒代价函数设计抑制异常值的鲁棒滤波方法；
第三步，在第二步基础上进行RTS平滑预处理；
第四步，在第三步基础上设计基于变分贝叶斯的未知参数估计器，完成基于自适应鲁棒滤波的仿生偏振多源融合定向方法。
2.根据权利要求1所述的一种基于自适应鲁棒滤波的仿生偏振多源融合定向方法，其特征在于：所述第一步中，
系统模型为：
Figure FDA00036566302500000111
Figure FDA0003656630250000011
量测模型为：
Figure FDA0003656630250000012
Figure FDA0003656630250000013
其中，x表示n维的状态矢量，
Figure FDA0003656630250000014
分别表示姿态误差角、速度矢量、位置矢量、陀螺漂移和加速度零偏，δυ与δp分别表示速度误差与位置误差，
Figure FDA00036566302500000112
为状态矢量的导数，f(x)为状态方程函数，w为过程噪声；zp,zg分别表示偏振传感器矢量和GNSS量测矢量，sn为地理系下的太阳矢量，
Figure FDA0003656630250000015
为从载体系到地理系的姿态转换的计算值，
Figure FDA0003656630250000016
为实际测得的偏振矢量，
Figure FDA0003656630250000017
和
Figure FDA0003656630250000018
分别为惯导和GNSS测得的导航系下的速度，
Figure FDA0003656630250000019
和
Figure FDA00036566302500000110
分别为惯导和GNSS测得的导航系下的位置，hp(φ)和hg(x)分别为偏振传感器函数和GNSS量测函数，vp和vg分别为偏振传感器噪声和GNSS量测噪声。
3.根据权利要求2所述的一种基于自适应鲁棒滤波的仿生偏振多源融合定向方法，其特征在于：所述第二步中，根据强跟踪滤波理论，预测误差协方差Pk|k-1校正，具体步骤如下：
Figure FDA0003656630250000021
其中，
Figure FDA0003656630250000022
分别表示校正后的预测误差协方差、校正对角阵和过程噪声协方差，基于正交原理引入的对角阵计算如下：
Figure FDA0003656630250000023
其中，
Figure FDA0003656630250000024
Hk为量测函数的雅可比矩阵，
Figure FDA0003656630250000025
表示求广义逆，Rk为量测噪声协方差，
Figure FDA0003656630250000026
Figure FDA0003656630250000027
η,b均为柔化因子，Gk(j,j)代表取矩阵Gk的第j个对角元素，Jk(j,j)代表矩阵Jk的第j个对角元素，k表示时刻，zk代表k时刻的量测，
Figure FDA0003656630250000028
代表量测预测值；
量测噪声协方差校正为
Figure FDA0003656630250000029
其中ψR,k＝diag[ρ′(ei,k)/ei,k]，ρ表示损失函数，ei,k为误差k时刻的第i个分量；
ρ的具体形式如下：
Figure FDA00036566302500000210
其中，σ为核的大小，
Figure FDA00036566302500000211
ei,k表示第k时刻e的第i个分量；
将以上计算的
Figure FDA00036566302500000212
与
Figure FDA00036566302500000213
代入扩展卡尔曼滤波的量测更新过程，滤波增益Kk与状态估计值
Figure FDA00036566302500000214
计算如下：
Figure FDA00036566302500000215
Figure FDA00036566302500000216
在滤波过程中对量测更新进行迭代，每次迭代中
Figure FDA00036566302500000217
与
Figure FDA00036566302500000218
会根据上面的校正方程进行修正，迭代终止的条件为
Figure FDA00036566302500000219
4.根据权利要求3所述的一种基于自适应鲁棒滤波的仿生偏振多源融合定向方法，其特征在于：所述第三步中，在第二步基础上进行RTS平滑预处理：
Figure FDA0003656630250000031
Figure FDA0003656630250000032
Figure FDA0003656630250000033
其中，Fk-1表示状态函数f(xk)在k-1时刻的雅可比矩阵。
5.根据权利要求4所述的一种基于自适应鲁棒滤波的仿生偏振多源融合定向方法，其特征在于：所述第四步，设计基于变分贝叶斯的未知参数估计方法，具体包括：
Figure FDA0003656630250000034
Figure FDA0003656630250000035
Figure FDA0003656630250000036
其中，
Figure FDA0003656630250000037
Figure FDA0003656630250000038
Figure FDA0003656630250000039
Figure FDA00036566302500000310
Figure FDA00036566302500000311
Figure FDA00036566302500000312
Figure FDA00036566302500000313
Figure FDA00036566302500000314
Figure FDA00036566302500000315
Figure FDA00036566302500000316
Figure FDA00036566302500000317
其中，n与m分别是状态矢量与量测矢量的维数；
Figure FDA00036566302500000318
ξ1，ξ2，
Figure FDA00036566302500000319
为缩放调整参数；
Figure FDA00036566302500000320
的初值为
Figure FDA00036566302500000321
参数右上标i表示迭代数；
Figure FDA00036566302500000322
属于递推参数，满足上述递推关系，它们的初值也就是k＝0时刻的值则预先设定；
Figure FDA00036566302500000323
为量测值，与具体的传感器相对应；上述
Figure FDA00036566302500000324
描述的分别是k时刻的滤波增益、估计的状态量、误差协方差，(i+1)表示迭代次数；
在第四步的具体滤波过程设计完成后，进行迭代处理，设置迭代次数N次，输出
Figure FDA0003656630250000041
Figure FDA0003656630250000042
CN202210561134.5A 2022-05-23 2022-05-23 一种基于自适应鲁棒滤波的仿生偏振多源融合定向方法 Active CN115014321B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210561134.5A CN115014321B (zh) 2022-05-23 2022-05-23 一种基于自适应鲁棒滤波的仿生偏振多源融合定向方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210561134.5A CN115014321B (zh) 2022-05-23 2022-05-23 一种基于自适应鲁棒滤波的仿生偏振多源融合定向方法

Publications (2)

Publication Number Publication Date
CN115014321A true CN115014321A (zh) 2022-09-06
CN115014321B CN115014321B (zh) 2024-03-19

Family

ID=83069294

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210561134.5A Active CN115014321B (zh) 2022-05-23 2022-05-23 一种基于自适应鲁棒滤波的仿生偏振多源融合定向方法

Country Status (1)

Country Link
CN (1) CN115014321B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116222582A (zh) * 2023-05-10 2023-06-06 北京航空航天大学 一种基于变分贝叶斯推断多物理场自适应组合导航方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106339608A (zh) * 2016-11-09 2017-01-18 中国科学院软件研究所 一种基于在线变分贝叶斯支持向量回归的交通事故率预测系统
US20180293711A1 (en) * 2017-04-06 2018-10-11 Disney Enterprises, Inc. Kernel-predicting convolutional neural networks for denoising
CN109556633A (zh) * 2018-11-26 2019-04-02 北方工业大学 一种基于自适应ekf的仿生偏振传感器多源误差标定方法
CN113834483A (zh) * 2021-11-23 2021-12-24 北京航空航天大学 一种基于可观测度的惯性/偏振/地磁容错导航方法
CN114018258A (zh) * 2021-11-05 2022-02-08 北京航空航天大学杭州创新研究院 基于偏振量测噪声方差自适应估计的仿生组合导航方法
CN114445459A (zh) * 2022-01-16 2022-05-06 中国人民解放军空军工程大学 基于变分贝叶斯理论的连续-离散最大相关熵目标跟踪方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106339608A (zh) * 2016-11-09 2017-01-18 中国科学院软件研究所 一种基于在线变分贝叶斯支持向量回归的交通事故率预测系统
US20180293711A1 (en) * 2017-04-06 2018-10-11 Disney Enterprises, Inc. Kernel-predicting convolutional neural networks for denoising
CN109556633A (zh) * 2018-11-26 2019-04-02 北方工业大学 一种基于自适应ekf的仿生偏振传感器多源误差标定方法
CN114018258A (zh) * 2021-11-05 2022-02-08 北京航空航天大学杭州创新研究院 基于偏振量测噪声方差自适应估计的仿生组合导航方法
CN113834483A (zh) * 2021-11-23 2021-12-24 北京航空航天大学 一种基于可观测度的惯性/偏振/地磁容错导航方法
CN114445459A (zh) * 2022-01-16 2022-05-06 中国人民解放军空军工程大学 基于变分贝叶斯理论的连续-离散最大相关熵目标跟踪方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
ZHENGBING QIU ET AL: "Improved Cubature Kalman Filter for Spacecraft Attitude Estimation", IEEE TRANSACTIONS ON INSTRUMENTATION AND MEASUREMENT, vol. 70, 31 December 2021 (2021-12-31) *
左云龙等: "基于变分贝叶斯的自适应鲁棒滤波算法", 《导航定位于授时》, vol. 4, no. 5, 30 September 2017 (2017-09-30), pages 48 - 53 *
王融;熊智;刘建业;钟丽娜;: "高超声速飞行器组合导航鲁棒滤波算法", 航空计算技术, no. 05, 25 September 2013 (2013-09-25), pages 11 - 14 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116222582A (zh) * 2023-05-10 2023-06-06 北京航空航天大学 一种基于变分贝叶斯推断多物理场自适应组合导航方法

Also Published As

Publication number Publication date
CN115014321B (zh) 2024-03-19

Similar Documents

Publication Publication Date Title
CN110487301B (zh) 一种雷达辅助机载捷联惯性导航系统初始对准方法
CN108226980B (zh) 基于惯性测量单元的差分gnss与ins自适应紧耦合导航方法
CN111156987B (zh) 基于残差补偿多速率ckf的惯性/天文组合导航方法
CN110823217B (zh) 一种基于自适应联邦强跟踪滤波的组合导航容错方法
CN111238467B (zh) 一种仿生偏振光辅助的无人作战飞行器自主导航方法
CN111024064B (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN109883426B (zh) 基于因子图的动态分配与校正多源信息融合方法
CN110779521A (zh) 一种多源融合的高精度定位方法与装置
CN112505737B (zh) 一种gnss/ins组合导航方法
CN110702143B (zh) 基于李群描述的sins捷联惯性导航系统动基座快速初始对准方法
CN112697138B (zh) 一种基于因子图优化的仿生偏振同步定位与构图的方法
CN109506660B (zh) 一种用于仿生导航的姿态最优化解算方法
CN108387236B (zh) 一种基于扩展卡尔曼滤波的偏振光slam方法
CN113203418B (zh) 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统
CN110849360B (zh) 面向多机协同编队飞行的分布式相对导航方法
CN116045970B (zh) 一种基于外部条件约束的多平台信息协同导航增强方法
CA2699137A1 (en) Hybrid inertial system with non-linear behaviour and associated method of hybridization by multi-hypothesis filtering
CN116086445A (zh) 一种基于因子图优化的多源信息时延融合导航方法
CN111207773B (zh) 一种用于仿生偏振光导航的姿态无约束优化求解方法
CN113252038A (zh) 基于粒子群算法的航迹规划地形辅助导航方法
CN110395297B (zh) 列车定位方法
CN115014321A (zh) 一种基于自适应鲁棒滤波的仿生偏振多源融合定向方法
CN113008229B (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN114018254A (zh) 一种激光雷达与旋转惯导一体化构架与信息融合的slam方法
CN111220151B (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