基于平面匹配与目标检测的视觉SLAM算法

娄 路，张 忍，李一天，隗寒冰，王桂平

(1.重庆交通大学 信息科学与工程学院，重庆 400074；2.重庆交通大学 机电与车辆工程学院，重庆 400074)

0 引 言

同时定位与建图(simultaneous localization and mapping，SLAM)由SMITH等[1]提出，而视觉SLAM(visual-simultaneous localization and mapping，V-SLAM)是指用相机作为传感器，在未知环境中估计相机位姿的同时建立周围环境模型。目前V-SLAM算法逐渐成熟，MUR-ARTAL等[2]提出的ORB-SLAM2算法在ORB-SLAM[3]基础上，增加双目和RGB-D模式，能够实现地图重用、闭环检测和重新定位的功能。YU等[4]提出的DS-SLAM是一种适用于动态环境中的语义SLAM算法，增加了语义分割模块和运动一致性检查模块，虽然在动态环境中提高了定位精度，但运行速率下降。BESCOS等[5]提出的DynaSLAM因为增加了动态物体检测和背景修复功能模块，所以导致检测耗时较长，实时性较差。WU等[6]提出的YOLO-SLAM包含轻量级目标检测网络，并结合一种新的几何约束方法来过滤检测区域的动态特征，该算法虽能达到实时效果，但无法应对低纹理、光照不足等环境问题。为解决V-SLAM遇到的这些问题，一些结合点和线特征来估计相机运动的方法[7-9]，或者结合点和面特征来估计相机位姿的方法被陆续提出来了[10-13]。

尽管V-SLAM算法已经取得了重大成就，但大多数算法只使用点特性作为特征信息，在低纹理环境下或者在光照变化场景下仍很难工作。本文提出一种基于单目和RGB-D视觉，融合快速目标检测和平面匹配的三维重建SLAM算法(fastest yolo plane mapping SLAM，FPM-SLAM)，实验结果表明，该算法同时满足户外和室内的工作场景需求，并解决动态复杂环境下可能存在的定位精度下降甚至失败的问题。

1 本文算法介绍

1.1 FPM-SLAM算法框架

本文提出的算法利用Yolo-Fastest[14]快速检测目标的特性，将检测到的运动物体上的特征点剔除掉，再结合室内结构化平面特性，应对可能存在的低纹理、光线较暗等情况。FPM-SLAM算法整体流程如图1所示，图2是FPM-SLAM算法的可视化示意图。FPM-SLAM算法在获得RGB图像和深度图像后，同时进行跟踪、可移动物体检测、平面匹配模块处理，主要步骤如下：

图1 FPM-SLAM算法整体流程

图2 FPM-SLAM算法流程可视化

(1)RGB图像传入目标检测模块，Yolo-Fastest算法检测到感兴趣区域(region of interest，ROI)，并将结果传入跟踪模块。

(2)深度图传入平面匹配模块，从深度图像中恢复点云地图，利用CAPE[16]方法完成平面提取，再利用几何约束方法进行平面匹配并将结果传入跟踪模块。

(3)跟踪模块首先提取ORB特征点，然后完成轨迹地图初始化。跟踪模块等待可移动物体检测模块传入的结果，剔除位于ROI上的ORB特征点，然后将关键帧传入局部建图模块和稠密建图模块。

(4)跟踪模块将关键帧传入稠密建图模块，重建三维点云，通过统计滤波剔除离群点，体素格滤波降采样，最后保存生成的稠密三维点云地图。

(5)局部建图为了增加局部地图点数目，关键帧之间会重新进行特征匹配，生成新的地图点；局部BA优化共视图里的关键帧位姿和地图点，删除不准确的地图点和冗余关键帧。

(6)闭环模块通过词袋法检测是否闭环，计算当前关键帧和闭环候选关键帧之间的位姿，进行闭环矫正。

(7)最后全局BA模块优化所有的关键帧及其地图点。

当机器人在户外工作时，FPM-SLAM算法只运行①②④⑥⑦模块。

1.2 目标检测算法

本文算法框架中的可移动物体检测模块采用了Yolo-Fastest[14]模型。Yolo-Fastest提出的超轻量级网络模型，由于搭配了移动端轻量级神经网络EfficientNet-lite[15]，该模型在运行性能方面表现很好，训练好的模型只有1.3 MB。图3和图4分别是EfficientNet-lite与其它网络模型的预测速度对比和模型体积对比，带五角星的线表示与之对比的骨干网络，从图中可以看出EfficientNet-lite网络模型体积小而且预测速度快，使用的参数和计算数量非常低，计算量仅为250 mflops，在移动终端可以达到178 fps。本文采用Yolo-Fastest训练Pascal VOC 2014数据集，该数据集有20个类别，训练环境：win11操作系统，Intel i5 CPU，内存16 GB，RTX3060，显存12 GB；参数设置100 000次迭代，Batch为64。最终将训练模型融合到FPM-SLAM算法中，既满足检测精度的要求，也能够完成对动态场景下的实时性检测。

