融合风险势场的离散优化局部路径规划方法研究

魏 凯,刘树伟,李 刚

(辽宁工业大学 汽车与交通工程学院,辽宁 锦州 121001)

0 引言

随着电动汽车逐渐向智能化领域发展,汽车的智能化已成为必然的发展趋势[1]。当车辆在行驶过程中检测到障碍物时,局部路径规划能够实时规划出一条安全可行的路径,引导车辆完成对障碍物的规避,保证车辆的行驶安全,是智能汽车的关键技术之一。局部路径规划算法可分为四类:基于采样的方法、基于搜索的方法、势场法以及离散优化的方法。基于采样的方法是在状态空间内随机采样节点,通过增加不同的运动约束最终生成规划路径[2]。该方法采样随机,收敛速度较慢,代表算法为快速随机扩展树法(rapidly random tree,RRT)与概率图法(probabilistic road map,PRM)。基于搜索的方法是在栅格地图内遍历节点间所有路径,最终搜索出一条无碰撞的最短路径[3]。该方法无法适应复杂地图且计算量较大,实时性较差,代表算法为A∗算法与Dijkstra 算法。以上两种方法因其算法的局限性,并未考虑车辆的动力学特性且无法满足实时性要求,更多应用于移动机器人规划领域。势场法基于势场理论,引入引力场与斥力场的概念,根据综合的场强分布完成规划[4]。该方法能够更加准确地反映规划场景内障碍物的风险影响,弥补传统算法中对于路径安全性的分析。离散优化的方法通过自主设计代价函数评价候选路径,能够综合考虑车辆动力学以及路径的安全性、平滑性等指标,更加适用于智能汽车领域[5]。

针对势场法以及离散优化方法的应用,国内外研究人员已经做了大量的研究工作。文献[6]应用人工势场法对自动驾驶汽车进行自适应运动规划,利用虚拟势场对车辆进行风险感知,适应不同障碍物的位置及速度变化,并通过硬件在环测试验证了方法的可行性。文献[7]提出建立基于道路纵向、横向和时间3 个维度的预测风险场,并使用多项式曲线和二次规划方法对轨迹进行优化。文献[8]通过建立成本函数对候选路径进行评价,并对评价结果进行加权处理,最终能够在局部障碍图中搜索出最优路径,避免车辆与障碍物发生碰撞。文献[9]提出一种综合路径采样和离散优化的混合轨迹规划方法,通过对侧向运动的采样生成多条轨迹,能够适应不同速度的复杂动态环境,并通过实车测试验证了方法的可行性。文献[10]提出建立s-ρ坐标系生成候选路径,能够完成对复杂障碍物的规避,但是并没有考虑动态障碍物的影响。文献[11]基于碰撞风险评估对车辆进行局部路径规划,对不同障碍物建立风险场,通过对候选路径进行风险评估,增强规划路径的安全性,但是并未考虑到道路本身对车辆的风险影响。

针对智能汽车在结构化道路内的紧急避障场景,本文提出一种融合风险势场的离散优化局部路径规划方法。基于势场理论建立道路和障碍物的风险分布模型,利用风险势场的场强分布描述规划场景中的风险源;针对静态和动态障碍物,结合风险势场的场强与离散高斯方法设计路径安全代价函数,评价候选路径的安全性;综合考虑路径的安全性、偏移量和连续性,建立统一多目标代价函数对候选路径进行评价。仿真结果表明,本文提出的局部路径规划方法能够实时为车辆规划出一条安全、可行的路径,引导车辆完成对障碍物的规避,且针对多种复杂场景都具备良好的稳定性。

1 候选路径

1.1 路径参考线

将车辆路径分割为多个路径片段,每个路径片段为一个局部路径规划周期。在一个路径片段内有多个离散路径点,将这些离散点拟合成路径线,并以此参考线为基准进行局部路径规划算法的设计。三次样条曲线计算简单、实时性好,曲线稳定且收敛性有保证。因此,本文使用三次样条曲线方程拟合路径离散点,将路径线作为局部路径规划的参考中心线,如图1 所示。弧长参数化后的三次样条曲线方程为

图1 路径参考线Fig.1 Path guides

式中,a3x、a2x、a1x、a0x、a3y、a2y、a1y和a0y为曲线系数,si表示第i个路径点的弧长值。根据弧长s,即可直接计算路径参考线任意一点的坐标值(x0,y0),此时车辆的航向角和曲率为

