CN112964271B - 一种面向多场景的自动驾驶规划方法及系统 - Google Patents

Abstract

本发明公开了一种面向多场景的自动驾驶规划方法及系统，方法包括：S1，获取全局参考路线；S2，根据是否已有最优轨迹、自车位置以及全局参考路线生成局部参考路线：若无最优轨迹，根据自车位置和全局参考路线进行路径点采样得到第一局部参考路线，同时利用混合A*算法搜索生成对应交通路段的参考路线并拼接整合到第一局部参考路线中；在考虑车辆运动学约束和障碍物约束条件下对参考路线进行平滑处理；若已有最优轨迹，根据自车位置和全局参考路线进行路径点采样得到第一局部参考路线，并进行平滑处理；S3，根据自车位置、障碍物信息和最终的局部参考路线获取最优轨迹；S4，循环进行S2‑S3。本发明使得自动驾驶车辆能够更鲁棒的应对多样化的交通场景。

Description

一种面向多场景的自动驾驶规划方法及系统

技术领域

本发明属于自动驾驶汽车技术领域，具体涉及一种面向多场景的自动驾驶规划方法及系统。

背景技术

路径规划或者说运动轨迹规划是自动驾驶车辆的基本技术之一，在考虑车辆的运动模型和周围的障碍物的情况下，路径规划会生成一系列状态，使得车辆从初始状态转换到期望状态。随着目标位置和交通环境的变化，自动驾驶汽车需要采用不同的行为来完成时变的驾驶任务。当在城市地区驾驶时，路面障碍物容易检测和跟踪，交通场景相对规范和简单，通过跟随或改变行车道和超车前方障碍物来导航自动驾驶车辆就足够了。当道路被交通基础设施重新改造，或者要求车辆到达指定位置时，简单的通过跟随或改变行车道和超车前方障碍物难以完成任务，需要利用更复杂和精确的规划算法来实现这一任务。

近年来，机器人学文献中提出了许多路径规划方法。但这些规划方法都只在特定的交通场景下或部分交通场景下表现出较好的效果，无法同时有效应对绝大多数的交通场景。对于结构化的城市交通场景，基于高精度地图规划出的全局参考路线与真实交通场景的可行驶路线偏差较小，从全局参考路线中提取局部参考路线并通过基于采样的规划算法就可以较好的满足自动驾驶轨迹规划的需求。但在例如在停车场、窄路掉头、施工占道等交通场景下，基于高精度地图规划出的全局参考路线和真实的可行驶路线存在较大的偏离，在这些场景下如果继续根据基于高精度地图规划的全局参考路线做轨迹规划，基于采样的规划算法所规划出的轨迹将变得不可行。

发明内容

本发明的目的在于克服上述现有技术的缺点，提出一种面向多场景的自动驾驶规划方法及系统，利用不同的参考路线生成方法应对不同的交通场景，生成一条在当前路段高度符合真实可行驶路线的局部参考路线，通过基于采样的规划算法就能在全路段下规划出可行的运动轨迹，使得自动驾驶车辆能够更鲁棒的应对多样化的交通场景。

本发明的目的采用以下技术方案实现：

一种面向多场景的自动驾驶规划方法，包括如下步骤：

S1，对地图拓扑信息进行解析，利用解析的结果获取全局参考路线；

S2，根据是否已有最优轨迹、自车位置以及全局参考路线生成局部参考路线，过程包括；

若无最优轨迹，则根据自车位置和全局参考路线进行路径点采样得到第一局部参考路线，同时利用混合A*算法搜索生成对应交通路段的参考路线；将参考路线拼接整合到第一局部参考路线中，得到完整的局部参考路线；在考虑车辆运动学约束和障碍物约束条件下对参考路线进行平滑处理，得到最终的局部参考路线；

若已有最优轨迹，则根据自车位置和全局参考路线进行路径点采样得到第一局部参考路线，并对第一局部参考路线进行平滑处理，得到最终的局部参考路线；

S3，根据自车位置、障碍物信息和最终的局部参考路线获取最优轨迹；

S4，循环进行S2-S3。

优选的，S1的过程包括：

对地图拓扑信息进行解析，得到包含道路物理信息和语义信息的路网；

