CN115855049A - 基于粒子群优化鲁棒滤波的sins/dvl导航方法 - Google Patents

基于粒子群优化鲁棒滤波的sins/dvl导航方法 Download PDF

Info

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
Application number
CN202310069689.2A
Other languages
English (en)
Other versions
CN115855049B (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.)
Hohai University HHU
Original Assignee
Hohai University HHU
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 Hohai University HHU filed Critical Hohai University HHU
Priority to CN202310069689.2A priority Critical patent/CN115855049B/zh
Publication of CN115855049A publication Critical patent/CN115855049A/zh
Application granted granted Critical
Publication of CN115855049B publication Critical patent/CN115855049B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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的鲁棒滤波算法研究迫在眉睫。卡尔曼滤波算法是SINS/DVL组合的最经典滤波算法，然而其无法应对外界干扰问题；近年来提出的Huber鲁棒滤波算法能够提高组合导航***的抗干扰能力，然而当外界干扰连续或者强非线性噪声干扰时Huber鲁棒滤波算法依然存在定位经度差的问题。
针对上述问题，本发明提出一种基于粒子群优化鲁棒滤波的SINS/DVL导航方法，一方面，为提高模型的鲁棒性，基于状态变换原理，建立了SINS/DVL导航模型；另一方面，为提高算法的鲁棒性，在Huber鲁棒滤波算法的基础上，分别改进噪声方差阵以及引入自适应粒子群优化算法，实现对外界干扰测量信息的鲁棒处理，进而提高SINS/DVL导航精度。
发明内容
发明目的：本发明的目的在于解决复杂环境下水下SINS/DVL组合导航问题，提供一种基于粒子群优化鲁棒滤波的SINS/DVL导航方法。
本发明的一种基于粒子群优化鲁棒滤波的SINS/DVL导航方法，SINS/DVL导航的已知量：
多普勒计程仪，缩写为DVL，输出的载体系下的三维速度信息
Figure SMS_1
，其中，/>
Figure SMS_2
表示载体坐标系下x方向的速度信息，/>
Figure SMS_3
表示载体坐标系下y方向的速度信息，
Figure SMS_4
表示载体坐标系下z方向的速度信息；
捷联惯导***，缩写为SINS，输出的导航坐标系下的三维速度信息：
Figure SMS_5
，其中，/>
Figure SMS_6
表示东向速度，/>
Figure SMS_7
表示北向速度，/>
Figure SMS_8
表示天向速度；
该方法包括如下步骤：
步骤1.基于状态变换原理构建的组合导航模型构建；
步骤2. 在步骤1的基础上进行滤波状态更新及***噪声方差阵Q k 和量测噪声方差阵S k 的构造；
步骤3. 引入自适应粒子滤波算法，对***噪声方差阵Q k 和量测噪声方差阵S k 进行修正，此过程分为两步，粒子滤波适应度函数构造和自适应粒子群优化算法；
步骤4.进行滤波量测更新。
进一步地，步骤1具体包括如下步骤：
首先，基于状态变换原理建立SINS/DVL组合导航状态方程：
Figure SMS_9
其中，
Figure SMS_10
表示组合导航状态向量，/>
Figure SMS_11
表示组合导航状态转移矩阵；/>
Figure SMS_12
表示组合导航***噪声；/>
Figure SMS_13
表示15维的组合导航状态向量，可表示如下：
Figure SMS_14
其中，
Figure SMS_15
分别表示东向速度误差、北向速度误差和天向速度误差；
Figure SMS_16
分别表示x方向姿态误差、y方向姿态误差和z方向姿态误差；/>
Figure SMS_17
分别表示经度误差、纬度误差和高度误差；/>
Figure SMS_18
分别表示x方向加速度计零偏误差、y方向加速度计零偏误差和z方向加速度计零偏误差；/>
Figure SMS_19
分别表示x方向陀螺仪零偏误差、y方向陀螺仪零偏误差和z方向陀螺仪零偏误差；
组合导航状态转移矩阵
Figure SMS_20
由四部分组成，具体如下：
Figure SMS_21
其中，
Figure SMS_22
表示速度误差对应的状态转移子矩阵；/>
Figure SMS_23
表示姿态误差对应的状态转移子矩阵；/>
Figure SMS_24
表示位置误差对应的状态转移子矩阵；/>
Figure SMS_25
表示一个6行15列的零矩阵，其中，/>
Figure SMS_26
表述如下：
Figure SMS_27
其中，
Figure SMS_28
表示地球系相对于惯性系的转动角速度；/>
Figure SMS_29
表示导航系相对于地球系的旋转角速度；/>
Figure SMS_30
表示导航坐标系下地球重力信息；/>
Figure SMS_31
表示载体坐标系到导航坐标系的姿态转移矩阵；符号/>
Figure SMS_32
表示求矩阵的反对称矩阵；
Figure SMS_33
表述如下：
Figure SMS_34
/>
其中，
Figure SMS_35
只是为了表述方便而定义的中间变量，不代表任何意义，且，/>
Figure SMS_36
的表达式分别为：
Figure SMS_37
Figure SMS_38
Figure SMS_39
Figure SMS_40
表示地球旋转角速率；L和h分别表示载体所在位置的地理纬度和高度；R M 和R N 分别表示载体所在位置的子午圈半径和卯酉圈半径；
Figure SMS_41
表述如下：
Figure SMS_42
其中，
Figure SMS_43
只是为了表述方便而定义的中间变量，不代表任何意义，且/>
Figure SMS_44
的表达式分别为：
Figure SMS_45
Figure SMS_46
接着，给出SINS/DVL组合导航***的量测方程：
Figure SMS_47
其中，
Figure SMS_48
表示量测转移矩阵；/>
Figure SMS_49
表示量测噪声矩阵；/>
Figure SMS_50
表示量测矩阵，可表示如下：
Figure SMS_51
其中，
Figure SMS_52
表示导航系到载体系的姿态转移矩阵；/>
Figure SMS_53
表示DVL输出的载体系下的三维速度信息，量测转移矩阵/>
Figure SMS_54
可表示如下：
Figure SMS_55
。
进一步地，步骤2具体包括如下步骤：
进行滤波状态更新如下：
Figure SMS_56
其中，
Figure SMS_57
表示k时刻的组合导航状态转移矩阵；/>
Figure SMS_58
表示k-1时刻的组合导航状态向量估计值，/>
Figure SMS_59
表示一步预测状态向量；
状态向量均方误差阵更新如下：
Figure SMS_60
其中，
Figure SMS_61
表示状态向量的均方误差阵；/>
Figure SMS_62
表示k时刻的***噪声方差阵，
Figure SMS_63
表示一步预测协方差矩阵，上标T表示矩阵的转置；
基于Huber M估计原理，构造量测噪声方差阵
Figure SMS_64
：
Figure SMS_65
其中，
Figure SMS_66
表示量测噪声方差阵。
进一步地，步骤3具体包括如下步骤：
首先适应度函数构造
将***噪声方差阵Q k 和量测噪声方差阵S k 作为粒子，而SINS/DVL组合导航***性能主要取决于位置精度，所以将滤波器输出的位置误差的均方差作为适应度函数：
Figure SMS_67
其中，f（Q k ,S k ）是Q k ,S k 为参数时SINS/DVL组合导航***位置误差的均方差；
Figure SMS_68
为估计的位置值；M为估计长度；/>
Figure SMS_69
为参考的实际位置值；
然后进行自适应粒子群优化，具体包括如下过程：
Figure SMS_70
初始化粒子群参数，在参数区间内初始化粒子群的位置和速度，随机产生若干粒子并初始化最优粒子；/>
Figure SMS_71
根据上述适应度函数，对每一个粒子的适应度进行评价，比较其与最优粒子的适应度值，若适应度更好则取代最优粒子；
Figure SMS_72
粒子t=（Q k ,S k ）迭代更新自己的速度和位置，每个粒子根据自身和群体经验调整轨迹向最优点靠拢，按以下方程式进行更新：
Figure SMS_73
Figure SMS_74
式中，G为当前迭代次数；
Figure SMS_76
为粒子t在第G代的位置，/>
Figure SMS_78
为粒子t在第G+1代的位置，/>
Figure SMS_80
为粒子t在第G+1代的速度；/>
Figure SMS_77
为粒子t第G代为止搜索到的最好位置；
Figure SMS_79
为整个粒子群第G代为止搜索到的最好位置，/>
Figure SMS_81
为惯性权重；/>
Figure SMS_82
为学习因子；
Figure SMS_75
为0~1之间的随机数；
对惯性权重的改进如下：
Figure SMS_83
其中，u start 和u end 分别为惯性权重的初值和末值，其取值范围是：0.4≤u end ≤u start ≤0.9；
Figure SMS_84
为迭代次数的最大值；
Figure SMS_85
重复/>
Figure SMS_86
和/>
Figure SMS_87
，更新最优粒子，直至满足迭代次数或精度要求，产生最优粒子。
进一步地，步骤4具体包括如下步骤：
进行滤波量测更新：
Figure SMS_88
其中，
Figure SMS_89
表示k时刻的状态向量，/>
Figure SMS_90
表示k时刻的量测向量，/>
Figure SMS_91
表示k-1时刻的状态估计权重；/>
Figure SMS_92
只是为了表述方便而定义的中间变量，不代表任何意义，具体表述如下：
Figure SMS_93
Figure SMS_94
Figure SMS_95
权值矩阵中各元素的取值为：/>
Figure SMS_96
其中，
Figure SMS_97
表示符号函数；/>
Figure SMS_98
表示因子；
均方误差阵更新为：
Figure SMS_99
其中，
Figure SMS_100
表示15行15列的单位矩阵；/>
Figure SMS_101
表示k时刻的滤波增益，/>
Figure SMS_102
表示k时刻的协方差矩阵，/>
Figure SMS_103
表示k时刻的量测转移矩阵，/>
Figure SMS_104
表示k时刻的量测向量矩阵中的第i个元素，/>
Figure SMS_105
表示k时刻矩阵A中的第i个元素，/>
Figure SMS_106
表示k时刻的量测转移矩阵。
与现有技术相比，本发明具有如下优点：
(1)本发明针对SINS/DVL导航模型鲁棒性问题，使用不易受干扰的地球重力信息代替易受干扰的加速度测量信息，构建了基于状态变换原理的导航模型，推导了状态方程和量测方程，进一步提高了模型的鲁棒性。
(2)本发明在Huber鲁棒滤波算法的基础上，考虑***噪声和量测噪声的强非线性问题，引入粒子群优化算法，设计了适应度函数，进一步提高了算法对干扰噪声的处理能力。
(3) 本发明在粒子群优化算法的基础上，针对权重对粒子群搜索速度和收敛性的影响，本发明提出了一种改进的惯性权重计算方法，进一步提高了粒子群算法的自适应性。
附图说明
图1为本发明所描述的基于粒子群优化鲁棒滤波的SINS/DVL导航方法流程图；
图2为本发明所描述的基于粒子群优化鲁棒滤波的SINS/DVL导航方法原理图；
图3为本发明方法的SINS/DVL组合导航东向位置误差曲线；
图4为本发明方法的SINS/DVL组合导航北向位置误差曲线。
具体实施方式
下面对本发明技术方案进行详细说明，但是本发明的保护范围不局限于所述实施例。
本发明的一种基于粒子群优化鲁棒滤波的SINS/DVL导航方法，首先已知量：
多普勒计程仪，缩写为DVL，输出的载体系下的三维速度信息
Figure SMS_107
，其中，/>
Figure SMS_108
表示载体坐标系下x方向的速度信息，/>
Figure SMS_109
表示载体坐标系下y方向的速度信息，
Figure SMS_110
表示载体坐标系下z方向的速度信息；
捷联惯导***，缩写为SINS，输出的导航坐标系下的三维速度信息：
Figure SMS_111
，其中，/>
Figure SMS_112
表示东向速度，/>
Figure SMS_113
表示北向速度，/>
Figure SMS_114
表示天向速度；/>
该方法包括如下步骤：
步骤1.基于状态变换原理构建的组合导航模型构建；
首先，基于状态变换原理建立SINS/DVL组合导航状态方程：
Figure SMS_115
其中，
Figure SMS_116
表示组合导航状态向量，/>
Figure SMS_117
表示组合导航状态转移矩阵；/>
Figure SMS_118
表示组合导航***噪声；/>
Figure SMS_119
表示15维的组合导航状态向量，可表示如下：
Figure SMS_120
其中，
Figure SMS_121
分别表示东向速度误差、北向速度误差和天向速度误差；
Figure SMS_122
分别表示x方向姿态误差、y方向姿态误差和z方向姿态误差；/>
Figure SMS_123
分别表示经度误差、纬度误差和高度误差；/>
Figure SMS_124
分别表示x方向加速度计零偏误差、y方向加速度计零偏误差和z方向加速度计零偏误差；/>
Figure SMS_125
分别表示x方向陀螺仪零偏误差、y方向陀螺仪零偏误差和z方向陀螺仪零偏误差；
组合导航状态转移矩阵
Figure SMS_126
由四部分组成，具体如下：
Figure SMS_127
其中，
Figure SMS_128
表示速度误差对应的状态转移子矩阵；/>
Figure SMS_129
表示姿态误差对应的状态转移子矩阵；/>
Figure SMS_130
表示位置误差对应的状态转移子矩阵；/>
Figure SMS_131
表示一个6行15列的零矩阵，其中，/>
Figure SMS_132
表述如下：
Figure SMS_133
其中，
Figure SMS_134
表示地球系相对于惯性系的转动角速度；/>
Figure SMS_135
表示导航系相对于地球系的旋转角速度；/>
Figure SMS_136
表示导航坐标系下地球重力信息；/>
Figure SMS_137
表示载体坐标系到导航坐标系的姿态转移矩阵；符号/>
Figure SMS_138
表示求矩阵的反对称矩阵；
Figure SMS_139
表述如下：
Figure SMS_140
其中，
Figure SMS_141
只是为了表述方便而定义的中间变量，不代表任何意义，且，/>
Figure SMS_142
的表达式分别为：/>
Figure SMS_143
Figure SMS_144
Figure SMS_145
Figure SMS_146
表示地球旋转角速率；L和h分别表示载体所在位置的地理纬度和高度；R M 和R N 分别表示载体所在位置的子午圈半径和卯酉圈半径；
Figure SMS_147
表述如下：
Figure SMS_148
其中，
Figure SMS_149
只是为了表述方便而定义的中间变量，不代表任何意义，且/>
Figure SMS_150
的表达式分别为：
Figure SMS_151
Figure SMS_152
接着，给出SINS/DVL组合导航***的量测方程：
Figure SMS_153
其中，
Figure SMS_154
表示量测转移矩阵；/>
Figure SMS_155
表示量测噪声矩阵；/>
Figure SMS_156
表示量测矩阵，可表示如下：
Figure SMS_157
其中，
Figure SMS_158
表示导航系到载体系的姿态转移矩阵；/>
Figure SMS_159
表示DVL输出的载体系下的三维速度信息，量测转移矩阵/>
Figure SMS_160
可表示如下：
Figure SMS_161
。
以上即为本发明提出的基于状态变换原理的组合导航模型。
步骤2. 在步骤1的基础上进行滤波状态更新***噪声方差阵Q k 和量测噪声方差阵S k 的构造；
进行滤波状态更新如下：
Figure SMS_162
其中，
Figure SMS_163
表示k时刻的组合导航状态转移矩阵；/>
Figure SMS_164
表示k-1时刻的组合导航状态向量估计值，/>
Figure SMS_165
表示一步预测状态向量；
状态向量均方误差阵更新如下：
Figure SMS_166
其中，
Figure SMS_167
表示状态向量的均方误差阵；/>
Figure SMS_168
表示k时刻的***噪声方差阵，
Figure SMS_169
表示一步预测协方差矩阵，上标T表示矩阵的转置；
基于Huber M估计原理，构造量测噪声方差阵
Figure SMS_170
：
Figure SMS_171
其中，
Figure SMS_172
表示量测噪声方差阵。
步骤3. 为了能够更好地满足滤波算法对鲁棒性的需求，在上述两个噪声方差阵的基础上，本发明引入自适应粒子滤波算法，对***噪声方差阵Q k 和量测噪声方差阵S k 进行修正，进而提高滤波的鲁棒性。此过程分为两步，粒子滤波适应度函数构造和自适应粒子群优化算法；
将***噪声方差阵Q k 和量测噪声方差阵S k 作为粒子，而SINS/DVL组合导航***性能主要取决于位置精度，所以将滤波器输出的位置误差的均方差作为适应度函数：
Figure SMS_173
其中，f（Q k ,S k ）是Q k ,S k 为参数时SINS/DVL组合导航***位置误差的均方差；
Figure SMS_174
为估计的位置值；M为估计长度；/>
Figure SMS_175
为参考的实际位置值；
然后进行自适应粒子群优化，具体包括如下过程：
Figure SMS_176
初始化粒子群参数，在参数区间内初始化粒子群的位置和速度，随机产生若干粒子并初始化最优粒子；
Figure SMS_177
根据上述适应度函数，对每一个粒子的适应度进行评价，比较其与最优粒子的适应度值，若适应度更好则取代最优粒子；
Figure SMS_178
粒子t=（Q k ,S k ）迭代更新自己的速度和位置，每个粒子根据自身和群体经验调整轨迹向最优点靠拢，按以下方程式进行更新：
Figure SMS_179
Figure SMS_180
式中，G为当前迭代次数；
Figure SMS_182
为粒子t在第G代的位置，/>
Figure SMS_185
为粒子t在第G+1代的位置，/>
Figure SMS_187
为粒子t在第G+1代的速度；/>
Figure SMS_183
为粒子t第G代为止搜索到的最好位置；
Figure SMS_184
为整个粒子群第G代为止搜索到的最好位置，/>
Figure SMS_186
为惯性权重；/>
Figure SMS_188
为学习因子；
Figure SMS_181
为0~1之间的随机数；
为了更合理地改变惯性权重得到更优的结果。本发明对惯性权重的改进如下：
Figure SMS_189
其中，u start 和u end 分别为惯性权重的初值和末值，其取值范围是：0.4≤u end ≤u start ≤0.9；
Figure SMS_190
为迭代次数的最大值；
Figure SMS_191
重复/>
Figure SMS_192
和/>
Figure SMS_193
，更新最优粒子，直至满足迭代次数或精度要求，产生最优粒子。
步骤4.在上述滤波状态更新和噪声矩阵修正的基础上，进行滤波量测更新：
Figure SMS_194
其中，
Figure SMS_195
表示k时刻的状态向量，/>
Figure SMS_196
表示k时刻的量测向量，/>
Figure SMS_197
表示k-1时刻的状态估计权重；/>
Figure SMS_198
只是为了表述方便而定义的中间变量，不代表任何意义，具体表述如下：
Figure SMS_199
Figure SMS_200
Figure SMS_201
权值矩阵中各元素的取值为：/>
Figure SMS_202
其中，
Figure SMS_203
表示符号函数；/>
Figure SMS_204
表示因子；
均方误差阵更新为：
Figure SMS_205
其中，
Figure SMS_206
表示15行15列的单位矩阵；/>
Figure SMS_207
表示k时刻的滤波增益，/>
Figure SMS_208
表示k时刻的协方差矩阵，/>
Figure SMS_209
表示k时刻的量测转移矩阵，/>
Figure SMS_210
表示k时刻的量测向量矩阵中的第i个元素，/>
Figure SMS_211
表示k时刻矩阵A中的第i个元素，/>
Figure SMS_212
表示k时刻的量测转移矩阵。
仿真参数设置如下：
陀螺仪零偏误差
Figure SMS_213
；加速度计零偏误差100ug；捷联惯导***数据输出频率200Hz；DVL刻度因子误差0.003；DVL输出频率2Hz。其中图3给出了三种方法对应的SINS/DVL组合导航东向位置误差；图4给出了三种方法对应的SINS/DVL组合导航北向位置误差曲线；从图3和图4中可以看出本发明方法导航精度优于卡尔曼滤波方法和Huber鲁棒滤波方法。
应理解这些实施例仅用于说明本发明而不用于限制本发明的范围，在阅读了本发明之后，本领域普通技术人员对本发明的各种等价形式的修改均落于本申请所附权利要求所限定的范围。

