Helicopter obstacle avoidance path planning method

Abstract

The invention provides a helicopter obstacle avoidance path planning method, which belongs to the technical field of aviation path planning, and comprises the following steps: marking obstacle points on an elevation grid map, and filling by adopting a water cup filling method; acquiring an end point meeting the constraint condition, and when the starting point or the end point is a filling barrier point, diffusing the filling barrier point cancelling from the starting point or the end point; a two-way A * algorithm is adopted for synchronous expansion, the obstacle avoidance path of the helicopter is searched, the obstacle avoidance path of the helicopter in a three-dimensional space is planned by designing the total cost of the starting point direction and the end point direction, an adjacent node list is generated, and new adjacent nodes are generated through a wall winding method; and when the two-way search encountering is carried out, backtracking is carried out along father nodes in two directions from the encountering node until the starting point and the ending point, and backtracking paths in the two directions are combined to obtain a complete path from the starting point to the ending point. According to the invention, the problem of low efficiency of a helicopter obstacle avoidance path search algorithm in a complex three-dimensional space is solved.

Description

Technical Field

[0001] The present invention belongs to the technical field of aviation path planning, and particularly relates to a method for obstacle avoidance path planning of a helicopter. Background Art

[0002] In recent years, with the occurrence of natural disasters and various emergencies, the demand for emergency rescue services has increased rapidly. Helicopters, with their characteristics of fast rescue speed, high rescue efficiency, and strong rescue professionalism, have been widely used in emergency rescue.

[0003] However, in the process of helicopter emergency rescue, the lack of efficient path planning is likely to result in insufficient helicopter emergency rescue efficiency, and the impact of limiting factors is often ignored, leading to the failure of rescue missions or even helicopter accidents.

[0004] Currently, the commonly used algorithms mainly include Dijkstra algorithm, Rapidly-exploring Random Trees (RRT) algorithm, A* algorithm, ant colony algorithm, etc. In the actual application process, the helicopter path planning problem faces many challenges and limitations. Firstly, factors such as terrain, meteorological conditions, and obstacles need to be considered during flight. Secondly, factors such as flight time, energy consumption, and flight safety also need to be taken into account. Therefore, it is particularly important to propose an effective path planning algorithm.

[0005] The A* algorithm is a heuristic search algorithm widely used in path planning and graph search of graphs. It combines the characteristics of best-first search and Dijkstra algorithm. The A* algorithm guides the search process by estimating the shortest distance from the starting point to the target, while considering the cost of the path already traveled and the search cost. Summary of the Invention

[0006] In view of the above deficiencies in the prior art, the present invention provides a method for obstacle avoidance path planning of a helicopter, which solves the problem of low efficiency of the obstacle avoidance path search algorithm for a helicopter in a complex three-dimensional space.

[0007] To achieve the above object, the technical solution adopted by the present invention is as follows: A method for obstacle avoidance path planning of a helicopter, comprising the following steps:

[0008] S1. Preprocess the elevation grid map, mark the obstacle points, and fill them using the water cup filling method;

[0009] S2. Based on the filling result, obtain the end point that meets the constraint conditions, and when the starting point or the end point is a filled obstacle point, cancel the filling of the obstacle point by spreading from the starting point or the end point;

[0010] S3. Based on the diffusion cancellation result, use the bidirectional A* algorithm to expand synchronously, search for the obstacle avoidance path of the helicopter, and plan the obstacle avoidance path of the helicopter in three-dimensional space by designing the actual cost and heuristic cost in the total cost of the starting direction and the ending direction, generating a list of adjacent nodes. Among them, when the bidirectional search meets, enter S5;

[0011] S4. Based on the generated list of adjacent nodes, use the wall following method to generate new adjacent nodes, take the optimal adjacent node in the new list of adjacent nodes as the next node, and return to S3;

[0012] S5. Starting from the meeting node, trace back along the parent nodes in the two directions respectively until the starting point and the ending point, and merge the trace-back paths in the two directions to obtain a complete path from the starting point to the ending point, completing the planning of the obstacle avoidance path of the helicopter.

[0013] The beneficial effects of the present invention are as follows: The present invention provides an improved method for efficiently planning the obstacle avoidance path of a helicopter based on the A* algorithm, mainly including preprocessing the map by the water cup filling method to reduce the search space; adopting the bidirectional A* algorithm to expand synchronously to accelerate the search efficiency; reasonably designing the actual cost and heuristic cost in the total cost to improve the efficiency of path planning in three-dimensional space; reducing unnecessary node actions to improve the search efficiency; avoiding falling into a dead loop by the wall following method, effectively improving the practicability in complex environments.

[0014] Further, the specific content of S1 is as follows:

[0015] Preprocess the elevation grid map, mark the area above 4800m as obstacle points, and use the water cup filling method for filling; among them,

[0016] The area above 4800m refers to the area where the current grid height in the elevation grid map is greater than 4800m, mark it as an obstacle area, and the helicopter cannot fly over; the water cup filling method refers to reducing the search space in the helicopter obstacle avoidance path planning by filling concave areas, including horizontal filling and vertical filling, and the scanning strategy refers to traversing the map horizontally row by row to check the passable points;

[0017] During horizontal filling, if the point on the left side of the current point is an obstacle point, and there are obstacle points above or below the current point, start spreading to the right with this point as the starting point, and add the spread points into the spread list. All spread points maintain the support points in the same direction as the starting point. If different-side support or no support appears during the spread, it is determined as a leak and the filling of the current point is terminated; if the spread encounters an obstacle point on the right side, mark the points in the spread list as filling obstacle points;

[0018] When filling vertically, if there is an obstacle point above the current point, and there is an obstacle point on the left or right side of the current point, start spreading downward from this point and add the spread points into the spread list. All spread points maintain the support points in the same direction as the starting point. If there is support on the opposite side or no support during the spread, it is determined as a leak and the filling of the current point is terminated; if the spread encounters an obstacle point on the lower side, mark the points in the spread list as filling obstacle points;

[0019] Continuously traverse the map until a traversal is completed when the spread list is empty, and the filling is completed.

