席志红，王洪旭，韩双全

（哈尔滨工程大学信息与通信工程学院，哈尔滨 150001）

（∗通信作者电子邮箱504715799@qq.com）

0 引言

同步定位与地图构建（Simultaneous Localization And Mapping，SLAM）技术随着智能家居机器人和特殊用途工业机器人的应用成为当前机器人技术领域的热点研究之一。目前SLAM 技术已在家庭服务机器人、无人机及无人车等领域大显身手，如：波士顿动力公司研发的家庭服务机器人已经可以实现简单自主导航、开门、扔垃圾等日常任务；大疆公司生产的消费级无人机已经完全实现了视觉定位，视觉避障以及目标跟踪等核心技术。视觉SLAM（Visual SLAM，VSLAM）因主要采用视觉传感器作为数据源而相比激光雷达等其他SLAM技术具有一定价格优势，近年来成为SLAM 研究领域的热点方向之一。

VSLAM 技术根据对图像信息的利用方式可以分为直接法和特征法。直接法是VSLAM研究的一重要方法，它在2011年才被提出，但是经过几年的迅速发展，它已经达到了和特征点法同样重要的地位。Engel 等［1］通过添加回环检测和关键帧管理模块来对半稠密视觉里程计进行改进，并在2014 年提出了LSD-SLAM（Large-Scale Direct monocular SLAM）系统，将直接法应用到了半稠密的单目SLAM 中，并在2015 年提出了分别用于双目相机和全景相机的LSD-SLAM［2］系统。该系统能够构建大尺度的，全局一致性的环境地图，并能够基于直接图像配准得到高度准确的相机位姿。直接法中是耦合的，而在特征点法中则是解耦的。耦合能够更整体性地处理数据关联；而解耦能够在位姿不确定的情况下，仅利用图像信息去解数据关联问题，所以直接法更擅长求解连续图像的定位，而特征点法则更适合回环检测与重定位［3］。基于特征点法的VSLAM 方法是VSLAM 研究另一个重要方向。2007 年，Klein等［4］提出了PTAM（Parallel Tracking And Mapping）系统，并实现了跟踪与建图过程的并行化。2015 年Mur-Artal 等［5］在完全继承PTAM 系统基础上提出ORB-SLAM2（Oriented FAST and Rotated BRIEF SLAM2）系统，该方法围绕ORB（Oriented FAST and Rotated BRIEF）特征计算具有良好的旋转和缩放不变性，提出了包括视觉里程计与回环检测的词袋字典模型，实现实时的回环检测与高鲁棒性的重定位，较好解决直接法的定位丢失问题。

图1 本文SLAM系统框架Fig.1 SLAM system framework in this paper

ORB-SLAM2 系统在特征匹配时采用随机抽样一致（RANdom SAmple Consensus，RANSAC）算法［6］，该算法在图像匹配上应用广泛。在二值化互信息筛选ORB 特征点得到的集合中选择内点，内点比例高，采用RANSAC算法能快速找到最优估计［7］。基于ORB特征进行的RANSAC改进算法来进行图像拼接，在已有特征点基础上根据内点计算变换矩阵H，有效节省匹配时间［8］。在RANSAC 算法基础上改进的渐进一致采样法（PROgressive SAmple Consensus，PROSAC）算法也得到广泛应用。将PROSAC 算法用于剔除图形匹配异常值，在保证数据精度基础上有效提高了采样效率，节省计算时间［9］。但由于RANSAC 算法的随机性特点不得不通过提高迭代次数来提高得到正确模型参数的概率，进而导致了该算法的低效。本文在ORB-SLAM2 系统基础上在特征匹配时采用PROSAC 算法替代RANSAC 算法［10］，PROSAC 算法是对经典的RANSAC 算法的一种优化［11］。相比经典的RANSAC 方法均匀地从整个集合中采样，PROSAC 方法是从不断增大的最佳对应点集合中进行采样的，所以这种方法可以节省计算量，提高运行速度。另一方面，ORB-SLAM2系统只利用稀疏点云地图，而稀疏点云地图并不能在机器人导航中使用［12］，所以本文在原有方法基础上添加稠密点云地图［13］与八叉树地图［14-15］的建立线程，无论稠密点云地图还是八叉树地图都可以用于机器人导航与路径规划。

