CN102420392B - Transformer substation inspection robot global path planning method based on magnetic navigation - Google Patents

Abstract

The invention relates to a transformer substation inspection robot global path planning method based on magnetic navigation. The method comprises the steps of: modeling an environment of a transformer substation to form a topology network graph and a connecting relationship matrix; determining a searching path matrix, namely the shortest path matrix, by using a Floyd algorithm, and increasing the computing speed by iterative optimization; computing the shortest connecting path between an initial point and a target point sequence by transversing nodes and edges; converting the shortest connecting path into a travel path indicated by an RFID (radio frequency identification device) sequence; and computing the rotating angle of a robot at each RFID at an intersection according to the connectivity of the path. The transformer substation inspection robot adopting the transformer substation inspection robot global path planning method provided by the invention is short in travel path, and rapid to be in place for detection, therefore, the energy of batteries is saved, and simultaneously the time for moving the robot equipment for verification after the operation of the transformer substation equipment can be effectively saved.

Description

Based on the transformer substation inspection robot global path planning method of magnetic navigation

Technical field

The present invention relates to a kind of robot path planning method, specifically a kind of transformer substation inspection robot global path planning method based on magnetic navigation.

Background technology

Inspecting substation equipment is an element task that effectively ensures substation equipment safe operation, improves power supply reliability.Intelligent Mobile Robot can replace operations staff and automatically complete the work such as transformer station's everyday devices tour, infrared measurement of temperature, the equipment state inspection of operation front and back, thereby conscientiously improve work efficiency and the quality of equipment-patrolling, reduce labour intensity and work risk, promote intelligent substation level, for transformer station's unmanned provides support.Intelligent Mobile Robot has become the important supplementary means of inspecting substation equipment.

The gordian technique of Intelligent Mobile Robot motion control is navigation and path planning.In outside transformer substation strong electromagnetic environment, magnetic navigation (road surface magnetic tracks is in conjunction with RFID tag location) is a kind of conventional but comparatively reliable navigator fix mode.In two patent CN101984382A of Chongqing Power Company Extra High Voltage Bureau " utilizing robot to carry out the method for substation equipment intelligent patrol detection " that China Intellectual Property Office announces and CN102082466A " Intelligent inspection robot system for transformer substation equipment ", all relate to and realize Navigation and localization effect with magnetic navigation and RFID, but the problem of two patent existence is that RFID is only embedded in one side of magnetic stripe, robot can only, along a unidirectional tour of closed magnetic track, not support bidirectional walking while tour; When the consequence causing is if particular device is maked an inspection tour, robot is not optimized path, need to walk some unnecessary paths, has both wasted robot battery energy, has lengthened again robot and has put in place the time of detecting, and detection efficiency is not high.

Along with the demand of Intelligent transformer station development, robot needs automatically to coordinate remote control or sequence operation, carries out the position checking of controlled device.When Intelligent Mobile Robot path planning, can there is following problem:

1, how robot obtains certain anchor point in transformer station's magnetic navigation path and arrives the shortest communication path between another anchor point; (so-called anchor point refers to and when robot patrols and examines, meets RFID, and robot is out of service and carry out the point to equipment Inspection, lower with)

2, robot coordinates sequence operation to detect substation transformer three-phase, need to stop detection at the different anchor point place of transformer, and therefore anchor point is a sequence node.At this moment how robot, from some nodes, travels through each point in sequence node, makes total path length the shortest, and the number of times minimum of duplicate node;

3, consider the overall targets such as distance, time, security and maneuverability, the optimal programming in how to do well crusing robot path;

Summary of the invention

The object of the invention is to solve the above-mentioned technical matters that Intelligent Mobile Robot exists, a kind of transformer substation inspection robot global path planning method based on magnetic navigation is provided.Adopt Intelligent Mobile Robot travel route of the present invention short, robot time of detecting that puts in place is fast, has saved the energy content of battery, can effectively save the time of robot moving equipment verification after substation equipment operation simultaneously.

For achieving the above object, the present invention adopts following technical scheme:

Based on a transformer substation inspection robot global path planning method for magnetic navigation, its step is:

The first step, substation modeling; Form topological network diagramming and connected relation matrix; Substation modeling refers to carries out modeling to substation with topological approach, will couple together and form topological network diagramming and connected relation matrix for the road of walking; Wherein intersection arranges multiple RFID, and simplifies a node of merging in figure, and road is exactly a limit in figure, and the length of road is the length on limit, is also the weights on limit in connected relation matrix;

