CN112172799B

CN112172799B - 一种狭小垂直泊车路径规划方法

Info

Publication number: CN112172799B
Application number: CN202011099875.3A
Authority: CN
Inventors: 管欣; 蔡磊; 贾鑫; 张浩伦; 周芝瑶; 詹军
Original assignee: Jilin University
Current assignee: Jilin University
Priority date: 2020-10-15
Filing date: 2020-10-15
Publication date: 2021-09-24
Anticipated expiration: 2040-10-15
Also published as: CN112172799A

Abstract

本发明是一种狭小垂直泊车路径规划方法。包括以下步骤：步骤一、终点位置确定，并从终点位置到车位外进行规划，根据车位走廊的宽度及车辆当前的位置来确定在停车过程中执行一次机动停车或二次机动停车或三次机动停车；步骤二、起始停车位置确定目标线；步骤三、确定目标线设置后，从当前位置选择目标线，并规划路径到达起始停车位置；步骤四、到达起始停车位置时，跟随规划路径。本发明可根据泊车位参数和车辆参数确定一次机动停车、两次机动停车和三次机动停车可执行的最小走廊宽度，能够为更加多变的交通情况适应，如车位狭小与停车走廊空间狭小的情况。具有拟人化的泊车路径规划方式，若乘坐驾驶员，能够满足驾驶员的心理接受能力。

Description

一种狭小垂直泊车路径规划方法

技术领域

本发明属于汽车技术领域，具体的说是一种狭小垂直泊车路径规划方法。

背景技术

2003年，自动泊车技术首次商业化。自动泊车越来越多地安装在车辆上。停车位可分为平行停车、垂直停车和倾斜停车。城市人口密度高、面积有限，随着汽车数量不断增加，停车位也越来越窄。由于空间狭小，机动次数增加，给驾驶员带来了很大的负担。

发明内容

本发明提供了一种狭小垂直泊车路径规划方法，该方法可根据泊车位参数和车辆参数确定一次机动停车、两次机动停车和三次机动停车可执行的最小走廊宽度。在特定的走廊空间中，提出了三种停车方式的选择，分别为一次机动、二次机动和三次机动，以满足在狭小车位环境要求，解决在垂直泊车场景下，车位狭小而无法停车的问题。

本发明技术方案结合附图说明如下：

一种狭小垂直泊车路径规划方法，包括以下步骤：

步骤一、终点位置确定，并从终点位置到车位外进行规划，该过程与车辆从现场取出的路径规划一致；起始停车位置是方位角为零的位置，根据车位走廊的宽度及车辆当前的位置来确定在停车过程中执行一次机动停车或二次机动停车或三次机动停车；

步骤二、起始停车位置确定目标线；目标线是与起始停车位置相切的线，目标线组是在不同起始停车位置处的目标线的集合；

步骤三、确定目标线设置后，从当前位置选择目标线，并规划路径到达起始停车位置；

步骤四、到达起始停车位置时，跟随规划路径。

所述步骤一的具体方法如下：

若车位走廊宽度大于h1_min+2e，且车辆y_loc在一次机动停车的目标线集范围内则选择一次机动停车；当走廊宽度小于h2_min时，则选用三次机动停车；其他条件下均使用二次机动停车。

机动包括前行和后退；所述一次机动停车为前行或者后退；所述一次机动停车的具体方法如下：

11)将车辆从车位内取出，车辆不与车位走廊的边界和车位碰撞，车位走廊的宽度为h1；车辆左部与车位的极限距离为a，车辆以Cr为转向中心，转弯半径为最小转弯半径R_min；当车辆左部与车位间的距离为a时，车辆从车位内取出的高度减小；若车辆后部与车位间的安全距离为b，可得泊车结束位置为(X_end，Y_end)；

其中，a为车辆左部与车位的极限距离；R_min为车辆最小转弯半径；W_car为车辆总宽；L_rear为车辆后悬的长度；X_end为车辆泊车结束位置的x坐标值；Y_end为车辆泊车结束位置的y坐标值；

12)车辆从车位内结束位置(X_end，Y_end)规划到起始泊车位置(xstart， ystart)，车辆初始规划直线L1，达到车辆以Cr为转向中心下C(Cr,R_min)，为满足不碰撞要求，定义车辆后轴延长线与车身的右侧交点为易碰撞点E，E点的轨迹在车位右上角F的上方或者重合；定义θ₁为车辆进行一次泊车时，当车辆由车辆结束位置的左前点B转到一次泊车后相应的车辆左前点B’点位置，车辆所需要转过的角度值，由几何关系得

其中

由车辆本身车辆参数决定的第二次机动转过的角度；其中，L_car为车辆总长；W_car为车辆总宽；L_rear为车辆后悬的长度；R为车辆转弯半径；在车辆位于结束位置时，易碰撞点E以车辆结束位置的最小转弯半径的圆心Cr_int为瞬时中心，与车位右边界的交点为P点，由几何约束关系得

