基于特征点改进的视觉SLAM定位研究

王 伟，汤琴琴，汪先伟

(1.南京信息工程大学 自动化学院，南京 210000；2.无锡学院 轨道交通学院，江苏 无锡 214000)

0 引言

视觉SLAM(simultaneous localization and mapping)是指通过视觉传感器对环境进行感知，同时实现机器人或无人驾驶汽车在未知环境下的自主定位和建图[1]。在解决机器人和自动驾驶等领域的应用问题中，视觉SLAM成为最受重视和广泛应用的技术之一[2-4]。通过研究SLAM技术，可以为实现更加智能的、精准的机器人定位导航提供支持[5]，同时也可为构建数字地图和城市规划等领域提供数据支撑。

目前，ORB(oriented FAST and rotated BRIEF)算法作为一种关键点检测与描述子生成算法[6]，在SLAM领域被广泛使用。然而，ORB特征点的数量在一定程度上受限于图像纹理信息，即图像分辨率较低、场景光照黑暗或者曝光等极端条件、结构信息较少导致图像平整度较高等因素使得环境的纹理信息不被突出时，难以提取足额数量的ORB特征点。因此，如何提高ORB特征点的数量和质量以提升SLAM定位精度成为当前SLAM领域需要解决的核心问题之一[7]。国内外学者们以往主要通过设立特征点检测阈值、图像预处理、图像处理与阈值结合等方式来解决在低纹理场景下ORB特征点数量不足的问题，使得ORB算法更加鲁棒。

为通过设置阈值增强ORB算法在低纹理环境下的鲁棒性以便提取更多的特征点，Murartal等人[8]在ORB_SLAM2中设置了固定阈值，即当最大固定灰度阈值搜索不到特征点时，系统自动启用最小固定灰度阈值进行ORB特征点检测。Wu等人[9]采用动态阈值的思想，去除最高亮度和最低亮度的像素后计算平均亮度，通过自适应阈值选择准则为每个点都设置专属阈值，解决ORB对光变化的敏感性问题。焦嵩鸣等[10]则针对固定阈值提出了一种全局和局部的自适应阈值结合的思路，针对整体与局部的亮度变化两种情况各自采取全局或半全局自适应阈值策略，并结合于局部自适应阈值，以此增加特征点提取的数量。另外，支持向量机(SVM，support vector machine)[11]和卷积神经网络(CNN，convolutional neural networks)[12]也被提出用以预测自适应阈值，基于机器学习的自适应阈值方法需要大量训练数据和计算资源的数量，以及对数据分布和功能有很高的要求，应用于现实有很大的局限性。这类方法的缺点：1)如果采用固定的全局或局部阈值，无法兼顾整个图像的各个部分，难以达到理想的效果；2)两次阈值可能会造成图像某些区域块的ORB特征点被重复提取，有些区域被遗忘检测，不仅使特征点冗余，又影响大规模特征匹配的准确性；3)阈值的方法依赖图像本身的灰度动态范围，在低纹理环境下，灰度动态范围往往较低，此类方法容易失效，适应性差，而且阈值受环境噪声的干扰，阈值的取值并不准确。

所以，学者们继而研究对图像进行预处理，增强图像的边缘、角落等细节以改变低纹理场景下的灰度范围。Cheng等人[13]通过对图像进行低光增强处理，增强了系统在低光环境的鲁棒性。陆佳嘉等人[14]使用二维高斯对图像金字塔进行降噪，并对像素进行划分，提高部分区域提取到特征点的概率。李宪华等人[15]通过自适应阈值伽马变换，对低照度、低纹理的图像块进行增强，提高在这些低光环境下提取到的特征点的数量。Dai等人[16]提出一种图像增强与截断自适应阈值相结合的方法改进ORB 特征提取算法，将原始图像转换为灰度后通过应用高斯滤波、截断自适应伽马亮度调节和非锐化掩蔽增强图像，将图像分成若干子区域，并利用改进的大津法获得相应阈值，以此解决了在光照过多或不足等低纹理环境中无法提取足够特征点的问题。现有方法：1)低纹理场景变化是复杂的，包括分辨率的高低变化、场景结构信息的差异导致平整度的不同，难以保证ORB算法在这些因素下的适应性与稳定性；2)结合额外的阈值后算法不准确性增加。

