CN113932823A - 基于语义道路地图的无人驾驶多目标点轨迹并行规划方法 - Google Patents

Abstract

本发明提出一种基于语义道路地图的无人驾驶多目标点轨迹并行规划方法，包括步骤：基于米级定位和语义道路地图进行全局路径规划；基于语义参考路径和实时感知信息生成局部规划地图；考虑车辆动力学约束的多目标点实时并行轨迹规划；最优轨迹的选择与局部路径保持。本发明融合了实时检测车道线的精确性和语义参考路径信息的完备性，实现了结构化和非结构化道路场景规划的统一，在满足轨迹规划的实时性要求下提高了规划轨迹的多样性和可选择性，从多条轨迹中选取最优轨迹供车辆执行，并且保证了车辆驾驶的稳定性和平滑性，适用于无人车在高速公路、城区道路、停车场等各类结构化和非结构化道路中的自主驾驶。

Description

基于语义道路地图的无人驾驶多目标点轨迹并行规划方法

技术领域

本发明是一种应用于无人驾驶使用非高精度语义道路地图情况下的实时轨迹规划方法。

技术背景

车道级高精度地图和厘米级高精度定位常常被认为是无人驾驶实现的基础，当前绝大多数无人驾驶车辆的路径规划都依赖于车道级高精度地图和厘米级高精度定位。给定无人驾驶车辆当前行驶道路的车道级高精度地图和厘米级高精度定位，无人驾驶车辆能精确地知道车辆周围的道路结构和车辆所处的位置，由此无人驾驶车辆可以决策换道或保持当前车道行驶，作出相应决策后就可依据车道级高精度地图中的车道中心线生成换道路径或当前车道保持路径。然而高精度地图的构建和更新代价高昂，高精度定位在复杂动态城市场景中经常受到干扰，容易出现鲁棒性的缺陷，若是高精度地图长期不更新，车辆所获取的车道结构信息便会与实际道路信息不符，若是高精度定位未达到厘米级，无人驾驶车辆便会在地图上出现位置偏移，这将导致车辆无法正常地行驶在车道中心上。然而，人类驾驶员在驾驶过程中不依赖车道级的高精度地图和厘米级的高精度全局定位，而是依靠语义道路地图和较为粗略的全局定位结合高精度的局部定位即可安全正确地行驶。

发明内容

本发明提出了一种使用语义道路地图的无人驾驶多目标点轨迹并行规划方法。其中，语义道路地图为仅包含单向道路几何表达的拓扑语义地图，其中通过单向道路构成的拓扑网络可进行全局路径规划，其结果为一条由单向道路组成的路径。路径中的每一段单向道路均记录了所包含的车道数量信息。通过米级的全局定位，如单点GPS定位或视觉SLAM重定位，可得到车辆与语义道路地图之间的精度为米级的位置关系。然后基于对局部道路环境，如车道线和障碍物信息的观测，实现车身坐标系下的局部规划地图的生成。为了同时实现在结构化和非结构化道路中的灵活轨迹规划，采用一种并行的路径搜索方法，对根据每一条车道或参考单向道路所生成的多个目标点进行符合车辆动力学约束的最优路径搜索，最后根据每一条路径的速度规划结果对轨迹进行择优。

本发明方法适用于无人车在高速公路、城区道路、停车场等各类结构化和非结构化道路中的自主驾驶。

本发明技术方案是：

基于语义道路地图的无人驾驶多目标点轨迹并行规划方法，其特征在于，包括：

步骤1、基于米级定位和语义道路地图进行全局路径规划；

步骤2、基于语义参考路径和实时感知信息生成局部规划地图；

步骤3、考虑车辆动力学约束的多目标点实时并行轨迹规划；(创新步骤)

步骤4、最优轨迹的选择与局部路径保持(创新步骤)；

所述步骤1：使用GPS定位系统获取车辆当前位置经纬度坐标，同时人为指定目的地经纬度坐标，在语义道路地图中使用最邻近单向道路搜索方法，得到车辆和目的地分别在语义道路地图中的对应最近位置，分别作为全局路径规划的起点和终点，再利用已有A^*搜索算法在语义道路地图中进行快速搜索，得到从起点到达终点的单向道路序列，所述单向道路序列同时记录有车道数量信息，作为全局语义参考路径提供给步骤2。

所述步骤2：由步骤1给出的全局语义参考路径仅包含不精确的道路信息，无人车从该全局语义参考路径中截取出当前车辆周围局部区域内的一段路径作为局部参考路径，将局部参考路径信息与实时检测结果进行融合，通过两者之间的互补得到稳定精确的局部规划地图。

步骤2，具体包括如下步骤：

步骤2.1局部参考路径车道线信息和实时检测车道线的匹配

步骤2.1.1通过局部参考路径中的车道数量信息和车道宽度信息生成虚拟道路结构；

步骤2.1.2通过摄像头检测获取车道线实时检测的结果；

步骤2.1.3使实时检测的最右侧车道线与虚拟道路结构中最右侧车道线进行匹配，剩下的车道线依次从右往左进行匹配，若数量不同，则多余的不进行匹配，计算此次互相匹配的车道线的平均位置偏差；依次将实时检测的最右侧车道线与虚拟道路结构中的剩下的车道线从右往左进行匹配，计算互相匹配车道线的平均位置偏差；

步骤2.1.4选取步骤2.1.3中平均位置偏差最小的匹配方式作为车道线匹配的最终结果；

步骤2.1.5以局部参考路径中记录的车道线数量为准，若实时检测出的车道线数量多于地图记录的车道线数量，则匹配后的结果中将多余的车道线删去，若实时检测出的车道线数量少于地图记录的车道线数量，则使用检测出的所有车道宽度的平均值作为剩下需要添加车道线的平移距离，最终得到与语义道路地图记录的车道结构相匹配的局部规划地图；

步骤2.2车道实线边界生成虚拟墙

在局部规划地图中将车道线中的实线标识为虚拟墙，虚拟墙作为一种障碍物既能避免规划结果跨越实线违反交通法规，又能减少路径规划的搜索范围；

步骤2.3封闭路口处不正确导向车道

在路口处，无人驾驶车辆按需行进方向行驶进入正确的导向车道，根据决策意图，将不应进入的车道进行封闭；

步骤2.3.1当无法检测出路口每条车道的导向类型时，对路口处的车道设默认导向规则：当路口处为单行道时，则认为该车道左转、直行、右转；若路口处为两车道，则认为左边车道为左转直行道，右边车道为右转直行道；若路口处车道数大于两车道，则认为最左侧为左转道，最右侧为右转道，其他所有车道为直行道；当检测出路口的导向类型时，以实时检测结果为准；

步骤2.3.2从路口实线处开始，对导向方向不符合车辆决策意图的车道生成虚拟墙；

步骤2.4路口停止线的处理

对于路口处的停止线，若此时为红灯禁止通行，则将在停止线处生成一个速度为0的动态障碍物，迫使车辆规划减速以在停止线前停止，若为绿灯放行，则不生成虚拟障碍物或去除虚拟障碍物的影响；

步骤2.5动态障碍物的处理

对于所有感知到的动态障碍物和步骤2.4通过停止线生成的动态障碍物，都将其从局部规划地图上去除，放入步骤3中进行处理；

通过以上步骤得到一张受车道规则约束的适用于动态场景的路径规划地图，提供给步骤3。

所述步骤3、考虑车辆动力学约束的多目标点实时并行轨迹规划：

步骤3.1在局部规划地图中采样规划目标点；

步骤3.2对所选目标点集的实时并行规划；

步骤3.3基于二次规划的速度规划；

所述步骤3.1在局部规划地图中采样规划目标点：

步骤3.1.1设参考路上各点到车辆的沿该点法向的距离为s，车辆当前速度为v，垂直参考路的偏移点的偏移距离为l，则可规划的目标点将在符合车辆当前速度的可到达偏移点中产生，假设车辆当前的位置为(s0,l0)；进入步骤3.1.2；

