CN114092714A - 基于增强回环检测与重定位的家用割草机器人的定位方法 - Google Patents

Abstract

本发明公开了一种基于增强回环检测与重定位的家用割草机器人的定位方法，包括：采集和读取传感器数据；获取到机器人位姿信息；将机器人位姿信息和处理后的激光数据传入Cartographer算法中的局部SLAM部分，获取到插入结果；接收采集的图像信息，提取图像的特征点，对特征点进行描述，即计算特征描述子，根据特征描述子，查询字典计算该帧的BOW向量，将BOW向量和图像index传送到Cartographer算法中的全局SLAM部分；在全局SLAM中，将图像index和当前node id捆绑在一起，记为node_to_image，更新数据库，在该node所处的位姿处使用当前激光数据与该处的地图进行匹配，完成建图。本发明利用视觉辅助Cartographer后端优化，有效的提高建图鲁棒性，使得地图更加精准，能够让机器人更加准确的定位与导航。

Description

基于增强回环检测与重定位的家用割草机器人的定位方法

技术领域

本发明属于割草机器人技术领域，涉及视觉辅助激光SLAM方法，具体涉及一种基于增强回环检测与重定位的家用割草机器人的定位方法。

背景技术

传统割草机需要人工辅助完成割草，人工割草的缺点：单调、耗时、障碍物、碎石、草丛有害蚊虫等，然而割草机器人的出现解决了上述这些问题，给人们带来了方便。由于割草机器人的准确定位是保证其正确完成导航、控制任务的关键之一。割草机器人沿规划好路径行走的精度主要取决于定位系统。所以割草机器人必须通过定位系统设备获得即时的高精度的位姿信息,从而才能决策下一步的运动状态。传统的定位模式采用GNSS定位，该方式鲁棒性较低，容易受到环境的干扰。

所以，需要一个新的技术方案来解决这个问题。

发明内容

发明目的：为了克服原有Cartographer中闭环检测模块检测速度慢、检测范围小的问题，提供一种基于增强回环检测与重定位的家用割草机器人的定位方法，其利用视觉辅助Cartographer后端优化，有效的提高建图鲁棒性，使得地图更加精准，能够让机器人更加准确的定位与导航。

技术方案：为实现上述目的，本发明提供一种基于增强回环检测与重定位的家用割草机器人的定位方法，包括如下步骤：

S1：采集和读取传感器数据，包括激光数据、IMU数据和相机采集数据；

S2：融合里程计数据和IMU数据，获取到机器人位姿信息，对激光数据进行体素滤波处理；

S3：将机器人位姿信息和处理后的激光数据传入Cartographer算法中的局部SLAM部分，获取到插入结果；

S4：接收采集的图像信息，提取图像的特征点，对特征点进行描述，即计算特征描述子，根据特征描述子，查询字典计算该帧的BOW向量，将BOW向量和图像index传送到Cartographer算法中的全局SLAM部分；

S5：在全局SLAM中，将图像index和当前node id捆绑在一起，记为node_to_image，更新数据库，根据图像的index找到对应的node index，在该node所处的位姿处使用当前激光数据与该处的地图进行匹配，完成建图。

进一步地，所述步骤S1中激光数据包括激光雷达检测到障碍物的距离值，里程计数据包括机器人运动速度，IMU数据包括IMU采集的加速度及角速度。

进一步地，所述步骤S2中机器人位姿信息的获取方式具体为：

将IMU采集的加速度进行积分，计算速度与位移，利用Cartographer算法中位姿推断器将pose数据队列开始和末尾的2个数据计算出的线速度乘以时间得出割草机器人平移的预测，通过imu的角速度乘以时间得出姿态预测，获取到机器人位姿信息。

进一步地，所述步骤S3中插入结果的具体获取过程为：

