Full-coverage path planning method and system of minimum spanning tree algorithm based on transverse and longitudinal control

Abstract

The invention provides a full-coverage path planning method and system based on a minimum spanning tree of transverse and longitudinal control and an A * algorithm, and the method comprises the steps: (1), carrying out the rasterization of an original map of a robot, and obtaining a raster map, and enabling each raster to be the size of one robot; (2) in order to improve the time efficiency and reduce the calculation complexity, carrying out partition processing on the grid map; (3) each grid partition is subjected to composition according to the up-down-left-right relation, and a graph structure represented by an adjacency list method is obtained; (4) executing a minimum spanning tree algorithm on each grid partition to fulfill the aim of fully covering the partition; (5) in order to obviously reduce the problem of excessive turning in full-coverage path planning, executing transverse and longitudinal control on the minimum spanning tree algorithm; (6) different grid partitions are migrated through A *; and then the steps (3)-(6) are repeated until all the grid partitions are covered. The method introduces the minimum spanning tree algorithm of transverse and longitudinal control and A * to solve the full-coverage path planning, has the advantages of high coverage rate and low repetition rate, reduces the traversal time, reduces the turning times through transverse and longitudinal control, and improves the system efficiency. And meanwhile, through partition processing, the complexity of the adjacency list representation graph structure is reduced, and the time consumption of algorithm planning is remarkably reduced.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning. Specifically, it relates to a full-coverage path planning method and system for partition processing, horizontal and vertical control, minimum spanning tree, and A* algorithm. Background Art

[0002] Full-coverage path planning refers to planning a path for a movable device to cover a given area under certain constraints. Full-coverage path planning has extensive applications in indoor and outdoor cleaning robots, outdoor UAVs for spraying pesticides, agricultural machinery for sowing and harvesting, etc. By achieving full coverage, cleaning, searching, sowing and harvesting, surveying and mapping, etc. can be realized.

[0003] Some criteria for full-coverage path algorithms: four indicators for measuring algorithm performance, namely area coverage rate, area repetition rate, number of corners, and total path length. For complete coverage of a given area, the planned path should completely avoid obstacles, have fewer turns, have a high coverage rate while having fewer repeated paths, and plan an optimal path under certain constraints.

[0004] Traditional full-coverage path planning is divided into random collision method, cell decomposition method, artificial potential field method based on map, and bio-inspired method; the cell decomposition method, whose principle is to decompose the entire working area into several obstacle-free sub-regions, make reciprocating motions within the sub-regions, and then connect each sub-region to achieve full-coverage path planning. Due to its simple method and easy implementation, etc., it is widely applied, but how to decompose the cells is its difficulty. This patent also performs sub-region processing on the grid map, but the decomposed ones are all simple structures such as squares and rectangles.

[0005] Previous full-coverage path planning relied on various sensors such as lidar, infrared ultrasonic waves, etc. for assistance; the more sensors it relied on, the worse the versatility; at the same time, the full-coverage method for indoor cleaning robots cannot be extended to outdoor UAVs and agricultural machinery, and the previous methods have a low full-coverage path coverage rate, high repetition rate, low efficiency, and waste of time and efficiency.

[0006] Therefore, a new technical solution needs to be proposed to improve the above technical problems. Summary of the Invention

[0007] Aiming at the defects of the prior art, the purpose of the present invention is to provide a full-coverage path planning method and system based on minimum spanning tree and A* with horizontal and vertical control, aiming to solve the problems of non-universality of full-coverage indoor and outdoor methods and low full-coverage efficiency.

[0008] The technical solution adopted by the present invention to solve its technical problems is:

[0009] In order to significantly improve time efficiency and reduce complexity, partition processing is performed on a larger map;

[0010] To reduce the duplication rate and improve the coverage rate, a method in graph theory is adopted. First, a graph is constructed for the grid partition, and the graph structure is represented by an adjacency list;

[0011] The minimum spanning tree algorithm is used to implement the full-coverage path planning;

[0012] To reduce the number of corners, horizontal and vertical controls are adopted to control the robot to move as straight as possible, and horizontal priority and vertical priority can be selected;

[0013] Between the grid partition maps, A* is used to achieve the transfer from one partition to another;

