CN115454068A - 一种考虑障碍物信息的基于采样的路径规划方法 - Google Patents

Abstract

本发明公开了一种加入障碍物信息的基于采样的路径规划方法。本发明初次在空间中采样障碍物附近的状态点，将邻近障碍物的采样点相连后再构建基于障碍物的路径地图，在路径地图上执行基于搜索的路径规划方法获取初始路径；以初始路径为基础在周围分段构建启发式采样集，再次随机采样，然后在二次采样点集上通过快速行进树方法搜索出一条最优路径。本发明有效解决了快速行进树方法采样点数过多导致运行速度慢的问题，提供了一种快速且最优的启发式采样与搜索方法，从而有效提升了路径规划的整体速度与最优性。

Description

一种考虑障碍物信息的基于采样的路径规划方法

技术领域

本发明涉及一种路径规划方法，尤其涉及一种基于障碍物信息的基于采样的路径规划方法。

背景技术

近年来自动驾驶逐渐走进大众视野，不管是传统汽车制造企业还是智能驾驶造车新势力，都投入了大量精力研究辅助驾驶和自动驾驶技术。自动驾驶的关键技术包括感知、建图、决策、规划和控制，其中规划模块接收上游感知、建图与决策的信息规划出可行轨迹并交由控制模块执行，是自动驾驶的核心技术之一。

路径规划方法可分为三种类别：其一是基于图搜索的方法，如A*方法、Dijkstra方法等；其二是基于采样的方法，如快速随机树(RRT)方法、概率地图方法(PRM)和状态格规划方法(State Lattice)；近些年随着深度学习的发展，基于学习的路径规划方法也逐渐受到关注。其中基于图搜索的方法由于其对栅格地图的依赖，在高维空间及非结构化空间中的搜索速度较慢，而基于学习的方法则需要大量数据集和训练开销，当前路径规划领域并无通用的训练集可供网络训练。基于采样的路径规划方法得益于其在高维空间中的快速搜索能力，被广泛应用于无人驾驶车辆、无人机等规划中。

然而基于采样的路径规划方法依然存在一些问题，如RRT方法并非渐进最优，仅可以保证在有解的情况下搜索到可行路径；RRT*方法，虽然保证了随着采样点数量增加和规划批次增加的最优性，但其规划速度不甚理想；快速行进树方法(FMT*)对初始采样点质量有很强的依赖，采样点不足时难以搜索到满意的路径，而采样点数量过多则会造成搜索速度严重受限。

发明内容

有鉴于此，为了解决背景技术中存在的问题，本发明的目的在于解决快速行进树方法对初始采样点质量的依赖与不确定问题，提出一种考虑障碍物信息的基于采样的路径规划方法，基于障碍物进行第一次采样，获得第一批采样集，基于第一批采样集建立路径地图并搜索初始路径，利用初始路径建立启发式采样集，获得第二批采样集，实现可控的采样点数量与范围，从而更加快速准确地搜索出最优路径。

为实现以上目的，本发明采用的技术方案如下：

步骤1：在已知障碍物位置、起始点与目标点的空间中，在该空间的各个障碍物边界附近进行随机采样，获得各个障碍物边界的采样点并记为第一批采样集；

步骤2：利用K近邻方法对第一批采样集中不同障碍物边界的采样点进行采样点连接后，获得当前空间的路径地图；

步骤3：根据当前空间的起始点与目标点，利用基于搜索的路径规划方法对当前空间的路径地图进行路径规划，获得初始路径；

步骤4：将初始路径分段后再在初始路径周围依次分段构建启发式采样区域，获得多个启发式采样区域，相邻两个启发式采样区域的范围前后部分重叠；

步骤5：在各个启发式采样区域内分别随机采样并由所有采样点构成第二批采样集；

步骤6：根据第二批采样集，利用快速行进树方法在初始路径的基础上进行采样点搜索，当搜索到目标点后，则连接快速行进树方法中每步选取的采样点后得到目标路径；否则，则重新执行快速行进树算法，直至得到目标路径。

