基于改进人工势场法的智能车辆避撞路径规划

孔慧芳, 夏 露, 张 倩

(合肥工业大学 电气与自动化工程学院,安徽 合肥 230009)

0 引 言

智能驾驶是汽车未来的发展趋势,它的核心主要包含环境感知、决策规划和控制执行3个部分[1]。路径规划是决策规划部分的一个重要模块,其结果的优劣影响着车辆行驶的舒适性和安全性。一直以来,避撞路径规划都是智能驾驶研究的热点问题之一。

目前,比较常见的路径规划算法有人工势场法[2]、A*算法[3]、遗传算法[4]和快速随机扩展树(rapidly-exploring random trees,RRT)[5]等,其中人工势场法以计算量小、实时性好等优点常被用于汽车的局部路径规划,但是该算法没有考虑到车辆动力学约束和道路边界的约束,且容易陷入局部最优值。针对这些问题,各国学者进行了大量的优化研究。文献[6]在传统的斥力场中加入调节因子,解决了障碍物与目标点过近的局部最优值问题,但是没有考虑到道路的约束;文献[7]针对局部最优值问题临时增设子目标点,通过变换目标点位置改变势场空间,但子目标点的选取方法不适用于道路上的车辆;文献[8]利用椭圆化距离代替传统斥力势场中的实际距离,同时引入道路边界斥力场模型,获得汽车避撞局部路径,但没有考虑陷入局部极小值的问题;文献[9]建立人工势场模型,采用梯度下降算法得到路径,但该方法规划的路径可能不符合车辆动力学约束。

模型预测控制最明显的特点是能在控制的过程中增加车辆动力学的多种约束,许多学者结合人工势场法和模型预测控制(model predictive control,MPC)进行避撞路径规划研究。文献[10]设计模型预测控制器,利用粒子群算法优化控制器参数,但是只考虑了障碍物而没有考虑道路边界的影响;文献[11]搭建智能汽车主动避障系统,基于改进人工势场模型规划路径,利用模型预测控制算法进行跟踪,但不能根据车辆状态实时调整规划的路径;文献[12]提出一种分层避撞控制方法,基于模型预测控制理论构建优化目标函数,采用五次多项式拟合局部避撞路径。

本文基于车辆动力学模型,考虑道路边界的约束,为了避免局部最优值,添加虚拟目标点,建立道路环境模型;将模型与模型预测控制相结合,规划层根据道路环境模型和参考路径信息重新规划出避开障碍物的局部路径;再将路径信息输入到跟踪控制层,实现对局部参考路径的跟踪,并将车辆状态实时反馈给规划层;最后通过MATLAB/Simulink与CarSim联合仿真验证路径规划及跟踪效果。

避撞路径规划结构如图1所示。

图1 避撞路径规划结构

1 改进的人工势场模型

1.1 方向引力势场模型

在实际情况中,车辆正常行驶处于车道中心,完成超车或避障动作后一般会回到原车道,因此本文以被控车辆自身为避撞的起点,在前方纵向距离yd设立终点,势场在转向区域沿车道中心线逐渐下降,所构建的方向引力势场函数表达式如下:

(1)

其中:katt为引力势能增益系数;xs为车辆起点横向坐标位置;yd为转向区域最大纵向距离。

根据(1)式得到引力势能场的三维分布,如图2所示。

图2 引力势场三维分布

1.2 障碍物斥力势场模型

斥力势场的建立保证被控车辆远离障碍物,在障碍物斥力势场作用域D之内,距离越小,被控车辆所受的斥力越大,离开作用域时,所受斥力为0,一般在横向和纵向的作用距离并不相同,将车辆的作用域膨胀为椭圆,椭圆方程为:

(2)

其中:(x0,y0)为障碍物所在的中心位置;L1、L2分别为椭圆的长半轴和短半轴。L1、L2的数学表达式为:

(3)

L2=vrt+Smin

(4)

其中:G为安全因子;L为车道的宽度;vr为被控车辆相对于障碍物的相对速度;t为制动前的反应时间;Smin为设定的最小安全距离。

根据被控车辆到障碍物的距离与势场大小成反比,远距离时随着距离减小上升平缓、近距离上升迅速的特点,采用类高斯函数模拟障碍物势能[13],建立障碍物斥力势场函数为:

Urep=

(5)

其中:Krep为斥力势能增益系数;d为被控车辆到障碍物的距离。

根据(5)式可以得到斥力势能场的三维分布,如图3所示。

