基于粒子群算法的平行泊车轨迹规划

陈乐

(长安大学汽车学院，陕西西安 710064)

0 引言

随着汽车“智能化、电动化、网联化”的提出，智能汽车逐渐走进了人们的视野。随着科技的发展，智能汽车的定义越来越复杂，包含的内容逐渐增多。根据汽车工程协会(society of automotive engineerings，SAE)对自动驾驶的等级划分，目前大多数车辆都处于L2或L3等级，这两种等级都是特指在一定封闭环境下的自动化行为。自动泊车技术(auto-parking system，APS)是智能汽车技术的重要组成部分[1]，也是比较适合目前科技水平的一项智能化技术。自动泊车技术主要通过使用车载传感器和自动转向技术实现车辆自动泊车功能，提高泊车效率的同时也可以减少因泊车导致的交通事故。国内外诸多大型实验室和汽车公司都对泊车策略进行研究。Paromtchik等[2]采用正弦曲线对泊车轨迹进行拟合；Lee等[3]采用模糊控制实现自动泊车功能；宋金泽等[4]采用微分平坦理论对泊车路径进行分析与规划；李红等[5]采用多约束方程对泊车轨迹进行规划，并采用MATLAB非线性求解器进行参数求解。近年来，随着智能算法研究的深入，越来越多的智能算法广泛应用在自动泊车轨迹规划中[6]。

自动泊车技术主要分为车位识别、路径规划、路径控制3个单元[7]。车位识别单元主要通过传感器模块和信息融合算法对泊车环境进行分析，主要实现寻找合适泊车环境和检测障碍物的功能，一般采用超声波传感器，如今越来越多的实验室采用广角摄像头采集泊车信息。路径规划单元是泊车系统的核心单元，通过建立车辆运动学模型，分析车辆转弯时方向盘的转角与车速、转弯半径、车位角的关系，同时对周围潜在碰撞区域进行分析，根据分析结果，拟合出可行且安全的泊车轨迹。路径控制单元是车辆能否按照理想轨迹实施泊车的关键。

目前自动泊车系统的研究方法主要分为2种：一种是基于路径规划进行泊车操作[8-11]；另一种是通过驾驶员的泊车经验对其建立模糊规则，设计模糊控制器实现自动泊车[12-17]。本文以最终车位角和初始车位角之和最小为目标，拟规划出可行且安全的泊车轨迹。

1 泊车路径规划

1.1 汽车运动学模型

由于整个泊车过程车速很低，大约不超过5 m/s，但需要较大的转向角度，所以不考虑汽车的滑移和侧倾，不采用较复杂的动力学模型，根据Ackerman原理,建立简单的运动学模型:

xr2+(yr+Lcot α)2=(Lcot α)2,

(1)

式中：xr、yr分别为汽车后轴中点的横、纵坐标，L为汽车的轴距，α为前轴中点的等效转角。

在泊车过程中，若方向盘转角固定，α基本为定值，由式(1)可知，泊车的路径会逐渐形成一个圆形轨迹，并且该泊车轨迹与泊车的速度无关。但在实际泊车过程中，方向盘的转速和泊车速度往往会对泊车轨迹产生影响[18]。为了简化泊车过程，忽略泊车中的速度变化，假设整个泊车过程以3 m/s的速度匀速进行，并且方向盘匀速转动。为了使方向盘的转动连续，不发生原地转向和转向突变的情况，规划的轨迹曲率必须限制在一定范围内。

本文选用改进的反正切拟合函数作为轨迹函数的基础函数，考虑泊车过程的车辆约束和环境约束，用粒子群算法对未知参数进行优化求解。

1.2 泊车路径规划

泊车路径规划，即以汽车后轴中点作为轨迹规划点，以该点的轨迹作为泊车的轨迹。为计算方便，将车辆简化为矩形ABCD，以停车位的左下角点c作为坐标原点，建立大地坐标系xOy，如图1所示。图1中：Lr为车辆后轴到车辆后端的距离，L为车辆的轴距，Lf为车辆前轴到车辆前端的距离，l为车辆宽度，L1为车位长，L2为车位宽，θ为车辆任意位置的方位角，S0为汽车后轴初始位置在x轴方向与车位边bc的距离，h0为汽车纵轴线初始位置在y轴方向与车位边cd的距离。

图1 平行泊车参数示意图

泊车轨迹的基础函数由修正的反正切函数构成

y=a1arctan(a2x+a3)(a4x3+a5x2+a6x+a7)+a8,

(2)

式中：y为泊车轨迹的纵坐标，x为泊车轨迹的横坐标，a1～a8为轨迹方程中的8个待优化参数。

由式(2)得车辆在任意位置的一阶导数、二阶导数、方位角和曲率分别为:

由于车辆后轴中点的位置与汽车外轮廓点A、B、C、D相对固定，所以可根据几何关系和每个时刻的后轴中点坐标(x，y)推导出各个汽车外轮廓点坐标。A、B、C、D点的坐标分别为：

2 车辆及环境约束

为找到最切合实际的泊车路线，将整个泊车过程模型化，建立多约束非线性模型，根据汽车实际车身限制约束和环境约束规划泊车路线，确定含有未知参数的目标函数作为优化参考，用粒子群算法在可行解中找到近似优化解，得到较好的泊车路径。

2.1 车辆约束

1)轨迹曲率半径

汽车转向时存在最小转向半径Rmin，要求泊车轨迹的曲率

ρ≤1/Rmin。

2)转向角速度

在自动泊车过程中，方向盘的转向角速度通常存在最大值，要求方向盘的转向角速度

ω≤ωmax。

2.2 环境约束

环境约束的精确程度直接影响求解结果，过多的约束条件可能造成求解结果太少，进而不能找到近似优化解，而约束条件太少或者不准确又导致求解结果错误。将整个泊车约束分为起始位置约束、避障约束、最终位置约束3部分。

1)起始位置约束

在起始位置阶段，泊车位置要求汽车后轴中心在图1(S0，h0)处，且方位角θ为0，即车辆纵轴线与图1中x轴方向平行，用公式表示为：

(3)

由于θ=0是一个确切的值，这对于优化的结果要求过于苛刻，为了得到更多可行解，并且要在一定范围内求解最优解，对式(3)进行改进,得：

式中ε1、ε2为约束参数。

2)避障约束

避障约束是在泊车过程中，对车辆躲避周围环境固定障碍进行限制的约束，防止泊车过程中发生事故，导致泊车失败。避障约束相对来说较为复杂。根据车辆自身转向限制，找出极限位置下的特殊状态点作为边界，以此来增加避障约束的条件。

图2表示泊车初始时对a点避障的极限位置。其中：

①以a点为圆心，以安全距离Rs为半径作圆，称此圆为a点的安全圆(图2中小圆)。

YC≥yq-Lrsinθ，x∈[xq，S0]。

当车辆绕开a点，进入车位中，如图3所示，车辆C点可能与车位cd边发生碰撞，有避障约束公式：

YC≥ys，x∈[xs+Lr，xq]，

式中xs、ys为车辆与车位边bc、cd的安全距离。

图2 车辆对a点避障的极限位置 图3 车辆对cd边的避障要求

3)最终位置约束

(4)

同式(3)一样，根据反正切函数的特性，对式(4)进行改进，得：

式中ε3、ε4为约束参数。

综合考虑上述约束，以泊车轨迹方程参数a1，a2，a3，a4，a5，a6，a7，a8为变量，以车辆到达路径终点时车辆纵轴线与cd边夹角最小为目标建立多约束非线性模型：

min|θend+θso|，

(5)

式中：θend为最终车位角，θso为初始车位角。

对式(5)的优化问题，存在2种约束条件，即以等式形式存在和以不等式形式存在的约束条件。只有同时满足等式条件和不等式条件的解，才能作为可行解。

在可行解中，根据目标函数的要求，求解最大或最小值，该最大或最小值的解作为本优化问题的最终解。

以等式形式存在的式(5)的约束条件

(6)

以不等式形式存在的式(5)的约束条件

(7)

3 仿真实例及求解

3.1 仿真参数

车辆参数见表1，根据式(5)～(7)建立泊车方程，求解{a1，a2，a3，a4，a5，a6，a7，a8}的值。根据参考文献[19]，车位标准尺寸为：车位长7 m、宽2.4 m，道路宽4 m。根据不同的车辆起始位置优化不同的泊车路线，车辆后轴中点各起始点坐标如表2所示。

表1车辆参数

L/mLr/mRmin/mLf/ml/m速度/(m·s-1)2.451.065.350.861.65

表2 车辆后轴中点各起始点坐标 m

3.2 仿真求解方法

采用粒子群算法(particle swarm optimization, PSO)进行求解。粒子群算法又称为鸟群觅食算法。PSO从随机解出发，通过约束条件和适应值大小筛选可行解，并且重复迭代寻找最优解。PSO的优点在于精度高，收敛快，规则简单，因此该算法被广泛使用。根据其规则的不同，PSO分为带罚函数的PSO、自适应PSO等。

本文考虑到式(6)(7)中存在较多的约束条件，求解过程比较复杂，可以采用带罚函数的PSO，进而将有约束的优化问题转化为无约束的问题。约束函数的某种组合组成的“惩罚”项，加在原来的目标函数上迫使迭代点逼近可行域。

