基于多传感融合的水下复杂环境动态避障导航方法及系统与流程

摘要: 本发明涉及避障导航,具体而言,涉及基于多传感融合的水下复杂环境动态避障导航方法及系统。背景技术:1、当前水下避障导航以波束前视声呐、三维激光雷达为主流设备,可实...
基于多传感融合的水下复杂环境动态避障导航方法及系统与流程

本发明涉及避障导航,具体而言,涉及基于多传感融合的水下复杂环境动态避障导航方法及系统。

背景技术:

1、当前水下避障导航以波束前视声呐、三维激光雷达为主流设备,可实时获取障碍物距离、轮廓信息;光学视觉、惯性导航与仿生侧线传感辅助,提升浑浊、复杂水流环境适应性。导航算法方面,人工势场法、快速随机树、模型预测控制广泛应用,结合深度学习实现声呐图像目标检测与语义分割。多源信息经卡尔曼滤波、贝叶斯估计融合。

2、但是目前的水下避障导航一般是随机给出可以完成避障操作的指令,而没有考虑到当前避障路线相对于目的地来说是否合适,是否会更加偏离目的地,没有进行对多条避障路线进行优选,得到最佳的避障路线,仅考虑是否能避障,导致为了避障而加长了到达目的的时间。

技术实现思路

1、本发明的目的是提供基于多传感融合的水下复杂环境动态避障导航方法及系统,来解决上述现有技术中所存在的问题。

2、本发明通过以下技术方案实现:

3、第一方面,本发明提供了一种基于多传感融合的水下复杂环境动态避障导航方法,包括:

4、基于传感器获取基于时间序列排序的激光雷达数据组,基于激光雷达数据组得到障碍物的相对执行机构的相对移动速度和相对移动方向;

5、获取导航路径及巡航速度,根据相对移动速度、相对移动方向、导航路径及巡航速度判断未来某时刻是否会与障碍物碰撞,若否,则发送正常巡航的信号至执行机构;

6、若是,则获取当前时刻的图像数据,对图像数据中的障碍物进行图像识别判断为生命体或非生命体,若为生命体,则发送正常巡航的信号至执行机构,若为非生命体或图像数据无法识别,则基于激光雷达数据获取当前障碍物的几何尺寸,基于几何尺寸获取若干个备选的偏离角度;

7、获取执行每个偏离角度后,与原始导航所产生的变化参数,建立角度执行评价模型,基于所述变化参数通过角度执行评价模型对每个偏离角度进行评价,得到每个偏离角度的评价值,基于所述评价值输出所选定的偏离角度,输出至执行机构。

8、优选的,所述基于激光雷达数据组得到障碍物的相对执行机构的相对移动速度和相对移动方向包括:

9、提取激光雷达数据组中的若干帧中障碍物的点云数据,基于点云数据获取障碍物的质心坐标, 基于若干个质心坐标得到当前的相对移动速度和相对移动方向;

10、

11、

12、

13、式中,为平均相对速度,为质心坐标的个数,为第帧的时刻,为第帧的时刻,、、分别为第帧的质心坐标,、、为第帧的质心坐标,为水平方位角,为俯仰角。

14、优选的,所述判断未来某时刻是否会与障碍物碰撞包括:

15、获取当前帧障碍物点云数据的几何中心距离执行机构的距离,基于巡航速度得到到达障碍物的目标时间,通过所述时间在导航路径查找未来目标时间后所到达的目标位置坐标;

16、获取当前帧执行机构的第一定位坐标,通过障碍物距离执行机构的距离计算得到障碍物的第二定位坐标;

17、基于当前帧激光雷达数据中的点云数据得到当前障碍物的几何尺寸,将带有执行机构尺寸的模型图写入至目标定位坐标,同时展示在第二坐标位置的带有几何尺寸的障碍物的模型,判断执行机构的模型与障碍物的模型是否干涉,若干涉,则判断为碰撞,若不干涉,则判断不碰撞。

18、优选的,所述对图像数据中的障碍物进行图像识别判断为生命体或非生命体包括:

