一种用于无人驾驶车辆的局部路径规划算法的制作方法
本发明属于无人驾驶路径规划技术领域，具体是一种用于无人驾驶车辆的局部路径规划算法。
背景技术：
无人驾驶车辆是一类能够在道路上复杂环境中自主完成驾驶任务的车辆。无人驾驶控制主要分如下几部分部分：1）环境感知模块，通过传感器如摄像头、激光雷达、毫米波雷达等感知和理解外界环境信息，如路上车辆、行人等静态和动态障碍物以及车道线、红绿灯、交通标识等。2）定位模块，该模块通过卫星导航、惯性导航、slam等进行高精度定位，得到车辆位置、速度、航向以及行驶曲率等。3）高精度地图，为无人驾驶提供道路上所有信息。4）全局路径规划模块，通常利用全局任务文件、全局地图（例如全局路网信息）等信息计算得到全局参考路径，全局任务规划将以系列坐标点形式输出。由于全局路径规划涉及的空间和时间尺度范围较大，通常可以采用离线方式进行规划，并且不需要进行周期性重规划。5）局部路径规划；根据全局路径规划完成精细规划。5）决策与控制该模块，该模块是最终向执行机构发出控制指令，实现横向和纵向控制，从而完成自主驾驶任务。无人驾驶汽车的诞生和发展得益于人工智能、模式识别、计算机视觉、控制理论等诸多学科的飞速发展，是这些领域的最新研究成果与现代汽车工业的完美结合。无人驾驶技术在国防和国民经济领域具有广阔的应用前景。
局部路径规划是无人驾驶车辆运动控制的关键技术之一，它利用全局参考路径或者全局坐标点的引导信息，并结合实时更新的局部环境感知信息，计算得到车辆在未来有限时域内需要执行的路径或者轨迹。由于外界环境是动态变化的，并且控制很难精确跟踪规划结果，因此，局部运动规划是周期性运行的，其计算周期与感知信息更新频率直接相关。局部路径规划根据路径规划问题复杂程度不同，有不同的规划策略，大致可以分为两类：一类是基于直接轨迹生成的局部路径规划方法；另外一类是图搜索的局部路径规划方法。前者主要为了处理复杂的系统模型约束，保证规划结果的可执行性，而后者主要为了应对复杂的障碍物环境，把规划问题转化为序列决策问题。由于后者计算量非常大，在特殊情况下使用，大部分采用直接轨迹规划。对于直接轨迹生成的局部路径规划方法局部路径规划分为空间域的轨迹规划和时间域的速度规划。一般轨迹规划是直接在笛卡尔坐标中进行轨迹规划。
由于城市道路复杂，有结构化道路，也有非结构化道路，包括小路，路的形状也各异。普通直接进行轨迹生成方法受到限制。根据起始点和终点参数规划的轨迹，由于道路形状复杂，规划轨迹点在道路形状复杂时规划轨迹点有可能在可行道路外，造成规划失败。还有，局部路径规划首先考虑的问题就是避障，包括静态障碍物（例如路牙，桩子，静止的行人、车辆等）避障和动态障碍物（即其它交通参与者，例如，车辆、自行车、行人等）避障。由于静态障碍物位置与时间无关，那么，只需要保证规划的路径在空间上与其不发生碰撞即可。但如果环境中存在动态障碍物，那么，需要同时考虑时间和空间因素，以确保生成轨迹的安全性。这就需要对动态障碍物进行轨迹预测，同时在规划多条轨迹上进行速度采样进行动态碰撞检测，才能得到优选轨迹和速度规划，缺点是算法复杂，而且计算量要求非常大。
技术实现要素：
本发明要解决的技术问题是提供一种用于无人驾驶车辆的局部路径规划算法，该局部路径规划算法能够在城市复杂道路中实现轨迹生成，并在复杂的动态城市环境中实现无人驾驶安全运行。
为实现上述发明目的，本发明采用以下技术方案：
一种用于无人驾驶车辆的局部路径规划算法，包括以下步骤：
1）输入局部路径规划所需参数；
2）选取预瞄点；
3）基于道路坐标系进行轨迹规划，生成基础轨迹；
4）将基础轨迹与道路形状匹配，生成备选轨迹；
5）计算基于备选轨迹的限速值及对应的限速位置；
6）对备选轨迹进行优选，得到无人驾驶车辆执行的轨迹，称为优选轨迹；
7）基于优选轨迹，规划无人驾驶车辆的速度。
进一步的，所述步骤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）的顺序可以任意调整。
进一步的，所述步骤2）选取预瞄点的过程如下：
首先确定预瞄断面，预瞄断面是根据全局路径规划点做垂直于规划路径的截面，截面与路面交点为线段。在该线段上选取可行驶区域的点为预瞄点。根据速度选取预瞄断面距离。预瞄点是在由预瞄断面确定的线段上按一定间隔距离选取。预瞄点的航向、曲率以及限速与预瞄断面上全局路径规划点相同。预瞄点在道路坐标系下坐标表示为：（sfylfyθfy），其中，下标f表示预瞄断面上的点，y表示预瞄点编号，如果选取n个预瞄点，则y=1、2、...n，从上可知，预瞄点有多个点。
进一步的，所述步骤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。
进一步的，所述步骤4）将基础轨迹与道路形状匹配，生成备选轨迹的过程为：
在道路坐标系下，将所述基础轨迹与道路形状匹配，得到备选轨迹；所述备选轨迹上的对应规划点在笛卡尔坐标系下的坐标采用以下公式表示：
其中，sbi为全局路径规划点在道路坐标下的横坐标，θbi为全局路径规划点航向角；xbi和ybi为全局路径规划点在笛卡尔坐标下的横坐标和纵坐标，下标j表示备选轨迹的标号，j=1、2、...n，n为备选轨迹总数，b表示全局规划点，i表示全局路径规划点标号，i=1、2、...nb，nb为规划点总的数量。
进一步的，所述步骤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、...；并对限速位置的限速根据以下方法处理：
一、同一位置有多种限速约束的情况下，以最小限速值计算；
二、同一条备选轨迹位置距离更远的规划点限速值若大于其前一点的限速值，则其限速值为前一点限速值。
进一步的，所述步骤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为道路宽度。
进一步的，所述步骤7）规划无人驾驶车辆的速度的过程如下：
规划无人驾驶车辆的速度是在前述步骤7）已经得到的优选轨迹上进行。
规划无人驾驶车辆的速度按照以下方法进行：
71）选取优选轨迹上限速值最小的点位置svmin，限速记vmin
72）根据该点位置和限速，规划一条从当前无人驾驶汽车到达该点速度曲线，该曲线按照梯形曲线构建。其中横坐标为时间，纵坐标为速度。梯形曲线分为加速段，平稳段、减速段。
vmaxlim为无人驾驶车辆至最低限速位置svmin内最大限速，
vc为无人驾驶车辆当前速度，
vf为优选轨迹上限速位置的限速vmin，
accs为加速段加速度，车辆正常行驶中依据舒适度确定，
decs为减速段减速度，车辆正常行驶中依据舒适度确定，
tsteady是平稳段时间，平稳段不能太短，要大于某一值；
已知上述参数，平稳段速度不能超过vmaxlim，建立一条速度曲线；
73）使用所述速度曲线对优选轨迹上限速位置的限速进行验证，如果满足所有限速则该函数曲线为规划的函数曲线；否则，用前一个限速位置重复步骤71）、72）和73），直到全部满足为止。
本发明提出的一种用于无人驾驶车辆的局部路径规划算法，先在道路坐标系下进行轨迹规划，然后再根据道路形状得到轨迹规划点。本发明的局部路径规划算法同时考虑了车辆运动学约束和道路坐标的几何约束，保证了路径规划结果的可执行性以及与道路几何形状的一致性，避免直接规划在特殊道路上超出道路情况。
本发明的技术方案把环境中所有障碍物，括静态障碍物（例如路牙，桩子，静止的行人、车辆等）避障和动态物体（即其它交通参与者，例如，车辆、自行车、行人等）避障以及交通信号灯、减速带等与道路限速按同等处理，都看做是备选轨迹上某一位置的限速，然后根据备选轨迹所有限速，进行轨迹优选并进行速度规划。不用进行碰撞检测，避免了大量速度采样，适用所有路况，减轻了计算压力。
本发明技术方案的时间域的速度规划算法，从安全性和舒适性角度出发，提出了一种多点速度规划方案，该方案根据优选轨迹上每个限速位置的限速进行速度规划。然后根据所有速度规划找到适合所有限速的速度规划方案，就是最终的执行的速度规划方案。采用了梯形速度规划方法，针对每个限速位置的限速规划。
本发明的一种用于无人驾驶车辆的局部路径规划算法，避免了复杂道路中规划失败，对复杂环境局部路径规划简单化处理，接近人的驾驶行为。本发明的=局部路径规划算法在复杂的城市道路环境中能够实时地、有效地处理静态障碍物以及动态物体、交通信号灯、限速带各种交通设施等对无人驾驶车辆的影响。
附图说明
图1是本发明用于无人驾驶车辆的局部路径规划算法流程框图；
图2是本发明定义道路坐标过程示意图；
图3是本发明速度规划中梯形规划示意图。
具体实施方式
下面结合附图，对本发明提出的一种用于无人驾驶车辆的局部路径规划算法进行详细说明。在本发明的描述中，需要理解的是，术语“左侧”、“右侧”、“上部”、“下部”、“底部”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，“第一”、“第二”等并不表示零部件的重要程度，因此不能理解为对本发明的限制。本实施例中采用的具体尺寸只是为了举例说明技术方案，并不限制本发明的保护范围。
如图1所示，一种用于无人驾驶车辆的局部路径规划算法，包括以下步骤：
8）输入局部路径规划所需参数；
9）选取预瞄点；
10）基于道路坐标系进行轨迹规划，生成基础轨迹；
11）将基础轨迹与道路形状匹配，生成备选轨迹；
12）计算基于备选轨迹的限速值及对应的限速位置；
13）对备选轨迹进行优选，得到无人驾驶车辆执行的轨迹，称为优选轨迹；
14）基于优选轨迹，规划无人驾驶车辆的速度。
具体来说，如图2所示，步骤1）输入局部路径规划所需参数的过程为：
11）首先，以道路长度方向为坐标横轴s轴，道路长度延伸方向为正方向；以垂直于道路的方向为纵轴
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）的顺序可以任意调整。
步骤2）选取预瞄点的过程如下：
首先确定预瞄断面，预瞄断面是根据全局路径规划点做垂直于规划路径的截面，截面与路面交点为线段。在该线段上选取可行驶区域的点为预瞄点。根据速度选取预瞄断面距离。预瞄点是在由预瞄断面确定的线段上按一定间隔距离选取。预瞄点的航向、曲率以及限速与预瞄断面上全局路径规划点相同。预瞄点在道路坐标系下坐标表示为：（sfylfyθfy），其中，下标f表示预瞄断面上的点，y表示预瞄点编号，如果选取n个预瞄点，则y=1、2、...n，从上可知，预瞄点有多个点。
步骤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。
步骤4）将基础轨迹与道路形状匹配，生成备选轨迹的过程为：
在道路坐标系下，将基础轨迹与道路形状匹配，得到备选轨迹；备选轨迹上的对应规划点在笛卡尔坐标系下的坐标采用以下公式表示：
其中，sbi为全局路径规划点在道路坐标下的横坐标，θbi为全局路径规划点航向角;xbi和ybi为全局路径规划点在笛卡尔坐标下的横坐标和纵坐标，下标j表示备选轨迹的标号，j=1、2、...n，n为备选轨迹总数，b表示全局规划点，i表示全局路径规划点标号，i=1、2、...nb，nb为规划点总的数量。
步骤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，当前车速vc531）情况一：当前是绿灯或当前为黄灯且无人驾驶车辆到达路口时间小于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、...；并对限速位置的限速根据以下方法处理：
一、同一位置有多种限速约束的情况下，以最小限速值计算；
二、同一条备选轨迹位置距离更远的规划点限速值若大于其前一点的限速值，则其限速值为前一点限速值。
步骤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为道路宽度。
步骤7）规划无人驾驶车辆的速度的过程如下：
规划无人驾驶车辆的速度是在前述步骤7）已经得到的优选轨迹上进行。
规划无人驾驶车辆的速度按照以下方法进行：
71）选取优选轨迹上限速值最小的点位置svmin，限速记vmin
72）如图3所示，根据该点位置和限速，规划一条从当前无人驾驶汽车到达该点速度曲线，该曲线按照梯形曲线构建。其中横坐标为时间，纵坐标为速度。梯形曲线分为加速段，平稳段、减速段。
vmaxlim为无人驾驶车辆至最低限速位置svmin内最大限速，
vmaxlim为无人驾驶车辆至最低限速位置svmin内最大限速，
vc为无人驾驶车辆当前速度，
vf为优选轨迹上限速位置的限速vmin，
accs为加速段加速度，车辆正常行驶中依据舒适度确定，
decs为减速段减速度，车辆正常行驶中依据舒适度确定，
tsteady是平稳段时间，平稳段不能太短，要大于某一值；
已知上述参数，平稳段速度不能超过vmaxlim，建立一条速度曲线；
73）使用速度曲线对优选轨迹上限速位置的限速进行验证，如果满足所有限速则该函数曲线为规划的函数曲线；否则，用前一个限速位置重复步骤71）、72）和73），直到全部满足为止。
基于对本发明优选实施方式的描述，应该清楚，由所附的权利要求书所限定的本发明并不仅仅局限于上面说明书中所阐述的特定细节，未脱离本发明宗旨或范围的对本发明的许多显而易见的改变同样可能达到本发明的目的。
- 还没有人留言评论。精彩留言会获得点赞！