CN116295414A - 基于鲸鱼算法优化的AUV-UFastSLAM算法 - Google Patents
基于鲸鱼算法优化的AUV-UFastSLAM算法 Download PDFInfo
- Publication number
- CN116295414A CN116295414A CN202310206258.6A CN202310206258A CN116295414A CN 116295414 A CN116295414 A CN 116295414A CN 202310206258 A CN202310206258 A CN 202310206258A CN 116295414 A CN116295414 A CN 116295414A
- Authority
- CN
- China
- Prior art keywords
- particles
- algorithm
- auv
- particle
- whale
- 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.)
- Pending
Links
- 241000283153 Cetacea Species 0.000 title claims abstract description 56
- 238000005457 optimization Methods 0.000 title claims abstract description 11
- 239000002245 particle Substances 0.000 claims abstract description 188
- 238000000034 method Methods 0.000 claims abstract description 55
- 238000012952 Resampling Methods 0.000 claims abstract description 32
- 238000001914 filtration Methods 0.000 claims abstract description 17
- 238000005070 sampling Methods 0.000 claims abstract description 17
- 238000005315 distribution function Methods 0.000 claims description 17
- 238000012216 screening Methods 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 5
- 230000007547 defect Effects 0.000 claims description 5
- 230000010076 replication Effects 0.000 claims description 5
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims description 3
- 238000010606 normalization Methods 0.000 claims description 3
- 238000013459 approach Methods 0.000 claims description 2
- 230000003044 adaptive effect Effects 0.000 claims 1
- 230000004927 fusion Effects 0.000 claims 1
- 230000001360 synchronised effect Effects 0.000 abstract description 3
- 230000015556 catabolic process Effects 0.000 description 3
- 238000006731 degradation reaction Methods 0.000 description 3
- 230000003247 decreasing effect Effects 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
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/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
- G06N3/00—Computing arrangements based on biological models
- G06N3/004—Artificial life, i.e. computing arrangements simulating life
- G06N3/006—Artificial life, i.e. computing arrangements simulating life based on simulated virtual individual or collective life forms, e.g. social simulations or particle swarm optimisation [PSO]
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Theoretical Computer Science (AREA)
- Biophysics (AREA)
- General Health & Medical Sciences (AREA)
- Biomedical Technology (AREA)
- Life Sciences & Earth Sciences (AREA)
- Computational Linguistics (AREA)
- Data Mining & Analysis (AREA)
- Evolutionary Computation (AREA)
- Artificial Intelligence (AREA)
- Molecular Biology (AREA)
- Computing Systems (AREA)
- General Engineering & Computer Science (AREA)
- Mathematical Physics (AREA)
- Software Systems (AREA)
- Health & Medical Sciences (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种基于鲸鱼算法优化的AUV‑UFastSLAM算法，包括：初始化，预测，采样,地图更新，重采样等一系列过程。该方法采用鲸鱼算法对粒子采样过程进行优化，实现了粒子集合向高似然区域移动，使AUV的位姿估计更接近真实值。引入惯性权重因子对鲸鱼算法位置更新公式进行了改进，提高种群收敛速度和精度；同时采用柯西变异对最优邻域进行随机扰动，来增加种群多样性，提高算法的全局搜索能力；对粒子滤波采用改进的重采样方法，保证了粒子的多样性。通过上述调整，实现了AUV同步定位与地图创建精度的提高。
Description
技术领域
本发明涉及自主水下机器人(AUV)同步定位与地图创建领域，具体来说是一种对基于鲸鱼算法优化的AUV-UFastSLAM算法，属于机器人导航技术领域。
背景技术
移动机器人在自身位置和姿态都不确定的条件下，利用自身的传感装置对未知环境进行地图创建，同时利用创建的地图进行定位的方法被称为同时定位与地图创建(simultaneous localization and mapping,SLAM)问题。
对自主水下机器人(AUV)自主导航的研究对AUV的发展有着重要意义。目前的大多数自主导航方式能确保移动机器人在已知环境中运动并执行任务，但在实际环境中，特别是水下环境，很少会存在地图信息已知的情况，因此要实现完全自主导航就必须使机器人具备同时定位与地图创建的能力。
自从Smith等人提出基于扩展卡尔曼滤波器(EKF)的同时定与地图创建算法后，扩展卡尔曼滤波器成为研究机器人SLAM问题的主要方法。但随着深入的研究，人们发现EKF方法在实际的应用中存在两个明显缺陷：一是计算复杂度与环境中已检测的路标数目成平方比，使得这种高计算复杂度难以满足大规模地图创建和实时性要求；二是EKF算法在数据关联问题处理方面能力不足。
针对扩展卡尔曼滤波器所存在的问题，Montemelo等人提出了FastSLAM算法，该算法将SLAM问题分成了机器人的位姿估计和基于位姿估计的路标位置估计两个过程。对于机器人位姿估计使用粒子滤波器，对于路标位置估计使用扩展卡尔曼滤波器。在位姿估计中，每个粒子代表机器人的一条可能轨迹，每一个粒子的权重代表了其对应的可能轨迹的好坏；扩展卡尔曼滤波器对每个粒子做路标位置估计，所以每个粒子的路标位置估计都是相互独立的。所以在FastSLAM算法中，每个粒子的数据关联也是相互独立的，当其中部分粒子出现问题时，会在重采样阶段衰退掉，从而不会影响机器人SLAM算法的整体估计，解决了EKF在SLAM算法中存在的两大缺陷。
但是FastSLAM自身仍然存在缺点：在一个标准FastSLAM过程中，粒子重采样过程会使具有较大权重的粒子被多次选中，导致样本的多样性下降，出现粒子贫乏的问题，最终可能导致粒子滤波精度的降低，进而使得机器人位姿估计的精度下降，甚至出现地图丢失的情况。
发明内容
本发明的目的在于提供一种基于鲸鱼算法优化的AUV-UFastSLAM算法，采用鲸鱼算法对AUV的UFastSLAM算法中的采样过程进行改进，通过对鲸鱼算法迭代过程中的位置更新公式和搜索策略的改进，在保证算法局部寻优能力的同时，扩大搜索范围，并且对粒子滤波中重采样方法进行了改进，通过这些改进解决UFastSLAM算法存在的粒子退化和多样性丧失的缺点，提高AUV自主导航的精度。
本发明的目的通过以下技术方案予以实现：
一种基于鲸鱼算法优化的AUV-UFastSLAM算法，该方法包含下列步骤：
(1)初始化：获取AUV初始位姿信息；
(2)预测：根据AUV的运动预测方程传感器观测方程，计算出t时刻粒子的状态值xt和观测值zt，采用无迹粒子滤波对AUV的位姿和路标进行预测；
(3)采样：采用无迹卡尔曼滤波算法产生建议分布函数，并从建议分布函数中采样N个粒子，对各粒子计算权重，对生成的N个粒子采用改进后的鲸鱼算法优化采样过程，在算法迭代过程中，引导粒子不断向真实系统状态的后验概率分布逼近，使粒子分布更接近实际AUV位姿的后验概率分布，迭代结束后，对N个粒子的权值进行更新，并归一化；
(4)地图更新：根据采样优化后输出的粒子状态关联的环境观测信息，采用无迹卡尔曼滤波对路标位置进行估计，更新当前时刻地图信息；
(5)重采样：计算粒子集的有效粒子数，当有效粒子数少于设定阈值时，采用改进的重采样方法对粒子集进行重采样；
(6)按照以上步骤迭代，进行AUV的同时定位与地图构建。
进一步地，本发明的目的还可以通过一下技术措施实现：
前述基于鲸鱼算法优化的AUV-UFastSLAM算法，其中步骤(1)初始化过程包括通过全球定位系统GPS或者超短基线USBL二者任一种或两种获得AUV初始位置信息，其中全球定位系统GPS获得AUV的经度信息和纬度信息，超短基线USBL获得AUV相对于超短基线发射站的位置，可单一使用一种，或二者融合可提高AUV初始位置估计的精度；通过多普勒计程仪DVL、陀螺仪获取初始速度和姿态信息；多普勒计程仪DVL采集AUV的三轴速度信息中的任一种或几种；陀螺仪采集AUV的航偏角信息、横滚角信息、俯仰角信息中的任一种或几种。
前述基于鲸鱼算法优化的AUV-UFastSLAM算法，其中步骤(2)根据AUV的运动预测方程传感器观测方程，计算出t时刻粒子的状态值xt和观测值zt，采用无迹粒子滤波对AUV的位姿和路标进行预测，所述AUV运动预测方程和传感器观测方程分别为：
xt＝f(xt-1,ut,ωt) (1)
zt＝h(xt,vt) (2)
式中，xt、zt分别为t时刻系统的状态向量和观测向量，xt-1为t-1时刻系统的状态向量，ut为t时刻的运动控制，f(·)、h(·)分别为状态和观测的非线性函数，wt为满足均值为零，协方差为Qt的过程噪声，vt为满足均值为零，协方差为Rt的观测噪声。
前述基于鲸鱼算法优化的AUV-UFastSLAM算法，步骤(3)具体包括：
步骤(3.1)：采用无迹卡尔曼滤波算法产生建议分布函数，并从建议分布函数中采样N个粒子，并对各粒子计算权重，并将当前时刻权重最大的粒子作为全局最优值X*(t)；
步骤(3.2)：在鲸鱼算法迭代过程中，存在鲸鱼个体优于当前最优值时，才会对当前的最优值进行更新，这样会使最优值的更新次数较少，从而影响算法的搜索效率。引入柯西变异对鲸鱼算法的搜索策略进行改进，来增加种群多样性，提高算法的全局搜索能力，标准柯西分布函数公式如式(3)所示：
在得到最优解后，最优邻域随机扰动公式如式(4)所示：
其中，r为[0,1]之间的随机数，
为新生成的邻域位置，当r＜0.5时，使用公式(4)对X*(t)邻域随机扰动；当r≥0.5时，新生成的邻域位置仍为最优鲸鱼位置，当新生成的邻域位置适应度/>
高于原最优值适应度w(X*(t))时，则对X*(t)进行更新。更新过程如式(5)所示：
步骤(3.3)：针对鲸鱼算法前期最优鲸鱼位置对其他个体影响弱，在迭代后期容易陷入局部最优的缺点，在鲸鱼位置更新过程中引入惯性权重因子wt，对位置更新公式进行改进，调节算法的全局搜索和局部寻优能力。自适应权重公式如式(6)所示，改进后的位置更新公式如式(7)所示：
其中，wt为惯性权重因子，t为当前迭代次数，Tmax为最大迭代次数,w1为最初权重，w2为最终权重，且w1>w2；X*(t)为t时刻最优位置鲸鱼状态，X(t+1)为t+1时刻其余鲸鱼位置更新后的状态；结合式(6)中e的负次方在(-∞,+∞)上单调递减的变化规律，算法前期wt会有较大值，会使算法以较快速度到达最优值附近，提升鲸鱼算法前期的全局搜索能力；随着迭代次数增加，wt的值逐渐减小，加强了算法后期的局部寻优能力，使得算法在迭代中越来越接近最优值，进而提高种群收敛速度和精度；
步骤(3.4)：判断判断是否达到最大迭代次数Tmax，若未达到，转到步骤(3.2)；
步骤(3.5)：更新粒子权重并归一化，输出粒子状态。
前述基于鲸鱼算法优化的AUV-UFastSLAM算法，步骤(5)具体包括：
步骤(5.1)：进行重采样判定，设定有效粒子数阈值Nth,当有效粒子数Neff小于有效粒子数阈值Nth时，执行重采样，否则不进行，有效粒子数计算如式(8)所示：
其中，
为归一化处理后的第i个粒子权重；
步骤(5.2)：执行重采样时，首先根据有效粒子数设定两个门限值wl和wh作为粒子的筛选门限值，wl和wh的取值在0和1之间且wl<wh，将该时刻的N个粒子根据权值分为三个区间，其中权值满足
的粒子为权值适中的粒子，不进行重采样，仅对于/>
和/>
的粒子进行重采样；
步骤(5.3)：采用公式(9)求N个粒子均值，并筛选出L个权值满足
示
的粒子，设定粒子数目门限值Np，当L≥Np时，说明粒子权重过大或过小的粒子较多，需要将少数大权重粒子多次复制，当L＜Np时，说明粒子权重分布较均匀，即权重过大或过小的粒子较少，该情况，需要尽可能的对更多大权重粒子进行复制，具体复制公式如式(10)所示：
其中，Mc为粒子复制次数，wmeans为N个粒子的权值平均值，
为对/>
的结果向上取整，/>
为对/>
的结果向下取整，复制过程仅针对/>
和/>
的粒子，按粒子权值从大到小进行，当复制粒子数达到L个时停止，将新复制的L个粒子与未进行重采样的N-L个粒子组合成新的粒子集，增加粒子的多样性，若复制过程结束后，复制粒子数不足L个，则从/>
随机复制粒子使粒子数达到L个；
步骤(5.4)：将新生成的N个粒子的权值重置为
与现有技术相比，本发明的有益效果是：
1、本发明采用的Unscented FastSLAM算法相对于其他SLAM算法而言，对AUV自主导航的鲁棒性和准确性更好；
2、本发明采用改进的鲸鱼算法优化UFastSLAM的采样过程，通过改进的搜索策略，增加了种群多样性，提高算法的全局搜索能力，通过改进的位置更新公式，实现了全局搜索能力和局部寻优能力的平衡，通过上述调整，实现了粒子集向高似然区域集中，解决了粒子退化和粒子多样性丧失的问题，提升了AUV的自主导航精度；
3、本发明采用改进的重采样方式，减少了重采样的时间，增加了粒子的多样性。
附图说明
图1是本发明的基于鲸鱼优化的AUV-UFastSLAM算法流程图；
图2是本发明的改进鲸鱼算法流程图；
图3是本发明的改进重采样流程图。
具体实施方式
下面结合附图和具体实施例对本发明作进一步说明。
如图1所示，是采用基于改进鲸鱼算法优化的应用于AUV的Unscented FastSLAM算法流程图，描述了改进的AUV-UFastSLAM的处理过程。具体步骤如下：
步骤一：AUV通过全球定位系统GPS或者超短基线USBL获得初始位置信息；通过多普勒计程仪DVL和陀螺仪获得AUV的初始姿态信息。
步骤二：根据AUV的运动预测方程传感器观测方程，计算出t时刻粒子的状态值xt和观测值zt，采用无迹粒子滤波UPF对AUV的位姿和路标进行预测。所述AUV运动预测方程和传感器观测方程分别为：
xt＝f(xt-1，ut，ωt) (1)
zt＝h(xt，vt) (2)
式中，xt、zt分别为t时刻系统的状态向量和观测向量，xt-1为t-1时刻系统的状态向量，ut为t时刻的运动控制，f(·)、h(·)分别为状态和观测的非线性函数，wt为满足均值为零，协方差为Qt的过程噪声，vt为满足均值为零，协方差为Rt的观测噪声。
步骤三：如图2所示，采用无迹卡尔曼滤波产生建议分布函数，并从建议分布函数中采样N个粒子，对各粒子计算权重，对生成的N个粒子采用改进后的鲸鱼算法优化采样过程，在算法迭代过程中，引导粒子不断向真实系统状态的后验概率分布逼近，使粒子分布更接近实际AUV位姿的后验概率分布，迭代结束后，对N个粒子的权值进行更新，并归一化，具体包含以下步骤：
步骤(3.1)：采用无迹卡尔曼滤波算法产生建议分布函数，并从建议分布函数中采样N个粒子，并对各粒子计算权重，并将当前时刻权重最大的粒子作为全局最优值X*(t)；
步骤(3.2)：在鲸鱼算法迭代过程中，存在鲸鱼个体优于当前最优值时，才会对当前的最优值进行更新，这样会使最优值的更新次数较少，从而影响算法的搜索效率。引入柯西变异对鲸鱼算法的搜索策略进行改进，来增加种群多样性，提高算法的全局搜索能力，标准柯西分布函数公式如式(3)所示：
在得到最优解后，最优邻域随机扰动公式如式(4)所示：
其中，r为[0，1]之间的随机数，
为新生成的邻域位置，当r＜0.5时，使用公式(4)对X*(t)邻域随机扰动；当r≥0.5时，新生成的邻域位置仍为最优鲸鱼位置，当新生成的邻域位置适应度/>
高于原最优值适应度w(X*(t))时，则对X*(t)进行更新。更新过程如式(5)所示：
步骤(3.3)：针对鲸鱼算法前期最优鲸鱼位置对其他个体影响弱，在迭代后期容易陷入局部最优的缺点，在鲸鱼位置更新过程中引入惯性权重因子wt，对位置更新公式进行改进，调节算法的全局搜索和局部寻优能力。自适应权重公式如式(6)所示，改进后的位置更新公式如式(7)所示：
其中，wt为惯性权重因子，t为当前迭代次数，Tmax为最大迭代次数，w1为最初权重，w2为最终权重，且w1＞w2；X*(t)为t时刻最优位置鲸鱼状态，X(t+1)为t+1时刻其余鲸鱼位置更新后的状态；结合式(6)中e的负次方在(-∞，+∞)上单调递减的变化规律，算法前期wt会有较大值，会使算法以较快速度到达最优值附近，提升鲸鱼算法前期的全局搜索能力；随着迭代次数增加，wt的值逐渐减小，加强了算法后期的局部寻优能力，使得算法在迭代中越来越接近最优值，进而提高种群收敛速度和精度；
步骤(3.4)：判断判断是否达到最大迭代次数Tmax，若未达到，转到步骤(3.2)；
步骤(3.5)：更新粒子权重并归一化，输出粒子状态。
步骤四：根据每个粒子关联的环境观测信息，采用无迹卡尔曼滤波对路标位置进行估计，更新当前时刻地图信息。
步骤五：如图3所示，计算粒子集的有效粒子数，当有效粒子数少于设定阈值时，采用改进的重采样方法对粒子集进行重采样，改进的重采样方法包含以下步骤：
步骤(5.1)：进行重采样判定，设定有效粒子数阈值Nth，当有效粒子数Neff小于有效粒子数阈值Nth时，执行重采样，否则不进行，有效粒子数计算如式(8)所示：
其中，
为归一化处理后的第i个粒子权重；
步骤(5.2)：执行重采样时，首先根据有效粒子数设定两个门限值wl和wh作为粒子的筛选门限值，wl和wh的取值在0和1之间且wl＜wh，将该时刻的N个粒子根据权值分为三个区间，其中权值满足
的粒子为权值适中的粒子，不进行重采样，仅对于
和/>
的粒子进行重采样；
步骤(5.3)：采用公式(9)求N个粒子均值，并筛选出L个权值满足
和
的粒子，设定粒子数目门限值Np，当L≥Np时，说明粒子权重过大或过小的粒子较多，需要将少数大权重粒子多次复制，当L＜Np时，说明粒子权重分布较均匀，即权重过大或过小的粒子较少，该情况，需要尽可能的对更多大权重粒子进行复制，具体复制公式如式(10)所示：
其中，Mc为粒子复制次数，wmeans为N个粒子的权值平均值，
为对/>
的结果向上取整，/>
为对/>
的结果向下取整，复制过程仅针对/>
和/>
的粒子，按粒子权值从大到小进行，当复制粒子数达到L个时停止，将新复制的L个粒子与未进行重采样的N-L个粒子组合成新的粒子集，增加粒子的多样性，若复制过程结束后，复制粒子数不足L个，则从/>
随机复制粒子使粒子数达到L个；
步骤(5.4)：将新生成的N个粒子的权值重置为
步骤六：按照以上步骤迭代，进行AUV的同时定位与地图构建。
本发明对应用于AUV的Unscented FastSLAM算法做出改进，提出了基于鲸鱼算法优化的AUV-UFastSLAM算法，实现了粒子集合向高似然区域移动，使AUV的位姿估计更接近真实值，并解决了粒子退化的问题。采用惯性权重因子对鲸鱼算法位置更新公式进行了改进，提高种群收敛速度和精度；同时采用柯西变异对最优邻域进行随机扰动，来增加种群多样性，提高算法的全局搜索能力；对粒子滤波采用改进的重采样方法，保证了粒子的多样性。通过上述调整，实现了AUV同步定位与地图创建精度的提高。
除上述实施例外，本发明还可以有其他实施方式，凡采用等同替换或等效变换形成的技术方案，均落在本发明要求的保护范围内。
Claims (5)
1.一种基于鲸鱼算法优化的AUV-UFastSLAM算法，其特征在于，该方法包含下列步骤：
(1)初始化：获取AUV初始位姿信息；
(2)预测：根据AUV的运动预测方程传感器观测方程，计算出t时刻粒子的状态值xt和观测值zt，采用无迹粒子滤波对AUV的位姿和路标进行预测；
(3)采样：采用无迹卡尔曼滤波算法产生建议分布函数，并从建议分布函数中采样N个粒子，对各粒子计算权重，对生成的N个粒子采用改进后的鲸鱼算法优化采样过程，在算法迭代过程中，引导粒子不断向真实系统状态的后验概率分布逼近，使粒子分布更接近实际AUV位姿的后验概率分布，迭代结束后，对N个粒子的权值进行更新，并归一化；
(4)地图更新：根据采样优化后输出的粒子状态关联的环境观测信息，采用无迹卡尔曼滤波对路标位置进行估计，更新当前时刻地图信息；
(5)重采样：计算粒子集的有效粒子数，当有效粒子数少于设定阈值时，采用改进的重采样方法对粒子集进行重采样；
(6)按照以上步骤迭代，进行AUV的同时定位与地图构建。
2.如权利要求1所述的基于鲸鱼算法优化的AUV-UFastSLAM算法，其特征在于，所述步骤(1)初始化过程通过全球定位系统GPS或者超短基线USBL二者任一种或两种获得AUV初始位置信息，其中全球定位系统GPS获得AUV的经度信息和纬度信息，超短基线USBL获得AUV相对于超短基线发射站的位置，可单一使用一种，或二者融合可提高AUV初始位置估计的精度；通过多普勒计程仪DVL、陀螺仪获取初始速度和姿态信息；多普勒计程仪DVL采集AUV的三轴速度信息中的任一种或几种；陀螺仪采集AUV的航偏角信息、横滚角信息、俯仰角信息中的任一种或几种。
3.如权利要求1所述的基于鲸鱼算法优化的AUV-UFastSLAM算法，其特征在于，所述步骤(2)根据AUV的运动预测方程传感器观测方程，计算出t时刻粒子的状态值xt和观测值zt，采用无迹粒子滤波对AUV的位姿和路标进行预测，所述AUV运动预测方程和传感器观测方程分别为：
xt＝f(xt-1，ut，wt) (1)
zt＝h(xt，vt) (2)
式中，xt、zt分别为t时刻系统的状态向量和观测向量，xt-1为t-1时刻系统的状态向量，ut为t时刻的运动控制，f(·)、h(·)分别为状态和观测的非线性函数，wt为满足均值为零，协方差为Qt的过程噪声，vt为满足均值为零，协方差为Rt的观测噪声。
4.如权利要求1所述的基于鲸鱼算法优化的AUV-UFastSLAM算法，其特征在于，所述步骤(3)采用无迹卡尔曼滤波算法产生建议分布函数，并从建议分布函数中采样N个粒子，对各粒子计算权重，对生成的N个粒子采用改进后的鲸鱼算法优化采样过程包括以下步骤：
步骤(3.1)：采用无迹卡尔曼滤波算法产生建议分布函数，并从建议分布函数中采样N个粒子，并对各粒子计算权重，将当前时刻权重最大的粒子作为全局最优值X*(t)；
步骤(3.2)：在鲸鱼算法迭代过程中，存在鲸鱼个体优于当前最优值时，才会对当前的最优值进行更新，这样会使最优值的更新次数较少，从而影响算法的搜索效率，引入柯西变异对鲸鱼算法的搜索策略进行改进，来增加种群多样性，提高算法的全局搜索能力，标准柯西分布函数公式如式(3)所示：
在得到最优解后，最优邻域随机扰动公式如式(4)所示：
其中，r为[0，1]之间的随机数，
为新生成的邻域位置，当r＜0.5时，使用公式(4)对X*(t)邻域随机扰动；当r≥0.5时，新生成的邻域位置仍为最优鲸鱼位置，当新生成的邻域位置适应度/>
高于原最优值适应度w(X*(t))时，则对X*(t)进行更新，更新过程如式(5)所示：
步骤(3.3)：针对鲸鱼算法前期最优鲸鱼位置对其他个体影响弱，在迭代后期容易陷入局部最优的缺点，在鲸鱼位置更新过程中引入惯性权重因子wt，对位置更新公式进行改进，调节算法的全局搜索和局部寻优能力，自适应权重公式如式(6)所示，改进后的位置更新公式如式(7)所示：
其中，wt为惯性权重因子，t为当前迭代次数，Tmax为最大迭代次数，w1为最初权重，w2为最终权重，且w1＞w2；X*(t)为t时刻最优位置鲸鱼状态，X(t+1)为t+1时刻其余鲸鱼位置更新后的状态；结合式(6)中e的负次方在(-∞，+∞)上单调递减的变化规律，算法前期wt会有较大值，会使算法以较快速度到达最优值附近，提升鲸鱼算法前期的全局搜索能力；随着迭代次数增加，wt的值逐渐减小，加强了算法后期的局部寻优能力，使得算法在迭代中越来越接近最优值，进而提高种群收敛速度和精度；
步骤(3.4)：判断判断是否达到最大迭代次数Tmax，若未达到，转到步骤(3.2)；
步骤(3.5)：更新粒子权重并归一化，输出粒子状态。
5.如权利要求1所述的基于鲸鱼算法优化的AUV-UFastSLAM算法，其特征在于，所述步骤(5)计算粒子集的有效粒子数，当有效粒子数少于设定阈值时，采用改进的重采样方法对粒子集进行重采样包括以下步骤：
步骤(5.1)：进行重采样判定，设定有效粒子数阈值Nth，当有效粒子数Neff小于有效粒子数阈值Nth时，执行重采样，否则不进行，有效粒子数计算如式(8)所示：
其中，
为归一化处理后的第i个粒子权重；
步骤(5.2)：执行重采样时，首先根据有效粒子数设定两个门限值wl和wh作为粒子的筛选门限值，wl和wh的取值在0和1之间且wl＜wh，将该时刻的N个粒子根据权值分为三个区间，其中权值满足
的粒子为权值适中的粒子，不进行重采样，仅对于/>
和
的粒子进行重采样；
步骤(5.3)：采用公式(9)求N个粒子均值，并筛选出L个权值满足
和/>
的粒子，设定粒子数目门限值Np，当L≥Np时，说明粒子权重过大或过小的粒子较多，需要将少数大权重粒子多次复制，当L＜Np时，说明粒子权重分布较均匀，即权重过大或过小的粒子较少，该情况，需要尽可能的对更多大权重粒子进行复制，具体复制公式如式(10)所示：
其中，Mc为粒子复制次数，wmeans为N个粒子的权值平均值，
为对/>
的结果向上取整，/>
为对/>
的结果向下取整，复制过程仅针对/>
和/>
的粒子，按粒子权值从大到小进行，当复制粒子数达到L个时停止，将新复制的L个粒子与未进行重采样的N-L个粒子组合成新的粒子集，增加粒子的多样性，若复制过程结束后，复制粒子数不足L个，则从/>
随机复制粒子使粒子数达到L个；
步骤(5.4)：将新生成的N个粒子的权值重置为
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310206258.6A CN116295414A (zh) | 2023-03-06 | 2023-03-06 | 基于鲸鱼算法优化的AUV-UFastSLAM算法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310206258.6A CN116295414A (zh) | 2023-03-06 | 2023-03-06 | 基于鲸鱼算法优化的AUV-UFastSLAM算法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116295414A true CN116295414A (zh) | 2023-06-23 |
Family
ID=86791885
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310206258.6A Pending CN116295414A (zh) | 2023-03-06 | 2023-03-06 | 基于鲸鱼算法优化的AUV-UFastSLAM算法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116295414A (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117288209A (zh) * | 2023-11-27 | 2023-12-26 | 南昌工程学院 | 一种变压器声级测定方法及系统 |
-
2023
- 2023-03-06 CN CN202310206258.6A patent/CN116295414A/zh active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117288209A (zh) * | 2023-11-27 | 2023-12-26 | 南昌工程学院 | 一种变压器声级测定方法及系统 |
| CN117288209B (zh) * | 2023-11-27 | 2024-01-30 | 南昌工程学院 | 一种变压器声级测定方法及系统 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| WO2020154970A1 (en) | Deep learning–based feature extraction for lidar localization of autonomous driving vehicles | |
| CN114625151B (zh) | 一种基于强化学习的水下机器人避障路径规划方法 | |
| CN110118560A (zh) | 一种基于lstm和多传感器融合的室内定位方法 | |
| CN116295414A (zh) | 基于鲸鱼算法优化的AUV-UFastSLAM算法 | |
| CN112612290B (zh) | 一种考虑洋流的水下航行器三维多任务路径规划方法 | |
| CN109146922B (zh) | 一种基于自适应粒子群优化的前视声纳水下目标跟踪方法 | |
| CN110726415A (zh) | 一种自适应的水下多波束同步定位与建图方法 | |
| CN111610492A (zh) | 一种多声传感器阵列智能感知方法及系统 | |
| CN108364305A (zh) | 基于改进型dsst的车载摄像视频目标跟踪方法 | |
| CN110598804A (zh) | 一种基于聚类和膜计算的改进FastSLAM算法 | |
| Zhao et al. | Review of slam techniques for autonomous underwater vehicles | |
| CN114648584B (zh) | 一种用于多源融合定位的鲁棒性控制方法和系统 | |
| CN110765897A (zh) | 一种基于粒子滤波的水下目标跟踪方法 | |
| CN111307136B (zh) | 一种双智能水下机器人水下航行地形匹配导航方法 | |
| CN116255988B (zh) | 一种基于舰船动力学组合导航复合抗干扰自适应滤波方法 | |
| Dah-Jing et al. | Neural network aided adaptive Kalman filter for GPS/INS navigation system design | |
| CN113050658B (zh) | 一种基于狮群算法优化的slam算法 | |
| CN112857379B (zh) | 一种基于改进的Gmapping-SLAM地图更新方法及系统 | |
| CN112069997B (zh) | 一种基于DenseHR-Net的无人机自主着陆目标提取方法及装置 | |
| Wang et al. | Research on SLAM road sign observation based on particle filter | |
| CN114660641A (zh) | 一种自适应gps融合定位系统、方法及介质 | |
| CN113702941A (zh) | 一种基于改进icp的点云测速方法 | |
| Hu et al. | A modified particle filter for simultaneous robot localization and landmark tracking in an indoor environment | |
| CN116736219B (zh) | 基于改进粒子群算法的无源tdoa-fdoa联合定位优化布站方法 | |
| Yang et al. | Real-time terrain estimation based on multi-scale radial basis function for unmanned ground vehicle |
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 |