步骤3.1.2在可选择范围内根据当前车辆状态，选取相应的目标点集：在结构化道路时，若车辆为正常行驶状态，则选取当前所有同向车道中心线在局部规划地图中的最远处为目标点集，若车辆为U字掉头状态，则选取能掉头的对向车道中心线在局部地图中最远处为目标点集；在非结构化道路时，若车辆为正常行驶状态，则通过在语义参考路径最远处横向离散多个目标点作为目标点集，若车辆为泊车状态，则选取所有车辆周围能检测到的停车库位中心点为目标点集。

所述步骤3.2对所选目标点集的实时并行规划：

步骤3.2.1构建Reeds-Shepp曲线和Dubins曲线查找表，提供给步骤3.2.5 和步骤3.2.7使用；

步骤3.2.2构建不同车速下的扩展基元表，提供给步骤3.2.6；

车辆动力学约束，路径最小曲率κ与车辆速度v需要满足关系

其中μ为摩擦力系数，与路面粗糙程度有关，g是地球重力加速度；

将速度空间[0,V]以

为单位速度离散化为离散的速度空间 {v_i∣i∈[0,V_idx]}，通过人工实验的方法测量车辆在每个离散速度值v_i,i∈ [0,V_idx]行驶时正常人类乘客能够接受的最大曲率κ_i作为速度区间[v_i,v_i+1)的最大扩展曲率；对于每个离散速度值上的曲率约束κ_i，使用弧长l_i作圆弧C_i：

α_i(t)＝l_i·t·κ_i

其中t为弧长采样比例点，介于0到1之间，α_i为在采样比例点处的角度， x_i为在采样比例点处对应的横坐标值，y_i为在采样比例点处对应的纵坐标值；其中弧长l_i人为指定，随车速递增，以减小规划结果在高速度时的曲率变化率；

每次路径规划前，将当前车速v转换为离散后速度空间的索引

并访问查找表直接得到适用于当前速度的扩展基元集合 {C_i∣i∈[v_idx,V_idx]}；将基元C_i中各位姿的x分量数值取反得到对应曲率和弧长的倒车扩展基元，将基元C_i中各位姿的y分量数值取反得到对应曲率和弧长向另一方向转向的扩展基元；

步骤3.2.3基于路径规划地图与目标位姿进行无车辆动力学约束最短距离计算，计算结果提供给步骤3.2.7；

使用广度优先搜索算法计算出局部规划地图中每个离散格点到达每个目标位姿的最短距离d用于步骤3.2.7中路径搜索过程中启发值的计算；

对于搜索产生的某状态(x,y,θ,d)，x、y表示局部规划地图中的位置；θ为航向角，以规划起点处车的航向为0；d∈{1,0}是离散值，表示当前行驶状态车辆的行驶方向(正向、负向)；对于步骤3.2中采样得到的目标点集中的每个目标点使用单独的线程进行并行启发式搜索，所有搜索线程退出后收集搜索结果；

步骤3.2.4确定当前状态为规划起点车辆的位姿，设置其代价为0；进入步骤3.2.5；

步骤3.2.5每N(s)次普通曲线扩展进行一次动态拓展：即使用步骤3.2.1中计算的Reeds-Shepp曲线和Dubins曲线查找表在当前状态代表的位姿与目标位姿生成一条曲线，并使用两级圆环碰撞检测对该曲线上各个位姿进行碰撞检测；若沿该曲线行驶不会导致车辆发生碰撞，则直接将该曲线设为当前状态后的规划路径，搜索成功，此次路径搜索结束；若发生了碰撞，则放弃动态扩展，进行步骤3.2.6；其中N(s)与当前状态的估值有关，随着当前位姿靠近目标点，动态拓展将拥有更高的频率：

对于规划的两种模式，在前向规划中使用Dubins曲线进行拓展，在前后向规划中使用Reeds-Shepp曲线进行拓展；

步骤3.2.6在步骤3.2.2中预先计算的扩展基元表中根据当前速度选取满足当前车速下的最大曲率限制的扩展基元集合并将其中全部基元作旋转和平移变换使得变换后每个基元的起点状态为当前状态，终止状态表示了车辆下一可能的状态。对扩展基元中每个基元进行扩展，并使用两级圆环碰撞检测对该状态以及变换后的基元上的全部状态进行碰撞检测，若使用该基元进行扩展不会使车辆产生碰撞，才确定相应终止状态作为搜索算法当前扩展状态u的邻接状态 v，并进行步骤3.2.7；若产生了碰撞，则重新选择新的扩展基元进行扩展；

步骤3.2.7计算邻接状态v的估值，提供给步骤3.2.8；

A*路径搜索算法性能的关键在于良好的估值函数的选择，即：

F＝G_s+H_g

上式中F为格子的估值，G_s为该格子与起点的代价，H_g为该格子与终点的估计代价，H_g一般取该格子距离终点的直线欧式距离，且算法搜索过程中，相邻节点间的代价一般为两节点的直线距离；

结合步骤3.2.3得到的栅格地图最短距离，改进传统估值函数的计算；对于某状态s(x_t,y_t,θ_t,d_t)，估值函数兼顾车辆动力学约束与障碍物约束；具体计算方法为：

H_g(s)＝max(C(s),P(s))+W_κ(s)

其中，C(s)代表考虑车辆动力学约束时，从状态s行驶到目标状态所需的最小距离，从步骤3.2.1所计算的查找表中直接得到，在前向规划模式取两位姿之间最短Dubins曲线长度，前后向规划模式取最短Reeds-Shepp曲线长度；

P(s)为从状态s“自由”移动到目标位置且绕过障碍物所需的最小距离，即不考虑车辆动力学约束以及抵达终点时的航向角；P(s)由步骤3.2.3得到的栅格地图中无车辆动力学约束最短距离计算而来，设Eu(a,b)表示位置(位姿)a、b 间的欧式距离，则计算方法为：

P(s)＝min{Eu(s,s′)+P(s′)∣s′∈Ext(s)}

Ext(s)表示s表示的位置在离散空间中的邻域：

W_κ(s)表示状态s的曲率变化惩罚，用于约束曲率的变化使其趋向于不变或变小，以进一步提高搜索结果的平滑性；由于本算法使用圆弧作为扩展基元，当前曲率κ(s)与其父亲状态下的曲率κ(s′)可在扩展时直接获得，W_κ(s)的计算方法为：

W_κ(s)＝w₀(κ(s)-κ(s′))

步骤3.2.8选取估计值最小的状态作为新的当前状态，查看当前已搜索时间是否超出预先设定的最大限制，若超时则认为搜索失败，结束搜索；若没有结束搜索，则迭代步骤3.2.4～3.2.8直到目标状态成为邻接状态、动态拓展成功或超出时间限制，对于单个目标点的路径搜索结束。

所述步骤3.2两级圆环碰撞检测算法：

步骤3.2.5、步骤3.2.6运行了所述两级圆环碰撞检测算法：

首先以车辆中心为圆心，以半车身长度为半径形成第一级碰撞检测圆环，若在该第一级圆环内没有出现其他障碍物，则表示车辆未发生碰撞，若在第一级圆环内出现其他障碍物，则表示车辆可能发生碰撞，从而进行第二级圆环碰撞检测；第二级圆环碰撞检测由四个以半车身宽为半径组成的圆环构成，第二级圆环刚好包围了车辆的整体轮廓，当进行第二级圆环碰撞检测时，需要分别判断四个第二级圆环内是否包含其他障碍物，若任意一个第二级圆环内出现其他障碍物，则表示车辆发生碰撞，否则表示车辆未发生碰撞。

所述步骤3.3基于二次规划的速度规划，包括：

步骤3.3.1构建包含移动障碍物运动信息的路径-时间(S-T)二维坐标系