式中,x0′、x0′′和y0′、y0′′分别为x0(s) 和y0(s) 对弧长s的一阶导数和二阶导数。

1.2 车辆定位

当车辆在弯道行驶时,路径参考线为曲线,且车辆在行驶过程中轨迹跟踪存在误差,所以车辆的位置与路径参考线之间一定存在一个横向偏移量d0,在对车辆进行定位时这是不可忽略的。如果使用大地笛卡尔坐标系来计算车辆与路径参考线之间的横向距离,公式复杂且增加数据运算量。因此,本文引入局部Frenet 坐标系,用以描述当前路径片段内车辆对于路径参考线的相对位置关系,如图2 所示。所建立的局部Frenet 坐标系以路径参考线为基准,沿参考线的切线方向建立s轴,正方向为车辆当前的前进方向,数值表示路径的长度;纵轴为参考线切线的法线方向,垂直于横轴,即为d轴,数值表示车辆相对于参考线的横向偏移量。通过局部Frenet 坐标系,可以将车辆每个时刻的位置信息分解为s轴方向和d轴方向的状态,在进行局部路径规划时,减少处理车辆定位信息的工作量。在局部Frenet 坐标系内,使用二次最小化与牛顿法相结合的方法[12],可以准确计算得到车辆当前位置距离路径基准线的横向偏移量d0,根据坐标值(s0,d0) 即可清晰地表示出车辆的定位信息。

图2 车辆定位Fig.2 Location of vehicle

1.3 边界条件

在当前规划周期内,利用局部Frenet 坐标系并基于路径采样的方法,可以生成一组有限的候选路径簇。所有候选路径的起点都相同,即为规划周期开始时刻车辆的当前位置;路径的终点位置各不相同,但都与路径参考线的相切角相同。由于路径参考线是由三次样条曲线方程表示,为了保证路径的连续性,避免路径出现曲率突变,本文依然使用三次样条曲线方程来表示候选路径,则候选路径簇可表示为

式中,s∈[sstart,send],sstart为候选路径起点处所对应的弧长,send为候选路径终点处所对应的弧长,a3、a2、a1为曲线系数。

建立候选路径的边界条件,其中路径起点为车辆当前位置,终点处的send与起点处的sstart的差值Δs即为候选路径投影到路径参考线上的弧长;路径终点处的横向偏移量d取值范围根据当前道路宽度设定,且保证每条路径之间间距相等。所有路径起点处的切线方向与当前车辆前进方向保持一致,终点处路径的切线方向与参考线的切线方向保持一致。根据以上条件构建路径边界条件为

结合式(4)与式(5),将路径终点的横向采样范围等分,等间距地改变终点位置的横向偏移量d,即可得到当前规划周期内所有候选路径的曲线方程。

对计算得到的候选路径进行编号C1,C2,…,Cn,方便后续代价函数对候选路径的评估与统计,如图3 所示。此时规划出来的候选路径是基于局部Frenet 坐标系而产生的,在局部路径规划结束后,需将路径信息从局部Frenet 坐标系再转化至大地笛卡儿坐标系,传输至下层控制层进行轨迹跟踪控制。根据坐标系之间的相对几何关系,计算得到最终的规划路径信息。

图3 候选路径Fig.3 The candidate paths

2 风险势场

为了能够准确描述周围环境对智能汽车局部路径规划的影响,本文使用风险势场来描述车辆行驶环境内的风险源,用场强分布表示规划场景内的风险[13]。所构建风险势场由道路、静态障碍物与动态障碍物组成,可表示为

式中,Erisk表示当前规划周期内风险势场的总场强,ER、ES和ED分别表示道路风险势场、静态障碍物风险势场和动态障碍物风险势场。将这三个风险势场叠加,通过场强分布可以直观地判断出各个位置车辆的安全风险大小。

2.1 道路风险势场

在构建道路风险势场时,主要分析车道线、车道中心线以及道路边界这三类关键位置。在车辆正常行驶过程中,道路两侧边界位置的风险最高;车道中心线的风险相对较低,且当前车道中心线处的风险最低;而车道线可以约束车辆行驶于车道内,避免发生车道偏移,其风险应高于车道内部的风险,因此采用阶跃函数构建道路的风险分布模型。以同向双车道为例进行分析,将道路中线作为参考线建立局部Frenet 坐标系,在道路特征不变的前提下,其风险影响仅与横向偏移量d有关,具体模型可表示为

