多传感器融合的目标，并不是简单地将多个传感器的数据拼接在一起，而是在统一的时间基准、空间坐标系和概率模型下，综合不同传感器的互补信息，对机器人状态、环境结构或目标属性进行更加准确、连续和鲁棒的估计。

在机器人系统中，
IMU具有高频、低延迟的优势，但积分误差会随时间累积；
相机能够提供丰富的纹理和语义信息，但容易受光照、运动模糊和纹理缺失影响；
激光雷达能够准确测量几何距离，但在结构退化环境中可能缺少约束；
GNSS不存在局部积分漂移，但频率低、易受遮挡和多路径效应影响；
编码器能够准确反映机器人内部运动，却无法直接观测外部环境误差。

多传感器融合正是利用这些传感器的互补性，使一个传感器的优势弥补另一个传感器的不足。

本文从以下几个维度系统介绍多传感器融合：

• 数据级、特征级和决策级融合；
• 松耦合、紧耦合与深耦合；
• 集中式、分布式和联邦式架构；
• KF、EKF、ESKF、UKF和粒子滤波；
• 非线性优化、滑动窗口和因子图；
• 时间同步、外参标定和数据关联；
• 可观测性、估计一致性和异常观测处理；
• ROS 2、GTSAM、Ceres、OpenVINS、LIO-SAM和FAST-LIO2；

---

1. 多传感器融合

多传感器融合可以定义为：

利用来自多个传感器、多个时间点或多个信息源的观测，在不确定性条件下对目标状态或环境状态进行联合推断。

这里有三个关键词。

1.1 多源信息

参与融合的信息不一定全部来自物理传感器，也包括各种其他渠道的信息：

• 相机、激光雷达、毫米波雷达；
• IMU、轮速计、关节编码器；
• GNSS、UWB、全站仪；
• 地图、CAD模型、先验轨迹；
• 机器人运动学模型；
• 神经网络检测结果；
• 人工规则和工艺先验。

因此，严格来说，多传感器融合往往也是多源信息融合。在工业场景中，几何先验（如工件CAD模型）、工艺先验（如焊缝标准形状）是极强的约束信息，合理利用这类先验能够大幅降低感知难度，提升系统鲁棒性。

1.2 不确定性

任何传感器观测都不是绝对准确的。传感器数据中通常包含：

• 随机噪声；
• 固定偏差；
• 缓慢变化的零偏；
• 比例因子误差；
• 时间延迟；
• 外参误差；
• 环境干扰；
• 错误匹配和离群点。

融合算法需要回答的是：在已知传感器存在这些误差的情况下，系统状态最可能是什么

从工程视角看，融合系统的本质是对不确定性的量化与传递——每一份观测都携带其置信度，最终的估计结果是所有信息在概率意义上的最优折中。

1.3 联合推断

一个合理的融合系统应该根据：

• 传感器噪声；
• 当前环境；
• 观测残差；
• 状态可观测性；
• 传感器健康状态；

动态决定某项观测应当对最终结果产生多大影响。

自适应权重调整是高阶融合系统的核心能力之一：当传感器进入退化环境时，系统应自动降低其权重，甚至暂时隔离该传感器，而非固定权重运行。

---

2. 多传感器融合的概率本质

多传感器融合的核心可以用贝叶斯估计描述。

设：

• $x_k$ 为时刻 $k$ 的系统状态；
• $z_k$ 为时刻 $k$ 的传感器观测；
• $u_k$ 为控制输入或IMU测量；
• $z_{1:k}$ 表示从初始时刻到当前时刻的全部观测。

目标是计算后验概率：

$$p(x_k\mid z_{1:k},u_{1:k})$$

贝叶斯滤波通常包含两个步骤。

2.1 预测

根据运动模型，由上一时刻状态预测当前状态：

$$p(x_k\mid z_{1:k-1})=\int p(x_k\mid x_{k-1},u_k)p(x_{k-1}\mid z_{1:k-1})d{x}_{k-1}$$

预测阶段回答的是：根据机器人原来的状态和运动输入，现在机器人的位置

预测步骤仅依赖系统运动模型和历史状态，不依赖外部观测，因此可以高频运行，维持状态的连续性。

2.2 更新

利用当前传感器观测修正预测：

$$p(x_k\mid z_{1:k})\propto p(z_k\mid x_k)p(x_k\mid z_{1:k-1})$$

更新阶段回答的是：当前传感器的实际观测，它与预测之间相差多少

卡尔曼滤波、扩展卡尔曼滤波、粒子滤波和因子图优化，本质上都是在不同假设和不同计算形式下近似求解这个后验分布。其中卡尔曼滤波是线性高斯假设下的解析解，粒子滤波是非线性非高斯场景下的数值近似解，因子图优化则是批量最大后验估计的图式表达。

---

3. 多传感器融合的三条分类轴

多传感器融合经常出现“数据级”“紧耦合”“集中式”等术语。它们不是同一个维度，而是三条相互独立的分类轴。

3.1 按信息抽象层级分类

包括：

• 数据级融合；
• 特征级融合；
• 决策级融合。

3.2 按传感器耦合程度分类

包括：

• 松耦合；
• 紧耦合；
• 更深层的联合建模。

3.3 按系统拓扑分类

包括：

• 集中式；
• 分布式；
• 联邦式；
• 混合式。

例如，一个系统完全可能同时属于：

特征级、紧耦合、集中式融合系统。

也可能属于：

决策级、松耦合、分布式融合系统。

因此，不能简单地把“特征级”等同于“紧耦合”，也不能把“松耦合”等同于“卡尔曼滤波”。工程中大量的方案争议，本质上是混淆了不同分类维度的概念。选择融合方案时，应分别从三个维度独立决策，而非绑定某一种固定组合。

---

4. 数据级、特征级与决策级融合

4.1 数据级融合

数据级融合直接处理原始或接近原始的传感器数据。

例如：

• 多相机原始图像拼接；
• 多激光雷达点云拼接；
• RGB图像与深度图对齐；
• 将激光雷达点投影到图像平面；
• 将多个雷达回波联合处理。

典型流程为：

Camera Image ─┐
              ├── 时间和空间对齐 ── 原始数据融合 ── 感知算法
LiDAR Points ─┘

优点

• 原始信息保留最完整；
• 后端算法可以自行学习或选择有效信息；
• 理论上具有较高的信息上限。

缺点

• 数据量大；
• 时间同步和外参标定要求高；
• 不同模态的数据表示差异明显；
• 任意一个传感器的异常都可能污染融合输入。

需要注意，所谓“原始数据融合”通常也不是完全未经处理。例如，相机图像可能已经经过ISP处理，激光点云可能已经完成距离解算，IMU数据也可能经过内部温度补偿。

在深度学习端到端融合的趋势下，数据级融合重新受到关注——通过神经网络直接学习跨模态的原始数据关联，避免人工特征设计带来的信息损失，但对标定精度和同步精度的要求也更高。

4.2 特征级融合

特征级融合先从各传感器提取有意义的中间表示，再进行联合估计。

例如：

• 相机提取角点和描述子；
• 激光雷达提取边缘、平面或局部几何；
• IMU生成预积分约束；
• 雷达生成目标距离和径向速度；
• 神经网络生成语义特征。