构建Frenet坐标系并判断是否存在在一定路径宽度范围内与该路径发生交互的障碍物；假设移动障碍物处于匀速直线运动状态，S-T图上动态障碍物的运动信息为一平行四边形；对于速度为0的静态障碍物，其运动信息将直接投影于S-T图上；对于动态障碍物，其预测轨迹与本车路径的交互部分将被投影于S-T图上；

步骤3.3.2通过动态规划在S-T图上获取无碰撞的分段线性曲线

S-T图将被离散为网格；一系列采样点在时间轴上以间隔dt等距散布，标记为(t₀,t₁,…,t_n)；相应的，离散网格上的分段线性曲线表示为S_ref＝ (s₀,s₁,…,s_n)；分段线性曲线上的速度、加速度和加加速度将通过以下方式进行估计：

每一个网格的代价为障碍物代价、速度代价、加速度代价、加加速度代价与最小的前驱网格代价之和；代价的计算方法和递推关系如下：

C_obs＝w_obs∑ΔS_ref

C_vel＝w_vel|S_ref′-V_limit|

C_acc＝w_acc(S_ref″)²

C_jerk＝w_jerk(S_ref″′)²

C_jerk＝w_jerk(S_ref″′)²

C_total＝C_obs+C_vel+C_acc+C_jerk

Cost(s_t,t)＝min[Cost(s_t-1,t-1)]+C_total

障碍物代价C_obs通过无人车到所有障碍物的距离之和进行评估；速度代价 C_vel的计算中，V_limit表示基于车辆动力学约束与交通规则所得的速度限制；加速度平方与加加速度平方之和在一定程度上保证了曲线的平滑性；

步骤3.3.3通过求解二次规划问题获取平滑S-T曲线

二次规划步骤由目标函数、线性约束和spline QP求解器构成，最小化以下目标函数：

S_ref即为步骤3.3.2所得的分段线性曲线；该目标函数中第一项描述了到参考曲线的距离，加速度项与加加速度项保证了所生成spline曲线的平滑性；

线性约束表示为以下形式：

S(t_i)≤S(t_i+1),i＝0,1,2,…,n-1,

S′(t_i)≤V_limit,

-Dec_max≤S″(t_i)≤Acc_max

通过spline QP求解器，得到一个最优化的S-T函数；将该函数与步骤3.2 中规划所得的路径结合，生成无碰撞、乘坐舒适的运动轨迹。

所述步骤4最优轨迹的选择与局部路径保持，包括：

步骤4.1最优轨迹的选择

由步骤3得到轨迹曲线及其上各点的速度后，选出其中最优的一条轨迹执行，对每条轨迹进行评价，进行量化评价；

步骤4.1.1轨迹代价考虑的组成项极其量化方式

假设轨迹的代价越低越好，考虑轨迹的平均速度、平均曲率、最大曲率、距离障碍物最近距离以及与上一时刻的规划车道变化，那么有：轨迹的平均速度越大，轨迹效率越高，代价应越低，以平均速度的倒数形式

来量化其代价；轨迹的平均曲率越大，轨迹越陡，代价应越大，以平均曲率的正比例函数

来量化其代价；同理最大曲率与平均曲率一样，以其正比例函数k×κ_max来量化其产生的代价；轨迹距离障碍物的最近距离越小，则轨迹风险越高，代价应越大，以该障碍物最近距离的倒数

来量化其代价；轨迹目标点的横向偏移与上一次规划轨迹目标点的横向偏移相差得越大，则车辆行驶越不平稳，代价应越大，以轨迹目标点与上一次所选轨迹的目标点的关于上一次目标车道的横向偏差的正比例函数k×Δ_lane来量化其代价；

步骤4.1.2各量化组成项的统一规范化方法

在步骤4.1.1中确定了一条轨迹需考虑的评价的方面及其每个方面的基本量化形式，将每个方面都统一在一起形成一个规范的统一量化标准，对步骤4.1.1 中的每个量化项进行加权统一，得到以下轨迹代价量化公式：

步骤4.1.3选择出代价最小的轨迹

通过步骤4.1.2计算出每条轨迹的代价以后，选取其中代价最小的轨迹，将其作为车辆的执行轨迹供无人车行驶；

步骤4.2局部路径的保持，包括：

步骤4.2.1将规划出的路径轨迹投影至全局坐标系中，设为预保持路径，结合动态障碍物信息对预保持路径进行速度规划；

步骤4.2.2根据当前车速计算车辆安全保持距离；

步骤4.2.3将车辆安全保持距离内的预保持路径段设为保持路径，若保持路径突然出现静态障碍物，则减速并返回步骤4.2.2；

步骤4.2.4取保持路径的末端点为下一次规划起点进行规划，将规划出的路径与保持路径拼接得到新的预保持路径；若规划区域出现障碍物，则规划出避障路径；

如此循环以上步骤4.2.1、步骤4.2.2、步骤4.2.3、步骤4.2.4步骤。

本发明的有益效果是：

1.不依赖高精度车道级地图，和厘米级高精度定位。采用语义道路地图和粗略的米级定位即可实现无人驾驶的轨迹规划；

2.在结构化道路场景中使用实时检测的车道线修正语义参考路径的车道信息，融合了实时检测车道线的精确性和语义参考路径信息的完备性；

3.在非结构化道路场景中使用静态障碍物地图结合语义参考路径的车道信息生成虚拟车道线，实现了结构化和非结构化道路场景规划的统一；

4.使用局部语义参考路径和实时感知信息，生成合理准确的局部规划地图，并根据车速进行多个规划目标点的采样；

5.使用并行规划方法在规定时限高效地规划出多条候选轨迹，在满足轨迹规划的实时性要求下提高了规划轨迹的多样性和可选择性；

6.基于合理的轨迹代价计算方式从多条轨迹中选取最优轨迹供车辆执行。并且通过对轨迹的分段保持和对轨迹的延迟择机更新保证了车辆驾驶的稳定性和平滑性。

附图说明

图1本发明整体算法框图

图2基于语义道路地图的全局路径规划示意图

图3语义道路地图中的车道线和车辆实时检测的车道线示意图。图中，较长但位置不精确的浅灰色语义道路车道线和较短但位置精确的黑色实时检测车道线。

图4车道线匹配过程和结果示意图。图中，车道线匹配之后可得到较长且位置信息准确的车道线结果。

图5局部规划地图车道线处理示意图。图中，根据车道线实线生成的虚拟墙示意。

图6局部规划地图路口处理示意图。图中，车辆在路口处决定直行，则封闭其他转向车道。

图7两级圆环碰撞检测示意图

图8不同速度区间对应的最大曲率和该区间对应的扩展基元示意图

图9路径规划启发值计算示意图。

图10实时路径规划流程图

图11速度规划示意图。

具体实施方式

以下结合附图对本发明技术方案进一步说明。

如图2所示，步骤1、基于米级定位和语义道路地图进行全局路径规划； (常规技术)

使用GPS定位系统获取车辆当前位置经纬度坐标，同时人为指定目的地经纬度坐标，在语义道路地图中使用最邻近单向道路搜索方法，得到车辆和目的地分别在语义道路地图中的对应最近位置，分别作为全局路径规划的起点和终点，再利用已有A^*搜索算法在语义道路地图中进行快速搜索，得到从起点到达终点的单向道路序列，所述单向道路序列同时记录有车道数量信息，作为全局语义参考路径提供给步骤2。

步骤2基于语义参考路径和实时感知信息生成局部规划地图

由步骤1给出的全局语义参考路径仅包含不精确的道路信息，无人车从该全局语义参考路径中截取出当前车辆周围局部区域内的一段路径作为局部参考路径，该局部参考路径上包含限速、车道数量、车道宽度信息，虽然该局部参考路径位置精度仅为米级，但是其信息完整。无人驾驶系统能够通过摄像头和激光雷达传感器实时检测到当前区域内的车道线、动静态障碍物的精确位置信息，但检测效果不稳定。因此需要将局部参考路径信息与实时检测结果进行融合，通过两者之间的互补得到稳定精确的局部规划地图。

