基于激光雷达和摄像头的FSAC赛车锥桶感知算法研究

王华东，李 刚，李晓纲，张 东

(1.辽宁工业大学汽车与交通工程学院，辽宁锦州 121001；2.林肯大学，英国林肯市 LN1 1AB；3.布鲁内尔大学，英国伦敦市 UB8 3PH)

随着社会发展和科学技术的不断进步，人们越来越追求社会发展的智能化，汽车作为最主要的交通工具之一，早已成为人们生活不可或缺的一部分[1]。科技的发展和进步，带动智能汽车产业迅速发展，许多企业开始启动智能汽车的研究。为了培养新一代汽车科技创新人才，中国汽车工程学会(SAE)举办了大学生无人驾驶方程式大赛(FSAC)[2]。同智能网联汽车一样，无人驾驶方程式赛车主要分为3大模块：环境感知、决策规划以及运动控制。环境感知技术是实现无人驾驶的第1步，其结果的好坏对于决策规划和运动控制有重要的影响，近年来成为许多学者研究的热点问题。

WANG等[3]提出了激光点云与图像实时融合的方法，利用YOLO检测ROI内障碍物的点云，在自动驾驶数据集上验证，结果表明该方法对车辆的检测有较高的精确度和实时性。LEE等[4]提出了一种基于激光雷达和相机融合的道路检测方法，引入球面坐标变换来减小3D激光雷达数据点云之间的差距，并将相机的颜色数据和3D激光雷达的高度数据转换到同一个球面坐标下，然后送入到卷积神经网络中训练以实现路面分割。KOTUR等[5]提出了将LiDAR原始点云集成到相机的图像中，并使用深度学习方法检测相机图像中的对象，结果表明二者融合的方法对车辆的检测效果最好。DANAPAL等[6]提出了一种基于YOLO的改进传感器融合框架，联合处理来自相机和LiDAR的原始数据，该框架在车辆检测性能方面优于传统的决策融合框架，与传统的融合框架相比精度提高了5%。RAVINDRAN等[7]提出了一种贝叶斯神经网络用于相机和雷达传感器的融合，该方法提高了检测精度并降低了传感设备在不同驾驶情况下的不确定性。GU等[8]将离散的3D激光雷达点云转换成连续的2D激光雷达点云图像，提出了一种基于距离感知高度差的检测方法，以快速获得道路估计，并在摄像头的部分应用轻量级迁移学习的方法实现对道路的分割。MEYER等[9]提出利用激光点云和相机图像相结合的方式来实现三维对象检测，通过训练深度卷积神经网络来实现对车辆的检测。DING等[10]提出了一种基于深度相机的视觉定位方法，该方法通过使用改进的激光地图，引入了一种地图学习算法对激光地图中的集合进行采样，具有成本低，检测精度高的优点。王刚等[11]提出基于深度学习的三维目标检测改进方法，使用激光雷达点云与图像融合的方式，提高了对车辆检测的效果。ASVADI等[12]提出了基于激光雷达和彩色摄像机数据融合的车辆检测体系，利用单目彩色相机校准激光雷达，提高对车辆的检测效率。代加喜等[13]提出了一种基于激光雷达的锥桶检测算法，该算法对RANSAC算法进行了优化并用于地面的剔除，但在实际应用中可能会出现角度偏差，导致赛车触碰到较远位置的锥桶。PARK等[14]针对单独使用激光雷达或相机进行3D重建时检测密度和精度较低的问题，提出了一种用于高精度深度估计的深度传感器融合框架，解决了对于未校准的激光雷达和深度相机进行3D重建精度较低的问题。王凯歌等[15]对传统欧式聚类算法进行改进，较好地解决了传统欧式聚类选取大距离阈值时造成的欠分割问题。