图3 EfficientNet-Lite与其它网络模型的预测速度对比

图4 EfficientNet-Lite与其它网络模型体积对比

1.3 平面匹配算法

本文提出了一个同时使用点和平面的视觉SLAM算法来实现室内环境下机器人相机位姿的估计。室内环境中有许多平行和垂直的平面结构(如桌面、地面、墙面等)，使用这些结构约束可以帮助实现平面的长期关联，使得累积误差更小，解决低纹理环境中定位下降问题。采集图像中普遍存在平面特征，与点特征相比，平面特征具有受测量噪声影响小、容易提取、计算量低等优点。圆柱体和平面提取(cylinder and plane extraction，CAPE)是Proença等[16]提出的一种快速三维点云平面和圆柱体提取方法，利用分割算法从稠密三维重建点云提取平面和圆柱体。本文采用CAPE方法只对平面特征进行快速提取并匹配，与现有的方法相比，该方法更快且平面分割结果更具有一致性，平面生成匹配方法见表1。首先将输入的深度图像转换为三维点云，然后将点云分割成平面网格，通过主成分分析(principal component analysis，PCA)来计算网格的参数，再利用平面的法向量进行直方图的统计，从而完成每个网格的平面分割，最终采用平面匹配从候选平面中计算出最佳配对平面。

表1 平面生成匹配方法

平面匹配阶段，本算法利用RGB-D相机，从深度图像中生成点云来提取平面。RGB-D相机提供RGB图像和对齐的深度图，深度图保存了每个像素的距离信息，所以本文使用针孔相机模型(式(1))恢复点云，其中 (X，Y，Z)T是三维空间中的坐标点，K是内参矩阵，Z是深度值，P点在像素平面的像素坐标为u=(u，v)T，(cx，cy) 表示像素坐标系下的中心坐标，fx和fy是相机的焦距

(1)

由深度图像生成的点云被组织起来，具有类似网格的图像结构，有组织的结构可以从点云中快速分割平面，最终利用局部地图中的所有点和平面对当前位姿进行了优化。其次增加几何约束，给定两个Hessian平面方程A{N，d} 和B{N′，d′}， 图5给出了CAPE算法中两个平面匹配的几何表示，平面匹配需要满足以下3个条件：

图5 CAPE算法中两个平面的几何表示

(1)平面A和平面B法向量之间的夹角arccos(N·N′) 小于一定的角度θ；

(2)平面A和平面B各自原点到平面距离之间的差值 |d-d′| 小于一定阈值∂；

为了从候选平面之中选择最佳的平面，本文选择产生最小平面间距离的候选平面，则两个平面A和平面B之间的距离如式(2)所示，其中C和C′分别是平面A和平面B上的点

(2)

1.4 稠密三维点云重建

本文算法可在动态环境下输出稠密三维点云地图，用于机器人导航与避障等高级应用。三维稠密点云建图从跟踪模块中获得新的关键帧，利用关键帧的变换矩阵和深度图像来生成局部点云，具体流程如图6所示。

图6 稠密建图流程

(1)读取一帧RGB图像和深度图像，根据相机的内参矩阵估计相机位姿，同时跟踪模块会提取ORB特征点，并判断当前帧是否为关键帧，将得到的所有关键帧的深度图像转化为3D点云。

(2)统计滤波去除离群点，因为离群点的存在让局部点云特征的估计变得困难，可能导致点云配准失败，所以统计滤波会对每个点进行统计，计算每一点到其邻域所有点的平均距离，剔除大于平均距离的邻域点。如果得到的是高斯分布，可以计算出均值μ和标准差σ，标准差的倍数设置th=1， 对每个点的邻域点的个数设置为50，那么去除邻域距离大于μ+th*σ区间之外的点。

(3)体素网络滤波器降采样，由于多视角生成的点云存在重叠部分，这会占用大量存储空间。在体素滤波器中，用体素的重心来近似替代体素中其它点，相当于降采样。本文把分辨率设成0.01，表示只存储0.01×0.01×0.01格子中的一个点，体素中的大量点云被删除，仅保留体素重心的一点，因此点云存储空间大幅减少。

