CN106885574A - 一种基于重跟踪策略的单目视觉机器人同步定位与地图构建方法 - Google Patents

Abstract

本发明公开了一种基于重跟踪策略的单目视觉机器人同步定位与地图构建方法。本方法为：1)对每张图片提取ORB特征；2)利用相邻图片的特征匹配跟踪相机姿态；3)对跟踪丢失的图片执行重定位与重跟踪策略；4)若重定位成功执行，则估计当前相机的姿态，且停止执行重跟踪策略并删除其产生的临时变量；5)若重跟踪策略成功执行，则产生一条新的轨迹；6)对产生的轨迹数量进行判定，若数量超过阈值，则将最初始的轨迹剔除；7)对每个关键帧进行闭环检测，检测成功后进行轨迹融合；8)定位系统结束时，对所述轨迹进行筛选，得到一条姿态正确的轨迹。本发明的优势在于能在快速移动、遮挡、纹理不足、光照变化的条件下，定位完整的相机轨迹。

Description

一种基于重跟踪策略的单目视觉机器人同步定位与地图构建 方法

技术领域

本发明属于基于单摄像头的移动机器人自主定位与导航领域，特别涉及一种基于重跟踪策略的单目视觉同步定位与地图构建方法。

背景技术

随着计算机技术和人工智能的发展，机器人的研究得到越来越多的关注和投入，智能自主移动机器人成为机器人领域的一个重要研究方向和研究热点。同步定位与地图构建(simultaneous localization and mapping,SLAM)问题是指移动机器人在未知环境中，利用自身的传感器(如激光雷达、里程计、摄像头、超声波等)创建一个与环境相一致的地图，并同时利用该地图对机器人的位置和姿态进行估计。该问题被认为能否真正实现机器人自主导航的关键问题。

近年来，随着计算机视觉技术的发展以及计算机运算能力的增强，出现了大量利用摄像机作为传感器的视觉SLAM方法。视觉传感器具有信息量大、灵活性高、成本低等优点，这是其他传感器所无法比拟的。单目视觉SLAM仅使用一个摄像头作为传感器完成同步定位与地图创建操作，具有系统结构简单、成本低且易实现等优点。因此，基于单目视觉SLAM技术具有广阔的应用前景。

现有的解决单目SLAM的方法主要分为基于概率框架的单目视觉SLAM方法和非概率框架的单目视觉SLAM方法。基于概率框架的单目视觉SLAM方法主要通过递归的贝叶斯滤波方法估计相机姿态和地图特征的空间位置，常用的解决方法有基于扩展卡尔曼滤波器(extended Kalman filter)或粒子滤波器(particle filter)。非概率框架的单目视觉SLAM系统目前主要是基于关键帧和稀疏捆绑调整等优化算法实现，常用的解决方法有直接法或基于特征的方法。

非概率框架的单目视觉SLAM系统要求相邻两帧的基线较短(small baseline)，以便系统能够估计每帧的姿态。当跟踪丢失(tracking lost)，即当前帧的姿态无法被正确估计时，系统采用重定位策略，只有当机器人回到曾经经过的地点时，SLAM系统才能继续定位相机的位置并重建周围环境。这种策略的缺点在于从跟踪丢失到重定位成功之间，相机的姿态和地图无法重建出来。

发明内容

针对现有技术存在的技术问题，本发明的目的是提供一种基于重跟踪策略的单目视觉机器人同步定位与地图构建方法；本发明致力于解决传统方法中由于使用重定位策略导致跟踪丢失后部分相机姿态和环境无法估计与建模的问题，创新性地提出了一种重跟踪策略，提高了相机姿态估计的成功率以及地图重建的完整率。

本发明采用的技术方案如下：

一种基于重跟踪策略的单目视觉机器人同步定位与地图构建方法，其步骤为：

1)输入摄像头捕获的场景图像，对每张图片提取ORB特征；

