智能驾驶传感器后融合与前融合

完成车载自动驾驶系统功能，或设计制作一款机器人，抑或搭建车间中用于装配的工业移动型机械臂时，使用单一的传感器都具有一定局限性。 

以自动驾驶系统为例，如果单独使用雷达进行车辆检测，由于没有图像信息，仅使 用雷达点云会存在目标漏检和错检现象；而单独使用视觉传感器进行车辆识别时，相机 容易收到光照环境或者反射平面光线特性的影响，因此在雾天或雪天等极端环境中，车 载相机能记录的信息是极其有限的，会导致目标检测不精确。面对日益复杂的环境，使 用单一传感器会带来很多操作安全隐患，同时，识别精确度不足也会使得单传感器完成 SLAM 任务显得愈发艰巨。不同传感器的功能各不相同，在不同的场景里可以发挥各自 的优势，同时各自在一些特殊场景中都存在一些局限性。有没有一种方法能综合各类传 感器获得的数据，扬长避短，从而拓宽传感器在 SLAM 系统中的应用深度与广度呢？ 

多传感器融合（MSF，Multi-sensor Fusion）技术通过对不同传感器的信息进行综合处理，从而显著提高智能系统决策与规划的科学性及准确性。

在 SLAM 系统中各传感器工作原理不同，获得的信息也有所不同。在完成时间同步 与空间标定后，即可在同一时刻得到多传感器在同一坐标系下的信息。对于相互耦合的 信息，如何做到有效融合以解决实际问题是本节讨论的主题。 

传感器融合主要用于解决“我在哪”的建图定位问题与“路上有什么”的感知问题。具体而言，当输入各个传感器在 t 时刻的数据时，通过多传感器间融合运算可以获得此 时系统处于世界坐标系的位置（如车身坐标系相对于世界坐标系的变换矩阵等）与道路环 境特征元素。如图 3-10 所示，车辆在行驶中通过相机与 LiDar 构建图形地图与三维点云地图，同时明确自身定位，这便是建图定位问题。 

同样，当输入各个传感器在 t 时刻采集到的数据后，计算机通过融合运算获得此时 环境中的目标级障碍物与各种语义级元素（如车道线、花坛等）。如图 3-11 所示，车辆在 行驶中同时采集图片数据与点云数据，在点云中识别目标级障碍物并以颜色标注，便可 同时获得障碍物的语义信息与深度信息，这便是感知问题。

图：3-10 传感器融合解决建图定位问题

图：3-11 传感器融合解决感知问题

本节着重讨论感知问题的实现方式。传感器融合可分为后融合与前融合两类。后融合策略中每个传感器分别处理其采集到的数据信息，随后在融合模块中对处理后的信息 进行整合；前融合策略中直接对传感器采集到的原始数据进行直接融合，并根据融合后 的数据信息实现感知。接下来将分别对这两种实现方式及其相关算法展开介绍。 

一、 后融合 

后融合技术作为经典的传感器融合技术，在多种传感器数据分别完成检测与分割任 务后进行数据融合。如图 3-12 所示，LiDar 与 Camera 传感器分别进行 3D 和 2D 的检测及分割任务，随后进行多目标跟踪（Multiple Object Tracking，MOT）提高分割精度，各 传感器完成数据处理工作后进行融合，输出所需的感知结果。

图 3-12 后融合解决感知流程

在介绍实现后融合的算法前，要先明确图 3-12 流程中各传感器的输入与输出。

LiDar 输入的是 3D 点云数据，通过 3D 检测与分割任务输出鸟视图中的 3D 目标障 碍物。LiDar 检测任务会输出 LiDar Object 列表作为结果，其中每个 Object 中都包含位 姿信息（位置坐标、偏航角等）、运动信息（速度、加速度、偏航速率等）与形状信息（几 何尺寸等）。LiDar 分割任务输出新的点云格式或多边形形式，其中包含点云信息（位置 三维坐标、强度信息等）与点云类别。