上述研究主要针对道路车辆的检测，由于目标相对较大，易于实现，而基于深度学习的目标检测算法对车辆硬件的计算能力要求较高。对于方程式赛车，需要检测的目标锥桶相对较小，如果仅用摄像头进行检测，受光照、天气等环境干扰大，且由于比赛车速高，要求算法实时性好。本文根据比赛实际需要，针对赛场环境下方程式赛车对锥桶检测率较低的问题，进行基于激光雷达和摄像头的标定融合锥桶检测算法研究，将锥桶点云的空间位置信息与颜色信息相融合，解决赛场环境下锥桶检测的问题。

1 锥桶空间位置信息获取

赛场的环境比较空旷，需要检测的目标锥桶尺寸较小，并且赛道两侧锥桶之间的距离较近，锥桶摆放较为密集，考虑到算法的实时性和准确率，需要对激光雷达检测的范围进行合理限制，对距离车体较近的锥桶进行检测，舍弃较远的锥桶；由于赛场上锥桶的尺寸是确定的，因此可以结合激光雷达在车体中的位置限制激光雷达检测的宽度和高度，提高对锥桶的检测准确度。

无人驾驶方程式赛车采用以激光雷达为主、摄像头为辅的感知策略，基于激光雷达的锥桶检测算法流程如图1所示。利用Pandar40线激光雷达采集环境中的原始点云，进行感兴趣区域(ROI)提取、体素滤波等数据预处理工作，拟合并剔除地面点云，对锥桶及其附近的点云进行欧式聚类，将提取出聚类的中心点作为锥桶的空间位置信息。

图1 锥桶检测算法流程图Fig.1 Flow chart of cone bucket detection algorithm

1.1 感兴趣区域(ROI)的提取

图2 激光雷达和摄像头的安装位置图 Fig.2 Installation position diagram of the LiDAR and camera

为了更加精确地限制激光雷达在X，Y和Z轴上的扫描距离，依据大赛规则，以激光雷达的中心作为坐标系的原点，通过限定扫描的半径来限制激光雷达的扫描范围，有效滤除近处打在车体上的点云和远处无用的点云。激光雷达和摄像头在赛车上的具体安装位置如图2所示。

1.2 体素化网格算法

为降低系统执行机构的反应时间、保证锥桶检测算法的实时性，采用体素化网格算法[16-17]对提取ROI之后的锥桶点云进行下采样。计算下采样之后的锥桶点云在各个坐标轴方向上的最大值和最小值之差，确定出一个能充分容纳所有点云最大的长方体，根据锥桶大小划分出立方体，边长设定的大小取决于激光雷达检测出锥桶点云数量的多少，过程如下。

1) 计算出待检测区域内目标锥桶的分布范围，进而确定采样的分辨率和立方体栅格的大小，设定立方体长度为L。

2)计算立方体栅格和每个点的对照关系，假设某点的点云坐标值为(x,y,z)。

3)对应的立方体栅格编码为(i,j,k)，由式(1)计算得出。

(1)

4) 计算立方体栅格的重心坐标，其中sum为激光点云的激光点总数，见式(2)。

(2)

5) 对该帧点云中的所有激光点云进行遍历，并且重复以上步骤，就可以得到立方体栅格和每个激光点的对应关系。

1.3 随机采样一致性算法(RANSAC)

为了提高锥桶检测算法的运行效率，根据激光雷达的位置和地面上的锥桶高度，使用随机采样一致性算法(RANSAC)对地平面拟合并分割[18]，设置合理的参数，过滤掉地面的点云，从而获得只包含锥桶的点云。RANSAC[19-20]实际的拟合情况如下：随机从激光雷达检测到的点云中选取3个点，引入一个允许误差的范围值，找到误差范围内所有的点云并记录个数，重复以上2个步骤，经过反复迭代，直至找到一个尽可能包含更多点的平面，拟合结束。包含最多点的那次拟合所确定的平面即为最终需要过滤掉的地面点云。经过该算法处理，实现将地面点云和锥桶点云分开，对地面点云滤除，为下一步的锥桶点云聚类奠定基础。

