CN114274952A - 垂直车位自主泊车方法、系统、存储介质及电子设备 - Google Patents

Abstract

一种基于路径规划的垂直车位自主泊车方法，涉及智能车辆控制技术领域，结合超声波雷达、摄像头采集车位角点、宽度、余量相关信息、建立车辆动力学模型计算车辆最小转弯半径和最短行驶轨迹，基于超声波和摄像头获取目标垂直车位的4个角点位置坐标、车位可利用宽度，确定待进入车位信息，同时以最靠近车辆前轮的点为坐标原点，对障碍物信息和车位信息进行坐标转换；计算泊车起点PosOK点，建立路径规划方程,调整车辆姿态至PosOK点；车辆从PosOK点启动，以最小转弯半径，按照最短行驶轨迹倒车绕行经过P1点进入车位，到达目标点位置。能更好的适应未知环境的垂直车位进行路径规划，通过实时规划，快速安全的泊车。

Description

垂直车位自主泊车方法、系统、存储介质及电子设备

技术领域

本发明涉及智能车辆控制技术领域，具体是一种智能车辆自主泊车的路径规划控制技术。

背景技术

近年来，随着汽车的保有量呈指数增加，泊车难的问题越发凸显。随着智能驾驶的兴起，自主泊车系统也越来越普及，人们的生活也得到了改善。但基于现有的传感器资源和计算资源，暂不足以支撑激光雷达等传感器的量产和启发式搜索等路径规划算法的计算，故搭配超声波雷达和摄像头融合的感知系统、几何路径规划算法依旧是现存自主泊车系统的标配。

公开号CN106945662A，发明名称“一种自动垂直泊车控制系统”的中国发明专利申请公开了如下的技术：建立车辆转向运动学模型，利用传感器获取的待泊车辆周边环境信息建立全局坐标系，结合基于阿克曼转向机构的车辆转向特性，使车辆实现有效、安全地垂直泊车。公开号：CN110271539B，名称“一种自动垂直泊车控制系统”，包括：车位识别模块，由超声波传感器和环视摄像头传感器探测车位大小及坐标，通过融合获得准确的车位信息数据；限制条件决策模块，根据车位信息数据，解算车位的转弯半径及曲率，通过公式解算限制条件，判断转弯半径是否满足限制条件，进行泊车路径规划，计算限制条件；控制执行模块，获得泊车路径规划信息，实现泊车过程。但在现有架构中，无法对位置区域进行有效探测，导致泊车激活之后，有泊车失败或卡死的风险。本文根据雷达实时感知信息，获取内部障碍物，提高垂直泊车过程中的可靠性，提升垂直泊车的整体性能。

在已有量产的传感器架构中，如特斯拉model3、小鹏P7、长安Uni-T，均通过超声波雷达和摄像头对车位和目标进行检测。如图1所示为支持泊车的超声波雷达分布图，为现有量产车型超声波雷达安装位置，在进行车位识别时，前后角雷达和摄像头对车位周边环境进行探测，由于超声波雷达和摄像头探测的局限性，无法对未知区域进行有效探知和预测。

基于现有框架，泊车激活时对未知空间的探测距离和性能有限，若自主泊车过程中环境空旷，则不需要做额外处理。但是停车场环境错综复杂，不得不设计一种基于现有探测特性并能在未知复杂场景比如窄通道下进行垂直自主泊车的路径规划方法及系统。

发明内容

本发明所要解决的技术问题是，针对现有复杂场景如窄通道智能车辆自主泊车中通过超声波雷达和摄像头对未知空间的实时探测，解决在对环境未知的情况下，结合路径规划算法，进行更高效和安全的泊车。

本发明解决上述技术问题的技术方案是，结合路径规划算法，提供一种复杂场景下的垂直车位自主泊车路径规划方法。具体为，一种基于路径规划的垂直车位自主泊车方法，所述方法包括以下步骤：

步骤1，建立车辆动力学模型，根据车辆轴距、前轮转角的平均值计算车辆最小转弯半径和最短行驶轨迹；

步骤2，基于超声波和摄像头获取目标垂直车位的4个角点位置坐标、车位可利用宽度、目标停车位置；

步骤3，基于距离车辆前轮最近的角点位置P1点和最小转弯半径计算泊车起点PosOK点，建立路径规划方程,调整车辆姿态至PosOK点；

步骤4，车辆从PosOK点启动，根据路径规划方程计算确定的转弯半径，行驶最短行驶轨迹倒车绕行经过P1点进入车位，到达目标点位置。