此外，关键帧选择在视觉SLAM算法中也扮演重要角色，关键帧选择策略的质量直接影响着算法的定位精度和运行效率[17-19]。传统的关键帧选择策略通常只考虑了时间因素，根据距离阈值、角度阈值或关键帧密度等指标进行筛选[20]，将一些劣质的关键帧纳入队列，难以针对不同类型场景进行调整导致定位精度下降、模型计算量大。

为解决上述问题，本文利用多尺度分析与局部灰度的特征检测为一般ORB特征点添加尺度与旋转的描述，利用基于高斯模糊的图像信息增强方法加强ORB算法在低纹理环境下的稳定性，象限分割图像以使特征点均匀分布，并在只考虑时间因素的传统关键帧选择机制中兼容了特征点数量因素，降低定位误差。

1 视觉SLAM定位方法

基于特征点的视觉SLAM的定位过程如图1所示。

图1 视觉SLAM定位方法

1)相机调整参数之后利用ORB特征提取器检测图像特征点，在当前帧与上一帧之间通过随机采样一致性算法(RANSAC)[21]寻找匹配的特征点。

2)通过特征点坐标差异计算相机运动。

3)但是为了提升系统响应速度，降低系统定位误差，通过选择合适的关键帧，使系统只保留最必要的信息进行位姿估计。

4)基于词袋模型构成系统的闭环，用于减少定位过程中的累计误差。闭环检测[22]比较当前帧与之前帧，找到或代表同一区域的特征，计算相似度，并在相似度高于一定阈值时确立闭环。

5)根据运动估计获得的相对位置信息，进行后端的全局优化，使误差最小化。

本文将特征点看作视觉SLAM系统正式进行定位的输入数据，对图像中的特征点加以描述，并克服传统ORB算法在低纹理的环境下的不稳定性，过滤劣质关键帧，降低视觉SLAM的定位误差。

2 特征点提取过程以及改进

特征点对视觉SLAM定位的重要性是显著的。在视觉SLAM中，特征点是用于进行相邻帧间匹配的关键点，它们被用来计算相机的运动和场景的深度信息。特征点数量越充足时，视觉SLAM算法在定位中就有越多可用的信息。在视觉SLAM定位中，需要依靠匹配当前帧与先前帧之间的特征点来获取相机的运动估计。在低纹理、低分辨的环境中，传统ORB算法容易失效导致特征点数量不足，如果特征点数量太少，匹配误差就会增加，从而导致相机姿态估计不准确或者无法成功匹配，进而导致整个SLAM系统定位失效。

2.1 传统ORB特征点提取

传统ORB特征点提取方法使用FAST角点检测器寻找图像中明显的突变点，并将这些点组成候选关键点，原理如图2所示，比较圆心点O且半径等于3的圆上16个连续像素点与O的灰度差来提取FAST点。若圆周上连续着M个(M一般取12)像素点的灰度值与O的差已超规定阈值T，则判定被选中的圆心O作为特征点。详细过程如下：

图2 FAST角点提取示意图

1)在图像上选取一点O，灰度值假设为Io，以O为圆心且3为半径作圆，共有16个点。

2)设定灰度阈值T，比如T=IO× 0.2。

3)若圆上有连续12个点的灰度值大于T+IO或小于T+IO，判断O为关键点。

接着计算每个关键点的描述子，描述子是用来描述关键点周围区域的一系列二进制特征的向量，在ORB算法当中，使用BRIEF算法生成每个关键点的描述子，即在公式(1)中：P(x)、P(y)分别为点x、y处的灰度值大小。

(1)

其具体思想是在特征点P周围以特定模式选取N对点，比较灰度值的大小(比如x，y)。若x比y大，则取1，反之就取0。

故BRIEF描述子可以表示为：

(2)

2.2 改进的ORB特征点提取过程

