基于误匹配剔除和地面约束的视觉SLAM 算法

黄丹丹，郝文豪，杨 阳

（长春理工大学 电子信息工程学院，长春 130000）

随着计算机视觉和机器人技术的迅猛发展，视觉同步定位与建图（Simultaneous Localization and Mapping,SLAM）算法作为一种重要的技术手段，已经在无人驾驶、AR/VR和三维重建等领域得到广泛应用[1,2]。前端视觉里程计和后端优化是视觉SLAM中关键的研究内容，前端视觉里程计主要通过对连续帧之间的特征点进行匹配和跟踪，估计相机在三维空间中的运动；后端优化通过构建最小化重投影误差，对相机位姿和地图进行全局一致性调整[3]。前端特征点匹配的准确性为后端提供了优化的基础，而后端优化又反过来优化前端估计的相机运动和地图，使得整个SLAM系统的精度更高、鲁棒性更强，并在复杂场景下保持稳定性[4]。

近年来，众多学者在前端视觉里程计领域提出了基于特征点法的开源方案[5,6]，这些方案通常需要对初匹配的特征点对进行误匹配剔除。VINS-MONO[7]算法在计算本质矩阵时使用随机采样一致性（Random Sample Consensus,RANSAC）算法剔除误匹配特征点对；ORB-SLAM[8]则利用特征点旋转主方向与RANSAC算法剔除误匹配的特征点对；SuperPoint[9]设计了一种自监督网络框架，使用端到端的深度学习方法完成特征点匹配与误匹配剔除。总的来说，RANSAC算法是一种比较常用的误匹配剔除方法，该算法是一种基于随机采样的算法，其结果可能对初始采样点的选择敏感，并且算法的结果依赖于参数的设置，如采样数量、迭代次数等。因此，仅仅依赖RANSAC算法难以有效剔除误匹配特征点对，需要进一步提高特征点对的匹配精度来提高SLAM系统的准确性。

在SLAM后端优化方面，越来越多的学者引入新的特征来增强位姿约束[10,11]，以提高SLAM算法的鲁棒性。Yang等人提出了CubeSLAM[12]算法，该算法引入物体特征，利用2D平面物体与3D物体模型构建距离约束、角度约束以及形状约束来优化位姿；王立玲等人提出了基于点线特征视觉惯性融合的机器人SLAM算法[13]，该算法引入线特征与惯性约束来优化位姿；晏小彬等人[14]提出了基于地平面的单目视觉辅助激光雷达SLAM系统，该算法使用激光雷达提取地面特征，并通过视觉与激光相辅相成优化位姿。从以上分析可知，引入新的约束能使后端优化结果更加收敛，防止陷入局部最优，提高SLAM系统的鲁棒性。

根据上述分析，本文从SLAM系统前端的特征点匹配以及后端的位姿优化入手，提高算法对未知环境定位与建图的准确性与鲁棒性。在视觉SLAM前端设计特征点对剔除策略，进一步保证特征点匹配的准确性，提高SLAM算法的精度；在后端利用RGBD（Red Green Blue Depth）相机获取的深度数据提取地面向量信息，并将地面约束引入后端优化，抑制解算结果在z轴上的偏移，提高优化精度，从而增强系统的鲁棒性。

1 算法框架

本文提出的SLAM算法利用RGBD相机获取环境信息，并在ORB-SLAM2框架的基础上进行算法开发，整个算法的处理流程如图1所示。算法主要由四个线程组成，分别是：特征点跟踪线程、地面提取线程、局部建图线程和回环线程，虚线部分为本文新增算法。其中，特征点跟踪线程和地面提取线程属于SLAM算法的前端部分，局部建图线程和回环线程属于SLAM算法的后端优化部分。特征点跟踪线程包括特征点的提取、匹配、误匹配剔除以及计算相邻帧之间的相机位姿变换；地面提取线程则负责提取地面点，并计算地面向量；局部建图线程用于更新地图点，并对前端位姿进行优化；回环线程则负责闭环检测和闭环矫正等任务。

