CN111811514A - 一种基于正六边形栅格跳点搜索算法的路径规划方法 - Google Patents

Abstract

本发明公开了一种基于正六边形栅格跳点搜索算法的路径规划方法，包括以下步骤：对路径规划区域进行正六边形栅格化建模，根据顶角型正六边形栅格法建模下的四叉树搜索策略，将六种当前节点方向顺时针依次定义为东、东南、西南、西、西北、东北方向。本发明采用了正六边形栅格建模下的四叉树策略进行路径规划，使得智能体在路径规划过程中寻路方向多且转角角度小，规划路径短，且在考虑智能体运动约束情况下更符合实际运动约束。本发明采用了正六边形栅格建模下四叉树搜索策略进行路径规划，使得智能体在路径规划过程中节点扩展更为均衡，处理邻域关系时更具优势，可加快路径规划速度。本发明所得到的规划路径更为安全合理。

Description

一种基于正六边形栅格跳点搜索算法的路径规划方法

技术领域

本发明涉及智能体路径规划技术领域，具体涉及一种基于正六边形栅格跳点搜索算法的路径规划方法。

背景技术

随着现代科技的飞速发展，无人车、无人机、无人船等智能体应运而生，并被广泛应用于各个领域。智能体的路径规划技术是实现其自主导航的关键技术之一，也是其智能化水平的体现。智能体的路径规划旨在根据当前的任务需求，规划出一条从起始点至目标点的无碰撞、安全合理的路径，主要包括环境建模和路径规划两部分内容。

中国专利CN 107687859 A公开了一种基于A*算法的最短寻路方法，在对路径规划区域进行栅格化建模后，采用将启发式函数BFS和Dijkstra算法相结合的A*算法进行路径规划，使得规划路径最短并且速度快于Dijkstra算法，但 A*算法存在寻路过程中需要拓展大量冗余节点、路径规划效率不高的问题，上述方法也未考虑智能体的运动约束。

中国专利CN 110567477 A公开了一种基于改进A*算法的路径规划方法，该方法在路径生成步骤中采用消除共线节点和消除冗余转折点的方法优化路径，减少了路径代价和搜索时间，加快了路径规划进程。

中国专利CN 110487279 A公开了一种基于改进A*算法的机器人路径规划方法，该方法在A*算法基础上合并路径中的关键拐点，然后利用三次样条插值平滑路径，改进后算法规划所得路径长度更短、整体更平滑，并能够符合非完整型机器人运动约束。

中国专利CN 105955280 A公开了一种基于跳点搜索算法的移动机器人路径规划方法，跳点搜索算法改变了A*算法的邻居节点的选取规则，并采用搜索跳点的策略拓展节点，避免了对冗余节点的操作，极大提高了A*算法的路径规划效率。

但上述技术在环境建模部分均采用结构简单、计算量小的传统正方形栅格法建模。正方形栅格法建模下的路径搜索策略有四叉树和八叉树策略，四叉树策略搜索方向经过栅格中心点，八叉树策略搜索方向既经过栅格中心点，又经过栅格顶点。大幅度提高A*算法路径规划效率的传统跳点搜索算法使用的是八叉树搜索策略，存在节点拓展缺乏平衡性的问题，会影响到路径规划的速度；同时在避开障碍物的路径规划过程中存在穿越墙角的行为，从而会影响到最终规划路径的安全性与合理性。

发明内容

为了解决现有技术存在的上述问题，本发明要设计一种应用于智能体路径规划后寻得的路径更为安全、合理，同时寻路速度更快的基于正六边形栅格跳点搜索算法的路径规划方法。

为了实现上述目的，本发明的技术方案如下：

一种基于正六边形栅格跳点搜索算法的路径规划方法，包括以下步骤：

A、对路径规划区域进行正六边形栅格化建模，确定路径规划的起点和目标点所在栅格；

