CN109186606A - 一种基于slam和图像信息的机器人构图及导航方法 - Google Patents

Abstract

本发明公开了一种基于SLAM和图像信息的机器人构图及导航方法。该方法为：首先记录初始构建地图时机器人的位姿，设置起始标签；操作机器人移动到预设的切换点位置，记录特征点信息和语义信息，设置切换点标签；构建全局地图完成后，设置结束标签；然后将机器人放置在起始标签位置，收集特征点信息，判断语义信息和特征点图像信息是否匹配通过，是则调整机器人位姿，自主导航至下一个切换点标签位置；否则导航失败，记录导航失败信息，发送信号求助；最后判断是否到达终点，是则结束导航，否则继续进行自主导航。本发明提高了通过SLAM技术构建大范围地图的效率，能够消除大范围内累计误差，具有可靠性高、鲁棒性强、定位准确的优点。

Description

一种基于SLAM和图像信息的机器人构图及导航方法

技术领域

本发明涉及移动机器人和SLAM技术领域，特别是一种基于SLAM和图像信息的机器人构图及导航方法。

背景技术

机器人于上世纪被科学家首次提出，之后便得到研究人员的关注，世界上首台自主机器人1962年在美国问世。进入二十一世纪，围绕着高性能传感器、高适应性的数据关联方法和高精度的控制技术，移动机器人得到了深入的研究，移动机器人的控制方式从遥控向自主控制发展，综合应用机器视觉、传感器技术、计算机处理等智能技术研制自主移动机器人。移动机器人除用于太空探测，资源开发和核工业等领域外，在制造业自动化，建筑业，自主排险，军事领域、服务业等方面也有广泛的应用前景。

机器人实现自我定位，并实现在没有人牵引的情况下导航行走，对机器人在各个行业的应用具有重要的意义。特别是对工作在复杂、动态环境中的移动机器人，例如在工厂地板上或在医院实现运输任务，仅仅运用机载传感器快速实时地生成三维地图，对移动机器人的定位与导航非常重要。操纵机器人在一定空间中自由运动需要一个详细的空间模型，移动机器的定位导航也需要详细的三维地图。

目前全球先进的定位系统是GPS，由美国主导研制的GPS系统有着定位精度高、定位速度快、定位成本低等明显的优势，但是由于卫星传输的系统数据无法穿透建筑物到达室内，机器人接收不到来自GPS系统的数据，在这种情况下机器人的定位就需要借助编码器、激光、声纳等传感器，首先通过传感器采集距离和环境特征等信息数据，然后对数据信息进行分析运算，得到机器人在环境中的位置坐标，结合坐标实现全局定位和导航。

1983年Smith Self和Cheeseman针对移动机器人进行同步导航定位和地图创建(simultaneous location and mapping,SLAM)的研究，随后国内外的研究人员都认识到了SLAM研究的价值和应用前景，开始了对SLAM课题的研究。在现有的技术中，二维SLAM发展成熟，但在构建地图中丢失了很多有价值的形状和几何信息，而且激光传感器的价格往往过于昂贵。所以，研究人员利用基于结构光的RGB-D传感器的深度信息来模拟激光传感器得到的深度信息，并且RGB-D传感器能够充分利用环境三维属性，精确描述出局部环境形状以及物体的几何外形，但基于RGB-D传感器获得的距离信息往往精度不高，使得构图误差在大范围的区域内累计较为明显，导致构建地图任务的失败，使SLAM技术中的闭环检测环节难以实施。

发明内容

本发明的目的在与提供一种融合SLAM技术和图像信息的大范围内构图及导航方法，能够实现大范围内可靠性高、鲁棒性强、定位准确的构图和导航。

实现本发明目的的技术方案为：一种基于SLAM和图像信息的机器人构图及导航方法，包括以下步骤：

步骤1、记录初始构建地图时机器人的位姿，设置起始标签；

步骤2、操作机器人移动并构建地图，当机器人移动到预设的切换点位置时，记录切换点位置的特征点信息和语义信息，设置切换点标签；

步骤3、构建全局地图完成后，设置结束标签；

步骤4、将机器人放置在起始标签位置，并收集特征点信息；

步骤5、判断语义信息是否匹配通过：如果是，则进入步骤6；否则导航失败，记录导航失败信息，发送信号求助；

