一种基于三阶反正切函数模型的平行泊车方法与流程

本发明涉及仿真技术领域，特别是涉及一种基于三阶反正切函数模型的平行泊车方法。

背景技术：

汽车作为日常生活中最常用的交通工具，私家车几乎也都成了每个家庭的标配，在交通方便的同时也带来了更多问题，特别是停车难的问题日益严重。如何改善汽车的操控性，尤其是泊车过程中的不便利，消除安全隐患，迅速、准确、安全地将汽车停靠到合适的位置，逐渐引起了人们的关注。自动泊车技术的出现大大缓解了驾驶员泊车时的紧张程度，降低泊车难度，提高驾驶的舒适性，提高了泊车过程中的安全性。提高自动泊车方法的可靠性和实用性的关键模块之一就是合理的路径规划。基于路径规划的平行泊车方法根据车辆与停车位的相对的位置算出最佳泊车路径，便于泊车的全过程的规划控制。

Nelson W L提出的Continuous-curvature path for autonomous vehicles中车辆的最短路径规划是连接圆弧(最小转弯半径)的直线段，弧形轮廓和直线之间是不连续的圆弧，所以车辆停在弧的每个不连续的过渡点需重新调整车轮。但瞬间变化机械的转向在物理上是不可能实现的。Xu Jin,Xie Ming等人提出根据车位大小和周围障碍物的信息进行离线查表，获得一条无碰撞且连续的正弦曲线轨迹，该方法对实时性要求较低；而姜辉提出的自动平行泊车转向控制策略的研究提出的路径需要不停地变速及换挡，不符合驾驶员操作的实际情况；而双半径法需要在车速为零时改变转向角，对车辆损伤较大；未加约束空间的以五阶多项式作为平行泊车的规划路径需要较大的泊车空间；加了约束空间的改进的五阶多项式路径规划方法调用遗传算法时需要引入带罚函数的适应度函数，在复杂的泊车环境中并不好控制惩罚量，会出现一定的误差。

技术实现要素：

为解决上述问题，本发明提供了一种基于三阶反正切函数模型的平行泊车方法，包括如下步骤：

步骤一：将目标车辆根据其车辆轮廓的四个顶点将其简化为矩形框，根据泊车车辆提取待泊车车辆的相关参数、车位相关参数。

步骤二：构建最优平行泊车轨迹模型，所述模型包括模型表达式：y＝a₁tan^-1(a₂x³+a₃x²+a₄x+a₅)+a₆

及约束条件：

其中，(x,y)是车辆停车轨迹的坐标，a₁,a₂,a₃,a₄,a₅,a₆为未知的常数，(x₀,y₀)为车辆后轮轴中心坐标；代表车辆的矩形框的四个顶点分别表示为A、B、C、D,A、B在车前轮位置，C、D在车后轮位置，且A、B、C、D的四个位置是从右上方开始顺时针排布，A、B、C、D各自坐标分别为(x_A,y_A)、(x_B,y_B)、(x_C,y_C)、(x_D,y_D)，L为车辆轴距，L_f是车辆前悬长，d₀是行驶道路宽度，d₁、d₂分别为车位的宽度、长度；ρ是车辆泊车轨迹的曲率；R_min是车辆泊车过程中的最小转弯半径。

步骤三：根据模型表达式及约束条件求解出最优的轨迹参数，得到对应的a₁,a₂,a₃,a₄,a₅,a₆。

进一步的，步骤三具体为：

步骤3.1：启用遗传算法工具箱中的ga函数，初始化得到数组N随机产生的初始值A_i＝(a₁,a₂,a₃,a₅),i＝1,2,3,…N。

步骤3.2：将车辆后轮轴中心坐标的起始坐标赋值给(x₀,y₀)，将(x₀,y₀)及A_i＝(a₁,a₂,a₃,a₅),i＝1,2,3,…N代入约束条件中的最后两个等式计算A_i＝(a₄,a₆),i＝1,2,3,…N。

步骤3.3：判断经过步骤3.2得到的参数

A_i＝(a₁,a₂,a₃,a₄,a₅,a₆),i＝1,2,3,…N，能不能满足约束条件，如果不能，则计算适应度函数finessfcn[A_i＝(a₁,a₂,a₃,a₄,a₅,a₆)]＝inf,i＝1,2,3,…N，遗传算法的参数返回一个无穷大的值；如果满足约束条件，则计算当前参数下对应的适应度函数值finessfcn[A_i＝(a₁,a₂,a₃,a₄,a₅,a₆)],i＝1,2,3,…N。

步骤3.4：判断ga函数的终止条件是否得到满足，如果得到满足，则转到步骤3.5，否则根据遗传算法转到下一代的计算并转到步骤3.2。

步骤3.5：遗传算法停止运行，并输出一组最优的轨迹参数A＝(a₁,a₂,a₃,a₄,a₅,a₆)。

本发明的有益效果为：

从本发明的仿真结果中可得车位长度与车长的比值约为1.315，接近于张朝武在平行泊车最小车位探讨中提出的车位长与车长比值1.3，具有非常好的效果，多种车位和车型参数的仿真体现了该发明的鲁棒性很好。

附图说明

图1是反正切函示意图。

图2为泊车路径及相关参数示意图。

图3为泊车过程中障碍示意图。

图4为奇瑞泊A车位仿真图。

图5为奇瑞泊B车位仿真图。

图6为富康泊A车位仿真图。

图7为富康泊B车位仿真图。

图8为奥迪泊A车位仿真图。

图9为奥迪泊B车位仿真图。

具体实施方式

本发明的设计构思为：本发明根据平行泊车的轨迹曲线与反正切函数(如图1所示)的相似性，以及泊车轨迹的非线性特性，提出带有三阶扰动的反正切式的路径规划方法，根据车辆运动学计算出约束空间，由MATLAB中的遗传算法的工具箱调用ga函数，以泊车过程中的障碍约束、车辆自身的参数约束、泊车起始点和终点位置约束为轨迹函数的目标函数，以泊车终点车辆与车位的水平夹角最小为适应度函数，求得最优的轨迹函数的参数,确定最优的平行泊车轨迹。

下面对本发明的具体技术方案进行说明,主要包括如下步骤：

步骤一：将目标车辆根据其车辆轮廓的四个顶点将其简化为矩形框，根据泊车车辆提取待泊车车辆的相关参数、车位相关参数；

步骤二：构建最优平行泊车轨迹模型。

所述模型包括模型表达式：y＝a₁tan^-1(a₂x³+a₃x²+a₄x+a₅)+a₆；

及约束条件：

其中，(x,y)是车辆停车轨迹的坐标，a₁,a₂,a₃,a₄,a₅,a₆为未知的常数，(x₀,y₀)为车辆后轮轴中心E的坐标；代表车辆的矩形框的四个顶点分别表示为A、B、C、D；A、B在车前轮位置，C、D在车后轮位置，且A、B、C、D的四个位置是从右上方开始顺时针排布，A、B、C、D各自坐标分别为(x_A,y_A)、(x_B,y_B)、(x_c,y_C)、(x_D,y_D)，L为车辆轴距，L_f是车辆前悬长，d₀是行驶道路宽度，d₁、d₂分别为车位的宽度、长度；ρ是车辆泊车轨迹的曲率；R_min是最小转弯半径；

下面结合图2对上述模型的原理进行说明。

泊车过程中以E点为基准点，有：

由几何原理则有如下的数学关系：

根据图2所示的车位参数，期望车辆在完成泊车后，θ的绝对值越小越好，由此建立目标函数。若车辆完成泊车后在设定位置，则有

L_r为车辆后悬长；W、L分别为车辆宽度，车辆轴距；R为车辆泊车中的转弯半径；a,b,c,d为车辆轮廓顶点A、B、C、D完成泊车后对应的点；θ为车辆与车位水平方向的水平夹角；为车辆前轮转向角；d₃、d₅分别为车辆泊车后与车位左边缘、下边缘的距离；(x_e,y_e)为车辆完成泊车后，后轮轴中心E的坐标。

根据该轨迹曲线与反正切函数相似，以及泊车轨迹的非线性的特性，单一的反正切模型不能完全描述停车过程的复杂性和不确定性的轨迹。所以在选择的轨迹模型中加入一个非线性的扰动。在二维坐标中，三次多项式和反正切函数的轨迹曲线主要在第一和第三象限或第二和第四象限内，但二次多项式的轨迹曲线是主要在第一和第二象限或第三和第四象限。与二次多项式相比，选择一个三次多项式作为平行泊车轨迹模型的扰动会更贴切，因为三次多项式与反正切函数更为相似。如果选择了四或更高阶程度的扰动，轨迹模型就会更复杂，这将增加计算量和模型参数辨识难度。因此，选择与反正切函数结构类似的数学函数，用三阶扰动作为平行泊车轨迹模型如下：