采用scan-match的相关性扫描匹配技术，计算相邻激光点相对位姿，若已知初始位姿，则可获取所有scan节点全局绝对位姿。因其原理原因，会引入累计误差，但由于相对位姿较为精确，短时间内累加误差较小，Cartographer采用一段时间内的scan构建submap而非整个map；每个submap相互独立，并且采用scan-to-map思想进行scan-match进一步降低累计误差，最后输出子图、位姿、扫描数据等。

进一步地，所述步骤S4中图像的特征点的提取方式为：

基于图像金字塔方式的，在不同尺度的图像上面提取OrientedFAST关键点(增加了方向的FAST关键点)和BRIEF描述子，以此来实现尺度和方向的不变性。

进一步地，所述步骤S4中采用brief描述子对特征点进行描述，brief描述子是一种二进制描述子，总共具有256bit，即32个字节的长度，每位bit为0或1，根据一定的点对选取规则选择点对，该选取规则应当使点对与点对之间的相关性最低。

进一步地，所述步骤S4中BOW向量具体为：

BOW，Bag ofWords，词袋模型。姑且认为word等同于一个特征点，它是若干特征点的聚类中心，当然还是一个特征点，只不过若干个接近的特征点，都映射到同一个特征点，给它起个新的名字叫做word。那么一帧图像，若干个特征点，可以映射得到若干个word，word集合就是BOW。

进一步地，所述步骤S5具体包括如下过程：

A1：图像index和当前node id捆绑在一起，记为node_to_image，并更新“单词-图像index”数据库中，记为KeyFrameDataBase，该数据库保存了每个单词所在的图像的index；

A2：查询闭环(计算约束)：根据当前图像的单词，查询数据库中所有含有任一相同单词的历史图像，找到该图像后，根据图像的index找到对应的node index，然后在该node所处的位姿处使用当前激光数据与该处的地图进行匹配；

A3：对调整后位姿及地图进行保存，并将数据库保存到硬盘中。

本发明使用激光SLAM算法实现割草机器人定位与导航，可以实现割草机器人实时定位，提高其鲁棒性。在激光SLAM算法中选择了基于图优化的Cartographer算法实现草坪环境的精准建图，针对Cartographer后端优化与重定位耗时严重且易发生闭环失败问题，提出一种基于词袋库(BOW)的视觉辅助闭环检测和重定位方法。

有益效果：本发明与现有技术相比，具备如下优点：

1、本发明克服了传统Cartographer在室外草坪中建图耗时长、鲁棒性低等问题，实验表明有效提升了地图创建的观测区域、鲁棒性、容错性。

2、本发明所提出基于词袋库(BOW)的视觉辅助闭环检测和重定位方法，使得割草机器人在导航时，拥有更精准的建图与定位能力。

3、本发明利用双目相机获取三维环境信息，有效的提高割草机器人环境感知能力。

附图说明

图1为本发明的整体算法流程图；

图2为Gazebo创建教学楼草坪的环境图；

图3为Cartographer建立的二维环境地图；

图4为改进前耗时时间图；

图5为改进后耗时时间图；

图6为改进前后耗时时间对比图。

具体实施方式

下面结合附图和具体实施例，进一步阐明本发明，应理解这些实施例仅用于说明本发明而不用于限制本发明的范围，在阅读了本发明之后，本领域技术人员对本发明的各种等价形式的修改均落于本申请所附权利要求所限定的范围。

本发明提供一种基于增强回环检测与重定位的家用割草机器人的定位方法，如图1所示，其包括如下步骤：

S1：采集和读取传感器数据，包括激光数据、IMU数据和相机采集数据：

工控机通过USB接口实时读取IMU采集的加速度及角速度，双目摄像头采集的左右图像，二维激光雷达360°采集的距离值，编码器采集的机器人运动速度。

S2：融合机器人运动速度和IMU数据，获取到机器人位姿信息，对激光数据进行体素滤波处理：

本实施例中机器人位姿信息的获取方式具体为：