具体包括如下步骤：

步骤2.1局部参考路径车道线信息和实时检测车道线的匹配

由于局部参考路径的位置不精确，以该局部参考路径中的车道信息生成的道路结构与实际道路结构有较大的位置偏差，因此使用实时检测的精确车道线信息对其进行匹配修正：

步骤2.1.1通过局部参考路径中的车道数量信息和车道宽度信息生成虚拟道路结构，如图3浅灰色线所示。

步骤2.1.2通过摄像头检测获取车道线实时检测的结果，如图3黑色线所示。

步骤2.1.3使实时检测的最右侧车道线与虚拟道路结构中最右侧车道线进行匹配，剩下的车道线依次从右往左进行匹配，若数量不同，则多余的不进行匹配，计算此次互相匹配的车道线的平均位置偏差。依次将实时检测的最右侧车道线与虚拟道路结构中的剩下的车道线从右往左进行匹配，计算互相匹配车道线的平均位置偏差。

步骤2.1.4选取步骤2.1.3中平均位置偏差最小的匹配方式作为车道线匹配的最终结果，如图4所示。

步骤2.1.5以局部参考路径中记录的车道线数量为准，若实时检测出的车道线数量多于地图记录的车道线数量，则匹配后的结果中将多余的车道线删去，若实时检测出的车道线数量少于地图记录的车道线数量，则使用检测出的所有车道宽度的平均值作为剩下需要添加车道线的平移距离，最终得到与语义道路地图记录的车道结构相匹配的局部规划地图。

步骤2.2车道实线边界生成虚拟墙

得到最优的车道线匹配后，在局部规划地图中将车道线中的实线标识为虚拟墙，具体如图5所示。虚拟墙作为一种障碍物既能避免规划结果跨越实线违反交通法规，又能减少路径规划的搜索范围。

步骤2.3封闭路口处不正确导向车道

在路口处，无人驾驶车辆必须按需行进方向行驶进入正确的导向车道，因此需要根据决策意图，将不应进入的车道进行封闭，避免无人驾驶系统产生不按导向车道行驶的行为。

步骤2.3.1当无法检测出路口每条车道的导向类型时，对路口处的车道设默认导向规则：当路口处为单行道时，则认为该车道可以左转、直行、右转；若路口处为两车道，则认为左边车道为左转直行道，右边车道为右转直行道；若路口处车道数大于两车道，则认为最左侧为左转道，最右侧为右转道，其他所有车道为直行道。当检测出路口的导向类型时，以实时检测结果为准。

步骤2.3.2从路口实线处开始，对导向方向不符合车辆决策意图的车道生成虚拟墙，具体如图6所示。

步骤2.4路口停止线的处理

对于路口处的停止线，若此时为红灯禁止通行，则将在停止线处生成一个速度为0的动态障碍物，迫使车辆规划减速以在停止线前停止，若为绿灯放行，则不生成虚拟障碍物或去除虚拟障碍物的影响

步骤2.5动态障碍物的处理

对于所有感知到的动态障碍物，如车辆、行人、自行车，和步骤2.4通过停止线生成的动态障碍物，都将其从局部规划地图上去除，放入步骤3中进行处理。

通过以上步骤得到一张受车道规则约束的适用于动态场景的路径规划地图，提供给步骤3。

步骤3、考虑车辆动力学约束的多目标点实时并行轨迹规划

步骤3.1在局部规划地图中采样规划目标点

步骤3.1.1设参考路上各点到车辆的沿该点法向的距离为s，车辆当前速度为v，垂直参考路的偏移点的偏移距离为l，则可规划的目标点将在符合车辆当前速度的可到达偏移点中产生，假设车辆当前的位置为(s0,l0)；进入步骤3.1.2；

步骤3.1.2在可选择范围内根据当前车辆状态，选取相应的目标点集：在结构化道路时，若车辆为正常行驶状态，则选取当前所有同向车道中心线在局部规划地图中的最远处为目标点集，若车辆为U字掉头状态，则选取能掉头的对向车道中心线在局部地图中最远处为目标点集；在非结构化道路(为本领域规范术语，是指没有明显的车道标识的道路，即没有使用车道线等交通标识表示出道路结构等路段)时，若车辆为正常行驶状态，则通过在语义参考路径最远处横向离散多个目标点作为目标点集，若车辆为泊车状态，则选取所有车辆周围能检测到的停车库位中心点为目标点集。

对以上目标点集中的每一个目标点，使用步骤3.2进行实时并行规划。

步骤3.2对所选目标点集的实时并行规划

步骤3.2.1采用已有方法构建Reeds-Shepp曲线和Dubins曲线查找表，提供给步骤3.2.5和步骤3.2.7使用。

使用Reeds-Shepp曲线和Dubins曲线进行额外扩展是混合A*算法中用于提升搜索速度的重要步骤。由于在线实时计算上述曲线需要耗费较多计算时间，因此根据局部规划地图大小，根据已有方法预先离线计算好在局部规划地图中上述曲线所有可能的情况，形成上述曲线在该局部规划地图中的查找表。

步骤3.2.2构建不同车速下的扩展基元表，提供给步骤3.2.6。

为了保证路径规划结果的平滑性与灵活性，本发明首次提出了的搜索算法以多条不同曲率的圆弧作为搜索过程中的扩展基元(如图8所示)。为了满足车辆动力学约束，即路径最小曲率κ与车辆速度v需要满足关系

其中μ为摩擦力系数，与路面粗糙程度有关，g是地球重力加速度。因此，扩展基元的最小曲率以及单步扩展长度需要随车辆速度动态变化。故预计算出不同速度下扩展基元上各个位姿相对于初始位姿的精确坐标值和航向角，以位姿序列的方式存储。

具体地，将速度空间[0,V]以

为单位速度离散化为离散的速度空间 {v_i∣i∈[0,V_idx]}，通过人工实验的方法测量车辆在每个离散速度值v_i,i∈ [0,V_idx]行驶时正常人类乘客能够接受的最大曲率κ_i作为速度区间[v_i,v_i+1)的最大扩展曲率。对于每个离散速度值上的曲率约束κ_i，使用弧长l_i作圆弧C_i：

α_i(t)＝l_i·t·κ_i

其中t为弧长采样比例点，介于0到1之间，α_i为在采样比例点处的角度， x_i为在采样比例点处对应的横坐标值，y_i为在采样比例点处对应的纵坐标值；其中弧长l_i可人为指定，随车速递增，以减小规划结果在高速度时的曲率变化率。图8为按照上述方法计算出的全部扩展基元示意图。

每次路径规划前，将当前车速v转换为离散后速度空间的索引

并访问查找表直接得到适用于当前速度的扩展基元集合 {C_i∣i∈[v_idx,V_idx]}。类似地，将基元C_i中各位姿的x分量数值取反可得到对应曲率和弧长的倒车扩展基元，将基元C_i中各位姿的y分量数值取反可得到对应曲率和弧长向另一方向转向的扩展基元。

步骤3.2.3基于路径规划地图与目标位姿进行无车辆动力学约束最短距离计算，计算结果提供给步骤3.2.7。

使用广度优先搜索算法(本领域成熟算法)计算出局部规划地图中每个离散格点到达每个目标位姿的最短距离d用于步骤3.2.7中路径搜索过程中启发值的计算。这样求出的最短距离为绕过障碍物所需经过的最小距离，但未考虑车辆动力学约束。