[0020] The beneficial effect of the above further solution is: The present invention provides a preprocessing solution for the water cup filling method, which can reduce the search space of the helicopter in three-dimensional space and greatly improve the search efficiency.

[0021] Furthermore, the specific content of S2 is as follows:

[0022] S201. Based on the filling result, determine whether the end point meets the constraint conditions. If so, enter S203; otherwise, perform S202. Among them, the end point cannot be an obstacle point, that is, the height does not exceed 4800m, and at the same time, the slope of the end point is not higher than 30 degrees;

[0023] S202. Spread to find a new end point that meets the conditions and enter S203;

[0024] S203. When the starting point or the end point is a filling obstacle point, spread from the starting point or the end point to cancel the filling obstacle point.

[0025] The beneficial effect of the above further solution is: Avoid the situation where the starting point or the end point is filled as being surrounded by filled obstacles due to the preprocessing solution of the water cup filling method, resulting in the failure of path planning.

[0026] Furthermore, the specific content of spreading to find a new end point that meets the conditions is as follows:

[0027] Spread in eight directions around the end point, check whether the constraint conditions including slope and height are met. If not, continue to spread in eight directions based on the eight neighbor points until a new end point that meets the constraint conditions is found;

[0028] The expression of the slope is as follows:

[0029]

[0030]

[0031]

[0032] Among them, represents the slope, Represents the absolute value of the height difference between the neighbor point and the center point, Represents the absolute value of the horizontal distance between the neighbor point and the center point, Represents the height of the neighbor point, Represents the height of the center point, Represents the horizontal coordinate of the neighbor point, Represents the horizontal coordinate of the center point.

[0033] The beneficial effect of the above further solution is: avoiding setting the end point at a height of 4800m, which may cause the helicopter to be unable to reach the end point and the path planning to fail. The slope calculation is used to avoid selecting a point with too steep a slope, where the helicopter cannot safely and stably land.

[0034] Furthermore, for canceling the filled obstacle points by spreading from the starting point or the ending point, specifically:

[0035] When the starting point or the ending point is a filled obstacle point, spread in eight directions centered on the starting point or the ending point, and restore all the points marked as filled obstacles in the spreading path to a passable state; if an obstacle point or a passable point is encountered during the spreading process, the current point will not be spread until there are no more points to spread.

[0036] Furthermore, the specific content of S3 is:

[0037] S301. Based on the result of canceling by spreading, initialize the bidirectional A* algorithm, and create open lists starting from the starting point and the ending point respectively 、 and closed lists 、 ;

[0038] S302. Initialize the total costs in the directions of the starting point and the ending point, and add the starting point and the ending point to the open lists and respectively. Among them, the expressions of the total costs in the directions of the starting point and the ending point are as follows:

[0039]

[0040]

[0041]

[0042]

[0043]

[0044]

[0045] Among them, Represents the total cost in the starting direction, Represents the total cost in the ending direction, Represents the starting point, Represents the ending point, Represents the actual cost of the starting point, with the starting point being 0, Represents the current node 's actual cost, Represents the movement cost from the current node to the starting point node Since it is in the starting direction and the starting point is the first node, there is no situation of reaching the starting point from other nodes, so they are all 0, Represents the heuristic cost from the starting point to the ending point, Represents the actual cost of the ending point, with the ending point being 0, Represents the current node 's actual cost, Represents the movement cost from the current node to the starting point node ; Represents the heuristic cost from the ending point to the starting point, Represents the position coordinates of the starting point, Represents the position coordinates of the ending point;

[0046] S303. Judge whether the open list and are empty lists. If so, the obstacle avoidance path of the helicopter in the three-dimensional space cannot be planned, and the process ends. Otherwise, go to S304;

[0047] S304. If the open lists in both directions are non-empty, then respectively select the node in the open list with the smallest value and the node in the open list with the smallest value;

[0048] S305. Judge whether the node N with the smallest f value selected from the two open lists exists in the closed list in the other direction. If so, it is judged that the bidirectional search meets, and go to S5. Otherwise, go to S306, where the f value represents the total cost of the current node;

[0049] S306. Remove the current node N from the open list and add the current node N to the closed list , and at the same time generate a list of adjacent nodes , where the target nodes refer to the targets to be reached in two directions respectively. The target node starting from the starting direction is the end point, and the target node starting from the end direction is the starting point. The direction angle refers to the angle between the current node and the target node.

[0050] Furthermore, the generation process of the adjacent node list in S306 is as follows:

[0051] Remove the current node N from the open list and add the current node N to the closed list . Use the following formula to obtain the node with the smallest f value in the open list :

[0052]

[0053]

[0054] where N represents the node in the open list , represents the set composed of the f values of all nodes in the open list , represents the node with the smallest f value in the open list , represents taking the node with the smallest f value in the open list , here it is , represents the node with the smallest f value in the open list , represents taking the node with the smallest f value in the open list , here it is ;

[0055] Based on the addition result, in the forward running direction of the helicopter, generate a direction angle according to the direction between the current node N and the target node. Among them, the eight surrounding grids in the horizontal direction are named , , , , , , and :

[0056]

[0057] For the direction angle of , its horizontal expansion direction is , and the adjacent node list in the three-dimensional expansion direction Distributed among nine points: the front upper left node A, the front left node B, the front lower left node C, the front upper node D, the front node E, the front lower node F, the front upper right node G, the front right node H, and the front lower right node I on the entire front surface. Among them, if the coordinates of the current node N are , its adjacent node list The coordinates are as follows:

[0058]

[0059] Among them, represents the direction angle, , , , , , , , and respectively represent that when the coordinates of the current node N are , and the horizontal expansion direction is , the positions of nodes A, B, C, D, E, F, G, H, I in the adjacent node list . If the direction angle is , and its horizontal expansion direction is , then for the nodes in the adjacent node list except for the node , the x-direction and y-direction are changed to . Among them, for the nodes in the adjacent node list except for the node , they need to be changed based on the node as follows:

[0060] .

