CN114261388A

CN114261388A - 一种拟人化的自动泊车路径规划方法

Info

Publication number: CN114261388A
Application number: CN202210035186.9A
Authority: CN
Inventors: 管欣; 蔡磊; 贾鑫; 周芝瑶; 徐福来; 詹军
Original assignee: Jilin University
Current assignee: Jilin University
Priority date: 2022-02-23
Filing date: 2022-02-23
Publication date: 2022-04-01

Abstract

本发明属于汽车技术领域，具体的说是一种拟人化的自动泊车路径规划方法。自动泊车路径规划包括水平泊车路径规划和垂直泊车路径规划；所述水平泊车包括平行泊车和利用车位空间泊车；所述垂直泊车包括一次机动泊车、二次机动泊车和三次机动泊车。本发明通过确定了多次机动正向进入泊车位的空间大小，可以在不同大小的泊车位以及车位走廊大小的空间下确定泊车的机动次数。

Description

一种拟人化的自动泊车路径规划方法

技术领域

本发明属于汽车技术领域，具体的说是一种拟人化的自动泊车路径规划方法。

背景技术

现有的几何规划算法中，缺乏对于正向垂直泊车入位的算法；并且对于正向垂直泊车入位的多次机动规划没有探究。由于车位过道的大小以及车位的大小都会对正向垂直泊车机动次数有影响。并且现有的平行泊车规划算法鲜有考虑到泊车走廊的右边的空间的大小问题。

发明内容

本发明提供了一种拟人化的自动泊车路径规划方法，通过确定了多次机动正向进入泊车位的空间大小，可以在不同大小的泊车位以及车位走廊大小的空间下确定泊车的机动次数。

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

一种拟人化的自动泊车路径规划方法，自动泊车路径规划包括水平泊车路径规划和垂直泊车路径规划；所述水平泊车包括平行泊车和利用车位空间泊车；所述垂直泊车包括一次机动泊车、二次机动泊车和三次机动泊车。

所述平行泊车的具体方法如下：

定义平行泊车阶段位置1为车辆在停车位内的目标位置；

11)确定车辆起始位置，并且确定目标线；

12)确定水平泊车的目标线上的起始位置的左边界O1点坐标；

13)确定水平泊车的目标线上的起始位置的右边界O2点坐标；

所述利用车位空间泊车的具体方法如下：

定义利用车位空间泊车阶段位置1为车辆利用车位空间的极限位置；位置2为从位置1经过左转向达车位走廊限制的极限位置；位置5为车辆在停车位内的目标位置；位置3，位置4为从位置2到达位置5的过程位置，其中位置3为两段圆弧的切点位置，位置4为经过两段圆弧后到达车位的位置；

21)确定到达能够最大利用位置1的坐标；

22)确定到达位置2的坐标；

23)确定由位置2到位置5的过程；

24)确定根据位置1生成的目标集的过程；

所述一次机动泊车的具体方法如下：

定义一次机动泊车阶段位置0为车辆在停车位内的目标位置；位置1为车辆到达车位走廊空间限制的极限位置；

31)确定车辆位置0时的横纵坐标值；

32)确定先走过的直线的长度大小；

33)确定车位走廊宽度h1的大小；

所述二次机动泊车的具体方法如下：

定义二次机动泊车阶段位置0为车辆在停车位内的目标位置；位置1为车辆从位置0经过直线到达车辆执行转弯时不与车位发生碰撞的位置；位置2为车辆从位置1到达车位走廊空间的极限位置；位置3为车辆从位置2到达车位长度决定的上边界，与车位边界相切；

41)确定车辆位置0时的横纵坐标值；

42)计算车辆位置0到车辆位置1的直线的长度；

43)计算车辆位置1到车辆位置2所转过的角度以及车辆转弯半径大小；

44)计算车辆位置2到车辆位置3所转过的角度以及车辆转弯半径大小；

45)计算车辆走廊宽度h2的大小。

所述三次机动泊车的具体方法如下：

定义三次机动泊车阶段位置0为车辆在停车位内的目标位置；位置1为车辆从位置0经过直线到达车辆执行转弯时不与车位发生碰撞的位置；位置2为车辆从位置1到达车位走廊空间的极限位置；位置3为车辆从位置2到达车位长度决定的上边界，不与车位边界相碰；位置4为从位置3到达使得车辆的航向角为pi的位置；

51)确定车辆位置0的横纵坐标值；

52)计算车辆位置0到车辆位置2走过的直线的长度和所转过的角度以及车辆转弯半径的大小；

53)计算车辆位置2到车辆位置3所转过的角度以及车辆转弯半径的大小；

54)计算车辆位置3到车辆位置4所转过的角度以及车辆转弯半径的大小；

55)计算车辆走廊的宽度h3的大小，并且求h3的最小值。

本发明的有益效果为：

1)本发明符合人类在泊车时的行为习惯；在水平泊车位外生成了目标线，目标线的生成是任意角度与任意位置的，先确定出目标线上的起始泊车位置范围，从水平泊车位内往外规划；

2)本发明通过确定了多次机动正向进入泊车位的空间大小，可以在不同大小的泊车位以及车位走廊大小的空间下确定泊车的机动次数。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1为车辆参数示意图；