(4)进行点云拼接，得到由三维点组成的稠密点云地图。点云的创建需要提取图像中具有深度信息的所有像素，如果是带颜色通道的点云，则需要存储每个点云对应的RGB值。点云拼接的作用是将连续帧的点云拼接在一起，从而创建出整个环境的点云地图。在视觉SLAM算法中，通常将第一次出现的关键帧设置为世界坐标系，后续生成的关键帧需要以首帧为参考，将对应的点云转换到世界坐标系下，与之前的点云进行拼接，从而形成完整的点云地图。设点Pc=(Xc，Yc，Zc)T是相机坐标系下的坐标，点Pw=(Xw，Yw，Zw)T为世界坐标系下的坐标，Tcw为相机坐标系到世界坐标系的变换矩阵，R和t分别为旋转和平移矩阵，具体公式如下

(3)

(5)如果还有数据帧输入，则继续(1)操作；否则保存稠密三维点云数据并结束。

2 实验结果及分析

本文在户外自动驾驶公开数据集KITTI[17]和室内机器人数据集TUM[18]上对FPM-SLAM算法进行评估，并与ORB-SLAM2、DynaSLAM、DS-SLAM算法的实验结果进行比较，由于跟踪结果需要在单目中对齐和缩放，本文使用绝对轨迹误差(absolute trajectory error，ATE)的均方根误差(root mean square error，RMSE)来评估该算法在动态场景中的定位精度。均方根误差描述相机估计位姿与真值位姿之间的差值，RMSE值越小表示误差越小。假设相机估计轨迹为Testi，i， 真实轨迹Tgt，i， 其中i=1，…，N， 那么相机的绝对轨迹误差的均方根误差计算公式如下

(4)

2.1 在TUM RGB-D数据集上的评估

TUM RGB-D数据集[18]包含了微软Kinect传感器在室内环境下采集的RGB和深度图像。数据记录了速率(30 Hz)的640×480分辨率图像，真值轨迹是由具有8个高速跟踪摄像机(100 Hz)的高精度运动捕捉系统获得。在“坐着”的序列中，两个人坐在一张桌子前聊天，做着手势，运动幅度很小。在“行走”序列中，两个人绕着镜头走，然后坐在一张桌子前。由于该数据集一些序列是高度动态的，它将影响传统视觉SLAM算法的准确性。这些序列可分为以下几种相机运动类型：

(1)halfsphere表示相机沿半径为0.5 m的半球面轨迹移动。

(2)xyz表示相机沿xyz轴移动。

(3)rpy表示相机在俯仰角、偏航角、滚转角上旋转。

(4)static表示相机保持静止不动。

FPM-SLAM算法在TUM RGB-D数据集上运行部分序列(fr3_walking_rpy、fr3_walking_xyz)实验结果如图7、图8所示。图7(a)、图8(a)显示了FPM-SLAM算法提取ORB特征点的效果，输入一张RGB图像到跟踪模块中，当算法检测到可移动物体(比如图像中的人)时，用矩形框标记并剔除掉可移动物体上的ORB特征点，再对图像完成后续工作。在图7(b)光线较暗的环境中，算法提取的ORB特征点较少，容易导致跟踪丢失，因此将深度图输入到平面匹配模块，从深度图中恢复点云并分割成多个平面(对应图7(b)地面匹配的两个平面)，使用平面匹配算法解决这一问题。在图8(b)中，办公桌上有明显的平面特征，算法进行ORB特征点匹配时，也进行平面匹配(对应图8(b)桌面匹配的两个平面)。图7(c)、图8(c)显示了估计位姿和真实位姿的误差轨迹图，图中虚线部分表示真值轨迹，实线部分表示相机估计位姿轨迹，柱线条越靠近下端，表示轨迹误差越小，越靠近柱线条上端，表示绝对轨迹误差越大。因为本文的算法性能在这两个序列中表现良好，所以是实线呈现，虚线的真值轨迹被实线覆盖。

图7 fr3_walking_rpy序列效果

图8 fr3_walking_xyz序列效果

表2显示了在TUM RGB-D数据集的6个序列中，FPM-SLAM分别与几种经典SLAM算法ORB-SLAM2、DynaSLAM、DS-SLAM的绝对轨迹均方根误差对比结果。在表2中，前4个序列是高度动态的环境(有人在走动)，后两个序列是低动态的环境(有人坐着打手势)。总之，本文所提算法在TUM RGB-D数据集中的结果与DynaSLAM和DS-SLAM相似。当在高动态环境中消除动态对象(如行人)上的特征点时，可以提高定位精度(如fr3_w_halfsphere、fr3_w_xyz、fr3_w_pry、fr3_w_static序列)。但在低动态环境下(如fr3_s_xyz、fr3_s_halfsphere)，FPM-SLAM算法的轨迹误差高于DynaSLAM算法，这是因为在去除位于可移动物体区域的特征点后，剩余的特征点大部分位于较远区域，导致定位精度下降。

