基于鲸鱼算法与集成学习的移动机器人重定位方法和装置

本发明涉及移动机器人定位，特别涉及一种基于鲸鱼算法与集成学习的有限环境移动机器人重定位方法和装置。

背景技术：

1、定位是解决移动机器人在哪儿的关键技术，是移动机器人的核心功能模块。目前，移动机器人的定位方式主要包含卫星导航方式、uwb定位、磁导航定位以及slam定位等。其中，卫星导航定位方式主要应用于室外大范围开放式场景中，如无人驾驶汽车等，但在遮挡环境下，极易出现多径效应，导致位置出现漂移，特别是在室内等封闭场景，由于卫星信号缺失，导致定位不可用。uwb定位虽然定位精度可以达到10cm级，但是由于其功率因素直接限制了其有效作用距离，针对较大的结构化场景，需要安装多个uwb基站建设定位网络，额外增加成本。磁导航定位需要预设磁性引导带，制约着移动机器人的机动性灵活性。目前，移动机器人大多都装有激光雷达和imu等设备，依托移动机器人已有传感设备实现的激光slam定位方法因其灵活、高精度、节约成本等优势逐渐成为巡检、服务等移动机器人的主要定位方式。

2、由于巡检、服务等机器人的作业区域相对固定，目前常用的定位流程主要是通过激光雷达-imu耦合的方式构建全局地图，在建立全局地图的基础上，通过作业过程中实时采集的雷达点云数据与全局地图或局部地图进行匹配实现精准重定位。正态分布变换算法（ndt）是目前主流的点云匹配算法。申请人发现，在使用ndt进行重定位过程中，移动机器人出现颠簸，或者大曲率转弯的场景容易导致imu噪声增大，进而出现位置或姿态跳变，导致重定位失败。此外，ndt点云匹配算法易出现局部优化现象，致使迭代完成时，得到的位姿并不是最佳匹配状态。

3、因此，亟须对现有定位方法进行改进，以提高定位的精度和效率。

技术实现思路

1、针对现有方法的以上缺陷及改进需求，本发明的目的在于提供一种基于鲸鱼算法与集成学习的有限环境移动机器人重定位方法和装置，用于克服现有激光slam重定位过程中易出现的位置和姿态跳变问题。

2、根据本发明的目的，本发明提出了一种基于鲸鱼算法与集成学习的有限环境移动机器人重定位方法，所述方法具体包括以下步骤：

3、步骤s1. 加载作业场景下全局地图的三维点云数据，并基于第一预设条件将所述全局地图划分为若干子图；

4、步骤s2. 获取移动机器人实时局部规划后的运动轨迹，在映射到所述若干子图上之后匹配出满足预设匹配规则的匹配子图；

5、步骤s3. 构建帧间深度残差图像，通过将深度残差图像的像素信息作为判定条件，得到滤除动态目标的实时点云数据；

6、步骤s4. 将所述实时点云数据域与所述匹配子图进行鲸鱼算法适配以确定出所述移动机器人的精确位姿；

7、步骤s5. 将所述精确位姿输入预先构建的集成学习模型，输出移动机器人的实时最佳位姿。

8、作为本发明的进一步的方案，所述步骤s1的具体步骤如下：

9、步骤s11. 加载作业场景全局地图三维点云数据，将原始地图点云数据进行降采样处理；

10、步骤s12. 设置所述移动机器人作业区域的全局地图x-y平面平行于地面，z轴坐标为场景高程；根据第一预设条件将x-y平面全局地图区域划分为若干子图，其中，第一预设条件为：

11、；

12、其中，为子图在宽、长方向的索引，表示相邻两个子图在长宽方向的重叠区间宽度；、分别为子图的宽度和长度，、分别为全局地图的宽度和长度。

13、作为本发明的进一步的方案，所述步骤s2的具体步骤如下：

14、步骤s21. 获取移动机器人的实时局部规划后的运动轨迹，选择运动轨迹前方运动距离内的轨迹点以构造点集，从所述点集中抽取距离间隔为的离散点集；

15、步骤s22. 将所述离散点集中的轨迹点依次映射到划分的若干子图中，并根据预设匹配规则匹配出匹配子图；

16、其中，所述预设匹配规则为：若一个轨迹点对应一个子图，则该子图为移动机器人必经区域，该子图为匹配子图；若一个轨迹点对应多个子图，则统计关联子图对应的轨迹点个数，选择个数多的子图为必经子图区域且作为匹配子图；

17、步骤s23. 按照移动机器人规划轨迹采样点先后顺序，将匹配出的匹配子图构建单向子图链表；

18、其中，单向子图链表的更新过程为：当移动机器人完成当前子图路径，进入下一子图时，则删除当前头节点，将下一子图作为头节点；若随移动机器人前进，后续轨迹有新子图为必经区域时，将插入尾节点，以更新单向子图链表。

19、作为本发明的进一步的方案，所述步骤s3的具体步骤如下：

20、步骤s31. 将当前时刻实时采集的激光雷达三维点云数据转换为场景深度图像；

21、步骤s32. 将t-1时刻采集的激光雷达三维点云数据通过位姿坐标变换到与当前时刻t所在坐标系下，得到变换后的点云数据，并基于步骤s3.1的过程将点云数据转换为深度图像；

