CN115079702A - 一种混合道路场景下的智能车辆规划方法和系统 - Google Patents

Abstract

本发明公开了一种混合道路场景下的智能车辆规划方法和系统，其包括：步骤1，规划原始全局轨迹；步骤2，判断车辆与原始全局轨迹上存在碰撞风险的障碍物之间的距离是否小于最小安全距离，若是，则车辆进入紧急制动；否则设置安全停车点，进行重规划轨迹；重规划轨迹具体包括：步骤21，在障碍物后方的原始全局轨迹上获取采样点；步骤22，采样避障引导点；步骤23，计算规划完成时车辆的未来位姿点；步骤24，以未来位姿点向避障引导点进行路径搜索，规划第一路径和第二路径；步骤25，将第一路径和第二路径对应的轨迹分别与原始全局轨迹进行拼接，输出重规划轨迹。本发明能够提升混合道路场景下车辆行驶安全性与通行效率。

Description

一种混合道路场景下的智能车辆规划方法和系统

技术领域

本发明涉及智能车辆规划技术领域，特别是关于一种混合道路场景下的智能车辆规划方法和系统。

背景技术

路径规划是智能驾驶系统中一个关键的模块，其目标是在最小化设定路径评价函数的前提下，找到满足车辆运动学约束且无碰撞的曲线。高效、安全的路径规划策略是保证自动驾驶车辆行车安全性和通行效率的关键。

露天矿山等道路场景是典型的结构化与非结构化道路并存的混合道路场景，混合道路场景的环境复杂多变，当车辆按照初次规划的全局轨迹行驶时，会出现新增障碍物使原始全局轨迹失效的情况，需动态进行轨迹重规划。另外，在原始全局轨迹失效时，车辆仍然以一定的速度在移动，如果重规划过程较慢，那么当规划完成时，车辆会偏离规划器重规划的起点，导致重规划的轨迹失效。如何匹配重规划起点与车辆的实际位置，是重规划过程的重中之重。

目前处理这种问题的方法主要有两种，一种是基于采样的lattice算法，通过采样多条候选轨迹，最终选择代价值最小的轨迹作为重规划轨迹。另一种是障碍物出现时，采用数值优化的方法规划发现障碍物时刻的车身位置到终点的避障轨迹，同时在避障轨迹与原始全局轨迹间生成连接轨迹，再按照一定条件拼接三条轨迹形成重规划轨迹。这些技术都没有最大限度地利用原始规划轨迹，从而导致最终生成的重规划轨迹与原始规划轨迹有较大的偏差，容易绕道严重，进而降低道路通行率。

发明内容

本发明的目的在于提供一种混合道路场景下的智能车辆规划方法和系统，其能够提升混合道路场景下车辆行驶安全性与通行效率。

为实现上述目的，本发明提供一种混合道路场景下的智能车辆规划方法，其包括：步骤1，根据车辆的驾驶任务及其对应的地图的信息、车辆周围环境的信息和车辆定位的信息，规划原始全局轨迹；

步骤2，判断车辆与原始全局轨迹上存在碰撞风险的障碍物之间的距离是否小于最小安全距离，若是，则车辆进入紧急制动；否则设置安全停车点，车辆进入平滑制动模式，并进行重规划轨迹，以在达到安全停车点时速度为0；

其中，重规划轨迹具体包括：

步骤21，在障碍物后方的原始全局轨迹上获取采样点；

步骤22，在障碍物处设置辅助线，再在辅助线上采样避障引导点；

步骤23，计算规划完成时车辆的未来位姿点；

步骤24，以未来位姿点向避障引导点进行路径搜索，规划第一路径，并在所述第一路径规划成功的情形下，以避障引导点向步骤21获取的采样点进行路径搜索，规划第二路径；

步骤25，将第一路径和第二路径对应的轨迹分别与原始全局轨迹进行拼接，输出重规划轨迹；

其中，步骤22中的“在障碍物处设置辅助线，再在辅助线上采样避障引导点”的方法具体包括：

步骤221，计算突然出现的障碍物占据原始全局轨迹的曲线段的第一中点的坐标；

步骤222，以第一中点作垂直于原始全局轨迹的辅助线，获得辅助线与静态障碍物的两个交点均为障碍物交点；

步骤223，将步骤221中的障碍物的边界点向地图边界线作投影，得到两投影点；

步骤224，计算一障碍物交点与一投影点之间的第一距离和另一障碍物交点与另一投影点之间的第二距离，并将第一距离和第二距离中更大的一侧的障碍物交点与投影点的连接线段的中点作为避障引导点；

步骤225，判断避障引导点与其同侧的投影点的第三距离是否大于两倍车辆宽度，如果是,则将避障引导点移至辅助线上，且与避障引导点同侧的投影点的第三距离为两倍车辆宽度的位置处，此时的避障引导点与辅助线垂直。

进一步地，步骤23具体包括：

步骤231，初始化重规划次序i=1；

步骤232，按照下式分别计算第i次规划完成时刻t_i、速度v_i和曲线距离S_0i，确定未来位姿点P_i的信息（X_i,Y_i,θ_i）：

t_i=t₀+n·i·T_think

v_i=v₀-n·a₀·i·T_think

S_0i=v₀ ²-v_i ²/2a₀

其中，（X_i,Y_i）和θ_i分别表示在原始全局轨迹上获得第i次重规划的未来位姿点P_i的坐标和车辆航向角，T_think为预设的完成一次路径搜索的最大重规划时长，v₀为车辆在障碍物出现的时刻t₀的行驶速度，v_i为车辆在时刻t_i的行驶速度，n为预设的重规划总次数，S_0i为未来位姿点P_i与时刻t₀所在位置P₀的曲线距离。

进一步地，在障碍物和车辆均在道路内部情形下，步骤24具体包括：

步骤241，加载待规划区域的地图的信息，判断曲线距离S_0i是否大于距离S₀与S_安之差，如果是，则车辆进入紧急制动状态；反之，以未来位姿点P_i向避障引导点进行路径搜索，并将搜索获得的第一路径存储到第一容器中；其中，S₀为车辆在t₀时刻与障碍物的距离，S_安为安全停车点与障碍物之间的距离；

