点击下方卡片，关注「3D视觉工坊」公众号
选择星标，干货第一时间送达
来源：3D视觉工坊
星球内有20多门3D视觉系统课程、3DGS独家系列视频教程、顶会论文最新解读、海量3D视觉行业源码、项目承接、求职招聘等。想要入门3D视觉、做项目、搞科研，欢迎加入！
移动机器人在执行同步导航与定位建图工作任务时，往往具有非常多的复杂不确定性及环境干扰。其中之一便是动态环境带来的动态特征点扰动造成特征点难以关联。针对动态环境中视觉SLAM系统错误地特征关联将动态物体的特征点识别为路标点，从而导致定位精度下降的问题，该文提出基于语义信息和多视图几何相结合的SLAM算法。首先以ORB-SLAM3为主体进行拓展，升级基于四叉树的ORB特征提取方法，引入改进的YOLO目标检测线程，增加FasterNet网络和CBAM注意力，采用多视图几何约束进行动态特征点滤除，采用静态特征点进行状态估计以保证SLAM定位精度。在公开数据集TUM上进行测试。实验表明：相较于传统的ORB-SLAM3算法，YOLO-SLAM在动态环境下工作，APE误差的代表性指标均方差降低90%以上，可以相当程度上解决环境中动态特征点关联困难的问题。
0 引言
随着人工智能技术的不断推进与发展，机器人定位与导航技术越发成熟[1]。在视觉SLAM研究领域，绝大多数的SLAM算法都是假设或半假设：算法作用于静态场景或低速移动场景，这很大程度上影响了所得算法的实际应用与推广，因为明显不符合当前实际生产生活的需要[2-3]。
为了解决实际工作场景中，动态物体运动以及场景变化会对SLAM（simultaneous localization and mapping）算法带来的状态估计误差问题，很多国内外学者提出了自己的见解。这些想法主要包括三大类：第一种利用LK（Lucas-Kanade）光流法结合多视图几何去除异常显示的特征点，也即以间接法为代表性的算法有GTSAM算法以及ORB-SLAM算法系列[4-6]。第二种是直接法，主要代表算法有DSO以及LSD-SLAM等，通过直接使用图像像素值进行特征匹配和场景重建，不需要将图像转化为特征点，剔除异常像素区域点再进行状态估计误差，进而提高视觉SLAM算法的定位精度[7]。第三种则是近些年发展起来的，既有直接法又有间接法的使用，主要代表算法有Deep-SLAM、Dyna-SLAM等。通过利用现有的SLAM算法结合深度学习卷积神经网络语义分割等手段去除掩膜伪影，剔除不符合要求的特征匹配点，最后进行状态估计误差测算，也一定程度上缓解了该问题[8]。但是以上算法都存在一定程度上的问题，诸如检测效率低、无法做到实时，场景要求严苛，计算资源庞大等。
视觉SLAM通俗意义上讲是指移动机器人在移动过程中利用自身携带的多种传感器提供的信息来准确地确定自身位置，进而决策自身运动、导航以及建图[9]。
对于开源项目ORB-SLAM3进行机器人定位测试，研究发现环境中的动态物体对SLAM系统的定位精度造成了较大的干扰。这是因为视觉SLAM系统会从动态物体上提取特征点，并把这些特征点作为路标用于位姿估计。为了提高视觉SLAM的定位精度，系统需要剔除这些动态物体。为此，本文提出了一种基于语义信息的方法来去除动态环境中的动态特征点，其基本流程如图1所示。
首先，视觉前端使用目标识别算法对图像进行识别，得到目标物体的语义标签[10]。这是判断物体运动属性的基础。然后，基于得到的语义信息对动态特征进行判断和剔除，最后利用剩余的静态特征点并结合系统帧间位姿估计方法进行相机的位姿估计，最大限度地确保SLAM系统的实时性能，并提高系统整体的定位精度。
本研究的核心贡献在于提出了一种新型的视觉SLAM算法，该算法专门针对动态环境进行了优化。语义信息融合：在ORB-SLAM3前端引入了基于YOLOv5s-FasterNet的轻量级目标检测网络，实现了对动态物体的实时识别。通过结合语义信息，该算法能够在特征提取阶段有效去除动态特征点，从而减少了它们对相机位姿估计的干扰。多视图几何应用：利用多视图几何理论，提出了一种基于几何约束的动态特征点剔除方法。这种方法通过计算特征点的对极距离，并设定阈值来滤除动态特征点，进一步提高了系统的定位精度。实验结果表明方法的性能与鲁棒性显著提升，这使得SLAM系统能够在更广泛的应用场景中提供准确的定位服务。
1 改进的视觉SLAM框架设计
1.1 视觉SLAM系统框架概述
改进后的视觉SLAM系统在视觉前端加入了目标检测线程，对应改进的视觉SLAM框架如图2所示。该线程对输入的图像帧序列进行高斯滤波平滑处理，再进行直方图均衡图像处理，同时再利用多尺度方法对图像裁剪处理，目的是使得预处理过的图像具备更高的图像质量和可识别性，为后续特征提取筛选以及相机位姿估计打好基础。
根据图2的描述，系统通过在原有ORB-SLAM3线程架构中增设目标检测线程，使得SLAM（同步定位与地图构建）系统具备了获取环境语义信息的能力。这种增强使系统能够利用这些语义信息排除移动物体对SLAM过程的干扰，从而优化了系统性能。同时，在跟踪线程环节，引入了经过改进的特征点提取方法，这一策略增强了特征信息在不同照明条件下的鲁棒性，进而构筑了一个更为稳健的视觉处理前端。这些技术的使用和改进将有助于传统的视觉SLAM系统适应复杂多变的现实环境。
1.2 轻量级目标检测线程YOLOv5s-FasterNet
YOLOv5的演变经历了不断的挑战与创新。它起源于YOLOv1，经过不断迭代，发展至现在的YOLOv8[11]。在这一过程中，YOLOv5是其中较为优秀的算法之一，继承了前作的速度优势，在保持快速检测的同时，显著提升了识别精度。
1.2.1 FasterNet网络架构
得益于新型PConv卷积的流行[12]。FasterNet的设计巧妙地结合了三个关键组件：基础网络模块、快速特征融合模块和高效上采样模块。图3（图片来源于FasterNet作者Jierun Chen[12]）展示了FasterNet的整体结构，左侧是PConv卷积模块，与右侧的 (3 \times 3) PConv处理相对应。在FasterNet的每个构建块中，一个PConv层面首先跟着两个PwConv（或常规卷积）层。
1.2.2 FasterNet-T0集成YOLOv5s
FasterNet的集成是为了在YOLOv5中取得更好的目标检测性能，同时保持较高的检测速度[13]。在将FasterNet集成到YOLOv5s中时，需要对网络结构进行修改和对参数进行调整，以确保两者能够充分发挥优势。FasterNet集成YOLOv5s主要有两种做法：其一将YOLOv5s全部FasterNet化即替换掉其主干（backbone）网络以及头部（head）网络。
本文算法只涉及修改YOLOv5的主干网络部分，即将默认的CSPDarknet53主干网络替换为FasterNet-T0主干网络，采用GELU激活函数自适应舍弃矫正的预期与随机调整有紧密联系。同时在原有FasterNet的基础上引入了CBAM注意力机制，显著提高了模型对目标特征的注意和提取能力，使检测率提高了10% 以上。改进后的网络结构如图4所示。
1.3 改进的视觉里程计前端
如图5所示，改进的视觉前端包括两个主要线程：跟踪（Tracking）线程和目标检测（Object Detection）线程。跟踪线程的关键组成部分包括AtORB特征点的提取与筛选、异常特征点的去除、地图初始状态的估计、局部地图跟踪以及关键帧的选取等关键步骤。
在视觉里程计的改进过程中，特征点提取阶段采用AtORB特征点提取策略，能够在不同光照条件下提取出更稳定、分布更均匀的特征点。与标准版相比，AtORB方法不仅保留了ORB的特征点描述方法，还增加了图像增强、金字塔和四叉树数据结构模块。
目标检测线程会提供原始RGB图像，然后进行图像增强，再建立图像金字塔来增加比例、计算特征点的数量，然后结合自适应阈值手段来指定阈值，最后用四叉树来管理特征点，在特征匹配上与传统的ORB保持一致，暴力匹配加RANSAC来剔除不匹配的特征点。这些高质量的特征点对于后续的姿态估计和数据关联、姿态估计跟踪局部映射关键帧的选择跟踪线程动态特征点剔除至关重要。
2 动态特征点剔除方法
2.1 基于语义信息的帧间估计的动态特征点剔除
本文算法主要研究室内动态场景，针对该场景设置多种动态目标作为先验检测实体，主要目的是核验YOLOv5s-FasterNet网络是否具备具体先验目标的剔除能力。
简单来说利用深度学习技术，目标检测算法YOLOv5s-FasterNet，进行目标检测和语义分割，通过先验信息标签识别图像中的动态物体，如行人、车辆等。在实际的动态环境下，动态特征点可能与SLAM特征匹配系统形成错误的共视图约束关系。如图6所示，橙色圆点代表环境中的静态特征点，而绿色圆点指代为环境中的动态特征点。原ORB-SLAM3系统可能会错误地将动态物体（如行人、自行车、汽车、小鸟等）身上的特征点识别为地图点，用于位姿估计，从而使得相机位姿估计带来巨大偏差，把相机位姿从图中的蓝色位置估计成绿色位置。
视觉前端的目标检测线程通过获取图像的类别语义信息，采用YOLOv5s-FasterNet网络模型进行目标检测。通过读取目标框的标签类别，得到动态目标框区域信息后，并将此信息发送至跟踪线程。
在整合目标检测网络YOLOv5s-FasterNet与ORB-SLAM3跟踪系统的研究中，图像帧一旦输入系统，首先会在跟踪线程中提取出所有的特征点，形成特征点集合。与此同时，使用YOLOv5s-FasterNet网络对图像中的对象进行检测，得到一系列的目标检测框，组成集合，如下式所示：
式中：——第k帧图像中的第m个特征点；——第k帧图像中的第n个目标检测框。
如果某个目标检测框内包含的是动态目标如行人，并且特征点位于该检测框内部，即包含于，那么这个特征点就可以被认定为先验动态点。基于此，可以区分出动态特征点集合, , 和静态特征点集合。
在处理动态环境中的视觉SLAM问题时，目标检测框内一般都会包含静态与动态特征点，因为YOLO网络的检测框体是矩形的，而一般情况下动态目标都是不规则形状的。因此，静态特征点如果被错误地一并剔除，将导致可用于SLAM系统匹配的特征点数量大幅减少，进而影响定位精度。为了解决这一问题，引入了多视图几何来更准确地判断和剔除动态特征点。
2.2 基于多视图几何的动态特征点剔除
基于语义信息的帧间估计方法可以成功剔除动态行人的特征点，从而提升了帧间位姿估计的精度。然而，该方法并没有对行人的真实运动状态进行判断。假设标记识别到的行为人静止的人，对于检测到的潜在动态物体（如人），可以实施一个临时静态假设，即在一定时间窗口内观察该物体是否真正移动。如果在这段时间内物体没有移动，那么它应该被视为静态特征点。同时进行多帧一致性检查来进一步判断物体的真实运动状态。如果物体在连续几帧中的位置没有显著变化，那么可以认为它是静止的。本节将探讨一种语义信息和几何约束相结合的动态特征点剔除方案，即针对该情况考虑。该方案首先通过语义信息滤除部分特征点，然后利用静态特征计算基础矩阵。接着，根据基础矩阵的几何约束判断真实动态特征并进行滤除，最后利用最终保留的静态特征计算相机位姿，确保SLAM系统在动态环境中的鲁棒性和准确性。这一过程通常涉及以下3个步骤：1）特征点提取与匹配；2）利用基础矩阵与极线约束；3）运动一致性检查。下面进行简要叙述。
首先是进行特征点的提取与匹配，得益于2.1节的方法，可以得到具体对应的静态特征，通过8点法可以解算出基础矩阵 。依据于多视图几何中的静态特征对极约束关系，可以知道一对匹配上的静态特征定有如下束缚关系：当图像帧中的某一对特定特征点相匹配时，对应像素点在成像平面的极线之上。
下面通过图7论证：如果测定环境中点P没有移动，那么正确的匹配像素特征点应该是和，而 一定在极线 上。假设测定环境中点移动到 时，它在第二图像帧的像素位置变为 ，且 与极线的距离偏离较远。因此，可以利用到极线的距离来判定该特征点是否需要滤除。对极距离可以用以下方法计算：
已知图像帧序列中对应像素特征点坐标和 ：
求解齐次变换：
求解 (l_{2}) ：
求解距离 (h) ：
最后设定阈值 (d) ，滤除掉不满足阈值距离 (h) 的特征点，从而筛选出静态特征点。
得益于语义信息和多视图几何手段，避免动态特征对相机的位姿估计造成干扰，只使用保留的静态背景特征点进行地图初始化和跟踪，保留了静态特征点的有效性[14-15]。下面是效果展示，如（图8位置）所示，行人目标框内的特征点被正确剔除，只保留了实际场景中的静态特征。
对于动态环境下的特征点，系统采用目标检测线程来获取类别语义信息，使用YOLOv5网络模型来检测目标物体。通过识别动态目标框区域，目标检测线程将动态区域的特征点发送至跟踪线程，结合多视图几何手段在跟踪线程中对动态特征点进行删除，避免其对位姿估计的干扰，从而使用剩余的静态背景特征点进行地图初始化和特征点跟踪，提高系统的定位准确性和稳定性。
3 实验结果与数据分析
为确保目标检测算法实验的可靠性和可复现性，需要精心规划和执行一系列步骤来构建和维护合理的实验环境和配置。下述实验硬件配置以及环境如下：
硬件配置：GPU：NVIDIA GeForce RTX 30；CPU：Intel Core i9-13900K；Memory：64GB。
软件版本：Ubuntu：20.04；Python：3.9.13；PyTorch：1.11.0；CUDA：11.5。
同时为保证实验结果的可靠性、可复现性和算法的泛化能力，下述实验均采用公开数据集。针对YOLO目标检测部分的训练采用PASCAL VOC 2012数据集，对SLAM算法的性能评估部分使用TUM数据集。
PASCAL VOC 2012是一个常用的计算机视觉领域的数据集。PASCAL VOC数据集包含了20个类别的图像数据，每个图像都标记了相应的物体类别和位置信息，用于训练和评估物体检测、分类和分割等视觉任务的算法。这些数据集通常用于评估目标检测、语义分割等计算机视觉任务的算法性能。
TUM数据集由慕尼黑工业大学提供，包含了受控环境下的室内和室外数据，适用于定位、SLAM算法的研究和评估。数据集中包含了RGB相机、深度相机、IMU数据等多种传感器数据，用于不同类型算法的评估。TUM数据集在实验场景和数据类型方面更加多样化，适合评估不同条件下的算法性能。
3.1 YOLOv5s集成FasterNet算法实验
进行对照实验证明改进后的算法的有效性，利用相同的实验设定和PASCAL VOC数据集来进行训练和评估YOLOv5s集成FasterNet算法与原算法的效果。实验中发现经过backbone的修改致使网络参数量由7235389下降到5760609，锐减了20.4% 。
在表1中描述了YOLOv5s与集成FasterNet的性能数据指标。对此进行简要说明：mAP是目标检测领域中一个核心的评估指标，它用来衡量模型的性能。推理时间是评估模型实用性的重要指标之一，是指模型对单张图像进行目标检测所需的平均时间。从表格中可以明显观测到，YOLOv5s集成FasterNet后，在mAP方面较原网络有明显提高，从0.74提升至0.80。同时，在数据推理时间方面也有改进，从减少到了，平均下来减少了16.85%的推理时间。推理时间显著减少的同时并没有以牺牲检测精度为代价，极大程度提高了算法的竞争力。
3.2 算法改进与性能评估对比实验
本文为验证提出的YOLO-SLAM算法是有效的，考虑到算法的泛化性，决定采用德国慕尼黑工业大学提供的TUM公开数据集，该数据集适合评估不同条件下的算法性能。基于此，选用TUM数据集类别中的Dynamic Objects下的几个序列图像做验证。其中包括fr2/desk_with_person、fr3/walking_static、fr3/walking_xyz以及fr3/walking_rpy、fr3/walking_half图像序列。fr2图像序列表示一般场景、fr3图像序列表示高动态环境。数据集地址：https://cvg.cit.tum.de/data/datasets/rgbd-dataset/download。
在所提供的场景序列中，主要记录了典型的工作人员的小公场景。图像序列录制过程中，实验人员进行移动并与一些物体进行交互。众多SLAM研究表明该序列适用于核验评估动态环境下SLAM算法的定位精度与鲁棒性。
同时对序列名称的后缀进行详细说明：static、xyz、rpy表明手持相机录制时相机所处或运行时的状态，static表示相机静止、xyz表示相机沿着三个方向（xyz）手动移动、rpy表示相机在同一位置沿主轴（滚动-俯仰-偏航）旋转。
3.3 EVO评估结果误差对比
在SLAM领域，EVO（EuRoC MAV Visual Odometry）评估通常是指对视觉里程计（Visual Odometry）算法的性能评估。本文选取APE（绝对轨迹误差）和RPE（相对轨迹误差）作为算法的性能指标进行评判。将其与ORB-SLAM3算法在同一数据集上进行了比较。将算法性能提升定义为P ，其计算方法如下式所示：
其中， A代表优化后（After）的算法结果，即本文提出的算法； B是优化前（Before）的算法结果，即ORB-SLAM3算法。表2、表3分别展示了优化前后算法在APE、RPE（平移部分）方面的对比结果。
从表2和表3中不难看出，YOLO-SLAM算法在高动态环境序列fr3_xyz、fr3_half、fr3_static，相比于传统ORB-SLAM3在APE的均方根误差上降低了至少 90.46% ，在一般场景序列fr2_person上也降低了 16.44% ，但在fr3_rpy上只降低了56.92% ，推测很有可能在该动态序列评测实验中，由于该场景中的行人在距离相机较近的地方移动，致使相机拍摄到的特征点大多聚焦于人的身上，而衣服又是弱纹理环境，使得SLAM系统无法捕获较多的特征点，致使SLAM系统精度提升效果不大。而反观fr2_person，该场景序列亦有 16.44%的精度提升，说明ORB-SLAM3系统在一般场景下已取得较好的定位精度。相较于传统算法，YOLO-SLAM在动态场景下系统定位准确性和实时响应速度方面都表现优秀。
图9可视化显示了ORB-SLAM3算法与YOLO-SLAM算法的轨迹误差对比，从图中可以看出原ORB-SLAM3算法改进后的提升效果十分明显。从轨迹误差图像来看，YOLO-SLAM的轨迹估计（estimate）更倾向于真实轨迹（ground truth）。
如表4所示，对提出的YOLO-SLAM系统与原框架ORB-SLAM3以及当前同研究领域的代表算法如DS、Dyna、DP、RDS 4个算法进行了RPE中的均方根误差对比实验，实验数据集基于TUM下的高动态序列fr3和一般场景序列fr2。从该数据中不难看出YOLO-SLAM算法在5项序列对比实验中，有三项定位效果最优，两项次优。
从表2、表3和表4，不同动态环境下的序列实验数据进行APE以及RPE结果对比可以得出，YOLO-SLAM系统在能够实现高精度定位的同时具备了鲁棒性，可应用范围更广更大，表现出YOLO-SLAM系统的巨大优势。
4 结束语
本研究介绍了一种新型视觉SLAM系统，专门针对动态环境设计。该系统基于ORB-SLAM3的视觉里程计，通过引入目标检测模块，同时结合多视图几何和移除动态特征点的方式来进行系统优化。结合了YOLOv5s-FasterNet这一轻量级目标检测网络，实现对动态物体的实时识别，并在提取ORB特征点时，利用语义信息清除潜在的动态特征点。实验结果显示，与ORB-SLAM3算法相比，YOLO-SLAM算法在动态环境下的绝对轨迹误差（APE）均方根误差降低了 (90%) 以上。相较于现有的其他先进算法，该研究提出的算法在定位准确性和响应速度方面都显示出了其优势，预期该技术将在未来的移动机器人定位领域中得到更广泛的应用。
尽管当前算法展现出色，但仍有提升空间，特别是在极端环境下，系统的稳定性和准确性仍需进一步提升。未来的工作将集中在优化融合算法，增强系统的抗干扰能力，并扩展到更多类型的复杂环境。此外，我们计划深入研究机器学习和人工智能在多模态信息融合中的应用，以期实现更加智能化和自适应的定位系统。
本文仅做学术分享，如有侵权，请联系删文。
3D视觉工坊端午专属课程福利重磅来袭！为回馈新老学员一路支持，本次活动特推出重磅折扣福利，所有课程统一享8折特惠，其中10余门课程支持一次性全部打包购买(限时福利仅需279元)！
端午8折特惠
端午专属8折优惠
微信扫码领取，限时三天内使用
3D视觉工坊所涉及课程的包括但不限于：工业3D视觉、自动驾驶、SLAM、具身智能、扩散模型、无人机、大模型和3D视觉基础等。
专属打包福利
上图中的：ROS2、相机标定、线结构光、3D缺陷检测、激光-视觉-IMU-GPS融合SLAM、VINS-Fusion、模型部署、3D目标检测、深度估计、多传感器融合这10门课程，除单独购买享7折外，也支持一次性全部购买，限时福利仅需279元，扫描下方二维码加入一次性全部解锁！
活动咨询
