改进3D SLAM算法在移动机器人上的应用

于志鹏，蒋 林

（武汉科技大学机械学院自动化学院，湖北 武汉 430081）

1 引言

移动机器人是最早出现的智能机器人之一，多个领域有广泛的使用，这些任务都需要移动机器人进行实时的定位于建图（SLAM）。一般移动机器人需要通过多种传感器设备，如激光雷达、相机、GPS、陀螺仪、高精度码盘、红外和超声传感器等，用于获取自身以及周围环境信息。虽然搭载这些传感器设备可以提高系统的精确性星和鲁棒性，但也会造成系统过去复杂，同时造价高昂，为大规模的推广应用设置了较高的技术门槛。

一种RGB-DSLAM算法在2010年由文献[1]于2010年首次提出，方法应用于移动机器人的实时定位与建图（SLAM）。在2012年，一种RGB-D SLAM方法由文献[2]提出，该方法通过Octomap library创建环境的体素地图并采用g2o[3]进行图优化。传统的三维建模的方法是通过激光扫描或者可探深度摄像头来提供稠密的环境点云，或者将物体表面分割，建立匹配关系，分析构造出光滑的表面模型[4]。新型的RGB-D传感器，如Microsoft Kinect，可以直接提供密集的深度图像和彩色图像。然而，由于大量的内存消耗，最初的3D SLAM的速度和效率都不是很好。

主要从四个方面对传统RGB-D SLAM进行改进。（1）使用主成分分析法对SURF生成的特征点描述子进行降维处理。（2）将图像分割成若干个小区域，在每个区域内提取改进的SURF特征点并且限制每个区域内的最大特征点数，对候选点的搜索区域进行限制。（3）利用RATB-Map方案[5]，对全局地图进行三维重建，提高三维SLAM的实时性。

2 基于主成分分析的改进特征点检测

所提出的改进SURF算法与传统SURF算法在亚像素位置与主方向方面的描述相同，不同的是在描述子的生成，在提取到特征描述子之后，使用主成分分析法将特征点描述子进行降维处理。虽然此方法比传统的SURF特征检测步骤多了一个PCA降维过程，但在后续的特征点匹配部分运算速度更快。

基于主成分分析的SURF特征点检测主要有5个步骤：（1）建立 Hession 矩阵；（2）构建尺度空间；（3）特征点定位；（4）特征点主方向匹配；（5）使用传统的SURF算法生成特征点描述子；（6）使用主成分分析法对特征点描述子降维。

图像中某个像素点的Hession矩阵的定义为：

式中：Lxx（x，y，σ），Lxy（x，y，σ），Lxx（x，y，σ）—高斯二阶偏导数与该点的卷积；σ—二阶高斯滤波器的尺度因子。即：

式中：I（x，y）—图像函数；σ—尺度因子且σ=1.2；g（σ）—二维高斯函数。

为了提高运算速度，当使用盒式滤波器时利用可以利用积分图像加速运算。

因为盒式滤波器是近似代替二阶高斯滤波。所以通过引入权值来平衡准确值与近似值之间的误差，所以：

式中：Lxy，Lxx—高斯二阶偏导数与该点的卷积；Dxx，Dxy—盒式滤波器与该点的卷积。

因此Hessian矩阵的判别式为：

式中：det H—Hessian矩阵的判别式；Dxx，Dxy—盒式滤波器与该点的卷积。

根据判别式求得的值来判断该点是否是极值点。此步骤与SURF提取特征点的步骤相同。不同的是计算出相对于主方向的垂直与水平方向的harr小波特征，水平方向之和，水平方向绝对值之和，垂直方向之和，垂直方向绝对值之和构成一个4维向量[6]。把这4个值作为每个子块区域的特征向量，所以一共有4×4×4=64维向量作为SURF特征的描述子。

使用主成分分析法对特征点描述子降维的步骤为：

（1）将n个特征点的64维描述子组成64行n列的矩阵X，X=［x1，x2，…xn］；

（2）计算n个特征向量的平均值

