多传感器融合算法-后融合

  自动驾驶系统通常由定位、环境感知和规划控制等模块组成，近年来主流的环境感知传感器包括摄像头、激光雷达、毫米波雷达、超声波雷达等。不同种类传感器由于其探测原理不同，所具备的优劣势也有所差异，且现阶段单一种类的传感器往往很难覆盖所有场景，为了使感知层获得的信息更为丰富、准确，通常需要将不同传感器的数据进行融合。

  多传感器融合（Multi-sensor Fusion, MSF）是指借助计算机和数据处理技术，将来自多传感器或者多源的数据以一定准则进行分析综合，增强整个传感器信息系统信息利用率的方法。根据数据融合方式的不同，主要可以分为前融合和后融合。前融合主要是指数据/特征层次的融合，优点在于数据利用更加充分；而后融合主要是指目标/结果层次的融合，优点在于处理相对简单，运行效率更高。
  本文主要是对近期学习的感知-后融合模块进行简单介绍，仅做自学用途，如果不对之处欢迎大家留言沟通。


  这里多模数据选取激光雷达(LiDAR)、相机(Camera)和毫米波雷达(Radar)。利用算法进行多传感器数据融合，输出更加稳定可靠的感知结果。

• LiDAR：原始数据为三维空间点云数据，通过对其进行3D检测和分割，以及多目标跟踪，得到3D空间的目标元素；
• Camera：原始数据为图像，通过对其进行2D的检测，得到2D空间的目标元素；
• Radar：原始数据为稀疏的二维离散点云，但具备径向速度信息，通过对其进行数据处理，得到2.5D的目标元素。

1. 后融合系统输入输出解析

  输入主要包括LiDAR、Camera和Radar的检测处理结果。

  输出主要是3D空间下完整的boundingbox目标及其语义信息。

2. 后融合系统问题建模

融合系统的核心是希望对不同模态的传感器数据进行综合处理，提高传感器系统的信息丰富度，输出更加稳定可靠的感知结果。
1

1. 从上述后融合系统的输入输出角度来看，需要将多传感器获取的观测量与待估计的目标状态量进行融合，由此可以将问题转化为系统的状态估计问题。
2. 不同传感器的数据采集频率和数据处理的耗时不同，使得各模态数据结果接入到后融合模块的时间通常也是存在差异的，由此也会导致时空对齐的问题。
3. 通常传感器单帧数据中会存在多个目标，如何将不同传感器观测到的目标及单个传感器帧和帧之间的目标信息关联起来，由此也就涉及目标匹配问题。

3. 后融合系统结构设计

  针对上述多传感器后融合中涉及到的三个问题：多观测条件下的系统状态估计问题、时空对齐（预测）问题、目标匹配问题，进行多传感器后融合系统结构的设计构建。

3.1 预测-时空问题

  通常LiDAR的频率为10hz，Camera为30+hz，Radar为20hz。假设状态量是100ms更新一次，则在两次状态量之间，状态量相对于新到来的观测量在时空维度上都是滞后的，因此需要时空对齐，保证状态量与观测量在融合时是一致的。由于要做的工作是时空对齐，因此只需要关注引起空间位置变换的状态量和观测量即可：

• 位姿状态：如yaw和yaw_rate;
• 运动状态：如位置、速度和加速度。

对齐方法：
  对于不同频率的LiDAR、Camera、Radar检测结果，将其输入到预测模块中，根据自车的运动和目标的状态估计，进行时空对齐，使得融合时的状态量和观测量是位于同一时空下的。原则上下述两种方式均可以，但更推荐方式一，原因是State本身是基于多传感器数据的得到的状态量，精度上比各单传感器精度更高，预测误差也更小。

方式一：将 t-1 时刻的状态 $Stat{e}_{t-1}$ 对齐到 t 时刻的各传感器观测量（$Me{a}_t^{LiDAR}$ 、$Me{a}_t^{Camera}$ 、$Me{a}_t^{Radar}$） 处；

方式二：将 t 时刻的各传感器观测量（$Me{a}_t^{LiDAR}$ 、$Me{a}_t^{Camera}$ 、$Me{a}_t^{Radar}$）对齐到 t-1 时刻的状态 $Stat{e}_{t-1}$ 处；

3.2目标关联-匹配问题

  不同传感器经处理后得到的目标维度也不尽相同，那么如何进行目标级对象的匹配呢？首先，需要明确的是，融合的目的是希望通过多传感器（LiDAR + Camera + Radar）的目标数据，得到不同时刻下同一目标的3D bounding box信息。当然，如果只有Camera + Radar，则融合得到的可能是2D 的bounding box及其对应的距离、速度信息，但最终的目的还是希望得到3D 的bounding box信息。

