FSAC赛车动态视野算法研究

宫彦乔，刘立东，李 刚，张 东

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

0 引言

近些年，随着人工智能和机器人技术的飞速发展，无人驾驶汽车作为人工智能领域的一个重要的分支已经成为国内外的研究热点[1]。环境感知技术是无人驾驶技术的关键部分，主要通过布置大量车载传感器采集并识别车辆周围环境信息，确保无人驾驶车辆的安全性与可靠性[2]。无人驾驶方程式赛车是一种行驶在桩桶赛道里的特殊车辆，追求速度和良好的动态性能，对感知算法有更高的要求。李涛等[3]对YOLO算法进行改进，通过FSAC赛车上的相机对桩桶进行识别。张煌等[4]采用传感器融合的方法将激光雷达和相机信息融合，对桩桶进行识别，弥补了激光雷达分辨率低和相机感知距离差的问题。赵治国等[5]为消除车辆转向带来的不利影响，基于车载激光雷达坐标系的变换，提出了一种目标障碍物的跟踪方法。这些方法中纯视觉或相机与激光雷达融合的感知算法需要面对相机对光照的敏感问题，所以在环境感知任务中，激光雷达具有精度高、不易受光照影响的优点[6-7]，成为了FSAC赛车的最主要的环境感知设备。基于激光雷达的感知任务主要包括提取感兴趣区域、降采样、地面分割、障碍物检测[6]，在这些任务中感兴趣区域(region of interest)的提取是感知任务中最先进行的。在激光雷达采集的点云中，以长方体、圆柱体或其他不规则形状的立体区域框选出需要重点关注的点云信息进行算法处理，称这个区域为激光雷达的ROI，这样不仅可以大幅度减少需要处理的点云数量，而且将该方法应用于车辆时，可以降低非可行驶区域内障碍物的感知结果对路径规划算法的干扰[8]。随着激光雷达传感器的加速发展[9]，对激光雷达ROI方面的研究逐渐增多。尚业华等[10]采用在三维空间中框选出一个三维立方体的方法，这种方法相当于固定ROI，适应性较差，适合低速作业的农机。王涛等[1]依靠路沿的特征，将路沿作为ROI的边界，该方法依赖于路沿的识别。王晓原等[11]提出了一种划分多个ROI识别道路边界的方法，该方法将车辆周围的激光雷达点云分为4个部分，车辆左前方、右前方、左后方、右后方，然后分别处理每个部分的点云，达到简化环境的目的。上述研究方法为论文研究提供了重要参考。能使FSAC赛车在比赛场景中获得更多桩桶信息，有利于更好地路径规划。因此，论文提出了一种动态视野的算法，通过惯性导航获得FSAC赛车自身姿态，动态改变激光雷达感兴趣区域，获得更多桩桶信息，提高感知效率，并通过实车进行了验证。

1 传感器工作原理及系统软件

1.1 激光雷达原理

FSAC赛车所使用的激光雷达为机械式激光雷达。机械式激光雷达的测距方法为飞行时间测量法(time of flight)。雷达的内部设置激光发射器，随着内部电机的旋转，激光发射器发出多线束的超短激光脉冲，当激光遇到障碍物后发生漫反射，激光接收器接收漫反射返回的光束之后，通过激光光束在空中的飞行时间，可以精确计算得出目标物体到传感器间的距离[12]。

(1)

式中：d为激光雷达中心轴线到物体的直线距离；c为光速；t为激光光束在空中的飞行所用时间。

对激光雷达数据包进行解析时，点云数据UDP包中主要储存的测距数据为水平旋转角度和距离参量。研究过程中需要对点云进行可视化，为了达到将数据转换为点云图进行显示的目的，需要将极坐标下的点云数据三维坐标转化为笛卡尔坐标系下的三维坐标，可以通过下列关系式转化：

(2)

式中：r为激光雷达到障碍物激光点的距离；ω为激光雷达到该激光点的垂直角度；α为激光雷达到该激光点的水平旋转角度；x、y、z分别为极坐标投影到笛卡尔x、y、z轴上的坐标[13]。

1.2 惯性导航原理

FSAC赛车需要获取自身姿态信息，惯性导航设备具有全天候、完全自主且不受外部干扰、短时定位精度高等特点，被广泛应用于智能车辆上[14-15]。惯性导航设备由陀螺仪和加速度计等惯性器件组成，以牛顿力学定律为基础，测量车辆的运动加速度等信息，可以积分运算得到载体的速度、位置等导航参数，是一种航位推算式导航系统。

车辆航向角为地面坐标系下车辆质心速度与横轴的夹角，计算方法主要依靠惯性导航中的磁力计实时感受到的地球磁场，进而通过计算载体坐标系与导航坐标系的关系得到车辆当前的朝向。计算航向角的一般步骤为：① 首先利用惯性导航x、y、z三轴加速度计计算姿态角；② 再利用姿态角将磁场强度补偿到水平东北方向；③ 最后利用公式tan(y/x)计算得到航向角。

