CN113978452A - 一种自动平行泊车路径规划方法 - Google Patents

Abstract

本发明公开了一种自动平行泊车路径规划方法。所采用的方法是以车辆进入车位的航向角为基础，并基于车辆的运动学模型、B样条曲线、直线、回旋线、圆弧进行组合的路径规划方法，其具体的步骤如下：1)传感器检测车位的大小并建立车位坐标系；2)在建立坐标系的基础上判断是否满足最小泊车位的要求；3)根据车辆所在位置先利用圆弧‑直线‑圆弧方法求解出满足车辆碰撞约束的航向角；4)保证曲率的连续性并在求出航向角的基础上规划一条B样条曲线‑直线‑回旋线‑圆弧的自动平行泊车路径。本发明的方法结合各种曲线的优点很好的解决了规划路径长和曲率不连续的问题，并降低了规划路径算法复杂度，能够快速实现平行泊车路径规划。

Description

一种自动平行泊车路径规划方法

技术领域

本发明涉及自动平行泊车技术领域，尤其是涉及一种以车辆进入车位航向角为基础，并基于多曲线组合的自动平行泊车路径规划方法。

背景技术

随着汽车数量的不断增加，导致现在停车场的车位变得非常紧张，从而导致泊车区域附近的环境变得越来越复杂，增大了驾驶员成功进入车位的难度，这样也极易导致交通事故。

另一方面，无人驾驶在以后的汽车发展过程中是一个重要的方向。其中自动泊车是无人驾驶车辆的重要组成部分，当需要泊车时，驾驶员停到车位附近，车辆自动地寻找泊车位置，在正确寻找到满足泊车要求的车位之后进行路径规划，并根据规划的路径控制车辆的转向和速度，从而实现轨迹跟踪，最终到达车位内部。这样不仅能够节约大量时间，还能够有效地减少由于泊车所造成的交通事故，改善整个泊车秩序，提高泊车的成功率。

虽然目前在一些高档车型上已经搭载了特定环境下的自动平行泊车技术，但是该技术并不是很成熟，存在成本高，算法复杂，所规划的车辆路径曲率不能连续，也不能够很好地嵌入到车载ECU中，所需车位较长的问题。

发明内容

本发明的目的是为了解决现有的自动平行泊车所规划的路径曲率不连续，导致车辆不能很好的沿着所规划的路径进行跟踪行驶和现有的单一曲线规划方法中计算量大，运行效率不高的问题而提供了一种自动平行泊车路径规划方法。

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

一种自动平行泊车路径规划方法，该方法以车辆进入车位的航向角为基础，并基于车辆的运动学模型、B样条曲线、直线、回旋线、圆弧进行组合的路径规划方法，具体包括以下步骤：

1)传感器检测车位的大小并建立车位坐标系；

2)在车位坐标系基础上判断是否满足最小泊车位的要求；

3)根据车辆所在的位置先利用圆弧-直线-圆弧方法求解出满足车辆碰撞约束的航向角；

4)保证曲率的连续性并在求出航向角的基础上规划一条B样条曲线-直线-回旋线-圆弧的自动平行泊车路径。

进一步的讲传感器检测车位大小采用的是车辆的每一侧各安装四个超声波雷达传感器进行检测，在每一侧的轮胎上方的车身上各安装两个超声波雷达传感器。车位坐标系中车辆运行的方向为X轴，从车位指向车辆并垂直于X轴的为Y轴，坐标原点在车位前方外侧边缘点。

进一步的讲在车位坐标系基础上判断是否满足最小车位要求中满足泊车位要求的条件为：

为所测车位的长度，L为所需要的最小车位长度，R_h为车辆尾部中心所对应的最小转弯半径，W为车辆的宽度，l为车辆的轴距，l_f为车辆的前悬，l_r为车辆的后悬。