Camera 传感器输入的是 2D 图像数据，通过 2D 检测与分割任务输出图像中的边界 框。Camera 检测任务会输出 Camera Object 列表形式的结果，每个 Object 中包含形状信 息（像素宽高、边界框中心点等）与位置信息（位置像素坐标、偏航角等）。Camera 分割任务会输出带分类标签的像素信息，以同尺寸的图像的形式下发到后融合模块。 

Radar 传感器输入的是点云数据。Radar 传感器检测任务会输出的 Radar Object 列表形式结果，每个 Object 中包含位置信息（不含高度的位置坐标等）、运动信息（速度、加 速度等）与 RCS 信息（雷达截面积，用以衡量返回波强度）。上述输出内容中都会附带传 感器获取数据的时间戳，可按需控制传感器数据发送延迟。 

各类传感器通过检测与分割任务输出观测数据，在融合模块中通过对这些观测量整 合得到各类目标级对象与语义级对象，帮助中心处理器进行障碍物类别与位置的判断。由此可见后融合问题的核心是由传感器观测量并估计对象状态量的问题。当然在进行状态估计前，还需处理传感器信息发送延迟不同造成的观测量时空不对齐的问题。 

综上所述，后融合 Sensor Fusion 系统的结构流程建模为如图 3-13 所示。首先在预测阶段通过传感器发送的时间戳数据结合运动估计进行空间对齐，接着将当前帧率观测量与上一帧观测量依次进行目标关联，最后结合观测量与状态量进行当前帧状态估计，输出目标级与语义级对象。

图 3-13 后融合 Sensor Fusion 系统的结构流程建模

（1）预测问题 

传感器以固有频率输出观测量给融合模块，而系统状态量却以另一个固定频率（通 常更低）进行更新。例如，在车辆驾驶过程中的 t 时刻，传感器观测量为，此时车辆系统的状态量由于没有及时更新仍旧处于 t−1 时刻的，此时需要将车辆的状态量更新到从而完成时空对齐。与空间标定类似，通过变换矩阵即可完成位姿状态与运动状态的同步，对于 LiDar 与 Radar 而言，这两类传感器中都包含着位姿、速度与加速度信息，可用式（3-4）完成目标级与语义级对象的对齐，其中为从 t −1 时刻到 t 时刻的位姿变化量：

对于 Camera 而言，由于相机传感器记录下的是二维图片，缺少三维空间信息，则可利用式（3-5）完成二维目标级与三维语义级对象的对齐，其中 H 矩阵是包含的相 机内参矩阵： 

（2）目标关联 

对于单个目标级对象而言，通过判定两帧间此对象的状态量是否满足相关联的尺度 即可完成匹配工作。不同观测量的匹配度量方式有所不同，以 LiDar、Radar、Camera 的融合为例，它的状态量是一个完整的 3D 边界框，此时判定 LiDar、Radar、Camera 观测互相匹配的度量 d 如式（3-6）、式（3-7）所示。

式（3-8）中 IoU 是交并比（Intersection over Union），表示两个区域体积（面积）的交集与二者的并集之比。由于 LiDar 观测中可以得到 3D 边界框，故式（3-6）中表示的是两帧画面中观测到的 3D 边界框的交并比。而 Camera 只能观测到 2D 图片，因此式（3-8）中计算的是 3D 边界框投影的二维图片与观测到的图像的交并比。Radar 无法观测尺寸与高度信息，因此只要观测到两帧中的点位于整个 3D 边界框中即可认为其匹配，故式（3-7） 中的长宽为 3D 边界框的尺寸信息。这一思想同样可以应用于语义级对象的匹配工作，通过提取某一类别信息（如地面信息）的 2D 分割与 3D 云图中分割的投影，在栅格化后以栅格为单位计算 IoU，再根据这一指标实现 3D 空间和 2D 空间的语义级对象匹配。

