CN115560767B - 机器人路径的生成方法及装置、存储介质、电子设备 - Google Patents

Abstract

本发明公开了一种机器人路径的生成方法及装置、存储介质、电子设备，其中，该方法包括：定位机器人的起始位置，以及获取路网地图，其中，路网地图包括多个路网点及其连通关系；采用路网地图构建多个路网点的最短路径矩阵；根据起始位置在路网地图中查找距离机器人最近的路网起点；将起始位置到路网起点之间的路径确定为全局路径的起始段，获取任务地图，以路网起点为起始点，基于最短路径矩阵和任务地图遍历生成机器人待途经的有序任务点队列，其中，任务地图包括机器人在路网地图中待触达的多个任务点；基于任务点队列和最短路径矩阵生成机器人待途经的有序非任务点队列，并采用任务点队列和非任务点队列生成全局路径的剩余段。

Description

机器人路径的生成方法及装置、存储介质、电子设备

技术领域

本发明涉及计算机领域，具体而言，涉及一种机器人路径的生成方法及装置、存储介质、电子设备。

背景技术

相关技术中，巡检机器人被广泛应用于电厂、输电站、变电所和变电站等电力场景，以及工业园区、海关等场所，用以代替人工完成常规或特种巡检作业任务。

相关技术中，机器人的运行轨迹一般是由路径规划系统预先设置好，机器人按照设置好的规划路径运行，但是在地图更新或者是路网中出现障碍物时，不能重新规划路线，需要人为调度，导致机器人的控制效率低。

针对相关技术中存在的上述问题，目前尚未发现有效的解决方案。

发明内容

本发明实施例提供了一种机器人路径的生成方法及装置、存储介质、电子设备。

根据本申请实施例的一个方面，提供了一种机器人路径的生成方法，包括：定位机器人的起始位置，以及获取路网地图，其中，所述路网地图包括多个路网点及其连通关系；采用所述路网地图构建所述多个路网点的最短路径矩阵；根据所述起始位置在所述路网地图中查找距离所述机器人最近的路网起点；将所述起始位置到所述路网起点之间的路径确定为全局路径的起始段，获取任务地图，以所述路网起点为起始点，基于所述最短路径矩阵和所述任务地图遍历生成所述机器人待途经的有序任务点队列，其中，所述任务地图包括所述机器人在所述路网地图中待触达的多个任务点；基于所述任务点队列和所述最短路径矩阵生成所述机器人待途经的有序非任务点队列，并采用所述任务点队列和所述非任务点队列生成所述全局路径的剩余段，其中，所述全局路径包括所述起始段和所述剩余段。

根据本申请实施例的另一个方面，还提供了一种机器人路径的生成装置，包括：获取模块，用于定位机器人的起始位置，以及获取路网地图，其中，所述路网地图包括多个路网点及其连通关系；构建模块，用于采用所述路网地图构建所述多个路网点的最短路径矩阵；查找模块，用于根据所述起始位置在所述路网地图中查找距离所述机器人最近的路网起点；第一生成模块，用于将所述起始位置到所述路网起点之间的路径确定为全局路径的起始段，获取任务地图，以所述路网起点为起始点，基于所述最短路径矩阵和所述任务地图遍历生成所述机器人待途经的有序任务点队列，其中，所述任务地图包括所述机器人在所述路网地图中待触达的多个任务点；第二生成模块，用于基于所述任务点队列和所述最短路径矩阵生成所述机器人待途经的有序非任务点队列，并采用所述任务点队列和所述非任务点队列生成所述全局路径的剩余段，其中，所述全局路径包括所述起始段和所述剩余段。

进一步，所述构建模块包括：计算单元，用于计算所述路网地图中的每两个路网点之间的欧式距离；获取单元，用于获取所述路网地图中的每个路网点的点标识；第一创建单元，用于采用所述点标识和所述欧式距离创建原始二维矩阵，其中，所述原始二维矩阵的行列数量均与所述点标识的数量相同，矩阵元素 [i][j]的元素值Matrix[i][j]为路网点i到路网点j的欧式距离，i，j均为点标识；第二创建单元，用于采用所述原始二维矩阵创建最短路径值矩阵和最短路径点矩阵，其中，所述最短路径值矩阵用于存储所述路网地图中任意两个路网点之间的最短距离，所述最短路径点矩阵用于存储所述路网地图中任意两个路网点之间的最短中转点；其中，所述最短路径矩阵包括所述最短路径值矩阵和所述最短路径点矩阵。

进一步，所述第二创建单元包括：选择子单元，用于遍历所述原始二维矩阵中的每个矩阵元素 [i][j]，在所述路网地图中选择任意的路网点k作为中转点；获取子单元，用于在所述原始二维矩阵中获取矩阵元素 [i][j]的第一距离Matrix[i][j]，矩阵元素 [i][k]的第二距离Matrix[i][k]，矩阵元素 [k][j]的第三距离Matrix[k][j]；第一判断子单元，用于判断Matrix[i][j] 是否大于Matrix[i][k]+ Matrix[k][j]；第一配置子单元，用于若Matrix[i][j]大于Matrix[i][k]+ Matrix[k][j]，在第一初始矩阵中将Matrix[i][k]+Matrix[k][j]配置为矩阵元素[i][j]的最短路径值；若Matrix[i][j]小于或等于Matrix[i][k]+ Matrix[k][j]，在第一初始矩阵中将Matrix[i][j]配置为矩阵元素[i][j]的最短路径值；第一生成子单元，用于在所述原始二维矩阵中的所有矩阵元素遍历完成之后，得到最短路径值矩阵。

进一步，所述第二创建单元包括：第二判断子单元，用于针对所述最短路径值矩阵中的每两个矩阵元素 [i][j]，判断最短路径值的路径是否经过中转点；第二配置子单元，用于若最短路径值的路径经过中转点，在第二初始矩阵中将对应中转点的点标识配置为矩阵元素[i][j]的最短路径点；若最短路径值的路径不经过中转点，在第二初始矩阵中将预设值配置为矩阵元素[i][j]的最短路径点；第二生成子单元，用于在所述最短路径值矩阵中的所有矩阵元素遍历完成之后，得到最短路径点矩阵。