实际上，图像中的物体可能会以不同尺度或角度出现，故提出一种多尺度分析方法，在不同尺度下检测物体的特征，即通过对原始图像进行重复缩减或扩大操作，生成不同分辨率的图像，每缩放一次，图像的分辨率就会下降，并在这些不同尺度下进行特征提取，使得ORB特征点具备尺度不变性。如图3所示，本文将原始图像作为待缩图，长为W0，宽为H0，以1.2为尺度系数进行缩放，第n次得到的图像长为Wn，宽为Hn，面积为Sn。

图3 多尺度缩放示意图

图4 图像信息增强前后灰度直方图对比

Wn与W0的关系为：

(3)

Hn与H0的关系为：

(4)

故Sn可表示为：

(5)

由于本文以1.2为尺度系数，所以每缩放一次后的图像面积是上一次的69.44%。

ORB算法添加了通过平移模拟旋转的过程来应对BRIEF描述子对旋转不太敏感的问题，本文引入基于局部灰度分布的特征点检测方法以提高ORB算法对旋转的稳定性，即以特征点为原点构建局部坐标系，在构建好的坐标系中，通过对邻域内每个像素的灰度加权平均，计算出邻域内灰度值的中心位置，最后用特征点指向灰度质心的特征向量代表此特征点的方向。具体步骤如下：

1)定义某图像块A的矩定义为：

(6)

其中：I(i，j)表示在位置(i，j)处的像素灰度值，M与N分别表示图像块A的宽度与高度，p、q表示矩阵阶数，Cx与Cy分别表示图像块A灰度质心坐标，即图像块中所有像素的坐标的加权平均值。

2)图像块A灰度质心C的坐标公式为：

(7)

3)由于p，q={0，1}，所以C的坐标可以进一步表示为：

(8)

4)将图像块A几何中心O与质心相连得到方向向量，将特征点方向定义为：

(9)

在描述子的基础上，经过多尺度缩放和灰度质心方向描述，ORB特征点更加稳健，同时增强算法的性能和稳定性。从角点提取过程可以看出，ORB的提取依赖角点周围像素的差异程度，高纹理的图像像素之间的变化明显大于低纹理的图像，故而更容易提取到特征点。因此，基于扩大图像灰度动态范围的思想，在角点检测前提出一种基于高斯模糊的图像信息增强方法，提升图像灰度动态范围，突出图像的纹理信息，从而增加可提取的ORB特征点数量。

图像信息增强方法首先对原图像进行3个高斯模糊，得到3个不同尺度的模糊图像。因为高斯模糊会平滑掉图像中的细节信息，所以在每个模糊图像与原图之间做差，获得这些细节信息，并通过一定的组合公式将其融合到原图像中，进而增强原图像的信息表达能力，形成更高动态范围的灰度图像。具体步骤如下：

1)定义待处理图像为I*(x，y)，选择标准差分别为σ1、σ2、σ3的高斯核函数G1(x，y)、G2(x，y)、G3(x，y)，函数表达式如(10)所示，其中σ1=1.0，σ2=2.0，σ2=4.0表示模糊程度，x和y表示在高斯核函数的中心点的偏移量。

(10)

2)利用3个不同尺度的高斯核函数分别平滑处理I*(x，y)，得到处理后的图像称为B1(x，y)、B2(x，y)、B3(x，y)，公式如下：

(11)

3)如公式(12)所示，将所得的B1(x，y)、B2(x，y)、B3(x，y)分别与原图I*(x，y)做差，获得3个不同层次的细节信息，记作D1(x，y)、D2(x，y)、D3(x，y)。

(12)

4)选择w1，w2，w3三个加权系数，通过组合函数得到信息增强后的图像D*(x，y)。组合函数的公式如下：

D*(x，y)=(1-w1×sgn(D1(x，y)))×D1(x，y)+w2×D2(x，y)+w3×D3(x，y)

(13)

多次实验对比后得到w1=w2=0.25，w3=0.5时效果最佳。

