CN112683275B - 一种栅格地图的路径规划方法 - Google Patents

Abstract

本发明公开了一种格地图的路径规划方法，包括如下步骤：S1、构建栅格地图，对栅格地图中的障碍物进行膨胀处理；S2、构建拓扑地图，在狭道的出口及入口处设有拓扑点，两个拓扑点均位于狭道的安全区域；S3、基于用Floyd算法获取起始拓扑点至终止拓扑点的拓扑路径点集，删除拓扑路径点集中的起始拓扑点及终止拓扑点，将剩余的拓扑点加入路径点集中；S4、在路径点集的首尾出分别加入任务起点及任务终点，基于A‑stars算法依次规划路径点集中相邻路径点间的路径，生成全局路径点集。解决了Astar算法搜索具有沿墙走导致小车行走的安全系数不高的问题。

Description

一种栅格地图的路径规划方法

技术领域

本发明属于路径规划技术领域，更具体地，本发明涉及一种栅格地图的路径规划方法。

背景技术

作为机电一体化的最高成就，移动机器人涉及到了诸多传感器信息融合、系统工程、自动控制技术等学科领域的各类知识且交叉了众多科学领域技术，在工业、农业、医疗等行业广泛应用，要实现机器人在未知环境下作业，需要具备实时、自主运行的能力，路径规划是一个重要环节，其直接决定移动机器人行驶的路径。路径规划是指在一个静态或者动态、简单或复杂运行的空间中，如何为机器人搜索一条恰当的从起点至目标点的移动路径，并确保机器人在运动过程中能够安全的、无碰地绕过所有障碍物。

现有的基于改进Astar算法的港口无人机巡检路径规划方法，根据港口实际环境建立飞行场地地图，设置飞行起飞及所有任务点；然后采用改进Astar算法进行起点和任务点的路径规划，该方法并未考虑到经过狭道的问题，在通过狭道时，Astar算法搜索出的路径靠近一侧障碍物，存在安全性不高的问题。

发明内容

本发明提供了一种栅格地图的路径规划方法，旨在改善上述问题。

本发明是这样实现的，一种栅格地图的路径规划方法，所述方法具体包括如下步骤：

S1、构建栅格地图，对栅格地图中的障碍物进行膨胀处理；

S2、构建拓扑地图，在狭道的出口及入口处设有拓扑点，两个拓扑点均位于狭道的安全区域；

S3、基于用Floyd算法获取起始拓扑点至终止拓扑点的拓扑路径点集，删除拓扑路径点集中的起始拓扑点及终止拓扑点，将剩余的拓扑路径点加入路径点集中；

S4、在路径点集的首尾出分别加入任务起点及任务终点，基于A-stars算法依次规划路径点集中相邻路径点间的路径，生成全局路径点集。

进一步的，路径点集中相邻路径点间的路径规划方法具体包括如下步骤：

S41、生成一个包含开始节点n0的搜索图G，把n0放在OPEN表中，生成空表CLOSED表；

S42、选择OPEN表上的第一个节点，将其从OPEN表中移入CLOSE表，称该节点为n；

S43、检测节点n是否为目标节点，若检测结果为是，则在搜索图G中基于n到n0的指针找到相邻路径点间的路径，若检测结果为否，则转向步骤S44；

S44、扩展节点n，生成其后继节点集M，节点n的祖先不可在节点集M中，且节点集M中的节点均为可行走的节点；

S45、针对后继节点集M中每个不在搜索图G中的节点，建立一个指向n的指针，针对节点集M中每个已在OPEN表或CLOSE表中的节点m，若到目前为止到达节点m的估价函数f最小值通过节点n，则将节点m的指针修改为指向节点n；

S46、基于估价函数f从小至大的顺序重新排列OPEN表中的节点，并返回步骤S44。

进一步的，估价函数f(n)表示如下：

f(n)＝g(n)+h(n)

g(n)为任务起点到节点n的最短路径值，h(n)为节点n到任务终点的最短路径；

h(n)＝(|N(x)-E(x)|+|N(y)-E(y)|)*a+cross*b；

cross＝|point2(x)*point1(y)-point2(y)*point1(x))|

point1＝S-E；

point2＝N-E；

其中，S表示任务起点在栅格地图中的坐标(S(x)，S(y))；E表示任务终点在栅格地图中的坐标(E(x)，E(y))；N表示节点n在栅格地图中的坐标(N(x)，N(y))；point1(x)、point1(y)分别表示栅格地图下任务起点与任务终点之间的横坐标差值和纵坐标差值；point2(x)、point2(y)分别表示栅格地图下当前节点n与任务终点之间的横坐标差值和纵坐标差值；a为水平移动一个栅格的代价，b为对角线的代价值。

本发明提供的栅格地图的路径规划方法具有如下有益技术效果：

1)通过在狭道的出口及入口处设有拓扑点，且两个拓扑点均位于狭道的安全区域，先利用Floyd算法规划拓扑路径点，并对拓扑路径点进行处理后利用改进后的Astar算法完成整体路径的搜索，解决了Astar算法搜索具有沿墙走导致小车行走的安全系数不高的问题；