其中，y_p为P点的纵坐标；R为车辆结束位置(X_end，Y_end)到点Cr_int的距离；W_car为车辆总宽；X_end为车辆泊车结束位置的x坐标值；Y_end为车辆泊车结束位置的y坐标值；

13)车辆规划无碰撞轨迹，从结束位置所需要走的直线距离为L'₁，要求 L'₁≥L₁，其中L₁＝L_spot-y_p，L_spot为车位总长,y_p为P点的纵坐标；车辆一次泊车所需要的高度为h1，通过非线性优化求解，得到h1的最小值；约束条件如下：

其中，h1min为车辆一次泊车所需要的最小高度；L₁为规划无碰撞轨迹，从结束位置所需要走的最小直线距离；L'₁为从结束位置所需要走的直线距离；Y_end为车辆泊车结束位置的y坐标值；L_spot为车位总长；R为任意转弯半径；R_B为B 点的转弯半径；R_min为最小转弯半径；y_p为P点的纵坐标；

推导得车位F点在车辆E点以Cr为瞬时转向中心轨迹上，则为一次泊车下h1 的极限高度。

所述二次机动停车为前行切换到后退或后退切换到前行；具体方法如下：21)由于一次泊车下车位走廊空间宽度最小为h1_min，定义二次泊车最小走廊空间为h2_min，直线L2为规划无碰撞轨迹，从结束位置所需要走的最小直线距离，其中L2＝L1，R＝R_min，其中，R_min为车辆最小转弯半径；R为车辆转弯半径；；绕Cr转向中心下，车辆转过的角度值θ₁；在这种情况下车辆所对应的车位空间h2为最小值；车辆绕转向中心Cl，在最小转弯半径R_min下，车辆右后点A’的轨迹与车位左前点J相交，则θ₁为最小值；通过下式求得θ_1min；

其中，x_Cl为Cl点的x坐标值；y_Cl为Cl点的y坐标值；x_Cr为Cr点的x 坐标值；y_Cr为Cr点的y坐标值；x_J为J点的x坐标值；y_J为J点的y坐标值； R_min为最小转弯半径；R_A为车辆左下角A点的转弯半径；θ₁为绕Cr转向中心下，车辆转过的角度值；

若θ₁>90°-β，其中，β为第二次机动转过的角度，则二次泊车的与一次泊车的极限高度一致，所以h2＝h1；

若θ₁<90°-β，则一次泊车后车辆的左前点B’点没有达到一次泊车的高度，此时二次泊车的极限高度为

其中，y_Cl为Cl点的y坐标值；θ₁为绕Cr转向中心下，车辆转过的角度值；β为第二次机动转过的角度；R_min为最小转弯半径；L_spot为车位总长；W_car为车辆总宽；L_car为车辆总长；L_rear为车辆后悬的长度；

22)二次泊车在泊车位走廊存在右边界情况下，则右边界x_boundary为

其中，X_end为车辆泊车结束位置的x坐标值；θ₁为绕Cr转向中心下，车辆转过的角度值；R_min为最小转弯半径；W_car为车辆总宽；L_car为车辆总长；L_rear为车辆后悬的长度。

当车位走廊h3比二次泊车宽度h2_min低时，则需要进行三次机动停车；具体方法如下：

31)B’的坐标值

y_B'＝h3，其中，x_Cr为Cr点的 x坐标值；y_Cr为Cr点的y坐标值；d_CrB'为Cr点与B'点的距离；车辆从车位内开出，右打死下，车辆转过的极限角度θ₁如下:θ₁＝arctan((y_B'-y_Cr)/(x_Cr-x_B'))-β，推导出Cl坐标值(x_cl，y_cl)；d_ClF为瞬时转向中心Cl到F点的距离

其中x_Cr为Cr点的x坐标值；y_Cr为Cr点的y坐标值；x_B'为B'点的x坐标值；y_B'为B'点的y坐标值；x_Cl为Cl点的x坐标值；y_Cl为Cl点y坐标值；x_F为F点的x；y_F为F点的y坐标值；β为第二次机动转过的角度；

32)创建arc C(Cl,d_ClF)与arc C(Cr，R_Emin)相交于点G(x_G，y_G)，即为以车辆结束位置的转向中心Cr2车辆E点的轨迹与以瞬时转向中心Cl车位右上角F点轨迹的交点，确保车辆以Cr2为转向中心，以最小转弯半径运动，能够不与车位F点碰撞；由余弦定理，G点与F点距离d_GF，推导车辆以瞬时转向中心Cl，半径为R_min下转过的角度belta₁如下：

其中，x_Cr为Cr点的x坐标值；y_Cr为Cr点的y坐标值；x_G为G点的x 坐标值；y_G为G点的y坐标值；x_Cl为Cl点的x坐标值；y_Cl为Cl点y坐标值； d_ClF为瞬时转向中心Cl到F点的距离；d_GF为G点到F点的距离；R_E为E点的转弯半径；