图2a为平行泊车起始位置左边界示意图；

图2b为平行泊车起始位置右边界示意图；

图3为水平泊车利用车位空间位置确定示意图；

图4a为利用车位空间的水平泊车过程中，位置1到位置2规划过程示意图；

图4b为利用车位空间的水平泊车过程中，位置2到位置3规划过程示意图；

图5a为利用车位空间的水平泊车过程中，位置3到位置4规划过程示意图；

图5b为利用车位空间的水平泊车过程中，位置4到位置5规划过程示意图；

图6为利用车位内空间水平泊车的目标线生成示意图；

图7为一次机动垂直正向泊车示意图；

图8a为二次机动垂直正向泊车过程中，位置0到位置1规划过程示意图；

图8b为二次机动垂直正向泊车过程中，位置1到位置2规划过程示意图；

图8c为二次机动垂直正向泊车过程中，位置2到位置3规划过程示意图；

图9a为三次机动垂直正向泊车过程中，位置0到位置2规划过程示意图；

图9b为三次机动垂直正向泊车过程中，位置2到位置3规划过程示意图；

图9C为三次机动垂直正向泊车过程中，位置3到位置4规划过程示意图；

图10为车位走廊无限制的平行泊车示意图；

图11为利用车位空间的平行泊车示意图；

图12为一次机动垂直正向泊车的示意图；

图13为二次机动正向垂直泊车的示意图；

图14为三次机动正向垂直泊车的示意图。

具体实施方式

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

参阅图1，图1为车辆参数示意图，其中，C1为后轴中心点到车辆右后点的距离，C2为后轴中心点到车辆右前点的距离，θ1为后轴中心点到车辆右后点的距离与车辆坐标系X轴夹角，θ2为后轴中心点到车辆右前点的距离与车辆坐标系X轴夹角，

θ1＝atan((W_car/2)/L_rear)

θ2＝atan((W_car/2)/(L_car-L_rear))

W_car为车辆的宽度，L_car为车辆的长度，L_rear为车辆后轴长度。

参阅图2a和图2b，所述平行泊车的具体方法如下：

定义平行泊车阶段位置1为车辆在停车位内的目标位置；

11)确定车辆起始位置即位置1，并且确定目标线；

位置1需要满足当车辆以最小转弯半径下能够驶出车位，即在驶出过程中车辆的右前角B点不与车位碰撞；目标线需要满足当车辆驶出车位后，车辆的转向中心与后轴中心连线延长线与车辆右边轮廓相交A点不与车位发生碰撞。

12)确定水平泊车的目标线上的起始位置的左边界O1点坐标；

(xcr1-xcl)²+(ycr1-ycl)²＝4R_min ²

其中，x1，y1为车辆起始位置的后轴中心坐标；R_min为车辆最小转弯半径；

为车辆起始位置的后轴中心与水平线的夹角；xcr1，ycr1为车辆以最小转弯半径驶出车位的水平泊车目标线可泊位置的左极限位置O1点处转向中心Cr1的坐标；xcl，ycl为在车位内起始位置的转向中心Cl的横坐标与纵坐标；

为目标线与水平线的夹角；ylocal为车辆当前的后轴中心纵坐标；xo1，yo1为车辆驶出车位后，以最小转弯半径到达目标线的车辆位置，即为起始泊车位置的左边界的坐标。

13)确定水平泊车的目标线上的起始位置的右边界O2点坐标；

(xcr2-xcl)²+(ycr2-ycl)²＝(R_min+R_max)²

(xcr2-L_spot)²+(ycr2-Wspot)²＝(R_max-W_car/2)² (2)

其中，xcl，ycl为在车位内起始位置的转向中心Cl的横坐标与纵坐标；xcr2，ycr2为车辆以最大转弯半径驶出车位的水平泊车目标线可泊位置的右极限位置点O2处转向中心Cr2的坐标；R_min为车辆最小转弯半径；R_max为车辆最大转弯半径；

为目标线与水平线的夹角；ylocal为车辆当前的后轴中心纵坐标；L_spot为车位长度；W_spot为车位宽度；W_car为车辆宽度。

参阅图3，所述利用车位空间泊车的具体方法如下：

21)确定到达能够最大利用位置1的坐标，该位置满足C点转过角度不与F碰撞；B点绕转向中心Cl1转过的角度不与G点相碰撞，并且B点转过位置中对应的yB的位置为最小值；即保证了车辆在该位置下能够最大可能利用车位内的空间，可以减少对车位走廊空间的需求。通过上述的约束，如公式(3)可以得到该位置1的横坐标，纵坐标以及航向角。

其中，其中，y1为位置1处车辆后轴中心的纵坐标；C2为后轴中心点到车辆右前点的距离；θ₂为后轴中心点到车辆右前点的距离与车辆坐标系X轴夹角；

为位置1处车辆后轴中心与水平线的夹角；R_min为车辆最小转弯半径；W_car为车辆宽度；L_car为车辆长度；L_rear为车辆后轴长度；x_cl1，y_cl1为在位置1处转向中心Cl1的坐标；x_G，y_G为车位右上角点G的坐标；x_F，y_F为车位左上角点F的坐标；

