基于RGB-D相机的SLAM算法优化

陈宇翔，王 仁，张 健

(中国航天科工集团第二研究院 706所，北京 100854)

0 引 言

装备维修的传统方式依赖于纸质或电子维修手册，造成了维修时的同步性差、可视性效果不强、维修效率低下等问题。将增强现实技术用于主导式诱导装备维修建模时，可以提高维修保障的效能，提升装备保障维修过程的智能化程度和人机交互能力。此应用的关键技术之一，便是SLAM(simultaneous localization and mapping)，即时定位与地图构建技术。在实际增强现实维修建模中，常出现两个问题：一是三维场景建模速度慢；二是当维修员将视线再次移回场景时出现虚拟信息漂移。这是因为在把增强现实技术用于装备维修时所用的头戴设备硬件资源受限，计算能力不足。另一方面，相比于移动机器人领域，在增强现实技术领域中，使用的摄像头运动更加快速，自由度变得更加多样，真实环境纹理更复杂，对光照条件更苛刻。因此需要对移动机器人领域的视觉SLAM进行优化以满足增强现实维修建模的实时性和回环检测要求。

为解决上述问题，本文在相邻帧特征匹配过程中，将传统的SIFT特征描述符结合主要素提取法，转换成低维的PAC-SIFT特征描述符，在匹配时采用优化后的距离代替复杂度更高的欧氏距离，从而提高特征匹配的速度。另一方面，利用RGB-D相机Kinect可同时获取彩色与深度信息的优势，设计优化算法得到场景的轮廓，并利用轮廓匹配进行回环匹配检测，从而减少用于回环检测词袋模型中视觉单词的总数量，最终提高回环检测环节的实时性和鲁棒性。

1 算法原理与系统框架

一个视觉SLAM系统总体可以分为两个环节：前端和后端。前端用摄像头获取到场景信息之后，通过VO(visual odometry)[1]，即视觉里程计来增量式地计算出摄像头的位姿与两个时刻间的相对运动。后端负责两个功能：一是不断地通过与前几帧比较，修正上一环节中产生的累积误差。二是判断是否出现回环，即摄像头是否已检测过该位置。如果发现回环，则要修正SLAM过程中的摄像头的位姿。总结起来就是，前端快速探索定位，后端缓慢维护地图。

对于VO部分，求解位姿变换的核心是要得到一个描述三维空间的变换矩阵。主流方法是基于提取图像特征，即对于极短时刻摄像头获取到的两帧图像，提取两帧图像对应的特征点，然后计算出对应的特征描述符，利用这两组特征点进行特征匹配。之后利用求解PnP问题[2]的方法计算出相机的变换矩阵，即得到帧间的运动估计。

得到局部帧间运动估计后，把整个运动过程拼接起来，就得到了摄像头的运动轨迹。但是在VO过程中存在累计误差，最初一个微小的误差，都会影响后面的每一步精度。这样，最终得到的轨迹可能发生严重的漂移。所以在VO环节之后需要进行后端优化。早期求解SLAM利用了滤波器的特性[3]，将SLAM过程分为了两个方程：运动和观测。然后以这两项的噪声来构建目标方程，使目标方程最小化的过程便优化了运动轨迹。后来，借鉴SFM(structure-from-motion)问题，逐渐把BA(bundle adjustment)即集束调整带入了SLAM求解过程[4]。其考虑先前的每一帧图像的信息，把最终的误差经过平均分到过程中的每一次观测中进行优化。为了表示方便，经常用图(即点和边)的形式来表示集束调整，因而亦称为图优化方法，传统上可以利用稀疏代数对图优化进行求解。目前通用的方法是利用g2o(generalized graph optimizer)[5]，即图优化通用求解器对图优化问题进行求解。

