点击下方卡片，关注「3D视觉工坊」公众号
选择星标，干货第一时间送达
来源：3D视觉工坊
星球内有20多门3D视觉系统课程、3DGS独家系列视频教程、顶会论文最新解读、海量3D视觉行业源码、项目承接、求职招聘等。想要入门3D视觉、做项目、搞科研，欢迎加入！
为应对传统视觉惯性SLAM系统在动态环境中因静态世界假设失效而导致的定位精度下降和鲁棒性差，提出了一种前后端协同的动态鲁棒视觉惯性SLAM方法。在前端，引入了基于运动一致性的概率性识别框架，通过对比观测视差角与惯性测量单元（Inertial Measurement Unit，IMU）预积分值，结合一阶时序滤波，为每个特征点生成平滑、连续的动态置信度，对高置信度动态特征予以初步剔除或软抑制；在后端，设计了一种融合该动态置信度先验的自适应鲁棒优化器，将前端概率先验信息引入自适应截断最小二乘（Adaptive Truncated Least Squares，ATLS）代价函数框架下的加权光束法平差（Bundle Adjustment，BA），从而实现对动态外点的分层抑制与滤除。在VIODE（Visual-Inertial Odometry in Dynamic Environments）数据集上进行的实验表明：与DynaVINS算法相比，所提方法在高动态序列city_day_high和parking_lot_high上的绝对轨迹误差分别降低了 和 ，而且随着动态程度的增强，性能下降更为平缓，展现出更强的鲁棒性。实验结果表明，该方法能够有效提升在复杂动态环境下视觉惯性SLAM系统的精度与可靠性。
0 引言
同步定位与地图构建（Simultaneous Localization and Mapping，SLAM）是移动机器人在未知环境中实现自主导航的关键技术。其中视觉惯性SLAM（VI-SLAM）采用的传感器成本低、体积小，通过紧耦合相机提供的视觉观测与惯性测量单元（IMU）提供的运动信息来提供丰富环境信息，已成为学术界与工业界的研究热点[1-3]。然而，绝大多数经典的VI-SLAM算法，如VINS-Mono、VINS-Fusion等[4]，都建立在一个核心的静态世界假设之上。动态物体出现将持续污染地图数据并产生轨迹漂移，导致定位失败。因此，如何有效处理动态物体的干扰，已成为提升VI-SLAM系统鲁棒性的关键。
目前学术界对于动态鲁棒性的解决方案大致可分为前端剔除与后端抑制两类方法。前端剔除方法旨在数据关联前就识别并移除动态特征，这些方法主要依赖于几何信息或语义信息。基于几何信息的方法，如Bescos等提出DynaSLAM通过多视图几何检测运动不一致的区域，Gomez-Ojeda等提出利用PL-SLAM提取点线特征信息提高鲁棒性。基于语义信息的方法，房立金等通过深度学习分割语义信息，去除不可靠的动态特征，Zhang等提出FlowFusion光流残差来突出RGB-D点云中的动态特征，Basch等提出一种基于CNN（Convolution Neural Networks）预测的语义先验信息结合概率模型以及时间运动信息判断动态特征。但语义分割的方法算力要求较高、依赖预定义物体，限制了其应用场景。
后端抑制方法则主要在光束法平差（Bundle Adjustment，BA）中处理动态特征，例如DynaVINS通过为特征引入可优化的动态权重来降低其影响，DynaVINS++进一步将该思想发展为更鲁棒的自适应截断最小二乘（ATLS）框架，能够更彻底地剔除外点影响，但在动态物体占主导地位时，复杂的动态处理会使非线性优化器陷入局部最优的风险。
本文提出一个复杂动态环境中鲁棒的视觉惯性SLAM系统，在前端通过基于运动一致性的前端动态判断机制，通过比较观测与惯性测量单元（IMU）预测的运动信息，实现对动态特征的概率性识别与过滤，计算特征点动态置信度；对于后端优化问题，提出一种融合前端先验的自适应鲁棒光束法平差方法，将前端计算出的动态置信度作为BA中代价函数的先验信息。构建一个在复杂动态场景下，相较于其他针对动态场景的VI-SLAM算法具有更高定位精度和更强运行稳定性的系统，为动态环境中的可靠自主导航提供一种有效的技术方案。
1 系统框架
为解决视觉惯性SLAM系统在动态环境中定位精度低、鲁棒性差的问题，提出一种针对动态环境的前后端协同VI-SLAM系统，整体框架如图1所示。
系统在前端对动态特征进行概率识别与过滤，并将该先验信息传递到后端来指导鲁棒光束法平差（BA），以实现对动态物体干扰的分层抑制与高效过滤。
系统同步处理相机图像流与IMU测量数据，IMU预积分为后端提供高频的相对运动约束。在特征处理阶段，利用IMU运动先验建立基于视差角的运动一致性模型，用以量化每个特征点运动与相机自身运动的偏离程度，随后引入Sigmoid函数和时序一致性滤波，将该度量转化为一个在时间序列上稳定、连续的动态置信度，该置信度一方面用于直接剔除高动态概率的特征，另一方面作为加权先验传递给后端优化，为后端优化提供关于特征历史运动一致性的重要信息。
后端在滑动窗口内进行自适应鲁棒优化来达到高精度的状态估计。为高效抑制动态特征，在代价函数中引入基于自适应截断最小二乘（Adaptive Truncated Least Squares，ATLS）的鲁棒项，并提出一个前后端协同的权重更新机制。在BA的交替优化过程中，特征权重不仅取决于当前帧的几何残差，还融合了来自前端的动态置信度先验信息。交替优化可以使BA过程更好地识别并削弱动态外点的影响，从而提高系统在复杂动态场景下的精度与鲁棒性。
2 概率性的动态特征识别与处理
在视觉惯性SLAM系统中，对场景中非静态元素的处理是决定其在城市等复杂环境中鲁棒性的关键，一个理想的动态SLAM框架应当兼顾准确性、通用性以及轻量化，所以提出了一种概率性动态特征识别与处理方法。在前端通过一个轻量级的特征处理，对动态属性进行“软分类”，而非“硬剔除”，并将此概率信息作为先验知识指导后端优化。
2.1 基于视差角的运动一致性模型
特征处理框架的基础是建立一个几何模型来量化特征点的运动是否与相机自身运动一致，即运动一致性模型[13]。利用IMU预积分提供的运动先验来预测相机观测到的静态点信息，并将其与实际观测进行比较，所以本文选择基于视差角（Parallax Angle）来衡量运动。动态特征视差角模型如图2所示。
对于三维空间中的一个点，其在两个关键帧 和 上的观测向量的夹角即为视差角 ，同一个特征点在不同两帧的归一化图像坐标分别为 和 ，所以观测视差角 的计算为
基于静态世界假设来预测其理论值。假设该特征点在 中的深度为 ，其三维坐标为 。利用IMU预积分提供的相对位姿变换 ，可将其变换到 的相机坐标系下，得到预测的三维点 。该预测点定义了一个预测观测向量 。理论视差角 即为原始观测向量 与预测观测向量 的夹角。两者的差异可以定义为运动不一致性度量
直观地反映了特征点的运动与相机运动的偏离程度，在理想的静止环境中 应该接近于0。
2.2 时序一致的动态置信度估计
直接使用一个固定的阈值来判断 是不稳定的，易受单帧噪声干扰，所以将瞬时度量转化为时序上的稳定概率来处理。
将瞬时的运动不一致性度量 通过一个Sigmoid函数，平滑地映射为一个瞬时动态置信度
式中： 和 为Sigmoid函数的参数。
为了系统判断更具鲁棒性，对每个观测的特征点设置一个累积动态概率 ，该概率通过一个一阶低通滤波器在时间序列上进行递归更新，以平滑单帧噪声带来的抖动[15]：
式中： 和 分别为当前帧和上一帧的累积概率； 为当前帧的瞬时置信度； 为一个较大的平滑因子，代表了系统对历史判断的信任值；如果一个特征点连续多帧表现出动态性，其 将稳定地趋近于1。
2.3 层次化的动态特征处理
该方法的关键在于将前端识别出的概率信息与后端优化进行有效结合，形成一个层次化的动态特征协同处理策略。
在前端设置一个较高的剔除阈值 （实验中设为0.95）：对于累积动态概率 的特征点，其动态属性已非常明确，故在前端跟踪环节就将其剔除，这样可以有效阻止高动态特征进入后端，从而节省计算资源；对于动态性不明确的特征，把它作为“软信息”传递给后端处理。具体来说，每个特征点的鲁棒代价函数权重的初始值 ，将由前端计算的累积动态概率来决定：
将该先验作为权重引入后端，可以为非线性优化提供特征可靠的先验信息，若某特征在前端被判定为高动态概率，则其在后端的初始权重较低，有利于引导优化避开受动态点干扰的解，减少陷入局部最优的可能性，从而实现前后端在处理动态物体时的高效协同。
3 自适应鲁棒BA
在现代视觉惯性SLAM系统中，光束法平差通常作为后端优化的核心，它通过联合优化相机位姿和三维地图点来实现高精度的状态估计[17]。但是经典的BA框架是基于最小二乘的代价函数构建的，就算结合通用的鲁棒核函数（如Huber Loss）也难以完全剔除由动态物体引起的大幅值、非高斯分布的残差。
为此提出了一种融合动态置信度的自适应鲁棒BA框架，其核心思想是将前端概率先验与自适应截断最小二乘（ATLS）方法相结合，构建前后端协同的分层抑制机制，从而针对性地避免动态干扰。
3.1 代价函数
在一个固定大小的滑动窗口内，SLAM系统后端通过最小化一个联合代价函数来执行BA。该代价函数由3部分构成：来自边缘化的先验残差 ，IMU预积分残差 以及视觉重投影残差 ，所以传统的视觉惯性BA的优化目标可以表示为
式中： 为待优化的状态向量，包含窗口内的相机位姿、速度、偏置和特征点逆深度； 为一个标准的鲁棒核函数。
为了解决传统鲁棒核无法完全剔除外点影响的问题，引入Black-Rangarajan（B-R）[18]对偶性，可以得到视觉代价项为一个基于截断最小二乘的等价形式：
其中，权重 为一个与TLS代价函数严格等价的对偶变量， 为保证等价性的惩罚项。通过这种形式，本文将一个标准的BA问题，转化为了一个加权BA问题，可以更鲁棒地处理动态特征。
3.2 自适应截断与协同权重更新
在BA框架下提出的权重 的计算方式结合了自适应的残差截断和前端传递的动态先验。与DynaVINS++仅使用当前帧残差来动态调整截断范围相比，本文的方法进一步融合了前端多帧累积的动态先验，实现了更加可靠的外点判定与稳定的权重更新机制。
设计了一个自适应截断范围 ，该范围基于当前帧中高置信度静态特征集的最大重投影误差来确定：
其中 是一个用在静态特征集的参数，取一个较高的值可以确保用于估计 的是一个纯净的静态点集，此处 是一个用于在静态特征集的纯度和充分性之间进行权衡的参数（实验中设为0.9）。
截断范围被设定为 ，这种自适应机制的好处在于，BA可以根据当前帧的运动状态和IMU预测精度来动态调整其对特征的权重。
在B-R对偶框架下，权重的最优解存在一个闭式解[19]。根据当前残差和自适应阈值计算出一个中间权重
式中： 。
将前端计算出的、代表特征历史运动一致性的累积动态概率，与上述中间权重进行融合，得到用于BA的最终权重
这种融合策略实现了双重抑制：对于一个特征点，无论是因为当前帧的几何残差大（导致 变小），还是因为历史运动不一致（导致 变小），其最终在BA中的权重都会被有效降低。
3.3 交替BA优化
整个后端优化在一个交替优化的框架下进行，本文将其称为交替式BA优化，它迭代执行以下两个步骤直至收敛。
（1）加权BA求解：固定所有特征的最终权重 ，此时式(7)变为一个标准的加权非线性最小二乘问题。然后使用Ceres Solver[20]对状态向量 进行求解，执行一次加权的BA。
（2）权重再估计：固定上一步优化得到的状态 ，根据3.2节描述的流程，为每个视觉观测重新计算其最终权重 。
4 实验结果及算法对比分析
为了全面、客观地评估本文提出的VI-SLAM系统（Ours）的性能，在专为动态场景设计的视觉惯性公开数据集VIODE（Visual-Inertial Odometry in Dynamic Environments）[21]上进行了一系列实验。将本文的方法与当前先进的开源动态SLAM算法DynaVINS进行了全面的对比分析。此外，由于DynaVINS++未开源，为了充分地验证本文所提策略的有效性，本文在DynaVINS的开源框架上，实现了借鉴DynaVINS++核心思想的自适应截断最小二乘（ATLS）后端，并将其作为一个中间方法（简称为DynaVINS+TLS）加入对比。实验在搭载Ubuntu18.04系统，AMD Ryzen 57500F处理器，32GB内存的台式电脑上运行。
4.1 动态特征识别
一个稳定的动态VI-SLAM系统，其基础在于能够准确地识别并处理来自非静态物体的特征点。为了直观地评估本文提出的前后端协同动态抑制策略的有效性，将本文的方法（Ours）与DynaVINS及DynaVINS+TLS进行比较来展示本文的方法在动态特征识别上的优越性，如图3所示。
图3为VIODE数据集city_day_high序列中一个典型的动态场景，该场景中包含一辆正在从右向左行驶的货车，它是主要的动态干扰源，图3（a）、（b）、（c） 分别展示了DynaVINS、DynaVINS+TLS与本文方法Ours对该场景中特征点动静属性的判断结果。根据优化后收敛的权重，被判定为静态内点的特征点用绿色标出，被判定为动态外点的特征点用红色标出。图中可以观察到，DynaVINS能够识别出车身主体上的大部分动态点，但在车轮附近以及车身与背景交界处的区域存在漏检和误检。DynaVINS+TLS引入了后端TLS后，识别的动态点更集中了，但是也存在部分判断错误。这是因为该方法主要依赖后端残差与权重更新来优化，在应对运动幅度较小或存在歧义的特征时，容易陷入局部最优，导致判定不够准确。
相比之下，本文方法识别出的动态特征点区域与实际移动的汽车更加吻合，对背景静态特征的误判也显著减少。这说明了本文所提出的前后端协同机制能发挥作用：即使其在当前帧的残差不大时，前端的概率性识别通过时序累积，依然能够稳定地识别出那些持续表现出运动不一致性的特征点，并将这一带有高动态概率的先验信息传递给后端的自适应鲁棒优化器，在权重更新中进行针对性的抑制。
该对比结果表明，本文的层次化抑制策略相较于纯后端方法能更精确、更完整地分割出场景中的动态特征，为后续高精度的位姿估计提供更可靠的数据支撑。
4.2 轨迹精度评估
轨迹精度是评估VI-SLAM系统性能最核心的指标。在两个具有代表性的高动态序列city_day_high和parking_lot_high上，对算法的绝对轨迹误差（ATE）和相对位姿误差（RPE）进行综合评估。
图4中（a）与（c）展示了在两个序列中，各算法的估计轨迹与地面真值的轨迹对比图。在两个场景中，本文方法的轨迹与地面真值高度贴合，展现出最优的全局一致性，而其他算法的轨迹都出现了明显的漂移。（b）与（d）展示了本文算法（ours）评估的ATE轨迹误差曲线，在city_day_high序列的首层段与parking_lot_high的尾段是动态频发的时间段，各算法都受到一定的影响产生更大的误差，但本文的方法更加稳定。
在两个序列上，各算法在 ， ， 三个轴上的绝对位姿误差随时间变化的曲线，如图5所示。从图中可以清晰地观察到，DynaVINS和DynaVINS+TLS的误差曲线在复杂的动态场景中会出现更加剧烈的波动和偏移，而本文的算法误差曲线较为稳定，证明了本文的算法对动态干扰具有更强的鲁棒性和更快的抑制能力。
详细的评估结果如表1所示。从ATE RMSE指标可以看出，在city_day_high和parking_lot_high序列上，本文方法的全局精度相较于DynaVINS和DynaVINS+TLS均有所提升，证明了本文提出的算法在不同动态场景下的高精度。同时，最低的ATE Std指标也表明本文提出的SLAM系统运行最为稳定。
相对位姿误差（RPE）可以反映系统的局部性能，尤其值得注意的是RPE的旋转误差，在city_day_high序列上，本文的方法相比DynaVINS降低了 ，证明了本文的前后端协同策略能够有效避免优化器为了拟合动态特征而做出错误的姿态调整，从而保证了姿态估计的高度准确性。
4.3 不同动态程度下的鲁棒性分析
为了进一步验证算法在不同动态干扰程度下的鲁棒性，在VIODE的city_day系列数据集上进行了测试，该系列包含了从无动态物体（none）到高密度动态物体（high）的4个子序列。表2总结了3种算法在这些序列上的ATE RMSE值。
从表中数据可以看出，在无动态的None场景下，三者性能相近，证明本文方法在静态场景下不会造成性能损失。然而，随着动态干扰的加剧，算法间的性能差距逐渐拉大。
尤其在从Mid到High的转变中，DynaVINS的稳定性出现了剧烈的波动，误差增长了 ，而本文方法的误差仅增长了 。这表明，在高动态压力下，本文方法的性能下降更为平缓，展现出更强的鲁棒性。这一结果证明了，层次化动态抑制SLAM框架相比于其他算法能够更有效地应对动态干扰。
5 结束语
针对现有视觉惯性SLAM系统在动态环境中，因动态物体干扰导致定位精度下降、鲁棒性差的难题，本文提出了一种前后端协同的动态鲁棒视觉惯性SLAM系统。该系统在DynaVINS的算法框架基础上，通过在前端引入概率性的动态特征识别机制，并与本文改进的后端自适应鲁棒优化进行协同，实现了对动态物体干扰的层次化、高效抑制。
本文的主要研究工作与贡献可以概括为：在前端，提出了一种基于运动一致性的概率性动态特征识别框架。该框架通过比较观测视差角与IMU预测的理论值，为每个特征计算出一个连续、稳定的动态置信度，实现了对动态特征的“软分类”，有效避免了传统固定阈值方法所带来的误判和漏判；在后端，设计了一种融合前端动态置信度先验的自适应鲁棒优化器。引入了自适应截断最小二乘（ATLS）的思想，并创新性地将前端计算出的动态置信度作为权重先验，实现了前后端在处理动态物体上的紧密协同，显著提升了在复杂动态环境下的可靠性。
在公开数据集VIODE上的大量实验结果表明，本文提出的方法取得了优越的性能。与DynaVINS算法相比，本文方法在city_day_high和parking_lot_high等高动态序列上的均方根误差RMSE分别降低了 和 。在不同动态等级的场景测试中，本文方法也比其他方法展现出更强的鲁棒性。
本文仅做学术分享，如有侵权，请联系删文。
。
添加微信：cv3d001，备注：姓名+方向+单位，邀请入群。
