传感器融合入门——利用多种传感器构建全球统一模型

Contents — find the section you need

机器人和自动驾驶车辆通常会同时搭载多个传感器——摄像头、激光雷达、惯性测量单元 (IMU)、雷达和全球导航卫星系统 (GNSS)。然而，任何单个传感器都无法准确感知周围环境：摄像头难以分辨深度，激光雷达无法识别颜色或纹理，IMU 的误差也会随着时间推移而累积。传感器融合技术正是利用其他传感器的优势来弥补各个传感器的盲点，从而将各自存在缺陷的“世界模型”整合为一个统一的“世界模型”。本文将首先介绍其数学基础——概率和优化——然后阐述具体的传感器组合：视频-图像融合 (VIO)、激光-图像融合 (LIO) 以及摄像头×激光雷达融合。

IMU/INS示例

深度摄像头示例

图片：Xsens MTi-G (Kallap85, CC BY-SA 4.0) / Intel RealSense深度摄像头D435 (Marc Auledas, CC BY-SA 4.0), Wikimedia共享资源。

0. 本文内容

传感器融合的用途，以及为什么单个传感器永远不够用
基本的融合流程（传感器 → 测量 → 特征/状态 → 融合 → 估计状态）
早期融合、中期融合和后期融合的区别
概率融合的工作原理 — 卡尔曼滤波器、扩展卡尔曼滤波器 (EKF)、无偏卡尔曼滤波器 (UKF)、粒子滤波器
基于优化的融合的工作原理 — 因子图、姿态图
为什么 VIO（摄像头×IMU）、LIO（激光雷达×IMU）和摄像头×激光雷达是如此自然的组合
使用自动驾驶的四传感器集成示例（摄像头×激光雷达×雷达×IMU）
融合在实践中真正的难点 — 时间同步、校准、噪声
如何针对您的用例设计融合系统

1. 简答：什么是传感器融合

一句话概括：**传感器融合在数学上将……结合起来融合是指利用多个传感器的不完整、噪声较大的观测数据，生成比任何单个传感器单独提供的更准确、更可靠的“状态”估计。

这个定义包含了三个核心概念。首先是互补性——不同的传感器在不同的方面各有优劣，因此将它们结合起来可以相互弥补不足。其次是状态估计——这是一种统计框架，用于根据可观测的量推断无法直接观测的量（例如，我现在在哪里，速度是多少，方向是什么）。第三是世界模型——将每个传感器的零散信息整合到一个在时间和空间上都一致的单一表示中。机器人并非基于原始传感器数据行动，而是基于这个整合的世界模型行动。

2. 为什么一个传感器不够用？

列出每个传感器的优缺点，就能清楚地说明为什么融合是必不可少的。

传感器	优势	劣势
摄像头	可识别颜色、纹理和语义（物体类别）；高角分辨率	深度难以直接测量；在黑暗、逆光或恶劣天气下性能较差；比例尺无法确定（单目）
激光雷达	可精确、直接地测量距离和几何形状；不受光照影响	无颜色或纹理信息；在雨、雾或尘土天气下性能较差；点云在远距离会变得稀疏且精度降低
惯性测量单元 (IMU)	高采样率（数百赫兹以上）；对方向变化敏感；不受摄像头光照和可见度影响	受温度、振动和安装误差的影响；误差会通过积分累积（漂移）；无法提供绝对位置
雷达	不受恶劣天气（雨、雾、尘）影响；通过多普勒效应直接测量相对速度	角分辨率低——物体形状粗略；难以区分静止物体
全球导航卫星系统 (GNSS)	提供绝对的全球坐标；误差不会累积	更新频率低（通常为 1–10 Hz）；在室内、隧道或城市高层建筑中（多径效应）性能会下降或丢失

此表揭示了“高精度传感器”和“通用传感器”是两回事。激光雷达 (LiDAR) 在几何精度上优于相机，但无法分辨颜色或含义。全球导航卫星系统 (GNSS) 提供绝对位置，但更新速度慢，且在市中心不可靠。惯性测量单元 (IMU) 不依赖于光照或可见度，但会受到温度、振动和安装误差的影响；由于它通过积分来获取状态，因此随着时间的推移，漂移是不可避免的。传感器融合正是利用了这种互补关系。

3. 融合的基本结构

传感器融合流程的细节会根据融合对象和融合层级而有所不同（下一节），但其总体结构始终包含相同的五个阶段。

图 1 — 每个传感器的原始测量值被转换为特征或部分状态，融合阶段将它们集成到一个估计状态中。