首先确定智能体要进行路径规划的区域，以区域左上角点的经纬度坐标为原点构建全局坐标系，并根据设定的正六边形栅格的边长以及顶点朝上的正六边形的类型，确定每个正六边形栅格中心点的经纬度坐标，从而完成对路径规划区域的正六边形栅格划分；利用Python Shapely库中的Polygon相关函数判断障碍物是否与正六边形栅格相交，若相交，则该正六边形栅格为障碍物栅格，否则为无障碍栅格；利用Python Shapely库中的Polygon和Contains相关函数依次判断每个正六边形栅格是否包含起点和目标点的经纬度坐标，从而确定起点和目标点具体所在的正六边形栅格；

B、创建OPEN列表，用于存放未进行比较的跳点节点，创建CLOSE列表，用于存放构成最终规划路径的节点；

在Python程序中创建一个空列表，定义为OPEN列表，存放的跳点节点是伴有强制邻居的节点；其中强制邻居是指计算栅格地图中某一节点x的父节点到x的目标邻居节点距离时，若节点x的邻居节点为障碍物栅格，则影响其父节点不经过节点x到达目标邻居节点的行走距离，则节点x具有强制邻居，同时节点x为跳点节点；

C、将起点作为初始跳点放入OPEN列表中；

D、取出OPEN列表中代价函数F(x)值最小的跳点，并将其设置为当前节点；

代价函数F(x)的计算公式为：

F(x)＝G(x)+H(x)

式中，F(x)为对于节点x从起点到目标点的代价估计，G(x)为从起点到节点 x的实际代价，H(x)为节点x到目标点的估计代价，节点x是OPEN列表中未进行比较的跳点节点；

E、判断当前节点是否为目标点，若是，寻路结束；若不是，在OPEN列表中删除当前节点，并在CLOSE列表加入当前节点；

F、判断当前节点是否存在父节点，若存在，则定义其父节点指向当前节点的方向为当前节点方向；若不存在，当前节点为起点，依次取当前节点指向可达邻居节点的方向为当前节点方向；

根据选择的顶角型正六边形栅格法建模下的四叉树搜索策略，当前节点的方向共有六种，将六种当前节点方向顺时针依次定义为东、东南、西南、西、西北、东北方向；

G、判断当前节点方向是否为单方向，若是，则沿单方向按照搜索规则递归寻找伴有强制邻居的跳点；若不是，在双方向的两个单方向上按照规则递归寻找伴有强制邻居的跳点，并在双方向上递归这一过程；具体方法如下：

为确保地图栅格化建模后所有正六边形栅格只被遍历一次，定义东、西南和西北方向为单方向搜索，定义东南、西和东北方向为双方向搜索，每个双方向搜索包含两个单方向搜索，即东南方向包括东和西南方向，西方向包括西南和西北方向，东北方向包括东和西北方向。单方向上寻找跳点节点的过程是通过递归判断单方向上每个节点是否存在强制邻居而完成的，单方向上搜索强制邻居节点的规则为：以当前节点的方向为正，若当前节点正前方为无障碍栅格，沿当前方向递归寻找具有强制邻居的跳点；若当前节点左后方为障碍物栅格且左前方为无障碍栅格，沿当前方向和左前方方向寻找具有强制邻居的跳点；若当前节点右后方为障碍物栅格且右前方为无障碍栅格，沿当前方向和右前方方向寻找具有强制邻居的跳点。双方向上寻找跳点的过程是在其两个单方向上按照搜索规则递归地搜索具有强制邻居节点的跳点，并在双方向上递归这一过程，同时将搜索到跳点的双方向上的节点作为中间跳点返回；

H、将搜索到的跳点加入OPEN列表，并将当前节点设为搜索到的跳点的父节点，回到步骤D。

与现有技术相比，本发明具有以下有益效果：

1.正方形栅格建模下的四叉树策略寻路方向为4个，转向改变角度为90度，正六边形栅格建模下的四叉树策略寻路方向为6个，转向改变角度为60度。由于本发明采用了正六边形栅格建模下的四叉树策略进行路径规划，使得智能体在路径规划过程中寻路方向多且转角角度小，规划路径短，且在考虑智能体运动约束情况下更符合实际运动约束。

