CN104914866A - 基于拓扑点分类的巡检机器人全局路径规划方法及系统 - Google Patents

Abstract

本发明公开了一种基于拓扑点分类的巡检机器人全局路径规划方法及系统，考虑到变电站内机器人巡检路径均为直线路径，设备区外机器人停靠点较少，而设备区内机器人停靠点则均在同一直线路径上，基于这一特性而设计的全局路径规划方法，可有效降低路径规划计算和存储消耗，满足机器人根据特殊巡检任务在线规划机器人行驶路径的需求；通过停靠点的拓扑点据所属路径进行分类合并，建立对应拓扑点的有向图数据结构，之后在路径规划时则仅将机器人当前位置与目标位置拓扑点插入该有向图数据结构，可显著降低参与计算的拓扑点数量，提高了路径规划的计算效率并降低对存储空间的消耗。

Description

基于拓扑点分类的巡检机器人全局路径规划方法及系统

技术领域

本发明涉及一种基于拓扑点分类的巡检机器人全局路径规划方法及系统。

背景技术

变电站巡检机器人是一种全自主运行的地面移动机器人，其可携带可见光摄像机、红外热像仪、拾音器等传感器对电力设备进行自动巡检，有效降低人工巡检的劳动强度，保障站内设备安全运行。

在变电站常规巡检过程中，巡检机器人需要沿巡检任务规定路线在站内运行，并在一系列预先设定的停靠点处进行停靠以完成对电力设备的检测，但在变电站顺控或其它突发情况发生时，机器人需执行特殊巡检任务，此时需要规划机器人运行路线以得到机器人当前位置与目标位置间的最短路径，保证机器人迅速到达站内指定位置进行检测。

专利201110216379.6公布了一种基于磁导航变电站巡检机器人的全局路径规划方法，该方法通过对机器人运行路线上的停靠点及路径交叉点抽象为有向图数据结构的节点，之后利用Floyd(弗洛伊德)路径搜索算法计算环境模型拓扑地图中各节点间的最短路径，实现了机器人在站内顺控操作执行特殊巡检任务时沿最短路径运行，但该方法实际使用过程中还面临以下问题：

(1)该方法计算所有节点间任意两点的最短路径并存储，由于所用算法需要三重循环实现，这样算法运行时间与参与计算路径点数量的3次方成正比，而变电站内机器人停靠点一般数量较大，从而使参与计算的拓扑点也较多，导致该方法最短路径搜索效率不高，同时计算得到的最短路径的所需存储量也较大；

(2)变电站某些突发情况发生时，可能需要机器人迅速到达站内临时设定的停靠点执行检测任务，鉴于(1)所述原因，该方法只能采用离线方式计算节点间的最短路径，一旦计算并存储完成，每一个新节点的加入都必须对所有节点重新计算和存储，之后才能输出针对新加入节点的最短路径，因此难以实现对机器人巡检路径的在线规划。

发明内容

本发明为了解决上述问题，提出了一种基于拓扑点分类的巡检机器人全局路径规划方法及系统，本方法可有效降低路径规划计算和存储消耗，满足机器人根据特殊巡检任务在线规划机器人行驶路径的需求。

为了实现上述目的，本发明采用如下技术方案：

一种基于拓扑点分类的巡检机器人全局路径规划方法，包括以下步骤：

(1)在机器人运行路线上建立拓扑点；

(2)对所有拓扑点按路径分类，得到不同的拓扑点集合，并将每个集合中包含的所有拓扑点合并形成一个合并点；

(3)基于上述系建立以每条路径起点、终点和合并点为节点的有向图数据结构，确定拓扑点之间的弧长；

(4)当需要进行最短路径规划时，依据巡检任务给定的机器人当前位置和目标点位置，并寻找有向图中对应的拓扑点，并判断两个拓扑点所属集合；

(5)如果机器人当前位置和目标点位置所对应拓扑点属于同一拓扑点集，则表示机器人当前位置和目标点属于同一路径，此时无需执行复杂的路径规划算法，直接判定这两点的相对位置关系，并输出至巡检机器人执行；否则，执行步骤(6)；