根据路网信息、自车定位信息以及任务起点和目标点进行全局路线搜索，生成一组有前后关系的全局离散路线点，利用全局离散路线点生成全局参考路线。

优选的，S2中，利用混合A*算法搜索生成对应交通路段的参考路线时，将栅格分辨率采用距离自车位置近处较高、距离自车位置远处较低的形式。

优选的，S2中，利用混合A*算法搜索生成对应交通路段的参考路线时，确定自车相对全局路径的点，在距该点预设距离的范围内由远及近向自车相对全局路径的点回推，结合栅格图，寻找局部目标点，所述局部目标点满足：在栅格图范围内，并且这个点所在栅格和周围预设距离范围内没有障碍物；

如果直至回推到自车相对全局路径的点时未找到局部目标点，则将自车正前方某一个周围没有障碍物的点作为局部目标点。

优选的，S2中，将参考路线拼接整合到第一局部参考路线中的过程包括：

选取局部参考路线中的起始点和终止点作为拼接点，根据起始点和终止点查找第一局部参考路线中的对应的最近点并进行拼接。

优选的，S3中，根据自车定位信息计算最终的局部参考路线上的起始规划点，然后将局部参考路线中的笛卡尔坐标转换为Frenet坐标；

计算自动驾驶车辆在Frenet坐标系下的起始点以及起始点的状态信息，起始点的状态信息表示为T0时刻状态；

在采样之前，先对障碍物信息进行处理，对障碍物进行处理时基于参考路线建立对应的SL图(即纵向位移-横向位移图)和ST图(即纵向位移-时间图)；

障碍物信息处理完成后，开始采样下一个T1时刻的末状态，表示为T1时刻状态；

采样之后，将T1时刻状态和T0时刻状态做多项式拟合，生成横向和纵向的两个多项式轨迹，将横向和纵向的两个多项式轨迹进行二维合成，得到轨迹集；

从轨迹集中找到最优轨迹。

优选的，从轨迹集中找到最优轨迹的过程包括：

计算轨迹集中每一条轨迹的代价C_total：

对代价最低的轨迹进行物理限制检测和碰撞检测，若同时满足车辆运动约束条件和无碰撞这两种条件，则将Frenet坐标系下的轨迹转换成笛卡尔坐标系下的轨迹并返回该轨迹；若不满足车辆运动约束条件和无碰撞条件中至少一个条件，则将该轨迹剔除，再选择最低代价的轨迹继续检测，直到找到同时满足上述两个条件的轨迹；

如果未找到符合条件的轨迹，然后进行S4。

优选的：轨迹集中每一条轨迹的代价C_total通过下式计算：

C_total＝ω_{lon_obj}*C_{lon_obj}+ω_{lon_jerk}*C_{lon_jerk}+ω_collision*C_collision+ω_{lat_offset}*C_{lat_offset}

其中，ω_{lon_obj}、ω_{lon_jerk}、ω_collision和ω_{lat_offset}分别为错过目标的权重、纵向加加速度的权重、碰撞的权重和横向偏移的权重，C_{lon_obj}、C_{lon_jerk}、C_collision和C_{lat_offset}分别为错过目标代价、纵向加加速度代价、碰撞代价、横向偏移代价。

优选的，自车位置通过读取车上的GPS定位和激光SLAM定位获取。

本发明提供了一种面向多场景的自动驾驶规划系统，用于实现本发明如上所述面向多场景的自动驾驶规划方法，该系统包括：

全局规划模块：用于对地图拓扑信息进行解析，获取全局参考路线，并输出全局参考路线；

参考路线提供模块：用于根据是否已有最优轨迹、自车位置以及全局参考路线生成局部参考路线，过程包括；

若无最优轨迹，则根据自车位置和全局参考路线进行路径点采样得到第一局部参考路线，同时利用混合A*算法搜索生成对应交通路段的参考路线；将参考路线拼接整合到第一局部参考路线中，得到完整的局部参考路线；在考虑车辆运动学约束和障碍物约束条件下对参考路线进行平滑处理，得到最终的局部参考路线；

