1. 引言
SLAM (simultaneous localization and mapping)是同步定位与建图的缩写，是移动传感器在陌生环境中构建自身所处的周围陌生场景环境信息地图的一个过程 [1] 。SLAM自提出以来在增强现实(AR)和机器人领域得到了广泛的应用 [2] [3] [4] 。视觉SLAM (V-SLAM)使用相机作为采集图像信息的传感器设备，不仅价格低、携带方便、功耗低，而且可以提供更多的环境信息 [5] 。根据系统的实现方法，可以将视觉SLAM分为基于提取特征点的方法和直接方法 [6] 。在SLAM过程的传统特征点方法中，特征匹配是视觉SLAM中的关键步骤 [7] ，它解决了SLAM中数据关联的问题，意味着确定了当前帧看到的路标和之前看到的路标之间的对应关系 [8] 。然而，在SLAM中，只有静态的正确的匹配才能为后续位姿优化提供准确的匹配数据，由于图像特征的局部特性，误匹配普遍存在，这一问题长期以来没有得到有效解决 [9] 。目前，它已成为视觉SLAM中的瓶颈，限制了性能改进 [10] 。LPM [11] 推导了具有线性算法时间复杂性的闭合解，实现了令人惊讶的优异匹配性能。GMS [12] 近年来在特征匹配方面应用广泛，一些学者用GMS提高了匹配的正确率和缩短了误匹配过滤的运行时间 [13] [14] 。贾晓雪等 [15] 使用搜索半径自适应的惯导辅助图像特征匹配方法提高了定位精度。ORB-SLAM2 [16] 在PTAM [17] 架构的基础上，增加了局部地图和回环检测功能，其前端使用随机抽样一致(Random Sample Consensus, RANSAC)算法 [18] 剔除误匹配特征点，该算法通过随机抽取样本数据计算得到一个得分最高的模型参数，但是计算参数时选取最小子集是从效率的角度考虑，往往得到的是非最佳参数，为了在处理大量数据同时满足精度要求，必须增加迭代次数，增加了剔除误匹配所需的时间 [19] 。系统运行时间对实时性能有很大影响 [20] 。
为了提高V-SLAM定位精度同时降低系统的耗时，本文设计了一种基于多尺度图像金字塔自适应增加匹配数量并结合K-Means剔除误匹配的SLAM优化方法，本文的创新如下：
(1) 利用图像多尺度信息获取匹配点的相对位置一致性可在任意图像金字塔层里获取更多的特征匹配对；
(2) 融合并改进K-Means聚类来剔除误匹配，减小SLAM系统在视觉里程计位姿估计中产生的大量误差。基于聚类的思想减少了具有更多特征点的关键帧处理事件，大大提高了在复杂环境下的鲁棒性，更是解决了误匹配和匹配精度低的问题；
(3) 在系统跟踪失败后在重定位阶段融合多尺度自适应方法进行重定位，构建多层级金字塔，对图像进行缩放，从上到下匹配不同比例的特征点，快速获取更多的特征点匹配对。
2. 算法原理
系统在ORB-SLAM2原有的框架上进行改进，ORB-SLAM2按模块可分为跟踪线程，局部建图，回环检测三大部分。在原始算法基础上，本文使用改进K-Means算法替代部分RANSAC算法，并且为了提升系统鲁棒性，在重定位部分添加了多尺度匹配，扩大了特征点匹配范围。算法具体实现步骤为：首先，按帧读取图像，基于ORB-SLAM2特征提取器提取特征点，随后构建多层级图像金字塔，对图像进行缩放，在不同尺度上计算并圈定出当前帧和前一帧特征点的位置范围，从不同层级的图像由上到下匹配不同比例的特征点，从而获得更多的匹配对，增加系统的鲁棒性。然后，使用改进K-Means聚类算法对匹配点距离和匹配点旋转角度进行聚类，从运动一致性出发，可以将距离和角度异常的匹配对剔除，从而保留最多精确的匹配点对。最后，使用得到的匹配信息进行后续相机运动和位姿估计，算法流程图如图1所示。
Figure 1. The workflow of the proposed method
图1. 本文算法框架
2.1. 融合改进K-Means的聚类匹配算法
K-Means聚类秉持“簇内相似度高，簇间相似度低”的原则，将一组N个样本的特征矩阵划分为K个无交集的簇。直观上看，我们更希望“物以类聚”，即同一簇的样本尽可能的聚集在一起，不同簇的样本尽可能的不同。假设有n对像素坐标下匹配好的特征点对
，
，其中，
和
是匹配好的一对特征点对，分别求出每一对匹配特征点之间的距离，距离公式为：
(1)
得到
(2)
D就是我们提取的分类数据集，对D进行K-Means聚类，使用聚类距离总和L最小作为优化目标进行迭代k次：
(3)
聚类完成后将组内元素最多和次多的两簇保留，其他簇的匹配点直接剔除，对保留的匹配使用方向梯度直方图估计图像的旋转角度后剔除掉角度异常的匹配对，剩下的匹配对用在后续的位姿估计。改进K-Means误匹配剔除过程示意如图2所示。
Figure 2. Fusion K-Means clustering algorithm to eliminate mismatch
图2. 融合K-Means聚类算法剔除误匹配
图2中先计算出每一对匹配点对的距离dist，利用所有配对的距离之和最小进行迭代。根据角度、匹配对距离dist不同将数据集分类，分类后分别统计每一类的组内元素数量，将组内元素数量第一和第二的类别挑选出来，剔除其他所有的匹配，例如图红色虚线表示的匹配由于角度和匹配距离不对被剔除。之后将第一和第二类数量进行对比，若比值差距过小，重复上述分类过程，直至比值符合预期停止迭代，流程如图3所示。
Figure 3. Flow chart of fusion K-Means clustering algorithm
图3. 融合K-Means聚类算法流程图
2.2. 构建多尺度自适应网络
ORB-SLAM2容易失去跟踪，一些场景下提取不到足够的特征点，有效点比较少，在原始算法中，虽然提出了图像金字塔的概念，但是匹配时对图像金字塔的层级进行了限定，得到上一帧关键点所在金字塔的层数，然后得出当前帧特征点的搜索范围，只对特征点所在层或者上下层进行特征点匹配，限定了匹配的层级，得到的有效匹配点数量很少，在经过RANSAC剔除外点、方向梯度直方图判断角度变化等流程对匹配点剔除优化后，容易造成匹配数量不够，而导致跟踪失败。为解决这个问题，当系统匹配数极度少时，本文提出另一种多尺度自适应匹配特征点进行重定位的方法。基本思路是利用图像具有尺度信息的特点，处理图像时，可以对图像的尺度和分辨率进行变换，对图像进行放大或者缩小，进而可以转换为指定尺寸的目标图像，进行图像多尺度调整。如图4所示，系统实现重定位需要构建多层级金字塔，将图像缩放，得到当前帧特征点所处的金字塔层级，通过计算两帧间特征点不同层级的相对位置，对前一帧和当前帧的特征点进行从上到下不同尺度的匹配，然后根据两帧之间的相对位置对特征点进行粗略定位，可以在多个层级里面得到特征点在不同层级的位置，从而去扩大特征匹配的搜索范围，增加ORB特征点的匹配几率，因而能够提取更多的特征匹配点，使匹配对增多，为后续系统姿态优化提高准确率，进而达到提高系统的鲁棒性的目的。通过实时性实验证明，该方法可以中和K-Means聚类的耗时，使整个SLAM过程与ORB-SLAM2时间成本有所降低。
Figure 4. Schematic of multi-scale adaptive matching of feature points
图4. 特征点多尺度自适应匹配示意
3. 实验验证分析
本次实验平台为AMD Ryzen7 5800H with Radeon Graphics 3.20 GHz，系统环境为Ubuntu16.04，实验实例为单目视觉SLAM。为评价系统的有效性，实验采用公开数据集TUM进行验证分析。本次实验分为两个部分。第一部分，为了验证融合K-Means算法对误匹配剔除的有效性，先针对K-Means与ORB-SLAM2以及一些优化的匹配方法进行对比实验分析。该实验选取两张图片，进行暴力匹配后分别使用现阶段普遍选用的RANSAC算法、文献 [12] [13] [14] 选用的GMS算法以及本文选用的K-Means算法剔除误匹配；进一步验证融合多尺度自适应重定位分析相对于ORB-SLAM2的效果。第二部分，评估系统性能，验证在ORB-SLAM2中融合多尺度自适应与融合K-Means聚类算法剔除误匹配后对整体算法的影响，分别对准确率、实时性进行分析，选取表1中公开数据集TUM中共七个序列进行单目视觉SLAM实验。
Table 1. Dataset selected for experiment
表1. 实验选取的数据集
3.1. 算法优化验证实验
3.1.1. K-Means聚类误匹配剔除分析
ORB-SLAM2直接使用了暴力匹配，但是暴力匹配得到的误匹配数据很多，为了尽可能的得到更准确的特征点匹配数据，本文在暴力匹配后使用K-Means聚类进行误匹配剔除可以有效的减少异常匹配数量，得到高度准确的匹配。图5展示了在提取1000个ORB特征点时，使用暴力匹配、RANSAC匹配过滤，融合GMS匹配过滤，融合K-Means匹配过滤后的效果图。
(a) 暴力匹配结果
(b) RANSAC过滤实验结果
(c) GMS过滤实验结果
(d) 本文算法过滤结果
Figure 5. Effect picture of matching and filtering
图5. 匹配过滤效果图
表2中提取1000个ORB特征点条件下取三次匹配数据平均值对比三种匹配算法的实时性以及匹配的数量。相比ORB-SLAM2中使用暴力匹配和RANSAC剔除误匹配，改进后融合K-Means剔除误匹配能够保留暴力匹配63.68%的正确匹配，并且时间成本在三种过滤算法中最低。从表中可以明显看出GMS和RANSAC剔除太多匹配，会导致里程计后续位姿估计匹配数量不够，从而直接导致系统跟丢。本文融合K-Means的算保留了大部分有效匹配，可以为后续位姿估计提供更加精准匹配特征点对和足够多的匹配数量。
Table 2. Comparison of matching filtering effects
表2. 匹配过滤效果对比
Figure 6. Matchers filtered under different ORB features
图6. 不同ORB数目过滤后匹配对数量
Figure 7. Cost time filtered under different ORB features
图7. 不同ORB数目下过滤花费时间
由于在不同环境下提取的ORB特征点数目是不同的，为了验证不同ORB特征点数目下本文算法的有效性，图6、图7比较了不同特征点数目条件下分别使用GMS、RANSAC和本文算法过滤后获得的匹配数量和系统花费的时间，从图6可以看出，本文算法过滤后匹配数量最贴近原始暴力匹配对的数量，GMS平均只能保留28.78%的匹配数，RANSAC过滤后只能保留26.69%的匹配数量，而本文算法可以保留65%的匹配数量，这极大的保证了筛选后还能有足够的匹配对进行位姿估计，特别是在特征点提取较少的时候。图7中可以看到，本文算法使用的时间是在三种过滤算法之后时间最少的，仅为RANSAC算法的二分之一，比GMS算法剔除误匹配耗时更少。说明本文将K-Means融合进ORB-SLAM2中，提高了ORB-SLAM2算法的精确度和减少了系统整体运行时间。
3.1.2. 融合多尺度自适应重定位分析
自动重定位是ORB-SLAM2算法的一个亮点，但是在许多复杂场景下，ORB-SLAM2的鲁棒性仍然比较低，在许多复杂场景下相机运动过快，纹理较少就容易跟丢。本文在重定位算法中融合多尺度自适应算法后，如图8所示，在TUM fr1/room数据集上，经过多次实验，ORB-SLAM2在算法多次在实验中跟丢，轨迹如图8(a)所示，而本文算法跟踪状态良好。
(a) ORB-SLAM2实时轨迹
(b) 本文算法实时轨迹
Figure 8. Real time trajectory maps of two algorithms
图8. 两种算法实时轨迹图
(a) 关键帧数量
(b) 轨迹长度
Figure 9. Experimental results of two algorithms in different datasets
图9. 两种算法在不同数据集上实验结果
图9中可以看出，在前两组静态序列上，原始算法在静态环境中鲁棒性良好，不容易跟踪失败，两种算法关键帧的数量和跟踪轨迹长度差异不大。在后三组序列上，原始算法特别容易跟丢，融合多尺度自适应算法后，系统并没有大幅度的跟丢，关键帧数量增多，系统总跟踪轨迹长度增加，有效防止系统跟踪失败，提高系统的鲁棒性。可见，在ORB-SLAM2中融合K-Means和多适度自适应算法后，K-Means解决了误匹配剔除问题，使系统定位准确率上升，而融合多尺度自适应机制可以使系统适用于动态复杂环境，鲁棒性更好。
3.2. 性能评估实验
3.2.1. 轨迹对比实验
视觉里程计定位精度和鲁棒性是SLAM过程中最重要的评价标准。通常使用TUM数据集进行实验，一般为了评估SLAM系统的性能，会采用绝对轨迹误差(Absolute Trajectory Error, ATE)和相对轨迹误差(Relative Pose Error, RPE)对算法好坏进行评估，本文采用绝对轨迹误差(ATE)作为精度评价指标。为了体现算法的鲁棒性，选取不同场景进行实验，表3为不同场景下本文算法和ORB-SLAM2算法精度的对比结果。图10为ORB-SLAM2、本文算法与真实轨迹的对比图。
Table 3. Absolute Trajectory Error result (ATE, Unit: m)
表3. 绝对轨迹误差结果(ATE，单位：m)
Figure 10. Comparison between the two algorithms and the real trajectory in the direction of rotation
图10. 两种算法与真实轨迹在旋转对比
从表3和图10可以看出，在静态环境和低动态环境下，原始算法精度相对高动态环境下会高出不少，RMSE平均提升了20%，本文算法精度更加高。动态环境下，受相机速度影响，ORB提取效率降低，单目条件下还是容易跟丢，在融合多尺度自适应后匹配数目增多，在较少的特征点下可以从不同尺度计算特征点大致位置得到更多的匹配数据，从而得到更多的匹配对，保证系统不容易追踪失败，有效降低了系统跟丢概率，提高了系统的鲁棒性，并且RMSE提升可达60%以上，图10展示了本文算法轨迹更加接近真实轨迹。印证本文算法可以使里程计保持良好跟踪状态，从而提高系统的鲁棒性。
3.2.2. 实时性分析
无论任何SLAM系统，实时性都是非常重要的一项评估指标。本文添加了图像多尺度自适应搜索匹配和K-Means快速剔除误匹配，前者可以在上一帧和当前帧之间从不同层级开始进行匹配，加快匹配速度和提升匹配数量，后者可以快速剔除误匹配，保留高度正确的匹配，减少重定位次数，降低系统跟丢风险，提升整体算法精度。表4跟踪记录了在不同序列下ORB-SLAM2和本文算法对每一帧图像进行追踪的平均时间和系统总运行时间。
Table 4. Real time performance comparison between ORB-SLAM2 and ours
表4. ORB-SLAM2与本文算法实时性对比
考虑到ORB-SLAM2单目初始化的不确定性，每个序列进行三次实验，取平均值进行对比。从表4中可以看出，本文算法减少了系统追踪线程的运行时间。在静态和低动态环境下，初始化很稳定，会更容易得到许多有效的特征点，此时系统不容易丢失，本文算法时间成本会更低，相比原始算法时间节省最高达到1.9801 s，比原始算法平均节省1.45045 s。在高动态环境下，系统跟丢的可能性提高，金字塔多尺度自适应匹配需要花费一定的时间在有效关键点较少的情况下逐层搜索计算特征点的相对位置，获得更多的特征匹配点对，相较于原始算法时间优势没有在静态和低动态环境下明显，平均节省时间为0.0535 s。从轨迹的长度来看，在高动态环境下轨迹跟踪长度较原始算法提升了一倍，可以看出整体鲁棒性和准确度比原始算法更好。与此同时，追踪线程耗时明显优于ORB-SLAM2系统，在静态，低动态效率都有显著提升，高动态下耗时相当。
4. 结束语
针对传统特征点方法鲁棒性和精度低的问题，提出了一种改进的多尺度自适应特征点匹配和K-Means剔除误匹配的视觉KSLAM算法。提出的算法主要应用于SLAM经典框架前端，思路是首先对图像进行缩放，构建多层金字塔之后利用ORB对图像整体提取特征点，然后融合K-Means聚类原理对匹配后的特征点对进行挑选剔除最终得到更加准确的匹配数据，其次通过计算不同层级的特征点在当前帧和前一帧不同层级的大致位置，加快匹配速度和匹配质量，尽可能防止系统跟踪失败。实验结果表明，改进后的算法可以在较短的时间内得到大量正确率高的匹配，误匹配剔除后比原始ORB-SLAM2系统的匹配精准率平均提升了30%，且估计的轨迹更为接近真实轨迹。同时该算法用时平均降低1 s，系统鲁棒性和效率显著提升。
基金项目
贵州省基础研究(自然科学)项目(黔科合基础-ZK[2021]重点001)资助。
NOTES
*通讯作者。