22)确定到达车位走廊限制位置即位置2的坐标；

参阅4(a)，车辆在位置1时，以最小转弯半径R_min转向中心为Cl1转过角度α。转过角度α后车辆可以到达如图4(a)所示位置2，位置2由车位的走廊高度所确定。x_cl1，y_cl1为转向中心Cl1的坐标值。Cl1转向中心的值与Cl2的坐标是一致的。Cr2为车辆在位置2处的以R3为转向半径的转向中心。从位置2到位置3绕转向中心Cr2转过的角度为β。x_D1，y_D1为在位置1处时的D点的坐标值。x_D2，y_D2在位置2处D点相应的横坐标值与纵坐标值。x₂，y₂为位置2处的坐标值，通过转向中心Cl1与位置2的相对关系能够计算得到。

其中，x_cl1，y_cl1为在位置1处转向中心Cl1的坐标值；x₁，y₁为位置1处车辆后轴中心坐标；R为车辆转弯半径；

为位置1处车辆后轴中心与水平线的夹角；x_D1，y_D1为在位置1处时的车辆左前角点D点的坐标值；C2为后轴中心点到车辆右前点的距离；θ₂为后轴中心点到车辆右前点的距离与车辆坐标系X轴夹角；Cl1与Cl2的坐标值一致，通过走廊宽度对D点坐标进行确定；

参阅图4a和图4b，Cl1与Cl2的坐标值一致，通过走廊宽度对D点坐标进行确定；

y_D2＝h1+W_spot(x_D1-x_cl1)²+(y_D1-y_cl1)²＝(x_D2-x_cl1)²+(y_D2-y_cl1)² (5)

其中，x_D2，y_D2在位置2处时的D点的坐标值；h1为走廊宽度；W_spot为车位宽度；x_D1，y_D1为在位置1处时的D点的坐标值；x_cl1，y_cl1为在位置1处转向中心Cl1的坐标值；

其中，x_D1，y_D1为在位置1处时的D点的坐标值；x_D2，y_D2在位置2处时的D点的坐标值；x_cl1，y_cl1为在位置1处转向中心Cl1的坐标值；α为车辆绕转动中心Cl1从位置1转到位置2的转动角度；d₁为在位置1处时的D点与第三转向中心Cl1的距离；d₂为在位置2处时的D点与第三转向中心Cl1的距离；d₃为在位置1处时的D点与位置2处时的D点之间的距离；

结束位置的航向角大小：

其中，其中，

为位置1处车辆后轴中心与水平线的夹角；α为车辆绕在位置1处的转动中心Cl1从位置1转到位置2的转动角度；

参阅图4b，位置2的坐标如下：

其中，x₁，y₁为位置1处车辆后轴中心坐标；x_cl1，y_cl1为在位置1处转向中心Cl1的坐标值；R_min为车辆最小转弯半径；

为位置2处的航向角；

23)确定由位置2到位置5的过程；

参阅图5a，xcr2，ycr2为转向中心Cr2的坐标值。Cr2为车辆在位置2时以R3为转弯半径的转向中心。xcl4，ycl4为车辆转向中心的Cl4的坐标值，其中转向中心Cl4为车辆在位置4时以R4为转弯半径的转向中心，在位置4处，车辆左后角点与车位左侧相交。xb4，yb4为车辆右前的角点B的坐标值。x5，y5为位置5的车辆后轴中心坐标值。同时需要保证位置4到位置2的过程车辆的右前角点B不与车位角点G发生碰撞。对位置4的两个后角点C，E进行约束，保证位置4不与车位碰撞。对如图5(b)所示的位置5的前面两个角点B，D进行约束，保证在结束泊车位置5时，可以合法地停在车位内。根据上述过程，设未知变量β,

R₃,R₄,R₅，通过公式(9)求解车辆位置2到车辆位置5的路径。目标函数为：使位置2与位置4之间的距离最大；

min[1/((x2-x4)²+(y2-y4)²)]

Lcar-(x5+C2*cosθ₂)≤0

x5+C2*cosθ₂-Lspot≤0

W_car-(y5+C2*sinθ₂)≤0

y5+C2*sinθ₂-Wspot≤0 (9)

其中，x2，y2为位置2处车辆后轴中心坐标；x4，y4为位置4处车辆后轴中心坐标；x5，y5为位置5处车辆后轴中心坐标；xb4，yb4为4位置车辆右前的角点B的坐标值；xcr2，ycr2在位置2处转向中心Cr2的坐标值的坐标值；xcl4，ycl4为在位置4处转向中心Cl4的坐标值；R₃为以在位置2处转向中心Cr2为转动中心的转动半径；R₄为以在位置4处转向中心Cl4为转动中心的转动半径；R₅为以在位置5处转向中心Cr5为转动中心的转动半径；

为位置2处的航向角；

为位置4处的航向角；C1为后轴中心点到车辆右后点的距离；C2为后轴中心点到车辆右前点的距离；θ1为后轴中心点到车辆右后点的距离与车辆坐标系X轴夹角；θ2为后轴中心点到车辆右前点的距离与车辆坐标系X轴夹角；L_spot为车位长度；W_spot为车位宽度；L_car为车辆长度；W_car为车辆宽度；

