CN115950431A - 一种路径规划方法、系统、计算机设备、可读存储介质及机动车 - Google Patents

Abstract

本发明公开了一种路径规划方法、系统、计算机设备、可读存储介质及机动车，涉及自动驾驶技术领域，运行自动驾驶的车辆通过所述路径规划方法规划自动驾驶路径，包括如下步骤：初始化各项参数，通过逆向树确定目标点；以目标点作为起始点进行采样，生成新的节点；连接生成的节点，连接后进行障碍物测试，如果与障碍物无碰撞，生成路径并对生成的路径进行优化，形成车辆自动驾驶的路径。本发明所提供的路径规划方法满足车辆的任意初始位置姿态要求，加快路径规划速度，提高规划质量，更符合车辆实际行驶情况。

Description

一种路径规划方法、系统、计算机设备、可读存储介质及机动车

技术领域

本发明涉及自动驾驶技术领域，具体涉及一种路径规划方法、系统、计算机设备、可读存储介质及机动车。

背景技术

车辆的智能运动控制是自动驾驶技术的重要研究方向。近年来，自动驾驶技术发展迅速。但是，现有技术中自动驾驶的轨迹规划，是针对结构道路，即有规划的、经过交通部门划线的道路。在轨迹规划时，根据传统的几何方法，按照道路线进行几何划分，通过多段弧线生成轨迹。而现有的针对非结构道路，则是根据当前车辆位置点和目标置点进行多段几何曲线生成，或者以节点扩展的方式，需要对规划区域进行明显划分，实现在连续空间中扩展节点，进而生成规划轨迹。但是，这种方式对车辆的初始的位置姿态有着严格的要求。同时，现有技术非结构化道路基于搜索的轨迹规划算法，由于在搜索时扩展的有效节点少，无效节点多，无法构成路径点，导致搜索时间增加，严重影响规划的实时性。

发明内容

为解决前述问题，本发明提供了一种路径规划方法，满足车辆的任意初始位置姿态要求，加快路径规划速度，提高规划质量，更符合车辆实际行驶情况。

为了达到上述目的，本发明采用如下技术方案：

一种路径规划方法，用于车辆自动驾驶时规划行车路径，包括如下步骤：

初始化各项参数，通过逆向树确定目标点，所述目标点为当前车速下预设时间之后的位置；

以目标点作为起始点进行采样，生成新的节点；

连接生成的节点，连接后进行障碍物测试，如果与障碍物无碰撞，生成路径并对生成的路径进行优化，形成车辆自动驾驶的路径。

可选的，以目标点作为起始点进行采样包括如下步骤：

在起始点，以车辆行驶方向为采样区中心线方向生成新的节点，在新的节点，以新的节点与其父节点连线的方向作为采样区的中心线方向，随机数按照如下公式生成新的节点：

其中，x_i为第i个节点的横坐标，y_i为第i个节点的纵坐标；x_old为新的节点的父节点的横坐标，y_old为新的节点的父节点的纵坐标，r为采样区半径，θ为车辆的航向角，r_rand～N(μ_r,σ_r)和θ_rand～N(μ_θ,σ_θ)为随机数，u_r为新的节点与其父节点的相对距离，σ_r为采样区径向范围，σ_θ为采样区中心线与采样区边界的夹角，μ_θ为当前采样区中心线与x轴的夹角，

θ_init为起始点的航向角；

判断采样点是否落入死区，如果落入，舍弃该采样点，所述死区为采样区域障碍物重合的区域。

可选的，以目标点作为起始点进行采样时，通过直线生长的逆向拓展树对生成的节点进行调整，包括如下步骤：

从起始点开始，以固定步长沿逆向拓展树调整生成的节点，判断前一节点到达下一节点是否发生碰撞，如果无碰撞，前一节点即为调整后的节点，如果有碰撞，向前移动Δt，移动后的节作为调整后的节点。

可选的，前一节点到达下一节点的路径为基于车辆动力学模型计算出的最小转弯半径，车辆动力学模型为：

其中，x_r为车辆后轴中点的横坐标，y_r为车辆后轴中点的纵坐标，L为汽车轴距，v_r为后轴车速；δ_f为等效前轮转向角，等效前轮转向角δ_f受最大等效前轮转角δ_max约束，满足|δ_f|≤δ_max。

可选的，进行障碍物测试包括如下步骤：

自车作为矩形A，障碍物作为矩形B，获取矩形A和矩形B之间的距离，矩形A和矩形B中心连线与x轴的夹角；

根据如下条件判断前一节点到达下一节点是否发生碰撞：

其中，L为矩形A和矩形B之间的距离，β为矩形A和矩形B中心连线与x轴的夹角，L_A为矩形A的长度，W_A为矩形A的宽度，L_B为矩形B的长度，W_B为矩形B的宽度；

同时满足判断条件，则车辆与障碍物发生碰撞

可选的，每个新的节点均具有索引编号，对生成的路径进行优化包括如下步骤：

以生成的路径的终点为起始点，按照索引编号由小到大，通过RS曲线依次连接待优化的节点和起始点；

从起始点开始，在起始点和待优化的节点之间寻找无碰撞的RS路径，如果存在无碰撞的RS路径，则剔除起始点和待优化的节点之间的其他节点，并用新的路径替换原有路径；

以当前节点为新的起始点，进行下一轮优化，直至所有的原有路径均转化为RS路径，将所有优化后的路径合并，形成车辆自动驾驶的路径。

可选的，通过RS曲线连接生成的节点。

本发明所提供的技术方案，针对非结构道路，构建了车辆动力学模型，并在车辆动力学模型的约束下，使用高斯分布采样法及偏向性采样法进行融合采样，引入逆向拓展树调整快速搜索树形成的节点，加快了路径规划速度。同时，通过障碍物碰撞测试以及基于RS曲线进行的路径平滑优化，提高了路径规划质量，克服了现有技术中对于非结构道路规划采用几何方法或者搜索空间进行节点扩展方法所存在的过多冗余搜索、算法低效，无车辆运动约束等问题。

同时，本发明还提供了一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现前述任意一项所述的路径规划方法。

并且，本发明还提供了一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现前述任意一项所述的路径规划方法。

此外，本发明还提供了一种机动车，所述机动车具有自动驾驶功能，所述机动车运行自动驾驶功能时，通过权前述任意一项所述的路径规划方法规划自车的自动驾驶路径；

或所述机动车具有前述的计算机设备；

或所述机动车具有前述的计算机可读存储介质，所述计算机程序被处理器执行时实现前述任意一项所述的路径规划方法。

本发明所提供的机动车，其有益效果与前述路径规划方法的有益效果推理过程相类似，此处不再赘述。

本发明的这些特点和优点将会在下面的具体实施方式以及附图中进行详细的揭露。本发明最佳的实施方式或手段将结合附图来详尽表现，但并非是对本发明技术方案的限制。另外，在每个下文和附图中出现的这些特征、要素和组件是具有多个，并且为了表示方便而标记了不同的符号或数字，但均表示相同或相似构造或功能的部件。

附图说明

下面结合附图对本发明作进一步说明：

图1为本发明实施例的流程示意图；

图2为本发明实施例中采样的示意图；

图3为本发明实施例中车辆动力学模型的示意图；

图4为本发明实施例中障碍物测试的示意图；

图5为本发明实施例中路径优化的示意图。

具体实施方式

下面结合本发明实施例的附图对本发明实施例的技术方案进行解释和说明，但下述实施例仅为本发明的优选实施例，并非全部。基于实施方式中的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得其他实施例，都属于本发明的保护范围。

在本说明书中引用的“一个实施例”或“实例”或“例子”意指结合实施例本身描述的特定特征、结构或特性可被包括在本专利公开的至少一个实施例中。短语“在一个实施例中”在说明书中的各位置的出现不必都是指同一个实施例。

实施例：