图1 算法流程图Fig.1 Algorithm flow chart

图1中四个线程协同工作，使得本文算法能够实现对RGBD相机输入的环境信息进行实时定位与地图构建。特征点跟踪线程和地面提取线程提供了前端的关键功能，通过特征点提取与匹配、误匹配剔除和地面点提取等操作，为后端的局部建图线程和回环线程提供准确的输入数据。局部建图线程和回环线程通过地图点的更新和位姿优化提高定位精度和地图的一致性。相对于ORB-SLAM2框架，本文对特征点跟踪线程中的特征点剔除模块进行改进以提高位姿估计的准确性，并增加了地面提取线程以提高位姿优化的鲁棒性。

这种算法设计使得系统能够充分利用RGBD相机的深度信息，从而提高对环境的感知能力。同时，通过前后端的协同优化，可以实现较高精度和鲁棒性的定位与建图。

2 特征点对误匹配剔除

ORB-SLAM2采用RANSAC算法剔除误匹配点，对初值敏感且参数设置复杂。针对该问题，提出一种利用Boxplot四分位算法与直方图统计算法进行误匹配特征点对剔除的方法，该方法能够对匹配的特征点对进行评估，并剔除误匹配特征点对，提高SLAM系统前端的准确性。特征点对的表达式为：

其中，P为当前帧的特征点集，与之相对应匹配帧的特征点集为Q，点集元素pi和qi为匹配的特征点；(u1i,v1i)表示当前帧第i个特征点的像素坐标；(u2i,v2i)表示匹配帧第i个特征点的像素坐标；N为匹配特征点的个数。

将特征点对的欧氏距离作为误匹配剔除的预处理阶段。特征点对的欧式距离集合L的表达式为：

采用统计学计算分析特征点对的分布特性。首先，使用Boxplot算法对预处理的欧氏距离集合进行异常值检测与剔除。Boxplot四分位算法是一种常用的统计学数据分析方法，通过四分位数描述数据的分布情况，由上下四分位距离确定异常值的存在。该方法可以评估特征点对的欧式距离是否存在异常值，进而识别并剔除超过上下限的特征点对，减少误匹配特征点对的干扰。对集合L中的元素从小到大进行排序，得到新的集合记为L*。设有序集合L*的第1/4个数为Lb1，第3/4个数为Lb2，则经过Boxplot算法处理后的欧式距离集合L1为：

其中，IQR为四分位距。

其次，采用直方图算法分析特征点对欧氏距离L1的数据分布情况。在特征点对匹配较好的情况下，所有特征点对的欧氏距离应相对集中在一定范围内；当出现误匹配时，欧氏距离的变化范围就会增大。为此，本文算法设定固定的直方图组数，将特征点对的欧式距离集合数据放置于直方图中，并计算每个区间内数据点的数量或频率。相对集中的数据被认为匹配关系较好，分布于直方图边缘区间的数据被认为匹配关系较差，并对直方图按不同区间数据个数进行排序。为了防止过多地剔除特征点对，本文算法提取前80%的数据作为正确的匹配点，剔除其余特征点对。

本文算法的优点在于Boxplot算法和直方图算法可以有效互补，其中直方图算法能够直观地展示数据分布情况，当异常数据较多时会由于频数较高而将错误的匹配对当作正常值，而Boxplot算法能够有效地弥补直方图的缺陷从而更好地剔除异常值。通过综合使用这两种算法，能够在SLAM算法的前端进一步提高特征点匹配的质量，有效地降低误匹配对定位和建图的影响。

3 地面向量提取与地面约束

3.1 地面向量描述与提取

地平面一般方程表示为：

其中，n为地面向量；G为地面上的三维空间点；d为平面方程的截距。