进一步，所述查找模块包括：第一计算单元，用于计算所述起始位置到所述路网地图中任意路网点的点点距离，并选择距离所述起始位置最近的第一路网点，并记录最近的第一点点距离；第二计算单元，用于计算所述起始位置到所述路网地图中任意路网路径的点线距离，并选择点线距离最短的第一路网路径，并记录所述起始位置到所述第一路网路径的第一点线距离，其中，所述路网路径是任意两个路网点连成的路径段，所述第一路网路径的两端点为第二路网点和第三路网点；判断单元，用于判断所述第一点线距离是否小于所述第一点点距离；选择单元，用于若所述第一点线距离小于所述第一点点距离，在所述第二路网点和所述第三路网点中选择距离所述起始位置的点点距离最近的点为路网起点；若所述第一点线距离大于或等于所述第一点点距离，选择所述第一路网点为路网起点。

进一步，所述第一生成模块包括：生成单元，用于根据所述任务地图生成所述机器人待途经的无序任务点序列；查找单元，用于以所述路网起点为起始点，以所述无序任务点序列中的每个任务点为终点，在最短路径值矩阵中查找所述起始点与每个终点之间的最短距离，得到最短距离集合，其中，所述最短路径矩阵包括所述最短路径值矩阵，所述最短路径值矩阵用于存储所述路网地图中任意两个路网点之间的最短距离；处理单元，用于基于所述最短距离集合在所述无序任务点序列中查找最短距离最小的第一任务点，并将所述第一任务点加入有序任务点队列，在所述无序任务点序列中删除所述第一任务点；生成单元，用于以当前加入有序任务点队列的任务点为起始点，所述无序任务点序列中剩余的每个任务点为终点，继续遍历所述无序任务点序列中剩余的每个任务点，直到所述无序任务点序列为空，得到完整的有序任务点队列。

进一步，所述第二生成模块包括：第一读取单元，用于从所述有序任务点队列中按序读取相邻的任务段，其中，所述任务段由相邻的第二任务点和第三任务点组成；第二读取单元，用于针对每个任务段，从最短路径点矩阵中递归读取当前任务段经过的中转点集合，其中，所述最短路径矩阵包括所述最短路径点矩阵，所述最短路径点矩阵用于存储所述路网地图中任意两个路网点之间的最短中转点；生成单元，用于按序组合所有任务段的中转点集合，得到完整的有序非任务点队列。

根据本申请实施例的另一方面，还提供了一种存储介质，该存储介质包括存储的程序，程序运行时执行上述的步骤。

根据本申请实施例的另一方面，还提供了一种电子设备，包括处理器、通信接口、存储器和通信总线，其中，处理器，通信接口，存储器通过通信总线完成相互间的通信；其中：存储器，用于存放计算机程序；处理器，用于通过运行存储器上所存放的程序来执行上述方法中的步骤。

本申请实施例还提供了一种包含指令的计算机程序产品，当其在计算机上运行时，使得计算机执行上述方法中的步骤。

通过本发明，定位机器人的起始位置，以及获取路网地图，其中，路网地图包括多个路网点及其连通关系，采用路网地图构建多个路网点的最短路径矩阵，根据起始位置在路网地图中查找距离机器人最近的路网起点；将起始位置到路网起点之间的路径确定为全局路径的起始段，获取任务地图，以路网起点为起始点，基于最短路径矩阵和任务地图遍历生成机器人待途经的有序任务点队列，其中，任务地图包括机器人在路网地图中待触达的多个任务点，基于任务点队列和最短路径矩阵生成机器人待途经的有序非任务点队列，并采用任务点队列和非任务点队列生成全局路径的剩余段，通过获取到路网点间的最短路径，可以减少多路网点任务规划阶段的算法耗时，解决了相关技术生成机器人全局路径效率低的技术问题，提高了机器人的任务响应速度，同时实现了机器人巡检过程中的遇障重规划。

附图说明

此处所说明的附图用来提供对本发明的进一步理解，构成本申请的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1是本发明实施例的一种机器人的硬件结构框图；

图2是根据本发明实施例的一种机器人路径的生成方法的流程图；

图3是本发明实施例中路网无向图的示意图；

图4是本发明实施例中转更新路网的流程图；

图5是本发明实施例以每个路网点作为中转点的更新路网流程图；

图6是本发明实施例递归获取路径端点的流程图；

图7是本发明实施例计算机器人到路网路径的距离的示意图；

图8是本发明实施例获取有序任务点队列的流程示意图；

图9是根据本发明实施例的一种机器人路径的生成装置的结构框图。

具体实施方式

为了使本技术领域的人员更好地理解本申请方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分的实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本申请保护的范围。需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。

需要说明的是，本申请的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本申请的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

实施例1

本申请实施例一所提供的方法实施例可以在服务器、计算机、机器人或者类似的运算装置中执行。以运行在机器人上为例，图1是本发明实施例的一种机器人的硬件结构框图。如图1所示，机器人10可以包括一个或多个（图1中仅示出一个）处理器102（处理器102可以包括但不限于微处理器MCU或可编程逻辑器件FPGA等的处理装置）和用于存储数据的存储器104，可选地，上述机器人还可以包括用于通信功能的传输设备106以及输入输出设备108。本领域普通技术人员可以理解，图1所示的结构仅为示意，其并不对上述机器人的结构造成限定。例如，机器人10还可包括比图1中所示更多或者更少的组件，或者具有与图1所示不同的配置。

存储器104可用于存储机器人程序，例如，应用软件的软件程序以及模块，如本发明实施例中的一种机器人路径的生成方法对应的机器人程序，处理器102通过运行存储在存储器104内的机器人程序，从而执行各种功能应用以及数据处理，即实现上述的方法。存储器104可包括高速随机存储器，还可包括非易失性存储器，如一个或者多个磁性存储装置、闪存、或者其他非易失性固态存储器。在一些实例中，存储器104可进一步包括相对于处理器102远程设置的存储器，这些远程存储器可以通过网络连接至机器人10。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。

