CN112666937B - 一种与图像骨架结合的最优路径规划方法 - Google Patents

Abstract

本发明公开了一种与图像骨架结合的最优路径规划方法，在给定2D栅格地图、当前定位点以及目标点的前提下，计算合理的路径规划的方法。与传统方法不同的点在于，本发明方法不仅仅将距离作为考虑要素，还兼顾了行驶过程中的安全预留、跟随控制的平滑性等要素，从而实现多重约束条件下的最优规划，更切合机器人应用的实际情况。

Description

一种与图像骨架结合的最优路径规划方法

技术领域

本发明属于机器人技术领域，涉及一种与图像骨架结合的最优路径规划方法。

背景技术

目前，给定地图和定位下的路径规划，是机器人实现自主移动与避障功能的关键技术。一般而言，传统的路径规划方法及基于其变种的各种方法，多数把″路径最短″作为最优条件。目前的方法多数集中在如何快速的寻找到这一″最短路径″上。相关的专利主要有：

专利CN201911206405.X公开了一种用于最短路径的规划方法和系统。其中，根据环境信息建立初始栅格地图，并分别将栅格地图的栅格点标记为起点、候选点和障碍点；遍历候选点以计算候选点到起点的最小步长，从而形成最小步长矩阵图；以及选定候选点之一作为终点，通过反向深度优先算法进行递归遍历，以获得终点到起点的最短路径规划。该方案基于栅格地图全局扩算算法，不仅解决了最终路径为最短距离路径，而且大幅度降低了运算的时间复杂度和空间复杂度。

专利CN202010677271.6公开一种路径规划方法、装置以及计算机存储介质，该方法包括：获取起始路径点和终止路径点；在区域连接树上确定起始路径点和终止路径点的公共父路径点以及与起始路径点和终止路径点之间路径规划相关的相关路径点集合；根据起始路径点、终止路径点、公共父路径点从多层次拓扑图中确定与起始路径点和终止路径点之间路径规划相关的相关路径集合；以相关路径点集合和相关路径集合为基础对起始路径点和终止路径点进行最佳路径规划。

专利CN 201911404867.2公开了一种车辆路径生成方法、装置和存储介质；先获取车辆从起始点行驶到目标点的规划行驶路径、以及该规划行驶路径对应的行驶车道集，然后，当该车辆在行驶过程中驶离规划行驶路径时，判断该车辆所处的当前车道段是否属于行驶车道集，若该车辆所处的当前车道段属于行驶车道集，则获取当前车道段的邻近车道段信息，该邻近车道段包括当前车道段的左车道段、右车道段和后继车道段，接着，根据该邻近车道段信息生成该车辆的推荐行驶路径，该推荐行驶路径用于指示该车辆从当前车道段行驶回该规划行驶路径。该方案可以有效地提高车辆路径生成的效率。

专利CN201911001887.5公开了一种机器路径规划方法及移动机器人，通过搜索原始栅格地图上的待处理区域，并对待处理区域进行腐蚀以获得第一地图，然后通过第一预处理方式对所述第一地图进行处理以获得所述第二地图，第二地图包括骨架路径图，最后确定机器当前位置、终点位置，并结合第一搜索规则和所述骨架路径图获得规划路径。

上述四篇专利均公开的是如何寻找最短路径的方法，但是在机器人真正运行过程中，对于给定地图的全局最优路径规划，″路径最短″与实际运行″最优″并非等价的问题。

最短路径通常会过于靠近障碍物，会导致机器人贴着墙或者贴着拐角走，导致安全预留过小，反而不安全。现有的一些路径规划方法也考虑增加障碍膨胀区域及系数的方式，尝试规避前述问题，但又导致规划不稳定等问题，实用效果不好。

还有一些处理方式，就是通过人工手动设置参考路径(自动驾驶多数采用此方案)，来保证实际运行的可靠性，但是需要耗费大量人工，且操作复杂。

发明内容

本发明的目的是提供一种与图像骨架结合的最优路径规划方法，该方法考虑了机器人安全性、人在相似场景中的行进习惯这些因素，在用于规划的代价地图中引入推荐参考区域，从而使得机器人的轨迹规划更为合理。

本发明所采用的技术方案是，一种与图像骨架结合的最优路径规划方法，具体包括如下步骤：

步骤1，对2D格栅地图进行转化与处理；

步骤2，基于步骤1的处理结果生成代价地图；

步骤3，根据步骤2生成的代价地图求最优路径。

本发明的特点还在于：

步骤1的具体过程为：

