CN110806218A - 泊车路径规划方法、装置和系统 - Google Patents

Abstract

本公开提出一种泊车路径规划方法、装置和系统，涉及泊车技术领域。在本公开中，对局部起始位姿到终点位姿之间的局部泊车路径进行规划时，生成可避障的参考折线路径以及多条曲线路径，从多条曲线路径中选择一条与参考折线路径的累加距离偏差最小的曲线路径，相对于选择路径最短的曲线路径，提高了选择出的曲线路径避障的可能性，进而加快了泊车路径整体规划的速度，提高了泊车路径整体规划的效率。

Description

泊车路径规划方法、装置和系统

技术领域

本公开涉及泊车技术领域，特别涉及一种泊车路径规划方法、装置和系统。

背景技术

随着经济社会的不断发展和人们生活水平的日益提高，车辆在日常生活中发挥着越来越重要的工作。泊车时要将车辆驶入有限的车位空间，因此针对泊车时的路径进行规划不同于针对直线路径进行规划，要在同时考虑障碍物以及空间有限的情况下进行，最终实现高效率地泊车。

在一些相关技术中，使用混合A*算法实现泊车路径规划，其中“混合”的Reeds-Shepp曲线(以下简称RS曲线)的最短路径生成机制无法保障车辆不与障碍物发生碰撞，一旦生成的RS曲线与障碍物相撞，当前生成的RS曲线对泊车路径规划来说没有任何意义，需要再一次地重新生成RS曲线，当如此盲目生成的RS曲线始终与障碍物发生碰撞时，需要重复地生成RS曲线，就会严重降低泊车路径规划的效率。

发明内容

发明人发现，在相关技术中，在利用混合A*算法对泊车路径进行规划的过程中，RS曲线的生成是盲目的，若RS曲线与障碍物发生碰撞则需要重复地生成RS曲线，由此导致泊车路径规划的效率较低。

在本公开中，对局部起始位姿到终点位姿之间的局部泊车路径进行规划时，生成可避障的参考折线路径以及多条曲线路径，从多条曲线路径中选择一条与参考折线路径的累加距离偏差最小的曲线路径，相对于选择路径最短的曲线路径，提高了选择出的曲线路径可避障的可能性，进而加快了泊车路径整体规划的速度，提高了泊车路径整体规划的效率。

根据本公开的一些实施例，提供了一种泊车路径规划方法，其特征在于，包括：

生成衔接起始位姿到中间的局部起始位姿的折线路径；

生成从局部起始位姿到终点位姿的可避障的参考折线路径；

生成从局部起始位姿到终点位姿的多条曲线路径；

从所述多条曲线路径中选择一条与参考折线路径的累加距离偏差最小的曲线路径作为衔接局部起始位姿到终点位姿的连通路径；

当所述连通路径上没有障碍物时，将衔接起始位姿到中间的局部起始位姿的折线路径和衔接局部起始位姿到终点位姿的连通路径拼接形成泊车路径。

在一些实施例中，所述生成从局部起始位姿到终点位姿的可避障的参考折线路径包括：从当前节点的相邻节点中选择泊车路径代价最小且未被障碍物占据的相邻节点作为所述参考折线路径上的下一个节点，并将下一个节点作为下一次循环的当前节点，直到所述下一个节点为所述终点位姿对应的终点节点，其中，当前节点的初始值设置为局部起始位姿对应的局部起始节点，所述局部起始节点、选择出的节点以及终点节点依次连接形成所述参考折线路径。

在一些实施例中，所述生成从局部起始位姿到终点位姿的多条曲线路径包括：使用RS曲线生成算法生成从局部起始位姿到终点位姿的多条曲线路径。

在一些实施例中，所述从所述多条曲线路径中选择一条与参考折线路径的累加距离偏差最小的曲线路径作为衔接局部起始位姿到终点位姿的连通路径包括：利用若干第一剖分点将参考折线路径等分为预设个数的分段；利用若干第二剖分点将多条曲线路径分别等分为所述预设个数的分段；计算每一个第一剖分点与相应的第二剖分点的距离偏差并对各个所述距离偏差求和，将所述距离偏差之和最小时对应的曲线路径作为衔接局部起始位姿到终点位姿的连通路径。

在一些实施例中，当所述连通路径上有障碍物时，继续生成衔接局部起始位姿到下一个局部起始位姿的折线路径。

在一些实施例中，所述生成衔接起始位姿到中间的局部起始位姿的折线路径包括：利用基本A*算法或者利用混合A*算法的折线路径生成方法，生成衔接起始位姿到中间的局部起始位姿的折线路径。

根据本公开的另一些实施例，提供了一种泊车路径规划装置，其特征在于，包括：折线路径生成模块，被配置为生成衔接起始位姿到中间的局部起始位姿的折线路径；参考折线路径生成模块，被配置为生成从局部起始位姿到终点位姿的可避障的参考折线路径；曲线路径生成模块，被配置为生成从局部起始位姿到终点位姿的多条曲线路径；连通路径选择模块，被配置为从所述多条曲线路径中选择一条与参考折线路径的累加距离偏差最小的曲线路径作为衔接局部起始位姿到终点位姿的连通路径；泊车路径生成模块，被配置为确定衔接起始位姿到中间的局部起始位姿的折线路径和衔接局部起始位姿到终点位姿的没有障碍物的连通路径为泊车路径。

根据本公开的又一些实施例，提供了一种泊车路径规划装置，其特征在于，包括：存储器；以及耦接至所述存储器的处理器，所述处理器被配置为基于存储在所述存储器中的指令，执行任一实施例所述的泊车路径规划方法。

根据本公开的再一些实施例，提供了一种泊车路径规划系统，其特征在于，包括：如任一实施例所述的泊车路径规划装置；传感器，被配置为感知车辆的位姿和路径上的障碍物。

根据本公开的再一些实施例，提供了一种非瞬时性计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现任一实施例所述的泊车路径规划方法。

附图说明

下面将对实施例或相关技术描述中所需要使用的附图作简单的介绍。根据下面参照附图的详细描述，可以更加清楚地理解本公开。

显而易见地，下面描述中的附图仅仅是本公开的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本公开泊车路径规划方法的一些实施例的流程示意图。

图2为本公开一些实施例的基本A*算法的网格连通示意图。

图3为本公开一些实施例的基本A*算法的节点扩展方式示意图。

图4为本公开一些实施例的混合A*算法的节点扩展方式示意图。

图5为本公开的泊车路径规划装置的一些实施例的示意图。

图6为本公开的泊车路径规划系统的一些实施例的示意图。

具体实施方式

下面将结合本公开实施例中的附图，对本公开实施例中的技术方案进行清楚、完整地描述。

图1为本公开泊车路径规划方法的一些实施例的流程示意图。该方法例如可以由泊车路径规划装置执行。

如图1所示，该实施例的方法包括步骤101-105。

在步骤101，生成衔接起始位姿到中间的局部起始位姿的折线路径。

其中，车辆的位姿是根据构建的二维平面坐标系中的节点坐标以及车辆的姿态角确定的。例如可以利用基本A*算法或者利用混合A*算法的折线路径生成方法，生成衔接起始位姿到中间的局部起始位姿的折线路径。后面会具体描述利用基本A*算法生成折线路径的方法以及利用混合A*算法的折线路径生成方法。

利用基本A*算法或者混合A*算法生成的折线路径不仅可以躲避障碍物，而且可以保证是最短路径。

在步骤102，生成从局部起始位姿到终点位姿的可避障的参考折线路径。

生成从局部起始位姿到终点位姿的可避障的参考折线路径例如可以利用基本A*算法实现，主要流程包括：从当前节点的相邻节点中选择泊车路径代价最小且未被障碍物占据的相邻节点作为参考折线路径上的下一个节点，并将下一个节点作为下一次循环的当前节点，直到下一个节点为终点位姿对应的终点节点，其中，当前节点的初始值设置为局部起始位姿对应的局部起始节点，最后，将局部起始节点、选择出的节点以及终点节点依次连接形成参考折线路径。其中，泊车路径代价最小指的是泊车路径长度最短。

在步骤103，生成从局部起始位姿到终点位姿的多条曲线路径。

例如，使用RS(全称Reeds-Shepp)曲线生成算法生成从局部起始位姿到终点位姿的多条曲线路径。

