FSAC赛道地图构建算法研究

兰建平,郭文韬,汤文靖,佘依函

(湖北汽车工业学院 汽车工程师学院,湖北 十堰 442002)

0 引言

无人驾驶汽车是未来汽车的发展方向[1],汽车智能化的飞速发展推动了众多企业进军该领域。为培养智能网联汽车方向的创新型人才,中国汽车工程学会在大学生电动方程式大赛的基础上[2],举办了中国大学生无人驾驶方程式大赛(FSAC)。无人驾驶汽车主要分为感知定位、决策规划和执行控制组成。感知定位是无人驾驶的第一步也是最为重要的一步,其结果直接影响之后的决策规划和执行控制部分[3]。通过相机和雷达等各类传感器感知周围环境[4],决策规划基于感知系统确定最优行驶路径,通过控制部分沿着最优行驶路径行驶。通常,感知定位[5]系统会通过构建地图的方式为路径规划系统提供决策依据。

国内外学者对无人驾驶的地图构建算法进行了大量研究,常见的建图方式有基于激光雷达和基于视觉方法两大类,其中基于视觉的算法又可分为特征点匹配和深度学习方法。方哲等[6]提出了一种融合语义特征传播模型的前景对象感知定位算法,实现了一定精度和实时性的定位,但占用资源较大。Krylov等[7]提出一种使用街景图像自动检测和计算重复静态感兴趣对象坐标的方法,利用MRF估计物体的地理位置,能确定目标位置,但精度偏低。商磊等[8]提出了一种基于MeanShift聚类选取自适应KNN的混合相似度加权KNN定位算法,显著提高了算法的定位精度和稳定性。ORB-SLAM系列为主的稀疏点云地图由于缺乏一些必要的细节[9],不能作为路径规划的依据。对于稠密地图[10]的构建,虽然细节保留比较完全,但是建图速度较慢,难以满足实时性要求,对无人驾驶汽车构成安全隐患。

FSAC无人方程式赛车感知对象为不同颜色的锥桶,目的是获得锥桶的颜色和位置,并为之后的决策规划提供关键信息。通过对FSAC赛道锥桶地图构建方法进行研究,利用单目相机与组合惯导设计并实现了一种具备高实时性和高准确性的锥桶地图构建算法,解决赛场环境中赛道地图构建的问题。

1 实验平台及系统框架

1.1 实验平台

采用湖北汽车工业学院FSAC方程式赛车,Basler ace-acA1920-40gc相机;迈普时空M39组合惯性导航系统。计算单元采用Intel i7-10870H处理器和NVIDIA 2060显卡(6G显存)。使用机器人操作系统(robot operating system,ROS)作为中间件进行通信,通过话题发布和订阅完成信息交互和控制。将相机水平朝向赛车正前方,安装于赛车主环下方,便于其获取车辆前方障碍物,组合惯导安装于电池箱上方,计算单元安装于赛车右侧。FSAC方程式赛车及关键设备安装位置如图1所示。

图1 FSAC方程式赛车及关键设备安装位置

1.2 系统框架

首先对单目相机进行标定,使用张氏标定法利用7×8的棋盘格对图像进行标定,去除图像畸变,并获取相机的内参;接着对锥桶在相机坐标系下的位置进行标定,锥桶均放置在水平地面上,使锥桶分布满足平面方程,完成锥桶与相机的联合标定。当系统运行时,通过相机获取锥桶图像,通过YOLOv3算法进行目标检测,得到锥桶的目标检测框,取其下边界中点作为该锥桶在图像中的位置,通过相机内参和平面方程求出锥桶在相机坐标系下的空间位置;通过组合惯导解算得到赛车位置,进一步确定锥桶在车辆坐标系下的位置,并利用KNN算法进行滤波,进一步筛选锥桶在车辆坐标系下的位置。定义赛车起驶位置为赛道地图坐标系的原点,并将锥桶坐标转换到赛道地图坐标系下,最终得到赛道地图。设计流程如图2所示。

图2 系统设计流程

2 相关理论

2.1 相机标定方法

2.1.1相机畸变去除