ORB算法对角点的响应较为敏感，所以ORB算法提取的特征点往往集中在图像角点等边缘区域。此外，由于采用了在不同尺度空间下检测和描述特征点，使得特征点分布较为集中，因为在不同尺度下相邻像素的信息是高度相关的，因此在不同尺度下提取到的特征点往往存在冗余。本文通过对图像进行象限划分结合非极大值抑制的方法过滤部分冗余的特征点，使特征点分布均匀化，减小计算量，提高匹配效率，进而提升视觉SLAM的定位精度。

如图5所示，将图像划分为4个面积相等的矩形象限，并作为4个原始象限N1、N2、N3和N4，统计每个象限中的特征点的总数。对于每一个象限，若包含的特征点数量大于1，则作进一步细分，即将该象限分成4个子象限，继续对子象限进行特征点数量的统计与划分，直到每个象限中只包含1或0个特征点，停止划分。对于已经划分出来的象限进行后处理，将每个象限中的所有特征点按照响应强度进行排序，并保留最强的一个特征点，其余的特征点将被舍弃。在实际应用中，由于特征点可能会堆积在一起，导致检测结果出现重叠，因此需要设定一个参数k，限制象限的最大划分次数。当划分次数已达k时，对于仍然包含多个特征点的象限，只保留其中响应强度最高的一个特征点。

图5 象限划分原理图

总之，通过多尺度分析方法及局部灰度特征检测的方法使得FAST角点具备尺度和旋转的描述，利用基于高斯模糊的信息增强预处理视觉传感器抓取的图像，解决ORB算法提取特征点数量存在不足的缺陷，并对图像进行象限分割融合非极大值抑制将冗余特征点剔除，使特征点分布均匀化。具体的改进后ORB特征点提取流程如图6所示。

图6 改进的ORB特征点提取过程

3 关键帧选择机制

视觉SLAM系统将前后两帧图像里提取到的特征点进行匹配，得到它们在两幅图像中的对应关系，通过对这些点进行几何变换，估计前一帧到当前帧之间的相对运动，包括平移与旋转。如图7所示，O1和O2是两个视觉相机中心，后一帧KF2的特征点P2是前一帧KF1的特征点P1通过匹配得到的对应点，O1P1与O2P2交于点P，并构成一个极平面，O1O2为基线，线与KF1、KF2交于点e1、e2，极平面与KF1、KF2交于极线l1、l2。

图7 相对运动估计示意图

假设KF1帧与KF2帧对应的旋转和平移向量分别是R、t，点P、P1和P2三者之间的关系如式(14)所示：

(14)

取Z=1归一化平面，联解(14)中的两个式子，可以得到式(15)：

(15)

其中，记E=t^R，E为本质矩阵(Essential Matrix)，记F=K-TEK-1，F为基础矩阵(Fundamental Matrix)，E和F之间相差了相机内参K。

将式(15)写成本质矩阵E的形式，可以进一步改写为式(16)。E为一个三行三列的矩阵，(u1，v1，1)和(u2，v2，1)分别是P1和P2归一化平面上的投影。

(16)

通过八点法，可以求解本质矩阵E，通过本质矩阵进行奇异值分解求解旋转矩阵R和平移向量t。即通过P1和P2的匹配关系计算本质矩阵，奇异值分解得到旋转矩阵和平移向量，这些矩阵和向量描述了相机之间的运动。

利用相对运动信息更新位姿估计，生成最终的轨迹。但如果所有的帧都用来做匹配和运动估计，会导致计算量巨大。同时，由于两帧之间或存在很多相似之处，使用所有帧进行匹配可能造成冗余信息，降低SLAM系统的定位精度。故需要选择能够提高独特信息的帧作为关键帧，减少系统计算量和提升系统的效率。关键帧要选取能够代表整个场景的帧，以保证位置更加精确，选取描述信息能力强的帧以提高定位精度，同时时间间隔要适当。一般的关键帧选择机制没有着重考虑图像中的冗余信息，即便提取出的特征不同，但由于图像内容相似，所选帧之间信息重叠，导致匹配优化时间过长。为了剔除冗余的劣质关键帧，本文提出一种综合时间因素和特征点数量因素的关键帧选择机制。

