基于二段多项式的窄空间平行泊车路径规划方法①

熊 莹,毛雪松

1(武汉科技大学 计算机科学与技术学院,武汉 430065)

2(武汉科技大学 智能信息处理与实时工业系统湖北省重点实验室,武汉 430065)

3(武汉科技大学 信息科学与工程学院/人工智能学院,武汉 430081)

相比于垂直泊车,平行泊车对驾驶员难度更大,特别当车位长与车身长比值较小的情况下,很难完成连续转向的泊车任务.泊车的路径规划包括正向驶入和倒车入库,其中正向驶入的方法要求车位较长.当车位较短时,通常使用倒车入库的方法.当车身长接近车位长时,为减少泊车过程中的档位切换,在倒车入库前需确定最佳倒车起点位置与方向角(统称为位姿).本文基于倒车入库常用的二段泊车法,即在前进过程中预先调整车辆的方向角,再倒车进入泊车位的方法,提出使用二段五阶多项式,分别表示前进路段和倒车路段,实现在有限空间范围内的自动泊车.

自动泊车路径规划常用的方法包括几何路径法、基于地图的搜索法以及基于模糊控制的方法等.其中几何路径法包括使用圆弧与直线连接生成路径法[1,2]、B 样条曲线法[3,4]、五阶多项式法[5–7]等.这些方法没有考虑前行过程中调整车辆方向角,而是假定倒车的起始位姿与车位横向方向平行,因此需要更大的横向空间.在车位长度较短的情况下,不预先调整方位角有可能找不到连续泊车路径.圆弧与直线连接的方法产生的路径存在曲率不连续,泊车过程中需要多次停车调整方向盘转向角,轮胎磨损大的问题.使用B 样条曲线产生的路径,虽然能够满足曲率连续的要求,但由于方程的约束条件以及待求参数量很多,求解过程复杂且运算量大.

建立于地图搜索的方法与车辆行驶路径规划方法类似,包括常用的A*、D*、RRT[8–11]等,这些方法将地图分割为小的栅格,通过建立启发函数在栅格中搜索到一条长度最优的无碰撞路径,然后选择使用曲线对该路径平滑得到曲率连续路径.首先,栅格的大小对算法的计算效率影响很大;其次,路径搜索中没有考虑车辆运动学特征,即使经过平滑,在空间狭窄的情况下,车辆也不一定能够跟踪所得到的路径.基于控制的方法则根据驾驶员的驾驶经验,整理出一套控制规则,控制泊车过程中的转向角变化.这种方法在泊车过程中需要多次改变车辆的行驶速度和转向角、多次切换行驶方向,对车的前后所需要的空间较大.

本文根据驾驶员泊车经验,将泊车过程分解为前进调整方位角和倒车入库两个过程,对这两个过程分别使用五阶多项式规划最优路径,相比于前述文献中的方法,本文实现在更短的空间内泊车,且实现方向盘的连续转向.

1 仿真场景与车辆模型

考虑纵列驻车停车场位于车辆右侧,停车位长5.5 m,宽2 m,相邻停车位之间间隔20 cm,符合通常停车位的标准,如图1所示.在车辆长度较大,即泊车位相对较小的情况下,为了避免车辆多次切换档位,通常按图1所示,在倒车入库前调整自身的位置和方向角(统称为位姿).在前进调整位姿与倒车入库过程中,车辆不得触碰相邻车位,且不能触碰自身停车位的路沿边界.倒车入库后要求车身与车位长边平行,如果方向盘转向角不为零,可以在泊车完成后增加一个调整方向盘的步骤.

图1 平行泊车仿真场景

泊车过程中,车辆的速度很低,因此可以不考虑侧滑等因素,车辆的运动采用自行车模型[12].

这里(x,y)表示车辆后轴中心位置处的坐标,θ为方向角,定义为车头方向与x轴正方向之间的夹角,δ表示方向盘的转向角,l表示前后轮之间的轴距,v和分别表示车的径向速度和方向盘的转动角速率,w表示方向盘的转向速率.前3 个方程表示车辆的运动状态更新,最后一个方程表示对方向盘转向角的控制,该式本身为冗余项,需要根据搜索得出的无碰撞路径,使用专门的转向角控制算法来控制车辆行驶过程中的转向角,例如Pure Pursuit[13]、Stanely[14]等算法.规划无碰撞路径算法中,考虑一定的安全裕度同时为简化运动学计算,用矩形来定义汽车的外观.