基于以上基本计算，在连续空间X×Y×Θ×D内实现A*搜索算法，以保证规划结果的平滑性。对于搜索产生的某状态(x,y,θ,d)，x、y表示局部规划地图中的位置；θ为航向角，以规划起点处车的航向为0；d∈{1,0}是离散值，表示当前行驶状态车辆的行驶方向(正向、负向)。对于步骤3.2中采样得到的目标点集中的每个目标点使用单独的线程进行并行启发式搜索，所有搜索线程退出后收集搜索结果。对于目标点集中的每个目标点按照如下步骤进行搜索：

步骤3.2.4确定当前状态为规划起点车辆的位姿，设置其代价为0；进入步骤3.2.5。

步骤3.2.5每N(s)次普通曲线扩展进行一次动态拓展：即使用步骤3.2.1中计算的Reeds-Shepp曲线和Dubins曲线查找表在当前状态代表的位姿与目标位姿生成一条曲线，并使用两级圆环碰撞检测对该曲线上各个位姿进行碰撞检测。若沿该曲线行驶不会导致车辆发生碰撞，则直接将该曲线设为当前状态后的规划路径，搜索成功，此次路径搜索结束；若发生了碰撞，则放弃动态扩展，进行步骤3.2.6。其中N(s)与当前状态的估值有关，随着当前位姿靠近目标点，动态拓展将拥有更高的频率：

对于规划的两种模式，在前向规划中使用Dubins曲线进行拓展，在前后向规划中使用Reeds-Shepp曲线进行拓展。

步骤3.2.6在步骤3.2.2中预先计算的扩展基元表中根据当前速度选取满足当前车速下的最大曲率限制的扩展基元集合并将其中全部基元作旋转和平移变换使得变换后每个基元的起点状态为当前状态，终止状态表示了车辆下一可能的状态。对扩展基元中每个基元进行扩展，并使用两级圆环碰撞检测对该状态以及变换后的基元上的全部状态进行碰撞检测，若使用该基元进行扩展不会使车辆产生碰撞，才确定相应终止状态作为搜索算法当前扩展状态u的邻接状态 v，并进行步骤3.2.7；若产生了碰撞，则重新选择新的扩展基元进行扩展。

步骤3.2.7计算邻接状态v的估值，提供给步骤3.2.8。

本领域皆知，A*路径搜索算法性能的关键在于良好的估值函数的选择，即：

F＝G_s+H_g

上式中F为格子的估值，G_s为该格子与起点的代价，H_g为该格子与终点的估计代价，H_g一般取该格子距离终点的直线欧式距离，且算法搜索过程中，相邻节点间的代价一般为两节点的直线距离。在本算法中，相邻节点间的代价应取扩展时所用圆弧的弧长。

结合步骤3.2.3得到的栅格地图最短距离，改进传统估值函数的计算。对于某状态s(x_t,y_t,θ_t,d_t)，估值函数兼顾车辆动力学约束与障碍物约束。具体计算方法为：

H_g(s)＝max(C(s),P(s))+W_κ(s)

其中，C(s)代表考虑车辆动力学约束时，从状态s行驶到目标状态所需的最小距离，从步骤3.2.1所计算的查找表中直接得到，在前向规划模式取两位姿之间最短Dubins曲线长度，前后向规划模式取最短Reeds-Shepp曲线长度。

P(s)为从状态s“自由”移动到目标位置且绕过障碍物所需的最小距离，即不考虑车辆动力学约束以及抵达终点时的航向角。P(s)由步骤3.2.3得到的栅格地图中无车辆动力学约束最短距离计算而来，设Eu(a,b)表示位置(位姿)a、b 间的欧式距离，则计算方法为：

P(s)＝min{Eu(s,s′)+P(s′)∣s′∈Ext(s)}

Ext(s)表示s表示的位置在离散空间中的邻域：

图9具体展示了有障碍物和无障碍物两种场景下，估值函数中C(s)和P(s) 的含义。上述两个例子中，灰色箭头分别表示初始位姿和目标位姿；虚线代表考虑车辆动力学约束时，从状态s行驶到目标位姿所需的最小距离，即Dubins 距离或Reeds-Shepp距离；实线代表从初始状态“自由”移动到目标位置且绕过障碍物所需的最小距离，即不考虑车辆动力学约束以及抵达终点时的航向角。

W_κ(s)表示状态s的曲率变化惩罚，用于约束曲率的变化使其趋向于不变或变小，以进一步提高搜索结果的平滑性。由于本算法使用圆弧作为扩展基元，当前曲率κ(s)与其父亲状态下的曲率κ(s′)可在扩展时直接获得，W_κ(s)的计算方法为：

W_κ(s)＝w₀(κ(s)-κ(s′))

步骤3.2.8选取估计值最小的状态作为新的当前状态，查看当前已搜索时间是否超出预先设定的最大限制，若超时则认为搜索失败，结束搜索；若没有结束搜索，则迭代步骤3.2.4～3.2.8直到目标状态成为邻接状态、动态拓展成功或超出时间限制，对于单个目标点的路径搜索结束。

以上步骤3.2的全过程如图10所示。

为能够实时对一个状态空间中某一个状态进行碰撞检测，本发明提出两级圆环碰撞检测算法。步骤3.2.5、步骤3.2.6运行了所述两级圆环碰撞检测算法，如图7示：

首先以车辆中心为圆心，以半车身长度为半径形成第一级碰撞检测圆环，若在该第一级圆环内没有出现其他障碍物，则表示车辆未发生碰撞，若在第一级圆环内出现其他障碍物，则表示车辆可能发生碰撞，从而进行第二级圆环碰撞检测。第二级圆环碰撞检测由四个以半车身宽为半径组成的圆环构成，第二级圆环刚好包围了车辆的整体轮廓，当进行第二级圆环碰撞检测时，需要分别判断四个第二级圆环内是否包含其他障碍物，若任意一个第二级圆环内出现其他障碍物，则表示车辆发生碰撞，否则表示车辆未发生碰撞。两级圆环碰撞检测如图7示，其中r_veh表示第二级圆环的半径，l_min表示最靠近车尾的第二级圆环的圆心与车辆后轴中心的距离，l_max表示最靠近车头的第二级圆环的圆心与车辆后轴中心的距离。两级圆环碰撞检测将应用于步骤3.2路径搜索中。

步骤3.3基于二次规划的速度规划

真实场景中通常存在动态移动对象，因此无人车需要在动态场景中实现避障。除了需要适应动态场景，无人车的运动过程必须满足动力学约束。为了实现这个目的，本发明采用了基于空间时间维度上动态规划与二次规划的速度规划方法。

步骤3.3.1构建包含移动障碍物运动信息的路径-时间(S-T)二维坐标系

基于步骤3规划所得的路径用于构建Frenet坐标系(一种常见的二维坐标系，横轴表示弧长，纵轴表示到弧线的距离)，并判断是否存在在一定路径宽度范围内与该路径发生交互的障碍物。在本步骤中，需要对障碍物在未来5s内的运动轨迹有良好预测，并假设移动障碍物处于匀速直线运动状态，因此S-T 图上动态障碍物的运动信息为一平行四边形。对于速度为0的静态障碍物，其运动信息将直接投影于S-T图上；对于动态障碍物，其预测轨迹与本车路径的交互部分将被投影于S-T图上。如图11中灰色平行四边形区域所示。

步骤3.3.2通过动态规划在S-T图上获取无碰撞的分段线性曲线

在该步骤中，S-T图将被离散为网格。一系列采样点在时间轴上以间隔dt 等距散布，标记为(t₀,t₁,…,t_n)。相应的，离散网格上的分段线性曲线表示为S_ref＝(s₀,s₁,…,s_n)。分段线性曲线上的速度、加速度和加加速度将通过以下方式进行估计：

每一个网格的代价为障碍物代价、速度代价、加速度代价、加加速度(加加速度为专业术语，即加速度的导数)代价与最小的前驱网格代价之和。代价的计算方法和递推关系如下：

C_obs＝w_obs∑ΔS_ref

C_vel＝w_vel|S_ref′-V_limit|

