基于PER-APF算法的无人驾驶汽车换道轨迹规划

胡丹丹，张 琪

(中国民航大学 机器人研究所，天津 300300)

0 引言

我国制造业正逐步向智能化、数字化迈进，汽车产业作为制造业的龙头产业，正处于由传统汽车向无人驾驶汽车的转型阶段，被我国众多汽车厂商广泛研究。无人驾驶汽车的系统架构如下：车辆的感知模块将激光雷达、相机、组合惯性导航系统等传感器信息融合后输入到决策模块；决策模块接收感知模块的信息并结合周围环境信息输出合理的驾驶行为；运动规划模块接收驾驶行为指令，同时驱动底层油门、转向、刹车等系统控制车辆完成驾驶行为。车道变换是无人驾驶汽车最常见的驾驶行为[1]，换道过程的安全性与稳定性对提高无人驾驶汽车的通行效率与减少交通事故的发生具有重要意义[2]。无人驾驶汽车在换道过程中如何躲避障碍物进行合理的避障是车辆执行换道操作的前提。换道轨迹规划是指基于感知模块的车载传感器融合后的信息，获得车辆行驶过程中遇到的空间障碍物信息与车道线信息等，在满足车辆动力学、运动学约束的条件下，规划出一条从起点到终点且符合车辆稳定性与舒适性指标的最优轨迹[3]。

目前，国内外学者对无人驾驶技术涉及的换道轨迹规划算法的研究主要包括两类：以避让静态障碍物为目标的轨迹规划算法和以避让动态障碍物为目标的轨迹规划算法。大多数文献只考虑了存在单种障碍物时车辆的换道策略，忽略了动态与静态障碍物共存时换道算法的适用性。Lim[4]提出一种结合采样和优化方法的混合轨迹规划方案，横向运动采用采样的方法使得轨迹规划器能够处理不同的轨迹，有助于车辆结合不同的环境生成反应轨迹，纵向运动采用数值优化的方法使得车辆能够适应不同的路况，以满足特定驾驶的目的；Adeel[5]利用5次多项式生成轨迹并借助PID(proportion integral differential)控制器进行控制解决不同情形下自动驾驶车辆的变道问题；Julia[6]提出了一种变道机动算法，首先判断是否存在允许车辆变道的纵向轨迹并选择合适的车间距与时间间隔，之后，应用变道机动算法求解两个松散耦合的凸二次规划问题，得到车辆在相应的时间间隔内以特定的车间距行驶时的纵向和横向运动轨迹。

与上述基于采样与优化的方法相比，人工势场法[7](APF，artificial potential field)具有算法结构简单、收敛速度快、控制性强等特点，被广泛应用于解决无人驾驶汽车的轨迹规划问题[8]。张家旭等[9]通过不同类型的函数分别构建弯道引力势能场、同车道行驶较慢车辆斥力势能场和弯道边界斥力势能场，3种势能场构成了车辆弯道超车路径的搜索空间，同时构建增量搜索算法，搜索出路径的最快下降方向，从而规划出超车路径，该方法并未考虑存在不同类型障碍物时的轨迹规划过程。冀杰等[10]构建了道路和障碍物的三维虚拟危险势能场，使得车辆能够主动避撞；Yuan等[11]提出了纵向安全距离模型和变道安全间距模型，通过改进虚拟斥力与引力模型，使得车辆能够进行避障处理；Liu等[12]构建了势场融合控制器和自适应分配单元，通过将速度势场与传统APF算法融合，使得无人驾驶汽车能够适应不同类型的障碍物；上述方法未考虑在换道或超车等驾驶行为下方法的适用性。

综上所述，APF算法被广泛应用于换道轨迹规划过程，但是应用场景较单一，没有考虑存在多种不同类型障碍物时车辆的换道过程。同时对于APF算法存在的局限性问题，提出PER-APF算法，并设置动态与静态障碍物共存的两车道场景与存在不可通行区域的三车道场景，验证所提算法的适用性与有效性。

1 无人驾驶汽车行车安全环境判定

