自动驾驶系统中局部路径规划算法研究

崔 远

(兰州职业技术学院 信息工程学院, 甘肃 兰州 730070)

一、引言

近年来,随着人们生活水平的不断提高以及国内汽车工业的不断发展,我国汽车保有量不断增加,据公安部统计,2022年全国机动车保有量达4.17亿辆,其中汽车3.19亿辆[1]。汽车保有量的快速增加,导致交通事故的数量也急剧增多,据相关统计数据显示,2021年交通事故数量为27.3万起。对于城市交通而言,汽车保有量的增加和交通事故的增多,会造成城市交通系统效率低下。自动驾驶技术的出现,为缓解交通压力、提高行车效率和驾驶安全性提供了新的思路。自动驾驶汽车依托车载摄像头、激光雷达等传感器对车辆周围的环境进行实时扫描,获取车辆周围的道路路况,然后根据道路路况规划车辆行进路径,最后控制车辆按照规划的速度、转向等信息以及行进路径平稳自主地运行。路径规划应满足通行距离短、通行时间少、路径最平滑等特点,其核心问题在于获得一个最优解[2]。人工势场法是将车辆的路径模拟为力场的一种路径方法,目标对车辆产生引力,障碍物对车辆产生斥力,引力场和斥力场共同作用控制车辆运动。然而当前人工势场法容易出现不可抵达目标和无法获取局部最优两类问题,导致车辆无法按照规划路线到达目标。为解决上述问题,改进了人工势场法中引力和斥力生成的目标,通过构建结构化道路、车道引力势场、道路边界势场,改进障碍物斥力势场,同时将行进路径上的障碍物拓展为三维空间的椭圆形障碍物以平滑控制路径。

二、自动驾驶中人工势场建模

传统人工势场主要用于低速机器人的路径规划,鉴于其特殊的使用环境,在设计之初并没有考虑到边界环境对路径规划的影响。自动驾驶中的人工势场建模则需要考虑自动驾驶中的道路情况,如车道数量、车道限速、车辆数量、车辆速度、障碍物目标大小等信息,所以在自动驾驶的人工势场建模中,需要重构结构化道路场景。

(一)传统人工势场法

物体在人工势场法中的引力和斥力的受力如图1所示。

图1 引力场和斥力场

一般来说,自动驾驶中目的地为一个点,引力直指目标点;对于障碍物来说,其形状多样,为了方便计算,把障碍物简化为一个椭圆,所有的斥力都垂直于椭圆的切线[3]。在车辆驾驶场景中,距离目标点越远,则引力场越强。引力势场可以表示为:

Uatt(x)=Kaρ(px,ptar)2

(1)

其中Uatt(x)表示目标点tar对点x的引力大小,Ka表示引力增益系数,ρ(px,pt)表示目标点t到点x的距离。

障碍物对车辆的斥力势场强弱是由节点与障碍物之间的距离决定的,距离障碍物越近则斥力场越强,且当距离超过一定阈值后,将不再产生斥力作用。斥力势场可以表示为:

(2)

其中Urep(x)表示障碍物obs对点x的斥力大小,Ko表示斥力增益系数,ρ(px,pobs)表示障碍物obs与点x之间的距离。

在人工势场模型中,如果在目标点附近出现障碍物,而此时障碍物对节点产生的斥力与目标点对节点产生的引力大小相同、方向相反,则目标点因为没有动力而出现无法抵达目标和获得局部最优的问题。

(二)改进型人工势场法

1.结构化道路构建

相对于低速机器人的随机轨迹,自动驾驶车辆的行进轨迹具有明显特点,如车辆大部分时间运行在车道的中间位置,车道的宽度有限,且运行轨迹相对平滑。自动驾驶环境中的结构化道路构建,需要分辨如下概念:

车道:道路分为多个车道,且每个车道单向通行。

车道标线:标线包括实线、虚线、交替的虚实线,传感器需要分清车道线以保证车辆在正确的车道上行驶。

交叉路口和路口:主要识别交通信号、停车标志和交通指示等,需要根据交通标识选择行进路线。

障碍物检测:识别道路上的障碍物,如行人、自行车、其他车辆、道路占用标识等,需要采取适当的控制动作,避免碰撞。

道路几何形状:道路的整体结构,如直线、曲线、坡度、车道半径等。

车辆的激光雷达探测距离,毫米波雷达探测频率、距离、车道和道路宽度等。

