这是一篇发表在期刊《IEEE Robotics and Automation Letters》(Volume 9, Issue 8) 上，于2024年8月出版的研究型论文。该论文报告了一项名为“Switch-SLAM”的原创性研究工作。
Switch-SLAM：面向退化环境的基于切换机制的激光-惯性-视觉SLAM
Junwoon Lee, Ren Komatsu, Mitsuru Shinozaki, Toshihiro Kitajima, Hajime Asama, Qi An, 以及 Atsushi Yamashita等研究人员，主要来自日本东京大学（包括前沿科学研究生院和工程研究生院）与株式会社久保田（Kubota Corporation）的合作团队，共同提出并验证了一种新型的同时定位与地图构建（Simultaneous Localization and Mapping， SLAM）系统。这项研究旨在解决移动机器人在复杂、结构缺失（退化）环境中，因单一传感器局限性而导致定位和建图失败的关键难题。
研究的学术背景与目标 近年来，三维SLAM技术取得了显著进展，极大地提升了移动机器人感知环境、精确定位和构建详细地图的能力。然而，SLAM的性能严重受限于传感器本身的固有约束。具体而言，激光雷达（LiDAR）SLAM在缺乏明显结构特征的环境中，如长走廊、开阔田野等，其里程计（Odometry）容易发生退化（Degeneracy），导致定位失效。相反，视觉（Visual）SLAM在面对剧烈运动、光照条件快速变化以及纹理缺失的环境时，同样面临巨大挑战。为了融合两种传感器的优势，多种激光-视觉融合SLAM方法被开发出来。然而，现有方法大多依赖于基于最大后验（Maximum A Posteriori， MAP）估计的融合策略，如迭代卡尔曼滤波器（Iterated Kalman Filter）或因子图优化（Factor Graph Optimization）。当某个传感器（如激光雷达或相机）出现长期、持续的退化或故障时，其产生的错误信息会通过这种紧密耦合的融合框架传播至整个系统，从而损害整体性能的鲁棒性。
针对这一局限性，本研究团队提出了名为Switch-SLAM的解决方案。其核心目标是设计一个高鲁棒性和高精度的SLAM系统，专门应对激光或视觉里程计单独退化的情况。研究旨在通过一种创新的“切换结构”（Switching Structure），而非传统的紧密耦合融合，来隔离传感器退化带来的负面影响，并引入一种无需人工经验调参的、普适性强的退化检测方法。
研究方案的详细流程 Switch-SLAM的系统结构包含三个主要节点：视觉里程计节点、激光雷达里程计节点和核心的切换节点。整个工作流程可以详细拆解如下：
流程一：多传感器数据并行处理与状态传播 研究首先并行运行两个独立的里程计子系统。在视觉里程计节点，研究采用了VINS-Mono的方法，通过跟踪图像特征并进行滑动窗口优化（Sliding Window Optimization）来估计位姿。为了解决单目视觉的尺度模糊问题，系统利用惯性测量单元（Inertial Measurement Unit， IMU）数据进行初始化并对齐。估计出的视觉位姿随后与高频IMU测量值通过预积分（Preintegration）技术进行融合与传播，产生高频的“视觉-惯性”状态流。预积分技术有效整合了从时刻i到j的IMU数据，形成相对运动约束因子，避免了在优化中重复积分IMU数据带来的巨大计算负担。公式(1)-(3)描述了平移、速度和旋转的预积分因子及其伴随的高斯过程噪声。
在激光雷达里程计节点，研究采用了LOAM系列方法的框架。首先，利用来自切换节点提供的位姿对激光雷达点云因自身运动造成的畸变进行校正。接着，从每一帧激光扫描中提取平面和边缘特征，其方法是评估同一扫描线上局部表面的平滑度。然后，通过最近邻搜索将当前扫描的特征与已有地图中的特征进行关联。基于此关联，可以计算出点到边缘和点到平面的距离，如公式(4)和(5)所示。这些距离构成了扫描到地图匹配（Scan-to-Map Matching）的非线性代价函数f_l(x)，如公式(6)所示。该优化问题通常使用列文伯格-马夸尔特（Levenberg-Marquardt）方法迭代求解，如公式(7)所示，并可简化为求解一个线性系统以获得位姿增量δx，如公式(8)所示。其中，J_l是代价函数的雅可比矩阵，H_l ≈ J_l^T J_l是近似的海森矩阵（Hessian Matrix）。求解得到的激光雷达位姿同样会与IMU数据进行预积分和传播，产生高频的“激光-惯性”状态流。
流程二：非启发式激光雷达退化检测 这是本研究的核心创新之一。为了决定在扫描匹配时应该使用“激光-惯性”还是“视觉-惯性”传播的状态作为初始猜测（Initial Guess），系统需要实时、准确地检测激光雷达里程计是否发生退化。研究团队观察到，在缺乏结构的环境中（如开阔地、长廊、隧道），激光雷达里程计的退化通常不会超过三个自由度（Degrees of Freedom， DOF），因为在其感知范围内通常仍存在平面或边缘特征。基于此物理假设，研究将检测焦点放在最可能退化的三个自由度上。
检测方法利用激光雷达扫描匹配优化中海森矩阵H_l的特征值（Eigenvalues）信息。首先，提取H_l三个最小的特征值，对应最可能退化的三个方向，记为λ = [λ1, λ2, λ3]。将其归一化得到λ̅。关键创新在于设定阈值λ_t的方法。研究摒弃了需要针对不同环境进行经验性调参的启发式（Heuristic）阈值，转而采用基于卡方检验（Chi-squared Test）的统计方法。研究者假设归一化特征值的分布近似对称，并结合其取值范围，推导出在三个特征值相互独立且服从均匀分布（基于随机矩阵理论的半圆分布近似）的零假设下，每个特征值的期望值E_m。然后，在95%的置信水平下（卡方值为0.103，自由度为2），根据公式(12)计算出非启发式阈值λ_t = [0.12, 0.27, 0.48]。如果任何一个归一化特征值λ̅_i低于对应的阈值λ_t(i)，则判定激光雷达里程计在当前自由度上发生退化。
流程三：基于状态缓冲区的平滑切换机制 为了避免检测结果在阈值附近抖动导致初始猜测频繁跳变，进而引起位姿估计不连续，研究引入了“状态缓冲区”（Status Buffer）方法。系统维护一个固定长度的队列，持续存储过去一段时间内的退化检测状态。根据队列中状态的历史记录，将当前状态分类为“正常”、“开始/结束退化”或“完全退化”。 * 如果激光雷达状态正常或视觉里程计失败，则使用纯激光雷达传播状态作为扫描匹配的初始猜测。 * 如果激光雷达被判定为“完全退化”且视觉里程计正常，则切换至视觉里程计传播的状态作为初始猜测。 * 如果处于“开始/结束退化”的过渡状态，则使用线性插值来融合两种传感器的微分运动δt^l和δt^v，如公式(13)所示。插值权重由最小的归一化特征值λ̅_1决定，确保状态平滑过渡。
流程四：退化和故障感知的扫描到地图匹配优化 根据检测到的状态，扫描到地图匹配的优化过程会进行相应调整，这是确保系统鲁棒性的另一关键。算法1总结了整个切换节点的逻辑。 1. 激光雷达正常或视觉失败时：执行标准的扫描匹配优化，如公式(14)所示，直接使用完整的H_l矩阵求逆。 2. 激光雷达退化且视觉正常时：采用紧耦合融合优化。首先，对H_l进行特征分解H_l = UΛU^{-1}。然后，将Λ矩阵中对应退化自由度的特征值置零（或移除），得到Λ_p。最终，通过求解一个最小二乘问题来融合视觉和激光雷达的残差，如公式(15)所示。其中，e_v(δx)是视觉优化的残差项，e_l(δx)是使用修正后的信息矩阵(UΛ_p U^{-1})^{-1}加权的激光雷达残差项。这种融合方式在退化方向上依赖视觉约束，在健康方向上依赖激光雷达约束。 3. 两者均出现问题时：优化仅在激光雷达状态良好的自由度上进行，未确定的方向主要依赖IMU预积分来约束。
流程五：后端优化与实验验证 系统后端使用iSAM2进行位姿图优化（Pose Graph Optimization），以融合全球导航卫星系统（GNSS）信号或闭环检测（Loop Closure）信息，进一步提升长期精度和全局一致性。闭环检测采用Scan Context方法。 为了全面评估Switch-SLAM，研究团队在多种数据集上进行了广泛的实验，这些数据集涵盖了激光和视觉里程计退化的不同场景，包括模拟数据集（平面、快速旋转、农场）和真实世界数据集（手持设备采集、DARPA地下挑战赛的Cerberus数据集、Subt-MRS多楼层和长走廊数据集）。实验将Switch-SLAM与最先进的纯激光SLAM（LOAM， LIO-SAM）、纯视觉SLAM（VINS-Mono）以及激光-视觉惯性里程计（LVIO）方法（如LVI-SAM， R2LIVE， R3LIVE， Fast-LIVO等）进行了对比。所有比较均在相同硬件上以实时方式运行，且不使用GNSS和闭环以公平对比前端性能。
主要研究结果 实验结果表明，Switch-SLAM在多种挑战性环境中均表现出优异的鲁棒性和精度。 1. 在结构化环境（快速旋转数据集）中：Switch-SLAM表现与纯激光方法LIO-SAM相当。因为此时激光雷达状态良好，系统通过切换结构自动选择纯激光模式运行。 2. 在激光雷达退化环境（平面数据集）中：纯激光方法完全失败，而Switch-SLAM和VINS-Mono取得了最佳性能。Switch-SLAM成功检测到激光雷达退化并切换至视觉里程计作为主导，其性能也优于其他LVIO方法，证明了切换结构在隔离激光退化信息方面的有效性。 3. 在混合退化环境（农场数据集）中：该场景同时包含导致视觉退化的剧烈运动和导致激光退化的结构缺失环境。纯激光和纯视觉方法在各自不适应的阶段都会失败。Switch-SLAM则凭借其动态切换能力，在所有对比方法中取得了最佳性能，显著优于现有的LVIO方法。 4. 在真实世界复杂数据集（手持、Cerberus， Subt-MRS）中：Switch-SLAM consistently展现出强大竞争力或最优性能。特别是在Cerberus数据集的Anymal 3序列中，相机出现短暂视频中断导致视觉里程计失败，同时环境结构又导致激光雷达退化。VINS-Mono和依赖其作为初始猜测的LVI-SAM均告失败，而Switch-SLAM凭借其状态缓冲区和切换机制，成功完成了SLAM。在同时存在结构缺失和视觉挑战的Subt-MRS数据集中，Switch-SLAM也取得了最佳性能。 5. 退化检测精度评估：研究者还将提出的非启发式退化检测方法与两种基于特征值的启发式方法[16]， [17]进行了定量比较。在手持数据集的部分序列上，以GNSS和迭代最近点（Iterative Closest Point， ICP）匹配结果作为地面真值进行评估。结果显示，本方法的准确率（Accuracy）达到0.96，召回率（Recall）达到0.99，均优于或媲美经过精心调参后的对比方法（[16]：准确率0.91，召回率0.91；[17]：准确率0.96，召回率0.96）。更重要的是，本方法无需针对不同环境调整阈值参数。
研究的结论与价值 本研究的结论是，所提出的Switch-SLAM系统通过其创新的切换结构和非启发式退化检测方法，能够有效处理激光或视觉里程计单独退化的情况，在多种极具挑战性的环境中实现了比当前最先进的激光、视觉及激光-视觉融合SLAM方法更优的定位精度和鲁棒性。 这项工作的科学价值在于，它突破了传统紧密耦合多传感器融合框架在应对长期传感器退化时的固有局限性，提出了一种“择优而用、隔离故障”的新范式。其非启发式检测方法为SLAM系统中的退化问题提供了一个更具理论依据和普适性的解决方案。在应用价值上，Switch-SLAM极大地提升了移动机器人在农业、地下勘探、野外巡检等充满非结构化、光照变化、纹理缺失的复杂真实场景中的自主导航可靠性，具有重要的实际应用前景。
研究的亮点 1. 新颖的切换结构：不同于传统的紧耦合融合，该结构允许系统在激光和视觉里程计之间选择最优的初始猜测，并能有效阻止单一传感器的长期退化或失败信息污染整个系统状态，从而在根本上提升了系统的容错能力。 2. 创新的非启发式退化检测：首次将卡方检验与对激光雷达退化自由度的物理假设相结合，为海森矩阵特征值设定统计意义上的阈值，消除了对环境依赖的经验性参数调优，增强了方法的通用性和可移植性。 3. 全面的实验验证：研究在精心设计的模拟数据和多个公开的真实世界数据集上进行了广泛测试，这些数据集系统地覆盖了激光和视觉退化的各种典型及极端情况，充分证明了方法的有效性和优越性。 4. 工程实现细节完善：论文不仅提出了核心算法，还详细阐述了状态缓冲区平滑过渡、退化感知的扫描匹配优化等工程实现细节，确保了系统的实用性和稳定性。
其他有价值内容 论文还提及了视觉里程计故障检测的机制，该系统直接采用了VINS-Mono中的方法，通过追踪特征数量、IMU偏置变化以及关键帧间位姿变化等指标来判断视觉子系统是否失效，并在失效时触发系统重初始化，在此期间完全依赖激光雷达里程计。此外，论文通过可视化地图和轨迹对比图（图7，8），直观展示了Switch-SLAM在退化环境中构建的地图质量远优于其他失败的方法，提供了强有力的定性证据。