步骤1.1，基于给定的2D栅格地图M，将栅格地图中的障碍区域和未知区域对应设置为0；将栅格地图中的可通行区域对应位置设置为1，得到二值图像I；

步骤1.2，运用膨胀和腐蚀的形态学算法，去除对二值图像I中的细碎区域；

步骤1.3，对于经步骤1.2处理后的二值图像I，采用骨架提取算法，提取二值图像I中可通行区域的骨架。

步骤1.3中骨架是线状结构。

步骤1.3中，从一个骨架点到另一个骨架点的方向可作为推荐方向。

步骤1.3中根据所述推荐方向得到的一系列推荐点的集合S，如下所示：

S＝{(x1，y1，α1，β1)，(x2，y2，α2，β2)，…，(xn，yn，αn，βn)}

其中，x_i、y_i表示某骨架点的坐标，α_i、β_i表示该骨架点的推荐方向，其中i＝1，2，……，n。

推荐点可人为编辑，包括门口位置、充电桩正前方位置。

步骤2的具体步骤为：

步骤2.1，将代价地图中对应于障碍区域的方格位置，代价值设置为最高，表示机器人在规划中，不允许通过此区域；

步骤2.2，将代价地图中，按照方格到最近障碍物的远近，设置代价的数值，具体采用如下公式：

其中，C表示最高的代价值，设为255；d表示该方格到最近障碍方格的距离，r1表示机器人的半径，r2为预设值，表示机器人应该到障碍物的控制距离；

步骤2.3，将推荐区域的点的集合对应到代价地图对应的方格中，对每一个推荐点对应方格，按照如下的方式重新设置其附近的cost值：

A：当该方格到最近的障碍方格的距离小于r2时，保持原cost值不变；

B：当该方格到最近障碍方格的距离大于r2时，取该方格周边半径r3范围内的方格，设其到该方格距离d3，采用如下新的cost值：

代价地图中的方格被分为如下四类：

一类是代价值为255的方格，表示机器人规划绝对不能经过；

一类是代价值在50到255之间的方格，表示此处靠近障碍，机器人规划应当避免，路过会受到“惩罚”；

一类是代价值等于50的方格，表示机器人可通行；

一类是小于50的方格，表示机器人在附近时，优先通行该类方格。

步骤3的具体过程如下：

步骤3.1，初始化，将代价地图中所有的cost值为255的方格设置为不可达；创建待搜索方格库K1，并且将起始方格加入待搜索方格库；创建已确定方格库K2，并将方格库K2设置为空，方格库K1和K2是方格的集合，每个方格均包含如下元素：方格的位置、方格的前置方格、从起点到方格的最小路径代价G、从方格到终点的最小路径代价预估H、途经该方格的最小代价预估F＝G+H；初始化时，方格K1只有一个元素，也就是起点方格，G＝0；

步骤3.2，找到K1中F值最小的方格N，考虑该方格相邻的八个方格M_j，其中j＝1～8：

1)如果方格M_j到方格N不可达，加入K2；

2)如果方格M_j已在K2内，则不做处理；

3)如果方格M_j不在K1中，则将方格M_j加入K1,并将当前方格设置为方格M_j的前置方格，计算从起点到方格M_j的最小代价路径；

4)如果方格M_j已经在K1中，则检查从当前方格到方格M_j的路径是否为(3)中的最小代价路径，如果是最小代价路径，则更新，如果不是，则不更新。

5)若方格M_j是S中的推荐点，则优先计算推荐方向上的方格，其他方向先不计算；

步骤3.3，将K1中处理完全部相邻方格的方格加入K2；

步骤3.4，重复执行步骤3.2～3.3；

步骤3.5，当满足如下条件时，停止对步骤3.2～3.3的重复执行：

把终点加入到了K1中，此时路径已经找到了，或查找终点失败，并且K1是空的，此时没有路径；

步骤3.6，保存路径，从终点开始，每个方格沿着前置方格移动直至起点，即为路径。

步骤3.1中：

从起点到方格的最小路径代价G采用如下公式计算：

G＝G_f+d/2×(Cost+Cost_f)

其中,G_f表示从起点到该方格的前置方格的最小路径代价，d表示该方格与其前置方格的距离，Cost表示该方格的代价系数，Cost_f是前置方格的代价系数；

从方格到终点的最小路径代价预估H采用如下公式计算：

H＝∑d_i×Cost_i

其中，H由连接当前方格与目标方格的直线路径计算，d_i表示该直线路径上第i个方格的长度，Cost_i表示该直线路径上第i个方格的代价系数；

途经该方格的最小代价预估F采用如下公式计算：

F＝G+H。