其中，在位置5处转向中心Cr5的坐标值通过下式得出：

x₅＝x_cr3 y₅＝y_cr3+R₅ (10)

其中，R₄为以在位置4处转向中心Cl4为转动中心的转动半径；R₅为以在位置5处转向中心Cr5为转动中心的转动半径；

为位置4处的航向角；x_cl4，y_cl4为在位置4处转向中心Cl4的坐标值；x_cr3，y_cr3为在位置3处转向中心Cr3的坐标值；

24)确定根据位置1生成的目标集的过程；

参阅图6，车辆位置1确定之后，能够确定车辆位置0坐标从而得到目标线的大小；x0，y0为车辆起始位置的坐标值，y＝y0为目标线的坐标

其中，x1，y1为位置1处车辆后轴中心坐标；R₀为以在位置0处转向中心Cr0为转动中心的转动半径；

为位置1处的航向角；

在给定的走廊宽度h1下，根据式(12)确定在走廊宽度h1下的目标线坐标范围[y0min,y0max]

其中，y1为位置1处车辆后轴中心纵坐标；R_min为车辆最小转弯半径；R_max为车辆最大转弯半径；

为位置1处的航向角；W_car为车辆宽度；W_spot为车位宽度；L_rear为车辆后轴长度；h1为走廊宽度；y_Cr0为以R_min为转向半径的在位置0处转向中心Cr0的纵坐标；

由于车辆是任意起始位置的，因此需要在不同的目标线下，均能够生成路径完成泊车任务，将求解的y0min和y0max作为分类讨论的条件；分成以下两种情况对目标线求解；

当目标线位置大于y0min时(y0max>y0>y0min)，通过公式(13)计算在目标线为y0时到位置1的路径，即能够对R₀确定。

其中，R₀为以在位置0处转向中心Cr0为转向中心的转向半径；y0为位置0处车辆后轴中心纵坐标，也是目标线的坐标；y1为位置1处车辆后轴中心纵坐标；

为1位置处的航向角；

当目标线位置小于y0min时，即y0<y0min，首先，确定目标线，即y0的值是已知的；当目标线的位置确定时，车辆位置1通过公式(14)计算。

其中，R₀为以Cr0为转向中心的转向半径；y0为位置0处车辆后轴中心纵坐标；y1为位置1处车辆后轴中心纵坐标；

为位置1处的航向角；θ2为后轴中心点到车辆右前点的距离与车辆坐标系X轴夹角；R_min为车辆最小转弯半径；L_rear为车辆后轴长度；L_car为车辆的长度；W_car为车辆宽度； x_cl1，y_cl1为在位置1处转向中心Cl1的坐标；x_G，y_G为车位右上角点G的坐标；x_F，y_F为车位左上角点F的坐标。

由于泊车空间的大小会对垂直泊车的机动次数有影响。为了满足在空间变小时，仍能够完成驾驶任务，通过几何规划的方式将泊车规划过程进行划分。将垂直正向泊车分为了一次机动，二次机动与三次机动泊车。以下将说明垂直正向机动次数泊车与泊车走廊空间大小以及泊车位的大小的关系。

所述一次机动泊车的具体方法如下：

参阅图7，满足一次泊车入位的条件为车辆右前角点B点与车位左上角点F点不碰撞，位置1车辆右后角点C点绕转向中心Cr的轨迹与车位走廊的上边界相切。通过以上两个条件，即可得到车辆在一次正向机动下生成泊车轨迹。R₁为一次机/动的圆弧半径大小，其范围为R₁≥R_min。Z点为车辆的角点B点在如图7所示的位置0绕转向中心Cr_int与车位的左边界线相交的点。

31)确定车辆位置0时的横纵坐标值；

当车辆在位置0时，位置坐标为

32)确定先走过的直线的长度大小；

(x_F-x_{Cr_int})²+(y_Z-y_{Cr_int})²＝(R₁+W_car/2)²+(L_car-L_rear)² (15)

其中，x_F为车位左上角点F的横坐标；y_Z为车辆右前角点B点绕在位置0处转向中心Cr_int转动与车位左侧交点的纵坐标；x_{Cr_int}，y_{Cr_int}为在位置0处转向中心Cr_int的坐标；R₁为一次机动的圆弧半径大小；L_rear为车辆后轴长度；L_car为车辆的长度；W_car为车辆宽度；当x_F＝0,得y_Z以R₁为变量的方程；在一次泊车机动的过程中，所走的直线的距离为L1≥y_F-y_Z，得到点Cr(x_{Cr_int},y_{Cr_int}+L1)；

33)确定车位走廊宽度h1的大小；

点C到Cr的距离为

目标函数为h1,通过公式(16)能够求得在满足一次机动泊车的车位外的走廊大小；当R₁＝R_min时，h1的值为最小；

h1＝y_{Cr_int}+L1+d_C-L_spo (16)

