CN111158366A - 基于图搜索和几何曲线融合的路径规划方法 - Google Patents

Abstract

本发明公开了一种基于图搜索和几何曲线融合的路径规划方法，包括如下步骤：步骤1：得到地图信息，确定车辆的起始点和目标点；步骤2：通过Hybrid A*算法对节点进行拓展，得到新的节点及其状态信息；步骤3：判断新节点在直行状态下，是否能与目标点所在射线相交；步骤4：判断新节点能否通过几何曲线路径到达目标点；步骤5：判断ProState集合是否为空集。本发明的基于图搜索和几何曲线融合的路径规划方法，解决了传统的Hybrid A*无法精确到达目标点、无法满足车辆目标横摆角要求的问题。

Description

基于图搜索和几何曲线融合的路径规划方法

技术领域

本发明涉及智能网联汽车技术领域，更具体的说是涉及一种基于图搜索和几何曲线融合的路径规划方法。

背景技术

随着传感技术和人工智能的不断发展，自动驾驶系统的发展日益成熟。自动驾驶系统能减少驾驶员的操作负担，并减少交通事故发生的概率，将在生活中扮演愈发重要的角色。路径规划是自动驾驶系统中的关键一环，主要是在先验地图中，结合当前环境信息，规划出一条从起始点到目标点的可行路径。

目前，解决路径规划问题主要是用A*算法，是一种图搜索算法。A*算法在搜索的过程中，根据与目标点相关的启发式信息，朝着有利的方向展开搜索，可以避免许多无意义的搜索路径，大大减少搜索范围、降低问题的复杂度。但是，传统的A*算法规划出的路径不满足车辆的动力学约束，不适合车辆行驶。 Hybrid A*算法在传统A*算法的基础上，加入了车辆动力学约束，能够规划出一条车辆可行驶的路径。但Hybrid A*算法无法精确到达目标点，且未考虑车辆终点的角度约束。Hybrid A*算法与Dubins算法的融合，使得车辆能够精确到达目标点，但计算量较大，不适合车辆的实时规划。

发明内容

针对现有技术存在的不足，本发明的目的在于提供一种基于图搜索和几何曲线融合的路径规划方法。该方法前期采用Hybrid A*算法进行路径搜索，逐渐靠近目标点。在搜索过程中，不断检测是否能够通过几何曲线路径规划到达目标点。当满足几何曲线规划的条件时，将规划出一条由直线和圆弧组成的路径，直接到达目标点，完成路径规划。

为实现上述目的，本发明提供了如下技术方案：一种基于图搜索和几何曲线融合的路径规划方法，包括如下步骤：

步骤1：得到地图信息，确定车辆的起始点和目标点；

步骤2：通过Hybrid A*算法对节点进行拓展，得到新的节点及其状态信息；

步骤3：判断新节点在直行状态下，是否能与目标点所在射线相交，若能，则进

行步骤4；若不能，则计算该节点的f、h和g值，并将该节点放入Open集中，进行步骤5；

步骤4：判断新节点能否通过几何曲线路径到达目标点，若不能，则计算该节点的ΔL值，并将该节点放入ProState集合中，进行步骤5；若能，则采用几何路径到达目标点，并进行路径回溯；

步骤5：判断ProState集合是否为空集，若不为空集，则从ProState集合中挑出最佳节点，返回步骤2；若ProState集合为空集，则从Open中挑出最佳节点，返回步骤2。

作为本发明的进一步改进，所述步骤2用Hybrid A*算法对节点进行拓展的具体步骤如下：

步骤21，记当前节点状态为：(x_current，y_current，〖heading〗_current，x_discurrent， y_discurrent)，其中，x_current为当前状态的横坐标，y_current为当前状态的纵坐标，〖heading〗_current为当前状态的横摆角；

步骤22，记车辆最大转向角为α_max，则车辆可用转向角为：

α＝(-α_max，-α_max/2，0，α_max/2，α_max)；

令车辆轴距为L_x，每次拓展路径即：车辆以固定转向角前行，直至与当前点之间的距离为L时，所行驶过的路径，则得到新节点状态：

(x_new，y_new，heading_new，x_disnew，y_disnew)的公式为

