CN108961385B - 一种slam构图方法及装置 - Google Patents

Abstract

本发明涉及一种SLAM构图方法及装置，对序列图像进行处理，得到相机的位姿；根据相机的位姿，确定相机的运动轨迹；把序列图像中不同关键帧对应的点云变换到同一关键帧下，构建三维地图环境；其中，在构建三维地图环境时，对于一个关键帧，根据相机的运动轨迹确定该关键帧的运动方向，仅更新该关键帧中与其运动方向对应的图像区域。本发明在构图时，并没有使用整幅关键帧的构图方法，而是选择仅更新与关键帧对应的运动方向相同的区域，相对于传统的基于整幅关键帧的构图方法，大大减少了构图时关键帧里的冗余信息，算法速度快，实时性很好，很大幅度提高构图的效率，给使用者以很好的用户体验。

Description

一种SLAM构图方法及装置

技术领域

本发明属于机器人技术领域，具体涉及一种SLAM构图方法及装置。

背景技术

为了在未知环境中进行导航，移动机器人需要构建环境地图并且同时定位自身在地图中的位置，像这样同时解决这两个问题的过程就被称为同步定位与地图构建(Simultaneously Localization And Mapping，SLAM)。当机器人处于室外环境的时候，这个问题可以通过高精度的GPS来解决。但是，当机器人处于室内环境时，或者GPS不够精确无法满足高精度的需求时，或者当机器人所处环境涉密时，人们就必须使用其他方法来精确估计机器人的位置并同时构建环境地图。SLAM问题就是在这儿样的需求背景下提出的，是当前移动机器人研究领域的一个重要课题，并且已经发展了很多年。

SLAM主要解决两个问题：估计相机的位姿，并且同时对场景进行三维几何重建。最近几年，基于关键帧的SLAM方法几乎成为了解决各种计算机视觉任务的主流技术。Strasdat等人的研究表明，基于关键帧技术在每个计算单元确实比其他的滤波方法更高效，并且基于关键帧的PTAM曾被许多学者认为是单目SLAM算法的黄金标准算法。

现在最具代表性的基于关键帧的SLAM系统是ORB-SLAM。虽然跟其他SLAM系统比起来，ORB-SLAM的性能更好，但是基于关键帧的方法也不一定是最终的解决方案。目前我们构图时都是采用整幅关键帧的信息，但是关键帧里的大部分信息对构图都是冗余的，使得在进行地图构建时耗时较多。而且，现在的SLAM算法都要求机器人运动比较小，这样每一个关键帧里的大部分信息和前一帧是一样的。并且如果场景是动态的话，问题就会更严重，因为本来就不多的有用信息里还包含了动态目标，而对于动态目标，我们并不想把它构建在地图里面。

发明内容

本发明的目的在于提供一种SLAM构图方法及装置，用以解决采用整幅关键帧的方法来进行SLAM构图时，因冗余信息多而造成耗时较长的问题。

为解决上述技术问题，本发明的技术方案为：

本发明的一种SLAM构图方法，包括如下步骤：

1)对序列图像进行处理，得到相机的位姿；

2)根据相机的位姿，确定相机的运动轨迹；

3)把序列图像中不同关键帧对应的点云变换到同一关键帧下，构建三维地图环境；

在构建三维地图环境时，对于一个关键帧，根据所述相机的运动轨迹确定该关键帧的运动方向，仅更新该关键帧中与其运动方向对应的图像区域。

进一步地，所述对应关系包括：

若关键帧的运动方向为上，则更新图像的上部区域；

若关键帧的运动方向为下，则更新图像的下部区域；

若关键帧的运动方向为左，则更新图像的左部区域；

若关键帧的运动方向为右，则更新图像的右部区域。

进一步地，将图像平均分割成N*N个区域。

进一步地，所述N＝4，而且包括八个运动方向：

当运动方向为左上时，需要更新的区域为：第零区域、第一区域、第二区域、第三区域、第四区域、第八区域和第十二区域；