为此设地面参数为π=[nTd]T，设π(w)为世界坐标系地面参数，π(c)为相机坐标系地面参数，Rwc为相机坐标系地面参数到世界坐标系的旋转矩阵，twc为相机坐标系地面参数到世界坐标系的平移向量，则可以用式(6)描述地面参数在不同坐标系下的转换关系：

根据针孔相机模型，3D坐标点与像素点的转换关系为：

其中，u、v为像素坐标；fx、fy、cx、cy为相机内参；X、Y、Z为该像素坐标在相机坐标系下的3D空间坐标点云。

通过式(7)可以将RGBD相机采集的深度图恢复成像素点在相机坐标系下的3D坐标点云。设相机坐标系符合右手定则，坐标系x方向向右，y方向向前，z方向向上。当深度图转换为3D点后，会得到大量的点云数据，但是将所有的3D点进行平面拟合可能会导致计算复杂度较高，特别是在相机分辨率较大的情况下。为了提高算法的计算效率和性能，本文算法仅选取z轴负方向上的3D点云用于地面向量拟合。

由于地面特征的法线真值必然位于垂直方向，为了得到更准确的地面点云，根据每个点云的法向向量，剔除掉非垂直法线向量的点云，然而单纯利用一个点云数据无法得出该点云的法线向量。为此，本文采用最近邻搜索算法搜索该点云附近的点云作为点云簇，构建点云簇与待求方向向量的约束方程。

其中，k个点组成的点云簇记为M={mi|i=1,2 …k}；c为点云簇的中心点；n*为待求点云单位法向量；n为当前估计的点云法向量；Ω为向量归一化函数。

式(8)表示点云簇里每个点与中心点所构成的向量与当前估计方向向量的点乘之和最小即为所求的方向向量。

随后对剔除掉非垂直法线向量的点云进行平面拟合，为了提高拟合平面的鲁棒性，基于RANSAC算法，从地面垂直法向点云数据G*={gi|i=1,2…

k1}中随机选取三个点进行平面拟合，拟合方程为：

通过构建最小化点到拟合平面的距离作为平面拟合的代价函数，代价函数为：

通过算法多次迭代选点，取代价函数最小时的平面方程作为最终拟合的平面。

3.2 加入地面约束的后端优化

后端的作用是对前端视觉里程计得到的位姿进行优化，优化的过程主要依靠地图点约束和地面向量约束。优化因子图如图2所示。

图2 优化因子图Fig.2 Optimization factor graph

构建地图点约束时，采用ORB-SLAM2进行地图点的生成与剔除，利用每个特征点的像素坐标与其相对应的地图点坐标构建观测方程：

其中，γ为像素坐标，即γ=(u,v)；Tcw为待优化位姿；V为像素坐标对应的空间坐标；h函数为世界坐标系到像素坐标系的转换方程，即

其中，S为地图点深度值；K为相机内参矩阵。

为此，一个特征点的像素点与其地图点构成一个观测方程，设共有j对特征点与其地图点相匹配，则能够提供j个观测方程，组成一个超定方程。

采用最小二乘求解超定方程(13)的解，T*表示最小二乘的解。

当地面平整时，由于地面的平面法向量具有固定的方向和高度信息，后端可以使用地面向量约束相邻帧之间的相机运动，从而提高相机位姿的估计精度。构建地面向量约束时，利用当前帧采集的地面信息与实际地面信息建立角度误差与距离误差方程。

其中，α为角度因子；β为距离因子；nw和dw为实际地面的平面参数；nc和dc为当前帧拟合地面的平面参数。

后端优化总的约束方程为：

4 实验分析

4.1 实验环境

本文实验使用的硬件环境为搭载Intel i9-12900KF处理器的计算平台，软件环境为Ubuntu 20.04操作系统，并使用C++14编程语言在ORB-SLAM2框架下进行算法代码开发，平台没有使用GPU加速。本文选用TUM RGB-D数据集[15]与CollaborativeSLAM数据集[16]作为输入数据，TUM RGB-D数据集是一个多功能的RGBD数据集，包含多个室内和室外场景，涵盖静态和动态场景，提供了丰富多样的环境和运动情况；CollaborativeSLAM数据集提供了大范围场景下的RGBD数据序列，这两个数据集均提供了准确的真值信息和同步采集的RGB图像和深度图像，并附带精确的时间戳信息。在实际环境中，利用Orbbec RGBD相机进行数据采集，验证本文提出的算法。