传输装置106用于经由一个网络接收或者发送数据。上述的网络具体实例可包括机器人10的通信供应商提供的无线网络。在一个实例中，传输装置106包括一个网络适配器（Network Interface Controller，简称为NIC），其可通过基站与其他网络设备相连从而可与互联网进行通讯。在一个实例中，传输装置106可以为射频（Radio Frequency，简称为RF）模块，其用于通过无线方式与互联网进行通讯。

在本实施例中提供了一种机器人路径的生成方法，图2是根据本发明实施例的一种机器人路径的生成方法的流程图，如图2所示，该流程包括如下步骤：

步骤S202，定位机器人的起始位置，以及获取路网地图，其中，路网地图包括多个路网点及其连通关系；

本实施例的起始位置是机器人当前所在的位置，从当前位置开始执行路径规划。可以适用于轮式底盘或四足机器人等巡检机器人的路网全局路径规划。

步骤S204，采用路网地图构建多个路网点的最短路径矩阵；

可选的，最短路径矩阵包括最短路径值矩阵和最短路径点矩阵，最短路径值矩阵用于存储路网地图中任意两个路网点之间的最短距离，最短路径点矩阵用于存储路网地图中任意两个路网点之间的最短中转点。

步骤S206，根据起始位置在路网地图中查找距离机器人最近的路网起点；

步骤S208，将起始位置到路网起点之间的路径确定为全局路径的起始段，获取任务地图，以路网起点为起始点，基于最短路径矩阵和任务地图遍历生成机器人待途经的有序任务点队列，其中，任务地图包括机器人在路网地图中待触达的多个任务点；

任务点是机器人必须触达的路网点，算法可以在未获取到任务时即可进行预处理，同时可以快速进行多任务调度。

步骤S210，基于任务点队列和最短路径矩阵生成机器人待途经的有序非任务点队列，并采用任务点队列和非任务点队列生成全局路径的剩余段，其中，全局路径包括起始段和剩余段。

本实施例的机器人可以是扫地机器人，工业机器人（如仓库，流水线等），在得到全局路径之后，机器人按照全局路径进行移动，若中途故障或者停止，偏离全局路径的轨迹，则继续以当前位置为起始位置，继续生成并更新全局路径。

通过上述步骤，定位机器人的起始位置，以及获取路网地图，其中，路网地图包括多个路网点及其连通关系，采用路网地图构建多个路网点的最短路径矩阵，根据起始位置在路网地图中查找距离机器人最近的路网起点；将起始位置到路网起点之间的路径确定为全局路径的起始段，获取任务地图，以路网起点为起始点，基于最短路径矩阵和任务地图遍历生成机器人待途经的有序任务点队列，其中，任务地图包括机器人在路网地图中待触达的多个任务点，基于任务点队列和最短路径矩阵生成机器人待途经的有序非任务点队列，并采用任务点队列和非任务点队列生成全局路径的剩余段，通过获取到路网点间的最短路径，可以减少多路网点任务规划阶段的算法耗时，解决了相关技术生成机器人全局路径效率低的技术问题，提高了机器人的任务响应速度，同时实现了机器人巡检过程中的遇障重规划。

在本实施例的一个实施方式中，采用路网地图构建多个路网点的最短路径矩阵包括：

S11，计算路网地图中的每两个路网点之间的欧式距离；

S12，获取路网地图中的每个路网点的点标识；

S13，采用点标识和欧式距离创建原始二维矩阵，其中，原始二维矩阵的行列数量均与点标识的数量相同，矩阵元素 [i][j]的元素值Matrix[i][j]为路网点i到路网点j的欧式距离，i，j均为点标识；

S14，采用原始二维矩阵创建最短路径值矩阵和最短路径点矩阵，其中，最短路径值矩阵用于存储路网地图中任意两个路网点之间的最短距离，最短路径点矩阵用于存储路网地图中任意两个路网点之间的最短中转点；其中，最短路径矩阵包括最短路径值矩阵和最短路径点矩阵。

路网更新时，首先将路网转换为二维矩阵，由路网点Point ID（以ID为点标识）生成对应的矩阵索引index，矩阵内的值为两点间的距离权值，距离权值通过欧式距离表示。路网一般默认为无向的，即从p1点到p2点的距离为a时，从p2点到p1点的距离同样为a，图3是本发明实施例中路网无向图的示意图，示例了p1~ p6，五个路网点，其Point ID分别对应为0~5。两路网点的距离权值可以由公式：

考虑到巡检机器人的一般应用场景，或使用2D导航时，可以忽略路网点的Z值(z=0)。

在转换的原始二维矩阵Matrix中，行列索引相同的值设置为0，即路网点到其自身的距离权值为0。当路网中两点没有直接联通关系时，认为两点不可通行，将其对应索引行列的值设置为无穷大∞。当路网为无向图时，有

Matrix[i][j]== Matrix[j][i]（2）

二维矩阵Matrix第i行第j列的值Matrix[i][j]（即i对应的路网点到j对应路网点的距离权值）与第j行第i列的值Matrix[j][i]（即j对应的路网点到i对应路网点的距离权值）相等；而当路网为有向图时，在矩阵中两者可不相等。有向图与无向图的选择可根据实际的巡检业务需求设定，若为有向图，在原矩阵上同样可以实现，本实施例以无向图为实现。表1示意了本发明实施例中原始二维矩阵，如Matrix[0][0]=0，Matrix[0][1]= Matrix[1][0]=10.2。

表1

