CN102420392B - 基于磁导航的变电站巡检机器人全局路径规划方法 - Google Patents

Abstract

本发明涉及一种基于磁导航的变电站巡检机器人全局路径规划方法。它的步骤为：首先对变电站环境建模，形成拓扑网状图和连通关系矩阵；接着利用Floyd弗洛伊德算法确定搜索路径矩阵，即最短路径矩阵，并通过迭代优化，提高运算速度；然后通过对节点和边的遍历，计算始发点到目标点序列的最短连通路径；最后将最短连通路径转换到以RFID序列标示的行驶路线；在每个路口RFID，根据路径连通性，计算出机器人的转动角度。采用本发明的变电站巡检机器人行驶路线短，机器人到位检测时间快，节省了电池能量，同时能有效节省变电站设备操作后移动机器人设备校验的时间。

Description

基于磁导航的变电站巡检机器人全局路径规划方法

技术领域

本发明涉及一种机器人路径规划方法，具体是一种基于磁导航的变电站巡检机器人全局路径规划方法。

背景技术

变电站设备巡检是有效保证变电站设备安全运行、提高供电可靠性的一项基础工作。变电站巡检机器人可以取代运行人员自动完成变电站日常设备巡视、红外测温、操作前后设备状态检查等工作，从而切实提高设备巡视的工作效率和质量，降低劳动强度和工作风险，提升变电站智能化水平，为变电站无人值守提供支撑。变电站巡检机器人已经成为变电站设备巡检的重要辅助手段。

变电站巡检机器人运动控制的关键技术是导航与路径规划。在变电站室外强电磁环境中，磁导航(路面磁轨迹结合RFID标签定位)是一种常规但相对较为可靠的导航定位方式。中国知识产权局公布的重庆市电力公司超高压局两个专利CN101984382A《利用机器人进行变电站设备智能巡检的方法》和CN102082466A《变电站设备智能巡检机器人***》中都涉及用磁导航和RFID实现导航和定位作用，但两个专利存在的问题在于RFID只埋在磁条的一边，机器人巡视时只能沿一条闭合磁轨迹单向巡视，不支持双向行走；造成的后果是如果对特定设备进行巡视时，机器人没有对路径进行优化，需要走一些不必要的路径，既浪费了机器人电池能量，又加长了机器人到位检测的时间，检测效率不高。

随着智能化变电站发展的需求，机器人需要自动配合遥控或顺控操作，进行被控设备的位置校核。变电站巡检机器人路径规划时会存在以下问题：

1、机器人如何获得变电站磁导航路径中某个停靠点到另一个停靠点之间的最短连通路径；(所谓停靠点是指机器人巡检时遇见RFID，机器人停止运行并进行对设备检测的点，下同)

2、机器人配合顺控操作对变电站变压器三相进行检测，需要在变压器不同的停靠点处进行停靠检测，因此停靠点是一个节点序列。这时机器人从某一个节点开始，如何遍历节点序列中的每一个点，使得总路径长度最短，并且重复节点的次数最小；

3、考虑路程、时间、安全性和机动性等综合指标，如何做好巡检机器人路径的最优规划；

发明内容

本发明的目的在于解决变电站巡检机器人存在的上述技术问题，提供一种基于磁导航的变电站巡检机器人全局路径规划方法。采用本发明的变电站巡检机器人行驶路线短，机器人到位检测的时间快，节省了电池能量，同时能有效节省变电站设备操作后移动机器人设备校验的时间。

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

一种基于磁导航的变电站巡检机器人全局路径规划方法，它的步骤为：

第一步，变电站环境建模；形成拓扑网状图和连通关系矩阵；变电站环境建模是指用拓扑法对变电站环境进行建模，将供行走的道路连接起来形成拓扑网状图和连通关系矩阵；其中交叉路口设置多个RFID，并简化合并为图中的一个节点，道路就是图中的一条边，道路的长度即为边的长度，也是连通关系矩阵中边的权值；

第二步，路径搜索；

利用Floyd弗洛伊德算法确定搜索路径矩阵，即最短路径矩阵，并通过迭代优化，提高运算速度；

第三步，路径遍历；

通过对节点和边的遍历，计算始发点到目标点序列的最短连通路径；

第四步，行驶路线确定；

将最短连通路径转换到以RFID序列标示的行驶路线；在每个路口RFID，根据路径连通性，计算出机器人的转动角度。

所述第二步路径搜索包括以下步骤：

2-1)给距离矩阵赋初值

对一个有n个顶点的图G，每个顶点用自然数编号，分别为1，2...n；把图G的连通关系矩阵，即图G的带权邻接矩阵W，作为距离矩阵的初值，即其中n为大于等于2的正整数，i，j均为大于等于1且小于等于n的正整数；D⁽⁰⁾为赋初值后的距离矩阵，为顶点i和顶点j之间的距离初值。

2-2)利用Floyd弗洛伊德算法确定路径最短的距离矩阵；

第1步，构造其中是从v_i到v_j的只允许以v₁作为中间点的路径中最短路长度，其中i，j相邻时否则min{A，B}是从A、B两个元素中取最小值，A、B均为正实数，下同；∞表示值为无穷大。

第2步，构造其中是从v_i到v_j的只允许以v₁，v₂作为中间点的路径中最短路的长度，

第n步，构造其中是从v_i到v_j的只允许以v₁，v₂，…，v_n作为中间点的所有路径中最短路的长度，即是从v_i到v_j中间可***任何顶点的路径中最短路的长度；

2-3)通过迭代优化，提高运算速度；

在计算迭代矩阵D^(k)中的元素时，矩阵D^(k)前面i-1行的元素和i行前面j-1个元素已经全部计算出来，可以扩展矩阵维度存储利用这些新的元素来参与计算，加速迭代求解。迭代公式变为：

$d_{\mathrm{ij}}^{\left(k\right)}=\min \{\underset{r<j}{\min \{d_{\mathrm{ir}}^{\left(k\right)}}+\underset{r<i}{d_{\mathrm{rj}}^{\left(k\right)}}\},\underset{r<j}{\min \{d_{\mathrm{ir}}^{\left(k\right)}}+\underset{r>i}{d_{\mathrm{rj}}^{(k-1)}}\},\underset{r>j}{\min \{d_{\mathrm{ir}}^{(k-1)}}+\underset{r<i}{d_{\mathrm{rj}}^{\left(k\right)}}\},\underset{r>j}{\min \{d_{\mathrm{ir}}^{(k-1)}}+\underset{r>i}{d_{\mathrm{rj}}^{(k-1)}}\left\}\right\}$

同时每次迭代都需要判断一般先求和再比较，求和计算次数较多，如果或成立，则不需要求和比较即可判断否则再求和，从而加快了计算速度；

其中表示从v_i到v_j的只允许以v₁，v₂，…，v_k作为中间点的所有路径中最短路的长度，其它类同。

最后输出距离矩阵和最短路径矩阵，其中最短路径矩阵存储有始发点和目标点之间最短中继节点的集合，距离矩阵存储有始发点和目标点之间最短中继节点的距离。

所述第三步的具体步骤为：

3-1)通过最短路径矩阵找出始发点和目标点序列中每个目标点之间的最短的中继节点序列集合；

3-2)通过最终输出的距离矩阵，计算出始发点到每个目标点之间的最短的中继节点序列的距离之和，因为目标点序列中的目标点数目较少，通过穷举法对始发点对每个目标点之间距离进行遍历，确定最短路径。

所述第四步中，对于十字路口，在其四角设有四个RFID点；在丁字路口，则按照品字型设有三个RFID点；如果是拐角处，则设有两个RFID点，机器人通过其底部左右两侧对称安装的两个RFID读卡器，根据读到的RFID点确定自身的位置和行走的方向，并按照行走方向确定其是前进或左转90度或右转90度或转180度后行走。

本发明的有益效果是：

1、可以实现机器人双向行走，与机器人只能单向行走相比，避免多走一些必要的路径，缩短了一些不必要等待的时间；

2、采用本发明的变电站巡检机器人行驶路线短，机器人到位检测时间快，节省了电池能量，同时能有效节省变电站设备操作后移动机器人设备校验的时间；

3、采用路径规划的变电站巡检机器人自动和变电站倒闸操作顺序控制配合，代替运行人员实现对倒闸操作后设备状态的识别，大大减轻运行人员的劳动强度，缩短操作时间，消除现场人员的主观因素的影响，为变电站顺控操作的安全性提供了重要保障。

附图说明

图1为本发明的基于磁导航的变电站巡检机器人全局路径规划方法流程框图；

图2为本发明的基于磁导航的变电站巡检机器人全局路径规划方法中十字路口行驶路线示意图。

图3为本发明的基于磁导航的变电站巡检机器人全局路径规划方法中丁字路口行驶路线示意图。

图4为本发明的基于磁导航的变电站巡检机器人全局路径规划方法中拐角路口行驶路线示意图。