通过利用TUM（Technische Universität München）数据集的实验结果显示，本文方法与ORB-SLAM2系统对比，本文系统在均方根误差（Root Mean Square Error，RMSE）仅相差0.003 2 m，在进行误匹配对比实验时，在CPU 为Intel i7，内存为16 GB 平台上，RANSAC 算法处理图片速度14 fps（frames per second）左右，PROSAC算法处理速度26 fps左右，图片处理速度是原来的185%。

1 本文SLAM系统

ORB-SLAM系统基于ORB特征实现，包含跟踪定位、局部地图构建、回环检测三个主线程和全局BA（Bundle Adjustment）优化线程，在该系统基础上，本文用PROSAC 算法替代原RANSAC算法，并添加建立稠密点云地图和八叉树地图线程到ORB-SLAM2框架内，本文系统的技术框架图如图1所示。

1.1 系统线程

跟踪线程 这一部分主要工作是从RGB-D 图像中提取ORB 特征，根据提取ORB 特征与上一帧进行匹配或者通过全局重定位初始化位姿，并利用PROSAC 算法进行误匹配的剔除，然后跟踪已经重建的局部地图优化位姿，再确定当前帧是否为新的关键帧。

建图线程 这一部分主要完成局部地图构建。包括对关键帧的插入，验证最近生成的稀疏点云地图点并进行筛选，然后生成新的地图点，使用局部BA（Local BA）优化，最后再对插入的关键帧进行筛选，去除多余的关键帧。

稠密点云地图与八叉树地图构建线程 对稀疏点云地图的稀疏点进行提取，根据稀疏点云进行稠密点地图的构建，并在此基础上构建八叉树地图。在对关键帧进行插入和删除时也对建立的稠密点云地图与八叉树地图进行地图占有概率更新。

闭环检测线程 这一部分主要分为两个过程，分别是闭环探测和闭环校正：闭环检测先使用词袋模型进行探测，然后通过Sim3 算法计算相似变换；闭环校正主要是闭环融合和必要回环的图优化。

1.2 PROSAC算法原理

PROSAC 算法是对经典的RANSAC 算法中采样的一种优化。相比经典的RANSAC 方法均匀地从整个集合中采样，PROSAC 方法是从不断增大的最佳对应点集合中进行采样，根据相似性进行排序的半随机采样不会比完全随机采样的效果更差，所以这种方法可以在保持原有方法的鲁棒性基础上，获得与采用RANSAC 方法的内点基本相同的结果，并使计算速度更快。

PROCAS 方法的成立假设：具有更高相似性的数据点更可能是类内点。

引入评价函数q(uj)值：N个数据点的集合记为uN，根据评价函数，uN内数据点（如ui，uj）被降序满足条件：

根据特征匹配点的欧氏距离的比值β，对评价函数q(uj)定量表示，表达式如下：

式中：dmin表示最小欧氏距离，dmin2表示次小欧氏距离。比值越小，表示特征点匹配质量越好，即评价函数值越大；反之亦然。

将评价函数值最大的前n 个数据点集合记为un，并记集合M，M中元素个数为m。

由于PROSAC 算法不是从所有数据点中进行随机采样，而是需要先对数据点进行排序，然后在评价函数值较高的数据点子集中进行随机采样，这个子集被称为假设生成集，并且这个假设生成集的元素数量是不断增大的，所以PROSAC 算法需要确定生成数据集大小和采样过程的停止策略。

1）确定数据集大小。

生长函数用来确定假设生成集的大小，它既不能过分依赖评价函数的预排序作用，也不能地对所有数据点做相同处理。所以用P(uj)表示数据点uj是正确匹配点的概率，对这个概率和评价函数之间的联系做单调性假设：

用Tn表示中数据点仅来源于评价函数最好的前n个数据点集合un的平均数，则有：

最终它们之间的递归关系为：

