CN111397623A - 一种基于最佳泊车起始点的路径规划方法 - Google Patents

Abstract

本发明公开了一种基于最佳泊车起始点的路径融合方法。该方法规划的路径分为三部分：全局路径，过渡路径和泊车路径。首先在泊车位附近生成全局路径规划的目标区域，再用Jump Point Search(JPS)算法和Hybrid A*算法生成到目标区域的路径，完成全局路径规划。再根据最佳泊车起始点，以及全局路径的最终节点信息，生成过渡路径。最后通过C型垂直泊车生成泊车路径。将三条路径依次相连，即可得到最终路径。

Description

一种基于最佳泊车起始点的路径规划方法

技术领域

本发明涉及智能网联汽车技术领域，具体涉及一种基于最佳泊车起始点的路径规划方法。

背景技术

自动驾驶的快速发展，使得自动泊车成为人们关注研究热点之一。自动泊车系统能缓解城市的交通压力，并减少驾驶员的负担，使得泊车过程更加安全和高效。

作为自动泊车系统中的重要一环，路径规划决定着泊车过程是否顺利。现今市场上的自动泊车系统，大多要求驾驶员精确到达车位附近的某一个位置，才能开始进行泊车路径规划。对于驾驶员来说，如果自动泊车系统能从停车场开始规划路径，驾驶负担将会大大减少，并且省下大量时间。

发明内容

针对现有技术存在的不足，本发明的目的在于提供一种基于最佳泊车起始点的路径融合方法。该方法规划的路径分为三部分：全局路径，过渡路径和泊车路径。首先在泊车位附近生成全局路径规划的目标区域，再用Jump Point Search(JPS)算法和Hybrid A*算法生成到目标区域的路径，完成全局路径规划。再根据最佳泊车起始点，以及全局路径的最终节点信息，生成过渡路径。最后通过C型垂直泊车生成泊车路径。将三条路径依次相连，即可得到最终路径。

为实现上述目的，本发明提供了如下技术方案：一种基于最佳泊车起始点的路径规划方法，包括如下步骤：

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

步骤2：根据泊车目标点，确定全局目标点和全局目标区域；

步骤3：使用Jump Point Search(JPS)算法和Hybrid A*算法进行全局路径规划，生成从起始点到达全局目标区域的路径；

步骤4，通过C型垂直泊车中的碰撞约束条件，求出泊车起始横摆角的取值范围，以及每个泊车起始横摆角对应的最佳泊车起始点；

步骤5：根据全局路径的最终节点和最佳泊车起始点信息，生成全局规划路径与泊车路径之间的过渡路径；

步骤6：通过C型垂直泊车，生成泊车路径；

步骤7：连接规划出的全局路径，过渡路径和泊车路径，得到最终路径。

作为本发明的进一步改进，所述步骤2中，根据泊车目标点，确定全局目标点和全局目标区域具体过程如下：

全局目标点P_globalgoal(x_globalgoal,y_globalgoal,θ_globalgoal)以及全局目标区域Area_globalgoal的位置在车位附近；全局目标点P_globalgoal的横坐标x_globalgoal与泊车目标点P_parkinggoal的横坐标x_parkinggoal相同，纵坐标离为h_min；

其中h_min为在泊车起始横摆角为0的状态下，使用C型垂直泊车时，车辆与车位之间的最短距离，计算公式如下：

其中R_min为车辆的最小转向半径，W_c为车辆的宽度，W为车位的宽度。

全局目标区域是以P_globalgoal为圆心的圆，其半径R_l计算公式如下：

作为本发明的进一步改进，所述步骤3中，确定当前点，当前目标点及当前目标区域的具体过程如下：

令起始点S_start(x_start,y_start,θ_start)为当前点N_current(x_current,y_current,θ_current)，目标点列表中的第一个点P₁为当前目标点

目标区域中的第一个区域Area₁为当前目标区域Area_current。

作为本发明的进一步改进，所述步骤4中，求出泊车起始横摆角的取值范围具体过程如下：

泊车过程中，左右两侧的泊车路径对称，最大横摆角显然为

最小横摆角θ_min的计算过程如下所示：

W_s-S₁＝R_RF(sinε-sin(ε+θ_min))

其中，H为左侧障碍物与泊车目标点之间的距离，L_x为车的轴距长度，L_f为车的前悬长度。