将IMU采集的加速度进行积分，计算速度与位移，利用Cartographer算法中位姿推断器将pose数据队列开始和末尾的2个数据计算出的线速度乘以时间得出割草机器人平移的预测，通过imu的角速度乘以时间得出姿态预测，获取到机器人位姿信息。

S3：将机器人位姿信息和处理后的激光数据传入Cartographer算法中的局部SLAM部分，获取到插入结果：

插入结果的具体获取过程为：

采用scan-match的相关性扫描匹配技术，计算相邻激光点相对位姿，若已知初始位姿，则可获取所有scan节点全局绝对位姿。因其原理原因，会引入累计误差，但由于相对位姿较为精确，短时间内累加误差较小，Cartographer采用一段时间内的scan构建submap而非整个map；每个submap相互独立，并且采用scan-to-map思想进行scan-match进一步降低累计误差，最后输出子图、位姿、扫描数据等。

S4：接收采集的图像信息，提取图像的特征点，对特征点进行描述，即计算特征描述子，根据特征描述子，查询字典计算该帧的BOW向量，将BOW向量和图像index传送到Cartographer算法中的全局SLAM部分：

图像的特征点的提取方式为：

基于图像金字塔方式的，在不同尺度的图像上面提取OrientedFAST关键点(增加了方向的FAST关键点)和BRIEF描述子，以此来实现尺度和方向的不变性。

采用brief描述子对特征点进行描述，brief描述子是一种二进制描述子，总共具有256bit，即32个字节的长度，每位bit为0或1，根据一定的点对选取规则选择点对，该选取规则应当使点对与点对之间的相关性最低。

BOW向量具体为：

BOW，Bag of Words，词袋模型。姑且认为word等同于一个特征点，它是若干特征点的聚类中心，当然还是一个特征点，只不过若干个接近的特征点，都映射到同一个特征点，给它起个新的名字叫做word。那么一帧图像，若干个特征点，可以映射得到若干个word，word集合就是BOW。

S5：在全局SLAM中，将图像index和当前node id捆绑在一起，记为node_to_image，更新数据库，根据图像的index找到对应的node index，在该node所处的位姿处使用当前激光数据与该处的地图进行匹配，完成建图。

步骤S5具体包括如下过程：

A1：图像index和当前node id捆绑在一起，记为node_to_image，并更新“单词-图像index”数据库中，记为KeyFrameDataBase，该数据库保存了每个单词所在的图像的index；

A2：查询闭环(计算约束)：根据当前图像的单词，查询数据库中所有含有任一相同单词的历史图像，找到该图像后，根据图像的index找到对应的node index，然后在该node所处的位姿处使用当前激光数据与该处的地图进行匹配；

A3：对调整后位姿及地图进行保存，并将数据库保存到硬盘中。

基于上述方案，为了验证本发明方法的有效性和实际效果，进行如下仿真实验：

如图2所示，在Gazebo中建立模拟仿真环境，模拟环境包括花、树木和假山，他们高度不同，图中1表示白玫瑰花丛，它的高度低于单线激光雷达安装高度，而图中7表示花丛和树木(在树的周围种植花)，图中7中蓝色部分为花丛，红色与黄色表示为树木，树木的高度高于单线激光雷达设定高度，但是花丛低于单线激光雷达设定高度。图中6表示割草机器人，图中3、4、5表示其他种类的花品种且高度高于激光雷达高度。图中2为假山，其余都为树木。rviz是ROS自带的图形化工具，图中7是根据图中6创建的教学楼草坪，利用RVIZ(图形化工具)和Cartographer算法创建的二维环境地图，如图3所示。

对改进前与改后的Cartographer算法进行量化分析(红色字体表示改进的方法)，在该地图中从建图起始点到结束点之间选取共20个位姿点(绿色点)进行重定位实验并统计其耗时，可以看出，图4随着位姿次序的增加，重定位耗时逐步增加，在接近终点时，其重定位耗时长达约300s，图5可以看出，最长时耗仅为1.6s左右，图6为耗时对比图，可知其鲁棒性有了明显提高。

