A substation inspection robot path planning method

Abstract

The application discloses a substation inspection robot path planning method, comprising the following steps: (1) constructing a substation grid map; (2) dividing the substation grid map into multiple sub-area grid maps; (3) planning an access path between the sub-area grid maps; (4) selecting an inspection starting point and an inspection ending point in the sub-area grid map; (5) performing multi-node parallel directional heuristic path planning in the sub-area grid map; (6) determining an access sequence of all inspection points in the sub-area grid map; and (7) optimizing the inspection path in the sub-area grid map. The method plans a path for a substation inspection robot to access multiple inspection points, effectively reduces the number of grid searches of the substation inspection robot, shortens the path search time, reduces the number of path corners, thereby improving the efficiency of the substation inspection robot path planning, and the method also performs circular arc optimization on right-angle bends, increases the smoothness of the inspection path, and has a good application prospect.

Description

Technical Field

[0001] This invention relates to the field of mobile robot path planning technology, and in particular to a path planning method for a substation inspection robot. Background Technology

[0002] As the State Grid Corporation of China promotes the construction of smart energy infrastructure, more and more intelligent robots are being applied to the daily work of substations. In substations, inspection robots are mainly responsible for tasks such as reading instrument values ​​and infrared thermal imaging at different inspection points. The application of inspection robots has greatly improved the efficiency of substation inspection work. Path planning is an important part of the daily work of substation inspection robots, and the prerequisite for path planning is the establishment of a substation map. Grid maps are an effective way to construct substation maps, as they can reflect the true physical size of the map well. The location information of inspection robots, inspection points, and obstacles can be uniquely represented on the grid map. However, when the map size is large, using grid maps has the disadvantage of low path planning efficiency. Dijkstra's algorithm and A* algorithm are commonly used search-based path planning algorithms. Due to their probabilistic completeness and path optimization characteristics, they are widely used in grid map path planning. However, Dijkstra's algorithm and A* algorithm have limited search speed, and the paths they plan also have the problem of too many turns. Summary of the Invention

[0003] To address the aforementioned technical problems, this invention provides a path planning method for a substation inspection robot. By dividing the substation grid map into multiple sub-regions and performing multi-node parallel directional heuristic path planning within each sub-region, this method effectively reduces the number of grids searched by the substation inspection robot, shortens path planning time, and reduces path turns, thereby improving the efficiency of path planning for the substation inspection robot. Furthermore, this method also optimizes right-angle bends with arcs, increasing the smoothness of the inspection path.

[0004] The technical solution of the present invention is as follows:

[0005] A path planning method for a substation inspection robot includes the following steps:

[0006] (1) Construct a grid map of the substation;

[0007] (2) Divide the substation raster map into multiple sub-region raster maps;

[0008] (3) Plan access paths between sub-region grid maps;

[0009] (4) Select the inspection start point and inspection end point within the sub-region grid map;

[0010] (5) Perform multi-node parallel directional heuristic path planning within the sub-region grid map;

[0011] (6) Determine the access order of all inspection points within the sub-region grid map;

[0012] (7) Optimize the inspection path inside the sub-region grid map.

[0013] The steps in step (2) above to divide the substation raster map into multiple sub-region raster maps are as follows:

[0014] Step (2-1): Perform cluster analysis on the inspection points of the substation grid map;

[0015] Step (2-2): Divide the raster map according to the cluster boundaries.

[0016] The steps for planning access paths between sub-region raster maps in step (3) above are as follows:

[0017] Step (3-1): Traverse the cluster centers;

[0018] Step (3-2): Determine the access order between sub-region raster maps;

[0019] Step (3-3): Plan the path between two adjacent sub-region grid maps;

[0020] The steps for selecting the inspection start point and inspection end point within the sub-region grid map in step (4) above are as follows:

[0021] Step (4-1): Within the current sub-region raster map, calculate the cost distance C from the inspection point to the previous cluster center according to formula (1). pre ;

[0022]

[0023] In formula (1), α and β are weighting coefficients, and x i and y i Let a be the x and y coordinates of the i-th inspection point. pre and b pre These are the x and y coordinates of the previous cluster center, a cur and b cur These are the x and y coordinates of the current cluster center.

[0024] Step (4-2): Select the cost distance C pre The largest point is used as the starting point for sub-area inspection;

