一种结合规划层的无人驾驶路径跟踪方法

沈 鹏，曹 凯，刘秉政，杨 旭，奉 柳

(山东理工大学 交通与车辆工程学院，山东 淄博 255049)

局部路径规划[1-4]和路径跟踪技术[5-7]是无人驾驶车辆的核心技术。局部路径规划的目标是在动态环境下，寻找一条从当前点到目标点的无碰撞路径，目前常用的方法有人工势场法、栅格法、可视图法以及其他各种智能化算法。其中人工势场法以其数学计算简单明了而被广泛应用，当前研究主要集中在解决如局部极小问题、目标不可达问题和抖动消除问题等方面。

路径跟踪控制的目的是使车辆沿着期望路径行驶，同时保证车辆的横向稳定性，其控制算法是路径跟踪控制的关键。当前路径跟踪控制算法主要有模糊控制、滑模控制策略、单点或多点预瞄策略、模型预测控制、最优控制等。

无人驾驶车辆在实际环境中进行自主驾驶时，由于外部环境是动态变化的，所以给定期望路径下的跟踪控制并不能保证无人驾驶车辆准确地处理任何路径跟踪问题。主要原因有：(1)由于环境未知，预先给定的参考路径不一定能满足安全行驶的要求；(2)当所给定的期望路径上出现障碍物时，无人驾驶车辆需要根据障碍物信息进行重新规划。

为了解决以上问题，本文提出双闭环PID的控制方法(路径闭环，预瞄偏差闭环)进行路径跟踪，同时结合视觉传感器的信息来实时更新和矫正期望路径，并且提出路径跟踪灵敏度概念来分析路径跟踪的误差变化情况，以实现在跟踪期望路径的同时可以较好地避开障碍物。

1 路径跟踪的预瞄模型

1.1 双差分GPS定位

本文采用双差分GPS对车辆进行定位，其原理主要是根据基准站已知精密坐标，计算出基准站到卫星的距离改正数，并由基准站实时地将这一改正数发送。车辆自身载有两个移动站，它们不但接收GPS信号，同时接收基准站的改正数。通过这种方法提高了车辆自身的定位精度，为验证本文提出算法的有效性提供了可靠的硬件支持。

1.2 横向预瞄误差模型的建立

车辆在实际行驶过程中以当前车辆的运动状态为基础，预测车辆行驶至前方某位置时车辆质心与期望路径道路中心线之间的横向偏差，该偏差即为横向预瞄误差[8-9]。在车辆进行路径跟踪时，通过横向预瞄误差与车辆的运动状态得到汽车的前轮转角，进而根据前轮转角实现对目标路径的跟踪。横向预瞄误差模型如图1所示。

图1 横向预瞄误差模型Fig.1 Lateral preview error model

从图1中几何关系可知：

(1)

(2)

(3)

由式(2)得

(4)

将式(1)和式(4)带入式(3)，则横向预瞄误差模型可表示为

ep=e+Lsinψr

(5)

横向预瞄误差变化率可表示为

(6)

1.3 双闭环PID控制路径跟踪

基于横向预瞄误差模型的路径跟踪控制系统相当于一个驾驶员模型，驾驶员模型是导航技术的重要组成部分。驾驶员根据横向预瞄误差的正负和大小来转动方向盘调节前轮转角，实现对车辆行驶路径的调节。

为了较好地控制路径跟踪，这里采用了双闭环PID控制器(如图2所示)。PID控制器主要对偏差量进行比例、积分以及微分控制。首先采用路径闭环控制方法，在期望路径的基础上，根据视觉传感器得知的环境信息实时更新和规划新的路径，并且进行跟踪，通过这种方法实现期望路径与实时矫正路径之间的闭环控制；其次采用横向预瞄偏差的闭环控制方法，结合每一时刻差分GPS对车辆的定位信息(包含二维坐标以及航向角)，实时更新当前时刻的横向预瞄偏差，在车辆到达下一时刻位置时，重复此过程，从而实现了横向预瞄偏差的闭环控制。使用PID控制器进行控制具有算法简单、实现方便、鲁棒性好、参数调整容易、可靠性高等优点。