得到新节点的状态信息后，计算新节点与障碍物之间的距离是否大于安全距离。若小于安全距离，则放弃节点，不对该节点做任何处理。

作为本发明的进一步改进，所述步骤3中，判断新节点在直行状态下，是否能与目标点所在射线相交具体步骤如下：

步骤31，记目标点的状态为(x_goal，y_goal，heading_goal，x_disgoal，y_disgoal)，则射线方程为：

y＝tan(heading_goal)x-tan(heading_goal)x_goal+y_goal；

其中，x的取值范围是：

新节点所在直线方程为：

y＝tan(heading_newl)x-tan(heading_new)x_new+y_new；

通过求解方程，即可确定新节点在直行状态下是否能与目标点所在射线相交。作为本发明的进一步改进，所述步骤3中，计算该节点的f、h和g值的计算公式如下：

f＝g+h；

其中，g为新节点路径回溯至起始点的路径长度值。

作为本发明的进一步改进，所述步骤3中，节点放入Open集中具体过程为：步骤31，检测新节点的离散坐标与Open中已存在节点的离散坐标是否发生重合；若未发生重合，则将该节点放入Open集中；若发生重合，则比较两个节点的g值大小；若新节点的g值较小，则将新节点代替旧节点放入Open中；若新节点的g值较大，则放弃新节点，不对该节点做任何处理。

作为本发明的进一步改进，所述步骤4中，规划几何曲线路径分两种情况，分别是直线圆弧路径和圆弧直线路径，具体规划步骤如下：

步骤41，直线圆弧路径情况为：若B点到目标点goal的距离L₁小于新节点new 到B点的距离L₂，则几何路径为：车辆从新节点位置向前直行一段长度为L₂-L₁的线段至C点，然后朝目标方向转动方向盘，以R为转向半径，行驶一段弧度为θ₁的圆弧至目标点；

其中，R和θ₁计算公式为：

R＝Ltanβ₃；

θ₁＝π-2β₃；

L＝L₁；

圆弧直线路径情况为：若B点到目标点goal的距离L₁大于新节点new到B点的距离L₂，则几何路径为：车辆从新节点位置朝目标方向转动方向盘，以R为转向半径，行驶一段弧度为θ₁的圆弧直至与目标射线相切于C点，然后向前直行一段长度为L₂-L₁的线段至目标点goal；

其中，R和θ₁计算公式为：

R＝Ltanβ₃；

θ₁＝π-2β₃；

L＝L₁；

步骤42，确认几何路径后，若该路径与障碍物之间的距离大于安全距离，则可采用该路径到达目标点，否则放弃该节点，不对该节点做任何处理。

作为本发明的进一步改进，所述步骤4中，判断新节点能否通过几何路径到达目标点的具体过程为：

步骤43，记新节点直行状态下，与目标射线的交点为B，B点与目标点之间的距离为L₁，当且仅当L₁满足下面公式，且规划出的路径与障碍物的距离大于安全距离时，新节点才能通过几何路径达到目标点，公式如下：

L₁≥d_min；

其中，d_min的计算公式为：

β₁＝heading_new；

β₂＝π-heading_goal。

作为本发明的进一步改进，所述步骤5中，从ProState集合中挑出最佳节点的具体过程为：从ProState集合中挑出ΔL值最小的节点，当作最佳节点。

作为本发明的进一步改进，从Open中挑出最佳节点的具体过程为：从Open中挑出f值最小的节点，当作最佳节点。

本发明的有益效果，1.提出了基于图搜索和几何曲线融合的路径规划方法，该方法在前期通过Hybrid A*算法靠近目标点，并在合适时刻采用几何曲线路径规划算法，规划出到达目标点的路径。解决了传统的Hybrid A*无法精确到达目标点、无法满足车辆目标横摆角要求的问题；

2.提出了一种判断能否使用几何曲线进行路径规划的方法。该检测方法与Hybrid A*算法的融合，能够使算法快速判断在当前状态下是否应转变路径规划算法，并规划出一条考虑车辆动力学约束的路径。相比于Hybrid A*+Dubins算法，该算法的效率更高，实时性更强。

附图说明

图1是目标射线的示意图；

图2是拓展节点与目标射线不相交的示意图；

图3是拓展节点与目标射线相交的示意图；