22、步骤s33. 根据如下公式构造残差图像：

23、；

24、遍历残差图像，若所述残差图像的峰谷值大于设定阈值，则将该峰谷值对应的空间点标记为动态目标点；

25、步骤s34. 将当前时刻t得到的深度图像逆向映射到三维点云中，根据标记的动态目标属性，滤除动态目标点，得到滤除动态目标的实时点云数据。

26、作为本发明的进一步的方案，所述步骤s3.1 具体包括：

27、对于当前时刻的三维点云中的空间点，计算其到雷达坐标系的空间距离，

28、；

29、将空间点映射到深度图像中，其在深度图中对应的像素坐标为，像素点的值设置为，

30、；

31、；

32、；

33、其中，表示激光雷达的线束数量，表示激光雷达每个线束上的激光点数量，angt和angb分别表示顶部和底部线束的垂直视场角度。

34、作为本发明的进一步的方案，所述步骤s4中实时位姿匹配，主要利用鲸鱼优化算法，改进ndt匹配算法，解决陷入局部优化问题，具体步骤如下：

35、步骤s41. 计算实时点云数据与目标点云数据间的相对位姿变换关系；将所述实时三维点云数据与目标点云数据每次迭代计算的匹配结果，通过相对位姿向量定义为鲸鱼算法中的鲸鱼群体中的个体，将最优位姿定义为目标猎物，为表示移动机器人在场景中的实时位姿坐标；

36、第次迭代计算得到的匹配位姿记为，

37、；

38、式中，和分别表示第 k次迭代得到的横向和纵向相对位移，表示第 k次迭代得到的相对航向角。

39、于是，匹配点云和目标点云中点的坐标变换关系为：

40、；

41、式中，和表示实时点云中的三维空间点再经过坐标变换，得到在地图坐标系中的坐标。

42、步骤s42.计算鲸鱼算法的网格中每个匹配点的正态分布：

43、；

44、式中，表示坐标变换后的点云落入正态分布变换算法ndt网格内的点，；表示调整因子；m表示落入网格内空间点的均值，表示网格内空间点的协方差，即

45、；

46、；

47、；

48、式中，d表示点的空间维度。

49、步骤s43. 计算每个鲸鱼个体的匹配因子：

50、；

51、若小于设定阈值，则更新将当前匹配位姿作为所述移动机器人的精确位姿，若大于等于设定阈值，则返回步骤s41，继续迭代，直到满足小于设定阈值的条件或达到最大迭代次数。

52、作为本发明的进一步的方案，所述步骤s5的具体步骤如下：

53、步骤s51.生成集成学习训练集，根据移动机器人实时采集的点云数据和imu数据，利用多种位姿估计算法得到实时位姿结果，利用预设指标对同一时刻位姿估算结果进行评价，将评价指标作为特征值，构建训练样本；

54、步骤s52.构建集成学习器，分别设计三个基于决策树的弱学习器，结合加权投票算法，将弱分类器输出作为输入，构建强学习器实现准确的位姿估计；

55、步骤s53.集成学习器训练，对原始构建的训练集采用有放回的随机重采样，获得三个样本有交叉的子样本，分别对三个弱学习器进行训练，将输出结果输入强学习器，所述强学习器通过集成学习和加权投票法，预测最佳位姿估计的来源，即对应来源的序号和概率。

56、作为本发明的进一步的方案，所述预设指标包括：与规划轨迹预瞄点的相对平移误差、与规划轨迹预瞄点的相对旋转误差、相邻帧间的移动机器人转角与移动机器人的最大转角的比率绝对值、相邻采集数据帧间运动速度与移动机器人的实时规划速度比以及相邻帧间的相对位移与最大位移的比率。

57、作为本发明的进一步的方案，加权投票法中的加权投票公式如下：

58、；

59、；

60、；

61、；

62、其中，表示第个位姿估计来源的集成学习后概率输出，表示第个弱分类器对第个位姿估计来源的概率值。

63、本发明还提供了一种移动机器人装置，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现上述的方法。

64、与现有技术相比，本发明存在以下有益技术效果：

65、本发明主要以3d激光雷达和惯性测量单元imu(inertial measurement unit)为主要设备，基于先验构建的全局场景地图，实现移动机器人实时鲁棒定位。由于移动机器人易受动态目标干扰、抖动、急转弯等因素影响，导致重定位精度降低甚至失效问题。针对场景中的动态目标干扰问题，使用基于激光点云深度图构建帧间残差，滤除场景中的动态目标；针对ndt算法易陷入局部优化，难以获得最佳匹配问题，通过引入自适应惯性权重和遗传因子，利用鲸鱼优化算法优化ndt匹配流程；针对机器人抖动、急转弯工况下，imu噪声增大导致的ndt匹配失效问题，构建集成学习模型，实现对多种实时位姿估计的最佳预测，获取最佳位姿。

66、本方法在动态复杂的有限环境下，能够极大的提高基于先验地图的移动机器人重定位准确性和鲁棒性，可广泛应用于agv、清扫、巡检、停车场送电等移动机器人作业场景。