Camera ── 视觉特征 ─┐
                     │
LiDAR ── 几何特征 ──┼── 联合状态估计或特征融合
                     │
IMU ─── 预积分约束 ─┘

特征级融合是机器人定位、SLAM和多模态感知中非常常见的形式。在深度学习多模态感知中，中间特征融合也是当前的主流方案——各模态先通过独立编码器提取特征，再在统一特征空间内进行交互，兼顾了信息保留度与计算效率。

优点

• 数据规模明显小于原始数据；
• 可以保留关键几何或语义信息；
• 不同传感器更容易转换为统一的残差或特征表示。

缺点

• 特征提取会丢失部分原始信息；
• 特征错误会直接影响后端；
• 在弱纹理、重复结构、强反光等环境中可能退化。

4.3 决策级融合

决策级融合让各传感器独立完成检测、定位或分类，然后融合最终结果。

例如：

Camera Detector ── 车辆，置信度0.82 ─┐
                                      ├── 决策融合
Radar Detector ─── 车辆，置信度0.91 ─┘

常见方法包括：

• 加权投票；
• 贝叶斯决策；
• 置信度加权；
• Dempster–Shafer证据理论；
• 规则系统；
• 轨迹级融合；
• 故障仲裁。

优点

• 模块边界清晰；
• 单个传感器容易独立测试；
• 便于替换已有检测器；
• 易于进行故障隔离。

缺点

• 低层信息已经丢失；
• 错误决策往往难以被其他传感器修复；
• 多个子系统可能重复处理相同信息。

决策级融合是工业系统快速集成的首选方案，其模块化特性与故障隔离能力，非常适合高可靠性要求的工业场景。

---

5. 松耦合与紧耦合

5.1 松耦合

在松耦合系统中，每个传感器或子系统先独立估计状态，再由上层融合器融合这些状态结果。

例如：

GNSS ───────────── 位置 ─┐
                          │
LiDAR SLAM ──────── 位姿 ─┼── EKF或图优化 ── 最终位姿
                          │
Visual Odometry ── 位姿 ─┘

输入上层融合器的通常是：

• 位置；
• 姿态；
• 速度；
• 里程计；
• 目标框；
• 局部轨迹。

优点

• 系统模块化程度高；
• 接口简单；
• 易于快速集成已有算法；
• 某个子模块可以独立开发和替换。

局限

• 原始观测的信息被压缩；
• 不同子系统可能重复使用同一传感器；
• 子系统之间的相关性难以正确建模；
• 一旦某个子定位系统完全失效，其原始传感器信息也无法继续利用。

例如，将“视觉里程计位姿”和“激光里程计位姿”直接放入EKF，并不等于真正利用了图像特征和激光点云之间的全部约束。

5.2 紧耦合

紧耦合系统直接在同一个估计器中处理各传感器的原始观测或中间测量。

视觉惯性融合中可以联合使用：

• IMU传播模型；
• 视觉重投影误差；
• 相机与IMU外参；
• IMU零偏；
• 相机和IMU时间偏移。

激光惯性融合中可以联合使用：

• IMU运动传播；
• 点到平面残差；
• 点到线残差；
• 激光雷达与IMU外参；
• 点云运动畸变模型。

优点

• 信息利用率更高；
• 可以联合估计零偏、外参和时间差；
• 某个传感器部分退化时，其他观测仍可能提供约束；
• 通常能获得更高精度和更好一致性。

局限

• 系统复杂；
• 对时间同步和标定要求更高；
• 状态维度和计算量增加；
• 调试时难以快速定位问题来源；
• 错误观测可能直接污染整个估计器。

5.3 深耦合：信号层面的底层融合

在松耦合与紧耦合之外，还存在更深层级的深耦合（Deep Coupling），典型应用于GNSS/INS组合导航领域。

• 松耦合：融合GNSS解算后的位置、速度结果；
• 紧耦合：融合GNSS原始伪距、伪距率观测；
• 深耦合：将INS的运动预测信息直接注入GNSS接收机的基带跟踪环路，辅助载波与码相位跟踪。

深耦合能够在卫星信号弱、遮挡严重的场景下，大幅提升GNSS接收机的跟踪灵敏度与抗干扰能力，是最高层级的融合方式。但深耦合需要传感器底层固件与硬件的支持，普通开发者难以实现，常见于工业级、军工级组合导航设备中。

---

6. 集中式、分布式与联邦式融合

6.1 集中式融合

所有传感器数据进入一个中央估计器：

Camera ─┐
LiDAR ──┤
IMU ────┼── 中央融合器 ── 系统状态
GNSS ───┤
Encoder ─┘

集中式架构便于统一建模，并且能够完整维护不同传感器之间的相关性。

适合：

• 单机器人；
• 传感器数量有限；
• 中央计算机算力充足；
• 通信链路可靠。

主要风险是：

• 中央节点成为性能瓶颈；
• 存在单点故障；
• 数据带宽较大；
• 系统扩展性有限。

6.2 分布式融合

分布式系统中，各节点维护局部状态，并交换状态估计、协方差或信息矩阵。

Camera Estimator ─┐
                   │
LiDAR Estimator ──┼── 分布式融合
                   │
Robot Estimator ──┘

典型场景包括：

• 多机器人协同定位；
• 无人机集群；
• 车路协同；
• 大规模无线传感器网络。

分布式融合最困难的问题不是通信，而是：如何处理不同节点估计之间未知或重复的信息相关性。

如果两个节点都融合过同一个GNSS结果，然后再次直接融合两个节点的状态，就可能对同一份信息重复计数，导致协方差过小、估计过度自信。

当交叉相关性未知时，可以使用协方差交叉（Covariance Intersection, CI）等保守融合方法。在此基础上，还发展出广义协方差交叉（GCI）、逆协方差交叉（ICI）等衍生方法，适用于不同相关性假设的场景。

6.3 联邦式融合

联邦滤波一般由多个局部滤波器和一个主滤波器组成。

IMU + GNSS ─── 局部滤波器1 ─┐
                           │
IMU + 轮速计 ─ 局部滤波器2 ──┼── 主滤波器
                           │
IMU + LiDAR ── 局部滤波器3 ─┘

每个局部滤波器可以独立运行，主滤波器融合局部结果，并进行信息分配或重置。

联邦滤波由Carlson于1988年提出，核心是信息分配原则：将系统总信息按比例分配给各局部滤波器与主滤波器，融合后再进行信息反馈与重置，既保证全局最优性，又具备故障隔离能力。

这种结构特别适合：故障容错导航、航空航天系统、需要传感器隔离的高可靠系统、并行计算平台。

但联邦架构必须认真处理公共信息重复使用问题，尤其是多个局部滤波器共享同一套IMU时。

---

7. 传感器模型：融合之前必须理解传感器

融合算法并不能自动消除所有传感器缺陷。一个可靠系统必须先建立合理的传感器模型。
所谓“垃圾进，垃圾出”，传感器模型的精度决定了融合系统的精度上限。

7.1 IMU

IMU通常输出角速度和比力：
$${\omega }_m=\omega +b_g+n_g$$