每个传感器的原始测量值不能直接比较——它们的单位、坐标系和更新速率都不同。每个传感器数据流首先被转换为特征（图像关键点、点云边缘等）或部分状态（速度、相对位姿），然后才被送至融合阶段。融合阶段正是下文讨论的概率方法（卡尔曼滤波器等）或基于优化的方法（因子图等）实际运行的地方，其输出是最终估计的状态——位置、速度、方向等等。

4. 三个融合层级

融合大致分为三个层级，取决于多个传感器信息在流程中的哪个阶段进行组合。

层级	融合发生时间	特点	示例
早期融合	接近原始数据	信息损失小，可能非常精确，但要求传感器之间严格的时间同步和坐标对齐	将激光雷达点云投影到相机图像上，并将它们组合成一个输入
中级（特征）融合	在特征提取阶段	对同步的要求不如原始数据融合那么严格；实现起来更灵活	结合相机和激光雷达特征的BEV空间方法（BEV融合系列）
后期融合	在每个传感器的独立感知/估计输出（例如，检测结果）阶段	每个传感器的处理流程保持独立，易于实现，但早期丢失的信息无法在后期恢复	事后将相机目标检测与激光雷达目标检测相结合的集成式方法

选择哪个阶段需要在精度和实现成本之间进行权衡。原则上，早期融合利用的信息最多，但传感器之间毫秒级的时间偏移或毫米级的安装误差会直接降低结果质量，需要非常严格的校准。后期融合具有很大的实际优势——每个传感器的处理流程都可以独立开发和调试——但无论下游结果如何巧妙地组合，单个传感器“遗漏”的信息都无法恢复。中级融合，尤其是自动驾驶中的纯电动汽车融合，已成为介于这两种极端情况之间的一种越来越受欢迎的折中方案。

5. 概率融合

第一种用于融合多个传感器观测数据的数学框架基于概率论。其基础是贝叶斯估计：将“从上一步延续的估计值”（先验分布）与“新的观测值”（似然分布）相结合，以更新“当前估计值”（后验分布）。

p(x_t \mid z_{1:t}) \propto p(z_t \mid x_t) \int p(x_t \mid x_{t-1}) \, p(x_{t-1} \mid z_{1:t-1}) \, dx_{t-1}

左侧是给定从时间 $1$ 到 $t$ 的每个观测值 $z_{1:t}$ 时，状态 $x_t$ 的后验分布。右侧将之前的估计值 $p(x_{t-1}\mid z_{1:t-1})$ 向前传播一步，经过运动模型 $p(x_t\mid x_{t-1})$ （积分），然后根据新观测值 $p(z_t\mid x_t)$ 的似然性对该预测值进行加权——这是一个“预测，然后更新”的循环。

卡尔曼滤波器 是在上述贝叶斯更新的基础上，假设所有变量（状态和观测值）都服从线性高斯分布而得到的，这使得更新过程可以转化为闭式表达式。在预测步骤中，状态和误差协方差通过运动模型进行传播；在更新步骤中，新观测值与预测值之间的偏差（创新）会根据卡尔曼增益对状态进行加权修正。

K_t = P_t^{-} H^{\top} \left( H P_t^{-} H^{\top} + R \right)^{-1}, \qquad \hat{x}_t = \hat{x}_t^{-} + K_t \left( z_t - H \hat{x}_t^{-} \right)

$P_t^{-}$ 是预测误差协方差， $H$ 将状态映射到观测空间， $R$ 是观测噪声协方差。卡尔曼增益 $K_t$ 是一个权重，表示“模型预测”相对于“新观测”的信任程度：观测噪声 $R$ 越小（传感器可信度越高），观测权重越高；预测不确定性 $P_t^{-}$ 越小（模型置信度越高），预测权重越高——这种平衡会自动调整。

实际机器人的运动和传感器模型几乎都不是线性的，因此通常不能直接应用普通的卡尔曼滤波器。扩展卡尔曼滤波器 (EKF) 使用雅可比矩阵将非线性运动和观测模型围绕当前估计值线性化，然后应用与普通卡尔曼滤波器相同的更新方程。无迹卡尔曼滤波器（UKF；Julier & Uhlmann，1997）完全跳过了雅可比矩阵近似，而是将少量具有代表性的样本点（σ 点）直接输入非线性函数，并从结果中重建均值和协方差——在强非线性情况下，其性能优于扩展卡尔曼滤波器（EKF）。粒子滤波器（Gordon、Salmond & Smith，1993）完全不限制分布为高斯分布；它使用大量粒子（样本）来近似概率分布本身，这使得它能够表示多个假设共存的多峰分布——但代价是需要更多的粒子和计算资源才能获得更高的精度。