图2 双闭环PID控制器Fig.2 Double closed loop PID controller

本文PID控制参数的调整采用了参数模糊自整定的方法，通过该方法找出PID参数与横向预瞄误差以及其变化率之间的模糊关系。以车辆当前状态为基准，车载计算机通过感知层接收到的有关车辆位置坐标、航向角信息实时预测出下一时刻所到达的位置，计算出横向预瞄误差et以及横向预瞄误差的变化率ec，在车辆运行过程中通过不断检测et和ec，再根据模糊控制原理[10]的3个参数进行在线修改，以满足不同的et和ec对控制参数的不同要求，从而可以实时地计算出方向盘转角，达到较好的路径跟踪效果。PID控制律如下：

(7)

式中：KP,Ki,Kd分别为PID控制器的3个参数；e(t)为横向预瞄误差；δ(t)是方向盘转角。

2 基于人工势场法的局部路径规划

2.1 传统人工势场法

人工势场法由虚拟力法[11-12]据电荷间相互作用的规律理论演变而来。其基本思想是，在被控对象运动环境中人为建立势场，其中障碍物为斥力场，目标点为引力场，分别对被控对象产生斥力与引力，斥力与引力形成的合力控制被控对象的运动方向。

在人工势场法规划过程中，规定引力的大小与车辆和目标点之间的距离成正比，车辆相距目标点越远引力作用越明显。相反，斥力的大小与车辆和障碍物之间的距离成反比，车辆相距障碍物越近，斥力就越大。当车辆距离障碍物在斥力作用范围以外时，不考虑斥力对车辆的影响。其中目标点对车辆引力势能函数为

(8)

式中：k为引力系数；Xr表示车辆的位置坐标；Xg表示目标点的位置坐标；ρ(Xr,Xg)表示车辆与目标点之间的欧几里得距离。根据势能函数与引力函数的关系得到引力函数为

Fatt=kρ(Xr,Xg)

(9)

车辆所受到的障碍物的斥力处理方式与引力类似，假设Xo为障碍物的坐标位置，m为斥力系数，假设障碍物斥力场的最大影响范围是半径为p的圆环，当车辆距离障碍物大于p时，车辆所受到的斥力大小为0，因此斥力势能函数为

(10)

式中,ρ(Xr,Xo)表示车辆与障碍物之间的欧几里得距离。车辆所受到的斥力为斥力场的负梯度方向，斥力函数表示为

(11)

所以，车辆在人工势场下所受到的合力表示为

(12)

式中，n表示车辆在当前位置所受到的障碍物斥力影响的个数。

2.2 改进的人工势场法

(13)

式中,n为任意常数。改进后的斥力函数为

(14)

其中，

(15)

3 规划层与控制层结合的设计

本文在原有的路径跟踪控制基础上加入规划层后，形成新的控制系统，结构如图3所示。在该控制系统中，当环境感知系统探测到障碍物后，将障碍物的相对位置等信息传递给路径规划系统，由该系统根据需要选择是否重新规划路径[15-16]，再通过路径跟踪控制系统计算出被控车辆到达下一时刻预测位置所需要转动的方向盘转角。当被控车辆到达下一时刻预测位置时，将当前时刻车辆的状态信息反馈给跟踪控制系统以及环境感知系统，从而实现双闭环的控制。

图3 融入规划层的路径跟踪控制图Fig.3 The path tracking control combined with planning layer

4 仿真及实验分析

4.1 仿真实验

对以上提出的算法进行仿真实验，设定起始点、障碍物点以及目标点的位置，得到的结果如图4—图7所示。

图4 无规划层的路径跟踪图 Fig.4 Path tracking control without planning layer

图5 单个障碍物下的路径跟踪图Fig.5 Path tracking control with one obstacle

图6 3个障碍物下的路径跟踪图Fig.6 Path tracking control with three obstacles

图7 5个障碍物下的路径跟踪图Fig.7 Path tracking control with five obstacles

