本文题为 “IDMF-VINS: Improving Visual-Inertial SLAM for Complex Dynamic Environments with Motion Consistency and Feature Filtering”（IDMF-VINS：利用运动一致性和特征滤波改进面向复杂动态环境的视觉惯性SLAM），发表于 IEEE Sensors Journal 第25卷第4期，出版日期为2025年2月15日。该研究的通讯作者为Xuerong Yang和An-min Zou。主要作者包括Xuanzhi Peng、Pengfei Tong、Xuerong Yang、Chen Wang和An-min Zou，他们分别来自汕头大学工学院、中山大学航空航天学院以及中国科学院自动化研究所。这是一项针对复杂动态环境进行优化的原创性视觉惯性SLAM（Simultaneous Localization and Mapping，即时定位与地图构建）研究。
研究的学术背景 本研究隶属于机器人学、计算机视觉和自动驾驶等领域的核心技术——即时定位与地图构建（SLAM）。视觉SLAM（VSLAM）因其能利用相机提供丰富的纹理和结构信息而备受关注，但传统的VSLAM算法（如ORB-SLAM2， VINS-Mono）通常假设环境是静态的。在真实世界的复杂动态环境中（如街道、商场、校园），大量的移动物体会引入动态特征点，破坏静态假设下的多视图几何约束，从而导致相机位姿估计错误、定位精度下降甚至系统失效。虽然融合惯性测量单元（Inertial Measurement Unit， IMU）数据的视觉惯性SLAM（VI-SLAM）通过运动先验信息增强了鲁棒性，但在极端动态场景下，当动态物体占据图像较大比例时，仅靠IMU和传统的几何方法仍难以准确估计相机位置。现有的一些解决方案，如基于语义分割的方法，虽然能生成完整的物体轮廓，但计算成本高昂，难以满足实时性要求；而简单基于目标检测的方法，则倾向于直接剔除检测框内的所有特征点，这可能不必要地移除大量静态背景特征，导致系统约束信息不足。因此，本研究旨在开发一种能够在复杂动态环境中高效、准确地区分动态与静态特征点，并在保证实时性的前提下，显著提升SLAM系统定位精度和鲁棒性的新方法。具体目标包括：1）提出一种结合运动一致性检查的动态特征点检测新方法；2）设计一个融合聚类与滤波的特征点选择模块，以更有效地保留背景中的静态特征；3）构建一个能够处理复杂多变动态环境的鲁棒性VI-SLAM系统。
详细研究流程 本研究提出的系统名为IDMF-VINS，它建立在VINS-Mono框架之上，并进行了关键性改进。整个系统包含三个并行运行的线程：目标检测线程、特征跟踪线程和状态优化线程。核心创新主要集中在特征跟踪线程中的动态特征处理流程，主要包括“运动一致性检查”和“动态特征检测”两大模块。
第一，运动一致性检查（Moving Consistency Check）： 该模块的核心思想是利用视觉几何约束和IMU预积分信息来联合判断特征点的动态性，而不仅仅是将检测框内的点一概视为动态。其工作流程基于算法1进行，具体步骤如下： 1. 特征匹配与基础矩阵计算： 首先，在当前帧和上一帧之间进行特征点匹配。然后，使用RANSAC算法计算两帧图像间的基础矩阵，并找到内点。 2. 视觉几何约束检验： 基于计算得到的基础矩阵，对于每一对匹配点，计算当前帧特征点到其对应的极线（由上一帧特征点通过基础矩阵映射得出）的距离，记为d_vision（公式3）。这个距离衡量了匹配点是否符合两视图间的对极几何关系。在静态场景中，匹配点应靠近极线。 3. IMU预积分与重投影误差检验： 利用两帧之间高频的IMU测量数据，通过预积分（公式4-7）计算出相机在两个时刻之间的相对旋转和平移变换矩阵（公式8-10）。接着，使用此变换矩阵将上一帧中的特征点投影到当前帧的预测位置。然后，计算该预测位置与实际观测到的匹配点位置之间的差异，即重投影误差r_k（公式11）。这个误差衡量了基于IMU运动预测的特征点位置与实际视觉观测的一致性。 4. 双重约束动态点判定： 结合上述两个约束，为每个匹配点设定一个联合误差阈值（ε）。如果一个匹配点的d_vision和重投影误差r_k都超过了该阈值，则该点被判定为动态点（即“外点”）。这个过程不仅用于识别动态特征点，还可以通过统计目标检测框内被判定为外点的比例，来推断该预定目标（如行人、车辆）本身的运动状态（动态或静态）。这使得系统能够更智能地利用信息：当预定目标静止时，其上的特征点可以被用作静态约束；只有当目标运动时，才剔除其上的动态特征点。
第二，动态特征检测（Dynamic Feature Detection）： 这是处理目标检测框内特征点的精细化模块。对于被目标检测器（本研究采用YOLOv5）识别出的预定动态物体（如人）的边界框，不采取简单“一刀切”的删除策略，而是通过以下步骤进一步筛选，以尽可能多地保留框内的静态背景特征点： 1. 深度聚类： 首先，使用边界框中心点的深度值初始化两个聚类中心，然后利用K-means聚类算法，根据边界框内所有特征点的深度值，将它们大致分为两类：属于动态物体的点和属于背景的点（公式12）。聚类操作能够基于深度差异进行初步的分离。 2. 基于深度差异的异常值滤波： 然而，当动态物体与背景物体在深度上非常接近（例如人紧贴墙壁）时，聚类可能无法清晰区分。为此，研究提出了一种放大深度差异的滤波策略。对于每个特征点，计算其深度值d_pi与其所属聚类中心深度d_µj的绝对差值Δd_pi（公式13）。为了放大异常值（即与聚类中心深度差异过大的点，这些点很可能是误判的动态点或离动态物体过近的静态点），将差值乘以一个放大因子α（本文中α=1000），得到Δd_amplified_pi（公式14）。 3. 阈值过滤： 计算所有放大后深度差值的最大值d_max和最小值d_min，设定阈值为(d_max + d_min) / 2（公式15）。若某点的Δd_amplified_pi大于此阈值，则将其视为异常点（可能属于动态物体或离动态物体过近的静态干扰点）并过滤掉；否则，将其保留为正常点（可靠的静态背景点）（公式16）。这个过程可以形象地理解为，通过放大深度差异，将那些深度值与聚类中心有显著差异的“离群”特征点（通常是动态物体上的点或紧邻动态物体的静态点）剔除，从而更纯粹地保留背景静态特征。
实验验证与数据分析 研究在两个公开数据集上对IDMF-VINS进行了全面的实验评估：TUM RGB-D数据集和更具挑战性的OpenLORIS-Scene数据集。评估指标主要采用绝对轨迹误差（Absolute Trajectory Error， ATE）和相对位姿误差（Relative Pose Error， RPE）的均方根误差（Root Mean Square Error， RMSE），以及衡量系统鲁棒性的正确率（Correct Rate， CR）。实验平台硬件配置为AMD Ryzen 7 5800H CPU和RTX 3070 GPU。
在OpenLORIS-Scene数据集上的评估： 该数据集包含办公室、走廊、家庭、咖啡馆、市场等多种真实室内场景，具有光照变化、大量移动行人、运动模糊等多种挑战。研究将IDMF-VINS与多种经典及前沿的SLAM算法进行了对比，包括静态SLAM算法（VINS-Mono， ORB-SLAM2， DSO）、语义动态SLAM算法（DS-SLAM）以及另一种视觉惯性融合的动态SLAM算法（Dynamic-VINS）。结果表明： - 在低纹理环境（如走廊、家庭）中，DS-SLAM因依赖稀疏的视觉特征而性能受限。而融合IMU的VINS-Mono和IDMF-VINS则表现出更高的稳定性。IDMF-VINS在走廊和家庭场景中分别取得了98.9%和97.3%的正确率，与Dynamic-VINS相当，但在ATE和RPE精度上更优。 - 在高动态环境（如咖啡馆、市场）中，IDMF-VINS的优势更为明显。与Dynamic-VINS相比，IDMF-VINS在咖啡馆和市场场景中的正确率分别提升了3.3%和4.3%。在市场这类行人活动高度密集的场景中，IDMF-VINS不仅保持了最长的连续跟踪时间，而且在ATE和RPE精度上均超越了所有对比算法。图4、5、6展示了在市场场景下，IDMF-VINS的估计轨迹与真实轨迹的吻合度最高，漂移最小。 - 关键数据支撑： 如表I和表II所示，IDMF-VINS在多个场景序列的ATE和RPE RMSE上均取得了最低或次低的误差值，证明了其定位精度的优越性。
在TUM RGB-D数据集上的评估： 该数据集通过动作捕捉设备提供了精确的真实轨迹。研究选择了包含快速运动行人的“walking”序列（如fr3_walking_xyz， fr3_walking_rpy， fr3_walking_halfsphere）进行测试。由于该数据集不含IMU数据，研究主要与ORB-SLAM2和基于RGB-D的Dynamic-VINS进行对比。结果显示： - ORB-SLAM2在低动态场景（fr3_walking_static）表现良好，但在高动态场景下性能急剧下降。IDMF-VINS则能有效剔除动态特征点。 - 与ORB-SLAM2相比，在fr3_walking_xyz场景中，IDMF-VINS使旋转漂移和位移漂移的RMSE分别降低了87.5%和90.5%。 - 与Dynamic-VINS相比，在所有动态序列中，IDMF-VINS的ATE精度均有小幅提升。特别是在相机存在大量翻转和旋转、目标检测容易失效的fr3_walking_halfsphere序列中，IDMF-VINS将ATE RMSE从0.076米降低到0.0409米，定位精度提升了约42.1%。图7直观展示了IDMF-VINS在四个动态序列上的估计轨迹（蓝色实线）相较于真实轨迹（黑色实线）的误差（红线）远小于Dynamic-VINS。
消融研究（Ablation Study）： 为了验证各个模块的有效性，研究在OpenLORIS-Scene数据集的Market场景中进行了消融实验。 1. 移除聚类滤波模块（w/o CF）： 当直接删除目标检测框内的所有特征点时，系统ATE从1.6180米降至0.8844米，RPE从0.4830米降至0.1799米，证明了剔除动态点的必要性。但与完整的IDMF-VINS（ATE: 0.7261米）相比，ATE有所上升，说明不加区分地删除框内所有点，会导致丢失有用的静态背景约束，从而损害系统稳定性。 2. 移除概率学习模块（w/o PL， 即运动一致性检查中用于预测物体状态的部分）： 在Market1-3序列中，移除该模块后，ATE从0.9534米升至1.6477米。这证明了通过运动一致性检查来判断预定目标的运动状态，对于在目标静止时保留其特征点、建立更多有效视觉约束至关重要。 3. 移除重投影约束模块（w/o RC）： 在所有Market序列中，移除重投影误差约束后，ATE均有上升。这表明，在目标检测失败或不准的情况下，结合IMU预积分信息的重投影约束能够可靠地剔除异常特征点，维持系统稳定。
运行时间分析： 在咖啡馆序列上，IDMF-VINS各核心模块的平均耗时均表现良好。其“动态特征滤波”模块耗时仅为0.6794毫秒，远低于Dynamic-VINS的1.3424毫秒，表明新增的动态处理模块对系统整体计算负载影响甚微。特征跟踪和位姿优化模块分别耗时5.8571毫秒和44.9308毫秒，证明了系统具备实时运行的能力。
结论与价值 本研究成功地提出并验证了一个针对复杂动态环境优化的鲁棒性VI-SLAM系统——IDMF-VINS。该系统通过两项核心创新解决了现有方法在动态环境中面临的问题：1）提出了一种结合极线约束和IMU重投影误差的“运动一致性检查”双重约束机制，能更准确地检测动态特征点并推断预定目标的运动状态；2）设计了一个“深度聚类+异常值滤波”的动态特征检测模块，能够在目标检测框内精细化地区分动态物体和静态背景，最大化地保留有用的静态特征点。实验结果表明，IDMF-VINS在公开的TUM RGB-D和OpenLORIS-Scene数据集上，尤其是在高动态、高挑战性的场景中，其定位精度和系统鲁棒性均显著优于多个先进的对比算法。该系统在保持低计算开销（满足实时性）的同时，显著提升了动态环境下的SLAM性能，具有重要的科学价值和应用价值。
研究亮点 1. 新颖的双重约束动态点检测方法： 将基于视觉几何的极线约束与基于IMU预积分的重投影约束相结合，提供了一种更鲁棒、更准确的动态特征点判定方法，并能智能判断预定目标的运动状态。 2. 精细化的特征保留策略： 提出的“聚类+滤波”模块，颠覆了简单剔除检测框内所有特征点的粗暴做法，通过深度分析和异常值过滤，巧妙地保留了背景中的静态特征，为后端优化提供了更多有效约束，这是提升系统精度的关键。 3. 优异的工程实用性与平衡性： 该系统在显著提升动态环境下SLAM性能的同时，并未引入巨大的计算负担（如语义分割），各模块运行高效，整体系统可在CPU上实时运行，在精度、鲁棒性和计算效率之间取得了良好平衡，适用于自动驾驶、移动机器人等对实时性要求高的实际应用平台。 4. 全面的实验验证： 不仅在经典TUM数据集上测试，更在更具挑战性的OpenLORIS-Scene真实复杂场景数据集中进行了详尽的对比和消融实验，充分证明了算法的有效性和各模块的必要性。