6. 基于优化的融合

概率融合是按顺序逐步更新的，而第二种数学框架——基于优化的融合——则采用了不同的方法：它保存一个观测窗口，然后重新求解与所有观测值都最大程度一致的状态。

其核心公式是因子图。待估计的状态（机器人在每个时间步的位姿、观测到的地标位置）成为节点；它们之间的约束（给定的传感器观测值告诉我们两个状态之间必须如何关联）成为连接这些节点的因子，整个图被求解为一个非线性最小二乘问题。

\mathbf{x}^{*} = \arg\min_{\mathbf{x}} \sum_{k} \left\| \mathbf{e}_k(\mathbf{x}, \mathbf{z}_k) \right\|^{2}_{\Sigma_k^{-1}}

$\mathbf{x}$ 是待估计的完整状态集， $\mathbf{z}_k$ 是第 $k$ 个传感器观测值， $\mathbf{e}_k$ 是误差函数，用于衡量该观测值预测值与当前状态之间的偏差， $\Sigma_k^{-1}$ 是该观测值置信度的权重。当地标 3D 位置与相机位姿一起优化时，此过程专门称为捆绑调整。当节点是机器人轨迹上的位姿时，则称为位姿图优化——这是在闭环（识别先前访问过的位置）后校正累积漂移的标准框架。

概率融合（尤其是扩展卡尔曼滤波器）计算成本低，非常适合顺序处理，但它无法重新访问已被吸收的观测值——它只能从当前估计值向前移动。基于优化的融合方法可以在新的观测结果与旧的估计值发生冲突时，回溯并修正整个轨迹，从而提高精度——但代价是随着图的增长，计算量也会增加。像 Ceres Solver、g2o 和 GTSAM 这样的通用库被广泛用于高效地解决这类稀疏非线性最小二乘问题（其中任何一个观测值只涉及少量变量）。

7. 相机×惯性测量单元 (VIO)

将相机与惯性测量单元 (IMU) 结合使用称为视觉惯性里程计 (VIO)，它是实践中最广泛应用的融合组合之一。两者之所以如此契合，是因为它们的缺点几乎完美地互补。

相机可以根据特征点在图像中的移动来估计自身的运动，但单目相机原则上无法确定绝对尺度（真实世界的距离单位），而且快速旋转往往会导致图像模糊并破坏特征跟踪。惯性测量单元 (IMU) 与之正好相反：它以数百赫兹的频率直接测量加速度和角速度，因此对快速运动具有鲁棒性，并且通过对加速度进行积分可以得到真实尺度下的速度和位置变化——但同样的积分过程也意味着误差会随时间累积（漂移），因此它永远无法确定绝对位置。

v_{t+\Delta t} = v_t + a_t \, \Delta t, \qquad p_{t+\Delta t} = p_t + v_t \, \Delta t + \tfrac{1}{2} a_t \, \Delta t^{2}

正如该公式所示，IMU 通过对加速度 $a_t$ 进行两次积分来推导位置，因此即使是很小的加速度计偏差也会累积成位置误差，并且该误差会随着时间的平方而增大。视觉信息观测 (VIO) 可以利用视觉特征观测来约束这种漂移；在运动激励和可观测性足够的情况下，IMU 可以支持度量尺度估计，但它并不能在所有情况下消除尺度歧义。 MSCKF（Mourikis & Roumeliotis，2007，基于扩展卡尔曼滤波）、OKVIS（Leutenegger 等，2015，基于非线性优化）和 VINS-Mono（Qin 等，2018，单目 + IMU）是这两个框架下 VIO 的里程碑式实现。

8. LiDAR×IMU (LIO)

将激光雷达 (LiDAR) 与惯性测量单元 (IMU) 结合使用称为激光雷达惯性里程计 (LIO)，广泛应用于户外机器人和自动驾驶领域。激光雷达可以直接、精确地测量周围环境的几何形状，生成点云，但单次扫描需要几十到几百毫秒才能完成，如果传感器本身在扫描过程中发生移动，则单次扫描中的点云会发生畸变（运动畸变）。

同样，IMU的高采样率再次发挥了关键作用：通过插值扫描过程中捕获的精细方向变化，可以校正这种失真。而仅仅依靠点云匹配（例如ICP）在几何特征稀少的环境中（例如漫长的无特征走廊或开阔的田野）容易出现漂移，此时IMU的惯性信息可以填补空白，保持估计的稳定性。

