CN106441300A - 一种具有自适应的协同导航滤波方法 - Google Patents
一种具有自适应的协同导航滤波方法 Download PDFInfo
- Publication number
- CN106441300A CN106441300A CN201610810954.8A CN201610810954A CN106441300A CN 106441300 A CN106441300 A CN 106441300A CN 201610810954 A CN201610810954 A CN 201610810954A CN 106441300 A CN106441300 A CN 106441300A
- Authority
- CN
- China
- Prior art keywords
- model
- matrix
- filtering
- wave filter
- state
- 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
Links
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
Abstract
本发明涉及协同导航算法领域,具体涉及一种针对未知量测噪声下的具有自适应的协同导航滤波方法。本发明包括:输入交互过程;模型滤波;模型概率更新;交互输出。相比于传统的协同导航滤波方法,在噪声未知的环境下条件下,本方能有效减小因为噪声预设偏差过大导致的精度下降问题。本方法具有很好的模块化特性,所以我们不仅可以增加模型数量,而且还可以自由的选取具有各种特性的滤波器,因此其具有很好的后期修正性。
Description
技术领域
本发明涉及协同导航算法领域,具体涉及一种针对未知量测噪声下的具有自适应的协同导航滤波方法。
背景技术
自主水下航行器(Autonomous Underwater Vehicles,AUV)在民用还是军事方面都有广泛的应用。随着对海洋的探索越来越深入,使得AUV的使命也变得越来越复杂化和多样化,如果仅仅依靠单个AUV去实现这些功能,不仅在成本上会提高很多,而且可靠性也会降低,而由多台水下AUV组成的协作系统具有空间分布、功能分布、冗余等特点。然而不论单AUV还是多AUV系统以及其执行何种任务,导航系统的优劣直接影响系统的性能好坏。协同导航定位技术作为一种新的导航定位方法具有只对有限数目的AUV增加设备费从而来提高编队的导航精度等优点。由于水下环境和基于声学定位的特殊性,协同导航滤波过程中所需要量测噪声协方差阵具有时变性,传统算法中是通过提前设置预值来进行定位解算的,并未考虑到其它可能的变化,限制了AUV的适用范围。本方法提出通过提前预设多组不同的量测噪声协方差矩阵来提高协同导航滤波的自适应性,扩大AUV的适用范围。
发明内容
本发明的目的在于提出一种在未知量测噪声环境下时对协同编队进行定位能够抑制噪声未知下导致的从AUV定位精度下降问题的具有自适应的协同导航滤波方法。
本发明的目的是这样实现的
第一步:输入交互过程
如图1所示,交互式多模型算法是一个循环算法,通过输入交互过程、模型滤波、模型概率更新以及输出交互过程四个过程完成状态估计。
在输入交互过程中,首次通过预验模型概率获取混合状态,下次循环则通过计算得到的模型概率替换。
协同导航从AUV交互混合状态估计及协方差为:
式中为k-1时刻第j各滤波器的状态估计;Pj(k-1)为对应的协方差矩阵;P0i(k-1)为状态估计所对应的状态误差协方差矩阵。
首次滤波中,用初始模型预测概率代替模型条件转移概率进行运算,下个滤波周期中通过利用上一个周期各个滤波器的状态估计和模型条件转移概率进行运算。
第二步:模型滤波
基于EKF的协同导航滤波器1工作如下:
系统状态一步预测:其中表示从AUV在时刻k+1的一步估计状态,f为从AUV运动学方程,为从AUV交互混合估计状态1,uk量测输入。
一步预测协方差矩阵为:
其中Fk为状态转移矩阵,Pk为上一时刻的的协方差矩阵,Gk为过程噪声分布矩阵,Qk表示噪声协方差矩阵。
量测量预测为:
其中h表示量测方程,分别为领航者AUV1,AUV2的状态。
残差信息和其协方差:
其中Zk+1为传感器接收到的量测值,Hk+1为量测矩阵的雅可比矩阵,为滤波器1中的量测噪声协方差矩阵。
滤波增益:
状态估计及误差协方差矩阵:
P1,k+1=(I-Kk+1Hk+1)Pk+1,k,其中I为单位矩阵。
基于EKF的协同导航滤波器2中主要不同于滤波器1的是量测噪声协方差矩阵选取了另外一组其他步骤相同,并得到滤波器2下残差信息v2,k+1和其协方差S2,k+1,状态估计及其误差协方差矩阵P2,k+1。
第三步:模型概率更新
做为本算法的重要部分,通过获取各个滤波器的残差信息并且结合贝叶斯假设检验方法,得到各模型的似然函数,求法如下:
m=3表示协同导航中量测向量的维数;vi(k)为协同导航模型在k时刻时通过卡尔曼滤波获得的残差向量;Si(k)为对应残差协方差矩阵。
结合上一时刻的模型概率以及模型先验信息,得到k时刻的模型概率为
第四步:交互输出
循环算法中最后一步,由前面各个基于不同模型滤波器的状态估计值及其模型概率加权融合得到,结合各模型的从AUV联合状态估计和协方差矩阵P(k)计算如下:
至此完成一次输出,进入下一时刻的状态估计。
本发明方法具有以下优点:
相比于传统的协同导航滤波方法,在噪声未知的环境下条件下,本方能有效减小因为噪声预设偏差过大导致的精度下降问题。本方法具有很好的模块化特性,所以我们不仅可以增加模型数量,而且还可以自由的选取具有各种特性的滤波器,因此其具有很好的后期修正性。
附图说明
图1为本发明示意图。
具体实施方式
下面结合附图对本发明做进一步描述。
本发明提供一种关于协同导航自适应滤波方法。该方法通过事先构建多组关于不同量测噪声的状态模型,并通过交互式多模型算法将各组先验模型整合起来,达到自适应滤波的目的。该方法的特点在于在未知环境下通过应用交互式多模型算法能够有效抑制实际噪声与预测噪声不匹配时造成的导航精度过低的问题,同时该方法具有模块化特点,能够进一步针对具体噪声特点进行滤波算法的优化,适合应用于协同导航中。
实施例:
具体操作如下所述:
下面结合图1及实施例对本发明的内容做进一步详细的阐述。
第一步:输入交互过程
如图1所示,交互式多模型算法是一个循环算法,通过输入交互过程、模型滤波、模型概率更新以及输出交互过程四个过程完成状态估计。
在输入交互过程中,首次通过预验模型概率获取混合状态,下次循环则通过计算得到的模型概率替换。
协同导航从AUV交互混合状态估计及协方差为:
式中为k-1时刻第j各滤波器的状态估计;Pj(k-1)为对应的协方差矩阵;P0i(k-1)为状态估计所对应的状态误差协方差矩阵。
首次滤波中,用初始模型预测概率代替模型条件转移概率进行运算,下个滤波周期中通过利用上一个周期各个滤波器的状态估计和模型条件转移概率进行运算。
第二步:模型滤波
基于EKF的协同导航滤波器1工作如下:
系统状态一步预测:其中表示从AUV在时刻k+1的一步估计状态,f为从AUV运动学方程,为从AUV交互混合估计状态1,uk量测输入。
一步预测协方差矩阵为:
其中Fk为状态转移矩阵,Pk为上一时刻的的协方差矩阵,Gk为过程噪声分布矩阵,Qk表示噪声协方差矩阵。
量测量预测为:
其中h表示量测方程,分别为领航者AUV1,AUV2的状态。
残差信息和其协方差:
其中Zk+1为传感器接收到的量测值,Hk+1为量测矩阵的雅可比矩阵,为滤波器1中的量测噪声协方差矩阵。
滤波增益:
状态估计及误差协方差矩阵:
P1,k+1=(I-Kk+1Hk+1)Pk+1,k,其中I为单位矩阵。
基于EKF的协同导航滤波器2中主要不同于滤波器1的是量测噪声协方差矩阵选取了另外一组其他步骤相同,并得到滤波器2下残差信息v2,k+1和其协方差S2,k+1,状态估计及其误差协方差矩阵P2,k+1
第三步:模型概率更新
做为本算法的重要部分,通过获取各个滤波器的残差信息并且结合贝叶斯假设检验方法,得到各模型的似然函数,求法如下:
m=3表示协同导航中量测向量的维数;vi(k)为协同导航模型在k时刻时通过卡尔曼滤波获得的残差向量;Si(k)为对应残差协方差矩阵。
结合上一时刻的模型概率以及模型先验信息,得到k时刻的模型概率为
第四步:交互输出
循环算法中最后一步,由前面各个基于不同模型滤波器的状态估计值及其模型概率加权融合得到,结合各模型的从AUV联合状态估计和协方差矩阵P(k)计算如下:
至此完成一次输出,进入下一时刻的状态估计。
以上结合附图对本发明的具体实施方式做了详细说明,但这些说明不能被理解为限制了本发明的范围,本发明的保护范围由随附的权利要求书限定,任何在本发明权利要求基础上的改动都属于本发明的保护范围。
Claims (1)
1.一种具有自适应的协同导航滤波方法,其特征在于,包括如下步骤:
(1)输入交互过程:
通过输入交互过程、模型滤波、模型概率更新以及输出交互过程四个过程完成状态估计;
在输入交互过程中,首次通过预验模型概率获取混合状态,下次循环则通过计算得到的模型概率替换;
协同导航从AUV交互混合状态估计及协方差为:
式中为k-1时刻第j各滤波器的状态估计;Pj(k-1)为对应的协方差矩阵;P0i(k-1)为状态估计所对应的状态误差协方差矩阵;
首次滤波中,用初始模型预测概率代替模型条件转移概率进行运算,下个滤波周期中通过利用上一个周期各个滤波器的状态估计和模型条件转移概率进行运算;
(2)模型滤波
基于EKF的协同导航滤波器1工作:
系统状态一步预测:其中表示从AUV在时刻k+1的一步估计状态,f为从AUV运动学方程,为从AUV交互混合估计状态1,uk量测输入;
一步预测协方差矩阵为:
其中Fk为状态转移矩阵,Pk为上一时刻的的协方差矩阵,Gk为过程噪声分布矩阵,Qk表示噪声协方差矩阵;
量测量预测为:
其中h表示量测方程,分别为领航者AUV1,AUV2的状态。
残差信息和其协方差:
其中Zk+1为传感器接收到的量测值,Hk+1为量测矩阵的雅可比矩阵,为滤波器1中的量测噪声协方差矩阵;
滤波增益:
状态估计及误差协方差矩阵:
P1,k+1=(I-Kk+1Hk+1)Pk+1,k,其中I为单位矩阵。
基于EKF的协同导航滤波器2中不同于滤波器1的是量测噪声协方差矩阵选取了另外一组其他步骤相同,并得到滤波器2下残差信息v2,k+1和其协方差S2,k+1,状态估计及其误差协方差矩阵P2,k+1;
(3)模型概率更新
通过获取各个滤波器的残差信息并且结合贝叶斯假设检验方法,得到各模型的似然函数:
m=3表示协同导航中量测向量的维数;vi(k)为协同导航模型在k时刻时通过卡尔曼滤波获得的残差向量;Si(k)为对应残差协方差矩阵;
结合上一时刻的模型概率以及模型先验信息,得到k时刻的模型概率为
(4)交互输出
由前面各个基于不同模型滤波器的状态估计值及其模型概率加权融合得到估计结果,结合各模型的从AUV联合状态估计和协方差矩阵P(k)计算如下:
至此完成一次输出,进入下一时刻的状态估计。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610810954.8A CN106441300B (zh) | 2016-09-08 | 2016-09-08 | 一种具有自适应的协同导航滤波方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610810954.8A CN106441300B (zh) | 2016-09-08 | 2016-09-08 | 一种具有自适应的协同导航滤波方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN106441300A true CN106441300A (zh) | 2017-02-22 |
CN106441300B CN106441300B (zh) | 2019-10-18 |
Family
ID=58164558
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201610810954.8A Active CN106441300B (zh) | 2016-09-08 | 2016-09-08 | 一种具有自适应的协同导航滤波方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN106441300B (zh) |
Cited By (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107677272A (zh) * | 2017-09-08 | 2018-02-09 | 哈尔滨工程大学 | 一种基于非线性信息滤波的auv协同导航方法 |
CN108151737A (zh) * | 2017-12-19 | 2018-06-12 | 南京航空航天大学 | 一种动态互观测关系条件下的无人机蜂群协同导航方法 |
CN109212472A (zh) * | 2018-07-11 | 2019-01-15 | 中国科学院信息工程研究所 | 一种面向噪声环境下的室内无线定位方法及装置 |
CN109974706A (zh) * | 2019-03-08 | 2019-07-05 | 哈尔滨工程大学 | 一种基于双运动模型的主从式多auv协同导航方法 |
CN110146084A (zh) * | 2019-05-24 | 2019-08-20 | 北京航空航天大学 | 面向卫星故障的多星编队系统分布式协同导航滤波方法 |
CN111837048A (zh) * | 2018-05-17 | 2020-10-27 | 罗伯特·博世有限公司 | 用于对定位数据滤波的方法和装置 |
CN111928851A (zh) * | 2020-04-17 | 2020-11-13 | 中国科学院沈阳自动化研究所 | 基于tma技术的多自主水下机器人集群协同导航方法 |
CN113654559A (zh) * | 2021-08-20 | 2021-11-16 | 青岛澎湃海洋探索技术有限公司 | 基于多模型观测校正的auv导航方法 |
CN116401618A (zh) * | 2023-03-03 | 2023-07-07 | 南京航空航天大学 | 基于几何分布采样的跨域无人集群协同导航信息融合方法 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20020177951A1 (en) * | 2001-05-23 | 2002-11-28 | Nokia Mobile Phones | Two-stage interacting multiple models filter for use in a global positioning system |
CN101483805A (zh) * | 2009-02-11 | 2009-07-15 | 江苏大学 | 一种视距和非视距混合环境下的无线定位方法 |
CN103776453A (zh) * | 2014-01-22 | 2014-05-07 | 东南大学 | 一种多模型水下航行器组合导航滤波方法 |
CN104833357A (zh) * | 2015-04-16 | 2015-08-12 | 中国科学院光电研究院 | 一种多系统多模型混合交互式信息融合定位方法 |
-
2016
- 2016-09-08 CN CN201610810954.8A patent/CN106441300B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20020177951A1 (en) * | 2001-05-23 | 2002-11-28 | Nokia Mobile Phones | Two-stage interacting multiple models filter for use in a global positioning system |
CN101483805A (zh) * | 2009-02-11 | 2009-07-15 | 江苏大学 | 一种视距和非视距混合环境下的无线定位方法 |
CN103776453A (zh) * | 2014-01-22 | 2014-05-07 | 东南大学 | 一种多模型水下航行器组合导航滤波方法 |
CN104833357A (zh) * | 2015-04-16 | 2015-08-12 | 中国科学院光电研究院 | 一种多系统多模型混合交互式信息融合定位方法 |
Non-Patent Citations (1)
Title |
---|
曾浩瀚: "多舰艇协同导航误差分析与算法研究", 《中国优秀硕士论文全文数据库工程科技Ⅱ辑》 * |
Cited By (14)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107677272A (zh) * | 2017-09-08 | 2018-02-09 | 哈尔滨工程大学 | 一种基于非线性信息滤波的auv协同导航方法 |
CN107677272B (zh) * | 2017-09-08 | 2020-11-10 | 哈尔滨工程大学 | 一种基于非线性信息滤波的auv协同导航方法 |
CN108151737A (zh) * | 2017-12-19 | 2018-06-12 | 南京航空航天大学 | 一种动态互观测关系条件下的无人机蜂群协同导航方法 |
CN108151737B (zh) * | 2017-12-19 | 2021-08-10 | 南京航空航天大学 | 一种动态互观测关系条件下的无人机蜂群协同导航方法 |
CN111837048A (zh) * | 2018-05-17 | 2020-10-27 | 罗伯特·博世有限公司 | 用于对定位数据滤波的方法和装置 |
CN109212472B (zh) * | 2018-07-11 | 2020-10-27 | 中国科学院信息工程研究所 | 一种面向噪声环境下的室内无线定位方法及装置 |
CN109212472A (zh) * | 2018-07-11 | 2019-01-15 | 中国科学院信息工程研究所 | 一种面向噪声环境下的室内无线定位方法及装置 |
CN109974706A (zh) * | 2019-03-08 | 2019-07-05 | 哈尔滨工程大学 | 一种基于双运动模型的主从式多auv协同导航方法 |
CN110146084A (zh) * | 2019-05-24 | 2019-08-20 | 北京航空航天大学 | 面向卫星故障的多星编队系统分布式协同导航滤波方法 |
CN111928851A (zh) * | 2020-04-17 | 2020-11-13 | 中国科学院沈阳自动化研究所 | 基于tma技术的多自主水下机器人集群协同导航方法 |
CN111928851B (zh) * | 2020-04-17 | 2023-12-12 | 中国科学院沈阳自动化研究所 | 基于tma技术的多自主水下机器人集群协同导航方法 |
CN113654559A (zh) * | 2021-08-20 | 2021-11-16 | 青岛澎湃海洋探索技术有限公司 | 基于多模型观测校正的auv导航方法 |
CN116401618A (zh) * | 2023-03-03 | 2023-07-07 | 南京航空航天大学 | 基于几何分布采样的跨域无人集群协同导航信息融合方法 |
CN116401618B (zh) * | 2023-03-03 | 2023-12-01 | 南京航空航天大学 | 基于几何分布采样的跨域无人集群协同导航信息融合方法 |
Also Published As
Publication number | Publication date |
---|---|
CN106441300B (zh) | 2019-10-18 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106441300A (zh) | 一种具有自适应的协同导航滤波方法 | |
Sasiadek et al. | Fuzzy adaptive Kalman filtering for INS/GPS data fusion | |
Volponi et al. | The use of Kalman filter and neural network methodologies in gas turbine performance diagnostics: a comparative study | |
CN109709934B (zh) | 一种飞行控制系统故障诊断冗余设计方法 | |
CN106909718A (zh) | 一种在不确定性环境下的工程结构优化设计方法 | |
CN107544459B (zh) | 一种控制系统的多重故障诊断优化方法 | |
CN103439731A (zh) | 基于无迹卡尔曼滤波的gps/ins组合导航方法 | |
CN104408744A (zh) | 一种用于目标跟踪的强跟踪容积卡尔曼滤波方法 | |
CN103776453A (zh) | 一种多模型水下航行器组合导航滤波方法 | |
CN105424043B (zh) | 一种基于判断机动的运动状态估计方法 | |
CN109341690B (zh) | 一种鲁棒高效的组合导航自适应数据融合方法 | |
CN109599866B (zh) | 一种预测辅助的电力系统状态估计方法 | |
Luzar et al. | Actuators and sensors fault diagnosis with dynamic, state-space neural networks | |
Zhao et al. | Fusing vehicle trajectories and gnss measurements to improve gnss positioning correction based on actor-critic learning | |
CN102254184B (zh) | 一种多物理域特征信息融合方法 | |
CN106651090B (zh) | 一种归一化的人机系统飞行品质预测方法 | |
CN109655057B (zh) | 一种六推无人机加速器测量值的滤波优化方法及其系统 | |
CN105652795B (zh) | 一种基于残差观测器的3ptt-2r串并联数控机床伺服系统故障预测装置及方法 | |
CN112257893A (zh) | 一种考虑监测误差的复杂机电系统健康状态预测方法 | |
CN114567288B (zh) | 基于变分贝叶斯的分布协同非线性系统状态估计方法 | |
Van Nguyen et al. | Mobile robot localization using fuzzy neural network based extended Kalman filter | |
Fu et al. | Statistical diagnosis for quality-related faults in BIW assembly process | |
CN111444475B (zh) | 一种应用在飞行测试数据分析的容错ckf滤波融合方法 | |
CN112946697B (zh) | 一种基于深度学习的卫星信号周跳探测与修复方法 | |
CN111738407B (zh) | 一种基于深度学习的钟差预测方法、装置、介质及终端 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |