一种路径规划方法、装置、设备和介质

技术领域

本发明实施例涉及自动驾驶技术领域，尤其涉及一种路径规划方法、装置、 设备和介质。

背景技术

自动驾驶车辆具有最小转弯半径，因此需对路径规划的最大曲率进行限制； 自动驾驶车辆的方向盘转角速度具有上限，因此需保证路径规划的曲率连续并 对曲率变化率进行限制；自动驾驶车辆具备倒车功能，因此路径规划中需考虑 倒车操作。

现有技术一般采用直线、Dubins曲线或Reeds-Shepp曲线通过快速扩展随 机树算法进行路径规划，采用直线进行路径规划，不能满足最大曲率限制和曲 率连续性要求。采用Dubins曲线进行路径规划，不能满足曲率连续性要求并且 不能进行倒车规划。采用Reeds-Shepp曲线进行路径规划，由于快速扩展随机 树算法规划的路径不能满足曲率连续性需求，在快速扩展随机树路径规划完毕 后，对路径进行了平滑处理。由于平滑后的路径与原规划路径不完全重合，存 在原规划路径碰撞分析失效的风险。

发明内容

本发明实施例提供一种路径规划方法、装置、设备和介质，以实现所规划 的路径在保证曲率连续的条件下，能够满足最大曲率及最大曲率变化率限制。

第一方面，本发明实施例提供了一种路径规划方法，该方法包括：

分别获取车辆的行驶起点的坐标和航向角，以及行驶终点的坐标和航向角， 将所述行驶起点的坐标和航向角加入车辆行驶点集合中；

根据所述车辆行驶点集合中的行驶点的坐标和航向角和预设车辆控制集中 的曲线得到下一行驶点的坐标和航向角；所述预设车辆控制集中的曲线满足所 述车辆的最小转弯半径和最大曲率变化率；

将所述下一行驶点的坐标和航向角加入车辆行驶点集合中；所述行驶点集 合中的行驶点的曲率为零；

根据所述下一行驶点的坐标和航向角以及所述行驶终点的坐标和航向角， 计算所述下一行驶点和所述行驶终点之间的误差，当所述误差小于预设误差阈 值时，根据所述车辆行驶点集合中的行驶点确定车辆的行驶路径。

第二方面，本发明实施例还提供了一种路径规划装置，该装置包括：

坐标和航向角获取模块，用于分别获取车辆的行驶起点的坐标和航向角， 以及行驶终点的坐标和航向角，将所述行驶起点的坐标和航向角加入车辆行驶 点集合中；

下一行驶点确定模块，用于根据所述车辆行驶点集合中的行驶点的坐标和 航向角和预设车辆控制集中的曲线得到下一行驶点的坐标和航向角；所述预设 车辆控制集中的曲线满足所述车辆的最小转弯半径和最大曲率变化率；

行驶点加入模块，用于将所述下一行驶点的坐标和航向角加入车辆行驶点 集合中；所述行驶点集合中的行驶点的曲率为零；

行驶路径确定模块，用于根据所述下一行驶点的坐标和航向角以及所述行 驶终点的坐标和航向角，计算所述下一行驶点和所述行驶终点之间的误差，当 所述误差小于预设误差阈值时，根据所述车辆行驶点集合中的行驶点确定车辆 的行驶路径。

第三方面，本发明实施例还提供了一种车载设备，其中，所述车载设备包 括：

一个或多个处理器；

存储装置，用于存储一个或多个程序；

当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多 个处理器实现如本发明任意实施例所提供的路径规划方法。

第四方面，本发明实施例还提供了一种计算机可读存储介质，其上存储有 计算机程序，其中，该程序被处理器执行时实现如本发明任意实施例所提供的 路径规划方法。

本发明实施例通过分别获取车辆的行驶起点的坐标和航向角，以及行驶终 点的坐标和航向角，将所述行驶起点的坐标和航向角加入车辆行驶点集合中； 根据所述车辆行驶点集合中的行驶点的坐标和航向角和预设车辆控制集中的曲 线得到下一行驶点的坐标和航向角；所述预设车辆控制集中的曲线满足所述车 辆的最小转弯半径和最大曲率变化率；将所述下一行驶点的坐标和航向角加入 车辆行驶点集合中；所述行驶点集合中的行驶点的曲率为零；根据所述下一行 驶点的坐标和航向角以及所述行驶终点的坐标和航向角，计算所述下一行驶点 和所述行驶终点之间的误差，当所述误差小于预设误差阈值时，根据所述车辆 行驶点集合中的行驶点确定车辆的行驶路径。解决了现有的车辆路径规划方法不能满足曲率的连续性和曲率变化率的限制的问题，实现了行驶路径在保证曲 率连续的条件下，能够满足最大曲率及最大曲率变化率限制的效果。

附图说明

图1是本发明实施例一中的一种路径规划方法的流程图；

图2是本发明实施例一中的车辆控制集筛选示意图；

图3是本发明实施例一中的车辆控制集示意图；

图4是本发明实施例二中的一种路径规划装置的结构图；

图5是本发明实施例三中的一种车载设备的结构示意图。

具体实施方式

下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是，此 处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。另外还需 要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部结 构。

实施例一

图1为本发明实施例一提供的一种路径规划方法的流程图，本实施例可适 用于汽车路径规划的情况，该方法可以由路径规划装置来执行，具体包括如下 步骤：

S110、分别获取车辆的行驶起点的坐标和航向角，以及行驶终点的坐标和 航向角，将行驶起点的坐标和航向角加入车辆行驶点集合中。

根据车辆的状态空间分别获取车辆行驶起点的坐标和航向角，以及车辆行 驶终点的坐标和航向角，并将行驶起点的坐标和航向角加入车辆的行驶点集合 中，用于进行路径规划。将车辆行驶起点和行驶终点的曲率设置为零，以便得 到曲率连续的行驶路径。

S120、根据车辆行驶点集合中的行驶点的坐标和航向角和预设车辆控制集 中的曲线得到下一行驶点的坐标和航向角；预设车辆控制集中的曲线满足车辆 的最小转弯半径和最大曲率变化率。

根据车辆行驶点集合中的行驶点的坐标和航向角以及预设车辆控制集中的 曲线确定一行驶点坐标和航向角，下行驶点的坐标可以为正，也可以为负，当 坐标为正时，车辆为前进状态。当坐标为负时，车辆为倒车状态。

可选的，所述根据所述车辆行驶点集合中的行驶点的坐标和航向角和预设 车辆控制集中的曲线得到下一行驶点的坐标和航向角，包括：获取一目标位置； 根据所述预设车辆控制集中的曲线、所述目标位置、所述车辆行驶点集合中的 行驶点的坐标和所述行驶点的航向角得到可行行驶点的坐标和航向角；计算各 所述可行行驶点与所述目标位置之间的距离；将所述可行行驶点与所述行驶起 点通过连接曲线进行连接；将与所述目标位置距离最近且所在的连接曲线不与 障碍物发生碰撞的可行行驶点确定为所述下一行驶点，得到所述下一行驶点的 坐标和航向角。

可选的，所述获取一目标位置，包括：获取一随机数；当所述随机数大于 预设随机数阈值时，随机生成不包括所述行驶终点的所述目标位置；当所述随 机数小于预设随机数阈值时，将所述行驶终点作为所述目标位置。在获取目标 位置时，先获取一随机数，将获取的随机数与预设的随机数阈值进行比较，当 获取的随机数大于预设的随机数阈值时，随机生成一个不包含行驶终点的目标 位置，该目标位置的坐标和航向角是随机生成的。当获取的随机数小于预设的 随机数阈值时，将行驶终点作为目标位置。

预设车辆控制集为在线下计算得到的，预设车辆控制集只考虑(x,y,θ)三个 维度，其中x为横坐标，y为纵坐标，θ为车辆航向角。根据车辆的属性信息 获取车辆最小转弯半径。对一定范围内的车辆状态空间进行离散化，即在远大 于车辆最小转弯半径的区域建立二维直角坐标系，将车辆状态空间离散为维度 为(x，y，θ)的点。横坐标与纵坐标均不超过车辆最小转弯半径的预设倍数。 示例性的，在8倍车辆最小转弯半径范围内进行离散化，即x≤8r，y≤8r。将 航向角离散为4个角度，分别为0，pi/2，-pi/2及pi。假设起点为(0，0，pi/2)， 则其可到达的点为(0，1，pi/2)，(1，1，0)及(-1，1，-pi)，将起点和可到达点通 过曲线连接，将连接曲线作为预设车辆控制集。通过预设车辆控制集可以得到 车辆当前位置之后的可行行驶点的坐标和航向角。为了减小系统内存，提升系 统运行性能，可以将预设车辆控制集进行简化，示例性的，如图2所示，路径 1可以被路径2和路径3代替。车辆控制集示意图如图3所示。车辆控制集以 表的形式存储于车辆控制器中。

可选的，所述根据所述预设车辆控制集中的曲线、所述目标位置、所述行 驶点的坐标和所述行驶点的航向角得到可行行驶点，包括：根所述行驶点的坐 标获取所述车辆行驶点集合中与所述目标位置距离最近的靠近行驶点；根据所 述靠近行驶点的坐标、所述靠近行驶点的航向角和所述预设车辆控制集中的曲 线生成所述可行行驶点。通过Reeds-Shepp曲线计算车辆行驶点集合中的行驶 点与目标位置的距离。因车辆是沿曲线进行行驶的，故通过Reeds-Shepp曲线 计算车辆行驶点集合中的行驶点与目标位置的距离更加符合实际需要。将与目 标位置距离最近的行驶点作为靠近行驶点。根据靠近行驶点的坐标和航向角和 预设车辆控制集得到靠近行驶点之后的可行行驶点。

因可行行驶点只是车辆可以行驶到达的点，可行行驶点中从车辆的当前位 置行驶到可行行驶点的过程中会与障碍物发生碰撞的可行行驶点，需对可行行 驶点进行筛选，进而得到下一行驶点。可选的，所述计算各所述可行行驶点与 所述目标位置之间的距离，包括：通过Reeds-Shepp曲线计算各所述可行行驶 点与所述目标位置之间的距离。并按照距离从短到长的顺序将可行行驶点进行 排序。按照顺序依次选取可行行驶点，将选取的可行行驶点与靠近行驶点通过 曲线进行连接得到两点之间的路径，判断从靠近行驶点到选取的可行行驶点之 间的路径是否与障碍物发生碰撞，若发生碰撞则继续选取下一可行行驶点进行 判断，若不发生碰撞则将该可行行驶点作为下一行驶点，并停止判断。