(6)根据步骤(4)确定机器人当前位置和目标点距各自所属路径起点与终点间距离，更新该路径上合并点与路径起点和终点之间弧的权值；

(7)计算巡检机器人当前点与目标点之间的最短路径，并输出至机器人执行。

所述步骤(1)中，建立拓扑点,将机器人运行路线上的停靠点及路径交叉点抽象为有向图数据结构的拓扑点。

所述步骤(2)的具体方法为：将拓扑点按路径分类，具体包括：

(2-1)将巡检路线划分为巡检路径集合，每条巡检路径只通过起点和终点与其他路径连接；

(2-2)将所有停靠点的拓扑点据所属路径进行分类，得到每条路径包含的拓扑点集合，将该集合中的所有拓扑点合并，形成一个合并点，后续路径规划过程中合并点可以表示该路径上拓扑点集合内的任意一个拓扑点，参与路径规划计算。

所述步骤(3)的具体方法为：建立以每条路径起点、终点和合并点为节点的有向图数据结构G＝(V,A)，其中V为有向图数据结构包含的拓扑点集，假设有n个点，则Vi表示第i个拓扑点，而A则表示两拓扑点之间的弧集，每条弧的初始权值按下述步骤确定：

(a)每条路径起点和终点对应拓扑点间弧的权值初始化为该条路径的长度；

(b)合并点对应拓扑点与所属路径起点和终点对应拓扑点间弧的权值初始化为∞。

所述步骤(3)中，形成的有向图数据结构以临接矩阵方式进行存储，临接矩阵中A[i][j]表示弧＜v_i,v_j＞的权值，若弧＜v_i,v_j＞不存在或按合并点初始化权值设置则置A[i][j]为∞，其中i,j＝1,2,…,n，弧＜v_i,v_j＞表示为拓扑点v_i与v_j之间的弧。

所述步骤(4)中，当需要进行最短路径规划时，依据巡检任务给定的机器人当前位置和目标点位置，并寻找有向图中对应的拓扑点v_s和v_e，判定这两点所属步骤2建立的每条路径包含拓扑点的集合。

所述步骤(7)中，所述路径规划算法为Dijkstra(迪杰斯特拉)算法，具体执行步骤包括：

(7-1)定义S为已找到从机器人当前位置拓扑点v_s出发的最短路径的终点集合，它的初始状态为空集，那么从v_s出发到有向图上其余各顶点v_t可能达到的最短路径长度D[s]的初始值为：

D[s]＝A[s][t] v_s∈V，s,t＝1,2,…,n

(7-2)选择v_t，使得D[t]＝min{D[s]|v_s∈V-S}

v_t就是当前求得的一条从v_s出发的最短路径的终点，之后令S＝S∪{t}，从而将拓扑点t存入最短路径终点集合；

(7-3)修改从v_s出发到集合V-S上任意一个顶点v_k达到的最短路径长度D[k]，如果D[t]+A[t][k]＜D[k]，则修改D[k]为D[k]＝D[t]+A[t][k]

(7-4)重复操作(7-2)(7-3)，由此求得从v_s到有向图上其余各拓扑点的最短路径，之后依据目标点位置信息从S中选择机器人从当前点至该点最短路径所包含的拓扑点集合，从而完成巡检机器人路径规划。

一种应用上述方法的全局路径规划系统，包括基站控制模块、路径规划模块、运动控制模块和电机驱动模块，其中：

所述基站控制模块，用于存储巡检机器人巡检路线包含的所有停靠点及路径交叉点位置信息，常规巡检时期巡检任务直接下发巡检路径信息至运动控制模块执行，特殊巡检时则下发至路径规划模块；

所述路径规划模块，用于接收机器人基站控制模块下发的依据巡检任务，搜索巡检任务最短路径，并将需运行路径下发至运动控制模块执行；

所述运动控制模块，用于依据下发的需运行路径信息生成电机控制量，并下发至电机驱动模块；

