CN116105743B - 一种联邦滤波系统信息因子分配方法及水下导航系统 - Google Patents
一种联邦滤波系统信息因子分配方法及水下导航系统 Download PDFInfo
- Publication number
- CN116105743B CN116105743B CN202310401809.4A CN202310401809A CN116105743B CN 116105743 B CN116105743 B CN 116105743B CN 202310401809 A CN202310401809 A CN 202310401809A CN 116105743 B CN116105743 B CN 116105743B
- Authority
- CN
- China
- Prior art keywords
- sub
- federal
- carrying
- steps
- method comprises
- 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
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
- G01C21/203—Specially adapted for sailing ships
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种联邦滤波系统信息因子分配方法及水下导航系统,属于水下导航定位技术领域,在惯性导航系统作为公共参考系统、以多传感器为子系统对惯性导航系统的误差进行校正的AUV导航定位中,建立联邦滤波系统模型,采用矢量形式表征信息分配系数,相比标量形式能够更准确的反映各状态量的贡献度;进而利用子系统测量值的马氏距离剔除测量野值,避免测量野值对滤波结果的影响,采用状态估计量的马氏距离构建目标函数,可以消除状态量量纲不同带来的影响,提高系统容错性能;最后基于天牛须算法优化联邦信息分配系数,具有算法简单且优化效率高的优点。
Description
技术领域
本发明属于水下导航定位技术领域,具体地说,是涉及一种联邦滤波系统信息因子分配方法及水下导航系统。
背景技术
高精度的导航定位是AUV(自主式水下航行器)实现水下自主航行的关键。惯性导航系统以其自主性、隐蔽性、连续性等特点常作为AUV的主导航系统,但其误差随时间积累。因此,要实现高精度的水下导航定位必须借助其他传感器对惯性导航系统的误差进行校正。
随着传感器技术的发展,多传感器融合导航受到越来越多的关注,联邦滤波由于其分布式融合结构而被广泛应用于多传感器融合导航中。信息分配是联邦滤波中不可或缺的一环,是决定联邦滤波精度和容错性的重要因素。
现有技术中,通常以固定平均分配、动态标量分配和动态矢量分配的方法实施联邦滤波的信息分配,其中,固定平均分配为中,各子系统在滤波过程中始终分配相同的系数,而在实际的高动态导航中,各子系统的性能是不断变化的,固定平均分配无法实时跟踪系统的性能变化;动态标量分配中,分配系数采用标量的形式,这其中包括一步预测均方误差矩阵迹法、估计均方误差矩阵迹法和上步估计均方误差矩阵范数法等,以标量形式进行系数分配是将传感器状态估计作为一个整体进行考虑,但是多传感器导航中各传感器的测量信息不同,建立的状态方程和量测方程也不同,以标量形式进行信息分配显然会产生一定误差;动态矢量分配中,采用子系统协方差矩阵的特征值的倒数作为精度指标,系统观测矩阵的奇异值作为可观性指标,取二者平均值作为信息分配系数,特征值有正有负,直接相加可能无法得到合理的归一化结果。
发明内容
本发明提出一种联邦滤波系统信息因子分配方法及水下导航系统,利用马氏距离剔除传感器测量野值,信息分配系数采用矢量形式,考虑状态量量纲的不同,以状态估计的马氏距离作为优化目标,基于天牛须算法进行优化,能够准确反映各子系统状态估计的性能,提高系统容错性能,同时避免了测量野值和状态量量纲不同产生的影响,算法简单且计算效率高。
本发明采用以下技术方案予以实现:
提出一种联邦滤波系统信息因子分配方法,包括:
(1)建立联邦滤波系统模型:
以惯性导航系统作为公共参考系统,其他n个传感器子系统与惯性导航系统分别进行卡尔曼滤波,得到n个子滤波器;将第i个子滤波器的状态估计量表示为;其中,m为第i个子滤波器状态估计量的维数;得到各个子滤波器的滤波结果经过加权融合后得到主滤波器的状态估计量/>及其协方差阵/>为:/>,/>;其中,/>为第i个子滤波器的估计误差的协方差阵,/>为第i个子滤波器的信息分配系数,且满足:/>,;/>为信息分配系数/>的对角线元素,/>为m维的单位矩阵。
(2)利用马氏距离剔除测量野值:
假设第i个子系统采集的数据集,对于t时刻子系统采集的数据/>,计算其到该数据集/>的马氏距离:/>;其中,/>为的均值,/>为/>的协方差;当/>时,对该数据进行剔除,其中G为根据子系统传感器特性设定的剔除阈值;需要说明的是,当t较小时,此时数据集Z数据量较小,此时G应设置为较大的值以确保不会产生误剔除。
(4)采用天牛须算法优化联邦滤波信息分配系数。
在本发明一些实施例中,采用天牛须算法优化联邦滤波信息分配系数,包括:
(42)搜索变量空间:随机生成搜索方向向量,并按照下式进行变量空间的搜索:;其中,/>为t时刻天牛质心的位置,即信息分配因子的计算值,/>为t时刻天牛右须的位置,/>为t时刻天牛左须的位置,/>为触角长度。
(46)重复(42)-(45),直至满足终止条件。
提出一种水下导航系统,包括惯性导航系统和多个传感器导航子系统,其中惯性导航系统作为公共参考系统,其他传感器系统作为子系统,公共参考系统分别与子系统融合实现子滤波器功能,各子滤波器的输出通过主滤波器进行融合;其应用如上所述的联邦滤波系统信息因子分配方法确定各子滤波器的矢量形式的信息分配因子。
与现有技术相比,本发明的优点和积极效果是:本发明提出的联邦滤波系统信息因子分配方法及水下导航系统,在惯性导航系统作为AUV主导航系统、以多传感器为子系统对惯性导航系统的误差进行校正的AUV导航定位中,建立联邦滤波系统模型,采用矢量形式表征信息分配系数,相比标量形式能够更准确的反映各状态量的贡献度;利用子系统测量值的马氏距离剔除测量野值,可以避免测量野值对滤波结果的影响;进而采用状态估计量的马氏距离构建目标函数,可以消除状态量量纲不同带来的影响,提高系统容错性能;最后基于天牛须算法优化联邦信息分配系数,具有算法简单且优化效率高的优点。
结合附图阅读本发明实施方式的详细描述后,本发明的其他特点和优点将变得更加清楚。
附图说明
为了更清楚地说明本发明实施例的技术方案,下面将对实施例描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1为本发明中联邦滤波系统信息分配方法的执行流程示意;
图2为本发明给出的联邦滤波系统信息分配方法实施仿真的位置曲线示意;
图3为本发明给出的联邦滤波系统信息分配方法实施仿真的姿态曲线示意;
图4为本发明给出的联邦滤波系统信息分配方法实施仿真的速度曲线示意;
图5为本发明方法与现有固定分配法得到的位置曲线对比;
图6为本发明方法与现有固定分配法得到的姿态曲线对比;
图7为本发明方法与现有固定分配法得到的速度曲线对比。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
本发明提出的联邦滤波系统信息因子分配方法,应用于自主式水下航行器的导航定位中,公共参考系统采用惯性导航系统,作为子系统的各传感器包括但不限定于DVL、USBL、深度计、磁罗盘、重力仪、磁力仪等,先利用马氏距离剔除各子系统的测量野值,进而采用矢量形式表征信息分配系数,以马氏距离为依据建立目标函数,最后采用天牛须算法对目标函数进行优化,实现对组合导航误差状态量的最优估计。
具体的,结合图1所示,本发明包括如下步骤:
S1:建立联邦滤波系统模型。
本发明模型采用两级滤波器结构,由n个子滤波器和一个主滤波器组成,利用信息守恒原理在各个子滤波器和主滤波器之间进行信息分配。
公共参考系统与各子系统分别构成n个子滤波器,用于处理不同传感器的数据,主滤波器用于对各个子滤波器的结果进行融合处理。模型的输入包括陀螺和加速度计增量、DVL、USBL等测量数据,最终通过融合滤波输出组合导航的姿态、速度、位置信息。
各个子滤波器的滤波结果经过加权融合后得到主滤波器的状态估计量及其协方差阵/>为:/>,/>;其中,/>为第i个子滤波器的估计误差的协方差阵,/>为第i个子滤波器的信息分配系数,且满足:,/>;/>为信息分配系数的对角线元素,/>为m维的单位矩阵。
S2:利用马氏距离剔除测量野值。
假设第i个子系统采集的数据集,对于t时刻子系统采集的数据/>,计算其到该数据集/>的马氏距离,定义为/>:/>;其中,/>为/>的均值,/>为/>的协方差;当/>时,对该数据进行剔除,其中G为根据子系统传感器特性设定的剔除阈值。
需要说明的是,当t较小时,此时数据集Z数据量较小,此时G应设置为较大的值以确保不会产生误剔除。
S3:采用状态估计量的马氏距离构建目标函数。
由于联邦滤波系统的状态量中包含不同传感器的估计量,而不同传感器的测量精度及测量值的量纲均是不相同的,本发明采用状态估计的马氏距离作为目标函数进行优化可以很好的消除量纲不同带来的影响,马氏距离的表达模型为:;在实际应用中,为了方便计算,取目标函数为:。
S4:采用天牛须算法优化分配系数。
具体的,包括:
(41)参数初始化。
(42)搜索变量空间。
随机生成搜索方向向量,并按照下式进行变量空间的搜索:/>;其中,/>为t时刻天牛质心的位置,即信息分配因子的计算值,/>为t时刻天牛右须的位置,为t时刻天牛左须的位置,/>为触角长度。触角长度应确保能够覆盖合适的搜索区域,从而使搜索开始时能跳出局部最优,随着搜索的进行,触角长度逐渐变小。
(43)更新状态变量。
(44)更新目标函数最优值。
(46)重复(42)-(45)直至满足终止条件。
终止条件为迭代次数≥C。
基于上述联邦滤波系统信息因子分配方法,本发明将其应用于水下导航系统中,该水下导航系统以惯性导航系统作为公共参考系统,其他传感器为子系统,参考系统分别与子系统融合实现子滤波器功能,各子滤波器的输出通过主滤波器进行融合,利用子系统测量值的马氏距离剔除测量野值,利用子滤波器的加权估计与全局最优估计之间的马氏距离最短确定各子滤波器的矢量形式的信息分配因子;其中,用矢量形式表征信息分配系数,相比标量形式能够更准确的反映各状态量的贡献度,而以马氏距离为依据建立目标函数,可以消除状态量量纲不同带来的影响,提高系统容错性能,采用天牛须算法对目标函数进行优化,具有算法简单且优化效率高的优点。
下面采用仿真实验来验证本发明提出的联邦滤波系统信息因子分配方法的有效性。
以SINS为公共参考系统,子系统选取DVL和USBL两种传感器,传感器的参数如下表一所示。
表一
仿真的位置、姿态和速度曲线如图2至图4所示;仿真数据共1000S,为了验证本发明方法的容错性,USBL数据在550S-560S之间加入0.0001度的纬度误差,信息分配系数分别采用固定平均分配法和本发明提出的方法,实验结果如图5至图7所示,可以看出,在550S时,当USBL纬度测量值出现较大误差时,本发明提出的方法相比传统固定平均分配法所产生的误差更小,同时可以更快速的消除误差带来的影响。
需要说明的是,为了对本发明方法的效果进行验证,上述仅示出了本发明方法与传统固定分配法进行比较的对比,但并不代表本发明仅相对固定分配法具有以上优势。
需要说明的是,在具体实现过程中,上述的控制部分可以通过硬件形式的处理器执行存储器中存储的软件形式的计算机执行指令实现,此处不予赘述,而上述控制所执行的动作所对应的程序均可以以软件形式存储于系统的计算机可读存储介质中,以便于处理器调用执行以上各个模块对应的操作。
上文中的计算机可读存储介质可以包括易失性存储器,例如随机存取存储器;也可以包括非易失性存储器,例如只读存储器、快闪存储器、硬盘或固态硬盘;还可以包括上述种类的存储器的组合。
Claims (2)
1.一种联邦滤波系统信息因子分配方法,其特征在于,包括:
(1)建立联邦滤波系统模型,包括:
以惯性导航系统作为公共参考系统,其他n个传感器子系统与惯性导航系统分别进行卡尔曼滤波,得到n个子滤波器;将第i个子滤波器的状态估计量表示为;其中,m为第i个子滤波器状态估计量的维数;得到各个子滤波器的滤波结果经过加权融合后得到主滤波器的状态估计量/>及其协方差阵/>为:,/>;其中,/>为第i个子滤波器的估计误差的协方差阵,/>为第i个子滤波器的信息分配系数,且满足:/>,;/>为信息分配系数/>的对角线元素,/>为m维的单位矩阵;
(2)利用马氏距离剔除测量野值,包括:
假设第i个子系统采集的数据集,对于t时刻子系统采集的数据/>,计算其到该数据集/>的马氏距离:/>;其中,/>为/>的均值,/>为/>的协方差;当/>时,对该数据进行剔除;其中G为根据子系统传感器特性设定的剔除阈值;
(3)采用状态估计量的马氏距离构建目标函数:
(4)采用天牛须算法优化联邦信息分配系数;采用天牛须算法优化联邦信息分配系数,包括:
(42)搜索变量空间:随机生成搜索方向向量,并按照下式进行变量空间的搜索:;其中,/>为t时刻天牛质心的位置,即信息分配因子的计算值,/>为t时刻天牛右须的位置,/>为t时刻天牛左须的位置,/>为触角长度;
(46)重复(42)-(45),直至满足终止条件。
2.一种水下导航系统,包括惯性导航系统和多个传感器导航子系统,其中惯性导航系统作为公共参考系统,其他传感器系统作为子系统,参考系统分别与子系统融合实现子滤波器功能,各子滤波器的输出通过主滤波器进行融合;其特征在于,其应用如权利要求1所述的联邦滤波系统信息因子分配方法确定各子滤波器的矢量形式的信息分配因子。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310401809.4A CN116105743B (zh) | 2023-04-17 | 2023-04-17 | 一种联邦滤波系统信息因子分配方法及水下导航系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310401809.4A CN116105743B (zh) | 2023-04-17 | 2023-04-17 | 一种联邦滤波系统信息因子分配方法及水下导航系统 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN116105743A CN116105743A (zh) | 2023-05-12 |
CN116105743B true CN116105743B (zh) | 2023-06-27 |
Family
ID=86254735
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202310401809.4A Active CN116105743B (zh) | 2023-04-17 | 2023-04-17 | 一种联邦滤波系统信息因子分配方法及水下导航系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN116105743B (zh) |
Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111928846A (zh) * | 2020-07-31 | 2020-11-13 | 南京理工大学 | 一种基于联邦滤波的多源融合即插即用组合导航方法 |
Family Cites Families (17)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103323007B (zh) * | 2013-06-17 | 2015-08-19 | 南京航空航天大学 | 一种基于时变量测噪声的鲁棒联邦滤波方法 |
CN106679693A (zh) * | 2016-12-14 | 2017-05-17 | 南京航空航天大学 | 一种基于故障检测的矢量信息分配自适应联邦滤波方法 |
CN109756910B (zh) * | 2019-01-02 | 2020-07-14 | 河海大学 | 基于改进天牛须搜索算法的无人机网络资源分配方法 |
KR102119254B1 (ko) * | 2019-03-14 | 2020-06-04 | 국방과학연구소 | 연합형 필터 기반 관성항법, 위성항법 및 지형대조항법을 정보 융합한 통합 항법 설계 방법 및 이 방법을 저장한 컴퓨터 판독 가능 저장 매체 |
CN110487298A (zh) * | 2019-08-15 | 2019-11-22 | Oppo(重庆)智能科技有限公司 | 一种导航误差的校正方法及装置、设备和存储介质 |
CN110823217B (zh) * | 2019-11-21 | 2023-08-22 | 山东大学 | 一种基于自适应联邦强跟踪滤波的组合导航容错方法 |
CN111189441B (zh) * | 2020-01-10 | 2023-05-12 | 山东大学 | 一种多源自适应容错联邦滤波组合导航系统及导航方法 |
CN111238467B (zh) * | 2020-02-07 | 2021-09-03 | 西北工业大学 | 一种仿生偏振光辅助的无人作战飞行器自主导航方法 |
CN111780755B (zh) * | 2020-06-30 | 2023-05-05 | 南京理工大学 | 一种基于因子图和可观测度分析的多源融合导航方法 |
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 |
CN112213754B (zh) * | 2020-09-18 | 2022-11-25 | 山东大学 | 一种利用无人机进行地面目标定位跟踪的方法 |
CN112284414B (zh) * | 2020-10-10 | 2022-06-28 | 东南大学 | 一种基于多渐消因子自适应动机座旋转调制精对准方法 |
CN112418609A (zh) * | 2020-10-30 | 2021-02-26 | 暨南大学 | 一种基于面-网格-点的大气二次污染精准溯源方法 |
CN113673863B (zh) * | 2021-08-19 | 2023-07-07 | 南京信息工程大学 | 一种基于量子天牛须算法的电力系统经济调度方法 |
CN114221350B (zh) * | 2021-12-08 | 2023-08-22 | 国网山东省电力公司电力科学研究院 | 基于bas-iga算法的分布式光伏集群划分方法和系统 |
CN114841597A (zh) * | 2022-05-20 | 2022-08-02 | 广东电网有限责任公司 | 基于多智能算法的高渗透率新能源配电网评估方法及装置 |
CN115409397A (zh) * | 2022-09-06 | 2022-11-29 | 安徽工程大学 | 一种基于改进bas算法的智能调度方法 |
-
2023
- 2023-04-17 CN CN202310401809.4A patent/CN116105743B/zh active Active
Patent Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111928846A (zh) * | 2020-07-31 | 2020-11-13 | 南京理工大学 | 一种基于联邦滤波的多源融合即插即用组合导航方法 |
Also Published As
Publication number | Publication date |
---|---|
CN116105743A (zh) | 2023-05-12 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
KR102292277B1 (ko) | 자율 주행 차량에서 3d cnn 네트워크를 사용하여 솔루션을 추론하는 lidar 위치 추정 | |
KR102335389B1 (ko) | 자율 주행 차량의 lidar 위치 추정을 위한 심층 학습 기반 특징 추출 | |
KR102350181B1 (ko) | 자율 주행 차량에서 rnn 및 lstm을 사용하여 시간적 평활화를 수행하는 lidar 위치 추정 | |
WO2018039011A1 (en) | Asychronous training of machine learning model | |
CN109902329B (zh) | 一种油藏模拟辅助历史拟合方法、系统、存储介质及设备 | |
US20180101766A1 (en) | Efficient calculations of negative curvature in a hessian free deep learning framework | |
CN101871782B (zh) | 基于set2fnn的gps/mems-ins组合导航系统定位误差预测方法 | |
CN107110650A (zh) | 在能观测性方面受约束的导航状态的估计方法 | |
RU2762143C2 (ru) | Система определения курса и углового пространственного положения, выполненная с возможностью функционирования в полярной области | |
CN116105743B (zh) | 一种联邦滤波系统信息因子分配方法及水下导航系统 | |
CN114323007A (zh) | 一种载体运动状态估计方法及装置 | |
CN115687912A (zh) | 预测对象的轨迹数据的方法和系统及训练机器学习方法来预测对象的轨迹数据的方法和系统 | |
CN115690201A (zh) | 姿势估计方法和装置 | |
CN110378352B (zh) | 复杂水下环境中高精度抗干扰二维滤波导航数据去噪方法 | |
US20220237814A1 (en) | Method and apparatus with pose prediction | |
Huang et al. | Bayesian Real-Time System Identification: From Centralized to Distributed Approach | |
KR20220059754A (ko) | 센서 퓨전에 의한 차량의 위치 추정 방법 | |
Hostettler et al. | Basics of sensor fusion | |
Lenac et al. | Revival of filtering based SLAM? Exactly sparse delayed state filter on Lie groups | |
CN112364292B (zh) | 一种基于Ransac的密集目标跟踪方法、装置、设备和介质 | |
KR102319015B1 (ko) | 정밀 점유 지도 작성을 위한 적응형 커널 추론 방법 및 장치 | |
CN113124901B (zh) | 位置校正方法及装置、电子设备及存储介质 | |
Song et al. | Kalman filter based vehicle running data estimation | |
CN117872416A (zh) | 定位校准方法、装置、计算机设备及存储介质 | |
CN116794984A (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 |