CN110696819A - 一种自动泊车路径规划方法及*** - Google Patents

Abstract

本发明涉及一种自动泊车路径规划方法及***，所述方法基于几何信息的圆弧和直线的组合进行路径规划，包括以下步骤：1)获取库位信息；2)识别库位类型，若为垂直泊车，则执行步骤3)，若为平行泊车，则执行步骤4)；3)判断是否满足一次入库条件，若是，则进行一次入库规划，若否，则基于垂直泊车约束方程启动垂直泊车再规划；4)基于平行泊车约束方程实现平行泊车规划。与现有技术相比，本发明能够在没有时间信息的路径情况下，通过路径推算赋予***运行的时间信息，实现横纵向解耦和空间节省的垂直自动泊车规划和水平自动泊车规划。

Description

一种自动泊车路径规划方法及***

技术领域

本发明涉及自动泊车技术领域，尤其是涉及一种自动泊车路径规划方法及***。

背景技术

自动泊车在无人驾驶的环节有着很重要的作用。许多智能停车场的应用诸如自动充电、动态调度等等都依赖其实现。在此环节中，路径规划和一般道路上的路径规划有很大的不同。一般情况下路径规划的目标是希望车辆是在一个相对较大的空间中平顺且高速的行驶，多个相邻周期内的运动往往将曲率变化、横向位移变化等信息作为平顺性的优化指标，车辆没有一个精确的目标，只需在趋势上不断向着每个规划周期的虚拟目标即可。但是，自动泊车却大不相同，我们往往希望车辆在较小的空间中完成多次前进和后退，对空间的利用会使得车辆的转向经常处于满舵的极限状态。

现有的自动泊车***还存在以下不足：

1、规划路径与时间强相关，车辆行驶过程中产生的误差无法及时消除；

2、传统路径规划算法，在解耦横向纵向控制问题的同时，不能很好的满足空间经济型。因此，需要研发一种新型的自动泊车***。

发明内容

本发明的目的在于克服上述现有技术存在的缺陷而提供一种自动泊车路径规划方法及***。

本发明的目的可以通过以下技术方案来实现：

一种自动泊车路径规划方法，该方法基于几何信息的圆弧和直线的组合进行路径规划，包括以下步骤：

1)获取库位信息；

2)识别库位类型，若为垂直泊车，则执行步骤3)，若为平行泊车，则执行步骤4)；

3)判断是否满足一次入库条件，若是，则进行一次入库规划，若否，则基于垂直泊车约束方程启动垂直泊车再规划；

4)基于平行泊车约束方程实现平行泊车规划。

进一步地，通过环视相机和超声波雷达获得所述库位信息。

进一步地，所述一次入库条件为：

其中，第一个约束表示车辆中线与库位中线l相切，第二个约束表示车辆不能与库位入口点P₀(x₀,y₀)相撞，第三个约束表示车辆与库位平行且完成泊车后，车尾不能超过库位边界，第四个约束表示车辆左后角在行驶过程中不能与库位相撞，第五个约束表示车辆最小转弯半径约束；

P₀、P₁、P₂、P₃为库位的四个角点，P₀和P₃为库位入口，R为车辆瞬时转弯半径，R_min表示最小转弯半径，w_h表示半车宽，L_r表示车辆的后周轴心到车尾的长度，R_rr表示相对车辆即时旋转中心P车辆左后角的转弯半径。

进一步地，所述垂直泊车约束方程为：

其中，x_g,y_g为最终泊车完成终点的坐标，x_t,y_t为最终泊车入库路径与库位中线l相切的切点，l_final为可调节的阈值，R_min表示最小转弯半径，R_f表示前进圆半径，R_b表示后退圆半径，x_f,y_f为前进圆的圆心坐标，θ为库位侧面与X轴正方向的夹角。

进一步地，所述平行泊车约束方程为：

其中，x_g,y_g为最终泊车完成终点的坐标，R_min表示最小转弯半径，L_f表示车辆的后周轴心到车头的长度，δ表示车辆与库位侧边的最小距离，x₀,y₀为库位入口点P₀的坐标，x_offset、w_h表示半车宽，x_s1,y_s1、x_s2,y_s2、x_s3,y_s3分别为平行泊车时三条圆弧的圆心，θ为库位侧面与X轴正方向的夹角。

本发明还提供一种自动泊车路径规划***，该***基于几何信息的圆弧和直线的组合进行路径规划，包括：

库位信息获取模块，用于获取库位信息；

库类型识别模块，用于识别库位类型为垂直泊车或平行泊车；

