1.一种用于无人驾驶车辆的局部路径规划算法,其特征在于,包括以下步骤:
1)输入局部路径规划所需参数;
2)选取预瞄点;
3)基于道路坐标系进行轨迹规划,生成基础轨迹;
4)将基础轨迹与道路形状匹配,生成备选轨迹;
5)计算基于备选轨迹的限速值及对应的限速位置;
6)对备选轨迹进行优选,得到无人驾驶车辆执行的轨迹,称为优选轨迹;
7)基于优选轨迹,规划无人驾驶车辆的速度。
2.根据权利要求1所述的用于无人驾驶车辆的局部路径规划算法,其特征在于,所述步骤1)输入局部路径规划所需参数的过程为:
11)首先,以道路长度方向为坐标横轴s轴,道路长度延伸方向为正方向;以垂直于道路的方向为纵轴l轴,左侧为正方向;无人驾驶车辆起始点坐标设置为(siliθi),局部路径规划轨迹终点坐标设置为(sflfθf),其中,θi和θf为无人驾驶车辆起始点和局部路径规划轨迹终点航向角,si和sf为无人驾驶车辆起始点和局部路径规划轨迹终点在道路坐标系下横坐标值,li和lf为起无人驾驶车辆起始点和局部路径规划轨迹终点在道路坐标系下纵坐标值;
12)然后,输入局部路径规划所需参数:
121)全局路径规划模块向局部路径规划模块输入参数:
全局规划路径上一系列规划点笛卡尔坐标下的坐标bi(xbiybi)、规划点的航向角θbi、规划点道路曲率kbi,规划点限速vbi,其中,b表示全局规划点;i表示全局规划点标号,i=1、2、...nb,nb为规划点总的数量;
122)环境感知模块向局部路径规划模块输入参数:
环境感知模块输出为静态障碍物和动态障碍物在道路坐标系下的位置ak(saklak)、宽度wak、速度vak以及类型等,其中,k表示第k个静态障碍物或动态障碍物,k=1、2、...;
123)定位模块向局部路径规划模块输入参数:无人驾驶车辆的位置坐标、速度、航向等;
124)向局部路径规划模块输入车身参数:方向盘转角以及车辆轮距等;
步骤121)至124)的顺序可以任意调整。
3.根据权利要求1所述的用于无人驾驶车辆的局部路径规划算法,其特征在于,所述步骤2)选取预瞄点的过程如下:
首先确定预瞄断面,预瞄断面是根据全局路径规划点做垂直于规划路径的截面,截面与路面交点为线段;在该线段上选取可行驶区域的点为预瞄点;根据速度选取预瞄断面距离;预瞄点是在由预瞄断面确定的线段上按一定间隔距离选取;预瞄点的航向、曲率以及限速与预瞄断面上全局路径规划点相同;预瞄点在道路坐标系下坐标表示为:(sfylfyθfy),其中,下标f表示预瞄断面上的点,y表示预瞄点编号,如果选取n个预瞄点,则y=1、2、...n,从上可知,预瞄点有多个点。
4.根据权利要求1所述的用于无人驾驶车辆的局部路径规划算法,其特征在于,所述步骤3)基于道路坐标系进行轨迹规划,生成基础轨迹的过程为:
所述基础轨迹是根据不同预瞄点给出无人驾驶车辆未来可能行驶的多条轨迹,有n个预瞄点,就有n条备选轨迹;所述基础轨迹在道路坐标系下根据车辆初始参数和预瞄的终点参数求解方程得到,在道路坐标系下,所述基础轨迹方程表示如下:
其中,lj(s)为所述基础轨迹j在道路坐标系下纵坐标,是道路坐标系下所述基础轨迹横坐标的函数;下标j表示备选轨迹的标号,j=1、2、...n;n为备选轨迹总数;c0j、c1j、c2j、c3j、c4j、c5j为曲线方程未知参数,由下述方程组求解得到:
其中,sc、lc为无人驾驶车辆当前时刻在道路坐标系下坐标,
sfj、lfj第j条基础轨迹终点在道路坐标系下的坐标,
tanθfj为第j条基础轨迹终点航向角的正切值,本申请中θfj=0,
tanθc为无人驾驶车辆初始航向角正切值,θc由定位模块得到,
kc为无人驾驶车辆当前位置行驶轨迹曲率,与方向盘转角有关,由定位模块得到,
kfj为第j条基础轨迹终点的曲率,kfj=0。
5.根据权利要求1所述的用于无人驾驶车辆的局部路径规划算法,其特征在于,所述步骤4)将基础轨迹与道路形状匹配,生成备选轨迹的过程为:
在道路坐标系下,将所述基础轨迹与道路形状匹配,得到备选轨迹;所述备选轨迹上的对应规划点在笛卡尔坐标系下的坐标采用以下公式表示:
其中,sbi为全局路径规划点在道路坐标下的横坐标,θbi为全局路径规划点航向角;xbi和ybi为全局路径规划点在笛卡尔坐标下的横坐标和纵坐标,下标j表示备选轨迹的标号,j=1、2、...n,n为备选轨迹总数,b表示全局规划点,i表示全局路径规划点标号,i=1、2、...nb,nb为规划点总的数量。
6.根据权利要求1所述的用于无人驾驶车辆的局部路径规划算法,其特征在于,所述步骤5)计算基于备选轨迹的限速值及对应的限速位置的过程为:
无人驾驶车辆在备选轨迹上受到的限速分为以下几种情况:
51)道路路况对无人驾驶车辆的限速及限速位置:
道路路况对无人驾驶车辆的限速一般根据道路弯曲以及地图得到道路路况对车辆限制速度,全局路径规划点sbi对应的备选轨迹上的点(sbilj(sbi))限速v1limji计算如下:
如果限速v1limji大于道路路况要求的限速vrlim,则v1limji=vrlim;每条备选轨迹j对应的由于道路路况限速位置s1limji记为:
其中,vrlim为道路路况对车辆限速,
sbi为全局路径规划点在道路坐标下的横坐标,
kji为在备选轨迹上与全局路径规划点i对应的曲率,
下标j为备选轨迹标号,下标i为对应的全局路径规划点标号,
acclmax为按照舒适度要求最大侧向加速度,由人的舒适程度决定;
52)静态障碍物和动态障碍物对无人驾驶车辆限速及其限速位置
521)通过无人驾驶车辆上安装的传感器测量得到静态障碍物和动态障碍物在道路坐标系下的位置坐标ak(saklak)、宽度wak、速度vak等参数;
522)计算无人驾驶车辆对静态障碍物和动态障碍物的安全距离
纵向安全距离计算公式:
其中,dsak是纵向安全距离,vc是无人驾驶车辆车速,kc是系数,根据经验确定,δdsak是与障碍物类型有关的参数;
横向安全距离计算公式:
其中,dlak是横向安全距离,wak为静态障碍物或动态障碍物宽度,与类型有关,δdl为固定预留横向最小安全距离,wc为无人驾驶车辆宽度;
523)静态障碍物和动态障碍物限速v2limk以及对应限速位置s2k
5231)计算静态障碍物的限速以及对应限速位置
静态障碍物k对备选轨迹j的限速采用以下公式计算:
在道路坐标系下限速位置横坐标为:
其中,dlmax为最大横向安全距离,
vrlim为道路路况对无人驾驶车辆的限速,
δlakj为静态障碍物k与第二备选规划轨迹j的距离;
5232)计算动态障碍物的限速以及对应限速位置
动态障碍物的速度vak分解在道路坐标系下为(vsakvlak),vsak为障碍物k沿道路纵向速度;vlak为障碍物k沿道路的横向速度;
52321)情况一:如果vlak=0
动态障碍物k对无人驾驶车辆在备选轨迹j的限速为:
动态障碍物k对无人驾驶车辆限速位置在道路坐标系下横向坐标:
其中,dlmax为最大横向安全距离,
vrlim为道路路况对无人驾驶车辆的限速;
52322)情况二:vlak≠0时,计算动态障碍物k对无人驾驶车辆限速步骤如下:
523221)判断动态障碍物接近无人驾驶车辆还是离开备选轨迹j:
δlakj表示动态障碍物k距第二离备选规划轨迹j的距离;
vlak>0表示动态障碍物k离开备选轨迹j
vlak<0表示动态障碍物k接近备选轨迹j;
δlakj>0表示动态障碍物k在备选轨迹左侧,
δlakj<0表示动态障碍物k在备选轨迹右侧;
vlak<0表示动态障碍物k离开备选轨迹j
vlak>0表示动态障碍物k接近备选轨迹j;
动态障碍物k离开备选轨迹j不会对无人驾驶车辆造成阻碍;
523222)动态障碍物k接近备选轨迹j时,判断是否对无人驾驶车辆造成阻碍:
动态障碍物k进入备选轨迹j时间tjakin估计:
动态障碍物k进入备选轨迹j位置sjakin估计:
无人驾驶车辆行驶至动态障碍物k进入备选轨迹j处时间tjckin估计:
δtakin为冗余时间,根据动态障碍物类型和移动速度确定,
当tjckin<tjakin,则车辆可以超过动态障碍物,不会对无人驾驶车辆阻碍;
当tjckin≥tjakin则可能对车辆形成阻碍;
523223)计算动态障碍物横向速度不为零时对无人驾驶车辆的限速以及限速位置
对于对无人驾驶车辆没有阻碍及限制的动态障碍物,不用计算;
对于对无人驾驶车辆有阻碍的动态障碍物,动态障碍物k对备选轨迹j的限速v2limjk:
限速位置s2limjk:
53)计算交通信号灯对无人驾驶车辆限速及限速位置
交通信号灯路口车辆停止线位置在道路坐标系下横坐标为sstop,当前车速vc
531)情况一:当前是绿灯或当前为黄灯且无人驾驶车辆到达路口时间小于1s
则:备选轨迹j的限速v3limj为路口限速;
限速位置为停车线s3limj=sstop(17)
532)情况二:当前是红灯或当前为黄灯且无人驾驶车辆到达路口时间大于1s
则:备选轨迹j的限速v3limj=0(18)
限速位置为停车线s3limj=sstop(19)
54)计算道路交通管理部门要求限速以及限速位置
道路交通管理部门要求限速,包括路口、减速带、特定位置等交通安全要求的地方限速以及对应限速位置是从地图得到;
在道路坐标系上限速位置横坐标记为s4limjm,对应限速记为v4limjm,下标m为备选轨迹中道路交通管理部门要求的必须减速的位置数目;
对每条备选轨迹j有限速位置对应的限速:
在限速位置s1limji,限速为v1limji,
在限速位置s2limjk,限速为v2limjk,
在限速位置s1limj,限速为v3limj,
在限速位置s4limjm,限速为v4limjm;
将j条轨迹上所有限速点均按距离无人驾驶车辆从近到远排列重新组合序列(slimjnvlimjn),其中slimjn为限速位置、vlimjn为限速,下标n代表备选轨迹j限速总排序的序号,n=1、2、...;并对限速位置的限速根据以下方法处理:
一、同一位置有多种限速约束的情况下,以最小限速值计算;
二、同一条备选轨迹位置距离更远的规划点限速值若大于其前一点的限速值,则其限速值为前一点限速值。
7.根据权利要求1所述的用于无人驾驶车辆的局部路径规划算法,其特征在于,所述步骤6)对备选轨迹进行优选,得到无人驾驶车辆执行的轨迹的过程为:
按照每条备选轨迹计算的优化目标函数计算的最小值所对应的备选轨迹为最后无人驾驶车执行的轨迹;
优化目标函数i*(j)计算公式为:
其中,ωr为指标的权值,下标r表示指标序号,r=1、2、..n,n为总的指标数;j为备选轨迹,j=1、2....nl,nl为总的备选轨迹数;
第一项指标j1(j)为轨迹长度指标,计算如下:
其中,sj(t0)为一段时间t0内备选轨迹j内按照限速可以行驶的最远轨迹长度;
其中,smax为在时间t0内按最大限速计算的路程,
smin为在时间t0内按最低限速计算的路程;
第二项指标j2(j)为曲率项指标,计算如下:
其中:
sjf为备选轨迹j终点在道路坐标系下横坐标,
sc为无人驾驶车辆在道路坐标下横坐标,
s0为无人驾驶车辆在道路坐标下路径规划起始点横坐标
kj(s)为备选轨迹j坐标点s处曲率,
kmax为车辆行驶的最大曲率;
第三项指标j3(j)为轨迹偏离规划路径指标,计算公式如下:
其中:
lj(s)为备选轨迹j末端坐标在道路坐标系下纵坐标,
lmax为所有预瞄点中,在道路坐标系下纵向坐标最大的预瞄点的纵向坐标,
第四项指标j4(j)为备选轨迹前后一致性指标,计算公式如下:
其中:
l*(s)为上次局部规划轨迹预瞄点在道路坐标系下纵坐标,
lj(s)本次备选轨迹j预瞄点在道路坐标系下纵坐标,
l为道路宽度。
8.根据权利要求1所述的用于无人驾驶车辆的局部路径规划算法,其特征在于,所述步骤7)规划无人驾驶车辆的速度的过程如下:
规划无人驾驶车辆的速度是在前述步骤7)已经得到的优选轨迹上进行;
规划无人驾驶车辆的速度按照以下方法进行:
71)选取优选轨迹上限速值最小的点位置svmin,限速记vmin
72)根据该点位置和限速,规划一条从当前无人驾驶汽车到达该点速度曲线,该曲线按照梯形曲线构建;其中横坐标为时间,纵坐标为速度;梯形曲线分为加速段,平稳段、减速段;
vmaxlim为无人驾驶车辆至最低限速位置svmin内最大限速,
vc为无人驾驶车辆当前速度,
vf为优选轨迹上限速位置的限速vmin,
accs为加速段加速度,车辆正常行驶中依据舒适度确定,
decs为减速段减速度,车辆正常行驶中依据舒适度确定,
tsteady是平稳段时间,平稳段不能太短,要大于某一值;
已知上述参数,平稳段速度不能超过vmaxlim,建立一条速度曲线;
73)使用所述速度曲线对优选轨迹上限速位置的限速进行验证,如果满足所有限速则该函数曲线为规划的函数曲线;否则,用前一个限速位置重复步骤71)、72)和73),直到全部满足为止。