$$a_m=R^{\top }(a-g)+b_a+n_a$$

其中：

• $b_g$：陀螺仪零偏；
• $b_a$：加速度计零偏；
• $n_g,n_a$：测量噪声；
• $R$：机体与世界坐标系之间的旋转；
• $g$：重力加速度。

IMU的主要特点是：

• 更新频率高；
• 短时间运动连续性好；
• 角速度和加速度积分会产生漂移；
• 零偏和温度变化会显著影响长期精度。

IMU不是直接测量位置，也不是直接测量世界坐标系加速度。加速度计实际测量的是包含重力关系的比力。

高精度IMU建模通常采用Allan方差分析法，定量分离角度随机游走、速率随机游走、零偏不稳定性等多种误差项，为过程噪声矩阵Q提供定量依据，而非凭经验设置。

7.2 编码器和轮速计

编码器可以提供：

• 电机转角；
• 关节位置；
• 轮子转速；
• 机器人里程计。

它们的优势是低延迟和高重复性，但会受到以下影响：

• 轮胎打滑；
• 机械间隙；
• 结构柔性；
• 传动误差；
• 运动学参数误差；

对于机械臂，编码器位姿是由关节角和运动学模型计算得到的，并不等价于末端执行器在真实世界中的绝对位姿。关节柔性、连杆形变、负载变形都会引入额外误差，这也是工业机器人需要外部视觉传感器进行修正的核心原因。

7.3 相机

相机能够观测：纹理、边缘、特征点、目标类别、图像平面运动、语义信息。

但相机容易受到以下影响：光照变化、运动模糊、反光和弧光、重复纹理、遮挡、滚动快门、自动曝光变化

单目相机没有直接尺度测量能力，但与IMU、已知尺寸目标或深度约束结合后可以恢复尺度。

7.4 激光雷达与3D相机

激光雷达和3D相机可以直接提供空间几何信息。

主要误差来源包括：

• 距离噪声；
• 入射角影响；
• 反射率变化；
• 多路径反射；
• 点云运动畸变；
• 扫描模式；
• 有效视场；
• 温度漂移；
• 强光或电弧干扰。

点云并不天然处于同一个时刻。旋转式或扫描式传感器中的不同点通常对应不同采样时间，因此高速运动时必须进行去畸变。去畸变的精度直接依赖IMU的高频运动预测与时间同步精度。

7.5 GNSS、UWB和外部定位系统

这些系统提供全局或半全局位置约束，可用于消除局部里程计的长期漂移。

但其误差通常不是简单的独立白噪声，还可能包括：

• 多路径；
• 非视距；
• 卫星几何变化；
• 基站布局影响；
• 间歇性跳变；
• 系统级偏差。

因此，不能仅凭一个固定标准差描述所有工作环境。工程中通常会结合卫星数、PDOP值、残差大小等指标，动态调整GNSS观测的协方差，实现自适应融合。

---

8. 完整融合系统的工程链路

一个成熟的多传感器融合系统至少包含以下层次：

传感器驱动
    │
    ▼
时间同步与时间戳管理
    │
    ▼
内参、外参和坐标系管理
    │
    ▼
数据预处理与质量评估
    │
    ▼
特征提取和数据关联
    │
    ▼
状态预测与观测更新
    │
    ▼
异常检测和故障隔离
    │
    ▼
局部状态、全局状态和诊断输出

工程中常见的错误是直接从“读取传感器数据”跳到“实现EKF”，而忽略中间的时间、坐标、质量评估和数据关联。

实际上，融合效果差时，问题往往是：

1. 时间戳错误；
2. 坐标变换错误；
3. 外参不准确；
4. 观测关联错误；
5. 协方差设置错误；
6. 传感器存在系统性偏差。

在工业级融合系统中，预处理、质量评估与故障诊断模块的代码量，往往超过核心估计算法本身。80%的工程问题都出在链路，而非估计算法。

---

9. 时间同步：融合系统的基础

9.1 时间误差会转化为空间误差

设机器人线速度为 $v$，传感器时间偏差为 $\Delta t$，则仅由时间偏差引起的位置误差约为：

$$\Delta p\approx v\Delta t$$

如果机器人速度为 $1\text{m/s}$，时间偏差为 $20\text{ms}$：

$$\Delta p=1\times 0.02=0.02\text{m}$$

也就是2厘米。

对于旋转运动：

$$\Delta \theta \approx \omega \Delta t$$

如果角速度为 $90^{\circ }/\text{s}$，时间偏差为20毫秒：

$$\Delta \theta =90\times 0.02=1.8^{\circ }$$

在相机投影、点云配准和手眼标定中，1.8度的姿态误差可能产生非常明显的空间偏差。

对于高速运动场景（如无人机、高速AGV），时间同步的要求会更加严苛——10ms的时间偏差就可能带来数厘米甚至数十厘米的空间误差，足以让融合系统完全失效。

9.2 必须区分四种时间

同一帧数据可能至少存在以下时间：

1. 物理事件发生时间；
2. 设备内部采样时间；
3. 驱动或SDK接收时间；
4. ROS消息发布时间。

例如相机图像可能具有：

• 触发沿时间；
• 曝光开始时间；
• 曝光结束时间；
• 设备内部帧时间戳；
• 网络包到达时间；
• SDK回调时间；
• ROS消息发布时刻。

进行运动补偿时，应使用与物理采样事件最接近的时间，而不是ROS回调执行时刻。

工程调试中最常见的错误，就是用消息接收时间代替物理采样时间，从而引入系统性的延迟误差。

9.3 时间误差的组成

融合系统中应区分：

时钟偏移 Offset

两个时钟在同一时刻显示的时间不同。

频率偏差 Skew

两个时钟的走时速度不同，时间差会逐渐增长。

漂移 Drift

时钟频率误差随温度和时间变化。

抖动 Jitter

采样或传输延迟存在随机波动。

固定延迟 Latency

从物理采样到数据可用之间存在稳定延迟。

固定延迟可以通过标定或状态增广进行补偿，随机抖动则通常需要通过合理缓冲和噪声建模处理。

9.4 硬触发与PTP解决的问题不同

硬件触发主要解决：多个传感器是否在接近同一个物理时刻开始采样。

PTP主要解决：不同设备的时钟是否处于统一时间基准。

高精度多相机系统通常需要同时考虑：

• 统一硬件触发；
• 设备时钟同步；
• 曝光事件时间戳；
• 通道延迟差；
• 触发线路电气特性。

从精度等级来看，普通NTP协议只能达到毫秒级同步，IEEE 1588 PTPv2在硬件时间戳支持下可达到亚微秒级，而硬件硬触发的采样时刻偏差可以控制在微秒级，是精度最高的同步方式。

9.5 Linux中的硬件时间戳

Linux可以通过网卡的PTP Hardware Clock，即PHC，对硬件时钟进行访问。网络数据也可以通过SO_TIMESTAMPING获得软件或硬件时间戳。

典型链路为：

网卡硬件时间戳
       │
       ▼
PHC：/dev/ptpX
       │
       ├── ptp4l：同步网络中的PHC
       │
       └── phc2sys：同步PHC与系统时钟

硬件时间戳能够避免大量内核调度、驱动排队和用户态回调延迟对时间戳的影响。