2)对A-star算法进行了改进，增加了对角线代价，提高规划的路径质量，减小搜索时间。

附图说明

图1为本发明实施例提供的栅格地图的路径规划方法流程图；

图2为本发明实施例提供的栅格地图及拓扑地图的示意图；

图3为本发明实施例提供的栅格地图路径规划示意图。

具体实施方式

下面对照附图，通过对实施例的描述，对本发明的具体实施方式作进一步详细的说明，以帮助本领域的技术人员对本发明的发明构思、技术方案有更完整、准确和深入的理解。

图1为本发明实施例提供的栅格地图的路径规划方法流程图，该方法具体包括如下步骤：

S1、基于机器人运行构建栅格地图，对栅格地图中的障碍物进行膨胀处理，如图2所示；

栅格地图是机器人借助于二维激光雷达扫描周围环境下建立所在环境的栅格地图，每一个栅格点均具备扫描的栅格值，每个栅格所占据的尺寸为栅格分辨率值，并记录栅格地图中空闲区域、占用区域和未知区域，空闲区域即为激光雷达探测不存在障碍物的区域，占用区域即为激光雷达探测存在障碍物的区域，未知区域即为激光雷达无法探测的区域。

把机器人的车体当作质点后，需要将障碍物信息进行膨胀扩大，为了安全起见，扩大的范围比机器人的车体半径大。建立拓扑模型图是在小车的任务起点、任务终点及狭道的出入口处设置拓扑点，根据实际情况完成拓扑路径的连接，图2中共有7个拓扑点，7条拓扑路径，黑色的为栅格地图中扫到的障碍物，深灰色为根据车体尺寸及栅格分辨率进行膨胀处理。

S2、构建拓扑地图，在狭道的出口及入口处设有拓扑点，两个拓扑点均位于狭道的安全区域；在狭道的安全区域行驶时，机器人不会与两侧的障碍物发生接触，本发明的中的狭道是指狭窄通道。

S3、基于用Floyd算法获取起始拓扑点至终止拓扑点的拓扑路径点集，删除拓扑路径点集中的起始拓扑点及终止拓扑点，将剩余的拓扑路径点加入路径点集中；

在本发明实施例中，起始拓扑点是拓扑地图中距任务起点最近的拓扑点，终止拓扑点是指拓扑地图中距任务终点最近的拓扑点，在图3中，任务起点为S点，则起始拓扑点即为p1点，任务终点为E点，则终止拓扑点即为p6点。

任务终点为移动机器人所需行驶的目标位置，移动机器人根据当前的位姿，查找最近的拓扑点后，利用Floyd算法完成拓扑路径的搜索，Floyd算法解决的问题是图中找到i号拓扑点到j号拓扑点最短路径值(边的权值)的问题，主要步骤为：建立邻接n阶方阵W，其元素W[i][j]为权值单位，即两拓扑点的欧式距离，若不存在直连线段，则该值为无穷大，对角线元素为0；逐步试着在原直接路径中增加中间点，若加入中间点后路径变短，则修改之，否则维持原值；当所有顶点试探完毕，算法结束。假设搜索图中起点S至终点E的路径，则距离起点S最近的拓扑点为p1，距离终点最近的拓扑点为p6，搜索拓扑路径点集为{p1,p3,p4,p5,p6}，首先处理拓扑路径点，为了减小移动机器人行驶的路径，需要去除拓扑路径点集的起点和终点，剩下的路径点集为{p3,p4,p5}，

S4、在路径点集的首尾出分别加入任务起点及任务终点，基于A-stars算法依次来规划路径点集中相邻路径点间的路径，即生成全局路径点集。

然后将从任务起点S到任务终点E的路径搜索划分为从任务起点S搜索路径点p3、路径点p3搜索至路径点p4、路径点p4搜索至路径点p5、路径点p5搜索至任务终点E，即p3、p4、p5为路径点，用改进后的Astar搜索各路径点之间的路径，最后将搜索后的路径拼接，去除重复的路径起点，搜索完毕。

在本发明实施例中，需要创建两个表，OPEN表保存所有已生成而未考察的节点，CLOSED表中记录已访问过的节点，路径点集中相邻路径点间的路径规划方法具体包括如下步骤：

S41、生成一个包含开始节点n0的搜索图G，把n0放在OPEN表中，生成空表CLOSED表；

S42、选择OPEN表上的第一个节点，将其从OPEN表中移入CLOSE表，称该节点为n；

S43、检测节点n是否为目标节点，若检测结果为是，则在搜索图G中基于n到n0的指针找到一条路径，即为路径点集中相邻路径点间的路径，若检测结果为否，则转向步骤S44；

假定是查找路径点集中第(t-1)个路径点至第t个路径点之间的路径，则目标节点是指路径点集中第t个路径点。

S44、扩展节点n，生成其后继节点集M(前、后、左、右、左上、左下、右上、右下)，节点n的祖先不可在节点集M中，且节点集M中的节点均为可行走的节点；

S45、针对后继节点集M中每个不在搜索图G中的节点，建立一个指向n的指针，(如既不在OPEN表中，也不在CLOSED表中)，针对节点集M中每个已在OPEN表或CLOSE表中的节点m，若到目前为止到达节点m的估价函数f最小值通过节点n，则将节点m的指针修改为指向节点n。