图4是d_min的计算示意图；

图5是几何曲线路径为直线圆弧的示意图；

图6是几何曲线路径为圆弧直线的示意图；

图7是转向半径R及转向弧度θ₁的计算示意图；

图8是不满足几何曲线条件的示意图；

图9是本发明方法的实施流程图。

具体实施方式

下面将结合附图所给出的实施例对本发明做进一步的详述。

本实施例的一种基于图搜索和几何曲线融合的路径规划方法，前期采用HybridA*算法进行路径搜索，逐渐靠近目标点。在搜索过程中，不断检测是否能够通过几何曲线路径规划到达目标点。当满足几何曲线规划的条件时，将规划出一条由直线和圆弧组成的路径，直接到达目标点，完成路径规划。

因此，方案实施过程如图9所示，主要包含如下过程：

步骤1：得到地图信息，确定车辆的起始点和目标点。

步骤2：通过Hybrid A*算法对节点进行拓展，得到新的节点及其状态信息。

步骤3：判断新节点在直行状态下，是否能与目标点所在射线相交；若不能，则计算该节点的f、h和g值，并将该节点放入Open集中，进行步骤5；若能，则进行步骤4。

步骤4：判断新节点能否通过几何曲线路径到达目标点，若不能，则计算该节点的ΔL值，并将该节点放入ProState集合中，进行步骤5；若能，则采用几何路径到达目标点，并进行路径回溯。

步骤5：判断ProState集合是否为空集，若不为空集，则从ProState集合中挑出最佳节点，返回步骤2；若ProState集合为空集，则从Open中挑出最佳节点，返回步骤2。

下面一一予以细述步骤1：得到地图信息，确定车辆的起始点和目标点。

路径规划系统获得地图信息，设定起始点和目标点，并将起始点作为当前点。步骤2：通过Hybrid A*算法对节点进行拓展，得到新的节点及其状态信息。

记当前节点状态为：(x_current，y_current，heading_current，x_discurrent，y_discurrent)。其中，x_current为当前状态的横坐标，y_current为当前状态的纵坐标，heading_current为当前状态的横摆角。

记车辆最大转向角为α_max，则车辆可用转向角为：

令车辆轴距为L_x，每次拓展路径即：车辆以固定转向角前行，直至与当前点之间的距离为L时，所行驶过的路径。得到新节点状态： (x_new，y_new，heading_new，x_disnew，y_disnew)的公式为：

得到新节点的状态信息后，计算新节点与障碍物之间的距离是否大于安全距离。若小于安全距离，则放弃节点，不对该节点做任何处理。

步骤3：判断新节点在直行状态下，是否能与目标点所在射线相交。若不能，则计算该节点的f、h和g值，并将该节点放入Open集中，进行步骤5；若能，则进行步骤4。

1.判断新节点在直行状态下，是否能与目标点所在射线相交。

目标点所在射线如图1所示，记目标点的状态为 (x_goal，y_goal，heading_goal，x_disgoal，y_disgoal)，则射线方程为：

y＝tan(heading_goal)x-tan(heading_goal)x_goal+y_goal

其中，x的取值范围是：

新节点所在直线方程为：

y＝tan(heading_newl)x-tan(heading_new)x_new+y_new

通过求解方程，即可确定新节点在直行状态下是否能与目标点所在射线相交。若如图2所示不相交，则进行步骤5；若如图3所示相交，则进行步骤4。

2.若新节点不与射线相交，则计算节点的f，g和h值，并放入Open中

f＝g+h

其中，g为新节点路径回溯至起始点的路径长度值。

检测新节点的离散坐标与Open中已存在节点的离散坐标是否发生重合。若未发生重合，则将该节点放入Open集中。若发生重合，则比较两个节点的g值大小。若新节点的g值较小，则将新节点代替旧节点放入Open中；若新节点的g值较大，则放弃新节点，不对该节点做任何处理。

步骤4：判断新节点能否通过几何曲线路径到达目标点，若不能，则计算该节点的ΔL值，并将该节点放入ProState集合中，进行步骤5；若能，则采用几何路径到达目标点，并进行路径回溯。

1.判断新节点能否通过几何曲线路径到达目标点