图5使用本发明的全局路径规划方法后，变电站巡检机器人始发点和目标点之间机器人实际行驶的路线截图。

图6为使用本发明的全局路径规划方法后，变电站巡检机器人始发点和目标点序列之间机器人实际行驶的路线截图。

具体实施方式

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

本发明是通过以下技术方案实现的，本发明包括以下步骤：

第一步，变电站环境建模；

通过变电站环境建模，形成拓扑网状图和连通关系矩阵。变电站环境建模是指用拓扑法对变电站环境进行建模，将可以行走的道路连接起来形成拓扑网状图和连通关系矩阵。其中交叉路口多个RFID简化合并为图中的一个节点，道路就是图中的一条边，道路的长度即为边的长度，也是连通关系矩阵中边的权值。

第二步，路径搜索；

利用Floyd(弗洛伊德)算法确定搜索路径矩阵(即最短路径矩阵)，并通过迭代优化，提高运算速度。

路径搜索包括以下步骤：

1)给距离矩阵赋初值。

对一个有n个顶点的图G，将顶点用n个整数进行编号。把G的连通关系矩阵(即图G的带权邻接矩阵)W作为距离矩阵的初值，即(其中n为大于等于2的正整数，i，j均为大于等于1小于等于n的正整数；D⁽⁰⁾为赋初值后的距离矩阵，为顶点i和顶点j之间的距离初值。)

2)利用Floyd(弗洛伊德)算法确定路径最短的距离矩阵。

第1步，构造其中是从v_i到v_j的只允许以v₁作为中间点的路径中最短路长度。其中i，j相邻时否则min{A，B}是从A、B两个元素中取最小值，A、B均为正实数，下同；∞表示值为无穷大。

第2步，构造其中是从v_i到v_j的只允许以v₁，v₂作为中间点的路径中最短路的长度。

第n步，构造其中是从v_i到v_j的只允许以v₁，v₂，…，v_n作为中间点的所有路径中最短路的长度，即是从v_i到v_j中间可***任何顶点的路径中最短路的长度，因此D⁽ⁿ⁾即是距离矩阵。

3)通过迭代优化，提高运算速度。

在计算迭代矩阵D^(k)中的元素时，矩阵D^(k)前面i-1行的元素和i行前面j-1个元素已经全部计算出来，可以扩展矩阵维度存储利用这些新的元素来参与计算，加速迭代求解。迭代公式变为：

$d_{\mathrm{ij}}^{\left(k\right)}=\min \{\underset{r<j}{\min \{d_{\mathrm{ir}}^{\left(k\right)}}+\underset{r<i}{d_{\mathrm{rj}}^{\left(k\right)}}\},\underset{r<j}{\min \{d_{\mathrm{ir}}^{\left(k\right)}}+\underset{r>i}{d_{\mathrm{rj}}^{(k-1)}}\},\underset{r>j}{\min \{d_{\mathrm{ir}}^{(k-1)}}+\underset{r<i}{d_{\mathrm{rj}}^{\left(k\right)}}\},\underset{r>j}{\min \{d_{\mathrm{ir}}^{(k-1)}}+\underset{r>i}{d_{\mathrm{rj}}^{(k-1)}}\left\}\right\}$

同时发现每次迭代都需要判断一般先求和再比较，求和计算次数较多，如果或成立，则不需要求和比较即可判断否则再求和，加快了计算速度。

其中表示从v_i到v_j的只允许以v₁，v₂，…，v_k作为中间点的所有路径中最短路的长度，其它类同。

4)最后输出距离矩阵和最短路径矩阵，其中最短路径矩阵存储有始发点和目标点之间最短中继节点的集合，距离矩阵存储有始发点和目标点之间最短中继节点的距离。

第三步，路径遍历；

通过对节点和边的遍历，计算始发点到目标点序列的最短连通路径。

路径遍历包括以下步骤：

1)通过最短路径矩阵找出始发点和目标点序列中每个目标点之间的最短的中继节点序列集合

2)通过最终输出的距离矩阵，计算出始发点到每个目标点之间的最短的中继节点序列的距离之和，因为目标点序列中的目标点数目较少，通过穷举法对始发点对每个目标点之间距离进行遍历，确定最短路径。

第四步，行驶路线确定。

将最短连通路径转换到以RFID序列标示的行驶路线；在每个路口RFID，根据路径连通性，计算出机器人的转动角度。

关于机器人实际行驶路线的确定，以图2为具体实施例进行说明。