4.2 特征点对误匹配剔除算法的性能测试

为了测试误匹配剔除功能的鲁棒性，本文分别在静态和动态环境下评估算法的性能。选取了TUM数据集中的静态子序列：fr2_360_hemisphere、fr2_360_kidnap、fr2_desk、fr2_large_with_loop 以及TUM数据集中的动态子序列：fr3_walking_halfsphere。这些数据集子序列均提供了真实轨迹作为参考，以确保评估的准确性。在评估算法性能时，本文采用了绝对轨迹误差（Absolute Trajectory Error,ATE）和相对位姿误差（Relative Pose Error,RPE）作为评价指标。ATE衡量了估计轨迹与真实轨迹之间的整体误差，计算公式为：

其中，Nf为帧数；Tgt,i和Test,i分别表示第i帧的真实轨迹与估计轨迹位姿；⋁表示反对称矩阵到向量的映射。

RPE则关注相邻帧之间的相对位姿误差，计算公式为：

测试时，将原始ORB-SLAM2算法计算的特征点对与加入误匹配机制后剔除的特征点对进行图像化显示。原始ORB-SLAM2算法与增加误匹配特征点对剔除机制的本文算法在静态环境下的对比效果如图3所示，性能对比如表1所示；动态环境下两种算法的对比效果如图4所示，性能对比如表2所示。

表1 静态环境下本文算法与ORB-SLAM2 性能对比Tab.1 Performance comparison of the proposed algorithm with ORB-SLAM2 in static environment

表2 动态环境下本文算法与ORB-SLAM2 性能对比Tab.2 Performance comparison of the proposed algorithm with ORB-SLAM2 in dynamic environment

图3 静态环境算法对比效果Fig.3 Comparison effect of algorithms in static environment

图4 动态环境算法对比效果Fig.4 Comparison effect of algorithms in dynamic environment

图3(a)为原始ORB-SLAM2 算法在静态环境提取的特征点对，从图中可以观察到这些特征点对包含了误匹配的情况，从结构上看，误匹配的特征点对与正确匹配点对存在明显的角度偏差或者距离偏差。图3(b)为本文算法检测到的误匹配特征点对，从图中可以看出统计学方法能够很好地对特征点对进行异常值筛选，从而找出误匹配特征点对。

表1 展示了在静态数据序列中，本文提出的误匹配剔除算法与原始ORB-SLAM2 的性能对比。从表中可看出在平均剔除5.43%误匹配特征点对的情况下，本文算法的ATE 平均减小了17.10%，RPE 平均减小了21.67%，验证了剔除少量的误匹配特征点对能够提升SLAM 建图的精度。

图4(a)展示了原始ORB-SLAM2算法在动态环境中提取的特征点对，由于受行人影响，大量误匹配的特征点对出现在行人身上。在动态环境中，特征点对可能会出现更大的角度变化或距离变化，从而导致误匹配更加明显，而本文提出的误匹配剔除机制算法有效地检测出了行人身上的误匹配特征点对，如图4(b)所示。

从表2可以看出，与ORB-SLAM2算法相比，本文算法的ATE减小了19.72%，RPE减小了18.18%。此外，与静态环境相比，动态环境下剔除的误匹配特征点对提升了6.78%，表明了本文算法在动态环境中也有很好的鲁棒性。

4.3 增加地面约束的后端优化性能测试

本文首先对数据集中的图像序列进行地面提取测试，地面提取流程图如图5所示。

图5 地面提取流程可视化Fig.5 Visualization of the ground extraction process

