采用传感器融合的车辆前方碰撞预警系统

胡远志，董泰宏，罗 毅，蒲 浩

(重庆理工大学 汽车零部件先进制造技术教育部重点实验室，重庆 400054)

0 引言

自动驾驶汽车已经成为当前社会汽车行业发展的主流方向，汽车对主动安全系统的性能要求也因此提升。前碰撞预警系统FCW旨在提前对碰撞做出预警，避免或减轻由于追尾碰撞引起的伤害。设计良好的FCW需要对TTC特征参数进行深入研究。当前比较常见的基于雷达的车间TTC估计算法有一定的控制效果，但无法准确地识别出障碍物类别，同时对于一些边缘的静态障碍物会引发虚假警报，容易产生安全隐患。而基于单目视觉的车间TTC估计虽然能够准确地识别障碍物的类型，但是由于其自身的特点无法准确计算出TTC，容易造成车辆前方碰撞预警系统准确性、及时性差甚至失效的问题。

近年来，摄像头与激光雷达融合的技术在车辆领域逐渐发展起来[1]，薛培林等[2]提出一种激光雷达与相机特征级融合的城市自主车辆实时目标识别方法，融合识别系统在准确率、实时性方面达到实际行驶要求；胡远志等[3]提出一种基于4线激光雷达与摄像头融合的联合测距的方法，改善了单目视觉目标识别有着巨大优势，但在目标测距方面存在精度不足且测量过程不稳定的问题；HU等[4]在越野复杂环境中,分析了传感器融合的优势,阐述了单一传感器在范围、信号特征和检测工作条件方面存在的局限性；刘锴[5]使用多激光雷达与相机改善了场景识别的正确率；麦新晨等[6]针对城市道路环境提出了一种基于激光雷达与视觉的车辆检测与跟踪方法。受上述案例启发，本文针对视觉传感器检测车间TTC精度低、稳定性差的问题，设计了一种基于激光雷达与摄像头融合的车间TTC估计算法，结合2种传感器的优势，提升了TTC估计的稳定性、降低了估算误差。

1 车道前方车辆目标检测

基于激光雷达与单目视觉结合的前方车辆识别方法主要有2个步骤：系统工作前预先对激光雷达和摄像头进行联合标定，确定雷达坐标系和摄像头坐标系间的变换关系[7]；经过视觉识别车辆目标后，通过激光雷达点云聚类确定前方车道的障碍物点云，筛除不在区域内的车辆目标，得到最终位于车道前方的车辆信息。

1.1 传感器数据配准与同步方法

传感器数据需要从空间以及时间上配准，空间的配准通过传感器之间的标定完成，时间配准通过对时间戳的匹配以及帧率同步完成。

空间上激光雷达和摄像机的标定数据包括相机内部参数和坐标系间变换外部参数。相机内部参数主要包括相机焦距、像素中心点、畸变系数等。外部参数包括相机坐标系相对于雷达坐标系的旋转和平移。

设点在激光雷达雷达坐标系O1中的坐标为(X1,Y1,Z1)，对应在相机坐标系OC中的坐标为P(Xc,Yc,Zc)。

激光雷达点云到相机坐标系的转换为刚体变换，可通过旋转与平移表征，公式[8]如下：

(1)

式中：[R|T]为旋转平移矩阵，表示激光雷达坐标系通过空间旋转与平移转换到相机坐标系，构成外参矩阵。

从相机坐标系到图像坐标系的转换，是从三维坐标系转换为二维坐标系的过程，属于透视投影关系满足三角形的相似定理：

(2)

从图像到像素的转换主要涉及伸缩变换和平移变换：

(3)

综合式(1)(2)(3),可得到激光雷达坐标系往像素坐标系的转换公式:

(4)

式中:f为相机焦距，fx、fy分别为相机横、纵焦距，(u0,v0)为光学中心，这4个参数属于相机内参，构成内参矩阵,表征相机坐标到像素坐标的变换，内参矩阵可由张正友标定法得到[9]，(x,y)为对应点在图像坐标系的坐标,(u,v)为点在像素坐标系的坐标。