本实施例提供了一种路径规划方法，用于具有自动驾驶功能的车辆，运行自动驾驶的车辆通过本实施例所提供的路径规划方法规划自动驾驶路径。本实施例所提供的路径规划方法，尤其适用于车辆在非机构道路场景下的自动驾驶路径规划。非结构道路，是指未经交通部门规划的道路，或未铺装的道路，或未经交通部门划线的道路。

如图1所示，本实施所提供的路径规划方法包括如下步骤：

初始化各项参数，具体而言，对本实施例所提供的路径规划方法所用到的各个模型的参数进行初始化。

通过逆向树确定目标点，目标点为当前车速下预设时间之后的位置。具体到本实施例中，目标点为当前车速下3秒后的位置。在其他实施例中，预设时间可由本领域技术人员根据车型需求进行设置，此处不做限定。

将目标点作为起始点，通过快速搜索树按照如下步骤进行采样，生成新的节点，每个新的节点均具有索引编号：

现有技术中的算法，采样点均匀分布在搜索空间中，存在过多的冗余搜索，导致算法效率较低。而本实施例所提供的方法利用高斯分布采样，减少采样的随机性与盲目性，同时通过车辆动力学限制采样的方向及范围，使之更加符合车辆的约束，确保所规划的自动驾驶路径更加合理，符合车辆运动。在起始点，以车辆行驶方向为采样区中心线方向生成新的节点，在新的节点，以新的节点与其父节点连线的方向作为采样区的中心线方向，随机数按照如下公式生成新的节点：

其中，x_i为第i个节点的横坐标，y_i为第i个节点的纵坐标；x_old为新的节点的父节点的横坐标，y_old为新的节点的父节点的纵坐标，r为采样区半径，θ为车辆的航向角，r_rand～N(μ_r,σ_r)和θ_rand～N(μ_θ,σ_θ)为随机数，u_r为新的节点与其父节点的相对距离，σ_r为采样区径向范围，σ_θ为采样区中心线与采样区边界的夹角，μ_θ为当前采样区中心线与x轴的夹角，

初始阶段，快速搜索树的顶点只有x_init，θinit为起始点的航向角，采样阶段快速搜索树的顶点数大于1个。

判断采样点是否落入死区。死区是指采样区与障碍物重合的区域。由于高斯分布采样具有一定的偏向性，因此其在采样过程中存在采样点落入死区的可能性。如图2所示，为确保采样过程的稳定性，当采样点落入死区时，舍弃该采样点。

由于快速搜索树的随机性，其规划的路径在通常需要较多的位姿调整，因此容易造成车辆剐蹭等危险情况的发生。因此应当尽可能减少对于车辆的位姿调整。因此，在以目标点作为起始点进行采样时，通过直线生长的逆向拓展树对生成的节点进行调整，包括如下步骤：

逆向拓展树的起始点为车辆最终的位姿，因而从起始点开始，以固定步长沿逆向拓展树调整生成的节点，判断前一节点到达下一节点是否发生碰撞，如果无碰撞，前一节点即为调整后的节点，如果有碰撞，向前移动Δt，移动后的节作为调整后的节点。前一节点到达下一节点的路径为基于车辆动力学模型计算出的最小转弯半径，如图3所示，车辆动力学模型为：

其中，x_r为车辆后轴中点的横坐标，y_r为车辆后轴中点的纵坐标，L为汽车轴距，以向前为正，v_r为后轴车速；δ_f为等效前轮转向角，以逆时针方向维正。等效前轮转向角δ_f受最大等效前轮转角δ_max约束，满足|δ_f|≤δ_max。而根据车辆动力学模型计算最小转弯半径为本领域技术人员所公知的现有技术，此处不再赘述。

而后通过RS曲线连接生成的节点。RS曲线以Dubins曲线为基础，其假定车辆始终按照最小转弯半径进行转向且尽可能使用直线，起点到终点间的路径只由圆弧及直线构成。本实施例在规划路径的过程中采用RS曲线对节点进行直接连接，因此最终规划的路径将只由圆弧和直线组成，避免了多余的位姿调整，提高了路径的安全性。

连接后按照如下步骤进行障碍物测试：