1.4 欧式距离聚类算法

为使锥桶点云数据尽可能聚集成簇群，考虑算法的实时性和赛场环境下锥桶之间的距离，采用基于欧式距离聚类的方法对锥桶点云进行聚类。通过以上点云处理可以有效降低点云数量，尽可能只留下锥桶的点云。

定义欧式距离来描述2个点集之间的距离：

图3 欧式距离聚类流程图Fig.3 Flow chart of Euclidean distance clustering

(3)

2个点集中的点索引为i，j。

欧式聚类算法在进行预处理时采用Kdtree树来实现，预处理可以加快算法的运行效率，如图3所示。

经过欧式距离算法处理之后，聚类前后点云只保留目标锥桶的点云数据，滤除除目标锥桶之外的噪声点，锥桶检测算法处理的流程效果图如图4所示。

图4 锥桶检测算法处理过程综合对比图Fig.4 Comprehensive comparison diagram of cone bucket detection algorithm processing process

图5 坐标系位置图Fig.5 Each coordinate system shows the specific location map

2 激光雷达与摄像头的时空匹配

无人驾驶方程式赛车的环境感知模块由激光雷达和摄像头组成。针对二者的安装位置和角度都不相同，需要对激光雷达和摄像头进行标定，以此来建立二者之间的空间位置关联，同时二者的采样时间各异，还需要对二者进行时间同步处理。因此需要建立车体坐标系(OV-XVYVZV)、激光雷达坐标系(OL-XLYLZL)和摄像头坐标系(Oc-XcYcZc)来实现激光雷达和摄像头的时空匹配，各个坐标系位置如图5所示。

2.1 激光雷达与摄像头的空间标定

图6 针孔摄像头成像模型Fig.6 Camera imaging model

(4)

设空间中点P对应的像素坐标系下的坐标为(Xp,Yp)，像素坐标系在Xp轴方向上的比例系数为α，在Yp轴方向上的比例系数为β，沿原点的偏移为(cx，cy)。将点P′在虚拟像平面下的坐标转化为在像素平面下的坐标得到：

(5)

将式(4)代入式(5)，整理得：

(6)

其中αxf用fx替代；βyf用fy替代；K为摄像头的内参矩阵。

激光雷达和摄像头在空间的坐标系不同，需要坐标系的转换，使用R与t组成的增广矩阵来描述摄像头坐标系和激光雷达坐标系的转换，二者构成摄像头的外参矩阵。则有：

(7)

内参矩阵使用Matlab标定工具箱标定获得，将标定结果代入式(7)得到：

(8)

进一步化简，可以得到约束方程：

(9)

定义行向量T=(t1，t2，t3)，其中T为

(10)

得到：

(11)

由式(11)得知，1个特征点可以建立2个T的线性约束，T是12维，选取6组特征点使用奇异值分解法(SVD)求解T矩阵[22]，即得到激光雷达和摄像头的空间转换矩阵。图7为激光雷达和摄像头空间标定示意图。

图7 激光雷达和摄像头空间标定示意图Fig.7 Lidar and camera calibration diagram

2.2 激光雷达和摄像头的时间同步

针对激光雷达和摄像头的采样频率不一致问题，采用ROS操作系统中的时间同步模块实现二者的时间同步，在ROS节点中分别订阅激光雷达和摄像头的话题消息，通过时间同步模块实现统一接收多个传感器的话题消息，当所有的话题有相同的时间戳时，才会激发回调函数进行二者的数据处理，完成时间同步。

图8 激光雷达和摄像头时空匹配验证Fig.8 Joint calibration result of LiDAR and camera

在完成激光雷达和摄像头的时空同步后，放到赛车上进行验证。如图8所示，前景锥桶点云与背景图片中的锥桶一一对应，实现对激光雷达和摄像头的时空匹配。

