基于“Pure Pursuit”自动驾驶汽车的路径跟踪控制

王亮，陈齐平，罗玉峰，陈立伟，谢加超

(1.华东交通大学载运工具与装备教育部重点实验室，江西南昌 330013;2.江西江铃集团江铃新能源汽车有限公司智能电子开发部，江西南昌 330013)

0 引言

近年来，随着汽车保有量的持续大幅增加，智能汽车和自动驾驶技术有效地减缓交通拥堵、降低车辆事故发生率，其已成为研究热点[1]。路径跟踪作为自动驾驶技术的核心任务之一，其主要任务是控制车辆沿着参考轨迹行驶，路径跟踪过程中产生误差，交通事故率与交通通行效率等均由跟踪误差大小决定。而跟踪误差主要取决于车辆当前时刻前轮转角，文中直接通过当前车速确定预瞄距离，进一步确定预瞄点横向误差，利用Pure Pursuit算法输出前轮转角，保证了自动驾驶汽车沿着参考路径行驶且降低了行驶过程中交通事故率。因此，研究基于Pure Pursuit算法对路径跟踪控制可行性和稳定性有着重要意义。

自动驾驶汽车循迹控制方法主要有LQR、MPC和纯跟踪算法[2]，一般分为预瞄和非预瞄两种[3]，非预瞄控制方法借助于MPC算法[4-6]，在此基础上运用模糊控制算法，实现横纵向耦合路径跟踪控制[7]。建立车辆运动学模型并进行线性化处理，得到跟踪误差模型,利用未来时刻误差确定车辆方向盘转角[8]，但算法过程较复杂，不易实现。预瞄控制即纯跟踪算法[9-10]，纯跟踪控制方法更贴近真实驾驶风格，控制精度较高，且算法简单易实现，因此被广泛采用到路径跟踪控制中[11]，涉及农业机械、航空航天等各个领域。利用模糊控制确定插秧机预瞄距离，考虑到其转向系统为一阶惯性环节，因此可建立运动学模型，稳定地实现插秧机路径跟踪控制[12]。改进的智能飞机牵引车路径导航用偏差统计的方法实现不同预测距离下最优融合系数的选择，提高控制精度，该算法的路径轨迹误差控制在0.5 m范围内，满足飞机自动牵引滑行的精度要求[13]。但这些均缺乏了与农业机械、航空航天等领域相关的仿真软件去验证方法的准确性。起初，有学者将预瞄距离考虑定值去研究纯跟踪算法[14]，能够利用机器视觉保持车道。三辆车以10～60 km/h速度行驶，但较少考虑到车辆速度对循迹过程影响。OLLERO和HEREDIA等[15]给出了纯追踪模型曲线跟踪模型在极坐标系下的模型公式，对于曲线跟踪情况，采用极坐标更便于推导车辆运动学模型与曲线跟踪的控制量。但在实际应用中，大多数仪器给出的实时数值都是基于直角坐标系下的数据。

目前自动驾驶汽车路径跟踪控制算法运用到实践过程中较为复杂且缺乏与Carsim专业软件进行仿真验证，文中提出一种基于Pure Pursuit自动驾驶路径跟踪控制算法，该算法根据当前车速计算预瞄距离，进而通过自动驾驶平台获得预瞄点横向偏差确定前轮转角。取得了以下两个方面工程贡献：(1)提出的基于Pure Pursuit自动驾驶路径跟踪控制算法，算法稳定可行，控制计算过程更简单；(2)该控制方法误差小，精度高，满足路径跟踪控制要求，有助于提高新能源汽车量产。

基于以上文献针对自动驾驶汽车路径跟踪控制研究方法存在的不足，为了提高自动驾驶汽车路径跟踪控制中的可行性和稳定性，本文作者设计出路径跟踪控制技术路线图，如图1所示。

图1 路径跟踪控制技术图

1 参考轨迹设计

1.1 数据采集与转化

借助某集团新能源汽车自动驾驶平台可实现车辆高精度定位，选中园区智能电子开发部附近一条线路作为参考轨迹。为了获得参考轨迹上各点高精度经纬度值，因此车辆在满足匀低速行驶工况下，对参考轨迹各点经纬度值进行采集。

进一步研究Pure Pursuit算法在路径跟踪的应用，利用高精度定位获取各点经纬度值，涉及不同坐标系，存在坐标转换的问题[16]。将其转化为大地坐标下坐标值，地球长半轴为6 378 136 m，短半轴约6 356 755 m。针对单点转化大地坐标过程，x、y分别表示参考轨迹在大地坐标系横、纵坐标值，I、B分别记为经度系数和纬度系数，计算公式为

I=Lon·π/180

(1)

B=Lat·π/180

(2)

式中:Lat表示纬度值，Lon表示经度值。

(3)

式中:B0、Tan为常数。

(4)

(5)

式中:e1表示第一偏心率，e2表示第二偏心率。

常数K满足公式(6)：

(6)

则由以上表达式可知，大地坐标x、y可表示为公式(7)：

(7)