进一步优化，所述计算车辆最小转弯半径包括：调用公式：R_min＝L/tan(θ_max)计算最小转弯半径R_min，其中，L表示车辆轴距，θ_max表示前轮最大转角。

进一步地，所述计算PosOK点包括：根据车辆最小转弯半径、车辆当前纵坐标CurrentY、车辆宽度CarWidth、P₁点坐标(P_1x，P_1y)、安全余量SafeMargin，调用公式：

计算PosOK点位置。

进一步地，根据车辆宽度CarWidth、车辆轴距L、前轴到前保距离LF、通车道宽度W、车辆离P1点的横向距离CurrentY，建立路径规划方程：

计算基于靠近停车位的车辆前顶点倒车入库的绕行转弯半径Ra；从最小转弯半径R_min向上进行遍历，当满足公式R_a＜W-CurrentY+R时的转弯半径R为车辆倒车入库的绕行转弯半径。

进一步地，车辆倒车绕行转弯进入车位时，调用公式：

对通车道宽度进行更新。

第二方面，本发明还请求一种基于路径规划的垂直车位自主泊车系统，包括，超声波雷达、摄像头、驱动控制单元、车辆动力学模型、计算单元，超声波雷达和摄像头获取目标车位的4个角点位置坐标、车位可利用宽度、目标停车位置；计算单元调用车辆动力学模型根据车辆轴距、前轮转角的平均值计算车辆最小转弯半径和最短行驶轨迹，基于距离车辆前轮最近的角点位置P1点和车辆转弯半径R计算泊车起点PosOK点，建立路径规划方程规划路径；驱动控制单元从PosOK点启动车辆，按规划路径以车辆转弯半径倒车绕行车位前方距离车辆前轮最近角点P₁行驶最短行驶轨迹进入车位，到达目标点位置。

进一步地，计算单元调用公式：R_min＝L/tan(θ_max)计算最小转弯半径R_min，根据车辆最小转弯半径R_min、车辆本车激活泊车时纵坐标CurrentY、车辆宽度CarWidth、角点P₁点坐标(P_1x，P_1y)、安全余量SafeMargin，调用公式：

确定PosOK点，其中，L表示车辆轴距。

第三方面，本发明所述的一种存储介质，其内存储有计算机可读程序，所述计算机可读程序被控制器调用时，能执行本发明所述的垂直车位自主泊车路径规划方法。

第四方面，一种计算机设备，包括：处理器和存储器；其中，所述处理器通过读取所述存储器中存储的可执行程序代码来运行与所述可执行程序代码对应的程序，以用于实现本发明所述的垂直车位自主泊车路径规划方法。

本发明针对传感器性能，能更好的适应未知环境的垂直车位进行路径规划，有效解决初期探测不准的情况下，通过实时规划，快速安全的泊车。本发明通过建立车辆横向动力学模型，采用几何路径规划方法，采用圆与直线的组合，保证路径可行性和稳定性，同时，结合车位类型和车位宽度信息，设计出一种针对复杂场景窄通道的垂直车位泊车路径规划方法。

附图说明

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

图2为本发明PosOK点确定示意图；

图3为本发明泊车过程中直泊车路径示意图；

图4为本发明PosOK点更新过程和探测不准时绕P1点路径示意图。

具体实施方式

为便于理解本发明的技术实施方案，下面结合附图和具体实例对本发明的实施作进一步详细说明。

本发明结合路径规划算法，进行垂直路径规划，提供一种复杂场景下的垂直车位自主泊车路径规划方法。

步骤1，确定车辆最小转弯半径和最短行驶轨迹。基于阿克曼转向动力学的约束，建立自由度车辆动力学模型，根据车辆轴距L、前轮最大转角值θ_max，调用公式：R_min＝L/tan(θ_max)计算车辆转角最小转弯半径，值得一提的是，由于自主泊车过程中车速较低，可忽略车辆轮胎滑移。

步骤2，车上装载的超声波雷达和摄像头探测环境周围障碍物信息，基于超声波雷达和摄像头输入的车位环境信息，对初始探测的障碍物信息进行处理，基于超声波拟合线段和摄像头的输入，得到车位附近障碍物信息(比如柱子、车、锥桶等)和划线车位。计算车位的4个角点坐标(P₀,P₁,P₂,P₃)、车位可利用宽度W、目标停车位置(TargetP)，确定待进入车位信息，同时以最靠近车辆前轮的P1点为坐标原点，对障碍物信息和划线车位信息进行坐标转换；