[0025] Step (4-3): Within the current sub-region raster map, calculate the cost distance C from the inspection point to the next cluster center according to formula (2).next ;

[0026]

[0027] In formula (2), α and β are weighting coefficients, and x i and y i Let a be the x and y coordinates of the i-th inspection point. next and b next These are the x and y coordinates of the next cluster center, a cur and b cur These are the x and y coordinates of the current cluster center.

[0028] Step (4-4): Select the cost distance C next The largest point is used as the endpoint for sub-area inspection.

[0029] In step (5) above, the steps for multi-node parallel directional heuristic path planning within the sub-region grid map are as follows:

[0030] Step (5-1): Within the sub-region, inspection points perform independent path searches;

[0031] Step (5-2): Broadcast the current grid position being searched by the inspection point;

[0032] Step (5-3): Determine whether grids with the same location are found between inspection points. If so, proceed to step (5-4); otherwise, return to step (5-1).

[0033] Step (5-4): Calculate the path matrix between any two inspection points according to formula (3);

[0034]

[0035] In formula (3), route ij Parent represents the path between the i-th inspection point and the j-th inspection point within the current sub-region. i Parent represents the vector from the i-th inspection point to the parent node of the current grid. j This represents the vector from the j-th inspection point to the parent node of the current grid. The operator sign indicates that two vectors are joined end to end.

[0036] In step (5-1) above, within a sub-region, the inspection points perform independent path searches as follows:

[0037] Step (5-1-1): Determine whether more than 3 paths have been planned for the current inspection point. If yes, end the search for the inspection point; otherwise, proceed to step (5-1-2).

[0038] Step (5-1-2): Determine whether the current inspection point has been searched 10 times and no new path has been planned. If so, end the search for the inspection point; otherwise, proceed to step (5-1-3).

[0039] Step (5-1-3): Calculate the heuristic direction Dir of the current search grid position of the remaining inspection points relative to the current search grid position of this grid;

[0040] Step (5-1-4): According to formula (4), calculate the cosine of the angle between the current grid search direction Expand and the heuristic direction Dir at this inspection point.

[0041]

[0042] In formula (4), Expand is the direction vector of the current grid search direction, Dir is the direction vector of the heuristic direction, and cos(α) is the cosine value of the angle between the two directions.

[0043] Step (5-1-5): Search for rasters with a cosine value greater than zero.

[0044] The steps for determining the access order of all inspection points within the sub-region grid map in step (6) above are as follows:

[0045] Step (6-1): Traverse the path matrix to obtain the unobstructed paths between inspection points;

[0046] Step (6-2): Use a genetic algorithm to calculate the inspection path of the sub-region grid map starting from the inspection start point and ending at the inspection end point.

[0047] The steps for optimizing the inspection path within the sub-region grid map in step (7) above are as follows:

[0048] Step (7-1): Iteratively correct the stepped path;

[0049] Step (7-2): Smooth right angles in the path.

[0050] The iterative correction steps for the stepped path in step (7-1) above are as follows:

[0051] Step (7-1-1): Calculate the slope vector between adjacent grid cells along the path according to formula (5);

[0052]

[0053] In formula (5), Slope(i) is the i-th element in the slope vector, and routeY i+1 It is the y-coordinate of the (i+1)th element in the path matrix, routeY iIt is the ordinate of the i-th element in the path matrix, routeX i+1 It is the x-coordinate of the (i+1)th element in the path matrix, routeX i It is the x-coordinate of the i-th element in the path matrix.

[0054] Step (7-1-2): Iterate through the slope vector Slope;

[0055] Step (7-1-3): Determine whether there exists a case in the slope vector Slope where the value of the k-th element is 0 and the value of the (k+1)-th element is infinite. If so, proceed to step (7-1-5); otherwise, proceed to step (7-1-4).

[0056] Step (7-1-4): Determine if there exists a case in the slope vector Slope where the value of the k-th element is infinite and the value of the (k+1)-th element is 0. If so, proceed to step (7-1-6); otherwise, end the correction.

[0057] Step (7-1-5): Determine whether there is a case where the j-th element is 0 from the (k+2)-th element to the last element in the slope vector Slope. If so, proceed to step (7-1-7); otherwise, end the correction.