若已有最优轨迹，则根据自车位置和全局参考路线进行路径点采样得到第一局部参考路线，并对第一局部参考路线进行平滑处理，得到最终的局部参考路线；

并输出局部参考路线；

局部规划模块：用于根据自车位置、障碍物信息和最终的局部参考路线获取最优轨迹，并输出是否已有最优轨迹的结果。

相比现有技术，本发明的有益效果在于：

本发明的面向多场景的自动驾驶规划方法，针对如停车场、窄路掉头、施工占道这些复杂场景采用混合A*算法生成高度符合真实交通可行驶路线，并将其拼接整合到基于高精度地图规划出的局部参考路线中，使得生成的参考路线高度相似于交通场景实际可行驶路线，再通过基于采样的方法进行运动轨迹规划，增强了规划算法的鲁棒性，使得自动驾驶车辆能够有效应对复杂的、多样化的交通场景。

附图说明

图1为本发明全局规划流程示意图；

图2为本发明半结构化场景示意图；

图3为本发明方法中行为Frenet坐标系示意图；

图4为本发明方法中横向轨迹示意图；

图5为本发明方法中纵向轨迹示意图。

具体实施方式

下面结合附图和实施例对本发明做进一步详细描述：

参照图1、图3-图5，本发明面向多场景的自动驾驶规划方法包括如下步骤：

S1：全局规划模块输出包含高精度地图语义信息以及拓扑信息的全局参考路线；

S1包括以下子步骤：

S11：对地图拓扑信息进行解析，得到一个包含道路物理信息和语义信息的路网；

S12：根据起始点和终点位置进行全局路线搜索，得到一条初始的全局参考路线。

S2：参考路线提供模块根据全局参考路线和自车位置向局部规划模块输出局部参考路线；

S2包括以下子步骤：

S21：读取GPS定位和激光SLAM定位，确定自车位置；

S22：根据自车车辆状态、上一帧局部规划模块状态以及全局参考路线生成一条局部参考路线输出给局部规划模块。

步骤S22包括：如果上一帧局部规划模块状态返回成功(规划起始第一帧默认为成功)，则根据自车位置和全局参考路线进行路径点采样得到一条局部参考路线PATH_1，将PATH_1进行平滑处理之后发送给局部规划模块。如果上一帧局部规划模块的模块状态返回失败，首先根据自车位置和全局参考路线进行路径点采样得到一条局部参考路线PATH_1，同时利用混合A*算法搜索生成对应交通路段的参考路线PATH_2；然后将PATH_2拼接整合到PATH_1中，得到一条完整的参考路线；最后在考虑车辆运动学约束和障碍物约束条件下对参考路线进行平滑处理，并将平滑后的参考路线发送给局部规划模块。

所述子步骤S22中的PATH_1和PATH_2的拼接整合过程具体包括以下子步骤：

S221：选取PATH_2中的起始点和终止点作为拼接点，确保PATH_1全段保留；

S222：根据S241中的起始点和终止点查找PATH_1中的最近点；

S223：根据两个最近点和相关参数选择PATH_1上的前后两个拼接点；

S224：采用3次样条插值，拼接PATH_1前端、PATH_2全段、PATH_1末端，形成一条完整的参考路线。

S3：局部规划模块使用基于Frenet坐标系下的规划方法生成一条可行的、安全的运动轨迹；

S3具体包括以下子步骤：

S31：根据自车位置计算局部参考路线上起始规划点，并初始化Frenet信息；

S32：根据障碍物信息基于局部参考路线建立对应的SL图和ST图；

S33：基于采样的方法生成轨迹集；

S34：计算每一条轨迹的代价；

S35：循环检测，返回符合条件的运动轨迹；若无符合条件的运动轨迹，则跳转步骤S2，重新规划参考路线。

实施例

如图1所示，本实施例面向多场景的自动驾驶规划方法包含以下步骤：

S1：全局规划模块输出包含高精度地图语义信息以及拓扑信息的全局参考路线：

