面向自动泊车的改进混合A*最优路径算法研究

刘知阳, 崔立堃, 耿玺钧, 冯绪永, 韩 晋

陕西理工大学 机械工程学院, 陕西 汉中 723000

汽车的出现使人们的出行交通更加便利,但这种便利也带来了许多现实问题,其中泊车就是一大难题。泊车受限于狭窄的视野环境,不可控的紧张情绪,还要同时面对不同的泊车环境,一不当心就会发生事故。自动泊车系统(Automatic Parking System,APS)的出现便能很好地降低事故的发生概率,将其限制在一个可控的范围内[1]。自动泊车系统也分为垂直停车和平行停车。垂直停车也就是倒车入库,一般用于停车场、车库停车;平行停车也就是侧方位停车,一般用于路边停车。

在国内外研究者的持续努力下,自动泊车技术研究取得了许多显著成果。混合A*算法于2008年由斯坦福的Dmitri Dolgov等[2]首次提出,该算法是一种基于车辆运动学的规划算法,能够满足车辆运动学的要求,解决了连续空间内规划车辆朝向和位置的问题,但规划效率低下、规划时间过长、规划路径无法保证成功率。Sedighi S等[3]将著名的混合A*搜索引擎与Visibility Diagram规划相结合,在混合(连续-离散)环境下寻找最短的非完整路径,为混合A*规划非完整约束下的最优路径提供了正确的路径点,但需要一定要求的高清静态地图部署。瑞典斯德哥尔摩KTH皇家理工学院综合交通研究实验室(ITRL)的研究概念车(RCV)开发了适用于概念车的混合A*算法路径规划算法,生成的算法需要以一种便于部署的方式进行包装,并且可以与研究车上的其他不同系统进行交互,同时也对实现实验测试所需的实时能力的方法以及如何建立模拟和调试的可视化环境提供见解,证明了混合A*算法的实用性,但对算法的运行效率没有显著提升[4]。中国学者LI Chao等[5]提出了一种基于A*算法和RS算法的路径规划算法,该算法引入了导引点的概念,通过RS算法直接生成无碰撞路径,并针对各种工作场景,采用混合A*算法离线计算导引点,提升了算法的运行效率,但失去了算法最优路径的效果,且无法保证完全不会碰撞障碍物。因此,本文提出了融合双向搜索的混合A*算法,在保证避免碰撞且保持最优路径的前提下提升算法的运行效率。

本文主要设计了一种融合双向搜索算法的改进混合A*算法,同时设置起点和终点作初始点,结合车辆碰撞信息,并设置合理的代价函数,相互搜寻最佳路径信息,能有效地缩减单向搜索后期繁多的无效节点并减少搜索次数,同时利用栅格法划分地图,有效地避免了在相同区域内重复搜索的可能性,同时结合栅格法和双向搜索算法,极大地提高了正反向搜索过程中相交的可能性。

1 算法概述

1.1 传统混合A*算法

混合A*算法是自动驾驶汽车常用的全局路径规划算法,其方法可以看作是A*算法结合实际汽车动力学模型后生成的算法,其原理与A*算法类似,从起点开始向四周搜索子节点,并计算子节点的代价值,取其中最小代价值的点为下一个父节点,直至搜索到终点[6]。不同于A*算法的是,混合A*算法搜索方式主要分为前进和后退两种模式,并结合车辆模型,平均分割其转角,并在转角范围内进行子节点的搜索,计算当前的车辆位姿到子节点的车辆位姿变换是否会与障碍物发生碰撞,并建立Openlist表接收转角范围内搜索到的子节点,建立Closelist表接收已经选取过的父节点和最小代价的子节点[7]。

图1(a)所示为A*算法搜索点的搜索模式[8],图1(b)所示为混合A*算法搜索邻近点所遵循动力学约束的搜索模式。

(a) A*算法搜索方法 (b) 混合A*算法搜索方法图1 两种算法搜索路径对比

混合A*算法的代价函数为

f(n)=g(n)+h(n),

(1)

其中,f(n)为当前节点n的代价值,g(n)表示起始点到当前点n的路径代价值,h(n)为当前点到目标点的预估代价值[9]。混合A*算法沿用A*算法的启发函数,主要有曼哈顿距离、欧氏距离、切比雪夫距离等[10]。其中欧式距离在程序中容易实现,只占用少量的计算资源[11],且适合数据的值域比较相似的场景,因此,本文选用欧氏距离来计算节点的预估代价值h(n)。

欧氏距离表达式为

(2)

其中,p1(x1,y1)为当前点,p2(x2,y2)为终点。

传统的混合A*算法虽然比A*算法更加贴合无人驾驶的实际应用场景,但也存在些许不足,例如在泊车搜索路径中,若存在障碍物较多,搜索过程就会比较缓慢;若搜索路径较长,搜索节点数会迅速增大,搜索时间会比较长[12]。为了使混合A*算法在以上情况也能保持良好的时效性,本文选择双向搜索算法和混合A*算法相融合,以解决混合A*算法在障碍物较多时搜索时间较长的问题。

1.2 双向搜索算法