作为本发明的进一步改进，所述步骤4中，每个泊车起始横摆角对应的最佳泊车起始点确定方法如下：

碰撞约束为：

其中，R_min为车辆最小转向半径，W_c为车辆宽度，L_b为车辆后悬长度，W_o为车位宽度，L_o为车位宽度，L_x为车辆轴宽，L_f为车辆前悬宽度，d为安全距离，H₁为右侧障碍物的上边界与泊车目标点之间的距离。

上式中，在θ为某一给定值θ_i的情况下，未知量仅为R和L₁，可通过已知的碰撞约束求出R和L₁的取值范围；

当θ为θ_i，R和L₁的取值范围已知时，通过目标函数，即可求出R和L₁的最优值，目标函数如下：

求出R和L₁的最优值后，通过公式求出泊车起始点的横坐标，即可求得θ_i对应的最佳泊车起始点

作为本发明的进一步改进，所述步骤5中，生成过渡路径的具体过程如下：

Ⅰ.令全局路径的最终节点P_globalend为当前点N‘_i；

Ⅱ.用Hybrid A*拓展节点的方式，拓展当前点N‘_i，得到新节点N‘_i+1，N‘_i+2…N‘_i+7，并计算新节点的δ值；

Ⅲ.检查新节点中，是否有节点的δ值小于预设误差值E，如果没有，则将所有新节点放入Open列表中，并令δ值最小的节点为当前节点，返回Ⅱ；如果有，则在新节点中挑选δ值最小的节点，用直线段拓展该节点，直至与最佳泊车起始点距离最短，此时车辆位置即为泊车起始点

作为本发明的进一步改进，所述δ的计算方法如下：

生成一条与圆弧N‘_i+1N‘_i+5相切的直线，节点

是θ_i+5对应的最佳泊车起始点，节点

是红色直线中与

距离最近的节点，两点之间的距离，即为δ_i+5。

本发明的有益效果：

1.提出了一种路径规划方法，规划出的路径能从停车场入口开始引导车辆，直到车辆驶入车位。相比于现在市场上已有的路径规划技术，该方法能更大程度上减少驾驶员的负担，并使得泊车过程更加高效。

2.提出了一种融合全局路径和泊车路径的方法。该方法解决了全局路径和泊车路径之间的转换问题，并保证泊车起始点接近最优。

3.提出了一种计算最佳泊车起始点的方法。相比于已有的泊车路径规划技术，该方法使得泊车起始点的选取更加自由，增大了泊车选择的灵活性，使泊车成功几率更高。

附图说明

图1为路径规划算法的总体流程图；

图2为坐标系示意图；

图3为全局目标区域示意图；

图4为两侧泊车对称示意图；

图5为h_min的计算示意图；

图6为过渡路径示意图；

图7为C型垂直泊车过程示意图；

图8为车辆左前点碰撞示意图；

图9为车辆右前点碰撞示意图；

图10为过渡路径示意图；

图11为C型垂直泊车过程示意图。

具体实施方式

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

参照图1所示，本实施例的一种基于最佳泊车起始点的路径融合方法。该方法规划的路径分为三部分：全局路径，过渡路径和泊车路径。首先在泊车位附近生成全局路径规划的目标区域，再用JPS算法和Hybrid A*算法生成到目标区域的路径，完成全局路径规划。再根据最佳泊车起始点，以及全局路径的最终节点信息，生成过渡路径。最后通过C型垂直泊车生成泊车路径。将三条路径依次相连，即可得到最终路径因此，方案实施过程如图1所示，主要包含如下过程：

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

步骤2：根据泊车目标点，确定全局目标点和全局目标区域。

步骤3：使用Jump Point Search(JPS)算法和Hybrid A*算法进行全局路径规划，生成从起始点到达全局目标区域的路径。

步骤4，通过C型垂直泊车中的碰撞约束条件，求出泊车起始横摆角的取值范围，以及每个泊车起始横摆角对应的最佳泊车起始点。

步骤5：根据全局路径的最终节点和最佳泊车起始点信息，生成全局规划路径与泊车路径之间的过渡路径。

步骤6：通过C型垂直泊车，生成泊车路径。

步骤7：连接规划出的全局路径，过渡路径和泊车路径，得到最终路径。

下面一一予以细述。

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

路径规划系统获得地图信息，设定起始点S_start(x_start,y_start,θ_start)，和泊车目标点P_parkinggoal(x_parkinggoal,y_parkinggoal,θ_parkinggoal)。坐标系的建立如图2所示，取泊车目标点P_parkinggoal为坐标原点，其中d为安全距离。