记新节点直行状态下，与目标射线的交点为B，B点与目标点之间的距离为L₁，当且仅当L₁满足下面公式，且规划出的路径与障碍物的距离大于安全距离时，新节点才能通过几何路径达到目标点。

L₁≥d_min

其中，d_min的计算公式如图4所示，计算过程如下：

上述公式满足时，即可规划几何曲线，并判断与障碍物之间的距离是否大于安全距离。

几何曲线路径规划分两种情况，分别是直线圆弧路径和圆弧直线路径。

直线圆弧路径如图5所示，若B点到目标点goal的距离L₁小于新节点new到B 点的距离L₂，则几何路径为：车辆从新节点位置向前直行一段长度为L₂-L₁的线段至C点，然后朝目标方向转动方向盘，以R为转向半径，行驶一段弧度为θ₁的圆弧至目标点。

其中，R和θ₁如图7所示，计算公式为：

R＝Ltanβ₃

θ₁＝π-2β₃

L＝L₁

圆弧直线路径如图6所示，若B点到目标点goal的距离L₁大于新节点new到B 点的距离L₂，则几何路径为：车辆从新节点位置朝目标方向转动方向盘，以R为转向半径，行驶一段弧度为θ₁的圆弧直至与目标射线相切于C点，然后向前直行一段长度为L₂-L₁的线段至目标点goal。

其中，R和θ₁如图7所示，计算公式为：

R＝Ltanβ₃

θ₁＝π-2β₃

L＝L₁

确认几何路径后，若该路径与障碍物之间的距离大于安全距离，则可采用该路径到达目标点，开始路径回溯；否则放弃该节点，不对该节点做任何处理。

2.路径回溯

路径回溯过程为：以目标点为当前节点，将当前节点到其父节点的路径纳入可用路径中。再将其父节点作为当前点，重复上述过程，直到当前点为起始点。此时得到的可用路径，即为最终得到的路径。

3.该节点不可采用几何曲线路径到达目标点时，计算该节点的ΔL值，并将该节点放入ProState集合中

ΔL如图8所示，计算公式如下：

ΔL＝d_min-L₂

步骤5：判断ProState集合是否为空集，若不为空集，则从ProState集合中挑出最佳节点，返回步骤2；若ProState集合为空集，则从Open中挑出最佳节点，返回步骤2。

从ProState集合中挑出最佳节点的具体过程为：从ProState集合中挑出ΔL值最小的节点，当作最佳节点。从Open中挑出最佳节点的具体过程为：从Open中挑出f值最小的节点，当作最佳节点。

选出最佳节点后，即可返回步骤2，开始下一个路径搜索周期。

综上所述，本实施例的路径规划方法，在基于Hybrid A*算法的基础上结合了几何曲线路径规划算法，可有效的规划出到达目标点的路径。

以上所述仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (9)

1.一种基于图搜索和几何曲线融合的路径规划方法，其特征在于：包括如下步骤：

步骤1：得到地图信息，确定车辆的起始点和目标点；

步骤2：通过Hybrid A*算法对节点进行拓展，得到新的节点及其状态信息；

步骤3：判断新节点在直行状态下，是否能与目标点所在射线相交，若能，则进行步骤4；若不能，则计算该节点的f、h和g值，并将该节点放入Open集中，进行步骤5；

步骤4：判断新节点能否通过几何曲线路径到达目标点，若不能，则计算该节点的ΔL值，并将该节点放入ProState集合中，进行步骤5；若能，则采用几何路径到达目标点，并进行路径回溯；

步骤5：判断ProState集合是否为空集，若不为空集，则从ProState集合中挑出最佳节点，返回步骤2；若ProState集合为空集，则从Open中挑出最佳节点，返回步骤2。

2.根据权利要求1所述的基于图搜索和几何曲线融合的路径规划方法，其特征在于：所述步骤2用Hybrid A*算法对节点进行拓展的具体步骤如下：

步骤21，记当前节点状态为：(x_current，y_current，〖heading〗_current，x_discurrent，y_discurrent)，其中，x_current为当前状态的横坐标，y_current为当前状态的纵坐标，〖heading〗_current为当前状态的横摆角；

步骤22，记车辆最大转向角为α_max，则车辆可用转向角为：

α＝(-α_max，-α_max/2，0，α_max/2，α_max)；

