曹彦博 颜京才 李旭升 曹立波
（1.湖南大学，汽车车身先进设计制造国家重点实验室，长沙 410082；
2.毫末智行科技有限公司保定分公司，保定 071000）
主题词：自动泊车 混合A*算法 路径搜索
路径规划作为自动泊车系统的重要组成部分，是保障行驶安全、泊车效率和乘员舒适性的关键。路径搜索是路径规划的重要步骤，路径搜索算法一般可分为图搜索算法、树搜索算法和智能优化算法[1]。图搜索算法[2-3]的原理是将车辆附近的环境信息转换为离散图，然后通过一定的搜索算法得到基础路径，如A*算法[4]、D*算法[5]、混合A*算法[6]等；
树搜索算法在高维空间可行，可以得到安全无碰撞的路径，但由于采样具有随机性，导致前、后规划的路径有可能不一致，路径曲率连续性差；
智能优化算法[7]将路径规划视为集安全、边界和最短距离的约束，包括蚁群算法、遗传算法和细菌觅食法等。
传统的路径搜索算法得到的路径普遍存在一些缺点，如路径不够平滑、难以跟踪控制、搜索效率不高等。针对这些问题，本文基于较随机采样更加稳定的图搜索算法，提出一种改进的混合A*算法对短距离的自动泊车进行路径搜索：针对障碍物较多、避障更复杂的车位内的终点和周围相对较宽敞的起点，采用反向搜索的方法结合适用于该场景的线碰撞检测方法获得路径；
利用A*算法获得代价地图，在进行路径搜索时通过查表得到启发值，大幅缩短搜索时间；
通过设置可变的车辆转角分辨率在节点扩展时增加其扩展方向数量，提高路径的平滑度，使其更易于被优化和跟踪控制。
2.1 碰撞检测
在自动驾驶研究中描述车辆周围环境信息的方法一般包括栅格地图、拓扑地图和几何地图等方法[8]，而考虑到自动泊车在非结构化道路复杂环境下进行，并且本文所研究的混合A*算法基于栅格进行搜索，因此选用栅格地图来描述环境信息，即在感知系统获得真实世界坐标后通过设置合适的分辨率将其转变为栅格地图，利用栅格存储相应的节点信息进行节点扩展和路径回溯。
基于栅格地图的碰撞检测通常用包围盒来描述车身轮廓，然后通过逐一判断障碍物占据的栅格与车身的距离进行碰撞检测。常用的包围盒包括轴对齐包围盒（Axis-Aligned Bounding Box，AABB）、有向包围盒（Oriented Bounding Box，OBB）和球包围盒。其中，AABB使用与栅格平行的直线包围车身，OBB采用有向的边界框，能更好地描述物体的原形状，球包围盒则用若干个圆包裹车辆，各包围盒具体形式如图1所示。这种栅格占据的碰撞检测方式可以较好地描述车身轮廓并通过判断与障碍物的距离进行碰撞检测，但需要对所有障碍物占据的栅格进行判断，增加了搜索时间，并且将障碍物离散化为栅格容易出现误差，同时较小的分辨率会增加时间成本，较大的分辨率则会导致对障碍物的描述不准确。
图1 几种常见的包围盒
本文重点研究车辆已在车位附近的短距离自动泊车场景下的路径搜索，此场景下车辆起点周围环境简单，通常不会有复杂的障碍物，只需考虑停入车位过程中的避障即可。因此，为了简化碰撞检测的计算，本文将车位边框设置为障碍物线，只保留泊车的入口，如图2 所示。然后通过向量叉积法判断车身轮廓线与障碍物线是否相交进行碰撞检测，即在已经设定好的泊车场景中，利用车身参数和当前所处位置计算出车身4个顶点的坐标，如D点坐标(xD,yD)：
图2 障碍物简化形式
式中，(xr，yr)为车辆后轴中心坐标；
lr为车辆后轴到车身后端的距离；
W为车身宽度；
θ为车辆轴线与水平方向的夹角。
得到4个顶点的坐标即可获得描述车身轮廓的4条线段，然后通过判断其与障碍物线是否相交进行碰撞检测。这种碰撞检测方法计算简单，可以节省混合A*算法搜索路径的时间，并且这一过程在世界坐标下进行，无需进行障碍物占据栅格转化，误差较小。
2.2 基于混合A*算法的反向搜索
A*算法采用贪心策略，通过设计启发函数选择代价估计值小的点进行搜索，相较于广度优先搜索和深度优先搜索有着更高的搜索效率[9]。传统A*算法搜索出的路径是折线，显然不符合车辆的运动学条件。与传统A*算法相比，混合A*算法改变了连通图结构，额外考虑车辆位姿，引入θ这一状态量，优化了节点拓展方式，并以车辆的最小转弯半径作为约束条件，使得搜索出的路径满足车辆的运动特性，从而易被跟踪执行，2 种算法的节点扩展方式如图3所示。
图3 节点扩展方式对比
与A*算法相同，混合A*算法也使用评价函数选择更优的节点：
式中，f(n)为从初始状态经由状态n到目标状态的成本估计量；
g(n)为在状态空间中从初始状态到状态n的实际成本；
h(n)为从状态n到目标状态的最佳路径的成本估计。
但混合A*算法的内涵与普通A*算法存在较大差别。g函数除原有意义外，还应针对不理想因素施加惩罚，如对反复前进倒退、频繁变化转角等情况进行惩罚来体现当前节点的竞争力[10]。启发值h的计算方法一般为：
式中，hc_av为考虑避障因素但不考虑运动学可行性时的路径长度；
hk_c为考虑车辆运动学约束但忽略碰撞时的路径长度。
式（3）的含义为：如果当前节点与终点距离较近，生成路径的难度在于保证运动可行性，此时hk_c更能反映路径长度；
反之，路径生成难度在于避障，需要hc_av作为启发值。通常用RS（Reeds-Shepp）曲线估算hk_c，用经典A*算法确定hc_av的取值。
一般情况下，混合A*算法从起始位姿开始，朝着目标位姿扩展节点，同时尝试使用无碰的RS 曲线连接当前节点和目标点以加快搜索速度。但在车位较小或终点附近环境复杂时，连接终点的RS 曲线无法满足避障要求，导致节点扩展过多，最终出现搜索不到路径或耗时较久的情况。
在本文所研究的泊车场景中，车辆的起点处环境简单，根据2.1节的分析，仅考虑车位附近的避障，即车位线，因此目标点（车位内）避障要求较高。针对此情景，本文提出一种基于混合A*算法从目标位姿向起始位姿反向搜索的搜索策略，将泊车路径分为2 段，如图4 所示。第1 段从空间狭窄的车位中的目标点开始向起点不断进行节点扩展（E-O段），第2段尝试在满足避障要求的条件下用RS 曲线从当前节点连接起点（O-S段），最终将所有点逆序输出即可得到搜索出的泊车路径，而由于起点附近空间较大，RS 曲线会较早实现无碰连接起始位姿来加速搜索。同时，终点附近狭窄的车位也由扩展节点结合碰撞检测保证路径的最优性。
图4 分段泊车路径
此外，传统的混合A*算法在节点扩展时仅有6 个方向，即直行的前进、后退以及转向盘向左、向右最大转角状态下的前进和后退，导致车轮的转向角较大，难以实现车辆的控制并且影响乘员的乘坐体验。本文通过设置车轮转向角的分辨率增加搜索过程中的节点扩展方向，其扩展方式如图5 所示，同时考虑了转向角的代价值，使得搜索出的路径更加平滑，避免车辆转向角过大导致各段路径之间连接处曲率不连续，使路径易于被优化和跟踪。在代价函数方面，除为使路径更加平滑设置的转向角度代价外，还加入了车辆前进/后退挡位切换代价、转向代价等，并将其加权放入函数g中，共同保证了路径的平滑和泊车过程中乘员的舒适性。
图5 节点扩展方式
2.3 基于A*算法的代价地图
由于式（3）中hc_av指使用A*算法得到的当前节点到目标点的启发值，因此通常在搜索过程中需要不断使用A*算法计算当前栅格到终点栅格的h，耗费大量时间。
本文使用一种代价地图来获得启发值h，在混合A*算法开始路径搜索前先将地图栅格化，接着利用A*算法计算每个栅格到终点栅格的代价值，将该值与栅格对应存储即得到代价地图，如图6所示，图中数值为A*算法计算得到的栅格代价值，Inf 表示该栅格代价值为无穷大，为车辆不可到达的区域。因此，在后续路径搜索过程中无需再逐一进行计算，可以直接从代价地图中通过查表方式获取启发值，可大幅节省时间，提高路径规划效率。且本文设计的搜索算法从终点向起点反向搜索路径，再使用A*算法计算代价地图时的目标点即为泊车起点，即可得到所有栅格到起点的代价值，相当于同时得到了多个车位到起点的启发值。多车位下全自动的泊车路径规划同样具有重要意义，在本文研究的自动泊车系统中具体表现为：自动泊车系统启动后，系统首先自动计算代价地图，用户选择想要泊入的车位后泊车系统进行路径搜索，在搜索过程中省略了使用A*算法计算启发值的步骤，直接从代价地图中查表获得，极大节省了搜索时间。
图6 基于A*算法得到的代价地图
2.4 路径搜索算法框架
综上，本文提出的路径搜索算法流程如图7 所示。通过传感器等得到环境信息和起止点后首先通过A*算法得到整体的代价地图，再使用改进混合A*算法从目标位姿反向搜索路径，其间不断尝试用RS 曲线连接当前节点与起点并进行碰撞检测，直到成功利用无碰撞的RS 曲线连接或直接扩展到起点时结束搜索，最后逆序输出所有节点即可获得有效泊车路径。在搜索过程中通过open 和close 2 个集合对节点进行管理，open 集存放待扩展节点，将已经扩展过的节点存入close集。
图7 路径搜索算法流程
城市中常见的车位类型有水平式和垂直式，因此本文使用MATLAB 分别设计针对这2 种常见车位的泊车场景，对改进的混合A*算法和原始算法进行对比仿真分析，以验证其合理性和有效性。
3.1 水平车位
在水平车位泊车场景下分别使用普通混合A*算法和本文提出的改进混合A*算法进行泊车路径搜索，结果如图8所示。普通混合A*算法的节点扩展方向只有6种，因此本文在进行时间比较时将改进混合A*算法的节点扩展方向设置为与其相等，以验证本文设计的反向搜索结合代价地图查表的混合A*算法的实时性。2 种算法的仿真结果数据如表1所示。
表1 水平车位泊车场景仿真结果数据
图8 水平车位泊车场景下2种算法的路径搜索结果
结合图8和表1可以看出，改进混合A*算法用时更短，得到的路径也更短、更加平滑，提高了泊车效率，易于后续对车辆的控制，从而实现较好的轨迹跟踪。
3.2 垂直车位
垂直车位泊车场景下仿真条件设置与水平车位场景相同。图9 所示为普通混合A*算法和改进混合A*算法的路径搜索结果，其仿真结果数据对比如表2所示。
表2 垂直车位泊车场景仿真结果数据
图9 垂直车位泊车场景下2种算法的路径搜索结果
从图9 中可以看出，改进算法得到的路径更优，且由表2 可知，改进算法比原始算法节省了2/3 以上的时间，极大提高了规划效率。
综合2种车位泊车场景下的仿真结果，本文提出的改进混合A*算法在水平车位泊车场景和垂直车位泊车场景下均比传统混合A*算法用时更短，说明在短距离泊车、车位附近环境复杂的场景下，反向搜索结合代价地图的使用可以有效提高路径搜索效率。并且结合仿真数据和得到的路径可以看出，改进的算法获得的路径更短、更平滑，验证了变分辨率节点扩展的优越性。
本文基于混合A*算法，针对短距离自动泊车提出一种反向搜索的路径搜索算法，结合适合此泊车情景的较简单的线碰撞检测方法，使其从终点开始扩展，同时尝试用RS曲线连接起点，通过设置转向角分辨率增加路径搜索过程中的节点扩展方向，并结合使用A*算法得到的代价地图，使其能够适用于有多个可选车位的泊车场景，然后使用MATLAB分别设计了平行和垂直车位泊车场景，对提出的算法进行仿真分析，并与普通混合A*算法进行对比。结果显示，2种场景下改进的混合A*算法均有效提高了搜索效率，且得到的路径更短、更平滑。
完整的自动泊车路径规划系统应包括搜索、优化，并对速度进行规划得到完整的泊车轨迹，本文只开展了路径规划中的搜索工作，未对搜索到的路径进行优化，这也是本文后续需要研究的内容。
猜你喜欢碰撞检测泊车搜索算法基于插电式混动汽车的自动泊车控制策略汽车实用技术(2023年10期)2023-06-14基于MATLAB的平行泊车路径规划汽车实用技术(2022年19期)2022-10-19基于CarSim的平行泊车仿真分析汽车实用技术(2022年7期)2022-04-20全新预测碰撞检测系统汽车工程师(2021年12期)2022-01-17改进的和声搜索算法求解凸二次规划及线性规划烟台大学学报（自然科学与工程版）(2021年1期)2021-03-19Arrive平台新增智能泊车推荐引擎 帮助找到最佳泊车地点军民两用技术与产品(2020年3期)2020-04-07基于BIM的铁路信号室外设备布置与碰撞检测方法铁道通信信号(2020年10期)2020-02-07Unity3D中碰撞检测问题的研究电子测试(2018年1期)2018-04-18BIM技术下的某办公楼项目管线碰撞检测中国工程咨询(2016年12期)2016-01-29基于汽车接力的潮流转移快速搜索算法电测与仪表(2015年15期)2015-04-12