[0061] The beneficial effect of the above further solution is that in the node search design of the three-dimensional grid space, each node has the ability to expand in 26 directions, and these directions are evenly distributed on the six surfaces of the node: front, back, top, bottom, left, and right. When the helicopter is flying normally, it should maintain the movement in the forward direction. Therefore, the horizontal expansion direction is determined according to the direction angle between the current node and the target node. In this way, the nodes that the helicopter can choose to expand in the three-dimensional direction are limited to 9 grid actions in the forward direction. This optimization strategy greatly reduces unnecessary search nodes and significantly improves the search efficiency.

[0062] Furthermore, the specific content of S4 is as follows:

[0063] S401. Determine whether the nodes in the adjacent node list meet the conditions. If so, enter S402; otherwise, generate a new adjacent node M using the wall-following method and enter S402;

[0064] S402. Determine whether the adjacent node M is already in the open list in the current direction. If so, go to S404; otherwise, go to S403. In the list, if so, enter S404; otherwise, enter S403.

[0065] S403. Determine whether the adjacent node M is already in the closed list in the current direction. In the list, if so, enter S405; otherwise, enter S406.

[0066] S404. Determine Whether it is less than . If so, the actual cost from the current node N to the adjacent node M is the smallest, update the parent node of the adjacent node M to the node N, recalculate the total cost , and return to S303. Otherwise, do not update and return to S303. Among them, Represents the actual cost from the current node N to the adjacent node M. Represents the open list Records the actual cost of reaching the adjacent node M from a node other than the current node N.

[0067] S405. Determine Whether it is less than . If so, the adjacent node M is removed from the closed list , update its parent node, actual cost Value and total cost Value, return to S303. Otherwise, do not update and return to S303.

[0068] S406. Set the parent node of the adjacent node M to the node N, total cost , add the adjacent node M to the open list And return to S303.

[0069] The beneficial effect of the above further solution is as follows: Check whether the adjacent node M is in the open list. If so, process it to avoid repeated expansion, save computing resources, and improve the algorithm efficiency. If not, check whether the adjacent node M is in the closed list. If so, decide whether to re-enable it. If not, add it to the open list after setting the attributes , reasonably manage the search space, and make the search more orderly. And comparing Helps to find a globally better path and improve the result of the minimum path planning cost. And

[0070] Furthermore, the expression of the total cost is as follows:

[0071]

[0072] Among them, is the heuristic cost, representing the three-dimensional Manhattan distance between the adjacent node M and the target node G

[0073] The beneficial effect of the above further solution is that the total cost includes the actual cost and the heuristic cost , where ensures that the algorithm will not deviate from the explored better actual cost, and the three-dimensional Manhattan distance, as the heuristic cost, indicates the direction towards the target node. The algorithm comprehensively considers the two to accurately evaluate the nodes, so as to find a better path. BRIEF DESCRIPTION OF THE DRAWINGS

[0074] Figure 1 is the flowchart of the method of the present invention.

[0075] Figure 2 is the forward direction diagram determined according to the angle between the current node and the target node.

[0076] Figure 3 is the node search range diagram. DETAILED DESCRIPTION OF THE INVENTION

[0077] The following describes the specific implementation manners of the present invention to facilitate those skilled in the art to understand the present invention. However, it should be clear that the present invention is not limited to the scope of the specific implementation manners. For those of ordinary skill in the art, as long as various changes are within the spirit and scope of the present invention defined and determined by the appended claims, these changes are obvious, and all inventions made using the concept of the present invention are within the scope of protection.

[0078] Embodiment

[0079] The example of the present invention is completed in a grid environment of 10000×10000, where each grid contains its height information, the starting point of the helicopter and the target point of the helicopter each occupy 1 grid, and the obstacle area and the feasible area occupy 2 or more cells.

[0080] As Figure 1 shown, the present invention provides a method for path planning of helicopter obstacle avoidance, and its implementation method is as follows:

[0081] S1. Preprocess the elevation grid map, mark the obstacle points, and use the water cup filling method for filling. The implementation method is as follows:

[0082] Preprocess the elevation grid map, mark the area above 4800m as obstacle points, and use the water cup filling method for filling; among them,

[0083] The area above 4800m refers to the area where the current grid height in the elevation grid map is greater than 4800m, which is marked as an obstacle area and the helicopter cannot fly over it; the water cup filling method refers to reducing the search space in the helicopter obstacle avoidance path planning by filling concave areas, including horizontal filling and vertical filling, and the scanning strategy refers to traversing the map horizontally row by row to check passing points;

[0084] During horizontal filling, if the point to the left of the current point is an obstacle point (the cup wall) and there is an obstacle point (the support point) above (or below) the current point, start spreading to the right from this point and add the spread points to the spread list. All spread points maintain the support point in the same direction as the starting point. If different-side support or no support appears during spreading, it is determined as a leak and the filling of the current point is terminated; if the spread encounters an obstacle point on the right, the points in the spread list are marked as filling obstacle points;

[0085] During vertical filling, if the point above the current point is an obstacle point (the cup wall) and there is an obstacle point (the support point) to the left (or right) of the current point, start spreading down from this point and add the spread points to the spread list. All spread points maintain the support point in the same direction as the starting point. If different-side support or no support appears during spreading, it is determined as a leak and the filling of the current point is terminated; if the spread encounters an obstacle point below, the points in the spread list are marked as filling obstacle points;

[0086] Continue to traverse the map until one traversal is completed when the spread list is empty, and the filling is completed.

[0087] S2. Based on the filling result, obtain the end point that meets the constraint conditions. When the start point or the end point is a filling obstacle point, cancel the filling obstacle point by spreading from the start point or the end point. The implementation method is as follows:

[0088] S201. Based on the filling result, judge whether the end point meets the constraint conditions. If so, enter S203; otherwise, perform S202. Among them, the end point cannot be an obstacle point, that is, the height does not exceed 4800m, and at the same time, the slope of the end point is not higher than 30 degrees;

[0089] S202. Spread to find a new end point that meets the conditions and enter S203. Specifically:

[0090] Spread in eight directions around the end point and check whether the constraint conditions including slope and height are met. If not, continue to spread in eight directions based on the eight neighbor points until a new end point that meets the constraint conditions is found;