进一步的讲根据车辆所在位置利用圆弧-直线-圆弧方法求解出满足车辆碰撞约束的航向角中满足车辆碰撞约束条件分别为：1)在最后一段圆弧中车辆的左后端点的横坐标不能够与车位的后边缘发生碰撞；2)在最后一段圆弧中车辆的左后端点纵坐标能够成功的进入到车位内部；3)在最后一段圆弧中车辆的右后端点不能够与车位的下边缘发生碰撞；4)在最后一段圆弧中使车辆尽可能的停在车位的中间位置；5)车辆也要满足中间直线阶段防止与车位坐标系中的原点发生碰撞；6)在第一阶段车辆横坐标满足从X_D沿着圆弧到X_A进行转变；7)在第一阶段车辆纵坐标满足从Y_D沿着圆弧到Y_A进行转变。

(X_A,Y_A)：开始的第一段圆弧与中间直线阶段交点坐标；

(X_B,Y_B)：最后一段圆弧车辆左后方端点开始位置；

(X_D,Y_D)：车辆开始自动平行泊车左后方端点起始位置；

S：车辆直线阶段不与坐标原点发生碰撞的阈值

R_1min：车辆最后一段圆弧左后方端点的最小转弯半径；

R_min：车辆第一段圆弧左后方端点的最小转弯半径；

θ：车辆进入车位的航向角；

进一步的讲在航向角的基础上规划一条B样条曲线-直线-回旋线-圆弧的自动平行泊车路径，具体指在第一段圆弧采用B样条曲线的形式所代替，对最后一段圆弧与直线部分加入回旋线进行连接。

进一步的讲第一段圆弧采用B样条曲线是依照所行使的路径最短来选取B样条曲线的6个控制点依次为P₀～P₅，形成三次B样条曲线。其中控制点的选择以所规划的路径最短为目标，并满足的约束条件分别为：1)控制点P₀需要选择在车辆起始泊车位置上；2)控制点P₅需要选择在车辆第一段圆弧路径规划的终止点位置上；3)控制点P₁需要与P₀连线的斜率为0；4)控制点P₄与P₅的连线的斜率应该为tan θ；5)车辆在泊车的过程中不与车位的右上角发生碰撞(车位坐标系中原点的位置)；6)规划路径上每一个点的曲率要小于车辆最小转弯半径的要求。

x_P0～x_P5:控制点P₀～P₅的横坐标点；

y_P0～y_P5:控制点P₀～P₅的纵坐标点；

F：车位的右下端点坐标；

O：车位坐标系原点；

θ：车辆进入车位的航向角；

k_xi：B样条曲线上点的曲率；

K：车辆最小转弯半径下的曲率；

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

1、本发明在车位检测模块在车辆的两侧分别安装四个超声波雷达进行检测，在增加检测精度的同时能够有效地降低系统的成本，增加系统检测的速度。

2、本发明的自动平行泊车路径规划方法以车辆进入车位的航向角为基础，并采用B样条曲线、直线、回旋线、圆弧多种线型组合的方法进行路径规划，有效的解决了各个线型存在的缺点，同时很好的解决了所规划的路线曲率不连续，计算效率低的问题。

本发明的路径规划方法采用多段线型组合方式很好的解决了规划路径曲率不连续、计算效率低的问题。

附图说明

图1为本发明的路径规划的流程示意图；

图2为实例中计算自动泊车所需要的最小车位示意图；

图3为圆弧-直线-圆弧方法中第二段圆弧约束示意图；

图4为圆弧-直线-圆弧方法中第一段圆弧和直线段约束示意图；

图5为车辆右侧与车位发生碰撞示意图；

图6为车辆右侧与车位不发生碰撞的示意图；

图7为B样条曲线代替圆弧优化示意图；

图8为第二段圆弧与直线阶段加入回旋线示意图；

图9为所完成的一条路径规划示意图；

图10为路径规划仿真示意图；

图11为规划路径曲率变化过程图。

具体实施方式

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

本发明提供一种自动平行泊车的路径规划方法，其规划的流程示意图如图1所示，该方法是基于车辆运动学模型、B样条曲线、直线、回旋线、圆弧多线型组合的方式进行平行泊车的路径规划。具体实施过程中采用小的车模，车模的具体参数如下：

