自动驾驶激光雷达、摄像头、毫米波雷达融合算法
无人驾驶汽车多传感器冗余下的数据融合算法研究
【论文】详见知网链接、ELSEVIER链接、IEEE链接，【开源项目】详见github链接
【编译运行】详见DWD_sensor_fusion编译运行
目录
- 无人驾驶汽车多传感器冗余下的数据融合算法研究
- 0. 主要成果
- 1.主要研究内容
- 2.多传感器多目标检测
-
- 2.1激光雷达感知cnnseg算法
- 2.2 摄像头感知算法
- 2.3.基于激光对象的毫米波Box-Muller算法多目标生成
- 3.传感器坐标变换及多目标跟踪
-
- 3.1 激光雷达及毫米波雷达目标坐标变换
- 3.2 多传感器多目标跟踪
- 4.多传感器动态冗余权重分配算法研究
-
- 4.1 动态权重分配算法概述
- 4.2 考虑方差偏差均值的动态权重分配算法
- 4.3 考虑方差变化率的动态权重分配算法
- 4.4 传感器动态权重分配下降及传感器退出测试
- 4.5 权重归一化及权重分配规则
- 4.6 最终权重分配结果
- 5. 基于动态冗余权重的多传感器数据融合
0. 主要成果
1.建立了基于激光雷达、摄像头、毫米波雷达的多传感器冗余系统，检测端使用检测激光雷达对象的CNN_SEG、检测摄像头对象的YOLOV4以及生成毫米波对象的Box-Muller算法，在跟踪端又对每一个传感器的对象使用了卡尔曼滤波与匈牙利匹配相结合的SORT算法。
2.建立了基于卡尔曼滤波方差的多传感器冗余权重分配系统，该系统搭载了自研的DWD算法，该系统采用了考虑方差平均偏差的权重分配以及考虑方差平均变化率的权重分配，并且辅以传感器权重下降机制及传感器退出机制，最后通过权重归一化得到最终各传感器权重，并通过实验查看权重分配曲线，期间也能够看出权重下降机制及传感器退出机制的作用
3.建立了基于多传感器动态冗余权重的多传感器融合感知系统，该系统利用DWD算法对各传感器分配的权重作用于融合端对多传感器的融合提供了依据，某传感器权重越高，在融合过程中该传感器对融合结果的决定性作用越高，反之，传感器权重越低，在融合过程中该传感器对融合结果的决定性作用越低，如果传感器权重为零，即该传感器退出。
1.主要研究内容
1.前向冗余多传感器感知系统建立：
选定了三维激光雷达-毫米波雷达-视觉的前向冗余多传感器感知系统。激光雷达使用CNN_SEG算法，摄像头采用YOLOV4算法，毫米波雷达采用Box-Muller算法。
2.多传感器坐标变换数据对齐问题：
采用摄像头坐标系为基础坐标系，相对于彩色相机的坐标变换需要旋转矩阵及平移矩阵，有了这两个矩阵就可以将两个传感器的坐标系进行坐标变换，并将其他传感器如激光雷达、毫米波雷达的坐标转换到摄像头坐标系下。
3.传感器多目标跟踪状态估计问题：
世界的本质是非线性的，对于智能车辆来说，其感知的外部世界更是如此，十分有必要将非线性状态估计方法引入智能车辆多目标跟踪中，对关联好的目标进行滤波，进而得到该传感器目标跟踪状态，在传感器跟踪部分,采用了SORT（Simple Online And Realtime Tracking）算法,该算法包括匈牙利匹配数据关联以及卡尔曼滤波轨迹跟踪。
4.多传感器前向冗余权重分配：
通过各传感器的方差平均偏差、方差平均变化率，并给予每个传感器相应的权重，对传感器进行故障模拟，整个系统依然能对目标进行准确的感知，本文开发了一种动态权重分配算法——DWD（Dynamic Weight Distribution）算法。
5.多传感器目标关联与信息融合问题：
对各个传感器目标信息进行融合，得到对各传感器所检测到的同一目标的一致性的解释。将一些发生故障或检测精度下降的传感器目标进行剔除或者降低其对融合结果的影响。
2.多传感器多目标检测
2.1激光雷达感知cnnseg算法
CNN_SEG算法采用了基于鸟瞰俯视方向（Bird View, BV）点云训练的神经网络进行障碍物检测。其能够识别的对象类型有六类（car, bicycle, track，bus, pedestrian, sign and others）。测试所用的点云数据是Kitti开源数据。感谢知乎大神@牟加俊apollo感知算法解析之cnn_seg
cnnseg其实分为两步，第一步是用一个深度学习网络预测五层信息，center offset, objectness, positiveness, object height, class probability，第二步则是利用这五层信息进行聚类[14]。具体步骤如下：
-
根据objectness层信息，检测出障碍物栅格点目标
分别用图像和点云来说明objectness层的作用。objectness输出的是一个[0,1]的二维矩阵，与点云的栅格图对应（在cnn_seg中，对点云构建了2.5D栅格，每个栅格对应8维特征）。通过对objectness置信度取阈值（默认0.5），就可将object的像素点检测出来。下图用点云的反射强度信息显示被检测出的区域。
-
根据center offset层信息，对检测到的障碍物栅格点进行聚类，得到聚类目标
center offset是一个两层的二维矩阵，分别是x轴的offset和y轴的offset，将这两层结合在一起，就对应了每个栅格的offset，用箭头的方向和长短表示offset。
-
根据positiveness和object height层信息，对背景以及每个点云簇中高处的点进行过滤
positiveness是一个filter信息。从上一步其实可以看到，得到的点云簇，有很多的背景区域。通过统计每个点云簇的positiveness的均值，在经过阈值（默认0.1），就可以过滤掉一些背景的聚类了。
object height也是一个filter信息，过滤掉栅格中较高的点。
- 根据class probability，对每个聚类目标进行分类，得到最终的检测目标
通过以上几步，cnnseg已经将点云分割成一个个的点云簇了，类型分别是大车，小车，行人，自行车，未知。通过统计每个点云簇的每一类的概率值，得分高者为其类，从而实现了对每个点云簇的分类，得到最终的target。
下图中，用红色表示小车，蓝色表示大车，绿色表示行人。
下图展现了该算法检测出的对象，如图中方形盒子所示，上面还有各对象的类型及距本车的距离。
下图检测后的点云话题输出，可以看出路边的车辆以及对向行驶的一辆车。另一张为检测到的目标的话题输出，可以看出各种类型的对象检测的都比较正确，至此，该部分算法测试完成。
2.2 摄像头感知算法
本次训练采用Xeon W-2150B/3.00GHz CPU以及两张2080Ti GPU进行训练，使用10000张Kitti数据集，共训练50000次，训练时长约68小时，训练类别一共有5类（pedestrian、car、bicycle、truck、bus）右图为训练时loss值（公式如下）记录：
对训练好的权重文件进行AP（Average Precision）及mAP（mean Average Precision）检测精度测试，其结果如表2-1，检测精度处在一个较高的水平：
| 类别 | AP（%） |
|---|---|
| Pedestrian | 89 |
| Bicycle | 86 |
| Car | 94 |
| Truck | 78 |
| Bus | 79 |
| mAP | 85.2 |
| 该部分算法的检测效果，使用的输入图像为Kitti数据集的左边彩色相机的话题，话题名为/left_color_cam，得到的检测结果，橙色框为car，蓝色框为truck。由图中可以看出YOLOV4对各种对象的的检测效果均比较好。 | |
 |