[0091] The expression for the slope is as follows:

[0092]

[0093]

[0094]

[0095] Among them, represents the slope, represents the absolute value of the height difference between the neighbor point and the center point, represents the absolute value of the horizontal distance between the neighbor point and the center point, represents the height of the neighbor point, represents the height of the center point, represents the horizontal coordinate of the neighbor point, represents the horizontal coordinate of the center point.

[0096] In this embodiment, the height is such that the height of the neighbor point is less than 4800 m, and the distance between the new end point and the start point is less than the distance between the end point and the start point.

[0097] S203. When the start point and the end point are filled obstacle points, cancel the filled obstacle points by spreading from the start point or the end point. Specifically:

[0098] When the start point or the end point is a filled obstacle point, spread in eight directions centered on the start point or the end point, and restore all the points marked as filled obstacles in the spreading path to the passable state; if an obstacle point or a passable point is encountered during the spreading process, the current point will not spread until there are no more spreading points.

[0099] S3. Based on the spreading cancellation result, use the bidirectional A* algorithm to expand synchronously, search for the obstacle avoidance path of the helicopter, and plan the obstacle avoidance path of the helicopter in the three-dimensional space by designing the actual cost and the heuristic cost in the total cost of the start point direction and the end point direction, and generate an adjacent node list. Among them, when the bidirectional search meets, enter S5. The implementation method is as follows:

[0100] S301. Based on the spreading cancellation result, initialize the bidirectional A* algorithm, and create open lists starting from the start point and starting from the end point respectively , and closed lists , ;

[0101] In this embodiment, the open list only stores the nodes to be expanded, and the closed list is used to store the expanded nodes.

[0102] S302. Initialize the total cost of the start point direction and the end point direction, and add the start point and the end point to the open lists and respectively. Among them, the expressions of the total cost of the start point and the end point are as follows:

[0103]

[0104]

[0105]

[0106]

[0107]

[0108]

[0109] Among them, represents the total cost in the starting direction, represents the total cost in the ending direction, represents the starting point, represents the ending point, represents the actual cost of the starting point, and the starting point is 0, represents the current node 's actual cost, represents the movement cost from the current node to the starting point node Since it is in the starting direction and the starting point is the first node, there is no situation of reaching the starting point from other nodes, so they are all 0, represents the heuristic cost from the starting point to the ending point, represents the actual cost of the ending point, and the ending point is 0, represents the current node 's actual cost, represents the movement cost from the current node to the starting point node Since it is in the ending direction and the ending point is the first node, there is no situation of reaching the ending point from other nodes, so they are all 0, represents the heuristic cost from the ending point to the starting point, represents the position coordinates of the starting point, represents the position coordinates of the ending point; the heuristic cost is calculated by the three-dimensional Manhattan distance, and the movement cost refers to the movement cost from point A to point B, and the actual cost is the movement cost from point A to point B plus the accumulated actual cost before point A .

[0110] S303. Judge whether the open list and are empty lists. If so, the obstacle avoidance path of the helicopter in the three-dimensional space cannot be planned, and the process ends. Otherwise, go to S304;

[0111] S304. If the open lists in both directions If both are non-empty, then select the open lists respectively for the node with the smallest f-value and the open list respectively for the node with the smallest f-value;

[0112] S305. Determine whether the node N with the smallest f-value selected from the two open lists exists in the closed list in the opposite direction. If so, it is determined that the bidirectional search meets, and proceed to S5; otherwise, proceed to S306. Here, the f-value represents the total cost of the current node.

[0113] S306. Remove the current node N from the open list and add the current node N to the closed list . At the same time, generate a list of adjacent nodes based on the direction angle between the current node and the target node , where the target node refers to the target to be reached in each direction. The target node starting from the starting point direction is the end point, and the target node starting from the end point direction is the starting point. The direction angle refers to the angle between the current node and the target node. The implementation method is as follows:

[0114] Remove the current node N from the open list and add the current node N to the closed list . Use the following formula to obtain the node with the smallest f-value in the open list :

[0115]

[0116]

[0117] where N represents the node in the open list , represents the set of f-values of all nodes in the open list , represents the node with the smallest f-value in the open list , represents taking the node with the smallest f-value in the open list , which is here, represents the node with the smallest f-value in the open list , represents taking the node with the smallest f-value in the open list , which is here;

[0118] ​Based on the addition result, in the forward running direction of the helicopter, a direction angle is generated according to the direction between the current node N and the target node. Among them, the eight surrounding grids in the horizontal direction are named in sequence as , , , , , , and :

[0119]

[0120] For the direction angle of , its horizontal expansion direction is , and the list of adjacent nodes in the three-dimensional expansion direction is distributed at nine points: the front upper left node A, the front left node B, the front lower left node C, the front upper node D, the front node E, the front lower node F, the front upper right node G, the front right node H, and the front lower right node I on the entire front surface. Among them, if the coordinates of the current node N are , the coordinates of its adjacent node list are as follows:

[0121]

[0122] Among them, represents the direction angle, , , , , , , , and respectively represent the positions of nodes A, B, C, D, E, F, G, H, and I in the adjacent node list when the coordinates of the current node N are and the horizontal expansion direction is . If the direction angle is and its horizontal expansion direction is , the x-direction and y-direction of the node in the adjacent node list are changed to . Among them, for the nodes in the adjacent node list except the node , they need to be changed based on the node , and the specific changes are as follows:

[0123] .

[0124] In this embodiment, considering that the helicopter should maintain forward movement during normal flight, where the forward direction only analyzes the horizontal direction, and the forward movement direction generates an angle based on the direction between the end point and the starting point.

[0125] In this embodiment, as Figure 2 shown, the direction map is described as follows: The current node refers to the current position of the helicopter, and the target node refers to the target node to be reached in the current direction. When starting from the starting point, the target node refers to the end point; when starting from the end point, the target node refers to the starting point. And the forward direction only refers to the horizontal direction. Therefore, the grid direction is a two-dimensional direction, with only changes in the x and y coordinates. So when the current node is N, there are 8 directions for it to choose from, and the forward direction needs to be determined according to the direction angle.