步骤6、判断特征点图像信息是否匹配通过：如果是则进入步骤7，否则导航失败，记录导航失败信息，发送信号求助；

步骤7、融合特征点的图像信息和深度信息，生成3D点云数据，解算偏移量，调整机器人位姿，自主导航至下一个切换点标签位置；

步骤8、判断是否到达终点：如果是，则结束导航；否则返回步骤5。

进一步地，步骤2所述的操作机器人移动并构建地图，具体如下：

机器人平台采用两轮差速机器人；使用RGB-D传感器获取深度和图像信息，使用二维SLAM中的Gmapping算法构建地图。

进一步地，步骤2中所述的记录切换点位置的特征点信息和语义信息，设置切换点标签，具体如下：

在特定角度提取特征点信息，特定角度选择偏航角依次为0°、30°、60°、90°、120°、150°、180°、210°、240°、270°、300°和330°；

使用ORB算法提取图像特征点信息，使用MASK RCNN算法提取语义信息。

进一步地，步骤5中所述的判断语义信息是否匹配通过，具体如下：

分别为能自主移动的物体和不能自主移动的物体设置不同的匹配偏差权重，如果所有类别物体累加所得到匹配偏差不超过预设阈值，则认为匹配图像信息通过。

进一步地，步骤6中所述的判断特征点图像信息是否匹配通过，具体如下：

采用快速近似最近邻算法匹配特征信息，如果有超过预设阈值的特征点得到匹配，则认为匹配图像信息通过。

进一步地，步骤7中所述的解算偏移量，具体如下：

使用ICP算法来进行匹配好的两组点间的运动估计，对不同坐标系下的两个点云，找到两个点云集中对应的匹配点，不断的迭代，进而减小点云之间的距离，直到匹配的距离小于预设的阈值，最后得到对应点云之间的空间变换关系。

进一步地，步骤7中所述的调整机器人位姿，具体如下：

根据解算偏移量得到的旋转矩阵和平移矩阵，来调整机器人位姿，直至和在此切换位置起始构图位姿相同。

进一步地，步骤7中所述的自主导航至下一个切换点标签位置，具体如下：

机器人利用AMCL实现导航过程中自身的定位，利用Navigation导航包来完成路径规划和动态壁障功能。

本发明与现有技术相比，其显著优点为：(1)通过采用融合SLAM技术和图像信息的大范围内构图及导航方法，把要构建的大范围内的全局地图分为多个局部地图构建，从而实现大范围内可靠性高、鲁棒性强、定位准确的构图和导航方法；(2)在地图的切换点位置，保留机器人的位姿、图像特征点信息和物体的语义信息，作为导航时修正机器人位姿的依据，提高了构建大范围地图的效率，消除了累计误差。

附图说明

图1是本发明基于SLAM和图像信息的机器人构图及导航方法中构图方法流程图。

图2是本发明基于SLAM和图像信息的机器人构图及导航方法中导航方法的流程图。

图3是本发明本发明基于SLAM和图像信息的机器人构图及导航方法的系统框架图。

具体实施方式

结合图1、图2和图3，本发明基于SLAM和图像信息的机器人构图及导航方法，包括以下步骤：

步骤1、记录初始构建地图时机器人的位姿，设置起始标签；

步骤2、操作机器人移动并构建地图，当机器人移动到预设的切换点位置时，记录切换点位置的特征点信息和语义信息，设置切换点标签；

机器人平台采用两轮差速机器人；使用RGB-D传感器获取深度和图像信息，使用二维SLAM中的Gmapping算法构建二维栅格地图。

打开启动摄像头的程序，原地旋转机器人至特定角度并使用摄像头记录机器人当前位姿下的图像信息及深度信息，特定角度选择偏航角依次为0°、30°、60°、90°、120°、150°、180°、210°、240°、270°、300°和330°；使用ORB算法提取不同位姿下图像的特征信息，使用MASK RCNN算法提取不同位姿下图像的语义信息并记录当前切换点位置所含物体的信息。

步骤3、构建全局地图完成后，设置结束标签；

步骤4、将机器人放置在起始标签位置，并收集特征点信息；

原地旋转机器人至某些特定角度并使用摄像头记录机器人当前位姿下的图像信息和深度信息，提取不同位姿下图像的语义信息并记录当前切换点位置所含物体的信息，提取不同位姿下图像的特征信息。

