CN112762950A - 基于人工势场引导的Hybrid A*自主泊车路径规划方法 - Google Patents

Abstract

本发明公开了一种基于人工势场引导的Hybrid A*自主泊车路径规划方法。针对先验地图分区域构建人工势场图，然后将人工势场图和车辆实时位姿信息作为输入，以保持车身与势场方向相同的原则确定Hybrid A*算法节点扩展的整体方向，对节点的扩展进行剪枝操作，实现在访问较少节点的同时生成可行驶路径。本发明方法在保证最优路径规划的同时可以更加有效地提升算法运行时间，避免Hybrid A*算法在节点扩展过程中存在的缺少指向性造成的冗余计算的问题，进而在算法生成安全、可靠路径的同时，提升算法的实时性。

Description

基于人工势场引导的Hybrid A*自主泊车路径规划方法

技术领域

本发明涉及无人车路径规划技术，尤其涉及一种基于人工势场引导的Hybrid A*自主泊车路径规划方法。

背景技术

随着汽车行业的飞速发展以及人工智能技术的兴起，无人驾驶技术应运而生，并成为当今时代研究的重点。日常生活中，汽车作为人们出行的重要交通工具，其数量的增长使得停车成为人们面对的首要难题，逐渐紧缺的停车区域导致司机需要在有限的空间内安全、可靠地驶入停车位中，自动泊车系统可以一定程度上解决相关难题。

自主泊车规划是无人驾驶系统中的关键部分，其本质是一个路径搜索问题，其主要任务是利用先验地图进行模型抽象，并结合周围环境信息，通过一定的规则为智能驾驶车辆提供从起始位姿到目标位姿的一系列方向盘转角和档位信息，其中路径规划的优劣直接决定着泊车系统能否找到一条安全的、平滑的、最短的抵达目的地的行驶路径。

目前，国内外针对泊车路径规划问题研究了多种方法，包括螺旋线、样条插值、多项式等多种曲线拟合方法，还有通过简单的几何作图方法进行路径规划，也包括迪杰斯特拉、A*等基于二维栅格图的搜索算法。其中用曲线拟合的方法如三阶贝塞尔曲线规划的泊车路径存在控制点难以寻找、路径生成算法较复杂及无法实现对移动障碍物的有效避障等问题，且当实际轨迹与规划轨迹出现误差后对实际轨迹进行动态调整较困难；用几何作图方法进行路径规划的方式适用范围小，不灵活，且存在曲率突变等问题；基于图的搜索算法以常见的二维栅格环境模型为例，使用如迪杰斯特拉算法进行路径搜索所耗费的时间远远大于其他启发式搜索算法。如果将二维模型的分辨率设置的更精细，则由于搜索空间的增大，算法的实时性方面将面临更大的挑战。故准确灵活与简单高效不能同时在泊车轨迹规划上得到满足。

Dmitri Dolgov等在文献【"Practical search techniques in path planningfor au-tonomous driving."】中提出了一种基于Hybrid A*算法的无人车路径规划方法，该基于图的搜索技术在解决二维空间自主泊车规划问题时，一般考虑车辆运动学模型，将自由度约束为车辆前进或倒退、左转或右转的组合情况，以此来限制规划出的行驶路径一定是满足车辆运动学约束，从而保证路径的可行性。但是，这种方法的不足也是显而易见的，目前，现有方法在进行当前节点的扩展时，需要遍历周围符合条件的待访问栅格节点，探索时只关注该节点的占用情况，往往忽略待访问节点周围的环境情况，因此为保障安全还需要事先考虑车辆的实际尺寸并与先验信息中的障碍物以及道路边界等硬约束信息逐一进行碰撞检测，步骤比较繁琐，导致算法效率低下，实时性降低。

发明内容

为克服上述缺陷，本发明针对目前以Hybrid A*算法为核心的自主泊车规划方法中节点扩展数量多、缺少方向性信息、收敛速度慢等问题，提出一种以人工势场信息引导节点的扩展方式，以此来将两种方法结合得到一种改进的规划算法。

本发明采用以下技术方案实现：

一种基于人工势场引导的Hybrid A*自主泊车路径规划方法，针对先验地图分区域构建人工势场图，然后将人工势场图和车辆实时位姿信息作为输入，以保持车身与势场方向相同的原则确定Hybrid A*算法节点扩展的整体方向，生成可行驶路径。

上述技术方案中，进一步地，采用流场理论分区域对栅格地图构建人工势场图。

进一步地，所述的针对先验地图分区域构建人工势场图具体为：根据地图信息获取感兴趣区域，利用区域边界将泊车区域分为入库区和道路区两类，并据此确定人工势场建立策略。对于入库区，以最终位姿作为目标虚拟点计算人工势场；对于道路区域，以道路区中心与入库区边界之间的交点为目标虚拟点计算人工势场。

进一步地，所述的确定HybridA*算法节点扩展的整体方向过程中，对节点的扩展进行剪枝操作，具体为：

