针对FSAC赛场环境的全局锥桶建图算法研究

张要强,李 刚,薛玉斌

(辽宁工业大学 汽车与交通工程学院, 辽宁 锦州 121001)

0 引言

世界正迎来百年未有之大变局,汽车行业也逐渐趋于智能化与无人化。为推动我国无人驾驶汽车技术的蓬勃发展,中国汽车工程学会自2017年起逐年举办中国大学生无人驾驶方程式大赛(formula student autonomous China,FSAC)[1],要求参赛车队在一年内设计并制造出一台满足无人驾驶条件下,在由锥桶围成的赛道内,自主完成加速、减速、转向并稳定跟踪赛道路径的电动方程式赛车[2]。无人驾驶相关技术主要包括环境感知、决策规划和运动控制3大模块,其中环境感知是无人驾驶的关键环节,是车辆安全行驶的重要保障,也是决策与控制模块的前提[3]。激光雷达因其具有良好的抗干扰能力且精度高[4-5],是识别锥桶赛道的主要传感器。

目前,FSAC锥桶赛道的感知识别策略主要是在赛车行驶过程中,通过激光雷达的检测程序从当前帧点云中提取出锥桶位置信息,实时发布给路径规划算法[6]拟合行驶路线,通常表现为“边识别边跑动”。代加喜[7]、王华东等[8]分别提出了一种基于激光雷达的锥桶检测算法,将原始激光点云经过感兴趣区域(region of interest,ROI)选取、地面分割与欧式聚类后,得到锥桶在激光雷达坐标系下的位置。通过设置合理的ROI区域,能够显著提高检测速率,并防止赛车因检测到相邻赛道的锥桶而驶入的窘境。但在高速循迹的“U型弯道”等极限工况中, ROI的限制反而导致获得的桩桶数量少,无法规划行驶路径。针对此问题,宫彦乔等[9]提出了一种基于激光雷达和惯性导航的动态视野算法,利用惯性导航解算得到航向角的变化量,经过加权滑动平均滤波后,结合车速与车轮转角信息,预测下一时刻航向角变化趋势,动态调整ROI的范围,在转弯处获取更多锥桶信息。但该算法偶尔存在因计算复杂度提升而造成的高时延问题,同时高车速使得车辆不能及时避障并发生意外。因此,对于赛车行驶轨迹附近的环境信息进行提前预知显得十分重要,而对比赛场地的锥桶赛道进行地图构建则可实现这一目的,同时也可为路径规划程序和底层控制程序提供更多的先验信息,作出更合理、及时的判断。Giorgio等[10]提出的Gmapping算法能够建立2D栅格地图,有利于算法直接规划路径,但不存在回环检测,大场景下的建图效果漂移严重。Shan等[11]提出的LIO-SAM(一种激光SLAM算法)能够建立完整的赛道环境,但三维点云过于稠密,无法直接分辨出锥桶并优化路径。

本文以FSAC赛车为载体,针对特定比赛环境下的环境感知与建图进行算法开发,通过3D激光雷达和组合惯导系统采集到的锥桶和车体的空间位置信息,构建出完整的赛道锥桶地图,以供路径规划程序拟合出最优路径进行加速跟踪,能够为无人驾驶赛车提供准确的环境信息和稳健的提速效果。

1 激光雷达与组合惯导的外参标定

为了建立全局锥桶地图,需要将激光雷达坐标系下的点云转换到地图坐标系下[12]。首先将组合惯导安装在车辆质心位置并完成标定,车辆坐标系与惯导坐标系重合,进而减少计算量。以车辆起始时刻的惯导位姿作为地图坐标系原点,将激光点云先从雷达坐标系投影到车辆坐标系,再利用组合惯导系统得到车辆在地图坐标系中的位姿,进而将点云投影到地图坐标系中[13]。而为了获得上述坐标系之间精确的转换关系,需要对激光雷达与组合惯导系统的外参进行标定。图1为雷达坐标系和惯导坐标系的安装关系示意图。

图1 雷达坐标系和惯导坐标系的安装关系示意图

(2)

综上,粗略测得激光雷达相对组合惯导的外参矩阵,如式(3)所示。

(3)

接着记录一段轨迹内,通过激光里程计得到位置数据PL和组合惯导里程计得到位置数据PV:

(4)

(5)

2 锥桶点云去畸变与欧式聚类

2.1 点云去畸变

在利用机械旋转式激光雷达建图前,需要对点云进行畸变校正。点云畸变的本质是机械式激光雷达随车运动时,一帧点云中的所有点不是同一时刻采集到的。由于雷达扫描一帧的时间固定,可以通过计算每个点的采集时刻,将所有点都统一到同一时刻的坐标系中。假设选择投影到一帧点云的结束时刻,如图2所示。

图2 点云投影示意图