3 锥桶颜色识别与信息融合

赛场中需要检测的锥桶大小一致，但颜色不同，仅靠激光雷达检测无法获得锥桶的颜色信息，且赛场中不同颜色的锥桶摆放有序(左红右蓝)，将激光雷达和摄像头融合后不仅可以得到锥桶的空间信息，还可以得到锥桶的颜色信息，为接下来赛车路径的决策和规划打下基础。

3.1 HSV颜色空间的颜色识别算法设计

1)RGB颜色模型图像转化为HSV颜色模型图像

摄像头采集到的锥桶图片是在RGB颜色空间下表示的，将锥桶RGB颜色模型图像转化为HSV颜色模型图像，易于对锥桶的颜色特征进行提取，并且图像角度、尺寸和分辨率对HSV颜色模型的影响较小。RGB图像是一种三通道图像，对于空间中任意一点可以由R，G，B分量构成的向量来表示[23]。HSV颜色模型从RGB颜色模型演化而来，HSV颜色模型是建立在颜色本身的特征基础上，3个分量分别表示色调、饱和度和亮度[24]。图9、图10分别为RGB颜色模型和HSV颜色模型。

图9 RGB颜色模型Fig.9 RGB color model

图10 HSV颜色模型Fig.10 HSV color model

图11 H通道颜色分布图Fig.11 Color distribution of H channel

在HSV颜色模型中，H通道的像素值表示色调，取值范围0°～360°，将其看成角度，即一个闭环的取值范围，H通道颜色分布如图11所示。

对于图像坐标系下的任意坐标点，在RGB颜色空间表示为(R,G,B)，转化为在HSV颜色空间下，表示为(H,S,V)。将R，G，B数值转换到0～1之间，见式(12)。

(12)

然后分别计算该坐标点对应的H，S，V数值，见式(13)。

(13)

式中H≥0，则保留该数值；反之如果H<0，则需要对该值加上360，得到的新的数值作为最终的H值，即H=H+360。

2)遍历H通道

将采集到的锥桶图片转换为在HSV颜色空间下的图片，只保留每个像素点H通道的分量，对H通道的分量进行处理，通过对图像中颜色识别区域内像素点H通道进行遍历，记录该区域内所有像素点H通道数值并进行比较。具体处理过程是：将点云处理算法处理得到的锥桶的中心点投影到像素平面内，在像素平面内以该点为中心做一个矩形的框，矩形框的大小由数次实验得到，最终确定矩形框大小为60×60。

3)颜色基准范围匹配

对颜色识别算法中锥桶的H分量值进行探索，读出其H通道分量数值并记录。通过多次实验得到各个锥桶颜色H分量所对应经验值的取值范围，完成颜色基准范围的匹配。其中得到的红色锥桶H分量的取值范围为160～179，黄色锥桶H分量的取值范围为22～38，蓝色锥桶H分量取值范围为75～130。

3.2 信息融合算法设计

信息融合是对来自多个信息源进行多级、多层次的处理。本文采用信息融合方法，将激光雷达得到的锥桶的空间位置信息和摄像头在HSV颜色空间下得到锥桶的颜色信息，在时间和空间的匹配下进行标定融合，得到锥桶的空间位置和颜色信息，压缩了大量冗余信息并提高了算法的运行效率。信息融合流程图如图12所示。

图12 信息融合流程图Fig.12 Information fusion flow chart

4 实验验证与分析

4.1 实验环境

表1 工控机配置参数表Tab.1 Industrial computer configuration parameter table

实验地点选择在校园内一片开阔的平地，按照赛场规则摆放锥桶，雷达选用禾赛科技40线激光雷达，摄像头选用大恒图像MER-231-41U3C，工控机信息配置见表1。

使用方程式赛车对本文提出的算法进行验证，将激光雷达与摄像头的融合算法布置在工控机上并进行合理调试，在ROS操作系统下调出Rviz可视化界面对实验结果进行验证。