由公式(7)可知，自动驾驶平台对目标路径的经纬度数值进行采集，通过进一步转化到大地坐标系下，即可获得大地坐标系下坐标值。

1.2 数据处理

将已转化横、纵坐标数据导入到MATLAB新建变量中。横、纵坐标共两个变量，因此需建立两个表格，并分别命名为x1、y1。接着在命名窗口中输入cftool，选用“Sum of Sine”中选项作为参考轨迹的拟合曲线[17]，该多项式可详细描述出直线、连续弯道等复杂路径，便实现了对转化后的数据拟合。参考轨迹拟合结果，如公式(8)所示：

y1=a1sin(b1x+c1)+a2sin(b2x+c2)+a3sin(b3x+c3)

(8)

式中:a1，b1，c1，a2，b2，c2，a3，b3和c3均为相关系数，根据参考轨迹经纬度值及拟合过程，可得：

(9)

2 路径跟踪控制

2.1 运动学模型

低速通过弯道时，转向轮基本无侧向力作用，因此，无人车轮胎滚动时可忽略轮胎侧偏角。此时车辆转向必须如下所示，外侧轮胎转弯半径大于内侧轮胎[18]，即Ackermann转向模型，如图2所示。

图2 Ackermann转向模型

由图2可知，假定小转向角时，根据几何关系，可以得出转向角为

(10)

式中:d表示轮距，L表示轴距，R表示转弯半径，δ1表示左前轮转角，δ2表示右前轮转角，P表示转向中心。

为简化计算，将四轮车辆模型转换为车辆单轨模型，并假设车辆在平面行驶[19]，简化车辆单轨模型，如图3所示。

图3 车辆单轨模型

车辆单轨模型将4轮车辆简化成2轮的模型，并且假定车轮只在平面上行驶，采用该模型的优点能够在较低速度的场景下预测车辆运动，简化了前轮转向角与后轴遵循的曲率之间的几何关系，由图3及式(10)可知：

(11)

式中:δ表示前轮转角，R表示在给定转向角下后轴遵循的圆的半径。

2.2 Pure Pursuit算法

从车辆单轨模型出发，Pure Pursuit算法以车后轴为切点,车辆纵向车身为切线,通过控制前轮转角，使车辆可以沿着一条经过目标路点的圆弧行驶[20]。其中，XOY表示大地坐标系，xoy表示车辆坐标系，(goal_X,goal_Y)表示预瞄点在大地坐标系位置，(Veh_X,Veh_Y)表示车辆在大地坐标系XOY位置，跟踪示意图，如图4所示。

图4 跟踪示意

结合正弦定理，由图4可得：

(12)

式中：Ld表示车辆后轴位置到预瞄点距离，α表示目前车身姿态和预瞄点的夹角。

道路曲率k为

(13)

由式(11)—(13)可求出前轮转角为

(14)

设预瞄点在车辆坐标系xoy位置为(goal_x,goal_y)，在图4中，与点(goal_X,goal_Y)重合，通过大地坐标系与车辆坐标系之间转化[21]，如公式(15)所示：

(15)

式中：θ表示车辆航向角。

则由式(15)可得：

(16)

式中：goal_x表示车辆距离参考轨迹横向偏差，为了简化计算，可重新定义一个参数量e，表示车辆当前姿态和目标路点在横向上的误差，参考轨迹横向偏差goal_x即为横向上的误差e；goal_y表示车辆预瞄距离，可设为s。

则前轮转角为

(17)

预瞄距离s的选取与车速、跟踪误差有关[12]，预瞄距离过大会导致路径跟踪存在延迟，预瞄距离过小则会导致路径跟踪发生震荡，在实际运用过程中需要考虑到参考轨迹曲率[22]。因此，预瞄距离的选取对自动驾驶路径跟踪控制有着重要意义。文中自动驾驶路径跟踪预瞄距离选取考虑到车速与速度成正比，文中比例系数为1.9。

横向偏差e=goal_y成立条件[23]：

[(goal_x)x(i)-s]·[(goal_x)x(i+1)-s]≤0

(18)

2.3 联合仿真模型搭建

根据以上分析，建立Carsim车辆模型与Simulink算法模型，联合仿真模型如图5所示。通过对参考轨迹数据的采集及转化数据处理，获得参考轨迹在大地坐标系下横、纵坐标值分别存储到From Workspace1和From Workspace2两个工作空间中；利用MATLAB Function 模块，结合存储到工作空间中的数值、车辆实际坐标以及实际航向角，通过坐标转化，可计算出预瞄点在车辆坐标系纵坐标值goal_y；将预瞄点纵坐标值作为PP algorithm(Pure Pursuit算法)模块输入，另一个输入为车辆纵向速度，便可通过算法求出当前时刻方向盘转向角。

图5 联合仿真模型

由图5可知，Carsim模块输出当前时刻车辆实际位置信息、车辆航向角和纵向速度，为了实现自动驾驶汽车完成参考轨迹跟踪，根据车辆当前时刻纵向速度确定预瞄距离，寻找到当前时刻预瞄点，利用Pure Pursuit算法求出当前时刻方向盘转向角并输入到Carsim模块，从而建立闭环控制的自动驾驶汽车轨迹跟踪仿真模型。