2.传统正方形栅格建模下的跳点搜索算法采用八叉树策略进行路径规划，其对角线方向的遍历成本为1.414单位，大于水平或垂直方向的1单位的遍历成本；正六边形栅格建模下四叉树搜索策略的跳点搜索算法中正六边形的中心至其邻居节点的中心距离一致，可看作一个单位。由于本发明采用了正六边形栅格建模下四叉树搜索策略进行路径规划，使得智能体在路径规划过程中节点扩展更为均衡，处理邻域关系时更具优势，可加快路径规划速度。

3.传统正方形栅格建模下的跳点搜索算法采用八叉树策略进行路径规划，在绕过障碍物时存在穿越墙角的不安全行为。由于本发明采用了正六边形栅格建模下四叉树搜索策略的跳点搜索算法进行路径规划，使得智能体路径规划过程中的搜索方向不会经过栅格的顶点，避免了移动智能体在避开障碍物时的穿越墙角行为，所获得的最终规划路径相比正方形栅格建模下的跳点搜索算法所得到的规划路径更为安全合理。

附图说明

图1为基于正六边形栅格跳点搜索算法的路径规划方法流程图。

图2为正六边形栅格中顶角型正六边形以及六个寻路方向定义示意图。

图3为无障碍物情况下正六边形栅格跳点搜索算法的节点遍历方式示意图。

图4为正六边形栅格跳点搜索算法中强制邻居的示意图。

图5为正六边形栅格跳点搜索算法中单方向搜索因强制邻居而搜索方向发生变化的示意图。

图6为有障碍物情况下正六边形栅格跳点搜索算法的节点遍历方式示意图。

图7为正六边形栅格跳点搜索算法应用于寻路实例图。

具体实施方法

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实例对本发明进行进一步详细说明：

基于正六边形栅格跳点搜索算法的路径规划方法，通过对传统正方形栅格建模下的跳点搜索算法进行拓展优化，使得其应用于智能体路径规划后寻得的路径更为安全、合理，同时寻路速度更快。

图1为基于正六边形栅格跳点搜索算法的路径规划方法的整体实施流程。图2是本发明正六边形栅格建模选择的顶角型正六边形以及四叉树寻路策略下正六边形的六个寻路方向的示意图。定义东、西南和西北方向为单方向搜索，定义东南、西和东北方向为双方向搜索。首先对路径规划区域进行正六边形栅格化建模，确定起点、目标点所在栅格，并将起点作为初始跳点放入OPEN列表中。其次取出OPEN列表中代价函数F(x)值最小的跳点，将其设置为当前节点，并判断当前节点是否为目标点，如果是，寻路结束；如果不是，OPEN列表删除当前节点，CLOSE列表加入当前节点。然后判断当前节点是否存在父节点，若不存在，当前节点为起点，依次取起点指向可达邻居节点的方向为当前节点方向；若存在，则定义其父节点指向当前节点的方向为当前节点方向。继而判断当前节点方向是否为单方向，若是，则沿单方向按搜索规则递归寻找伴有强制邻居的跳点；若不是，在双方向的两个单方向上按搜索规则递归寻找伴有强制邻居的跳点，并在双方向上递归这一过程，同时将搜索到跳点的双方向上的节点作为中间跳点返回。最后将搜索到的跳点和中间跳点加入OPEN列表，并将当前节点设为搜索到的跳点的父节点，回到取出OPEN列表中代价函数F(x) 值最小跳点的步骤。

上述流程中寻找跳点实现的两个重要步骤是正六边形栅格下的遍历方式以及邻居剪枝和强制邻居节点判断。图3是在正六边形栅格建模地图中无障碍物情况下跳点搜索算法的遍历方式。该遍历方式达到了和正方形栅格建模下跳点搜索算法同样的遍历效果，即对每个节点只访问一次，避免了传统A*算法中的大量节点被重复遍历的缺点，大大提高了寻路效率。