所述电机驱动模块，用于接收运动控制模块的生成的电机控制量，依据电机轴编码器信号闭环控制控制巡检机器人驱动轮伺服电机，实现机器人在变电站内运行。

本发明的有益效果为：

(1)通过停靠点的拓扑点据所属路径进行分类合并，建立对应拓扑点的有向图数据结构，之后在路径规划时先判断机器人当前位置与目标位置是否属于同一路径，若属同一路径则无需复杂路径规划计算即可直接得到机器人运行路径，若不属同一路径则仅将机器人当前位置与目标位置拓扑点插入该有向图数据结构，计算方法简便，显著降低参与计算的拓扑点数量；

(2)考虑到变电站内机器人巡检路径均为直线路径，设备区外机器人停靠点较少，而设备区内机器人停靠点则均在同一直线路径上，基于巡检路径这一特点，设计全局规划方法，实用性强，适用范围广泛；

(3)提高了路径规划的计算效率并降低对存储空间的消耗，同时该路径规划方法可实现对机器人路径的在线规划，满足机器人特殊巡检任务需在线规划机器人行驶路径的需求。

附图说明

图1为本发明的路径规划执行流程图；

图2为本发明的拓扑点分类前有向图数据结构示意图；

图3为本发明的拓扑点分类后有向图数据结构示意图；

图4为本发明的变电站巡检机器人路径规划系统组成结构示意图。

具体实施方式：

下面结合附图与实施例对本发明作进一步说明。

如图1所示，本发明提出了一种基于拓扑点分类的路径规划方法，该方法包括以下步骤：

步骤1：拓扑点建立。将机器人运行路线上的停靠点及路径交叉点抽象为有向图数据结构的拓扑点，如图2所示；

步骤2：拓扑点按路径分类，包含以下子步骤：

(1)将巡检路线划分为巡检路径集合，每条巡检路径只通过起点和终点与其他路径连接；

(2)将所有停靠点的拓扑点据所属路径进行分类，得到每条路径包含的拓扑点集合，将该集合中的所有拓扑点合并，形成合并点，如图3所示虚线节点，后续路径规划过程中合并点可以表示该路径上拓扑点集合内的任意一个拓扑点，参与路径规划计算。

步骤3：建立以每条路径起点、终点和合并点为节点的有向图数据结构G＝(V,A)，其中V为有向图数据结构包含的拓扑点集，假设有n个点，则V＝{v_i|i＝1,2,…,n}，而A则表示两拓扑点之间的弧集，每条弧的初始权值按下述步骤确定：

(1)每条路径起点和终点对应拓扑点间弧的权值初始化为该条路径的长度；

(2)合并点对应拓扑点与所属路径起点和终点对应拓扑点间弧的权值初始化为∞，如图3所示虚线节点两侧虚线。

以上形成的有向图数据结构以临接矩阵方式进行存储，临接矩阵中A[i][j]表示弧＜v_i,v_j＞的权值，若弧＜v_i,v_j＞不存在或按上述(2)初始化则置A[i][j]为∞，其中i,j＝1,2,…,n；

步骤4：当需要进行最短路径规划时，依据巡检任务给定的机器人当前位置和目标点位置，并寻找有向图中对应的拓扑点v_s和v_e，判定这两点所属步骤2建立的每条路径包含拓扑点的集合；

步骤5：如果机器人当前位置和目标点位置所对应拓扑点v_s和v_e属于同一拓扑点集，则表示机器人当前位置和目标点属于同一路径，此时无需执行复杂的路径规划算法，就可以直接判定这两点的相对位置关系，并输出至巡检机器人执行。否则，执行步骤6；

步骤6：以步骤4选择的拓扑点v_s和v_e距各自所属路径起点与终点间距离，更新该路径上步骤3所述合并点与路径起点和终点之间弧的权值，从而将路径规划起始点和终止点加入所属路径，如图3所示路径规划起点和终点；

步骤7：利用路径规划算法计算巡检机器人当前点与目标点之间的最短路径，并输出至机器人执行。