图3 斥力势场三维分布

1.3 道路势场模型

车辆在道路上行驶不仅需要考虑障碍物的影响,还需要考虑道路边界和车道线的影响。道路边界用于防止车辆驶出道路,给车辆一种斥力的作用,因此在道路两边势场取最大值;车道线限制车辆不必要的换道,因此在车道线位置势场取极大值,并向两边递减,变化较为平缓。

根据道路特点构建道路势场函数如下:

Uroad=

(6)

其中:Kroad为道路边界势能增益系数;Kline为车道线势能增益系数;xborder为离车辆最近的道路边界线的位置;xmid为道路中心线的位置;δ为分道线势场收敛系数,由车道宽度决定。

根据(6)式可以得到道路上不同位置势能的三维分布,如图4所示。

图4 道路势场三维分布

1.4 虚拟目标点

为了解决人工势场法中易出现的局部最优值问题,本文拟设立虚拟目标点来摆脱陷入局部最优的困境。虚拟目标点选取在纵向上与障碍物保持同一水平线,横向上距离障碍物为M的位置,其计算方法为:

M=λL2

(7)

其中,λ为距离扩大系数。

根据障碍物的危险程度,通过改变λ的值可以动态调整安全距离。

根据虚拟目标点的位置信息构建势场函数:

(8)

其中,kvir为虚拟目标点势能增益系数。

当车辆根据传感器获取周围环境信息,计算并判断车辆在势场中是否陷入局部最优值。若是,则开始受到虚拟目标点势场的作用,当车辆运行到虚拟目标点处,虚拟目标点消失,继续原目标点的作用,直至到达目标点。虚拟目标点选取方式如图5所示。

图5 虚拟目标点选取方式

车辆在道路上主要受到方向引力势场、障碍物斥力势场和道路势场的作用,当车辆陷入局部最优困境时,受到虚拟目标点势场的作用,几种势场共同构成道路环境模型,即

UAPF=Uatt+Urep+Uroad+Uvir

(9)

通过改进后的人工势场模型,获取车辆行驶的周围环境信息,结合MPC算法重新规划局部避撞路径。

2 基于MPC的局部路径规划

2.1 车辆点质量模型

车辆在道路上行驶时,遇到障碍物需要迅速根据传感器获取的障碍物信息进行局部路径规划,实时性要求较高。为了减少计算量,规划层采用点质量模型。

车辆模型可以表示为:

(10)

2.2 路径规划器设计

将(10)式转化为状态空间形式,即

(11)

(12)

其中:Jf(ξ)为f相对于ξ的雅克比矩阵;Jf(u)为f相对于u的雅克比矩阵。将以上两式相减可以得到:

(13)

将新的状态方程离散化处理可得:

(14)

为了使规划的路径能够安全平稳地避开障碍物,将道路环境模型作为一部分引入到目标函数中,新的目标函数如下所示:

(15)

其中:ηref为全局参考路径信息;Δu(k+i|k)为k时刻第i步的预测控制增量;UAPF为道路环境势能值;Np、Nc分别为预测时域和控制时域;Q、R为各部分的权重。

考虑到车辆动力学的约束,加入的约束条件如下:

|ay|<μg

(16)

其中,μ为道路附着系数。

因为路径规划器的预测方程给出的都是离散的点,跟踪控制层难以完成跟踪,所以需要对离散点进行拟合。本文采用五次多项式拟合曲线,即

φrep=a1x5+a2x4+a3x3+a4x2+a5x+a6,

Yrep=b1x5+b2x4+b3x3+b4x2+b5x+b6

(17)

其中:φrep为拟合后的横摆角;Yrep为拟合后的路径;ai,bi(i=1,2,…,6)为多项式拟合系数。

3 车辆避撞路径跟踪控制

3.1 车辆动力学模型

本文主要考虑车辆的平面运行,只有纵向、横向和横摆3个方向的运动,因此跟踪控制层选择三自由度的车辆单轨模型。根据牛顿第二定律,分别得到沿x轴、y轴和绕z轴的受力平衡方程为:

(18)

在轮胎力的计算过程中,为了简化计算,基于小角度假设建立车辆动力学模型,即满足cosθ=1和sinθ=θ,θ表示各个角,包括前轮转角和侧偏角等。

通过简化,可以得到前、后轮胎侧偏角的计算式分别为:

(19)

(20)

前、后轮的轮胎力可以表示为:

(21)

