CN115855049A - 基于粒子群优化鲁棒滤波的sins/dvl导航方法 - Google Patents
基于粒子群优化鲁棒滤波的sins/dvl导航方法 Download PDFInfo
- Publication number
- CN115855049A CN115855049A CN202310069689.2A CN202310069689A CN115855049A CN 115855049 A CN115855049 A CN 115855049A CN 202310069689 A CN202310069689 A CN 202310069689A CN 115855049 A CN115855049 A CN 115855049A
- Authority
- CN
- China
- Prior art keywords
- representing
- matrix
- error
- navigation
- sins
- 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
- 239000002245 particle Substances 0.000 title claims abstract description 90
- 238000001914 filtration Methods 0.000 title claims abstract description 42
- 238000000034 method Methods 0.000 title claims abstract description 42
- 238000005457 optimization Methods 0.000 title claims abstract description 26
- 239000011159 matrix material Substances 0.000 claims abstract description 103
- 238000005259 measurement Methods 0.000 claims abstract description 35
- 230000009466 transformation Effects 0.000 claims abstract description 10
- 230000008569 process Effects 0.000 claims abstract description 7
- 238000010276 construction Methods 0.000 claims abstract description 6
- 238000004422 calculation algorithm Methods 0.000 claims description 21
- 230000007704 transition Effects 0.000 claims description 18
- 230000006870 function Effects 0.000 claims description 15
- 238000012546 transfer Methods 0.000 claims description 12
- 230000003044 adaptive effect Effects 0.000 claims description 4
- 230000005484 gravity Effects 0.000 claims description 4
- 238000013459 approach Methods 0.000 claims description 3
- 230000006872 improvement Effects 0.000 claims description 2
- 238000012545 processing Methods 0.000 description 2
- 241000047218 Dayia Species 0.000 description 1
- 230000001133 acceleration Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000000737 periodic effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 1
Images
Landscapes
- Navigation (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Feedback Control In General (AREA)
Abstract
本发明提供一种基于粒子群优化鲁棒滤波的SINS/DVL导航方法,属于水下机器人导航定位技术领域,其主要目的在于增强复杂水下环境中SINS/DVL组合导航***的抗干扰能力,进一步提高SINS/DVL***的组合导航精度。本发明的主要步骤包括:基于状态变换原理的导航模型构建、滤波状态更新及噪声矩阵构造、适应度函数构造、自适应粒子群优化过程以及滤波量测更新。本发明可以解决复杂水下环境中SINS/DVL组合导航***受干扰导致的定位精度差的问题。相比基于卡尔曼滤波方法、Huber鲁棒滤波方法。本方法能提高***的够抗干扰能力,进一步提高SINS/DVL的组合导航精度。
Description
技术领域
本发明属于水下机器人导航定位技术,具体涉及一种基于粒子群优化鲁棒滤波的SINS/DVL导航方法。
背景技术
海洋资源丰富,而水下机器人又是开发利用海洋资源的有效载体。同时,水下机器人高效作业离不开高精度的导航定位信息。因此水下导航定位在水下机器人领域发挥着不可替代的作用。目前常用的水下导航方式有捷联惯导***(SINS)、多普勒计程仪(DVL)、水声定位***、地球物理导航***等。虽然水声定位***具有精度高的特点,但是需要在水下事先铺设基阵,维护复杂,且在未知海域无法使用。地球物理导航方法如:地形匹配、地磁匹配等,此类方法需要事先建立庞大的地形、地磁数据库,且需要定期更新操作复杂。因此,以捷联惯导***为主多普勒计程仪为辅的组合导航方式成为水下机器人常用且自主的导航方式。
由于水下环境复杂,声学信号传播存在不确定性和干扰性。因此基于SINS/DVL的鲁棒滤波算法研究迫在眉睫。卡尔曼滤波算法是SINS/DVL组合的最经典滤波算法,然而其无法应对外界干扰问题;近年来提出的Huber鲁棒滤波算法能够提高组合导航***的抗干扰能力,然而当外界干扰连续或者强非线性噪声干扰时Huber鲁棒滤波算法依然存在定位经度差的问题。
针对上述问题,本发明提出一种基于粒子群优化鲁棒滤波的SINS/DVL导航方法,一方面,为提高模型的鲁棒性,基于状态变换原理,建立了SINS/DVL导航模型;另一方面,为提高算法的鲁棒性,在Huber鲁棒滤波算法的基础上,分别改进噪声方差阵以及引入自适应粒子群优化算法,实现对外界干扰测量信息的鲁棒处理,进而提高SINS/DVL导航精度。
发明内容
发明目的:本发明的目的在于解决复杂环境下水下SINS/DVL组合导航问题,提供一种基于粒子群优化鲁棒滤波的SINS/DVL导航方法。
本发明的一种基于粒子群优化鲁棒滤波的SINS/DVL导航方法,SINS/DVL导航的已知量:
该方法包括如下步骤:
步骤1.基于状态变换原理构建的组合导航模型构建;
步骤2. 在步骤1的基础上进行滤波状态更新及***噪声方差阵Q k 和量测噪声方差阵S k 的构造;
步骤3. 引入自适应粒子滤波算法,对***噪声方差阵Q k 和量测噪声方差阵S k 进行修正,此过程分为两步,粒子滤波适应度函数构造和自适应粒子群优化算法;
步骤4.进行滤波量测更新。
进一步地,步骤1具体包括如下步骤:
首先,基于状态变换原理建立SINS/DVL组合导航状态方程:
其中,分别表示东向速度误差、北向速度误差和天向速度误差;分别表示x方向姿态误差、y方向姿态误差和z方向姿态误差;/>分别表示经度误差、纬度误差和高度误差;/>分别表示x方向加速度计零偏误差、y方向加速度计零偏误差和z方向加速度计零偏误差;/>分别表示x方向陀螺仪零偏误差、y方向陀螺仪零偏误差和z方向陀螺仪零偏误差;
接着,给出SINS/DVL组合导航***的量测方程:
进一步地,步骤2具体包括如下步骤:
进行滤波状态更新如下:
状态向量均方误差阵更新如下:
进一步地,步骤3具体包括如下步骤:
首先适应度函数构造
将***噪声方差阵Q k 和量测噪声方差阵S k 作为粒子,而SINS/DVL组合导航***性能主要取决于位置精度,所以将滤波器输出的位置误差的均方差作为适应度函数:
然后进行自适应粒子群优化,具体包括如下过程:
式中,G为当前迭代次数;为粒子t在第G代的位置,/>为粒子t在第G+1代的位置,/>为粒子t在第G+1代的速度;/>为粒子t第G代为止搜索到的最好位置;为整个粒子群第G代为止搜索到的最好位置,/>为惯性权重;/>为学习因子;为0~1之间的随机数;
对惯性权重的改进如下:
进一步地,步骤4具体包括如下步骤:
进行滤波量测更新:
均方误差阵更新为:
其中,表示15行15列的单位矩阵;/>表示k时刻的滤波增益,/>表示k时刻的协方差矩阵,/>表示k时刻的量测转移矩阵,/>表示k时刻的量测向量矩阵中的第i个元素,/>表示k时刻矩阵A中的第i个元素,/>表示k时刻的量测转移矩阵。
与现有技术相比,本发明具有如下优点:
(1)本发明针对SINS/DVL导航模型鲁棒性问题,使用不易受干扰的地球重力信息代替易受干扰的加速度测量信息,构建了基于状态变换原理的导航模型,推导了状态方程和量测方程,进一步提高了模型的鲁棒性。
(2)本发明在Huber鲁棒滤波算法的基础上,考虑***噪声和量测噪声的强非线性问题,引入粒子群优化算法,设计了适应度函数,进一步提高了算法对干扰噪声的处理能力。
(3) 本发明在粒子群优化算法的基础上,针对权重对粒子群搜索速度和收敛性的影响,本发明提出了一种改进的惯性权重计算方法,进一步提高了粒子群算法的自适应性。
附图说明
图1为本发明所描述的基于粒子群优化鲁棒滤波的SINS/DVL导航方法流程图;
图2为本发明所描述的基于粒子群优化鲁棒滤波的SINS/DVL导航方法原理图;
图3为本发明方法的SINS/DVL组合导航东向位置误差曲线;
图4为本发明方法的SINS/DVL组合导航北向位置误差曲线。
具体实施方式
下面对本发明技术方案进行详细说明,但是本发明的保护范围不局限于所述实施例。
本发明的一种基于粒子群优化鲁棒滤波的SINS/DVL导航方法,首先已知量:
该方法包括如下步骤:
步骤1.基于状态变换原理构建的组合导航模型构建;
首先,基于状态变换原理建立SINS/DVL组合导航状态方程:
其中,分别表示东向速度误差、北向速度误差和天向速度误差;分别表示x方向姿态误差、y方向姿态误差和z方向姿态误差;/>分别表示经度误差、纬度误差和高度误差;/>分别表示x方向加速度计零偏误差、y方向加速度计零偏误差和z方向加速度计零偏误差;/>分别表示x方向陀螺仪零偏误差、y方向陀螺仪零偏误差和z方向陀螺仪零偏误差;
接着,给出SINS/DVL组合导航***的量测方程:
以上即为本发明提出的基于状态变换原理的组合导航模型。
步骤2. 在步骤1的基础上进行滤波状态更新***噪声方差阵Q k 和量测噪声方差阵S k 的构造;
进行滤波状态更新如下:
状态向量均方误差阵更新如下:
步骤3. 为了能够更好地满足滤波算法对鲁棒性的需求,在上述两个噪声方差阵的基础上,本发明引入自适应粒子滤波算法,对***噪声方差阵Q k 和量测噪声方差阵S k 进行修正,进而提高滤波的鲁棒性。此过程分为两步,粒子滤波适应度函数构造和自适应粒子群优化算法;
将***噪声方差阵Q k 和量测噪声方差阵S k 作为粒子,而SINS/DVL组合导航***性能主要取决于位置精度,所以将滤波器输出的位置误差的均方差作为适应度函数:
然后进行自适应粒子群优化,具体包括如下过程:
式中,G为当前迭代次数;为粒子t在第G代的位置,/>为粒子t在第G+1代的位置,/>为粒子t在第G+1代的速度;/>为粒子t第G代为止搜索到的最好位置;为整个粒子群第G代为止搜索到的最好位置,/>为惯性权重;/>为学习因子;为0~1之间的随机数;
为了更合理地改变惯性权重得到更优的结果。本发明对惯性权重的改进如下:
步骤4.在上述滤波状态更新和噪声矩阵修正的基础上,进行滤波量测更新:
均方误差阵更新为:
其中,表示15行15列的单位矩阵;/>表示k时刻的滤波增益,/>表示k时刻的协方差矩阵,/>表示k时刻的量测转移矩阵,/>表示k时刻的量测向量矩阵中的第i个元素,/>表示k时刻矩阵A中的第i个元素,/>表示k时刻的量测转移矩阵。
仿真参数设置如下:
陀螺仪零偏误差;加速度计零偏误差100ug;捷联惯导***数据输出频率200Hz;DVL刻度因子误差0.003;DVL输出频率2Hz。其中图3给出了三种方法对应的SINS/DVL组合导航东向位置误差;图4给出了三种方法对应的SINS/DVL组合导航北向位置误差曲线;从图3和图4中可以看出本发明方法导航精度优于卡尔曼滤波方法和Huber鲁棒滤波方法。
应理解这些实施例仅用于说明本发明而不用于限制本发明的范围,在阅读了本发明之后,本领域普通技术人员对本发明的各种等价形式的修改均落于本申请所附权利要求所限定的范围。
Claims (5)
1.一种基于粒子群优化鲁棒滤波的SINS/DVL导航方法,SINS/DVL导航的已知量:
其特征在于,该方法包括如下步骤:
步骤1.基于状态变换原理构建的组合导航模型构建;
步骤2. 在步骤1的基础上进行滤波状态更新及***噪声方差阵Q k 和量测噪声方差阵S k 的构造;
步骤3. 引入自适应粒子滤波算法,对***噪声方差阵Q k 和量测噪声方差阵S k 进行修正,此过程分为两步,粒子滤波适应度函数构造和自适应粒子群优化算法;
步骤4.进行滤波量测更新。
2.根据权利要求1所述的基于粒子群优化鲁棒滤波的SINS/DVL导航方法,其特征在于,步骤1具体包括如下步骤:
首先,基于状态变换原理建立SINS/DVL组合导航状态方程:
其中,分别表示东向速度误差、北向速度误差和天向速度误差;分别表示x方向姿态误差、y方向姿态误差和z方向姿态误差;/>分别表示经度误差、纬度误差和高度误差;/>分别表示x方向加速度计零偏误差、y方向加速度计零偏误差和z方向加速度计零偏误差;/>分别表示x方向陀螺仪零偏误差、y方向陀螺仪零偏误差和z方向陀螺仪零偏误差;
接着,给出SINS/DVL组合导航***的量测方程:
4.根据权利要求3所述的基于粒子群优化鲁棒滤波的SINS/DVL导航方法,其特征在于,步骤3具体包括如下步骤:
首先适应度函数构造
将***噪声方差阵Q k 和量测噪声方差阵S k 作为粒子,而SINS/DVL组合导航***性能主要取决于位置精度,所以将滤波器输出的位置误差的均方差作为适应度函数:
然后进行自适应粒子群优化,具体包括如下过程:
式中,G为当前迭代次数;为粒子t在第G代的位置,/>为粒子t在第G+1代的位置,/>为粒子t在第G+1代的速度;/>为粒子t第G代为止搜索到的最好位置;/>为整个粒子群第G代为止搜索到的最好位置,/>为惯性权重;/>为学习因子;/>为0~1之间的随机数;
对惯性权重的改进如下:
5.根据权利要求4所述的基于粒子群优化鲁棒滤波的SINS/DVL导航方法,其特征在于,步骤4具体包括如下步骤:
进行滤波量测更新:
均方误差阵更新为:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310069689.2A CN115855049B (zh) | 2023-02-07 | 2023-02-07 | 基于粒子群优化鲁棒滤波的sins/dvl导航方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310069689.2A CN115855049B (zh) | 2023-02-07 | 2023-02-07 | 基于粒子群优化鲁棒滤波的sins/dvl导航方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN115855049A true CN115855049A (zh) | 2023-03-28 |
CN115855049B CN115855049B (zh) | 2023-05-12 |
Family
ID=85657690
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202310069689.2A Active CN115855049B (zh) | 2023-02-07 | 2023-02-07 | 基于粒子群优化鲁棒滤波的sins/dvl导航方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115855049B (zh) |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116255988A (zh) * | 2023-05-11 | 2023-06-13 | 北京航空航天大学 | 一种基于舰船动力学组合导航复合抗干扰自适应滤波方法 |
CN116295538A (zh) * | 2023-05-24 | 2023-06-23 | 河海大学 | 一种基于改进粒子滤波的usbl安装误差标定方法 |
CN117804444A (zh) * | 2024-01-08 | 2024-04-02 | 河海大学 | 基于ukf和滚动时域估计的水下机器人组合定位方法 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111024064A (zh) * | 2019-11-25 | 2020-04-17 | 东南大学 | 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法 |
CN112556697A (zh) * | 2020-12-08 | 2021-03-26 | 江苏科技大学 | 一种基于联邦结构的浅耦合数据融合导航方法 |
WO2022028286A1 (zh) * | 2020-08-04 | 2022-02-10 | 东南大学 | 一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法 |
CN114459477A (zh) * | 2022-03-09 | 2022-05-10 | 东南大学 | 一种基于改进的pso-anfis辅助的sins/dvl紧组合导航方法 |
-
2023
- 2023-02-07 CN CN202310069689.2A patent/CN115855049B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111024064A (zh) * | 2019-11-25 | 2020-04-17 | 东南大学 | 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法 |
WO2022028286A1 (zh) * | 2020-08-04 | 2022-02-10 | 东南大学 | 一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法 |
CN112556697A (zh) * | 2020-12-08 | 2021-03-26 | 江苏科技大学 | 一种基于联邦结构的浅耦合数据融合导航方法 |
CN114459477A (zh) * | 2022-03-09 | 2022-05-10 | 东南大学 | 一种基于改进的pso-anfis辅助的sins/dvl紧组合导航方法 |
Non-Patent Citations (3)
Title |
---|
CHENGCHENG LIANG等: "SINS/DVL Integrated Based on Interacting Multiple Model Approach by Integrating Feedback Particle Filter for ship navigation" * |
徐晓苏;董亚;童金武;代维;: "基于5阶球面最简相径的改进型容积卡尔曼滤波在SINS/DVL组合导航中的应用" * |
王博;刘泾洋;刘沛佳;: "SINS/DVL组合导航技术综述" * |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116255988A (zh) * | 2023-05-11 | 2023-06-13 | 北京航空航天大学 | 一种基于舰船动力学组合导航复合抗干扰自适应滤波方法 |
CN116295538A (zh) * | 2023-05-24 | 2023-06-23 | 河海大学 | 一种基于改进粒子滤波的usbl安装误差标定方法 |
CN117804444A (zh) * | 2024-01-08 | 2024-04-02 | 河海大学 | 基于ukf和滚动时域估计的水下机器人组合定位方法 |
Also Published As
Publication number | Publication date |
---|---|
CN115855049B (zh) | 2023-05-12 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109443379B (zh) | 一种深海潜航器的sins/dvl水下抗晃动对准方法 | |
CN115855049A (zh) | 基于粒子群优化鲁棒滤波的sins/dvl导航方法 | |
CN112097763B (zh) | 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法 | |
WO2020062807A1 (zh) | 改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法 | |
CN106871928B (zh) | 基于李群滤波的捷联惯性导航初始对准方法 | |
CN104655131B (zh) | 基于istssrckf的惯性导航初始对准方法 | |
CN111024064B (zh) | 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法 | |
CN103759742B (zh) | 基于模糊自适应控制技术的捷联惯导非线性对准方法 | |
CN110579740A (zh) | 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法 | |
CN110779518B (zh) | 一种具有全局收敛性的水下航行器单信标定位方法 | |
CN110514203B (zh) | 一种基于isr-ukf的水下组合导航方法 | |
CN110231029B (zh) | 一种水下机器人多传感器融合数据处理方法 | |
CN105509739A (zh) | 采用固定区间crts平滑的ins/uwb紧组合导航***及方法 | |
CN105424036A (zh) | 一种低成本水下潜器地形辅助惯性组合导航定位方法 | |
CN113834483B (zh) | 一种基于可观测度的惯性/偏振/地磁容错导航方法 | |
CN112284384A (zh) | 考虑量测异常的集群式多深海潜航器的协同定位方法 | |
CN104374405A (zh) | 一种基于自适应中心差分卡尔曼滤波的mems捷联惯导初始对准方法 | |
CN111156986B (zh) | 一种基于抗差自适应ukf的光谱红移自主组合导航方法 | |
CN105547300A (zh) | 用于auv的全源导航***及方法 | |
CN111207773B (zh) | 一种用于仿生偏振光导航的姿态无约束优化求解方法 | |
CN108508463B (zh) | 基于Fourier-Hermite正交多项式扩展椭球集员滤波方法 | |
CN111323049B (zh) | 一种粒子群算法的粗对准方法 | |
CN110221331B (zh) | 基于状态变换的惯性/卫星组合导航动态滤波方法 | |
CN111964675A (zh) | 一种黑障区的飞行器智能导航方法 | |
CN107944467A (zh) | 一种Adaboost优化的车载MIMUs/GPS信息融合方法及*** |
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 |