早期的LOAM（Zhang & Singh，2014）通过从点云中提取边缘和平面特征点并进行匹配，开创了激光雷达里程计的应用。LIO-SAM（Shan等人，2020）将激光雷达和IMU信息紧密集成到一个因子图上，该因子图的设计使得GPS和回环闭合约束可以整合到同一个图中。 FAST-LIO / FAST-LIO2（Xu 等人，2021 / 2022）采用迭代卡尔曼滤波器进行紧耦合的激光雷达-惯性测量单元（LiDAR-IMU）融合，其中 FAST-LIO2 特别地直接处理点云，而非通过稀疏特征提取。紧耦合可以利用联合信息，但精度和成本取决于初始化、退化、实现和传感器时序；它并非在所有方面都优于松耦合。

9. 相机×激光雷达

相机和激光雷达的组合是最直观互补的融合组合——“知道含义但不知道距离”与“知道距离但不知道含义”的结合。但这种组合存在其他组合所没有的困难：两个传感器的坐标系本质上不同（相机的二维图像平面与激光雷达的三维点云），因此将两者关联起来需要显式的标定和投影过程。

标定求解相机的内部参数（焦距、镜头畸变、内部矩阵 $K$ ）以及相机与激光雷达之间的相对位姿（外部参数：旋转 $R$ ，平移 $t$ ）。有了这些参数，就可以将激光雷达测量的三维点 $\mathbf{X}$ 投影到相机像素坐标 $\mathbf{u}$ 中。

\mathbf{e} = \mathbf{u} - \pi\left( K \, [R \mid t] \, \mathbf{X} \right)

$\pi(\cdot)$ 是将齐次坐标映射到二维图像平面的透视投影函数，而 $[R\mid t]$ 是将激光雷达坐标变换到相机坐标的变换。标定是求解 $R$ 和 $t$ 以最小化误差 $\mathbf{e}$ 的过程。完成上述步骤后，同样的投影即可确定“哪个激光雷达点对应于哪个图像像素”（点-像素关联）。这种对应关系可以将摄像头的颜色或类别信息附加到激光雷达点，或者将摄像头检测到的物体的精确距离传递给激光雷达。

现代自动驾驶领域的主流方法是在特征级别而非逐点进行这种投影：鸟瞰图融合（BEV Fusion）。BEVFusion（Liang等人，2022；Liu等人，2022）分别将摄像头图像特征和激光雷达点云特征独立地转换到鸟瞰图（自上而下的二维）空间，然后在该空间中将它们融合——它被广泛认为是摄像头×激光雷达中级融合的旗舰示例。

10. 摄像头×激光雷达×雷达×惯性测量单元

以自动驾驶为例，实际应用中的融合远不止于单一的组合：摄像头、激光雷达、雷达和惯性测量单元（加上全球导航卫星系统）会同时集成，每个单元都扮演着明确的角色。

摄像头：能够识别纯几何图形无法提供的“含义”，例如交通信号灯颜色、标志文字和车道类型。
激光雷达：能够精确测量周围物体的三维形状和距离，不受光照影响。
雷达：即使在雨天、雾天或逆光等摄像头和激光雷达无法正常工作的情况下，也能通过多普勒效应直接测量相对速度。
惯性测量单元：能够高频捕捉车辆自身的姿态变化和加速度，并根据其他传感器的读数进行插值。
全球导航卫星系统：提供与地图进行全球定位的参考（通常在经过高精度校正后与惯性测量单元集成，例如RTK）。

这四到五种传感器类型可以相互弥补彼此的不足，因此几乎任何单一故障模式都能被其他传感器检测到：例如，浓雾会降低摄像头和激光雷达的精度，但雷达仍能正常工作；隧道会阻挡GPS信号，但惯性测量单元和激光雷达的里程计可以保证车辆的自主定位。这种冗余设计——降低多个传感器在相同条件下同时失效的概率——是多传感器融合在自动驾驶等应用场景中不可或缺的主要原因，因为在这些场景中，任何故障都是不可接受的。

11. 融合的难点

尽管传感器融合在理论上看起来很完美，但实际应用却会遇到一系列非常现实的难题。

时间同步：每个传感器的数据输出速率和延迟都不同，因此要将它们视为“同一时刻”，就需要精确的时间对齐——硬件级触发同步或时间戳插值。
校准：传感器之间的相对位置和方向（外参）以及每个传感器的内部参数（内参）都必须精确已知。即使是轻微的安装偏差也会在整个融合结果中引入系统误差。
坐标变换：每个传感器都处于各自的坐标系中（相机坐标系、激光雷达坐标系、车辆坐标系、全局坐标系），这些坐标系必须进行一致且连续的变换。
传感器噪声：每个传感器的噪声特性（是否为高斯噪声、是否存在偏差）各不相同，如果建模不当，会影响概率融合中的权重（例如卡尔曼增益）。
异常值：由动态物体引起的误检或不匹配会引入模型无法预料的噪声——普通的统计模型无法单独处理这种情况，因此需要使用稳健估计技术（例如 RANSAC）或显式异常值剔除方法。
不确定性传播：除非每个传感器的不确定性（协方差）在整个融合过程中得到正确传播，否则最终估计的实际可信度会被错误判断——过度自信或过度谨慎都会导致后续决策出现偏差。

