一种同步建立激光地图和标签地图的方法与流程

本发明涉及机器人建图，尤其是一种同步建立激光地图和标签地图的方法。

背景技术：

1、送餐机器人室内定位的主流方法为激光定位和标签定位，但两种定位方式都有明显的缺点。

2、对于单纯的激光定位，它对环境的变化比较敏感，障碍物位置的移动会影响激光定位的评分；对于单纯的标签定位，需要识别天花板上粘贴的标签进行定位计算，而在天花板较高的区域，标签识别效果的变差会影响标签定位的评分。

3、在上述的情景中，单纯的激光地图无法满足机器人稳定定位的需求，又因为标签的分布是稀疏，因此普通的标签建图方法效果也不理想，为解决这个问题，提出一种同步建立激光地图和标签地图的方法。

技术实现思路

1、针对现有技术的不足，本发明提供一种同步建立激光地图和标签地图的方法，本发明能够适应标签稀疏的环境，建立准确无误的机器人定位地图。

2、本发明的技术方案为：一种同步建立激光地图和标签地图的方法，包括以下步骤：

3、s1)、记录当前机器人的里程计数据，记录当前激光雷达数据，并将当前激光雷达数据加入临时激光子图，将当前机器人位姿记为激光子图坐标系的原点；建立第一个子图时，机器人的位姿认为是(0，0，0)；

4、s2)、持续获取机器人的里程计数据和激光雷达数据，并根据前后两个时刻机器人的里程计数据计算前后两个时刻机器人的位移增量δs和角度增量δθ；并将其分解为激光子图坐标系的x轴、y轴位移，然后将增量位移增量δs和角度增量δθ与前一时刻的机器人位姿(xlast，ylast，θlast)进行叠加，计算得到机器人当前的预测位姿(xguess，yguess，θguess)；具体如下：

5、

6、s3)、激光匹配定位

7、将当前的激光雷达数据与临时激光子图中的数据进行匹配，选取重合度最高的结果作为当前机器人的真实位姿(xlocal，ylocal，θlocal)；

8、s4)、获取标签数据

9、利用红外相机获取天花板图像，并对该天花板图像进行图像识别得到标签数据，如果当前没有拍摄到标签则跳转到步骤s6)，否则执行步骤s5)；

10、s5)、计算标签在激光子图中的位姿

11、根据步骤s4)得到的标签数据，获取标签相对于相机中心的偏移距离(xbias，ybias)、相对于相机正方向的角度偏差θbias；将基于相机坐标系的偏移距离(xbias，ybias)和角度偏差θbias变换为激光子图坐标系下的数据，并与当前机器人在激光子图下的真实位姿(xlocal，ylocal，θlocal)进行叠加，以得到标签在激光子图的局部坐标(xlabel，ylabel，θlabel)，计算方式如下：

12、

13、计算完成后，执行步骤s6)；

14、s6)、激光子图更新：

15、将当前的激光雷达数据加入临时激光子图，并判定临时激光子图是否已有40帧激光雷达数据，如果临时激光子图已有40帧激光雷达数据，则该子图定义为一个激光子图；并判定建图是否完成，如果是，则执行步骤s7)；如果建图未完成，则执行步骤s2)；

16、s7)、回环修正

17、通过人工输入或者子图间自主匹配的方式对所有的激光子图进行回环数据计算，得到回环数据；将回环数据按照预设的图优化算法对激光子图进行运算，以调整子图的位置，使整个激光地图更符合实际情况，同时，按照预设的图优化算法对标签数据进行运算，以修正标签位置。

18、作为优选的，步骤s1)中，所述的临时激光子图是指不足40帧的激光数据的集合。

19、作为优选的，步骤s1)、s2)中，获取的所述的激光雷达数据为极坐标系下的激光雷达数据，需要将其转换为直角坐标系下的激光雷达数据。