表2 TUM数据集绝对轨迹误差的RMSE对比/m

FPM-SLAM算法在TUM RGB-D数据集上运行的部分序列(fr3_walking_xyz、fr3_walking_halfsphere)建图效果如图9、图10所示。图9(a)、图10(a)显示未剔除移动物体的建图效果，由于人的走动给建图带来了不便，重建的点云图中有许多重影；图9(b)、图10(b)显示了剔除移动物体后的建图效果，重影消失，重建效果较好。

图9 fr3_walking_xyz建图效果

图10 fr3_walking_halfsphere建图效果

2.2 在KITTI数据集的实验

KITTI数据集[17]是由德国卡尔斯鲁厄理工学院和美国丰田理工学院联合采集，用于评估自动驾驶场景中的计算机视觉算法。采集车上安装了两台高分辨率的彩色和灰度相机，RTK-GPS提供了精确的运动轨迹真实值。数据集包含了中等城市街道、乡村道路和高速公路上的单目、双目相机、激光雷达、GPS等多种传感器数据，提供了11个序列带有真值轨迹进行测试评估。

FPM-SLAM算法在KITTI数据集的实验结果如图11、图12所示。图11(a)、图12(a)显示了FPM-SLAM算法提取ORB特征点的效果，输入一张RGB图像到跟踪模块中，当算法检测到可移动物体(比如图像中的车)时，用矩形框标记并剔除掉可移动物体上的ORB特征点，再对图像完成后续工作。图11(b)、图12(b)显示了具有多个闭环的城市环境轨迹和稀疏重建。图11(c)、图12(c)显示了估计位姿和真实位姿的误差轨迹图，采用KITTI00和KITTI05序列对定位轨迹的精度进行评估。

图11 KITTI_00序列效果

图12 KITTI_05序列效果

表3显示了3种算法在KITTI数据集的11个序列中轨迹精度的比较。从表中可以看出，本文的算法在某些情况下的结果与ORB-SLAM2和DynaSLAM相似。当场景中有许多动态物体时(如移动的汽车、自行车和行人等)，该算法可以有效地提高SLAM(如KITTI03、KITTI04、KITTI09)的定位精度。但当大多数物体处于静止状态，FPM-SLAM比DynaSLAM有更大的误差，因为这些序列(如KITTI00和KITTI02)包含许多近距离的静止车辆，大多数ORB特征点在较远地区，当剔除车辆上的特征点，这可能导致定位精度下降。

表3 KITTI数据集单目绝对轨迹误差的RMSE对比/m

2.3 算法实时性评估

为测试算法的实时性，本文分别选取TUM数据集“高动态”3个序列，“低动态”两个序列。测试图像分辨率设置为640×480，由于SLAM算法的跟踪模块对图像实时性要求高，所以本文只对比跟踪模块所耗费时间。本实验的环境采用Intel i5 CPU，内存16 GB进行测试。

表4中DynaSLAM算法在测试序列的跟踪模块算法平均耗时2.667 s；本文提出的FPM-SLAM算法平均耗时29 ms。在fr3_s_halfsphere序列中，FPM-SLAM算法在跟踪模块中耗时0.026 ms，算法整体运行速率22.20 fps；DynaSLAM算法耗时2.138 s，算法整体运行速率0.48 fps。因此，本文算法较DynaSLAM算法有明显提升，满足室内环境实时跟踪的需求。

表4 跟踪模块耗时对比/s

3 结束语

本文提出FPM-SLAM算法，结合点和面特征及深度学习，提高视觉SLAM在低纹理和动态复杂环境下的精确性和鲁棒性。但在光照条件差的环境下，算法的鲁棒性有待提升，后续将引入IMU传感器提升算法的定位精度；在稠密三维地图重建部分，即使经过统计学滤波、体素格滤波后，最后得到的地图文件仍然很大，后续将转换为八叉树地图减少存储空间。本文主要创新点如下：

(1)将轻量级目标检测算法Yolo-Fastest融入所提出的算法中，在户外KITTI数据集上的实验中，与ORB-SLAM2相比，本文的方法绝对轨迹误差RMSE指数显著降低66.67%；

(2)将平面特征匹配方法融入到所提出的算法中，在室内TUM数据集上实验，本文的方法在大多数测试序列中效果比DS-SLAM和DynaSLAM要好，与ORB-SLAM2算法比较，绝对轨迹误差RMSE指数显著降低98.77%，而且运行速度得到了有效提升，跟踪模块平均耗时仅29 ms；

(3)将稠密建图方法融入到所提算法中，剔除掉动态物体后生成稠密三维点云地图，可用于机器人后续执行导航、避障和环境理解等高级应用任务。