步骤2：根据泊车目标点，确定全局目标点和全局目标区域。

全局目标点P_globalgoal(x_globalgoal,y_globalgoal,θ_globalgoal)以及全局目标区域Area_globalgoal的位置在车位附近，如图3所示。全局目标点P_globalgoal的横坐标x_globalgoal与泊车目标点P_parkinggoal的横坐标x_parkinggoal相同，纵坐标离为h_min。其中h_min为在泊车起始横摆角为0的状态下，使用C型垂直泊车时，车辆与车位之间的最短距离，计算公式如下：

其中R_min为车辆的最小转向半径，W_c为车辆的宽度，W为车位的宽度。

全局目标区域是以P_globalgoal为圆心的圆，其半径R_l计算公式如下：

步骤3：使用Jump Point Search(JPS)算法和Hybrid A*算法进行全局路径规划，生成从起始点到达全局目标区域的路径。

设JPS找到的路径点为{P₁,P₂…P_n}，则以以各个路径点{P₁,P₂…P_globalgoal}为圆心，R_l为半径，生成对应的目标区域{Area₁,Area₂…Area_globalgoal}，再用Hybrid A*算法依次生成到达各个目标区域路径，直到到达最终目标区域Area_globalgoal。步骤4，通过C型垂直泊车中的碰撞约束条件，求出泊车起始横摆角的取值范围，以及每个泊车起始横摆角对应的最佳泊车起始点。

首先确定泊车起始横摆角的取值范围。泊车过程中，左右两侧的泊车路径对称，如图4所示，因此只讨论右侧的泊车情景。最大横摆角显然为

最小横摆角θ_min的计算过程如图5所示。

W_s-S₁＝R_RF(sinε-sin(ε+θ_min))

其中H为左侧障碍物与泊车目标点之间的距离，L_x为车的轴距长度，L_f为车的前悬长度。

在已知泊车起始横摆角θ范围的情况下，根据碰撞约束条件及目标函数，确定最佳泊车起始点。

避障约束为：

其中，R_min为车辆最小转向半径，W_c为车辆宽度，L_b为车辆后悬长度，W_o为车位宽度，L_o为车位宽度，L_x为车辆轴宽，L_f为车辆前悬宽度，d为安全距离，H₁为右侧障碍物的上边界与泊车目标点之间的距离。

上式中，在θ为某一给定值θ_i的情况下，未知量仅为R和L₁。可通过上式求出R和L₁的取值范围。

当θ为θ_i，R和L₁的取值范围已知时，通过目标函数，即可求出R和L₁的最优值。

目标函数如下：

求出R和L₁的最优值后，如图7所示，通过公式求出泊车起始点的横坐标，即可求得θ_i对应的最佳泊车起始点

步骤5：根据全局路径的最终节点和最佳泊车起始点信息，生成全局规划路径与泊车路径之间的过渡路径。

Ⅰ.令全局路径的最终节点P_globalend为当前点N‘_i。

Ⅱ.如图10所示，用Hybrid A*拓展节点的方式，拓展当前点N‘_i，得到新节点N‘_i+1，N‘_i+2…N‘_i+7，并计算新节点的δ值。以新节点

为例，δ的计算方法如下：

如图10所示，红色直线与圆弧N‘_i+1N‘_i+5相切，节点

是θ_i+5对应的最佳泊车起始点，节点

是红色直线中与

距离最近的节点，两点之间的距离，即为δ_i+5。同理，即可求得其他新节点的δ值。

Ⅲ.检查新节点中，是否有节点的δ值小于预设误差值E。如果没有，则将所有新节点放入Open列表中，并令δ值最小的节点为当前节点，返回Ⅱ；如果有，则在新节点中挑选δ值最小的节点，按图6所示，用直线段拓展该节点，直至与最佳泊车起始点距离最短。此时车辆位置即为泊车起始点

步骤6：通过C型垂直泊车，生成泊车路径。

C型垂直泊车过程如图11所示，在已知泊车起始点

后，即可计算路径参数：

计算出路径参数R和L₁后，即可规划出泊车路径。

步骤7：连接规划出的全局路径，过渡路径和泊车路径，得到最终路径。

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

Claims (7)