式中:d1和d2分别为左、右两车道中心线处的横向偏移量;p为车道线处的风险值,取值与车道线类型有关,若为双黄线,则车辆不可越线,取值为1,若为白色虚线,则车辆可以越线行驶,取值为0.3。

在构建道路风险势场时,假设车辆越靠近道路两侧边界位置其风险越高,且最大风险值为1;而车辆沿着当前车道的中心线行驶时风险最小,且最小风险值为0。在无障碍物的情况下,车辆应尽可能地沿着当前车道的中心线行驶,避免发生车道偏移;而在有障碍物的情况下,车辆应及时转向,在规避障碍物之后回到原车道继续行驶,同时在避障过程中避免与道路两侧边界相撞。根据假设可知,车辆沿着车道中心线行驶时的风险小于车辆换道的风险,使得在无障碍物的情况下,车辆能够保持在原车道行驶。根据式(7)所建立的风险分布模型,构建道路风险势场并进行归一化处理

式中,D为车道宽度,P(d) 为道路风险分布模型。此风险势场是在局部Frenet 坐标系下构建,因此适用于任何形状曲率的道路,且面对交叉路口等道路交汇处依然适用,可将两条道路分别建模后将势场进行叠加加以表示。

无障碍物情况下的道路风险势场如图4 所示,场景为同向双车道,车道宽度取3.5 m,长度取40 m。由图可以看出,风险势场的场强分布仅与横向偏移量有关。在道路两侧边界位置取得最大值,数值为1;而在车道中心线位置取得最小值,数值为0,且车道线处风险高于车道内区域。

图4 道路风险势场示意图Fig.4 Diagram of road risk potential field

2.2 障碍物风险势场

2.2.1 静态障碍物风险势场

在结构化道路环境内,静态障碍物主要为路面上静止的车辆、临时增加的路障等。面对静态障碍物,车辆越靠近它,越容易与其发生碰撞,相对应的风险也就越高。由于静态障碍物本身不具备速度,场强分布表现为各向同性,即无论从哪个方向靠近它,场强的分布都是均匀的。所分析的风险势场是在局部Frenet 坐标系下进行的,设静态障碍物中心点坐标为O(s0,d0) 且质量分布均匀,则它所形成的风险势场在(s,d)处的场强可表示为

式中,k1为比例系数,r=(s-s0,d-d0) 表示与障碍物中心点的距离矢量。静态障碍物风险势场的场强变化趋势为距离矢量的幂函数形式,同时在计算距离时增加σs与σd两个风险分布因子,表示风险势场在s轴和d轴方向上的分布范围。

设静态障碍物的中心点坐标为(0,0),其质量均匀分布,风险分布因子σs=σd=1,则它形成的风险势场如图5 所示。由图可知,障碍物中心点处场强最大,场强方向由中心点均匀地向四周辐射,且沿场强方向,场强减小速度最快。随着与障碍物中心点的距离增大,场强逐渐减小,表明障碍物所形成的风险在降低,直至距离足够远,减小至0。

图5 静态障碍物风险势场示意图Fig.5 Diagram of static obstacle risk potential field

2.2.2 动态障碍物风险势场

将动态障碍物与静态障碍物相比较,其共同点在于风险影响也具有方向性,而不同点在于,动态障碍物本身具备速度,不同的速度方向及大小所造成的风险是不同的,具有各向异性。针对动态障碍物的特点,在局部Frenet 坐标系下,将障碍物的速度v分解为s轴方向和d轴方向的速度分量,通过选取不同的分布因子σs和σd,可以准确表示不同速度方向所产生的风险影响。动态障碍物O(s0,d0) 所形成的风险势场在(s,d)处的场强可表示为

式中,k2为比例系数,r=(s-s0,d-d0) 表示与障碍物中心点的距离矢量,θ为r与障碍物速度v之间的夹角,exp(vcosθ)描述场强随速度大小、方向的变化趋势。在相同速度大小下,越小,cosθ越大,场强也就越大,且当θ=0 时达到最大,而θ=π 时场强最小,障碍物所形成的风险也最低。在相同夹角下,cosθ> 0 时,速度越大风险也就越大,而cosθ< 0 时则相反。

