【Ultra-Fusion】一套后端通吃轮式/四足/无人机：因子级退化调度+在线时空校准，60+系统对比全SOTA

摘要

多传感器融合 SLAM 方案众多，但几乎都绑定特定传感器组合：VINS 只做视觉惯性，LIO-SAM 只支持激光 IMU，跨平台切换就得重写状态变量和因子图。Ultra-Fusion 提出统一滑动窗口估计器，将激光、视觉、IMU、轮速、GNSS 五类残差放进同一个优化窗口，配合因子级可靠性调度（FRS）和在线时空自校准（OSC），在自动驾驶、仓储机器人、四足配送、无人机巡检四类平台上均完成验证，5 大公开数据集对比 60+ 开源系统全部达到 SOTA。

论文：Ultra-Fusion: A Resilient Tightly-Coupled Multi-Sensor Fusion SLAM Framework under Sensor Degradation and Spatiotemporal Perturbation for Intelligent Transportation Systems

代码：Ultra-Fusion (GitHub)

项目主页 ：ultrafusion-web

数据集 ：M3DGR

一、问题背景：为什么需要"大一统"？

现有多传感器 SLAM 框架面临三大痛点：

痛点	具体表现	代表方案
传感器绑定	方案只支持固定传感器组合，增减一个传感器就要重写	VINS-Mono (V+I), LIO-SAM (L+I), FAST-LIVO (L+V+I)
退化脆弱	隧道 GNSS 丢失、暗光相机失效、狭长走廊激光退化，一碰就崩	多数方案无退化检测或仅前端简单过滤
外参/时序漂移	长时间运行后标定参数偏移，误差持续累积	多数方案依赖离线标定，不支持在线修正

Ultra-Fusion 的核心思路：一套后端兼容所有模态，内置退化调度 + 在线校准，跨平台免改代码。

图 0：Ultra-Fusion 多平台部署总览 -- 一套代码适配轮式、四足、无人机三类移动平台。来源： $Ultra-Fusion$ Fig 1

二、核心方法

2.1 整体框架：统一滑动窗口估计器

图 1：Ultra-Fusion 整体架构 -- 五类传感器观测统一进入滑动窗口，由 FRS 动态调权，OSC 在线校准时空参数。重绘自 design skill

图 3：Ultra-Fusion 完整系统流程图 -- 展示从传感器原始数据到统一估计器的完整数据流。来源： $Ultra-Fusion$ Pipeline Figure

传统方案把激光里程计、视觉里程计当作外部先验塞进优化窗口，本质是二次信息压缩，丢失原始观测的精细约束。Ultra-Fusion 直接将五类原始残差放入同一窗口：

激光：点到平面残差（Slerp 球面插值校正扫描内运动畸变）
视觉：重投影残差
IMU：预积分残差
轮速：轮速预积分残差
GNSS：位置约束

优化目标统一为：

X ∗ = arg ⁡ min ⁡ X { ∥ r p ∥ 2 + ∑ k w k ⋅ ρ ⁣ ( ∥ r k ∥ Σ k 2 ) } \mathcal{X}^* = \arg\min_{\mathcal{X}} \left\{ \|\mathbf{r}p\|^2 + \sum{k} w_k \cdot \rho\!\left(\|\mathbf{r}k\|^2{\Sigma_k}\right) \right\} X∗=argXmin{∥rp∥2+k∑wk⋅ρ(∥rk∥Σk2)}

其中 X \mathcal{X} X 为窗口内所有状态（位姿、速度、IMU 零偏）， r p \mathbf{r}_p rp 为边缘化先验残差， r k \mathbf{r}_k rk 为各传感器残差， w k w_k wk 由 FRS 动态分配， ρ ( ⋅ ) \rho(\cdot) ρ(⋅) 为鲁棒核函数。

无论启用 WIO、VIO、LIO 还是 LVWIO，优化目标形式完全一致，仅增减残差项。单帧优化耗时 5.48-10.73ms（i9-14900K），满足实时。

2.2 可观测性自适应初始化

传统 SLAM 初始化固定流程：静止等待重力对齐或依赖充足视觉特征，在无光/无几何场景下初始化动辄数秒。