（3）将X的每一行进行零均值化，得到零均值向量di=xi-x，i=1，2，…n；

（5）计算出Q所对应的64个特征值λi和特征向量ei；

（6）将特征向量按对应特征值大小从大到小按行排列成矩阵，取前k行组成矩阵P；

（7）Y=PX即为降维到k维都的数据。

如果k等于18，那么矩阵P的大小为18×64，xi的大小为64×1，所以 P×xi为 18×1 的矩阵，即 yi的维度为 18，把 64 维的传统SURF特征描述符降维到了18维[7]。

3 区域分块匹配的特征点匹配

首先将图像分割成若干个小区域，然后在每个区域内分别提取改进后的SURF特征点且限制每个区域内的最大特征点数，例如 Kinect获得的（640×480）像素的图像可以以（80×80）像素为子区域大小分割为48个区域，在每个区域内只保留最大25个强度（response）最高的特征点。这种方法能够在图像上提取到分布更加均匀的特征。目前移动机器人实时定位与建图的过程比较缓慢，平移和旋转时产生的位移和角度都比较小，所以Kinect采集到的前后两帧图像的偏移量都较小，在匹配阶段也对候选点的搜索区域进行限制。假设目标帧及源帧图像分别为It、Is、pt为It上一特征点且其图像坐标为It（x，y），则定义pt在上Is的搜索区域为：以Is（x，y）为中心，nσ为半径的圆（σ为图像分割尺度，n为大于0的整数，这里取n=3）。这种限制搜索范围的策略不但节省了匹配所需的时间，同时也避免了一些位置差异较大的误匹配。此外，采用FLANN（Fast Approximate Nearest Neighbor Search）算法进行双向匹配[8]。

4 高效的3D SLAM

RTAB-MAP是一个与时间无关的基于外观的定位与建图建图方案。当一个闭环被采用时，一个新的约束被添加到地图的图优化中，图优化器就会将地图中的错误最小化。内存管理算法用于限制闭环检测和图形优化的位置和数量。在该方案中，地图的结构是一个具有节点和链接的图。地图中的每个位姿由节点保存。在节点中包含了用于闭环检测的RGB图像与深度图像。记忆由STM（短期记忆），WM（工作记忆）和LTM（长期记忆），如图1所示。

图1 RTAB-Map的内存管理模型Fig.1 RTAB’s Memory Management Model

当获得新数据时，STM是添加到图中的新节点的入口点，它是一个固定大小的S。STM中的节点不考虑环路闭合检测。当STM大小到达S节点时，储存时间最长的节点被移动到WM。WM的大小间接确定为一个固定的时间限制T。当新的数据需要处理的时间到达T时，图中的某些节点从WM转移到LTM，从而是WM保持几乎恒定。

5 SLAM后端

基于g2o框架优化姿态图，创建全局一致的轨迹。g2o[3]框架是一个易于扩展的图形优化器，可以应用于广泛的问题，包括多种SLAM和光束平差法的变体。

（5）中描述了非线性误差函数的最小化。

通过后端的非线性误差函数e（xi，xjzij）计算成对变换，使得xi和xj的位姿满足约束zij。

当重新访问到地图的已知部分时，全局优化在闭环的情况下特别有利。因为图优化中的闭环可以减少累计误差。

6 实验结果及分析

实验使用的移动机器人平台为旅行家II号机器人，RGB-D相机为微软Kinect相机。系统环境为：英特尔I7 4590 CPU，DDR3 8GRAM的Ubuntu14.04平台上。本实验用到的旅行家II号机器人与Kinect，如图2所示。提出的改进SURF特征点检测与匹配算法与传统SURF算法所提取到的图像的实验结果，如图3、图4所示。实验比较可知，传统SURF检测到的特征点数量为75个，检测用时0.310s，匹配用时0.195s，准确率为92.7%，总共用时0.505s。改进SURF检测28个特征点，消耗时间0.375s，匹配用时为0.027s，准确率为：92.8%，总的消耗时间为0.402s，相比于传统SURF，改进SUFR算法能够降低特征点检测与匹配的总时间，并且匹配的准确率与传统的SURF的准确率相等。