Second step, route searching;

Utilize Floyd Freud algorithm to determine searching route matrix, i.e. shortest path matrix, and by iteration optimization, improve arithmetic speed;

The 3rd step, traversal path;

By the traversal to node and limit, calculate the shortest communication path of originating point to impact point sequence;

The 4th step, travel route is determined;

The shortest communication path is transformed into the travel route indicating with RFID sequence; At each crossing RFID, according to path connectivity, calculate the rotational angle of robot.

Described second step route searching comprises the following steps:

2-1) give distance matrix initialize

To a figure G who has a n summit, number with natural number on each summit, is respectively 1,2...n; The connected relation matrix of figure G, scheme the cum rights adjacency matrix W of G, as the initial value of distance matrix, wherein n is more than or equal to 2 positive integer, i, and j is and is more than or equal to 1 and be less than or equal to the positive integer of n; D ⁽⁰⁾for the distance matrix after initialize, for the distance initial value between summit i and summit j.

2-2) utilize Floyd Freud algorithm to determine the distance matrix of shortest path;

The 1st step, structure wherein from v _ito v _jonly allow with v ₁as shortest path length in the path of intermediate point, wherein i, when j is adjacent otherwise min{A, B} gets minimum value from A, two elements of B, and A, B are arithmetic number, lower same; ∞ represents that value is for infinitely great.

The 2nd step, structure wherein from v _ito v _jonly allow with v ₁, v ₂as the length of shortest path in the path of intermediate point,

N step, structure wherein from v _ito v _jonly allow with v ₁, v ₂..., v _nas the length of shortest path in all paths of intermediate point, be from v _ito v _jthe length of shortest path in the middle path that can insert any summit;

2-3) by iteration optimization, improve arithmetic speed;

Calculating Iterative Matrix D ^(k)in element time, matrix D ^(k)before the element that i-1 is capable above and i are capable, j-1 element all calculates, and can these new units of extended matrix dimension memory by using usually participate in calculating, and accelerates iterative.Iterative formula becomes:

$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\}$

Simultaneously each iteration all needs judgement generally first summation is compared again, and read group total number of times is more, if or set up, do not need summation relatively can judge otherwise sue for peace again, thereby accelerated computing velocity;

Wherein represent from v _ito v _jonly allow with v ₁, v ₂..., v _kas the length of shortest path in all paths of intermediate point, other roughly the same.

Finally export distance matrix and shortest path matrix, wherein shortest path matrix stores the set of short wave relay node between originating point and impact point, and distance matrix stores the distance of short wave relay node between originating point and impact point.

The concrete steps of described the 3rd step are:

3-1) find out the shortest relay node sequence set between each impact point in originating point and impact point sequence by shortest path matrix;

3-2) by the distance matrix of final output, calculate the distance sum of originating point to the shortest relay node sequence between each impact point, because the impact point number in impact point sequence is less, by the method for exhaustion, originating point is traveled through distance between each impact point, determine shortest path.

In described the 4th step, for crossroad, be provided with four RFID points at Qi Sijiao; In T-shaped road junction, be provided with three RFID points according to Chinese character pin-shaped; If corner, be provided with two RFID points, robot is by its symmetrical two RFID card reader of installing in left and right sides, bottom, according to the RFID point of reading determine self position and the direction of walking, and according to direction of travel determine its be advance a left side turn 90 degrees or the right side turn 90 degrees or turnback after walk.

The invention has the beneficial effects as follows:

1, can realize robot bidirectional walking, can only unidirectional walking with robot compared with, avoid walking some necessary paths more, shortened the time of some unnecessary waits;

2, adopt Intelligent Mobile Robot travel route of the present invention short, robot is fast detection time that puts in place, has saved the energy content of battery, can effectively save the time of robot moving equipment verification after substation equipment operation simultaneously;

3, the Intelligent Mobile Robot that adopts path planning automatically and Substation Switch Changing Operation sequential control coordinate, replace operations staff to realize the identification to equipment state after grid switching operation, greatly alleviate operations staff's labour intensity, shorten the running time, eliminate the impact of field staff's subjective factor, for the security of substation sequence control operation provides important leverage.

Brief description of the drawings

Fig. 1 is the transformer substation inspection robot global path planning method FB(flow block) based on magnetic navigation of the present invention;