Ultra-Fusion 设计四分支启动逻辑，系统自动选择：
#mermaid-svg-gNKPkattRfzO8Azr{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-gNKPkattRfzO8Azr .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-gNKPkattRfzO8Azr .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-gNKPkattRfzO8Azr .error-icon{fill:#552222;}#mermaid-svg-gNKPkattRfzO8Azr .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-gNKPkattRfzO8Azr .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-gNKPkattRfzO8Azr .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-gNKPkattRfzO8Azr .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-gNKPkattRfzO8Azr .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-gNKPkattRfzO8Azr .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-gNKPkattRfzO8Azr .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-gNKPkattRfzO8Azr .marker{fill:#333333;stroke:#333333;}#mermaid-svg-gNKPkattRfzO8Azr .marker.cross{stroke:#333333;}#mermaid-svg-gNKPkattRfzO8Azr svg{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-gNKPkattRfzO8Azr p{margin:0;}#mermaid-svg-gNKPkattRfzO8Azr .label{font-family:"trebuchet ms",verdana,arial,sans-serif;color:#333;}#mermaid-svg-gNKPkattRfzO8Azr .cluster-label text{fill:#333;}#mermaid-svg-gNKPkattRfzO8Azr .cluster-label span{color:#333;}#mermaid-svg-gNKPkattRfzO8Azr .cluster-label span p{background-color:transparent;}#mermaid-svg-gNKPkattRfzO8Azr .label text,#mermaid-svg-gNKPkattRfzO8Azr span{fill:#333;color:#333;}#mermaid-svg-gNKPkattRfzO8Azr .node rect,#mermaid-svg-gNKPkattRfzO8Azr .node circle,#mermaid-svg-gNKPkattRfzO8Azr .node ellipse,#mermaid-svg-gNKPkattRfzO8Azr .node polygon,#mermaid-svg-gNKPkattRfzO8Azr .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-gNKPkattRfzO8Azr .rough-node .label text,#mermaid-svg-gNKPkattRfzO8Azr .node .label text,#mermaid-svg-gNKPkattRfzO8Azr .image-shape .label,#mermaid-svg-gNKPkattRfzO8Azr .icon-shape .label{text-anchor:middle;}#mermaid-svg-gNKPkattRfzO8Azr .node .katex path{fill:#000;stroke:#000;stroke-width:1px;}#mermaid-svg-gNKPkattRfzO8Azr .rough-node .label,#mermaid-svg-gNKPkattRfzO8Azr .node .label,#mermaid-svg-gNKPkattRfzO8Azr .image-shape .label,#mermaid-svg-gNKPkattRfzO8Azr .icon-shape .label{text-align:center;}#mermaid-svg-gNKPkattRfzO8Azr .node.clickable{cursor:pointer;}#mermaid-svg-gNKPkattRfzO8Azr .root .anchor path{fill:#333333!important;stroke-width:0;stroke:#333333;}#mermaid-svg-gNKPkattRfzO8Azr .arrowheadPath{fill:#333333;}#mermaid-svg-gNKPkattRfzO8Azr .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-gNKPkattRfzO8Azr .flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-gNKPkattRfzO8Azr .edgeLabel{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-gNKPkattRfzO8Azr .edgeLabel p{background-color:rgba(232,232,232, 0.8);}#mermaid-svg-gNKPkattRfzO8Azr .edgeLabel rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-gNKPkattRfzO8Azr .labelBkg{background-color:rgba(232, 232, 232, 0.5);}#mermaid-svg-gNKPkattRfzO8Azr .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-gNKPkattRfzO8Azr .cluster text{fill:#333;}#mermaid-svg-gNKPkattRfzO8Azr .cluster span{color:#333;}#mermaid-svg-gNKPkattRfzO8Azr div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-gNKPkattRfzO8Azr .flowchartTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-gNKPkattRfzO8Azr rect.text{fill:none;stroke-width:0;}#mermaid-svg-gNKPkattRfzO8Azr .icon-shape,#mermaid-svg-gNKPkattRfzO8Azr .image-shape{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-gNKPkattRfzO8Azr .icon-shape p,#mermaid-svg-gNKPkattRfzO8Azr .image-shape p{background-color:rgba(232,232,232, 0.8);padding:2px;}#mermaid-svg-gNKPkattRfzO8Azr .icon-shape .label rect,#mermaid-svg-gNKPkattRfzO8Azr .image-shape .label rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-gNKPkattRfzO8Azr .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-gNKPkattRfzO8Azr .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-gNKPkattRfzO8Azr :root{--mermaid-font-family:"trebuchet ms",verdana,arial,sans-serif;} 是
否
是
否
是
否
是
否
传感器数据流入
运动激励充足?
视觉特征充足?
有轮速/IMU?
动态视觉惯性初始化

SfM对齐
激光有效点充足?
静止/轮速辅助初始化
延迟初始化

缓存数据等待
激光辅助MAP初始化

实验数据（18 组跨数据集序列）：

指标	Ultra-Fusion	FAST-LIVO2	Ground-Fusion++
平均初始化延迟	0.153s	4.642s	--
20s 短时轨迹误差	0.483m	--	--

关闭自适应模块后延迟飙升至 4.6s，误差扩大数十倍。

2.3 因子级可靠性调度（FRS）

图 2：FRS 因子级退化调度流程 -- 每类传感器独立计算退化分数，动态调整残差权重 w_k，退化严重时权重趋零。重点看各传感器的评分指标差异和消融收益。重绘自 design skill

这是论文最核心的创新。区别于传统前端简单过滤，FRS 在后端层面为五类传感器分别设计退化评分，实时调权：