步骤5、判断语义信息是否匹配通过：如果是，则进入步骤6；否则导航失败，记录导航失败信息，发送信号求助；

采用快速近似最近邻算法，根据图像的特征信息和语义信息判断机器人是否可以将现在得到的信息和构图阶段得到的信息准确匹配；

分别为能自主移动的物体和不能自主移动的物体设置不同的匹配偏差权重，如果所有类别物体累加所得到匹配偏差不超过预设阈值，则认为匹配图像信息通过。

步骤6、判断特征点图像信息是否匹配通过：如果是则进入步骤7，否则导航失败，记录导航失败信息，发送信号求助；

采用快速近似最近邻算法匹配特征信息，如果有超过预设阈值的特征点得到匹配，则认为匹配图像信息通过。

步骤7、融合特征点的图像信息和深度信息，生成3D点云数据，解算偏移量，调整机器人位姿，自主导航至下一个切换点标签位置；

使用迭代最近点(ICP)算法来进行匹配好的两组点间的运动估计，通过3D点云数据求机器人偏移量，对不同坐标系下的两个点云，找到两个点云集中对应的匹配点，不断的迭代，进而减小点云之间的距离，直到匹配的距离小于预设的阈值，最后得到对应点云之间的空间变换关系。

根据解算偏移量得到的旋转矩阵和平移矩阵，来调整机器人位姿，直至和在此切换位置起始构图位姿相同，利用AMCL实现导航过程中自身的定位，利用Navigation导航包来完成路径规划和动态壁障功能。

步骤8、判断是否到达终点：如果是，则结束导航；否则返回步骤5。

实施例1

结合图1，本发明基于SLAM和图像信息的机器人构图及导航方法中，其中构图方法具体如下：

步骤1、记录初始构建地图时机器人的位姿，设置起始标签；

记录机器人在构建地图时的起始位姿，是为了让机器人在导航阶段能够发布相同的起始位姿，这是导航程序所需要的。机器人发布的初始位姿和其实际所在地图中的位姿越接近，随后的导航阶段的累计误差越小。

步骤2、操作机器人移动并构建地图，当机器人移动到预设的切换点位置时，记录切换点位置的特征点信息和语义信息，设置切换点标签；

步骤2.1、操作机器人移动并构建地图：

开启二维SLAM中的Gmapping算法，机器人利用RGB-D传感器得到的深度信息，构建二维地图，同时在另外一个终端开启操作机器人移动的程序，使用手柄或是键盘远程控制机器人进行移动。在机器人移动过程中，Gmapping融合机器人编码器返回的里程信息和RGB-D传感器得到的深度信息，构建二维栅格地图。

步骤2.2、判断移动机器人是否到预设的切换点位置：

预设的切换点位置一般选取在路段的拐角处，因为这些位置一般特征点较多，更利于之后过程的特征匹配，在某些较长的路段中，可以选取路段处周围物体较多，环境较为复杂的地方作为切换点。两个切换点之间的距离不宜过长，否者利用RGB-D传感器构建的二维地图会有较大的累计误差。如果当前区域环境较为复杂，路段两侧有较多的参考物的种情况下，Gmapping算法可以得到更多的2维特征信息，构图精度很高，可以适当加大切换点之间的距离。到达预设点之后，结束当前区域的构图程序，调用map server程序保存刚刚构建的地图。

步骤2.3、记录切换点位置的特征点信息和语义信息并设置标签：

特征点信息包括特征点的图像信息和深度信息。打开启动摄像头的程序，原地旋转机器人至某些特定角度并使用RGB-D传感器记录机器人当前位姿下的图像信息和深度信息。因为turtlebot2移动机器人平台采用的是两轮差分式移动方式，所以原地旋转可以不改变机器人的位置，而只改变其姿态。记录多个角度的图像信息是为了使后续根据特征点计算的偏移量更加准确。

使用语义分割技术将图像中的物体分割出来，并打上对应的标签。通过这种技术，可以得到在该切换点位置有几类物体，每类物体的数量。只关注物体的语义信息可以最大程度的降低环境的变化所带来的干扰。要将图像中的物体分割出并识别出其类型，需要预先训练出相应的模型。本发明利用公开的语义训练集，在GPU服务器上训练出MASK RCNN的模型，所以机器人只需要调用预先训练好的模型去对拍摄到的图像进行预测即可。