当运动方向为上时，需要更新的区域为：第零区域、第一区域、第二区域和第三区域；

当运动方向为右上时，需要更新的区域为：第零区域、第一区域、第二区域、第三区域、第七区域、第十一区域和第十五区域；

当运动方向为右时，需要更新的区域为：第三区域、第七区域、第十一区域和第十五区域；

当运动方向为右下时，需要更新的区域为：第三区域、第七区域、第十一区域、第十二区域、第十三区域、第十四区域和第十五区域；

当运动方向为下时，需要更新的区域为：第十二区域、第十三区域、第十四区域和第十五区域；

当运动方向为左下时，需要更新的区域为：第零区域、第四区域、第八区域、第十二区域、第十三区域、第十四区域和第十五区域；

当运动方向为左时，需要更新的区域为：第零区域、第四区域、第八区域和第十二区域；

其中，所述十六个区域按照从左往右、从上往下的方向依次排布，分别为第零区域、第一区域到第十五区域。

进一步地，通过查找表的方法来更新关键帧中与其运动方向对应的图像区域；其中，表中存储着运动方向与需要更新的图像区域的对应关系。

进一步地，步骤1)还包括：将序列图像分割成至少四个区域，然后分别对每帧序列图像的每个区域进行处理，最终得到相机的位姿。

进一步地，将序列图像分割成至少四个区域后：

首先，对序列图像的每个区域进行特征点检测与描述符提取，根据提取到的特征描述符对相邻两帧之间的图像进行特征匹配；

然后，对于序列图像，根据相邻帧间图像的摄影几何关系，得到相机在相邻帧间的相对位姿关系。

进一步地，所述确定该关键帧运动方向的步骤包括：

将相邻两帧序列图像的特征点在x和y方向坐标作差，得到的差值与阈值作比较，根据比较结果确定该特征点的运动方向；直到将所有的特征点比较完毕，得到每个特征点的运动方向；统计每个运动方向的特征点数量，数量最多的方向为该关键帧的运动方向。

本发明的一种SLAM构图装置，包括如下单元：

用于对序列图像进行处理，得到相机的位姿的单元；

用于根据相机的位姿，确定相机的运动轨迹的单元；

用于把序列图像中不同关键帧对应的点云变换到同一关键帧下，构建三维地图环境的单元；

在构建三维地图环境时，对于一个关键帧，根据所述相机的运动轨迹确定该关键帧的运动方向，仅更新该关键帧中与其运动方向对应的图像区域。

进一步地，所述对应关系包括：

若关键帧的运动方向为上，则更新图像的上部区域；

若关键帧的运动方向为下，则更新图像的下部区域；

若关键帧的运动方向为左，则更新图像的左部区域；

若关键帧的运动方向为右，则更新图像的右部区域。

本发明的有益效果：

本发明的SLAM构图方法及装置，在将每一帧图像进行处理后，得到相机的位姿；然后根据相机的位姿，确定相机的运动轨迹；最后根把不同关键帧对应的点云变换到同一关键帧下，构建三维地图环境；其中，在构建三维地图环境时，对于一个关键帧，根据所述相机的运动轨迹确定该关键帧的运动方向，仅更新该关键帧中与其运动方向对应的图像区域。本发明在构图时，并没有使用整幅关键帧的构图方法，而是选择仅更新与关键帧对应的运动方向相同的区域，相对于传统的基于整幅关键帧的构图方法，大大减少了构图时关键帧里的冗余信息，算法速度快，实时性很好，很大幅度提高构图的效率，给使用者以很好的用户体验。

进一步的，在对每一帧图像进行处理时，先将图像进行分割，接着针对每帧图像的每个区域分别进行处理来得到相机的位姿。将图像进行分割处理后，使得特征点增多并且均匀分布，可以提高相机位姿解算的精度。

附图说明

图1是本发明的整体方法流程图；

图2是针对室内场景的RGB-D SLAM构图方法流程图；

