CN106127739B - 一种结合单目视觉的rgb-d slam方法 - Google Patents

Abstract

本发明公开了一种结合单目视觉的RGB‑D SLAM方法，对于深度传感器获取的数据，都分别采用单目视觉方法和RGB‑D深度视觉方法进行特征提取、特征匹配、运动估计和优化，然后建立单目局部位姿地图和RGB‑D局部位姿地图，机器人在运动的过程中，对单目局部位姿地图和RGB‑D局部位姿地图进行三维地图融合，构建全局地图，并对融合的位姿地图进行闭环检测，优化得到最优的全局地图，本发明是一种实时性高、适应性强、稳定性好的RGB‑D视觉SLAM方法，解决了RGB‑D深度传感器获取无法获取深度信息或者获取深度信息获取不足的问题。

Description

一种结合单目视觉的RGB-D SLAM方法

技术领域

本发明涉及一种结合单目视觉的RGB-D SLAM方法。

背景技术

视觉传感器可以提供，结构紧凑、精确、无创和易于理解视觉信息，同时，除去提供丰富的视觉信息以外，视觉传感器价格低廉，性价比高。目前移动机器人领域中，比较常用的有单目视觉方法和RGB-D SLAM方法。

单目视觉SLAM方法指的是，在机器人运动状态下的，采用一步三回头的方法，利用三角法计算出深度信息，把深度信息放入地图中构建出三维地图环境模型，但是在实际的过程中，深度计算消耗大量计算资源且速度比较慢。

从深度传感器被应用于SLAM领域之后，越来越多的开发者将目标转移到RGB-DSLAM领域中，并且在这上面做了很多工作，但是在传感器获取数据准确度不高的情况下，大环境下SLAM仍然是一个非常具有挑战的问题。因为深度传感器的深度信息有效距离只有4米左右，在环境快速变化的过程中，超过4米的空旷环境下并不一定有深度信息反馈，这给我们RGB-D SLAM带来了许多困难。

发明内容

本发明的目的在于提供一种结合单目视觉的RGB-D SLAM方法，是一种实时性高、适应性强、稳定性好的RGB-D视觉SLAM方法，解决了RGB-D深度传感器获取无法获取深度信息或者获取深度信息获取不足的问题。

参照图1，本发明采用单目视觉RGB信息和RGB-D深度信息分别构建局部地图，然后通过算法对单目SLAM局部地图和RGB-D深度局部地图进行地图融合，融合后经过闭环检测优化，生成全局最优地图，通过单目视觉信息对RGB-D深度信息的结合，使得在地图构建的过程中深度信息获取更加迅速，同时在大环境下空旷地带，以及在深度信息收到影响时，都可以完成SLAM任务，达到良好的实时性和稳定性。其实现的技术方案为：

首先利用深度传感器提供RGB信息I_RGB和深度信息I_Depth分别利用RGB-D SLAM和单目SLAM的方法分别构建局部地图，其中具体内容如下：

1、RGB-D局部地图生成

RGB-D算法包含三个部分：前端、后端以及最终地图，前端主要负责对RGB-D获取的I_RGB和I_Depth信息进行处理，I_RGB信息通过特征提取，描述转换、以及结合I_Depth信息进行特征匹配；后端主要完成，特征转换，图姿优化、闭环检测；最后得到最优的三维环境重建。

(1)前端算法主要分为以下几个部分：特征检测与描述符的提取，特征匹配，状态估计和优化，特征检测和提取可以使用SIFT、SURF、ORB算法，对RGB图像进行特性检测和描述符的提取。

对于训练帧和查找帧中提取出来的描述符，可以采用单匹配算法(BruteForce)或者快速近似最近邻搜索(FLANN)算法，对不同帧之间的特征进行匹配。

一般情况下，在得到匹配信息之后，要对二维的环境模型进行三维重建，因为RGB-D信息包含RGB和深度信息，因此可以很快的从二维的环境模型中重出建三维环境模型。

匹配后的三维点云集合，使用随机采样RANSAC方法对运动进行估计，根据匹配点之间的相对位置变化关系可以估算出一个可信度较高的运动变换模型。

运动变化估计采用RANSAC(随机采样的方法进行采样)，具体过程是在每一次迭代过程中，随机选择少量采样点建立模型，并对每一个数据模型进行评估，采样中小于阈值的符合要求，大于阈值的则重新计算模型，优化过程中选择使用ICP方法，ICP实际上是求解非线性最小值问题，假设有点(p_i,q_i)，则求解模型可以表示为：