在本实施例的一个示例中，采用原始二维矩阵创建最短路径值矩阵包括：遍历原始二维矩阵中的每个矩阵元素 [i][j]，在路网地图中选择任意的路网点k作为中转点；在原始二维矩阵中获取矩阵元素 [i][j]的第一距离Matrix[i][j]，矩阵元素 [i][k]的第二距离Matrix[i][k]，矩阵元素 [k][j]的第三距离Matrix[k][j]；判断Matrix[i][j]是否大于Matrix[i][k]+ Matrix[k][j]；若Matrix[i][j]大于Matrix[i][k]+ Matrix[k][j]，在第一初始矩阵中将Matrix[i][k]+ Matrix[k][j]配置为矩阵元素[i][j]的最短路径值；若Matrix[i][j]小于或等于Matrix[i][k]+ Matrix[k][j]，在第一初始矩阵中将Matrix[i][j]配置为矩阵元素[i][j]的最短路径值；在原始二维矩阵中的所有矩阵元素遍历完成之后，得到最短路径值矩阵。

在本实施例的另一个示例中，采用最短路径值矩阵创建最短路径点矩阵包括：针对最短路径值矩阵中的每两个矩阵元素 [i][j]，判断最短路径值的路径是否经过中转点；若最短路径值的路径经过中转点，在第二初始矩阵中将对应中转点的点标识配置为矩阵元素[i][j]的最短路径点；若最短路径值的路径不经过中转点，在第二初始矩阵中将预设值配置为矩阵元素[i][j]的最短路径点；在最短路径值矩阵中的所有矩阵元素遍历完成之后，得到最短路径点矩阵。

在本实施例中，利用动态规划的相关算法计算最短路径的算法，当要求路网中任意的路网点i到路网点j的最短距离时，可能存在两点不能直接到达或者可以直接到达但不是最短路径的问题。算法引入第三个路网点k作为中转点，即i->k->j时会缩短从路网点i到路网点j的路径距离,所以选择某个路网点k作为中转点，通过遍历原始二维矩阵，计算任意路网点i和j经过k时的路径距离，当Matrix[i][j]＞Matrix[i][k]+ Matrix[k][j]（3）时，即经过路网点k时（若存在多个符合公式（3）的路网点，则可以在穷尽所有路网点k之后，可以选择一个中转距离最短的一个），路径最短，创建最短路径值矩阵，使得

Matrix[i][j] ==Matrix[i][k]+ Matrix[k][j]（4）

同时，创建中转点矩阵Matrix_k，将使得当前路径最短的中转点k保存在矩阵中，即Matrix_k[i][j]=k（5）

图4是本发明实施例中转更新路网的流程图，其中n为路网点的总个数，实际的最短路径可能会经过多个中转点，所以需要遍历二维矩阵，将任意点设置为中转点k,重复进行上述公式（3）（4）（5）的操作。

图5是本发明实施例以每个路网点作为中转点的更新路网流程图，由于每次中转点遍历时，已经对最短路径值矩阵进行了更新，所以遍历最终会得到保存有任意俩路网点i和j全局最短路径值的二维值矩阵，以及保存中转点k的点矩阵。表2示意了最短路径值矩阵，在该矩阵中，每个元素的值表示两个路网点之间的最短距离，如Matrix[0][1]=10.2，Matrix[0][2]=13.1。

表2

表3示意了最短路径点矩阵，在该矩阵中，预设的默认值为-1，表示不存在中转路径点，如，Matrix[0][1]=-1，表示路网点1和路网点2之间没有中转路径点（两点之间的欧式距离即为最短距离），每个元素的值表示两个路网点之间的最短中转点，在矩阵中用点标识表示，如Matrix[0][3]=1，表示路网点1和路网点4之间的最短路径点为路网点1，Matrix[2][1]=4，表示路网点2和路网点1之间的最短路径点为路网点4。

表3

当要获取最短路径对应的路网点列表时，可以从点矩阵中递归获取中转点，将i->j转变为i->k0(matrix_k[i][j])->j，再获取i->k1(matrix_k[i][k0])->k0,和k0->k2(matrix_k[k0][j])->j，依此递归直到中转点矩阵的matrix_k[kn][km]值为默认值-1，最终得到最短路径对应的路网点序列路径。图6是本发明实施例递归获取路径端点的流程图，若设置的路径端点为[5][3]，首先通过表3所示的矩阵确定其最短路径点为路径点4，然后继续确定路径端点[5][4]和路径端点[4][3]的最短路径点，分别为路径点2和路径点1，然后继续确定路径端点[5][2] 和路径端点[2][4]，路径端点[4][1] 和路径端点[1][3] 的最短路径点，直到确定的矩阵值为-1，则终止递归。

在本实施例的一个实施方式中，根据起始位置在路网地图中查找距离机器人最近的路网起点包括：计算起始位置到路网地图中任意路网点的点点距离，并选择距离起始位置最近的第一路网点，并记录最近的第一点点距离；计算起始位置到路网地图中任意路网路径的点线距离，并选择点线距离最短的第一路网路径，并记录所述起始位置到所述第一路网路径的第一点线距离，其中，路网路径是任意两个路网点连成的路径段，第一路网路径的两端点为第二路网点和第三路网点；判断第一点线距离是否小于第一点点距离；若第一点线距离小于第一点点距离，在第二路网点和第三路网点中选择距离起始位置的点点距离最近的点为路网起点；若第一点线距离大于或等于第一点点距离，选择第一路网点为路网起点。

在进行路网规划时，需要获取机器人当前实时位置作为参考起点，由于参考起并不在预先设置的路网上，所以需要据此计算路网规划的起始路网点。

获取路网起点时，使用公式（1）计算当前位置到任意路网点的距离dis_point-point（点点距离），通过遍历可得到距离当前位置最近的路网点。

同时考虑到实际路网部署往往与现实道路相对应，仅依靠两点间距离时，可能存在跨越路网路径（即驶离道路）的不安全路线生成，所以需计算当前位置到路网路径的距离值dis_{point-segmentt}（点线距离），图7是本发明实施例计算机器人到路网路径的距离的示意图，P点为起始位置，C点为P点到路网路径AB的投影点，θ为AP与AB之间的夹角，设置r为向量AB和AP的点积与向量AB模长平方的比值。