步骤242，判断第一容器是否为空集，如果是，则确定为第一路径规划失败，并令i=i+1，跳转步骤232，否则确定为第一路径规划成功，并进入步骤243；

步骤243，以避障引导点分别向步骤21获取的采样点进行路径搜索，并将搜索获得第二路径存储到第二容器中；

步骤244，判断第二容器是否为空集，如果是，则确定为第二路径规划失败，并令i=i+1，跳转步骤232；否则确定为第二路径规划成功。

进一步地，在障碍物处于非结构化和结构化道路的交界处情形下，步骤24具体包括：

步骤241，将非结构化道路和结构化道路的地图进行融合，生成新地图，并加载新地图的信息；

步骤242，判断曲线距离S_0i是否大于距离S₀与S_安之差，如果是，则车辆进入紧急制动状态；反之，以步骤23确定的未来位姿点向避障引导点进行路径搜索，并将搜索获得的第一路径存储到第一容器中；其中，S₀为车辆在t₀时刻与障碍物的距离，S_安为安全停车点与障碍物之间的距离；

步骤243，判断第一容器是否为空集，如果是，则确定为第一路径规划失败，并令i=i+1，跳转步骤232，否则确定为第一路径规划成功，并进入步骤244；

步骤244，以避障引导点分别向各个采样点进行路径搜索，并将搜索获得第二路径存储到第二容器中；

步骤245，判断第二容器是否为空集，如果是，则确定为第二路径规划失败，并令i=i+1，跳转步骤232；否则确定为第二路径规划成功。

进一步地，步骤25具体包括：

步骤251，将第二容器中第一路径对应的轨迹与第一容器中的第二路径对应的轨迹进行拼接，并将拼接好的轨迹存入第三容器中；

步骤252，遍历第三容器中的所有拼接好的轨迹，选取其中曲率最小的拼接好的轨迹与原始全局轨迹进行再次拼接，并对再次拼接的拼接点进行数值优化，获得新的全局轨迹。

本发明还提供一种混合道路场景下的智能车辆规划系统，其包括：

任务规划单元，其用于提供驾驶任务的信息；

地图单元，其用于为驾驶任务构建地图的信息；

感知单元，其用于获取车辆周围环境的信息；

定位单元，其用于采集车辆定位的信息；

全局路径规划单元，其用于根据车辆的驾驶任务及其对应的地图信息、车辆周围环境信息和车辆定位信息，规划原始全局轨迹；

重规划单元，其用于判断车辆与原始全局轨迹上存在碰撞风险的障碍物之间的距离是否小于最小安全距离，若是，则车辆进入紧急制动；否则设置安全停车点，车辆进入平滑制动模式，并进行重规划轨迹，以在达到安全停车点时速度为0；

其中，重规划单元具体包括：

采样点采样子单元，其用于在障碍物后方的原始全局轨迹上获取采样点；

避障引导点采样子单元，其用于在障碍物处设置辅助线，再计算突然出现的障碍物占据原始全局轨迹的曲线段的第一中点的坐标，并以第一中点作垂直于原始全局轨迹的辅助线，获得辅助线与静态障碍物的两个交点均为障碍物交点，然后将障碍物的边界点向地图边界线作投影，得到两投影点，计算一障碍物交点与一投影点的第一距离和另一障碍物交点与另一投影点的第二距离，并将第一距离和第二距离中更大的一侧的障碍物交点与投影点的连接线段的中点作为避障引导点，最后判断避障引导点与其同侧的投影点的第三距离是否大于两倍车辆宽度，如果是,则将避障引导点移至辅助线上，且与避障引导点同侧的投影点的第三距离为两倍车辆宽度的位置处，此时的避障引导点与辅助线垂直；

未来位姿点计算子单元，其用于计算规划完成时车辆的未来位姿点；

路径搜索子单元，其用于以未来位姿点向避障引导点进行路径搜索，规划第一路径，并在所述第一路径规划成功的情形下，以避障引导点向采样点进行路径搜索，规划第二路径；

重规划轨迹输出子单元，其用于将第一路径和第二路径对应的轨迹分别与原始全局轨迹进行拼接，输出重规划轨迹。

进一步地，未来位姿点计算子单元具体包括：

姿态信息计算模块，其用于按照下式分别计算第i次规划完成时刻t_i、速度v_i和曲线距离S_0i，确定未来位姿点P_i的信息（X_i,Y_i,θ_i）：

t_i=t₀+n·i·T_think

v_i=v₀-n·a₀·i·T_think

S_0i=v₀ ²-v_i ²/2a₀

其中，（X_i,Y_i）和θ_i分别表示在原始全局轨迹上获得的时刻t_i的未来位姿点P_i的坐标和车辆航向角，T_think为预设的完成一次路径搜索的最大重规划时长，v₀为车辆在障碍物出现的时刻t₀的行驶速度，v_i为车辆在时刻t_i的行驶速度，n为预设的重规划总次数，S_0i为未来位姿点P_i与时刻t₀所在位置P₀的曲线距离。

进一步地，在障碍物和车辆均在道路内部情形下，路径搜索子单元具体包括：

第一路径搜索模块，其用于加载待规划区域的地图的信息，判断曲线距离S_0i是否大于距离S₀与S_安之差，如果是，则车辆进入紧急制动状态；反之，以未来位姿点向避障引导点进行路径搜索，并将搜索获得的第一路径存储到第一容器中；其中，S₀为车辆在t₀时刻与障碍物的距离，S_安为安全停车点与障碍物之间的距离；

第一路径判断模块，其用于判断第一容器是否为空集，如果是，则确定为第一路径规划失败，并由姿态信息计算模块计算第i+1次规划的未来位姿点的信息，否则确定为第一路径规划成功；

第二路径搜索模块，其用于确定为第一路径规划成功后，以避障引导点分别向各个采样点进行路径搜索，并将搜索获得第二路径存储到第二容器中；

第二路径判断模块，其用于判断第二容器是否为空集，如果是，则确定为第二路径规划失败，并由姿态信息计算模块计算第i+1次规划的未来位姿点的信息，否则确定为第二路径规划成功。

进一步地，在障碍物处于非结构化和结构化道路的交界处情形下，路径搜索子单元具体包括：