min∑_i||(Rp_i+t)-q_i||²

(2)后端算法主要是针对前端获得的初始位姿图进行优化，因为理想的计算总是有差距的，在实际过程中，不断有误差产生，而且每一帧的误差都可能对以后的运动轨迹产生影响，其中使用的最多的优化方法就是回环检测(Loop closure detection)，没有回环检测的时候，误差将对后续计算产生影响，当入回环检测之后，整个运动过程之间就多了一些约束，因此，误差就相对减少了。

在得到运动节点的运动变化之后，利用该运动变化，生成位姿图，位姿图构建完成后，使用Canny算子，对轮廓信息进行提取和储存，再使用回环检测(Loop closuredetection)对位姿图进行回环检测，生成RGB-D局部位姿地图。

2、单目局部地图生成

单目视觉地图一般包含两个部分，参照图2：一个是预测跟踪，另外一个是循环检测和地图构建，追踪部分用来估计相机位置，地图构建部分主要是计算相机深度，并构建三维地图环境。

首先需要对摄像头进行标定，标定的过程中，可以结合深度信息，镜像标定，同时可以利用标定过程，获取尺度信息。

然后预测并且建立运动模型，扩展卡尔曼滤波可以有效应用于预测当中，预测之后对关键点进跟踪，跟踪过程中，不断选择关键点，并对关键点进行校准，为下一次计算和循环检测做准备，同时利用现有的运动图像序列的位姿变化，恢复出深度信息，

循环检测和地图构建部分，利用扩展卡尔曼方法进行滤波，并对标记进行评价，恢复出局部地图。

在此局部地图构建中有两个非常重要的步骤，一个是利用对极几何进行位置估计，另外一个是三角法求深度：

(1)相机位置估计

位姿估计主要使用到对极点几何中的内容，参照图3，

C₀,C₁,P三点共面，可以得到，

同时，p₀,p₁在坐标系C₀,C₁下可以表示为：

和

此时由共面可以得知，p₀·(t×Rp₁)＝0，其中t是相机光心的平移，R表示的是两坐标的旋转变换。通过矩阵变换可以得到：p₀ ^T[t]_xRp₁＝0。

在此定义本征矩阵E为[t]_xR，是一个3×3的矩阵，表示为：

通过本征可以恢复出平移向量t和坐标旋转R。

(2)三角法求深度

在已知计算机姿态的情况下，可以通过三角法，求出三维空间中的一点P的深度，假设C₁为世界坐标系，P点可以表示为P＝(x,y,z,1)^T。C₁外参数矩阵为单位矩阵M₁，C₂的外参数矩阵可以表示为M₂：

通过矩阵变换和最小二乘法求解线性方程组的方法，求解出P点的三维坐标，就是世界坐标。

3、地图融合

当局部地图建立完成之后，控制管理器选择两种地图，进行创建，当有深度信息的时候，选择使用RGB-D SLAM进行局部地图构建，并将局部地图放入到全局地图中；当深度信息缺失，或者深度信息不足的时候，使用单目SLAM的方法，构建局部地图并放入到全局地图中，在每个局部题图中建立一些关键帧，以便之后的循环检测，然后对全局地图进行融合和循环检测，最终生成最优的全局地图，其整体过程参照图4。

控制管理过程需要根据图像中获得的深度信息反馈，来决定是否单独使用RGB-D地图还是使用单目RGB地图，同时更加深度信息反馈结果，决定是否把单目RGB局部地图融合进入RGB-D局部地图中，融合过程中采用共有特征点进行优化，对于不相同的特征点进行保留，同时对于相同的特征点，加入权值进行匹配，当有深度信息并且深度信息比较完整的时候，选择使用RGB-D SLAM进行局部地图构建，并将局部地图放入到全局地图中；当深度信息获取不完整，误差比较大的时候，使用单目SLAM的方法对把两种地图进行融合，在加入到全局地图构建当中；当没有深度信息的时候，使用单目局部地图构建全局地图。

对于非共有特征完全保留，因为在现实过程中，RGB-D提供精确度较高的深度信息，而实际在大多数情况下，单目视觉RGB局部地图中的深度信息误差较大，但是可以利用RGB信息中深度信息对RGB-D深度缺失进行弥补，提高RGB-D SLAM方法稳定性和适用范围。

两种方法结合的SLAM算法中，包含了两种类型的局部地图，其中一种是使用深度信息和RGB信息创建的局部地图；另外一种是单目RGB地图，使用单目RGB图像信息构建地图，其选择控制与融合过程参照图5。