根据车辆当前航向确定节点扩展的趋势，当前栅格地图中节点的车辆航向与人工势场方向之间角度误差的计算公式如下：

E_ij＝θ-G_ij

其中，θ表示当前栅格地图中节点的车辆航向，θ∈[-π，+π]；i，j分别表示栅格地图中节点的横坐标、纵坐标；E_ij为角度误差；G_ij为栅格地图中节点的势场方向；

首先需要保证|E_ij|＜π，当|E_ij|＞π，若G_ij＞0，E_ij＝E_ij+π，若G_ij＜0时，E_ij＝E_ij-π；

设置阈值δ来表示车身和人工势场方向之间角度误差的容忍度，具体的剪枝策略如下：

当

时，若E_ij＞0，此时节点向右前方和左后方扩展，否则节点向左前方和右后方扩展；

当

时，若E_ij＞0，此时节点向左前方和右后方扩展，否则节点向右前方和左后方扩展。

本发明方法将人工势场图和车辆实时位姿信息作为输入，通过Hybrid A*算法扩展带有车辆运动学约束的可行驶路径节点，同时结合人工势场方向信息确定车辆当前航向下的行驶趋势，对节点的扩展进行剪枝操作，可以实现在访问较少节点的同时生成可供车辆行驶的路径。

本发明的有益效果是：

本发明提出一种以人工势场信息引导节点的扩展方式，人工势场信息可以引导车辆避开障碍物、道路边界等区域，同时指引车辆行驶至安全、空旷的区域，在此情况下，当前节点和目标点之间生成的Reeds-Shepp曲线能够有较大的可能性通过碰撞检测，提前结束整个规划算法流程。同时结合人工势场先验信息可以有效对Hybrid A*算法进行剪枝，扩展节点数明显减少，降低了无效节点参与的计算，计算效率更高，内存需求更小。本发明方法在保证最优路径规划的同时可以更加有效地提升算法运行时间，避免Hybrid A*算法在节点扩展过程中存在的缺少指向性造成的冗余计算的问题，进而在算法生成安全、可靠路径的同时，提升算法的实时性。

附图说明

图1是本发明基于人工势场引导的Hybrid A*自主泊车路径规划方法的流程图；

图2是本发明关于泊车区域栅格图建立的人工势场图；

图3是本发明的Hybrid A*算法节点的扩展原理，其中，(a)图是在未经过人工势场引导的车辆运动学约束下扩展的细节，(b)图表示车身经过人工势场引导的车辆运动学约束下扩展的细节，在扩展过程中进行剪枝操作；

图4是两种方法在标准的垂直泊车情景所规划的路径结果，其中(a)图表示无人工势场下得到的规划结果，(b)图表示本发明基于人工势场得到的规划结果。

具体实施方式

下面结合附图对本发明具体实施方式做进一步说明。

如图1所示，本发明提供的一种基于人工势场引导的Hybrid A*自主泊车路径规划方法，具体实施方式如下：

(1)自主泊车规划系统根据事先采集的地图获取停车区域的关键点信息、通过相关传感器获取车辆起始位姿(X_s，Y_s，θ_s)，本实例设置声

并根据停车位信息计算求解最终位姿(X_e，Y_e，θ_e)，本实例解算为

其中X_s，Y_s，θ_s分别为车辆当前位置的横坐标，纵坐标和航向；X_e，Y_e，θ_e分别为车辆最终位置的横坐标，纵坐标和航向。

(2)根据应用场景以及障碍物大小设置栅格地图分辨率为L，将边长为L的方格作为泊车规划二维空间栅格地图的基本组成单元，其中心点来表示该方格，方格的空间位置即为中心点的二维坐标，也即后续算法中的扩展节点。障碍物不足一个栅格的填充为一个栅格处理，最终得到包含障碍物的栅格地图M，本实施例地图大小设置为27mX20m的区域，栅格尺寸设置为边长为L＝0.3m的方形。

(3)根据泊车区域生成的栅格图建立人工势场图，每个栅格节点的势场指引方向为节点扩展的参考方向。流函数法首先在规划空间中引入某种流动的流势场，原流势场中若存在障碍物，则可以通过建立障碍物的流势场并叠加至原流势场得到总的流势场，速度势的计算公式如下：

其中，x，y分别表示栅格地图中节点的横坐标、纵坐标，C是流势场强度，本实施例设置C＝1，a为障碍物的等效半径，b_x，b_y为障碍物中心在栅格地图中的横坐标、纵坐标。

速度势函数分别对x、y求偏导，可得x、y方向的速度分量为：

其中：

m＝a²(y-b_y)+b_y[(x-b_x)²+(y-b_y)²]

n＝a²(x-b_x)+b_x[(x-b_x)²+(y-b_y)²]

每一个栅格节点流势场G_ij的计算如公式如下：

其中i，j分别表示栅格的横坐标、纵坐标；G_ij为栅格地图中节点的势场方向。