Fig. 2 is crossroad travel route schematic diagram in the transformer substation inspection robot global path planning method based on magnetic navigation of the present invention.

Fig. 3 is T-shaped road junction travel route schematic diagram in the transformer substation inspection robot global path planning method based on magnetic navigation of the present invention.

Fig. 4 is crossing, turning travel route schematic diagram in the transformer substation inspection robot global path planning method based on magnetic navigation of the present invention.

Fig. 5 uses after global path planning method of the present invention, the route sectional drawing of robot actual travel between Intelligent Mobile Robot originating point and impact point.

Fig. 6 uses after global path planning method of the present invention, the route sectional drawing of robot actual travel between Intelligent Mobile Robot originating point and impact point sequence.

Embodiment

Below in conjunction with accompanying drawing and embodiment, the present invention will be further described.

The present invention is achieved by the following technical solutions, the present invention includes following steps:

The first step, substation modeling;

By substation modeling, form topological network diagramming and connected relation matrix.Substation modeling refers to carries out modeling to substation with topological approach, and the road that can walk couples together and forms topological network diagramming and connected relation matrix.Wherein the multiple RFID in intersection simplify a node of merging in figure, and road is exactly a limit in figure, and the length of road is the length on limit, is also the weights on limit in connected relation matrix.

Second step, route searching;

Utilize Floyd (Freud) algorithm to determine searching route matrix (being shortest path matrix), and by iteration optimization, improve arithmetic speed.

Route searching comprises the following steps:

1) give distance matrix initialize.

To a figure G who has a n summit, summit is numbered with n integer.Initial value using the connected relation matrix of G (scheming the cum rights adjacency matrix of G) W as distance matrix, (wherein n is more than or equal to 2 positive integer, i, and j is and is more than or equal to 1 positive integer that is less than or equal to n; D ⁽⁰⁾for the distance matrix after initialize, for the distance initial value between summit i and summit j.)

2) utilize Floyd (Freud) algorithm to determine the distance matrix of shortest path.

The 1st step, structure wherein from v _ito v _jonly allow with v ₁as shortest path length in the path of intermediate point.Wherein i, when j is adjacent otherwise min{A, B} gets minimum value from A, two elements of B, and A, B are arithmetic number, lower same; ∞ represents that value is for infinitely great.

The 2nd step, structure wherein from v _ito v _jonly allow with v ₁, v ₂as the length of shortest path in the path of intermediate point.

N step, structure wherein from v _ito v _jonly allow with v ₁, v ₂..., v _nas the length of shortest path in all paths of intermediate point, be from v _ito v _jthe length of shortest path in the middle path that can insert any summit, therefore D ⁽ⁿ⁾be distance matrix.

3), by iteration optimization, improve arithmetic speed.

Calculating Iterative Matrix D ^(k)in element time, matrix D ^(k)before the element that i-1 is capable above and i are capable, j-1 element all calculates, and can these new units of extended matrix dimension memory by using usually participate in calculating, and accelerates iterative.Iterative formula becomes:

$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\}$

Find that each iteration all needs judgement simultaneously generally first summation is compared again, and read group total number of times is more, if or set up, do not need summation relatively can judge otherwise sue for peace again, accelerated computing velocity.

Wherein represent from v _ito v _jonly allow with v ₁, v ₂..., v _kas the length of shortest path in all paths of intermediate point, other roughly the same.

4) finally export distance matrix and shortest path matrix, wherein shortest path matrix stores the set of short wave relay node between originating point and impact point, and distance matrix stores the distance of short wave relay node between originating point and impact point.

The 3rd step, traversal path;

By the traversal to node and limit, calculate the shortest communication path of originating point to impact point sequence.

Traversal path comprises the following steps:

1) find out the shortest relay node sequence set between each impact point in originating point and impact point sequence by shortest path matrix

2) by the distance matrix of final output, calculate the distance sum of originating point to the shortest relay node sequence between each impact point, because the impact point number in impact point sequence is less, by the method for exhaustion, originating point is traveled through distance between each impact point, determine shortest path.

The 4th step, travel route is determined.

The shortest communication path is transformed into the travel route indicating with RFID sequence; At each crossing RFID, according to path connectivity, calculate the rotational angle of robot.

About determining of robot actual travel route, describe taking Fig. 2 as specific embodiment.