2.3.基于激光对象的毫米波Box-Muller算法多目标生成
由于传感器检测目标大致处于一个高斯分布，所以采用激光雷达目标的上下左右四个边缘加入高斯分布的随机数来处理激光雷达目标，得到的结果作为毫米波雷达目标，由于C++语言可以用rand函数产生均匀分布的随机数，所以本研究拟采用Box-Muller算法[16]，该算法能根据均匀分布产生高斯分布，具体算法见知网链接
经过测试，决定采用范围为(-30,30)的高斯分布，这样就需要一个期望为0，方差为10的高斯分布，即Y~N（0，10）。实际输出如下图，该分布为一个期望为-1.891986493192650，方差为9.79255102687986的高斯分布。
3.传感器坐标变换及多目标跟踪
3.1 激光雷达及毫米波雷达目标坐标变换
坐标变换的方法采用了Geiger, A等对kitti数据集进行坐标变换的方法，最终将激光点云转换到相机坐标系下，并生成毫米波雷达目标对象。坐标变化代码github有。
对象散点图
3.2 多传感器多目标跟踪
卡尔曼滤波模块的主要功能是通过卡尔曼滤波算法对已识别到的目标进行下一帧的状态预测和最优估计。卡尔曼滤波过程：
SORT（Simple Online And Realtime Tracking）算法[19]包括匈牙利匹配及卡尔曼滤波。
首先对匈牙利匹配进行说明，上下两图分别为T1时刻和T2时刻，假设T1时刻成功跟踪了某个单个物体，ID为1，绘制物体跟踪BBox（紫色）。T2时刻物体检测BBox总共有3个（橙色），预测T2时刻物体跟踪的BBox（紫色）有1个，解决紫色物体跟踪BBox如何与橙色物体检测BBox关联的算法，就是SORT跟踪算法要解决的核心问题。
经检验，SORT部分程序实现IOU匈牙利匹配，匹配过后的目标位置输出能够很好的用于后续的卡尔曼滤波。使用2D MOT 2015 benchmark dataset[20]开源数据集对该模块进行测试，多对象匹配指标如下，大部分的对象匹配度比较高，匹配模块性能良好
4.多传感器动态冗余权重分配算法研究
4.1 动态权重分配算法概述
在权重分配过程中，研究了一种动态权重分配（DWD，Dynamic Weight Distribution）算法，首先设定方差初始值为1，查看多个跟踪良好的对象的方差变化如图。将跟踪超过20帧的对象筛选出来，针对这些对象的方差稳定值，通过求算数平均确立一个方差中心稳定值σ0=0.65514125，将其设置为方差初始值，初始值σ0能够使对象的方差快速收敛。
在长时间测试过程中，所有跟踪器的方差输出如下，图中出现了一些离群点，对于这些离群点，应当在设计方差门限值时予以剔除。
在剔除离群点之后，发现所有的方差都在σmax=0.6730585，σmin=0.639224范围内波动，并且大部分点都集中在σ0初始值位置，所以将最大门限设置为σmax，最小方差门限设置为σmin。
4.2 考虑方差偏差均值的动态权重分配算法
具体算法见论文知网链接
对于激光雷达、毫米波雷达和摄像头对象均采用此方法进行处理，平均偏差越大，就说明传感器的检测对象可能出现了问题，权重就越小。
4.3 考虑方差变化率的动态权重分配算法
具体算法见论文知网链接
对于激光雷达、毫米波雷达和摄像头对象均采用此方法进行处理，平均变化率越大，就说明传感器的检测对象可能出现了问题，权重就越小。
4.4 传感器动态权重分配下降及传感器退出测试
主要是测试系统的自动权重分配下降机制和传感器退出机制是否起作用，当某一传感器处于检测或跟踪效果不好的情况时，该系统应该对该传感器的权重予以降低，严重时将直接使该传感器退出。通过对每个帧使用大的高斯噪声处理来检查各传感器的权重是否会减少或退出。
4.5 权重归一化及权重分配规则
上述的考虑方差平均偏差的权重分配以及考虑方差变化率的权重分配，两部分各分配0.5的权重，归一化就是把数据变成(0,1)之间的小数。本研究采用的权重归一化方法为区间放缩法，并且是反式的区间放缩法，即原数值越大在将区间放缩到(0, 1)之间后数值越小。
根据上一小节所述，根据传感器是否退出可以得到以下八种情况，O为未退出，X为退出：
4.6 最终权重分配结果
DWD算法通过以上初始值设定、考虑方差平均偏差的权重分配、考虑方差平均变化率的权重分配、传感器权重下降及传感器退出机制、权重归一化，得到最终的多传感器权重，另外，该系统不止适用于三传感器感知系统，如需增减感知传感器，只需要更改传感器动态权重系统的规则，总体思想不变，所以该系统有很强的延展性。
系统大部分时间都在进行三传感器的权重分配，但是系统中也有一些时间进行了两传感器的权重分配，例如5-10帧、170-185帧附近，另外还有一些时间进行了单传感器权重分配，例如155帧附近、158帧附近、160帧附近、220帧附近、233帧附近，最特殊的，系统也进行了三传感器同时退出的权重分配，由于三传感器均退出时权重分配按照1/3平均分配，所以平均分配的情况即为三传感器均退出的情况，例如163-170帧、212-216帧。
5. 基于动态冗余权重的多传感器数据融合
具体算法见论文知网链接
下图中C31、L16、R17融合成为了一个对象，C33、L17、R18融合成为了一个对象。
下图中C14、L4、R4对象成功融合成一个对象，C11、L5、R5成功融合成为一个对象，另外该帧也存在一个特殊情况，即C18对象只有摄像头成功跟踪，但是激光雷达和毫米波雷达都没有对该对象的跟踪，但是检测框均有输出，所以归对于该对象的融合也是成功的。
使用动态冗余权重与不使用动态冗余权重直接进行融合相比，能够成功融合成目标的帧数明显增多，对比结果如下。图中绿色为使用了动态权重的融合结果，蓝色为未使用动态权重的结果，在使用动态冗余权重后，产生融合对象的帧数由182帧提高到228帧，该数据集总帧数250帧；另外融合对象数量也有明显提升，将每帧融合成功的对象求和，融合成功的对象数由827个提高到1089个，这说明加入动态冗余权重对多传感器融合有积极作用，能够提高融合的成功率。
【论文】详见知网链接、ELSEVIER链接、IEEE链接，【开源项目】详见github链接