本发明的有益效果是，与已有的导航规划方案相比，本发明引入了推荐点的概念，同时提出了一种采用图像处理自动得到推荐点的方法，保证机器人路径规划的安全性最优，而不仅仅是基于路径最短的最优。本发明的方法，也可以人为指定推荐点，并使得推荐点加入了自动规划的流程，实现更加智能而不是呆板的规划。

附图说明

图1是本发明一种与图像骨架结合的最优路径规划方法的流程图。

具体实施方式

下面结合附图和具体实施方式对本发明进行详细说明。

本发明一种与图像骨架结合的最优路径规划方法，以移动机器人的自动规划为例子，这里的移动机器人，可以是室内/室外平面移动的机器人，机器人以2D激光雷达作为主要传感器，探测机器人周围的障碍物。机器人通过预先构建的场景2D栅格地图，作为导航地图M；机器人可通过2D激光雷达数据或者其他手段，实时的感知自己在地图中的对应位置，记为p；机器人通过人工给定或者任务给定或者其他方式，获知自己当前需要去往的对应于地图中的位置，记为T。

本发明在给定2D栅格导航地图M，给定机器人当前起点位置p和目标位置T(p和T都是对应于M中的位置)，求最优的路径，它是一系列从起点到目标方格的连续点的集合。

如图1所示，具体包括如下步骤：

步骤1，对2D格栅地图进行转化与处理；

步骤1的具体过程为：

步骤1.1，基于给定的2D栅格地图M(一般由激光SLAM等算法创建)，将栅格地图中的障碍区域和未知区域对应设置为0；将栅格地图中的可通行区域对应位置设置为1，得到二值图像I；

步骤1.2，运用膨胀和腐蚀的形态学算法，去除对二值图像I中的细碎区域；

步骤1.3，对于经步骤1.2处理后的二值图像I，采用骨架提取算法，提取二值图像I中可通行区域的骨架。对于骨架上的某点，它到它相邻点(同样为骨架点)的方向，可作为推荐方向。

步骤1.3中根据所述推荐方向得到的一系列推荐点的集合S，如下所示：

S＝{(x1，y1，α1，β1)，(x2，y2，α2，β2)，…，(xn，yn，αn，βn)}

其中，x_i、y_i表示某骨架点的坐标，α_i、β_i表示该骨架点的推荐方向，i＝1，2，……，n。

推荐点可人为编辑，包括门口位置、充电桩正前方位置。

步骤2，基于步骤1的处理结果生成代价地图；

步骤2的具体步骤为：

步骤2.1，将代价地图中对应于障碍区域(如，墙面、桌椅等)的方格位置，代价值设置为最高，表示机器人在规划中，不允许通过此区域；

步骤2.2，将代价地图中，按照方格到最近障碍物的远近，设置代价的数值，具体采用如下公式：

其中，C表示最高的代价值，设为255；d表示该方格到最近障碍方格的距离，r1表示机器人的半径，r2为预设值，表示机器人应该到障碍物的控制距离；

步骤2.3，将推荐区域的点的集合(可以是步骤1中提到的骨架点集合S，也可以包含人为指定的关键点)对应到代价地图对应的方格中，对每一个推荐点对应方格，按照如下的方式重新设置其附近的cost值：

A:当该方格到最近的障碍方格的距离小于r2时，保持原cost值不变；

B：当该方格到最近障碍方格的距离大于r2时(同样要求到最近障碍距离大于r2)，取该方格周边半径r3范围内的方格，设其到该方格距离d3,采用如下新的cost值：

代价地图中的方格被分为如下四类：

一类是代价值为255的方格，表示机器人规划绝对不能经过；

一类是代价值在50到255之间的方格，表示此处靠近障碍，机器人规划应当避免，路过会受到“惩罚”；

一类是代价值等于50的方格，表示机器人可通行；

一类是小于50的方格，表示机器人在附近时，优先通行该类方格。

步骤3，基于步骤2得到的代价地图，以及给定的起始方格与目标方格，本发明采用如下的改进A*的方法，实现路径规划。

步骤3的具体过程如下：

步骤3.1，初始化，将代价地图中所有的cost值为255的方格设置为不可达；创建待搜索方格库K1，并且将起始方格加入待搜索方格库；创建已确定方格库K2，并将方格库K2设置为空，方格库K1和K2是方格的集合，每个方格均包含如下元素：方格的位置、方格的前置方格、从起点到方格的最小路径代价G、从方格到终点的最小路径代价预估H、途经该方格的最小代价预估F＝G+H；初始化时，方格K1只有一个元素，也就是起点方格，G＝0；