[0058] Step (7-1-6): Determine whether there is a case where the value of the j-th element is infinite from the (k+2)-th element to the last element in the slope vector Slope. If so, proceed to step (7-1-7); otherwise, end the correction.

[0059] Step (7-1-7): Calculate the raster position matrix around the stepped path according to formula (6);

[0060]

[0061] In formula (6), P1 is the position matrix of the grid to be detected below the stepped path, P2 is the position matrix of the grid to be detected above the stepped path, x is the horizontal coordinate of the grid to be detected, y is the vertical coordinate of the grid to be detected, k is the index of the first grid in the stepped path, and j is the index of the last grid in the stepped path.

[0062] Step (7-1-8): Determine whether there are obstacles at the grid positions corresponding to the grid position matrix in the sub-region grid map. If yes, return to step (7-1-2); otherwise, proceed to step (7-1-9).

[0063] Step (7-1-9): Correct the path and return to step (7-1-1).

[0064] The beneficial effects of this invention are as follows: By dividing the substation grid map into multiple sub-regions and performing multi-node parallel directional heuristic path planning within the sub-regions, this method can effectively reduce the number of grids searched by the substation inspection robot, shorten the path planning time, and reduce the number of path turns, thereby improving the efficiency of path planning for the substation inspection robot. At the same time, this method also optimizes right-angle bends with arcs to increase the smoothness of the inspection path. Attached Figure Description

[0065] Figure 1 This is a system flowchart of the present invention;

[0066] Figure 2 This is a schematic diagram of the clustering results for inspection points;

[0067] Figure 3 This is a schematic diagram of the path planning results in a sub-region raster map. Detailed Implementation

[0068] The present invention will be further described below with reference to the accompanying drawings.

[0069] like Figure 1 As shown, a novel path planning method for a substation inspection robot includes the following steps:

[0070] (1) Construct a grid map of the substation;

[0071] (2) Divide the substation raster map into multiple sub-region raster maps;

[0072] (3) Plan access paths between sub-region grid maps;

[0073] (4) Select the inspection start point and inspection end point within the sub-region grid map;

[0074] (5) Perform multi-node parallel directional heuristic path planning within the sub-region grid map;

[0075] (6) Determine the access order of all inspection points within the sub-region grid map;

[0076] (7) Optimize the inspection path inside the sub-region grid map.

[0077] The steps in step (2) of dividing the substation raster map into multiple sub-region raster maps are as follows:

[0078] Step (2-1): Perform cluster analysis on the inspection points of the substation grid map;

[0079] Step (2-2): Divide the raster map according to the cluster boundaries.

[0080] The results of cluster analysis on inspection points of the substation grid map are as follows: Figure 2 As shown, Figure 2 Inspection points were divided into different categories using different symbols. Based on the clustering results, in Figure 2 The substation raster map is divided into four sub-regions using solid lines, and the substation sub-region raster map within the lower left corner is selected for further explanation.

[0081] The steps for planning access paths between sub-region grid maps in step (3) are as follows:

[0082] Step (3-1): Traverse the cluster centers;

[0083] Step (3-2): Determine the access order between sub-region raster maps;

[0084] Step (3-3): Plan the path between two adjacent sub-region grid maps;

[0085] The steps for selecting the inspection start point and inspection end point within the sub-region grid map in step (4) are as follows:

[0086] Step (4-1): Within the current sub-region raster map, calculate the cost distance C from the inspection point to the previous cluster center according to formula (1). pre ;

[0087]

[0088] In formula (1), α and β are weighting coefficients, and x i and y i Let a be the x and y coordinates of the i-th inspection point. pre and b pre These are the x and y coordinates of the previous cluster center, a cur and b cur These are the x and y coordinates of the current cluster center.

[0089] Step (4-2): Select the cost distance C pre The largest point is used as the starting point for sub-area inspection;

[0090] Step (4-3): Within the current sub-region raster map, calculate the cost distance C from the inspection point to the next cluster center according to formula (2). next ;

[0091]

[0092] In formula (2), α and β are weighting coefficients, and x i and y i Let a be the x and y coordinates of the i-th inspection point.next and b next These are the x and y coordinates of the next cluster center, a cur and b cur These are the x and y coordinates of the current cluster center.

[0093] The steps in step (5) of performing multi-node parallel directional heuristic path planning within the sub-region grid map are as follows:

[0094] Step (5-1): Within the sub-region, inspection points perform independent path searches;

[0095] Step (5-2): Broadcast the current grid position being searched by the inspection point;

[0096] Step (5-3): Determine whether grids with the same location are found between inspection points. If so, proceed to step (5-4); otherwise, return to step (5-1).

[0097] Step (5-4): Calculate the path matrix between any two inspection points according to formula (3);

[0098]

[0099] In formula (3), route ij Parent represents the path between the i-th inspection point and the j-th inspection point within the current sub-region. i Parent represents the vector from the i-th inspection point to the parent node of the current grid. j This represents the vector from the j-th inspection point to the parent node of the current grid. The operator sign indicates that two vectors are joined end to end.

[0100] Within the sub-region, the steps in step (5-1) for each inspection point to perform independent path searches are as follows:

[0101] Step (5-1-1): Determine whether more than 3 paths have been planned for the current inspection point. If yes, end the search for the inspection point; otherwise, proceed to step (5-1-2).

[0102] Step (5-1-2): Determine whether the current inspection point has been searched 10 times and no new path has been planned. If so, end the search for the inspection point; otherwise, proceed to step (5-1-3).

[0103] Step (5-1-3): Calculate the heuristic direction Dir of the current search grid position of the remaining inspection points relative to the current search grid position of this grid;

[0104] Step (5-1-4): According to formula (4), calculate the cosine of the angle between the current grid search direction Expand and the heuristic direction Dir at this inspection point.

[0105]

[0106] In formula (4), Expand is the direction vector of the current grid search direction, Dir is the direction vector of the heuristic direction, and cos(α) is the cosine value of the angle between the two directions.

[0107] Step (5-1-5): Search for rasters with a cosine value greater than zero.

[0108] The steps in step (6) to determine the access order of all inspection points within the sub-region grid map are as follows:

[0109] Step (6-1): Traverse the path matrix to obtain the unobstructed paths between inspection points;

[0110] Step (6-2): Use a genetic algorithm to calculate the inspection path of the sub-region grid map starting from the inspection start point and ending at the inspection end point.

[0111] The steps for optimizing the inspection path within the sub-region grid map in step (7) are as follows:

[0112] Step (7-1): Iteratively correct the stepped path;

[0113] Step (7-2): Smooth right angles in the path.

[0114] The iterative correction steps for the stepped path in step (7-1) are as follows:

[0115] Step (7-1-1): Calculate the slope vector between adjacent grid cells along the path according to formula (5);

[0116]

[0117] In formula (5), Slope(i) is the i-th element in the slope vector, and routeY i+1 It is the y-coordinate of the (i+1)th element in the path matrix, routeY i It is the ordinate of the i-th element in the path matrix, routeX i+1 It is the x-coordinate of the (i+1)th element in the path matrix, routeX i It is the x-coordinate of the i-th element in the path matrix.

[0118] Step (7-1-2): Iterate through the slope vector Slope;

[0119] Step (7-1-3): Determine whether there exists a case in the slope vector Slope where the value of the k-th element is 0 and the value of the (k+1)-th element is infinite. If so, proceed to step (7-1-5); otherwise, proceed to step (7-1-4).

[0120] Step (7-1-4): Determine if there exists a case in the slope vector Slope where the value of the k-th element is infinite and the value of the (k+1)-th element is 0. If so, proceed to step (7-1-6); otherwise, end the correction.

[0121] Step (7-1-5): Determine whether there is a case where the j-th element is 0 from the (k+2)-th element to the last element in the slope vector Slope. If so, proceed to step (7-1-7); otherwise, end the correction.

[0122] Step (7-1-6): Determine whether there is a case where the value of the j-th element is infinite from the (k+2)-th element to the last element in the slope vector Slope. If so, proceed to step (7-1-7); otherwise, end the correction.

[0123] Step (7-1-7): Calculate the raster position matrix around the stepped path according to formula (6);

[0124]

[0125] In formula (6), P1 is the position matrix of the grid to be detected below the stepped path, P2 is the position matrix of the grid to be detected above the stepped path, x is the horizontal coordinate of the grid to be detected, y is the vertical coordinate of the grid to be detected, k is the index of the first grid in the stepped path, and j is the index of the last grid in the stepped path.