对于优化问题

(8)

式中：gi(x)0为约束条件，m为约束条件的个数，A为x的约束条件，i为约束条件的个数。

由于不等式约束gi(x)≥0，等价于等式约束min(0,gi(x))=0。因此可将不等式约束问题转化为等式约束问题

(9)

(10)

式中：F(x，M)为罚函数；M为罚因子，为正常数；Mp(x)为罚项。

当M充分大时，F(x，M)的最优解能逼近约束问题的最优解。因此，罚函数F(x，M)对可行点不实行惩罚，而对非可行点给予很大的惩罚，从而将求解约束极值的问题转化为求解无约束极值问题。

3.3 仿真求解步骤

图4 粒子群算法求解过程

PSO的求解过程如图4所示。

1)建立仿真初始化函数。根据式(8)和表1、2及道路参数建立初始化函数，设置相关参数值。

2)建立泊车方程。根据实际泊车数据，对比得到曲率相近的反正切函数，以此作为泊车轨迹方程，对其进行参数化，并由后轴中心点的轨迹推算汽车轮廓点的轨迹，建立轮廓轨迹方程。

3)建立约束条件函数并设置相关罚函数。根据式(6)(7)对泊车过程的车辆约束和环境约束建立约束条件函数，并对不符合约束条件的情况，根据式(9)(10)对其设置罚函数，以此优化模型。

4)设置目标函数。根据式(5)，以车辆到达路径终点时车辆纵轴线与cd边夹角最小设置目标函数。

5)设置PSO参数。对粒子群的粒子数、迭代次数、衰减因子进行设置，确保粒子算法的计算精度。

6)算法求解过程。初始化粒子群，即随机分配各个取值区间的参数值，带入目标函数中求解，并带入约束函数中，根据满足约束条件的个数，给各个粒子群设置不同的适应度。根据适应度的大小，选择最优的粒子作为最优选择，并且在最优选择附近对粒子群进行衰减更新，以此寻找最优的粒子。

7)通过Matlab进行编程，然后迭代求解，结果如表3所示。

表3 泊车工况参数

4 结果分析

车辆4种起始位置泊车轨迹如图5所示。

通过PSO算法求解的参数，可以较好地完成泊车任务。O1为a点的安全圆，O2为cd边的安全圆。有

式中：μ为安全阈值，可以根据车型和车位尺寸进行取值，本文安全阈值为0.3 m。

当车辆行驶至a点附近时，由于有安全阈值的存在，只要泊车轨迹上的点至O1的距离大于RO1,就可以保证车辆在转弯时不会碰撞a点。同理，在车辆行驶到cd边附近时，由于有安全阈值的存在，只要泊车轨迹上的点至O2的距离小于RO2，就可以保证车辆在泊车结束时，始终不会碰撞到cd边。

从图5中可以看出：4种泊车轨迹都符合该要求，所以泊车过程中，4种轨迹都可很好的避开a点和cd边,保证了泊车过程中的行驶安全。

图6为该泊车路径的曲率，从图6中可以看出：整个泊车过程中，只有第2个起始位置在行驶到3.8 m时超过了曲率的限制，其余路径的曲率都小于1/Rmin=0.18 m-1，说明该规划方法存在一定的依赖性，对于不同的起始位置，规划的泊车路径可能存在不符合事实的情况，这也是后期需要改进的地方；4种轨迹开始曲率都很小，几乎可以忽略不计，避免了泊车初始阶段出现原地转向的情况，图6a)的最大曲率与极限曲率仍然存在一定余量，余量越大说明车辆转动角度的变化越小，所以图6a)的车位初始位置视为最佳理想的初始位置，即：S0=10 m，h0=4 m。

a)起始点1 b) 起始点2 c) 起始点3 d) 起始点4图6 4种起始位置的泊车曲率

5 结语

1)规划的泊车路径可以很好的避开其他车辆和道路边界，整个转向过程较平稳，无原地转向的情况出现，增加了泊车的平顺性。

2)由于反正切函数的特性，导致停车状态和起始位置状态不能较好的满足现实停车的要求，也导致粒子群算法在迭代过程中很难得出一个较优解，所以对约束进行了修改，将一个确切的值改为范围区间，这不仅符合现实停车的不规则性，也有利于对更多较好解进行筛选。

3)本文从3个自动泊车模块中的路径规划模块联合Matlab对规划出来的路径进行了验证，后续还需要选择合适的跟踪控制模型对该路径的可行性进行仿真验证，可以在理论支持的基础上做出硬件，搭建实车平台，进行实车验证。