只考虑时间因素时，首先要保证当前帧的内点数大于最小阈值且重叠度不高，以确保关键帧跟踪指令的准确性。一旦条件满足，就会对以下条件进行判断，满足条件之一，即可插入关键帧。

1)为了避免追踪失效，当与上一个关键帧的时间间隔T超过预设的最大值MAX时，需要插入新的关键帧。

2)为了避免图像重叠度过高，当与上一个插入关键帧的时间间隔T达到预设的最小值MIN时，且局部建图线程空余，插入新的关键帧。

3)由于局部建图线程处理能力限制，需要在该线程中保证关键帧队列的帧数量不超过3个。

只考虑时间因素，在视觉相机转弯或移动速度过慢时，帧与帧之间的信息重叠过大导致定位精度下降，针对这一问题，在考虑时间因素的基础上，本文将特征点因素作为考量，设计下述劣质关键帧判断机制，减少计算开销，提升视觉SLAM系统的定位精度。

1)3个相邻的关键帧利用随机采样一致性方法匹配ORB特征点。

2)分别将相邻的两组关键帧成功匹配点的数量记为Num1和Num2，相隔的两个关键帧成功匹配的特征点数则记为Num3。

3)如果满足Num3>Num1和Num3>Num2，则中间帧被认为是劣质关键帧。

4)剔除被选的劣质关键帧，则进行下一次判断。

本文关键帧选择策略伪代码如下：

IfT>MAXThen

插入关键帧 or

IfT≥MINand 局部建图线程空闲 Then

插入关键帧 or

If 局部建图线程关键帧队列帧数<3 Then

插入关键帧

If 关键帧库内关键帧总数超过3 Then

IfNum3>Num1Then

IfNum3>Num2Then

剔除劣质的关键帧

EndIf

EndIf

EndIf

EndIf

EndIf

EndIf

4 实验测试与分析

选用CPU为i7-11800H，内存16 GB的计算机，软件平台为基于Ubuntu18.04的ROS系统，选用Kinect V2视觉相机拍摄真实图像和TUM数据集作为测试数据。

4.1 提取特征点的数量实验

为验证本文改进的ORB特征点提取方法在不同分辨率、不同光照、不同平整度等因素导致环境纹理信息不被突出时的稳定性，分别选取3组各5张图像作为实验对象，A1～A5图像的分辨率分别为100、150、200、250、300 dpi，B1～B5图像的光照程度分别为黑暗、稍暗、正常、明亮、高光，C1～C5图像因为环境结构的差异平整度各不相同。提取效果如图8所示，每组的第一行图像采用传统ORB算法提取特征点，第二行图像利用本文提出改进ORB特征点提取策略提取特征点。

图8 改进前后提取特征点对比

图9 图像分割示意图

将传统ORB算法提取的特征点总数记为Sum1，本文方法提取的特征点总数记为Sum2。如公式(17)所示，引入ORB特征点增长系数Q，用于对改进后的ORB特征点提取方法在不同因素下的性能表现进行评价，Q值越大，说明算法的适应性与稳定性越好。

Q=Sum2/Sum1

(17)

首先，通过表1数据可以看出，a1至c5所有图像在使用本文改进的方法提取特征点后，特征点数量均有明显增加，表明本文方法在不同分辨率、不同光照、不同平整度的情况下均有良好效果。

表1 图像ORB特征点数量对比

其次，经计算，分辨率因素下的Q平均值为1.44；光照因素下的Q平均值为1.31；平整度因素下Q平均值为1.57。对于分辨率因素而言，Q与分辨率大致成反比；就光照而言，越是黑暗和高光这种极端条件下，Q值越是突出；就平整度而言，结构信息越少，平整度越高，Q值也越大。

综上所述，本文改进后的ORB特征点能够在纹理信息不被突出的环境下有良好的性能表现，尤其是在分辨率较低、光照极端、结构信息越少的条件下，具有良好的适应性和稳定性。

4.2 均匀度实验