显然，在两帧画面中存在着多个需要匹配的目标级对象，因此需要进行 Object 集合 的匹配。在匹配时将两帧中各个 Object 抽象为点，根据其位置与运动情况赋予其权重， 转化为二分图最大权匹配问题。这类问题通常使用 Kuhn-Munkres 算法（K-M 算法）解决。K-M 算法将二分图中的权值作为每个顶点的一个标号（称作顶标），通过体系的多次 降权，将最大权完美匹配求解的问题转化为完美匹配求解的问题。 

（3）状态估计 

解决融合模型中的预测问题与目标匹配问题后，即可步入由观测量出发的状态估计 问题。根据传感器获取观测量的精确度与实际应用价值，通常而言融合的优先级由高到 低为目标级运动属性融合、语义级对象融合、目标级其他属性融合。 

首先对状态估计问题进行数学建模。设系统在 t 时刻的观测量为，状态量为。这两个变量是符合某种分布的随机变量，由于二者相互联系，所以不相互独立。状态估计需要通过历史观测量计算 t 时刻的的状态量，其中是系统初值，表示历史观测量。根据贝叶斯法则将系统状态的概率求解问题拆分

其中是已知值，从前 t 次状态量与初值可知第 t 次状态量，故。同时由前t −1次的状态可表示为，因此式（3-8）可改写为正 比例形式

假设系统符合马尔可夫性质，即仅与有关，而与更早的状态无关，则式（3-10） 中可由上一时刻观测量表示而不涉及状态量，化简为

其中是似然项，可由观测方程得到；是先验项，可由状态转移方程推导得到。 

至此便完成了状态估计问题的数学建模。这一简化后的问题可由滤波器相关算法， 如卡尔曼滤波（Kalman Filter）或扩展卡尔曼滤波（Extented Kalman Filter）来解决。卡尔曼滤波算法将状态空间的概念与随机估计理论相结合，利用系统状态方程、观测方程及噪声的统计特征形成一套完整的滤波算法，用以解决高斯白噪声下对随机过程的最优估计问题。 

对于目标级运动属性融合而言，通常假设对象满足匀加速直线运动与匀速旋转运动， 目标状态转移方程为

其中是状态转移矩阵，表示对象从t −1时刻到 t 时刻的状态变化；为外界输入与 系统状态变化的转换关系矩阵；是 t 时刻系统输入量，例如车辆运行时在背后被人向前推了一段距离，显然这种情况极少出现，因此通常为 0，此时是系统噪声，满足高斯分布。 

运用卡尔曼滤波算法得先验项为

其中是t −1时刻的协方差矩阵，表示该时刻各状态量的不确定性。例如在某一运 动中需要关注位置 p 与速度 v 的变化关系，此时有

其中代表状态分量的方差，代表状态分量的协方差。 

根据传感器获得的观测值建立状态转移方程，即可利用式（3-12）与式（3-13）表 达状态转移方程与先验项间的关联。

目标级运动属性融合中观测方程为

其中为观测矩阵，用以修正观测值与状态之间的误差。LiDar 与 Camera 对系 统的观测和状态一致，因此

Radar 切向速度不够精确，但径向速度精度高，可以引入下式进行修正：

θ为自车坐标系下转角；是观测噪声，满足高斯分布。

运用卡尔曼滤波算法得似然项为

根据传感器获得的观测值建立观测方程即可利用式（3-14）与式（3-15）完成观测 方程与似然项间的关联。在获得似然项与先验项分布后将其代入式（3-11）即可完成状 态估计问题。语义级对象融合与目标级其他对象融合与之类似。 

在语义级对象融合问题中将 3D 鸟视图下的栅格图设置为状态量，将LiDar 和 Camera 的语义对象栅格图设置为观测量，寻找状态栅格与观测栅格的对应关系。在这类问题中， 栅格图上某点(i , j )的状态量, 可以简单地设定为：若物体存在，则；若物体不存在，则。在目标级其他对象融合中关注的是尺寸数据，因此在这之中的状 态量可设置为几何尺寸矩阵。值得一提的是，实际情况下形如式（3-11）与式（3-15）的线性方程并不成立，因此需要先将非线性问题线性化， 使用扩展卡尔曼滤波求解算法完成状态估计。