Tn个采样点集合只包含un的数据点，Tn+1个采样点集合包含un+1的数据点。由于un+1=un∪{un+1}（un+1是一个数据点），所以Tn+1个采样点集合包括1 个数据点un+1和m -1个来源于un的数据点。因此Tn+1采样点集合是在un中随机抽取m -1个采样点，在此基础上从m 开始到N -1随机取值获得数据点un+1，因此可以快速得到采样点排序后集合

由于Tn值一般是非整数，所以令且有：

所以生长函数可以被定义为：

式中：n表示采样次数大于等于t次时最小的n值。

2）采样过程的停止策略。

如果集合un*中的类内点数In*满足下面条件，PROSAC 算法就停止运算：

非随机性 n*个数据点中的类内点恰好也是任意一个错误模型的类内点的概率小于Ψ（一般为5%）；

极大性 存在一个解在un*中有比In*更多的内点情况，并且在这种情况下k 次采样后还未被发现的概率小于η0（一般小于5%）。

这两个条件中非随机性可以防止由类外点产生的模型也与算法相同。随机类内点集合的元素个数的分布是二项分布：

其中β 为与一个错误模型相对应的点不在采样集合中的概率，并且该采样点包含一个类外点。

在un*中发现一个非随机的结果必须要满足

极大性定义了需要多少次采样才能保证解的置信度，这同样也是RANSAC 算法唯一的停止策略。从un中采样得到的m个点都来自类内点的概率为：

其中：In是un中类内点的数量，εn=In/n 表示类内点数的比例。那么k次采样也都是非类内点的概率为(g(k) ≤n)：

给定概率阈值η0后，采样次数需要满足下列的条件：

算法流程如下所示。

输入 最大迭代次数Im，判断是否为内点的误差极限δ，设定内点数目阈值T，判断是否为内点的误差阈值ε；

输出 单应矩阵H。

1）计算特征点的最小欧氏距离dmin，求解欧氏距离比值β。

1）根据假设生成集g(t)确定n的值。

2）根据β衡量匹配质量，根据匹配质量选取前n个特征点进行降序排列。

3）选取排序后4 个点为一组，计算每组匹配质量之和，再根据质量之和进行排序。

4）在排序匹配对中选取匹配质量最高的4 组匹配点，计算单应矩阵H。

5）将上述4 组匹配点剔除，对其他特征点根据单应矩阵计算相应投影点。

6）计算其他点与投影点的误差e，并与ε 进行比较：如果e ＜ε，表示该点为内点；反之为外点。

7）计算内点数目t与内点数目阈值T进行比较：t ＞T更新内点数目t；否则重复上述步骤，并对迭代次数加1。

8）用更新后t重新计算单应矩阵H及新内点。

9）若迭代次数不大于Im时满足n ＜n*，返回H 及内点集合；否则，找不到模型。

1.3 稠密点云地图与八叉树地图的构建

ORB-SLAM2系统只利用了稀疏点云地图，所以本文在此基础上，利用稀疏点云建立稠密点云地图，并建立八叉树地图。

1）利用RGB-D图像的信息来进行稠密点云地图的构建。

通过定义点序列：X={x1，x2，…，xn}表示空间点的集合，其中xi=[x，y，z]表示点的三维坐标，根据相机模型成像原理：

其中：C 是相机内参，R 和t 是相机的旋转矩阵和平移矩阵，s是深度值和实际空间距离的比例关系因子。

本文利用PCL（Point Cloud Library）软件来处理由RGB-D图像计算来的点云数据。点云数据点pi的pi.x，pi.y，pi.z可以根据深度信息计算：

其中：d 为深度图像m 行n 列像素点的深度值，cx、cy、fx、fy为相机内参。

根据ORB-SLAM2 系统选取的关键帧对应的相机位姿与根据式（16）获取的点云数据创建局部稠密点云地图，对局部稠密点云地图进行点云拼接，得到全局稠密点云地图。但点云地图存在细节冗余导致其体积过大，所以在保留关键地图的基础上建立八叉树地图，可有效缩减地图大小。

2）八叉树地图（octomap）是通过八叉树结构进行信息存储的地图，常用于机器人导航等方向。

