CN114132305A - 用于垂直与斜列泊车路径规划方法、系统、存储介质及车辆 - Google Patents

Abstract

本发明公开了一种用于垂直与斜列泊车路径规划方法、系统、存储介质及车辆，包括：步骤1，建立车辆动力学模型，得出车辆最小转弯半径R和最短行驶轨迹；步骤2，基于超声波和摄像头的输入，对初始探测的障碍物信息进行处理，得出车位的四个角点坐标、车位可利用空间宽度和目标点位置；步骤3，使车辆行驶至最佳入库点PosOK；步骤4，根据不同车位类型，对库内信息进行探测，得出绕右前角点P1的最佳路径；步骤5，搜索出最佳揉库路径，求出车辆最佳出库调整路径与入库路径。本发明能够解决在超声波雷达与摄像头探测不充分的情况下，结合路径规划算法，对车位进行更加充分探测并安全泊车。

Description

用于垂直与斜列泊车路径规划方法、系统、存储介质及车辆

技术领域

本发明属于自动泊车技术领域，具体涉及一种用于垂直与斜列泊车路径规划方法、系统、存储介质及车辆。

背景技术

近年来，随着汽车的保有量呈指数增加，泊车难的问题越发凸显。该问题吸引了外界不少关注，尤其是各大主机厂。当今智能驾驶的兴起，装配自主泊车系统的车辆越来越普及。在现有硬件资源不足以支撑启发式搜索与深度优先搜索的前提下，采用几何法对自主泊车进行局部路径规划，依然为一大趋势。另外，几何法路径规划更有利于车辆对未知环境的探测，增加泊车成功率。如专利文献CN106515722B公开的一种垂直泊车轨迹规划方法，建立车辆转向运动学模型，采用绕墙走策略和动态窗口法完成规划，使车辆实现有效、安全地垂直泊车。

在现行的智能驾驶车辆已完成量产的框架中，由于成本与可靠性原因，一些相对较为准确车位与障碍物检测方式如激光雷达、高精地图等并没有得到广泛应用。在量产APA的车型中，如特斯拉model3、小鹏P7、长安Uni-T，均通过超声波雷达和摄像头对车位和目标进行检测，由于超声波雷达装配位置和摄像头探测能力的局限性，对车位角点等初始探测精度难以有效保证，需要通过路径规划方法，对车位进行充分探测，达到安全快速的泊车。

因此，有必要开发一种新的用于垂直与斜列泊车路径规划方法、系统、存储介质及车辆。

发明内容

本发明的目的是提供一种用于垂直与斜列泊车路径规划方法、系统及车辆，以解决在超声波雷达与摄像头探测不充分的情况下，结合路径规划算法，对车位进行更加充分探测并安全泊车。

本发明的另一目的是提供一种多传感器多目标关联匹配方法、系统及车辆，能实现多源异构传感器之间的目标关联，还原驾驶真实场景。

第一方面，本发明所述的一种用于垂直与斜列泊车路径规划方法，包括以下步骤：

步骤1，基于车辆物理学约束，建立车辆动力学模型，得出车辆最小转弯半径R和最短行驶轨迹Lmin；

步骤2，基于超声波和摄像头的输入，对初始探测的障碍物信息进行处理，得出车位的四个角点坐标、车位可利用空间宽度Wroad和泊车完成车辆位置TargetP；其中，四个角点坐标分别为左前角点P0、右前角点P1、左后角点P2和右后角点P3；

步骤3，基于车位的角点坐标、本车姿态、车辆宽度和最小转弯半径，进行库外调整车辆姿态，使车辆行驶至最佳入库点PosOK；

步骤4，当车辆行驶至最佳入库点PosOK之后，根据不同车位类型，进行安全绕障，且逼近左前角点P0点，对库内信息进行探测，得出绕右前角点P1的最佳路径；

步骤5，基于车位的角点坐标、本车位姿、车位可利用空间宽度Wroad、目标点位置TargetP和最佳入库点P，通过不同转弯半径、转向弧度进行遍历，以最短路径为代价函数，搜索出最佳揉库路径，求出车辆最佳出库调整路径与入库路径；

