CN104077809B - 基于结构性线条的视觉slam方法 - Google Patents

Abstract

本发明提供了一种基于结构性线条的视觉SLAM方法，其中，提供了用以采集周围环境图像的摄像设备；利用建筑的结构性线条作为特征线条来实现实时定位和地图构建（SLAM）；该方法包括如下步骤：SLAM初始化：选择主导方向，采集主导方向上的线条作为特征线条，并对新增的特征线条进行参数化；SLAM过程中：针对每一帧图像，预测所述摄像设备的运动，并依此预测特征线条在下一帧图像中的位置，然后在下一帧图像中在预测位置附近寻找匹配线条，得到该特征线条在下一帧图像中的实际位置，进而计算预测位置与实际位置的偏差，再利用卡尔曼滤波器更新特征直线的位置以及所述摄像设备的位置和姿态。

Description

基于结构性线条的视觉SLAM方法

技术领域

本发明涉及视觉同步定位与地图构建(SLAM)领域，可应用于移动机器人、无人机的自主导航，也可用于移动终端的增强现实及虚拟建模。

背景技术

目前，同步定位与地图构建(SLAM)是实现自主导航的基本问题与研究热点。它的目标是解决在进入未知环境后，如何感知周围环境构建增量式地图，并同时进行自身定位的问题。用于感知周围环境的传感器有很多种，摄像设备凭借其廉价，体积小，便于安装等优点使视觉SLAM方法成为领域中重要研究内容。传统的理论主要是利用环境中的特征点制图和定位，优点是特征点便于检测和跟踪，而缺点则是对于一些人造建筑的环境如走廊的墙面等，往往因为缺少特征点而严重影响SLAM的准确度。现有技术也有利用线段实现SLAM的，但其通过跟踪线段的两个端点的方法使其在本质上无异于之前的特征点SLAM。还有一些技术，将垂直线量化成地面上的一点，而这将原本的六个自由度局限在了2D平面。

发明内容

本发明要解决的技术问题是提供一种能在特征数目很少的情况下依然保证高精度的SLAM方法，同时使得最后地图的描述与三位重建更清晰。

为了解决这一技术问题，本发明提供了一种基于结构性线条的视觉SLAM方法，其中，提供了用以采集周围环境图像的摄像设备；利用建筑的结构性线条作为特征线条来实现实时定位和地图构建(SLAM)；

该方法包括如下步骤：

SLAM初始化：选择主导方向，采集主导方向上的线条作为特征线条，并对新增的特征线条进行参数化；

SLAM过程中：针对每一帧图像，预测所述摄像设备的运动，并依此预测特征线条在下一帧图像中的位置，然后在下一帧图像中在预测位置附近寻找匹配线条，得到该特征线条在下一帧图像中的实际位置，进而计算预测位置与实际位置的偏差，再利用卡尔曼滤波器更新特征直线的位置以及所述摄像设备的位置和姿态。

SLAM初始化时，对采集的环境图像进行线条检测，根据检测到的线条计算消失点，再依据不同的消失点对检测到的线条进行分类，选择其中线条最多的三类或N类作为主导方向。

在对新增线条进行参数化时，先确认该特征线条属于哪一主导方向，利用该特征线条所属的主导方向表示其方向，再求得该特征线条与参考平面的交点，作为特征线条的参数记录在卡尔曼滤波器的状态向量中；

其中参考平面为世界坐标系下两两垂直的XY、YZ或ZX平面，参考平面的选择依据该特征线条与三个平面法线方向的夹角，夹角最小的平面被选为该特征线条的参考平面。

确定该特征直线属于哪一主导方向时，将三个主导方向所对应的三个消失点分别与该特征直线的中点连接，形成三条参考直线，其中一条所述参考直线与该特征直线共线或接近共线时，则判定该线条为对应主导方向上的特征直线。

通过更新所述摄像设备的参数实现所述摄像设备的定位，所述摄像设备的参数至少包括了其在世界坐标系中的位置、姿态，角速度和速度。