所述步骤1中，障碍物边界附近具体为每个障碍物的边界至距离当前障碍物边界0.5m之间的空间组成的边界环，边界环不包含障碍物所在的空间。

所述步骤3中，基于搜索的路径规划方法为A*方法。

所述步骤4中，根据第一批采样集的采样点，将初始路径按采样点数量进行分段，获得各个局部路径，每个局部路径的采样点数量为3个，相邻两个局部路径之间的采样点至少重合2个；对每个局部路径构建对应的启发式采样区域，启发式采样区域为椭圆，第一个采样点和最后一个采样点作为椭圆的两个焦点，当前局部路径的总路径长度作为焦距。

所述步骤6中，快速行进树方法的搜索半径R的确定公式如下：

其中，r_n表示第n个采样集上的搜索半径，σ为搜索半径控制系数，d为规划问题的维度，μ(X_free)为可通行区域的隆贝格测度，ζ_d为d维空间中的单位球体/圆的体积/面积，n为第二批采样集中对应启发式采样区域内的采样点数量。

本发明的有益效果为：

(1)本发明结合了单批次采样方法和多批次采样方法的思想，分两批采样构型空间，克服了单批次采样过度依赖初始采样点集质量的缺点，以及避免了反复多批次采样造成的时间与计算空间的浪费，从而提高了路径搜索的计算速度；

(2)本发明借助第一批采样点集构建的初始路径地图使用图搜索方法建立初始解，为基于采样的路径规划方法提供了一种有效的启发式策略，不同于传统的启发式策略，由图搜索方法引导的初始解有着更好的收敛性，即可以快速利用采样方法搜索到最优解，同时此方法可以拓展到其他基于采样的路径规划方法中。

附图说明

图1是本发明的系统流程图。

图2是本发明与现有方法在最优路径上的实验结果对比图。

图3是本发明与现有方法在计算时间上的实验结果对比图。

具体实施方式

下面结合本发明实施例中的附图，详细描述本发明的实施过程。

如图1所示，本发明包括以下步骤：

步骤1：在已知障碍物位置、起始点与目标点的空间中，在该空间的各个障碍物边界附近进行随机采样，获得各个障碍物边界的采样点并记为第一批采样集；具体实施中，空间为300×300㎡仿真区域。障碍物边界附近具体为每个障碍物的边界至距离当前障碍物边界0.5m(不包含障碍物所在的空间)之间的空间组成的边界环。

步骤2：利用K近邻方法对第一批采样集中不同障碍物边界的采样点进行采样点连接后，获得当前空间的路径地图；具体实施中，K近邻方法中的K为5。

步骤3：根据当前空间的起始点与目标点，利用基于搜索的路径规划算法对当前空间的路径地图进行路径规划，获得初始路径；

步骤3中，基于搜索的路径规划算法为A*算法。步骤4：将初始路径分段后再在初始路径周围依次构建启发式采样区域，获得多个启发式采样区域，相邻两个启发式采样区域的范围前后部分重叠；A*方法的启发函数为：

F(n)＝H(n)+G(n)

其中，F(n)表示每一个采样点的代价函数，G(n)表示初始点到采样点的实际代价值，H(n)表示采样点到目标点的启发式估计代价值。

步骤4：将初始路径分段后再在初始路径周围依次分段构建启发式采样区域，获得多个启发式采样区域，相邻两个启发式采样区域的范围前后部分重叠；

步骤4中，根据第一批采样集的采样点，将初始路径按采样点数量进行分段，获得各个局部路径，每个局部路径的采样点数量为3个，相邻两个局部路径之间的采样点至少重合2个；对每个局部路径构建对应的启发式采样区域，启发式采样区域为椭圆，第一个采样点和最后一个采样点作为椭圆的两个焦点，当前局部路径的总路径长度作为焦距。

启发式采样区域定义为：

S_i＝{x∈X_free|‖x-x_start‖₂+‖x-x_goal‖₂<c_i}

其中，S_i表示第i个启发式采样区域，x表示地图中的状态点，X_free表示当前空间中的可通行区域，x_start和x_goal分别为局部路径的起点和终点，c_i表示当前局部路径的起点(即第一个采样点)到终点(即第三个采样点)的代价值，即路径距离，也为总路径长度。