其中，R₁为一次机动的圆弧半径大小；W_car为车辆宽度；L_rear为车辆后轴长度；L_spot为车位长度；y_{Cr_int}为转向中心Cr_int的纵坐标；L1为一次泊车机动的过程中，所走的直线的距离。

参阅8a、图8b和图8c，所述二次机动泊车的具体方法如下：

若车位的走廊宽度为h2，则一次机动泊车无法满足泊车的驾驶任务。路径从终点位置往外规划，当车辆在0位置时，位置坐标为(Xend，Yend)。车辆从位置0到位置1需要走过直线距离L1≥y_F-y。从位置1以R₁转弯半径绕转向中心Cr转过的角度为α。从位置2到位置3，通过以R₂为转弯半径绕转向中心Cl转过的角度为π/2-α。该过程D点以Cl为转向中心，转弯半径为d_D，轨迹与y＝y_F相切。

41)确定车辆在位置0时的横纵坐标值；

若车位的走廊宽度为h2，则一次机动泊车无法满足泊车的驾驶任务。路径从终点位置往外规划，当车辆在0位置时，位置坐标为(Xend，Yend)；

42)计算车辆位置0到车辆位置1的直线的长度；

车辆从位置0到位置1需要走过直线距离L1≥y_F-y；

从位置1以R₁转弯半径绕在位置1处转向中心Cr转过的角度为α；

以R₂为转弯半径绕在位置2处转向中心Cl转过的角度为π/2-α，其中D点以在位置2处转向中心Cl为转向中心，转弯半径为d_D，轨迹与y＝y_F相切；

45)计算车辆走廊宽度h2的大小。

x_Cr1＝x1+R₁

y_Cr1＝y1

(x_F-x_{Cr_int})²+(y_Z-y_{Cr_int})²＝(R₁+W_car/2)²+(L_car-L_rear)²

x_F＝0

L1≥y_F-y_Z

y1＝y_{Cr_int}+L1

x1＝W_spot/2

x2＝x1+R₁(1-cosα)

y2＝y1+R₁sinα

x_Cl2＝x2-R₂cosαy_Cl2＝y2+R₂sinα

其中，d_C为车辆的右后角点到转向中心的距离；R_min为车辆最小转弯半径；W_car为车辆宽度；L_rear为车辆后轴长度；W_spot为车位宽度；L_spot为车位长度； x1，y1为位置1的坐标值；x2，y2为位置2的坐标值；R₁为一次机动的圆弧半径大小；x_F，y_F为车位左上角点F的坐标；y_Z为车辆右前角点B点绕在位置0处转向中心Cr_int转动与车位左侧交点的纵坐标；x_Cr1，y_Cr1为在位置1处转向中心Cr的坐标；x_{Cr_int}，y_{Cr_int}为在位置0处转向中心Cr_int的坐标； x_Cl2，y_Cl2为在位置2处转向中心Cl2的坐标值；L1为一次泊车机动的过程中，所走的直线的距离；C1，C2为后轴中心到车辆右后点以及右前点的距离；θ1为后轴中心点到车辆右后点的距离与车辆坐标系X轴夹角；θ2为后轴中心点到车辆右前点的距离与车辆坐标系X轴夹角；α为从位置1以R₁转弯半径绕在位置1处转向中心Cr转过的角度；x_C2，y_C2为位置2车辆右后的角点C的坐标值；x_D2，y_D2为位置2车辆左前的角点D的坐标值；β为第二次机动所转过的角度；R1≥R_min，R2≥R_min，0≤α≤π/2， y_C2为目标函数。

所述三次机动泊车的具体方法如下：

参阅图9a、图9b和图9c，从位置1到位置2，需要以转向中心Cr1为转向中心，以R1为转弯半径，转过的角度为α。车辆在位置2处，通过第十二Cl2为转向中心，以R2为转弯半径，转过的角度为β。在位置3处，车辆的角点y_D′在y＝y_F。在位置3到位置4的过程，绕转向中心Cr3，以R3为转弯半径，转过的角度为π/2-α-β。且在位置3到位置4的过程，需要保证车辆角点C在绕Cr3运动时，不与车位走廊的上边界发生碰撞。变量为R1≥R_min，R2≥R_min，0≤α≤π/2，R3≥R_min，0≤β≤π/2-α，目标函数y_C2。根据上述过程，得公式(17)，求得y_C2的最小值即为h3的最小值。

51)确定车辆位置0的横纵坐标值；

当车辆在位置0时，位置坐标为

从位置0到位置2，需要以Cr1为转向中心，以R1为转弯半径，转过的角度为α，走过的直线长度为L1≥y_F-y；

车辆在位置2处，通过在位置2处转向中心Cl2为转向中心，以R2为转弯半径，转过的角度为β；在位置3处，车辆的角点y_D′在y＝y_F；

在位置3到位置4的过程，绕在位置3处转向中心Cr3，以R3为转弯半径，转过的角度为π/2-α-β；且在位置3到位置4的过程，需要保证车辆角点C在绕在位置3处转向中心Cr3运动时，不与车位走廊的上边界发生碰撞；

55)计算车辆走廊的宽度h3的大小，并且求h3的最小值。

x_Cr1＝x1+R₁

