计算机视觉 · SLAM · 智能交通系统Computer Vision · SLAM · Intelligent Transportation Systems
Ultra-Fusion：面向智能交通系统的抗退化多传感器紧耦合 SLAM 框架Ultra-Fusion
Ultra-Fusion: A Resilient Tightly-Coupled Multi-Sensor Fusion SLAM Framework under Sensor Degradation and Spatiotemporal Perturbation for Intelligent Transportation Systems
Yihong Tian, Junjie Zhang, Liuyang Li, Deteng Zhang, Yunfei Zuo, Jie Yin（Beijing Institute of Technology / Chongqing University / Sichuan University / Northwestern Polytechnical University / Shanghai Jiao Tong University）Yihong Tian, Junjie Zhang, Liuyang Li, Deteng Zhang, Yunfei Zuo, Jie Yin (Beijing Institute of Technology / Chongqing University / Sichuan University / Northwestern Polytechnical University / Shanghai Jiao Tong University)
一个用统一滑窗估计器把 LiDAR、相机、IMU、轮速、GNSS 全部变成可插拔因子的紧耦合定位框架：观测性感知初始化选启动模式，逐因子可靠性调度过滤退化观测，在线 LiDAR–IMU 时空标定纠正漂移的时间偏移与旋转外参——在 60 余个开源 SLAM 系统的对比中，于轮式车辆、四足机器人与无人机上都取得有竞争力的精度。A tightly-coupled localization framework that turns LiDAR, camera, IMU, wheel odometry and GNSS into pluggable factors inside a single sliding-window estimator: observability-aware initialization selects the startup mode, factor-wise reliability scheduling filters out degraded observations, and online LiDAR–IMU spatiotemporal calibration corrects the drifting time offset and rotational extrinsics — in a comparison against more than 60 open-source SLAM systems it attains competitive accuracy on wheeled vehicles, quadruped robots and UAVs alike.
智能交通系统（ITS）——自动驾驶车、四足最后一公里配送机器人、基建巡检无人机——都依赖可靠定位，但真实部署中传感器会退化：光照不足、LiDAR 几何退化（走廊/电梯间）、轮子打滑、GNSS 遮挡，这些问题在城市峡谷、隧道、高速路段尤其常见，会破坏路线跟踪、隧道通过连续性与局部地图对齐。多平台/多传感器组合还带来时空标定误差（时间偏移、外参漂移），进一步累积定位漂移。Intelligent transportation systems (ITS) — autonomous vehicles, quadruped last-mile delivery robots, UAVs for infrastructure inspection — all rely on reliable localization, yet sensors degrade in real deployments: insufficient illumination, LiDAR geometric degeneracy (corridors / elevator rooms), wheel slippage and GNSS occlusion. Such conditions are especially common in urban canyons, tunnels and highway segments, where they break route tracking, tunnel-traversal continuity and local map alignment. Multi-platform / multi-sensor combinations further introduce spatiotemporal calibration errors (time offset, extrinsic drift), which accumulate into further localization drift.
"we argue that ITS localization should be treated as a unified estimation problem rather than as configuration-specific pipelines" — 论文主张：定位应被当作统一估计问题，而不是为每种传感器组合单独设计管线。the paper argues that localization should be treated as a unified estimation problem instead of designing a separate pipeline for each sensor configuration.
现有紧耦合/松耦合框架（VINS-RGBD、GR-Fusion、LVI-SAM、R3LIVE、FAST-LIVO2、Ground-Fusion(++) 等）大多绑定固定传感器栈，切换传感器组合往往要改状态定义、初始化、因子激活或标定变量；退化感知和标定通常局限于子系统级别。Ultra-Fusion 把 LiDAR 几何残差、视觉重投影、惯性、轮速、GNSS 因子都放进同一个滑窗共享目标函数，用同一套初始化、可靠性调度器和标定逻辑支持 WIO / VIO / LIO / LVIO 及其增强变体。Existing tightly-/loosely-coupled frameworks (VINS-RGBD, GR-Fusion, LVI-SAM, R3LIVE, FAST-LIVO2, Ground-Fusion(++), etc.) are mostly tied to a fixed sensor stack: switching the sensor combination usually requires changing the state definition, the initialization, the factor activation or the calibration variables, and degradation awareness and calibration are typically confined to the subsystem level. Ultra-Fusion places LiDAR geometric residuals, visual reprojection, inertial, wheel-odometry and GNSS factors into one shared objective function over a single sliding window, and supports WIO / VIO / LIO / LVIO and their augmented variants with one common initialization, reliability scheduler and calibration logic.
Fig. 1（论文 Overview 图）：Ultra-Fusion 作为跨传感器、跨平台、跨场景的统一多传感器 SLAM 框架——支持异构传感器输入，可部署于地面车辆、无人机、四足机器人等平台，并在传感器退化、时空不确定性、长时/高速运行下提升定位鲁棒性。Fig. 1 (Overview figure of the paper): Ultra-Fusion as a unified multi-sensor SLAM framework across sensors, platforms and scenes — it accepts heterogeneous sensor inputs, can be deployed on ground vehicles, UAVs, quadruped robots and other platforms, and improves localization robustness under sensor degradation, spatiotemporal uncertainty and long-duration / high-speed operation.
60+对比的开源 SLAM 系统数量open-source SLAM systems compared
0.59% / 0.24mM2DGR-Plus 上最低平均漂移率 / RMSElowest average drift rate / RMSE on M2DGR-Plus
0.37%–0.58%KAIST 25.2–96.9 km/h 高速下 LVWIO 平均漂移率LVWIO average drift rate on KAIST at 25.2–96.9 km/h high speed
Ultra-Fusion 建立在作者团队 IROS 2025 的 Ground-Fusion++ 之上，通过五项扩展重构为期刊级 ITS 定位框架：(i) 统一的可选因子滑窗估计器（而非子系统耦合）、(ii) 可观测性感知初始化、(iii) 图内逐因子可靠性调度、(iv) 在线 LiDAR–IMU 时空标定、(v) 扩展的 M3DGR 基准与跨平台验证（KAIST / GrandTour / MARS-LVIG）。Ultra-Fusion builds on the authors' Ground-Fusion++ (IROS 2025) and is restructured into a journal-level ITS localization framework through five extensions: (i) a unified sliding-window estimator with optional factors (rather than coupled subsystems), (ii) observability-aware initialization, (iii) factor-wise reliability scheduling inside the graph, (iv) online LiDAR–IMU spatiotemporal calibration, and (v) an extended M3DGR benchmark with cross-platform validation (KAIST / GrandTour / MARS-LVIG).
Fig. 2（Pipeline 图）：可配置的传感器流按时间戳排序、在线标定，经可观测性感知的启动过程进入统一滑窗因子图；逐因子可靠性调度抑制退化观测，保留的因子在共享状态、边缘化、可选轮速/GNSS 增强与在线标定下联合优化，覆盖 WIO / VIO / LIO / LVIO 各配置。Fig. 2 (Pipeline figure): configurable sensor streams are ordered by timestamp and calibrated online, then enter the unified sliding-window factor graph through an observability-aware startup procedure; factor-wise reliability scheduling suppresses degraded observations, and the retained factors are jointly optimized under a shared state, marginalization, optional wheel-odometry / GNSS augmentation and online calibration, covering the WIO / VIO / LIO / LVIO configurations.
平台状态由姿态、位置、速度和 IMU 偏置（rotation、translation、velocity、accelerometer/gyroscope bias）构成，在一个滑动窗口内联合优化。与"scan-to-map 得到 LiDAR 里程计再作为外部先验"的常见做法不同，Ultra-Fusion 把点到面 LiDAR 几何残差直接保留在共享目标函数里，同时纳入 IMU 预积分、轮速预积分、视觉重投影、GNSS 位置锚定因子，以及基于 Schur 补/QR 边缘化得到的先验因子。不同传感器可用性只改变哪些因子被激活，不改变状态定义或边缘化接口，从而在一套估计器里支持 WIO、VIO、LIO、LVIO 及其增强变体。The platform state consists of attitude, position, velocity and IMU biases (rotation, translation, velocity, accelerometer/gyroscope bias), jointly optimized within one sliding window. Unlike the common practice of obtaining LiDAR odometry by scan-to-map and then feeding it in as an external prior, Ultra-Fusion keeps the point-to-plane LiDAR geometric residuals directly in the shared objective function, while also incorporating IMU pre-integration, wheel-odometry pre-integration, visual reprojection and GNSS position-anchoring factors, together with prior factors obtained from Schur-complement / QR marginalization. Different sensor availability only changes which factors are activated, not the state definition or the marginalization interface, so a single estimator supports WIO, VIO, LIO, LVIO and their augmented variants.
初始化被建模为"依据运动激励和几何可观测性做模式选择"：根据激励强度、视觉支持度、LiDAR 几何支持度，从 SfM 视觉惯性对齐（动态假设 D）、静止/轮速辅助惯性对齐（静止假设 S）、LiDAR 里程计辅助的短窗 MAP 估计（M）三种启动模式中选择，若证据不足则延迟启动（A）。实验显示：完整系统平均初始化延迟 0.153s、中位延迟 0.150s、20s 平均 ATE 为 0.483m，在 18 个序列中 15 个取得最快初始化、11 个取得最低 20s ATE；关闭自适应初始化后延迟升至 4.642s，20s ATE 升至 16.808m。Initialization is modeled as mode selection driven by motion excitation and geometric observability: according to excitation strength, visual support and LiDAR geometric support, the system picks among three startup modes — SfM visual-inertial alignment (dynamic hypothesis D), stationary / wheel-aided inertial alignment (stationary hypothesis S), and LiDAR-odometry-aided short-window MAP estimation (M) — and defers startup (A) when the evidence is insufficient. Experiments show an average initialization latency of 0.153s and a median of 0.150s for the full system, with a 20s average ATE of 0.483m; it achieves the fastest initialization on 15 of 18 sequences and the lowest 20s ATE on 11. With adaptive initialization disabled, the latency rises to 4.642s and the 20s ATE to 16.808m.
对 LiDAR、视觉、IMU、轮速、GNSS 五种模态，每个关键帧都计算一个归一化到 [0,1] 的退化分数（数值越大表示越不可靠），映射为二值激活指示变量（必要时做协方差膨胀），并用短程滞回平滑避免频繁切换，直接嵌入统一滑窗目标函数而不改变状态或边缘化接口。LiDAR 退化由点到面 Hessian 的特征值/条件数、法向协方差与匹配点数联合判定；视觉退化看特征数、空间分布均匀度、KLT 内点率与重投影残差；IMU 看激励强度与预积分残差；轮速看与惯性/视觉预测增量的差异；GNSS 看定位质量、协方差迹与新息一致性。For the five modalities — LiDAR, vision, IMU, wheel odometry and GNSS — a degradation score normalized to [0,1] (the larger, the less reliable) is computed at every keyframe, mapped to a binary activation indicator (with covariance inflation where necessary) and smoothed by short-range hysteresis to avoid frequent switching; it is embedded directly into the unified sliding-window objective without changing the state or the marginalization interface. LiDAR degradation is judged jointly from the eigenvalues / condition number of the point-to-plane Hessian, the normal covariance and the number of matched points; visual degradation from the feature count, the uniformity of spatial distribution, the KLT inlier ratio and the reprojection residual; IMU from excitation strength and pre-integration residuals; wheel odometry from the discrepancy with the inertial/visual predicted increments; GNSS from positioning quality, the covariance trace and innovation consistency.
标定变量为 LiDAR–IMU 时间偏移 Δt_LI 与旋转外参 R_IL（平移外参固定），由两个轻量并行工作线程在线细化：时间标定线程通过 IMU 角速度/加速度与前端 LiDAR 里程计运动量的互相关 + 鲁棒对齐代价估计时间偏移；外参标定线程用一条独立于主 SLAM 前端的 scan-to-scan LiDAR 里程计分支，结合惯性预积分做旋转一致性约束求解旋转外参（闭式 Procrustes 解 + 精细化）。候选更新需通过残差、置信度与短历史一致性检验才会被采纳；锁定外参时会保持世界坐标下 LiDAR 位姿不变，避免地图不连续。The calibration variables are the LiDAR–IMU time offset Δt_LI and the rotational extrinsics R_IL (the translational extrinsics are fixed), refined online by two lightweight parallel worker threads: the temporal-calibration thread estimates the time offset from the cross-correlation between IMU angular velocity/acceleration and the motion magnitude of the front-end LiDAR odometry, plus a robust alignment cost; the extrinsic-calibration thread uses a scan-to-scan LiDAR odometry branch independent of the main SLAM front end and combines it with inertial pre-integration as a rotation-consistency constraint to solve for the rotational extrinsics (closed-form Procrustes solution plus refinement). A candidate update is adopted only after passing residual, confidence and short-history consistency checks; when the extrinsics are locked, the LiDAR pose in world coordinates is kept unchanged so that the map stays continuous.
03 Experiments
评估围绕 5 个问题展开：整体基准对比、退化鲁棒性、时空标定鲁棒性、长时/高速运行、跨平台迁移。数据集覆盖 M3DGR（含新增的 M3DGR Sim 仿真退化序列）、M2DGR-Plus（校园轮式）、KAIST（城市高速驾驶，速度最高 96.9 km/h）、GrandTour（四足机器人）、MARS-LVIG（低空无人机）。基线包括轮速/GNSS 参考、纯视觉系统（ORB-SLAM2/3、VINS-Mono、TartanVO、MASt3R-SLAM 等）、LiDAR/LiDAR-惯性系统（FAST-LIO2、KISS-ICP、Coco-LIC 等）与 LiDAR-视觉系统（R3LIVE、FAST-LIVO2、Ground-Fusion(++) 等），accuracy 用 EVO-aligned ATE RMSE，失败运行标记为 ✗ 并按论文注记方式计入聚合误差。The evaluation is organized around 5 questions: overall benchmark comparison, robustness to degradation, robustness of spatiotemporal calibration, long-duration / high-speed operation, and cross-platform transfer. The datasets cover M3DGR (including the newly added M3DGR Sim simulated degradation sequences), M2DGR-Plus (campus, wheeled), KAIST (urban high-speed driving, up to 96.9 km/h), GrandTour (quadruped robots) and MARS-LVIG (low-altitude UAVs). Baselines include wheel-odometry/GNSS references, purely visual systems (ORB-SLAM2/3, VINS-Mono, TartanVO, MASt3R-SLAM, etc.), LiDAR / LiDAR-inertial systems (FAST-LIO2, KISS-ICP, Coco-LIC, etc.) and LiDAR-visual systems (R3LIVE, FAST-LIVO2, Ground-Fusion(++), etc.); accuracy is measured by EVO-aligned ATE RMSE, and failed runs are marked ✗ and counted into the aggregated error following the paper's annotation convention.
上表数字均逐字摘自论文各表格（Table rmse_tab / time_asynchronous_tab / tab:isaac_lidar_degeneracy / tab:extrinsic_hilti / tab:preinjected_imu_delay_wild01 / lvig）；不是所有场景 Ultra-Fusion 都拿第一——例如 M3DGR 的 Grass01 / GNSS-denial01 序列上，纯 GNSS SPP 基线仍以 0.48m / 11.61m 优于 Ultra-Fusion(WIO) 的 20.65m / 失败(✗)；GrandTour 的 ARC-2 序列上 FAST-LIVO2（0.70cm）略优于 Ultra-Fusion(LVIO)（0.90cm）。All numbers in the table above are quoted verbatim from the tables of the paper (Table rmse_tab / time_asynchronous_tab / tab:isaac_lidar_degeneracy / tab:extrinsic_hilti / tab:preinjected_imu_delay_wild01 / lvig); Ultra-Fusion does not come first in every scenario — on the Grass01 / GNSS-denial01 sequences of M3DGR, for instance, the pure GNSS SPP baseline still beats Ultra-Fusion(WIO) with 0.48m / 11.61m against 20.65m / failure (✗); on the ARC-2 sequence of GrandTour, FAST-LIVO2 (0.70cm) is slightly better than Ultra-Fusion(LVIO) (0.90cm).
典型传感器退化场景下的估计轨迹（蓝）与彩色点云地图，红色标记为首尾重合点：(a) M3DGR Corridor01——长走廊 LiDAR 几何退化；(b) M3DGR GNSS-denial01——长时间 GNSS 中断叠加局部视觉退化；(c) GrandTour ARC-2——四足爬楼梯，机体振荡叠加视觉/LiDAR 同时退化；(d) M3DGR Z-Rough-Road01——密集植被中的长距离导航，视觉歧义性强。Estimated trajectories (blue) and colored point-cloud maps in representative sensor-degradation scenarios, with the red marker denoting the coincident start/end point: (a) M3DGR Corridor01 — LiDAR geometric degeneracy in a long corridor; (b) M3DGR GNSS-denial01 — a long GNSS outage combined with local visual degradation; (c) GrandTour ARC-2 — a quadruped climbing stairs, body oscillation combined with simultaneous visual/LiDAR degradation; (d) M3DGR Z-Rough-Road01 — long-distance navigation in dense vegetation with strong visual ambiguity.
Ablations
FRS 消融（下图）显示：开启 FRS 后 LiDAR 平均 ATE 降低 0.45m（75.3%），视觉降低 1.60m（36.2%），轮速降低 1.56m（41.3%）；GNSS 门控在 Dark01 上几乎无影响（0.0628m），在 Grass01 上从 0.618m 降到 0.539m，在 GNSS-denial01 上从 2.77m 降到 1.79m——说明它能有选择地拒绝不可靠的卫星更新，而不是无差别丢弃。轮速方面，Ultra-Fusion (WIO) 把平均 ATE 从原始轮速里程计的 35.6m、Ground-Fusion WIO 的 33.68m 降到 26.99m。长时序列上（Longtime01/02，均超过 30 分钟）Ultra-Fusion (LVWIO) 取得最低误差 4.3m / 2.8m，对比 FAST-LIVO2 的 5.13m / 8.4m 与 Ground-Fusion++ 的 7.5m / 15.9m。运行时方面，在 Intel i9-14900K 上单次优化步耗时 5.48–10.73ms，满足实时要求。The FRS ablation (figure below) shows that enabling FRS lowers the average ATE by 0.45m (75.3%) for LiDAR, by 1.60m (36.2%) for vision and by 1.56m (41.3%) for wheel odometry; GNSS gating has almost no effect on Dark01 (0.0628m), takes the error from 0.618m down to 0.539m on Grass01 and from 2.77m down to 1.79m on GNSS-denial01 — showing that it selectively rejects unreliable satellite updates instead of discarding them indiscriminately. For wheel odometry, Ultra-Fusion (WIO) brings the average ATE down from 35.6m for the raw wheel odometry and 33.68m for Ground-Fusion WIO to 26.99m. On the long-duration sequences (Longtime01/02, both over 30 minutes) Ultra-Fusion (LVWIO) attains the lowest errors, 4.3m / 2.8m, against 5.13m / 8.4m for FAST-LIVO2 and 7.5m / 15.9m for Ground-Fusion++. As for runtime, a single optimization step takes 5.48–10.73ms on an Intel i9-14900K, which meets real-time requirements.
Factor-Wise Reliability Scheduling（FRS）在 M3DGR 上的消融：分别在保留对应传感器流的情况下比较开启/关闭 FRS 各子模块的效果（LiDAR FRS 在 LVWIO 下评估，视觉/轮速 FRS 在 VWIO 下评估，避免被强 LiDAR 约束掩盖）。Ablation of Factor-Wise Reliability Scheduling (FRS) on M3DGR: the effect of enabling / disabling each FRS submodule is compared while the corresponding sensor stream is retained (LiDAR FRS is evaluated under LVWIO, visual/wheel FRS under VWIO, so as not to be masked by strong LiDAR constraints).
04 Limitations
Note：下面第一条为论文 Conclusion and Limitations 一节中的原文陈述（stated）；其余两条基于论文的设计范围与实验设置推断（inferred），供参考。Note: the first item below is a verbatim statement from the Conclusion and Limitations section of the paper (stated); the other two are inferred from the paper's design scope and experimental setup (inferred), given here for reference.
仅覆盖定位与几何建图，未处理语义/动态场景（论文明确指出）Covers only localization and geometric mapping; semantic and dynamic scenes are not handled (explicitly stated in the paper)
原文："the study focuses on localization and geometric mapping; semantic scene understanding and explicit dynamic-object reasoning remain outside the evaluated scope." 作者认为把可靠性调度框架扩展到语义与动态环境是未来 ITS 部署的重要方向。Quoted from the paper: "the study focuses on localization and geometric mapping; semantic scene understanding and explicit dynamic-object reasoning remain outside the evaluated scope." The authors see extending the reliability-scheduling framework to semantic and dynamic environments as an important direction for future ITS deployment.
源码与数据集尚未开放（inferred）Source code and datasets are not released yet (inferred)
摘要与正文多处提到"we will release source code and datasets upon paper acceptance"，即论文发表时尚处于接收前状态，复现依赖后续开源计划，当前无法独立验证实现细节。The abstract and the main text state in several places that "we will release source code and datasets upon paper acceptance", i.e. the work was still pre-acceptance, so reproduction depends on the promised release and the implementation details cannot be verified independently for now.
部分场景下仍非全面最优（inferred，来自实验数据本身）Still not uniformly optimal in some scenarios (inferred, from the experimental data itself)
如实验部分数据所示，纯 GNSS SPP 基线在 Grass01/GNSS-denial01 上数值优于 Ultra-Fusion(WIO) 配置，GrandTour ARC-2 上 FAST-LIVO2 略优于 Ultra-Fusion(LVIO)；作者用"average rank 比单点最优更有信息量"来解释这类跨场景权衡，但也说明该框架并非在所有子场景都是绝对最优。As part of the experimental data shows, the pure GNSS SPP baseline is numerically better than the Ultra-Fusion(WIO) configuration on Grass01/GNSS-denial01, and FAST-LIVO2 is slightly better than Ultra-Fusion(LVIO) on GrandTour ARC-2; the authors explain such cross-scenario trade-offs with the argument that "average rank is more informative than single-point optima", which also shows that the framework is not absolutely optimal in every sub-scenario.