遍历路网路径，可以求取距离当前位置最近的路径及其两端路网点。考虑到机器人运行的安全性，一般需设定阈值，使得获取的dis_{point-segmentt}小于dis_point-point不超过该阈值。当dis_{point-segmentt}小于dis_point-point时，将使用该路径上距离参考起点最近的端点作为路网起点，否则以dis_point-point对应的路网点作为路网起点。

在本实施例中，以路网起点为起始点，基于最短路径矩阵和任务地图遍历生成机器人待途经的有序任务点队列包括：根据任务地图生成机器人待途经的无序任务点序列；以路网起点为起始点，以无序任务点序列中的每个任务点为终点，在最短路径值矩阵中查找起始点与每个终点之间的最短距离，得到最短距离集合，其中，最短路径矩阵包括最短路径值矩阵，最短路径值矩阵用于存储路网地图中任意两个路网点之间的最短距离；基于最短距离集合在无序任务点序列中查找最短距离最小的第一任务点，并将第一任务点加入有序任务点队列，在无序任务点序列中删除第一任务点；以当前加入有序任务点队列的任务点为起始点，无序任务点序列中剩余的每个任务点为终点，继续遍历无序任务点序列中剩余的每个任务点，直到无序任务点序列为空，得到完整的有序任务点队列。

可选的，基于有序任务点队列和最短路径矩阵生成机器人待途经的有序非任务点队列包括：从有序任务点队列中按序读取相邻的任务段，其中，任务段由相邻的第二任务点和第三任务点组成；针对每个任务段，从最短路径点矩阵中递归读取当前任务段经过的中转点集合，其中，最短路径矩阵包括最短路径点矩阵，最短路径点矩阵用于存储路网地图中任意两个路网点之间的最短中转点；按序组合所有任务段的中转点集合，得到完整的有序非任务点队列。

只考虑距离当前起点最近的任务终点，不依赖于已做出的选择，使得规划路径在当前局部是最优的解。

当获取到下发的任务点序列时，基于贪心策略，将上述获取到的路网起点作为起始点start，从任务点队列中选取某个任务点作为终点end，从已完成更新的最短路径值矩阵中可获取到起始点与终点的最短距离matrix[start][end]，遍历任务点队列并比较各点与起点的距离，直到获取到与起始点最近的任务点。然后将此任务点从任务点队列中取出，并作为下一次计算的起始点，遍历计算直到任务点队列为空。图8是本发明实施例获取有序任务点队列的流程示意图。

当得到有序的任务点队列后，则可以从全局最短路径中转点矩阵中递归取出相邻俩任务点间经过的若干中转点，最终获取到全局路径，其中第一段路径为当前位置到路网起点。

在机器人从路网点start前往路网点end的运行过程中，会出现遇到障碍物导致当前任务点无法正常到达，或任务点不安全的情况，此时需要基于当前的运行状态对任务进行重规划。如果是任务点不安全，则需要从任务队列中剔除该任务点，同时将路网中该点设置为不可连通，即该点对应的矩阵索引行列值均为无穷大；如果是中途遇障或者中转点不安全，则可以将路网中start->end的路径设置为不可通行或中转点相连接的路径设置为不可通行，对路网的重规划前设置是临时性的。然后再进行路网更新，以及贪心策略寻找重规划路径，应该注意的是，重规划时不再需要进行获取路网起点操作，而直接使用遇障时路径起点start作为规划的路网起点。

本实施例的方案涉及一种针对巡检机器人应用的路网路径规划算法。算法基于事先部署的路网（包含点集及其连通关系），同时使用实时反馈的机器人所在地图位置，首先寻找到当前最近且安全的路网起点，然后对业务需执行的任务点集进行快速地全局路径搜索规划，生成遍历所有任务点集的局部最优的路网路径，以便最终由局部规划与控制模块执行此全局规划路径。流程包括：

第一步，根据业务需求，基于导航地图信息和实时定位，人工部署任务点及辅助点，设置路网以及地图虚拟墙等信息；

第二步，输入设置好的路网，以及地图虚拟墙、遇障状态等信息，进行路网更新，生成最短路径值矩阵与最短路径点矩阵，矩阵的索引由路网点ID生成；

第三步，基于当前实时位置，计算距离机器人最近的路网点或路网路径，生成路网起点，当前点到路网起点为全局路径的第一段；

第四步，从路网起点开始，基于贪心策略遍历任务点集，从最短路径值矩阵中获取离当前点最短的任务点，然后将此任务点作为下一次遍历的起点，最终得到局部最优的任务点顺序。然后依据任务点顺序，从最短路径点矩阵中递归查找途径的非任务点，最终生成全局路径。

本实施例提出了一种基于路网的全局路径规划算法，将算法分解成路网更新、获取路网起点及任务规划三个环节，使得算法可以在未获取到任务时即可进行预处理，同时可以快速进行多任务调度。改进了路网的遇障重规划策略。基于路网的全局规划算法，将规划任务拆解成多个子任务，通过在任务规划前进行路网更新预处理，获取到路网点间的最短规划路径，减少多路网点任务规划阶段的算法耗时，提高任务响应速度。算法简洁、效率高，同时解决了巡检过程中遇障重规划问题。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到根据上述实施例的方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质（如ROM/RAM、磁碟、光盘）中，包括若干指令用以使得一台终端设备（可以是手机，计算机，服务器，或者网络设备等）执行本发明各个实施例所述的方法。

实施例2

在本实施例中还提供了一种机器人路径的生成装置，用于实现上述实施例及优选实施方式，已经进行过说明的不再赘述。如以下所使用的，术语“模块”可以实现预定功能的软件和/或硬件的组合。尽管以下实施例所描述的装置较佳地以软件来实现，但是硬件，或者软件和硬件的组合的实现也是可能并被构想的。

图9是根据本发明实施例的一种机器人路径的生成装置的结构框图，如图9所示，该装置包括：获取模块90，构建模块92，查找模块94，第一生成模块96，第二生成模块98，其中，