地图融合模块，其用于将非结构化道路和结构化道路的地图进行融合，生成新地图，并加载新地图的信息；

第一路径搜索模块，其用于判断曲线距离S_0i是否大于距离S₀与S_安之差，如果是，则车辆进入紧急制动状态；反之，以未来位姿点向避障引导点进行路径搜索，并将搜索获得的第一路径存储到第一容器中；其中，S₀为车辆在t₀时刻与障碍物的距离，S_安为安全停车点与障碍物之间的距离；

第一路径判断模块，其用于判断第一容器是否为空集，如果是，则确定为第一路径规划失败，并由姿态信息计算模块计算第i+1次规划的未来位姿点的信息，否则确定为第一路径规划成功；

第二路径搜索模块，其用于确定为第一路径规划成功后，以避障引导点分别向各个采样点进行路径搜索，并将搜索获得第二路径存储到第二容器中；

第二路径判断模块，其用于判断第二容器是否为空集，如果是，则确定为第二路径规划失败，并由姿态信息计算模块计算第i+1次规划的未来位姿点的信息，否则确定为第二路径规划成功。

进一步地，重规划轨迹输出子单元具体包括：

轨迹拼接模块，其用于将第二容器中第一路径对应的轨迹与第一容器中的第二路径对应的轨迹进行拼接，并将拼接好的轨迹存入第三容器中；

全局轨迹优化模块，其用于遍历第三容器中的所有拼接好的轨迹，选取其中曲率最小的拼接好的轨迹与原始全局轨迹进行再次拼接，并对再次拼接的拼接点进行数值优化，获得新的全局轨迹。

本发明由于采取以上技术方案，其具有以下优点：

本发明通过在障碍物旁提供引导点，使重规划的轨迹能够尽量靠近原始全局轨迹，很大程度利用了原始规划轨迹数据，从而降低了规划过程中计算的复杂度，能够实时稳定的完成规划。而在车辆的未来姿态采样规划起点，可以使规划完成时，车辆实际位姿在规划起点附近，解决了规划完成时车辆位姿偏离规划起点的问题。

附图说明

图1为本发明实施例提供的混合道路场景下的智能车辆规划系统的结构示意图。

图2为本发明实施例提供的非结构化道路区域的示意图。

图3为本发明实施例提供的结构化道路区域的示意图。

图4为露天矿场的连通示意图。

图5为本发明实施例提供的全局轨迹规划方法的流程图。

图6为本发明实施例提供的重规轨迹规划方法的流程图。

图7为利用本发明方法对非结构化道路重规轨迹规划的示意图。

图8为利用本发明方法对非结构化和结构化道路的交界处的重规轨迹规划的示意图。

具体实施方式

在附图中，使用相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面结合附图对本发明的实施例进行详细说明。

如图2和图3所示，本发明实施例提供的混合道路场景包括非结构化和结构化道路中的一种或两种。

非结构化道路的地图的信息包括地图编号和随着环境变化不断改变的地图边界线、边界目标点和静态障碍物信息。

结构化道路的宽度规整，变动小，几何特征也更明显，因此，结构化道路的地图的信息基本是长久不变的，可以为恒定，其中的信息包括地图编号、道路边界线、边界目标点、静态障碍物信息以及参考路径。

进一步地，地图编号表示区域在地图上对应的编号，也就是说，各区域都有不同的地图编号。地图边界线如图2和图3中的长划线点虚线，其将地图划分为不同的区域。边界目标点如图2和图3中的圆点，均处于地图边界线内，相邻两个区域的边界目标点由参考路径直接连接，当行驶到边界目标点（起点）时，车辆会按照预设的参考路径行驶到下一区域的边界目标点（终点）。

如图1所示，本发明实施例提供的混合道路场景下的智能车辆规划方法包括：

步骤1，根据车辆的驾驶任务及其对应的地图的信息、车辆周围环境的信息和车辆定位的信息，规划原始全局轨迹。

在一个实施例中，按照不同的策略对结构化道路和非结构化道路的区域进行路径规划，然后将各区域的规划结果按照连通关系依次拼接形成全局路径，再根据各路段限速和车辆加减速性能，规划路径速度，为全局路径上的每一个路径点赋一个期望速度值，最终生成一个全局轨迹。需要说明的是，路径速度规划可以采用现有方法实现，比如梯度下降法或其他现有路径速度方法。

因此，如图5所示，步骤1可以具体化为如下子步骤组成：

步骤11，输入起点和终点，并加载起点和终点的地图信息。其中，将车辆当前的实际位置作为原始全局轨迹的起点，终点由任务规划单元确定。

步骤12，再利用搜索算法，计算所经历区域的连通关系，通过规划器加载最短通行次序以及所经历的各区域的地图信息。

比如：如图4所示，搜索算法选择的是Dijkstra算法，计算所经历区域的连通关系为：图4下方圆和箭头形成的连通图，箭头表示各个区域之间的距离中最小的连通次序。当然，也可以使用现有的其它搜索算法，在此不再一一列举。

步骤13，将步骤12计算的上述连通关系所经历区域可以划分为中间区域、起点区域、终点区域和中间区域，按照如下情形为车辆所途径的每一区域确定区域路径，直至到终点区域。其中，起点区域为车辆当前所在的区域，终点区域为终点所在的区域，连通关系所途经除起点区域和终点区域之外的的区域为中间区域。

情形一：车辆所途径的区域为中间区域，且为结构化道路区域，则可以将当前待规划区域的参考路径确定为该区域路径，而不需要调用搜索算法规划路径。

情形二：车辆所途径的区域为中间区域，且为非结构化道路区域，则按照步骤12获得的最短通行次序设置起点和终点，并使用现有的混合A*算法或其他方法进行路径搜索，获得该区域路径。

情形三：车辆所途径的区域为起点区域，设置起点和终点，并使用现有的混合A*算法或其他方法进行路径搜索，获得该区域路径。

情形四：车辆所途径的区域为终点区域，则按照步骤12获得的最短通行次序设置起点和终点，并使用现有的混合A*算法或其他方法进行路径搜索，获得该区域路径。

