同步定位与建图(simultaneous localization and mapping，SLAM）是搭载各种传感器的移动平台在未知环境下实现自主定位与构建周围环境地图的技术^[1]. 视觉SLAM借助相机传感器，依靠视觉感知环境，实时进行自身定位与周围环境地图构建，在小场景中的定位误差多为厘米级，优于传统里程计的分米级，是精度高、优势显著的技术^[2]. 视觉SLAM的前端可分为特征点法^[3]和直接法^[4]. 特征点法依靠特征点的提取与匹配追踪多帧图像间同名特征点的运动轨迹，依据三角测量原理算出特征点的三维空间位置，进而确定相机位姿与地图点位置. 直接法使用图像像素灰度信息，依据光度一致性假设，通过两帧图像之间的像素灰度值构建最小化光度误差求解相机位姿并进行地图构建.

传统基于特征点法和直接法的视觉SLAM算法均设置于静态场景下，如PTAM^[5]、LSD-SLAM^[6]、ORB-SLAM^[7]、SVO^[8]. 这些算法在静态或者少量动态场景下的位姿估计与地图构建精度较高，但在动态场景下的实时性、定位精度、检测精度、鲁棒性降低. 基于深度学习和几何的方法^[9]能够提高系统在动态场景下的定位精度、检测精度和鲁棒性，减少运动物体对系统的影响以及减小位姿误差. Bescos等^[10]使用Mask R-CNN像素级实例分割技术与多视图几何方法剔除动态物体. Hu等^[11]提出CFP-SLAM 算法，利用目标检测网络YOLOv5 和几何约束区分高动态物体和低动态物体，并使用关键点由粗到精的两阶段静态概率计算方法提高匹配像素点对在定位时的利用效率. Chang等^[12]使用YOLACT（you only look at coefficients）分割动态对象，在去除内部异常值后，引入几何约束来过滤动态点. Zhang等^[13]提出 VDO-SLAM算法，通过MaskR-CNN 实例分割以及 PWC-Net 光流网络提取图像中的移动目标，对相机位姿、动态和静态特征点、运动物体的位姿进行联合计算与优化. 张玮奇等^[14]提出SUI-SLAM算法，基于Mask R-CNN的语义分割结果获取动态先验信息，并结合深度信息，优化特征点匹配关联与位姿求解精度. 翟伟光等^[15]提出YSG-SLAM算法，通过增双并行线程，分别用于语义分割与语义建图，同时引入动态特征剔除、自适应阈值以此来提高定位精度. 刘钰嵩等^[16]提出RGBD-SLAM算法，通过透视矫正结合 RAFT-S 与 BiSeNetv2分离动静特征构建静态语义地图. 上述SLAM算法均遵循动态干扰抑制—静态特征保留—位姿与地图协同优化的核心逻辑，存在以下不足： 1）动态区域特征点的过度剔除，导致可用特征点数量不足；2）静态特征保留策略的复杂性问题，在一定程度增加了算法的复杂度；3）稠密地图构建的碎片化问题，无法获取丰富的环境感知与理解.

本研究提出融合YOLOv11n目标检测的优化ORB-SLAM3^[17]算法. 1）采用基于开放神经网络交换格式（open neural network exchange, ONNX）推理的YOLOv11n目标检测算法获取先验动态区域；2）通过双阶段位姿优化方法筛选出动态区域中的静态特征点，精准剔除动态场景下的动态特征点，同时进行位姿优化；3）利用关键帧中所有位于静态区域的像素点及经筛选得到的动态区域静态特征点执行投影操作生成3D点云，通过点云拼接构建稠密地图.

本研究所提算法在ORB-SLAM3的框架上改进，系统在跟踪线程的前部引入基于ONNX推理的YOLOv11n目标检测算法，用于提取图像中的动态区域. 如图1所示，静态区域特征点生成初始位姿，再将地图点投影至动态区域；基于双阶段位姿优化算法筛选出动态区域的静态特征点以及剔除动态特征点，提升位姿估计精度与增加优质特征点数量；在ORB-SLAM3算法原有的跟踪线程、局部建图、回环检测3个线程外，加入稠密地图构建线程.

采用基于YOLO系列中的YOLOv11n作为动态目标检测算法. YOLOv11n是先进的实时目标检测模型，具有比其他目标检测算法更紧凑的架构和更高效的特征提取能力，它在精度和速度方面的性能表现均衡. 为了进一步满足动态场景下的目标检测和系统实时运行的需求，本研究将YOLOv11n基于PyTorch的推理改为基于ONNX的推理. 相较于基本PyTorch推理，ONNX推理可以显著减少模型体积、内存占用且更易于部署. YOLOv11n在COCO数据集上训练后，可以同时识别80种物体，基本满足室内环境中物体类别需求. 本研究将YOLOv11n识别到的人、凳子、背包、瓶子、椅子、笔记本电脑、鼠标、键盘和书9类物体标记为动态物体，为了提高YOLOv11n检测的召回率，将非极大值抑制（non-maximum suppression, NMS）的阈值设置为0.45.

在提取输入图像的特征点后，采用3种基于双阶段位姿优化的方法进行位姿估计，分别为恒速模型法、参考关键帧法与重定位法. 在开始跟踪后，按以下逻辑选择估计方法. 1）若上一帧跟踪成功且运动模型已完成初始化，则优先使用恒速跟踪模型法；2）若上一帧跟踪失败或模型未初始化，则启用参考关键帧法；3）当恒速模型法估计失败时，自动切换至参考关键帧法；4）若参考关键帧法也失败，则启用重定位法进行恢复. 3种位姿优化算法的触发条件如图2所示，双阶段位姿优化算法中的数值如未说明，则参考ORB-SLAM3算法原有的数值.

在恒速跟踪模型中，ORB-SLAM3算法的位姿估计分3步进行. 1）更新上一帧位姿，目的是对齐参考关键帧或者生成虚拟地图点. 2）假设相机保持恒速运行，当前帧在世界坐标系下的位姿为

(1)$ {\boldsymbol{T}}_{{\mathrm{cw}}}={\boldsymbol{V}} \times {\boldsymbol{T}}_{{\mathrm{lw}}}. $

式中：V为前两帧到前一帧的位姿变换矩阵，T_lw为前一帧图像在世界坐标系下的位姿. 根据上一帧位姿，由式（1）估计出当前帧的初始位姿. 3）将上一帧的地图点根据初始位姿重投影至当前帧，再根据金字塔约束以及计算描述子之间的汉明距离，获取匹配点. 若成功匹配点数目大于10，则进行位姿优化，跟踪成功. 如图3所示为恒速跟踪模型的位姿更新过程，其中F_r为参考关键帧，F_k−2为前两帧，F_k−1为前一帧，F_k为当前帧，T_r,k−1为由参考关键帧到前一帧的相对位姿.

针对动态区域中的动态特征点剔除与静态特征点保留问题，在原有恒速模型中引入双阶段位姿优化算法，其执行流程如图4所示，具体步骤如下. 1）在动态区域与静态区域内进行特征点提取时，均采用ORB特征点提取技术；提取后，将动态区域提取的特征点与静态区域的特征点分开存储. 2）在利用恒速模型匹配静态区域后，进行初步位姿优化；利用现有位姿将上一帧图片中未匹配的地图点投影至动态区域中，同时缩小投影点的搜索距离，根据实验选定，将搜索距离缩小到2个单位. 当投影点与动态区域特征点的描述子距离之间的最小距离小于0.7的次小距离时，认为动态区域特征点属于静态特征点，压入静态特征点容器. 3）将步骤2）筛选出的动态区域的静态特征点存入静态特征点，根据当前的静态特征点进一步优化位姿.

由于无法通过恒速跟踪模型从上一帧的地图点中获得足够的匹配点，采用跟踪参考关键帧模型进行匹配. 跟踪参考关键帧模型位姿估计步骤如下：1）为了加快当前帧与参考关键帧之间的特征点匹配，将当前普通帧的描述子转化为词袋向量，如图5所示；2）利用特征匹配函数将当前帧与参考关键帧进行特征点匹配；3）将上一帧的位姿作为当前帧位姿的初始值，通过优化3D-2D的重投影误差获得准确位；4）剔除优化后的匹配错误点.

针对动态区域中的动态特征点剔除与静态特征点保留问题，在跟踪参考关键帧模型中引入双阶段位姿优化算法，其执行流程如图6所示，具体步骤如下所示. 1）在动态区域与静态区域内进行特征点提取时，均采用ORB特征点提取技术；提取后，将动态区域提取的特征点与静态区域的特征点分开存储. 2）在计算当前帧静态点词袋模型的同时计算动态区域特征点的词袋模型. 3）利用特征匹配函数将当前帧的静态特征点与动态区域特征点同时与参考关键帧进行特征点匹配，保存动态区域特征点匹配的地图点. 4）将上一帧的位姿作为当前帧位姿的初始值，通过优化3D-2D的重投影误差获得位姿. 根据获得的位姿，通过3D-2D投影将动态区域特征点匹配的地图点投影至动态区域；计算投影点与特征点的位置距离，根据实验选定，当小于阈值8时，将其视为静态特征点. 计算完成后，重新进行位姿优化.

重定位是拯救式的跟踪算法，一旦应用此算法就代表恒速跟踪模型与参考关键帧模型在估计位姿时均失效. 该算法的实现步骤比前2种算法复杂，具体如下. 1）计算当前帧CF的词袋向量，并找到与当前帧相似的重定位候选关键帧KF1、KF2，如图7所示. 2）遍历所有的候选关键帧，使用词袋进行快速匹配，在匹配点数大于15时，使用EPnP迭代方法获得初始位姿. 3）对于上一步结果中的内点进行光束法平差（bundle adjustment, BA）优化，统计BA优化后内点的数量，如果数量小于50，则将关键帧中未匹配的地图点投影到当前帧. 4）如果匹配点数量足够，则再进行一次BA优化；优化后，如果成功匹配数目达到30~50，则使用更小的窗口、更严格的描述子阈值重新进行投影匹配搜索来增加匹配点数目. 5）经过上述步骤，总匹配点数达到设定阈值50，则认为重定位成功；进行最后一次BA优化.

为了改善重定位模型在动态环境中的位姿估计效果不佳的问题，在跟踪参考关键帧模型中引入双阶段位姿优化算法，执行流程如图8所示，具体步骤如下. 1）在动态区域与静态区域内进行特征点提取时，均采用ORB特征点提取技术；提取后，将动态区域提取的特征点与静态区域的特征点分开存储. 2）在计算当前帧静态点词袋模型的同时计算动态区域特征点的词袋模型，找到与当前帧静态特征点计算的单词相似的重定位候选关键帧. 3）遍历所有的候选关键帧，使用静态特征点词袋进行匹配，当匹配点大于15时，使用EPnP迭代方法获得初始位姿. 4）对前序结果中的内点执行 BA 优化，随后统计优化后内点的数量；若该数量小于 50，则将关键帧中未匹配的地图点投影至当前帧并完成匹配. 5）如果匹配点数量足够，则再进行一次BA优化；优化后，如果成功匹配数目达到30~50，则使用更小的窗口、更严格的描述子阈值重新进行投影匹配搜索来增加匹配点数目. 6）经过上述步骤，总匹配点数达到设定阈值50，则认为重定位成功；再进行一次BA优化. 7）根据最后一次优化获得的位姿，通过3D-2D投影将动态区域特征点匹配的地图点投影至动态区域，计算投影点与动态区域特征点的位置距离，根据实验选定，当小于设定值8时，将其视为静态特征点；计算完成后，进行最后一次BA优化.

ORB-SLAM3算法仅构建稀疏地图，不利于后续研究. 在完成机器人定位之后，处理机器人路径规划问题就要利用稠密地图. 本研究所提算法在ORB-SLAM3三大线程的基础上添加稠密地图重建线程. 1）在开始跟踪线程之前，先进行动态区域判定，标记动态区域. 2）通过跟踪线程进行位姿预测，将确定为关键帧的图像传入稠密地图重建线程，同时传入关键的位姿，像素点的深度. 3）在利用关键帧图像的像素点进行构建点云时，须判断该像素点是否属于动态区域；若是，则进行第二次判断，判断其是否属于动态区域的静态特征点；若是，保留该像素点，并获取三维地图点的坐标：

(2)$ \left.\begin{array}{l} x_i=(u-c_x)\times z/f_x,\\ y_i=(v-c_y)\times z/f_y,\\ z_i=z.\end{array}\right\} $