令车辆轴距为L_x，每次拓展路径即：车辆以固定转向角前行，直至与当前点之间的距离为L时，所行驶过的路径，则得到新节点状态：

(x_new，y_new，heading_new，x_disnew，y_disnew)的公式为

得到新节点的状态信息后，计算新节点与障碍物之间的距离是否大于安全距离。若小于安全距离，则放弃节点，不对该节点做任何处理。

3.根据权利要求1或2所述的基于图搜索和几何曲线融合的路径规划方法，其特征在于：所述步骤3中，判断新节点在直行状态下，是否能与目标点所在射线相交具体步骤如下：

步骤31，记目标点的状态为(x_goal，y_goal，heading_goal，x_disgoal，y_disgoal)，则射线方程为：

y＝tan(heading_goal)x-tan(heading_goal)x_goal+y_goal；

其中，x的取值范围是：

新节点所在直线方程为：

y＝tan(heading_newl)x-tan(heading_new)x_new+y_new；

通过求解方程，即可确定新节点在直行状态下是否能与目标点所在射线相交。

4.根据权利要求3所述的基于图搜索和几何曲线融合的路径规划方法，其特征在于：所述步骤3中，计算该节点的f、h和g值的计算公式如下：

f＝g+h；

其中，g为新节点路径回溯至起始点的路径长度值。

5.根据权利要求4所述的基于图搜索和几何曲线融合的路径规划方法，其特征在于：所述步骤3中，节点放入Open集中具体过程为：

步骤31，检测新节点的离散坐标与Open中已存在节点的离散坐标是否发生重合；若未发生重合，则将该节点放入Open集中；若发生重合，则比较两个节点的g值大小；若新节点的g值较小，则将新节点代替旧节点放入Open中；若新节点的g值较大，则放弃新节点，不对该节点做任何处理。

6.根据权利要求1或2所述的基于图搜索和几何曲线融合的路径规划方法，其特征在于：所述步骤4中，规划几何曲线路径分两种情况，分别是直线圆弧路径和圆弧直线路径，具体规划步骤如下：

步骤41，直线圆弧路径情况为：若B点到目标点goal的距离L₁小于新节点new到B点的距离L₂，则几何路径为：车辆从新节点位置向前直行一段长度为L₂-L₁的线段至C点，然后朝目标方向转动方向盘，以R为转向半径，行驶一段弧度为θ₁的圆弧至目标点；

其中，R和θ₁计算公式为：

R＝Ltanβ₃；

θ₁＝π-2β₃；

L＝L₁；

圆弧直线路径情况为：若B点到目标点goal的距离L₁大于新节点new到B点的距离L₂，则几何路径为：车辆从新节点位置朝目标方向转动方向盘，以R为转向半径，行驶一段弧度为θ₁的圆弧直至与目标射线相切于C点，然后向前直行一段长度为L₂-L₁的线段至目标点goal；

其中，R和θ₁计算公式为：

R＝Ltanβ₃；

θ₁＝π-2β₃；

L＝L₁；

步骤42，确认几何路径后，若该路径与障碍物之间的距离大于安全距离，则可采用该路径到达目标点，否则放弃该节点，不对该节点做任何处理。

7.根据权利要求6所述的基于图搜索和几何曲线融合的路径规划方法，其特征在于：所述步骤4中，判断新节点能否通过几何路径到达目标点的具体过程为：步骤43，记新节点直行状态下，与目标射线的交点为B，B点与目标点之间的距离为L₁，当且仅当L₁满足下面公式，且规划出的路径与障碍物的距离大于安全距离时，新节点才能通过几何路径达到目标点，公式如下：

L₁≥d_min；

其中，d_min的计算公式为：

β₁＝heading_new；

β₂＝π-heading_goal。

8.根据权利要求1或2所述的基于图搜索和几何曲线融合的路径规划方法，其特征在于：所述步骤5中，从ProState集合中挑出最佳节点的具体过程为：从ProState集合中挑出ΔL值最小的节点，当作最佳节点。

9.根据权利要求8所述的基于图搜索和几何曲线融合的路径规划方法，其特征在于：从Open中挑出最佳节点的具体过程为：从Open中挑出f值最小的节点，当作最佳节点。

Images