地图数据通常可以用图(Graph)这类数据结构表示,在图结构中用到的搜索算法就叫图搜索算法[13]。双向搜索算法是一种图搜索算法,用于寻找图中一点到另一点之间的路径。Ira Pohl在1971年第一次设计并实现了双向启发式搜索算法[14],该算法同时从起点和终点进行搜索,当两者在中间搜索的点重复时,链接两条路径并停止搜索,所得到的由两条路径拼接后的生成路径就是双向搜索算法的最佳路径[15]。

如图2所示,假设我们要查找0到14的路径,普通搜索流程是从节点0开始查找,每个节点查找一次共需要14次,而双向搜索各自从节点0和14同时开始查找,在查找到7时就已经找到了0到14的节点路径。假设搜索一棵分支因子为b的树,初始节点到目标节点的距离为d,该算法的正向和反向搜索复杂度都是O(bd/2),算法路径复杂时,两者相加远远小于单向搜索的复杂度O(bd)[16]。

图2 双向搜索原理示意图

2 算法优化与融合

2.1 双向搜索算法优化

从起点到终点的搜索过程是具有不确定性的,即从起点到终点的搜索路径与从终点到起点的搜索路径不一定能完全相交,而不相交的情况下两条路径都会执行至搜索到目标点为止,但这并不利于优化混合A*算法。本文为了解决路径不一定相交的问题,运用栅格法划分地图,将地图按一定规律进行行和列的规则划分,形成许多有规律的网格,来方便管理已扩展节点和未扩展结点之间的关系[17]。当正向搜索和反向搜索的子节点在同一个栅格中时,中断搜索过程,链接反向搜索的父节点至正向搜索的子节点,最后输出整个路径,这样就得到了一条由正向搜索和反向搜索组成的最终路径。优化算法前后的最终输出路径如图3所示。

(a) 算法优化前 (b) 算法优化后图3 双向搜索优化图

图3(a)为算法优化前的双向搜索交互情形,图3(b)为算法优化后的双向搜索交互情形。图3(a)描述了原来双向搜索算法搜索过程中,正向搜索与反向搜索同时进行时,即使大致方向一致,正常相遇的情况下,其正向搜索的子节点与反向搜索的子节点也不一定会完全重合,其搜索的子节点不同时在正反向搜索的Openlist表中,则正反向搜索过程都不会中止,最终会形成两条搜索路径。利用地图尺寸与设置的栅格分辨率划分栅格,把路径末端点的实际坐标转换为栅格坐标,公式为

(3)

式中,Xindex、Yindex、θindex为栅格坐标的行索引、列索引、朝向编号,x、y、θ为当前路径末端点的横坐标、纵坐标、车辆摆角,gres为路径分辨率,yres为车辆摆角分辨率。当正向搜索和反向搜索的路径末端点的栅格行索引和列索引相同时,只保留正向搜索的子节点并与反向搜索的父节点重新执行混合A*算法,保留正向搜索的子节点至Closelist表,搜索成功后合成正向搜索和反向搜索的搜索路径并输出曲线。

2.2 算法融合

本文在传统混合A*算法中融入双向搜索算法,得到改进混合A*搜索算法。其中的正向搜索算法流程图见图4。

图4 改进混合A*算法正向搜索流程图

其反向搜索流程大体与正向一致。判断D是否为0,若不为0,则进入反向搜索流程。经过与图4的正向搜索流程相同的反向搜索,若没有找到路径,则重置D为0,再次经过判别式,并进入正向搜索流程。如此循环直到输出最终路径。

整体代码设计流程：预先设置地图形状、车辆的参数信息和运动分辨率,再设置起点和终点的位姿信息(x,y,θ),其中x、y、θ为坐标及车辆偏角的真实值;在已知车辆的起始位置、最终停车位置和地图信息后,开始自动泊车全局路径的路径规划;初始化栅格和4个列表;计算起始点到终点的欧氏距离,并以此为混合A*算法的启发值;正向搜索以车辆目前位置为起始点,最终停车位置为终点(反向搜索相反),选取最小代价值的节点作为父节点;利用混合A*算法搜寻邻近点并每搜寻一次邻近点就获得搜寻行为的代价值;判断父节点到子节点的代价值和子节点到终点欧氏距离的代价值,获得预估代价h(n),判断父节点到起始点的花费,获得实际代价g(n),判断正向搜索和反向搜索是否有相同的扩展点;重复操作直至正反向搜索有相同的扩展点,合并列表并输出曲线信息。

3 实验与分析

经由MATLAB搭建仿真平台[18],对混合A*双向搜索算法的时效性、可行性、可靠性进行验证[19]。通过仿真平台模拟真实车辆的行驶情况,模拟垂直入库实验和侧方位停车实验,从中选出一条符合动力学逻辑最小代价的路径并验证该算法的可靠性和时效性[20],同时对比传统的混合A*算法,对双向搜索的混合A*算法的时效性进行比较验证。

这里比较改进前后的算法在垂直入库和侧方位停车时的搜索时长和路径节点搜索情况。

3.1 垂直入库实验与分析