上述步骤7中路径规划用拓扑点为图3中实线轮廓节点，虚线轮廓节点不参与计算，比较图2和3，参与计算路径节点数量分别为：46个和11个，两者比较，经拓扑点分类后得到的路径规划拓扑地图可显著降低参与路径规划计算的拓扑点数量，可有效提高路径规划计算效率并降低对存储空间的消耗，在变电站现场拓扑点数量较大情况下效果将更为明显。

步骤7所述路径规划算法为Dijkstra(迪杰斯特拉)算法，具体执行步骤如下：

(1)定义S为已找到从机器人当前位置拓扑点v_s出发的最短路径的终点集合，它的初始状态为空集。那么从v_s出发到有向图上其余各顶点v_t可能达到的最短路径长度D[s]的初始值为：

D[s]＝A[s][t] v_s∈V，s,t＝1,2,…,n

(2)选择v_t，使得

D[t]＝min{D[s]|v_s∈V-S}

v_t就是当前求得的一条从v_s出发的最短路径的终点，之后令S＝S∪{t}，从而将拓扑点t存入最短路径终点集合；

(3)修改从v_s出发到集合V-S上任意一个顶点v_k可达的最短路径长度D[k]。如果

D[t]+A[t][k]＜D[k]

则修改D[k]为

D[k]＝D[t]+A[t][k]

(4)重复操作(2)(3)，由此求得从v_s到有向图上其余各拓扑点的最短路径，之后依据目标点位置信息从S中选择机器人从当前点至该点最短路径所包含的拓扑点集合，从而完成巡检机器人路径规划。

如图4所示，基于以上全局路径规划方法构建了变电站巡检机器人全局路径规划系统，该系统包含模块的组成结构、功能及连接关系如下：

(1)基站控制模块：存储巡检机器人巡检路线包含的所有停靠点及路径交叉点位置信息，常规巡检时期巡检任务直接下发巡检路径信息至运动控制模块执行，特殊巡检时则下发至路径规划模块；

(2)路径规划模块：接收机器人基站控制模块下发的依据巡检任务，搜索巡检任务最短路径，并将需运行路径下发至运动控制模块执行；

(3)运动控制模块：依据下发的下发需运行路径信息生成电机控制量，并下发至电机驱动模块；

(4)电机驱动模块：接收运动控制模块的生成的电机控制量，依据电机轴编码器信号闭环控制控制巡检机器人驱动轮伺服电机，实现机器人在变电站内运行。

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (8)

1.一种基于拓扑点分类的巡检机器人全局路径规划方法，其特征是：包括以下步骤：

(1)在机器人运行路线上建立拓扑点；

(2)对所有拓扑点按路径分类，得到不同的拓扑点集合，并将每个集合中包含的所有拓扑点合并形成一个合并点；

(3)基于上述系建立以每条路径起点、终点和合并点为节点的有向图数据结构，确定拓扑点之间的弧长；

(4)当需要进行最短路径规划时，依据巡检任务给定的机器人当前位置和目标点位置，并寻找有向图中对应的拓扑点，并判断两个拓扑点所属集合；

(5)如果机器人当前位置和目标点位置所对应拓扑点属于同一拓扑点集，则表示机器人当前位置和目标点属于同一路径，此时无需执行复杂的路径规划算法，直接判定这两点的相对位置关系，并输出至巡检机器人执行；否则，执行步骤(6)；

(6)根据步骤(4)确定机器人当前位置和目标点距各自所属路径起点与终点间距离，更新该路径上合并点与路径起点和终点之间弧的权值；

(7)计算巡检机器人当前点与目标点之间的最短路径，并输出至机器人执行。

2.如权利要求1所述的巡检机器人全局路径规划方法，其特征是：所述步骤(1)中，建立拓扑点,将机器人运行路线上的停靠点及路径交叉点抽象为有向图数据结构的拓扑点。

3.如权利要求2所述的巡检机器人全局路径规划方法，其特征是：所述步骤(2)的具体方法为：将拓扑点按路径分类，具体包括：

