[发明专利]路径规划方法、路径规划系统、机器人及存储介质

说明书

本发明提供了一种路径规划方法、路径规划系统、机器人及存储介质，包括如下步骤：设定最小膨胀距离d_min，最大膨胀距离d_max；接受需要规划的原始栅格地图，根据d_min和d_max生成新的地图类型distmap；在distmap里输入起点和终点，将起点列为当前点P’；判断当前点P’的临近网格是否可通行，并根据新启发式函数O(x)计算临近网格的cost值；将P’附近可通行的点加入openlist队列，并根据每个点的cost值建立最小堆；判断P’是否为终点，否则重复上述步骤，是则结束搜索。本发明降低栅格地图的搜索空间，解决大地图下规划速度过慢的问题，并在保证通行性的前提下自适应选择离障碍物远的、更容易通行和安全的路径。

技术领域

本发明涉及机器人算法的技术领域，具体地，涉及一种路径规划方法、路径规划系统、机器人及存储介质。

背景技术

目前常用的基于栅格地图的服务机器人路径规划方法，如dijstra、A*、存在规划速度过慢(随着地图面积变大，规划速度呈指数级下降，见图1)的问题、规划出的路径虽然最短但非最优(距离障碍物过近存在安全隐患，见图2)的问题。此外，目前基于costmap的路径规划，其静态的膨胀范围存在路径最短和路径离障碍物距离太近的矛盾(见图3)。

公开号为CN110361009B的专利文献公开了一种路径规划方法、路径规划系统以及移动机器人，通过获取待测环境中的障碍物位置信息，根据障碍物位置信息建立势场地图，提取所述势场地图中的第一关键点并对第一关键点进行筛选以获得第二关键点，由第二关键点获得拓扑地图，获取从机器人当前位置到目标点位置的最优路径。但是该专利文献仍然存在大地图下规划速度过慢的缺陷。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种路径规划方法、路径规划系统、机器人及存储介质。

根据本发明提供的一种路径规划方法，包括如下步骤：

步骤1：设定最小膨胀距离d_min，最大膨胀距离d_max；

步骤2：接受需要规划的原始栅格地图，根据d_min和d_max生成新的地图类型distmap；

步骤3：在distmap里输入起点和终点，将起点列为当前点P’；

步骤4：判断当前点P’的临近网格是否可通行，并根据新启发式函数O(x)计算临近网格的cost值；

步骤5：将P’附近可通行的点加入openlist队列，并根据每个点的cost值建立最小堆；

步骤6：判断P’是否为终点，否则重复步骤3~6，是则结束搜索；

所述步骤2包括如下步骤：

步骤2.1：将原始栅格地图中的所有被占据网格加入待处理队列occ_list，未被占据的网格的dist值初始化为d_max，其中，dist值为未被占据网格到被占据网格的最小距离，d_max为最大膨胀距离；

步骤2.2：将occ_list中每个被占据的网格记为网格点p_i，将周围未被占据的网格加入dist_expand_list，并计算每个未被占据的网格离p_i的dist值；

步骤2.3：如果未被占据的网格距p_i的dist值 大于 d_max，则停止将周围未被占据的网格加入dist_expand_list，将当前处理的未被占据的网格从dist_expand_list中出列；否则将点p_i附近的未被占据的网格继续加入dist_expand_list，如果dist_expand_list中的网格的dist值有改变，则取最小值；

步骤2.4：如果dist_expand_list队列不为空，重复步骤2.3，否则重复步骤2.2；

步骤2.5：如果occ_list队列不为空，重复步骤2.2，否则结束，生成新的地图类型distmap；