y_Cr1＝y1

(x_F-x_{Cr_int})²+(y_Z-y_{Cr_int})²＝(R₁+W_car/2)²+(L_car-L_rear)²

x_F＝0

L1＝y_F-y

y1＝y_{Cr_int}+L1

x1＝W_spot/2

x2＝x1+R₁(1-cosα)

y2＝y1+R₁sinα

xc2＝x2-C2*cos(θ2+α)

yc2＝y2+C2*sin(θ2+α)

xd2＝x2-C1*sin(π-α-αC1)

yd2＝y2-C1*cos(π-α-αC1)

x_Cl2＝x2-R₂cosα y_Cl2＝y2+R₂sinα

x3＝x_Cl2-R₂cos(α+β)

y3＝y_Cl2-R₂sin(α+β)

x_Cr3＝x3-R₃cos(α+β)

y_Cr3＝y3-R₃sin(α+β)

其中，R_min为车辆最小转弯半径；R₁为第一次机动的圆弧半径大小；R₂为第二次机动的圆弧半径大小；R₃为第三次机动的圆弧半径大小；W_car为车辆宽度；L_rear为车辆后轴长度；W_spot为车位宽度；L_spot为车位长度；C1，C2为后轴中心到车辆右后点以及右前点的距离；L1为一次泊车机动的过程中，所走的直线的距离；α为从位置1到位置2，需要以在位置1处转向中心Cr1为转向中心，以R1为转弯半径转过的角度；β为车辆在位置2处，通过在位置2处转向中心Cl2为转向中心，以R2为转弯半径转过的角度；x1，y1为位置1的坐标值；x2，y2为位置2的坐标值；x3，y3为位置3的坐标值；x_Cr1，y_Cr1为位置1的在位置1处转向中心Cr1的坐标；x_{Cr_int}，y_{Cr_int}为在位置0处转向中心Cr_int的坐标； x_Cl2，y_Cl2为位置2的在位置2处转向中心Cl2的坐标值；x_Cr3，y_Cr3为位置3的在位置3处转向中心Cr3的坐标；x_F，y_F为车位左上角点F的坐标；y_Z为车辆右前角点B点绕转向中心Cr_int转动与车位左侧交点的纵坐标；xc2，yc2为位置2处车辆右后角点坐标；xd2，yd2为位置2处车辆左前角点坐标；θ1为后轴中心点到车辆右后点的距离与车辆坐标系X轴夹角；θ2为后轴中心点到车辆右前点的距离与车辆坐标系X轴夹角；变量为R1≥R_min，R2≥R_min，0≤α≤π/2，R3≥R_min，0≤β≤π/2-α， y_C2为目标函数，y_C2的最小值即为h3的最小值。

实施例

将本发明应用到模拟情景中，具体参数如表1所示：

表1

参数	符号	取值
			车位的长度	Lspot	5.6m
车位的宽度	Wspot	2.5m
			最小转弯半径	Rmin	4.2m
轴距	L	2.405m
			前悬长度	Lfront	0.8m
后悬长度	Lrear	0.95m
			车辆长度	Lcar	4.155m
车辆宽度	Wcar	1.645m

1、水平泊车

水平泊车车位参数如下所示：

当车位走廊右边的空间是足够时，确定车辆的位置能够满足在以最小转弯半径，左转弯的情况下不与车位的角点碰撞。

参阅图10，目标线的生成可以根据当前的起始位置进行确定。若当前的位置生成的目标线存在能够进行泊车范围，则车辆通过直行便可到达目标线。若不存在可行的泊车范围，则将车身调整到

后，再重新确定目标线集，目标线可以根据调整结束的位置纵坐标来确定，也可根据经验进行确定。

当车位走廊右边的空间不足够时，通过利用车位的空间，生成泊车路径，如图11所示。这种方法能够避免走廊右边的空间存在障碍物，现有几何规划无法泊车的情况。该种泊车方式同时也会生成许多目标线集，从而满足在不同车辆起始位置生成泊车路径。

2、垂直正向泊车

参阅图12-14，分别为一次机动垂直泊车，二次机动垂直泊车和三次机动垂直泊车的规划结果。当h＝7.519m时，规划的结果为一次机动泊车。当h＝5.4058m时，规划的结果为二次机动泊车。当h＝5.3257m时，规划的结果为三次机动泊车。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。

Claims

1.一种拟人化的自动泊车路径规划方法，其特征在于，自动泊车路径规划包括水平泊车路径规划和垂直泊车路径规划；所述水平泊车包括平行泊车和利用车位空间泊车；所述垂直泊车包括一次机动泊车、二次机动泊车和三次机动泊车。

2.根据权利要求1所述的一种拟人化的自动泊车路径规划方法，其特征在于，所述平行泊车的具体方法如下：

定义平行泊车阶段位置1为车辆在停车位内的目标位置；

11)确定车辆起始位置，并且确定目标线；

12)确定水平泊车的目标线上的起始位置的左边界O1点坐标；

13)确定水平泊车的目标线上的起始位置的右边界O2点坐标；

所述利用车位空间泊车的具体方法如下：