步骤14，按照步骤12获得的最短通行次序，依次拼接路径，获得全局路径及其曲率，再根据各路段限速和车辆加减速性能，为全局路径规划速度，给每个路径点赋一个期望速度值，获得原始的全局轨迹，并输出。

下面结合图4对上述步骤1进行示例说明：

图4中，001到010是10个单独的区域，其中002、004、005、007、009为结构化的连接道路，001、003、006、008、010为停车场、卸载区等非结构化的区域。根据它们之间的连通关系，可以将每个区域设置为有向图的节点。边的大小设置规则为：各节点的边大小等于相邻的结构化道路曲线长度的一半，例如，001区域到002区域的边距离L1为002区域两个区域边界点间参考轨迹曲线长度的一半，005区域到006区域的边距离L2为005区域两个区域边界点间曲线距离的一半。其最终，“曲线距离”可以理解为沿着原始全局轨迹行驶的距离。

鉴于此，已知节点和边的大小，可以采用Dijkstra图搜索算法搜索连接起始位姿所在区域与目标位姿所在区域的最短区域通行次序。然后将最短区域通行次序中的地图加载到规划器中即可。例如，起点在001区域，终点在010区域，通过Dijkstra图搜索算法得到连通结果001→002→003→004→008→009→010，此时只需要依次加载这7个图的数据，则足以完成此次全局路径规划。

上述方法可以降低总规划时间以及地图加载的时间，结构化道路为中间区域时，无论调用多少次搜索算法，最终得到的路径都是一样的，所以没必要每次都进行一次搜索，直接预存储即可。

步骤1输出的原始全局轨迹呈现为一条平滑的曲线，连接起点和终点，并包含其上任意一点的车辆位姿点的信息，包括车辆位姿点的在大地坐标系下的（X、Y）坐标和车辆航向角θ。那么，车辆沿着原始全局轨迹行驶过程中，根据未来一段时间轨迹预测车辆未来时刻的位置，并将未来时刻的位置和障碍物位置进行碰撞检测，若存在碰撞风险，则需要重规划轨迹，即文中的步骤2。通过步骤2提供的重规划轨迹，对原始全局轨迹进行修正，以保证车辆安全地绕开所预测到的车辆在未来时刻遇到的障碍物。其中，碰撞检测可以通过现有的OBB碰撞检测算法或其他方法实现。

步骤2，如图6所示，判断车辆与原始全局轨迹上存在碰撞风险的障碍物之间的距离是否小于最小安全距离，若是，则车辆进入紧急制动；否则设置安全停车点，车辆进入平滑制动模式，并进行重规划轨迹，以在达到安全停车点时速度为0。

例如：车辆在t₀时刻以v₀的速度行驶到P₀点，车辆前方S₀处存在碰撞风险的障碍物。此时，若S₀小于预设的最小安全距离S_安，通常情况S_安=5m。那么，为了保证行车安全，设置安全停车点P_park，安全停车点P_park与障碍物之间的曲线距离为预设的最小安全距离S_安，根据公式v₀ ²=2a₀（S₀-S_安）计算加速度a₀。

如果a₀小于0或者a₀大于车辆的最大制动减速度a_break时，则判定为车辆与原始全局轨迹上存在碰撞风险的障碍物之间的距离小于最小安全距离S_安，车辆直接进入紧急制动状态。

如果0<a₀<a_break，那么，本实施例采用如下的重规划轨迹方法为车辆争取更 多的时间，车辆进入平滑制动模式，即：车辆开始以a₀的加速度做匀减速运动，并在安全停车点P_park停止。

进一步地，根据原始全局轨迹上存在碰撞风险的障碍物所处的位置的情形不同，步骤2中的重规划轨迹方法具体包括：

第一种情形：障碍物和车辆均在道路内部，其中的道路既可以是结构化道路，也可以是非结构化道路。

步骤21，在障碍物后方的原始全局轨迹上获取采样点。例如在障碍物后方的曲线距离S₁处的原始全局轨迹上间隔进行采样，每间隔∆S采样一个采样点，采样点如图7中示出的三角行黑块所示，图7中的P_S1、P_S2、P_S3、P_S4点均为采样点。一般S₁=10m，∆S=2m。

步骤22，在障碍物处设置辅助线，再在辅助线上采样避障引导点。为了能够保证生成的新的全局轨迹贴近原始全局轨迹，极大利用原始全局轨迹的数据，充分利用计算资源，减小了重规划轨迹的计算量，并且新的全局轨迹相比原始全局轨迹曲率变化量小，可以保证车辆的行驶平稳性。

如图7和图8所示，步骤22具体包括：

步骤221，计算突然出现的障碍物占据原始全局轨迹的曲线段的第一中点G的坐标。

步骤222，以第一中点G作垂直于原始全局轨迹的辅助线，获得辅助线与静态障碍物的两个交点为障碍物交点M和障碍物交点N。

步骤223，将步骤221中的障碍物的边界点向地图边界线作投影，得到两投影点E和投影点F。

步骤224，计算一障碍物交点M与一投影点E之间的第一距离d_ME和另一障碍物交点N与另一投影点F之间的距离d_FN，并将第一距离d_ME和第二距离d_FN中更大的一侧的障碍物交点与投影点的连接线段的中点作为避障引导点P_mid。图中的d_FN的数值大于d_ME，所以在FN线段上的中点作为避障引导点P_mid。

步骤225，判断避障引导点P_mid与其同侧的投影点的第三距离d_{F_P}是否大于两倍车辆宽度2d_car，如果是,则将避障引导点P_mid移至辅助线上，且与避障引导点P_mid同侧的投影点的第三距离为两倍车辆宽度的位置处，此时的避障引导点P_mid与辅助线垂直。这样可以防止通行空隙较大时，重规划轨迹过于偏移原始全局轨迹。

步骤23，计算规划完成时车辆的未来位姿点P_i,未来位姿点P_i的信息包括在原始全局轨迹上获得第i次规划的未来位姿点P_i的坐标和车辆航向角，其被描述为（X_i,Y_i,θ_i）。

步骤23具体包括：

步骤231，初始化重规划次序i=1。