33)以转向中心Cl运动，存在与车位左上角J点或者车位左界线JQ碰撞的可能性，因此需要对于两种情况判断；通过比较两种不同的情况来得到belta2， belta2取两种情况下最小的值；

比较d_ClA’与d_ClJ，转向中心Cl到车辆左下角A’的距离为d_ClA’，Cl到车位左上角J点距离为d_ClJ；当d_ClJ>d_ClA'，车位J点与车辆底边相交；以转向中心 Cl为圆心，d_ClJ为半径J点相对车辆的运动轨迹为C(Cl，d_ClJ)，此轨迹与经过A’点的车辆底边直线的交点为PJA(x_JA，y_JA)；d_JA’为J点与A’点距离，使用三角形的余弦定理，可以得到belta2，如下所示：

其中，x_A'为A'点的x；y_A'为A'点的y坐标值；x_CrCr点的x坐标值；y_Cr为 Cr点的y坐标值；x_Cl为Cl点的x坐标值；y_Cl为Cl点的y坐标值；J点圆弧与车身后的交点为PJA点；x_JA为PJA点的x坐标值；y_JA为PJA点的y坐标值；R_min为最小转弯半径；W_car为车辆总宽；L_rear为车辆后悬的长度；θ₁为绕 Cr转向中心下，车辆转过的角度值；x为以车位左下角为原点的坐标轴的x方向上的大小；y为以车位左下角为原点的坐标轴的y方向上的大小；d_ClJ为Cl 到车位左上角J点距离；d_JA'为车辆左下角A'到车位左上角J点距离；x_J为车位左上角J点的x坐标值；y_J为车位左上角J点的y坐标值。

若d_ClJ<d_ClA'，车辆左下角A’与车位左边JQ相交于点PJQ(x_JQ，y_JQ)，按下式推导belta2：

其中，x_JQ为PJQ点的x坐标值；y_JQ为PJQ点的y坐标值；x_J为J点的x 坐标值；x_Cl为Cl点的x坐标值；y_Cl为Cl点的y坐标值；x_A'为A'点的x坐标值；y_A'为A'点的y坐标值；d_ClA'为Cl到车辆左下角A'点距离；d_JA'为J点到车辆左下角A'点距离；

考虑到两种碰撞的情况，取较小的belta来进行规划，belta＝min(belta1,belta2)，从而避免了在规划过程中，车辆与车位的碰撞；通过下面的非线性约束推导得到三次泊车下最小高度h3。

其中，y_Cr为Cr点的y坐标值；y_{Cr_int}为Cr_int点的y坐标值；d_CrB'为Cr 到车辆左上角A'点距离；θ₁为绕Cr转向中心下，车辆转过的角度值；β为第二次机动转过的角度；R_min为最小转弯半径；W_car为车辆总宽；L_spot为车位总长； W_spot为车位总宽；L₃为规划无碰撞轨迹，从结束位置所需要走的最小直线距离； X_end为车辆泊车结束位置的x坐标值；Y_end为车辆泊车结束位置的y坐标值。

所述步骤二的具体方法如下：

当车位走廊宽度为h1_min时，一次机动停车位置的目标线只存在一条，若控制系统存在控制误差，则会导致一次机动停车无法成功；选择二次机动停车，生成连续的目标线集，对于在当前位置选择目标线，若存在误差则选择起始位置调整具有优势；车辆绕瞬时转向中心Cr转过角度θ₁，则车辆在绕瞬时转向中心Cl转过角度

确定θ₁的极大值与极小值，得到y_hmax和y_hmin；因此能够在宽度h下生成多个转弯半径为R_min的圆；目标线是与第二段圆弧水平切线，在起始可泊区域高度的最大值y_hmax与最小值y_hmin范围内连续的目标线集，即可泊车的目标线集；若车辆存在控制误差，则将目标线集设为(y_hmax-e， y_hmin+e)；得到的约束条件如下：

其中，x_Cl为转向中心Cl点的x坐标值；y_Cl为转向中心Cl点的y坐标值； x_Cr为转向中心Cr点的x坐标值；y_Cr为转向中心Cr点的y坐标值；R_min为最小转弯半径；θ₁为绕Cr转向中心下，车辆转过的角度值；θ_1min为绕Cr转向中心下，车辆转过的角度值的最小值；θ_1max为绕Cr转向中心下，车辆转过的角度值的最大值；y_hmin为起始可泊区域高度的最小值；y_hmax为起始可泊区域高度的最大值；θ₁为绕Cr转向中心下，车辆转过的角度值；θ₂为绕Cl转向中心下，车辆转过的角度值；θ_{two_manuver}为二次机动车辆转过的角度值；

根据当前车辆的位置信息(x_loc，y_loc)，以及航向角信息Φ，从目标线集中选择；将选择的目标线作为从当前位置规划到起始泊车位置的中介；由于起始位置为任意的，则需要对起始位置分类讨论，以选择合理的目标线，具体为：

1)对于车辆的航向角Φ，当Φ＝0，则根据下式进行选择目标线；若车辆 y_loc不在目标线时，使车辆向前或向后规划，规划后Φ＝0；