在步骤104，从多条曲线路径中选择一条与参考折线路径的累加距离偏差最小的曲线路径作为衔接局部起始位姿到终点位姿的连通路径。

从多条曲线路径中选择一条与参考折线路径的累加距离偏差最小的曲线路径作为衔接局部起始位姿到终点位姿的连通路径的一种方法包括：利用若干第一剖分点将参考折线路径等分为预设个数的分段；利用若干第二剖分点将多条曲线路径分别等分为相同预设个数的分段；计算每一个第一剖分点与相应的第二剖分点的距离偏差，并对各个距离偏差求和，将距离偏差之和最小时对应的曲线路径作为衔接局部起始位姿到终点位姿的连通路径。其中，第一剖分点与对应的第二剖分点的距离偏差例如可以通过计算欧式距离得到。

由此，从多条曲线路径中选择出了一条与参考折线路径最接近的曲线路径，由于参考折线路径是可以避障的，从而增加了被选择出的曲线路径能够避障的可能性，减少了因不能避障而导致重复生成曲线路径的情况的发生，提高了泊车路径规划的效率。

在步骤105，当选择出的连通路径上没有障碍物时，将衔接起始位姿到中间的局部起始位姿的折线路径和衔接局部起始位姿到终点位姿的连通路径拼接形成泊车路径。

此外，如果生成的连通路径上有障碍物时，那么当前轮次继续生成衔接局部起始位姿到下一个局部起始位姿的折线路径，下一个轮次重复执行步骤101-105，继续生成可避障的曲线路径。

下面对利用剖分的RS曲线进行泊车路径规划进行详细描述。

首先，定义一些参数。定义OpenList为保存预搜索节点的动态集合，N_RS为预设的迭代循环次数，f(Node_k)指标表征节点Node_k的代价值，用g(Node_k)代表从起始节点扩展至当前节点Node_k的累积代价值，h(Node_k)代表从当前节点扩展至终止节点的代价估计值，则f(Node_k)为g(Node_k)与h(Node_k)求和的值。OpenList为动态集合，其中保存着未来预备被探索节点的下标，如果节点Node_k在某一刻保存于OpenList中，则状态is_opened(Node_k)应置为1；OpenList中的节点一旦经过探索则被移出OpenList，并且立即被关闭，这意味着is_opened(Node_k)＝0并且is_closed(Node_k)＝1。如果Node_child可以被Node_current搜索得到，则将其父节点设置为Node_current，即parent_id(Node_child)＝Node_current。假设Node_child也可由Node_current探索到，在“子节点Node_child的父节点是Node_current”这一假设下重新估算Node_child的代价值，将这一备选代价值记为f^*：如果f^*(Node_child)＜f(Node_child)，则说明Node_current较之Node_{old_parent}会令Node_child更具优势，此时应将Node_child的父节点重置为Node_current，并相应地更新Node_current的g、f值。反过来，如果f^*(Node_child)≥f(Node_child)则无需变更Node_child的父节点。

基于上述定义的参数，利用剖分的RS曲线进行泊车路径规划的具体步骤如下：

1)初始化动态集合

以及外层迭代循环次数iter＝0，并将起始节点Node_start加λOpenList；

2)当OpenList不为空集或iter小于预设的最大阈值时，循环执行步骤3)至14)；

3)iter加1，从OpenList选择f值最小的节点，记为Node_current；

4)如果iter能够被N_RS整除，使用剖分RS曲线的方法生成衔接Node_current位姿与终点位姿的路径χ，如果χ能够避障，记录节点Node_current及χ，构建空向量

向Λ中存入Node_current，执行步骤13)至14)；

5)扩展Node_current得到子节点Node_child，将其保存于集合Γ；

6)如果Node_child∈Γ，并且is_closed(Node_child)＝1，执行步骤7)；

7)如果Node_child∈Γ，并且is_opened(Node_child)＝0，此时如果从Node_current到Node_child的局部路径不避障，设置is_closed(Node_child)＝1，并执行步骤8)；

8)计算Node_child的相应属性数值；

9)判断节点Node_child，如果Node_child是终止节点，则记录节点Node_child，构建空向量

向Λ中存入Node_child，并执行步骤12)，如果节点Node_child不是终止节点，则将Node_child加入OpenList，并执行步骤10)；

10)计算f^*(Node_child)；

11)如果f^*(Node_child)＜f(Node_child)，将Node_childNode_child父节点置为Node_current并更新g(Node_child)、f(Node_child)，将Node_current从OpenList中移除；

12)如果(parent_id(Node_child)≠Null)，则向Λ中存入parent_id(Node_child)，将parent_id(Node_child)设置为Node_child，逆序输出向量Λ；

13)如果(parent_id(Node_current)≠Null)，向Λ中存入parent_id(Node_current)，将parent_id(Node_current)设置为Node_current；

14)将向量Λ按逆序重排，随后在其中存入χ，输出向量Λ。

上述实施例，对局部起始位姿到终点位姿之间的局部泊车路径进行规划时，生成可避障的参考折线路径以及多条曲线路径，从多条曲线路径中选择一条与参考折线路径的累加距离偏差最小的曲线路径，相对于选择路径最短的曲线路径，提高了选择出的曲线路径可避障的可能性，进而加快了泊车路径整体规划的速度，提高了泊车路径整体规划的效率。

下面描述利用基本A＊算法生成折线路径。上述步骤101的折线路径和步骤102的参考折线路径均可以利用基本A＊算法生成。

将起始位置与终止位置所在的二维平面划分为网格，则连续平面空间中的无穷点可被有限个数的网格代表。如图2所示的网格连通示意图。通过逐一判断各障碍物是否与网格重叠将所有网格分为空白网格以及被障碍物占据的网格两类。其中，将这种经离散化而形成的网格称为节点。

在利用基本A＊算法生成折线路径时，首先，确定路径规划任务的起始位置(也称为起始节点)、终止位置所在节点(也称为终止节点或终点节点)后，基本A＊算法从起始节点开始不断向周围扩大搜索范围，不断按照某一标准选择最有可能抵近终止节点的位置来展开搜索，直到搜索到终止节点为止。通过搜索得到一条衔接起始节点与终止节点的节点序列，构成衔接起始节点与终止节点的折线路径。

每一节点Node_k记录以下属性：f(Node_k)指标值、g(Node_k)指标值、h(Node_k)指标值、父节点的索引下标parent_id(Node_k)、体现当前节点是否在OpenList集合中的布尔型状态量is_opened(Node_k)、体现当前节点是否已被关闭的布尔型状态量is_closed(Node_k)。f(Node_k)指标表征了节点的代价值，根据公式

f(Node_k)＝g(Node_k)+h(Node_k) (1)

而确定，其中g(Node_k)代表从起始节点扩展至当前节点Node_k的累积代价值，h(Node_k)代表从当前节点扩展至终止节点的代价估计值。

基本A＊算法的第一步是为起始节点Node_start完善属性信息，包括设置g(Node_start)＝0、计算h(Node_start)、f(Node_start)＝g(Node_start)+h(Node_start)、parent_id(Node_start)＝Null、is_opened(Node_start)＝1以及is_closed(Node_k)＝0，其中将h(·)设置为当前节点与终止节点之间的曼哈顿距离(Manhattan distance)，即两个节点的网格轴距绝对值总和。在将Node_start加入空集OpenList之中充当第一个元素后，基本A＊算法开始循环迭代。

在每一轮循环开始时，从集合OpenList中选出f指标最小的那一个节点(如果多个节点同时取得最小f值，一般选择最新加入OpenList之中的那个节点)，假设选出的节点为Node_current。随后对Node_current进行扩展。如图3所示，描述了利用基本A＊算法对节点进行扩展的方法。对当前节点进行扩展，在基本A＊算法中一般将节点周围与其相邻的4连通或8连通区域的节点视为可扩展节点。由于这些“相邻节点”均由Node_current扩展而来，因此将它们称为Node_current的子节点。需要补充说明的是，如果Node_current处在网格图的边沿，则至少有3个子节点无从扩展，在扩展过程中过滤掉不合法情况，保留其中有效的子节点。在完成至多8个子节点的扩展后，对其逐一分析。