C_acc＝w_acc(S_ref″)²

C_jerk＝w_jerk(S_ref″′)²

C_jerk＝w_jerk(S_ref″′)²

C_total＝C_obs+C_vel+C_acc+C_jerk

Cost(s_t,t)＝min[Cost(s_t-1,t-1)]+C_total

障碍物代价C_obs通过无人车到所有障碍物的距离之和进行评估。速度代价 C_vel的计算中，V_limit表示基于车辆动力学约束与交通规则所得的速度限制。加速度平方与加加速度平方之和在一定程度上保证了曲线的平滑性。本步骤不仅为步骤3.3.3提供了分段线性曲线作为参考曲线，同时也提供了需要满足的可行区域约束。该步骤所得的分段线性曲线如图11中离散点所示。

步骤3.3.3通过求解二次规划问题获取平滑S-T曲线

分段线性曲线显然不满足车辆动力学约束，因此本步骤采用二次规划方法生成满足动力学约束的平滑S-T曲线。二次规划步骤主要由目标函数、线性约束和spline QP求解器构成。为了满足对曲线平滑性的要求，需要最小化以下目标函数：

S_ref即为步骤3.3.2所得的分段线性曲线。该目标函数中第一项描述了到参考曲线的距离，加速度项与加加速度项保证了所生成spline曲线的平滑性。该二次规划问题中需要满足的线性约束来自于车辆动力学约束、交通规则约束和步骤3.3.2所提供的可行区域约束。因此，线性约束可表示为以下形式：

S(t_i)≤S(t_i+1),i＝0,1,2,…,n-1,

S′(t_i)≤V_limit,

-Dec_max≤S″(t_i)≤Acc_max

通过spline QP求解器，我们可以得到一个最优化的S-T函数。将该函数与

步骤3.2中规划所得的路径结合，可以生成无碰撞、乘坐舒适的运动轨迹。优化后曲线如图11中曲线所示，图11中给出了跟车场景下速度规划结果的示例。灰色平行四边形区域为步骤3.3.1中动态障碍物的预测轨迹在时空图上的投影；离散点为步骤3.3.2通过动态规划所得的无碰撞分段线性曲线；连接散点的曲线为步骤3.3.3通过构造二次规划问题并求解所得的平滑S-T函数曲线。

步骤4最优轨迹的选择与局部路径保持

步骤4.1最优轨迹的选择

由步骤3得到轨迹曲线及其上各点的速度后，需选出其中最优的一条轨迹执行，因此需要对每条轨迹进行评价，而对轨迹的评价则需要具体量化轨迹的每一方面

步骤4.1.1轨迹代价考虑的组成项极其量化方式

假设轨迹的代价越低越好，考虑轨迹的平均速度、平均曲率、最大曲率、距离障碍物最近距离以及与上一时刻的规划车道变化，那么有：轨迹的平均速度越大，轨迹效率越高，代价应越低，因此以平均速度的倒数形式

来量化其代价；轨迹的平均曲率越大，轨迹越陡，代价应越大，因此以平均曲率的正比例函数

来量化其代价；同理最大曲率与平均曲率一样，也应以其正比例函数 k×κ_max来量化其产生的代价；轨迹距离障碍物的最近距离越小，则轨迹风险越高，代价应越大，因此以该障碍物最近距离的倒数

来量化其代价；轨迹目标点的横向偏移与上一次规划轨迹目标点的横向偏移相差得越大，则车辆行驶越不平稳，代价应越大，因此以轨迹目标点与上一次所选轨迹的目标点的关于上一次目标车道的横向偏差的正比例函数k×Δ_lane来量化其代价。

步骤4.1.2各量化组成项的统一规范化方法

在步骤4.1.1中确定了一条轨迹需考虑的评价的方面及其每个方面的基本量化形式，因此还需要将每个方面都统一在一起形成一个规范的统一量化标准，对步骤4.1.1中的每个量化项进行加权统一，得到以下轨迹代价量化公式：

步骤4.1.3选择出代价最小的轨迹

通过步骤4.1.2计算出每条轨迹的代价以后，选取其中代价最小的轨迹，将其作为车辆的执行轨迹供无人车行驶

步骤4.2局部路径的保持

步骤3.2中提出的路径规划算法具有很强的灵活性，因此在动态场景中容易出现因权值变化而引起轨迹抖动的情况，这也是最优路径搜索算法在中、高速下应用的瓶颈。

在实际规划过程中，规划地图约为30m×80m，单次规划的轨迹长度为40 至50米，规划时间视环境的复杂程度而定，因此在车辆行驶过程中，前后两次规划的时间可能相差几百毫秒，而车辆控制需要实时规划结果，规划与控制产生了冲突。

步骤4.2.1将规划出的路径轨迹投影至全局坐标系中(经纬度坐标系或者 UTM坐标系)，设为预保持路径，结合动态障碍物信息对预保持路径进行速度规划

步骤4.2.2根据当前车速计算车辆安全保持距离

步骤4.2.3将车辆安全保持距离内的预保持路径段设为保持路径，若保持路径突然出现静态障碍物，则减速并返回步骤4.2.2

步骤4.2.4取保持路径的末端点为下一次规划起点进行规划，将规划出的路径与保持路径拼接得到新的预保持路径。若规划区域出现障碍物，则规划出避障路径

循环以上4个步骤，构成局部路径保持方法。

Claims (9)

1.基于语义道路地图的无人驾驶多目标点轨迹并行规划方法，其特征在于，包括如下步骤：

步骤1、基于米级定位和语义道路地图进行全局路径规划；

步骤2、基于语义参考路径和实时感知信息生成局部规划地图；

步骤3、考虑车辆动力学约束的多目标点实时并行轨迹规划；

步骤4、最优轨迹的选择与局部路径保持。

2.如权利要求1所述的基于语义道路地图的无人驾驶多目标点轨迹并行规划方法，其特征在于，所述步骤1：使用GPS定位系统获取车辆当前位置经纬度坐标，同时人为指定目的地经纬度坐标，在语义道路地图中使用最邻近单向道路搜索方法，得到车辆和目的地分别在语义道路地图中的对应最近位置，分别作为全局路径规划的起点和终点，再利用已有A^*搜索算法在语义道路地图中进行快速搜索，得到从起点到达终点的单向道路序列，所述单向道路序列同时记录有车道数量信息，作为全局语义参考路径提供给步骤2。

3.如权利要求1所述的基于语义道路地图的无人驾驶多目标点轨迹并行规划方法，其特征在于，所述步骤2：由步骤1给出的全局语义参考路径仅包含不精确的道路信息，无人车从该全局语义参考路径中截取出当前车辆周围局部区域内的一段路径作为局部参考路径，将局部参考路径信息与实时检测结果进行融合，通过两者之间的互补得到稳定精确的局部规划地图；

步骤2具体包括如下步骤：

步骤2.1局部参考路径车道线信息和实时检测车道线的匹配

步骤2.1.1通过局部参考路径中的车道数量信息和车道宽度信息生成虚拟道路结构；

步骤2.1.2通过摄像头检测获取车道线实时检测的结果；

步骤2.1.3使实时检测的最右侧车道线与虚拟道路结构中最右侧车道线进行匹配，剩下的车道线依次从右往左进行匹配，若数量不同，则多余的不进行匹配，计算此次互相匹配的车道线的平均位置偏差；依次将实时检测的最右侧车道线与虚拟道路结构中的剩下的车道线从右往左进行匹配，计算互相匹配车道线的平均位置偏差；

步骤2.1.4选取步骤2.1.3中平均位置偏差最小的匹配方式作为车道线匹配的最终结果；

步骤2.1.5以局部参考路径中记录的车道线数量为准，若实时检测出的车道线数量多于地图记录的车道线数量，则匹配后的结果中将多余的车道线删去，若实时检测出的车道线数量少于地图记录的车道线数量，则使用检测出的所有车道宽度的平均值作为剩下需要添加车道线的平移距离，最终得到与语义道路地图记录的车道结构相匹配的局部规划地图；