[0014] The beneficial effects of the present invention are as follows: The minimum spanning tree is used to implement the full-coverage path planning, reducing the total path length, improving the coverage rate, reducing the duplication rate, and avoiding the problems of missed scanning and repeated scanning; The horizontal and vertical controls are used to achieve straight-line walking, reducing the number of rotations of the robot; The shortest transfer path between partitions is achieved through A*, with the highest time efficiency; The execution time complexity is reduced exponentially through partitioning; The entire invention effectively improves the efficiency of full coverage. Detailed implementation manners

[0015] To better understand the technical solution of the present invention, the present invention will be described in detail in combination with specific embodiments. The following embodiments will help those skilled in the art better understand the present invention, but do not limit the present invention in any form. It should be noted that for those of ordinary skill in the art, without departing from the concept of the present invention, several changes and improvements can still be made, and these all belong to the protection scope of the present invention. Embodiment

[0016] According to a full-coverage path planning method based on the minimum spanning tree and A* algorithm provided by the present invention, the method includes the following steps: Step S1: Partition the original grid map, simplify the partitioning method through image processing, and each partition is a rectangle or a square.

[0017] Step S2: For each grid partition, connect each grid to the four grids above, below, left, and right to form a graph structure, and represent the graph structure by an adjacency list; Step S3: For the grid partition, the minimum spanning tree of each graph structure is implemented by the Prim algorithm, and the transfer from one grid partition to another grid partition is implemented by the A* algorithm between partitions; Embodiment

[0018] A-Satr algorithm planning [1] Given a grid map, a starting point and an ending point, use a heuristic method to plan a path from the starting point to the ending point, and use an eight-connected method to search for paths between adjacent grids; [2] Initialize the vector queue obtained by heuristic search and put the starting point into the queue; [3] Set the actual cost of all points in the grid map to a very large value, and set the actual cost of the starting point to 0; [4] Enter a loop, get the index of the position with the minimum actual cost. If this position points to the ending point, end the loop; [5] In the above loop, first sort the vector queue obtained by heuristic search in ascending order of cost, obtain the point with the minimum cost and delete it; then add the points that are not obstacle points and have not been visited among the eight neighborhood points of the minimum cost point to the vector queue of heuristic search, and update the cost value. Increase the cost value of the four neighborhood points by 1, and increase the cost value of the other four neighborhood points by 1.414; [6] Obtain the actual cost array from the starting point to any visited point; [7] According to the aforementioned actual cost array, start searching its eight neighborhoods from the ending point, obtain the position of the cost point closest to the starting point, and loop until reaching the starting point to obtain the path from the starting point to the ending point; Dijkstra algorithm planning [1] Given an adjacency list and a starting point, calculate the shortest path between the starting point and any other vertex, the array of predecessor vertices of the shortest path, and the array of distances between the starting point and any vertex; [2] Set the flag array all to 0, indicating that the shortest path between the starting point and any vertex has not been obtained; [3] Loop through each vertex of the adjacency list in turn, set the predecessor vertex of this vertex to -1 indicating no predecessor vertex, and set the distance from the starting point to this vertex to the weight between these two points; [3] Initialize the starting point, set the flag of the starting point in the flag array to 1, and set the distance of the starting point in the distance array to 0; [4] Loop through each vertex of the adjacency list in turn, and each time find the shortest path from the starting point; [5] In the above loop, first find the vertex closest to the starting point among the vertices whose shortest paths have not been obtained, and set the flag of this vertex in the flag array to 1; loop through all vertices of the adjacency list in turn, obtain the weights from this vertex to each vertex, and update the predecessor vertex array and the distance array according to the weights; Prim algorithm planning [1] Given an adjacency list and a starting point, plan the shortest path starting from the starting point and covering all vertices in the adjacency list [2] Loop through each vertex in the adjacency list and update the weight array from the starting point to each vertex; [3] Set the current point as the starting point, find the four adjacent points of the current point. If the weight of an adjacent point in the weight array is equal to 1, set the current position to this adjacent point. The points with a weight equal to 1 are adjacent points, and the points with a weight equal to 0 are the visited vertices.

[0019] [4] Implement horizontal and vertical control when finding the four adjacent points of the current point in the above steps, that is, if horizontal priority is adopted, first search the left and right neighborhoods, and then search the upper and lower neighborhoods; the same is true for vertical priority.