1.3 FSAC赛车感知系统软件

FSAC赛车软件部分选用基于 Linux内核的Ubuntu 18.04系统。在系统中部署ROS(robot operating system)[16]，利用其分布式框架结构的优势，可以通过系统内多个节点(node)之间通讯的方式让机器人分别运行不同的进程完成不同的任务。另一个优势是ROS中有很多优秀的、开源的、支持调用的API(application programming interface)如可以对点云进行处理的PCL点云库，对机器人进行仿真的Gazebo，支持可视化的Rviz，还有支持当前主流编程语言的编译器。ROS系统的另一个特点是开源性质，对从事相关工作的开发者十分友好，避免开发者们重复“造轮子”，在各开发者的共同努力下使其具有强大的生态链[17]。

2 FSAC赛车动态视野算法设计

FSAC赛车激光雷达感知系统目标是尽可能多地识别桩桶，使路径规划模块获得更远的规划路线，或者对先验地图有更好的匹配效果。

2.1 算法流程

算法的整体思路是利用FSAC赛车横向运动幅度大的特点，基于固定ROI方法进行改进，融入车辆的运动特性，从点云处理初期对赛车前方视野加以限制。

算法流程如图1所示：由惯性导航获取当前帧航向角Yaw_t，并与保存的上一帧航向角Yaw_t-1求差得到Diff_yaw，对Diff_yaw进行加权滑动平均滤波，由路径规划程序节点获取当前车速V、转向方向C，将各个参数进行关联，计算得出ROI参数前方视野减少量X和两侧视野变化量Y，通过同步后的ROS消息传入点云处理模块，得到当前时刻的感兴趣区域。

图1 算法流程框图

2.2 加权滑动平均滤波

在传感器工作过程中，采集的数据难以避免地受到多种形式的影响和干扰，引起某些时刻采集的传感器信号与基线偏差大的问题，这些问题会影响最终结果的可靠性。论文采用加权滑动平均滤波的方法对数据进行平滑处理，使传感器采集的信号曲线更加光滑，从而达到降低干扰信号的影响，提高数据可信度的目的。滑动平均滤波(又称递推平均滤波)，该方法的原理为：连续取N个采样值构造一个长度为N的队列，队列中的内容随数据的进入实时向前流动，将采样得到最新的数据插入队尾，同时队首的第一个数据从队列中流出保持队列的长度，对队列中的N个数据求平均值，得到的结果便是经过滤波的值[18]。滤波公式为:

(3)

式中：N为固定队列的长度，根据传感器使用场景的不同，选取不同的值，队列长度越短对曲线的平滑效果越差；反之，队列长度越长，得到的曲线平滑度越高，但是响应速度也随之变慢。

为了提高滤波的响应速度，采用加权的方法对越早时刻数据分配更小权重，即加权滑动平均滤波[19]，公式为：

(4)

式中：h(n)为加权因子；Y(n)称为X(n)的加权平均，可以把Y(n)当作X(n)和h(n)的卷积。按照距离当前时刻越近的采样点权值越大的原则，并考虑FSAC赛车采集的数据和传感器特性确定加权平均因子。

IMU中的陀螺仪和加速度计等测量器件存在一定的系统误差和随机误差。论文使用的航向角之差数据由上下帧作差得到，减小了受误差影响的效果，并利用加权滑动平均滤波使数据更加接近真实值。图2为FSAC赛车在八字环绕赛道中，以15 km/h匀速行驶右侧单个圆形赛道所采集的的航向角差随时间变化曲线。

图2 航向角差随时间变化曲线

图2中位于最上方曲线为采集到航向角差Diff_yaw的原始图像，位于中间的曲线为经过窗口为10的滑动平均滤波后的曲线，位于最下方的曲线为经过窗口为10的加权滑动平均滤波后的曲线。由图2可知，加权滑动平均滤波有更好的效果，经过加权滑动平均滤波，以当前车速航向角差稳定分布在0°～0.8°。分析不同车速下的上下帧航向角变化量随时间变化曲线图像关系，为接下来确定不同车速对应的参数P作准备。

2.3 建立联系

根据赛道情况确定初始ROI参数X、Y，X表示前方视野距，Y表示左右方视野距离变化。通过传感器和其他程序传递来的各项参数与之关联。

2.3.1确定初始ROI参数

确定FSAC赛车初始感兴趣区域，经过实地实验激光雷达在0～30 m范围内对桩桶有很好的检测效果，综合考虑路径规划所需要的规划距离，选取初始感兴趣区域前方视野距离设置为X始，左右方视野距离设置为各±Y始。

2.3.2确定X、Y参数