如图1所示，首先传感器检测车位的大小并且建立车位坐标系，然后判断是否满足最小泊车位的要求，随后根据车辆所在的位置利用圆弧-直线-圆弧方法求解满足约束进入车位的航向角，最后在保证曲率连续性并在求出航线角的基础上规划一条B样条曲线-直线-回旋线-圆弧的自动平行泊车路径。

如图2所示最小泊车位计算，平行泊车的最终位置航向角应尽可能地接近0°，为了保证倒车过程的安全，设置在最终平行泊车位置时车辆后端距离车位后边缘的距离为a，车辆左端距离车位的上边缘有一个安全距离为b，根据车辆从车位行驶出来可以知道，距离b越大，车辆右前方轮廓更加容易与车位的前边缘发生碰撞。因此可以得出当距离b＝0时，所需要的车位尺寸是最小的，因此所需要的最小车位为：

如图3所示第二段圆弧约束，车辆的左后端点在B点时(记B点的坐标用(X_B,Y_B)进行表示)，此时车辆的航向角为θ，从该位置车辆的方向盘往左打到极限位置沿着圆弧轨迹进行行驶，当车辆的航向角为0时，车辆的左后端从B点位置在X轴方向和Y轴反方向移动的距离分别用△X、△Y来进行表示，表示如下：

如果车辆能够顺利地进入到车位内部同时车辆后端不与车位的后端发生碰撞，车辆的右端不与停车位的下边缘发生碰撞、同时使车辆最终停车位置尽可能地在车位中间位置，车辆左后端点行驶到B的位置时应该满足以下四个条件：

结合以上两个式子得：

如图4所示第一段圆弧和直线段约束，从沿着圆弧行驶的起始点D点到终止点A点的过程是车辆方向盘达到右极限的位置，车辆沿着最小转弯半径进行行驶，当行驶到航向角θ时，车辆转入直线行驶阶段，因此A点就是从圆弧到直线之间的一个过渡点，已知车辆的起始位置D点的坐标为(x_D,y_D)，因此可以表示出交点A的坐标(x_A,y_A)为

A点位置求解出来之后，车辆沿着第二段直线行驶的轨迹方程则可以表示如下：

y＝kx+b

在上式中k和b分别表示为：

因此，最终的直线轨迹AB阶段的表达式为：

y＝tan θx+y_A-tan θx_A

图5所示车辆右侧与车位发生碰撞，车辆在直线阶段进行运动时，还要保证车辆的右后端点F以及右后端点到右前端点之间的连线FG不能够和车位的右上角进行碰撞。

图6所示车辆右侧与车位不发生碰撞约束，只要车辆的右后端点在A点时不与车位的右上端点发生碰撞就可以，这是因为在以后阶段车辆沿着直线行驶，与规划的路线是平行。因此只要在规划直线AB段往下平移车辆宽度w得到直线方程A′B′的截距大于阈值S就能够满足。

其满足约束的表达式可以表示为：

y_A-tan θx_A-w≥S

综合整理以上的所有约束，表示为如下：

(X_A,Y_A)：开始的第一段圆弧与中间直线阶段交点坐标；(X_B,Y_B)：最后一段圆弧车辆左后方端点开始位置；(X_D,Y_D)：车辆开始自动平行泊车左后方端点起始位置；S：车辆直线阶段不与坐标原点发生碰撞的阈值R_1min：车辆最后一段圆弧左后方端点的最小转弯半径；R_min：车辆第一段圆弧左后方端点的最小转弯半径；

在上式约束的情况下进行求解满足约束的未知量，由于未知量较多，接下来对上式中一些条件进行简化，使得在计算量尽可能小情况下，求得车辆进入车位的航向角。进行简化如下：

(1)车辆进入车位的航向角θ在0°～90°之间，但是如果在这么大的范围内进行遍历，造成一些没有必要的计算浪费。根据实际的倒车经验。于是把航向角θ的范围缩小到20°～70°之间进行遍历。