相机内包含凸透镜,在实际使用中安装广角镜头以增大视角,但透镜会引起图像失真,为获取较好的成像效果,首先进行图像畸变去除。径向畸变多项式关系为：

(1)

在安装过程中,镜头不能与成像平面完全平行,否则会导致切向畸变,切向畸变多项式关系为：

(2)

2.1.2相机内参获取

单目相机的成像过程可以简化为小孔成像[11],建立相机坐标系O-X-Y-Z和成像坐标系O′-X′-Y′-Z′,其中f为相机焦距。设空间内任意一点在相机坐标系下的坐标为P(x,y,z),通过光心O投影到成像坐标系下为P′(x′,y′),相机投影模型如图3所示。

对小孔成像进行分析,建立相似三角形,得式(3)：

(3)

同理,可得：

(4)

将物理成像平面翻转放置到光心O点的f处,得到：

(5)

成像平面和像素坐标存在式(6)的关系：

(6)

式中：u和v为P′在像素坐标系的位置;α和β为像素坐标系轴方向和轴方向上的比例系数;cx和cy为偏移量。

结合式(5)和式(6),将结果转化为矩阵形式：

(7)

其中：fx为αf,fy为βf,K为相机的内参矩阵。通过内参可以求得像素点与对应空间点的归一化空间坐标点之间的关系。

2.2 YOLOv3算法介绍

卷积神经网络(convolutional neural networks,CNN)是典型的深度学习算法,根据CNN原理不同,基于CNN的目标检测算法可分为二阶段检测和一阶段检测2种。经典的二阶段检测算法有R-CNN、Fast-RCNN和Faster-RCNN等,有代表性的一阶段检测算法有YOLO[12]和SSD(single shot mutibox detector)等算法。YOLO算法相较于传统的二阶段检测算法绕开了候选区域分类和评估,极大程度提升了运算速度。

YOLO算法在保持速度优势的前提下提升了预测精度,加强了对小物体的检测能力,在工业界应用广泛。其中,YOLOv3算法既能同时提取多个通道的特征,又能保证一定准确性,满足无人驾驶方程式赛车的检测需求。与此同时,相比于当前新版本的YOLO检测算法,YOLOv3自2018年提出以来,已推广近5年时间,更为成熟可靠,适用于无人驾驶赛车等实际工程[13]。

YOLOv3[14]主体由特征提取网络与多尺度检测器组成,前者用于图像特征提取,后者在前者基础上进行抽象和融合,得到3个尺度预测特征。进一步对这3个尺度进行检测,得到检测目标中锥桶类别及检测框对应坐标[9]。

YOLOv3为YOLO算法的改进版本,对YOLOv2[15]和YOLOv1改进时借鉴了特征金字塔(feature pyramid network,FPN)思想,利用多个卷积层和池化层来提取图像特征,并将这些特征组合成一个特征金字塔,用于对各类尺寸图像检测。

YOLOv3算法将输入的待检图片划分为S×S的网格,每个网格预测一定数量的初始边界框和其对应的置信度,预测每个状态下目标物的类别概率,即边界框中存在目标物的条件概率。将每个状态下目标物的类别概率和置信度相乘,得到该状态下的分类概率。最后,将所有状态下的分类概率相加,得到最终的分类概率。对于每个类别,可以计算其边界框的置信度,然后将分类概率和边界框的置信度相乘,得到该类别的置信度。YOLOv3算法检测原理[16]如图4所示。

图4 YOLOv3算法检测原理示意图

2.3 K近邻算法介绍

K近邻算法[17]是基础的机器学习算法之一,可用于分类和回归。它通过测量不同特征值之间的距离来进行分类。对于任意维度输入向量,输出为该特征向量所对应的类别标签或预测值。相较于最近邻算法,K近邻算法的定位准确性有一定提升,缺点是在进行滤波处理时,直接采用取平均值确定定位点的位置可能导致误差增大。

对K近邻算法进行改进,引入权重概念[18],在K个位置直接平均赋值的基础上,对每个近邻目标点的贡献程度分配权重值,对K个近邻的锥桶位置点进行加权平均,得到估计位置坐标：