Claims (5)

1.一种基于粒子群优化鲁棒滤波的SINS/DVL导航方法，SINS/DVL导航的已知量：
多普勒计程仪，缩写为DVL，输出的载体系下的三维速度信息
Figure QLYQS_1
，其中，/>
Figure QLYQS_2
表示载体坐标系下x方向的速度信息，/>
Figure QLYQS_3
表示载体坐标系下y方向的速度信息，
Figure QLYQS_4
表示载体坐标系下z方向的速度信息；
捷联惯导***，缩写为SINS，输出的导航坐标系下的三维速度信息：
Figure QLYQS_5
，其中，/>
Figure QLYQS_6
表示东向速度，/>
Figure QLYQS_7
表示北向速度，/>
Figure QLYQS_8
表示天向速度；
其特征在于，该方法包括如下步骤：
步骤1.基于状态变换原理构建的组合导航模型构建；
步骤2. 在步骤1的基础上进行滤波状态更新及***噪声方差阵Q k 和量测噪声方差阵S k 的构造；
步骤3. 引入自适应粒子滤波算法，对***噪声方差阵Q k 和量测噪声方差阵S k 进行修正，此过程分为两步，粒子滤波适应度函数构造和自适应粒子群优化算法；
步骤4.进行滤波量测更新。
2.根据权利要求1所述的基于粒子群优化鲁棒滤波的SINS/DVL导航方法，其特征在于，步骤1具体包括如下步骤：
首先，基于状态变换原理建立SINS/DVL组合导航状态方程：
Figure QLYQS_9
其中，
Figure QLYQS_10
表示组合导航状态向量，/>
Figure QLYQS_11
表示组合导航状态转移矩阵；/>
Figure QLYQS_12
表示组合导航***噪声；/>
Figure QLYQS_13
表示15维的组合导航状态向量，可表示如下：
Figure QLYQS_14
其中，
Figure QLYQS_15
分别表示东向速度误差、北向速度误差和天向速度误差；
Figure QLYQS_16
分别表示x方向姿态误差、y方向姿态误差和z方向姿态误差；/>
Figure QLYQS_17
分别表示经度误差、纬度误差和高度误差；/>
Figure QLYQS_18
分别表示x方向加速度计零偏误差、y方向加速度计零偏误差和z方向加速度计零偏误差；/>
Figure QLYQS_19
分别表示x方向陀螺仪零偏误差、y方向陀螺仪零偏误差和z方向陀螺仪零偏误差；
组合导航状态转移矩阵
Figure QLYQS_20
由四部分组成，具体如下：
Figure QLYQS_21
其中，
Figure QLYQS_22
表示速度误差对应的状态转移子矩阵；/>
Figure QLYQS_23
表示姿态误差对应的状态转移子矩阵；/>
Figure QLYQS_24
表示位置误差对应的状态转移子矩阵；/>
Figure QLYQS_25
表示一个6行15列的零矩阵，其中，/>
Figure QLYQS_26
表述如下：
Figure QLYQS_27
其中，
Figure QLYQS_28
表示地球系相对于惯性系的转动角速度；/>
Figure QLYQS_29
表示导航系相对于地球系的旋转角速度；/>
Figure QLYQS_30
表示导航坐标系下地球重力信息；/>
Figure QLYQS_31
表示载体坐标系到导航坐标系的姿态转移矩阵；符号/>
Figure QLYQS_32
表示求矩阵的反对称矩阵；
Figure QLYQS_33
表述如下：
Figure QLYQS_34
其中，
Figure QLYQS_35
只是为了表述方便而定义的中间变量，不代表任何意义，且，
Figure QLYQS_36
的表达式分别为：
Figure QLYQS_37
Figure QLYQS_38
Figure QLYQS_39
Figure QLYQS_40
表示地球旋转角速率；L和h分别表示载体所在位置的地理纬度和高度；R M 和R N 分别表示载体所在位置的子午圈半径和卯酉圈半径；
Figure QLYQS_41
表述如下：
Figure QLYQS_42
其中，
Figure QLYQS_43
只是为了表述方便而定义的中间变量，不代表任何意义，且/>
Figure QLYQS_44
的表达式分别为：/>
Figure QLYQS_45
Figure QLYQS_46
接着，给出SINS/DVL组合导航***的量测方程：
Figure QLYQS_47
其中，
Figure QLYQS_48
表示量测转移矩阵；/>
Figure QLYQS_49
表示量测噪声矩阵；/>
Figure QLYQS_50
表示量测矩阵，可表示如下：
Figure QLYQS_51
其中，
Figure QLYQS_52
表示导航系到载体系的姿态转移矩阵；/>
Figure QLYQS_53
表示DVL输出的载体系下的三维速度信息，量测转移矩阵/>
Figure QLYQS_54
可表示如下：
Figure QLYQS_55
。
3.根据权利要求2所述的基于粒子群优化鲁棒滤波的SINS/DVL导航方法，其特征在于，步骤2具体包括如下步骤：
进行滤波状态更新如下：
Figure QLYQS_56
其中，
Figure QLYQS_57
表示k时刻的组合导航状态转移矩阵；/>
Figure QLYQS_58
表示k-1时刻的组合导航状态向量估计值，/>
Figure QLYQS_59
表示一步预测状态向量；
状态向量均方误差阵更新如下：
Figure QLYQS_60
其中，
Figure QLYQS_61
表示状态向量的均方误差阵；/>
Figure QLYQS_62
表示k时刻的***噪声方差阵，/>
Figure QLYQS_63
表示一步预测协方差矩阵，上标T表示矩阵的转置；
基于Huber M估计原理，构造量测噪声方差阵
Figure QLYQS_64
：
Figure QLYQS_65
其中，
Figure QLYQS_66
表示量测噪声方差阵。
4.根据权利要求3所述的基于粒子群优化鲁棒滤波的SINS/DVL导航方法，其特征在于，步骤3具体包括如下步骤：
首先适应度函数构造
将***噪声方差阵Q k 和量测噪声方差阵S k 作为粒子，而SINS/DVL组合导航***性能主要取决于位置精度，所以将滤波器输出的位置误差的均方差作为适应度函数：
Figure QLYQS_67
其中，f（Q k , S k ）是Q k , S k 为参数时SINS/DVL组合导航***位置误差的均方差；
Figure QLYQS_68
为估计的位置值；M 为估计长度；/>
Figure QLYQS_69
为参考的实际位置值；
然后进行自适应粒子群优化，具体包括如下过程：
Figure QLYQS_70
初始化粒子群参数，在参数区间内初始化粒子群的位置和速度，随机产生若干粒子并初始化最优粒子；
Figure QLYQS_71
根据上述适应度函数，对每一个粒子的适应度进行评价，比较其与最优粒子的适应度值，若适应度更好则取代最优粒子；
Figure QLYQS_72
粒子t=（Q k ,S k ）迭代更新自己的速度和位置，每个粒子根据自身和群体经验调整轨迹向最优点靠拢，按以下方程式进行更新：
Figure QLYQS_73
Figure QLYQS_74
式中，G为当前迭代次数；
Figure QLYQS_76
为粒子t在第G代的位置，/>
Figure QLYQS_79
为粒子t在第G+1代的位置，/>
Figure QLYQS_81
为粒子t在第G+1代的速度；/>
Figure QLYQS_77
为粒子t第G代为止搜索到的最好位置；/>
Figure QLYQS_78
为整个粒子群第G代为止搜索到的最好位置，/>
Figure QLYQS_80
为惯性权重；/>
Figure QLYQS_82
为学习因子；/>
Figure QLYQS_75
为0~1之间的随机数；
对惯性权重的改进如下：
Figure QLYQS_83
其中，u start 和u end 分别为惯性权重的初值和末值，其取值范围是：0.4≤u end ≤u start ≤0.9；
Figure QLYQS_84
为迭代次数的最大值；
Figure QLYQS_85
重复/>
Figure QLYQS_86
和/>
Figure QLYQS_87
，更新最优粒子，直至满足迭代次数或精度要求，产生最优粒子。
5.根据权利要求4所述的基于粒子群优化鲁棒滤波的SINS/DVL导航方法，其特征在于，步骤4具体包括如下步骤：
进行滤波量测更新：
Figure QLYQS_88
其中，
Figure QLYQS_89
表示k时刻的状态向量，/>
Figure QLYQS_90
表示k时刻的量测向量，/>
Figure QLYQS_91
表示k-1时刻的状态估计权重；/>
Figure QLYQS_92
只是为了表述方便而定义的中间变量，不代表任何意义，具体表述如下：
Figure QLYQS_93
Figure QLYQS_94
Figure QLYQS_95
权值矩阵中各元素的取值为：
Figure QLYQS_96
其中，
Figure QLYQS_97
表示符号函数；/>
Figure QLYQS_98
表示因子；
均方误差阵更新为：
Figure QLYQS_99
其中，
Figure QLYQS_100
表示15行15列的单位矩阵；/>
Figure QLYQS_101
表示k时刻的滤波增益，/>
Figure QLYQS_102
表示k时刻的协方差矩阵，/>
Figure QLYQS_103
表示k时刻的量测转移矩阵，/>
Figure QLYQS_104
表示k时刻的量测向量矩阵中的第i个元素，
Figure QLYQS_105
表示k时刻矩阵A中的第i个元素，/>
Figure QLYQS_106
表示k时刻的量测转移矩阵。/>
CN202310069689.2A 2023-02-07 2023-02-07 基于粒子群优化鲁棒滤波的sins/dvl导航方法 Active CN115855049B (zh)

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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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紧组合导航方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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