结构化道路如图2所示,其中车辆的坐标为(x,y),道路中间线的横坐标为xline,车载传感器的有效探测范围为s,障碍物所在坐标为(x,yobs)。

图2 结构化道路

2.车道中心线引力势场构建

正常情况下车辆驾驶在车道中间,车道中心线的引力势能与车辆距离车道中心的距离成正比,距离越远则引力越大;车道边界线对车辆产生斥力,距离越近则斥力越大。车辆的行车路径由引力和斥力共同作用决定。以传感器获取最远距离且范围内无障碍物的场景为例,前方车道线对车辆的引力可以计算为:

(3)

其中k表示引力势场增益系数[4],x表示车辆在车道上的横向坐标,y表示车辆在车道上的纵向距离。

3.车道边界线斥力势场构建

车辆在正常行驶过程中出现变道时,越靠近车道边缘则斥力越大,当回到车道中心时,车辆左右两端的斥力相同,斥力势场为0。设当车辆的边缘挨着车道线时的斥力势场的值0≤δ=p(x,y)≤100,根据常识,车辆越靠近车道边界线则行驶越危险,因此斥力势场就越大,可以采用阶跃函数表示车辆在不同位置所受到的斥力,表示为:

P(x)=|sgn(x-xl)+sgn(x-xr)|(1-0.01δ)δ/2

(4)

其中xl、xr分别表示车辆左右侧标线的x坐标。道路边界的斥力与车速和车长相关,此时的道路斥力可以表示为:

Uroad(x,y)=P(x)×(cosx+1)

(5)

4.障碍物斥力势场构建

在自动驾驶环境中,路径规划是沿着结构化道路进行的,道路上主要的障碍物是移动或者静止的汽车、行人、路障等,且正常情况下车辆的横向避让距离远低于纵向避让距离。由于车辆在行驶过程中,通过雷达探测到的障碍物的坐标会随着行进轨迹的变化而不断变化,为了方便计算,需要将传感器检测到的障碍物的坐标转化到车辆或者车载地图的坐标系中。假设车辆的位置为(xt,yt),障碍物的探测位置为(xs,ys),转换后的坐标为(xtran,ytran)=(xs-xt,ys-yt),经过坐标转换后的障碍物在车辆坐标系或全局坐标系下的位置为[xtrancos(θ)-ytransin(θ),xtransin(θ)+ytrancos(θ)]。

为了自动驾驶的平稳性,规划的路径需要一定的平滑度,因此需要对障碍物模型进行平滑,如前文所述,需要把车辆类化为椭圆,则车辆的椭圆障碍模型可以表示为:

(6)

其中σ1和σ2是椭圆势场的外形控制参数。由于行进路线大概率是在一个三维空间内,因此可以把式(2)的斥力势场分布在三维空间中,并引入梯度的概念,则斥力在三维空间中的分布表示为:

(7)

为了方便计算,对三维空间中的斥力势场进行二维切层,具体到二维层面则有:

(8)

在式(6)中,通过参数σ1和σ2控制障碍物的外形曲线才能在最大程度上保证路径的曲率和平滑性。为了简化设计,设ρ=0,令β=(2πσ1σ2)-1,则式(6)可以简化为:

(9)

其中β表示斥力势场增益系数,μp是一个非零的极小正数,以保证e的指数在特殊情况下为0。由于规划的路线可能与实际的道路有一定的夹角,具体到某个点,规划路线和实际路线的夹角由参数σ1和σ2决定[5]。

如图3所示,假设车辆的初始避障位置为A(xa,ya),避障中点为B(xb,yb),避障终点为C(xc,yc),在避障过程中,以点A到点B为例,汽车的纵向位移为dy=|yb-ya|,横向位移为dx=|xb-xa|。当汽车从点B到点C之后,车辆将不再受到障碍物的斥力影响,在该点斥力为0,因此可以计算σ2的值为:

图3 车辆避障过程

(10)

在局部路径规划中,如图3所示的弧形路径上,各个点的斥力相等,因此可以计算σ1的值为:

(11)

经过上述介绍,车辆在自动驾驶过程中,所受到的力可以表示为:

Usum=Utar+Uobs+Uroad

(12)

三、改进人工势场避障路径规划