在找出匹配线条的过程中，先通过几何信息匹配过程和外观信息匹配过程选出类似直线，然后再通过局部更新滤波器剔除异常值，得到最佳的一个或多个线段作为匹配线段；

在几何信息匹配过程中，选择与所述预测线条距离最近，且斜率最相似的若干线段；在外观信息匹配过程中，将线段中点的一块图像块与所述预测线条的图像块进行相似度的计算，选择相似度最佳的若干线段；同时经几何信息匹配过程和外观信息匹配过程选择得到的若干线段作为匹配线段。

得到了直线的预测位置及其实际匹配的匹配线段后，计算预测位置与实际位置的偏差时，计算所述匹配线段的两个端点到该预测直线的有符号距离，计算过程中，若遇到长线段，则将长线段截取成长度一定的若干短线段，再分别计算。

处理完每一帧图像后，还包括进行地图管理的过程，对于每一个主导方向，给定一个直线的最小数目，仅当现有的直线数目小于这个最小数目时，才对该主导方向上的新检测到特征线条进行参数化；

在进行地图管理的过程中，用一个变量记录每个特征线条连续未被匹配的次数，该次数会在重新成功匹配后被清零，否则一直累加，当地图中的特征线条数目超过一定上限，我们则从地图中删除对应未被匹配次数最大的特征线条。

所述摄像设备运动过程中，还实时进行特征点检测，通过所述特征线条和特征点共同实现实时定位和地图构建(SLAM)。

本发明创造性地发现到，对于人造建筑的场景，包含了若干个主导方向的平面，而沿着这些主导方向的直线可以大致勾勒出建筑的结构，故可称之为结构性线条。例如绝大部分情况下，建筑都垂直于地面，那么就有一个主导方向是垂直于地面的，继而所有垂直于地面的直线都是属于这一主导方向的结构性线条，当然主导方向应该不只有一个。本发明利用摄像设备拍摄和采集人造建筑的结构性线条来实现六个自由度的视觉同步定位与地图构建(SLAM)。可见，本发明对特征线条进行了创造性地选择，并进一步地创造性地将其应用于SLAM方法中，充分利用了建筑的结构信息，即使在特征数目很少的情况下依然能够达到很高的精度。在最后的地图描述和三维重建上，结构性线条也能够比特征点更清晰的重现实际的地图场景。

附图说明

图1和图2为本发明一实施例中结构性线条在环境图像中和三维的世界坐标系中的对比示意图；

图3为本发明一实施例中特征线条参数化的示意图；

图4为本发明一实施例中结构性线条并结合特征点的SLAM和传统的基于点的MonoSLAM的结果比较示意图；

图5为本发明一实施例中三维重建的结果示意图。

具体实施方式

以下将结合图1至图5对本发明提供的基于结构性线条的视觉SLAM方法进行详细的描述，其为本发明一可选的实施例，可以认为，本领域的技术人员在不改变本发明精神和内容的范围内能够对其进行修改和润色。

本实施例是在传统SLAM上的改进，其提供了一种基于结构性线条的视觉SLAM方法，其中，提供了用以采集周围环境图像的摄像设备；利用建筑的结构性线条作为特征线条来实现实时定位和地图构建(SLAM)；

主要过程通过matlab来实现，该方法包括如下步骤：

SLAM初始化：选择主导方向，采集主导方向上的线条作为特征线条，并对新增的特征线条进行参数化；将其参数化后才可以将特征线条投射到世界坐标系中，也能通过对参数进行进一步处理和更新；

SLAM过程中：针对每一帧图像，预测所述摄像设备的运动，并依此预测特征线条在下一帧图像中的位置，然后在下一帧图像中在预测位置附近寻找匹配线条，得到该特征线条在下一帧图像中的实际位置，进而计算预测位置与实际位置的偏差，再利用卡尔曼滤波器更新特征直线的位置以及所述摄像设备的位置和姿态。