式中：x_i、y_i、z_i为三维空间点云的坐标，f_x、f_y、c_x、c_y为相机的内参，u、v为图像像素点的像素坐标，z为该像素点的深度值. 若不是，则剔除该像素点；处理完该关键帧后，得到局部点云. 4）随着不断添加的新关键帧，系统会持续生成局部点云图；系统会将后续生成的局部点云图与之前所生成的局部点云拼接，并进行全局优化，以保证地图的一致性和准确性. 5）系统生成稠密地图. 执行流程如图9所示.

实验采用TUM公开数据集进行测试. TUM数据集是德国慕尼黑工业大学机器视觉组发布的用于评估视觉SLAM、视觉里程计算法的权威数据集. 选用TUM RGB中的fr3高动态数据集序列，该序列包括4个子集：xyz、halfsphere、rpy和static. 算法运行环境：Ubuntu20.04操作系统，ROS和CUDA-11.3，CPU为Intel i9-12900，显卡为NVIDIA 3080.

单纯使用目标检测来剔除动态区域特征点，虽然可以保证获取的特征点均为静态特征点，但是会丢失一部分静态特征点. 在进行位姿优化的过程中，采用的特征点的数目越多，获取的估计位姿就越准确. 加入位姿优化的特征点须为静态的特征点，否则将导致估计的位姿出现较大的误差. 本研究所提算法的具体表现如图10所示.