假设针对某一个子节点Node_child进行分析。首先，检查子节点Node_child所属的状态is_closed(Node_child)是否为1：如果是，则意味着该节点已被探索过，则直接放弃该子节点，继而分析下一子节点；如果不是，则进一步判断is_opened(Node_child)是否为0：如果是0，则子节点Node_child不在OpenList内，即其从未被探索过，为此触发操作A；如果不是0，则触发操作B。后面会对操作A、B展开详细介绍。至此，针对当前子节点的分析处理完成。在针对所有子节点均进行分析处理后，将Node_current从OpenList中删除，并对其设置is_opened(Node_current)＝0、is_closed(Node_current)＝1。至此当前迭代轮次结束。

首次被探索到的子节点Node_child将被实施操作A。具体而言，首先需判断Node_child否被障碍物占据：如果是，则设置is_closed(Node_child)＝1并结束操作；如果不是，则需要进一步处理。将其父节点设置为Node_current，即

parent_id(Node_child)＝Node_current (2a)

按照以下公式计算g(Node_child)：

g(Node_child)＝g(Node_current)+Euclidean_dist(Node_current，Node_child)

(2b)

其中，Euclidean_dist(Node_a，Node_b)是用于计算Node_a与Node_b之间欧氏距离的函数。在完成h(Node_child)计算之后，可进一步计算

f(Node_child)＝g(Node_child)+h(Node_child) (2c)

接下来，判断Node_child是否为终止节点：如果是，则跳出整个外层迭代循环，搜索过程成功完成，输出路径(输出路径的方式稍后介绍)；如果不是，则将Node_child加入OpenList之中，并相应地设置

is_opened(Node_child)＝1 (2d)

以及

is_closed(Node_child)＝0 (2e)

如果某一子节点Node_child存在于OpenList中，说明它在此前已设置了父节点且父节点一定不是Node_current，此时应执行操作B。操作B的主要内容是判断将Node_child的父节点重新设置为Node_current能否令当前子节点具有更小的f值，如果是，则将其父节点重置为Node_current。具体而言，假设当前子节点Node_child的父节点为Node_{old_parent}，则有

f(Node_child)＝g(Node_{old_parent})+Euclidean_dist(Node_{old_parent}，Node_child)+h(Node_child) (3a)

另一方面，既然Node_child也可由Node_current探索到，可以在“子节点Node_child的父节点是Node_current”这一假设下重新估算节点Node_child的代价值，将这一备选代价值记为f^*：

f^*(Node_child)＝g(Node_current)+Euclidean_dist(Node_current，Node_child)+h(Node_child) (3b)

如果f^*(Node_child)＜f(Node_child)则说明Node_current较之Node_{old_parent}会令Node_child更具优势，此时应将Node_child的父节点重置为Node_current，并相应地更新Node_current的g、f值。反过来，如果f^*(Node_child)≥f(Node_child)则无需变更Node_child的父节点。

基本A＊算法外层迭代循环的中断条件可分为成功、失败两类，前面已介绍了成功中断的判断过程。失败的判据包括：1)OpenList为空；2)外层迭代循环次数超过某一阈值。如果外层迭代成功，应从最后得到的子节点开始，确定其父节点，并继续确定父节点的父节点，直到出现某一父节点为Null为止，将这些节点按照逆序排列即对应着衔接起点与终点的粗略决策路径。如果外层迭代因失败而中断，则判定路径搜索失败并退出整个算法。

基本A＊算法的基本执行流程如下。

1)初始化动态集合

以及iter＝0，并将起始节点Node_start加入OpenList；

2)当OpenList不为空集或iter小于预设的最大阈值时，并循环执行步骤3)至12)；

3)iter加1，从OpenList选择f值最小的节点，记为Node_current；

4)扩展Node_current得到子节点Node_child保存于集合Γ；

5)如果Node_child∈Γ，并且is_closed(Node_child)＝1，执行步骤6)；

6)如果Node_child∈Γ，并且is_opened(Node_child)＝0，此时如果节点Node_child被障碍物占据，设置is_closed(Node_child)＝1，并执行步骤7)；

7)执行公式(1)；

8)接着，判断节点Node_child，如果Node_child是终止节点，则记录节点Node_child，构建空向量

向Λ中存入Node_child，并执行步骤12)；如果节点Node_child不是终止节点，则将Node_child加入OpenList，并执行步骤9)；

9)如果Node_child∈Γ，并且is_opened(Node_child)＝1，执行步骤10)；