这里的实际位置其实为观测位置，卡尔曼滤波器依据预测位置和观测位置进行计算，通过状态向量和协方差矩阵进行估算，从而得到最佳的值，最终依据该值可以在三维世界坐标系中进行更精确的实时定位和地图构建(SLAM)。

至于如何在三维世界坐标系下进行特征线条的投射，进而实现三维的地图构建以及具体的定位，在本领域都已有诸多文献有所记载，本实施例仅就与现有技术的区别部分进行详细阐述，故而不应因为本发明和实施例中未记载现有技术中已有的内容而认为本发明未充分公开，换言之，本领域的技术人员在将现有技术与本发明的技术方案结合的情况下，完全可以实现本发明的技术手段和技术效果。

SLAM初始化时，对采集的环境图像进行线条检测，根据检测到的线条计算消失点，再依据不同的消失点对检测到的线条进行分类，选择其中线条最多的三类或N类作为主导方向。本实施例以三类主导方向进行阐述，N的取值可以为任意。

在对新增线条进行参数化时，先确认该特征线条属于哪一主导方向，利用该特征线条所属的主导方向表示其方向，再求得该特征线条与参考平面的交点，作为特征线条的参数记录在卡尔曼滤波器的状态向量中；换言之，通过消失点和与参考平面的交点可确定一条直线。

其中参考平面为世界坐标系下两两垂直的XY、YZ或ZX平面，参考平面的选择依据该特征线条与三个平面法线方向的夹角，夹角最小的平面被选为该特征线条的参考平面。

确定该特征直线属于哪一主导方向时，将三个主导方向所对应的三个消失点分别与该特征直线的中点连接，形成三条参考直线，其中一条所述参考直线与该特征直线共线或接近共线时，则判定该线条为对应主导方向上的特征直线。

通过更新所述摄像设备的参数实现所述摄像设备的定位，所述摄像设备的参数至少包括了其在世界坐标系中的位置、姿态，角速度和速度。

在找出匹配线条的过程中，先通过几何信息匹配过程和外观信息匹配过程选出类似直线，然后再通过局部更新滤波器剔除异常值，得到最佳的一个或多个线段作为匹配线段；

在几何信息匹配过程中，选择与所述预测线条距离最近，且斜率最相似的若干线段；在外观信息匹配过程中，将线段中点的一块图像块与所述预测线条的图像块进行相似度的计算，选择相似度最佳的若干线段；同时经几何信息匹配过程和外观信息匹配过程选择得到的若干线段作为匹配线段。

得到了直线的预测位置及其实际匹配的匹配线段后，计算预测位置与实际位置的偏差时，计算所述匹配线段的两个端点到该预测直线的有符号距离，计算过程中，若遇到长线段，则将长线段截取成长度一定的若干短线段，再分别计算。

处理完每一帧图像后，还包括进行地图管理的过程，对于每一个主导方向，给定一个直线的最小数目，仅当现有的直线数目小于这个最小数目时，才对该主导方向上的新检测到特征线条进行参数化；

在进行地图管理的过程中，用一个变量记录每个特征线条连续未被匹配的次数，该次数会在重新成功匹配后被清零，否则一直累加，当地图中的特征线条数目超过一定上限，我们则从地图中删除对应未被匹配次数最大的特征线条。

所述摄像设备运动过程中，还实时进行特征点检测，通过所述特征线条和特征点共同实现实时定位和地图构建(SLAM)。

以下将对其中内容进行进一步展开，其中只详细描述本发明提出的关于结构性线条部分的方案内容，而不对特征点进行阐述。