9.6 ROS 2消息同步不是时钟同步

ROS 2中的message_filters可以根据消息头中的时间戳，对多个话题进行精确或近似配对。
解决的是：已经带有时间戳的消息如何配对。

message_filters并不能修正：设备时钟偏差、错误采样时间戳、驱动延迟、相机曝光延迟、网络硬件时间误差。

因此，ApproximateTimeSynchronizer不能代替PTP或硬触发。它只是时间同步链路的最后一环，而非时间同步的解决方案。

---

10. 空间标定与坐标系管理

10.1 内参与外参

相机内参描述相机内部成像模型，包括：

• 焦距；
• 主点；
• 像素尺度；
• 畸变参数。

外参描述两个传感器坐标系之间的刚体变换：

$$T_A^B=\left[\begin{array}{cc}{R}_A^B& t_A^B\\ 0& 1\end{array}\right]$$

变换方向必须明确定义。例如：

$${}^{A}p={}^A{T}_B{}^{B}p$$

表示将坐标系 $B$ 中的点转换到坐标系 $A$。

实际工程中，大量错误来自：

• 将 $T_A^B$ 和 $T_B^A$ 混淆；
• 左乘和右乘约定不统一；
• 四元数顺序不同；
• 毫米和米单位混用；
• 相机光学坐标系和机器人坐标系定义不同。

外参标定分为离线标定与在线标定：离线标定精度高，但无法应对使用过程中的机械形变、温度漂移；在线标定将外参作为状态量加入估计器实时估计，但需要充分的运动激励，且对系统可观测性有要求。

10.2 手眼标定

工业机器人中经常需要求解相机与机器人末端之间的外参。

经典手眼标定问题写为：

$$AX=XB$$

其中：

• $A$：机器人在两次姿态之间的相对运动；
• $B$：相机观测到的相对运动；
• $X$：待求的相机与末端之间固定变换。

常见配置包括：

• Eye-in-Hand：相机安装在末端；
• Eye-to-Hand：相机固定在机器人外部。

手眼标定结果只是静态几何关系。若支架存在振动、热变形或机械松动，外参仍可能随时间变化。

除了经典的解析解法，基于非线性优化的手眼标定方法可以融合多组运动数据，同时优化外参与标定板位姿，精度更高，是工业场景的主流方案。

10.3 tf2的意义

ROS 2的tf2不仅维护空间坐标关系，还维护坐标变换随时间的历史缓存。

这意味着：对时刻 $t$ 采集的图像或点云，应查询时刻 $t$ 对应的机器人位姿，而不是直接使用当前最新位姿。

eg：

1. 使用图像实际采集时间；
2. 查询或插值该时刻的机器人位姿；
3. 再进行坐标转换。

tf2的时间插值能力是多传感器融合中极其重要的基础设施，正确使用它可以避免大量由时间错位带来的空间误差。

---

11. 噪声、协方差与权重

11.1 过程噪声与测量噪声

状态模型通常写为：
$$x_k=f(x_{k-1},u_k)+w_k$$
观测模型写为：
$$z_k=h(x_k)+v_k$$

其中：

• $w_k\sim \mathcal{N}(0,Q)$：过程噪声；
• $v_k\sim \mathcal{N}(0,R)$：测量噪声。

$Q$表示系统运动模型没有解释的变化，$R$表示传感器观测的不确定性。

11.2 协方差

在卡尔曼滤波中，协方差具有明确的概率含义。

如果人为将某个传感器的 $R$ 设置得非常小，相当于告诉滤波器：这个传感器几乎不会出错。

滤波器会强烈跟随该传感器，即使它实际上存在系统偏差。

反之，若 $R$ 设置过大，传感器观测几乎不会影响结果。

因此，协方差不应仅根据“想更信任哪个传感器”进行主观调整，而应尽可能来自：

• 设备规格；
• 静态采样统计；
• 重复实验；
• Allan方差分析；
• 在线残差统计；
• 随环境变化的噪声模型。

工程中调参的科学方法是：通过观测归一化新息平方（NIS）的统计特性来校准R矩阵。如果NIS均值远大于观测自由度，说明R设置过小；如果NIS均值远小于自由度，说明R设置过大。理想状态下NIS均值应接近观测自由度。

11.3 相关噪声问题

标准卡尔曼滤波常假设不同测量噪声相互独立，但实际系统中可能存在相关性。

例如：

• 两个视觉里程计使用同一相机；
• 多个局部滤波器共享同一IMU；
• 激光和相机都依赖同一个机器人位姿；
• 两个算法都使用同一地图。

如果忽略相关性并重复融合，估计器会过度自信，协方差被不合理地缩小，最终导致估计不一致。这也是分布式融合与联邦滤波中最核心的难点之一。

---

12. 数据关联：融合中的隐含核心

在很多融合系统中，真正困难的部分不是滤波公式，而是判断：当前观测到底对应哪个目标、哪一个地图特征或哪一段焊缝

这就是数据关联问题。错误的数据关联是导致估计器发散的最主要原因之一——其危害远大于漏检，因为漏检只是不更新，而错误关联会主动将状态拉向错误方向。

12.1 常见关联对象

• 图像特征点与历史特征；
• 激光点与局部地图平面；
• 雷达目标与历史轨迹；
• 检测框与已有目标；
• 当前焊缝轮廓与CAD模型；
• 多相机中的同一目标。

激光SLAM中的ICP、NDT等配准算法，本质上也是迭代式的数据关联过程——通过不断寻找点与点、点与面的对应关系，逐步优化位姿。

12.2 马氏距离门限

设观测残差为：
$$r=z-h(\hat{x})$$
残差协方差为：
$$S=HP{H}^{\top }+R$$
归一化创新平方为：
$$d^2=r^{\top }{S}^{-1}r$$
它综合考虑了不同维度的单位和不确定性。可以将 $d^2$ 与卡方分布门限比较，决定是否接受观测。

与直接判断欧氏距离相比，马氏距离能够考虑：

• 状态不确定性；
• 测量不确定性；
• 不同方向上的方差差异。

12.3 常见关联算法

最近邻

选择距离最近或残差最小的观测。
优点是简单快速，缺点是在目标密集或误检较多时容易关联错误。

PDA

概率数据关联不是只保留一个候选观测，而是根据候选观测的关联概率进行加权。

JPDA

联合概率数据关联进一步考虑多个目标和多个观测之间的联合分配。

MHT

多假设跟踪在一段时间内保留多个关联假设，再根据后续观测选择最可能的假设。

学习式关联

利用外观特征、深度网络嵌入和运动信息进行联合关联。

错误关联通常比漏掉一次观测更危险。因为漏掉观测只是暂时不更新，而错误关联会主动把状态拉向错误位置。

---

13. 基于滤波的融合方法

13.1 卡尔曼滤波（KF）