20、作为优选的，步骤s1)、步骤s2)中、将处于极坐标系下的激光雷达数据转换为直角坐标系下的激光雷达数据，具体为：

21、已知极坐标系下的雷达数据为(r，θ)，则直角坐标系下的激光雷达数据(xlaser，ylaser)为：

22、

23、作为优选的，步骤s2)中，所述的根据前后两个时刻机器人的里程计数据计算前后两个时刻机器人的位移增量δs和角度增量δθ，具体如下：

24、已知前一时刻的里程计数据为(xodom_t，yodom_t，θodom_t)，后一时刻的里程计数据为(xodom_(t+1)，yodom_(t+1)，θodom_(t+1))，则可以计算得到：

25、

26、作为优选的，步骤s3)中，将直角坐标系下的激光雷达数据与临时激光子图进行匹配，选取重合度最高的结果作为当前机器人的位姿(xlocal，ylocal，θlocal)，具体如下：

27、s31)、已知当前激光雷达数据为laser＝{(xl_i，yl_i)|i＝1,2，...，m}，临时激光子图数据为submaptemporary＝{(xi，yi)|i＝1,2，...，n}，以及机器人当前的匹配位姿(xtest，ytest，θtest)；

28、其中，m为激光雷达数据点的数量，n为临时激光子图激光点的数量；

29、将临时激光子图数据映射到二维栅格中：

30、

31、其中，(minx，miny)分别为临时激光子图数据的x坐标最小值和y坐标最小值，round()代表取整操作；

32、s32)、将激光雷达数据转换到临时激光子图坐标系中：

33、

34、s33)、将步骤s32)中的数据映射到二维栅格中：

35、

36、s34)、统计gridlaser和gridsubmap中有多少重合的数据；

37、s35)、不断调整机器人当前的匹配位姿(xtest，ytest，θtest)的值，使得gridlaser和gridsubmap重合度最高，将该值记为(xlocal，ylocal，θlocal)。

38、作为优选的，步骤s31)中，所述的机器人当前的匹配位姿(xtest,ytest,θtest)与步骤s2)中出机器人当前的预测位姿(xguess,yguess,θguess)之间的关系为：

39、

40、其中，deltax为x坐标的变化量，deltay为y坐标的变化量，deltaθ为角度的变化量。

41、作为优选的，步骤s7)中，所述的通过人工输入或者子图间自主匹配的方式对所有的激光子图进行回环数据计算具体如下：

42、在建图过程中，由于施工人员知道激光子图submapa和submapb对应着现实中的同一个位置，因此人为将submapa和submapb的数据按照步骤s4)进行匹配，可以得到一个回环数据代表着子图a和b间的偏移关系。

43、作为优选的，步骤s7)中，将回环数据按照预设的图优化算法对激光子图进行运算，以调整子图的位置，具体如下：

44、已知每个激光子图的坐标系原点对应的坐标集合p，回环数据集合loop，标签数据label；

45、p＝{xi,yi,θi|i为子图序号}

46、

47、

48、将p和loop作为输入，传入ceres solver图优化算法库中运算，得到修正后的坐标集合plooped；

49、

50、已知i号子图修正后的原点对应的全局坐标为属于i号子图的j号标签修正前的坐标为因此，修正后的标签坐标的计算方法为：

51、

52、本发明的有益效果为：

53、1、本发明增强了机器人的环境适用性，面对标签比较稀疏的场合，以激光为主，标签为辅的融合地图更能有效地为机器人描述环境特征；

54、2、本发明增强了机器人定位的稳定性，单纯的激光定位很容易受到环境变化的影响，本发明的融合地图在激光地图的基础上添加了标签，利用标签定位进行位姿修正，可以让机器人定位更加稳定；

55、3、本发明在建立激光子图的时候，同步记录标签在激光子图中的局部坐标，在完成激光图的整体建立后，使用激光子图间的回环信息，对标签进行回环修正，得到符合激光地图坐标系的标签地图。