本发明创造性地发现到，对于人造建筑的场景，包含了若干个主导方向的平面，而沿着这些主导方向的直线可以大致勾勒出建筑的结构，故可称之为结构性线条。例如绝大部分情况下，建筑都垂直于地面，那么就有一个主导方向是垂直于地面的，继而所有垂直于地面的直线都是属于这一主导方向的结构性线条，当然主导方向应该不只有一个。本发明利用摄像设备拍摄和采集人造建筑的结构性线条来实现六个自由度的视觉同步定位与地图构建(SLAM)。可见，本发明对特征线条进行了创造性地选择，并进一步地创造性地将其应用于SLAM方法中，充分利用了建筑的结构信息，即使在特征数目很少的情况下依然能够达到很高的精度。在最后的地图描述和三维重建上，结构性线条也能够比特征点更清晰的重现实际的地图场景。

步骤一、结构性线条的主导方向的确定

在图1中，一个走廊环境里的主导方向有三个，一个是垂直于地面的，还有两个是平行与地面的(沿着走廊方向和垂直于走廊方向)。图2则展现了在matlab中具体如何建模和展现结构性线条的图像截图。

为了确定主导方向，我们先利用现有的LSD算法检测直线，再用J-linkage算法根据消失点的不同对图像中直线进行分组。最后选取直线数量最多的三组作为我们的主导方向。在每组直线内，消失点可以通过公式(1)求直线交点获得。其中，s表示一个3×M的矩阵，3×M表示M条直线，v是需要求解的消失点的3×1齐次坐标表示。

s^Tv＝0 (1)

通过解该方程，可得图像中消失点的位置。得到了图像中的消失点，可以通过公式(2)，得到消失点在世界坐标系中的方向。而我们所说的主导方向和结构性线条的方向都可以通过世界坐标系中消失点的方向来表达。

η∝R^wcK^-1v (2)

公式(2)中，R和K分别表示摄像设备的外部旋转矩阵和内部参数矩阵，则表示所求的世界坐标系下消失点的方向。

消失点方向被全局的保留下来，不在每一步迭代中进行更新。但如果SLAM中途遇到某一方向的直线数量超过一定阈值，且又与现有的主导方向不一致时，我们会增加一个主导方向，此时，这个主导方向的消失点方向会被加入地图的状态向量，参与更新。

步骤二、结构性线条的初始化

所述摄像设备运动过程中，针对每一帧图像，先对特征直线和摄像设备的运动进行参数化，然后利用卡尔曼滤波器对每帧环境图像中的特征直线的参数和摄像设备运动的参数进行估算，得到最佳的值，从而根据该最佳的值将特征直线和摄像设备的运动轨迹投射到世界坐标系中。

这里我们采用传统的基于扩展的卡尔曼滤波器(EKF)的框架，其包含一个状态向量(记录摄像设备位置、姿态和动态参数以及周围特征直线、特征点的位置)和一个与其对应的协方差矩阵。

所以，需将结构性线条，即特征线条通过参数来表征。

由于我们只考虑结构性线条，所以初始化直线的第一步是查看该直线属于哪个主导方向。这里需将世界坐标系下的主导方向η反投到图像坐标系下：

v＝KR^cwη (3)

针对某一条直线，我们分别连接直线中点和每一个消失点v_i形成一条参考直线，如果检测直线靠近参考直线，则认为属于该主导方向。直线的主导方向的信息被保存下来，但并不计入状态向量。状态向量中记录的是直线与XY或YZ或ZX平面的交点的反深度表示，如图2所示，[c_ac_b]^T是摄像设备沿主导方向投影到参考平面上的位置，θ表示摄像设备投影与直线和参考平面交点的方向，h是摄像设备投影到直线与参考平面交点的距离的倒数：

整体的状态向量及其协方差矩阵，包括摄像设备部分x_c(具体又包括摄像设备朝向q^wc，世界坐标系下的位置p^w，平移速度v^w，旋转速度ω^c)，点部分x_p和结构性线条部分x_l：

这里

步骤三、摄像设备的运动模型

运动模型可以依情况而选择，这里我们选用恒定平移速度，恒定角速度的运动模型：

所述摄像设备运动的参数至少包括了角速度、速度、姿态参数和其在世界坐标系中的位置参数。姿态也可理解为摄像设备朝向。

步骤四、直线匹配