步骤232，按照下式分别计算第i次规划完成时刻t_i、速度v_i和曲线距离S_0i，再根据曲线距离S_0i，确定（X_i,Y_i,θ_i）：

t_i=t₀+n·i·T_think

v_i=v₀-n·a₀·i·T_think

S_0i=v₀ ²-v_i ²/2a₀

其中，T_think为预设的完成一次路径搜索的最大重规划时长，v₀为车辆在障碍物出现的时刻t₀的行驶速度，v_i为车辆在时刻t_i的行驶速度，n为预设的重规划总次数，n的数值可以取5至10范围内的整数，S_0i为未来位姿点P_i与时刻t₀所在位置P₀的曲线距离。

步骤24，以未来位姿点向避障引导点进行路径搜索，规划第一路径，并在所述第一路径规划成功的情形下，以避障引导点向步骤21获取的采样点进行路径搜索，规划第二路径。

在一个实施例中，步骤24具体包括：

步骤241，加载待规划区域的地图的信息，判断曲线距离S_0i是否大于距离S₀与S_安之差，如果是，说明采样点在安全停车点P_park后方，车辆不能在安全停车点P_park之前完成重规划，则结束重规划轨迹方法，车辆进入紧急制动状态，进而避免发生碰撞。反之，以步骤23确定的未来位姿点P_i向避障引导点P_mid进行路径搜索，并将搜索获得的第一路径存储到第一容器Vector0中。其中，S₀为车辆在t₀时刻与障碍物的距离，S_安为安全停车点P_park与障碍物之间的距离。

步骤242，判断第一容器Vector0是否为空集，如果是，则确定为第一路径规划失败，并令i=i+1，跳转步骤232，否则确定为第一路径规划成功，并进入步骤243。

步骤243，以避障引导点P_mid分别向步骤21获取的采样点进行路径搜索，并将搜索获得第二路径存储到第二容器Vector1中。

步骤244，判断第二容器Vector1是否为空集，如果是，则确定为第二路径规划失败，并令i=i+1，跳转步骤232；否则确定为第二路径规划成功。

步骤25，将第一路径和第二路径对应的轨迹分别与原始全局轨迹进行拼接， 输出重规划轨迹。

在一个实施例中，步骤25具体包括：

步骤251，将第二容器Vector1中第一路径对应的轨迹与第一容器Vector0中的第二路径对应的轨迹进行拼接，并将拼接好的轨迹存入第三容器Vector3中。

步骤252，遍历第三容器Vector3中的所有拼接好的轨迹，选取其中曲率最小的拼接好的轨迹与原始全局轨迹进行再次拼接，并对再次拼接的拼接点进行数值优化，获得新的全局轨迹。

第二种情形：障碍物处于非结构化和结构化道路的交界处。此种情形与上述第一种情形大致相同，区别仅在于步骤24。相同之处不再赘述，下面着重描述第二种情形下的步骤24，其具体包括：

步骤241，将非结构化道路和结构化道路的地图进行融合，生成新地图，并加载新地图的信息。

步骤242，判断曲线距离S_0i是否大于距离S₀与S_安之差，如果是，则车辆进入紧急制动状态；反之，以步骤23确定的未来位姿点P_i向避障引导点P_mid进行路径搜索，并将搜索获得的第一路径存储到第一容器Vector0中，如图7中未来位姿点P_i与避障引导点P_mid之间的虚线示意的是第一路径。其中，S₀为车辆在t₀时刻与障碍物的距离，S_安为安全停车点P_park与障碍物之间的距离。

步骤243，判断第一容器Vector0是否为空集，如果是，则确定为第一路径规划失败，并令i=i+1，跳转步骤232，否则确定为第一路径规划成功，并进入步骤244。

步骤244，以避障引导点P_mid分别向各个采样点（例如图中的P_S1，P_S2，P_S3，P_S4点）进行路径搜索，并将搜索获得第二路径存储到第二容器Vector1中，如图7中避障引导点P_mid与采样点P_Si之间的虚线示意的是第二路径。

步骤245，判断第二容器Vector1是否为空集，如果是，则确定为第二路径规划失败，并令i=i+1，跳转步骤232；否则确定为第二路径规划成功。

上述实施例中，“i=i+1”可以立即为在当前规划次序的基础上加上1。

如图1所示，本发明另外提供的混合道路场景下的智能车辆规划系统包括任务规划单元、地图单元、感知单元、定位单元和规划器，其中：

任务规划单元用于提供驾驶任务的信息，或者也可以理解为用于提供整个驾驶任务的总体目标，如智能车辆行驶的最优化目标、终点位置等。

地图单元用于为驾驶任务构建地图的信息。

感知单元用于获取车辆周围环境的信息。

定位单元用于采集车辆定位的信息。

规划器在全局原始路径规划开始前，需要获取任务规划单元、地图单元、感知单元和定位单元的数据，再根据这些数据规划出一条车辆的运动轨迹，最终运动控制系统会根据运动轨迹来控制车辆的油门、刹车、方向盘等执行机构使车辆按照运动轨迹行驶。

具体地，规划器包括全局路径规划单元和重规划单元。

其中，全局路径规划单元用于根据车辆的驾驶任务及其对应的地图信息、车辆周围环境信息和车辆定位信息，规划原始全局轨迹。全局轨迹规划是指在得到给定车辆当前位置与终点目标后，通过搜索选择一条最优的路径，这里的“最优”也就是根据任务规划单元的总体目标而确定的路径最短，或者到达时间最快等条件，该规划过程类似于日常生活中经常用到的“导航”功能，其具体过程在上面实施例中已经介绍，在此不再赘述。

重规划单元用于判断车辆与原始全局轨迹上存在碰撞风险的障碍物之间的距离是否小于最小安全距离，若是，则车辆进入紧急制动；否则设置安全停车点，车辆进入平滑制动模式，并进行重规划轨迹，以在达到安全停车点时速度为0。

其中，重规划单元具体包括采样点采样子单元、避障引导点采样子单元、未来位姿点计算子单元、路径搜索子单元和重规划轨迹输出子单元，其中：

采样点采样子单元用于在障碍物后方的原始全局轨迹上获取采样点，采样方法在上述实施例中已经说明。