图3是示例1直接从整幅图像提取特征点且良好匹配条件为5倍最小距离的示意图；

图4是示例2直接从整幅图像提取特征点且良好匹配条件为5倍最小距离的示意图；

图5是示例1将图像分割成16份后提取特征点的示意图；

图6是示例2将图像分割成16份后提取特征点的示意图；

图7是重投影误差的均方根误差；

图8是本发明的图像分割区域示意图；

图9是相机运动方向的示意图；

图10是本发明的运动方向和需更新区域的表格示意图；

图11是基于查找表的构图结果示意图；

图12是基于整幅关键帧的构图结果示意图；

图13是将整幅图像的处理转变成只处理每幅图片的一部分的示意图。

具体实施方式

为使本发明的目的、技术方案及优点更加清楚，下面结合附图及实施例，对本发明作进一步的详细说明，但本发明的实施方式并不局限于此。

本发明的SLAM构图方法实施例：

现有的视觉SLAM算法，在构造密集点云图时由于对每一个关键帧都整幅处理，因而使用了大量的冗余信息，使得处理速度较慢，因而本发明提出了一种SLAM构图方法，流程图如图1所示，在对相继得到相机的位姿和相机的运动轨迹后，在构建三维地图环境时，根据相机的运动轨迹确定该关键帧的运动方向，仅更新该关键帧中与其运动方向对应的图像区域。

其中，图像中需要更新的区域，与关键帧的运动方向有一个相对应的关系。这种“对应关系”是指，例如：根据两帧图像之间的关系，判定运动轨迹方向为向上时，需要更新的区域为图像上边部分的区域；判定运动轨迹方向为向下时，需要更新的区域为图像下边部分的区域；判定运动轨迹方向为向左时，需要更新的区域为图像左边部分的区域；依次类推。

具体的，上边部分的区域、下边部分的区域、左边部分的区域的大小、形状等特征可以根据需要，结合实际情况自行设定。

下面具体介绍一张基于RGB-D的SLAM构图方法，其中就采用了具有8个运动方向、16个区域的构图方式；并且采用基于查找表(Look-Up-Table，LUT)的视觉SLAM构图方法来高效地构建密集点云图。LUT里存储着构图时每个运动方向所对应的需要更新的区域编号，构图时只需根据查找表在地图中更新必须的区域即可，这样便可大幅减少构图时关键帧里的冗余信息，大幅度提高构图的效率。另外，在初始图像处理时就将图像分割成多个区域，在每个区域里提取特征点，这样便可得到更符合整幅图像的相机位姿信息。

下面详细说明，流程图如图2所示：

首先，将RGB-D图像被平均分成16个区域，如图8所示，具体的区域编号为0～16。在每一区域里分别提取一定的SIFT特征点并计算ORB描述符。我们一般让每个区域里的特征点数量相等。但是为了在纹理不明显的场景也能正确跟踪，我们也让每个区域的特征点数量可变，如果有些区域没有特征点，则其他区域的特征点就相对多一点。当有足够的特征匹配后，用上一帧的深度图把上一帧的特征点通过针孔相机模型转换成3维相机坐标，然后通过RANSAC-PnP算法来求解相机的位姿。接着判断当前帧是否是关键帧，这里我们采用了跟PTAM相同的帧间距离条件来作为判断准则。

然后，在上述步骤中得到了一系列的特征点和特征匹配，那么通过这些匹配的特征点做差我们就可以估计出照片的运动方向。用当前帧的特征点分别在x和y方向的坐标减去上一帧匹配特征点的x和y方向的坐标，然后用差值跟阈值相比，便可以确定该特征点的运动方向，把所有的特征点做相同处理然后把统计数量最多的那个方向作为图像的最终运动方向。具体的运动方向设置如图9所示，设置八邻域的图像方向，具体为：运动方向0表示左上，运动方向1表示上，运动方向2表示右上，运动方向3表示右，运动方向4表示右下，运动方向5表示下，运动方向6表示左下，运动方向7表示左。