(2)车辆在B点的横纵坐标都未知的情况下对以上方程很难进行求解，于是把B点的横坐标设定为x_B＝R_min sinθ-L，便能够进行判断B点纵坐标y_B是否满足要求。

(3)A点的横纵坐标根据给定车辆初始泊车位置D点的坐标和所求解的航向角θ之后就能够进行求解。

通过采用以上3个条件进行简化之后，求解最优航向角的算法过程如下：

(1)设置航向角θ的变化范围为20°～70°之间进行遍历，每次遍历所选用的步长为1°。

(2)根据已知的停车位置D点、车辆的最小转弯半径R_min、车位长度和每次遍历的航向角θ进行计算A点的横纵坐标(x_A,y_A)和B点的横坐标x_B。

(3)根据计算出来的A点坐标进行计算直线段AB的表达式。

(4)根据B点的横坐标x_B计算出B点的纵坐标y_B判断是否满足约束要求。如果满足要求，执行第五步，不满足要求，在车辆所在的水平位置上进行搜寻下一个车辆起始位置点，再从第二步开始进行执行。

(5)对A′B′段表达式上的每一个坐标点进行判断是否满足要求，如果满足要求执行第六步，不满足要求，在车辆所在的水平位置上进行搜寻下一个车辆起始位置点，再从第二步开始进行执行。

(6)以规划路径最短为目标，从而求解出较优的进入车位的航向角。

图7所示B样条曲线代替圆弧所示，假设从泊车开始位置到目标位置的六个控制点分别用P0、P1、P2、P3、P4、P5来进行表示，首先以车辆的行驶的路径最短为目标，在B样条曲线上每隔一个小的距离取一个点，假设在B样条曲线上一共有n个点，分别用S₀,S₁...S_n-1进行表示，定义所优化的目标函数为：

在进行控制点的选择时，控制点P₀需要选择在车辆起始泊车位置上，控制点P₅需要选择在车辆第一段路径规划的终止点位置上，控制点P₁需要与P₀连线的斜率为0。车辆在第一段路径行驶完成后的航向角应该为θ，因此控制点P4与P₅的连线的斜率应该为tan θ。还需要车辆在泊车的过程中不与车库的右上角发生碰撞(车位坐标系中原点的位置)，车辆右后方到原点的距离应该大于0。该轨迹上每一个点的曲率要小于车辆的最小转弯半径的要求，因此这些基本约束的表达式表示为：

x_P0～x_P5:控制点P₀～P₅的横坐标点；y_P0～y_P5:控制点P₀～P₅的纵坐标点；F：车位的右下端点坐标；O：车位坐标系原点；θ：车辆进入车位的航向角；k_xi：B样条曲线上点的曲率；K：车辆最小转弯半径下的曲率；

图8所示第二段圆弧与直线阶段加入回旋线，在直线和圆弧之间接入回旋曲线进行过渡，最终使整个所规划的路径曲率连续。

图9所示最终规划的B样条曲线-直线-回旋线-圆弧自动平行泊车路径。

图10所示采用车模尺寸起始位置坐标为(10，20)的自动平行泊车仿真过程。

图11所示规划的路径曲率从开始泊车到泊车完成的变化过程。

Claims (6)

1.一种自动平行泊车路径规划方法，其特征在于：所采用的方法是以车辆进入车位的航向角为基础，并基于车辆的运动学模型、B样条曲线、直线、回旋线、圆弧进行组合的路径规划方法，具体包括以下步骤：

1)传感器检测车位的大小并建立车位坐标系；

2)在建立坐标系的基础上判断是否满足最小泊车位的要求；

3)根据车辆所在的位置先利用圆弧-直线-圆弧方法求解出满足车辆碰撞约束的航向角；

4)保证曲率的连续性并在求出航向角的基础上规划一条B样条曲线-直线-回旋线-圆弧的自动平行泊车路径。