tk为一帧点云开始时刻,tk+1为一帧点云结束时刻,tk+1-tk即为一帧点云的扫描时间,水平箭头表示将所有点都投影到tk+1时刻。Pk为这一帧扫描生成的点云,显然不同的点具有不同的时间戳[15]。通过惯导中的IMU积分出激光雷达tk时刻相对于tk+1时刻的相对位姿,记为TLk+1。通过插值法,计算出每个点对应时刻的激光坐标系相对于tk+1时刻激光坐标系的补偿变换矩阵TL(k+1,i),如式(7)所示。

(7)

每一个激光点乘以其对应的补偿变换矩阵,可得到矫正后的激光点坐标。图3(a)和图3(b)分别为点云去畸变前后对比效果,红点为锥桶和地面点,蓝点为算法计算的锥桶中心。

图3 点云去畸变效果

2.2 锥桶点云的欧式聚类

在激光雷达点云信息中,需要利用聚类方法进行锥桶检测任务。考虑赛场环境下锥桶之间的距离,采用基于欧式距离聚类的方法对锥桶点云进行聚类。

欧式聚类算法的具体流程如图4所示,在当前帧激光点云中任取一点p,通过KD-Tree近邻搜索算法找到k个离p点最近的点,并将距离小于设定阈值(0.56 m)的点加入集合Q中。如果Q中元素的数目不再增加,整个聚类过程结束;否则,须在集合Q中选取p点以外的点,重复上述过程,直到Q中元素的数目不再增加为止。图5(a)和图5(b)分别为欧式聚类前后的效果。

图4 锥桶点云的欧式聚类流程框图

图5 欧式聚类前后效果

3 锥桶地图构建算法设计

3.1 算法框架

首先将激光雷达采集到的一帧原始点云经过预处理(ROI选取、体素栅格降采样、地面分割),接着利用组合惯导中的IMU数据对该帧点云去除畸变;去除畸变后的点云进行欧式聚类得到一个个带有标签信息的锥桶点云簇,计算其中心点在雷达坐标系中的坐标;通过雷达到惯导的外参矩阵,将其投影到车辆坐标系中,再通过组合惯导系统解算出当前车辆坐标系相对于地图坐标系的位姿变换矩阵,将锥桶投影到地图坐标系中;最终利用均值滤波对锥桶地图进行更新,得到所有锥桶在地图坐标系中的位置,通过ROS话题发布给规划算法。算法流程如图6所示。

图6 算法流程框图

3.2 锥桶的局部定位

图7 锥桶局部定位逻辑框图

(8)

3.3 车辆定位

由于组合惯导中的GNSS(RTK)输出的原始坐标数据为WGS-84坐标系下的经纬高坐标,而规控需要的是其在导航坐标系下的直角坐标,因此需要将WGS-84坐标系下的坐标数据转换到导航坐标系中。定义组合惯导第一帧位姿P0为导航坐标系(地图坐标系)原点,其对应的WGS-84系坐标为(lat0,lon0,alt0),经过解算对应的ECEF系坐标为(x0,y0,z0);t时刻惯导位姿Pt所对应的WGS-84系坐标为(latt,lont,altt),经过解算对应的ECEF系坐标为(xt,yt,zt),则Pt在地图坐标系中的位置为:

(9)

此时,INS测得Pt相对P0的滚转角、俯仰角和偏航角对应的角度依次为φ、θ、μ,则车辆坐标系相对地图坐标系的旋转矩阵为:

故t时刻车辆坐标系相对地图坐标系的变换矩阵为:

(11)

3.4 锥桶全局定位

图8 雷达、车辆和地图坐标系位姿关系图

(12)

3.5 锥桶地图构建

锥桶地图实际上就是获取真实环境中的锥桶在地图坐标系中的位置,如果只是将每一帧锥桶点云简单的从雷达坐标系投影到地图坐标系中,势必导致“裂桶”现象的发生,如图9(a)所示,即真实世界中的一个桶对应地图中多个坐标,无法确定其实际位置。所以需要进行融合更新,新计算的锥桶坐标和之前一定范围内计算的锥桶坐标取平均值,作为这个桶的位置,图9(b)为融合效果。

图9 裂桶效果与融合效果

具体过程如图10所示,每隔3 s,对当前帧的锥桶通过K近邻搜索找到锥桶地图中最近的桶,如果二者之间的距离小于设定的阈值,就进行均值滤波融合,如果大于阈值,说明是新扫描到的桶,将其加入地图内。

图10 锥桶地图的构建与更新过程框图

4 实验与分析

4.1 实验平台

选用“辽宁工业大学万得FSAC赛车”作为实验平台。

硬件方面,FSAC赛车上搭载了激光雷达、组合惯导系统和计算单元等设备,实现无人驾驶功能。其中,激光雷达选用的是禾赛科技Pandar40机械式激光雷达,可以采集赛车周围环境的三维点云信息。组合惯导系统选用导远INS570D(支持RTK),能够提供构建锥桶地图所需的车体实时、精准的位姿信息。二者将点云信息和定位信息分别发送给计算单元——研华MIC-7700(Intel i7-6700TE处理器和NVIDIA 2060显卡)进行处理,该工控机具有稳定的性能,可以24 h不间断地工作,且不受环境限制,满足比赛环境的要求。