然后，通过图像特征提取算法提取不同位姿下图像的特征信息。本发明采用ORB算法提取图像特征，ORB特征由关键点和描述子两部分组成，关键点是指该特征点在图像里的位置，描述子是一个向量，描述了该关键点周围像素信息。由于考虑到了旋转和缩放，使得ORB在平移、旋转、缩放的变化下仍有良好的表现，同时ORB算法实时性很高，满足本发明的需求。

保存以上所有信息，并设置一个和当前切换点位置唯一联系的位置标签；

步骤3、构建全局地图完成后，设置结束标签；

如果构建完成全局地图则结束构图，否则重复上述步骤，构建下一段地图，直至构建完成全局地图。

结合图2，本发明基于SLAM和图像信息的机器人构图及导航方法中，导航方法具体如下：

步骤4、将机器人放置在起始标签位置，并收集特征点信息；

步骤4.1、设置位置标签：

通过在程序中设置在构图阶段记录下的位置标签的顺序，决定机器人自主导航的轨迹。

步骤4.2、将机器人放置起始标签位置；

步骤4.3、收集特征点信息：

与构图步骤中收集图像信息的方式相同，打开启动摄像头的程序后，原地旋转机器人至某些特定角度并使用摄像头记录机器人当前位姿下的图像信息及深度信息，然后利用特征点提取算法得到图像的特征点。

步骤5、判断语义信息是否匹配通过；是则进入步骤6，否则导航失败，记录导航失败信息，发送信号求助；

匹配机器人现在得到的语义信息和构图时得到的语义信息，即比较图像中出现的物体类别及每类物体的数量。对于动态的物体，比如人，设定较低的匹配偏差权重，而对与静态物体，比如桌椅，则设置较高的匹配偏差权重。因为语义信息对物体所在位置不敏感，而只关注该物体是否出现，所以该方式对环境的动态变化有较高的鲁棒性。如果该切换点所有类别物体累加所得到匹配偏差不超过预设阈值，则认为匹配图像信息通过，否则导航失败，发出信号进行求助。

步骤6、判断特征点图像信息是否匹配通过；是则进入步骤7，则导航失败，记录导航失败信息，发送信号求助；

根据步骤4得到的图像特征点，和在构建地图阶段得到的该位置的图像特征点进行匹配。因为两个特征点的描述子在向量空间的距离相近，就认为他们是相同的特征点，所以判断基于描述的特征点是否匹配，可通过计算欧式距离来实现，设匹配图像的特征点集合分别为X和Y，如下式：

欧式距离越小表示特征点相似程度越高，本发明在匹配过程中采用了最大紧邻向量匹配方法，即通过比较特征点集合中的每一个特征点与特征点集合中各特征点的欧式距离，D₁和D₂分别代表最近和次近的欧式距离，若D₁≤αD₂(α为设定的最近次近距离比值)，则认为两个特征点匹配，反之丢弃该点。

如果一副图像中有超过预设阈值的特征点得到匹配，则认为匹配图像信息通过，否则导航失败，发出信号进行求助。

步骤7、融合特征点的图像信息和深度信息，生成3D点云数据，解算偏移量，调整机器人位姿，自主导航至下一个切换点标签位置；

步骤7.1、融合特征点的图像信息和深度信息生成3D点云数据：

在完成RGB-D标定后，可根据图像中任意一点对应的深度值得到该点三维坐标，进而生成三维彩色点云数据。给定任意深度图像像素点(x_d，y_d)，投射到三维空间点p(x，y，z)，其坐标计算如下式所示：

z＝depth(x_d，y_d)

式中：为当前像素点深度值，c_x和c_y为图像光心坐标，f_x和f_y为深度相机焦距。根据公式可以将匹配过的特征点融合深度信息得到匹配过的3D点云数据。

步骤7.2、解算偏移量。

使用ICP来解决匹配好的两组点间运动估计问题。ICP算法是对不同坐标系下的两个点云，找到两个点云集中对应的匹配点，不断的迭代，进而减小点云之间的距离，直到匹配的距离小于预设的阈值，最后可以得到对应点云之间的空间变换关系。假设有两片点云P和Q，则其目标函数为F(R，T)，如下式所示：

其中，R和T分别为旋转矩阵与平移矩阵，P_i为源点云，Q_i为目标点云。

步骤7.3、调整机器人位姿：

根据得到的旋转矩阵和平移矩阵来调整机器人位姿，直至和在此切换位置起始构图位姿相同。