当摄像头识别出已检测过的场景时，表示进行了一次回环检测。如果SLAM算法能够识别成功，则可以十分有效地修正位姿与轨迹，从而显著地减小整个VO过程中产生的累积误差。回环检测的本质是图像信息相似性的快速检测。当前用于检测回环的方法有两大类：基于概率和基于图像匹配。前者通过递归贝叶斯估计方法[6]检测回环，但是在检测时常忽略环境重要信息，因而鲁棒性不强，且该算法计算量高，实时性不高。后者则是利用图像的帧间匹配来检测回环，当当前帧与已经检测过的场景图像的相似度高于阈值时即判定为回环。由于图像匹配较费时间，因而本文采用了BoW(bag of words)即词袋模型[7]代替图像进行匹配。

本文利用Kinect对传统视觉SLAM方法中计算费时的环节进行了优化。通过将帧间匹配的SIFT算法结合主要素提取方法，对特征描述符合理降维，并优化特征匹配距离，从而减少帧间匹配运算量；同时利用Kinect获取到的轮廓减少关键帧的数目;最后在回环检测中利用轮廓粗匹配以减少词袋模型中用于聚类的特征描述子数量。通过以上优化，使得视觉SLAM的实时性得以提高，回环性能更优。

本文提出的基于Kinect的视觉SLAM算法的总体框架如图1所示。

图1 本文提出的视觉SLAM框架

2 特征提取与匹配优化

在VO环节，通过将帧间匹配的SIFT算法结合主要素提取方法，对特征描述符合理降维，并优化传统的特征匹配距离，从而减少帧间匹配的运算量。

2.1 PCA-SIFT算法

SIFT(scale invariant feature transform)，即尺度不变特征变换算法，是DavidG. Lowe[8]发表并完善的检测图像局部特征的算法。最终用来描述每个特征点的尺度和旋转不变特征描述符向量大小是16×8=128维，因而在匹配时计算量大，匹配时间长。

结合PCA(principal component analysis)，即主要素提取方法，PCA-SIFT算法实现步骤为：