图2 本实验用到的旅行家II号机器人与KinectFig.2 The Traveler II Robot and Kinect Used in This Experiment

图3 传统SURF特征点检测与匹配结果Fig.3 Traditional SURF Feature Point Detection and Matching Results

图4 改进SURF特征点检测与匹配结果Fig.4 Improved SURF Feature Point Detection and Matching Results is Proposed in This Paper

为了对所提出的算法进行测试，对从Kinect提取到的30组图像进行特征点检测与匹配，实验结果，如图5所示。根据图5可知进行区域分块后，虽然匹配的对数减少，但是匹配正确率与分块前的准确率相等，并且据实验统计，特征点匹配阶段所消耗的时间减少了约20%。

图5 区域分块搜索前后的比较Fig.5 Comparison of Region Block Search Before and After

特征点检测与匹配的平均时间进行测试的结果，如表1所示。由表1可知，因为引入了主成分分析方法（PCA），使得在生成特征点描述子的时候耗时比传统SURF要长，但是因为在特征点匹配时的特征描述子的维度降低以及特征点匹配阶段采用了区域块分割，所以在特征匹配阶段用时更少，总体的耗时更少。虽然使用ORB的特征提取比使用SURF与SIFT描述符的特征提取快得多。但是在误差上比其他两种更高；利用SURF和SIFT特征点实现的RBG-D SLAM误差相当，均优于ORB方法，但是SURF方法相比于SIFT方法更快些。所以综合考虑，这里的方法更优。

表1 特征检测与提取的时间分析Tab.1 Time Analysis of Feature Detection and Extraction

为了验证测试所提出的RGB-D SLAM的RTAB-MAP方法所建立的地图是否具有全局一致性，采用NYUv2中的数据集作为测试数据。传统RGB-D SLAM生成的点云图，如图6所示。采用这里方法所得出的点云地图，如图7所示。相比而言这里所提出的RGB-D SLAM方法生成的点云信息更加清晰且全局一致性更好，冗余地图结构更少。因为该数据集中有人物的移动，会对生成的点云地图产生较大影响，采用这里的优化算法可以使所生成的点云图中的移动物体具有清晰的轮廓，如图8所示，此结果可以为后续的动态物体识别提供先决条件。为验证所提出的3D SLAM系统框架的实时性与鲁棒性，采用慕尼黑工业大学计算机视觉团队的RGB-D SLAM的测试数据集进行实验。这里的方法与传统方法的对比结果，如表2所示。从表2中可以看出这里的RGB-D SLAM方法在ATE误差与RPE误差两项数据均小于传统RGB-D SLAM方法。结果表明，这里的RBG-D SLAM相比于传统得RBG-D SLAM方法在实时性与鲁棒性以及地图的全局一致性具有更优越的表现。

图6 传统RGB-D SLAM生成的点云地图Fig.6 Traditional RGB-D SLAM Generates Point Cloud Maps

图7 所提出的RGB-D SLAM方法生成的点云地图Fig.7 The Point Cloud Map Generated by the RGB-D SLAM Method Proposed

图8 优化算法对动态物体影像图Fig.8 Optimization Algorithm for Dynamic Object Image Proposed in This Paper

表2 对比传统RGB-D SLAM方法在不同数据集上的ATE与RPE误差Tab.2 Comparison of ATE and RPE Errors for Traditional RGB-D SLAM Methods on Different Data Sets

7 结语

提出了一种应用于移动机器人的高效的RGB-D SLAM算法，提高了机器人在实时定位与建图过程的实时性与鲁棒性。利用主成分分析法（PCA）对特征描述子进行降维处理，同时将图像区域分块匹配，限制所划分的区域内的特征点数目。使用RTABMAP方案来实现RGB-D SLAM的建图效率，在后端，g2o用于机器人的轨迹和全局地图优化。实验结果表明，这里所提出的改进SURF算法可以加快RGB-D SLAM的特征提取速度，相比于传统RGB-D SLAM的实时性与鲁棒性以及全局一致性更高。