一种基于遗传算法的测深同步定位与建图闭环检测方法

文档序号:37645885发布日期:2024-04-18 18:13阅读:17来源:国知局
一种基于遗传算法的测深同步定位与建图闭环检测方法

本发明属于水下机器人定位与导航,更具体地说,是涉及一种基于遗传算法的测深同步定位与建图闭环检测方法。


背景技术:

1、水下机器人(autonomous underwater vehicle,auv)搭载多波束测深声呐等水声探测设备近海底观测是实现高分辨率、高精度海底地形测绘的重要手段。auv测深同步定位与建图无需多余辅助条件即可为海洋测绘任务提供稳定与准确的导航结果,准确的闭环检测结果是实现auv精确的测深同步定位与建图最重要部分。然而长时间测绘时auv存在潮位变化,且机器人的姿态测量也存在有界误差,这都可能导致误匹配,传统模板匹配方法无法解决该问题;且六自由度内寻优需大量计算资源,无法满足在线测绘与定位的实时性,因此,急需一种高效的闭环检测寻优方法实现auv测深同步定位与建图的准确性与实时性。

2、目前auv的闭环检测方法主要停留在二维的平面寻优检测与普通优化算法的变种上,无法完全满足测深同步定位与建图时所需准确、实时的前端闭环要求。

3、公开日为2019年12月6日,公开号cn107132521a,发明名称为“一种bslam中地形匹配结果正确性判别方法”的专利申请。该方法将地形匹配结果通过多窗口一致性方法进行判别,从而获取准确的前端闭环结果。然而该方法只是错误的闭环进行剔除,而非直接取得准确的闭环结果,且闭环的剔除无法满足在线测绘所需的实时性要求。

4、公开日为2023年5月16日,公开号cn116124151a,发明名称为“无卫星导航下的地形匹配辅助导航方法”的专利申请。该方法通过与高程地形图进行平移、卷积、测高、相乘等操作,得到当前时刻地形匹配的预估位置经纬度、后验标准差、地图高度标准差和置信度。然而该方法是将序列高程值与整块先验地图进行匹配而非auv需将两块子地图进行匹配,且该方法也无需考虑潮差问题与姿态测量误差问题,因此,无法直接应用在auv深海测绘场景下。


技术实现思路

1、本发明的目的在于提供一种基于遗传算法的测深同步定位与建图闭环检测方法,旨在解决现有技术中无法完全满足测深同步定位与建图时所需准确、实时的前端闭环要求的技术问题。

2、为实现上述目的,本发明采用的技术方案是:提供一种基于遗传算法的测深同步定位与建图闭环检测方法,包括:

3、步骤1:设置六自由度闭环匹配对准区间;

4、六自由度闭环匹配对准区间为[x,y,z,θ,ψ];

5、步骤2:在所述六自由度闭环匹配对准区间分布父代种群,所述父代种群

6、在x、y自由度区间上分布,在其他自由度上的值设为0,种群个体为t[dx,dy,0,0,0,0];

7、步骤3:对所述父代种群进行迭代,获取子代与弟代种群,并获取最大适应度值个体;

8、步骤4:取最大适应度值个体为闭环检测结果。

9、可选的,步骤3包括:

10、步骤3.1:计算父代种群的适应度值;

11、步骤3.2:基于父代种群的适应度值对父代种群进行种群划分,得到子代种群;

12、步骤3.3:对子代种群进行种群合并,得到合并后的子代种群;

13、步骤3.4:对合并后的子代种群进行降维策略计算,得到混合弟代种群;

14、步骤3.5:将混合弟代种群和父代种群混合,得到得到新子代种群;

15、步骤3.6:基于新子代种群获取最大适应度值个体。

16、可选的,步骤3.2包括:

17、步骤3.2.1:计算每个种群个体的适应度值和健康多样性值;

18、步骤3.2.2:计算初始父代种群的标准种群多样性值;

19、步骤3.2.3:基于初始父代种群的标准种群多样性值计算交叉概率;

20、步骤3.2.4:基于交叉概率对父代种群进行划分,得到探索子代种群和开发子代种群。

21、可选的,步骤3.2.1包括:

22、基于种群个体适应度计算公式计算每个种群个体的适应度值;

23、基于种群个体健康多样性计算公式计算每个种群个体的健康多样性值;

24、种群个体适应度计算公式为:

25、

26、其中,f(t,m1,m2)为种群个体的适应度值,m1为海底地形图m1对应的地图点对,m2为海底地形图m2转移t后对应的地图点对,t为偏移对准值,c(m1,m2)为点对m1和点对m2的马氏距离,n为地图的点对总个数;