2)利用相邻图片的特征匹配跟踪相机姿态并估计特征点的深度信息；因特征匹配失败而导致相机姿态无法被估计定义为跟踪丢失；

3)对所述跟踪丢失的图片执行重定位与重跟踪策略；所述重跟踪策略采用局部初始化算法；

4)若所述重定位成功执行，则估计当前相机的姿态，且停止执行所述重跟踪策略并删除其产生的临时变量；

5)若所述重跟踪成功执行，则产生一条新的轨迹；

6)对所述产生的轨迹数量进行判定，若数量超过阈值，则将最初始的轨迹剔除；

7)对每个关键帧进行闭环检测，检测成功后进行轨迹融合；

8)定位系统结束时，对所述轨迹进行筛选，得到一条姿态正确的轨迹。

上述方法对跟踪丢失这种情况采用了重定位与重跟踪(局部初始化)两种策略。下面进一步具体说明本发明的技术内容：

1.局部初始化

本发明的局部初始化算法改进于ORB-SLAM(参考文献：R.Mur-Artal,J.M.MMontiel,and J.D.Tardos.ORB-SLAM:a versatile and accurate monocular SLAMsystem.IEEE Transactions on Robotics,2015)的自动地图初始化算法。当系统检测到跟踪丢失时，局部初始化的过程如下：

a)查找初始匹配点对：

提取当前帧的ORB特征，搜索当前帧与参考帧(首次迭代时，以跟踪丢失前的最后一个关键帧为参考帧)的特征点匹配个数。若匹配数量大于设定阈值，则执行b)；否则，将当前帧设置为参考帧，重新执行a)。

b)模型选择：

采用RANSAC(Random Sample Consensus，随机抽样一致性)策略，分别用八点法与四点法估计基本矩阵F(Foundamental matrix)与单应矩阵H(Homography matrix)，并计算相应的重投影误差，选择误差小的矩阵作为系统模型。

c)运动估计与地图构建：

利用上述b)中选择的模型，计算相机的姿态以及构建环境地图。

2.轨迹修正与融合

Loop closure即轨迹闭环，是指相机捕获的当前场景与先前走过的场景一样，即相机回到曾经经过的地方。利用该闭环，可以大大消除累计误差。具体步骤如下：

a)计算当前关键帧c与闭环关键帧l的相似变换S_c,l，获取累计误差：

其中，s_c,l为正实数，表明尺度因子；R_c,l与t_c,l分别为旋转矩阵与平移矩阵，Sim(3)为相似变换群。

b)用S_c,l修正当前关键帧的姿态T_c,w：

其中w表示世界坐标系，c表示当前关键帧，l表示闭环关键帧，T_l,w为闭环关键帧l的姿态，为修正后的当前关键帧的姿态。

c)修正当前关键帧所在轨迹的其余关键帧。该步骤分为两种情况：一、若当前关键帧与闭环关键帧属于同一条轨迹，则修正当前关键帧的所有邻接关键帧，该过程与ORB-SLAM的方法一致；二、若当前关键帧与闭环关键帧属于不同的轨迹，则按从后向前的顺序修正当前轨迹上的所有关键帧，并将当前轨迹融合进闭环轨迹。该过程分两步执行，首先计算当前轨迹上相邻关键帧的相对姿态：

其中σ＝(1,2,...,n-1),n为当前轨迹的关键帧数量；是T_c-σ,w的逆矩阵，T_c-σ,w是第c-σ帧在世界坐标系w的姿态。

其次，利用计算出的相对姿态从后向前修正当前轨迹剩余关键帧的姿态：

其中σ＝(1,2,...,n-1),n为当前轨迹的关键帧数量；是ΔT_c-σ+1,c-σ的逆矩阵

d)将当前轨迹的标签更改为闭环关键帧所在轨迹的标签。

3.轨迹删除

实际应用中，遮挡、抖动、纹理不足等状况可能使跟踪丢失的情况频繁出现，进而使SLAM系统产生多条轨迹。然而，SLAM系统只需要输出一条准确的相机轨迹，因此需要删除多余的相机轨迹。