无人驾驶汽车在实际道路行驶过程中，需要时刻保持与不确定性因素较多[13]的周围交通环境进行交互，为了保证车辆换道过程的安全性且不影响其他交通参与者，在车辆运动学模型[14]基础上，根据周围环境特征建立车辆横、纵向安全距离模型。

道路边界与交通流分割线分别约束车辆在可通行区域与不同的行车道行驶，根据车辆在道路不同位置行驶的危险系数不同，建立带有斥力区分的道路边界约束条件，以综合表征车辆换道过程的安全性。

1.1 多约束车辆换道轨迹规划模型

1.1.1 无人驾驶汽车运动学模型

为了保证规划的换道轨迹符合实际驾驶需求且满足车辆行驶过程中的运动学几何约束，在无人驾驶汽车轨迹规划阶段建立车辆的运动学模型[15]，如图1所示。由于第3部分设置无人驾驶汽车以较低速度行驶，车辆稳定性控制力较强，因此忽略动力学对车辆的影响。

图1 车辆运动学模型

根据车辆前后轴运动学约束条件和前后轮几何关系进行公式推导，得到车辆运动学模型表达式如式(1)所示:

(1)

式中，(xr,yr)为车辆后轴轴心的坐标；φ为车体的横摆角(航向角)；δf为前轮偏角；vr为车辆后轴中心速度；l为轴距。

1.1.2 无人驾驶汽车换道安全距离模型

1) 纵向安全距离模型：无人驾驶汽车在换道过程中，假设车辆的制动力发挥出最大效能时会产生最大制动减速度，用Ds表示两车的纵向距离。结合1.1.1节建立的车辆运动学模型并考虑道路环境与周围交通参与者的影响，为了保证本车不与当前车道前车发生碰撞，利用最大制动减速度、本车行驶速度、当前车道前车行驶速度等信息建立纵向安全距离模型。纵向安全距离Df_safe如式(2)所示，当Ds的值足够大且Ds≥Df_safe时，满足换道纵向安全距离的要求。

(2)

式中，Df_safe表示本车与当前车道前车的纵向安全距离；v、vf分别表示本车和当前车道前车的行驶速度；t表示本车从当前状态到开始制动时的反应时间；θ表示车辆行驶方向与水平纵向的夹角；a表示本车的最大制动减速度；af表示当前车道前车的加速度。

2)横向安全距离模型:车辆行驶时不仅要规划纵向速度保证本车与当前车道前车不会发生碰撞，同时还要在当前车道因存在障碍物不允许通行时执行换道操作，本车与目标车道后车的横向距离是保证车辆在换道过程中不影响后方交通参与者的首要考虑因素。因此，根据本车与后方交通参与者的速度、加速度等信息，建立车辆换道的横向防碰撞模型如式(3)所式。

(3)

式中，Db_safe表示本车与目标车道后车的安全距离；Db表示本车与目标车道后车的纵向距离；ab、ax分别表示相邻车道后车与本车的加速度；tc表示换道时间；vb表示目标车道后车的速度；Dr表示最大预留安全距离。

1.2 带有斥力区分的道路边界约束条件

车辆在结构化道路行驶过程中，道路边界与交通流分割线能够约束车辆在车道中心行驶，可以避免车辆因偏离行驶车道而发生交通事故。由实际驾驶经验可知，车辆靠近道路边沿的位置行驶时危险系数较大；相反，车辆处于交通流分割线位置行驶时危险系数较小。因此，根据车辆在道路不同位置的危险系数不同，采用变化快慢程度不同的函数类型构建道路边界斥力函数[11]，如式(4)所示：

Fr(x,y)=

(4)

式中，α1、α2表示道路边界约束函数的安全系数；l表示路宽。