其中，所述最佳入库点PosOK是指在满足障碍物、车辆模型的约束条件下，以1R绕P1点之后车辆入库最深为代价，求出的1D单步最佳停车点。

所述最佳入库点P为指以最短路径为代价，求出的2D单步最佳停车点。

可选地，所述步骤1中，最小转弯半径R的求解方法如下：

R＝L/tan(θ_max)；

式中，R表示最小转弯半径，L表示车辆轴距，θ_max表示车辆最大前轮转角。

可选地，所述步骤3中，所述最佳入库点PosOK的求解方法如下：

其中P_1x，P_1y为P1的坐标，R为车辆最小转弯半径，CurrentY为车辆当前纵坐标，CarWidth为车辆宽度，SafeMargin为安全余量，θ为本车当前位置到最近碰撞点的行驶角度；

所述最佳入库点PosOK的坐标为(PosOK_x，CurrentY)。

可选地，所述步骤4中，当车辆达到最佳入库点PosOK之后，需要对右前角点P1进行绕行，通过对不同路径进行遍历搜索，当搜索路径中车辆碰到左前角点P0时，判断车辆与障碍物发生碰撞，跳出遍历，并得出绕右前角点P1的最佳路径。

可选地，判断车辆与障碍物发生碰撞的方法如下：

计算S₁，

如果λS_car>S₁，则判定发生碰撞；

其中：λ为安全阈值；

为P0点和车辆的RL顶点、FL顶点组成的三角形面积；

为P0点和车辆的RL顶点、RR顶点组成的三角形面积；

为P0点和车辆的RR顶点、FR顶点组成的三角形面积；

为P0点和车辆FR顶点、FL顶点组成的三角形面积；S₁为

和

之和；S_car为车辆四个顶点所围矩形面积；P_RL、P_FL、P_RR、P_FR分别表示车辆的左后顶点、左前顶点、右后顶点和右前顶点。

可选地，所述步骤5中，最佳揉库路径还需包含最短行驶轨迹L_min的限制。

可选地，所述步骤5中，所述最佳入库点P的求解方法如下：

选取

为起点，设i＝10,20，…，300cm；

PiontR＝(p_x+R*sin(θ_target),p_y-R*cos(θ_target))

其中，P_0x和P_0y为P₀点的坐标；P_x和P_y为p的坐标；θ_target为泊车完成时目标停车角度；采用10cm为分辨率对i进行遍历搜索，其中PointR为车辆转向中心，当发现车辆离P1点最近距离Dist<0时，此时p点为最佳入库点。

第二方面，本发明所述的一种用于垂直与斜列泊车路径规划系统，包括存储器和控制器，所述存储器内存储有计算机可读程序，所述计算机可读程序被控制器调用时能执行如本发明所述的用于垂直与斜列泊车路径规划方法的步骤。

第三方面，本发明所述的一种车辆，采用如本发明所述的用于垂直与斜列泊车路径规划系统。

第四方面，本发明所述的一种存储介质，其内存储有计算机可读程序，所述计算机可读程序被调用时能执行如本发明所述的用于垂直与斜列泊车路径规划方法的步骤。

本发明具有以下优点：本发明采用圆弧与直线的组合，遵守车辆动力学约束，保证了路径可行性和稳定性。同时，针对传感器性能，对垂直和斜列车位进行路径规划，能够有效解决初始探测车位角点不准的情况下，安全快速的泊车。本发明通过建立车辆横向动力学模型，采用几何路径规划方法，结合车位类型和车位宽度信息，设计出一种垂直与斜列车位泊车路径规划方法。在本发明的规划算法之上，能够对初始探测不充分的角点进行有效检测，并对车位进行实时更新。

附图说明

图1为现有量产支持泊车的超声波雷达分布图；

图2为本实施例中PosOK点求解示意图；

图3为本实施例中完整泊车过程中，垂直泊车路径示意图。

具体实施方式

下面结合附图对本发明作进一步说明。