图4是正六边形栅格跳点搜索算法中单方向邻居剪枝规则以及强制邻居的示意图。对于节点x，父节点为节点6，则其当前方向为父节点6指向节点x的方向，也就是单方向中的东方向。定义路径1是父节点p经过x到邻居n的最短路径，路径2是p不经过x到n的最短路径。对满足以下条件的邻居n进行修剪：单方向上，路径1大于等于路径2；双方向上，路径1大于路径2。考虑左图中的邻居2，经过节点x的为路径1，不经过节点x的为路径2，因为路径1等于路径2，所以邻居2被剪枝，同理可得1、4、5均被剪枝，只有邻居3被考虑，这是无障碍物情况下邻居节点的剪枝方式。当节点x的邻居节点为障碍物栅格时，会产生强制邻居，节点x也因为强制邻居的发现而变为跳点。如右图中邻居6为父节点p，邻居1变为障碍物栅格，此时考虑邻居2，经过节点x的为路径1，不经过节点x的为路径2，因为路径1小于路径2，所以邻居2在这种情况下不会被修剪，定义这样的邻居节点为强制邻居，同时节点x因为强制邻居节点2的存在变为跳点。上述就是强制邻居的判断规则。

图5所示为正六边形栅格跳点搜索算法单方向上所有的跳点及强制邻居的情况，双方向上不直接寻找跳点，而是转为在其两个单方向上搜索跳点，值得注意的是，在发现强制邻居找到跳点之后，原本的单方向搜索后续会变为双方向搜索，也就是跳点会引起遍历搜索方向的变化，这也是单方向上搜索跳点的搜索规则，即以当前节点的方向为正，若当前节点正前方为无障碍栅格，沿当前方向递归寻找具有强制邻居的跳点；若当前节点左后方为障碍物栅格且左前方为无障碍栅格，沿当前方向和左前方方向寻找具有强制邻居的跳点；若当前节点右后方为障碍物栅格且右前方为无障碍栅格，沿当前方向和右前方方向寻找具有强制邻居的跳点。该搜索规则使得在有障碍物情况下跳点搜索算法依然可以只一次遍历地图中的所有正六边形栅格。

图6为有障碍物情况下正六边形栅格跳点搜索算法的遍历方式。如图5所示，首先将起点节点S作为初始跳点加入OPEN列表并作为F(x)值最小的跳点节点被取出，因为没有父节点，所以根据剪枝规则遍历其6个方向的邻居节点，在西方向的多方向遍历中的西北方向搜索发现了强制邻居N和跳点J1，则将发现强制邻居的西方向上的节点作为一个跳点返回，并将起点定义为该节点的父节点，因为Jm1节点不是伴随强制邻居产生的跳点，本发明将其定义为中间跳点。Jm1作为中间跳点被返回后，加入OPEN列表进行最小值选取，则Jm1被选出并加入CLOSE列表，现在当前节点Jm1的方向为其父节点也就是起点S 指向Jm1的方向，则从Jm1进行西、西北和西南方向进行搜索。在西北搜索方向上又会找到之前发现的强制邻居和跳点，也就是J1，再重复之前对Jm1中间跳点的操作，将J1加入OPEN列表进行最小值选取，需要注意的是，因为J2 是伴随强制邻居产生的跳点，因而从当前方向(父节点Jm1指向J1的方向)的后继遍历搜索方向因此发生变化，即图5(b)中西北方向情况，最后完成此有障碍物正六边形栅格地图的遍历。

图7为本发明的设置目标点的实例图，结合上述流程及规则，通过寻路实例更明了的叙述基于正六边形栅格跳点搜索算法的路径规划方法，当在图6有障碍物的正六边形栅格地图中加入目标点G时，遍历方式与上述介绍的有障碍物环境下正六边形栅格跳点搜索算法的遍历方式一样。目标点也被看作是跳点，则寻路过程以目标跳点找到而结束，依次回溯目标跳点G的父节点直至找到起点跳点S的一系列跳点为最终路径节点，如图7所示，S-Jm1-J1-Jm2-G为最优路径。

本发明不局限于本实施例，任何在本发明披露的技术范围内的等同构思或者改变，均列为本发明的保护范围。