获取模块90，用于定位机器人的起始位置，以及获取路网地图，其中，所述路网地图包括多个路网点及其连通关系；

构建模块92，用于采用所述路网地图构建所述多个路网点的最短路径矩阵；

查找模块94，用于根据所述起始位置在所述路网地图中查找距离所述机器人最近的路网起点；

第一生成模块96，用于将所述起始位置到所述路网起点之间的路径确定为全局路径的起始段，获取任务地图，以所述路网起点为起始点，基于所述最短路径矩阵和所述任务地图遍历生成所述机器人待途经的有序任务点队列，其中，所述任务地图包括所述机器人在所述路网地图中待触达的多个任务点；

第二生成模块98，用于基于所述任务点队列和所述最短路径矩阵生成所述机器人待途经的有序非任务点队列，并采用所述任务点队列和所述非任务点队列生成所述全局路径的剩余段，其中，所述全局路径包括所述起始段和所述剩余段。

可选的，所述构建模块包括：计算单元，用于计算所述路网地图中的每两个路网点之间的欧式距离；获取单元，用于获取所述路网地图中的每个路网点的点标识；第一创建单元，用于采用所述点标识和所述欧式距离创建原始二维矩阵，其中，所述原始二维矩阵的行列数量均与所述点标识的数量相同，矩阵元素 [i][j]的元素值Matrix[i][j]为路网点i到路网点j的欧式距离，i，j均为点标识；第二创建单元，用于采用所述原始二维矩阵创建最短路径值矩阵和最短路径点矩阵，其中，所述最短路径值矩阵用于存储所述路网地图中任意两个路网点之间的最短距离，所述最短路径点矩阵用于存储所述路网地图中任意两个路网点之间的最短中转点；其中，所述最短路径矩阵包括所述最短路径值矩阵和所述最短路径点矩阵。

可选的，所述第二创建单元包括：选择子单元，用于遍历所述原始二维矩阵中的每个矩阵元素 [i][j]，在所述路网地图中选择任意的路网点k作为中转点；获取子单元，用于在所述原始二维矩阵中获取矩阵元素 [i][j]的第一距离Matrix[i][j]，矩阵元素 [i][k]的第二距离Matrix[i][k]，矩阵元素 [k][j]的第三距离Matrix[k][j]；第一判断子单元，用于判断Matrix[i][j] 是否大于Matrix[i][k]+ Matrix[k][j]；第一配置子单元，用于若Matrix[i][j]大于Matrix[i][k]+ Matrix[k][j]，在第一初始矩阵中将Matrix[i][k]+Matrix[k][j]配置为矩阵元素[i][j]的最短路径值；若Matrix[i][j]小于或等于Matrix[i][k]+ Matrix[k][j]，在第一初始矩阵中将Matrix[i][j]配置为矩阵元素[i][j]的最短路径值；第一生成子单元，用于在所述原始二维矩阵中的所有矩阵元素遍历完成之后，得到最短路径值矩阵。

可选的，所述第二创建单元包括：第二判断子单元，用于针对所述最短路径值矩阵中的每两个矩阵元素 [i][j]，判断最短路径值的路径是否经过中转点；第二配置子单元，用于若最短路径值的路径经过中转点，在第二初始矩阵中将对应中转点的点标识配置为矩阵元素[i][j]的最短路径点；若最短路径值的路径不经过中转点，在第二初始矩阵中将预设值配置为矩阵元素[i][j]的最短路径点；第二生成子单元，用于在所述最短路径值矩阵中的所有矩阵元素遍历完成之后，得到最短路径点矩阵。

可选的，所述查找模块包括：第一计算单元，用于计算所述起始位置到所述路网地图中任意路网点的点点距离，并选择距离所述起始位置最近的第一路网点，并记录最近的第一点点距离；第二计算单元，用于计算所述起始位置到所述路网地图中任意路网路径的点线距离，并选择点线距离最短的第一路网路径，并记录所述起始位置到所述第一路网路径的第一点线距离，其中，所述路网路径是任意两个路网点连成的路径段，所述第一路网路径的两端点为第二路网点和第三路网点；判断单元，用于判断所述第一点线距离是否小于所述第一点点距离；选择单元，用于若所述第一点线距离小于所述第一点点距离，在所述第二路网点和所述第三路网点中选择距离所述起始位置的点点距离最近的点为路网起点；若所述第一点线距离大于或等于所述第一点点距离，选择所述第一路网点为路网起点。

可选的，所述第一生成模块包括：生成单元，用于根据所述任务地图生成所述机器人待途经的无序任务点序列；查找单元，用于以所述路网起点为起始点，以所述无序任务点序列中的每个任务点为终点，在最短路径值矩阵中查找所述起始点与每个终点之间的最短距离，得到最短距离集合，其中，所述最短路径矩阵包括所述最短路径值矩阵，所述最短路径值矩阵用于存储所述路网地图中任意两个路网点之间的最短距离；处理单元，用于基于所述最短距离集合在所述无序任务点序列中查找最短距离最小的第一任务点，并将所述第一任务点加入有序任务点队列，在所述无序任务点序列中删除所述第一任务点；生成单元，用于以当前加入有序任务点队列的任务点为起始点，所述无序任务点序列中剩余的每个任务点为终点，继续遍历所述无序任务点序列中剩余的每个任务点，直到所述无序任务点序列为空，得到完整的有序任务点队列。

可选的，所述第二生成模块包括：第一读取单元，用于从所述有序任务点队列中按序读取相邻的任务段，其中，所述任务段由相邻的第二任务点和第三任务点组成；第二读取单元，用于针对每个任务段，从最短路径点矩阵中递归读取当前任务段经过的中转点集合，其中，所述最短路径矩阵包括所述最短路径点矩阵，所述最短路径点矩阵用于存储所述路网地图中任意两个路网点之间的最短中转点；生成单元，用于按序组合所有任务段的中转点集合，得到完整的有序非任务点队列。