传感器	退化打分方式	退化场景
激光	Hessian 矩阵特征值 → 几何丰富度	狭长走廊、单一平面
视觉	有效特征数 + 空间分布均匀度 + 重投影误差	暗光、遮挡
轮速	IMU 推算 vs 轮速增量差值	草地打滑、车轮悬空
GNSS	卫星数量 + 定位精度 + 残差	隧道、城市峡谷
IMU	零偏漂移监测	长时间静止

消融实验收益：

FRS 模块	退化场景误差降低
激光 FRS	-75.3%（-0.45m ATE）
视觉 FRS	-36.2%（-1.60m ATE）
轮速 FRS	-41.3%（-1.56m ATE）

2.4 在线时空自校准（OSC）

多数在线校准方案的问题：弱激励环境下持续更新参数，反而引入更大偏差。Ultra-Fusion 拆分两条独立校准线程，设置双重准入门槛：

时序校准：互相关匹配 IMU 角速度与激光运动曲线，搜索最优时间偏移。

最大支持 ± 500 ms \pm 500\text{ms} ±500ms 偏差修正
人为注入 ± 200 ms \pm 200\text{ms} ±200ms 延迟：基线方案误差米级，Ultra-Fusion 维持分米级
Wild01 序列（ ± 300 ms \pm 300\text{ms} ±300ms 偏移）：0.0403m RMSE（基线 >15m）

旋转外参校准：SVD 闭式求解旋转矩阵，多轴运动不足时自动锁定外参。

注入 10 度旋转误差：HILTI22 上 0.25m（FAST-LIVO2: 940m）

图 5：仿真场景中不同时序偏移注入下的预测值分布 -- 验证 OSC 模块的时序校准精度。来源： $Ultra-Fusion$ Temporal Offsets

校准更新时同步修正全局地图位姿，不产生地图断层。

三、实验分析

3.1 五大 Benchmark 全 SOTA

数据集	场景	Ultra-Fusion	最强对手
M3DGR（10 序列）	室内外混合	0.15m ATE（rank 1.4）	--
M2DGR-Plus	校园/城市	0.59% 漂移率, 0.24m RMSE	FAST-LIVO2: 2.32%, 1.48m
KAIST（96.9km/h 高速）	城市自动驾驶	0.38% 漂移率	FAST-LIVO2: ~30%
GrandTour	四足机器人	0.26-0.90cm RTE（4 序列中 3 个第一）	--
MARS-LVIG	无人机	1.47m RMSE（rank 1.5）	--

图 4：四类典型传感器退化场景轨迹对比 -- 暗光、走廊、打滑、GNSS 失锁下 Ultra-Fusion vs 基线方案。重点看退化区段的轨迹偏移。来源： $Ultra-Fusion$ Degradation Grid

3.2 跨平台通用性

平台	传感器配置	验证场景
城市自动驾驶	L+V+I+W+G	KAIST 高速序列（最高 96.9km/h）
园区仓储机器人	L+V+I+W	室内外穿越
四足配送机器人	L+V+I	楼梯、颠簸地形（GrandTour）
低空巡检无人机	L+V+I	机场远距离（MARS-LVIG）

同类方案极少同时覆盖地面、足式、空中三类平台。

3.3 关键场景对比

高速场景（KAIST 96.9km/h）：轮速约束让漂移率从 30% 降至 0.37%
激光退化走廊：主流 LIO 误差数米，Ultra-Fusion 靠视觉/IMU 兜底控制在 0.3m 以内
30min+ 长时巡航：LVWIO 平均 2.8m（FAST-LIVO2: 8.4m），长期漂移抑制优势明显

四、M3DGR 基准数据集

Ultra-Fusion 团队扩展了 M3DGR 数据集：

37 组真实 + 仿真序列（总量 300GB+）
覆盖暗光、遮挡、狭长几何、车轮打滑、GNSS 失锁全部典型退化场景
仿真序列提供精准真值，可人为控制时序/外参扰动
填补了业内缺乏统一退化评测基准的空白

小结

Ultra-Fusion 真正做到了"一套代码通吃所有平台"，四大模块各有明确工程价值：

统一滑动窗口 -- 消除多子系统信息损耗，增减传感器只改残差项
自适应初始化 -- 0.153s 冷启动，比 FAST-LIVO2 快 30 倍
FRS 退化调度 -- 后端级退化处理，激光退化误差降 75%
OSC 在线校准 -- 容忍 +/-500ms 时序偏差和 10 度外参误差

客观局限：

无语义分割/动态目标剔除，纯静态几何假设
无回环检测模块，超长场景仍有累积漂移
所有测试基于离线数据集，缺少大规模实车路测
代码尚未正式开源（承诺 accepted 后公开）

个人判断：这是 2026 年多传感器融合 SLAM 领域最完整的系统级工作。FRS 因子级退化调度的思路值得所有做多模态融合的团队借鉴 -- 传感器退化不该只在前端做，后端加权才能真正利用冗余信息。但缺少回环和语义仍是硬伤，实际部署需要叠加额外模块。