Disclosure of Invention
The invention provides a high-precision long-distance off-road path planning method for a four-corner grid network, which is used for solving the problems that the method in the prior art is low in operation efficiency and easy to operate and collapse.
In order to solve the technical problems, the technical scheme and the corresponding beneficial effects of the technical scheme are as follows:
the invention provides a high-precision long-distance off-road path planning method for a four-corner grid network, which comprises the following steps of:
1) rasterizing an off-road environment area needing path planning;
2) judging whether the distance between the starting point and the end point is greater than a set distance;
3) if the distance between the starting point and the end point is greater than the set distance, carrying out merging operation on the grid pixels in the cross-country environment area; after the merging operation is carried out, path planning is carried out between the starting point and the end point to obtain a better path;
4) extracting a plurality of reference points on the superior path, dividing the superior path into a plurality of sections of segmented paths by the plurality of reference points, the starting point and the end point, and re-planning the path by taking two end points of each section of segmented path as the starting and end points respectively to obtain a plurality of sections of superior segmented paths;
5) and connecting the paths of the sections of the better sub-sections to obtain a final path.
The beneficial effects of the above technical scheme are: under the condition that the distance between a starting point and an end point is far, the method firstly carries out merging operation on grid pixels in the cross-country environment area so as to reduce the processing amount of overall calculation and improve the processing speed, carries out path planning after merging operation so as to plan a better path, then carries out segmentation processing on the planned better path, carries out high-precision path planning by taking two end points of each segment of the segmented path as the starting point and the end point so as to plan the better segmented path, enables the planning of each segment of the segmented path to be completed under high-precision grids, and finally connects each segment of the better segmented path to form a final path, thereby meeting the requirement of the precision of the cross-country path planning and improving the operation efficiency.
Further, in step 3), after the merging operation is performed, path planning is performed between the starting point and the end point by using a Dijkstra algorithm.
Further, in order to make the planned path more smooth, in step 4), taking two end points of each segment of the segmented path as starting and ending points, and adopting an improved a-algorithm to perform path planning again; the cost function of the improved A-algorithm is F-G + H; g is a compensation value from the initial node to the current node; h is a composite value and is determined by Euclidean distance and elevation difference.
Further, if the judgment result of the step 2) is that the distance between the starting point and the end point is larger than the set distance, path planning is directly performed between the starting point and the end point.
Further, an improved A-star algorithm is adopted to directly plan a path between the starting point and the end point; the cost function of the improved A-algorithm is F-G + H; g is a compensation value from the initial node to the current node; h is a composite value and is determined by Euclidean distance and elevation difference.
Further, in step 2), the following method is adopted to determine whether the distance between the starting point and the end point is greater than the set distance:
calculating the product of the absolute value of the difference value of the starting and ending point row numbers and the absolute value of the difference value of the starting and ending point column numbers;
judging whether the product is larger than a set value: if the distance is larger than the set value, the distance between the starting point and the end point is larger than the set distance; otherwise, the distance between the starting point and the end point is less than or equal to the set distance.
Further, in step 4), the rule for extracting a plurality of reference points is as follows: the number of grid pixels between adjacent reference points is equal.
Detailed Description
The invention discloses an embodiment of a method for planning a high-precision long-distance off-road path by using a four-corner grid, which has a flow chart shown in figure 1 and comprises the following specific processes:
the method comprises the steps of firstly, rasterizing the off-road environment needing path planning, converting the geographic coordinates of the off-road environment area between a starting point and an end point into grid row and column numbers, and calculating the product n of the absolute value multiplied by the row and column numbers of the starting point and the end point.
Step two, judging whether the n value obtained by the step one is larger than a set value (for example, 300000):
if the value of n is larger than the set value, the distance between the starting point and the end point is far, and then the third step is executed;
if the value of n is not larger than the set value, the distance between the starting point and the end point is not far, and in the off-road environment area, the route planning is directly carried out between the starting point and the end point by using the improved A-algorithm. The flow of the improved a-algorithm is shown in fig. 2, and the improved a-algorithm adds an influence factor of the height difference for the path to preferentially select the gentle path. In addition, the algorithm is different from the traditional heuristic A-x algorithm, intermediate data are not stored too much, the memory pressure can be reduced, and the operation efficiency is improved. The whole process is as follows:
1) first, an open table and a closed table are established, and each table is initialized. Wherein the open table comprises all pixels to be searched, and all pixels are initialized to 0; the closed table records that the read image element is marked as the image element which cannot pass, and marks the two types of image elements as 1.
2) And creating a vector container m _ openList to accommodate points on the path.
3) And converting the geographical coordinates of the starting point and the ending point into row and column numbers, and taking out the values of the corresponding pixels from the data according to the row and column numbers. And taking the planning start coordinate as a start node m _ startPos, and taking the planning end coordinate as a target node m _ endPos.
4) A move pointer is created to point to the current node.
5) And storing the starting point into the m _ openList in a row and column number form to serve as a starting father node, moving a pointer to the node, and sequentially judging the trafficability and the F value of eight points around the father node. The trafficability is judged by whether the child node is in a closed table, and on the premise of being trafficable, F consists of two parts, namely a G value and an H value. The calculation formula of the F value is as follows:
F=G+H
the G value is a compensation value from the starting node m _ startPos to the current node, if the adjacent child nodes do not have the weight values, the calculation is carried out by respectively using 10 or 14 at equal distance, and if the adjacent child nodes have the weight values, the weight values are used as judgment standards; the H value is a composite value and is determined by Euclidean distance and elevation difference, the Euclidean distance is the distance from a current point to a terminal point and mainly plays a role in pointing, the elevation difference is the difference value of the elevations of a father point and the current point and is mainly used for selecting relatively flat terrain and finally calculating the F value of the current point.
6) And respectively comparing the F values of the eight points, selecting the minimum value, adding the minimum value into the m _ openList to serve as the current point of the road, moving the pointer to the point, and marking the point as 1 to represent the point on the road.
7) And (3) searching the conditions of the nodes in the eight directions around the current pixel serving as a father node, firstly judging whether the nodes are marked, if so, judging the positions on the path, if not, directly skipping to indicate that the nodes are impassable points, and if so, finding the positions. On the road, the paths are overlapped, and then the compromise processing is carried out, namely, the cross path occurs, and the crossed part is removed. If the parent node is passable and not on the road, the storage comparison is carried out, and the node with the minimum F value in the peripheral eight direction child nodes is selected as the next node of the path.
8) And if the eight directions of the father node are judged to be not feasible, carrying out rollback, searching the minimum value point in the remaining child points in the father node of the previous stage again, and if the eight directions of the father node are judged to be infeasible, indicating that no path is found and finishing the algorithm.
9) And repeating the steps 4) -7) until the terminal node is added into the m _ openList, which indicates that the path is found.
It should be noted that, in the improved a-x algorithm, a factor of elevation difference is added to the H-composite value, so that the path for assisting planning is more gradual. The improvement of the algorithm causes the calculated path not to be optimal, but can improve the efficiency of the operation, and the operation is not broken even under huge data volume. In testing, it was found that the conventional a algorithm took seven to eight minutes to plan a path ten kilometers away, and that an hour could not be the result even further away (e.g., approaching thirty kilometers), and that improved a took less than ten seconds. Therefore, the practical application value of the improved A-x algorithm under the condition of higher-precision map is higher.
And step three, under the condition that the value of n is larger than the set value, carrying out merging operation on the grid pixels. The merging operation here is to perform merging operation processing on all grid pixels, that is, to change the resolution of the grid map. For example, the size of the merged grid pixel is 64 × 64, as shown in fig. 3, the whole square is one merged grid pixel, and the small cells in the square are the original grid pixels. In general, when the values are combined to 64 × 64, the value of n does not exceed the set value of 300000. However, if the n value after merging still exceeds the set value of 300000, the merging operation is performed again, and the merging operation is 128 × 128, and so on.
And step four, after the merging operation is carried out, path planning is carried out between the starting point and the end point by utilizing a Dijkstra algorithm, and a better path is planned. The process of the Dijkstra algorithm is as follows:
1) a distance matrix distanceFromStart of nodes from the starting point is created, the value of the starting point is set to 0, and the remaining elements are set to infinity inf.
2) A parent node matrix parent of the node is created, initialized to 0, and represents no parent node.
3) When iteration updating is carried out each time, finding out a node with the lowest total evaluation cost in incomplete traversal nodes in the distanceFromStart, and setting the node as a current node; current _ node.dist ═ inf, (prevent traversal again), and mark as having completed traversal; the non-traversed node in the four adjacent nodes of the current _ node is judged, whether the adjacent node N.g is larger than current _ node.g + edge.cost is judged, if so, N.g is current _ node.g + edge.cost, and n.parent _ node is current _ node.
4) And repeating the step 3) until the current _ node is the target point.
5) And backtracking the optimal path according to the parent node matrix parent.
And step five, extracting a plurality of reference points on the planned better path according to a set rule, and dividing the planned better path in the step four into a plurality of sections of segmented paths by the reference points, the starting point and the end point. In this embodiment, the formulated rule is: starting from the start point, one reference point is taken for each number (e.g., 20) of grid pixels until the end point is reached.
And step six, taking two end points of each segment of the segmented path as starting and ending points, and adopting an improved A-x algorithm to perform path planning again to obtain a plurality of segments of the preferred segmented paths.
And step seven, communicating the sections of the optimal subsection paths obtained in the step six to obtain a final path, and using the final path as an optimal path planned between the starting point and the end point.
Thus, path planning can be completed. Under the condition that the distance between a starting point and an end point is far, grid pixels in a cross-country environment area are merged to reduce the processing amount of overall calculation and improve the processing speed, path planning is carried out after merging processing to plan a better path, then the planned better path is segmented, high-precision path planning is carried out again by taking two end points of each segmented path as the starting point and the end point to plan the better segmented path, planning of each segmented path is completed under high-precision grid, and finally the better segmented paths are communicated to form a final path, so that the requirement for the planning precision of the cross-country path is met, the operation efficiency is improved, and the problems of low operation efficiency and easy operation collapse in the prior art are solved.
The method of the present invention is applied to specific examples to illustrate the effectiveness of the method. Image data, DEM data and SHP data (including roads, residential areas, vegetation, geology and the like) of a certain area (as shown in figure 4) are selected for implementation, the off-road environment of the planned area is quantized into raster data, and then the sectional path planning is carried out by using the method. Some of the parameters used are as follows: the setting value is 300000, and when the reference points are extracted, one reference point is extracted every 20 grid pixels (as shown in fig. 5). The final planning result is shown in fig. 6-1 to 6-4, the length of the path plan is 32.717 km, and the planning calculation time is 9 seconds.