不同传感器的采样频率不同，每个传感器会按照自己固定的时间基准进行数据采集，从而导致两者采集的数据来自于不同的时刻，使数据存在时间戳上的不同步，激光雷达的采样频率为10帧/s，相机的采样频率为30帧/s，无法提升激光雷达的采样频率达到与相机一致，因此，以采样频率低的激光雷达为准，对相机数据进行采样，每3帧数据保留1帧，实现相机的采样频率为10帧/s，以达到时间戳匹配，实现时间上的数据匹配。

1.2 点云聚类检测障碍物

通过激光雷达直接获取的点云数据分布不均匀且量大不便于处理。点云数据是激光反射形成的点云，能够直接反映目标的外表面信息，但在处理数据中，更多的是处理几何拓扑信息。因此，建立离散点间的拓扑关系是点云处理的首要步骤。目前广泛应用在点云数据处理中的是建立空间索引方法，常见空间索引一般是自顶向下逐级的划分空间。代表性的k-d树最常应用于点云处理。

1.2.1k-d 树创建

k-d树是计算机科学中使用的一种数据结构，用来组织表示k维空间中的点集合，它是一种带有约束条件的二分查找树[10]。建立k-d树最高效的方法是，像快速分类一样使用分割法，把指定维度的值放在根上，在该维度上较小数值的放在左子树，较大数值的放在右子树，然后分别在左右子树上重复这个过程[11]。

由于只需要考虑当前车道前方的障碍物，为了节省计算资源，可以先过滤掉激光雷达左右2 m外以及后方的多余点云，仅保留当前车道前方的点云。

1.2.2欧式聚类

欧式聚类算法是一种基于欧式距离度量的聚类算法。对于三维点云，两点之间的欧式距离计算公式为：

(5)

欧式聚类算法的流程：

1)对输入的点云创建其k-d树数据结构；

2)对任意一个点Q，遍历其余点与Q的欧式距离E，将E小于设定阈值的点均放入聚类集合C中；

3)针对每一个还没有被计算的点循环上述操作，直至所有点都被处理。

最终产生的聚类集合C则为点云聚类所检测到的障碍物。通过几何关系筛选出障碍物点云的尾部点云，并通过障碍物尾部点云计算两车之间的相对距离。

1.3 车道前方车辆识别

2012年，Krizhevsk[12]提出了基于卷积神经网络的图像分类。此后大量关于图像分类、目标检测的网络模型都是基于该基础模型不断演化产生。目前主流的有Faster R-CNN[13]、SSD[14]、YOLO等。

本文采用YOLOv3作为车辆的检测算法，使用经过COCO数据集训练后的权重文件。YOLO全名You Only Look Once[15]，是一种快速和准确的实时对象检测算法。YOLOv3算法的主干网络为Darknet-53，使用残差网络进行特征提取，使其在加深网络的情况下又解决了梯度消失的问题，并能输出3个不同尺度的特征图，用于检测不同大小的物体。相比于它的前两代版本，YOLOv3弥补了小目标检测精度差的短板，同时保留了速度的优势。

在目标检测算法检测出图像中的所有车辆之后，将激光雷达聚类检测出的前方障碍物点云投影到图像中，仅保留含有一定点云阈值的目标检测框，以此作为最终的车道前方车辆识别结果。

由YOLOv3算法识别图像中所有车辆效果如图2所示，它包括了除车道以外的所有车辆检测结果。加入了激光雷达筛选系统后的最终检测效果(图3)，仅保留了当前车道前方的目标车辆识别框，舍弃了道路边缘以及旁边车道的无关车辆的识别框。

图1 传感器位置示意图

图2 传感器坐标变化流程框图

图3 车道前方车辆识别系统框图

图4 筛选前的车道前方车辆识别场景图

图5 筛选后的车道前方车辆识别场景图

2 车辆目标跟踪

车辆目标可以用一组恒定不变的特征集合来表达。采用基于目标特征的跟踪方法：在连续帧图像中提取车辆的特征信息(车尾部特征)，将车辆的特征以旋转不变性的方法进行描述，在下一帧中通过特征匹配进行车辆的跟踪。

2.1 特征提取

车辆目标的变化比较稳定，各个尺度的特征突变小，因此采用FAST提取特征[16]。FAST特征提取算法提取速度快、实时性强，算法步骤如下：