[0126] In this embodiment, as Figure 3 shown, the node search range map is described as: After determining the forward direction, when the current node is N, there are 9 points in the forward plane, namely A, B, C, D, E, F, G, H, and I, and these are the adjacent nodes in the adjacent node list.

[0127] In this embodiment, the water cup filling method is a prior art, and the process is described as follows:

[0128] 1. Initialization: Initialize the current grid point;

[0129] 2. Main loop:

[0130] a. Traverse the map grid points row by row;

[0131] b. Determine whether the map traversal is completed?

[0132] Y: The map traversal is completed. Determine whether the diffusion list is empty?

[0133] Y: The diffusion list is empty, no need to fill, end;

[0134] N: The diffusion list is not empty, still need to fill, initialize the current grid point, and start from the beginning;

[0135] N: The map traversal is not completed, go down, and take the next grid point;

[0136] c. Determine whether the current point is a passable point?

[0137] Y: It is a passable point, go down, and determine the filling type;

[0138] N: It is not a passable point, return to traverse the map;

[0139] d. Determine whether the left side of the current point is an obstacle point?

[0140] Y: It is an obstacle point, and horizontal filling is triggered;

[0141] N: It is not an obstacle point. Next, determine whether the upper side of the current point is an obstacle point;

[0142] e. Determine whether the upper side of the current point is an obstacle point?

[0143] Y: It is an obstacle point, and vertical filling is triggered;

[0144] N: It is not an obstacle point, return to traverse the map;

[0145] 3. Diffusion filling:

[0146] a. Detect points one by one along the trigger direction. The horizontal filling direction is to the right, and the vertical filling direction is downwards;

[0147] When encountering a same-side support, continue to spread and add the spread points to the spread list;

[0148] When encountering an opposite-side support or no support, it is determined as a leak opening, terminate the spread, and clear the spread list;

[0149] When encountering an obstacle point, mark the spread points in the spread list as filling obstacles;

[0150] 4. Loop termination condition: End when the entire map has been traversed and the spread list is empty.

[0151] In this embodiment, the central diffusion method is a prior art, and the process is described as follows:

[0152] 1. Initialize the list: Initialize the list of points to be checked and the list of points that have been checked;

[0153] 2. Main loop

[0154] a. Set the point to be checked as the end point;

[0155] b. Add the point to be checked to the list of points to be checked;

[0156] c. Determine whether the list of points to be checked has been traversed?

[0157] Y: Traversed, there is no point that meets the constraint conditions, and the spread ends;

[0158] N: Not traversed, proceed normally;

[0159] d. Obtain the point to be checked that has been traversed and remove it from the list of points to be checked;

[0160] e. Determine whether the point to be checked exists in the list of points that have been checked?

[0161] Y: Exists, then return to continue traversing;

[0162] N: Does not exist, then proceed normally;

[0163] f. Obtain the angles between the checkpoint to be inspected and its 8 neighboring points, and add the neighboring points with an altitude lower than 4800m to the list of checkpoints to be inspected;

[0164] g. Add the neighboring points to the list of checkpoints to be inspected, and add the checkpoint to be inspected to the list of inspected checkpoints;

[0165] h. Determine whether the angle between the checkpoint to be inspected and the neighboring point is less than 30 degrees and the altitude is less than 4800m?

[0166] Y: If the checkpoint to be inspected meets the constraint conditions, then the checkpoint to be inspected is the new end point, and the process ends;

[0167] N: If the checkpoint to be inspected does not meet the constraint conditions, then return to continue traversing the list of checkpoints to be inspected;

[0168] 3. Loop termination condition: Find a checkpoint to be inspected that meets the constraint conditions or finish traversing the list of checkpoints to be inspected.

[0169] In this embodiment, the water cup filling cancellation method is a prior art, and the process is described as follows:

[0170] 1. Initialize the lists: Initialize the list of checkpoints to be inspected and the list of inspected checkpoints

[0171] 2. Main loop

[0172] a. Determine whether the current point is a filling obstacle point?

[0173] Y: It is a filling obstacle point, and proceed normally downwards;

[0174] N: It is not a filling obstacle point, a point where diffusion does not need to be cancelled, and the process ends;

[0175] b. Let the checkpoint to be inspected be the current point;

[0176] c. Add the checkpoint to be inspected to the list of checkpoints to be inspected, and cancel the filling obstacle mark of the checkpoint to be inspected;

[0177] d. Determine whether the list of checkpoints to be inspected has been traversed?

[0178] Y: Traversal is completed, a point where diffusion does not need to be cancelled, and the process ends;

[0179] N: Traversal is not completed, and proceed normally downwards;

[0180] e. Obtain the checkpoint to be inspected during traversal and remove it from the list of checkpoints to be inspected;

[0181] f. Determine whether the checkpoint to be inspected exists in the list of inspected checkpoints?

[0182] Y: If it exists, then return to continue traversing the list of checkpoints to be inspected

[0183] N: If not present, proceed normally downwards;

[0184] g. Obtain 8 neighbor points of the point to be checked;

[0185] h. Determine whether the 8 neighbor points are filled obstacle points?

[0186] Y: If it is a neighbor point of a filled obstacle point, add this neighbor point to the list to be checked and cancel the filled obstacle mark of this neighbor point;

[0187] N: If it is not a neighbor point of a filled obstacle point, skip the process and return to continue traversing the list of points to be checked;

[0188] 3. Loop termination condition: Finish traversing the list of points to be checked.

[0189] In this embodiment, the left - hand wall - following method is a prior art, and the process is described as follows:

[0190] 1. Initialization: Initialize the node traversable list, the list of walls to be followed, and the wall - following count to 0;

[0191] 2. Main loop

[0192] a. Determine whether there are non - traversable points in the adjacent node list?

[0193] Y: If present, add the non - traversable points to the list of walls to be followed;

[0194] N: If not present, there is no need to follow the wall, and directly end;

[0195] b. Determine whether the adjacent node list has been traversed?