使用苏黎世联邦理工学院SLAM系统评估评测工具EVO（Python package for the evaluation of odometry and SLAM）评估所提算法在定位精度方面的性能. EVO能够对所提算法的位姿估计值与数据集中提供的真实相机位姿进行比较，2个关键评估指标为绝对轨迹误差（absolute trajectory error，ATE）与相对位姿误差（relative pose error，RPE）. 绝对轨迹误差用于评价算法的整体性能，它是相机位姿的真实值与估计值之间的差，直观反应算法精度和轨迹全局一致性. 相对位姿误差用于评价算法的漂移程度，它是相同2个时间戳内的位姿变换量的差. 二者的均方根误差公式分别为

(3)$ {\mathrm{RMSE}}_{\mathrm{A}}=\sqrt{\frac{1}{n}\sum \limits_{{i}=1}^{n}||\boldsymbol{P}_{{i}}-\boldsymbol{Q}_{{i}}|{|}^{2}}, $

(4)$ {\mathrm{RMSE}}_{\mathrm{R}}=\sqrt{\frac{1}{|I|_{({i,j})\in I}}||{\left({{\Delta }}\boldsymbol{P}_{{{i,j}}}\right)}^{-1}{{\Delta }}\boldsymbol{Q}_{{i},{j}}|{|}^{2}}. $