其中，y_loc为车辆当前位置的y坐标值；y_hmin为起始可泊区域高度的最小值； y_hmax为起始可泊区域高度的最大值；e为误差；R_locmin为车辆当前位置的最小转弯半径；C为当前位置与起始泊车位置的在y轴方向上的差值；θ为执行最小转弯半径下车辆绕转向中心转过的角度；

2)若航向角为Φ>0，当车辆的位置(x_loc，y_loc)已知；判断在一次操作下，向前规划能否到达(y_hmax-e，y_hmin+e)内，且航向角为0，则在该位置下的最大的航向角

其中y_hmax为起始可泊区域高度的最大值；e为误差；y_loc为车辆初始位置的y坐标值；R_min为最小转弯半径；

a)若Φ<Φ_forwardmax，则选择向前规划；受到当前位置x_loc的值影响，选择目标线是不一样的，因为这决定着是否超过起始泊车位置，而需要进行倒退的操作，按下式进行选择；当存在多个路径可以进行选择，对R_loc离散后求得路径距离，选择路径最短方案；

其中，R_min为最小转弯半径；Φ为车辆航向角；y_hmax为起始可泊区域高度的最大值；e为误差；x_loc为车辆当前位置的x坐标值；y_loc为车辆当前位置的 y坐标值；x_Cl为转向中心Cl点的x坐标值；y_Cl为转向中心Cl点的y坐标值； R_min为最小转弯半径；R_locmax为车辆当前位置的最大转弯半径；R_loc为车辆当前位置的转弯半径；θ_2max为绕Cl转向中心下，车辆转过的角度值的最大值；θ_2min为绕Cl转向中心下，车辆转过的角度值的最小值；θ₁为绕Cr转向中心下，车辆转过的角度值；θ₂为绕Cl转向中心下，车辆转过的角度值；x_Clθ2max为转动θ_2max时转动中心Cl的x坐标值；

b)若Φ>Φ_forwardmax，则执行一次向前操作不可以满足目标线集条件；该情况下判断是否存在绕瞬时转向中心后退，转弯半径如为最小转弯半径R_min，能够满足目标线集目标线集条件；满足条件的最大的航向角

其中y_hmin为起始可泊区域高度的最小值；e为误差；y_loc为车辆当前位置的y坐标值；R_min为最小转弯半径；若Φ<Φ_backmax，并且

其中y_current为当前车辆的y坐标值；R_min为最小转弯半径；Φ为车辆航向角；W_car为车辆总宽；L_spot为车位总长； L_rear为车辆后悬；按下式选取圆半径：

其中，y_loc为车辆当前位置的y坐标值；R_min为最小转弯半径；R _max为最大转弯半径；Φ为车辆航向角；y_hmin为起始可泊区域高度的最小值；e为误差；θ₁为绕Cr转向中心下，车辆转过的角度值；θ₂为绕Cl转向中心下，车辆转过的角度值；x_loc为车辆当前位置的x坐标值；x_Cl为转向中心Cl点的x坐标值；

3)若航向角为Φ<0时，按照相同方法选择R_loc；若当前车辆位置均无法满足上述条件时，则需要调整车辆的位置，通过向前或向后打死，转弯半径为最小转弯半径，在一次操作结束时，判断是否满足上述条件；若满足，则选择 R_loc；若不满足，则继续进行下一次操作，直到满足条件；车辆从当前位置规划到起始泊车位置，由于目标线集为连续的区间，若存在控制误差则可以进行调整。

本发明的有益效果为：能够为更加多变的交通情况适应，如车位狭小与停车走廊空间狭小的情况。具有拟人化的泊车路径规划方式，若乘坐驾驶员，能够满足驾驶员的心理接受能力。

附图说明

图1为本发明所述的停车路径规划方法原理示意图；

图2为本发明所述的路径规划算法在停车场中的应用实例示意图；

图3为本发明所述的一次机动示意图；

图4a和图4b为本发明所述的二次机动示意图；

图5a—图5c为本发明所述的三次机动示意图；

图6为本发明所述的目标线的生成和选择示意图；

图7为本发明所述的一次机动实例示意图；

图8为本发明所述的二次机动实例示意图；

图9为本发明所述的三次机动实例示意图；

图10和图11为本发明所述的二次机动路径优化、目标线生成与选择示意图；

图12为本发明所述的坐标系示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

参阅图1和图2，一种狭小垂直泊车路径规划方法，包括以下步骤：

所述步骤一的具体方法如下：

参阅图10和图11，当车位走廊宽度为h1_min时，一次机动停车位置的目标线只存在一条，若控制系统存在控制误差，则会导致一次机动停车无法成功；选择二次机动停车，生成连续的目标线集，对于在当前位置选择目标线，若存在误差则选择起始位置调整具有优势；车辆绕瞬时转向中心Cr转过角度θ₁，则车辆在绕瞬时转向中心Cl转过角度