接着，通过回环检测和全局光束法平差来将相机的位姿进行优化，得到最优相机位姿。

回环检测的本质就是判断机器人是否曾到过某个地点。如果可以检测到回环，那么相机的定位误差就可以大幅减小。最简单的回环检测策略是比较当前帧和之前的每一个关键帧来判断它们的距离是否在阈值之内，但是随着系统的运行，后面要比较的帧数会越来越多，效率会特别低。另一个较快的方法是随机在之前的关键帧中找几帧和当前帧作比较。这里把这两个方法结合起来检测回环。包含边和节点的位姿图可以很直观的表示出关键帧之间的内部关系。其中图的节点就是相机的位姿，边是相邻两个相机位姿之间的变换矩阵。只要检测到一个关键帧，那么我们就在图里添加一个节点来表示当前帧的相机位姿，并且添加一个边来表示当前帧的相机位姿与上一关键帧的相机位姿之间的变换关系。

当知道了位姿图网络和相机位姿的初值后，我们就可以用全局光束法平差来估计相机位置的精确值。把第一个关键帧保持固定，然后用所有的关键帧来做整体平差。为了解决非线性优化的问题我们采用了全局位姿图优化方法(Global Pose GraphOptimization，g2o)里的Levenberg-Marquadt方法。

最后，在知道了相机位姿的精确值后，可通过相应的透视变换矩阵把所有的关键帧都变换到第一个关键帧的视角下。为了得到场景的3D几何重建，把2D的图像坐标和相对应的深度信息通过针孔相机模型转换成3D坐标。做一个查找表，里面存储着构图时，每个移动方向所对应的需要构图的区域。那么根据图像的运动方向，用查找表来确定需要更新的区域，并根据需要更新的区域来构建当前帧的环境地图。若把关键帧分成了n份，用f(cell)来描述每个区域的信息，那么一幅图就可以表示成：

构图时，每个方向需要更新的信息为：

其中，D为构图时某一方向所对应的需要更新区域的集合。

则最终生成的地图为：

f(map)＝∑f(direction)

具体的查找表LUT如图10所示：

图像向左上移动，即运动方向为0时，那么构图时只用编号为0,1,2,3,4,8,12的图像信息；

图像向上移动，即运动方向为1时，那么构图时只用编号为0,1,2,3的图像信息；

图像向右上移动，即运动方向为2时，那么构图时只用编号为0,1,2,3,7,11,15的图像信息；

图像向右移动，即运动方向为3时，那么构图时只用编号为3,7,11,15的图像信息；

图像向右下移动，即运动方向为4时，那么构图时只用编号为3,7,11,12,13,14,15的图像信息；

图像向下移动，即运动方向为5时，那么构图时只用编号为12,13,14,15的图像信息；

图像向左下移动，即运动方向为6时，那么构图时只用编号为0,4,8,12,13,14,15的图像信息；

图像向左移动，即运动方向为7时，那么构图时只用编号为0,4,8,12的图像信息。

下面做实验来验证该方法的有效性。使用处理器为因特尔i7-4600U CPU@2.10GHz×4，只用了1个线程来验证算法。另外，我们用的是TUM RGB-D的数据集，因为它提供了深度信息可以减少算法设计时的复杂性。

该实验比较了图像分割16份然后提取特征点和不做任何处理直接提取特征点对相机位姿解算精度的影响。

图3和图4是直接从整幅图像提取的特征点，图5和图6是图像分割16份后分别提取的特征点。通过对比，发现图像分割后，提取的特征点数量增加了，并且分布比较均匀。然后通过计算重投影误差的均方根误差来表示相机误差的大小，重投影误差对比的实验结果如图8所示，发现图像分割后的重投影误差比不做处理的重投影误差小，也就是说，图像分割后，特征点增多并且均匀分布，可以提高相机位姿解算的精度。