本发明的有益效果是：

本发明提供了一种基于重跟踪策略的单目视觉机器人同步定位与地图构建方法，能够解决传统方法中由于使用重定位策略导致跟踪丢失后部分相机姿态和环境无法估计与建模的问题，通过重跟踪策略提高了相机姿态估计的成功率以及地图重建的完整率。本发明的优势在于能在快速移动、遮挡、纹理不足、光照变化的条件下，定位完整的相机轨迹。

附图说明

图1.本发明方法的总体框架图；

图2.本发明方法中生成新轨迹以及修正与融合轨迹的示意图。

具体实施方式

下面通过具体实施例和附图，对本发明做进一步说明。

参考图1，本发明流程图的具体设计如下：

1.输入摄像头捕获的场景图像：

本发明以30fps的速率处理普通数码摄像机采集的图像。需要注意的是，由于纯旋转运动没有时差，不能估计特征点的深度，因此要求系统开始运行时摄像机有一定的水平运动，且运动速度不宜太快，以便于完成系统的初始化。

2.判断是否跟踪丢失

若通过上一帧图像无法估计出相机姿态，则认为系统跟踪丢失，执行步骤4；否则，执行步骤3。

3.提取ORB特征并估计相机姿态

ORB是一种具有尺度与旋转不变性的特征提取算法。本发明利用开源计算机视觉数据库(OpenCV)中自带的特征点提取算法提取每帧图像的ORB特征。用RANSAC迭代算法与PnP(Perspective-N-Points)算法获取该图像与参考关键帧的特征点匹配以及相机的姿态。若特征点匹配个数太少导致无法估计出相机姿态，则跟踪丢失。

4.判断重定位是否成功

当系统处于跟踪丢失的状态时，需要对每一帧图像执行重定位算法(relocalization)，该算法为现有的一种算法，其将图像转换成词袋模型(bag of words)并且在识别词典(recognition database)中查询匹配的关键帧(keyframe candidates)，若查询到相匹配的关键帧，则说明重定位成功，删除由局部初始化产生的临时变量，然后估计相机的姿态与恢复地图构建，执行步骤6；若查询不到相匹配的关键帧，则说明重定位失败，执行步骤5。

5.判断局部初始化是否成功

若局部初始化成功，系统首先判断轨迹数量是否超过设定阈值T_r，如超过阈值，则首先删除由局部初始化生成的标签最小的轨迹，然后再生成新的轨迹，否则直接从当前位置生成新的轨迹。如图2(a)所示，l₀是系统跟踪的原始轨迹，在帧i处跟踪丢失，由于重定位与局部初始化均失败，相机轨迹出现中断，如虚线b₁所示。在虚线末尾，局部初始化成功，生成新的轨迹l₁。

6.修正与融合轨迹

当系统检测到闭环时，需要修正轨迹的累计误差。若当前关键帧与闭环关键帧分别属于不同的轨迹，还需要将当前轨迹融合进闭环轨迹。如图2(b)所示，当前关键帧j与闭环关键帧k形成闭环，两帧之间存在累计误差，如j与k之间虚线所示。此时，需要修正当前轨迹l₂的误差，并将其融合进轨迹l₀，表示为l'₀，如图2(c)所示。

7.删除冗余轨迹

实际应用中，遮挡、抖动等状况可能使跟踪丢失的情况频繁出现，进而使SLAM系统产生多条轨迹。然而，对轨迹跟踪或者场景重建这类任务来说，SLAM系统只需要输出一条准确的相机轨迹。在这里，冗余轨迹定义为未能与初始轨迹形成闭环的轨迹，若第一次跟踪丢失前的轨迹标签为0，由局部初始化算法生成的轨迹的标签逐渐递增，则在系统即将结束时，需要将标签不为0的轨迹删除。具体方法为：首先在关键帧数据库(keyframe database)上删除冗余轨迹上所有的关键帧，然后在重建地图中(map point)中删除这些关键帧对应的点云图。