如图1所示，本实施例中，一种用于垂直与斜列泊车路径规划方法，包括以下步骤：

步骤1，基于车辆物理学约束，建立车辆动力学模型，得出车辆最小转弯半径R和最短行驶轨迹Lmin。

步骤2，基于超声波和摄像头的输入，对初始探测的障碍物信息进行处理，得出车位的四个角点坐标、车位可利用空间宽度Wroad和泊车完成车辆位置TargetP；其中，四个角点坐标分别为左前角点P0、右前角点P1、左后角点P2和右后角点P3。

步骤3，基于车位的角点坐标、本车姿态、车辆宽度和最小转弯半径，进行库外调整车辆姿态，使车辆行驶至最佳入库点PosOK；

步骤4，当车辆行驶至最佳入库点PosOK之后，根据不同车位类型，进行安全绕障，且逼近左前角点P0点，对库内信息进行探测，得出绕右前角点P1的最佳路径；

步骤5，基于车位的角点坐标、本车位姿、车位可利用空间宽度Wroad、目标点位置TargetP和最佳入库点P，通过不同转弯半径、转向弧度进行遍历，以最短路径为代价函数，搜索出最佳揉库路径，求出车辆最佳出库调整路径与入库路径。

本实施例中，所述最佳入库点PosOK是指在满足障碍物、车辆模型等约束条件下，以1R(即第一次后退挡)绕P1点之后车辆入库最深为代价，求出的1D(即第一次前进挡)单步最佳停车点。所述最佳入库点P为指以最短路径为代价，求出的2D(即第二次前进挡)单步最佳停车点。

本实施例中，所述步骤1中，最小转弯半径R的求解方法如下：

R＝L/tan(θ_max)；

式中，R表示最小转弯半径，L表示车辆轴距，θ_max表示车辆最大前轮转角。

值得一提的是，由于自主泊车过程中车速较低，本实施例中忽略了车辆轮胎滑移、车辆内外轮转角不一致等因素。另外，所述步骤1中最短行驶轨迹Lmin为考虑了本车轮脉冲分辨率、泊车控制过程中最大加速度和最大减速度、乘坐舒适性的经验值。

本实施例中，所述步骤2中，基于超声波拟合线段输入，以及车位附近障碍物类型、位置和大小信息，对整个泊车过程中可行驶区域进行处理。值得一提的是，可行驶区域的处理可针对不同车位类型、不同障碍物类型，不同车位宽度等方面进行考量，由此得出车位的4个角点坐标(P0,P1,P2,P3)、车位可利用空间宽度(Wroad)、目标点位置(TargetP)。

本实施例中，所述步骤3中，库外姿态调整主要目的为使车辆行驶至PosOK点。所述最佳入库点PosOK的求解方法如下：

其中P_1x，P_1y为P1的坐标，R为车辆最小转弯半径，CurrentY为车辆当前纵坐标，CarWidth为车辆宽度，SafeMargin为安全余量，θ为本车当前位置到最近碰撞点的行驶角度；所述最佳入库点PosOK的坐标为(PosOK_x，CurrentY)。

本实施例中，所述步骤4中，当车辆达到最佳入库点PosOK之后，需要对右前角点P1进行绕行，通过对不同路径进行遍历搜索，当搜索路径中车辆碰到左前角点P0时，判断车辆与障碍物发生碰撞，跳出遍历，并得出绕右前角点P1的最佳路径。

本实施例中，判断车辆与障碍物发生碰撞的方法如下：

计算S₁，

如果λS_car>S₁，则判定发生碰撞；

其中：λ为安全阈值；

为P0点和车辆的RL顶点、FL顶点组成的三角形面积；

为P0点和车辆的RL顶点、RR顶点组成的三角形面积；

为P0点和车辆的RR顶点、FR顶点组成的三角形面积；

为P0点和车辆FR顶点、FL顶点组成的三角形面积；S₁为

和

之和；S_car为车辆四个顶点所围矩形面积；P_RL、P_FL、P_RR、P_FR分别表示车辆的左后顶点、左前顶点、右后顶点和右前顶点。