可选的，连接曲线为三阶螺旋曲线，行驶路径可根据行驶起点坐标和行驶 起点坐标的下一行驶点坐标计算得到。示例性的，假设行驶起点表示为 (0，0，0，k

对上式进行求解，得到参数a，b，c，d的值。进而得到连接曲线。

S130、将下一行驶点的坐标和航向角加入车辆行驶点集合中；行驶点集合 中的行驶点的曲率为零。

将得到的下一行驶点加入车辆行驶点集合中，用于生成行驶路径。经过筛 选得到的下一行驶点行驶过程中不会与障碍物发生碰撞，提高了规划的路径的 安全性与实用性。

S140、根据下一行驶点的坐标和航向角以及行驶终点的坐标和航向角，计 算下一行驶点和行驶终点之间的误差，当误差小于预设误差阈值时，根据车辆 行驶点集合中的行驶点确定车辆的行驶路径。

得到下一行驶点后，根据下一行驶点的坐标和航向角以及行驶终点的坐标 和航向角，计算下一行驶点和行驶终点之间的误差。当误差在预设误差阈值范 围内时，将下一行驶点作为规划路径的行驶终点，在车辆行驶点集合中进行反 向查找得到车辆的行驶路径。

可选的，计算所述行驶起点之后的下一行驶点坐标和所述行驶终点坐标的 误差之后，还包括：当所述误差大于预设误差阈值时，根据车辆行驶点集合中 的行驶点重复执行下一行驶点生成步骤直到所述误差小于预设误差阈值。当下 一行驶点坐标和行驶终点坐标的误差大于预设误差阈值时，需要重新获取一新 的目标位置，根据车辆行驶点集合中的行驶点重复执行下一行驶点生成步骤， 直到生成的下一行驶点小于预设误差阈值，或达到了重复执行下一行驶点生成 步骤的最大重复次数，此时根据车辆行驶点集合中的行驶点生成车辆的行驶路 径。

本实施例的技术方案，通过分别获取车辆的行驶起点的坐标和航向角，以 及行驶终点的坐标和航向角，将所述行驶起点的坐标和航向角加入车辆行驶点 集合中；根据所述车辆行驶点集合中的行驶点的坐标和航向角和预设车辆控制 集中的曲线得到下一行驶点的坐标和航向角；所述预设车辆控制集中的曲线满 足所述车辆的最小转弯半径和最大曲率变化率；将所述下一行驶点的坐标和航 向角加入车辆行驶点集合中；所述行驶点集合中的行驶点的曲率为零；根据所 述下一行驶点的坐标和航向角以及所述行驶终点的坐标和航向角，计算所述下 一行驶点和所述行驶终点之间的误差，当所述误差小于预设误差阈值时，根据 所述车辆行驶点集合中的行驶点确定车辆的行驶路径。解决了现有的车辆路径规划方法不能满足曲率的连续性和曲率变化率的限制的问题，实现了行驶路径 在保证曲率连续的条件下，能够满足最大曲率及最大曲率变化率限制的效果。

实施例二

图4为本发明实施例二提供的一种路径规划装置的结构图，该路径规划装 置包括：坐标和航向角获取模块310、下一行驶点确定模块320、行驶点加入模 块330和行驶路径确定模块340。

其中，坐标和航向角获取模块310，用于分别获取车辆的行驶起点的坐标 和航向角，以及行驶终点的坐标和航向角，将所述行驶起点的坐标和航向角加 入车辆行驶点集合中；下一行驶点确定模块320，用于根据所述车辆行驶点集 合中的行驶点的坐标和航向角和预设车辆控制集中的曲线得到下一行驶点的坐 标和航向角；所述预设车辆控制集中的曲线满足所述车辆的最小转弯半径和最 大曲率变化率；行驶点加入模块330，用于将所述下一行驶点的坐标和航向角 加入车辆行驶点集合中；所述行驶点集合中的行驶点的曲率为零；行驶路径确 定模块340，用于根据所述下一行驶点的坐标和航向角以及所述行驶终点的坐 标和航向角，计算所述下一行驶点和所述行驶终点之间的误差，当所述误差小 于预设误差阈值时，根据所述车辆行驶点集合中的行驶点确定车辆的行驶路径。

在上述实施例的技术方案中，下一行驶点确定模块320，包括：

目标位置获取单元，用于获取一目标位置；

可行行驶点获取单元，用于根据所述预设车辆控制集中的曲线、所述目标 位置、所述车辆行驶点集合中的行驶点的坐标和所述行驶点的航向角得到可行 行驶点的坐标和航向角；

距离计算单元，用于计算各所述可行行驶点与所述目标位置之间的距离；

可行行驶点连接单元，用于将所述可行行驶点与所述行驶起点通过连接曲 线进行连接；

下一行驶点确定单元，用于将与所述目标位置距离最近且所在的连接曲线 不与障碍物发生碰撞的可行行驶点确定为所述下一行驶点，得到所述下一行驶 点的坐标和航向角。

可选的，目标位置获取单元，包括：

随机数获取子单元，用于获取一随机数；

目标位置生成子单元，用于当所述随机数大于预设随机数阈值时，随机生 成不包括所述行驶终点的所述目标位置；当所述随机数小于预设随机数阈值时， 将所述行驶终点作为所述目标位置。

可选的，可行行驶点获取单元，包括：

靠近行驶点获取子单元，用于根所述行驶点的坐标获取所述车辆行驶点集 合中与所述目标位置距离最近的靠近行驶点；

可行行驶点生成子单元，用于根据所述靠近行驶点的坐标、所述靠近行驶 点的航向角和所述预设车辆控制集中的曲线生成所述可行行驶点。

可选的，距离计算单元，具体用于通过Reeds-Shepp曲线计算各所述可行 行驶点与所述目标位置之间的距离。

可选的，所述连接曲线为三阶螺旋曲线。

可选的，路径规划装置，还包括：

下一行驶点生成模块，用于当所述误差大于预设误差阈值时，重复执行下 一行驶点生成步骤直到所述误差小于预设误差阈值。

分别获取车辆的行驶起点的坐标和航向角，以及行驶终点的坐标和航向角， 将所述行驶起点的坐标和航向角加入车辆行驶点集合中；根据所述车辆行驶点 集合中的行驶点的坐标和航向角和预设车辆控制集中的曲线得到下一行驶点的 坐标和航向角；所述预设车辆控制集中的曲线满足所述车辆的最小转弯半径和 最大曲率变化率；将所述下一行驶点的坐标和航向角加入车辆行驶点集合中； 所述行驶点集合中的行驶点的曲率为零；根据所述下一行驶点的坐标和航向角 以及所述行驶终点的坐标和航向角，计算所述下一行驶点和所述行驶终点之间 的误差，当所述误差小于预设误差阈值时，根据所述车辆行驶点集合中的行驶 点确定车辆的行驶路径。解决了现有的车辆路径规划方法不能满足曲率的连续 性和曲率变化率的限制的问题，实现了行驶路径在保证曲率连续的条件下，能 够满足最大曲率及最大曲率变化率限制的效果。

本发明实施例所提供的路径规划装置可执行本发明任意实施例所提供的方 法，具备执行方法相应的功能模块和有益效果。

实施例三

图5为本发明实施例三提供的一种车载设备的结构示意图，如图5所示， 该车载设备包括处理器410、存储器420、输入装置430和输出装置440；车载 设备中处理器410的数量可以是一个或多个，图5中以一个处理器410为例； 车载设备中的处理器410、存储器420、输入装置430和输出装置440可以通过 总线或其他方式连接，图5中以通过总线连接为例。

存储器420作为一种计算机可读存储介质，可用于存储软件程序、计算机 可执行程序以及模块，如本发明实施例中的路径规划方法对应的程序指令/模块 (例如，路径规划装置中的坐标和航向角获取模块310、下一行驶点确定模块 320、行驶点加入模块330和行驶路径确定模块340)。处理器410通过运行存 储在存储器420中的软件程序、指令以及模块，从而执行车载设备的各种功能 应用以及数据处理，即实现上述的路径规划方法。

存储器420可主要包括存储程序区和存储数据区，其中，存储程序区可存 储操作系统、至少一个功能所需的应用程序；存储数据区可存储根据终端的使 用所创建的数据等。此外，存储器420可以包括高速随机存取存储器，还可以 包括非易失性存储器，例如至少一个磁盘存储器件、闪存器件、或其他非易失 性固态存储器件。在一些实例中，存储器420可进一步包括相对于处理器410 远程设置的存储器，这些远程存储器可以通过网络连接至车载设备。上述网络 的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。

输入装置430可用于接收输入的数字或字符信息，以及产生与车载设备的 用户设置以及功能控制有关的键信号输入。输出装置440可包括显示屏等显示 设备。

实施例四

本发明实施例四还提供一种包含计算机可执行指令的存储介质，所述计算 机可执行指令在由计算机处理器执行时用于执行一种路径规划方法，该方法包 括：

分别获取车辆的行驶起点的坐标和航向角，以及行驶终点的坐标和航向角， 将所述行驶起点的坐标和航向角加入车辆行驶点集合中；

根据所述车辆行驶点集合中的行驶点的坐标和航向角和预设车辆控制集中 的曲线得到下一行驶点的坐标和航向角；所述预设车辆控制集中的曲线满足所 述车辆的最小转弯半径和最大曲率变化率；

将所述下一行驶点的坐标和航向角加入车辆行驶点集合中；所述行驶点集 合中的行驶点的曲率为零；

根据所述下一行驶点的坐标和航向角以及所述行驶终点的坐标和航向角， 计算所述下一行驶点和所述行驶终点之间的误差，当所述误差小于预设误差阈 值时，根据所述车辆行驶点集合中的行驶点确定车辆的行驶路径。

当然,本发明实施例所提供的一种包含计算机可执行指令的存储介质,其计 算机可执行指令不限于如上所述的方法操作,还可以执行本发明任意实施例所 提供的路径规划方法中的相关操作。

通过以上关于实施方式的描述，所属领域的技术人员可以清楚地了解到， 本发明可借助软件及必需的通用硬件来实现，当然也可以通过硬件实现，但很 多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上 或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机 软件产品可以存储在计算机可读存储介质中，如计算机的软盘、只读存储器 (Read-Only Memory,ROM)、随机存取存储器(RandomAccess Memory,RAM)、 闪存(FLASH)、硬盘或光盘等，包括若干指令用以使得一台计算机设备(可以 是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述的方法。

值得注意的是，上述路径规划装置的实施例中，所包括的各个单元和模块 只是按照功能逻辑进行划分的，但并不局限于上述的划分，只要能够实现相应 的功能即可；另外，各功能单元的具体名称也只是为了便于相互区分，并不用 于限制本发明的保护范围。

注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员 会理解，本发明不限于这里所述的特定实施例，对本领域技术人员来说能够进 行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽 然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以 上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例， 而本发明的范围由所附的权利要求范围决定。