图2中，十字路口的4个RFID点每个点都具有不同的属性，1号RFID点，属性是向上，代表机器人向上行走，2号RFID点，属性是向右，代表机器人向右行走，3号RFID点，属性是向下，代表机器人行走，4号RFID点，属性向左，代表机器人向左。

当机器人路径规划的中继点序列中，有1号RFID、2号RFID点时，则机器人经过1号RFID点后，右转90度，向2号点方向行走，走到2号RFID点后，机器人继续向右行走；

当机器人路径规划的中继点序列中，有1号RFID、3号RFID点时，则机器人经过1号RFID点后，向上行走规定的一段距离，逆时针转弯180度，向3号点方向行走，走到3号RFID点后，机器人继续向下行走；

当机器人路径规划的中继点序列中，有1号RFID、4号RFID点时，则机器人经过1号RFID点后，左转90度，向4号点方向行走，走到4号RFID点后，机器人继续向左行走；

同理，机器人可以在十字路口的4个RFID点之间任意组合行走，可以在丁字路口3个RFID点之间任意组合行走，也可以在拐弯处2个RFID点之间组合行走，如图3、图4所示。其中，图3中的三个RFID点，每次可通过路径规划其中的两个中继节点序列来判断其下一步的运行方向，例如路径规划中继节点序列中有11号RFID点和13号RFID点，则机器人经过11号RFID点后左转90度，向13号RFID点方向行走，反之，机器人首先经过13号RFID点后右转90度向11号RFID点方向行走；如果路径规划中继节点序列中有11号RFID点和12号RFID点，则机器人经过11号RFID点后右转90度向12号RFID点方向行走，反之，机器人首先经过12号RFID点左转90度向11号RFID点方向行走；如果路径规划中继节点序列中有12号RFID点和13号RFID点，则机器人经过12号RFID点后就继续直行向13号RFID点方向行走，反之，机器人首先经过13号RFID时继续直行向12号RFID点方向行走；图4中，则更为简单，根据路径规划要求，如果中继点序列有21号RFID点和22号RFID点时，如果首先经过21号RFID点，则左转90度向22号RFID点行走，反之，如果首先经过22号RFID点，则右转90度向21号RFID点行走。

移动机器人的底部左右两侧对称安装两个RFID读卡器，机器人行驶过程中读到任一RFID时，均能识别出行所在自身的位置和行走的方向。

图5为使用本发明的全局路径规划方法后，变电站巡检机器人始发点和目标点之间机器人实际行驶的路线截图。它从1号RFID点出发到第12号RFID点，中途进行一次拐弯，然后返回。

图6为使用本发明的全局路径规划方法后，变电站巡检机器人始发点和目标点序列之间机器人实际行驶的路线截图。它从1号RFID点出发到第12、13、14号目标RFI D点序列时，中途进行一次拐弯，然后分别在12、13、14号目标点机器人停靠检测，检测完毕最后在14号点拐弯返回。

Claims (3)

1.一种基于磁导航的变电站巡检机器人全局路径规划方法，其特征是，它的步骤为：

第一步，变电站环境建模；

形成拓扑网状图和连通关系矩阵；变电站环境建模是指用拓扑法对变电站环境进行建模，将供行走的道路连接起来形成拓扑网状图和连通关系矩阵；其中交叉路口设置多个RFID，并简化合并为图中的一个节点，道路就是图中的一条边，道路的长度即为边的长度，也是连通关系矩阵中边的权值；

第二步，路径搜索；

利用Floyd弗洛伊德算法确定搜索路径矩阵，即最短路径矩阵，并通过迭代优化，提高运算速度；

第三步，路径遍历；

通过对节点和边的遍历，计算始发点到目标点序列的最短连通路径；

第四步，行驶路线确定；

将最短连通路径转换到以RFID序列标示的行驶路线；在每个路口RFID，根据路径连通性，计算出机器人的转动角度；机器人的底部左右两侧对称安装两个RFID读卡器，机器人行驶过程中读到任意RFID时，均识别出行所在自身的位置和行走的方向；

所述第四步中，对于十字路口，在其四角设有四个RFID点；在丁字路口，则按照品字型设有三个RFID点；如果是拐角处，则设有两个RFID点，机器人通过其底部左右两侧对称安装的两个RFID读卡器，根据读到的RFID点确定自身的位置和行走的方向，并按照行走方向确定其是前进或左转90度或右转90度或转180度后行走。