(8)

(9)

式中：ωi为第i个参考点所占的权重;di为K个近邻点中第i个锥桶点到待定位点的距离;ε为无限趋近于0的数,防止ωi在di为0时成为无穷大。采用改进版的K近邻算法降低远处点对定位点的影响,提高定位的准确性。

2.4 M39组合导航系统

M39组合惯导由武汉迈普时空导航科技有限公司生产,包含IMU和GNSS两个主要部分,在GPS信号良好时,能够准确对无人驾驶车辆进行定位。M39连接示意图如图5所示。

图5 M39连接示意图

对M39组合导航系统进行标定,使用M39对组合导航系统进行位姿解算。在位姿解算中心,输入的数据被转换成惯性空间中的导航信息,包括速度、加速度和位置信息。这些信息被用来计算当前位置的姿态,并将这些姿态传输到车辆的控制系统中,最终获得当前位置的经纬度、头指向、俯仰角、横滚角和偏航角。

3 锥桶空间定位

以无人驾驶赛车起驶点建立赛道地图坐标系O0-X0-Y0-Z0,定义k时刻相机坐标系为Oc-Xc-Yc-Zc,组合导航系统为该时刻车辆坐标系原点,即：Ov-Xv-Yv-Zv为车辆坐标系,赛道地图坐标系如图6所示。

图6 赛道地图坐标系示意图

3.1 相机与锥桶标定方法

相机与锥桶标定方法通过相机对锥桶所在平面进行标定[19]。获取锥桶在单目相机下的平面参数,设A锥桶在相机坐标系下坐标为(xA,yA,zA),H为A锥桶到相机坐标系原点的距离[20],如图7所示。

图7 相机与锥桶标定图

设锥桶所在平面方程满足：

axA+byA+czA=1

(10)

(11)

结合式(7)、(10)和(11),通过测量获得u、v和H,对式(10)中系数a、b和c进行求解。

3.2 锥桶空间定位计算

相机通过YOLOv3获取锥桶检测框,以检测框下方框线中点为锥桶位置。设空间中存在B锥桶,代入组合方程(12),即可得到B锥桶的空间位置。

(12)

求得B锥桶在相机坐标系下的空间位置(xB,yB,zB)后,相机朝向为赛车的正前方,锥桶投影在Oc-Xc-Zc平面的坐标为(xB,zB)。为方便表示,令该坐标为(xcone,ycone)。锥桶在该平面投影如图8所示,左侧为目标检测,右侧为对应的锥桶位置。

图8 锥桶投影示意图

3.3 车辆位置获取

定义起始时刻为0时刻,通过经纬度对车辆位置进行解算,获取当前车辆位置(x0,y0)和头指向θ0,在k时刻,通过经纬度进行车辆的位置解算,获取k时刻车辆位置(xk,yk)和头指向θk,车辆的位置(xv,yv)在赛道地图坐标系的位置为：

(13)

3.4 锥桶空间定位

(14)

θ0k=θk-θ0

(15)

由此求得赛车前方障碍物在赛道坐标系中对应位置。

3.5 基于KNN的滤波算法

若每一次获取到锥桶都放入地图内,会导致每个锥桶在地图上对应多个位置,影响规划判断,通过改进的KNN算法对所得锥桶进行滤波处理。

当目标检测系统检测到一个新锥桶时,分为3种情况进行处理：

1) 若该锥桶附近2 m没有之前建立的锥桶时,设该锥桶权重为1;

2) 若新锥桶附近2 m有1个之前建立权重为n的锥桶时,对2个锥桶位置进行加权平均,设置新得到锥桶的权重为n+1;

3) 若新锥桶附近2 m有2个及以上之前建立的锥桶时,取离该锥桶最近的权重为m的锥桶,对该锥桶位置进行加权平均,设置新得到锥桶权重为m+1。

通过OpenCV制作一个锥桶全局坐标显示,可以得到锥桶全局坐标位置和初始经纬度坐标,获得锥桶在对应赛道地图坐标系下的二维锥桶赛道地图。

4 实验与分析