步骤3.2，找到K1中F值最小的方格N，考虑该方格相邻的八个方格M_j，其中j＝1～8：

1)如果方格M_j到方格N不可达，加入K2；

2)如果方格M_j已在K2内，则不做处理；

3)如果方格M_j不在K1中，则将方格M_j加入K1,并将当前方格设置为方格M_j的前置方格，计算从起点到方格M_j的最小代价路径；

4)如果方格M_j已经在K1中，则检查从当前方格到方格M_j的路径是否为(3)中的最小代价路径，如果是最小代价路径，则更新，如果不是，则不更新。

5)若方格M_j是S中的推荐点，则优先计算推荐方向上的方格，其他方向先不计算；

步骤3.3，将K1中处理完全部相邻方格的方格加入K2；

步骤3.4，重复执行步骤3.2～3.3；

步骤3.5，当满足如下条件时，停止对步骤3.2～3.3的重复执行：

把终点加入到了K1中，此时路径已经找到了，或查找终点失败，并且K1是空的，此时没有路径；

步骤3.6，保存路径，从终点开始，每个方格沿着前置方格移动直至起点，即为路径。

步骤3.1中：

从起点到方格的最小路径代价G采用如下公式计算：

G＝G_f+d/2×(Cost+Cost_f)

其中,G_f表示从起点到该方格的前置方格的最小路径代价，d表示该方格与其前置方格的距离，Cost表示该方格的代价系数，Cost_f是前置方格的代价系数；

从方格到终点的最小路径代价预估H采用如下公式计算：

H＝∑d_i×Cost_i

其中，H由连接当前方格与目标方格的直线路径计算，d_i表示该直线路径上第i个方格的长度，Cost_i表示该直线路径上第i个方格的代价系数；

途经该方格的最小代价预估F采用如下公式计算：

F＝G+H。

本发明提出一种新的最优路径求解方法，该方法包括：2D栅格地图的转化与处理、代价地图生成、最优路径求解三大关键步骤。

在2D栅格地图的转化与处理步骤中，将栅格地图转化为二值图像，并且提取可同行区域的骨架点，作为机器人路径规划时的推荐点。在这一步骤，可以允许人为编辑推荐点。

在代价地图生成这一步骤中，对于不适宜到达的位置设置较高的cost值，对于推荐点位置设置更低的cost值，便于后续规划的计算。

在路径规划计算这一步骤中，本发明提出一种以A*为基础的改进的规划方法，改进了传统的cost值计算，从而加速了搜索过程，并且使得机器人在自主导航时，优先沿着“推荐点”+“推荐方向”行走，可以使得路径更加稳定，且最大限度保证机器人行走的安全裕度。

在给定地图输入时，先计算推荐区域，这里的方法为：对可通行区域图像进行骨架提取，找到一系列推荐区域点，同时，将相邻的推荐区域点的连线方向作为该点对应的推荐方向。

结合推荐区域，以及障碍区域，计算用于规划的代价地图。代价地图的原则是，在靠近推荐区域附近时，代价逐渐减小，而靠近障碍物时，代价逐渐增加。

最后，结合代价地图、导航指令，计算最优的路径。主要采用改进的A*算法，改进了传统的cost值计算，从而加速了搜索过程，并且使得机器人在自主导航时，优先沿着“推荐点”+“推荐方向”行走，可以使得路径更加稳定，且最大限度保证机器人行走的安全裕度。

Claims (9)

1.一种与图像骨架结合的最优路径规划方法，其特征在于：具体包括如下步骤：

步骤1，对2D格栅地图进行转化与处理；

步骤2，基于步骤1的处理结果生成代价地图；

步骤3，根据步骤2生成的代价地图求最优路径；

其中，所述步骤2的具体步骤为：

步骤2.1，将代价地图中对应于障碍区域的方格位置，代价值设置为最高，表示机器人在规划中，不允许通过此区域；

步骤2.2，将代价地图中，按照方格到最近障碍物的远近，设置代价的数值，具体采用如下公式：

其中，C表示最高的代价值，设为255；d表示该方格到最近障碍方格的距离，r1表示机器人的半径，r2为预设值，表示机器人应该到障碍物的控制距离；

步骤2.3，将推荐区域的点的集合对应到代价地图对应的方格中，对每一个推荐点对应方格，按照如下的方式重新设置其附近的cost值：

A:当该方格到最近的障碍方格的距离小于r2时，保持原cost值不变；

B：当该方格到最近障碍方格的距离大于r2时，取该方格周边半径r3范围内的方格，设其到该方格距离d3,采用如下新的cost值：