10)执行公式(2b)；

11)如果f^*(Node_child)＜f(Node_child)，将Node_child父节点置为Node_current并更新g(Node_child)、f(Node_child)，将Node_current从OpenList中移除；

12)如果(parent_id(Node_child)≠Null)，则向Λ中存入parent_id(Node_child)，将parent_id(Node_child)设置为Node_child，逆序输出向量Λ。

利用基本A＊算法生成折线路径，可以避障，而且可以保证规划的折线路径为最短路径，提高了泊车效率。

下面描述利用混合A*算法的折线路径生成部分来生成折线路径，上述步骤101的折线路径可以利用混合A*算法生成。

混合A*算法是一种在离散x-y-θ状态空间中使用基本A*策略并融合RS曲线生成机制的路径规划方法。与基本A*算法相比，混合A*算法的差异体现在以下几点：

1)混合A*算法更改了连通图的构建方式。与基本A*算法在X-Y平面上构建网格相比，混合A*算法增加了车辆i姿态角θ_i(也称车辆位姿角)作为第三个维度，这意味着每一节点代表了一定邻域内的车辆状态[x_i，y_i，θ_i]。

2)混合A*算法新增了节点属性类别。除记录f、g、h、parent_id、is_opened、is_closed之外，每一节点还记录着扩展至该节点时的具体运动状态值[x_i，y_i，θ_i]以及该节点由何种输入控制量v_i(车辆速度)、φ_i(车辆前轮转向角)得来。

如图4所示，描述了利用混合A*算法对节点进行扩展的方法。假设坐标值表示为{x_i，y_i}的当前节点Node_current对应的运动状态为

要求车辆i以s_current为初始状态并在Δt时间内将v_i、φ_i固定为某给定值，通过前向模拟可得到一段始于s_current的路径，路径终点对应的节点即为一个子节点。在v_i∈{-v_max,v_max}、φ_i∈{-Φ_max,0,Φ_max}条件下至多可扩展6个子节点。

3)混合A*算法变更了搜索策略，引入RS曲线生成机制是混合A*算法的本质。RS曲线生成方法将所有的圆弧及直线段的排列组合方式归纳为48种，据此可对平面上的任意起、终点位姿实现衔接，并在车辆运动学意义下保证路径长度最短。尽管RS曲线无法避障，但其计算速度快，可以采取“先构造RS曲线再检验是否碰撞”的方式来增强混合A*算法的搜索能力，尤其是精准衔接终点位姿的能力。具体而言，在混合A*算法的迭代循环中，设置每隔N_RS代触发一次RS曲线生成，即从当前节点记录的准确位姿向终点位姿直接衔接，如果用于衔接的RS曲线恰好避障，则立刻终止搜索，输出从起点到当前节点的路径连同RS曲线，即完成路径生成的任务；相反，如果用于衔接的RS曲线与障碍物相撞，则退出RS曲线模式，随后继续正常搜索。

4)混合A＊算法更改了节点碰撞检验方式。与基本A＊算法直接在二维网格图中查询相比，混合A＊算法需依据节点运动状态具体数值[x_i，y_i，θ_i]获得车辆矩形轮廓，并验证车身轮廓所覆盖的二维网格中是否存在障碍物。

5)混合A＊算法更改了启发函数h(·)的定义。由于混合A＊算法旨在提供精度较高的路径，因此不适合沿用曼哈顿距离估算h(·)。理想的h(·)应反映从当前节点到终点的避障、运动学可行的路径的长度。为此，混合A＊算法使用两个子启发函数：h_{nonholonomics}(·)与h_{collision_avoidance}(·)，其中h_{nonholonomics}(·)代表一条符合车辆运动学约束但无视碰撞的路径的长度，h_{collision_avoidance}(·)代表一条考虑避障但未必运动学可行的路径的长度，据此我们定义h(·)＝max{h_{collision_avoidance}(·)，h_{nonholonomics}(·)}。混合A＊算法调用RS曲线来计算h_{nonholonomics}(·)，调用基本A＊算法来计算h_{collision_avoidance}(·)。需要澄清的是，这里调用RS曲线的步骤与第3)点中每隔一定迭代次数触发RS曲线的操作是相互独立的。