软件方面,选用机器人操作系统(robot operating system,ROS)[16]作为软件通讯平台。ROS内部集成了大量调试机器人所需要的各种工具,通过话题和服务的方式完成信息交互,具有很好的通用性。激光雷达、组合惯导系统和计算单元的安装位置如图11所示。

图11 传感器与计算单元安装位置示意图

4.2 实车验证

图12为“高速循迹”项目赛道,黄色圆圈标注的是计时器。设定FSAC方程式赛车第一圈以15 km/h速度行驶,使用ROS中Rviz可视化界面进行测试结果显示。

图12 “高速循迹”项目赛道图

4.2.1 算法验证

图13为当前赛车所处位置,可以看出,赛车正进入U型弯道,图14和图15为仅采用雷达当前帧锥桶识别算法的检测效果,由于ROI的限制,图14中赛车只能感知到6个锥桶的位置,特别是左侧只检测到1个锥桶,赛车无法规划合理路径,并且可能由于高速冲出赛道。如果增加ROI区域范围,如图15所示,赛车的每一帧计算量都将加大,且存在遮挡(红色圆圈处锥桶被遮挡)。

图13 当前赛车所处位置

图14 常用ROI检测效果

图15 扩大ROI检测效果

图16和图17为本文提出的锥桶地图构建算法的检测效果。P0为地图坐标系原点,Pt为当前车辆坐标系,白色点为放大后的锥桶地图,红色点为当前帧扫描到的锥桶,红色线为组合惯导的轨迹,图16是激光雷达当前帧扫描到的锥桶中心,正在和锥桶地图融合更新,可以看出更新效果良好。

图16 锥桶地图更新效果

图17 锥桶地图构建完成效果

图17是建完地图后赛车行驶的第2圈的效果,第一圈车辆缓慢行驶,经算法判断再次经过起点前就能得到完整的赛道,再将地图坐标系中的锥桶投影到当前车辆坐标系中进路径规划。由于发车起点附近赛道为直线,算法能够提前检测到回环,预留出足够时间供算法得出全局最优路径,赛车按照拟合的全局路径进行直线加速与弯道减速行驶,极大提高了效率。

4.2.2 结果分析

高速循迹赛道共设置176个实体锥桶,总共进行3次实验,获取锥桶地图结果如表1所示,借助目标检测评价方法[17]进行评价:真阳性(true positive,TP)为实检个数,假阳性(false positive,FP)为误检个数,假阴性(false negative,FN)为漏检个数;召回率(Recall)和精准度(Precision)根据公式(13)计算。3次实验结果的平均召回率为98.7%,平均精准度为98.1%。从实验结果中可以看出,通常假阳性结果是由于激光雷达锥桶识别算法判断精度不够导致的,当存在形状大小类似锥桶的障碍物时,算法会误检为锥桶,如图12所示,黄色圆圈中误检目标为计时器,但是并没有产生太大影响。假阳性结果一般发生在弯道处,是由于弯道曲率过大,设置的ROI区域以及激光遮挡导致的,在同一直线后的桶会被前面的桶遮挡,由于二者过于接近,被融合算法认为是一个桶,故存在漏检现象。

表1 锥桶检测结果

(13)

由于车辆定位是基于组合惯导中的RTK位姿,精度为厘米级,所以认为依靠定位结果建出的锥桶地图整体误差满足要求,由图17中的赛车轨迹可知,车辆根据锥桶地图顺利完成高速循迹第1圈,此时行驶在第2圈弯道处,说明锥桶的定位精度可靠,能够为无人驾驶赛车提供可靠的锥桶位置信息,获取实时准确的锥桶赛道地图。

5 结论

1) 考虑到FSAC高速循迹的赛道特点,提出了一种基于激光雷达和组合惯导系统的锥桶地图构建算法,赛车第一圈以15 m/s低速行驶,识别并记录锥桶在地图坐标系中的位置,在第二圈开始前完成赛道地图的构建,供给规划算法完成全局最优路径规划,实现赛车提速。

2) 实验结果表明,相比于当前帧雷达检测识别算法,所提出的算法能够将赛车历史扫描到的锥桶记录下来,当车辆再次到达极限工况之前,对环境进行预知,为FSAC无人方程式赛车的决策规划提供了更为准确的判断依据,且在精度和召回率等方面表现较好。

3) 由于依靠纯激光雷达对锥桶进行检测识别,对于形状和大小与锥桶类似的计时器等物体无法进行良好识别,在弯道处,由于锥桶遮挡及算法处理问题引起少数锥桶漏检,今后将对算法进一步升级,添加视觉因素进行融合判断。