[0196] Y: After traversing, obtain the list of walls to be followed;

[0197] N: If not traversed, return to continue determining whether there are non - traversable points in the adjacent node list;

[0198] c. Determine whether the list of walls to be followed has been traversed?

[0199] Y: After traversing, there are no nodes that need to follow the wall in the list of walls to be followed, end;

[0200] N: If not traversed, obtain the horizontal action direction according to the difference between the x and y coordinates of the non - traversable point and the current point;

[0201] d. Right - hand wall - following method, the sequence of right - hand wall - following actions directions: (1,0) →(1, - 1) →(0, - 1) →(-1, - 1)→(-1,0) →(-1,1) →(0,1) →(1,1) →(1,0);

[0202] e. Perform wall - following according to the right - hand wall - following action directions sequence. This is a loop. Based on the horizontal action direction, select where to start traversing. Each time the wall is passed, the wall - following count is incremented by 1;

[0203] f. Determine whether the wall - following count is greater than 8?

[0204] Y. The wall - following count is greater than 8. Return to determine whether the list of walls to be followed has been traversed;

[0205] N: The wall - following count is not greater than 8. Obtain the position after wall - following, that is, perform the next action in the left - hand wall - following action directions sequence from the current position;

[0206] g. Determine whether there is a passable point at the position after wall - following?

[0207] Y: There is a passable point. Add the passable point to the node pass list;

[0208] N: Return to the wall - following loop;

[0209] 3. Loop termination condition: There are no non - passable points in the adjacent node list, or the list of walls to be followed has been traversed.

[0210] S4. Based on the generated adjacent node list, use the wall - following method to generate new adjacent nodes, and use the optimal adjacent node in the new adjacent node list as the next node, and return to S3. The implementation method is as follows:

[0211] S401. Determine the adjacent node list Check whether the nodes inside meet the conditions. If so, enter S402; otherwise, use the wall - following method to generate new adjacent node M and enter S402;

[0212] In this embodiment, the wall - following method is an obstacle - avoidance strategy that follows the edge of the obstacle, including the left - hand wall - touching method and the right - hand wall - touching method. When encountering an obstacle, the left - hand method preferentially turns left. If the upper, middle, and lower three nodes on the left are still obstacles, continue to turn left until no obstacle is encountered or the number of turns exceeds 8 times; the right - hand method is vice versa, preferentially turning right. If the three nodes on the right are still obstacles, continue to turn right until the same termination condition is met.

[0213] S402. Determine whether the adjacent node M is already in the open list in the current direction If so, enter S404; otherwise, enter S403;

[0214] In this embodiment, this S402 is to ensure that the open list always stores the minimum - cost path to reach each node, avoiding missing a better solution due to a high - cost path recorded earlier.

[0215] S403. Determine whether the adjacent node M is already in the closed list in the current direction. If so, proceed to S405; otherwise, proceed to S406. In this embodiment, S403 is to reactivate nodes when discovering a better path to ensure the possibility of a globally optimal solution.

[0216] In this embodiment, S403 is to reactivate nodes when discovering a better path to ensure the possibility of a globally optimal solution.

[0217] S404. Determine Whether it is less than If so, the actual cost from the current node N to the adjacent node M is the minimum. Update the parent node of the adjacent node M to the node N and recalculate the total cost so that the actual cost to reach the adjacent node M is the minimum, and return to S303. Otherwise, do not update and return to S303, where represents the actual cost from the current node N to the adjacent node M, represents the open list records the actual cost of reaching the adjacent node M from nodes other than the current node N;

[0218] S405. Determine Whether it is less than If so, remove the adjacent node M from the closed list and update its parent node, actual cost value and total cost value, return to S303. Otherwise, do not update and return to S303;

[0219] S406. Set the parent node of the adjacent node M to the node N, total cost , add the adjacent node M to the open list and return to S303.

[0220] S5. Starting from the meeting node, trace back along the parent nodes in both directions until the start point and the end point, and merge the trace-back paths in both directions to obtain the complete path from the start point to the end point, completing the planning of the helicopter obstacle avoidance path.

[0221] In this embodiment, the left-hand wall-following method is a prior art, and the process is described as follows:

[0222] 1. Initialization: Initialize the node pass list, the list of walls to be followed, and the wall-following count to 0;

[0223] 2. Main loop

[0224] a. Determine whether there is a non-passable point in the adjacent node list?

[0225] Y: If there is, add the non-passable point to the list of walls to be followed;

[0226] N: If it doesn't exist, there is no need to bypass the wall, and it ends directly;

[0227] b. Determine whether the adjacent node list has been traversed?

[0228] Y: If traversed, obtain the list of walls to be bypassed;

[0229] N: If not traversed, return to continue to determine whether there is a non-passable point in the adjacent node list;

[0230] c. Determine whether the list of walls to be bypassed has been traversed?

[0231] Y: If traversed, there is no node that needs to bypass the wall in the list of walls to be bypassed, and it ends;

[0232] N: If not traversed, obtain the horizontal movement direction by calculating the coordinate differences between the non-passable point and the current point x and y

[0233] d. Left wall bypass method, the sequence of left wall bypass actions directions: (1,0) →(1,1) →(0,1) →(-1,1) →(-1,0) →(-1,-1) →(0,-1) →(1,-1) →(1,0);

[0234] e. Bypass the wall according to the left wall bypass action directions sequence. This is a loop. Select where to start traversing according to the horizontal movement direction. Each time a wall is bypassed, the bypass count is incremented by 1;

[0235] f. Determine whether the bypass count is greater than 8?

[0236] Y: If the bypass count is greater than 8, return to determine whether the list of walls to be bypassed has been traversed;

[0237] N: If the bypass count is not greater than 8, obtain the position after bypassing the wall, that is, perform the next action in the left wall bypass action directions sequence from the current position;

[0238] g. Determine whether there is a passable point at the position after bypassing the wall?

[0239] Y: If there is a passable point, add the passable point to the node pass list;

[0240] N: Return to the wall bypass loop;