3 实验与分析

3.1 试验平台搭建

某集团新能源易至EV3微型车经过改装后，包括GPS天线、4G天线和惯性导航等定位设备、摄像头和传感器等感知设备、车体和工控台。文中搭建了自动驾驶汽车实车试验平台，如图6所示。

图6 实车试验平台

DTU模块支持移动、联通2G/3G/4G，电信4G网络，网络支持最大下行速率150 Mb/s和最大上行速率50 Mb/s，在缺乏3G和4G网络覆盖的偏远地区也能正常工作，接收4G天线信号并输出差分数据等相关报文信息。982-RG具有实现差分定位和GPS定位功能，接收DTU模块差分数据和无线GPS天线信号，输出车辆高精度定位，包括经度、纬度值等，可以用于自动驾驶汽车参考轨迹获取。RT由牛津大学提供的一种集成技术设备，由惯性导航(实时进行精确运动测量的仪器，内部具有六轴陀螺仪和加速度计等主要零件组成)、基站(移动设备接入互联网的接口设备)和设备固定件组成，该设备用于接收982-RG输出的GPS信号和差分数据，信息输入到自动驾驶汽车工控台。

3.2 路径跟踪实验分析

为了验证联合仿真结果的正确性，进行了实车试验，3种实验工况车速分别为10、20和30 km/h。预瞄距离s分别为5、10、15 m，车辆整备质量为1 115 kg，仿真和试验横向距离为60 m，路面附着系数为0.85。考虑到试验车为纯电动汽车，Carsim中传动系设置为No Dataset，即电动汽车模式。分别对仿真轨迹与参考轨迹、实车轨迹与参考轨迹结果进行了分析与对比。v=10 km/h的轨迹曲线如图7所示。

图7 v=10 km/h的轨迹曲线

由图7可知，速度v=10 km/h时，仿真轨迹与参考轨迹、实车轨迹与参考轨迹变化趋势接近一致。仿真轨迹起点坐标为(0，-3)，因此初始阶段存在振荡现象。与参考轨迹相比，仿真试验过程中，整个仿真过程中，最大误差不超过0.1 m。而实车验证过程中，横向位移在区间[0,20 m]和[30 m,40 m]误差约为0.2 m，区间[20 m,30 m]和[40 m,60 m]误差达到0.3 m。仿真轨迹和实车轨迹最大误差均出现在拐弯处，v=20 km/h的轨迹曲线如图8所示。

图8 v=20 km/h的轨迹曲线

由图8可知，速度v=20 km/h时，仿真轨迹与参考轨迹、实车轨迹与参考轨迹变化趋势也基本相同。与速度10 km/h跟踪过程相比，仿真轨迹最大误差约为0.2 m，实车轨迹误差在横向位移区间[20 m,30 m] 和[40 m,60 m]时达到了1.2 m，在区间[20 m,30 m]和[40 m,60 m]实车轨迹误差约为0.4 m。v=30 km/h的轨迹曲线如图9所示。

图9 v=30 km/h的轨迹曲线

由图9可知，当速度v=30 km/h时，仿真轨迹最大误差约为0.3 m，实车轨迹误差超过了1.5 m，不满足路径跟踪要求。为了直观地描述不同速度对应仿真实验和实车试验的跟踪效果，其路径跟踪误差对比如表1所示。

表1 路径跟踪误差对比

由图7—9及表1可知，仿真轨迹基本上与参考轨迹一致，对应误差均小于0.3 m，可以实时输出车辆方向盘转角，完成路径跟踪控制。但实车轨迹存在显著差别，自动驾驶车辆速度低于20 km/h，误差均小于0.4 m，符合自动驾驶路径跟踪要求。Pure Pursuit算法仅考虑到车辆运动学模型，未考虑到车辆动力学，因此进一步验证了该算法适用于低速且路况良好的自动驾驶车辆路径跟踪控制。自动驾驶车辆执行机构转向电机存在延迟现象，导致试验车辆未能及时跟踪参考轨迹。因此横向位移在区间[0,25 m]内，实车轨迹处于参考轨迹内侧，在区间[25 m,50 m]，实车轨迹处于参考轨迹外侧。

4 结论

文中借助某集团新能源自动驾驶平台进行了目标路径设计。针对参考轨迹跟踪控制问题，利用Carsim搭建车辆模型和Simulink搭建Pure Pursuit算法模型，计算出当前时刻前轮转角，进行了路径跟踪控制仿真分析。最后，以江铃新能源汽车为试验平台对所提出的方法进行了实车试验。仿真实验工况中，最大误差均小于0.3 m，实车试验工况中车速低于20 km/h，实车轨迹误差小于0.4 m，符合自动驾驶汽车路径跟踪控制要求，考虑到实车试验执行机构存在延迟，跟踪误差较大，但低速工况下均能有效地实现自动驾驶汽车沿着参考轨迹行驶。文中预瞄距离仅取决于纵向车速，未来将进一步考虑到车速变化、循迹误差大小以及道路曲率等工况对路径跟踪控制影响。