21)确定到达能够最大利用位置1的坐标；

22)确定到达位置2的坐标；

23)确定由位置2到位置5的过程；

24)确定根据位置1生成的目标集的过程；

所述一次机动泊车的具体方法如下：

31)确定车辆位置0时的横纵坐标值；

32)确定先走过的直线的长度大小；

33)确定车位走廊宽度h1的大小；

所述二次机动泊车的具体方法如下：

41)确定车辆位置0时的横纵坐标值；

42)计算车辆位置0到车辆位置1的直线的长度；

45)计算车辆走廊宽度h2的大小。

所述三次机动泊车的具体方法如下：

51)确定车辆位置0的横纵坐标值；

55)计算车辆走廊的宽度h3的大小，并且求h3的最小值。

3.根据权利要求2所述的一种拟人化的自动泊车路径规划方法，其特征在于，所述步骤11)的具体方法如下：

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

(xcr1-xcl)²+(ycr1-ycl)²＝4R_min ²

为目标线与水平线的夹角；ylocal为车辆当前的后轴中心纵坐标；xo1，yo1为车辆驶出车位后，以最小转弯半径到达目标线的车辆位置，即为起始泊车位置的左边界的坐标；

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

(xcr2-xcl)²+(ycr2-ycl)²＝(R_min+R_max)²

(xcr2-L_spot)²+(ycr2-Wspot)²＝(R_max-W_car/2)² (2)

其中，xcl，ycl为在车位内起始位置的转向中心Cl的横坐标与纵坐标；xcr2，ycr2为车辆以最大转弯半径驶出车位的车辆在水平泊车目标线可泊位置的右极限位置点O2处转向中心Cr2的坐标；R_min为车辆最小转弯半径；R_max为车辆最大转弯半径；

4.根据权利要求2所述的一种拟人化的自动泊车路径规划方法，其特征在于，所述步骤21)的具体方法如下：

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

y_D2＝h1+W_spot

(x_D1-x_cl1)²+(y_D1-y_cl1)²＝(x_D2-x_cl1)²+(y_D2-y_cl1)² (5)

其中，x_D1，y_D1为在位置1处时的D点的坐标值；x_D2，y_D2在位置2处时的D点的坐标值；x_cl1，y_cl1为在位置1处转向中心Cl1的坐标值；α为车辆绕转动中心Cl1从位置1转到位置2的转动角度；d₁为在位置1处时的D点与第三转向中心Cl1的距离；d₂为在位置2处时的D点与第三转向中心Cl1的距离；d₃为在位置1处时的D点与位置2处时的D点之间的距离。

结束位置的航向角大小：

其中，

为位置1处车辆后轴中心与水平线的夹角；α为车辆绕在位置1处转动中心Cl1从位置1转到位置2的转动角度；

位置2的坐标如下：

为位置2处的航向角；

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

min[1/((x2-x4)²+(y2-y4)²)]

Lcar-(x5+C2*cosθ₂)≤0

x5+C2*cosθ₂-Lspot≤0

W_car-(y5+C2*sinθ₂)≤0

y5+C2*sinθ₂-Wspot≤0 (9)

其中，x2，y2为位置2处车辆后轴中心坐标；x4，y4为位置4处车辆后轴中心坐标；x5，y5为位置5处车辆后轴中心坐标；xb4，yb4为4位置车辆右前的角点B的坐标值；xcr2，ycr2为在位置2处转向中心Cr2的坐标值；xcl4，ycl4为在位置4处转向中心Cl4的坐标值；R₃为以在位置2处转向中心Cr2为转动中心的转动半径；R₄为以在位置4处转向中心Cl4为转动中心的转动半径；R₅为以在位置5处转向中心Cr5为转动中心的转动半径；

为位置2处的航向角；

其中，在位置5处转向中心Cr5的坐标值通过下式得出：

x₅＝x_cr3 y₅＝y_cr3+R₅ (10)

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

车辆位置1确定之后，能够确定车辆位置0坐标从而得到目标线的大小；x0，y0为车辆起始位置的坐标值，y＝y0为目标线的坐标

为位置1处的航向角；

为位置1处的航向角；

为位置1处的航向角；θ2为后轴中心点到车辆右前点的距离与车辆坐标系X轴夹角；R_min为车辆最小转弯半径；L_rear为车辆后轴长度；L_car为车辆的长度；W_car为车辆宽度；x_cl1，y_cl1为在位置1处转向中心Cl1的坐标；x_G，y_G为车位右上角点G的坐标；x_F，y_F为车位左上角点F的坐标。

5.根据权利要求2所述的一种拟人化的自动泊车路径规划方法，其特征在于，

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

当车辆在位置0时，位置坐标为

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

(x_F-x_{Cr_int})²+(y_Z-y_{Cr_int})²＝(R₁+W_car/2)²+(L_car-L_rear)² (15)

所述33)的具体方法如下：

点C到Cr的距离为

h1＝y_{Cr_int}+L1+d_C-L_spot (16)

6.根据权利要求2所述的一种拟人化的自动泊车路径规划方法，其特征在于，所述步骤41)的具体方法如下：

当车辆在0位置时，位置坐标为(Xend，Yend)；

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