地图构建过程中，同时生成了RGB-D局部地图和单目RGB地图，优化过程中，把单目RGB地图加入到RGB-D地图中，并对地图进行融合生成全局最优化地图，其具体过程如下：

首先确定两幅地图中特征点位置，然后对特征点位置进行融合匹配，共有的特征点进行特征匹配，对于不同的特征点进行保留。

同时使用Canny算子对图像中的轮廓进行提取，对提取的并对轮廓进行标记，同时对两个局部地图通过ICP的方法进行点云精确配准。

配准之后，对不同视角的点云进行融合面元更新、增加、移除，以及对提取出的纹理信息进行纹理映射操作，生成新的三维全局地图。

本发明的技术效果是：本发明是一种实时性高、适应性强、稳定性好的RGB-D视觉SLAM方法，解决了RGB-D深度传感器获取无法获取深度信息或者获取深度信息获取不足的问题。

附图说明

图1为结合单目视觉的RGB-D SLAM原理图。

图2为单目视觉下的局部SLAM地图构建过程图。

图3为单目视觉SLAM方法中对极几何原理图。

图4为结合单目视觉的RGB-D SALM详细过程图。

图5为选择控制以及地图融合过程图。

具体实施方式

下面将结合附图1—5和实施例详细说明本发明所具有的有益效果，旨在帮助阅读者更好地理解本发明的实质，但不能对本发明的实施和保护范围构成任何限定。

一种结合单目视觉的RGB-D SLAM方法，包括以下内容：

1、分别采用单目视觉方法和RGB-D深度视觉方法进行单目局部位姿地图和RGB-D局部位姿地图进行构建。

在工作的过程中，深度传感器可以获得两种信息，一种是RGB彩色信息I_RGB和深度信息I_Depth，其中这两种信息分别来自不同的通道。然而深度传感器的实际工作距离有限，因此不是任何时候都有深度信息反馈，提出的一种结合单目视觉的RGB-D SLAM方法中，同时对两种方法进行联合，有深度信息情况下，使用深度信息构建局部地图。同时，利用RGB信息构建单目视觉地图，地图构建部分主要分为以下两个部分：

(1)单目局部位姿地图构建

SLAM的具体问题可以描述如下：在位置的场景地图m中，已知初始化位置x₀，控制信息U^k，然后确定移动机器人的位置x_k，同时构建地图m。

在工作的过程中，采用扩展卡尔曼解决SLAM问题。首先要对系统进行初始化，主要包括相机内外参数初始，全状态向量和协方差初始化，以及滤波器初始化等。初始过程中，采用张正友提出的方法进行标定，同初始过程中还得选取初始化图像帧，并获取SIFT特征分布。

运动模型和观察模型用p(x_k|x_k-1,u_k)和p(z_k|x_k,m)表示，在SLAM的过程中，摄像机状态和x_k和路标信息y_i都需要不断进行估计和更新。SLAM全状态的EKF(扩展卡尔曼滤波)过程一般分为预测和更新两个步骤，先对状态进行预测，预测完成之后更新。如图2所示，是整个单目SLAM局部地图的生成过程。

位姿估计主要使用到对极点几何中的内容，参照图3，

C₀,C₁,P三点共面，可以得到，

同时，p₀,p₁在坐标系C₀,C₁下可以表示为：

和

此时由共面可以得知，p₀·(t×Rp₁)＝0，其中t是相机光心的平移，R表示的是两坐标的旋转变换。通过矩阵变换可以得到：p₀ ^T[t]_xRp₁＝0。

在此定义本征矩阵E为[t]_xR，是一个3×3的矩阵，表示为：

通过本征可以恢复出平移向量t和坐标旋转R。

同时，在已知计算机姿态的情况下，可以通过三角法，求出三维空间中的一点P的深度，假设C₁为世界坐标系，P点可以表示为P＝(x,y,z,1)^T。C₁外参数矩阵为单位矩阵M₁，C₂的外参数矩阵可以表示为M₂：

通过矩阵变换和最小二乘法求解线性方程组的方法，求解出P点的三维坐标，就是世界坐标。

然后继续扩展卡尔曼滤波的方法，进行状态估计和更新，更新的过程中，把数据关联不一致的新环境特征集合，添加到系统的状态向量里，实现全局地图的更新。

(2)RGB-D局部位姿地图构建

RGB-D算法包含三个部分：前端、后端以及最终地图。

前端主要负责对RGB-D获取的I_RGB和I_Depth信息进行处理，I_RGB信息通过特征提取，描述转换、以及结合I_Depth信息进行特征匹配等，主要分为以下几个部分：特征检测与描述符的提取，特征匹配，状态估计和优化等。