直线匹配首先要得到图像中的预测线条。该预测直线是经过运动模型之后，将现有的世界坐标系下的特征线投影到图像中的线条。对于某一条特征直线，我们只需将其记录在状态向量中的点和该直线对应的消失点的方向分别投影回到环境图像平面中，根据这两个点便可以确定预测直线。有了预测直线和用LSD算法检测出来的图像中的直线，我们可以利用表1所示的匹配算法找出预测直线分别匹配的图像中的直线。该算法同时考虑直线的几何信息和外观信息，选出关联直线。对于几何信息，我们考虑线段与预测直线的投影距离，以及线段与预测直线的角度差别。对于外观信息，我们简单地比较线段中点的一块11×11像素的图像块和预测直线的关联的初始图像块的相似程度，相似度的计算采用ZNCC(零均值归一化互相关)算法，如公式(8)所示；然后再通过局部更新滤波器的方法剔除异常值，能够达到鲁棒的匹配。

其中，

以下表1为线条匹配算法：

步骤五、结构性线条的观测模型

假设是图像中的结构性线条，s_j是想匹配的线段，那么观测模型即利用匹配的线段到对应的结构性线条的带符号的距离表示。其观测方程如下：

由于我们允许一条直线与多个线段匹配，所以针对某个结构性线条，其观测方程可以写成如下的形式：

步骤六、卡尔曼滤波器运算

利用上述步骤的计算结果，通过以下公式，更新卡尔曼滤波器。即状态向量及其协方差矩阵。

其中，H为观测方程的雅克比矩阵，和分别为经过运动模型后的状态向量及其协方差矩阵，N为观测误差的协方差矩阵，r为预测值与观测值的差值，即

步骤七、地图的管理

增加特征线或点：对于每一个主导方向，我们给定一个直线数目的最小阈值，当现有的直线数目小于这个最小数目时，才会去初始化新的直线。点也是相同的方案。

删除特征线或点：我们用一个变量记录每个特征线或特征点连续未被匹配的次数，该次数会在重新成功匹配后被清零，否则一直累加。当地图中的特征数超过一定上限，我们则从地图中删除对应未被匹配次数最大的特征点或线。

我们利用以上步骤对一组在矩形走廊里的闭合数据进行实验，对比我们的方法和传统的单纯利用点的方法，结果如图4，图4是结构性线条并结合点的SLAM和传统的基于点的MonoSLAM的结果比较。方块代表结构性线条并结合点的SLAM的方法，圆圈代表基于点的MonoSLAM的方法。由于走廊中墙面简单，特征点较少，只利用点的SLAM在一段时间后便不能工作。而我们的点线结合的方法可以很好的估计摄像设备轨迹和周围的地图信息，有着较高的精度。此外，利用结构性线条，更加有利于最后的地图三维重建，其效果如图5所示。

综上所述，相比现有技术，本发明具有以下有益效果：

一、本发明利用结构性线条，使得即使在特征数目很少的情况下(如走廊两侧干净的墙面)也可以顺利的进行定位和制图。

二、本发明利用结构性线条的主导方向信息，可以全局的限制预测的方向，很大程度上提高轨迹和地图的精度。

三、本发明同时利用结构性线条和特征点，达到了比只利用特征点更高的精度。

四、采用结构性线条，可以比点更好的重建三维地图，将建筑的结构清晰的勾勒出来。

此外，本发明还进行了如下的改进：

1)结构性线条用所对应的主导方向以及直线与参考平面的交点表示。

2)结构性线条匹配时，允许一个直线匹配多个线段，并采用鲁棒的算法进行匹配。

3)结构性线条的观测模型为匹配线段到对应直线的有符号距离。为保证长线段发挥更多作用，将长线段截成特定长度的几根短线段，再进行观测。

4)为了进一步提高精度和适用范围，在利用结构性线条的同时，同时加入特征点。

Claims (9)