需要说明的是，上述各个模块是可以通过软件或硬件来实现的，对于后者，可以通过以下方式实现，但不限于此：上述模块均位于同一处理器中；或者，上述各个模块以任意组合的形式分别位于不同的处理器中。

实施例3

本发明的实施例还提供了一种存储介质，该存储介质中存储有计算机程序，其中，该计算机程序被设置为运行时执行上述任一项方法实施例中的步骤。

可选地，在本实施例中，上述存储介质可以被设置为存储用于执行以下步骤的计算机程序：

S1，定位机器人的起始位置，以及获取路网地图，其中，所述路网地图包括多个路网点及其连通关系；

S2，采用所述路网地图构建所述多个路网点的最短路径矩阵；

S3，根据所述起始位置在所述路网地图中查找距离所述机器人最近的路网起点；

S4，将所述起始位置到所述路网起点之间的路径确定为全局路径的起始段，获取任务地图，以所述路网起点为起始点，基于所述最短路径矩阵和所述任务地图遍历生成所述机器人待途经的有序任务点队列，其中，所述任务地图包括所述机器人在所述路网地图中待触达的多个任务点；

S5，基于所述任务点队列和所述最短路径矩阵生成所述机器人待途经的有序非任务点队列，并采用所述任务点队列和所述非任务点队列生成所述全局路径的剩余段，其中，所述全局路径包括所述起始段和所述剩余段。

可选地，在本实施例中，上述存储介质可以包括但不限于：U盘、只读存储器（Read-Only Memory，简称为ROM）、随机存取存储器（Random Access Memory，简称为RAM）、移动硬盘、磁碟或者光盘等各种可以存储计算机程序的介质。

本发明的实施例还提供了一种电子设备，包括存储器和处理器，该存储器中存储有计算机程序，该处理器被设置为运行计算机程序以执行上述任一项方法实施例中的步骤。

可选地，上述电子设备还可以包括传输设备以及输入输出设备，其中，该传输设备和上述处理器连接，该输入输出设备和上述处理器连接。

可选地，在本实施例中，上述处理器可以被设置为通过计算机程序执行以下步骤：

S1，定位机器人的起始位置，以及获取路网地图，其中，所述路网地图包括多个路网点及其连通关系；

S2，采用所述路网地图构建所述多个路网点的最短路径矩阵；

S3，根据所述起始位置在所述路网地图中查找距离所述机器人最近的路网起点；

S4，将所述起始位置到所述路网起点之间的路径确定为全局路径的起始段，获取任务地图，以所述路网起点为起始点，基于所述最短路径矩阵和所述任务地图遍历生成所述机器人待途经的有序任务点队列，其中，所述任务地图包括所述机器人在所述路网地图中待触达的多个任务点；

S5，基于所述任务点队列和所述最短路径矩阵生成所述机器人待途经的有序非任务点队列，并采用所述任务点队列和所述非任务点队列生成所述全局路径的剩余段，其中，所述全局路径包括所述起始段和所述剩余段。

可选地，本实施例中的具体示例可以参考上述实施例及可选实施方式中所描述的示例，本实施例在此不再赘述。

上述本申请实施例序号仅仅为了描述，不代表实施例的优劣。

在本申请的上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。

在本申请所提供的几个实施例中，应该理解到，所揭露的技术内容，可通过其它的方式实现。其中，以上所描述的装置实施例仅仅是示意性的，例如所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，单元或模块的间接耦合或通信连接，可以是电性或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备（可为个人计算机、服务器或者网络设备等）执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、只读存储器（ROM，Read-Only Memory）、随机存取存储器（RAM，Random Access Memory）、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述仅是本申请的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本申请原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本申请的保护范围。

Claims (9)

1.一种机器人路径的生成方法，其特征在于，包括：

定位机器人的起始位置，以及获取路网地图，其中，所述路网地图包括多个路网点及其连通关系；

采用所述路网地图构建所述多个路网点的最短路径矩阵；

根据所述起始位置在所述路网地图中查找距离所述机器人最近的路网起点；

将所述起始位置到所述路网起点之间的路径确定为全局路径的起始段，获取任务地图，以所述路网起点为起始点，基于所述最短路径矩阵和所述任务地图遍历生成所述机器人待途经的有序任务点队列，其中，所述任务地图包括所述机器人在所述路网地图中待触达的多个任务点；

基于所述任务点队列和所述最短路径矩阵生成所述机器人待途经的有序非任务点队列，并采用所述任务点队列和所述非任务点队列生成所述全局路径的剩余段，其中，所述全局路径包括所述起始段和所述剩余段；

其中，采用所述路网地图构建所述多个路网点的最短路径矩阵包括：计算所述路网地图中的每两个路网点之间的欧式距离；获取所述路网地图中的每个路网点的点标识；采用所述点标识和所述欧式距离创建原始二维矩阵，其中，所述原始二维矩阵的行列数量均与所述点标识的数量相同，矩阵元素 [i][j]的元素值Matrix[i][j]为路网点i到路网点j的欧式距离，i，j均为点标识；采用所述原始二维矩阵创建最短路径值矩阵和最短路径点矩阵，其中，所述最短路径值矩阵用于存储所述路网地图中任意两个路网点之间的最短距离，所述最短路径点矩阵用于存储所述路网地图中任意两个路网点之间的最短中转点；其中，所述最短路径矩阵包括所述最短路径值矩阵和所述最短路径点矩阵。

2.根据权利要求1所述的方法，其特征在于，采用所述原始二维矩阵创建最短路径值矩阵包括：

遍历所述原始二维矩阵中的每个矩阵元素 [i][j]，在所述路网地图中选择任意的路网点k作为中转点；

在所述原始二维矩阵中获取矩阵元素 [i][j]的第一距离Matrix[i][j]，矩阵元素[i][k]的第二距离Matrix[i][k]，矩阵元素 [k][j]的第三距离Matrix[k][j]；

判断Matrix[i][j]是否大于Matrix[i][k]+ Matrix[k][j]；