2.如权利要求1所述一种自动平行泊车路径规划方法，其特征在于：所述步骤1)中传感器检测车位大小采用的是车辆的每一侧各安装四个超声波雷达传感器进行检测，在每一侧的轮胎上方的车身上各安装两个超声波雷达传感器；车位坐标系中车辆运行的方向为X轴，从车位指向车辆并垂直于X轴的为Y轴，坐标原点在车位前方外侧边缘点。

3.如权利要求1所述一种自动平行泊车路径规划方法，其特征在于：所述步骤2)中在建立坐标系的基础上判断是否满足最小泊车位要求中满足泊车位要求的条件为：

X为所测车位的长度，L为所需要的最小车位长度，R_h为车辆尾部中心所对应的最小转弯半径，W为车辆的宽度，l为车辆的轴距，l_f为车辆的前悬，l_r为车辆的后悬。

4.如权利要求1所述一种自动平行泊车路径规划方法，其特征在于：所述步骤3)中根据车辆所在位置利用圆弧-直线-圆弧方法求解出满足车辆碰撞约束的航向角中满足车辆碰撞约束条件分别为：1)在最后一段圆弧中车辆的左后端点的横坐标不能够与车位的后边缘发生碰撞；2)在最后一段圆弧中车辆的左后端点纵坐标能够成功的进入到车库内部；3)在最后一段圆弧中车辆的右后端点不能够与车位的下边缘发生碰撞；4)在最后一段圆弧中使车辆尽可能的停在车位的中间位置；5)车辆也要满足中间直线阶段防止与车位坐标系中的原点发生碰撞；6)在第一阶段车辆横坐标满足从X_D沿着圆弧到X_A进行转变；7)在第一阶段车辆纵坐标满足从Y_D沿着圆弧到Y_A进行转变；

(X_A,Y_A)：开始的第一段圆弧与中间直线阶段交点坐标；

(X_B,Y_B)：最后一段圆弧车辆左后方端点开始位置；

(X_D,Y_D)：车辆开始自动平行泊车左后方端点起始位置；

S：车辆直线阶段不与坐标原点发生碰撞的阈值

R_1min：车辆最后一段圆弧左后方端点的最小转弯半径；

R_min：车辆第一段圆弧左后方端点的最小转弯半径；

θ：车辆进入车位的航向角。

5.如权利要求1所述一种自动平行泊车路径规划方法，其特征在于：所述步骤4)中航向角的基础上规划一条B样条曲线-直线-回旋线-圆弧的自动平行泊车路径，代表在权利要求1步骤3中求出航向角的基础上对第一段圆弧采用B样条曲线的形式所代替，对最后一段圆弧与直线部分加入回旋线进行连接。

6.如权利要求1所述一种自动平行泊车路径规划方法，其特征在于：所述步骤4)中航向角的基础上规划一条B样条曲线-直线-回旋线-圆弧的自动平行泊车路径，针对第一段B样条曲线的规划为：第一段圆弧采用B样条曲线来代替是依照所行使的路径最短来选取B样条曲线的6个控制点依次为P₀～P₅，形成三次B样条曲线。其中控制点的选择以所规划的路径最短为目标，并满足的约束条件分别为：1)控制点P₀需要选择在车辆起始泊车位置上；2)控制点P₅需要选择在车辆第一段路径规划的终止点位置上；3)控制点P₁需要与P₀连线的斜率为0；4)控制点P₄与P₅的连线的斜率应该为tanθ；5)车辆在泊车的过程中不与车库的右上角发生碰撞(车位坐标系中原点的位置)；6)规划路径上每一个点的曲率要小于车辆最小转弯半径的要求；

x_P0～x_P5:控制点P₀～P₅的横坐标点；

y_P0～y_P5:控制点P₀～P₅的纵坐标点；

F：车位的右下端点坐标；

O：车位坐标系原点；

θ：车辆进入车位的航向角；

k_xi：B样条曲线上点的曲率；

K：车辆最小转弯半径下的曲率。

Images