6)混合A＊算法修改了函数g(·)的定义。为防止混合A＊搜索得到形态扭曲、反复振荡的路径，在历史路径长度g(·)函数中适当施加对频繁切换输入控制量v_i、φ_i的惩罚。由于各节点记录自身由何种输入控制量扩展到，因此通过将当前子节点与父节点的输入控制量进行对比，可以评估控制量的偏差绝对值，将其补入g(·)中。频繁切换速度方向或前轮转向均会导致行车过程损耗时间或能源，虽然在路径生成时不考虑时间相关限制，但基于上述设计的g(·)会使得输出的路径更为合理。

总结起来，混合A＊算法的基本执行流程如下。本实施例利用其中的步骤1)至3)以及5)至15)来生成折线路径。

1)初始化动态集合

以及iter＝0，并将起始节点Node_start加入OpenList；

2)当OpenList不为空集或iter小于预设的最大阈值时，并循环执行步骤3)至15)；

3)iter加1，从OpenList选择f值最小的节点，记为Node_current；

4)如果iter能够被N_RS整除，使用RS方法生成衔接Node_current位姿与终点位姿的路径χ，如果χ能够避障，记录节点Node_current及χ，构建空向量

向Λ中存入Node_child，并执行步骤(14)；

5)扩展Node_current得到子节点保存于集合Γ；

6)如果Node_child∈Γ，并且is_closed(Node_child)＝1，执行步骤7)；

7)如果Node_child∈Γ，并且is_opened(Node_child)＝0，此时如果从Node_current到Node_child的局部路径不避障，设置is_closed(Node_child)＝1，并执行步骤8)；

8)计算Node_child的相应属性数值；

9)判断节点Node_child，如果Node_child是终止节点，则记录节点Node_child，构建空向量

向Λ中存入Node_child，并执行步骤14)至15)，如果节点Node_child不是终止节点，则将Node_child加入OpenList，并执行步骤10)；

10)如果Node_child∈Γ，并且is_opened(Node_child)＝1，执行步骤11)；

11)执行公式(2b)；

12)如果f^*(Node_child)＜f(Node_child)，将Node_child父节点置为Node_current并更新g(Node_child)、f(Node_child)，将Node_current从OpenList中移除；

13)如果(parent_id(Node_child)≠Null)，则向Λ中存入parent_id(Node_child)，将parent_id(Node_child)设置为Node_child，逆序输出向量Λ；

14)如果(parent_id(Node_current)≠Null)，向Λ中存入parent_id(Node_current)，将parent_id(Node_current)设置为Node_current；

15)将向量Λ按逆序重排，随后在其中存入χ，输出向量Λ。

利用混合A*算法生成折线路径，可以避障，而且可以保证规划的折线路径为最短路径，提高了泊车效率。

图5为本公开的泊车路径规划装置的一些实施例的示意图。

如图5所示，该实施例的泊车路径规划装置500包括：存储器501以及耦接至该存储器501的处理器502，处理器502被配置为基于存储在存储器501中的指令，执行前述任意一些实施例中的泊车路径规划方法。

其中，存储器501例如可以包括系统存储器、固定非易失性存储介质等。系统存储器例如存储有操作系统、应用程序、引导装载程序(Boot Loader)以及其他程序等。

装置500还可以包括输入输出接口503、网络接口504、存储接口505等。这些接口503，504，505以及存储器501和处理器502之间例如可以通过总线506连接。其中，输入输出接口503为显示器、鼠标、键盘、触摸屏等输入输出设备提供连接接口。网络接口504为各种联网设备提供连接接口。存储接口505为SD卡、U盘等外置存储设备提供连接接口。

图6为本公开的泊车路径规划系统的一些实施例的示意图。

如图6所示，该实施例的泊车路径规划系统包括：泊车路径规划装置601、传感器602。

泊车路径规划装置601，被配置为利用本公开任意一些实施例中的泊车路径规划方法进行泊车路径规划。泊车路径规划装置601例如可以是泊车路径规划装置500。

传感器602，被配置为感知车辆的位姿和路径上的障碍物。例如可以是利用激光雷达或图像扫描设备来实现。

泊车路径规划系统可以装配在车辆上，用来实现泊车路径规划。