避障引导点采样子单元用于在障碍物处设置辅助线，再计算突然出现的障碍物占据原始全局轨迹的曲线段的第一中点G的坐标，并以第一中点G作垂直于原始全局轨迹的辅助线，获得辅助线与静态障碍物的两个交点为障碍物交点M和障碍物交点N，然后将障碍物的边界点向地图边界线作投影，得到投影点E和投影点F，计算一障碍物交点M与一投影点E之间的第一距离d_ME和另一障碍物交点N与另一投影点F之间的第二距离d_FN，并将第一距离d_ME和第二距离d_FN中更大的一侧的障碍物交点与投影点的连接线段的中点作为避障引导点P_mid，最后判断避障引导点P_mid与其同侧的投影点的第三距离d_{F_P}是否大于两倍车辆宽度2d_car，如果是,则将避障引导点P_mid移至辅助线上，且与避障引导点P_mid同侧的投影点的第三距离为两倍车辆宽度的位置处，此时的避障引导点P_mid与辅助线垂直。

未来位姿点计算子单元用于计算规划完成时车辆的未来位姿点。

路径搜索子单元用于以未来位姿点向避障引导点进行路径搜索，规划第一路径，并在所述第一路径规划成功的情形下，以避障引导点向采样点进行路径搜索，规划第二路径。

重规划轨迹输出子单元用于将第一路径和第二路径对应的轨迹分别与原始全局轨迹进行拼接，输出重规划轨迹。

在一个实施例中，未来位姿点计算子单元具体包括：

姿态信息计算模块，其用于按照下式分别计算第i次规划完成时刻t_i、速度v_i和曲线距离S_0i，确定未来位姿点P_i的信息（X_i,Y_i,θ_i）：

t_i=t₀+n·i·T_think

v_i=v₀-n·a₀·i·T_think

S_0i=v₀ ²-v_i ²/2a₀

其中，（X_i,Y_i）和θ_i分别表示在原始全局轨迹上获得的时刻t_i的未来位姿点P_i的坐标和车辆航向角，T_think为预设的完成一次路径搜索的最大重规划时长，v₀为车辆在障碍物出现的时刻t₀的行驶速度，v_i为车辆在时刻t_i的行驶速度，n为预设的重规划总次数，S_0i为未来位姿点P_i与时刻t₀所在位置P₀的曲线距离。

在一个实施例中，在障碍物和车辆均在道路内部情形下，路径搜索子单元具体包括第一路径搜索模块、第一路径判断模块、第二路径搜索模块和第二路径判断模块，其中：

第一路径搜索模块用于加载待规划区域的地图的信息，判断曲线距离S_0i是否大于距离S₀与S_安之差，如果是，则车辆进入紧急制动状态；反之，以未来位姿点向避障引导点P_mid进行路径搜索，并将搜索获得的第一路径存储到第一容器Vector0中；其中，S₀为车辆在t₀时刻与障碍物的距离，S_安为安全停车点P_park与障碍物之间的距离。

第一路径判断模块用于判断第一容器Vector0是否为空集，如果是，则确定为第一路径规划失败，并由姿态信息计算模块计算第i+1次规划的未来位姿点的信息，否则确定为第一路径规划成功。

第二路径搜索模块用于确定为第一路径规划成功后，以避障引导点P_mid分别向各个采样点进行路径搜索，并将搜索获得第二路径存储到第二容器Vector1中。

第二路径判断模块用于判断第二容器Vector1是否为空集，如果是，则确定为第二路径规划失败，并由姿态信息计算模块计算第i+1次规划的未来位姿点的信息，否则确定为第二路径规划成功。

在一个实施例中，在障碍物处于非结构化和结构化道路的交界处情形下，与上述实施例中的在障碍物和车辆均在道路内部情形的区别仅仅在于：路径搜索子单元还包括地图融合模块，地图融合模块用于将非结构化道路和结构化道路的地图进行融合，生成新地图，并加载新地图的信息。其他模块则在地图融合模块在加载新地图的基础上进行来实现路径搜索子单元的功能。

在一个实施例中，重规划轨迹输出子单元具体包括轨迹拼接模块和全局轨迹优化模块，其中：

轨迹拼接模块用于将第二容器Vector1中第一路径对应的轨迹与第一容器Vector0中的第二路径对应的轨迹进行拼接，并将拼接好的轨迹存入第三容器Vector3中。

全局轨迹优化模块用于遍历第三容器Vector3中的所有拼接好的轨迹，选取其中曲率最小的拼接好的轨迹与原始全局轨迹进行再次拼接，并对再次拼接的拼接点进行数值优化，获得新的全局轨迹。

需要说明的是，上述实施例中提供的重规划轨迹方法也可以采用如论文《B. Li,Z. Yin, Y. Ouyang, Y. Zhang, X. Zhong and S. Tang, "Online TrajectoryReplanning for Sudden Environmental Changes during Automated Parking: AParallel Stitching Method," in IEEE Transactions on Intelligent Vehicles,doi: 10.1109/TIV.2022.3156429.》中所记载的方法实现，当然也可以采用公知的其他方法实现。

上述实施例中使用的混合A*搜索方法可以采取其他路径规划方法进行替代，如随机树、数值优化或机器学习等规划方法。

在同一场景下使用本方法，三段轨迹拼接的方法和lattice算法分别进行10次实验，规划结束时，本方法平均重规划路径长度为20m，规划起点平均偏离系数为0.3（偏离系数=偏移距离/规划周期内车辆的平均速度），规划起点8次在车辆实际位置的前方，平均耗时20ms；lattice算法平均重规划路径长度为33m，平均偏离系数为0.5，规划起点6次在车辆实际位置的前方，平均耗时150ms；三段轨迹拼接的方法平均重规划路径长度为24m，平均偏离系数为为0.6，规划起点5次在车辆实际位置的前方，平均耗时323ms。由此可见，本方法更多的利用了原始全局轨迹，重规划路径最短，在规划时间上有很大的提升，而且规划起点更多的落在实际位置的前方并且偏移实际位置比率更小，车辆能更好的进行轨迹跟踪。