1)以像素p为中心，半径为3的圆上，有16个像素点(p1,p2,p3,…,p16)。

2)定义一个阈值，计算p1、p5、p9、p13与中心p的像素差，若都超过阈值则中心点p为候选特征点。

3)计算候选特征点与(p1,p2,p3,…,p16)的像素差，若至少有连续9个超过阈值，则判断其为特征点。

4)对特征点进行非极大值抑制，选取以特征点p为中心的55邻域，若邻域内有多个特征点，则判断每个特征点各自的像素差总和s，保留s最大的特征点[17]。

(6)

使用FAST特征提取算法检测的效果如图6。

图6 图像特征提取效果图

2.2 特征描述与匹配

使用BRISK特征描述对FAST提取的特征点赋予其描述符[18]，仅需要对车道前方车辆检测框内的特征点进行描述。BRISK特征描述采用自定义的邻域采样模式，以特征点pi为中心的40×40像素块内构建多个同心圆，采样点等间距分布在圆周上，共60个采样点[19]。

图7 BRISK邻域采样模式示意图

采样完成后依据欧式距离划分采样点对集Ω为短距采样点对集S和长距采样点对集P。

S={(pi,pj)∈Ω∣pj-pi<δmax}⊆Ω

(7)

P={(pi,pj)∈Ω∣pj-pi>δmin}⊆Ω

(8)

阈值δmax=9.75t，δmin=13.67t。

使用长距采样点对集P来计算特征点的主方向，然后将采样区域旋转到主方向，最后在短距采样点集S中逐一比对与采样点的像素强度，形成二值化特征描述。

对BRISK描述子得到的二值化特征描述，采用蛮力算法进行前后两帧之间的特征搜索匹配，若前后两帧之间的特征匹配大于一定的程度，就认为是同一目标，以此实现车辆目标的跟踪。

3 传感器融合计算TTC

TTC(time to collision)是指前后两车某时刻保持状态不变时两车相互碰撞所需的时间。针对乘用车FCW系统，TTC的预警值一般设置为2.7 s，当提前2.5 s给予一个车辆碰撞警告，根据人的反应时间和刹车的距离，可以做到车辆刹停下来。

3.1 基于单目视觉的TTC计算

在前车突然停止或主车减速以避免碰撞时，TTC的计算避免不了两车之间相对加速度的影响，本文采用一种常值加速度假设的TTC计算方法[20]。根据前车尺寸在图像中的变化率可以直接估算TTC。

图8 单目摄像头几何投影原理示意图

设dw为图像中车宽相对变化率，w为图像中车辆宽度，有：

(9)

根据相似三角形原理，焦点与车辆尾部W构成的三角形相似于相机成像的三角形，则焦距f、成像宽度w、车辆宽度W、车距d的关系如下：

(10)

结合式(9)(10)可得到车宽相对变化率dw与车距之间的关系：

(11)

则车宽相对于时间的变化率如下：

(12)

考虑到相对加速度，两车之间的相对距离模型为(v1为第一帧与第二帧之间的两车相对速度)：

(13)

将相对距离模型式(13)代入式(12):

(14)

由于是常值加速度模型，代入第二帧以及第三帧的数据可得到加速度的表达式：

(15)

在式(14)中，相机的帧率是10帧/s，则dt=0.1，d2t=0.01，当作分子可忽略，于是：

(16)

TTC是d=0时的时间：

(17)

(18)

约去(18)等式两端的d2：

(19)

根据式(19)，TTC仅与图像中车宽相对变化率dw存在关系，而dw可直接通过两帧图像之间的变化得到，遂可解方程得到TTC。

3.2 基于激光雷达的TTC计算

激光雷达测速可采用帧距离差法，连续两帧数据均可得到与目标的距离，每帧数据的时间间隔固定不变，即通过两帧之间的距离变化与时间的对应关系可得到车间相对速度的大小与方向。同样考虑到相对加速度，采用常值加速度假设的TTC计算方法。

图9 激光雷达测距原理示意图

设v1为第一帧到第二帧的相对速度，v2为第二帧到第三帧的相对速度，有：

(20)

(21)

根据三帧之间的数据可求得相对加速度：

(22)

代入常值加速度模型的TTC计算公式：

(23)