图4是在单个障碍物环境下没有规划层算法的仿真结果；图5是在单个障碍物环境下具有规划层算法的仿真结果。由图4与图5的对比可以看出，具有规划层的路径跟踪算法可以在车辆遇到障碍物的时候，实时矫正与更新车辆的期望跟踪路径，提高了路径跟踪的安全性与可靠性。图6和图7是在不同障碍物个数的环境下具有规划层算法的仿真结果，以此来验证当环境由简单变向复杂时上文所提出算法的有效性。

4.2 路径跟踪灵敏度分析

为了验证该算法的有效性，本文提出路径跟踪灵敏度这一概念来表示当仿真环境由简单变向复杂时路径跟踪误差的变化情况，误差变化大说明车辆路径跟踪的灵敏度低，误差变化小说明车辆路径跟踪的灵敏度高。为了说明误差的变化情况，提出了平均偏差P，其计算公式如下：

(16)

从表1中可以看出，当仿真环境中的障碍物个数从1个到3个时，平均偏差变化了2.05 cm，障碍物个数从3个变化到5个时平均偏差变化了1.28 cm，由此可见当仿真环境由简单变向复杂时，平均偏差变化不大，路径跟踪灵敏度较高。

表1 不同环境下轨迹跟踪的平均偏差
Tab.1 Mean deviation of path tracking in different environments

障碍物个数偏差累计量/m轨迹点数目/个平均偏差/m1576.524 81 1680.493 63628.744 31 2230.514 15740.821 41 4060.526 9

4.3 实车试验

为了验证本文算法的有效性，利用改进的人工势场法结合预瞄模型的跟踪控制算法进行了实车试验。试验的车辆为改制的越野车，驾驶系统采用线控转向系统，控制器采用Dspace公司的AUTObox，车辆的定位采用NovAtel公司的GPS定位系统，算法的实现环节是将Simulink自动代码生成的sdf文件与dspace上位机连接，再通过下位机AUTObox控制器来控制车辆的行为。

1)控制层与规划层结合算法的试验

图8所示为本文规划层与控制层结合的算法在实车上试验的结果图。图中蓝色线为期望轨迹线，黑色虚线为车辆遇到障碍物之后重新规划的轨迹，红色线为车辆跟踪的轨迹。

图8 控制层与规划层结合的跟踪结果图 Fig.8 Path tracking result diagram with control layer and planning layer combined

图9 跟踪路径中转角对比图 Fig.9 Corner comparison diagram in path tracking

2)试验方向盘实际转角与目标转角结果图

图9是无人驾驶车辆在以上提出的算法中进行路径跟踪的转角对比图。图中红色线代表目标转角，是通过预瞄模型计算出来的、不偏离期望路径所需要转动的方向盘角度,蓝色线代表实际转角。由于实际执行的过程中，方向盘会存在转向的阻力与摩擦力，从而出现实际转角不会完全与理想的目标转角吻合的情况。由图9可知，转向效果较好，路径跟踪效果较好。

5 结论

针对传统路径跟踪方法中不能在未知环境下避开障碍物的问题，本文提出了规划层与控制层结合的方法。规划层采用改进的人工势场法，控制层采用了双闭环的PID控制方法，并且根据模糊控制原理在线实时调节PID控制算法的3个参数，从而在车辆进行路径跟踪过程中可以较好地躲避障碍物，提高了路径跟踪的安全性与鲁棒性。研究结论如下：

1)通过仿真结果可以看出，当障碍物个数从少变多时，路径跟踪的平均误差有所增加，但不是线性增长且增加幅度不大，由此看出双闭环PID控制算法的灵敏度较高。

2)结合实车试验可以看出，在实际环境中，虽然部分路径的跟踪效果不是特别理想，但是排除客观存在的原因如路面情况以及方向盘的摩擦阻力，整体来看本文提出的算法可以较为完整地实现预期效果。

3)整体来看，本文算法中规划层与控制层能形成较好的衔接，并且基本实现了在动态环境中在线实时地调整参数重新规划路径， 提高了路径跟踪的安全性和鲁棒性。