假设动态障碍物质点坐标为(0,0),具有沿s轴正方向的速度v,则它所形成的风险势场如图6所示,其中分布因子σs=3,σd=1。由图可知,沿s轴正方向的场强辐射范围明显大于沿s轴负方向。风险势场在中心点处场强最大,且数值最大为1;由于障碍物具有s轴向的速度,沿速度方向形成的整体场强分布远大于背离速度方向。

3 最优路径

局部路径规划的目的是根据当前的环境信息,为车辆规划出一条安全的、连续的、可行的路径,能够躲避道路上的障碍物,保障车辆的正常行驶。所以,本文基于离散优化的方法,设计了一种统一多目标代价函数用以评价候选路径,综合考虑路径的安全性、偏移量以及连续性,分别建立相应的代价函数,对路径评价结果进行加权处理,最终评价结果最低的路径即为最优路径。代价函数表达式为

式中,f(i) 表示第i条候选路径的总代价函数,fs、fe、fc分别表示路径安全代价函数、路径偏移代价函数和路径连续代价函数,代价函数的评价结果在[0,1]内,ws、we和wc为各代价函数的权重系数,ws+we+wc=1。

3.1 路径安全代价函数

3.1.1 静态障碍物安全代价函数

建立安全代价函数的主要目的为避免车辆与障碍物发生碰撞,保证车辆避障过程的安全性。传统的碰撞检测方法是将候选路径上的所有车辆点与所有障碍物点进行遍历检测,发生碰撞则结果为1;若未发生碰撞则结果为0。该方法中较小的坐标计算误差即可造成完全相反的检测结果,误差较大,无法完全体现避障过程中障碍物所形成的风险影响;规划算法执行频率较快,该方法计算量大且耗时较大,无法满足复杂环境下的局部路径规划要求。本文结合障碍物风险势场的场强分布,设计一种全新的路径安全代价函数。对当前规划周期内的道路和障碍物建立风险势场,将路径点位置的场强作为安全性评价指标,场强越大,则表明风险越大,路径安全性越低。

在分析候选路径的安全性时,必须考虑车辆的形状。为减少计算量同时保证结果的精确,本文采用四圆分解策略,将车辆分解为4 个半径为rvehicle的圆,使用这4 个包络圆将车辆完全覆盖,如图7 所示。图中虚线表示车辆的后轴,其中,候选路径的参考点为后轴中心点。

图7 四圆分解Fig.7 Four-circle decomposition

针对多车道、多障碍物的复杂路况,考虑到存在传感器的检测误差以及车辆行驶的跟踪误差,需要对风险势场结果做进一步处理。本文利用高斯卷积方法构建路径安全代价函数,在对某一条候选路径的安全性进行评价的同时,充分考虑相邻路径对其产生的影响[14]。路径安全代价函数可表示为

式中,fs1(i) 为静态障碍物安全代价函数,Erisk(Ci) 为第i条候选路径在风险势场中的场强结果,gs1(j) 为离散高斯卷积函数,σs1为标准差。当评价结果显示相邻的几条候选路径的风险都过高,则表示此局部区域内发生碰撞的风险较大,即使某一条候选路径的结果较低,仍然认为其安全风险较高;同理,当某局部区域内相邻几条路径的风险都较低,那么最优路径很有可能就在此区域内产生。

路径安全代价函数的分析如图8 所示,图8(a)为当前规划场景,车辆在右侧主车道行驶,前方遇到静态障碍物,此时有候选路径11 条,编号C1～C11。图8(b)为候选路径在风险势场的场强结果,其中C1～C4在左侧超车道内,C6～C11在原主车道内。路径C7～C9与场强最大,结果为1,同时路径C11在道路边界处,结果同样为1。由图8(c)可以看出,由于C10周围的路径风险很高,在经过离散高斯处理之后其最终结果也为1,而非原车道上的路径风险普遍较低,其中C2评价结果最低,安全性最高,为最优路径。

图8 路径安全代价函数Fig.8 The cost function of path security

3.1.2 动态障碍物安全代价函数