S46、估价函数f从小至大重新排列OPEN表中的节点，并返回步骤S44。

在本发明实施例中，估价函数f(n)可表示为：

f(n)＝g(n)+h(n)

g(n)为任务起点到节点n的最短路径值，h(n)为节点n到任务终点的最短路径的启发值，在启发函数中，应用的启发信息越多，扩展的节点就越少，可减少搜索路径的时间，本发明采用了对启发函数进行了改进，即添加了对角线的代价cross，即启发函数由曼哈顿距离和对角线代价决定:

point1＝S-E；

point2＝N-E；

cross＝|point2(x)*point1(y)-point2(y)*point1(x))|

h(n)＝(|N(x)-E(x)|+|N(y)-E(y)|)*a+cross*b；

其中，S表示任务起点在栅格地图中的坐标(S(x)，S(y))；E表示任务终点在栅格地图中的坐标(E(x)，E(y))；N表示节点n在栅格地图中的坐标(N(x)，N(y))；point1(x)、point1(y)分别表示栅格地图下任务起点与任务终点之间的横坐标差值和纵坐标差值；point2(x)、point2(y)分别表示栅格地图下当前节点n与任务终点之间的横坐标差值和纵坐标差值；a为水平移动一个栅格的代价，b为对角线的代价值。

本发明提供的栅格地图的路径规划方法具有如下有益技术效果：

1)通过在狭道的出口及入口处设有拓扑点，且两个拓扑点均位于狭道的安全区域，先利用Floyd算法规划拓扑路径点，并对拓扑路径点进行处理后利用改进后的Astar算法完成整体路径的搜索，解决了Astar算法搜索具有沿墙走导致小车行走的安全系数不高的问题；

2)对A-star算法进行了改进，在估价函数中增加了对角线的代价，使搜索方向更加智能的趋向终点，从而减少搜索的节点，缩短搜索时间。

上面结合附图对本发明进行了示例性描述，显然本发明具体实现并不受上述方式的限制，只要采用了本发明的方法构思和技术方案进行的各种非实质性的改进，或未经改进将本发明的构思和技术方案直接应用于其它场合的，均在本发明的保护范围之内。

Claims (1)

1.一种栅格地图的路径规划方法，其特征在于，所述方法具体包括如下步骤：

S1、构建栅格地图，对栅格地图中的障碍物进行膨胀处理；

S2、构建拓扑地图，在狭道的出口及入口处设有拓扑点，两个拓扑点均位于狭道的安全区域；

S3、基于用Floyd算法获取起始拓扑点至终止拓扑点的拓扑路径点集，删除拓扑路径点集中的起始拓扑点及终止拓扑点，将剩余的拓扑路径点加入路径点集中；

S4、在路径点集的首尾处分别加入任务起点及任务终点，基于A-stars算法依次规划路径点集中相邻路径点间的路径，生成全局路径点集；

其中，起始拓扑点是拓扑地图中距任务起点最近的拓扑点，终止拓扑点是指拓扑地图中距任务终点最近的拓扑点；

路径点集中相邻路径点间的路径规划方法具体包括如下步骤：

S41、生成一个包含开始节点n0的搜索图G，把n0放在OPEN表中，生成空表CLOSED表；

S42、选择OPEN表上的第一个节点，将其从OPEN表中移入CLOSE表，称该节点为n；

S43、检测节点n是否为目标节点，若检测结果为是，则在搜索图G中基于n到n0的指针找到相邻路径点间的路径，若检测结果为否，则转向步骤S44；

S44、扩展节点n，生成其后继节点集M，节点n的祖先不可在节点集M中，且节点集M中的节点均为可行走的节点；

S45、针对后继节点集M中每个不在搜索图G中的节点，建立一个指向n的指针，针对节点集M中每个已在OPEN表或CLOSE表中的节点m，若到目前为止通过节点n到达节点m的估价函数f最小，则将节点m的指针修改为指向节点n；

S46、基于估价函数f从小至大的顺序重新排列OPEN表中的节点，并返回步骤S44；

节点n的估价函数f(n)表示如下：

f(n)＝g(n)+h(n)

g(n)为任务起点到节点n的最短路径值，h(n)为节点n到任务终点的最短路径；

h(n)＝(|N(x)-E(x)|+|N(y)-E(y)|)*a+cross*b；

cross＝|point2(x)*point1(y)-point2(y)*point1(x))|

point1＝S-E；

point2＝N-E；

其中，S表示任务起点在栅格地图中的坐标(S(x)，S(y))；E表示任务终点在栅格地图中的坐标(E(x)，E(y))；N表示节点n在栅格地图中的坐标(N(x)，N(y))；point1(x)、point1(y)分别表示栅格地图下任务起点与任务终点之间的横坐标差值和纵坐标差值；point2(x)、point2(y)分别表示栅格地图下当前节点n与任务终点之间的横坐标差值和纵坐标差值；a为水平移动一个栅格的代价值，b为对角线的代价值，cross为对角线的代价。