[0020] [5] If all four neighborhoods of the current point have been visited, starting from the current point, execute the Dijkstra algorithm to plan, obtain the distances from the current point to each vertex, find the vertex closest to the current point among the unvisited vertices, and the path between the current point and the closest vertex, and set the current point as the closest vertex; [6] Set the weight of the current vertex in the weight array to 0, loop through each vertex in the adjacency list in turn, and update the weight array to the weights between the current point and each vertex; [7] Return to step [3], execute in a loop until all vertices are covered; Full Coverage Path Planning [1] Read the original map image and perform a 5*5 erosion operation on the original map image, aiming to keep the robot as far away from obstacles as possible; [2] Perform rasterization processing on the original map image to obtain a raster map, and merge n*n rasters into 1 raster, where n depends on the size of the robot and the resolution of the original map; [3] Perform binarization processing on the raster map. The areas with pixel values of 255 are set to 255, indicating free passage areas, and other pixel values are set to 0, indicating obstacles; [4] Perform zoning processing on the raster map, and each zone is a standard rectangle or square; save each raster zone image and its position in the upper left corner of the raster map as an array of raster zone images; [5] Set the initial position of the robot, and calculate which zone the starting position is in according to the position and size of the upper left corner of each raster zone; [6] Obtain the raster zone image of the corresponding zone and the corresponding upper left corner position; [7] Construct an adjacency list based on the raster zone image. The specific method is as follows: construct the vertices of the adjacency list for the points with pixel values of 255; if the current raster is a non-occupied raster and its right raster is also a non-occupied raster, add an edge between the current raster and its right raster; if the current raster is a non-occupied raster and its lower raster is also a non-occupied raster, add an edge between the current raster and its lower raster; loop through each raster, and perform the first two operations on each raster until the entire map structure is constructed; and establish the correspondence between the vertex numbers of the adjacency list and the pixel indices of the raster zone image; [8]Execute the Prim algorithm based on the vertex number of the adjacency list corresponding to the initial position of the robot to implement the minimum spanning tree for horizontal and vertical control, so as to achieve the purpose of covering this grid partition; [9]Use the last position point of the Prim algorithm in the foregoing steps as the starting point, and calculate the nearest point in the remaining partitions to this starting point as the end point; the calculation method of the end point is as follows: On the grid image, set all the pixels of the already fully covered grid partition to 0, indicating the non-walkable area; calculate the contour on this image, obtain the contour closest to the starting point, and loop through each point on the nearest contour to find the nearest point on the nearest contour as the end point;

[10] Execute the A* algorithm between the foregoing starting point and end point;

[11] Calculate the grid partition corresponding to the end point, and repeat steps [7]-

[10] to fully cover other grid partitions until all partitions are covered.

[0021] The specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the above specific embodiments, and those skilled in the art can make various changes and modifications within the scope of the claims, which do not affect the essence of the present invention. Without conflict, the embodiments and features of this application can be combined arbitrarily with each other.

[0022] In order to exponentially reduce the calculation time and complexity, the present invention first partitions the grid map to effectively reduce the size of the connected graph structure to achieve the purpose; secondly, each grid partition is connected together according to the up, down, left, and right relationships to form a graph structure, which is convenient for subsequent solution using graph theory methods; the minimum spanning tree of the grid partition realizes full area coverage; the minimum spanning tree with horizontal and vertical control is used to try to go straight and reduce the number of turning angles; the transfer from one grid partition to another is realized through A*. Description of the Drawings

[0023] Figure 1 is the overall system flowchart of the full coverage path planning method of the present invention.

[0024] Figure 2 is a schematic diagram of the three-partition of the indoor original grid map. The black dots in the figure represent occupied grids, indicating obstacles; the white grids represent free grids, indicating the walkable area without obstacles; the gray area represents the uncertain area and is not walkable.. Figure 3 is a schematic diagram of the four-partition of the indoor original grid map. Figure 4 is a schematic diagram of the three-partition of the outdoor original grid map. Figure 5 is a schematic diagram of the four-partition of the outdoor original grid map. Figure 6 is the grid map image after corrosion treatment. FIG. 7 is a simulation effect diagram of a single partition of the full coverage path planning of the present invention when there are many indoor obstacles. The red dot is the starting position of the partition, and the green dot is the end position of the partition. FIG8 is a simulation effect diagram of four partitions of the full coverage path planning of the present invention when there are many indoor obstacles. The red dots are the starting positions of each partition, the green dots are the end positions of each partition, and the lines from the green dots to the red dots are the transfer paths from one partition to another. FIG. 9 is a simulation effect diagram of four partitions of the full coverage path planning of the present invention under the condition of few outdoor obstacles, with lateral walking being given priority. FIG. 10 is a simulation effect diagram of four partitions of the full coverage path planning of the present invention under the condition of few outdoor obstacles, with vertical walking being given priority.