车辆从位置0到位置1需要走过直线距离L1≥y_F-y；

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

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

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

x_Cr1＝x1+R₁

y_Cr1＝y1

(x_F-x_{Cr_int})²+(y_Z-y_{Cr_int})²＝(R₁+W_car/2)²+(L_car-L_rear)²

x_F＝0

L1≥y_F-y_Z

y1＝y_{Cr_int}+L1

x1＝W_spot/2

x2＝x1+R₁(1-cosα)

y2＝y1+R₁sinα

x_Cl2＝x2-R₂cosα y_Cl2＝y2+R₂sinα

其中，d_C为车辆的右后角点到转向中心的距离；R_min为车辆最小转弯半径；W_car为车辆宽度；L_rear为车辆后轴长度；W_spot为车位宽度；L_spot为车位长度； x1，y1为位置1的坐标值；x2，y2为位置2的坐标值；R₁为一次机动的圆弧半径大小；x_F，y_F为车位左上角点F的坐标；y_Z为车辆右前角点B点绕在位置0处转向中心Cr_int转动与车位左侧交点的纵坐标；x_Cr1，y_Cr1为在位置1处转向中心Cr的坐标；x_{Cr_int}，y_{Cr_int}为在位置0处转向中心Cr_int的坐标； x_Cl2，y_Cl2在位置2处转向中心Cl2的坐标值；L1为一次泊车机动的过程中，所走的直线的距离；C1，C2为后轴中心到车辆右后点以及右前点的距离；θ1为后轴中心点到车辆右后点的距离与车辆坐标系X轴夹角；θ2为后轴中心点到车辆右前点的距离与车辆坐标系X轴夹角；α为从位置1以R₁转弯半径绕在位置1处转向中心Cr转过的角度；x_C2，y_C2为位置2车辆右后的角点C的坐标值；x_D2，y_D2为位置2车辆左前的角点D的坐标值；β为第二次机动所转过的角度；R1≥R_min，R2≥R_min，0≤α≤π/2，y_C2为目标函数。

7.根据权利要求2所述的一种拟人化的自动泊车路径规划方法，其特征在于，所述步骤51)的具体方法如下：

当车辆在位置0时，位置坐标为

所述52)的具体方法如下：

从位置0到位置2，需要以Cr1为转向中心，以R1为转弯半径，转过的角度为α，走过的直线长度为L1≥y_F-y

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

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

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

x_Cr1＝x1+R₁

y_Cr1＝y1

(x_F-x_{Cr_int})²+(y_Z-y_{Cr_int})²＝(R₁+W_car/2)²+(L_car-L_rear)²

x_F＝0

L1＝y_F-y

y1＝y_{Cr_int}+L1

x1＝W_spot/2

x2＝x1+R₁(1-cosα)

y2＝y1+R₁sinα

xc2＝x2-C2*cos(θ2+α)

yc2＝y2+C2*sin(θ2+α)

xd2＝x2-C1*sin(π-α-αC1)

yd2＝y2-C1*cos(π-α-αC1)

x_Cl2＝x2-R₂cosα y_Cl2＝y2+R₂sinα

x3＝x_Cl2-R₂cos(α+β)

y3＝y_Cl2-R₂sin(α+β)

x_Cr3＝x3-R₃cos(α+β)

y_Cr3＝y3-R₃sin(α+β)

其中，R_min为车辆最小转弯半径；R₁为第一次机动的圆弧半径大小；R₂为第二次机动的圆弧半径大小；R₃为第三次机动的圆弧半径大小；W_car为车辆宽度；L_rear为车辆后轴长度；W_spot为车位宽度；L_spot为车位长度；C1，C2为后轴中心到车辆右后点以及右前点的距离；L1为一次泊车机动的过程中，所走的直线的距离；α为从位置1到位置2，需要以在位置1处转向中心Cr1为转向中心，以R1为转弯半径转过的角度；β为车辆在位置2处，通过在位置2处转向中心Cl2为转向中心，以R2为转弯半径转过的角度；x1，y1为位置1的坐标值；x2，y2为位置2的坐标值；x3，y3为位置3的坐标值；x_Cr1，y_Cr1为位置1的在位置1处转向中心Cr1的坐标；x_{Cr_int}，y_{Cr_int}为在位置0处转向中心Cr_int的坐标； x_Cl2，y_Cl2为位置2的在位置2处转向中心Cl2的坐标值；x_Cr3，y_Cr3为位置3的在位置3处转向中心Cr3的坐标；x_F，y_F为车位左上角点F的坐标；y_Z为车辆右前角点B点绕转向中心Cr_int转动与车位左侧交点的纵坐标；xc2，yc2为位置2处车辆右后角点坐标；xd2，yd2为位置2处车辆左前角点坐标；θ1为后轴中心点到车辆右后点的距离与车辆坐标系X轴夹角；θ2为后轴中心点到车辆右前点的距离与车辆坐标系X轴夹角；变量为R1≥R_min，R2≥R_min，0≤α≤π/2，R3≥R_min，0≤β≤π/2-α，y_C2为目标函数，y_C2的最小值即为h3的最小值。