状态估计问题中存在着的系统噪声与观测噪声不容忽视。方差描述状态转移 引入的系统不确定性，将状态转移方程式（3-7）移项变换得

通过对大量的与观测量进行统计学正态分布拟合，得到上述差值的分布。在设置时需要考虑 t 与t −1 时刻之间的时间差Δt，时间跨度的增大会带来不 确定性的增强，通常认为与呈线性关系，如式（3-16）所示，λ值根据系统特点进行调整。

方差描述观测量中引入的不确定性，可以使用与确定类似的方法。由于观测量 可以直接获得，所以也可以通过大量观测量的统计学正态分布得到。

二、前融合 

前融合技术是指直接将传感器采集到的原始数据输入融合模型，再根据数据融合的 结果进行综合判断。前融合作为一项新兴技术，相较于后融合而言可以规避融合前大量 的传感器前缀的分析工作，直接从传感器数据入手进行融合，在原始数据间建立关联性。如图 3-14 所示，将 LiDar、Camera 与 Radar 的数据直接输入融合模型进行检测与分割， 最后使用多目标跟踪与后处理提高分割精度。显然，前融合通过直接信息融合可以获得 每个像素级别的深度信息，最终获得的信息量更大。

图 3-14 前融合解决感知流程

这类直接输入 LiDar、Camera 与 Radar 数据给感知系统，将感知系统作为类似黑盒的角色来汇总融合所有数据信息的融合策略称为多模态融合。模态指信息的某种特定的 表达形式，诸如 LiDar 采集到的三维点云、Camera 采集到的像素图片、Radar 采集到的 毫米波信息均满足模态的定义。深度学习是多模态融合的主要技术手段，接下来对实现 多模态融合的卷积神经网络（Convolutional Neural Network，CNN）进行简单介绍。 

神经网络由大量的神经元相互连接而成，包含神经元接受输入讯息的输入层（Input layer），讯息在神经元内分析权衡后输出结果的输出层（Output layer），由输入层和输出 层之间众多神经元链接组成的隐藏层（Hidden layer）。由输入层起，通过对相应的连接权 值进行加权运算，将运算结果代入非线性函数从而得到下一层的结果，这样循环往复由 输入层预测输出层的算法称为前向传播算法（Forward propagation）。将预测结果与实际 结果之差定义为误差函数，误差函数会对每个权重求梯度。使用梯度下降法反向修正连接权值的过程称为反向传播算法（Back propagation）。结合前向传播与反向传播算法，能够进行对多层神经网络的训练，使得模型达到设计者的预期要求，如图 3-15 所示。

图 3-15 神经网络训练

随着神经网络的发展，“卷积”的概念逐步走入人们的视野。卷积神经网络是一种为 了处理二维输入数据而特殊设计的多层人工神经网络，输入的图像通过与可训练的滤波 器（卷积核）和可加偏置向量进行卷积运算来提取图像的特征。在后融合中的图像检测与 图像分割任务都可以使用 CNN 进行实现，通过建立特定的特征提取网络可以从图像中提 取目标级元素并分割语义级元素。 

除此之外，对于 LiDar 记录下的点云模态也能使用 CNN 方法进行语义级别对象的识 别与分类。通过将三维点云投影至二维图像，并对每一个投影点特征（高度、强度）加以 描述，即可使用 CNN 方法完成 LiDar 检测任务。显然，在含有多传感器特征信息的图像 上应用 CNN 方法能大大提高融合效率。 

依据不同的融合策略，多模态融合又分为数据融合、特征融合与目标融合 3 类，如 图 3-16 所示。