2.根据权利要求1所述的一种与图像骨架结合的最优路径规划方法，其特征在于：所述步骤1的具体过程为：

步骤1.1，基于给定的2D栅格地图M，将栅格地图中的障碍区域和未知区域对应设置为0；将栅格地图中的可通行区域对应位置设置为1，得到二值图像I；

步骤1.2，运用膨胀和腐蚀的形态学算法，去除二值图像I中的细碎区域；

步骤1.3，对于经步骤1.2处理后的二值图像I，采用骨架提取算法，提取二值图像I中可通行区域的骨架。

3.根据权利要求2所述的一种与图像骨架结合的最优路径规划方法，其特征在于：所述步骤1.3中骨架是线状结构。

4.根据权利要求3所述的一种与图像骨架结合的最优路径规划方法，其特征在于：所述步骤1.3中，从一个骨架点到另一个骨架点的方向作为推荐方向。

5.根据权利要求4所述的一种与图像骨架结合的最优路径规划方法，其特征在于：所述步骤1.3中根据所述推荐方向得到的一系列推荐点的集合S，如下所示：

S＝{(x1,y1,α1,β1),(x2,y2,α2,β2),...,(xn,yn,αn,βn)}

其中，x_i、y_i表示某骨架点的坐标，α_i、β_i表示该骨架点的推荐方向，其中i＝1，2，……,n。

6.根据权利要求5所述的一种与图像骨架结合的最优路径规划方法，其特征在于：所述推荐点可人为编辑，包括门口位置、充电桩正前方位置。

7.根据权利要求1所述的一种与图像骨架结合的最优路径规划方法，其特征在于：所述代价地图中的方格被分为如下四类：

一类是代价值为255的方格，表示机器人规划绝对不能经过；

一类是代价值在50到255之间的方格，表示此处靠近障碍，机器人规划应当避免，路过会受到“惩罚”；

一类是代价值等于50的方格，表示机器人可通行；

一类是小于50的方格，表示机器人在附近时，优先通行该类方格。

8.根据权利要求1所述的一种与图像骨架结合的最优路径规划方法，其特征在于：所述步骤3的具体过程如下：

步骤3.1，初始化，将代价地图中所有的cost值为255的方格设置为不可达；创建待搜索方格库K1，并且将起始方格加入待搜索方格库；创建已确定方格库K2，并将方格库K2设置为空，方格库K1和K2是方格的集合，每个方格均包含如下元素：方格的位置、方格的前置方格、从起点到方格的最小路径代价G、从方格到终点的最小路径代价预估H、途经该方格的最小代价预估F＝G+H；初始化时，方格K1只有一个元素，也就是起点方格，G＝0；

步骤3.2，找到K1中F值最小的方格N，考虑该方格相邻的八个方格M_j，其中j＝1～8：

1)如果方格M_j到方格N不可达，加入K2；

2)如果方格M_j已在K2内，则不做处理；

3)如果方格M_j不在K1中，则将方格M_j加入K1,并将当前方格设置为方格M_j的前置方格，计算从起点到方格M_j的最小代价路径；

4)如果方格M_j已经在K1中，则检查从当前方格到方格M_j的路径是否为3)中的最小代价路径，如果是最小代价路径，则更新，如果不是，则不更新；

5)若方格M_j是S中的推荐点，则优先计算推荐方向上的方格，其他方向先不计算；其中，S为推荐点的集合；

步骤3.3，将K1中处理完全部相邻方格的方格加入K2；

步骤3.4，重复执行步骤3.2～3.3；

步骤3.5，当满足如下条件时，停止对步骤3.2～3.3的重复执行：

把终点加入到了K1中，此时路径已经找到了，或查找终点失败，并且K1是空的，此时没有路径；

步骤3.6，保存路径，从终点开始，每个方格沿着前置方格移动直至起点，即为路径。

9.根据权利要求8所述的一种与图像骨架结合的最优路径规划方法，其特征在于：所述步骤3.1中：

从起点到方格的最小路径代价G采用如下公式计算：

G＝G_f+d/2×(Cost+Cost_f)

其中,G_f表示从起点到该方格的前置方格的最小路径代价，d表示该方格与其前置方格的距离，Cost表示该方格的代价系数，Cost_f是前置方格的代价系数；

从方格到终点的最小路径代价预估H采用如下公式计算：

H＝∑d_i×Cost_i

其中，H由连接当前方格与目标方格的直线路径计算，d_i表示该直线路径上第i个方格的长度，Cost_i表示该直线路径上第i个方格的代价系数；

途经该方格的最小代价预估F采用如下公式计算：

F＝G+H。

Images