Claims

1. A full coverage path planning method based on the minimum spanning tree and A* algorithm for lateral and longitudinal control, characterized in that The following steps are involved: Step S1, rasterizing the original map, with each grid being the size of a robot; Step S2: Perform a 3*3 corrosion operation on the original map to prevent the robot from planning a path too close to obstacles. Step S3, in order to avoid the exponential growth of the amount of calculation of the adjacency table representing the graph structure as the grid map increases, the robot grid map is partitioned; an image of each grid partition and its upper left corner position in the grid map are obtained; Step S3: Set the robot's starting position and calculate the grid partition where the starting position is located; Step S4, constructing an adjacency list vertex for the non-occupied grid points with a pixel value of 255 in the grid partition; if the current grid is a non-occupied grid, the grid on its right is also a non-occupied grid, and adding an edge between the current grid and the grid on its right; if the current grid is a non-occupied grid, the grid below it is also a non-occupied grid, and adding an edge between the current grid and the grid below it; looping and calculating each grid, performing the first two steps on each grid; establishing a corresponding relationship between the vertex number and the pixel index of the grid partition image; establishing an adjacency list based on the vertices and edges, and using the adjacency list to represent the graph structure; Step S5, executing a minimum spanning tree algorithm with horizontal and vertical control on the adjacency list graph structure of the grid partition, and calculating a string of path nodes covering the grid partition from the starting point; Step S6, setting the pixels of the aforementioned grid partition on the grid map image to 0, indicating that the area is no longer walkable; Step S6: Execute horizontal and vertical control on the minimum spanning tree algorithm in the above steps to control the walking direction, avoid the problem of non-uniqueness of the minimum spanning tree, reduce turns, and control to walk in a straight line as much as possible. Horizontal walking priority and vertical walking priority can be set according to actual needs; Step S7, determine whether all grid partitions have been traversed, and if so, end the planning; If not all partitions are covered, proceed to the next step; Step S8, taking the last vertex of the minimum spanning tree in step 5 as the starting point, extracting the contour on the grid map image, obtaining the contour index closest to the starting point, traversing each point on the nearest contour, and obtaining the point closest to the starting point as the end point; Step S9, using A* to plan a path between the starting point and the end point; Step S10, calculate the grid partition where the aforementioned end point is located, and repeat steps S4-S9.

2. A robot full coverage path planning method based on minimum spanning tree and A* algorithm for lateral and longitudinal control according to claim 1, characterized in that 1. Only one robot map is needed, which does not rely on hardware such as sensors and has low cost; 2. This method only relies on maps and is applicable to solving full coverage paths both indoors and outdoors, solving the problem of universality of indoor and outdoor methods; 3. This method partitions the grid map, avoiding the overly complex adjacency table of each grid partition to represent the graph structure, which can reduce the time consumption of path planning exponentially and significantly improve efficiency; at the same time, each partition is a rectangle or square, the partition method is simple, and effectively solves the complex partitioning problem of the unit decomposition method; 4. This method uses the minimum spanning tree to plan the full coverage path, with extremely low repetition rate and high coverage rate, which effectively solves the time and efficiency problems; 5. This method uses the minimum spanning tree algorithm with horizontal and vertical control to solve the problem of too many turns caused by the non-uniqueness of the minimum spanning tree. The planned path will try to go in a straight line; 6. This method uses the method of extracting contours and obtaining the closest point on the contour closest to the end point of the previous partition planning path as the starting point of the next partition, and obtains the problem of how to plan to the next partition after completing a partition. The A* algorithm can significantly improve the speed and avoid the problem of too much time spent on planning the end point of the previous partition planning path to the next partition through the Dijkstra algorithm.

Images