如图1所示，全局规划模块首先是对地图拓扑信息进行解析，主要包括车道相关信息和车道拓扑关系，解析得到一个包含道路物理信息和语义信息的路网。根据路网信息、定位信息以及任务起点和目标点进行全局路线搜索。搜索算法采用Dijkstra算法，初步确定需要经过的路段，再根据连接关系确定路段内的路线，生成一组有前后关系的全局离散路线点，根据该组有前后关系的全局离散路线点生成全局参考路线。

S2：参考路线提供模块根据自车位置和全局参考路线向局部规划模块输出参考路线：

参考路线提供模块的流程主要包括：读取GPS定位和激光SLAM定位，确定自车位置；根据车辆状态、上一帧局部规划模块状态以及全局参考路线生成一条局部参考路线输出给局部规划模块：如果上一帧局部规划模块状态返回成功(规划起始第一帧默认为成功)，则根据自车位置和全局参考路线进行路径点采样得到一条局部参考路线PATH_1，将PATH_1进行平滑处理之后发送给局部规划模块。如果上一帧局部规划模块状态返回失败(一般发生在停车场、施工占道等半结构化场景中，如图2所示)，首先根据自车位置和全局参考路线进行路径点采样得到一条局部参考路线PATH_1，同时利用混合A*算法搜索生成对应交通路段的参考路线PATH_2；然后将PATH_2拼接整合到PATH_1中，得到一条完整的参考路线；最后在考虑车辆运动学约束和障碍物约束条件下对参考路线进行平滑处理，并将平滑后的参考路线发送给局部规划模块。