1.一种基于最佳泊车起始点的路径规划方法，其特征在于：包括如下步骤：

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

步骤2：根据泊车目标点，确定全局目标点和全局目标区域；

步骤3：使用Jump Point Search(JPS)算法和Hybrid A*算法进行全局路径规划，生成从起始点到达全局目标区域的路径；

步骤4，通过C型垂直泊车中的碰撞约束条件，求出泊车起始横摆角的取值范围，以及每个泊车起始横摆角对应的最佳泊车起始点；

步骤5：根据全局路径的最终节点和最佳泊车起始点信息，生成全局规划路径与泊车路径之间的过渡路径；

步骤6：通过C型垂直泊车，生成泊车路径；

步骤7：连接规划出的全局路径，过渡路径和泊车路径，得到最终路径。

2.根据权利要求1所述的基于最佳泊车起始点的路径规划方法，其特征在于：所述步骤2中，根据泊车目标点，确定全局目标点和全局目标区域具体过程如下：

全局目标点P_globalgoal(x_globalgoal,y_globalgoal,θ_globalgoal)以及全局目标区域Area_globalgoal的位置在车位附近；全局目标点P_globalgoal的横坐标x_globalgoal与泊车目标点P_parkinggoal的横坐标x_parkinggoal相同，纵坐标离为h_min；

其中h_min为在泊车起始横摆角为0的状态下，使用C型垂直泊车时，车辆与车位之间的最短距离，计算公式如下：

其中R_min为车辆的最小转向半径，W_c为车辆的宽度，W为车位的宽度。

全局目标区域是以P_globalgoal为圆心的圆，其半径R_l计算公式如下：

3.根据权利要求1或2所述的基于最佳泊车起始点的路径规划方法，其特征在于：所述步骤3中，确定当前点，当前目标点及当前目标区域的具体过程如下：令起始点S_start(x_start,y_start,θ_start)为当前点N_current(x_current,y_current,θ_current)，目标点列表中的第一个点P₁为当前目标点Goal_current(x_currentgoal,y_currentgoal)，目标区域中的第一个区域Area₁为当前目标区域Area_current。

4.根据权利要求1或2所述的基于最佳泊车起始点的路径规划方法，其特征在于：所述步骤4中，求出泊车起始横摆角的取值范围具体过程如下：

泊车过程中，左右两侧的泊车路径对称，最大横摆角显然为

最小横摆角θ_min的计算过程如下所示：

W_s-S₁＝R_RF(sinε-sin(ε+θ_min))

其中，H为左侧障碍物与泊车目标点之间的距离，L_x为车的轴距长度，L_f为车的前悬长度。

5.根据权利要求4所述的基于最佳泊车起始点的自主泊车路径规划方法，其特征在于：所述步骤4中，每个泊车起始横摆角对应的最佳泊车起始点确定方法如下：

碰撞约束为：

在θ为某一给定值θ_i的情况下，未知量仅为R和L₁，可通过已知的碰撞约束求出R和L₁的取值范围；

当θ为θ_i，R和L₁的取值范围已知时，通过目标函数，即可求出R和L₁的最优值，目标函数如下：

求出R和L₁的最优值后，通过公式求出泊车起始点的横坐标，即可求得θ_i对应的最佳泊车起始点

6.根据权利要求1或2所述的基于最佳泊车起始点的自主泊车路径规划方法，其特征在于：所述步骤5中，生成过渡路径的具体过程如下：

Ⅰ.令全局路径的最终节点P_globalend为当前点N‘_i；

Ⅱ.用Hybrid A*拓展节点的方式，拓展当前点N‘_i，得到新节点N‘_i+1，N’_i+2…N‘_i+7，并计算新节点的δ值；

Ⅲ.检查新节点中，是否有节点的δ值小于预设误差值E，如果没有，则将所有新节点放入Open列表中，并令δ值最小的节点为当前节点，返回Ⅱ；如果有，则在新节点中挑选δ值最小的节点，用直线段拓展该节点，直至与最佳泊车起始点距离最短，此时车辆位置即为泊车起始点

7.根据权利要求6所述的基于最佳泊车起始点的自主泊车路径规划方法，其特征在于：所述δ的计算方法如下：

生成一条与圆弧N‘_i+1N’_i+5相切的直线，节点

是θ_i+5对应的最佳泊车起始点，节点

是红色直线中与

距离最近的节点，两点之间的距离，即为δ_i+5。

Images