y＝a₁tan^-1(a₂x³+a₃x²+a₄x+a₅)+a₆

其中(x,y)是车辆停车轨迹的坐标，a₁,a₂,a₃,a₄,a₅,a₆为未知的常数。

模拟泊车环境，车辆不与障碍物或其他车辆相碰撞，分析要求模型中的ABCD四点不落入可能发生碰撞的区域，障碍示意图如图3所示，则满足的约束条件如下：

此外行驶轨迹的曲率满足在整个过程中对最小转弯半径的约束，即|ρ|≤1/R_min。

将E(x₀,y₀)点的坐标代入(8)得到：

a₆＝y₀-a₁tan^-1(a₂x₀³+a₃x₀²+a₄x₀+a₅) (10)

对(10)求导变形得：

设定泊车起始车身平行，式(11)中分子为零，则

a₄＝-(3a₂x₀²+2a₃x₀) (12)

综上所述，在整个泊车过程中需满足的约束条件为：

步骤三：根据模型表达式及约束条件对具体轨迹进行规划，求解出最优的轨迹参数，得到对应的a₁,a₂,a₃,a₄,a₅,a₆。

本步骤具体实现方式如下：

仿真过程中使用MATLAB软件，调用遗传算法工具箱中的ga函数，具体轨迹规划步骤如下：

(1)启用遗传算法工具箱中的ga函数，初始化得到数组N随机产生的初始值A_i＝(a₁,a₂,a₃,a₅),i＝1,2,3,…N；

(2)将已知的起始点和A_i＝(a₁,a₂,a₃,a₅),i＝1,2,3,…N代入约束条件(13)中的最后两个等式计算A_i＝(a₄,a₆),i＝1,2,3,…N；

(3)对于参数A_i＝(a₁,a₂,a₃,a₄,a₅,a₆),i＝1,2,3,…N，如果不能满足约束条件(13)，则计算适应度函数finessfcn[A_i＝(a₁,a₂,a₃,a₄,a₅,a₆)]＝inf,i＝1,2,3,…N，遗传算法的参数返回一个无穷大的值，如果满足约束条件，则计算当前参数下对应的适应度函数值finessfcn[A_i＝(a₁,a₂,a₃,a₄,a₅,a₆)],i＝1,2,3,…N。

(4)如果ga函数的终止条件得到满足，则转到步骤(5)，否则根据遗传算法转到下一代的计算并转到步骤(2)；

(5)遗传算法停止运行，并输出一组最优的轨迹参数A＝(a₁,a₂,a₃,a₄,a₅,a₆)。

下面对本发明的技术效果进行印证说明。

在仿真中，根据中华人民共和国行业标准——车库建筑设计规范JGJ100–2015中以一般的私家车车型选择以下三种车型作为参考车型：奇瑞SQR7080Sll6(简称奇瑞)、神龙富康1.4(简称富康)和奥迪1.8，设定A、B两种车位。各车型参数和车位参数如表1所示。根据秦建军等人的《我国城市道路、桥梁车道宽度标准研究》选择道路宽度d₀为7米。三种车型在两种车位的仿真结果如图5-图9所示，各轨迹模型参数如表2-表4所示表示车辆在初始位置时后轮轴中心对应的坐标值。

表1：各参考车型及各车位参数

d₄表示对应的车位理论上可停的最长车长。

表2：奇瑞平行泊车轨迹模型参数

A车位、B车位泊车的仿真结果分别如图4、5。

表3：富康平行泊车轨迹模型参数

A车位、B车位泊车的仿真结果分别如图6、7。

表4：奥迪平行泊车轨迹模型参数

A车位、B车位泊车的仿真结果分别如图8、9。

从仿真结果中可得车位长度与车长的比值约为1.315，优于现有技术中其它提出方法。

以上的轨迹仿真结果不仅说明了遗传算法的鲁棒性，还说明了所选的带有扰动的反正切的多参数泊车轨迹模型的合理性。