通过均匀度量化，对比在真实场景下传统ORB特征点提取方法、ORB_SLAM2和本文方法在提取特征点上均匀化的表现。

实验采用的均匀化评价方法如下：

先对图像进行水平、垂直、中心和四周(中心与四周面积相等)、左上至右下和左下至右上5种方式分割得到10个区域。分别计算每个区域内的特征点数量，并构建一个区域统计分布向量。再计算特征点区域统计分布的标准差，以此表示均匀度，具体如公式(18)所示。标准差越小，特征点分布越是均匀，反之亦然。

(18)

图10 均匀度测试结果

第一行是传统ORB算法测试结果，第二行是本文提出的改进ORB特征点提取方法的测试结果。根据式(18)量化后的结果如表2所示。

表2 均匀度量化结果图像

通过表2计算，本文方法在均匀度性能上相较于ORB_SLAM2与传统ORB特征点提取方法，分别提升了7.14%和20.78%，表明了改进过后的算法可使特征点分布更加均匀。

4.3 关键帧选择机制效果测试

为验证在只考虑时间因素的传统关键帧选择机制上融合特征点数量因素的方法更加有效，将本文提出的关键帧选择机制移植于ORB_SLAM2，与只使用传统关键帧选择机制的ORB_SLAM2共同在TUM数据集上进行测试，在考虑了特征点数量因素之后，视觉SLAM需要处理的关键帧数量减少，结果如表3所示。

表3 关键帧数量 个

将移植前后视觉SLAM系统求解的轨迹与真实轨迹进行均方根误差(RMSE)误差对比，RMSE越小说明系统定位精度越高，轨迹越拟合于真实轨迹，定位精度结果如表4所示，RMSE的减少说明了剔除劣质关键帧可以降低视觉SLAM的定位误差，从而证明本文的关键帧选择机制可以优化关键帧质量。

表4 剔除劣质帧前后定位精度对比

4.4 轨迹精度测试

为验证本文提出的改进ORB特征点提取和设计的关键帧选择机制结合使用的有效性与可行性，首先，将本文提出的改进ORB特征点提取策略与ORB_SLAM2结合为ORB_V1系统，精度提升结果如表5所示。

表5 ORB_V1提升效果

在ORB_V1系统的基础上结合本文设计的关键帧选择机制，组成ORB_V2系统，精度测试结果如表6所示。

表6 ORB_V2提升效果

如图11所示，以fr3_cabinet为例，ORB_SLAM2求解出的轨迹traj1的RMSE为0.056，在未添加本文提出的关键帧选择机制时，ORB_V1求解轨迹traj2的RMSE为0.050，ORB_V2求得轨迹traj3的RMSE是0.048。相比于traj2和traj3，traj1因为环境纹理太少，一般ORB算法几乎失效，没有足够的特征点用来计算相机的运动，使得估算轨迹初始阶段很大程度偏离真实轨迹，且导致中途定位跟踪失败。traj3在traj2的基础上，再加入了综合时间因素和特征点因素的关键帧选取机制，降低了劣质关键帧对定位精度的影响，定位精度从10.714%进一步提升至14.286%，更贴合真实轨迹。

图11 以fr3_cabinet为例的轨迹对比

5 结束语

本文利用多尺度分析与局部灰度的特征检测为一般ORB特征点添加尺度与旋转的描述，提升ORB算法的稳定性，利用基于高斯模糊的信息增强方法克服传统ORB算法在纹理信息不被突出的环境下的不稳定性，通过对图像进行象限分割去除多余的特征点，使特征点均匀分布，提升视觉SLAM系统的定位精度。在只考虑时间因素的传统关键帧选择机制中结合了特征点数量因素，过滤劣质关键帧，进一步降低定位误差。

经计算，整个系统的定位精度误差平均减少了14.688%。但是由于提取了更多的特征点，造成运动估计计算需要更长的运行时间，因此可考虑将深度学习与视觉SLAM结合，提升ORB算法识别的速度，或是研究基于SLAM方法的传感器(例如激光雷达、惯性测量器件等)数据融合技术，以提高定位精度。