本实施例中，所述步骤5中，最佳揉库路径还需包含最短行驶轨迹L_min的限制。

本实施例中，所述步骤5中，所述最佳入库点P的求解方法如下：

选取入库点初始起点(可从车库中心点开始)往上搜索，具体为：

选取

为起点，设i＝10，20，…，300cm;

PiontR＝(p_x+R*sin(θ_{t arg et})，p_y-R*cos(θ_{t arg et}))

其中，P_0x和P_0y为P₀点的坐标；P_x和P_y为p的坐标；θ_{t arg et}为泊车完成时目标停车角度；采用10cm为分辨率对i进行遍历搜索，其中PointR为车辆转向中心，当发现车辆离P1点最近距离Dist<0时，此时p点为最佳入库点。

本实施例中，一种用于垂直与斜列泊车路径规划系统，包括存储器和控制器，所述存储器内存储有计算机可读程序，所述计算机可读程序被控制器调用时能执行如本实施例中所述的用于垂直与斜列泊车路径规划方法的步骤。

本实施例中，一种车辆，采用如本实施例中所述的用于垂直与斜列泊车路径规划系统。

本实施例中，一种存储介质，其内存储有计算机可读程序，所述计算机可读程序被调用时能执行如本实施例中所述的用于垂直与斜列泊车路径规划方法的步骤。

如图1所示，为现有量产车型超声波雷达安装位置(UPA是一种短程超声波传感器，APA是一种远程超声波传感器)，在进行车位识别时，前后角雷达对车库内进行探测，由于车辆侧面并未装有超声波雷达传感器，故初始探测对库内探测并不充分。

如图2所示，为本实施例中，求解Pos0K点的过程，可以看出，Pos0K点目的为在尽量短的路径下，对车位进行更充分探测。在达到Pos0K点之后，保证安全余量的同时，车辆路径绕过P1点，对P0点进行充分探测，并尽可能多的克服初始角度与目标角度的差距。

如图3所示，为本实施例中路径规划各步骤示意图。其中状态①为库外调整，主要目的为车库外调整车辆和目标车位的相对姿态，并使车辆达到Pos0K点；状态②为1R绕P1入库，主要目的为在保证不碰P1点的情况下，入库更深入，并进行更充分的探测；状态③为D挡前进调整，目的为不碰到障碍物的情况下，调整更好的入库位姿，状态④为R挡入库，目的为不碰到障碍物的情况下，达到库内目标停车点。值得一提的是，若P1点探测位置不准，会重复状态①和状态②，重新绕P1点入库；若状态④无法达到指定停车点，会重复状态③和状态④，调整姿态后重新入库。

本方法针对传统几何法进行改进，相对于启发式搜索与深度优先搜索等算法，对资源占用更低；同时相对于传统几何法，对于部分路径规划步骤，运用了部分启发式搜素思想；更加重要的是，结合现有智能驾驶车辆探测性能，对路径规划进一步约束，求解出更安全、效率更高的路径。

因此，本实施例中所述的垂直与斜列车位自主泊车路径规划方法，解决了传统路径不够智能、行驶路径长的问题；也没有启发式搜索资源占用问题；对现有的传感器更加友好，能够很好地在障碍物环境已知的情况下，完成垂直与斜列车位自主泊车。

Claims (10)

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

步骤1，建立车辆动力学模型，得出车辆最小转弯半径R和最短行驶轨迹Lmin。

步骤2，基于超声波和摄像头的输入，对初始探测的障碍物信息进行处理，得出车位的四个角点坐标、车位可利用空间宽度Wroad和泊车完成车辆位置TargetP；其中，四个角点坐标分别为左前角点P0、右前角点P1、左后角点P2和右后角点P3；

步骤3，基于车位的角点坐标、本车姿态、车辆宽度和最小转弯半径，进行库外调整车辆姿态，使车辆行驶至最佳入库点PosOK；

步骤4，当车辆行驶至PosOK点之后，根据不同车位类型、实时车位变化，进行安全绕障，且逼近左前角点P0点，对库内信息进行探测；