步骤5：在各个启发式采样区域内分别随机采样并由所有采样点构成第二批采样集；每个启发式采样区域在采样中的采样密度ρ的公式满足：

n＝ρ*S

其中，S为当前启发式采样区域所覆盖面积，n表示当前启发式采样区域的采样点数量。

步骤6：根据第二批采样集，利用快速行进树算法在初始路径的基础上进行采样点搜索，当搜索到目标点后，则连接快速行进树算法中每步选取的采样点后得到目标路径；否则，则重新执行快速行进树算法，直至得到目标路径。

步骤6中，快速行进树算法的搜索半径R的确定公式如下：

其中，r_n表示第n个采样集上的搜索半径，σ为搜索半径控制系数，为一正常数，d为规划问题的维度，μ(X_free)为可通行区域的隆贝格测度，ζ_d为d维空间中的单位球体/圆的体积/面积，n为第二批采样集中对应启发式采样区域内的采样点数量。

具体实施中，设计300m×300m的二维地图以供实验，对于每个实验结果，选取十次实验的平均值作为最终结果，规划结果的评判标准为搜索时间、路径长度以及最优性，其中最优性的定义为路径长度与理论最优路径长度之比。实验结果如表1所示，各方法的效果随采样密度的变化如图2和图3所示。

表1本发明与现有方法的实验结果对比表

Claims (5)

1.一种加入障碍物信息的基于采样的路径规划方法，其特征在于，包括以下步骤：

步骤1：在已知障碍物位置、起始点与目标点的空间中，在该空间的各个障碍物边界附近进行随机采样，获得各个障碍物边界的采样点并记为第一批采样集；

步骤2：利用K近邻方法对第一批采样集中不同障碍物边界的采样点进行采样点连接后，获得当前空间的路径地图；

步骤3：根据当前空间的起始点与目标点，利用基于搜索的路径规划方法对当前空间的路径地图进行路径规划，获得初始路径；

步骤4：将初始路径分段后再在初始路径周围依次分段构建启发式采样区域，获得多个启发式采样区域，相邻两个启发式采样区域的范围前后部分重叠；

步骤5：在各个启发式采样区域内分别随机采样并由所有采样点构成第二批采样集；

步骤6：根据第二批采样集，利用快速行进树方法在初始路径的基础上进行采样点搜索，当搜索到目标点后，则连接快速行进树方法中每步选取的采样点后得到目标路径；否则，则重新执行快速行进树算法，直至得到目标路径。

2.根据权利要求1所述的一种加入障碍物信息的基于采样的路径规划方法，其特征在于，所述步骤1中，障碍物边界附近具体为每个障碍物的边界至距离当前障碍物边界0.5m之间的空间组成的边界环，边界环不包含障碍物所在的空间。

3.根据权利要求1所述的一种加入障碍物信息的基于采样的路径规划方法，其特征在于，所述步骤3中，基于搜索的路径规划方法为A*方法。

4.根据权利要求1所述的一种加入障碍物信息的基于采样的路径规划方法，其特征在于，所述步骤4中，根据第一批采样集的采样点，将初始路径按采样点数量进行分段，获得各个局部路径，每个局部路径的采样点数量为3个，相邻两个局部路径之间的采样点至少重合2个；对每个局部路径构建对应的启发式采样区域，启发式采样区域为椭圆，第一个采样点和最后一个采样点作为椭圆的两个焦点，当前局部路径的总路径长度作为焦距。

5.根据权利要求1所述的一种加入障碍物信息的基于采样的路径规划方法，其特征在于，所述步骤6中，快速行进树方法的搜索半径R的确定公式如下：

其中，r_n表示第n个采样集上的搜索半径，σ为搜索半径控制系数，d为规划问题的维度，μ(X_free)为可通行区域的隆贝格测度，ζ_d为d维空间中的单位球体/圆的体积/面积，n为第二批采样集中对应启发式采样区域内的采样点数量。

Images