(1)对于两幅需要匹配图像的全部SIFT描述符，全部图像中一共有M个特征点，每个特征点的特征向量Xi(i=1,2,3,…,128) 的维数是128。则考虑去构造一个样本矩阵X,其中矩阵的元素Xij表示第j(0

(2)计算样本矩阵X的协方差矩阵。过程如下：

对每一个Xi，计算Xi-Xav，这样得到的矩阵记为C；

得到协方差矩阵Z=C×CT。

(3)对得到的协方差矩阵Z求解特征值和特征向量。并将求得的128个特征值从大到小进行排列，之后将前T个特征值相对应的特征向量拿出来，把其构建成一个128×T的矩阵，记为S。这个矩阵S即特征矩阵；

(4)把SIFT描述符投影到子空间中，得到PCA-SIFT描述符。用样本矩阵X乘以上述特征矩阵S，便将原128维SIFT特征降维成T维向量。根据实验验证，T取20以内较为合适。

SIFT和PCA-SIFT算法的优缺点各不相同，其比较结果见表1。

结合表格数据，可以看出PCA-SIFT算法通过提取主要素的方法将高维特征点投影到较低维数，不仅拥有传统上SIFT算法稳定性较高以及对光照、尺度和旋转具有不变性等优点，同时还能够大大地降低特征点对应的特征描述子的维数，从而减少匹配时所用的时间。

表1 SIFT和PCA-SIFT比较

2.2 用组合距离进行相似性匹配

假设两幅相邻帧图像P和Q需要进行匹配，图像P对应的特征点集合为Px={X1,X2,…,Xm}，图像Q对应的特征点集合为Qy={Y1,Y2,…,Yn}。 然后对集合Px里的每个元素Xi(0

移动机器人领域中用于SLAM特征匹配为了能够得到最佳的效果，一般都把上述距离取欧式距离De，见式(1)

(1)

原始SIFT算法得到的特征点对应的特征描述符的维度为128，因而根据式(1)每计算一次欧氏距离，就需要进行乘法运算128次与开方运算1次，就算用改进的PCA-SIFT算法，当T取20时也要20次乘法运算与1次开方运算。所以匹配时如果采用欧式距离造成了计算的复杂性高，从而匹配的实时性差。不利于用在增强现实这种需要实时性较高的技术上面。因而本文采用了在高维空间欧式距离的替代算法，即用距离的线性组合来进行匹配对准，从而能降低匹配的复杂度。

在数学定义里，曼哈顿距离D1和棋盘距离D∞如式(2)和式(3)所示

(2)

(3)

从式中可以看出，相比复杂度高的欧氏距离，上述两个距离定义计算起来复杂度低。并不需要任何乘法与开方的运算。经验证，D∞≤De≤D1，因此可以用上述两种距离的线性结合代替欧氏距离，这样便可以在一定误差范围内简化计算，从而达到整个SLAM匹配过程效率提高。

有两种线性组合方式代替欧氏距离，分别是α(D1(x,y)+D∞(x,y)) 和αD1(x,y)+βD∞(x,y)。这样在计算距离时只需要一次乘法和两次乘法即可。本文采用了一种简单的线性组合来代替欧氏距离，其中：当n为偶数时，α由式(4)计算出

(4)

当n为奇数时，由式(5)计算出

(5)

3 轮廓匹配及回环优化

利用Kinect可以同时获取场景的彩色图和深度图，从而可以迅速提取场景的主要轮廓信息。对轮廓优化处理后，在回环检测环节中首先利用场景的轮廓进行初步回环，得到一些相似的场景，从而大大减少了做相似性检测的图像数量，但其中有些匹配是误匹配，因而需要结合BoW模型进一步进行回环检测。

3.1 轮廓优化

回环检测的本质是图像信息相似性的快速检测，传统方法是利用图像直接进行匹配，信息含量多，计算量大，造成实时性差。而利用图像中最基本的轮廓特征则可以在保留原始图像中主要形状信息的同时大幅度减少参与运算的信息量。本文利用Robert算子、Marr算子、Prewitt算子[9]、Canny算子等对图像进行了快速轮廓提取。效果如图2所示。

图2 场景的轮廓提取

通过比较发现，Robert算子提取的信息相对较少，并且有些边缘部分模糊不清；Marr算子对于场景的提取包含细节较为复杂；Canny算子对于场景的提取细节过于丰富，因为运行速度非常缓慢；而Prewitt算子不仅可以提取出场景的重要轮廓特征，运行速度也较为快速。所以本文选用了Prewitt方法对彩色图像和深度图像进行轮廓提取。

之后分别对彩色图和深度图进行轮廓提取，发现对于复杂场景，不可避免有提取错误的地方，如图3(c)中的彩色图轮廓丢失了桌子拐角的部分。而在深度图像的轮廓中则丢失局部细节部分。经过图像处理的手段，利用彩色图与深度图的互相补充与优化，最终得到了混合后的优化轮廓，可以较好地得到场景的主要轮廓，如图3(e)所示。

图3 场景的轮廓提取与优化

3.2 轮廓匹配

获得场景的轮廓特征之后即可利用轮廓匹配来代替图像匹配。轮廓特性中，矩是最实用的特征，故本文采用轮廓的矩进行匹配，定义如式(6)所示

mij=∬xiyjf(x,y)dxdy

(6)

式中：i是x维度上的矩，j是y维度上的矩。在装备维修应用中，视角存在旋转、平移等情况，直接用传统的矩定义匹配效果较差，匹配效果不佳。故而需要改进：

(1)中心矩

为了实现平移不变性，故需要利用图像的质心进行改造，中心矩如式(7)所示

(7)

(2)归一化中心矩

为了实现尺度不变性，则需要对中心距进行归一化。如式(8)所示，式中，B00代表的是物体的面积

(8)

(3)Hu矩[10]

Hu矩通过归一化中心矩计算得到，式(9)所示的7个由归一化中心矩组成的矩即为Hu矩

H1=(β20+β02)
H2=(β20-β02)2+4β112
H3=(β30-3β12)2+(3β21-β03)2
H4=(β30+β12)2+(β21+β03)2
H5=(β30-3β12)(β30+β12)(β30+β122-3β21+β032)+
(3β21-β03)(β21+β03)(3β30+β122-β21+β032)
H6=(β20-β02)(β30+β122-β21+β032)+
4β11(β30+β12)(β21+β03)
H7=(3β21-β03)(β21+β03)(3β30+β122-β21+β032)-
(β30-3β12)(β21+β03)(3β30+β122-β21+β032)

(9)

式(9)中的7个不变矩构成了一组特征量，并具有旋转不变形、缩放不变形和平移不变性。用Hu矩组成的这组特征量对图片进行相似性对比，每两个轮廓匹配时得到一个匹配系数，值越大，代表两个轮廓越不相似，反之则越相似。这样利用轮廓匹配便提高了后端运行的速度，提升了回环检测的实时性。

3.3 BoW模型

BoW(bag of words)模型是在分类文本中的一种方法，以特征矢量表示文档。针对每个文本，不考虑词的顺序、语言规则、句式规则等，而是认为其是一些词语的汇总。其中的词语完全孤立存在，与其它词语无关。最后根据文本中的词语将其进行分类。

结合BoW模型与回环检测，本文提出的基于BoW模型进行回环检测的步骤如下：

(1)根据图像轮廓的Hu矩进行初步快速匹配，找到部分与当前关键帧粗匹配的图像，但是其中有很多是误匹配，需要在BoW模型中建立视觉单词来更加精细地匹配；

(2)利用PCA-SIFT算法从包括原图像以及利用轮廓匹配得到的图像中提取视觉词汇向量，这些向量代表的是图像中局部不变的特征点；

(3)利用K-Means算法将上述所有的特征点向量进行合并，从而构造出仅包含K个特殊视觉单词的表格；

(4)利用特征点向量在上述表中出现的频率，可以将每一幅图像转换成一个K维的向量；

(5)利用这个K维数值的向量去求当前图像与其它图像的相关性，得到相关性系数，如果这个系数超过设定的阈值，那么认为发生了回环，从而去优化运动估计。

4 实验结果与分析

为验证本文提出算法的实时性和鲁棒性，本文利用TUM标准数据集进行测试，同时用Kinect实时采集实验室的场景，并与传统的基于SIFT特征的rgbdslamv2算法进行对比验证。本文实验处理数据为一台Intel四核2.8GHz主频的笔记本电脑，运行Ubuntu16.04系统。

4.1 轮廓的匹配

在本文中利用Hu矩来进行场景的轮廓匹配。首先釆集场景信息，在实验室这种室内复杂环境中进行场景采集，用Prewitt算子提取轮廓，然后利用opencv库计算Hu矩，并调用matchShapes函数进行轮廓匹配，计算出匹配系数，系数越小，表示匹配性越高。这里选出实验室的6个场景图。如图4所示，可以发现图4(a)和图4(b)、图4(d)和图4(e)为相似场景，表2为下列6幅图像用Hu矩进行轮廓匹配得到的匹配系数的结果。

图4 实验室环境下的6帧场景

表2 用Hu矩进行轮廓匹配后的匹配系数结果

通过表2可以看出，相似轮廓间匹配得到的匹配系数值较小，场景a和场景b匹配值为0.002 53，d和e的匹配值为0.001 57。而其它不匹配的情况下匹配系数值较大。但发现了场景d，e在和场景f匹配时，匹配值也比较小，因为前端区域的有效信息都有椅子的轮廓。所以当设定一个阈值时，提取出匹配系数小于这个阈值的场景时，提取的相似场景会存在一些并不是整个场景都相似的场景。因此需要结合BoW模型的方法将提取的场景进行进一步匹配，以消除误匹配。

4.2 仿真测试

为了评估本文提出的基于Kinect实现视觉SLAM的综合性能，本文首先进行了离线测试，利用TUM提供的RGB-D标准测试集进行离线测试。测试的数据包为fr1_360、fr1_desk2、frl_floor等，这些数据集中提供出机器人移动过程中的真实的路径，为了检验本文提出算法的健壮性，用一个均方根函数来考查绝对路径误差，具体公式如式(10)所示

(10)

图5 预测路径与实际路径对比

从图5中可以看到，本文提出的SLAM算法进行的路径移动预估与机器人真实的路径移动基本一致，并且整体数据集的绝对路径误差值平均为0.031 m，绝对路径误差均方差平均为0.049 m,因而算法具有较强的鲁棒性。

另一方面，本文提出的改进算法和rgbdslamv2算法所运算的时间见表3。

从表3中可以看出本文改进的算法相对比原有的rgbdslamv2算法在实时性方面有了较大提升，关键帧的个数大幅度降低。做到了每帧的处理时间为32 ms左右，而Kinect的fps为30 Hz，因此可以满足实时性的要求。

之后本文又选取了实验室作为实验场景。如图6所示，是在用增强现实技术实现装备维修项目的真实场景，提取在进行视觉SLAM过程中的几个关键帧。

基于以上实验环境，用原rgbslamv2程序进行视觉SLAM过程后，当回环检测中所检测的关键帧数目数量大时，效果如图7(a)所示,当回环检测中所检测的关键帧数目数量小时，效果如图7(b)所示。而经过改进后，本文提出的视觉SLAM过程演示效果见图8可以看出，在相同时间内，源程序已经提取了420个关键帧，而用本文的算法仅仅提取了217个关键帧。并且从效果图的右下角的特征提取与匹配图可以看出，原程序所用的SIFT算法提取到的特征点更多，而本文改进的PCA-SIFT算法能够在保持主要素特征的情况下减少特征点的提取。此外从运行时的实时性来看，本文改进的SLAM算法能够快速地进行SLAM的整个过程，不会出现卡顿的情况，并且在较快地移动摄像头的情况下保持轨迹不跟丢。反之，在用原程序进行视觉SLAM的过程中时常出现卡顿，并且摄像头移动速度不能很快，动作幅度需要保持较小。另一方面，在回环检测的对比上，当源程序回环检测中所检测的关键帧数目数量大时，效果如图7(a)所示,此时回环检测能力尚可，但是所需等待时间较长；当回环检测中所检测的关键帧数目数量小时，效果如图7(b)所示，此时回环检测能力较差，点云图中装备出现略微重叠效果。

表3 rgbdslamv2与本文方法比较

图6 实验场地环境

图7 源程序效果演示

图8 本文改进程序演示效果

5 结束语

SLAM(simultaneous localization and mapping)，即时定位与地图构建技术，是增强现实技术中用于三维场景建模的核心技术。在利用增强现实进行装备维修的操作中，会出现三维场景建模速度慢和虚拟信息漂移的问题。这主要是由于头戴设备中硬件资源受限，计算能力不足。为满足在增强现实技术领域的实时性和回环检测要求，可利用Kinect对传统视觉SLAM方法中计算费时的环节进行优化。通过将帧间匹配的SIFT算法结合主要素分析方法，对其特征描述符合理降维，并将特征匹配距离优化，从而减少帧间匹配运算量;同时利用Kinect获取到的轮廓减少关键帧的数目;最后在回环检测中利用轮廓粗匹配以减少词袋模型中用于聚类的特征描述子数量。设计实验结果表明：本文提出的基于Kinect的视觉SLAM算法实时性更高，回环检测的鲁棒性更高。从而解决了利用增强现实技术进行装备维修过程中出现的三维建模速度慢和虚拟信息漂移的问题。