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

权利要求书

1.一种路径规划方法，其特征在于，包括如下步骤：

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

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

步骤3：在distmap里输入起点S（xs, ys）和终点F(xf, yf)，将起点列为当前点P’(xp,yp)；

步骤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；

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

步骤4.1：遍历当前网格的邻近网格，判断当前网格的dist值，dist值通过步骤2生成distmap的算法得到，若dist值 d_min，则不加入openlist队列，否则加入openlist队列；

步骤4.2：根据新提出的启发式搜索函数O(x) = H(x) + G(x) + D(x)来计算网格的O(x)值；

所述步骤4.2中，H（x）表示当前网格P’（xp, yp）点到起点S(xs, ys)的欧式距离，H(x)= SQRT((xs - xp)^2 + (ys - yp)^2)；

所述步骤4.2中，G(x)表示当前网格P’（xp, yp）点到终点F(xf, yf)的欧式距离，G(x)= SQRT((xf - xp)^2 + (yf - yp)^2)；

所述步骤4.2中，D(x)表示当前网格在distmap下的权重值，D(x) = -(p_dist * safe_scale)；

其中p_dist为distmap生成时得到的当前网格的dist值；safe_scale为根据实际场景配置的参数，safe_scale表示安全系数，该系数越大说明整体的O(x)cost值受dist的影响越大，越倾向于走离障碍物远的网格。

2.根据权利要求1所述的路径规划方法，其特征在于，所述步骤6中，判断当前点P’(xp,yp) 和终点F(xf, yf)，若xp == xf yp == yf即认为P’为终点，否则将P’周围网格加入openlist继续循环处理。

3.一种机器人，其特征在于，所述机器人包括处理器和存储器，所述存储器储存有若干指令，所述处理器通过执行所述若干指令实现权利要求1或2任意一项所述的路径规划方法。

4.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1或2中任一项所述的路径规划方法。