04-传感器与感知-传感器融合

作者：姚博文 · 2026-09-15

传感器融合（Sensor Fusion：EKF / UKF / 因子图）

定位：把轮式里程计、IMU、激光等多路传感器融合成一套"既平滑又不漂移"的位姿估计——融合层次、EKF/UKF 公式与 1D 完整 Python 示例、小车典型融合架构、时间同步 | 适用：智能小车/移动机器人开发

1\. 概述

1.1 这篇解决什么问题

单看任何一个传感器都有致命短板：

| 传感器 | 给什么 | 短板 |

| --- | --- | --- |

| 轮式里程计 | 高频（10–50 Hz）连续位移/转角 | 无绝对基准，轮滑/打滑/安装误差 → 长期漂移（典型累计误差为行驶距离的 5%–15%） |

| IMU | 高频（100 Hz+）角速度/加速度 | 积分漂移，秒级就不可信（消费级陀螺仪零偏漂移） |

| 2D 激光 | 绝对的环境几何 | 需要地图/特征才有"绝对位置"，无特征区域失效，频率低（10 Hz） |

\*\*融合（Sensor Fusion）\*\*就是让互补的传感器互相纠错：用里程计的高频连续性"撑场"，用 IMU 的高频角速度补短，用激光的绝对观测"拽住"漂移。融合层的输出——带协方差的位姿估计（/odom 或 /map）——是定位层的核心产品，直接决定规划和 SLAM 的质量。

1.2 在自主导航链路中的位置

1.3 本文覆盖

1. 融合的三种层次（数据级/特征级/决策级）与紧/松耦合、集中/分布式；

2. EKF/UKF 的完整方程与一个**可直接运行的 1D 定位 EKF Python 示例**；

3. 小车典型融合架构（谁做基准、时间同步、robot\_localization 配置）；

4. 因子图与 GTSAM/iSAM2 入门，理解"滤波 vs 优化"；

5. 时间同步方案与"时钟不同步导致 SLAM 抖动"等调试清单。

2\. 基本原理

2.1 融合层次：数据级 / 特征级 / 决策级

按"融合时各传感器信息被处理到什么程度"分三层：

| 层次 | 做法 | 例子 | 优点 | 缺点 |

| --- | --- | --- | --- | --- |

| 数据级（Raw-data） | 原始数据直接进一个统一估计器 | EKF 里同时用里程计 + IMU 原始数据 | 信息保留最完整、理论上最优 | 要求时间严格对齐、量纲统一，复杂 |

| 特征级（Feature） | 各传感器先提取特征，再融合 | 激光提取"角点/直线段"，视觉提取"ORB 特征点"，在特征层匹配融合 | 数据量小、鲁棒性好 | 特征提取本身有损、有延迟 |

| 决策级（Decision） | 各传感器各自出结论，仲裁选择/加权 | 里程计说"在动"、视觉说"没动" → 仲裁；多传感器投票避障 | 模块解耦、最易实现、故障隔离好 | 信息损失最大，非最优 |

两个正交的维度：

· **紧耦合（Tightly-coupled） vs 松耦合（Loosely-coupled）**：紧耦合 = 原始数据/中间量进同一个估计器（数据级/特征级，性能上限高，调试难）；松耦合 = 各传感器独立估计后在状态层拼接（如"里程计出 EKF1，激光出 EKF2，再融合"，易实现，信息有损）。**小车实践：里程计+IMU 用紧耦合 EKF（robot\_localization 一把梭），激光位姿以观测形式进 EKF 或独立成 AMCL（松耦合）**，是性能与复杂度的甜点。

· **集中式 vs 分布式**：集中式 = 一个估计器收所有数据（小车都是集中式）；分布式 = 各节点本地估计、交换信息（多机协同/车队场景才需要，本文不展开）。

2.2 卡尔曼滤波家族：EKF

**贝叶斯框架**：状态 $x_k$（小车 = 位置、速度、姿态…），两个方程：

· 状态方程（预测，用控制输入 $u_k$ 和过程噪声 $w_k$）：$x_{k} = f(x_{k-1}, u_k) + w_k,\quad w_k \sim \mathcal