19、构建神经网络目标检测模型,获取当前水域水下常规鱼类的历史图像数据,通过该历史图像数据对神经网络目标检测进行训练,输出最优的神经网络目标检测模型;

20、对图像数据进行预处理,通过神经网络目标检测模型中的主干网络对与处理后的图像数据进行特征提取;

21、将提取后的特征进行特征融合,将融合后的特征输入神经网络目标检测模型中的检测头进行检测,输出当前图像数据中目标的类别概率,并设置目标概率,基于当前类别概率和目标概率输出当前图像数据中的障碍物为生命体或非生命体。

22、优选的,所述通过神经网络目标检测模型中的主干网络对与处理后的图像数据进行特征提取包括:

23、

24、式中,为当前层特征,为激活函数,为上一层特征,为卷积核,为第层卷积层的偏置。

25、优选的,所述将提取后的特征进行特征融合包括:

26、

27、式中,为融合后的图像,为放大操作,为拼接操作,为比大的尺度特征,为当前尺度特征。

28、优选的,所述若为生命体,则发送正常巡航的信号至执行机构包括:

29、设置极限距离报警阈值,当执行机构与障碍物的距离小于极限距离报警阈值时,则发送停止前进的信号至执行机构;

30、实时获取若干帧激光雷达数据,基于若干帧激光雷达数据判断当前障碍物是否移动,若未移动,则改变路径进行巡航,若在移动,则向障碍物移动方向相反的方向移动。

31、优选的,所述基于几何尺寸获取若干个备选的偏离角度包括:

32、基于当前帧的点云数据获取面向执行机构投影的平面图,提取所述平面图中点云数据的轮廓线图;

33、以轮廓线图的几何中心为圆点将轮廓线图划分为若干个区域,任意选取每个区域中的一个点作为目标点,以目标点在点云数据中的位置与执行机构的第一定位坐标之间的水平方位角和俯仰角作为偏离角度。

34、优选的,所述建立角度执行评价模型包括:

35、

36、式中,为第个偏离角度评价值,为第个偏离角度的水平方位角和巡航方向之间的夹角,为第个偏离角度的水平方位角,为第个偏离角度的俯仰角,当前执行机构到达第个偏离角度的目标点的距离。

37、第二方面,本发明还提供了基于多传感融合的水下复杂环境动态避障导航系统,用于执行上述的基于多传感融合的水下复杂环境动态避障导航方法,包括:

38、数据处理模块,基于传感器获取基于时间序列排序的激光雷达数据组,基于激光雷达数据组得到障碍物的相对执行机构的相对移动速度和相对移动方向;获取导航路径及巡航速度,根据相对移动速度、相对移动方向、导航路径及巡航速度判断未来某时刻是否会与障碍物碰撞,若否,则发送正常巡航的信号至执行机构;若是,则获取当前时刻的图像数据,对图像数据中的障碍物进行图像识别判断为生命体或非生命体,若为生命体,则发送正常巡航的信号至执行机构,若为非生命体或图像数据无法识别,则基于激光雷达数据获取当前障碍物的几何尺寸,基于几何尺寸获取若干个备选的偏离角度;

39、执行模块,被配置为获取执行每个偏离角度后,与原始导航所产生的变化参数,建立角度执行评价模型,基于所述变化参数通过角度执行评价模型对每个偏离角度进行评价,得到每个偏离角度的评价值,基于所述评价值输出所选定的偏离角度,输出至执行机构。

40、本发明的技术方案至少具有如下优点和有益效果:

41、采用本发明所提供的方法,基于几何尺寸获取若干个备选的偏离角度;获取执行每个偏离角度后,与原始导航所产生的变化参数,建立角度执行评价模型,基于所述变化参数通过角度执行评价模型对每个偏离角度进行评价,得到每个偏离角度的评价值,基于所述评价值输出所选定的偏离角度,输出至执行机构。通过上述方法,先是对可能可以进行避障的路线进行随机生成,对生成后的避障路线进行评价,选择评价值最大的偏离角度进行执行,尽可能的提高执行机构的前进效率。