在估计运动方向并用LUT来构图时，仍旧定义相机的运动方向为8邻域方向，如图9所示。通过分别计算匹配特征点在x和y方向的视差，并把这2个视差与阈值相比较来判断图像的移动方向。最后通过统计每个方向的数量，取最多的那个方向为图像的移动方向。然后我们就可以根据图10所示的查找表，找到构图所需的格网编号，并且在构图时只更新这些区域的信息。

图11显示了使用该方法所构建的最终地图，图12是使用整幅关键帧构建的最终地图。下表1显示了这两种地图构建方法所消耗的时间。通过对比，发现使用基于查找表的方法，可以用少于三分之一的时间构建出噪声更少的密集点云地图。

表1两种地图构建方法所消耗的时间

另外，图13形象的表示了为什么通过把基于关键帧的构图方法转变为基于查找表的方法就可以减少构图的噪声并缩短构图的时间。

在该实施例中，把图像平均分成了16份，并且它确实可以更好的完成定位和构图。作为其他实施例方式，可把每个关键帧分割成需求的份数，或者不将图像进行平均分割，根据需求设定区域，来保证既能有足够的图像信息来保证相机位姿解算的精度，又没有多余的冗余信息来构建一个较好的地图。

同时，估计相机运动方向时只考虑了8邻域方向而没有考虑图像的放大和缩小。如果场景放大，那么当前帧可能就不需要更新，而如果场景变小的话，那么当前帧的四周可能都需要更新。没有考虑这两种情况是因为它们的误判率很高，并且它们对构图的最终结果影响不大。作为其他实施方式，在设置相机的运动方向时，可不仅仅设置上述的八个方向，可相应增加以更加细化，或者相应减少以使算法更加简单，只要能够准确完成定位和构图即可。

而且，通过回环检测和全局光束法平差来将相机的位姿进行优化，得到最优相机位姿。作为其他实施方式，可采用其他现有的方法来对相机的位姿进行优化，例如GraphSLAM、TORO(Tree-based network Optimizer)、HOG-Man(HierarchicalOptimization on Manifolds for Online 2D and 3D mapping)等图模型框架。

本发明的SLAM构图装置实施例：

该装置包括如下单元：用于对序列图像进行处理，得到相机的位姿的单元；用于根据相机的位姿，确定相机的运动轨迹的单元；用于把序列图像中不同关键帧对应的点云变换到同一关键帧下，构建三维地图环境的单元；其中，在构建三维地图环境时，对于一个关键帧，根据相机的运动轨迹确定该关键帧的运动方向，仅更新该关键帧中与其运动方向对应的图像区域。

上述装置，实际上是基于本发明对应方法流程的一种计算机解决方案，即一种软件构架，上述单元即为与方法流程相对应的各处理进程或程序。由于对上述方法的介绍已经足够清楚完整，故不再对该装置进行详细描述。

尽管本发明的内容已经通过上述优选实施例作了详细介绍，但应当认识到上述的描述不应被认为是对本发明的限制。在本领域技术人员阅读了上述内容后，对于本发明的多种修改和替代都将是显而易见的。因此，本发明的保护范围应由所附的权利要求来限定。

Claims (10)

1.一种SLAM构图方法，包括如下步骤：

1）对序列图像进行处理，得到相机的位姿；

2）根据相机的位姿，确定相机的运动轨迹；

3）把序列图像中不同关键帧对应的点云变换到同一关键帧下，构建三维地图环境；

其特征在于，在构建三维地图环境时，对于一个关键帧，根据所述相机的运动轨迹确定该关键帧的运动方向，仅更新该关键帧中与其运动方向对应的图像区域；该关键帧中与其运动方向对应的图像区域的获取方式为：在初始图像处理时将图像分割成多个图像区域，建立关键帧的各运动方向与需更新的图像区域的对应关系并存储，利用所述对应关系确定该关键帧的运动方向对应的图像区域。

2.根据权利要求1所述的SLAM构图方法，其特征在于，所述对应关系包括：