算法仿真中使用的车辆特征参数如表1所示.

表1 车辆特征参数

表1中,轴距表示前后轴之间的垂直距离,前悬为车头中心到前轴中心的距离,后悬表示车尾中心到后轴中心的距离.泊车完成后,车位前后方自由空间长度总和为50 cm.

2 二段多项式泊车路径规划方法

2.1 5 阶多项式连接相邻路点

有很多文献使用5 阶多项式作为连接汽车起点(x0,y0)与终点(xf,yf)之间的路径规划的方法,所得到的路径满足起点和终点的约束条件,且二阶连续,车辆控制算法对路径的跟踪精度高,但是起点与终点路径并不总是能够通过一条多项式曲线连接.如图1所示,车辆行驶路径包括前进路径与后退路径,虽然整条路径是连续的,但不满足二阶连续,因此不能使用单个多项式来表示从起点到终点的路径.对车辆的行驶路径分段,保证每段路径二阶连续,且相邻路径之间满足一阶连续的条件,则称每段路径的端点为一个路点.图1的路径中包含3 个路点,分别称为起点、中间点和终点.考虑车辆的方向角和转向角,分别定义3 个点的位姿为(x0,y0,θ0,δ0)、(xi,yi,θi,δi)和(xf,yf,θf,δf).

从路径上看,车辆的方向角θ即为路径对x的导数dy/dx,转向角δ与路径之间满足关系:

其中,l为车的轴距,κ为路径的曲率,定义为:

在一阶导数已知的情况下,它是一个仅与二阶导数有关的量,因此根据曲线点的参数将车辆位姿修改为(x,y,y′,y′′).

由于Stanely 方法控制车辆方向盘转向角需要计算前轮与路径之间的最小距离,因此规划前进路径时,以前轮中心所能到达的位置为前进路径的终点.倒车时,为减小控制误差,使用基于参数的方法控制方向盘转向角,从后轮中心所在位置处为起点计算第二条多项式路径.将这两个点均称为泊车路径的中间路点,分别记为(xi1,yi1,y′i1,y′′i1)和(xi2,yi2,y′i3,y′′i3).二段多项式法生成的前进路径、倒车路径以及各路点的表示如图2所示.

图2 前进与倒车路径以及路点标识

2.2 多项式系数的求解

设连接路点的起点与终点多项式表示为:

将空间内无碰撞路径搜索问题转化为多项式系数的求解问题.

取泊车路径的起始位置坐标为(x0,y0),位于待泊车位左后方,起始位置处的方向角和转向角均为0,因此:

终点坐标为(xf,yf),满足:

其中,C为车转向角限定的常数.

泊车路径包括前进与倒车两段路径,每段路径均由一条多项式曲线连接两个路点得到.式(5)确定前进路段的起点,式(6)确定倒车路段的终点.根据图2,在中间点已知的情况下,即可根据式(4)计算连接起点和终点的两段多项式.

将每段曲线两个路点的位姿给出的6 个限定条件代入式(4),得出在r=5的情况下,可以找到连接路点起点和终点的唯一曲线.令:

图3 高阶系数修正的泊车路径

将6 个限定条件分别代入式(7)～式(9),求出曲线方程的系数为:

这里,

式(10)和式(11)中的下标l0和lf分别表示路点的起点和终点.对于平行泊车,两个路点上车的方向角之差不会很大,因此B矩阵不会出现奇异解.

2.3 中间点的搜索

图2中间点(xi1,yi1)和(xi2,yi2)的搜索以及在中间点位置处的最佳方向角和转向角的计算是泊车路径规划的关键.可以根据驾驶经验,确定点(xi1,yi1)存在的区域范围以及方向角范围.为了使前进与后退过程中方向盘的连续转向,可取点(xi1,yi1)处的转向角δ为0,即y′′|x=xi1=0.

使用搜索算法找到点(xi1,yi1)以及方向角后,根据车体的位姿求出点(xi2,yi2)的位姿为:

作为倒车路段的起点.

2.4 边界条件的数学模型

要求在前进过程中车的v1v2边、v2v3边不能与相邻车位的a0b0边相接触,倒车过程中v1v2边、v2v3边不能与a2b2边、a2d2边接触,另外v1v2边、v1v0边不能与c1d1边接触.转化为数学模型,即要求前进路段:

其中,v2(x)、v2(y)分别表示顶点v2的x坐标和y坐标.

倒车路段要求:

2.5 目标函数的定义

按照两个路段的曲率最优原则定义目标函数为:

其中,κ1和κ2分别为前进路段和倒车路段曲线各点的曲率.α为对目标函数贡献的权重系数.

3 仿真结果

取待泊车位的4 个顶点坐标为(3,0)、(8.5,0)、(8.5,2)、(3,2),车的起始坐标位置为(2.5,3.5),方向角和转向角均为0.泊车位置为(3.9,1.05),方向角为0,转向角设为方向盘最大转向范围内的一个数,这里取34°.在x从9 米到14 米、y从4 米到6 米、θ从0 到π/4的范围内搜索最优泊车路径,得到的泊车路径如图4所示.由于通常期望目标函数中倒车的曲率贡献值大一些,因此仿真中定义权重因子α=0.1.从泊车过程的行驶轨迹看,二段多项式法可以正确实现平行泊车,且泊车过程中转向角连续变化.前进过程中适当调整车的方向角,可以使倒车过程更容易完成.

图4 最优二段多项式泊车路径以及泊车行驶轨迹

泊车时,期望将车尽可能停向车位内侧,因此取终点位置处的y值分别为1.01 m、1.02 m、1.03 m、1.05 m等几种情况,在xi1从9 m 到14 m、yi1从4 m 到6 m、θ从0 到π/4的范围内搜索存在安全路径的中间点.某一中间点处,可能有多个θ值对应安全路径,在这些方向中选择一个θ值,使路径的曲率最小.存在安全路径的中间点区域如图5所示.从图可以看出,车辆终点位置越靠近外侧,中间点存在区域就越大.当xi1值较小时,安全路径的中间点θ值较大,这与驾驶经验吻合.

取yf=1.05 m,对应于图5(d)各点处泊车路径的曲率如图6所示,可见对应于每一个xi1,存在一个最佳yi1和方向角.同样对应于每一个yi1值,存在一个最优xi1值,使得曲率最低.另外,对于固定xi1或yi1,观察θ对另一变量也具有下凸性,因此,在搜索区域范围内式(15)定义的T函数具有下凸性,可使用常规优化算法在搜索空间内找到最优解.例如选取前轮中间点位姿xi1=18.0 m,yi1=10 m和θ=π/4,利用最速下降法搜索,设置收敛精度为2 cm,程序运行于Intel(R)Core(TM)i5-4460 CPU@3.2GHz,内存8 GB,得到最优解的时间约xi1=14.6 m,yi1=7.3 m和θ=0.79,所用时间为0.73 s.

图5 泊车终点位置的纵向坐标对中间点存在区域的影响,箭头指向该位置处的最佳方向角

图6 对应于图5(d)各点的安全路径曲率

前向路径上,使用Stanely 算法控制转向角.当参考路径为多项式时,低速情况下Stanely 具有良好的跟踪性能.倒车路径上,根据计算得到路径的位置、一阶和二阶导数,计算出在每一点位置处的方向盘转向角,将其保存为倒车控制文件(Speed Profile).仿真中,设置仿真时间步长参数为0.025 s,倒车速度为0.5 m/s,对路径的跟踪误差不到0.2 cm,因此通过分段得到的路径可以运用于车辆的平行泊车中.

本文通过二段曲线泊车法实现车位长度5.5 m 情况下的连续泊车,车位长与车身长的比值约为1.2,与相关文献中已实现的车位长与车身长的比值对比,结果如表2所示.

表2 车位长/车身长的比较

4 结束语

本文提出了一种基于二段五阶多项式的方法完成平行泊车,给出存在安全泊车路径的中间点存在范围,设计代价函数并验证其下凸性.基于通常平行车位的尺寸以及B 型家用轿车尺寸,执行泊车仿真.仿真结果表明,通过调整中间点方位角的方法,可以在狭窄的空间范围完成平行泊车任务.相比于过去假定倒车起点位置处车辆的方向与车位平行的方法,本文所提的方法更加符合驾驶员的驾驶习惯,实现更窄空间的平行泊车,可以为智能驾驶车辆泊车系统的开发提供通用的方案.