本领域内的技术人员应当明白，本公开的实施例可提供为方法、系统、或计算机程序产品。因此，本公开可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本公开可采用在一个或多个其中包含有计算机可用程序代码的计算机可用非瞬时性存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本公开是参照根据本公开实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解为可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

以上所述仅为本公开的较佳实施例，并不用以限制本公开，凡在本公开的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本公开的保护范围之内。

Claims (10)

1.一种泊车路径规划方法，其特征在于，包括：

生成衔接起始位姿到中间的局部起始位姿的折线路径；

生成从局部起始位姿到终点位姿的可避障的参考折线路径；

生成从局部起始位姿到终点位姿的多条曲线路径；

从所述多条曲线路径中选择一条与参考折线路径的累加距离偏差最小的曲线路径作为衔接局部起始位姿到终点位姿的连通路径；

当所述连通路径上没有障碍物时，将衔接起始位姿到中间的局部起始位姿的折线路径和衔接局部起始位姿到终点位姿的连通路径拼接形成泊车路径。

2.根据权利要求1所述的泊车路径规划方法，其特征在于，所述生成从局部起始位姿到终点位姿的可避障的参考折线路径包括：

从当前节点的相邻节点中选择泊车路径代价最小且未被障碍物占据的相邻节点作为所述参考折线路径上的下一个节点，并将下一个节点作为下一次循环的当前节点，直到所述下一个节点为所述终点位姿对应的终点节点，其中，当前节点的初始值设置为局部起始位姿对应的局部起始节点，所述局部起始节点、选择出的节点以及终点节点依次连接形成所述参考折线路径。

3.根据权利要求1所述的泊车路径规划方法，其特征在于，所述生成从局部起始位姿到终点位姿的多条曲线路径包括：

使用RS曲线生成算法生成从局部起始位姿到终点位姿的多条曲线路径。

4.根据权利要求1所述的泊车路径规划方法，其特征在于，所述从所述多条曲线路径中选择一条与参考折线路径的累加距离偏差最小的曲线路径作为衔接局部起始位姿到终点位姿的连通路径包括：

利用若干第一剖分点将参考折线路径等分为预设个数的分段；

利用若干第二剖分点将多条曲线路径分别等分为所述预设个数的分段；

计算每一个第一剖分点与相应的第二剖分点的距离偏差并对各个所述距离偏差求和，将所述距离偏差之和最小时对应的曲线路径作为衔接局部起始位姿到终点位姿的连通路径。

5.根据权利要求1所述的泊车路径规划方法，其特征在于，

当所述连通路径上有障碍物时，继续生成衔接局部起始位姿到下一个局部起始位姿的折线路径。

6.根据权利要求1所述的泊车路径规划方法，其特征在于，所述生成衔接起始位姿到中间的局部起始位姿的折线路径包括：

利用基本A*算法或者利用混合A*算法的折线路径生成方法，生成衔接起始位姿到中间的局部起始位姿的折线路径。

7.一种泊车路径规划装置，其特征在于，包括：

折线路径生成模块，被配置为生成衔接起始位姿到中间的局部起始位姿的折线路径；

参考折线路径生成模块，被配置为生成从局部起始位姿到终点位姿的可避障的参考折线路径；

曲线路径生成模块，被配置为生成从局部起始位姿到终点位姿的多条曲线路径；

连通路径选择模块，被配置为从所述多条曲线路径中选择一条与参考折线路径的累加距离偏差最小的曲线路径作为衔接局部起始位姿到终点位姿的连通路径；

泊车路径生成模块，被配置为确定衔接起始位姿到中间的局部起始位姿的折线路径和衔接局部起始位姿到终点位姿的没有障碍物的连通路径为泊车路径。

8.一种泊车路径规划装置，其特征在于，包括：

存储器；以及

耦接至所述存储器的处理器，所述处理器被配置为基于存储在所述存储器中的指令，执行权利要求1-6中任一项所述的泊车路径规划方法。

9.一种泊车路径规划系统，其特征在于，包括：

如权利要求7或权利要求8所述的泊车路径规划装置；

传感器，被配置为感知车辆的位姿和路径上的障碍物。

10.一种非瞬时性计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现权利要求1-6中任一项所述的泊车路径规划方法。

Images