无人驾驶汽车在道路行驶过程中，车辆的形状、尺寸以及路面宽度、道路边界均已经确定。应用APF算法解决无人驾驶汽车的换道轨迹规划问题时，大多数文献只考虑了道路边界斥力[10-11]对本车的影响，未考虑本车受到障碍物斥力与道路边界斥力的双重斥力影响时车辆的运动状态。为了避免无人驾驶汽车因受到两种斥力叠加使得车辆受到的斥力增大，从而导致车辆运动轨迹偏移并且影响车辆方向盘的转角与驾乘人员的舒适性。因此，根据障碍物与路边沿的距离不同，将车辆受到的道路边界斥力与障碍物的斥力进行区分，确定车辆受到的斥力类型。在图2所示的道路边界示意图中，rbound表示道路边界，rdiv_line表示交通流分割线，rcons表示障碍车辆与道路边界的距离。取rcons=0.3 m[17]，当障碍车辆与道路边界的横向距离小于0.3 m时，忽略障碍车的斥力作用，无人驾驶汽车仅受到道路边界的斥力作用；相反，当障碍车辆与道路边界的横向距离大于0.3 m时，无人驾驶车辆受到障碍车斥力与道路边界斥力双重作用，算法步骤如下：

图2 道路边界示意图

1)无人驾驶汽车的决策模块输出换道指令，运动规划模块接收指令，控制车辆执行换道操作。

2) 基于激光雷达、组合惯性导航系统、相机等传感器的信息，得到障碍车辆与路边沿的距离rcons的值。

3) 如果rcons<0.3 m，车辆受到道路边界的斥力作用，即:

Freq(X,Y)=Fr(X,Y)

4) 相反，如果rcons≥0.3 m，车辆受到道路边界斥力与障碍车斥力的双重作用，即:

2 PER-APF算法

传统APF算法[7]解决无人驾驶汽车换道轨迹规划问题的基本思想是：将车辆的行驶环境抽象成势能场，车辆在势能场中运动，势能场中存在目标点产生的吸引力与障碍物产生的排斥力，车辆沿着吸引力与排斥力的合力方向运动，最终运动到目标点位置，车辆在势能场中运动过程如图3所示。

图3 车辆在势能场中运动过程示意图

应用传统APF算法解决车辆局部轨迹规划问题时具有以下局限性：

1) 目标不可达:车辆运动到目标位置附近时，距离目标位置较近的障碍物产生的斥力可能大于目标点的吸引力，车辆因受到的引力小于障碍物斥力而无法继续运动到目标位置。

2) 局部最优值:车辆运动到某位置时，其受到目标位置的引力与障碍物的斥力大小相等方向相反，总合力为零，导致无法准确判断车辆的下一步移动方向。

由于无人驾驶汽车的运动特性复杂度较高，在真实道路行驶时所面临的不确定性因素较多，传统APF算法在解决换道轨迹规划问题中所暴露的问题愈加明显。因此，通过对传统APF算法的引力势场与斥力势场进行重新设计，在解决局限性问题的同时使得无人驾驶汽车能够规划出一条安全、合理的运动轨迹。

2.1 解决目标不可达问题

假设车辆在空间中的位置具有随机性，目标位置与障碍物位置已确定。假设车辆位置为(X,Y)，目标位置与障碍物位置分别为(Xg,Yg)与(Xo,Yo)，车辆在空间中受到目标位置的引力Fatt(x)如式(5)所示：

Fatt(X)=ηρ(q,qg)

(5)

式中，η为正比例增益系数；ρ(q,qg)为矢量，其大小为|q-qg|，方向由车辆指向目标点。

当车辆无法运动到目标位置时，构建以目标点为圆心，rg为半径的虚拟圆形区域，虚拟圆形区域用来确定造成目标不可达局限性的障碍物以及障碍物的位置。假设距离目标点最近的障碍物与目标点的距离为rs，当障碍物位置在目标点产生的虚拟圆域内时，增大目标点的吸引力作用，使得车辆所受的目标位置的引力大于障碍物的斥力作用，最终运动到目标位置。车辆在强吸引力作用下会导致运动速度过快影响驾乘人员的舒适性，因此在构建引力势场函数时，距离的指数值应选取适当。改进的引力势场函数如式(6)所示:

(6)

式中，m表示障碍物在目标点产生的虚拟圆形区域内的数量。

对改进的引力势场函数求解负梯度，得到车辆在势场中受到的引力大小，如式(7)所示:

(7)