确定θ₁的极大值与极小值，得到y_hmax和y_hmin；因此能够在宽度h下生成多个转弯半径为R_min的圆；目标线是与第二段圆弧水平切线，在起始可泊区域高度的最大值y_hmax与最小值y_hmin范围内连续的目标线集，即可泊车的目标线集；若车辆存在控制误差，则将目标线集设为(y_hmax-e，y_hmin+e)；得到的约束条件如下：

其中，x_Cl为转向中心Cl点的x坐标值；y_Cl为转向中心Cl点的y坐标值； x_Cr为转向中心Cr点的x坐标值；y_Cr为转向中心Cr点的y坐标值；R_m _in为最小转弯半径；θ₁为绕Cr转向中心下，车辆转过的角度值；θ_1min为绕Cr转向中心下，车辆转过的角度值的最小值；θ_1max为绕Cr转向中心下，车辆转过的角度值的最大值；y_hmin为起始可泊区域高度的最小值；y_hmax为起始可泊区域高度的最大值；θ₁为绕Cr转向中心下，车辆转过的角度值；θ₂为绕Cl转向中心下，车辆转过的角度值；θ_{two_manuver}为二次机动车辆转过的角度值；

1)对于车辆的航向角Φ，当Φ＝0，则根据下式进行选择目标线；若车辆 y_loc不在目标线时，则执行如图6类似方法，使车辆向前或向后规划，规划后Φ＝0；

步骤四、到达起始停车位置时，跟随规划路径。

机动包括前行和后退；

参阅图7，所述一次机动停车为前行或者后退；所述一次机动停车的具体方法如下：

11)将车辆从车位内取出，车辆不与车位走廊的边界和车位碰撞，车位走廊的宽度为h1，如图3所示；车辆左部与车位的极限距离为a，车辆以Cr为转向中心，转弯半径为最小转弯半径R_min；当车辆左部与车位间的距离为a时，车辆从车位内取出的高度减小；若车辆后部与车位间的安全距离为b，可得泊车结束位置为(X_end，Y_end)；

12)车辆从车位内结束位置(X_end，Y_end)规划到起始泊车位置(xstart， ystart)，车辆初始规划直线L1，达到车辆以Cr为转向中心下C(Cr,R_min)，为满足不碰撞要求，定义车辆后轴延长线与车身的右侧交点为易碰撞点E，E点的轨迹在车位右上角F的上方或者重合；如图3所述，定义θ₁为车辆进行一次泊车时，当车辆由车辆结束位置的左前点B转到一次泊车后相应的车辆左前点 B’点位置，车辆所需要转过的角度值，由几何关系得

其中

由车辆本身车辆参数决定的第二次机动转过的角度；其中， L_car为车辆总长；W_car为车辆总宽；L_rear为车辆后悬的长度；R为车辆转弯半径；在车辆位于结束位置时，易碰撞点E以车辆结束位置的最小转弯半径的圆心 Cr_int为瞬时中心，与车位右边界的交点为P点，由几何约束关系得

其中，h1_min为车辆一次泊车所需要的最小高度；L₁为规划无碰撞轨迹，从结束位置所需要走的最小直线距离；L'₁为从结束位置所需要走的直线距离；Y_end为车辆泊车结束位置的y坐标值；L_spot为车位总长；R为任意转弯半径；R_B为B 点的转弯半径；R_min为最小转弯半径；y_p为P点的纵坐标；

参阅图8，所述二次机动停车为前行切换到后退或后退切换到前行；具体方法如下：

21)由于一次泊车下车位走廊空间宽度最小为h1_min，定义二次泊车最小走廊空间为h2_min，直线L2为规划无碰撞轨迹，从结束位置所需要走的最小直线距离，其中L2＝L1，R＝R_min，其中，R_min为车辆最小转弯半径；R为车辆转弯半径；如图4a—图4b所示，绕Cr转向中心下，车辆转过的角度值θ₁；在这种情况下车辆所对应的车位空间h2为最小值；车辆绕转向中心Cl，在最小转弯半径R_min下，车辆右后点A’的轨迹与车位左前点J相交，则θ₁为最小值；通过下式求得θ_1min；

其中，x_Cl为Cl点的x坐标值；y_Cl为Cl点的y坐标值；x_Cr为Cr点的x 坐标值；y_Cr为Cr点的y坐标值；x_J为J点的x坐标值；y_J为J点的y坐标值；R_min为最小转弯半径；R_A为车辆左下角A点的转弯半径；θ₁为绕Cr转向中心下，车辆转过的角度值；

若θ₁<90°-β，则一次泊车后车辆的左前点B’没有达到一次泊车的高度，此时二次泊车的极限高度为

参阅图9，31)B’的坐标值