步骤2.2车道实线边界生成虚拟墙

在局部规划地图中将车道线中的实线标识为虚拟墙，虚拟墙作为一种障碍物既能避免规划结果跨越实线违反交通法规，又能减少路径规划的搜索范围；

步骤2.3封闭路口处不正确导向车道

在路口处，无人驾驶车辆按需行进方向行驶进入正确的导向车道，根据决策意图，将不应进入的车道进行封闭；

步骤2.3.1当无法检测出路口每条车道的导向类型时，对路口处的车道设默认导向规则：当路口处为单行道时，则认为该车道左转、直行、右转；若路口处为两车道，则认为左边车道为左转直行道，右边车道为右转直行道；若路口处车道数大于两车道，则认为最左侧为左转道，最右侧为右转道，其他所有车道为直行道；当检测出路口的导向类型时，以实时检测结果为准；

步骤2.3.2从路口实线处开始，对导向方向不符合车辆决策意图的车道生成虚拟墙；

步骤2.4路口停止线的处理

对于路口处的停止线，若此时为红灯禁止通行，则将在停止线处生成一个速度为0的动态障碍物，迫使车辆规划减速以在停止线前停止，若为绿灯放行，则不生成虚拟障碍物或去除虚拟障碍物的影响；

步骤2.5动态障碍物的处理

对于所有感知到的动态障碍物和步骤2.4通过停止线生成的动态障碍物，都将其从局部规划地图上去除，放入步骤3中进行处理；

通过以上步骤得到一张受车道规则约束的适用于动态场景的路径规划地图，提供给步骤3。

4.如权利要求1所述的基于语义道路地图的无人驾驶多目标点轨迹并行规划方法，其特征在于，所述步骤3考虑车辆动力学约束的多目标点实时并行轨迹规划，包括如下步骤：

步骤3.1在局部规划地图中采样规划目标点；

步骤3.2对所选目标点集的实时并行规划；

步骤3.3基于二次规划的速度规划。

5.如权利要求4所述的基于语义道路地图的无人驾驶多目标点轨迹并行规划方法，其特征在于，所述步骤3.1在局部规划地图中采样规划目标点：

步骤3.1.1设参考路上各点到车辆的沿该点法向的距离为s，车辆当前速度为v，垂直参考路的偏移点的偏移距离为l，则可规划的目标点将在符合车辆当前速度的可到达偏移点中产生，假设车辆当前的位置为(s0,l0)；进入步骤3.1.2；

步骤3.1.2在可选择范围内根据当前车辆状态，选取相应的目标点集：在结构化道路时，若车辆为正常行驶状态，则选取当前所有同向车道中心线在局部规划地图中的最远处为目标点集，若车辆为U字掉头状态，则选取能掉头的对向车道中心线在局部地图中最远处为目标点集；在非结构化道路时，若车辆为正常行驶状态，则通过在语义参考路径最远处横向离散多个目标点作为目标点集，若车辆为泊车状态，则选取所有车辆周围能检测到的停车库位中心点为目标点集。

6.如权利要求4所述的基于语义道路地图的无人驾驶多目标点轨迹并行规划方法，其特征在于，所述步骤3.2对所选目标点集的实时并行规划：

步骤3.2.1构建Reeds-Shepp曲线和Dubins曲线查找表，提供给步骤3.2.5和步骤3.2.7使用；

步骤3.2.2构建不同车速下的扩展基元表，提供给步骤3.2.6；

车辆动力学约束，路径最小曲率κ与车辆速度v需要满足关系

其中μ为摩擦力系数，与路面粗糙程度有关，g是地球重力加速度；

将速度空间[0,V]以

为单位速度离散化为离散的速度空间{v_i∣i∈[0,V_idx]}，通过人工实验的方法测量车辆在每个离散速度值v_i,i∈[0,V_idx]行驶时正常人类乘客能够接受的最大曲率κ_i作为速度区间[v_i,v_i+1)的最大扩展曲率；对于每个离散速度值上的曲率约束κ_i，使用弧长l_i作圆弧C_i：

α_i(t)＝l_i·t·κ_i

其中t为弧长采样比例点，介于0到1之间，α_i为在采样比例点处的角度，x_i为在采样比例点处对应的横坐标值，y_i为在采样比例点处对应的纵坐标值；其中弧长l_i人为指定，随车速递增，以减小规划结果在高速度时的曲率变化率；

每次路径规划前，将当前车速v转换为离散后速度空间的索引

并访问查找表直接得到适用于当前速度的扩展基元集合{C_i∣i∈[v_idx,V_idx]}；将基元C_i中各位姿的x分量数值取反得到对应曲率和弧长的倒车扩展基元，将基元C_i中各位姿的y分量数值取反得到对应曲率和弧长向另一方向转向的扩展基元；

步骤3.2.3基于路径规划地图与目标位姿进行无车辆动力学约束最短距离计算，计算结果提供给步骤3.2.7；

使用广度优先搜索算法计算出局部规划地图中每个离散格点到达每个目标位姿的最短距离d用于步骤3.2.7中路径搜索过程中启发值的计算；

对于搜索产生的某状态(x,y,θ,d)，x、y表示局部规划地图中的位置；θ为航向角，以规划起点处车的航向为0；d∈{1,0}是离散值，表示当前行驶状态车辆的行驶方向(正向、负向)；对于步骤3.2中采样得到的目标点集中的每个目标点使用单独的线程进行并行启发式搜索，所有搜索线程退出后收集搜索结果；

步骤3.2.4确定当前状态为规划起点车辆的位姿，设置其代价为0；进入步骤3.2.5；

步骤3.2.5每N(s)次普通曲线扩展进行一次动态拓展：即使用步骤3.2.1中计算的Reeds-Shepp曲线和Dubins曲线查找表在当前状态代表的位姿与目标位姿生成一条曲线，并使用两级圆环碰撞检测对该曲线上各个位姿进行碰撞检测；若沿该曲线行驶不会导致车辆发生碰撞，则直接将该曲线设为当前状态后的规划路径，搜索成功，此次路径搜索结束；若发生了碰撞，则放弃动态扩展，进行步骤3.2.6；其中N(s)与当前状态的估值有关，随着当前位姿靠近目标点，动态拓展将拥有更高的频率：

对于规划的两种模式，在前向规划中使用Dubins曲线进行拓展，在前后向规划中使用Reeds-Shepp曲线进行拓展；

步骤3.2.6在步骤3.2.2中预先计算的扩展基元表中根据当前速度选取满足当前车速下的最大曲率限制的扩展基元集合并将其中全部基元作旋转和平移变换使得变换后每个基元的起点状态为当前状态，终止状态表示了车辆下一可能的状态。对扩展基元中每个基元进行扩展，并使用两级圆环碰撞检测对该状态以及变换后的基元上的全部状态进行碰撞检测，若使用该基元进行扩展不会使车辆产生碰撞，才确定相应终止状态作为搜索算法当前扩展状态u的邻接状态v，并进行步骤3.2.7；若产生了碰撞，则重新选择新的扩展基元进行扩展；

步骤3.2.7计算邻接状态v的估值，提供给步骤3.2.8；

A*路径搜索算法性能的关键在于良好的估值函数的选择，即：

F＝G_s+H_g

上式中F为格子的估值，G_s为该格子与起点的代价，H_g为该格子与终点的估计代价，H_g一般取该格子距离终点的直线欧式距离，且算法搜索过程中，相邻节点间的代价一般为两节点的直线距离；

结合步骤3.2.3得到的栅格地图最短距离，改进传统估值函数的计算；对于某状态s(x_t,y_t,θ_t,d_t)，估值函数兼顾车辆动力学约束与障碍物约束；具体计算方法为：