式中：第一帧时两车距离d0、第二帧时两车距离d1、第三帧时两车距离d2可以由点云直接得到，即求出每一帧两车之间的距离后，就可解方程求得TTC。

3.3 卡尔曼滤波

基于单个传感器设计的TTC估计受限于传感器本身的特点，结果的异常值较多，鲁棒性差，采用卡尔曼滤波从结果级融合传感器。

卡尔曼滤波器是一种最优化递归处理数据的算法，当系统不存在完美的数学模型、系统的挠动不可控很难建模并且传感器测量存在误差时，卡尔曼滤波算法能很好地根据预测数据以及观测数据得到系统最优的状态估计[21-22]。传感器融合计算TTC刚好具备这几点特性。

(24)

式中:xk为预测值，zk为观测值，wk-1为过程噪音，vk为测量噪音，两者近似符合高斯分布，A为传递矩阵，H为观测矩阵。预测部分由先验估计、先验误差协方差矩阵组成，校正部分有卡尔曼增益计算、后验估计、后验误差协方差矩阵更新。

先验估计值：

(25)

先验误差协方差矩阵：

(26)

卡尔曼增益计算：

(27)

后验估计值：

(28)

后验误差协方差矩阵更新：

(29)

卡尔曼滤波算法的核心是运用递归算法达到最优状态估计：利用先验估计值和现时刻的观测值更新当前状态变量的后验估计。在TTC计算中，激光雷达由于传感器本身的优势，与摄像头相比表现得更加稳定，也更加接近真实值，遂选用激光雷达完成预测步骤，得到先验估计值，摄像头则作为观测传感器，提供观测值完成状态变量后验估计的更新。

4 验证与分析

为了验证本文提出的基于传感器融合的车间TTC估计算法，通过KITTI数据集中一段匀减速工况进行验证。图10中两车距离初始值为11.3 m，目标车辆速度为0 m/s，本车初始速度为2.7 m/s，制动减速度大小为0.34 m/s2。

图10 TTC计算实景

在逐渐匀减速靠近前方车辆时，两车距离变化如图11。图12为传感器融合计算的TTC曲线与单个传感器计算的TTC曲线相对于实际TTC曲线的关系。

图11 本车与目标的距离变化曲线

从图12可以看到，基于单目视觉的TTC估计算法出现异常变化值的概率较高，是由于单目视觉TTC估计是基于两帧图像间车辆尺寸的像素变化率求得，而图像中的车辆像素区域是车辆识别算法分别对每帧图像数据处理得到的，在每次的计算中不可避免的会产生像素区域的突变问题，造成TTC估计值突变；同时除去异常值，其剩余稳定值整体偏离实际值幅度也较大，是因为摄像头本身的测距原理是利用相似三角形的特性，受限于摄像头本身的参数精度问题，单目视觉TTC估计值的精确度较差。

图12 TTC计算值曲线

与单目视觉相比基于单激光雷达的TTC估计曲线比较稳定，整体偏离实际曲线较小，因为激光雷达的测距精度高，其采集的点云数据稳定，而激光雷达的TTC估计是基于两帧之间目标距离的变化率求得，因此激光雷达的TTC值误差小，异常变化值出现概率低。

本文提出的基于传感器融合的TTC估计算法以激光雷达数据作为预测值，单目视觉数据为观测值，融合得到的TTC估计值整体准确度表现更加优良，稳定的收敛于实际曲线附近，表明该算法有效提升了TTC估计的稳定性，降低了总体估计误差。

5 结论

基于自动驾驶汽车常见搭载运用的激光雷达、摄像头2种传感器，介绍了激光雷达与摄像头基于卡尔曼滤波融合测量车间TTC的计算方法，在匀减速工况中进行验证，证明其能够改善TTC估计的稳定性与准确性，为FCW系统的参数优化提供了依据。

该方法的不足之处在于：单个传感器TTC计算的模型为常值加速度假设模型，在匀速行驶以及匀加速度行驶工况下表现良好，但在日常驾驶场景中会出现两车的相对加速度不断变化，此时算法的精度将受到影响。针对此问题，提出一种解决思路：加入预测模型估计未来状态，以提升模型在两车的相对加速度在不断变化的实际工况下的适应性。