垂直泊车模块，在所述库类型识别模块的输出为垂直泊车时响应，用于判断是否满足一次入库条件，若是，则进行一次入库规划，若否，则基于垂直泊车约束方程启动垂直泊车再规划；

平行泊车模块，在所述库类型识别模块的输出为平行泊车时响应，用于基于平行泊车约束方程实现平行泊车规划。

进一步地，通过环视相机和超声波雷达获得所述库位信息。

进一步地，所述一次入库条件为：

其中，第一个约束表示车辆中线与库位中线l相切，第二个约束表示车辆不能与库位入口点P₀(x₀,y₀)相撞，第三个约束表示车辆与库位平行且完成泊车后，车尾不能超过库位边界，第四个约束表示车辆左后角在行驶过程中不能与库位相撞，第五个约束表示车辆最小转弯半径约束；

P₀、P₁、P₂、P₃为库位的四个角点，P₀和P₃为库位入口，R为车辆瞬时转弯半径，R_min表示最小转弯半径，w_h表示半车宽，L_r表示车辆的后周轴心到车尾的长度，R_rr表示相对车辆即时旋转中心P车辆左后角的转弯半径。

进一步地，所述垂直泊车约束方程为：

其中，x_g,y_g为最终泊车完成终点的坐标，x_t,y_t为最终泊车入库路径与库位中线l相切的切点，l_final为可调节的阈值，R_min表示最小转弯半径，R_f表示前进圆半径，R_b表示后退圆半径，x_f,y_f为前进圆的圆心坐标，θ为库位侧面与X轴正方向的夹角。

进一步地，所述平行泊车约束方程为：

其中，x_g,y_g为最终泊车完成终点的坐标，R_min表示最小转弯半径，L_f表示车辆的后周轴心到车头的长度，δ表示车辆与库位侧边的最小距离，x₀,y₀为库位入口点P₀的坐标，x_offset、w_h表示半车宽，x_s1,y_s1、x_s2,y_s2、x_s3,y_s3分别为平行泊车时三条圆弧的圆心，θ为库位侧面与X轴正方向的夹角。

与现有技术相比，本发明具有如下有益效果：

1、本发明在传感器方面仅使用360环视和超声波雷达，形成一套可以单独成立的自动泊车***。

2、传统路径规划应该为位置与时间的函数x(t),y(t)，如此无法将速度解耦增加复杂度。本发明就是要让车辆在没有时间信息的路径情况下，通过路径推算赋予***运行的时间信息，路径推算通过定位***将过去时刻路径转换到当前时刻下的车身坐标。

3、本发明可实现横纵向解耦和空间节省的垂直自动泊车规划和水平自动泊车规划。

附图说明

图1为本发明的流程示意图；

图2为实施例中车辆初始位置示意图；

图3为实施例中垂直泊车第一次泊车倒车规划示意图；

图4为实施例中垂直泊车再规划示意图；

图5为实施例中平行泊车规划示意图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

本发明提供一种自动泊车路径规划方法，运行流程图如图1所示，该方法基于几何信息的圆弧和直线的组合进行路径规划。车辆的设定时速为2-5km/h，因此可以忽略车辆的动力学模型，只使用运动学模型。利用几何特征的车辆泊车路径规划，同时包括了垂直泊车与侧方位泊车两种情况。

如图1所示，环境感知模块为环视和超声波雷达识别到的库位信息。与正常行驶中的路径规划不同，泊车规划仅仅是由任务驱动的，只有当车辆触发重规划的条件时车辆才会进行一次规划，重规划的条件一般都是在前进后退切换的时候发生的，或者是在泊车接近完成的时候做最后的姿态调整。这么考虑的原因是车辆在泊车过程中大都处于比较极限的状态，如果是因为车辆控制造成的误差几乎不可能通过调整规划来及时消除。此处为了方便***独立，视觉SLAM或者差分GPS的定位作为可选项以虚线连入融合模块。

1、实时路径推算

在车辆正常行驶过程中，规划由时间和事件两种模式驱动。时间频率依据速度和路况一般设定在1s以上，以保证车辆的平顺性，而事件主要包括由状态机驱动的变道、超车、红绿灯停车等等可能改变未来路径情况的任务。对于车辆控制模块而言，需要实时高频的接收每时每刻相对车辆坐标系的路径坐标序列，通常为30ms。因此需要将低频的路径转化为实时的路径。路径推算通过定位***将过去时刻路径转换到当前时刻下的车身坐标。路径推算赋予路径时间信息方法具体如公式(1)所示。