y_B'＝h3，其中，x_Cr为Cr点的x坐标值；y_Cr为Cr点的y坐标值；d_CrB'为Cr点与B'点的距离；如图4a—图4b所示，车辆从车位内开出，右打死下，车辆转过的极限角度θ₁如下:θ₁＝arctan((y_B'-y_Cr)/(x_Cr-x_B'))-β，推导出Cl坐标值(x_cl，y_cl)；d_ClF为瞬时转向中心Cl到F点的距离

其中x_Cr为Cr点的x坐标值；y_Cr为Cr点的y坐标值；x_B'为B'点的x坐标值；y_B'为B'点的y坐标值；x_Cl为Cl点的x坐标值；y_Cl为Cl点y坐标值；x_F为F点的x；y_F为F点的y 坐标值；β为第二次机动转过的角度；

32)如图5a—图5c所示位置，创建arc C(Cl,d_ClF)与arc C(Cr，R_Emin) 相交于点G(x_G，y_G)，即为以转向中心Cr2车辆E点的轨迹与以瞬时转向中心Cl车位右上角F点轨迹的交点，确保车辆以Cr2为转向中心，以最小转弯半径运动，能够不与车位F点碰撞；由余弦定理，G点与F点距离d_GF，推导车辆以瞬时转向中心Cl，半径为R_min下转过的角度belta₁如下：

33)以转向中心Cl运动，存在与车位左上角J点或者车位左界线JQ碰撞的可能性，因此需要对于两种情况判断；belta2为考虑J点与车辆碰撞下，车辆绕转向中心Cl转过的角度；

belta2＝arc cos((2d_ClJ ²-d_JA' ²)/(2d_ClJ ²))

参阅图12，其中，x_A'为A'点的x；y_A'为A'点的y坐标值；x_Cr为Cr点的x 坐标值；y_Cr为Cr点的y坐标值；x_Cl为Cl点的x坐标值；y_Cl为Cl点的y 坐标值；J点圆弧与车身后的交点为PJA点，x_JA为PJA点的x坐标值；y_JA为 PJA点的y坐标值；R_min为最小转弯半径；W_car为车辆总宽；L_rear为车辆后悬的长度；θ₁为绕Cr转向中心下，车辆转过的角度值；x为以车位左下角为原点的坐标轴的x方向上的大小；y为以车位左下角为原点的坐标轴的y方向上的大小；d_ClJ为Cl到车位左上角J点距离；d_JA'为车辆左下角A'到车位左上角J点距离；x_J为车位左上角J点的x坐标值；y_J为车位左上角J点的x坐标值。

其中，y_Cr为Cr点的y坐标值；y_{Cr_int}为Cr_int点的y坐标值；d_CrB'为Cr到车辆左上角A'点距离；θ₁为绕Cr转向中心下，车辆转过的角度值；β为第二次机动转过的角度；R_min为最小转弯半径；W_car为车辆总宽；L_spot为车位总长；W_spot为车位总宽；L₃为规划无碰撞轨迹，从结束位置所需要走的最小直线距离；X_end为车辆泊车结束位置的x坐标值；Y_end为车辆泊车结束位置的y坐标值。

实施例

采用如下仿真参数进行垂直泊车路径规划。

Lcar	4.155m
		Wcar	1.645m
L	2.405m
		Lfront	0.8m
Lrear	0.95m
		Rmin	4.2m
Lspot	5.3m
		Wspot	2.5m

根据仿真参数，一次泊车，二次泊车，三次泊车，得到相应的仿真结果。一次机动下的最小空间为h1min＝3.8162m，二次机动下的最小空间为h2min＝3.8162m，三次机动下的最小空间为h3min＝3.55m，在仿真中构建了虚拟走廊宽度不同的垂直停车位，其中车位的宽度和长度以及走廊宽度是已知的。若走廊宽度大于一个车位宽度(h＝4.0162m)，则选择停车路径平面图进行对比，如图7所示。若不满足1个机动停车走廊宽度(h＝3.9m)，则选择2个机动停车路径方案进行停车，如图8所示。若走廊宽度在两个机动停车高度和三个机动停车高度之间(h＝3.8m)，则选择三个机动停车，如图9所示。生成从当前位置 (x＝1.1781m.y＝6.8891m，航向角为Φ＝π/10)到目标线的路径，如图10所示。生成从当前位置(x＝5.5m,y＝7m，Φ＝π/10)到目标线的路径，如图11所示。

Claims

1.一种狭小垂直泊车路径规划方法，其特征在于，包括以下步骤：

步骤一、终点位置确定，并从终点位置到车位外进行规划，从终点位置到车位外进行规划的过程与车辆从现场取出的路径规划一致；起始停车位置是方位角为零的位置，根据车位走廊的宽度及车辆当前的位置来确定在停车过程中执行一次机动停车或二次机动停车或三次机动停车；

步骤四、到达起始停车位置时，跟随规划路径；

所述步骤一的具体方法如下：

若车位走廊宽度大于h1_min+2e，且车辆y_loc在一次机动停车的目标线集范围内则选择一次机动停车；当走廊宽度小于h2_min时，则选用三次机动停车；其他条件下均使用二次机动停车；

12)车辆从车位内结束位置(X_end，Y_end)规划到起始泊车位置(xstart，ystart)，车辆初始规划直线L1，达到车辆以Cr为转向中心下C(Cr,R_min)，为满足不碰撞要求，定义车辆后轴延长线与车身的右侧交点为易碰撞点E，E点的轨迹在车位右上角F的上方或者重合；定义θ₁为车辆进行一次泊车时，当车辆由车辆结束位置的左前点B转到一次泊车后相应的车辆左前点B’点位置，车辆所需要转过的角度值，由几何关系得

其中

13)车辆规划无碰撞轨迹，从结束位置所需要走的直线距离为L'₁，要求L'₁≥L₁，其中L₁＝L_spot-y_p，L_spot为车位总长,y_p为P点的纵坐标；车辆一次泊车所需要的高度为h1，通过非线性优化求解，得到h1的最小值；约束条件如下：