式中：P_i、Q_i分别为第i个时刻的真实位姿和估计位姿，n为关键帧数量，I为包含所有相邻帧对的索引集，$ \Delta {\boldsymbol{P}}_{i,j} $，$ \Delta {\boldsymbol{Q}}_{i,j} $分别为第i，j时刻间的相对位姿变换.

如表1所示为在ORB-SLAM算法中仅加入目标检测算法和在目标检测处理后加入双阶段位姿优化算法所估计位姿与真实位姿的绝对轨迹误差结果，其中σ为标准差. 实验结果表明，所提算法可以显著降低绝对轨迹误差. 在xyz序列中，仅加入目标检测技术的均方根误差和标准差分别由ORB-SLAM3算法中的0.897 6 m和0.402 2 m降低至0.019 4 m和0.009 9 m，分别降低了97.8%和97.5%；与仅加入目标检测技术相比，加入双阶段位姿优化算法的均方根误差和标准差分别降低为0.015 2 m和0.007 0 m， 分别降低了21.6%和29.3%.

如表2所示为在ORB-SLAM3算法中仅加入目标检测算法和在目标检测处理后加入双阶段位姿优化算法所估计位姿与真实位姿的相对位姿误差结果. 实验结果表明，所提算法可以显著降低相对位姿误差. 在xyz序列中，仅加入目标检测技术的均方根误差和标准差分别由ORB-SLAM3算法中的0.681 4 m和0.386 1 m降低至0.034 8 m和0.015 3 m，分别降低了94.8%和96.0%；与仅加入目标检测技术相比，所提算法的均方根误差和标准差分别降低为0.020 0 m和0.006 8 m，分别降低了42.5%和55.5%.

如图11所示为2种算法在4个序列中绝对轨迹误差的可视化对比. 可以看出，ORB-SLAM3算法在动态环境中极易发生漂移现象，这主要由于环境中动态物体自身的运动和其对于静态物体操作所造成的（如动态物体对于静态物体的遮挡或移动）影响，这种现象极不利于位姿的准确估计. 所提算法在动态环境下，估计位姿与真实位姿的差值比ORB-SLAM3小，在xzy和halfphere序列中，2种算法的绝对轨迹误差结果显著不同.