3.2.1 目标级对象匹配算法—关联的度量

  以LiDAR + Camera + Radar融合为例：三者融合之后得到的目标状态量通常为完整的 3D bounding box，估状态量与观测量之间的匹配度量可以表示如下：

1. LiDAR观测量 与 3D bounding box

  通常LiDAR输入的观测量也会3D bounding box，因此两者之间的匹配可以通过计算两个目标 3D bounding box之间的交并比** IoU(Intersection over Union)** 来度量。
$$d=3DIoU(bbo{x}_{3D,1},bbo{x}_{3D,2})$$

2. Camera 观测量 与 3D bounding box

  通常Camera输入的观测量是2D bounding box，而状态量是3D bounding box，因此需要先将 3D bounding box 投影到图像上以获取其 2D bounding box信息，然后计算两个2D bounding box 的 IoU 来度量。
$$d=2DIoU(Project(bbo{x}_{3D,1}),bbo{x}_{2D,1})$$

3. Radar观测量 与 3D bounding box

  通常毫米波雷达输入的观测量没有尺寸和高度信息（这里暂不考虑4D毫米波雷达，仅以L2常用车载毫米波雷达为例），因此通常认为其检测到的目标点处于3D bounding box 内部时，及认为满足匹配要求。
d = { ∣ x ′ − x ∣ < l e g n t h ∣ y ′ − y ∣ < w i d t h d=

$$\begin{cases} |x' - x| < legnth \\ |y' - y| < width \end{cases}$$ d={∣x′−x∣<legnth∣y′−y∣<width​

3.2.2目标级对象匹配算法—帧间匹配算法

  通常每帧数据检测出的Object可能会有若干个，而为了实现不同模态检测结果的融合，势必设计到Object集合之间的匹配问题，即下图中红色点和绿色点的匹配。

  一般而言，待匹配的目标是带有权重的（如单个红色与其他任意绿色点之间的权重可以通过前面提到的目标关联的度量值来表示），这样帧间的目标匹配工作就可以转化成两个带权重集合的匹配问题。假设有两个目标点集 f ( O ) t − 1 = { x 0 , x 1 , x 2 , x 3 , x 4 } 和 f ( O ) t = { y 0 , y 1 , y 2 , y 3 , y 4 } f(O)_{t-1} = \{x_0,x_1,x_2,x_3,x_4\} 和 f(O)_t = \{y_0, y_1, y_2,y_3,y_4\} f(O)t−1​={x0​,x1​,x2​,x3​,x4​}和f(O)t​={y0​,y1​,y2​,y3​,y4​} 需要进行匹配。对于目标x0 而言，如果取最大权重的y4 与之对应，则会出现一个目标匹配多个对象的情况，如下图所示。

  对于这种带权二分图最优匹配问题，可以考虑使用KM算法（Kuhn-Munkres Algorithm）。待补充。。。

3.3 状态估计问题

  多传感器融合的数学本质：在输入、输出和状态空间已知的条件下，获取系统状态最优估计。
  经过前面的时空对齐和目标关联处理之后，已经可以拿到时空对齐后的 t-1 时刻目标状态量、t 时刻的目标观测量，以及状态量和观测量之间的目标匹配关系了。现在就需要在已知这些信息的基础上，对目标状态量进行更新，获取 t时刻 下目标状态的最优估计量。
首先需要进行融合信息的拆解，融合对象包括目标级的运动属性融合（位置、速度、角速度、加速度等）、语义级的对象融合（位置、属性）以及目标级其他属性融合（尺寸、类型等），各部分融合都可以单独看做状态估计问题。

3.3.1 状态估计问题的求解思路

  假设系统 k 时刻的观测量为$z_k$，状态量为$x_k$ ，这两个变量是符合某种分布的随机变量，且两者不相互独立。目的是需要根据初始状态量和观测量，获取当前 k 时刻的状态量$P(x_k|x_0,z_{1:k})$。根据贝叶斯法则，将系统状态的概率求解拆分如下：
$P(x_k|x_0,z_{1:k})\propto P(z_k|x_k)P(x_k|x_0,z_{1:k-1})$
对于上述，可以采取如下两类解法：

解法一：假设系统 满足马尔可夫性质，即 $x_k$ 仅与 $x_{k-1}$ 有关，与更早的状态无关；

  基于上述假设，系统状态的概率求解，可进一步简化为：
$$P(x_k|x_0,z_{1:k})\propto P(z_k|x_k)P(x_k|x_{k-1})$$
其中：

• $P(z_k|x_k)$为似然项，可由观测方程给出。（似然项是指结果已知的情况下，该事件在不同条件下发生的可能性）；
• $P(x_k|x_{k-1})$为先验项，可通过状态转移方程得到。（先验项是指根据历史规律推测该事件发生的可能性）。
• 这类问题可用滤波器相关算法解决，如卡尔曼滤波KF/EKF。

解法二：假设 k 时刻状态与之前时刻的状态相关；

  基于上述假设，根据状态受历史状态影响的不同，又可以进一步分为两种：

• k 时刻的状态与之前所有时刻相关：

$$P(x_k|x_0,z_{1:k})=P(x_{1:k}|x_0,z_{1:k})$$

• k 时刻的状态与前 s 个时刻相关：

$$P(x_k|x_0,z_{1:k})=P(x_{k-s:k}|x_0,z_{1:k})$$
  这类问题可用非线性优化方法来求解之前所有时刻的系统状态 $x_{1:k}$ 。

3.3.2 状态估计问题求解：卡尔曼滤波器

  卡尔曼滤波是一种时域递推算法，能够根据上一时刻状态的估计值和当前时刻状态的观测值推测出当前时刻状态的最优值，是一种能排除随机干扰，提高测量精度的方法。
首先需要进行状态方程和观测方程的构建，先明确几个变量的含义：

• $x_k$— 系统状态的真实值（通常无法得到）；
• $\overline{x_k}$— 系统状态的预测值（可以通过状态转移方程及上一时刻状态估计值$\widehat{x_{k-1}}$得到，也叫先验状态）；
• $\widehat{x_k}$— 系统状态的估计值（可以通过预测量及观测量联合估计得到，也叫后验状态）。

状态方程的构建：

  假设 k 时刻系统的状态量为$x_k$ ，包含位置和速度信息： x k = [ p k v k ] x_k=

$$\begin{bmatrix}p_k\\v_k\\ \end{bmatrix}$$ xk​=[pk​vk​​]，同时引入$x_k=\left[\begin{array}{cc}{\Sigma }_{pp}& {\Sigma }_{pv}\\ {\Sigma }_{vp}& {\Sigma }_{vv}\end{array}\right]$来表示 $x_k$ 各成员的不确定性和各维度之间的相关关系。
其中：${\Sigma }_{pp}$和${\Sigma }_{vv}$为状态分量的方差，${\Sigma }_{pv}$和${\Sigma }_{vp}$描述 p 和 v 之间的协方差。

  在已知 k-1 时刻状态量$x_{k-1}$ 情况下，如何预测 k 时刻的状态 $x_k$ ，也就是状态方程的构建。 这里可以通过运动学模型来构建状态方程，假设系统短时间内满足匀速运动的条件，那么

x k ˉ = [ 1 Δ t 0 1 ] x ^ k − 1 = F k x ^ k − 1 \bar{x_k} =

$$\begin{bmatrix} 1 & \Delta t\\ 0& 1\\ \end{bmatrix}$$ \hat{x}_{k-1} = F_k \hat{x}_{k-1} xk​ˉ​=[10​Δt1​]x^k−1​=Fk​x^k−1​
其中：

• $\overline{x_k}$为 k 时刻系统状态的先验分布

• ${\hat{x}}_{k-1}$为k-1 时刻系统状态的后验分布

• $F_k$为状态转移矩阵
    进一步，如果系统存在外部干预 $u_k$ 以及考虑系统噪声$w_k$ 时，该怎样对系统状态转换关系进行建模？
  $$\overline{x_k}=F_k{\hat{x}}_{k-1}+B_k{u}_k+w_k$$
  其中：

• $u_k$表示外部输入

• $B_k$表示外部输入与系统状态变化的转换关系矩阵

• $w_k\sim N(0,Q_k)$— 高斯噪声

  同样的，基于误差协方差传播定律，预测得到的先验状态量的误差协方差可以表示为：

考虑过程噪声$w_k$的影响，先验状态量$\overline{x_k}$的误差协方差$\overline{P_k}$：
$$\overline{P_k}=F_k{\hat{P}}_{k-1}{F}_k^T+Q_k$$

观测方程的构建：

  假设通过一些传感器得到了系统 k 时刻的观测量$z_k$，而观测量与状态量可能不是同一种表现形式下，需要通过线性变化H 来转换得到。
$$z_k=H_k{x}_k$$
进一步，传感器本身是存在观测误差的，考虑观测的高斯噪声$v_k\sim N(0,R_k)$情况下，可以整理为：
$$z_k=H_k{x}_k+v_k$$
即认为观测量$z_k$是 k 时刻的系统真实状态$x_k$经过线性变换H，再加上高斯噪声得到的。
同样的，之前通过状态方程预测得到的$\overline{x_k}$也可以经过线性变化转换到观测量$z_k$所在的维度。这样我们就拿到了两个表示 k 时刻系统的状态量（观测域下）：先验状态$H_k\overline{x_k}$和观测量$z_k$，现在就需要根据这两个带有噪声的状态量来对 k 时刻系统真正的状态量进行估计。

卡尔曼滤波原理的推导：

  常见的有两种方法，一种是利用高斯乘积定理和贝叶斯公式推导，可以参考：卡尔曼滤波基本公式推导（高斯乘积法）；一种是基于方差最小准则进行推导，可以参考：卡尔曼增益推导。此处不再赘述。

卡尔曼滤波的工作过程：

• 

  总的来说，卡尔曼滤波器的工作流程分为预测和更新两部分：

• 预测：

  • 输入：过去的最优状态$({\hat{x}}_{k-1},\widehat{P_k})$，外界对过程的影响$u_k$，环境的不确定度$Q_k$；
  • 输出：预测的当前时刻系统状态$({\bar{x}}_k,\overline{P_k})$；
  • 其他：对过程的描述转换$(F_k,B_k)$，与时间有关；
    $$\overline{x_k}=F_k{\hat{x}}_{k-1}+B_k{u}_k\text{\hspace{1em}(1)}$$
    $$\overline{P_k}=F_k{\hat{P}}_{k-1}{F}_k^T+Q_k\text{\hspace{1em}(2)}$$

• 更新：

  • 输入：预测的当前时刻系统状态$({\bar{x}}_k,\overline{P_k})$，观测值的状态$(z_k,R_k)$，状态量到观测量维度的变换矩阵$H_k$；
  • 输出：经过观测值修正后的最优估计状态$({\hat{x}}_k,\widehat{P_k})$；
    $$K=\overline{P_k}{H}_k^T(H_k\overline{P_k}{H}_k^T+R_k{)}^{-1}\text{\hspace{1em}(3)}$$
    $$\widehat{x_k}=\overline{x_k}+K(z_k-H_k\overline{x_k})\text{\hspace{1em}(4)}$$
    $$\widehat{P_k}=\overline{P_k}-K{H}_k\overline{P_k}=(I-K{H}_k)\overline{P_k}\text{\hspace{1em}(5)}$$
      补充：因卡尔曼滤波本身是基于线性马尔科夫性质进行求解的，当状态方程和观测方程不不满足线性系统时，可以考虑使用扩展卡尔曼滤波。主要差别在于，需要将原有状态方程中的状态转移矩阵$F_k$和观测方程中的测量矩阵$H_k$进行线性转换才可以使用。通过泰勒公式一阶展开来对其进行局部线性化展开，进而来实现线性系统的状态估计。展开过程可参考如下：
    

3.3.3状态估计-目标级运动属性融合建模

运动方程：
  在自动驾驶系统中，通常假设对象满足匀加速/匀速旋转运动，状态转移方程（运动方程）如下：
$$x_k==F_k{x}_{k-1}+B_k{u}_k+w_k$$
其中，$F_k$为状态转移矩阵，$u_k$为 k 时刻系统的输入量（通常为0），$w_k\sim N(0,Q_k)$，则可进一步简化为：
$$x_k==F_k{x}_{k-1}+w_k$$
观测方程：
  在自动驾驶系统中，通常假设对象满足匀加速/匀速旋转运动，观测方程如下：
$$z_k=H_k{x}_k+v_k$$
其中，$H_k$为观测矩阵，$v_k$为观测噪声，$v_k\sim N(0,R_k)$。

3.3.4 状态估计-语义级对象融合建模

3.3.5 状态估计-目标级其它属性融合

3.3.6关于卡尔曼滤波建模中的噪声协方差

参考资料：

1. 多传感器融合系列之感知后融合_多传感器融合感知-CSDN博客
2. 自动驾驶感知——多传感器融合技术_汽车多传感器融合-CSDN博客
3. 多传感器融合感知-深蓝学院 - 专注人工智能与自动驾驶的学习平台
4. （三十九）通俗易懂理解——卡尔曼滤波与扩展卡尔曼滤波