其中，h1min为车辆一次泊车所需要的最小高度；L₁为规划无碰撞轨迹，从结束位置所需要走的最小直线距离；L'₁为从结束位置所需要走的直线距离；Y_end为车辆泊车结束位置的y坐标值；L_spot为车位总长；R为任意转弯半径；R_B为B点的转弯半径；R_min为最小转弯半径；y_p为P点的纵坐标；

推导得车位F点在车辆E点以Cr为瞬时转向中心轨迹上，则为一次泊车下h1的极限高度。

2.根据权利要求1所述一种狭小垂直泊车路径规划方法，其特征在于，所述二次机动停车为前行切换到后退或后退切换到前行；具体方法如下：

21)由于一次泊车下车位走廊空间宽度最小为h1_min，定义二次泊车最小走廊空间为h2_min，直线L2为规划无碰撞轨迹，从结束位置所需要走的最小直线距离，其中L2＝L1，R＝R_min，其中，R_min为车辆最小转弯半径；R为车辆转弯半径；绕Cr转向中心下，车辆转过的角度值θ₁；在这种情况下车辆所对应的车位空间h2为最小值；车辆绕转向中心Cl，在最小转弯半径R_min下，车辆右后点A’的轨迹与车位左前点J相交，则θ₁为最小值；通过下式求得θ_1min；

其中，x_Cl为Cl点的x坐标值；y_Cl为Cl点的y坐标值；x_Cr为Cr点的x坐标值；y_Cr为Cr点的y坐标值；x_J为J点的x坐标值；y_J为J点的y坐标值；R_min为最小转弯半径；R_A为车辆左下角A点的转弯半径；θ₁为绕Cr转向中心下，车辆转过的角度值；

3.根据权利要求1所述一种狭小垂直泊车路径规划方法，其特征在于，当车位走廊h3比二次泊车宽度h2_min低时，则需要进行三次机动停车；具体方法如下：

31)B’的坐标值

y_B'＝h3，其中，x_Cr为Cr点的x坐标值；y_Cr为Cr点的y坐标值；d_CrB'为Cr点与B'点的距离；车辆从车位内开出，右打死下，车辆转过的极限角度θ₁如下:θ₁＝arctan((y_B'-y_Cr)/(x_Cr-x_B'))-β，推导出Cl坐标值(x_cl，y_cl)；d_ClF为瞬时转向中心Cl到F点的距离

其中，x_Cr为Cr点的x坐标值；y_Cr为Cr点的y坐标值；x_G为G点的x坐标值；y_G为G点的y坐标值；x_Cl为Cl点的x坐标值；y_Cl为Cl点y坐标值；d_ClF为瞬时转向中心Cl到F点的距离；d_GF为G点到F点的距离；R_E为E点的转弯半径；

33)以转向中心Cl运动，存在与车位左上角J点或者车位左界线JQ碰撞的可能性，因此需要对于两种情况判断；通过比较两种不同的情况来得到belta2，belta2取两种情况下最小的值；

比较d_ClA’与d_ClJ，转向中心Cl到车辆左下角A’的距离为d_ClA’，Cl到车位左上角J点距离为d_ClJ；当d_ClJ>d_ClA'，车位J点与车辆底边相交；以转向中心Cl为圆心，d_ClJ为半径J点相对车辆的运动轨迹为C(Cl，d_ClJ)，此轨迹与经过A’点的车辆底边直线的交点为PJA(x_JA，y_JA)；d_JA’为J点与A’点距离，使用三角形的余弦定理，可以得到belta2，如下所示：

其中，x_A'为A'点的x；y_A'为A'点的y坐标值；x_CrCr点的x坐标值；y_Cr为Cr点的y坐标值；x_Cl为Cl点的x坐标值；y_Cl为Cl点的y坐标值；J点圆弧与车身后的交点为PJA点；x_JA为PJA点的x坐标值；y_JA为PJA点的y坐标值；R_min为最小转弯半径；W_car为车辆总宽；L_rear为车辆后悬的长度；θ₁为绕Cr转向中心下，车辆转过的角度值；x为以车位左下角为原点的坐标轴的x方向上的大小；y为以车位左下角为原点的坐标轴的y方向上的大小；d_ClJ为Cl到车位左上角J点距离；d_JA'为车辆左下角A'到车位左上角J点距离；x_J为车位左上角J点的x坐标值；y_J为车位左上角J点的y坐标值；