Claims (8)

1.基于增强回环检测与重定位的家用割草机器人的定位方法，其特征在于，包括如下步骤：

S1：采集和读取传感器数据，包括激光数据、IMU数据和相机采集数据；

S2：融合里程计数据和IMU数据，获取到机器人位姿信息，对激光数据进行体素滤波处理；

S3：将机器人位姿信息和处理后的激光数据传入Cartographer算法中的局部SLAM部分，获取到插入结果；

S4：接收采集的图像信息，提取图像的特征点，对特征点进行描述，即计算特征描述子，根据特征描述子，查询字典计算该帧的BOW向量，将BOW向量和图像index传送到Cartographer算法中的全局SLAM部分；

S5：在全局SLAM中，将图像index和当前node id捆绑在一起，记为node_to_image，更新数据库，根据图像的index找到对应的node index，在该node所处的位姿处使用当前激光数据与该处的地图进行匹配，完成建图。

2.根据权利要求1所述的基于增强回环检测与重定位的家用割草机器人的定位方法，其特征在于，所述步骤S1中激光数据包括激光雷达检测到障碍物的距离值，里程计数据包括机器人运动速度，IMU数据包括IMU采集的加速度及角速度。

3.根据权利要求2所述的基于增强回环检测与重定位的家用割草机器人的定位方法，其特征在于，所述步骤S2中机器人位姿信息的获取方式具体为：

将IMU采集的加速度进行积分，计算速度与位移，利用Cartographer算法中位姿推断器将pose数据队列开始和末尾的2个数据计算出的线速度乘以时间得出割草机器人平移的预测，通过imu的角速度乘以时间得出姿态预测，获取到机器人位姿信息。

4.根据权利要求1所述的基于增强回环检测与重定位的家用割草机器人的定位方法，其特征在于，所述步骤S3中插入结果的具体获取过程为：

采用scan-match的相关性扫描匹配技术，计算相邻激光点相对位姿，若已知初始位姿，则可获取所有scan节点全局绝对位姿，Cartographer采用设定时间内的scan构建submap，每个submap相互独立，并且采用scan-to-map思想进行scan-match进一步降低累计误差，最后输出子图、位姿和扫描数据。

5.根据权利要求1所述的基于增强回环检测与重定位的家用割草机器人的定位方法，其特征在于，所述步骤S4中图像的特征点的提取方式为：

基于图像金字塔方式的，在不同尺度的图像上面提取Oriented FAST关键点和BRIEF描述子，以此来实现尺度和方向的不变性。

6.根据权利要求1所述的基于增强回环检测与重定位的家用割草机器人的定位方法，其特征在于，所述步骤S4中采用brief描述子对特征点进行描述，brief描述子是一种二进制描述子，总共具有256bit，即32个字节的长度，每位bit为0或1，根据点对选取规则选择点对，该选取规则应当使点对与点对之间的相关性最低。

7.根据权利要求1所述的基于增强回环检测与重定位的家用割草机器人的定位方法，其特征在于，所述步骤S4中BOW向量具体为：

将word等同于一个特征点，它是若干特征点的聚类中心，一帧图像，若干个特征点，可以映射得到若干个word，word集合就是BOW。

8.根据权利要求1所述的基于增强回环检测与重定位的家用割草机器人的定位方法，其特征在于，所述步骤S5具体包括如下过程：

A1：图像index和当前node id捆绑在一起，记为node_to_image，并更新“单词-图像index”数据库中，记为KeyFrameDataBase，该数据库保存了每个单词所在的图像的index；

A2：查询闭环：根据当前图像的单词，查询数据库中所有含有任一相同单词的历史图像，找到该图像后，根据图像的index找到对应的node index，然后在该node所处的位姿处使用当前激光数据与该处的地图进行匹配；

A3：对调整后位姿及地图进行保存，并将数据库保存到硬盘中。

Images