八叉树的基本结构如图2 所示。八叉树（Octree）是一种用于描述三维空间的树状数据结构，八叉树的每个节点表示一个正方体的体积元素，每个节点有8 个子节点，这8 个子节点所表示的体积元素加在一起就等于父节点的体积。一般将物体的中心点作为节点的分叉中心。八叉树地图基于八叉树结构对三维空间不断切割，直到无法切割为止。八叉树地图的每个节点都保存了占有率，当某个方块的子节点都没被占或完全被占时，则不用对该方块继续展开，方便数据的压缩。

图2 八叉树原理Fig.2 Principle of octree

八叉树地图构建具体原理如下所示：

假设1 表示节点被完全占据，0 表示节点是空白的，则节点的占据状态可以用0～1 的浮点数x 来表示。为了防止x 超过0～1的范围，可对x进行对数变换，其公式如下所示：

这样将y ∈(-∞，+∞)转换到x ∈[0，1]。用转换概率数值y来表示节点是否被占据，继而通过逆变换即可得到概率x的值，0.5 表示未确定，越接近1 表示该节点被占据的可能性越高。占有概率更新公式如下：

其中：P(n)为节点的先验概率，P(n|zT)为第n 个节点基于T 时刻的后验概率为1 到T 时间段的观测集合，P(n|z1：T)为第n 个节点在1 到T 时间段观测集合的后验概率。通过式（17）对P(n)进行对数变换，则式（19）可以转化为：

通过上述公式，不断对当前的观测数据进行相关融合，实现节点的不断更新，从而实现八叉树地图的构建。

2 实验结果及分析

2.1 RANSAC算法与PROSAC算法的匹配对比实验

为验证基于ORB特征的图片在采用RANSAC与PROSAC算法情况下误匹配剔除的效果，本文设置两组单张图片对比实验。实验平台：系统Ubuntu16.04；CPU Intel-i7；内存16 GB，用Kinect2 相机采集实验室真实环境的RGD-B 图片，将匹配结果用Opencv 软件进行效果显示。实验共做6 组，结果相似，只选取其中一组实验结果展示如图3。

图3 两种匹配算法效果对比Fig.3 Comparison of matching effects of two algorithms

从图3中可以看出，基于ORB特征的直接匹配，匹配对较多，结果存在很多误匹配，而基于ORB 特征的RANSAC 与PROSAC 算法进行剔除误匹配后的匹配对明显减少。用图3图片的匹配时间与误匹配剔除时间作为指标对RANSAC算法与PROSAC 算法进行比较，结果如表1 所示，两种算法获得内点数基本相同，但采用PROSAC 算法无论匹配总时间与误匹配剔除时间都大幅度减少，这在实时VSLAM 系统中有很大作用。

表1 RANSAC与PROSAC算法对相同图片的匹配时间和误匹配剔除时间Tab.1 Matching time and mismatch elimination time of RANSAC and PROSAC algorithms on the same image

2.2 选取RGB-D数据集与误差分析分析标准

本文选用公开TUM 数据集中的RGB-D 相关数据在前文提到过平台进行实验。ORB-SLAM2 系统在相机快速运动时容易出现跟踪丢失情况，即无法正确对相机位姿进行估计与定位，所以引入回环检测来减小累计误差。基于此问题，本文选取有回环和无回环的两组RGB-D 数据进行对比实验，分别是freiburg2_large_no_loop（下文简称fr2_np）和freiburg2_large_with_loop（下文简称fr2_wp）。fr2_np 是一组时长112.37 s，轨迹长度26.086 m序列；fr2_wp是一组时长173.19 s，轨迹长度39.111 m 序列。通常采用TUM 数据集进行相关实验时，会采用以下两个指标作为系统误差评判的标准。

1）绝对轨迹误差（Absolute Trajectory Error，ATE）。直接计算相机位姿的真实值与SLAM 系统的估计值之间的差，代表轨迹的全局一致性，该标准非常适合于评估视觉SLAM 系统的性能。

2）相对位姿误差（Relative Pose Error，RPE）。用于计算相同两个时间戳上的位姿变化量的差。在用时间戳对齐之后，真实位姿和估计位姿均每隔一段相同时间计算位姿的变化量，然后对该变化量做差，以获得相对位姿误差，该标准适合于估计系统的漂移。