H_g(s)＝max(C(s),P(s))+W_κ(s)

其中，C(s)代表考虑车辆动力学约束时，从状态s行驶到目标状态所需的最小距离，从步骤3.2.1所计算的查找表中直接得到，在前向规划模式取两位姿之间最短Dubins曲线长度，前后向规划模式取最短Reeds-Shepp曲线长度；

P(s)为从状态s“自由”移动到目标位置且绕过障碍物所需的最小距离，即不考虑车辆动力学约束以及抵达终点时的航向角；P(s)由步骤3.2.3得到的栅格地图中无车辆动力学约束最短距离计算而来，设Eu(a,b)表示位置(位姿)a、b间的欧式距离，则计算方法为：

P(s)＝min{Eu(s,s′)+P(s′)∣s′∈Ext(s)}

Ext(s)表示s表示的位置在离散空间中的邻域：

W_κ(s)表示状态s的曲率变化惩罚，用于约束曲率的变化使其趋向于不变或变小，以进一步提高搜索结果的平滑性；由于本算法使用圆弧作为扩展基元，当前曲率κ(s)与其父亲状态下的曲率κ(s′)可在扩展时直接获得，W_κ(s)的计算方法为：

W_κ(s)＝w₀(κ(s)-κ(s′))

步骤3.2.8选取估计值最小的状态作为新的当前状态，查看当前已搜索时间是否超出预先设定的最大限制，若超时则认为搜索失败，结束搜索；若没有结束搜索，则迭代步骤3.2.4～3.2.8直到目标状态成为邻接状态、动态拓展成功或超出时间限制，对于单个目标点的路径搜索结束。

7.如权利要求6所述的基于语义道路地图的无人驾驶多目标点轨迹并行规划方法，其特征在于，所述步骤3.2两级圆环碰撞检测算法：

步骤3.2.5、步骤3.2.6运行了所述两级圆环碰撞检测算法：

首先以车辆中心为圆心，以半车身长度为半径形成第一级碰撞检测圆环，若在该第一级圆环内没有出现其他障碍物，则表示车辆未发生碰撞，若在第一级圆环内出现其他障碍物，则表示车辆可能发生碰撞，从而进行第二级圆环碰撞检测；第二级圆环碰撞检测由四个以半车身宽为半径组成的圆环构成，第二级圆环刚好包围了车辆的整体轮廓，当进行第二级圆环碰撞检测时，需要分别判断四个第二级圆环内是否包含其他障碍物，若任意一个第二级圆环内出现其他障碍物，则表示车辆发生碰撞，否则表示车辆未发生碰撞。

8.如权利要求4所述的基于语义道路地图的无人驾驶多目标点轨迹并行规划方法，其特征在于，所述步骤3.3基于二次规划的速度规划，包括：

步骤3.3.1构建包含移动障碍物运动信息的路径-时间(S-T)二维坐标系

构建Frenet坐标系并判断是否存在在一定路径宽度范围内与该路径发生交互的障碍物；假设移动障碍物处于匀速直线运动状态，S-T图上动态障碍物的运动信息为一平行四边形；对于速度为0的静态障碍物，其运动信息将直接投影于S-T图上；对于动态障碍物，其预测轨迹与本车路径的交互部分将被投影于S-T图上；

步骤3.3.2通过动态规划在S-T图上获取无碰撞的分段线性曲线

S-T图将被离散为网格；一系列采样点在时间轴上以间隔dt等距散布，标记为(t₀,t₁,…,t_n)；相应的，离散网格上的分段线性曲线表示为S_ref＝(s₀,s₁,…,s_n)；分段线性曲线上的速度、加速度和加加速度将通过以下方式进行估计：

每一个网格的代价为障碍物代价、速度代价、加速度代价、加加速度代价与最小的前驱网格代价之和；代价的计算方法和递推关系如下：

C_obs＝w_obs∑ΔS_ref

C_vel＝w_vel|S_ref′-V_limit|

C_acc＝w_acc(S_ref″)²

C_jerk＝w_jerk(S_ref″′)²

C_jerk＝w_jerk(S_ref″′)²

C_total＝C_obs+C_vel+C_acc+C_jerk

Cost(s_t,t)＝min[Cost(s_t-1,t-1)]+C_total

障碍物代价C_obs通过无人车到所有障碍物的距离之和进行评估；速度代价C_vel的计算中，V_limit表示基于车辆动力学约束与交通规则所得的速度限制；加速度平方与加加速度平方之和在一定程度上保证了曲线的平滑性；

步骤3.3.3通过求解二次规划问题获取平滑S-T曲线

二次规划步骤由目标函数、线性约束和spline QP求解器构成，最小化以下目标函数：

S_ref即为步骤3.3.2所得的分段线性曲线；该目标函数中第一项描述了到参考曲线的距离，加速度项与加加速度项保证了所生成spline曲线的平滑性；

线性约束表示为以下形式：

S(t_i)≤S(t_i+1),i＝0,1,2,…,n-1,

S′(t_i)≤V_limit,

-Dec_max≤S″(t_i)≤Acc_max

通过spline QP求解器，得到一个最优化的S-T函数；将该函数与步骤3.2中规划所得的路径结合，生成无碰撞、乘坐舒适的运动轨迹。

9.如权利要求1所述的基于语义道路地图的无人驾驶多目标点轨迹并行规划方法，其特征在于，所述步骤4最优轨迹的选择与局部路径保持，包括：

步骤4.1最优轨迹的选择

由步骤3得到轨迹曲线及其上各点的速度后，选出其中最优的一条轨迹执行，对每条轨迹进行评价，进行量化评价；

步骤4.1.1轨迹代价考虑的组成项极其量化方式

假设轨迹的代价越低越好，考虑轨迹的平均速度、平均曲率、最大曲率、距离障碍物最近距离以及与上一时刻的规划车道变化，那么有：轨迹的平均速度越大，轨迹效率越高，代价应越低，以平均速度的倒数形式

来量化其代价；轨迹的平均曲率越大，轨迹越陡，代价应越大，以平均曲率的正比例函数

来量化其代价；同理最大曲率与平均曲率一样，以其正比例函数k×κ_max来量化其产生的代价；轨迹距离障碍物的最近距离越小，则轨迹风险越高，代价应越大，以该障碍物最近距离的倒数

来量化其代价；轨迹目标点的横向偏移与上一次规划轨迹目标点的横向偏移相差得越大，则车辆行驶越不平稳，代价应越大，以轨迹目标点与上一次所选轨迹的目标点的关于上一次目标车道的横向偏差的正比例函数k×Δ_lane来量化其代价；

步骤4.1.2各量化组成项的统一规范化方法

在步骤4.1.1中确定了一条轨迹需考虑的评价的方面及其每个方面的基本量化形式，将每个方面都统一在一起形成一个规范的统一量化标准，对步骤4.1.1中的每个量化项进行加权统一，得到以下轨迹代价量化公式：

步骤4.1.3选择出代价最小的轨迹

通过步骤4.1.2计算出每条轨迹的代价以后，选取其中代价最小的轨迹，将其作为车辆的执行轨迹供无人车行驶；

步骤4.2局部路径的保持，包括：

步骤4.2.1将规划出的路径轨迹投影至全局坐标系中，设为预保持路径，结合动态障碍物信息对预保持路径进行速度规划；

步骤4.2.2根据当前车速计算车辆安全保持距离；

步骤4.2.3将车辆安全保持距离内的预保持路径段设为保持路径，若保持路径突然出现静态障碍物，则减速并返回步骤4.2.2；

步骤4.2.4取保持路径的末端点为下一次规划起点进行规划，将规划出的路径与保持路径拼接得到新的预保持路径；若规划区域出现障碍物，则规划出避障路径；

如此循环以上步骤4.2.1、步骤4.2.2、步骤4.2.3、步骤4.2.4步骤。

Images