在人工势场局部路径规划算法中,车辆在行驶过程中的方向由各种引力和斥力共同作用决定,主要包括道路边界斥力势场、障碍物斥力势场、车道中线引力势场,以及目标引力势场等各种引力和斥力的矢量叠加。一般来说,路径规划的过程如下:

一是在车辆运行到T时刻,已知车载传感器感知距离S内,每次路径规划长度为S,行驶过程中的最大移动角度(车道宽度及车辆的长度)为θ;

二是获取当前路段的路况信息,如车道数量、车道宽度、车道限速、导航信息、障碍物信息以及车辆的重量和刹车距离等;

三是车辆在T时刻的位置为(xT,yT),根据式(12)计算车辆受力,包括目的点对车辆的引力、障碍物对车辆的斥力以及车道线对车辆的引力等,如图4所示。

图4 车辆在行驶过程中的受力分析

四是根据上一步各种力的矢量和避障方向夹角分析计算,可以确定车辆在下一路径规划时刻的位置为(xT+1,yT+1)=(xT+γcosθ,yT+1+γcosθ)。

五是重复(3)、(4),直至车辆抵达目的地。

四、实验结果与分析

本文采用Carsim[6]和Simulink[7]搭建联合仿真测试平台,使用Carsim构建车辆动力学模型和结构化道路场景,同时使用Simulink搭建控制器仿真模型。在该仿真测试环境中,笔者模拟了常见的双向四车道,车道中间为虚线,在无障碍道路、单一障碍物道路以及多障碍物道路三种路况下,对局部路径规划算法进行测试,测试环境如图5所示。

图5 模拟仿真测试环境

模拟环境道路长2000 m,宽度12 m,单条车道宽度3 m,中间车道限速80 km/h,两边车道限速50 km/h,预测时域为15,控制时域为10,车载激光雷达探测时间为0.01 s,有效探测距离为200 m[8]。

(一)无障碍物道路下的测试结果

无障碍道路路况下,车辆在一条车道正常行驶,因此如图6(a)所示,车辆在行驶初期预测结果与实际结果有一些区别,但是随着时间和行驶距离的增加,预测路径和实际路径的拟合程度逐渐提高,说明了该路径规划算法符合正常的驾驶逻辑。图6(b)是预测路线和实际路线的航向角误差,可以看出预测结果与实际结果相差较小,角误差范围在(-0.3,0.3)之间,在正常驾驶误差范围之内。图6(c)是车辆在预测路径行驶过程中的横向加速度,横向加速度直接关系到车辆驾驶的安全性和舒适性,从图中可以看出,横向加速度在(-1,1.5)之间,也处于正常驾驶的范围之中,相对于实际驾驶路径,预测路径的横向加速度更为平滑,因此驾驶倾向较为保守。

图6 无障碍物道路路况测试结果

(二)单一障碍物道路下的测试结果

由于在本模拟环境中,每个方向有两条车道,因此在单一障碍物路况下,如图7(a)所示,实际行驶的路线可能有两条,一条是越过障碍物后返回原车道,另外一条路径是绕过障碍物后在另一条车道行驶。在预测路线为返回原车道测试中,预测路线与实际运行的返回原车道的路线基本相同。在返回原车道的实际路线和预测路线中,航向角误差也基本维持在(-0.3,0.2)之间,满足日常行驶要求。和无障碍物路况类似,在横向加速度上预测路线也相对较为平稳,可以满足变道过程中驾驶人的舒适性。

图7 单一障碍物道路路况测试结果

(三)多障碍物道路路况下的测试结果

在该测试环境下,车辆模拟了遇到三个车辆的情况,通过图8(a)可以看出,本预测方法和实际驾驶情况较为类似,且横向移动距离较为平滑,在遇到三个障碍物时,车辆可以按照合适的横向加速度进行避障,当避障结束后,车辆可以迅速返回原车道,偏移角速度也在合理范围内,横向加速度同样偏向舒适。

图8 多障碍物道路路况测试结果

五、结语

智能驾驶系统的安全和稳定性取决于局部路径的选择是否平顺稳定,为此提出了改进型人工势场路径规划算法。该算法构建了结构化道路,同时对车道中心线、车道边界线、道路障碍物进行建模,以局部目的地为目标,构建局部环境内的引力和斥力势场,同时将障碍物拓展为三维空间的椭圆形障碍物以平滑控制路径。实验结果表明,该算法生成路径的平滑性更好,精度更高,可有效提高自动驾驶的安全性。