对于线性系统：
$$x_k=F_k{x}_{k-1}+B_k{u}_k+w_k$$
$$z_k=H_k{x}_k+v_k$$
预测为：
$${\hat{x}}_k^{-}=F_k{\hat{x}}_{k-1}^{+}+B_k{u}_k$$
$$P_k^{-}=F_k{P}_{k-1}^{+}{F}_k^{\top }+Q_k$$
更新为：
$$r_k=z_k-H_k{\hat{x}}_k^{-}$$
$$S_k=H_k{P}_k^{-}{H}_k^{\top }+R_k$$
$$K_k=P_k^{-}{H}_k^{\top }{S}_k^{-1}$$
$${\hat{x}}_k^{+}={\hat{x}}_k^{-}+K_k{r}_k$$
协方差可使用Joseph形式更新：

$$P_k^{+}=(I-K_k{H}_k)P_k^{-}(I-K_k{H}_k{)}^{\top }+K_k{R}_k{K}_k^{\top }$$

Joseph形式在数值计算中通常具有更好的对称性和半正定保持能力。

卡尔曼滤波是线性高斯系统下的最优递推估计器，也是所有滤波方法的基础。

13.2 扩展卡尔曼滤波（EKF）

对于非线性系统：
$$x_k=f(x_{k-1},u_k)+w_k$$

$$z_k=h(x_k)+v_k$$

EKF在当前估计附近对 $f$ 和 $h$ 进行一阶线性化：

$$F_k={\frac{\partial f}{\partial x}|}_{\hat{x}}$$

$$H_k={\frac{\partial h}{\partial x}|}_{\hat{x}}$$

EKF的优势是：递推计算、内存开销固定、延迟较低、适合高频实时系统。

主要问题包括：

• 线性化误差；
• 初始状态敏感；
• 雅可比推导复杂；
• 可能出现估计不一致；
• 难以直接重新线性化很久以前的状态。

当非线性较强时，一阶泰勒展开的误差会显著增大，导致滤波精度下降甚至发散。

13.3 误差状态卡尔曼滤波（ESKF）

ESKF并不是一种完全独立于EKF的新概率方法，而是一种特别适合惯性导航和旋转状态的EKF状态表达方式。

ESKF将状态分成：

• 名义状态；
• 小量误差状态。

名义状态可以写为：
$$x=\left[\begin{array}{ccccc}p& v& q& b_a& b_g\end{array}\right]$$

误差状态可以写为：
$$\delta x={\left[\begin{array}{ccccc}\delta p& \delta v& \delta \theta & \delta b_a& \delta b_g\end{array}\right]}^{\top }$$

其中姿态误差使用三维小角度表示，而不是直接对四元数四个分量进行普通加法。

处理流程通常为：

1. 使用IMU传播名义状态；
2. 传播误差状态协方差；
3. 使用外部传感器更新误差状态；
4. 将误差注入名义状态；
5. 将误差状态重置为零；
6. 对协方差执行相应重置变换。

ESKF特别适合：

• GNSS/INS；
• 视觉惯性里程计；
• 激光惯性里程计；
• 无人机姿态和位置估计；
• 高频机器人状态估计。

ESKF的核心优势在于：误差状态始终是小量，线性化精度更高；同时避免了四元数过参数化带来的数值问题，是惯性导航领域的标准工程实现。

13.4 迭代扩展卡尔曼滤波（IEKF）

当观测模型非线性较强时，可以在一次测量更新中多次重新线性化。

这类方法通常称为Iterated EKF。

它通过围绕更新后的状态反复计算观测残差和雅可比，减小一次线性化带来的误差。FAST-LIO系列就是迭代卡尔曼滤波在激光惯性紧耦合中的代表。

13.5 无迹卡尔曼滤波（UKF）

UKF使用一组Sigma点近似状态分布，通过非线性模型传播这些点，再恢复均值和协方差。

优点：

• 不需要显式计算雅可比；
• 对部分强非线性问题可能比普通EKF更准确。

局限：

• 状态维度升高后计算量明显增加；
• 仍然依赖正确的状态模型和噪声模型；
• 不会自动解决可观测性、异常值和时间同步问题。

UKF并不必然优于EKF。对于维度较高、模型结构明确的惯性导航问题，ESKF通常更常见，计算效率也更高。

13.6 粒子滤波

粒子滤波使用带权粒子表示状态后验：
$$p(x_k\mid z_{1:k})\approx \sum _{i=1}^N{w}_k^{(i)}\delta (x_k-x_k^{(i)})$$

它能够处理：

• 强非线性；
• 非高斯噪声；
• 多峰分布；
• 全局定位。

典型应用是二维机器人定位中的AMCL。

主要限制是：

• 粒子数量可能很大；
• 高维状态空间中效率低；
• 容易发生粒子退化；
• 需要设计重采样策略。

13.7 MSCKF与FEJ：滤波框架下的多状态约束

MSCKF（Multi-State Constraint Kalman Filter，多状态约束卡尔曼滤波）是视觉惯性里程计领域的经典滤波框架，也是OpenVINS的核心基础。

传统EKF只维护当前时刻的状态，每个特征只能对当前状态施加一次约束；而MSCKF将多个历史相机位姿增广到状态向量中，当一个路标点被多个相机观测到后，构建多状态约束，一次性更新所有相关的相机状态，随后将路标点边缘化。

MSCKF既保留了滤波的低延迟、固定内存特性，又利用了多帧观测信息，精度显著优于单步EKF。

为了解决多次线性化导致的估计不一致问题，MSCKF引入了首次估计雅可比（First-Estimate Jacobian, FEJ）：对同一个状态的雅可比，始终使用第一次线性化时的状态点计算，而非每次更新都重新计算。FEJ能够保证可观测性矩阵的零空间正确，避免估计器过度自信，是提升滤波一致性的关键技术。

---

14. 基于优化的融合方法

14.1 最大后验估计

在高斯噪声假设下，多传感器融合可以转化为加权非线性最小二乘：

$$X^{\ast }=\arg \underset{X}{\min }\sum _i{r}_i(X{)}^{\top }{\Sigma }_i^{-1}{r}_i(X)$$

其中：

• $X$：所有待估计状态；
• $r_i$：第 $i$ 项观测残差；
• ${\Sigma }_i$：残差协方差。

不同传感器只需要构造不同残差：

$$\underset{X}{\min }\left(\sum r_{\text{IMU}}^2+\sum r_{\text{Camera}}^2+\sum r_{\text{LiDAR}}^2+\sum r_{\text{GNSS}}^2\right)$$

14.2 因子图

因子图将联合概率分布分解为多个局部因子：

$$p(X\mid Z)\propto \prod _i{\varphi }_i(X_i)$$

其中：

• 变量节点表示位姿、速度、偏置、地图点和外参；
• 因子节点表示传感器观测或运动约束。

因子图的优势不是图形本身，而是它能够清晰表达：

• 哪些测量约束哪些状态；
• 哪些状态之间存在稀疏关系；
• 哪些变量可以被消元；
• 如何使用稀疏矩阵结构提高计算效率。

因子图不仅是一种可视化工具，更是一种问题建模范式——它将复杂的联合估计问题拆解为独立的因子模块，便于扩展与维护。

14.3 批量优化、滑动窗口和增量平滑

批量优化

同时优化全部历史状态。
优点是信息利用充分，缺点是计算规模持续增长。

滑动窗口

只保留最近一段时间的状态：

Xk-4 ─ Xk-3 ─ Xk-2 ─ Xk-1 ─ Xk