其中，x_JQ为PJQ点的x坐标值；y_JQ为PJQ点的y坐标值；x_J为J点的x坐标值；x_Cl为Cl点的x坐标值；y_Cl为Cl点的y坐标值；x_A'为A'点的x坐标值；y_A'为A'点的y坐标值；d_ClA'为Cl到车辆左下角A'点距离；d_JA'为J点到车辆左下角A'点距离；

考虑到两种碰撞的情况，取较小的belta来进行规划，belta＝min(belta1,belta2)，从而避免了在规划过程中，车辆与车位的碰撞；通过下面的非线性约束推导得到三次泊车下最小高度h3；

4.根据权利要求1所述一种狭小垂直泊车路径规划方法，其特征在于，所述步骤二的具体方法如下：

其中，x_Cl为转向中心Cl点的x坐标值；y_Cl为转向中心Cl点的y坐标值；x_Cr为转向中心Cr点的x坐标值；y_Cr为转向中心Cr点的y坐标值；R_min为最小转弯半径；θ₁为绕Cr转向中心下，车辆转过的角度值；θ_1min为绕Cr转向中心下，车辆转过的角度值的最小值；θ_1max为绕Cr转向中心下，车辆转过的角度值的最大值；y_hmin为起始可泊区域高度的最小值；y_hmax为起始可泊区域高度的最大值；θ₁为绕Cr转向中心下，车辆转过的角度值；θ₂为绕Cl转向中心下，车辆转过的角度值；θ_{two_manuver}为二次机动车辆转过的角度值；

1)对于车辆的航向角Φ，当Φ＝0，则根据下式进行选择目标线；若车辆y_loc不在目标线时，使车辆向前或向后规划，规划后Φ＝0；

if(y_loc<y_hmin+e)then

select(y_hmin+e)

y_hmin+e-y_loc＝C

θ＝arccos(1-C/(2R_locmin))

if(y_loc>y_hmax-e)then

y_loc-y_hmax+e＝C

θ＝arccos(1-C/(2R_locmin))

select(y_hmax-e)

if(y_hmin+e<y_loc<y_hmax-e)then

select(y_loc)

其中，y_loc为车辆当前位置的y坐标值；y_hmin为起始可泊区域高度的最小值；y_hmax为起始可泊区域高度的最大值；e为误差；R_locmin为车辆当前位置的最小转弯半径；C为当前位置与起始泊车位置的在y轴方向上的差值；θ为执行最小转弯半径下车辆绕转向中心转过的角度；

其中，R_min为最小转弯半径；Φ为车辆航向角；y_hmax为起始可泊区域高度的最大值；e为误差；x_loc为车辆当前位置的x坐标值；y_loc为车辆当前位置的y坐标值；x_Cl为转向中心Cl点的x坐标值；y_Cl为转向中心Cl点的y坐标值；R_min为最小转弯半径；R_locmax为车辆当前位置的最大转弯半径；R_loc为车辆当前位置的转弯半径；θ_2max为绕Cl转向中心下，车辆转过的角度值的最大值；θ_2min为绕Cl转向中心下，车辆转过的角度值的最小值；θ₁为绕Cr转向中心下，车辆转过的角度值；θ₂为绕Cl转向中心下，车辆转过的角度值；x_Clθ2max为转动θ_2max时转动中心Cl的x坐标值；

b)若Φ>Φ_forwardmax，则执行一次向前操作不可以满足目标线集条件；判断是否存在绕瞬时转向中心后退，转弯半径如为最小转弯半径R_min，能够满足目标线集目标线集条件；满足条件的最大的航向角

其中y_current为当前车辆的y坐标值；R_min为最小转弯半径；Φ为车辆航向角；W_car为车辆总宽；L_spot为车位总长；L_rear为车辆后悬；按下式选取圆半径：

其中，y_loc为车辆当前位置的y坐标值；R_min为最小转弯半径；R_max为最大转弯半径；Φ为车辆航向角；y_hmin为起始可泊区域高度的最小值；e为误差；θ₁为绕Cr转向中心下，车辆转过的角度值；θ₂为绕Cl转向中心下，车辆转过的角度值；x_loc为车辆当前位置的x坐标值；x_Cl为转向中心Cl点的x坐标值；

3)若航向角为Φ<0时，按照相同方法选择R_loc；若当前车辆位置均无法满足上述条件时，则需要调整车辆的位置，通过向前或向后打死，转弯半径为最小转弯半径，在一次操作结束时，判断是否满足上述条件；若满足，则选择R_loc；若不满足，则继续进行下一次操作，直到满足条件；车辆从当前位置规划到起始泊车位置，由于目标线集为连续的区间，若存在控制误差则可以进行调整。