当车辆面对动态障碍物时,自车与障碍物都处于运动状态,此时分析候选路径的安全性,不能仅仅依据当前时刻障碍物的运动状态。应通过对动态障碍物的当前运动状态和历史移动轨迹分析其运动意图,根据预测状态进行路径安全性的分析。利用车载传感器获得当前时刻动态障碍物的位置及速度信息,以及历史移动轨迹,运用拉格朗日插值法构建动态障碍物的运动预测模型[15]。通过传感器数据记录动态障碍物的先后3 个位置为为(ti,xi,yi),(ti+1,xi+1,yi+1),(ti+2,xi+2,yi+2),将这三组数据带入拉格朗日插值函数形成该障碍物的运动预测模型,根据此模型可以快速预测出此后任意时刻障碍物的运动位置(tn,xn,yn) 。根据预测位置以及道路环境构建当前规划周期的风险势场,结合势场的场强分布与离散高斯卷积建立动态障碍物安全代价函数

式中,fs2(i) 为动态障碍物安全代价函数,Erisk(Ci) 为候选路径在风险势场中的场强分布结果,gs2(j) 为动态障碍物的离散高斯卷积函数,σs2为标准差。

将静态障碍物安全代价函数与动态障碍物安全代价函数的评价结果进行加权求和,即得到路径安全代价函数

式中,ws1和ws2为权重系数。当规划周期内只有静态障碍物时,ws1=1,ws2=0; 当只有动态障碍物时,ws1=0,ws2=1;而场景内同时存在静态障碍物与动态障碍物时,ws1=ws2=0.5。

3.2 路径偏移代价函数

当车辆完成对障碍物的规避之后,应尽快回到原车道上,跟踪路径参考线继续行驶。在对候选路径进行评价时,候选路径相对于路径参考线的偏移量也需作为重要的参考指标。建立路径偏移代价函数

式中,Δs为候选路径簇在路径参考线上的投影弧长值,Δs=send-sstart,di为候选路径的横向偏移量,dmax为整个候选路径簇的最大横向偏移量。

3.3 路径连续代价函数

在进行局部路径规划,特别是面对动态障碍物时,每一时刻障碍物的位置都会在发生变化,候选路径也会随之进行更新;同时自车也是实时前进的状态,对于相邻两个规划周期之间路径的连续性,也成为评价候选路径的关键指标。如果两个相邻规划周期内路径的偏差过大,会导致车辆在短时间内产生较大的转角,增大车辆失稳的概率,车辆的舒适性与稳定性都无法得到保障。所以,为防止出现路径突变的情况,保证规划路径平滑且连续,建立路径连续代价函数

式中,di为当前规划周期内候选路径的横向偏移量,dpre为上一个规划周期内最优路径的横向偏移量,dmax为整个候选路径簇的最大横向偏移量。fc(i) 的值越小,表示路径之间的连续性越好,整体规划路径更加平滑。

3.4 权重系数自适应调节

在离散优化局部路径规划方法中,各代价函数权重系数是综合路径安全性、连续性等性能指标的重要参数,不同的取值将直接影响最终最优路径的选择结果。路径安全代价函数反映候选路径的安全风险,保证路径不发生碰撞,是局部路径规划的根本目标,在总代价函数中应占较大比重;路径偏移代价函数能保证车辆以较短的路径完成避障,并回到路径参考线方向;路径连续代价函数则保证路径连续光滑,防止出现路径突变。在规划周期内,各代价函数之间也相互影响,拥有较低安全风险的路径,其偏移量与连续性往往难以保证。本文采用自适应权重调节策略,将当前规划周期内所有路径的安全代价函数评价结果进行升序整理,分成两半分别计算平均路径安全代价函数评价结果将规划周期内所有候选路径的安全代价函数评价结果与相比较并进行分类。

①fs(i) ≤

当前路径的安全风险低于较优群体的平均安全风险值,说明此路径在候选路径簇中安全性较好,发生碰撞的概率较低。此时应更多地考虑路径的偏移量与连续性,ws应取值较小,为0.5;而we与wc应相对占比较大,各为0.25。

此时路径处于整个候选路径簇的一般位置,路径安全性介于两个均值之间,则权重系数应尽量平衡各个代价函数所占比重,ws取0.6,we与wc各取0.2。

③fs(i) ≥

路径的安全代价函数评价结果高于较差路径群体的平均值,说明此路径安全性很不理想,此时应增大安全代价函数在总代价函数中的占比,更多的考虑路径的安全性,ws取0.8,we与wc各取0.1。

采用这种自适应权重调节策略,使得权重系数不再固定不变,而是根据代价函数的评价结果进行自适应调节,增强代价函数的可行性,提高最优路径的综合性能。

4 仿真结果与分析

