赵奉奎,葛 振,董锋威,张 涌
(南京林业大学 汽车与交通工程学院,江苏 南京 210037)
避障路径规划技术是自动驾驶汽车关键技术之一,在环境感知层获得行驶环境的信息后,轨迹规划模块可按照不同的行为决策,为自动驾驶汽车规划出一条从起点到目标位置的最优路径。根据行驶场景的不同,路径规划技术主要分为全局路径规划和局部路径规划。全局路径规划主要是根据预先存储的地图进行路径规划,如在现有电子地图中设定一条连接两城市的最优路径。汽车在道路上行驶时,行驶环境复杂,需要利用视觉或雷达实时感知周围环境[1],进行局部路径规划。路径规划算法不仅能及时准确的规划出避障路径,而且能规划出易于汽车跟踪的路径,以及具有较高的乘坐舒适性[2]。
国内外学者对局部路径规划开展了广泛的研究。王富奎[3]将避障路径规划问题转换多约束优化问题,设定了参考速度、加速度、车辆动力学约束、车道约束等若干约束目标,据此建立优化模型高维图,采用图优化法进行求解;雷艳敏等[4]将最短路径、安全性能和平滑度综合考虑,得到了一种用于路径规划的全新的遗传算法;田欣[5]基于遗传算法提出了一种新的自适应调整方式,提高了算法的效率,引入了模拟退火算法来克服局部最优解的缺陷。此外,仍有基于传统路径规划算法展开的研究,如基于采样的RRT算法[6]和基于图搜索的A*算法[7],以及针对结构化道路提出的路径规划算法[8]。但当前多数算法较为复杂,求解难度较大,算法效率低,不利于实时路径规划。
人工势场法是一种常用的路径规划的算法,具有算法结构简单,实时性好,规划出的路径较为平滑等优点。当前对于人工势场法的改进主要集中在解决其固有的两个缺陷——局部最优解和目标不可达[9-12],也有将其与其他算法相结合,段建民等[13]提出一种改进人工势场法与遗传算法结合的方法来寻求全局最优解,并引入填平势场跳出局部极小点。同时也出现许多针对自动驾驶汽车的改进方案,谭宝成等[14]优化了经典人工势场法中圆形虚拟势场的作用域,能够实现结构化道路中较小角度的避障;王树凤等[15]将障碍物模型改进为椭圆模型,引入了基于前车工况的斥力系数以及局部虚拟目标点;曹昊天等[16]在人工势场法的基础上,增加了引导车辆避障的引导势场,使避障过程更为安全。当前基于人工势场法的路径规划算法存在很多问题,如算法未考虑车辆在不同速度下的势场影响范围,未考虑车辆运动学约束和车辆实际行驶的环境,以及未考虑对动态障碍物的处理等[17]。
因此提出了一种改进型的人工势场法,在进行路径规划时加入车辆运动学约束,使规划的路径满足当前速度下车辆前轮最大转角约束。研究了超车场景路径规划,设计了基于安全车距的不规则障碍物模型,在设定的道路约束下,实现该算法对动态障碍物的避障路径规划处理。
目标点产生的引力势场记为Uatt,假设车辆为一质点,引力势能的大小与目标点和车辆所在位置间的距离有关,距离越远,车辆受引力势能的影响就越强,距离越近,车辆受引力势能的影响就越小,目标点处的引力势能为0。将车辆的运动空间抽象成二维平面,在整个运动空间内的引力势能可表示为:
(1)
式中:Katt为引力势能比例增益因子;X为车辆当前位置的坐标;Xg为目标点位置坐标;m为引力势场因子,一般取值为2;ρ为车辆当前位置距目标点的距离。
引力势场对车辆产生的引力记为Fatt,那么Fatt为引力势能的负梯度,即沿引力势能下降最快的方向,其可表示为:
Fatt(X)=-Katt·ρ(X,Xg)
(2)
与引力势场相似,车辆在行驶过程中还会受到由障碍物产生的斥力势场的作用,其对车辆的影响也与车辆与障碍物的距离有关。当车辆与障碍物越近时,障碍物产生的斥力势场对车辆的影响越大,超过一定距离时,斥力势场便不再对车辆产生影响。与引力势能函数类似,构建函数为:
(3)
式中:Krep为斥力势能比例增益因子;Xo为障碍物位置坐标;ρo为车辆受障碍物影响的最大距离。
与引力的求法同理,斥力势场对车辆产生的斥力记为Frep,那么Frep为斥力势能的负梯度,即沿斥力势能下降最快的方向,可表示为:
(4)
由于运动空间中每个位置处的引力势能大小和斥力势能大小为标量,根据叠加定理,运动空间中的总势能大小如式(5):
U(X)=Uatt(X)+Urep(X)
(5)
从而,车辆所受到的合力F(X)为:
F(X)=Fatt(X)+Frep(X)
(6)
由于车辆运动学系统极为复杂,为了便于分析,将四轮车辆简化为两轮的单车模型,需做如下假设:
1)汽车的纵向行驶速度u恒定不变。
2)忽略转向系统的影响,以前轮转角作为输入,后轮转角始终为0。
3)忽略悬架系统的影响,认为车辆仅做平行于地面的平面运动,即忽略汽车的垂向运动、俯仰运动以及侧倾运动。
4)不考虑载荷的左右转移,即前轴和后轴的左右轮分别等效为一个轮。
5)忽略横纵向空气动力学。
6)侧向加速度小于0.4g,以保证侧偏刚度为常数。
于是汽车被简化成仅有侧向和横摆两自由度的单车模型,如图1。根据余志生[18]的《汽车理论》,当汽车运动时,汽车质心加速度ay沿Oy轴方向上的分量和线型二自由度模型的运动微分方程分别为:
(7)
(8)
式中:k1,k2分别为前轮侧偏刚度、后轮侧偏刚度;β为质心侧偏角;v1为车辆质心速度;u,v分别为车辆质心速度在车辆坐标系中沿x轴和y轴的分量;a,b分别为质心至前轴的距离和质心至后轴的距离;δ为前轮转角;ωr为车辆横摆角速度;a1,a2分别为前轮侧偏角和后轮侧偏角;m为车辆的质量;IZ为车辆的转动惯量。
(9)
式(9)便是车辆前轮转角和速度的运动学约束,现假设K=0,车辆为中性转向,轴距L为3 m,代入式(9),以前轮最大转角为因变量,速度为自变量,作出关系图,如图2。由图2可知,前轮最大转角与车速呈反比关系。因此,车辆在某一速度下,只需前轮转角的值位于图2的曲线下方,即可满足车辆的运动学约束。
图1 二自由度车辆运动学模型Fig. 1 Two-degree-of-freedom vehicle kinematics model
图2 前轮角度随速度的变化曲线Fig. 2 Variation curve of front wheel angle changing with speed
道路约束是指由右车道中心线产生的引力势场,如图3。在该势场内的车辆会受到指向右车道中心线的引力,以保持沿右车道行驶,同时防止越过道路边界。因此,距右车道中心线越远,引力势能的值越大。
为满足上述要求,采用基于二次函数的势场分布模型。如图3(a),当车辆位于右车道中心线的左侧时,记车辆质心所在位置为点A,dLM为点A距右车道中心线的距离,ULroad(dLM) 为右车道中心线在其左侧dLM处产生的引力势能,简称左引力势能,取:
(10)
式中:KLroad为左侧道路约束系数,用于左引力势能的缩放。同理,如图3(b),右引力势能为:
(11)
式中:KRroad为右侧道路约束系数,用于右引力势能缩放;dRM为点A距右车道中心线的距离。
图3 道路约束示意Fig. 3 Schematic diagram of road restraint
为保证道路的左右边界具有相同的约束作用,道路左右边界处的引力势能的大小应相等,即:
(12)
化简可得:
9×KLroad=KRroad
(13)
于是可得到道路约束引力势场的总分布图,如图4。
图4 道路约束引力势场分布Fig. 4 Distribution of road constrained gravitational potential field
在经典人工势场法中,自车遇到障碍物进行避障时,人工势场法会规划出现近乎直角的路径,如图5,显然车辆很难跟踪该路径,因此需要在路径规划时引入车辆模型约束。车辆按照某一速度行驶时,可根据图2得到此时的最大前轮转角,算法所规划的路径点如图6,任意相邻的3个路径点角度差Δθi不能大于当前速度最大前轮转角[19],如式(14):
Δθ≤δmax
(14)
图5 经典人工势场法路径规划结果Fig. 5 Path planning results of the classical artificial potential field method
图6 相邻路径点示意Fig. 6 Schematic diagram of adjacent path points
在车辆实际行驶过程中,遇到的障碍物多为静止或运动的汽车,其外形可近似为矩形。在人工势场法中,当车辆前方存在障碍车辆需要进行超车路径规划时,由于障碍车辆后方的势场影响范围在沿车辆横向方向(如图7,Y方向)各点大小相等,不利于引导自车进行变道,据此,有学者提出了Yukawa车辆模型。Yukawa模型将车身后部按照距离衰减的斥力势场用一个楔形势场来表示,其分布规律按照某一特定函数来约束,如图8。
图7 车辆-障碍物坐标Fig. 7 Vehicle-obstacle coordinates
图8 Yukawa车辆障碍物模型Fig. 8 Yukawa vehicle obstacle model
为了简化Yukawa模型,采用椭圆形的改进斥力势场模型,从而便于引导后方车辆进行超车或避障时的变道。势场分布图如图9。该模型由车辆矩形、斥力势能峰值区和斥力势场影响范围3个同心图形组成。车辆矩形描述了车辆的二维空间尺寸。
经典人工势场法中的单个障碍物是由若干“作用点”及其势能作用范围组成的,由式(3)可知,“作用点”中心处的斥力势能为峰值(趋近于正无穷)。将前述的“作用点”转化为单个车辆的及其对应的安全范围,即斥力势能峰值不再是存在于“作用点”的中心处,而是“作用点”周围的一整个区域内,即斥力势能峰值区。由图9可知,该斥力势能峰值区由前部半椭圆区、中部矩形区和后部半椭圆区共3部分组成。后半椭圆区是为了引导车辆超车时第1次变道而设定的,相比矩形和圆形的势场范围,其曲率较小且沿道路方向的变化较为缓慢,极易引导车辆进行变道。此外,车辆与前方障碍车辆具有最小的跟车距离,即安全车距。因此,取后半椭圆长轴长AR为:
AR=KERDmin
(15)
式中:KER为后半椭圆的长轴缩放系数,取值为1;Dmin为安全车距,根据游峰等[20]提出的基于纵向安全距离的超车预警模型(图10),可确定不同相对速度下的安全距离Dmin。
前部半椭圆区用于在一定程度上引导后方车辆超车后进行并道,同时为了保持一定安全车距。由此确定该半椭圆的长轴长AF为:
AF=KEFDmin
(16)
式中:KEF为前半椭圆的长轴缩放系数。因实际驾驶时超车车辆的速度较快,故超车后可再为抵达安全车距的位置时提前返回右向车道,因此可以取KEF为0.5。
图9 改进障碍物斥力作用域模型Fig. 9 Improved obstacle repulsion scope model
图10 恒速条件下自车与障碍车辆的碰撞区域Fig. 10 The collision area between the own vehicle and the obstacle vehicle under constant speed conditions
由于在结构化道路上,同一车道仅允许1辆车行驶,因此可以取中部矩形区的宽度为车道宽度,长度为车长,两个半椭圆的短轴长与中部矩形区的宽度相等,即半椭圆的短轴长为车道宽度。
斥力势能峰值区外部即为障碍物产生的斥力势场的作用范围,大于此范围,斥力势能为0。考虑驾驶环境为双向车道和实际超车环境,确定斥力势场作用范围边界与斥力势能峰值区边界之间的距离为2 m。
车辆在实际避障和超车时应考虑诸多因素,如障碍物(车流)密度、与障碍物的安全距离以及对向是否有车辆等。为了简化模型,笔者设定驾驶环境为双向车道,且仅存在自车和1辆行驶中的障碍车辆,在该环境下模拟超车路径规划。车辆和道路尺寸参数来自表1。设定自车车速为匀速25 m/s,障碍车辆车速为匀速15 m/s,相对速度大小为10 m/s,根据图2可得到该车速下前轮最大转角约为π/200 rad(约为0.9°)。根据图10,确定纵向安全车距Dmin为23 m。设定目标点为车辆前方Dmin处,而车辆在实际运行过程中,自车和障碍车辆的位置是不断变化的,因此,在设定好目标点后,自车每到达一个位置都要更新目标点的位置,以保持车辆在避障后仍然与周围车辆保持安全车距,然后重新计算到达新的目标点的路径。
表1 仿真参数Table 1 Simulation parameter table
在MATLAB中脚本文件中编写仿真程序,运行结果如图11。从图11中可以看出,改进的人工势场法能较好的完成针对运动障碍物的避障路径规划,其所规划的路径更加接近车辆实际行驶的路径,规划出的路径较为光滑,说明改进的障碍物模型能较好的引导车辆做出变道行为。
图11 仿真结果的6个时间节点Fig. 11 Six time nodes of the simulation results
通过对比基于Yukawa模型和改进模型的仿真规划路径来验证所提出的模型的优越性,对比效果如图12。由于改进模型的后部采用椭圆,使得规划出的路径更接近于左侧车道中心位置,超车车辆与被超车车辆之间的横向距离相对较大,增加了超车的安全性;改进模型的前部也采用椭圆,从图12中可以看出在超车即将完成,改进模型使得超车车辆回到右侧车道的时间比较晚,两车之间的纵向距离明显较大,能更有效避免后车突然加速导致的交通事故,并且规划出的路径更为平滑。
图12 不同模型规划路径对比Fig. 12 Comparison of planning paths of different models
为了验证引进二自由度运动学约束的有效性,在保证其它条件不变的情形下,对没有引进二自由度运动学约束进行仿真,规划出的路径如图13,前轮转角如图14(a),尽管能实现超车的任务,但是可以看出在超车任务后半程,车头会剧烈晃动,经过计算最大晃动角度可达到1.14 rad(约为65.39°),严重影响了车辆行驶的稳定性和乘坐舒适性。图14(b)为引入二自由度运动学约束时各路径点处的车辆前轮转角分布图,车辆前轮转角被控制在π/200内,转角变化较小,最大晃动角度只有0.016 rad(约为0.9°),车辆行驶较为稳定。经过对比和分析,发现引进二自由度运动学约束极大得增加了车辆行驶的稳定性,验证了引进二自由度运动学约束的有效性。
图13 无运动学约束规划路径Fig. 13 Planning path without kinematic constraints
图14 前轮转角分布Fig. 14 Front wheel angle distribution
为了验证引进道路约束得有效性,笔者在保证其它条件不变的情形下,删除道路约束对仿真的影响,规划出的路径如图15。对比图12和图15可以看出当超车车辆在左侧车道时,在无道路约束的条件下车辆与道路左侧边界的距离非常近,容易造成车辆脱离道路的风险。通过对比和分析,说明引进道路约束是有效的。
图15 无道路约束规划路径Fig. 15 Planning path without road constraints
对经典人工势场法进行了改进,引入道路约束和运动学约束,使规划的路径更加符合车辆实际行驶的特性;依据超车安全距离模型改进了障碍物模型,调整了障碍物模型的结构以及作用范围,使其便于引导后方车辆进行变道;建立了针对移动障碍物中特殊情况——超车避障的路径规划仿真模型。仿真结果表明:采用改进的人工势场法具有良好的避障路径规划效果,能引导车辆做出满足车辆运动学约束的变道行为,且规划出的避障路径更为光滑。