窗口外状态通过边缘化转换为先验。

这种方法兼顾：1历史信息、2实时计算量、3多次重新线性化能力。

滑动窗口大小是关键参数：窗口越大，历史信息越充分，精度越高，但计算量也越大。VIO系统通常保留10-20个关键帧，LIO系统窗口可以更小。

增量平滑

iSAM2等方法只对新增因子影响到的部分变量进行重新线性化和更新，而不是每次重新执行完整批量优化。

适合：

• 在线SLAM；
• 回环更新；
• 长时间运行的图优化系统。

iSAM2的核心是贝叶斯树（Bayes Tree）数据结构，通过变量消元将因子图转化为贝叶斯网络，实现增量式更新，大幅提升大规模图优化的效率。

14.4 IMU预积分

IMU频率通常远高于相机或激光雷达。如果将每一个IMU采样都作为单独优化变量，问题规模会快速增长。

IMU预积分的思想是：将两个关键帧之间的多次IMU测量压缩成一个相对运动约束。

预积分通常生成：相对旋转、相对速度、相对位置、对零偏的雅可比、预积分噪声协方差。
这样可以在零偏发生小幅变化时进行一阶修正，而不必重新积分全部IMU数据。

IMU预积分是优化式VIO/LIO的核心技术之一，它将高频IMU数据转化为关键帧之间的约束，大幅降低了优化问题的规模。

14.5 边缘化

当状态离开滑动窗口时，可以通过Schur补消除旧变量。
设线性化后的正规方程为：
$$\left[\begin{array}{cc}{H}_{aa}& H_{ab}\\ H_{ba}& H_{bb}\end{array}\right]\left[\begin{array}{c}\delta x_a\\ \delta x_b\end{array}\right]=\left[\begin{array}{c}{b}_a\\ b_b\end{array}\right]$$

消除旧变量 $x_a$ 后得到：
$$\left(H_{bb}-H_{ba}{H}_{aa}^{-1}{H}_{ab}\right)\delta x_b=b_b-H_{ba}{H}_{aa}^{-1}{b}_a$$

这会把旧状态的信息压缩为保留状态的先验。

边缘化需要注意：

• 先验与线性化点相关；
• 不恰当的重复线性化可能破坏一致性；
• 消元可能增加矩阵稠密度（Fill-in问题）；
• 消元顺序会影响计算效率。

边缘化带来的一致性问题是滑动窗口优化的核心难点之一，工程中通常结合FEJ策略来缓解该问题。

14.6 滤波和优化的比较

维度	滤波	滑动窗口/因子图
状态范围	当前状态为主	一段历史状态
计算方式	递推预测与更新	联合非线性优化
内存	通常固定	与窗口或图规模相关
重新线性化	能力有限	可以重复线性化
延迟观测	处理较复杂	在窗口内较自然
回环约束	不方便	非常适合
高频实时性	通常较好	需要控制窗口和稀疏求解
实现复杂度	中等	较高

实际系统经常混合使用：

• 高频IMU使用ESKF传播；
• 激光或视觉执行低频更新；
• 后端因子图进行全局优化；
• 回环模块修正长期漂移。

从本质上看，卡尔曼滤波等价于线性系统的增量式最大后验估计；当滑动窗口大小缩减为1时，优化方法也退化为类似滤波的形式。二者并非对立关系，而是同一概率问题在不同计算范式下的求解路径。

---

15. 鲁棒估计与异常观测处理

高斯噪声模型无法描述所有错误。实际系统中还存在：

• 特征误匹配；
• 点云动态物体；
• GNSS跳点；
• 视觉误检测；
• 焊缝伪边缘；
• 雷达杂波。

15.1 创新门限

使用归一化创新平方：
$$\mathrm{NIS}=r^{\top }{S}^{-1}r$$
如果NIS超过相应自由度下的卡方门限，可以：

• 拒绝该观测；
• 增大测量协方差；
• 降低传感器可信度；
• 进入故障状态。

创新门限是滤波系统中最基础、最常用的异常检测手段。

15.2 鲁棒核函数

非线性优化中可以将普通平方损失替换为：

$$\sum _i\rho \left(|r_i{|}^2\right)$$

常见鲁棒核包括：

• Huber；
• Cauchy；
• Tukey；
• Soft L1。

不同鲁棒核适用于不同离群点场景：Huber核保留小残差的平方特性、大残差转为线性损失，适合离群点较少的场景；Cauchy核下降更平缓，适配中等离群点比例；Tukey核对超过阈值的残差权重降为0，相当于硬截断，适合离群点比例较高的场景。

鲁棒核可以降低大残差对优化结果的影响，但它不能代替正确的数据关联。

如果观测与错误目标建立了系统性关联，即使使用鲁棒核，也可能得到错误但残差较小的解。当离群点比例过高时，还需要结合RANSAC等随机采样一致性方法，先剔除大部分离群点，再进行优化。

---

16. 可观测性与估计一致性

16.1 可观测性

可观测性描述：根据现有输入和观测，系统状态是否能够被唯一确定。

如果一个状态方向不可观，那么无论增加多少滤波代码，都无法从现有传感器中恢复它。

例如，纯局部视觉惯性系统在没有全局测量时，通常无法确定：

• 全局绝对位置；
• 绕重力方向的全局偏航基准。

它可以获得连续、局部一致的运动，但整体坐标原点和全局朝向仍然是人为选择的。

可观测性分析是设计融合系统的前提——先明确哪些状态可观、哪些不可观，再合理设计状态量与观测模型，避免做无用功。

16.2 退化运动

即使理论上某些参数可观，在特定运动条件下也可能暂时不可观。

例如：

• 机器人长期静止；
• 只进行匀速直线运动；
• 长期绕单一轴旋转；
• 相机和IMU缺少充分六自由度激励；
• 激光雷达只看到单一平面；
• 相机始终观察远处近似平面目标。

在线估计外参或时间偏移时，必须保证运动能够对这些参数形成足够激励。工程中通常会设计特定的标定运动轨迹，保证所有待估参数都能被充分激励。

16.3 一致性

一致性描述估计器给出的不确定性是否与实际误差相符。
如果实际误差很大，但滤波器协方差非常小，系统就是过度自信、不一致的。

造成不一致的常见原因包括：

• 错误线性化；
• 不可观方向被错误地当成可观；
• 重复融合相关信息；
• 协方差设置过小；
• 忽略外参和时间误差；
• 错误的数据关联。

一致性测试可以使用：

• NEES；
• NIS；
• Monte Carlo仿真；
• 真实轨迹误差与协方差包络比较。

估计一致性是比精度更重要的可靠性指标——精度差只是不准，一致性差会让系统“不知道自己不准”，从而做出错误的决策，引发安全问题。

---

17. 深度学习多模态融合

深度学习融合主要用于感知任务，例如：

• 目标检测；
• 语义分割；
• 可行驶区域识别；
• BEV地图构建；
• 多目标跟踪。

17.1 前融合

在网络输入端融合不同模态：

Image + Point Cloud + Radar → 统一网络

优点是保留低层信息，缺点是不同数据表示差异大，对标定和同步高度敏感。

17.2 中间融合

各模态先独立编码，再融合特征：