步骤5，基于车位的角点坐标、本车位姿、车位可利用空间宽度Wroad、目标点位置TargetP和最佳入库点P，通过不同转弯半径、转向弧度进行遍历，以最短路径为代价函数，搜索出最佳揉库路径，求出车辆最佳出库调整路径与入库路径；

其中，所述最佳入库点PosOK是指在满足障碍物、车辆模型的约束条件下，以1R绕P1点之后车辆入库最深为代价，求出的1D单步最佳停车点。

所述最佳入库点P为指以最短路径为代价，求出的2D单步最佳停车点。

2.根据权利要求1所述的用于垂直与斜列泊车路径规划方法，其特征在于：所述步骤1中，最小转弯半径R的求解方法如下：

R＝L/tan(θ_max)；

式中，R表示最小转弯半径，L表示车辆轴距，θ_max表示车辆最大前轮转角。

3.根据权利要求1或2所述的用于垂直与斜列泊车路径规划方法，其特征在于：所述步骤3中，所述最佳入库点PosOK的求解方法如下：

PosOK_x＝(R-CarWidth/2-SafeMargin)*sin(θ)+P_1x

其中：P_1x，P_1y为P1的坐标，R为车辆最小转弯半径，CurrentY为车辆当前纵坐标，CarWidth为车辆宽度，SafeMargin为安全余量，θ为本车当前位置到最近碰撞点的行驶角度；

所述最佳入库点PosOK的坐标为(PosOK_x，CurrentY)。

4.根据权利要求3所述的用于垂直与斜列泊车路径规划方法，其特征在于：所述步骤4中，当车辆达到最佳入库点PosOK之后，需要对右前角点P1进行绕行，通过对不同路径进行遍历搜索，当搜索路径中车辆碰到左前角点P0时，判断车辆与障碍物发生碰撞，跳出遍历，并得出绕右前角点P1的最佳路径。

5.根据权利要求4所述的用于垂直与斜列泊车路径规划方法，其特征在于：判断车辆与障碍物发生碰撞的方法如下：

计算

如果λS_car>S₁，则判定发生碰撞；

其中：λ为安全阈值；

为P0点和车辆的RL顶点、FL顶点组成的三角形面积；

为P0点和车辆的RL顶点、RR顶点组成的三角形面积；

为P0点和车辆的RR顶点、FR顶点组成的三角形面积；

为P0点和车辆FR顶点、FL顶点组成的三角形面积；S₁为

和

之和；S_car为车辆四个顶点所围矩形面积；P_RL、P_FL、P_RR、P_FR分别表示车辆的左后顶点、左前顶点、右后顶点和右前顶点。

6.根据权利要求5所述的用于垂直与斜列泊车路径规划方法，其特征在于：所述步骤5中，最佳揉库路径还需包含最短行驶轨迹L_min的限制。

7.根据权利要求6所述的用于垂直与斜列泊车路径规划方法，其特征在于：所述步骤5中，所述最佳入库点P的求解方法如下：

选取

为起点，设i＝10,20，…，300cm；

PiontR＝(p_x+R*sin(θ_target),p_y-R*cos(θ_target))

其中，P_0x和P_0y为P₀点的坐标；P_x和P_y为p的坐标；θ_target为泊车完成时目标停车角度；采用10cm为分辨率对i进行遍历搜索，其中PointR为车辆转向中心，当发现车辆离P1点最近距离Dist<0时，此时p点为最佳入库点。

8.一种用于垂直与斜列泊车路径规划系统，其特征在于：包括存储器和控制器，所述存储器内存储有计算机可读程序，所述计算机可读程序被控制器调用时能执行如权利要求1至7任一所述的用于垂直与斜列泊车路径规划方法的步骤。

9.一种车辆，其特征在于：采用如权利要求8所述的用于垂直与斜列泊车路径规划系统。

10.一种存储介质，其特征在于：其内存储有计算机可读程序，所述计算机可读程序被调用时能执行如权利要求1至7任一所述的用于垂直与斜列泊车路径规划方法的步骤。

Images