[0241] 3. Loop termination condition: There is no non-passable point in the adjacent node list, or the list of walls to be bypassed has been traversed.

[0242] In this embodiment, the right hand wall bypass method is an existing technology, and the process is described as follows:

[0243] Initialization: Initialize the node pass list, the list of walls to be bypassed, and the bypass count to 0;

[0244] Main loop

[0245] a. Determine whether there is an impassable point in the adjacent node list?

[0246] Y: If there is, add the impassable point to the list of points to bypass the wall;

[0247] N: If not, there is no need to bypass the wall, and directly end;

[0248] b. Determine whether the adjacent node list has been traversed?

[0249] Y: If traversed, obtain the list of points to bypass the wall;

[0250] N: If not traversed, return to continue to determine whether there is an impassable point in the adjacent node list;

[0251] c. Determine whether the list of points to bypass the wall has been traversed?

[0252] Y: If traversed, and there is no node to bypass the wall in the list of points to bypass the wall, end;

[0253] N: If not traversed, obtain the horizontal movement direction according to the difference between the x and y coordinates of the impassable point and the current point;

[0254] d. Right wall bypass method, the sequence of right wall bypass actions: (1,0) →(1,-1) →(0,-1) →(-1,-1)→(-1,0) →(-1,1) →(0,1) →(1,1) →(1,0);

[0255] e. Bypass the wall according to the sequence of right wall bypass actions. This is a loop. Select where to start traversing according to the horizontal movement direction, and increment the wall bypass count by 1 each time a wall is bypassed;

[0256] f. Determine whether the wall bypass count is greater than 8?

[0257] Y: If the wall bypass count is greater than 8, return to determine whether the list of points to bypass the wall has been traversed;

[0258] N: If the wall bypass count is not greater than 8, obtain the position after bypassing the wall, that is, perform the next action in the left wall bypass action sequence from the current position;

[0259] g. Determine whether there is a passable point at the position after bypassing the wall?

[0260] Y: If there is a passable point, add the passable point to the node pass list;

[0261] N: Return to the wall bypass loop;

[0262] Loop termination condition: There are no non-passable points in the adjacent node list, or the list of walls to be bypassed has been traversed completely.

[0263] In summary, the embodiments of the present invention provide an improved helicopter efficient obstacle avoidance path planning method based on the A* algorithm. Through the water cup filling method for preprocessing, the search space is reduced; the bidirectional A* algorithm is adopted for synchronous expansion to accelerate the search efficiency; the actual cost and heuristic cost in the total cost are reasonably designed to improve the efficiency of path planning in three-dimensional space; unnecessary node actions are reduced to improve the search efficiency; the wall bypass method is used to avoid falling into an infinite loop, effectively improving the practicability in complex environments.

[0264] The above is only the specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art within the technical scope disclosed by the present invention can easily think of changes or substitutions, which should all be covered within the protection scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the protection scope of the claims.

Claims

1. A helicopter obstacle avoidance path planning method, characterized in that: The following steps are involved: S1. Preprocess the elevation grid map, mark the obstacle points, and fill them using the cup filling method; S2. Based on the filling result, obtain the end point that meets the constraint condition, and when the starting point or the end point is a filling obstacle point, diffuse and cancel the filling obstacle point from the starting point or the end point; S3, based on the diffusion cancellation result, the bidirectional A* algorithm is used for synchronous expansion to search for the helicopter obstacle avoidance path, and the actual cost and the heuristic cost in the total cost of the design starting direction and the end point direction are used to plan the helicopter obstacle avoidance path in the three-dimensional space, and a list of adjacent nodes is generated. When the bidirectional search encounters, enter S5; S4. Based on the generated adjacent node list, a new adjacent node is generated using the wall-circling method, and the optimal adjacent node in the new adjacent node list is used as the next node, and the process returns to S3. S5. Starting from the encounter node, backtrack along the parent nodes in two directions until the starting point and the end point, and merge the backtracking paths in the two directions to obtain the complete path from the starting point to the end point, completing the planning of the helicopter obstacle avoidance path.

2. The helicopter obstacle avoidance path planning method according to claim 1, characterized in that: The S1 is specifically: The elevation grid map was preprocessed, and the area above 4800m was marked as an obstacle point, which was filled using the cup filling method. The area above 4800m refers to the area where the current grid height in the elevation grid map is greater than 4800m, which is marked as an obstacle area and the helicopter cannot fly over it; the cup filling method refers to reducing the search space in the helicopter obstacle avoidance path planning by filling the concave area, including horizontal filling and vertical filling, and the scanning strategy refers to traversing the map horizontally row by row to check the pass points; When filling horizontally, if there is an obstacle point on the left side of the current point, or an obstacle point exists above or below the current point, the point is used as the starting point to diffuse to the right, and the diffused point is added to the diffusion list. All diffused points maintain the support points in the same direction as the starting point. If there is support on the opposite side or no support during diffusion, it is determined to be a leak and the filling of the current point is terminated; if the diffusion encounters an obstacle point on the right side, the point in the diffusion list is marked as a filling obstacle point; When filling vertically, if there is an obstacle point above the current point, or an obstacle point on the left or right side of the current point, diffuse downward from that point as the starting point, and add the diffused point to the diffusion list. All diffused points maintain the support points in the same direction as the starting point. If there is support on the opposite side or no support during diffusion, it is determined to be a leak and the filling of the current point is terminated; if the diffusion encounters an obstacle point on the lower side, the point in the diffusion list is marked as a filling obstacle point; Continue to traverse the map until the diffusion list is empty and has been traversed once, completing the filling.

3. The helicopter obstacle avoidance path planning method according to claim 1, characterized in that: The S2 is specifically: S201, based on the filling result, determine whether the end point meets the constraint conditions, if so, proceed to S203, otherwise, proceed to S202, wherein the end point cannot be an obstacle point, that is, the height does not exceed 4800m, and the slope of the end point is not higher than 30 degrees; S202, diffuse and search for a new end point that meets the conditions, and proceed to S203; S203: When the starting point or the end point is a filled obstacle point, the filled obstacle point is cancelled by spreading from the starting point or the end point.