步骤3，库外姿态调整主要目的使车辆行驶至PosOK点。路径规划的激活需要考虑车位宽度、车位周边环境如通道宽度等信息。根据本车位置确定包括车辆X,Y坐标位置及角度的本车姿态(CurrentX,CurrentY,Currentθ)，基于对待进入车位的探测信息和本车姿态，确定倒车启动位置PosOK点。

如图2所示为确定PosOK点示意图，可以看出，PosOK点目的为在尽量短的路径下，对车位进行更充分探测。在达到PosOK点之后第一次倒退过程中，保证安全余量的同时，车辆倒车行使路径绕过P1点，通过车辆后雷达对远离车辆前轮的车位前角点P0点进行充分探测，并保证车辆入库尽可能深入。

当目标车位位于车辆后侧时，根据车位右前角点P₁点坐标(P_1x，P_1y)，车辆最小转弯半径R_min、车辆激活泊车时纵坐标CurrentY、车辆宽度CarWid th、角点P₁点坐标(P_1x，P_1y)、安全余量SafeMargin，调用公式：

确定PosOK点的横纵坐标PosOK_x，及PosOK点与P₁点x方向之间的角度θ。

转弯半径R和安全余量SafeMargin需要根据外部障碍物信息进行调整。

步骤4，根据不同障碍物类型，采用不同的安全距离，得出安全绕障的路径，绕行经过P1点之后，倒车入库。

如图3所示为泊车过程中垂直泊车路径示意图。其中状态①为库外调整，主要目的为车库外调整车辆和目标车位的相对姿态，并使车辆达到PosOK点，在此步会进行实时规划，根据通车道宽度的更新，实时更新PosOK点；

状态②为绕P1入库，主要目的为在保证不碰P1点的情况下，入库更深入，并进行更充分的探测；入库之后，可正常进行上提与入库动作，达到最终目标点位置(TargetP)。

安全余量SafeMargin与通道宽度W、P₁点障碍物类型(柱子、车、锥桶等)、车位宽度有关。在已知通道宽度较小时，安全余量SafeMargin较小；车位宽度较宽时，安全余量SafeMargin较大。

根据gridmap(栅格地图)和车位顶点位置确定上述通道宽度，也可以采用gridmap根据超声波探测信息得出，由于超声波探测特性，无法对较远距离障碍物进行有效检测，故需进行路径规划采取相应措施增加泊车安全性和成功率。

假设车位在车辆右侧(车位位于车辆左侧的采用类似方法)，根据车辆宽度CarWidth、车辆轴距L、前轴到前保距离L_F、通车道宽度W、本车当前纵坐标距离CurrentY。

建立车辆绕行方程：

R_a＜W-CurrentY+R ②

计算基于车辆前顶点倒车入库的绕行转弯半径Ra；根据障碍物信息，从最小转弯半径R_min向上进行遍历，当满足公式R_a＜W-CurrentY+R时的转弯半径R为车辆倒车入库时绕行P₁点的转弯半径。

车辆绕P₁点转弯半径的确定步骤为：对R_a从最小转弯半径向上进行遍历，当满足公式②时，此时得出此场景下的转弯半径。

第一次D档前行时由于前方空间未知，在1D前行之时激活通车道宽度，只有通车道宽度变小时，才会进行更新，通车道宽度根据车位和侧雷达距离USS进行实时更新，1D前行路径也会随着车辆绕P₁转弯半径变化而变化。根据公式：

确定车辆更新宽度，对通车道宽度进行更新，其中CurrentY为本车当前纵坐标距离。

如图4所示，在PosOK点更新过程中探测不准时(车速较高或者天气条件恶劣时易出此现象)绕P1点路径示意图。

其中，图①为PosOK点更新过程示意图，开启泊车时，障碍物obstacle未知，1D前行时才能对障碍物进行探测，并更新车辆绕P1转弯半径，同时对PosOK点进行更新，达到避开障碍物的目的。图②为探测不准时绕P1点路径示意图，即使做到图①所示方法，由于障碍物探测不准依然可能出现图②所示泊车卡死情况，此时将方向盘回正，避开障碍物。详细的，对于右侧车位，当出现左上角雷达处卡死，需要路径进行更新。选择方向盘回正的路径(如直线)进行绕P1点，消除碰撞风险。

本方法利用现有平台和传感器信息，对路径进行实时更新。相对于传统几何法无法针对相应的输入做出最好的决策规划。本发明结合现有智能驾驶车辆探测性能，对路径规划进一步约束，求解出更安全、效率更高的路径。