其中:Fcf、Fcr分别为前、后轮受到的侧向力;Flf、Flr分别为前、后轮受到的纵向力;Ccf、Ccr分别为前、后轮侧偏刚度;Clf、Clr分别为前、后轮纵向刚度;sf、sr分别为前、后轮滑移率。

结合(10)式、(18)～(21)式可以得到车辆的动力学模型如下:

(22)

3.2 跟踪控制器设计

在对重规划的路径进行跟踪过程中,应该使实际路径与参考路径的偏差和控制输入增益尽可能小,设计控制器的目标函数为:

(23)

其中:ηrep为路径规划器输出的局部路径;ρ为权重系数;ε为松弛因子,防止出现目标函数无法得到可行解的情况。

保证车辆跟踪过程中的平稳性和安全性,需要满足的控制量、控制增量和横向加速度的约束条件如下:

(24)

根据博世公司的车辆稳定性研究结果,在正常路面下保证汽车的稳定性和舒适性,质心侧偏角β的约束为[-12°,12°],结合目标函数和约束条件求解得到每个控制周期内最优控制增量序列为:

ΔUt=[ΔutΔut+1… Δut+Nc-1]

(25)

将序列的第1个元素作为实际控制增量作用于系统,得到下一时刻的控制量,重复求解计算,完成对路径的跟踪控制。

4 联合仿真验证

为了验证路径规划器的有效性,采用MATLAB/Simulink与CarSim 进行联合仿真实验。在CarSim中建立车辆模型,模型具体参数见表1所列。

表1 车辆模型参数

环境模型选择城市道路下宽度7 m的单向双车道,通过设置静态障碍物和动态障碍物来验证车辆避撞能力,在车辆前方50 m处设置一辆障碍物车辆。

4.1 静态障碍物环境

为了验证本文方法能够更好地符合车辆动力学性能,将其与人工势场法[9]对比仿真,设置被控车辆车速为54 km/h,仿真结果如图6所示。

图6 静态障碍物下车辆避撞过程仿真结果

由图6a可以看出,相较于文献[9]方法,本文规划器规划的路径更为平滑,车辆行驶过程中稳定性更高,且提前6 m开始避障行为,安全性更高;由图6b～图6e可以看出,本文方法规划的路径下,车辆的前轮转角、横摆角、横摆加速度和横摆角速度变化均小于文献[9]方法,且曲线变化平稳没有明显突变,车辆能够很好地进行避障跟踪。

为了验证车辆在不同速度下的避撞效果,分别在72、54、36 km/h的车速下进行仿真验证,仿真结果如图7所示。从图7a中可以看出,在不同车速下,车辆均可以规划出安全的局部避撞路径,速度为72、54、36 km/h时分别从10、13、17 m处开始避撞行为。从图7b～图7d可以看出:车辆横摆角变化在±5°内,前轮转角变化在±2°内,变化范围较小,满足基于小角度假设的模型建立;质心侧偏角变化在±0.5°内,满足[-12°,12°]的约束条件;横向加速度变化在±0.2 m/s2内,满足μg的约束条件,符合车辆运动约束,车辆在整个避撞过程中处于平稳状态。

4.2 动态障碍物环境

为了验证路径规划器对动态障碍物的避障效果,在右车道前50 m处放置一辆动态障碍物车辆,以36 km/h速度前进,仿真结果如图8所示。

图8a所示为车辆以72 km/h的速度在3、6、9 s时刻障碍物车辆和被控车辆的位置关系。

从图8a可以看出,在不同时刻下被控车辆与障碍物车辆都不会发生碰撞。从图8b～图8d可以看出动态障碍物环境下车辆的状态变化:前轮转角变化在±1°以内,横摆角变化在±4°以内,横向加速度变化在±0.2 m/s2内,符合车辆运动约束条件;曲线变化平稳没有明显突变,符合实际驾驶习惯;质心侧偏角变化在±0.4°以内,变化较小,说明车辆在避撞过程中行驶具有良好的稳定性。

5 结 论

本文设计了一种基于道路环境模型和MPC的避撞路径规划控制器。对人工势场法进行改进,加入道路边界的约束,添加虚拟目标点摆脱局部最优困境;将道路环境模型引入路径规划层的目标函数中,规划出局部最优路径;再将路径信息输入到跟踪控制层进行实时跟踪。仿真结果表明,对于动、静态障碍物,车辆在不同的速度下本文避撞路径规划均可以实时规划出安全平稳的局部避撞路径。