[0126] Step (7-1-8): Determine whether there are obstacles at the grid positions corresponding to the grid position matrix in the sub-region grid map. If yes, return to step (7-1-2); otherwise, proceed to step (7-1-9).

[0127] Step (7-1-9): Correct the path and return to step (7-1-1).

[0128] A schematic diagram of path planning results in a sub-region raster map is shown below. Figure 3 As shown, in Figure 3 The sequence number is used to indicate the order in which the substation inspection robot visits the inspection points. Figure 3 The planned path is an inspection grid path that visits all inspection points in the sub-region. This path is an iterative optimization of the stepped path to minimize the number of turns. At the same time, right-angle bends are also optimized with arcs to increase the smoothness of the path.

[0129] The above-mentioned novel substation inspection robot path planning method was simulated under the following conditions: (1) Software environment: MATLAB 2016a; (2) Hardware environment: Intel i5-6300HQ processor @ 2.30GHz, memory size 8GB; (3) Grid map size: 50*50; (4) Number of inspection points: 6.

[0130] Table 1 Performance Comparison of Path Planning Algorithms

[0131] algorithm Search raster number Path planning time Number of turns Path length Dijkstra 5410 2.558s 61 136m A* 3144 1.592s 67 145m Method of the present invention 948 0.675s 10 136m

[0132] Table 1 shows the simulation results comparing the performance of path planning algorithms within the sub-region grid map. As can be seen from Table 1, the novel path planning method for substation inspection robots provided by this invention, compared with Dijkstra's algorithm, reduces the number of searched grids by 82.4%, path planning time by 73.6%, and the number of turns by 83.6%, while maintaining the same path length; compared with the A* algorithm, it reduces the number of searched grids by 69.8%, path planning time by 57.6%, the number of turns by 85.1%, and the path length by 6.2%. Therefore, the method provided by this invention effectively improves the efficiency of path planning for substation inspection robots and has good application prospects.

[0133] The above embodiments are merely preferred technical solutions of the present invention and should not be regarded as limitations on the present invention. The scope of protection of the present invention is not limited thereto, but also includes equivalent substitutions of the technical features in the technical solutions described in the claims.

Claims

1. A path planning method for a substation inspection robot, characterized in that: Includes the following steps: (1) Construct a grid map of the substation; (2) Divide the substation raster map into multiple sub-region raster maps; (3) Plan access paths between sub-region grid maps; (4) Select the inspection start point and inspection end point within the sub-region grid map; (5) Perform multi-node parallel directional heuristic path planning within the sub-region grid map; The steps in step (5) of performing multi-node parallel directional heuristic path planning within the sub-region grid map are as follows: Step (5-1): Within the sub-region, inspection points perform independent path searches; Step (5-2): Broadcast the current grid position being searched by the inspection point; Step (5-3): Determine whether grids with the same location are found between inspection points. If so, proceed to step (5-4); otherwise, return to step (5-1). Step (5-4): Calculate the path matrix between any two inspection points according to formula (3); （3） In formula (3), Indicates the number of sub-regions in the current sub-region The first inspection point and the first The path between inspection points Indicates the first The vector from each inspection point to the parent node of the current grid. Indicates the first The vector from each inspection point to the parent node of the current grid. The operator symbol indicates that two vectors are concatenated end to end; (6) Determine the access order of all inspection points within the sub-region grid map; (7) Optimize the inspection path within the sub-region grid map. The steps for optimizing the inspection path within the sub-region grid map in step (7) are as follows: Step (7-1): Iteratively correct the stepped path; Step (7-2): Smooth right angles in the path.

2. The path planning method for a substation inspection robot according to claim 1, characterized in that: The steps in step (2) of dividing the substation raster map into multiple sub-region raster maps are as follows: Step (2-1): Perform cluster analysis on the inspection points of the substation grid map; Step (2-2): Divide the raster map according to the cluster boundaries.

3. The path planning method for a substation inspection robot according to claim 1, characterized in that: The steps for planning access paths between sub-region grid maps in step (3) are as follows: Step (3-1): Traverse the cluster centers; Step (3-2): Determine the access order between sub-region raster maps; Step (3-3): Plan the path between two adjacent sub-region grid maps.