设参数P为滤波得到的下一时刻x方向减小值，对不同车速下的航向角差Diff_yaw数据进行采集并分析，按照保证路径规划距离合适的前提条件设计参数P。举例如下：假设在速度为 60 km/h时，采集到的上下帧航向角差Diff_yaw角度的区间为0～9°，那么将其分成1～9份(1°，2°，3°，4°，5°，6°，7°，8°，9°)；假设按照路径规划需求，下一时刻x方向减小值P设计为最大9 m，那么也将其分成1～9份(1 m，2 m，3 m，4 m，5 m，6 m，7 m，8 m，9 m)；将上下帧航向角差Diff_yaw角度与下一时刻x方向减小值P以正比例关系相对应。

如表1所示，不同车速下航向角差Diff_yaw与参数P的对应关系，实际使用时通过数值查表的方式快速获得P值，进行后续计算。

表1 车速V与P值、Diff_yaw关系参数

X=X始-P

(5)

(6)

(7)

式中：车轮转角C向左为正，向右为负；控制Y参数视野变化方向，视野向左侧转移量为Y左，视野向右变化为Y右。

因为FSAC赛车的航向角差Diff_yaw反应了赛车此时横向运动的幅度大小，幅度越大，说明赛车正处于急转弯的工况，感兴趣区域应该向弯道的内侧移动。当FSAC赛车在八字环绕赛道中行驶到当前位置时，原算法激光雷达感知区域为蓝色方框与红色虚线框联合的区域，只有约1/4的区域可以覆盖赛道，感知效率低,如图3所示。而经过动态视野算法处理后激光雷达感知区域为蓝色方框与绿色方框联合的区域，有大约1/2的区域可以覆盖到赛道，可以多感知到6个桩桶。

图3 视野示意图

3 实验验证与分析

论文将满足大赛规则的FSAC赛车作为实验车，赛车装有禾赛40线激光雷达和导远电子INS570D惯性导航系统。让FSAC赛车在随机摆放的赛道中进行测试，测试车速为10～35 km/h。

3.1 实验平台介绍

论文使用的禾赛40线激光雷达性能参数如表2所示，导远电子INS570D组合导航系统参数如表3所示，禾赛40线激光雷达通过网口通信的方式与工控机通信，组合导航以串口通信段方式与工控机通信。

表2 激光雷达基本参数

表3 惯性导航基本参数

FSAC赛车软件部分开发并部署于ROS，主要完成环境感知以及路径规划等功能。利用ROS中所提供的开发工具降低调试难度，通过Rosbag工具进行数据记录与重放，按照需求记录FSAC赛车在赛道中行驶时的某个节点发布的话题，将数据包移动至开发人员的电脑中，便于还原实验场景进行算法分析和数据处理。Rviz是ROS中一款三维可视化平台，利用Rviz实现对话题消息数据接收并进行可视化展示，实车测试中可以通过Rviz给对象发布控制信息，从而实现实时观察硬件的响应和控制效果。

3.2 实车验证

为了测试算法在实际使用中的效果，在FSAC赛车平台上部署本文算法，随机摆放桩桶进行高速循迹测试。实验赛车在赛道中行驶如图4所示，激光雷达布置在车头之下前翼之上，坐标系x轴向前;惯性导航布置在赛车后轴与主环之间，坐标系x轴向前。

图4 在试验场中的实验赛车行驶场景

测试过程包括：

1) 为了描述高速循迹赛道情况，对实验场地进行建图如图5所示，抽取代表性弯道AB部分，A处为赛车即将进入弯道，B处为赛车已经进入弯道。

图5 试验场建图

2) 为了避免干扰因素对实验的干扰，同时部署原ROI算法与动态视野算法，并且同时运行2种算法。

3) 启动赛车以30 km/h的速度经由AB处进入弯道。记录激光雷达感知到的桩桶个数，路径规划轨迹，路径规划距离。

在赛道AB段激光雷达实时扫描的原始点云如图6所示，地面比较平坦且视野清晰，激光雷达视野内干扰物体较少，满足实验要求。

图6 弯道AB处原始点云

实验结果如图7—10所示，当赛车行驶到A处时，原算法ROI区域可以获得11个桩桶位置信息，动态视野算法可以获得15个桩桶位置信息，感知效率提升36%。当赛车行驶到B处时，原算法ROI区域可以获得9个桩桶位置信息，动态视野算法可以获得12个桩桶位置信息，感知效率提升33%。观察2种算法赛车路径轨迹，动态视野算法由于激光雷达感知到的桩桶个数更多，获得的路径规划距离更远，轨迹更为平滑。

4 结论

1) 考虑FSAC赛车的运动特性和传感器的布置特点，设计了一种基于激光雷达信息和惯性导航信息的动态视野感知算法，根据激光雷达提供的赛车周围桩桶信息，惯性导航提供的赛车自身姿态信息，动态视野算法通过动态调整赛车视野，实现在弯道行驶时获得更多的桩桶信息，提高了赛车弯道感知效率30%以上。

2) 实车实验验证了算法具有准确性高、实时性好的特点。研究方法对ROI的动态调节和考虑载体的运动姿态，既可以应用于FSAC赛车上，也可以推广至工作环境更为复杂的道路车辆中。