2.3 实验结果及误差分析

对于绝对轨迹误差与相对位姿误差进行分析，采用评价指标是均方根误差、平均误差（Mean）、中值误差（Median）和标准偏差（Standard Deviation，Std Dev），其中RMSE 反映估计值与真实值之间的偏差，平均误差反映所有估计误差的平均水平，中值误差代表所有误差的中等水平，标准偏差（Std Dev）反映系统轨迹估计的离散程度。通过以上几种误差分析可以综合地反映系统估计的轨迹与真实轨迹值的误差，并可以借此分析系统的稳定性和可靠性。表2 是ORB-SLAM2 系统与本文系统在ATE 与RPE 分析上的对比。为进一步分析在相对位姿误差上ORB-SLAM2系统与本文系统的差别，本文将两种方法的估计轨迹与实际轨迹进行对比，并绘制相对位姿误差图如图4和绝对轨迹误差图如图5。

图4 fr2_np与fr2_wp序列中ORB-SLAM2与本文系统的相对位姿误差Fig.4 Relative pose error of ORB-SLAM2 and the proposed system for fr2_np and fr2_wp sequences

图5 fr2_np与fr2_wp序列中ORB-SLAM2与本文系统的绝对轨迹误差Fig.5 Absolute trajectory error of ORB-SLAM2 and the proposed system for fr2_np and fr2_wp sequences

可以看出本文系统在剔除误匹配时将时间缩短为原ORB-SLAM2 系统的一半的基础上，保持与ORB-SLAM2 基本一致的绝对轨迹误差和相对位姿误差。从表2 也可以得到，在RMSE 的比较上，本文系统与ORB-SLAM2系统的ATE 差值在0.01 m 以内，平移漂移RPE 差值在0.02 m 以内，旋转漂移RPE差值也在0.5°内，基本相同。

表2 ORB-SLAM2系统与本文系统绝对轨迹误差与相对位姿误差对比Tab.2 Comparison of absolute trajectory error and relative pose error between ORB-SLAM2 system and the proposed system

2.4 稠密点云地图与八叉树地图

在ORB-SLAM2系统中构建的是稀疏点云地图，可以用于机器人定位，却无法进一步应用于机器人导航和机器人路径规划。本文在ORB-SLAM2 系统基础上基于RGB-D 图像信息进行稠密点云地图构建和八叉树地图构建。在ORB-SLAM2系统中已将构建了稀疏点云地图并提取了关键帧，所以本文在此基础上对稀疏点云进行拼接，并基于关键帧的加入和剔除进行了稠密点云地图的局部加入与剔除，在不影响机器人定位的情况下成功建立了稠密点云地图与八叉树地图，为进一步在机器人领域应用做出了一定贡献。因为fr2_np 与fr2_wp数据集是同一场景下的数据，所以建图效果相同，下面只列出fr2_np的建图结果。

图6 fr2_np数据集稠密点云地图和八叉树地图Fig.6 Dense point cloud map and octree map of fr2_np dataset

从图6（a）可以看出，整个数据集的信息保留完整，其中桌子、椅子和墙面等细节可以清楚地看到，说明稠密点云地图的构图效果表现良好，可以应用于机器人的进一步导航等途径。但与图6（b）比较发现，稠密点云地图存在细节过多，和部分冗余的情况，而八叉树地图在保留基本信息的基础上减少了冗余，缩小地图体积，同样表现良好。

3 结语

基于ORB 特征进行轨迹估计与位姿估计时，快速有效的误匹配剔除过程十分重要，为此本文在ORB-SLAM2系统的基础上，采用PROSAC 算法替换原ORB-SALM2 框架中的RANSAC 算法进行误匹配的剔除，在保证与ORB-SLAM2系统绝对轨迹误差和相对位姿误差基本一致的情况下有效提高了误匹配剔除速率。另外，本文针对ORB-SLAM2系统只有稀疏点云地图的不足，添加了稠密点云的构建与八叉树地图的构建环节，可以直接用于机器人的导航与路径规划，这是稀疏点云地图无法提供的。在下一步的工作中，将研究如何构建语义地图与语义地图的定位。