2.如权利要求1所述的基于磁导航的变电站巡检机器人全局路径规划方法，其特征是，所述第二步路径搜索包括以下步骤：

2-1)给距离矩阵赋初值

对一个有n个顶点的图G，将顶点用n个整数进行编号；把图G的连通关系矩阵，即图G的带权邻接矩阵W，作为距离矩阵的初值，即其中n为大于等于2的正整数，i，j均为大于等于1小于等于n的正整数；D⁽⁰⁾为赋初值后的距离矩阵，为顶点i和顶点j之间的距离初值；

2-2)利用Floyd弗洛伊德算法确定路径最短的距离矩阵；

第1步，构造 $D^{\left(1\right)}={\left(d_{\mathrm{ij}}^{\left(1\right)}\right)}_{n*n},$ 其中 $d_{\mathrm{ij}}^{\left(1\right)}=\min \{d_{\mathrm{ij}}^{\left(0\right)}{,d}_{i1}^{\left(0\right)}+d_{1j}^{\left(0\right)}\}$ 是从v_i到v_j的只允许以v₁作为中间点的路径中最短路长度，其中i，j相邻时否则min{A,B}是从A、B两个元素中取最小值，A、B均为正实数，下同；∞表示值为无穷大；

第2步，构造 $D^{\left(2\right)}={\left(d_{\mathrm{ij}}^{\left(2\right)}\right)}_{n*n},$ 其中 $d_{\mathrm{ij}}^{\left(2\right)}=\min \{d_{\mathrm{ij}}^{\left(1\right)}{,d}_{i2}^{\left(1\right)}+d_{2j}^{\left(1\right)}\}$ 是从v_i到v_j的只允许以v₁，v₂作为中间点的路径中最短路的长度，

第n步，构造 $D^{\left(n\right)}={\left(d_{\mathrm{ij}}^{\left(n\right)}\right)}_{n*n},$ 其中 $d_{\mathrm{ij}}^{\left(n\right)}=\min \{d_{\mathrm{ij}}^{(n-1)}{,d}_{\mathrm{in}}^{(n-1)}+d_{\mathrm{nj}}^{(n-1)}\}$ 是从v_i到v_j的只允许以v₁，v₂，…，v_n作为中间点的所有路径中最短路的长度，即是从v_i到v_j中间可***任何顶点的路径中最短路的长度，因此D⁽ⁿ⁾即是距离矩阵；

2-3)通过迭代优化，提高运算速度；

在计算迭代矩阵D^(k)中的元素时，矩阵D^(k)前面i-1行的元素和i行前面j-1个元素已经全部计算出来，可以扩展矩阵维度存储利用这些新的元素来参与计算，加速迭代求解，迭代公式变为：

$d_{\mathrm{ij}}^{\left(k\right)}=\min \{\min \underset{r<j}{\{}{d}_{\mathrm{ir}}^{\left(k\right)}+\underset{r<i}{d_{\mathrm{rj}}^{\left(k\right)}}\},\min \underset{r<j}{\{}{d}_{\mathrm{ir}}^{\left(k\right)}+\underset{r>i}{d_{\mathrm{rj}}^{(k-1)}}\},\min \underset{}{\underset{r>j}{\{}}{d}_{\mathrm{ir}}^{(k-1)}+\underset{r<i}{d_{\mathrm{rj}}^{\left(k\right)}}\},\min \underset{r>j}{\{}{d}_{\mathrm{ir}}^{(k-1)}+\underset{r>i}{d_{\mathrm{rj}}^{(k-1)}}\left\}\right\}$

同时每次迭代都需要判断先求和再比较，求和计算次数较多，如果或成立，则不需要求和比较即可判断否则再求和，从而加快了计算速度；

其中表示从v_i到v_j的只允许以v₁，v₂，…，v_k作为中间点的所有路径中最短路的长度；

最后输出距离矩阵和最短路径矩阵，其中最短路径矩阵存储有始发点和目标点之间最短中继节点的集合，距离矩阵存储有始发点和目标点之间最短中继节点的距离。

3.如权利要求1所述的基于磁导航的变电站巡检机器人全局路径规划方法，其特征是，所述第三步的具体步骤为：

3-1)通过最短路径矩阵找出始发点和目标点序列中每个目标点之间的最短的中继节点序列集合；

3-2)通过最终输出的距离矩阵，计算出始发点到每个目标点之间的最短的中继节点序列的距离之和，因为目标点序列中的目标点数目较少，通过穷举法对始发点对每个目标点之间距离进行遍历，确定最短路径。