进一步验证所提算法的有效性，对比不同SLAM算法在4个数据集上的实验效果，结果如表3所示. 参与对比的算法均为当前改进ORB-SLAM效果较好的算法，其中DS-SLAM 采用实例分割与几何方法；SG-SLAM 结合语义与几何信息；OVD-SLAM 融合深度、语义及几何信息；CFP-SLAM通过 YOLOv5目标检测网络与几何约束区分高低动态物体，采用关键点由粗到精的两阶段静态概率计算方法. 可以看出，在绝对轨迹误差对比中，所提算法在 rpy 和 static序列的均方根误差和标准差最小、误差最优，在其他序列中与其他算法误差相近.

如图12所示，为了验证所提稠密地图构建方法的有效性，选取 TUM 动态数据集的 static 序列作为测试数据，对比 ORB-SLAM3 原生方法构建的稀疏地图与所提算法构建的稠密地图的性能差异. ORB-SLAM3 算法仅能构建稀疏地图，该稀疏地图仅包含离散分布的无关联点云，无法完整表征场景结构与物体轮廓. 所提算法采用位姿关联与深度拼接方法构建稠密地图：通过算法估计的精准位姿关联不同帧点云，再结合深度信息完成融合拼接，最终生成稠密地图. 对比ORB-SLAM3 算法生成的稀疏地图可以看出，本研究构建的稠密地图能呈现场景物体形状、空间布局等细节，提升了地图真实感.

为了测试所提算法在实际场景中的有效性，采用Orbbec Astra Pro深度相机进行相关实验. 实验环境设定于室内实验室场景，场景内包含人、座椅、办公桌、实验设备等常见室内物体，以模拟真实应用场景的复杂性. 场景测试分为数据采集、场景重建与结果分析3个核心流程，相机以640×480 分辨率与 30 FPS 帧率进行数据采集，采集过程如图13 所示. 在数据采集过程中，相机采集数据，在机器人操作系统（robot operating system，ROS）中发布相应话题，由算法订阅这些话题以获取传感器数据. 在场景重建过程中，检测图像中的动态区域，运用双阶段位姿优化算法进行位姿估计. 在相同场景下对比所提算法与ORB-SLAM3的实验效果，结果如图14所示. 经分析验证可知，相较于ORB-SLAM3算法，所提算法对动态环境具有更强的鲁棒性，能够有效保留动态物体内部的静态特征点. 如图15所示，所提算法利用已获取的关键帧构建稠密地图，该稠密地图细节完整，能够清晰地还原场景的三维结构.

在SLAM系统中，除了保证位姿估计的准确外，还要保证系统的实时性能. 为了分析所提算法的实时性能，统计系统中位姿估计各模块的运行时间（仅用CPU，CPU为Intel i9-12900），具体处理时间结果如表4所示. 其中t_Y11n为当前帧在YOLOv11n检测花费的时间， t_f为当前帧特征点提取操作花费的时间，t_t为跟踪阶段花费的时间，t_a为处理每帧花费的时间. 可以看出，所提算法在4组数据集实验下有较好的实时性能.

本研究提出融合YOLOv11n目标检测的ORB-SLAM3优化算法. 对于当前ORB-SLAM3算法在动态环境中位姿估计不准确的现象，采用目标检测技术提取动态区域，利用静态特征点进行位姿的初始估计，通过计算出的初始位姿，使用双阶段位姿优化算法筛选出动态区域的静态特征点，再对位姿进行进一步优化. 对于ORB-SLAM3算法存在的只能构建稀疏地图的问题，所提算法加入稠密地图构建线程，对于传入该线程的关键帧，获取对应的点云信息，并结合所估计的位姿进行点云拼凑，生成稠密地图. 在真实场景中进行相关实验验证. 实验结果表明，所提算法在动态环境下取得良好的位姿估计效果，且与当前改进较好的算法相比，在精度上表现相当. 所提算法在动态环境中的位姿优化过程，较为依赖于静态区域中所提取的静态特征点质量. 当面对当动态目标占据视场角较大时，默认为静态语义的物体，实际可能处于运动状态时，易出现检测效果欠佳的情况. 后续的研究将不断优化与改进算法，以提高算法的鲁棒性.