如图4所示，自车作为矩形A，障碍物作为矩形B，A_x、A_y为以Ao为坐标原点的局部坐标系下的单位向量，B_x、B_y为以Bo为坐标原点的局部坐标系下的单位向量，获取矩形A和矩形B之间的距离，矩形A和矩形B中心连线与x轴的夹角；

判断矩形A和矩形B在上述四个分离轴A_x、A_y、B_x、B_y上的投影是否满足如下条件：

进而判断前一节点到达下一节点是否发生碰撞。其中，L为矩形A和矩形B之间的距离，β为矩形A和矩形B中心连线与x轴的夹角，L_A为矩形A的长度，W_A为矩形A的宽度，L_B为矩形B的长度，W_B为矩形B的宽度；

同时满足上述四个判断条件，则车辆与障碍物发生碰撞。如果与障碍物无碰撞，生成路径并对生成的路径进行优化，形成车辆自动驾驶的路径。

针对快速搜索树的路径存在的曲率不连续、路径连接不平滑等问题，本实施例通过RS曲线替换原有的快速搜索树路径，使优化后的路径能够满足车辆动力学模型要求，当两点间不存在障碍物时，RS路径即为最短路径。对生成的路径进行优化包括如下步骤：

如图5所示，以生成的路径的终点为起始点，按照索引编号由小到大，通过RS曲线依次连接待优化的节点和起始点；

从起始点开始，在起始点和待优化的节点之间寻找无碰撞的RS路径，如果存在无碰撞的RS路径，则剔除起始点和待优化的节点之间的其他节点，并用新的路径替换原有路径；

以当前节点为新的起始点，进行下一轮优化，直至所有的原有路径均转化为RS路径，将所有优化后的路径合并，形成车辆自动驾驶的路径。

本实施例所提供的技术方案，构建了车辆动力学模型，并在车辆动力学模型的约束下，使用高斯分布采样法及偏向性采样法进行融合采样，引入逆向拓展树调整快速搜索树形成的节点，加快了路径规划速度。同时，通过障碍物碰撞测试以及基于RS曲线进行的路径平滑优化，提高了路径规划质量，克服了现有技术中采用几何方法或者搜索空间进行节点扩展方法所存在的过多冗余搜索、算法低效，无车辆运动约束等问题。

需要指出的是，本实施例所提供的路径规划方法，不仅仅适用于行进间自动驾驶路径的规划，还可应用于自动泊车场景。在应用于自动泊车场景时，目标点即为车辆最终停进目标车位时的位姿，其余步骤与行进间的自动驾驶路径规划无异。

与此同时，本实施例还提供了一种计算机设备，包括存储器和处理器，存储器存储有计算机程序，计算机程序被处理器执行时，使得处理器执行上述路径规划方法的步骤。