步骤7.4、自主导航至下一个切换点：

机器人利用AMCL实现导航过程中自身的定位，利用Navigaton导航包来完成路径规划和动态壁障功能。

步骤8、判断是否到达终点，是则结束导航，否则返回步骤5。

本发明融合SLAM技术和图像信息的大范围内构图及导航方法，通过采用将SLAM技术和图像信息结合的方式，将大范围构建全局地图的任务划分为多个小范围内构建局部地图的任务，在切换点位置记录该位置的特征信息和语义信息。在导航阶段，依次调用之前所构建的局部地图，在切换点位置通过匹配特征信息和语义信息判断导航是否成功，如果成功则可以根据匹配结果调整机器人位姿，从而实现大范围内可靠性高、鲁棒性强、定位准确的构图和导航方法。

Claims (8)

1.一种基于SLAM和图像信息的机器人构图及导航方法，其特征在于，包括以下步骤：

步骤1、记录初始构建地图时机器人的位姿，设置起始标签；

步骤2、操作机器人移动并构建地图，当机器人移动到预设的切换点位置时，记录切换点位置的特征点信息和语义信息，设置切换点标签；

步骤3、构建全局地图完成后，设置结束标签；

步骤4、将机器人放置在起始标签位置，并收集特征点信息；

步骤5、判断语义信息是否匹配通过：如果是，则进入步骤6；否则导航失败，记录导航失败信息，发送信号求助；

步骤6、判断特征点图像信息是否匹配通过：如果是则进入步骤7，否则导航失败，记录导航失败信息，发送信号求助；

步骤7、融合特征点的图像信息和深度信息，生成3D点云数据，解算偏移量，调整机器人位姿，自主导航至下一个切换点标签位置；

步骤8、判断是否到达终点：如果是，则结束导航；否则返回步骤5。

2.根据权利要求1所述的基于SLAM和图像信息的机器人构图及导航方法，其特征在于，步骤2所述的操作机器人移动并构建地图，具体如下：

机器人平台采用两轮差速机器人；使用RGB-D传感器获取深度和图像信息，使用二维SLAM中的Gmapping算法构建地图。

3.根据权利要求1所述的基于SLAM和图像信息的机器人构图及导航方法，其特征在于，步骤2中所述的记录切换点位置的特征点信息和语义信息，设置切换点标签，具体如下：

在特定角度提取特征点信息，特定角度选择偏航角依次为0°、30°、60°、90°、120°、150°、180°、210°、240°、270°、300°和330°；

使用ORB算法提取图像特征点信息，使用MASK RCNN算法提取语义信息。

4.根据权利要求1所述的基于SLAM和图像信息的机器人构图及导航方法，其特征在于，步骤5中所述的判断语义信息是否匹配通过，具体如下：

分别为能自主移动的物体和不能自主移动的物体设置不同的匹配偏差权重，如果所有类别物体累加所得到匹配偏差不超过预设阈值，则认为匹配图像信息通过。

5.根据权利要求1所述的基于SLAM和图像信息的机器人构图及导航方法，其特征在于，步骤6中所述的判断特征点图像信息是否匹配通过，具体如下：

采用快速近似最近邻算法匹配特征信息，如果有超过预设阈值的特征点得到匹配，则认为匹配图像信息通过。

6.根据权利要求1所述的基于SLAM和图像信息的机器人构图及导航方法，其特征在于，步骤7中所述的解算偏移量，具体如下：

使用ICP算法来进行匹配好的两组点间的运动估计，对不同坐标系下的两个点云，找到两个点云集中对应的匹配点，不断的迭代，进而减小点云之间的距离，直到匹配的距离小于预设的阈值，最后得到对应点云之间的空间变换关系。

7.根据权利要求1所述的基于SLAM和图像信息的机器人构图及导航方法，其特征在于，步骤7中所述的调整机器人位姿，具体如下：

根据解算偏移量得到的旋转矩阵和平移矩阵，来调整机器人位姿，直至和在此切换位置起始构图位姿相同。

8.根据权利要求1所述的基于SLAM和图像信息的机器人构图及导航方法，其特征在于，步骤7中所述的自主导航至下一个切换点标签位置，具体如下：

机器人利用AMCL实现导航过程中自身的定位，利用Navigation导航包来完成路径规划和动态壁障功能。