这些问题都无法通过简单地应用教科书上的卡尔曼滤波方程来解决——它们都属于系统工程的范畴。

12. 最终世界模型

将目前为止讨论的所有内容——各个传感器、各种融合方法——整合起来，机器人或自动驾驶车辆最终承载的就是如下所示的“世界模型”，它将每个传感器的特性整合为一个整体。

图 2 — 摄像头、激光雷达、惯性测量单元 (IMU)、雷达和全球导航卫星系统 (GNSS) 各自提供不同的信息，融合层将这些信息整合到一个统一的世界模型中。

世界模型不仅仅是“我在哪里”——它包含了周围物体的几何形状（来自激光雷达）、物体的种类（来自摄像头）、它们的相对速度（来自雷达）、车辆自身的姿态变化（来自 IMU）以及在地图上的全局位置（来自 GNSS），所有这些信息在时间和空间上都无缝整合。机器人的路径规划或自动驾驶车辆的驾驶决策都是基于这个世界模型做出的，而不是基于原始传感器数据。从这个意义上讲，传感器融合是感知和行动之间的桥梁：它将每个传感器的感知能力转换成一个连贯的表示，供系统实际操作。

13. 融合系统的实际设计

在实际设计传感器融合系统时，以下四个问题通常有助于决策。

设计决策	权衡因素
融合哪些传感器	首先确定应用实际需要弥补哪些传感器的弱点（例如，如果必须具备恶劣天气下的恢复能力，则需要雷达传感器；如果需要绝对定位，则需要GNSS传感器等）。
融合阶段	如果精度是首要考虑因素，则在早期/中期阶段融合；如果开发独立性和可维护性更为重要，则在后期阶段融合。
滤波与优化	如果需求是顺序执行、低延迟、低计算量，则使用滤波方法（例如扩展卡尔曼滤波器）；如果精度优先、计算余量充足且过去的估算值可以修正，则使用基于优化的方法（例如因子图）。
精度与计算成本	紧耦合通常精度更高，但实现和运行成本更高；松耦合（分别估算每个传感器的精度，然后进行融合）更容易实现且成本更低，但精度会有所降低。

这些决策并非相互独立，而是相互影响的。例如，大规模生产的汽车系统通常由于计算资源的限制，不得不采用基于滤波器的松耦合设计；而研究应用或离线地图构建则可以采用基于优化的紧耦合设计，以追求最高精度。并不存在所谓的“完美融合方法”——实用传感器融合设计的本质在于，根据车载传感器、可用计算资源以及应用所需的精度和延迟，选择最合理的组合。

14. 总结

传感器融合通过数学方法将来自摄像头、激光雷达、惯性测量单元 (IMU)、雷达和全球导航卫星系统 (GNSS) 的观测数据（每个传感器在不同区域各有优劣）进行组合，利用概率方法（卡尔曼滤波器/扩展卡尔曼滤波器/无偏卡尔曼滤波器/粒子滤波器）或基于优化的方法（因子图/光束法平差），生成比任何单个传感器更精确、更可靠的状态估计。诸如 VIO（摄像头×IMU）、LIO（激光雷达×IMU）和 BEV Fusion（摄像头×激光雷达）等具体组合都基于相同的理念——将互补性转化为方程式——而融合的位置和方式的设计选择最终决定了融合的精度、计算成本和实现复杂度。

融合测量前的检查

将来自同一观测的位置和速度视为独立测量值可能会导致估计过于自信。在添加传感器之前，请检查时间戳、坐标系、外部参数和误差相关性。在相同条件下（包括中断情况），将融合结果与单传感器基线进行比较。

OpenVINS：IMU 模型、状态和不确定性传播

检查你的理解

如果相同的信息被融合两次会怎样？

将相关的信息视为独立的信息会导致过度自信。检查信息来源和相关性，而不是仅仅统计传感器数量。

Try it in the browser

Lab Change the conditions in a browser experiment传感器融合失败的原因——时序、帧和外部校准 Lab Change the conditions in a browser experiment在 Python 中运行卡尔曼滤波器：噪声、缺失数据和异常值