其中，R是2×2维的旋转矩阵

T是2×1维的平移矩阵[ΔX,ΔY]^T，p(x_n,y_n)表示数量为n个点的路径。一般这个模块会记录5秒内的所有更新记录，在实际应用中，只需输入周期号即可得到过去某个时刻的路径情况。

2、参数说明

如图2所示，车辆被近似为一个矩形。w_h表示半车宽，L_f,L_r分别表示车辆的后周轴心到车头和车尾的长度，图中的四条灰色圆弧为车辆在固定方向盘转角的情况下四个虚拟顶点的行驶轨迹，R_fl,R_fr,R_rl,R_rr分别表示相对车辆即时旋转中心P四个虚拟定点的转弯半径，用以判断是否可以一次入库，R_min表示最小转弯半径。根据车辆固有机械特性，R_fl,R_fr,R_rl,R_rr的计算可由公式(2)给出。

其中，R为车辆瞬时转弯半径，可以由当前方向盘转角进行计算。显而易见，车辆行驶过程中扫过的最大半径为R_fl，而最小半径在右后轮(内轮)处，为R-w_h。为了避免分母为零的情况，这里定义曲率为：

无论是垂直泊车还是平行泊车，车辆和库位的初始相对位置关系都可以由图2表示。其中，(x_i,y_i),i＝0,1,2,3表示识别到的库位的4个角点，且满足0和3为库位入口，0，1，2，3顺时针方向排列。θ为1，0点连线与X轴正方向的夹角。分别表示库位开口和另一边的长度，通过常识设定阈值即可区分垂直和平行库位(已在库位识别模块中给出)，(x_m,y_m)为开口中心点的坐标。

3、垂直泊车一次入库判断

为了节省车辆横向行驶空间，默认车辆在第一次倒车前先以直线前进。如需保证成功入库且不与库位碰撞，则需满足公式(3)，示意图如图3所示。

其中，第一个约束表示车辆中线与库位中线l相切，第二个约束表示车辆不能与库位入口点P₀相撞，第三个约束表示车辆与库位平行且完成泊车后，车尾不能超过库位边界，第四个约束表示车辆左后角在行驶过程中不能与库位相撞，第五个约束表示车辆最小转弯半径约束。

4、垂直泊车再规划

如果无法完成一次泊车，则先进行一次倒车，当车辆与库位相撞，或者左后方超声波检测出障碍物2秒以上，则需进行前进调整。本发明将一次前进调整和后退作为一次完整的计算过程，只要车辆实时判断出需要前进的事件，则启动重规划，如此反复执行直至最终完成泊车。

如图4所示，前进和后退的路径由两个相切的圆构成，紫色的圆为前进圆，其圆心和半径分别为P_f(x_f,y_f)和R_f，蓝色圆为后退圆，其圆心和半径分别为P_b(x_b,y_b)和R_b。由相切关系可以获得如下约束：

求解圆心坐标时固定R_f,x_b,y_b,R_b这四个变量，具体由公式(5)给出。

其中，x_g,y_g为最终泊车完成终点的坐标，x_t,y_t为最终泊车入库路径与库位中线l相切的切点，l_final为可调节的阈值，由车辆尺寸和希望泊车完成后车头离库口的距离所决定。需要注意的是该方程有两组解，需要根据距离车库相对位置进行验证。确定了两个圆的位置便能够确定最终的泊车路径。

5、平行泊车规划

平行泊车需最大程度节省库位纵向空间，P_s1,P_s2,P_s3分别表示黄色、紫色、蓝色三条圆弧的圆心，均采用最小车辆转弯半径。平行泊车无需进行一次入库判断，其余同垂直路径规划类似，求解如下约束方程：

其中，δ表示车辆与库位侧边的最小距离，x_offset、w_h表示半车宽，x_s1,y_s1、x_s2,y_s2、x_s3,y_s3分别为平行泊车时三条圆弧的圆心，θ为库位侧面与X轴正方向的夹角。

以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术人员无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由本发明所确定的保护范围内。

Claims (10)

1.一种自动泊车路径规划方法，其特征在于，该方法基于几何信息的圆弧和直线的组合进行路径规划，包括以下步骤：

1)获取库位信息；

2)识别库位类型，若为垂直泊车，则执行步骤3)，若为平行泊车，则执行步骤4)；

3)判断是否满足一次入库条件，若是，则进行一次入库规划，若否，则基于垂直泊车约束方程启动垂直泊车再规划；

4)基于平行泊车约束方程实现平行泊车规划。