(2-1)将巡检路线划分为巡检路径集合，每条巡检路径只通过起点和终点与其他路径连接；

(2-2)将所有停靠点的拓扑点据所属路径进行分类，得到每条路径包含的拓扑点集合，将该集合中的所有拓扑点合并，形成一个合并点，后续路径规划过程中合并点可以表示该路径上拓扑点集合内的任意一个拓扑点，参与路径规划计算。

4.如权利要求1所述的巡检机器人全局路径规划方法，其特征是：所述步骤(3)的具体方法为：建立以每条路径起点、终点和合并点为节点的有向图数据结构G＝(V,A)，其中V为有向图数据结构包含的拓扑点集，假设有n个点，则V＝{v_i|i＝1,2,…,n}，Vi表示第i个拓扑点，而A则表示两拓扑点之间的弧集，每条弧的初始权值按下述步骤确定：

(a)每条路径起点和终点对应拓扑点间弧的权值初始化为该条路径的长度；

(b)合并点对应拓扑点与所属路径起点和终点对应拓扑点间弧的权值初始化为∞。

5.如权利要求4所述的巡检机器人全局路径规划方法，其特征是：所述步骤(3)中，形成的有向图数据结构以临接矩阵方式进行存储，临接矩阵中A[i][j]表示弧＜v_i,v_j＞的权值，若弧＜v_i,v_j＞不存在或按合并点初始化权值设置则置A[i][j]为∞，其中i,j＝1,2,…,n，弧＜v_i,v_j＞表示为拓扑点v_i与v_j之间的弧。

6.如权利要求1所述的巡检机器人全局路径规划方法，其特征是：所述步骤(4)中，当需要进行最短路径规划时，依据巡检任务给定的机器人当前位置和目标点位置，并寻找有向图中对应的拓扑点v_s和v_e，判定这两点所属步骤2建立的每条路径包含拓扑点的集合。

7.如权利要求1所述的巡检机器人全局路径规划方法，其特征是：所述步骤(7)中，所述路径规划算法为Dijkstra算法，具体执行步骤包括：

(7-1)定义S为已找到从机器人当前位置拓扑点v_s出发的最短路径的终点集合，它的初始状态为空集，那么从v_s出发到有向图上其余各顶点v_t可能达到的最短路径长度D[s]的初始值为：

D[s]＝A[s][t] v_s∈V，s,t＝1,2,…,n；

(7-2)选择v_t，使得D[t]＝min{D[s]|v_s∈V-S}，

v_t就是当前求得的一条从v_s出发的最短路径的终点，之后令S＝S∪{t}，从而将拓扑点t存入最短路径终点集合；

(7-3)修改从v_s出发到集合V-S上任意一个顶点v_k达到的最短路径长度D[k]，如果D[t]+A[t][k]＜D[k]，则修改D[k]为D[k]＝D[t]+A[t][k]；

(7-4)重复操作(7-2)(7-3)，由此求得从vs到有向图上其余各拓扑点的最短路径，之后依据目标点位置信息从S中选择机器人从当前点至该点最短路径所包含的拓扑点集合，从而完成巡检机器人路径规划。

8.如权利要求1-7中任一项所述的方法的全局路径规划系统，其特征是：包括基站控制模块、路径规划模块、运动控制模块和电机驱动模块，其中：

所述基站控制模块，用于存储巡检机器人巡检路线包含的所有停靠点及路径交叉点位置信息，常规巡检时期巡检任务直接下发巡检路径信息至运动控制模块执行，特殊巡检时则下发至路径规划模块；

所述路径规划模块，用于接收机器人基站控制模块下发的依据巡检任务，搜索巡检任务最短路径，并将需运行路径下发至运动控制模块执行；

所述运动控制模块，用于依据下发的需运行路径信息生成电机控制量，并下发至电机驱动模块；

所述电机驱动模块，用于接收运动控制模块的生成的电机控制量，依据电机轴编码器信号闭环控制控制巡检机器人驱动轮伺服电机，实现机器人在变电站内运行。