使用湖北汽车工业学院无人驾驶方程式赛车对提出的算法进行测试验证,共检测3种物体类型：红色锥桶、蓝色锥桶和黄色锥桶,并获取锥桶空间位置,构建锥桶赛道平面地图。

4.1 目标检测算法验证

采用由北京理工大学、福州大学、湖南大学、湖北汽车工业学院等10余所高校共同构建的FSAC-COCO锥桶数据集,如图9所示,从中选取 6 000张图片(包含56 857个锥桶)作为训练集,2 000张图片(包含17 694个锥桶)作为测试集。在不同地点、不同时间、不同光照及不同赛道进行采集,锥桶分为3个目标类别,分别为红色锥桶、蓝色锥桶和黄色锥桶。为增加图像多样性,通过轻微平移、缩放和添加随机颜色噪声对数据集进行增强,再将图像归一化后送入目标检测模型进行特征提取。

图9 FSAC-COCO锥桶数据集示意图

采用YOLOv3算法对FSAC-COCO锥桶数据集进行训练测试,训练过程参数曲线如图10所示。

图10 训练过程参数曲线

图10(a)为损失曲线,训练过程前35轮下降最快,在265次迭代训练之后,损失曲线下降趋于平缓,最终稳定在最小值。图10(b)为在验证集测试的平均精度均值曲线,最终平均检测精度为96.2%。

训练完成后,加载获取的权重文件,对获取的图像进行检测,检测结果如图11所示。通过ROS发布锥桶在图像中的位置信息,供建图使用。

图11 目标检测结果示意图

4.2 赛道地图实验结果

在校园环境中模拟高速循迹赛道,设定FSAC方程式赛车以30 km/h速度行驶[21],使用ROS中Rviz可视化界面进行测试结果检测[22]。赛车以30 km/h速度行驶时,图12(a)和图12(b)为高速循迹测试中不同时刻测试结果,左侧为锥桶检测结果,右侧为实时建图结果。可以看到,锥桶的颜色信息和空间位置匹配良好,完成高速循迹测试赛道地图构建,具有较高准确度和实时性。

图12 模拟高速避障赛道实车测试示意图

高速循迹赛道共设置70个锥桶实例,其中有34个红色锥桶和36个蓝色锥桶,获取锥桶地图结果如表1所示,真阳性(true positive,TP)为68个,召回率(recall)为98.6%,精准度(precision)为97.1%。对比文献[7]中的MRF算法,召回率提升了3.1%,精准度提升了4.6%,且每帧都可以输出一组锥桶位置,即使在图像目标少检情况下,仍能较好地实现赛道构建目标,减小假阳性(true positive,FP)发生概率。同时,较多锥桶位置结果也能更好地剔除假阴性(false negative,FN)发生概率。从实验结果可以看出,通常假阴性结果在改进KNN滤波时仅占很小权重,通过设置阈值进行剔除。

表1 获取锥桶地图结果

对建图结果进行分析,定位精度如表2所示,平均误差为0.245 m,中值误差为0.142 m,相比MRF算法降低了52.8%,中值误差降低了43.4%,顺利完成循迹任务,结果满足竞赛要求,能够为无人驾驶赛车提供可靠的环境感知信息,获取实时准确的锥桶赛道地图。

表2 定位精度

5 结论

1) 为满足实际环境中无人驾驶赛车地图构建实时性和准确性,提出了一种基于相机和组合导航系统的锥桶地图构建算法,可在不依赖激光雷达的情况下满足赛场环境下无人驾驶赛车赛道地图的构建。

2) 实验结果表明,相比基于MRF的地图构建算法,所提出算法在提升精度和召回率、降低误差等方面有较好表现,为FSAC无人方程式赛车的决策规划提供了更为准确的判断依据。

3) 所提出算法可扩展到行人地图构建及车辆地图构建等,将其部署于车辆前置相机时,可操作性高且易于部署。

4) 所提出算法主要针对单目相机与组合惯导融合的锥桶地图构建进行研究,未来可从提高算法检测精度、速度及轻量化与可移植性等多方面对检测算法进一步迭代提升,以满足算法的商用化及量产化需求。