为验证本文所提出的局部路径规划方法的可行性,应用软件进行仿真验证。利用MATLAB 软件的Driving Scenario Designer 模块构建仿真所需交通环境,所构建场景包含直道、弯道和多车道等多种常见道路环境,同时设置有多个不同状态的静态、动态障碍物,在软件中对局部路径规划算法进行编程,验证方法的可行性。

4.1 仿真参数设置

在使用Driving Scenario Designer 模块构建仿真场景时,车辆为交通参与者,是具有特定长度、宽度和高度的长方体箱形对象,如图9 所示。在仿真环境中,车辆长度设置为3 m,宽度设置为2 m,后轴比为0.25。本文仿真场景为结构化道路上的紧急避障场景,所以设置车辆最大速度为10 m/s,仿真采样时间为0.01 s。

图9 仿真车辆设置Fig.9 Simulated vehicle settings

4.2 仿真场景一

仿真场景一主要验证算法在直线道路上面对静态障碍物的连续避障规划能力,如图10 所示。图10(a)表示初始状态,其中车辆A 为控制车辆,即自车,起始坐标为(0,0);其他为静态障碍车辆B、C、D,速度为0,静止在道路内。场景内点划线为道路中心线,即为规划算法中的路径参考线。图10(b)表示车辆完整的规划轨迹,由图可以看出,车辆能够完成对静态障碍物的连续规避,同时能够在避障之后迅速回到原路径方向,沿着路径参考线继续行驶,符合局部路径规划的要求。

当t=5.89 s 时,车辆已经检测到前方障碍车辆B 并开始进行局部路径规划,选取规划片段进行分析,如图11 所示。其中图11(a)为此时刻规划场景的局部放大图,当车辆检测到障碍车辆B时,会生成一组局部候选路径C1,C2,…,C11;图11(b)为候选路径的代价函数评价结果,由图可以看出,位于道路中心线左侧的候选路径安全代价函数结果为1,安全性较差。最终路径C9的路径安全代价函数评价结果最低,同时总评价结果也为最低,为此规划片段内的最优路径。

图11 规划片段分析Fig.11 Analysis of planning section

分析规划路径的曲率变化,如图12 所示,其中虚线表示路径参考线,即为无障碍物时的路径,实线为规划路径。在整个规划过程中,路径曲率始终维持在-0.02 m-1与0.02 m-1之间,在对障碍物B、C、D 进行连续避障时达到曲率变化的最大值。路径整体曲率连续且波动范围较小,保证了车辆在避开障碍物的同时,有较好的稳定性。

图12 路径曲率Fig.12 Curvature of path

4.3 仿真场景二

场景二主要模拟行车过程中的换道超车场景,如图13 所示,场景内有两条车道,上方为超车道,下方为主车道。车辆A 为控制车辆,即为自车,行驶于主车道上,速度为10 m/s;车辆B 为障碍车辆,也行驶于主车道上,速度为5 m/s。场景内间虚线为车道线,点划线为主车道中心线,即为规划算法中的路径参考线。由图13 可以直观地看出车辆A 在换道超车过程中的完整轨迹。

图13 仿真场景示意图Fig.13 Schematic diagram of simulation scene

当t=11.36 s 时,车辆已经检测到动态障碍车辆B 并开始进行局部路径规划,选取避障规划片段进行分析,如图14 所示。图14(a)为当前规划场景的车辆追逐图视角,图14(b)表示各个候选路径的代价函数评价结果,由图中结果可以得出,原车道上的候选路径整体评价结果较大,性能较差,而左侧超车道上的路径整体结果较好,其中路径的路径安全代价函数结果最小,同时总体评价结果也为最小,为最优路径。

图14 规划片段分析Fig.14 Analysis of planning section

分析路径的曲率变化,如图15 所示,其中虚线表示路径参考线,即为无障碍物时的路径,实线为规划路径。由图可以看出,路径曲率最大为0.035 m-1,最小为-0.02 m-1,且在自车刚检测到障碍车辆并开始进行局部规划时达到最大曲率值,而在车辆进行变道超车过程中曲率一直稳定于 -0.02～0.02 m-1之间。在面对动态障碍物时,路径曲率连续,并且在变道超车过程中曲率保持稳定,验证了算法的可靠性。

图15 路径曲率Fig.15 Curvature of path

4.4 仿真场景三