In Fig. 2,4 each points of RFID point of crossroad have different attributes, No. 1 RFID point, attribute is upwards, represents that robot upwards walks, No. 2 RFID points, attribute is to the right, represent that robot walks to the right, No. 3 RFID points, attribute is downward, represent robot ambulation, No. 4 RFID points, attribute, represents that robot left left.

When in robot path planning's relay point sequence, while having No. 1 RFID, No. 2 RFID points, robot is after No. 1 RFID point, and the right side turn 90 degrees, and to No. 2 point direction walkings, goes to after No. 2 RFID points, and robot continues walking to the right;

When in robot path planning's relay point sequence, while having No. 1 RFID, No. 3 RFID points, robot is after No. 1 RFID point, one segment distance of upwards walking regulation, 180 degree of turning counterclockwise, to No. 3 point direction walkings, go to after No. 3 RFID points, robot continues walking downwards;

When in robot path planning's relay point sequence, while having No. 1 RFID, No. 4 RFID points, robot is after No. 1 RFID point, and a left side turn 90 degrees, and to No. 4 point direction walkings, goes to after No. 4 RFID points, and robot continues walking left;

In like manner, robot can 4 RFID points at the parting of the ways between combination in any walking, can between the RFID point of 3, T-shaped road junction, combination in any walk, also can between the RFID point of 2 of corners, combine walking, as shown in Figure 3, Figure 4.Wherein, three RFID points in Fig. 3, can judge its next step traffic direction by path planning two relay node sequences wherein at every turn, for example in path planning relay node sequence, there are No. 11 RFID points and No. 13 RFID points, robot turn 90 degrees through No. 11 RFID point rear lefts, to No. 13 RFID point direction walkings, otherwise first robot turn 90 degrees to No. 11 RFID point direction walkings through No. 13 RFID point rear rights; If have No. 11 RFID points and No. 12 RFID points in path planning relay node sequence, robot turn 90 degrees to No. 12 RFID point direction walkings through No. 11 RFID point rear rights, otherwise first robot turn 90 degrees to No. 11 RFID point direction walkings through No. 12 RFID point left sides; If have No. 12 RFID points and No. 13 RFID points in path planning relay node sequence, robot just continues to keep straight on to No. 13 RFID point direction walkings after No. 12 RFID points, otherwise robot continues during first through No. 13 RFID to keep straight on and walks to No. 12 RFID point directions; In Fig. 4, more simple, according to path planning requirement, if when relay point sequence has No. 21 RFID points and No. 22 RFID points, if first through No. 21 RFID points, a left side turn 90 degrees to No. 22 RFID point walkings, otherwise, if first through No. 22 RFID points, the right side turn 90 degrees to No. 21 RFID point walkings.

Two RFID card reader of the symmetrical installation in the left and right sides, bottom of mobile robot, while reading arbitrary RFID, all can identify the trip position at place self and the direction of walking in robot ride process.

Fig. 5 uses after global path planning method of the present invention, the route sectional drawing of robot actual travel between Intelligent Mobile Robot originating point and impact point.It is pointed out and is dealt into RFID point No. 12 from No. 1 RFID, once turns round midway, then returns.

Fig. 6 uses after global path planning method of the present invention, the route sectional drawing of robot actual travel between Intelligent Mobile Robot originating point and impact point sequence.It is pointed out while being dealt into the 12nd, 13, No. 14 target RFI D point sequences from No. 1 RFID, once turns round midway, then stops and detects at 12,13, No. 14 impact point robots respectively, detects completely finally to turn round and return at No. 14 points.

Claims (3)

1. the transformer substation inspection robot global path planning method based on magnetic navigation, is characterized in that, its step is:

The first step, substation modeling;

Form topological network diagramming and connected relation matrix; Substation modeling refers to carries out modeling to substation with topological approach, will couple together and form topological network diagramming and connected relation matrix for the road of walking; Wherein intersection arranges multiple RFID, and simplifies a node of merging in figure, and road is exactly a limit in figure, and the length of road is the length on limit, is also the weights on limit in connected relation matrix;

Second step, route searching;

Utilize Floyd Freud algorithm to determine searching route matrix, i.e. shortest path matrix, and by iteration optimization, improve arithmetic speed;

The 3rd step, traversal path;

By the traversal to node and limit, calculate the shortest communication path of originating point to impact point sequence;

The 4th step, travel route is determined;

The shortest communication path is transformed into the travel route indicating with RFID sequence; At each crossing RFID, according to path connectivity, calculate the rotational angle of robot; Two RFID card reader of the symmetrical installation in the left and right sides, bottom of robot, while reading any RFID in robot ride process, all identification trip position at place self and directions of walking;