(4)栅格地图中节点的势场方向表示车辆在当前节点按照该指引信息的行驶方向可以安全、可靠地行驶至下一个节点。为了使车辆能够沿着势场方向行驶，Hybrid A*算法在节点扩展过程中遵循车身向势场方向靠近的原则。将每个栅格地图节点的势场方向作为参考，根据车辆当前航向确定节点扩展的趋势，当前栅格地图节点的车辆航向与势场方向之间角度误差的计算公式如下：

E_ij＝θ-G_ij

其中，θ表示当前栅格地图中节点的车辆航向，θ∈[-π，+π]；i，j分别表示栅格地图中节点的横坐标、纵坐标；E_ij为角度误差；G_ij为栅格地图中节点的势场方向；

设置阈值δ来表示车身和势场方向之间角度误差的容忍度，当车身方向几乎和势场方向平行或者几乎垂直时，无法确定节点扩展的方向，此时，应考虑周围所有可能的节点加入计算，本实施例设置阈值

具体的判定策略如下：

当

时，若E_ij＞0，此时节点向右前方和左后方扩展，否则节点向左前方和右后方扩展；

当

时，若E_ij＞0，此时节点向左前方和右后方扩展，否则节点向右前方和左后方扩展。

实施例1

本实施例的应用场景设置为标准的垂直泊车情景，主要对比分析了加入人工势场引导信息前后泊车路径规划算法的性能。本实施例主要包括如图2所示的人工势场图的建立、如图3所示的Hybrid A*算法扩展细节等，规划结果如图4所示，主要的性能指标分析见表1，其中，Total Time指路径规划的总体耗时，node个数是Hybrid A*算法在生成最终路径结果过程中所访问的节点数目，RS无效连接次数指算法在每一次循环尝试使用Reeds-Shepp曲线连接当前节点和目标点，但是没有通过碰撞检测，导致此次运算无效的总次数，通过RS time可以看出，无效连接次数越少，算法时间就越短。Path是指最终路径在二维空间中的总长度。可以发现，一方面参与计算的节点数目减少，另一方面由于本方法利用了人工势场的先验信息，引导车辆驶向更开阔区域，生成的Reeds-Shepp曲线通过无碰撞检测的概率增加，因此提升了算法的实时性，能得到鲁棒、灵活、高质量的较优路径。

表1

	node(个)	RS无效连接(次)	RS time(s)	Total time(s)	Path(m)
						无势场	2734	1578	0.13	0.22	23.4
有势场	1903	1139	0.09	0.15	23.1

以上所述的具体实施方式对本发明的技术方案和有益效果进行了详细说明，应理解的是以上所述仅为本发明的优选实施例子，并不用于限制本发明，凡在本发明的原则范围内所做的任何修改、补充和等同替换等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种基于人工势场引导的Hybrid A*自主泊车路径规划方法，其特征在于，针对先验地图分区域构建人工势场图，然后将人工势场图和车辆实时位姿信息作为输入，以保持车身与势场方向相同的原则确定Hybrid A*算法节点扩展的整体方向，生成可行驶路径。

2.根据权利要求1所述的基于人工势场引导的Hybrid A*自主泊车路径规划方法，其特征在于，采用流场理论分区域对栅格地图构建人工势场图。

3.根据权利要求1所述的基于人工势场引导的Hybrid A*自主泊车路径规划方法，其特征在于，所述的针对先验地图分区域构建人工势场图具体为：根据地图信息获取感兴趣区域，利用区域边界将泊车区域分为入库区和道路区两类；对于入库区，以最终位姿作为目标虚拟点计算人工势场；对于道路区，以道路区中心与入库区边界之间的交点为目标虚拟点计算人工势场。

4.根据权利要求1所述的基于人工势场引导的Hybrid A*自主泊车路径规划方法，其特征在于，所述的确定Hybrid A*算法节点扩展的整体方向过程中，对节点的扩展进行剪枝操作，具体为：

根据车辆当前航向确定节点扩展的趋势，当前栅格地图中节点的车辆航向与人工势场方向之间角度误差的计算公式如下：

E_ij＝θ-G_ij

其中，θ表示当前栅格地图中节点的车辆航向，θ∈[-π，+π]；i，j分别表示栅格地图中节点的横坐标、纵坐标；E_ij为角度误差；G_ij为栅格地图中节点的势场方向；

首先需要保证|E_ij|＜π，当|E_ij|＞π，若G_ij＞0，E_ij＝E_ij+π，若G_ij＜0时，E_ij＝E_ij-π；

设置阈值δ来表示车身和人工势场方向之间角度误差的容忍度，具体的剪枝策略如下：

当

时，若E_ij＞0，此时节点向右前方和左后方扩展，否则节点向左前方和右后方扩展；

当

时，若E_ij＞0，此时节点向左前方和右后方扩展，否则节点向右前方和左后方扩展。

Images