以上实施例仅用以说明本发明的技术方案而非对其进行限制，本领域的普通技术人员可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明的精神和范围，本发明的保护范围应以权利要求书所述为准。

Claims (8)

1.一种基于重跟踪策略的单目视觉机器人同步定位与地图构建方法，其步骤为：

1)输入摄像头捕获的场景图像，对每张图片提取ORB特征；

2)利用相邻图片的特征匹配跟踪相机姿态；

3)在跟踪丢失时，对跟踪丢失的图片执行重定位与重跟踪策略；所述重跟踪策略采用局部初始化算法；

4)若所述重定位成功执行，则估计当前相机的姿态，且停止执行所述重跟踪策略并删除其产生的临时变量；

5)若所述重跟踪策略成功执行，则产生一条新的轨迹；

6)对产生的轨迹数量进行判定，若数量超过阈值，则将最初始的轨迹剔除；

7)对每个关键帧进行闭环检测，检测成功后进行轨迹融合；

8)对轨迹进行筛选，删除冗余轨迹，得到一条姿态正确的轨迹。

2.如权利要求1所述的方法，其特征在于，其具体执行流程为：

a)输入摄像头捕获的场景图像；

b)判断是否跟踪丢失，若是则执行步骤c)，否则执行步骤d)；

c)重定位当前图像，若成功，则删除临时变量，执行步骤d)；否则执行步骤e)；

d)提取ORB特征并估计相机姿态；执行步骤g)；

e)局部初始化相机轨迹，若成功，执行步骤f)，否则执行步骤g)；

f)判断轨迹数量，若超过给定阈值，先删除标签最小的轨迹，再从当前位置生成新的轨迹；否则直接从当前位置生成新的轨迹；

g)检测闭环，若形成闭环，则修正并融合轨迹；

h)判断系统状态，若系统即将结束，则删除冗余轨迹；否则返回步骤a)。

3.如权利要求1或2所述的方法，其特征在于，所述局部初始化包括以下步骤：

a)查找初始匹配点对：提取当前帧的ORB特征，搜索当前帧与参考帧的特征点匹配个数，若匹配数量大于设定阈值，则执行b)；否则将当前帧设置为参考帧，重新执行a)；

b)模型选择：采用RANSAC策略，分别用八点法与四点法估计基本矩阵F与单应矩阵H，并计算相应的重投影误差，选择误差小的矩阵作为系统模型；

c)运动估计与地图构建：利用上述b)中选择的模型，计算相机的姿态以及构建环境地图。

4.如权利要求1所述的方法，其特征在于，对图像提取ORB特征，并采用RANSAC策略与PnP算法估计相机姿态。

5.如权利要求1所述的方法，其特征在于，系统在第一次跟踪丢失发生前，产生的轨迹标签设置为0。

6.如权利要求1所述的方法，其特征在于，在检测到闭环后，执行轨迹修正与融合，其方法为：首先计算当前关键帧与闭环关键帧的相似变换矩阵S_c,l；然后用S_c,l修正当前关键帧的姿态T_c,w，获得修正后的姿态然后修正当前关键帧所在轨迹的关键帧；最后将当前轨迹的标签更改为闭环关键帧所在轨迹的标签。

7.如权利要求7所述的方法，其特征在于，修正当前关键帧所在轨迹的关键帧时，分两种情况考虑：一，若当前关键帧与闭环关键帧属于同一条轨迹，则修正当前关键帧的所有邻接关键帧；二，若当前关键帧与闭环关键帧属于不同的轨迹，则按从后向前的顺序修正当前轨迹上的所有关键帧。

8.如权利要求1所述的方法，其特征在于，系统在即将结束时删除冗余的轨迹的方法为：标签不为0的轨迹都属于冗余的轨迹，首先在关键帧数据库上删除这些轨迹上的关键帧；然后在重建地图中中删除这些关键帧对应的点云图。