本领域普通技术人员可以理解，实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成。据此，的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可实现上述任意一项实施例的方法。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPROM)或闪存。易失性存储器可包括随机存取存储器(RAM)或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM(SRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据率SDRAM(DDRSDRAM)、增强型SDRAM(ESDRAM)、同步链路(Synchlink)DRAM(SLDRAM)、存储器总线(Rambus)直接RAM(RDRAM)、直接存储器总线动态RAM(DRDRAM)以及存储器总线动态RAM(RDRAM)等。

并且，本实施例还提供了一种机动车，具有自动驾驶功能，本实施例所提供的机动车运行自动驾驶功能时，通过前述路径规划方法规划自车的自动驾驶路径；

或机动车具有前述的计算机设备；

或机动车具有前述的计算机可读存储介质，所述计算机程序被处理器执行时实现前述任意一项所述的路径规划方法。

以上，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，熟悉该本领域的技术人员应该明白本发明包括但不限于附图和上面具体实施方式中描述的内容。任何不偏离本发明的功能和结构原理的修改都将包括在权利要求书的范围中。

Claims (10)

1.一种路径规划方法，用于车辆自动驾驶时规划行车路径，其特征在于，所述路径规划方法包括如下步骤：

初始化各项参数，通过逆向树确定目标点，所述目标点为当前车速下预设时间之后的位置；

以目标点作为起始点进行采样，生成新的节点；

连接生成的节点，连接后进行障碍物测试，如果与障碍物无碰撞，生成路径并对生成的路径进行优化，形成车辆自动驾驶的路径。

2.根据权利要求1所述的路径规划方法，其特征在于，以目标点作为起始点进行采样包括如下步骤：

在起始点，以车辆行驶方向为采样区中心线方向生成新的节点，在新的节点，以新的节点与其父节点连线的方向作为采样区的中心线方向，随机数按照如下公式生成新的节点：

其中，x_i为第i个节点的横坐标，y_i为第i个节点的纵坐标；x_old为新的节点的父节点的横坐标，y_old为新的节点的父节点的纵坐标，r为采样区半径，θ为车辆的航向角，r_rand～N(μ_r,σ_r)和θ_rand～N(μ_θ,σ_θ)为随机数，u_r为新的节点与其父节点的相对距离，σ_r为采样区径向范围，σ_θ为采样区中心线与采样区边界的夹角，μ_θ为当前采样区中心线与x轴的夹角，

θ_init为起始点的航向角；

判断采样点是否落入死区，如果落入，舍弃该采样点，所述死区为采样区域障碍物重合的区域。

3.根据权利要求1所述的路径规划方法，其特征在于，以目标点作为起始点进行采样时，通过直线生长的逆向拓展树对生成的节点进行调整，包括如下步骤：

从起始点开始，以固定步长沿逆向拓展树调整生成的节点，判断前一节点到达下一节点是否发生碰撞，如果无碰撞，前一节点即为调整后的节点，如果有碰撞，向前移动Δt，移动后的节作为调整后的节点。

4.根据权利要求3所述的路径规划方法，其特征在于，前一节点到达下一节点的路径为基于车辆动力学模型计算出的最小转弯半径，车辆动力学模型为：

其中，x_r为车辆后轴中点的横坐标，y_r为车辆后轴中点的纵坐标，L为汽车轴距，v_r为后轴车速；δ_f为等效前轮转向角，等效前轮转向角δ_f受最大等效前轮转角δ_max约束，满足|δ_f|≤δ_max。

5.根据权利要求1所述的路径规划方法，其特征在于，进行障碍物测试包括如下步骤：

自车作为矩形A，障碍物作为矩形B，获取矩形A和矩形B之间的距离，矩形A和矩形B中心连线与x轴的夹角；

根据如下条件判断前一节点到达下一节点是否发生碰撞：

其中，L为矩形A和矩形B之间的距离，β为矩形A和矩形B中心连线与x轴的夹角，L_A为矩形A的长度，W_A为矩形A的宽度，L_B为矩形B的长度，W_B为矩形B的宽度；

同时满足判断条件，则车辆与障碍物发生碰撞。

6.根据权利要求1所述的路径规划方法，其特征在于，每个新的节点均具有索引编号，对生成的路径进行优化包括如下步骤：

以生成的路径的终点为起始点，按照索引编号由小到大，通过RS曲线依次连接待优化的节点和起始点；

从起始点开始，在起始点和待优化的节点之间寻找无碰撞的RS路径，如果存在无碰撞的RS路径，则剔除起始点和待优化的节点之间的其他节点，并用新的路径替换原有路径；

以当前节点为新的起始点，进行下一轮优化，直至所有的原有路径均转化为RS路径，将所有优化后的路径合并，形成车辆自动驾驶的路径。

7.根据权利要求1至6之一所述的路径规划方法，其特征在于，通过RS曲线连接生成的节点。

8.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至7中任意一项所述的路径规划方法。

9.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至7中任意一项所述的路径规划方法。

10.一种机动车，其特征在于，所述机动车具有自动驾驶功能，所述机动车运行自动驾驶功能时，通过权利要求1至7中任意一项所述的路径规划方法规划自车的自动驾驶路径；

或所述机动车具有权利要求8所述的计算机设备；

或所述机动车具有权利要求9所述的计算机可读存储介质，所述计算机程序被处理器执行时实现权利要求1至7中任意一项所述的路径规划方法。

Images