因此，本实施例中所述的针对复杂场景窄通道的垂直车位自主泊车路径规划方法解决了传统路径不够智能、探测不准容易卡死的问题；更对现有的传感器更加友好，能够很好的在障碍物环境未知的情况下，完成垂直车位自主泊车。

Claims (10)

1.一种基于路径规划的垂直车位自主泊车方法，其特征在于，所述方法包括以下步骤：

步骤1，建立车辆动力学模型，根据车辆轴距、前轮转角的平均值计算车辆最小转弯半径和最短行驶轨迹；

步骤2，基于超声波和摄像头获取目标垂直车位的4个角点位置坐标(P₀,P₁,P₂,P₃)、车位可利用宽度，确定待进入车位信息，同时以最靠近车辆前轮的P1点为坐标原点，对障碍物信息和车位信息进行坐标转换；

步骤3，基于距离车辆前轮最近的角点位置P1点和车辆最小转弯半径计算泊车起点PosOK点，建立路径规划方程,调整车辆姿态至PosOK点；

步骤4，车辆从PosOK点启动，以最小转弯半径，按照最短行驶轨迹倒车绕行经过P1点进入车位，到达目标点位置。

2.根据权利要求1所述的方法，其特征在于，所述计算车辆最小转弯半径包括：调用公式：R_min＝L/tan(θ_max)计算最小转弯半径R_min，其中，L表示车辆轴距，θ_max表示前轮最大转角。

3.根据权利要求1所述的方法，其特征在于，所述计算PosOK点包括：根据车辆最小转弯半径R_min、车辆当前纵坐标CurrentY、车辆宽度CarWidth、P₁点坐标(P_1x，P_1y)、安全余量SafeMargin，调用公式：

计算PosOK点。

4.根据权利要求1所述的方法，其特征在于，根据车辆宽度CarWidth、车辆轴距L、前轴到前保距离L_F、通车道宽度W、车辆离P1点的横向距离CurrentY，建立路径规划方程：

计算车辆绕行前顶点倒车入库的转弯半径Ra；从最小转弯半径R_min向上进行遍历，当满足公式R_a＜W-CurrentY+R时的转弯半径R为车辆倒车入库的绕行转弯半径。

5.根据权利要求4所述的方法，其特征在于，车辆绕行转弯倒车入库时，调用公式：

对通车道宽度进行更新,根据通车道宽度的更新，实时更新PosOK点。

6.一种基于路径规划的垂直车位自主泊车系统，包括，超声波雷达、摄像头、驱动控制单元、车辆动力学模型、计算单元，超声波雷达和摄像头获取目标车位的4个角点位置坐标、车位可利用宽度、目标停车位置；计算单元调用车辆动力学模型根据车辆轴距、前轮转角的平均值计算车辆最小转弯半径和最短行驶轨迹，基于距离车辆前轮最近的角点位置P1点和车辆最小转弯半径确定泊车起点PosOK点，建立路径规划方程规划路径；驱动控制单元从PosOK点启动车辆，按规划路径以车辆转弯半径倒车绕行车位前方距离车辆前轮最近角点P₁行驶最短行驶轨迹进入车位，到达目标点位置。

7.根据权利要求6所述的系统，其特征在于，计算单元调用公式：

R_min＝L/tan(θ_max)计算最小转弯半径R_min，根据车辆最小转弯半径R_min、本车激活泊车时纵坐标CurrentY、车辆宽度CarWidth、角点P₁点坐标(P_1x，P_1y)、安全余量SafeMargin，调用公式：

确定PosOK点横纵坐标PosOK_x及PosOK点与P₁点x方向之间的角度θ。

8.根据权利要求6或7所述的系统，其特征在于，规划路径具体为：根据车辆宽度CarWidth、车辆轴距L、前轴到前保距离L_F、通车道宽度W、车辆离P₁点的横向距离CurrentY，建立路径规划方程：

计算基于车辆左前顶点倒车入库的绕行转弯半径Ra；从最小转弯半径R_min向上进行遍历，当满足公式R_a＜W-CurrentY+R时的转弯半径R为车辆倒车入库的绕行转弯半径；车辆倒车绕行转弯进入车位时，调用公式：

对通车道宽度进行更新。

9.一种计算机可读存储介质，其内存储有计算机可读程序，其特征在于，所述计算机可读程序被控制器调用时，能执行如权利要求1-5任一项所述的自主泊车方法。

10.一种计算机设备，其特征在于，包括：处理器和存储器；其中，所述处理器通过读取所述存储器中存储的可执行程序代码来运行与所述可执行程序代码对应的程序，以用于实现如权利要求1-5任一项所述的自主泊车方法。

Images