In described the 4th step, for crossroad, be provided with four RFID points at Qi Sijiao; In T-shaped road junction, be provided with three RFID points according to Chinese character pin-shaped; If corner, be provided with two RFID points, robot is by its symmetrical two RFID card reader of installing in left and right sides, bottom, according to the RFID point of reading determine self position and the direction of walking, and according to direction of travel determine its be advance a left side turn 90 degrees or the right side turn 90 degrees or turnback after walk.

2. the transformer substation inspection robot global path planning method based on magnetic navigation as claimed in claim 1, is characterized in that, described second step route searching comprises the following steps:

2-1) give distance matrix initialize

To a figure G who has a n summit, summit is numbered with n integer; The connected relation matrix of figure G, scheme the cum rights adjacency matrix W of G, as the initial value of distance matrix, wherein n is more than or equal to 2 positive integer, i, and j is and is more than or equal to 1 positive integer that is less than or equal to n; D ⁽⁰⁾for the distance matrix after initialize, for the distance initial value between summit i and summit j;

2-2) utilize Floyd Freud algorithm to determine the distance matrix of shortest path;

The 1st step, structure $D^{\left(1\right)}={\left(d_{\mathrm{ij}}^{\left(1\right)}\right)}_{n*n},$ Wherein $d_{\mathrm{ij}}^{\left(1\right)}=\min \{d_{\mathrm{ij}}^{\left(0\right)}{,d}_{i1}^{\left(0\right)}+d_{1j}^{\left(0\right)}\}$ From v _ito v _jonly allow with v ₁as shortest path length in the path of intermediate point, wherein i, when j is adjacent otherwise min{A, B} gets minimum value from A, two elements of B, and A, B are arithmetic number, lower same; ∞ represents that value is for infinitely great;

The 2nd step, structure $D^{\left(2\right)}={\left(d_{\mathrm{ij}}^{\left(2\right)}\right)}_{n*n},$ Wherein $d_{\mathrm{ij}}^{\left(2\right)}=\min \{d_{\mathrm{ij}}^{\left(1\right)}{,d}_{i2}^{\left(1\right)}+d_{2j}^{\left(1\right)}\}$ From v _ito v _jonly allow with v ₁, v ₂as the length of shortest path in the path of intermediate point,

N step, structure $D^{\left(n\right)}={\left(d_{\mathrm{ij}}^{\left(n\right)}\right)}_{n*n},$ Wherein $d_{\mathrm{ij}}^{\left(n\right)}=\min \{d_{\mathrm{ij}}^{(n-1)}{,d}_{\mathrm{in}}^{(n-1)}+d_{\mathrm{nj}}^{(n-1)}\}$ From v _ito v _jonly allow with v ₁, v ₂..., v _nas the length of shortest path in all paths of intermediate point, be from v _ito v _jthe length of shortest path in the middle path that can insert any summit, therefore D ⁽ⁿ⁾be distance matrix;

2-3) by iteration optimization, improve arithmetic speed;

Calculating Iterative Matrix D ^(k)in element time, matrix D ^(k)before the element that i-1 is capable above and i are capable, j-1 element all calculates, and can these new units of extended matrix dimension memory by using usually participate in calculating, and accelerates iterative, and iterative formula becomes:

$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\}$

Simultaneously each iteration all needs judgement first summation is compared again, and read group total number of times is more, if or set up, do not need summation relatively can judge otherwise sue for peace again, thereby accelerated computing velocity;

Wherein represent from v _ito v _jonly allow with v ₁, v ₂..., v _kas the length of shortest path in all paths of intermediate point;

Finally export distance matrix and shortest path matrix, wherein shortest path matrix stores the set of short wave relay node between originating point and impact point, and distance matrix stores the distance of short wave relay node between originating point and impact point.

3. the transformer substation inspection robot global path planning method based on magnetic navigation as claimed in claim 1, is characterized in that, the concrete steps of described the 3rd step are:

3-1) find out the shortest relay node sequence set between each impact point in originating point and impact point sequence by shortest path matrix;

3-2) by the distance matrix of final output, calculate the distance sum of originating point to the shortest relay node sequence between each impact point, because the impact point number in impact point sequence is less, by the method for exhaustion, originating point is traveled through distance between each impact point, determine shortest path.