图5(a)为利用RGBD相机采集的RGB图和深度图，结合相机内参矩阵恢复出来的RGB点云图；图5(b)通过搜索50个最近邻点来拟合点云方向向量，图中小线段为拟合的方向向量，从图中可以看出拟合的方向向量与表面垂直度较好，拟合效果较好；图5(c)为剔除掉非垂直方向向量的效果图，从图中可以看出本文方法能够剔除图5(b)中的非垂直向量，利用剩余的点云进行平面提取，提取效果如图5(d)所示。图5(d)中的正方形平面即为提取的地面，由图可知本文算法能够准确地提取地平面。加入地面约束后，本文算法（图中用ours表示）与原始ORB-SLAM2算法的位姿对比如图6所示。

图6 位姿对比Fig.6 Comparison of postures

从图6可以看出，在z轴轨迹中，ORB-SLAM2算法相对于真值轨迹不稳定，会发生漂移，波动较大，而本文算法与真值轨迹更加贴合，更加稳定。本文将地平面向量引入后端优化过程，系统可以更精确地调整相机位姿，确保地面点在不同帧中保持一致性，从而有效减小地面区域的漂移问题。地面约束在后端优化中促使地图点云与相机位姿更加紧密地匹配，提升了SLAM系统的准确性和鲁棒性。

4.4 本文SLAM 算法的性能测试

实验对原始ORB-SLAM2算法与本文算法性能分别进行测试，并在CollaborativeSLAM数据集与实际环境下进行多次测试。CollaborativeSLAM数据集包含了不同的室内场景，并且数据集在多个环境下进行录制，具有较高的挑战性。取连续30次测量的中位数作为算法性能的最终结果，通过这种方式可以获得更可靠且准确的实验结果，排除算法运行中由于随机性引起的偶然误差。本文算法与ORB-SLAM2算法的运行轨迹对比如图7所示，两种算法在实际场景中的测试结果如图8所示。

图7 轨迹对比Fig.7 Track comparison

图8 实际环境算法对比Fig.8 Comparison of algorithms for real-world environment

从图7可以看出，本文算法的效果与真值轨迹更贴合。图8为实际场景中本文算法和ORB-SLAM2算法的对比，实际场景是从起点出发行驶一圈后再回到出发点。由图8可知，本文算法的轨迹路径为封闭曲线，与实际位姿变化情况相符；ORB-SLAM2算法路径未封闭，轨迹误差相对本文算法较大。

本文算法与ORB-SLAM2算法在TUM数据集序列上的测试结果如表3所示。

表3 本文算法与ORB-SLAM2 算法在不同数据集测试结果（单位：米）Tab.3 Test results of the proposed algorithm and ORB-SLAM2 algorithm on different datasets (unit: m)

由表 3 可知，与ORB-SLAM2算法相比，使用误匹配剔除算法的ATE平均减小了17.62%，RPE平均减小了20.97%；使用地面约束算法的ATE平均减小了39.20%，RPE平均减小了44.20%，验证了本文算法能够提高SLAM系统精度。

5 结论

本文提出了一种基于误匹配剔除和地面约束的SLAM算法，该算法基于ORB-SLAM2框架。在前端视觉里程计阶段，采用Boxplot四分位算法和直方图统计算法识别和剔除误匹配的特征点对，从而降低前端视觉里程计的误差，使得前端能够提供更精确的初始估计位姿，为后续优化奠定了坚实的基础；在后端优化阶段，通过计算点云方向向量剔除非垂直的地面向量以进行地面拟合，并将地面约束引入后端优化方程，使得SLAM系统更加鲁棒。在实验部分，本文在静态数据集与动态数据集上分别测试了误匹配剔除机制的效果，并对地面进行拟合，最后给出了该算法在不同数据集上的测试效果。由测试结果可以看出，增加了误匹配剔除机制与地面约束有效地优化了前端和后端，能够提升SLAM系统的准确性与鲁棒性。