混合A*算法的输入是激光雷达检测得到的障碍物栅格图，以及车辆的初始状态s₀＝〈x,y,θ〉₀和目标状态s_g＝〈x,y,θ〉_g，其中<x,y,θ>分别是车辆的位置和车头朝向。期望的输出是安全平滑并且满足汽车运动学约束(转弯半径)的轨迹(车辆状态序列s₀,s₁,...s_n＝s_g，具有特定分辨率δ_s(||s_i-s_i-1||≤δ_s)。本发明对混合A*方法进行了改进和补充。其一是根据感知范围的特性，本发明更改了栅格的分辨率，将栅格分辨率改为近处高远处低，这样能够加速搜索速度，同时在车身周围仍保留有较高路径精度。其二是对在全局路径确定的情况下如何确定一次搜索的局部目标进行了分析描述。全局参考路径指明了大致的行驶方向，但是由于现实中随时会出现路况的变化和新的障碍物出现，所以需要结合全局参考路径和车辆周围的障碍物情况决定一次混合A*算法规划的局部终点。本发明的做法是首先确定自身车辆相对全局路径的点，从距离该点100米远的距离(即从第200个点)开始回推，结合栅格图，尝试找到第一个这样的点：即在栅格图范围内，并且这个点所在栅格和周围2～5米范围内都没有障碍物。将这个点作为局部目标点。如果一直回推到自身车辆所在的点都没有找到局部目标点，则尝试使用车辆正前方某一个周围没有障碍物的点作为局部目标点。

两条参考路径拼接成参考路线的处理过程包括，选取混合A*规划出的路径PATH_2中的起始点和终止点作为拼接点，根据起始点和终止点查找全局参考路径上提取的参考路线PATH_1中的最近点。对于PATH_1中的拼接点选取，留取1.5～2.5米的拼接间隔，以保证经过插值和平滑处理后的路径较好的接近真实环境。本申请所用的插值方法为三次样条插值，若具体需要也可采用更高阶的样条插值方法。

S3：局部规划模块使用基于Frenet坐标系下的规划方法生成一条可行的、安全的运动轨迹

在确定了参考路线之后，首先要做的是根据自车GPS定位计算参考路线上的起始规划点(即每次循环轨迹规划的起点)，然后将参考路线中的笛卡尔坐标(X-Y)转换为Frenet坐标(S-L，纵向位移-横向位移)。Frenet坐标系如图3所示，S代表车辆在Frenet坐标系下的纵向位移量，即沿参考路线上距离；L代表车辆在Frenet坐标系的横向位移量，即从参考路线到左右位置的距离。转换坐标之后，计算自动驾驶车辆在Frenet坐标系下的起始点以及起始点的状态信息，表示为T0时刻状态。在采样之前，需要先对障碍物进行处理，我们采用的方法是基于参考路线建立对应的SL图和ST图。障碍物信息处理完成后，开始采样下一个T1时刻的末状态，表示为T1时刻状态。采样之后，将T1时刻状态和T0时刻状态做多项式拟合，生成横向和纵向的两个多项式轨迹，如图4和图5所示。最后，将横向和纵向的两个多项式轨迹进行二维合成。根据上述，生成轨迹过程包括：给定一系列时刻，计算车辆这一系列时刻的纵向和横向的偏移量，根据参考路线生成一系列二维平面的轨迹点。通过一系列的时间点，计算得到一系列的轨迹点，然后生成一个轨迹集。得到了轨迹集，开始计算每一条轨迹的代价，这个代价主要考虑了轨迹的可行性、舒适性和安全性：

C_total＝ω_{lon_obj}*C_{lon_obj}+ω_{lon_jerk}*C_{lon_jerk}+ω_collision*C_collision+ω_{lat_offset}*C_{lat_offset}，其中，ω_{lon_obj}、ω_{lon_jerk}、ω_collision和ω_{lat_offset}分别为错过目标的权重、纵向加加速度的权重、碰撞的权重和横向偏移的权重，C_{lon_obj}、C_{lon_jerk}、C_collision和C_{lat_offset}分别为错过目标代价、纵向加加速度代价、碰撞代价、横向偏移代价。

最后一步，找到最优轨迹，这是一个对轨迹进行循环检测的过程。在这个过程中，首先对代价最低的轨迹进行物理限制检测和碰撞检测。若同时满足这两种条件，则将该Frenet坐标系下的轨迹转换成笛卡尔坐标系下的轨迹并返回该轨迹；若不满足任意一个条件，则将该轨迹剔除，再选择最低代价的轨迹继续检测，直到找到同时满足两个条件的轨迹。如果无法找到符合条件的运动轨迹，则将局部规划模块状态设置为失败，然后跳转到参考路线提供模块重新生成操参考路线。

综上所述，本发明面向多场景的自动驾驶规划方法，用于导航自动驾驶车辆。将停车场、窄路掉头、施工占道等半结构化场景和城市道路、高架桥等结构化场景的参考路线有效的整合到一起，保证了参考路线与实际可行驶路线的高度相似，然后通过同一个算法框架进行运动轨迹的规划，大大增强了规划方法的鲁棒性，使得自动驾驶车辆能够有效应对复杂的、多样化的交通场景。

Claims (7)

1.一种面向多场景的自动驾驶规划方法，其特征在于，包括如下步骤：

S1，对地图拓扑信息进行解析，利用解析的结果获取全局参考路线；

S2，根据是否已有最优轨迹、自车位置以及全局参考路线生成局部参考路线，过程包括；

若无最优轨迹，则根据自车位置和全局参考路线进行路径点采样得到第一局部参考路线，同时利用混合A*算法搜索生成对应交通路段的参考路线；将参考路线拼接整合到第一局部参考路线中，得到完整的局部参考路线；在考虑车辆运动学约束和障碍物约束条件下对参考路线进行平滑处理，得到最终的局部参考路线；

若已有最优轨迹，则根据自车位置和全局参考路线进行路径点采样得到第一局部参考路线，并对第一局部参考路线进行平滑处理，得到最终的局部参考路线；

S3，根据自车位置、障碍物信息和最终的局部参考路线获取最优轨迹；

S4，循环进行S2-S3；

S1的过程包括：

对地图拓扑信息进行解析，得到包含道路物理信息和语义信息的路网；

根据路网信息、自车定位信息以及任务起点和目标点进行全局路线搜索，生成一组有前后关系的全局离散路线点，利用全局离散路线点生成全局参考路线；

S2中，利用混合A*算法搜索生成对应交通路段的参考路线时，确定自车相对全局路径的点，在距该点预设距离的范围内由远及近向自车相对全局路径的点回推，结合栅格图，寻找局部目标点，所述局部目标点满足：在栅格图范围内，并且这个点所在栅格和周围预设距离范围内没有障碍物；

如果直至回推到自车相对全局路径的点时未找到局部目标点，则将自车正前方某一个周围没有障碍物的点作为局部目标点；

S3中，根据自车定位信息计算最终的局部参考路线上的起始规划点，然后将局部参考路线中的笛卡尔坐标转换为Frenet坐标；

计算自动驾驶车辆在Frenet坐标系下的起始点以及起始点的状态信息，起始点的状态信息表示为T0时刻状态；

在采样之前，先对障碍物信息进行处理，对障碍物进行处理时基于参考路线建立对应的纵向位移-横向位移图和纵向位移-时间图；

障碍物信息处理完成后，开始采样下一个T1时刻的末状态，表示为T1时刻状态；

采样之后，将T1时刻状态和T0时刻状态做多项式拟合，生成横向和纵向的两个多项式轨迹，将横向和纵向的两个多项式轨迹进行二维合成，得到轨迹集；

从轨迹集中找到最优轨迹。

2.根据权利要求1所述的一种面向多场景的自动驾驶规划方法，其特征在于，S2中，利用混合A*算法搜索生成对应交通路段的参考路线时，将栅格分辨率采用距离自车位置近处较高、距离自车位置远处较低的形式。

3.根据权利要求1所述的一种面向多场景的自动驾驶规划方法，其特征在于，S2中，将参考路线拼接整合到第一局部参考路线中的过程包括：

选取局部参考路线中的起始点和终止点作为拼接点，根据起始点和终止点查找第一局部参考路线中的对应的最近点并进行拼接。

4.根据权利要求1所述的一种面向多场景的自动驾驶规划方法，其特征在于，从轨迹集中找到最优轨迹的过程包括：

计算轨迹集中每一条轨迹的代价C_total：

对代价最低的轨迹进行物理限制检测和碰撞检测，若同时满足车辆运动约束条件和无碰撞条件，则将Frenet坐标系下的轨迹转换成笛卡尔坐标系下的轨迹并返回该轨迹；若不满足车辆运动约束条件和无碰撞条件中至少一个条件，则将该轨迹剔除，再选择最低代价的轨迹继续检测，直到找到同时满足上述两个条件的轨迹；

如果未找到符合条件的轨迹，然后进行S4。

5.根据权利要求4所述的一种面向多场景的自动驾驶规划方法，其特征在于：

轨迹集中每一条轨迹的代价C_total通过下式计算：

C_total＝ω_{lon_obj}*C_{lon_obj}+ω_{lon_jerk}*C_{lon_jerk}+ω_collision*C_collision+ω_{lat_offset}*C_{lat_offset}

其中，ω_{lon_obj}、ω_{lon_jerk}、ω_collision和ω_{lat_offset}分别为错过目标的权重、纵向加加速度的权重、碰撞的权重和横向偏移的权重，C_{lon_obj}、C_{lon_jerk}、C_collision和C_{lat_offset}分别为错过目标代价、纵向加加速度代价、碰撞代价、横向偏移代价。

6.根据权利要求1所述的一种面向多场景的自动驾驶规划方法，其特征在于，自车位置通过读取车上的GPS定位和激光SLAM定位获取。

7.一种面向多场景的自动驾驶规划系统，其特征在于，用于实现权利要求1-6任意一项所述面向多场景的自动驾驶规划方法，该系统包括：

全局规划模块：用于对地图拓扑信息进行解析，获取全局参考路线，并输出全局参考路线；

参考路线提供模块：用于根据是否已有最优轨迹、自车位置以及全局参考路线生成局部参考路线，过程包括；

若无最优轨迹，则根据自车位置和全局参考路线进行路径点采样得到第一局部参考路线，同时利用混合A*算法搜索生成对应交通路段的参考路线；将参考路线拼接整合到第一局部参考路线中，得到完整的局部参考路线；在考虑车辆运动学约束和障碍物约束条件下对参考路线进行平滑处理，得到最终的局部参考路线；

若已有最优轨迹，则根据自车位置和全局参考路线进行路径点采样得到第一局部参考路线，并对第一局部参考路线进行平滑处理，得到最终的局部参考路线；

并输出局部参考路线；

局部规划模块：用于根据自车位置、障碍物信息和最终的局部参考路线获取最优轨迹，并输出是否已有最优轨迹的结果。

Images