4.2 实验结果与分析

在校园内模仿赛场环境，在静态和动态2种场景下对提出的标定融合算法进行验证，并针对比赛要求，将赛车在8字环绕的场景下进行实车测试，证明本文提出的标定融合算法的有效性。

其中静态场景下对锥桶的检测效果如图13所示。从图13 a)可以观察到经过对点云进行滤波、地平面滤除、欧式聚类之后显示出的锥桶点云的空间位置；图13 b)为激光雷达与摄像头融合之后的效果图，可以看到锥桶点云被赋予了颜色信息，显示出了锥桶的颜色。

图13 静态场景下的融合算法效果图Fig.13 Image of fusion algorithm in static scene

在校园内模仿赛场环境进行动态测试，设定方程式赛车以25 km/h的速度行驶，在ROS操作系统下调出Rviz可视化界面，并订阅激光雷达和摄像头的话题消息，观察实验结果。动态场景下融合算法对锥桶的检测效果如图14所示。其中图14 a)中为左右相机对锥桶颜色的识别情况，对锥桶的颜色识别具有较高的准确度；图14 b)为激光雷达和摄像头融合之后的效果，可以看到锥桶的空间位置信息和颜色信息匹配良好，具有较高的准确度和实时性。

图14 动态场景下融合算法效果图Fig.14 Effect of fusion algorithm in dynamic scene

图15 8字赛道示意图Fig.15 Schematic diagram of the figure-eight track

依据比赛要求，方程式赛车需要进行直线加速测试、8字环绕测试和高速循迹测试。其中8字环绕测试布局图如图15所示。红色锥桶和蓝色锥桶为行径锥桶，中间的黄色锥桶为换向锥桶，比赛要求赛车先右圈行驶2圈后在黄色切点处换道再进入左圈行驶2圈后出道，即完成比赛。如果仅依靠激光雷达执行感知任务，赛车虽然能在直线加速和高速循迹测试中完成行驶，但在8字赛道切点处无法完成赛车换道的决策任务。

为了更好地测试本文提出的融合算法在8字赛场环境下的有效性，在校园内依照8字环绕的比赛要求摆放锥桶，对算法进行验证。测试结果如图16所示。赛车在完成右侧的2圈行驶后完成换道并顺利完赛，证明了本文提出的标定融合算法的有效性。

5 结 语

为了解决赛场环境下无人驾驶方程式赛车感知系统中锥桶检测准确度较低的问题，提出了一种基于激光雷达和摄像头标定融合的锥桶检测算法。通过对ROI内的锥桶点云采用地平面分割、欧式聚类的方法，实现了对地平面点云的滤除，在获取锥桶空间位置信息的同时，达到了检测算法的实时性；通过对激光雷达和摄像头的空间位置进行联合标定和时间同步，实现了激光雷达和摄像头较为准确的时空匹配。将激光雷达采集的锥桶空间位置信息与摄像头采集的锥桶颜色信息进行匹配，实现了激光雷达和摄像头的信息融合，更好地发挥了激光雷达和摄像头融合的优势，提高了对锥桶检测的准确度。实验结果表明，该算法能够满足方程式赛车在赛场环境下对锥桶的检测要求，具有良好的准确性和实时性，解决了仅使用单一传感器造成的检测锥桶的信息丢失问题，为方程式大赛中使用激光雷达和摄像头传感器的融合方法提供了参考。

图16 模拟8字赛道实车测试Fig.16 Simulated real-life test of the figure-eight track

本文主要针对激光雷达和摄像头的标定融合检测算法进行了研究，还存在一定的局限性，在基于激光雷达的目标检测方面，需要多次实验才能确定激光雷达对锥桶检测设置的参数。未来可以考虑在点云处理过程中加入特征提取模块，提高锥桶检测的精度，有效降低误检率，并减少为获取经验值而进行实验的次数。