27、种群个体健康多样性计算公式为:

28、

29、其中,hpdi为种群个体的健康多样性值,wi为个体i在种群中的适应度值权重,tδave·w为种群中适应度值权重的平均值,δ是父代种群内个体数,为种群个体i的变量值。

30、可选的,步骤3.2.2包括:

31、基于标准种群多样性计算公式计算父代种群中每个基因变量的标准种群多样性值;

32、对每个基因变量的标准种群多样性值进行归一化后并取均值,得到父代种群的标准种群多样性值;

33、标准种群多样性计算公式为:

34、

35、其中,spdj为父代种群中每个基因变量的标准种群多样性值,δ是父代种群内的个体数,tδave[j]是父代种群中j变量的平均值,tδi[j]是个体i的变量j值;

36、父代种群的标准种群多样性值为:

37、

38、其中,dof为种群变量个数,δ(j)为各变量对应的种群取值范围长度,spdj为父代种群中每个基因变量的标准种群多样性值。

39、可选的,步骤3.2.3包括:

40、交叉概率为:

41、

42、其中,spdmax为种群能达到的最高spd值,k1为[0.2,0.8]区间内的第一随机常数,k2为[0.2,0.8]区间内的第二随机常数。

43、可选的,步骤3.2.4包括:

44、计算探索子代种群的个体的健康多样性值,根据探索子代种群的每个个体的健康多样性值进行锦标赛选择,得到锦标赛选择后的探索子代种群;

45、将锦标赛选择后的探索子代种群进行变异率大的多项式变异,并将多项式变异后的探索子代种群往未到达空间上发展;

46、计算开发子代种群的健康多样性值,根据开发子代种群的每个个体的健康多样性值进行锦标赛选择,得到锦标赛选择后的开发子代种群;

47、根据交叉概率对锦标赛选择后的开发子代种群进行模拟二次交叉,并进行变异率小的多项式变异,将多项式变异后的开发子代种群往当前个体分布区域附近发展。

48、

49、其中,k为常数。

50、可选的,步骤3.4包括:

51、步骤3.4.1:计算合并后的子代种群的个体适应度值;

52、步骤3.4.2:取子代种群中适应度值最高的10%个体;

53、步骤3.4.3:基于个体适应度值获取残差值,计算适应度值最高的10%个体的残值最小对应的高程值dz,并代替子代种群个体,得到替换后的种群个体t′[dx,dy,dz,0,0,0];

54、步骤3.4.4:对替换后的种群个体t′[dx,dy,dz,0,0,0]在剩余三个自由度闭环匹配区间内取n个个体,得到n个新种群个体t″[dx,dy,dz,dθ,dψ];

55、步骤3.4.5:取替换后的种群个体t′和n个新种群个体t″的最大适应度值个体取代种群个体t,直至适应度值最高的10%个体全部替换完成,得到混合弟代种群{t[dx,dy,0,0,0,0],tmax[dx,dy,dz,dθ,dψ]},其中,tmax为种群个体t′和n个新种群个体t″的最大适应度值个体;

56、高程值dz为:

57、

58、其中,n为地图点对个数,m1.z表示地图m1的地图点对的高程值,m2.z为地图m2偏移对准后的地图点对的高程值,t为偏移对准值。

59、可选的,步骤3.5包括:

60、步骤3.5.1:将混合弟代种群和父代种群进行混合,得到混合后的种群;

61、步骤3.5.2:基于精英策略对混合后的种群进行选择最优数量的个体,得到新子代种群。

62、可选的,步骤3.6包括:

63、计算新子代种群的标准种群多样性值,判断新子代总群的标准种群多样性值是否小于第一预设阈值,若是,则直接输出最大适应度值个体;若不是,则判断迭代次数是否大于第二预设阈值,若大于第二预设阈值,则输出最大适应度值个体,若小于第二预设阈值,则进行步骤3.1-步骤3.5继续迭代。

64、本发明提供的一种基于遗传算法的测深同步定位与建图闭环检测方法与系统的有益效果在于:与传统的地形闭环二维模板匹配方法相比,本发明额外考虑了闭环时由潮差导致的z方向误差与三个角度的测量误差,可实现更准确的闭环检测结果;与传统的寻优算法相比,本发明设置了自适应遗传算子以减少种群过早收敛问题,同时考虑了各自由度误差特征差异,设计降维策略,极大的降低了寻优计算复杂度。

当前第1页1 2 
网友询问留言 已有0条留言
  • 还没有人留言评论。精彩留言会获得点赞!
1