Claims (1)

1.一种基于正六边形栅格跳点搜索算法的路径规划方法，其特征在于：包括以下步骤：

A、对路径规划区域进行正六边形栅格化建模，确定路径规划的起点和目标点所在栅格；

首先确定智能体要进行路径规划的区域，以区域左上角点的经纬度坐标为原点构建全局坐标系，并根据设定的正六边形栅格的边长以及顶点朝上的正六边形的类型，确定每个正六边形栅格中心点的经纬度坐标，从而完成对路径规划区域的正六边形栅格划分；利用Python Shapely库中的Polygon相关函数判断障碍物是否与正六边形栅格相交，若相交，则该正六边形栅格为障碍物栅格，否则为无障碍栅格；利用Python Shapely库中的Polygon和Contains相关函数依次判断每个正六边形栅格是否包含起点和目标点的经纬度坐标，从而确定起点和目标点具体所在的正六边形栅格；

B、创建OPEN列表，用于存放未进行比较的跳点节点，创建CLOSE列表，用于存放构成最终规划路径的节点；

在Python程序中创建一个空列表，定义为OPEN列表，存放的跳点节点是伴有强制邻居的节点；其中强制邻居是指计算栅格地图中某一节点x的父节点到x的目标邻居节点距离时，若节点x的邻居节点为障碍物栅格，则影响其父节点不经过节点x到达目标邻居节点的行走距离，则节点x具有强制邻居，同时节点x为跳点节点；

C、将起点作为初始跳点放入OPEN列表中；

D、取出OPEN列表中代价函数F(x)值最小的跳点，并将其设置为当前节点；

代价函数F(x)的计算公式为：

F(x)＝G(x)+H(x)

式中，F(x)为对于节点x从起点到目标点的代价估计，G(x)为从起点到节点x的实际代价，H(x)为节点x到目标点的估计代价，节点x是OPEN列表中未进行比较的跳点节点；

E、判断当前节点是否为目标点，若是，寻路结束；若不是，在OPEN列表中删除当前节点，并在CLOSE列表加入当前节点；

F、判断当前节点是否存在父节点，若存在，则定义其父节点指向当前节点的方向为当前节点方向；若不存在，当前节点为起点，依次取当前节点指向可达邻居节点的方向为当前节点方向；

根据选择的顶角型正六边形栅格法建模下的四叉树搜索策略，当前节点的方向共有六种，将六种当前节点方向顺时针依次定义为东、东南、西南、西、西北、东北方向；

G、判断当前节点方向是否为单方向，若是，则沿单方向按照搜索规则递归寻找伴有强制邻居的跳点；若不是，在双方向的两个单方向上按照规则递归寻找伴有强制邻居的跳点，并在双方向上递归这一过程；具体方法如下：

为确保地图栅格化建模后所有正六边形栅格只被遍历一次，定义东、西南和西北方向为单方向搜索，定义东南、西和东北方向为双方向搜索，每个双方向搜索包含两个单方向搜索，即东南方向包括东和西南方向，西方向包括西南和西北方向，东北方向包括东和西北方向；单方向上寻找跳点节点的过程是通过递归判断单方向上每个节点是否存在强制邻居而完成的，单方向上搜索强制邻居节点的规则为：以当前节点的方向为正，若当前节点正前方为无障碍栅格，沿当前方向递归寻找具有强制邻居的跳点；若当前节点左后方为障碍物栅格且左前方为无障碍栅格，沿当前方向和左前方方向寻找具有强制邻居的跳点；若当前节点右后方为障碍物栅格且右前方为无障碍栅格，沿当前方向和右前方方向寻找具有强制邻居的跳点；双方向上寻找跳点的过程是在其两个单方向上按照搜索规则递归地搜索具有强制邻居节点的跳点，并在双方向上递归这一过程，同时将搜索到跳点的双方向上的节点作为中间跳点返回；

H、将搜索到的跳点加入OPEN列表，并将当前节点设为搜索到的跳点的父节点，回到步骤D。

Images