相比现有的轨迹拼接方法，本发明提供的规划方法计算复杂度更小，只需要在障碍物附近形成一条较短的避障轨迹即可，规划器的规划耗时更低。在发现障碍物后，车辆进入平滑制动并设立了安全停车点，车辆的舒适性得到保证，同时降低了规划失败时的碰撞风险。

相比较lattice算法而言，本方法规划失败率更低，拥有更高的稳定性，并且轨迹的曲率更平滑更加满足车辆的运动学规律。

最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (10)

1.一种混合道路场景下的智能车辆规划方法，其特征在于，包括：

步骤1，根据车辆的驾驶任务及其对应的地图的信息、车辆周围环境的信息和车辆定位的信息，规划原始全局轨迹；

步骤2，判断车辆与原始全局轨迹上存在碰撞风险的障碍物之间的距离是否小于最小安全距离，若是，则车辆进入紧急制动；否则设置安全停车点，车辆进入平滑制动模式，并进行重规划轨迹，以在达到安全停车点时速度为0；

其中，重规划轨迹具体包括：

步骤21，在障碍物后方的原始全局轨迹上获取采样点；

步骤22，在障碍物处设置辅助线，再在辅助线上采样避障引导点；

步骤23，计算规划完成时车辆的未来位姿点；

步骤24，以未来位姿点向避障引导点进行路径搜索，规划第一路径，并在所述第一路径规划成功的情形下，以避障引导点向步骤21获取的采样点进行路径搜索，规划第二路径；

步骤25，将第一路径和第二路径对应的轨迹分别与原始全局轨迹进行拼接，输出重规划轨迹；

其中，步骤22中的“在障碍物处设置辅助线，再在辅助线上采样避障引导点”的方法具体包括：

步骤221，计算突然出现的障碍物占据原始全局轨迹的曲线段的第一中点的坐标；

步骤222，以第一中点作垂直于原始全局轨迹的辅助线，获得辅助线与静态障碍物的两个交点均为障碍物交点；

步骤223，将步骤221中的障碍物的边界点向地图边界线作投影，得到两投影点；

步骤224，计算一障碍物交点与一投影点之间的第一距离和另一障碍物交点与另一投影点之间的第二距离，并将第一距离和第二距离中更大的一侧的障碍物交点与投影点的连接线段的中点作为避障引导点；

步骤225，判断避障引导点与其同侧的投影点的第三距离是否大于两倍车辆宽度，如果是,则将避障引导点移至辅助线上，且与避障引导点同侧的投影点的第三距离为两倍车辆宽度的位置处，此时的避障引导点与辅助线垂直。

2.如权利要求1的混合道路场景下的智能车辆规划方法，其特征在于，步骤23具体包括：

步骤231，初始化重规划次序i=1；

步骤232，按照下式分别计算第i次规划完成时刻t_i、速度v_i和曲线距离S_0i，确定未来位姿点P_i的信息（X_i,Y_i,θ_i）：

t_i=t₀+n·i·T_think

v_i=v₀-n·a₀·i·T_think

S_0i=v₀ ²-v_i ²/2a₀

其中，（X_i,Y_i）和θ_i分别表示在原始全局轨迹上获得第i次重规划的未来位姿点P_i的坐标和车辆航向角，T_think为预设的完成一次路径搜索的最大重规划时长，v₀为车辆在障碍物出现的时刻t₀的行驶速度，v_i为车辆在时刻t_i的行驶速度，n为预设的重规划总次数，S_0i为未来位姿点P_i与时刻t₀所在位置P₀的曲线距离，a₀为车辆在位置P₀到安全停车点做匀减速运动的加速度。

3.如权利要求2的混合道路场景下的智能车辆规划方法，其特征在于，在障碍物和车辆均在道路内部情形下，步骤24具体包括：

步骤241，加载待规划区域的地图的信息，判断曲线距离S_0i是否大于距离S₀与S_安之差，如果是，则车辆进入紧急制动状态；反之，以未来位姿点P_i向避障引导点进行路径搜索，并将搜索获得的第一路径存储到第一容器中；其中，S₀为车辆在t₀时刻与障碍物的距离，S_安为安全停车点与障碍物之间的距离；

步骤242，判断第一容器是否为空集，如果是，则确定为第一路径规划失败，并令i=i+1，跳转步骤232，否则确定为第一路径规划成功，并进入步骤243；

步骤243，以避障引导点分别向步骤21获取的采样点进行路径搜索，并将搜索获得第二路径存储到第二容器中；

步骤244，判断第二容器是否为空集，如果是，则确定为第二路径规划失败，并令i=i+1，跳转步骤232；否则确定为第二路径规划成功。

4.如权利要求2的混合道路场景下的智能车辆规划方法，其特征在于，在障碍物处于非结构化和结构化道路的交界处情形下，步骤24具体包括：

步骤241，将非结构化道路和结构化道路的地图进行融合，生成新地图，并加载新地图的信息；

步骤242，判断曲线距离S_0i是否大于距离S₀与S_安之差，如果是，则车辆进入紧急制动状态；反之，以步骤23确定的未来位姿点向避障引导点进行路径搜索，并将搜索获得的第一路径存储到第一容器中；其中，S₀为车辆在t₀时刻与障碍物的距离，S_安为安全停车点与障碍物之间的距离；

步骤243，判断第一容器是否为空集，如果是，则确定为第一路径规划失败，并令i=i+1，跳转步骤232，否则确定为第一路径规划成功，并进入步骤244；

步骤244，以避障引导点分别向各个采样点进行路径搜索，并将搜索获得第二路径存储到第二容器中；

步骤245，判断第二容器是否为空集，如果是，则确定为第二路径规划失败，并令i=i+1，跳转步骤232；否则确定为第二路径规划成功。

5.如权利要求3或4的混合道路场景下的智能车辆规划方法，其特征在于，步骤25具体包括：

步骤251，将第二容器中第一路径对应的轨迹与第一容器中的第二路径对应的轨迹进行拼接，并将拼接好的轨迹存入第三容器中；

步骤252，遍历第三容器中的所有拼接好的轨迹，选取其中曲率最小的拼接好的轨迹与原始全局轨迹进行再次拼接，并对再次拼接的拼接点进行数值优化，获得新的全局轨迹。

6.一种混合道路场景下的智能车辆规划系统，其特征在于，包括：