4. The path planning method for a substation inspection robot according to claim 1, characterized in that: The steps for selecting the inspection start point and inspection end point within the sub-region grid map in step (4) are as follows: Step (4-1): Within the current sub-region raster map, calculate the cost distance from the inspection point to the previous cluster center according to formula (1). ; （1） In formula (1), and These are weighting coefficients. and It is the first The x and y coordinates of each inspection point and These are the x and y coordinates of the previous cluster center. and These are the x and y coordinates of the current cluster center; Step (4-2): Select the cost distance The largest point is used as the starting point for sub-area inspection; Step (4-3): Within the current sub-region raster map, calculate the cost distance from the inspection point to the next cluster center according to formula (2). ; （2） In formula (2), and These are weighting coefficients. and It is the first The x and y coordinates of each inspection point and These are the x and y coordinates of the next cluster center. and These are the x and y coordinates of the current cluster center; Step (4-4): Select the cost distance The largest point is used as the endpoint for sub-area inspection.

5. The path planning method for a substation inspection robot according to claim 1, characterized in that: Within the sub-region, the steps in step (5-1) for each inspection point to perform independent path searches are as follows: Step (5-1-1): Determine whether more than 3 paths have been planned for the current inspection point. If yes, end the search for the inspection point; otherwise, proceed to step (5-1-2). Step (5-1-2): Determine whether the current inspection point has been searched 10 times and no new path has been planned. If so, end the search for the inspection point; otherwise, proceed to step (5-1-3). Step (5-1-3): Calculate the heuristic direction of the current search grid position of the remaining inspection points relative to the current search grid position of this grid. ; Step (5-1-4): Calculate the current grid search direction for this inspection point according to formula (4). Inspirational direction The cosine of the included angle; （4） In formula (4), It is the direction vector of the current grid search direction. It is the direction vector of the heuristic direction. It is the cosine of the angle between the two directions; Step (5-1-5): Search for rasters with a cosine value greater than zero.

6. The path planning method for a substation inspection robot according to claim 1, characterized in that: The steps in step (6) for determining the access order of all inspection points within the sub-region grid map are as follows: Step (6-1): Traverse the path matrix to obtain the unobstructed paths between inspection points; Step (6-2): Use a genetic algorithm to calculate the inspection path of the sub-region grid map starting from the inspection start point and ending at the inspection end point.

7. The path planning method for a substation inspection robot according to claim 1, characterized in that: The iterative correction steps for the stepped path in step (7-1) are as follows: Step (7-1-1): Calculate the slope vector between adjacent grid cells along the path according to formula (5). ; （5） In formula (5), It is the first in the slope vector One element, It is the th in the path matrix The y-coordinate of each element, It is the th in the path matrix The y-coordinate of each element, It is the th in the path matrix The x-coordinate of each element, It is the th in the path matrix The x-coordinate of each element; Step (7-1-2): Iterate through the slope vector ; Step (7-1-3): Determine the slope vector Does the first one exist in the middle? The value of the i-th element is 0, and the i-th element is 0. If the value of an element is infinite, proceed to step (7-1-5); otherwise, proceed to step (7-1-4). Step (7-1-4): Determine the slope vector Does the first one exist in the middle? The value of the nth element is infinite, and the nth element is infinite. If the value of each element is 0, proceed to step (7-1-6); otherwise, end the correction. Step (7-1-5): Determine the slope vector The Middle From the first element to the last element, does the first element exist? If the value of an element is 0, proceed to step (7-1-7); otherwise, end the correction. Step (7-1-6): Determine the slope vector The Middle From the first element to the last element, does the first element exist? If the value of an element is infinite, proceed to step (7-1-7); otherwise, end the correction. Step (7-1-7): Calculate the grid position matrix around the stepped path to be detected according to formula (6); （6） In formula (6), It is the matrix of the positions of the grid cells to be detected diagonally below the stepped path. It is the matrix of the positions of the grid cells to be detected diagonally above the stepped path. y is the x-coordinate of the grid to be detected, y is the y-coordinate of the grid to be detected, k is the index of the first grid in the stepped path, and j is the index of the last grid in the stepped path. Step (7-1-8): Determine whether there are obstacles at the grid positions corresponding to the grid position matrix in the sub-region grid map. If yes, return to step (7-1-2); otherwise, proceed to step (7-1-9). Step (7-1-9): Correct the path and return to step (7-1-1).

Images