Image ── Image Encoder ─┐
                         ├── Fusion ── Detection
LiDAR ── Point Encoder ─┘

这是当前多模态感知中非常常见的形式。

PointPainting将图像语义分割结果投影到激光点上，将语义类别概率附加到点云特征中，再交给点云检测器处理。

BEVFusion则将相机和激光雷达特征转换到统一的鸟瞰图表示空间后进行融合，使几何信息和语义信息能够在共同空间内交互。

17.3 后融合

各模态独立检测，再融合目标框、类别和轨迹。

这种方式便于复用现有单模态模型，但其信息交互发生得较晚。

17.4 深度融合系统的特殊问题

深度学习融合仍然无法绕开传统融合问题：

• 时间和空间对齐；
• 传感器缺失；
• 域外环境；
• 置信度校准；
• 训练数据偏差；
• 外参轻微变化；
• 模态失效；
• 推理延迟不一致。

一个在同步数据集上表现良好的融合网络，不一定能直接适应实际机器人中的异步数据和动态延迟。

工业场景中，深度学习融合目前更多作为语义辅助约束，与传统几何融合方法结合使用，而非完全替代几何估计——其可解释性、泛化能力与实时性仍是落地的主要瓶颈。

---

18. 主流开源框架与工具

18.1 robot_localization

robot_localization是ROS生态中常见的状态估计工具，主要提供：

• EKF状态估计节点；
• UKF状态估计节点；
• GNSS坐标转换相关节点；
• 多里程计、IMU和位置源的松耦合融合。

适合：

• 移动机器人快速集成；
• 轮速计、IMU、GNSS融合；
• 视觉里程计和激光里程计结果融合。

不适合直接完成：

• 原始视觉特征紧耦合；
• 原始点云紧耦合；
• 大规模回环图优化；
• 高维在线标定。

它是移动机器人松耦合融合的首选工具，开箱即用，接口规范，适合快速验证系统方案。

18.2 GTSAM

GTSAM是以因子图和贝叶斯网络为核心的机器人状态估计库。

适合：

• 位姿图优化；
• SLAM；
• VIO和LIO；
• IMU预积分；
• GNSS因子；
• 增量优化；
• 自定义传感器因子。

GTSAM不仅是一个通用最小二乘求解器，还提供了与概率图模型、变量消元、Bayes Tree和iSAM2相关的完整抽象。它的因子图建模范式非常优雅，适合学术研究与原型验证。

18.3 Ceres Solver

Ceres是通用非线性最小二乘优化库。
它适合：

• Bundle Adjustment；
• 相机标定；
• 手眼标定；
• 滑动窗口估计；
• 外参优化；
• 自定义残差问题。

Ceres提供：

• 自动微分；
• 数值微分；
• 解析雅可比；
• 鲁棒损失函数；
• 稀疏线性求解器；
• 信赖域方法。

GTSAM偏向“用因子图表达概率估计问题”，Ceres偏向“定义残差块并求解通用非线性最小二乘问题”。工业级产品开发中，Ceres因其通用性与灵活性，使用更为广泛。

18.4 OpenVINS

OpenVINS是滤波式视觉惯性估计研究平台，其核心使用扩展卡尔曼滤波融合：

• IMU传播；
• 稀疏视觉特征轨迹；
• 多相机状态；
• 内外参标定；
• 时间偏移等状态。

它特别适合学习：

• ESKF；
• MSCKF；
• 状态增广；
• 特征边缘化；
• First-Estimate Jacobian；
• 可观测性和一致性。

OpenVINS代码规范、文档完善，是学习视觉惯性滤波的最佳开源教材。

18.5 VINS-Mono

VINS-Mono是优化式视觉惯性系统的代表，包含：

• IMU预积分；
• 特征跟踪；
• 初始化；
• 滑动窗口优化；
• 外参估计；
• 回环检测；
• 位姿图优化。

它体现了典型的：

前端特征跟踪 + 局部滑动窗口 + 全局位姿图

架构。

VINS-Mono是视觉惯性SLAM领域的里程碑式工作，奠定了优化式VIO的标准技术框架。

18.6 LIO-SAM

LIO-SAM采用因子图实现激光雷达和IMU紧耦合，并可以加入：

• GNSS约束；
• 回环因子；
• 激光里程计因子；
• IMU预积分因子。

其重要设计包括：

• 使用IMU进行点云去畸变；
• 使用局部子地图进行匹配；
• 通过关键帧控制计算规模；
• 通过因子图维护状态和偏置。

LIO-SAM架构清晰、扩展性强，非常适合作为多传感器融合系统的基础框架进行二次开发。

18.7 FAST-LIO2

FAST-LIO2采用紧耦合迭代卡尔曼滤波，将原始激光点直接与地图进行配准，而不强制依赖人工设计的边缘和平面特征提取。

其主要特点包括：

• 迭代EKF；
• 点到地图直接残差；
• 增量k-d树地图；
• 高频状态估计；
• 适配不同扫描模式的激光雷达。

FAST-LIO2说明：

紧耦合、高精度融合并不一定必须使用滑动窗口或因子图。

FAST-LIO系列凭借高频、低延迟、鲁棒性强的特点，在工业移动机器人、无人机等领域得到了极其广泛的工程应用。

18.8 Autoware

Autoware并不是一个单独的融合算法，而是一套自动驾驶软件栈。

其融合相关模块通常涵盖：

• GNSS和IMU定位；
• 激光雷达地图匹配；
• 多传感器目标检测；
• 占据栅格融合；
• 目标跟踪；
• 全局和局部状态管理。

它适合研究大型融合系统中的：

• 模块边界；
• 数据接口；
• 多传感器健康管理；
• 定位与感知解耦；
• 系统级冗余。

---

19. ROS 2多传感器融合工程架构

一个推荐的ROS 2架构如下：

sensor_drivers
├── camera_driver
├── lidar_driver
├── imu_driver
├── gnss_driver
└── robot_driver

time_manager
├── device_clock_monitor
├── timestamp_converter
├── measurement_buffer
└── pose_interpolator

calibration
├── camera_intrinsics
├── sensor_extrinsics
├── hand_eye_calibration
└── static_tf_publisher

preprocessing
├── image_rectification
├── pointcloud_filter
├── motion_compensation
└── measurement_quality

local_estimator
├── imu_propagation
├── visual_or_lidar_update
├── wheel_update
└── local_odometry

global_estimator
├── gnss_factor
├── map_matching
├── loop_closure
└── global_graph

diagnostics
├── residual_monitor
├── covariance_monitor
├── time_sync_monitor
├── sensor_health
└── rejection_statistics

19.1 数据接收和估计线程应解耦

传感器回调不宜直接执行耗时优化。

推荐流程：

ROS Callback
    │
    ├── 校验消息
    ├── 转换时间戳
    ├── 写入有序缓冲区
    ▼
Estimator Thread
    │
    ├── 按时间顺序取数据
    ├── 状态传播
    ├── 观测更新
    └── 输出状态

这样可以：

• 减少回调阻塞；
• 保证测量时间顺序；
• 处理不同频率传感器；
• 支持延迟数据和回放；
• 提高估计器确定性。