任务规划单元，其用于提供驾驶任务的信息；

地图单元，其用于为驾驶任务构建地图的信息；

感知单元，其用于获取车辆周围环境的信息；

定位单元，其用于采集车辆定位的信息；

全局路径规划单元，其用于根据车辆的驾驶任务及其对应的地图信息、车辆周围环境信息和车辆定位信息，规划原始全局轨迹；

重规划单元，其用于判断车辆与原始全局轨迹上存在碰撞风险的障碍物之间的距离是否小于最小安全距离，若是，则车辆进入紧急制动；否则设置安全停车点，车辆进入平滑制动模式，并进行重规划轨迹，以在达到安全停车点时速度为0；

其中，重规划单元具体包括：

采样点采样子单元，其用于在障碍物后方的原始全局轨迹上获取采样点；

避障引导点采样子单元，其用于在障碍物处设置辅助线，再计算突然出现的障碍物占据原始全局轨迹的曲线段的第一中点的坐标，并以第一中点作垂直于原始全局轨迹的辅助线，获得辅助线与静态障碍物的两个交点均为障碍物交点，然后将障碍物的边界点向地图边界线作投影，得到两投影点，计算一障碍物交点与一投影点的第一距离和另一障碍物交点与另一投影点的第二距离，并将第一距离和第二距离中更大的一侧的障碍物交点与投影点的连接线段的中点作为避障引导点，最后判断避障引导点与其同侧的投影点的第三距离是否大于两倍车辆宽度，如果是,则将避障引导点移至辅助线上，且与避障引导点同侧的投影点的第三距离为两倍车辆宽度的位置处，此时的避障引导点与辅助线垂直；

未来位姿点计算子单元，其用于计算规划完成时车辆的未来位姿点；

路径搜索子单元，其用于以未来位姿点向避障引导点进行路径搜索，规划第一路径，并在所述第一路径规划成功的情形下，以避障引导点向采样点进行路径搜索，规划第二路径；

重规划轨迹输出子单元，其用于将第一路径和第二路径对应的轨迹分别与原始全局轨迹进行拼接，输出重规划轨迹。

7.如权利要求6的混合道路场景下的智能车辆规划系统，其特征在于，未来位姿点计算子单元具体包括：

姿态信息计算模块，其用于按照下式分别计算第i次规划完成时刻t_i、速度v_i和曲线距离S_0i，确定未来位姿点P_i的信息（X_i,Y_i,θ_i）：

t_i=t₀+n·i·T_think

v_i=v₀-n·a₀·i·T_think

S_0i=v₀ ²-v_i ²/2a₀

其中，（X_i,Y_i）和θ_i分别表示在原始全局轨迹上获得的时刻t_i的未来位姿点P_i的坐标和车辆航向角，T_think为预设的完成一次路径搜索的最大重规划时长，v₀为车辆在障碍物出现的时刻t₀的行驶速度，v_i为车辆在时刻t_i的行驶速度，n为预设的重规划总次数，S_0i为未来位姿点P_i与时刻t₀所在位置P₀的曲线距离，a₀为车辆在位置P₀到安全停车点做匀减速运动的加速度。

8.如权利要求7的混合道路场景下的智能车辆规划系统，其特征在于，在障碍物和车辆均在道路内部情形下，路径搜索子单元具体包括：

第一路径搜索模块，其用于加载待规划区域的地图的信息，判断曲线距离S_0i是否大于距离S₀与S_安之差，如果是，则车辆进入紧急制动状态；反之，以未来位姿点向避障引导点进行路径搜索，并将搜索获得的第一路径存储到第一容器中；其中，S₀为车辆在t₀时刻与障碍物的距离，S_安为安全停车点与障碍物之间的距离；

第一路径判断模块，其用于判断第一容器是否为空集，如果是，则确定为第一路径规划失败，并由姿态信息计算模块计算第i+1次规划的未来位姿点的信息，否则确定为第一路径规划成功；

第二路径搜索模块，其用于确定为第一路径规划成功后，以避障引导点分别向各个采样点进行路径搜索，并将搜索获得第二路径存储到第二容器中；

第二路径判断模块，其用于判断第二容器是否为空集，如果是，则确定为第二路径规划失败，并由姿态信息计算模块计算第i+1次规划的未来位姿点的信息，否则确定为第二路径规划成功。

9.如权利要求7的混合道路场景下的智能车辆规划系统，其特征在于，在障碍物处于非结构化和结构化道路的交界处情形下，路径搜索子单元具体包括：

地图融合模块，其用于将非结构化道路和结构化道路的地图进行融合，生成新地图，并加载新地图的信息；

第一路径搜索模块，其用于判断曲线距离S_0i是否大于距离S₀与S_安之差，如果是，则车辆进入紧急制动状态；反之，以未来位姿点向避障引导点进行路径搜索，并将搜索获得的第一路径存储到第一容器中；其中，S₀为车辆在t₀时刻与障碍物的距离，S_安为安全停车点与障碍物之间的距离；

第一路径判断模块，其用于判断第一容器是否为空集，如果是，则确定为第一路径规划失败，并由姿态信息计算模块计算第i+1次规划的未来位姿点的信息，否则确定为第一路径规划成功；

第二路径搜索模块，其用于确定为第一路径规划成功后，以避障引导点分别向各个采样点进行路径搜索，并将搜索获得第二路径存储到第二容器中；

第二路径判断模块，其用于判断第二容器是否为空集，如果是，则确定为第二路径规划失败，并由姿态信息计算模块计算第i+1次规划的未来位姿点的信息，否则确定为第二路径规划成功。

10.如权利要求8或9的混合道路场景下的智能车辆规划系统，其特征在于，重规划轨迹输出子单元具体包括：

轨迹拼接模块，其用于将第二容器中第一路径对应的轨迹与第一容器中的第二路径对应的轨迹进行拼接，并将拼接好的轨迹存入第三容器中；

全局轨迹优化模块，其用于遍历第三容器中的所有拼接好的轨迹，选取其中曲率最小的拼接好的轨迹与原始全局轨迹进行再次拼接，并对再次拼接的拼接点进行数值优化，获得新的全局轨迹。

Images