2.根据权利要求1所述的自动泊车路径规划方法，其特征在于，通过环视相机和超声波雷达获得所述库位信息。

3.根据权利要求1所述的自动泊车路径规划方法，其特征在于，所述一次入库条件为：

其中，第一个约束表示车辆中线与库位中线l相切，第二个约束表示车辆不能与库位入口点P₀(x₀,y₀)相撞，第三个约束表示车辆与库位平行且完成泊车后，车尾不能超过库位边界，第四个约束表示车辆左后角在行驶过程中不能与库位相撞，第五个约束表示车辆最小转弯半径约束；

P₀、P₁、P₂、P₃为库位的四个角点，P₀和P₃为库位入口，R为车辆瞬时转弯半径，R_min表示最小转弯半径，w_h表示半车宽，L_r表示车辆的后周轴心到车尾的长度，R_rr表示相对车辆即时旋转中心P车辆左后角的转弯半径。

4.根据权利要求1所述的自动泊车路径规划方法，其特征在于，所述垂直泊车约束方程为：

其中，x_g,y_g为最终泊车完成终点的坐标，x_t,y_t为最终泊车入库路径与库位中线l相切的切点，l_final为可调节的阈值，R_min表示最小转弯半径，R_f表示前进圆半径，R_b表示后退圆半径，x_f,y_f为前进圆的圆心坐标，θ为库位侧面与X轴正方向的夹角。

5.根据权利要求1所述的自动泊车路径规划方法，其特征在于，所述平行泊车约束方程为：

其中，x_g,y_g为最终泊车完成终点的坐标，R_min表示最小转弯半径，L_f表示车辆的后周轴心到车头的长度，δ表示车辆与库位侧边的最小距离，x₀,y₀为库位入口点P₀的坐标，x_offset、w_h表示半车宽，x_s1,y_s1、x_s2,y_s2、x_s3,y_s3分别为平行泊车时三条圆弧的圆心，θ为库位侧面与X轴正方向的夹角。

6.一种自动泊车路径规划***，其特征在于，该***基于几何信息的圆弧和直线的组合进行路径规划，包括：

库位信息获取模块，用于获取库位信息；

库类型识别模块，用于识别库位类型为垂直泊车或平行泊车；

垂直泊车模块，在所述库类型识别模块的输出为垂直泊车时响应，用于判断是否满足一次入库条件，若是，则进行一次入库规划，若否，则基于垂直泊车约束方程启动垂直泊车再规划；

平行泊车模块，在所述库类型识别模块的输出为平行泊车时响应，用于基于平行泊车约束方程实现平行泊车规划。

7.根据权利要求6所述的自动泊车路径规划***，其特征在于，通过环视相机和超声波雷达获得所述库位信息。

8.根据权利要求6所述的自动泊车路径规划***，其特征在于，所述一次入库条件为：

其中，第一个约束表示车辆中线与库位中线l相切，第二个约束表示车辆不能与库位入口点P₀(x₀,y₀)相撞，第三个约束表示车辆与库位平行且完成泊车后，车尾不能超过库位边界，第四个约束表示车辆左后角在行驶过程中不能与库位相撞，第五个约束表示车辆最小转弯半径约束；

P₀、P₁、P₂、P₃为库位的四个角点，P₀和P₃为库位入口，R为车辆瞬时转弯半径，R_min表示最小转弯半径，w_h表示半车宽，L_r表示车辆的后周轴心到车尾的长度，R_rr表示相对车辆即时旋转中心P车辆左后角的转弯半径。

9.根据权利要求6所述的自动泊车路径规划***，其特征在于，所述垂直泊车约束方程为：

其中，x_g,y_g为最终泊车完成终点的坐标，x_t,y_t为最终泊车入库路径与库位中线l相切的切点，l_final为可调节的阈值，R_min表示最小转弯半径，R_f表示前进圆半径，R_b表示后退圆半径，x_f,y_f为前进圆的圆心坐标，θ为库位侧面与X轴正方向的夹角。

10.根据权利要求6所述的自动泊车路径规划***，其特征在于，所述平行泊车约束方程为：

其中，x_g,y_g为最终泊车完成终点的坐标，R_min表示最小转弯半径，L_f表示车辆的后周轴心到车头的长度，δ表示车辆与库位侧边的最小距离，x₀,y₀为库位入口点P₀的坐标，x_offset、w_h表示半车宽，x_s1,y_s1、x_s2,y_s2、x_s3,y_s3分别为平行泊车时三条圆弧的圆心，θ为库位侧面与X轴正方向的夹角。

Images