4. The helicopter obstacle avoidance path planning method according to claim 3, characterized in that: The diffusion search for a new endpoint that meets the conditions is specifically: Diffusion is performed from the end point to the eight surrounding directions to check whether the constraints including slope and height are met. If not, the eight neighboring points are continued to diffuse in eight directions until a new end point that meets the constraints is found. The expression for slope is as follows: ； ； ； in, Indicates the slope, Represents the absolute value of the height difference between the neighbor point and the center point, Represents the absolute value of the horizontal distance between the neighbor point and the center point, represents the height of the neighbor point, Indicates the height of the center point, represents the horizontal coordinates of neighbor points, Indicates the horizontal coordinate of the center point.

5. The helicopter obstacle avoidance path planning method according to claim 3, characterized in that: The step of spreading and canceling the filling obstacle points from the starting point or the end point is as follows: When the starting point or end point is a filled obstacle point, the path diffuses in eight directions with the starting point or end point as the center, and restores all points marked as filled obstacles in the diffusion path to a passable state; if an obstacle point or a passable point is encountered during the diffusion process, the current point will not diffuse until there are no diffusion points.

6. The helicopter obstacle avoidance path planning method according to claim 3, characterized in that: The S3 is specifically: S301, based on the diffusion cancellation result, initialize the bidirectional A* algorithm and create an open list starting from the starting point and the end point respectively , and close list , ; S302: Initialize the total cost of the starting point direction and the end point direction, and add the starting point and the end point to the open list respectively. and In which, the expressions of the total cost in the starting direction and the ending direction are as follows: ； ； ； ； ； ； in, represents the total cost in the direction of the starting point, represents the total cost in the direction of the end point, Indicates the starting point, Indicates the end point, Indicates the actual cost of the starting point, the starting point is 0, Indicates the current node The actual cost, Indicates that from the current node Arrival at the starting point The moving cost is 0 because it is the starting point direction and the starting point is the first node. There is no situation of reaching the starting point from other nodes. represents the heuristic cost from the starting point to the end point, Indicates the actual cost of the end point, the end point is 0, Indicates the current node The actual cost, Indicates that from the current node Arrival at the starting point The cost of movement, represents the heuristic cost of reaching the starting point from the end point, Indicates the position coordinates of the starting point, Indicates the location coordinates of the end point; S303: Determine the open list and Is there an empty list? If so, the obstacle avoidance path of the helicopter in the three-dimensional space cannot be planned, and the process ends. Otherwise, enter S304; S304, if the open lists in both directions If both are not empty, select the open list respectively. middle The node with the smallest value and open list middle The node with the smallest value ; S305: Determine whether two open lists are Whether the node N with the smallest f value selected from the above is in the closed list of the other party If yes, it is determined that the two-way search meets and the process goes to S5, otherwise, it goes to S306, where the f value represents the total cost of the current node; S306: Remove the current node N from the open list Remove it and add the current node N to the closed list At the same time, a list of adjacent nodes is generated according to the direction angle between the current node and the target node. , where the target node refers to the target to be reached in two directions respectively. The target node starting from the starting direction is the end point, and the target node starting from the end point direction is the starting point. The direction angle refers to the angle between the current node and the target node.

7. The helicopter obstacle avoidance path planning method according to claim 6, characterized in that: The adjacent node list in S306 The generation process is as follows: Remove the current node N from the open list Remove it and add the current node N to the closed list In the example, we can use the following formula to get the open list: The node with the smallest f value: ； ； Where N represents an open list The nodes in Indicates an open list The set of f values ​​of all nodes in , Indicates an open list The node with the smallest f value, Indicates taking an open list The node with the smallest f value in , Indicates an open list The node with the smallest f value, Indicates taking an open list The node with the smallest f value in ; Based on the added results, in the direction of the helicopter's forward movement, the direction angle is generated according to the direction of the current node N and the target node. The eight surrounding grids in the horizontal direction are named , , , , , , and : ； For the direction angle When , its horizontal expansion direction is , the neighbor node list in the three-dimensional expansion direction Distributed on the front upper left node A, front left node B, front left lower left node C, front upper node D, front node E, front lower node F, front upper right node G, front right node H and front lower right node I of the entire front face. If the coordinate of the current node N is , its adjacent node list The coordinates are as follows: ； in, Indicates the direction angle, , , , , , , , and Respectively represent the coordinates of the current node N , the horizontal expansion direction is When , the adjacent node list The positions of the midpoints A, B, C, D, E, F, G, H, and I, if the direction angle is When , its horizontal expansion direction is When Midpoint The x and y directions are changed to , where the adjacent node list Remove Node Nodes outside the node must be The changes are as follows: 。 8. The helicopter obstacle avoidance path planning method according to claim 6, characterized in that: The S4 is specifically: S401, determine the adjacent node list Whether the node inside meets the conditions, if so, enter S402, otherwise, use the wall-circling method to generate a new adjacent node M and enter S402; S402: Determine whether the adjacent node M is already in the open list in the current direction If yes, go to S404, otherwise go to S403; S403: Determine whether the adjacent node M is already in the closed list in the current direction If yes, go to S405, otherwise go to S406; S404, judgment Is it less than If so, the actual cost from the current node N to the adjacent node M is the smallest, and the parent node of the adjacent node M is updated to node N, and the total cost is recalculated. , and returns to S303, otherwise, no update is performed and returns to S303, wherein, It represents the actual cost of the current node N to reach the adjacent node M. Indicates an open list The actual cost of reaching the adjacent node M from nodes other than the current node N is recorded in; S405, judgment Is it less than , if so, then the adjacent node M is removed from the closed list Remove it and update its parent node and actual cost Value and total cost value, returns to S303, otherwise, does not update and returns to S303; S406, set the parent node of the adjacent node M to node N, the total cost , add the adjacent node M to the open list and returns to S303.

9. The helicopter obstacle avoidance path planning method according to claim 8, characterized in that: The total consideration The expression is as follows: ； in, is the heuristic cost, which represents the three-dimensional Manhattan distance between the adjacent node M and the target node G.

Images