若Matrix[i][j]大于Matrix[i][k]+Matrix[k][j]，在第一初始矩阵中将Matrix[i][k]+ Matrix[k][j]配置为矩阵元素[i][j]的最短路径值；若Matrix[i][j]小于或等于Matrix[i][k]+ Matrix[k][j]，在第一初始矩阵中将Matrix[i][j]配置为矩阵元素[i][j]的最短路径值；

在所述原始二维矩阵中的所有矩阵元素遍历完成之后，得到最短路径值矩阵。

3.根据权利要求2所述的方法，其特征在于，采用所述最短路径值矩阵创建最短路径点矩阵包括：

针对所述最短路径值矩阵中的每两个矩阵元素 [i][j]，判断最短路径值的路径是否经过中转点；

若最短路径值的路径经过中转点，在第二初始矩阵中将对应中转点的点标识配置为矩阵元素[i][j]的最短路径点；若最短路径值的路径不经过中转点，在第二初始矩阵中将预设值配置为矩阵元素[i][j]的最短路径点；

在所述最短路径值矩阵中的所有矩阵元素遍历完成之后，得到最短路径点矩阵。

4.根据权利要求1所述的方法，其特征在于，根据所述起始位置在所述路网地图中查找距离所述机器人最近的路网起点包括：

计算所述起始位置到所述路网地图中任意路网点的点点距离，并选择距离所述起始位置最近的第一路网点，并记录最近的第一点点距离；

计算所述起始位置到所述路网地图中任意路网路径的点线距离，并选择点线距离最短的第一路网路径，并记录所述起始位置到所述第一路网路径的第一点线距离，其中，所述路网路径是任意两个路网点连成的路径段，所述第一路网路径的两端点为第二路网点和第三路网点；

判断所述第一点线距离是否小于所述第一点点距离；

若所述第一点线距离小于所述第一点点距离，在所述第二路网点和所述第三路网点中选择距离所述起始位置的点点距离最近的点为路网起点；若所述第一点线距离大于或等于所述第一点点距离，选择所述第一路网点为路网起点。

5.根据权利要求1所述的方法，其特征在于，以所述路网起点为起始点，基于所述最短路径矩阵和所述任务地图遍历生成所述机器人待途经的有序任务点队列包括：

根据所述任务地图生成所述机器人待途经的无序任务点序列；

以所述路网起点为起始点，以所述无序任务点序列中的每个任务点为终点，在最短路径值矩阵中查找所述起始点与每个终点之间的最短距离，得到最短距离集合，其中，所述最短路径矩阵包括所述最短路径值矩阵，所述最短路径值矩阵用于存储所述路网地图中任意两个路网点之间的最短距离；

基于所述最短距离集合在所述无序任务点序列中查找最短距离最小的第一任务点，并将所述第一任务点加入有序任务点队列，在所述无序任务点序列中删除所述第一任务点；

以当前加入有序任务点队列的任务点为起始点，所述无序任务点序列中剩余的每个任务点为终点，继续遍历所述无序任务点序列中剩余的每个任务点，直到所述无序任务点序列为空，得到完整的有序任务点队列。

6.根据权利要求1所述的方法，其特征在于，基于所述有序任务点队列和所述最短路径矩阵生成所述机器人待途经的有序非任务点队列包括：

从所述有序任务点队列中按序读取相邻的任务段，其中，所述任务段由相邻的第二任务点和第三任务点组成；

针对每个任务段，从最短路径点矩阵中递归读取当前任务段经过的中转点集合，其中，所述最短路径矩阵包括所述最短路径点矩阵，所述最短路径点矩阵用于存储所述路网地图中任意两个路网点之间的最短中转点；

按序组合所有任务段的中转点集合，得到完整的有序非任务点队列。

7.一种机器人路径的生成装置，其特征在于，包括：

获取模块，用于定位机器人的起始位置，以及获取路网地图，其中，所述路网地图包括多个路网点及其连通关系；

构建模块，用于采用所述路网地图构建所述多个路网点的最短路径矩阵；

查找模块，用于根据所述起始位置在所述路网地图中查找距离所述机器人最近的路网起点；

第一生成模块，用于将所述起始位置到所述路网起点之间的路径确定为全局路径的起始段，获取任务地图，以所述路网起点为起始点，基于所述最短路径矩阵和所述任务地图遍历生成所述机器人待途经的有序任务点队列，其中，所述任务地图包括所述机器人在所述路网地图中待触达的多个任务点；

第二生成模块，用于基于所述任务点队列和所述最短路径矩阵生成所述机器人待途经的有序非任务点队列，并采用所述任务点队列和所述非任务点队列生成所述全局路径的剩余段，其中，所述全局路径包括所述起始段和所述剩余段；

其中，所述构建模块包括：计算单元，用于计算所述路网地图中的每两个路网点之间的欧式距离；获取单元，用于获取所述路网地图中的每个路网点的点标识；第一创建单元，用于采用所述点标识和所述欧式距离创建原始二维矩阵，其中，所述原始二维矩阵的行列数量均与所述点标识的数量相同，矩阵元素 [i][j]的元素值Matrix[i][j]为路网点i到路网点j的欧式距离，i，j均为点标识；第二创建单元，用于采用所述原始二维矩阵创建最短路径值矩阵和最短路径点矩阵，其中，所述最短路径值矩阵用于存储所述路网地图中任意两个路网点之间的最短距离，所述最短路径点矩阵用于存储所述路网地图中任意两个路网点之间的最短中转点；其中，所述最短路径矩阵包括所述最短路径值矩阵和所述最短路径点矩阵。

8.一种存储介质，其特征在于，所述存储介质包括存储的计算机程序，其中，所述计算机程序运行时执行上述权利要求1至6中任一项所述的方法的步骤。

9.一种电子设备，包括处理器、通信接口、存储器和通信总线，其中，处理器，通信接口，存储器通过通信总线完成相互间的通信；其中：

存储器，用于存放计算机程序；

处理器，用于通过运行存储器上所存放的计算机程序来执行权利要求1至6中任一项所述的方法的步骤。

Images