若关键帧的运动方向为上，则更新图像的上部区域；

若关键帧的运动方向为下，则更新图像的下部区域；

若关键帧的运动方向为左，则更新图像的左部区域；

若关键帧的运动方向为右，则更新图像的右部区域。

3.根据权利要求1所述的SLAM构图方法，其特征在于，将图像平均分割成N*N个区域。

4.根据权利要求3所述的SLAM构图方法，其特征在于，所述N=4，而且包括八个运动方向：

当运动方向为左上时，需要更新的区域为：第零区域、第一区域、第二区域、第三区域、第四区域、第八区域和第十二区域；

当运动方向为上时，需要更新的区域为：第零区域、第一区域、第二区域和第三区域；

当运动方向为右上时，需要更新的区域为：第零区域、第一区域、第二区域、第三区域、第七区域、第十一区域和第十五区域；

当运动方向为右时，需要更新的区域为：第三区域、第七区域、第十一区域和第十五区域；

当运动方向为右下时，需要更新的区域为：第三区域、第七区域、第十一区域、第十二区域、第十三区域、第十四区域和第十五区域；

当运动方向为下时，需要更新的区域为：第十二区域、第十三区域、第十四区域和第十五区域；

当运动方向为左下时，需要更新的区域为：第零区域、第四区域、第八区域、第十二区域、第十三区域、第十四区域和第十五区域；

当运动方向为左时，需要更新的区域为：第零区域、第四区域、第八区域和第十二区域；

其中，十六个区域按照从左往右、从上往下的方向依次排布，分别为第零区域、第一区域到第十五区域。

5.根据权利要求1所述的SLAM构图方法，其特征在于，通过查找表的方法来更新关键帧中与其运动方向对应的图像区域；其中，表中存储着运动方向与需要更新的图像区域的对应关系。

6.根据权利要求1所述的SLAM构图方法，其特征在于，步骤1）还包括：将序列图像分割成至少四个区域，然后分别对每帧序列图像的每个区域进行处理，最终得到相机的位姿。

7.根据权利要求6所述的SLAM构图方法，其特征在于，将序列图像分割成至少四个区域后：

首先，对序列图像的每个区域进行特征点检测与描述符提取，根据提取的特征描述符对相邻两帧之间的图像进行特征匹配；

然后，对于序列图像，根据相邻帧间图像的摄影几何关系，得到相机在相邻帧间的相对位姿关系。

8.根据权利要求1所述的SLAM构图方法，其特征在于，所述确定该关键帧运动方向的步骤包括：

将相邻两帧序列图像的特征点在x和y方向坐标作差，得到的差值与阈值作比较，根据比较结果确定该特征点的运动方向；直到将所有的特征点比较完毕，得到每个特征点的运动方向；统计每个运动方向的特征点数量，数量最多的方向为该关键帧的运动方向。

9.一种SLAM构图装置，包括如下单元：

用于对序列图像进行处理，得到相机的位姿的单元；

用于根据相机的位姿，确定相机的运动轨迹的单元；

用于把序列图像中不同关键帧对应的点云变换到同一关键帧下，构建三维地图环境的单元；

其特征在于，在构建三维地图环境时，对于一个关键帧，根据所述相机的运动轨迹确定该关键帧的运动方向，仅更新该关键帧中与其运动方向对应的图像区域；该关键帧中与其运动方向对应的图像区域的获取方式为：在初始图像处理时将图像分割成多个图像区域，建立关键帧的各运动方向与需更新的图像区域的对应关系并存储，利用所述对应关系确定该关键帧的运动方向对应的图像区域。

10.根据权利要求9所述的SLAM构图装置，其特征在于，所述对应关系包括：

若关键帧的运动方向为上，则更新图像的上部区域；

若关键帧的运动方向为下，则更新图像的下部区域；

若关键帧的运动方向为左，则更新图像的左部区域；

若关键帧的运动方向为右，则更新图像的右部区域。

Images