特征检测和提取可以使用SIFT、SURF、ORB算法，对RGB图像进行特性检测和描述符的提取。在具体实现的过程中，采用了SIFT特征，因为SIFT包含的信息量大，同时，在和单目局部地图中所采集的特征点相同，方便融合。

对于训练帧和查找帧中提取出来的描述符，可以采用单匹配算法(BruteForce)或者快速近似最近邻搜索(FLANN)算法，对不同帧之间的特征进行匹配。在实现的过程中，快速近似最近邻搜索(FLANN)效果明显要优于单匹配法，快速近似最近邻算法，精度更高，计算量小，运行速度较快。

一般情况下，在得到匹配信息之后，要对二维的环境模型进行三维重建。因为RGB-D信息包含RGB和深度信息，因此可以很快的从二维的环境模型中重出建三维环境模型，存在一个像素点m:(u,v)，同时针对像素点m所对应的深度信息值为depth，相机的焦距为focal_length，那么像素点m所对应的三维环境模型可以通过下列公式求得：

匹配后的三维点云集合，使用随机采样RANSAC方法对运动进行估计，根据匹配点之间的相对位置变化关系可以估算出一个可信度较高的运动变换模型。

运动变化估计采用RANSAC(随机采样的方法进行采样)，具体过程是在每一次迭代过程中，随机选择少量采样点建立模型，并对每一个数据模型进行评估。采样中小于阈值的符合要求，大于阈值的则重新计算模型。优化过程中选择使用ICP方法，ICP实际上是求解非线性最小值问题，假设有点(p_i,q_i)，则求解模型可以表示为：

min∑_i||(Rp_i+t)-q_i||²

后端主要完成特征转换，图姿优化、闭环检测等，主要是针对前端获得的初始位姿图进行优化，因为理想的计算总是有差距的，在实际过程中，不断有误差产生，而且每一帧的误差都可能对以后的运动轨迹产生影响。

其中使用的最多的优化方法就是回环检测(Loop closure detection)。没有回环检测的时候，误差将对后续计算产生影响，当入回环检测之后，整个运动过程之间就多了一些约束，因此，误差就相对减少了。

具体过程为，在构建好位姿地图之后，对于位姿地图中的每一个节点，查看与节点对应的观测结果与之前的最近的M个节点对应结果是否相似。如果两个结果足够相似，则对两结果运动关系进行评估，如果满足闭环约束阀值，则求出相对于的运动变化关系。

在添加闭环约束以后，需要对整个位姿地图进行优化，优化过程中，选择目前比较流行的g2o框架进行优化，得到局部最优地图。

2、机器人在运动的过程中，对单目局部位姿地图和RGB-D局部位姿地图进行三维地图融合，构建全局地图，并对融合的位姿地图进行闭环检测，优化得到最优的全局地图。

当局部地图建立完成之后，两种类型的局部地图，其中一种是使用深度信息和RGB信息创建的局部地图；另外一种是RGB地图，使用图像信息构建地图，其总体方法过程如图5所示。

控制管理过程需要根据图像中获得的深度信息反馈，来决定是否单独使用RGB-D地图还是使用单目RGB地图。同时加入深度信息反馈结果，决定是否把单目RGB局部地图融合进入RGB-D局部地图中。融合过程中采用共有特征点进行优化，对于不相同的特征点进行保留，同时对于相同的特征点，加入权值进行匹配。

因为不是任何时候，传感器都能获得深度数据，因此，控制管理器根据深度信息反馈，对不同情况下的两种地图进行选择创建，构建全局地图。当有深度信息并且深度信息比较完整的时候，选择使用RGB-D SLAM进行局部地图构建，并将局部地图放入到全局地图中；当深度信息获取不完整，误差比较大的时候，使用单目SLAM的方法对把两种地图进行融合，在加入到全局地图构建当中；当没有深度信息的时候，使用单目局部地图构建全局地图。

其中比较关键的是，在深度信息缺少，误差比较大时，要有一定的判断准则，其实我们是根据SIFT特征点的深度数据来判断的，在特征点的深度数据大于70％，则认为深度信息丰富，当深度信息小于20％则认为无深度信息。

构建局部地图并放入到全局地图中，在每个局部题图中建立一些关键帧，以便之后的循环检测。然后对全局地图进行融合和循环检测，最终生成最优的全局地图。