搭建复杂仿真场景三,验证算法在弯道下同时面对静态和动态障碍物的规划能力,如图16 所示。道路是一条连续的“S”形弯道,一共有3 个车道,中间车道为主车道。车辆A 为控制车辆,即自车,行驶于中间车道,其他为障碍车辆。其中车辆C、D 静止于中间车道,B 车在右侧车道沿车道中心线行驶,E 车在中间车道行驶,但是速度低于自车。图16(b)表示车辆完整的规划轨迹。

图16 仿真场景示意图Fig.16 Schematic diagram of simulation scene

选取t=5.80 s 时的规划片段分析,如图17 所示。其中同时包含了静态和动态障碍物,C 车静止于中间车道内,而B 车行驶于右侧车道。道路场景为三车道,因此为了能够规划出更好的路径,扩大候选路径的约束范围,此时共有19 条候选路径。由图17(b)可以看出,位于右侧车道的候选路径受到动态障碍车辆B 的影响,整体评价结果较高,不仅安全性较差,而且路径连续性的评价结果也较高;位于左侧车道内的路径整体评价结果较低,其中路径C5的安全代价函数与连续代价函数结果最低,同时总体评价结果也为候选路径组内最低,为最优路径。

图17 规划片段分析Fig.17 Analysis of planning section

分析此场景下车辆的路径曲率变化,如图18所示,其中虚线表示路径参考线,即为无障碍物时的路径,实线为规划路径。由图可知,曲率变化范围为-0.04～0.08 m-1。在路径长度为0～200 m时,车辆同时面对静态、动态障碍车辆,曲率变化幅度为0.08 m-1;而在路径长度为200～400 m 时,车辆同时面对两个速度不同的动态障碍车辆,此时曲率变化幅度为0.06 m-1。在复杂的多车道弯道工况下,且同时面对多个静态、动态障碍车辆时,局部环境内路径曲率依然能够保持稳定状态,波动幅度较小,能够实时为车辆规划出路径完成对静态、动态障碍物的规避,验证了算法的有效性。

图18 路径曲率Fig.18 Curvature of path

4.5 仿真场景四

仿真场景四主要模拟交叉路口场景,验证算法在面对横向移动障碍物时的规划能力,如图19所示。车辆A 为控制车辆,即自车,在地图中由左向右沿道路中心线行驶;车辆B 为障碍车辆,在地图中由下向上沿道路中心线行驶。图19(b)表示车辆完整的规划轨迹。

图19 仿真场景示意图Fig.19 Schematic diagram of simulation scene

自车与障碍物车辆各自沿着道路中心线行驶,当t=5.33 s 时,自车检测到障碍物车辆B,并根据其运动状态开始进行局部路径规划。选取此时刻的规划片段进行分析,如图20 所示。由图20(b)可以看出,位于障碍车辆B 前进方向的候选路径与位于后方的候选路径在代价函数的评价结果上相差很大,前者安全性整体偏低,而后者明显安全代价函数评价结果较小,同时连续性也更好。其中路径的安全代价函数结果最低,同时连续性也较好,总体评价结果为所有候选路径中最低,为最优路径。

图20 规划片段分析Fig.20 Analysis of planning section

分析此场景下车辆的路径曲率变化,如图21所示,其中虚线表示路径参考线,即为无障碍物时的路径,实线为规划路径,曲率整体的变化范围为-0.04～0.04 m-1。由于障碍车辆也始终处于运动状态,且速度方向与自车速度方向垂直,所以局部路径规划的范围长度相较换道超车场景更短,曲率变化幅度增大。但是考虑到规划时间较超车场景缩短,并且曲率能够快速恢复稳定,依然能够在车辆安全避开横向移动障碍车辆的同时,保持车辆行驶的稳定性。

图21 路径曲率Fig.21 Curvature of path

5 结论

本文以结构化道路下智能汽车的局部路径规划为研究对象,提出融合风险势场的离散优化局部路径规划方法。应用离散优化构建代价函数对路径的综合性能进行评价,将路径规划转化为优化问题;使用风险势场描述场景对路径的潜在风险,结合高斯卷积设计安全代价函数,增强路径的安全性。仿真结果表明,所提出的局部路径规划方法能够实时为车辆规划出安全且连续的路径,引导车辆完成对障碍物的规避,保证车辆的正常行驶,同时在面对多种复杂交通场景依然具有良好的可行性,方法鲁棒性较好,为进一步优化智能汽车的局部路径规划研究提供了依据。