1.一种基于结构性线条的视觉SLAM方法，其中，SLAM指实时定位和地图构建，提供了用以采集周围环境图像的摄像设备；利用建筑的结构性线条作为特征线条来实现实时定位和地图构建SLAM；

该方法包括如下步骤：

SLAM初始化：选择主导方向，采集主导方向上的线条作为特征线条，并对新增的特征线条进行参数化；

SLAM过程中：针对每一帧图像，预测所述摄像设备的运动，并依此预测特征线条在下一帧图像中的位置，然后在下一帧图像中在预测位置附近寻找匹配线条，得到该特征线条在下一帧图像中的实际位置，进而计算预测位置与实际位置的偏差，再利用卡尔曼滤波器更新特征线条的位置以及所述摄像设备的位置和姿态；

在对特征线条进行参数化时，先确认该特征线条属于哪一主导方向，利用该特征线条所属的主导方向表示其方向，再求得该特征线条与参考平面的交点，作为特征线条的参数记录在卡尔曼滤波器的状态向量中；

其中参考平面为世界坐标系下两两垂直的XY、YZ或ZX平面，参考平面的选择依据该特征线条与三个平面法线方向的夹角，夹角最小的平面被选为该特征线条的参考平面。

2.如权利要求1所述的基于结构性线条的视觉SLAM方法，其特征在于：SLAM初始化时，对采集的环境图像进行线条检测，根据检测到的线条计算消失点，再依据不同的消失点对检测到的线条进行分类，选择其中线条最多的N类作为主导方向。

3.如权利要求1所述的基于结构性线条的视觉SLAM方法，其特征在于：确定该特征线条属于哪一主导方向时，将三个主导方向所对应的三个消失点分别与该特征线条的中点连接，形成三条参考直线，其中一条所述参考直线与该特征线条共线或接近共线时，则判定该线条为对应主导方向上的特征线条。

4.如权利要求1所述的基于结构性线条的视觉SLAM方法，其特征在于：通过更新所述摄像设备的参数实现所述摄像设备的定位，所述摄像设备的参数至少包括了其在世界坐标系中的位置、姿态，角速度和速度。

5.如权利要求1所述的基于结构性线条的视觉SLAM方法，其特征在于：在找出匹配线条的过程中，先通过几何信息匹配过程和外观信息匹配过程选出匹配的直线，然后再通过局部更新滤波器剔除异常值，得到最佳的一个或多个线段作为匹配线段；

在几何信息匹配过程中，选择与预测线条距离最近，且斜率最相似的若干线段；在外观信息匹配过程中，将线段中点的一块图像块与所述预测线条的图像块进行相似度的计算，选择相似度最佳的若干线段；同时经几何信息匹配过程和外观信息匹配过程选择得到的若干线段作为匹配线段。

6.如权利要求5所述的基于结构性线条的视觉SLAM方法，其特征在于：得到了直线的预测位置及其实际匹配的匹配线段后，计算预测位置与实际位置的偏差时，计算所述匹配线段的两个端点到预测直线的有符号距离，计算过程中，若遇到长线段，则将长线段截取成长度一定的若干短线段，再分别计算。

7.如权利要求1所述的基于结构性线条的视觉SLAM方法，其特征在于：处理完每一帧图像后，还包括进行地图管理的过程，对于每一个主导方向，给定一个直线的最小数目，仅当现有的直线数目小于这个最小数目时，才对该主导方向上的新检测到特征线条进行参数化。

8.如权利要求7所述的基于结构性线条的视觉SLAM方法，其特征在于：在进行地图管理的过程中，用一个变量记录每个特征线条连续未被匹配的次数，该次数会在重新成功匹配后被清零，否则一直累加，当地图中的特征线条数目超过预设上限，则从地图中删除对应未被匹配次数最大的特征线条。

9.如权利要求1所述的基于结构性线条的视觉SLAM方法，其特征在于：所述摄像设备运动过程中，还实时进行特征点检测，通过所述特征线条和特征点共同实现实时定位和地图构建SLAM。