地图构建过程中，同时生成了RGB-D局部地图和单目RGB地图，优化过程中，把局部地图加入到全局地图中，并对地图进行融合生成全局最优化地图，其具体过程如下：

点云配准：配准的过程就是把不同的点云数据集，对齐到一个统一完整的模型中，其基本方法有ICP(迭代最近点)和最近邻搜索算法。在实施实例中，采用ICP的方法。先，确定两幅地图中SIFT特征点位置，然后对特征点位置进行融合匹配，共有的特征点进行特征匹配，对于不同的特征点进行保留，同时去除噪声点；再对误差进行度量和最小化。对于非共有特征进行保留，因为在现实过程中，RGB-D提供精确度较高的深度信息，而实际在大多数情况下，单目视觉RGB局部地图中的深度信息误差较大，但是可以利用RGB信息中深度信息对RGB-D深度缺失进行弥补，提高RGB-D SLAM方法稳定性和适用范围。

配准之后，对不同视角的点云进行融合面元更新、增加、移除，以及纹理映射等操作。在循环检测之前，采用Canny算子对图像中的轮廓进行提取，对提取的并对轮廓进行标记，通过纹理映射，加入纹理信息，得到最优的全局地图。

以上所述的实施例仅仅是对本发明的优选实施方式进行描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案作出的各种变形和改进，均应落入本发明权利要求书确定的保护范围内。

Claims (8)

1.一种结合单目视觉的RGB-D SLAM方法，其特征在于，包括以下几个步骤：对于深度传感器获取的数据，都分别采用单目视觉方法和RGB-D深度视觉方法进行特征提取、特征匹配、运动估计和优化，然后建立单目局部位姿地图和RGB-D局部位姿地图，机器人在运动的过程中，对单目局部位姿地图和RGB-D局部位姿地图进行三维地图融合，构建全局地图，并对融合的位姿地图进行闭环检测，优化得到最优的全局地图；

其中，在构建全局地图时，包含一种选择控制机制；所述选择控制机制依据传感器获得的深度信息，选择融合过程选择哪种局部地图，根据判断准则，当有深度信息并且深度信息比较完整的时候，选择使用RGB-D SLAM进行局部地图构建，并将局部地图放入到全局地图中；当深度信息获取不完整、误差比较大的时候，使用单目SLAM的方法对把两种地图进行融合，在加入到全局地图构建当中；当没有深度信息的时候，使用单目局部地图构建全局地图。

2.根据权利要求1所述的一种结合单目视觉的RGB-D SLAM方法，其特征在于，在RGB-D局部位姿地图中，采用SIFT、SURF、ORB算法中任意一种，对RGB图像进行特性检测和描述符的提取，对于训练帧和查找帧中提取出来的描述符，采用单匹配算法或者快速近似最近邻搜索算法，对不同帧之间的特征进行匹配。

3.根据权利要求1所述的一种结合单目视觉的RGB-D SLAM方法，其特征在于，在RGB-D局部位姿地图中，运动变化估计采用随机采样的方法进行采样，优化过程中选择使用ICP方法，对于点(p_i，q_i)，则求解模型可以表示为：min∑_i||(Rp_i+t)-q_i||²，其中R表示两坐标的旋转变换，t表示相机光心的平移；在运动的过程中加入闭环检测，不断对位姿进行修正，以减少误差，最后采用g2o的算法进行位姿图优化，对优化后的位姿进行提取，然后进行三维局部地图重建。

4.根据权利要求1所述的一种结合单目视觉的RGB-D SLAM方法，其特征在于，在单目视觉方法中，先对摄像头进行标定，并采用SIFT算法进行特征提取和特征匹配，匹配完成后使用扩展卡尔曼滤波完成下一时刻系统状态预测。

5.根据权利要求1所述的一种结合单目视觉的RGB-D SLAM方法，其特征在于，在单目视觉方法中，采用对极集合的方法进行位置状态估计，并用三角法求解深度信息。

6.根据权利要求1所述的一种结合单目视觉的RGB-D SLAM方法，其特征在于，在构建全局地图过程中，采用ICP的方法对两种不同的地图点云进行配准，并对配准后的点云进行选择性的融合。

7.根据权利要求1所述的一种结合单目视觉的RGB-D SLAM方法，其特征在于，在闭环检测过程，先对图像中轮廓进行提取，再进行回环检测。

8.根据权利要求7所述的一种结合单目视觉的RGB-D SLAM方法，其特征在于，轮廓进行提取中使用Canny算子，对RGB图像中轮廓进行提取，把提取出的纹理信息映射三维模型当中，构建全局最优的三维地图。

Images