• 将传感器获得的原始模态在数据层面拼接后通过 CNN 进行特征提取实现模态融合 的方法称为数据融合。例如，将 LiDar 获得的点云数据投影到 Camera 采集到的 像素图片上后，使用 CNN 进行图像提取与分类便是典型的数据融合。 

  
  

• 将传感器获得的数据经过 CNN 方法获得处理，再将处理后的模态信息进行融合的方法称为目标融合。例如，首先使用 CNN 方法提取 LiDar 点云与 Camera 照片中 某一共同对象，再对这一对象的点云与图片进行信息融合便是典型的目标融合。

  
  

• 在使用 CNN 方法分别提取传感器获得的原始数据中的信息时，还可以将提取过程 的中间层进行融合，依照此融合结果进一步使用 CNN 获得最终的融合结果，这 种方法称为特征融合。数据融合要求对原始数据进行像素级别与点级别的对齐， 时空对齐程度高，而特征融合的计算时间介于数据融合与目标融合之间，因此它 与目标融合一样，都允许一定程度的数据不对齐。

• 
  

图 3-16 多模态融合

通常在前融合中采用 LiDar+Camera 与 Camera+Radar 的融合模式。 

（1）LiDar+Camera 的目标融合 MV3D 方法 

LiDar与 Camera 收集到的数据信息量大，要做到严格的时空对齐较为困难，因此通 常不采用数据融合的策略。如图 3-17 所示，该模型分别对 LiDar 鸟视图、LiDar 前视图 与 Camera 图像进行卷积神经网络分析。通过对 LiDar 鸟视图信息进行卷积特征提取，采 用 two stage 网络提取需要的 3D 候选框（3D Proposal）。以 3D 候选框为依据汇聚 3 个维 度的特征，提取卷积后鸟视图中特征信息、卷积后前视图的特征信息与卷积后图像的特 征信息。将这 3 类提取后的信息置于对应的通道（Pooling）后进行融合，获得对应的分 类与 3D 边界框。 

（2）LiDar+Camera 的特征融合 ContFuse 方法 

如图 3-18 所示，首先在 Camera 通道对采集到的图像数据通过 ResNet Block 卷积网 络提取其浅层特征与深层特征，对这些图像特征进行多尺度融合（Multi-scale Fusion）。随后在融合后的图像中寻找 LiDar 二维鸟视图中每个像素空间对应的特征向量，用以由 二维图像特征反推三维空间中的特征。这一三维特征量与 LiDar 通道中每一次卷积得到 的二维鸟视图进行连续融合（Continuous Fusion），实现了图像和点云的联合推理，摆脱 了 2D 图像检测对 3D 检测性能的约束。

图：3-17 LiDar+Camera 目标融合模型

图 3-18 LiDar+Camera 特征融合模型

（3）Camera+Radar 的目标融合 CRFNet 方法 

Radar 获取的点云较 LiDar 点云而言更为稀疏，且存在一定的噪声，因此可将 Radar 点云进行多周期叠加及滤波，从而获得更为密集的点云。使用与 LiDar 鸟视图类似的方法获取 Radar 叠加点云的距离信息通道与强度信息通道，这两个通道可作为 Radar 的特 征信息与 Camera 卷积得到的图像特征信息同时进入网络进行特征融合，最终获得需要的检测目标。 

对比后融合与前融合两类融合策略，前融合作为新兴技术能帮助后融合解决多个小 模型计算信息冗杂的问题，但是也会引入 CNN 模型庞大导致计算量增大的问题。相对于 后融合这一较为传统可控的技术而言，前融合技术仍旧需要一定的时间发展成熟。 

以自动驾驶系统为例，当今 L4 级智能驾驶系统（高度自动驾驶系统）倾向于使用 前后融合并行使用的方式，而 L2 级智能驾驶系统（部分自动化驾驶系统）中倾向于使用 Camera+Radar 的后融合系统。因为 Radar 相较于 LiDar 而言存在点云稀疏且噪声多的瓶 颈，诸多汽车厂商已逐步部署 LiDar+Camera+Radar 的自动驾驶系统。