垂直入库实验通过上述仿真平台,模拟自动泊车中垂直入库的情况。该实验只对其路径规划的长度、车辆前进后退的频率、转向的曲率大小作要求,不对车辆的速度、受力做分析[21]。

垂直入库是大部分停车时要遇到的情况,且通常困难的情况是空出一个车位的同时两旁的车位都已经被占用,这里就仿真该情况下采用改进混合A*算法来进行路径规划的过程。垂直入库实验具体路线如图5所示。

图5 传统混合A*算法路径曲线图

分析图5可知,传统混合A*算法规划的路径并非实际最优,还存在转角倒车的情况,虽然符合车辆运动的动力学要求,但是改变方向的实际代价值很高,因此最终代价值就不一定是最小代价值,最后路径生成曲线就不一定是最优路径。

更换融合算法后,我们保持相同的环境再次重复相同的步骤,并保持相同的起点位姿为(x,y,θ),其中x为车辆矩形框中心点的x坐标值,y为车辆矩形框中心点的y坐标值,θ为车辆矩形框车头方向(x的反方向)与x轴的夹角。图6为更换融合算法后生成的路径曲线图,其中图6(a)是在无阻挡物时生成的路径曲线图,6(b)是在设置了阻挡物后生成的路径曲线图。

(a) 融合算法路径曲线 (b) 改变障碍物生成路径 图6 融合算法生成路径曲线图

分析图6(a)可知,在与传统混合A*算法相同的条件下,本文改进的混合A*算法生成路径曲线降低了车辆改变方向的次数,运行轨迹虽然不平滑,但无停车和倒车的情况,降低了融合算法中实际路径代价值,且其路径搜索时间比传统混合A*算法路径搜索时间更短。图6中无障碍与有障碍物两种情况对比可知,融合算法生成路径受障碍物影响较大,当障碍物产生不同的变动时,其路径规划的时间存在较大的波动,其产生的结果不利于直观地比较数据间的差异。因此可以改变不同起点位姿,比较改进混合A*算法的单向路径搜索和双向路径搜索的搜索时间、改变方向次数、节点数,其结果见表1。

表1 垂直入库改进算法比较

由表1可知,在垂直入库实验中,融合算法在路径搜索时间上比传统混合A*算法用时平均减少了22.22%,传统混合A*算法至少改变一次行驶方向,而融合算法则减少了改变方向次数,减少了部分实际路径代价值的生成,表中双向搜索节点数虽然有部分的减少,但也有未改变的情况,这与设置的起点和终点位姿有关,设置的起点位姿不同,生成节点数就不固定,单向搜索出个别路径时,双向搜索则无法更新最优路径,生成的节点数也无法减少。

3.2 侧方位停车实验与分析

侧方位停车通常是在道路旁停车,与倒车入库实验相同,用融合算法和传统混合A*算法对该情况进行路径规划,传统混合A*算法侧方位停车路径曲线如图7所示。

图7 传统混合A*算法侧方位停车路径图

在该侧方位停车的过程中起点位姿为(-20,20,0),终点位姿为(1,7.5,-π)。由图7可知,侧方位停车中传统混合A*算法生成路径时,路径存在多次改变行驶方向的问题,且其重复路径规划过多,无法在狭窄的环境中准确生成适合的路径曲线,实际代价值过大,无法确保该路径曲线为最优路径。由图8可知,在侧方位停车实验中,融合算法能更好地规划正确的路径,减少改变行驶方向的次数,且在狭窄的空间中也能很好地完成路径规划,比传统混合A*算法生成的路径实际代价值更低,更接近最优路线的标准。

图8 融合算法侧方位停车路径图

更改起始点,比较传统混合A*算法和融合算法在侧方位停车情况下的搜索路径时间、改变方向次数、节点数,结果见表2。

表2 侧方位停车改进算法比较

由表2可知,在侧方位停车中,融合算法的路径搜索时间比传统混合A*算法平均减少了28.20%,减少了改变方向次数,与垂直入库情况相同的是,当单向搜索算法搜索到个别路径时,双向搜索算法无法更新最优路径,搜索的路径节点数无法减少。

综合分析垂直入库和侧方位停车,融合算法比传统混合A*算法的搜索时间平均缩短了25.21%。

4 结语

针对传统混合A*算法在自动泊车路径规划过程中搜索时间过长的问题,本文提出了融入双向搜索算法的混合A*算法,设计了融合方法,制定了算法的具体计算流程。并针对双向搜索中正向搜索和反向搜索扩展节点不容易重合的问题,引用栅格法优化了双向搜索算法来解决该问题。对比了传统混合A*算法与融合算法的搜索时间、改变方向次数、节点数,结果表明融合算法不仅比传统混合A*算法的搜索时间平均缩短了25.21%,而且改变方向次数普遍比传统混合A*算法少。说明融合算法最后路径执行代价更小、实时性更好,更能满足车辆的运动条件。

本文虽然融合了双向搜索算法,利用栅格法优化了双向搜索时的相遇机率,但还是存在有双向搜索无法相遇的情况,导致生成的路径并非最短路径,这也是后续研究需要解决的问题。