此外，ROS 2的QoS策略也需要针对性配置：IMU等高频数据使用Best Effort降低延迟，定位结果与诊断信息使用Reliable保证可靠性；节点内通信（Intra-process）可以减少消息拷贝，进一步降低延迟。

19.2 多速率异步更新

不能要求所有传感器以相同频率运行。

合理的模式是：

IMU到达      → 高频状态传播
编码器到达   → 速度或运动学更新
相机到达     → 视觉观测更新
激光到达     → 几何观测更新
GNSS到达     → 全局位置更新

融合器的时间轴由测量时间决定，而不是由某个固定主循环简单轮询决定。

异步更新是多传感器融合系统的标准运行模式——每个传感器按自身频率到达，触发对应的预测或更新步骤，而非强制同步到统一帧率。

19.3 局部状态与全局状态分离

机器人系统通常需要区分：

局部连续状态

要求：

• 高频；
• 平滑；
• 不突然跳变；
• 用于控制。

全局一致状态

要求：

• 与地图或世界坐标一致；
• 可以通过GNSS或回环修正漂移；
• 修正时可能出现位置变化。

因此常见设计是：

局部估计器 → odom坐标系中的连续运动
全局估计器 → map到odom的缓慢或离散修正

控制器通常更适合使用局部连续状态，而不是直接使用可能因回环而跳变的全局位姿。这一设计能够避免全局修正带来的控制抖动，是ROS导航体系中的标准最佳实践。

---

20. 如何选择融合算法

20.1 IMU + 轮速计

优先考虑：

• EKF；
• ESKF；
• robot_localization。

重点是轮速打滑检测和IMU零偏。

20.2 IMU + GNSS

优先考虑：

• ESKF；
• 组合导航误差状态模型；
• 对GNSS跳点执行创新门限。

20.3 相机 + IMU

资源受限、追求低延迟：

• MSCKF；
• OpenVINS。

需要高精度、回环和重定位：

• 滑动窗口VIO；
• VINS-Mono类系统。

20.4 激光雷达 + IMU

追求高频局部里程计：

• 迭代ESKF；
• FAST-LIO2类框架。

需要回环、GNSS和全局图优化：

• LIO-SAM类框架；
• 局部LIO加全局因子图。

20.5 多个独立定位源

快速工程集成：

• 松耦合EKF；
• robot_localization。

但必须检查多个定位源是否使用了重复传感器数据。

20.6 多机器人或分布式系统

• 分布式因子图；
• 协方差交叉；
• 联邦滤波；
• 通信延迟和公共信息管理。

选择融合方案的核心原则是需求驱动，够用就好。不要盲目追求紧耦合、因子图等复杂方案——很多工业场景下，松耦合EKF就足以满足需求，且开发维护成本低、可靠性高。只有当松耦合的精度确实达不到瓶颈时，再考虑升级到紧耦合。

---

21. 融合系统的测试与验证

21.1 单传感器测试

在融合之前，必须分别验证：

• 时间戳是否真实；
• 坐标系是否正确；
• 数据频率和延迟；
• 噪声分布；
• 掉线行为；
• 静态零偏；
• 动态响应。

很多人急于调试融合算法，却跳过了单传感器验证，最终在错误的基础数据上浪费大量时间。

21.2 时间偏差扫描

人为给某个传感器增加不同时间偏移，观察误差变化：
$$\Delta t\in [-100\text{ms},100\text{ms}]$$
如果某个偏移能明显降低残差，说明系统存在未补偿延迟。

这是排查时间同步问题最直接、最有效的工程方法。

21.3 外参扰动测试

对外参施加小扰动：

• 平移1～10毫米；
• 旋转0.1～1度；

观察轨迹、投影或点云重叠误差的变化。

这可以评估系统对标定误差的敏感性。

21.4 传感器失效测试

测试场景应包括：

• 相机遮挡；
• 激光数据中断；
• IMU短时饱和；
• GNSS跳点；
• 网络延迟；
• 时间同步失锁；
• 外部定位丢失。

融合系统不仅要在所有传感器正常时精度高，还要在部分传感器异常时保持可控退化。故障鲁棒性是工业级系统与实验室原型的核心区别。

21.5 记录残差和诊断数据

至少记录：

• 每类观测残差；
• NIS；
• 状态协方差；
• 观测接受和拒绝次数；
• 传感器延迟；
• 缓冲区长度；
• 时间同步状态；
• 外参版本；
• 估计器重置次数。

只记录最终位姿，通常无法有效分析融合系统为什么失败。完善的诊断日志是定位问题、迭代优化的基础。

除此之外，长时运行测试、极端场景测试与回归测试也必不可少。一套覆盖多场景、带真值的标准化测试数据集，能够大幅提升融合系统的开发效率与质量稳定性。

---

22. 常见失败原因

多传感器融合项目中最常见的问题包括：

1. 使用消息到达时间代替物理采样时间；
2. 使用当前机器人位姿匹配过去采集的数据；
3. 坐标变换方向写反；
4. 米、毫米、度和弧度单位混用；
5. 外参标定姿态激励不足；
6. 将协方差全部填写为零或极小值；
7. 重复融合共享同一传感器的信息；
8. 不处理异常值和错误关联；
9. 估计不可观状态；
10. 将低频传感器数据强行插值成高频“新观测”；
11. 把算法输出频率等同于物理信息更新频率；
12. 忽略传感器处理延迟；
13. 在多线程中无序更新滤波器；
14. 使用回环修正后的跳变位姿直接进行高速控制；
15. 只关注平均误差，不评估最坏情况和故障行为；
16. 只调权重，不检查模型、时间和坐标是否正确。

这些失败原因高度集中于基础工程环节，而非算法本身。调试融合系统的正确顺序应当是：先查时间，再查坐标，再查数据关联，最后才调整算法参数。绝大多数问题都能在前三个环节找到根源。

---

总结

多传感器融合不是一个单独算法，而是一套完整的系统工程。

它至少包含：

概率建模+时间同步+空间标定+数据关联+状态估计
+异常检测+软件架构+测试验证

KF、EKF、ESKF、因子图和深度学习只是其中的求解工具。一个融合系统最终是否可靠，更多取决于：

• 时间戳是否对应真实采样事件；
• 传感器是否处于统一坐标关系；
• 噪声和协方差是否合理；
• 观测关联是否正确；
• 状态是否可观；
• 重复信息是否被正确处理；
• 异常传感器是否能够被隔离；
• 系统是否保留足够诊断信息。

对于机器人项目，一个务实的发展路径通常是：

1. 首先保证单传感器数据正确；
2. 完成可靠的时间和空间对齐；
3. 使用松耦合方法快速打通系统；
4. 建立残差、协方差和故障监控；
5. 明确精度瓶颈；
6. 仅在确有必要时改为紧耦合；
7. 根据实时性和全局一致性需求，选择滤波、滑动窗口或因子图；
8. 通过传感器失效和退化场景验证鲁棒性。

真正成熟的多传感器融合系统，不是“传感器越多越好”，而是每一份信息都具有明确的时间、坐标、概率含义和故障边界。从原型到产品的差距，不在于算法的复杂度，而在于工程细节的完备度与异常场景的鲁棒性。