2.2 解决局部最优值问题

车辆在运动空间中受到障碍物的斥力表达式如式(8)所示。

Freq(X)=

(8)

式中，k为正比例增益系数；ρ(q,qo)为矢量，大小为|q-qo|，方向由障碍物指向车辆；ρo表示障碍物产生斥力势场的作用范围，当车辆所在位置到最近障碍物的距离大于ρo时，没有斥力作用。

传统 APF算法陷入局部最优值时，车辆表现为在某处震荡或转圈[18]。当车辆无法到达目标点位置时，首先判断车辆是否处于局部最优值状态。记Dm为车辆在一段时间内能够到达的位置集合，Dp为车辆当前所处可规划区域的位置集合。当U(Dm)≤minU(Dp)时，表示在该位置车辆陷入了局部最优值状态；当U(Dm)>minU(Dp)时，表示车辆未陷入局部最优值状态。

通过在空间中的非平衡点位置增加虚拟斥力的方式，改变车辆受到的合外力大小，使车辆驶出局部最优值状态，同时可以避免因在平衡点处增大斥力而导致车辆受较大斥力而驶向相邻车道的现象。在空间中非平衡点处增加虚拟斥力Freq2(X)的方式如下：将空间中的车辆看成为一个质点，分别连接车辆与障碍物、车辆与目标点所在的直线。用障碍物与无人驾驶汽车的连线作为基准线，以障碍物为中心，呈顺时针30°角的方向向车辆与目标点的连线做直线，与车辆和目标点连线相交于一点，交点即为非平衡点。虚拟斥力的构建过程如图4所示。

图4 虚拟障碍物构建过程示意图

改进后的斥力表达式如式(9)～(10)所示：

(9)

(10)

式中，Freq1(x)表示障碍物对车辆的斥力作用；Freq2(x)表示车辆处于非平衡点位置时受到的虚拟斥力作用。

结合重新设计后引力与斥力的表达式，得到车辆在空间中受到的合力表达式如式(11)所示:

(11)

在不同的驾驶场景中，车辆受到的引力与斥力也互不相同，应用APF算法规划出换道轨迹的时间也不同。因此，根据车辆所处实际环境的复杂度不同，采用步长自调节搜索的方法进行轨迹规划。在环境情况复杂时增大搜索步长，环境情况单一时减少搜索步长，平衡车辆在局部最优值处搜索的时间，提高搜索效率，从而缩短车辆行驶至目标点的时间。

2.3 算法流程描述

基于无人驾驶汽车的感知系统，获取车辆与障碍物的距离、障碍物类型以及数量等信息，计算无人驾驶汽车运动到障碍物的斥力势场作用范围内时受到的合力大小。若合力为0，则通过在非平衡点处构建虚拟斥力的方式使得车辆驶出局部最优值状态；若合力不为0，则继续采用变步长的搜索方式进行轨迹规划，重复上述步骤直至车辆运动到目标位置。如果车辆无法行驶到目标位置，判断原因是否为目标位置的引力值较小，是则根据式(6)～(7)增大目标位置的引力大小，直至车辆运动到目标位置，算法流程如图5所示。

图5 算法流程图

3 仿真分析

设定不同的工况进行仿真试验，基于APF算法与PER-APF算法分别规划换道轨迹，对轨迹性能进行对比，选取不同时刻车辆与障碍物相对位置变化的示意图，并选取不同指标对轨迹数据进行对比分析，验证所提算法的合理性与有效性。应用Matlab R2014b仿真软件进行仿真分析，仿真参数设置如表1所示。

表1 仿真参数设置

3.1 双车道场景换道仿真分析

设置道路区域全部可通行的双车道场景，静止障碍物阻碍无人驾驶汽车(本车)继续在当前车道行驶，故本车向相邻车道运动。相邻车道存在一辆行驶速度低于本车行驶速度的动态车辆，由动态车辆运行轨迹可以看出，当本车运动到目标位置时动态车辆恰好运动到(62, 1.7)处，换道过程不存在本车与动态障碍车碰撞的风险。

基于两种算法的换道轨迹仿真结果如图6所示，PER-APF算法规划出的换道轨迹比APF算法规划出的换道轨迹波动范围小，轨迹平滑且轨迹长度短，驾驶稳定性高，在一定程度上减少了车辆的能耗。

选取了换道过程中车辆偏转角度的变化曲线如图7所示，基于PER-APF算法的车辆在换道过程中角度变化较小，剧烈转动方向盘次数少，提升了驾乘人员的舒适性体验。基于图6中PER-APF算法规划出的换道轨迹，选取了本车与相邻车道行驶速度较慢车辆在不同时刻的相对位置变化图，如图8所示。可以看出，PER-APF换道轨迹规划算法可以安全的引导无人驾驶汽车超越相邻车道行驶速度较慢的车辆，并在超越后引导本车安全的驶向目标位置。

图6 双车道场景下车辆换道轨迹规划结果

图7 车辆的偏转角

图8 本车与相邻车道行驶较慢车辆在不同时刻的相对位置

3.2 三车道场景换道仿真分析

设置道路存在部分不可通行区域的三车道场景，换道终点在第三车道。本车在当前车道行驶时遇到静止障碍物阻碍本车继续运动，于是换道至中间车道。由动态障碍车辆运行轨迹可以看出，当本车完全换道至中间车道时，在第二车道行驶的动态障碍车恰好运动到(43, 0)的位置；之后，本车由第二车道向第三车道的目标位置行驶。无人驾驶汽车在换道过程中能够以较大的安全距离避开第三车道的禁止通行区域行驶到目标点，不存在与障碍物以及禁行区域碰撞的风险。

基于APF算法与PER-APF算法的本车运行轨迹仿真结果如图9所示，可以看出，应用PER-APF算法规划出的换道轨迹波动范围小，并能与静态、动态障碍物以及禁止通行区域保持足够大的横、纵向距离，换道轨迹曲率小且轨迹长度更短，减少了车辆的能耗。本车在换道过程中车辆转角的变化曲线如图10所示，基于PER-APF算法，车辆在换道过程中转角次数少且角度变化幅度小，提升了驾乘人员的舒适性。基于图9中PER-APF算法规划出的换道轨迹，选取本车与中间车道行驶速度较慢的动态车辆在不同时刻相对位置的变化如图11所示。可以看出，基于PER-APF算法规划出的换道轨迹可以安全的引导无人驾驶汽车超越中间车道行驶速度较慢的车辆，并以较大的横、纵向距离避开第三车道的禁止通行区域，引导汽车安全的驶向目标位置。

图9 三车道场景下车辆换道轨迹规划结果

图10 车辆的偏转角

图11 本车与中间车道车辆在不同时刻的相对位置

基于上文两种不同仿真场景得到的仿真结果﹐将算法运行时间、汽车的转角次数以及所规划的换道轨迹长度作为衡量传统APF算法与PER-APF算法优劣性的指标，不同指标的数据对比结果如表2所示。无论基于哪种仿真场景，PER-APF算法较传统APF算法的运行时间更短﹑换道轨迹更加平滑且轨迹长度更短,并且可以以较少的转弯次数完成换道操作。综上，PER-APF算法的轨迹规划性能明显优于传统APF算法，验证了所提PER-APF算法的可行性与有效性。

表2 不同指标的数据对比结果

4 结束语

针对传统APF算法在解决无人驾驶汽车换道轨迹规划问题中所出现的目标不可达和局部最优问题，提出了一种基于PER-APF算法的新型换道轨迹规划方法。通过建立了具有斥力区分的道路边界约束条件和多约束换道轨迹规划模型来保证换道规划的合理性和安全性。另外，通过构建虚拟区域以及势能重构策略，提出了PER-APF算法，使换道轨迹规划过程能够有效避免目标不可达以及局部最优问题。实验数据充分验证了所设计的PER-APF算法在解决无人驾驶汽车换道轨迹规划问题中的可行性与有效性。

在接下来的工作中，将所提出的PER-APF算法应用在无人驾驶汽车实验平台上进行调试，对所提PER-APF算法做进一步的验证。