Unmanned aerial vehicle route planning method and system based on improved three-dimensional A* algorithm

Abstract

The invention provides an unmanned aerial vehicle route planning method and system based on an improved three-dimensional A * algorithm, and the method comprises the following steps: S1, loading a three-dimensional grid map, and building an OPEN list and a CLOSED list; s2, a starting point heuristic value is calculated and added into an OPEN list; s3, the node with the minimum f value is taken out from the OPEN list to serve as the current node; s4, judging whether the current node is a target point or not, if so, backtracking the path, otherwise, entering step S5; s5, adding the current node into the CLOSED list; s6, candidate neighborhood nodes are generated based on 5 * 5 * 5 neighborhoods and turning angle limitation; s7, traversing the candidate neighborhood nodes, and updating the OPEN list; and S8, repeating the steps S3-S7 until the path is found or the OPEN list is empty. The method has the beneficial effects that the generated air route meets the maneuvering characteristic condition of the unmanned aerial vehicle, the safety is remarkably improved, the searching efficiency is effectively improved, and the practicability is higher.

Description

Technical Field

[0001] This invention belongs to the field of UAV route planning technology, and in particular relates to a UAV route planning method and system based on an improved three-dimensional A* algorithm. Background Technology

[0002] With the rise of the low-altitude economy, the demand for UAVs in complex low-altitude airspaces such as mountainous areas and cities is increasing, posing significant challenges to their route planning. The traditional A* algorithm, a classic graph search method, is an efficient algorithm for finding optimal paths, but its direct application in three-dimensional space has obvious shortcomings: it does not consider the maneuverability constraints of UAVs (such as turning angle limitations), resulting in potentially overly convoluted routes that are difficult for the UAV's flight control system to track; in complex environments, navigation signals such as satellites are easily blocked, leading to increased positioning errors; and traditional route planning algorithms treat unoccupied grids as completely safe free space, ignoring the collision risks caused by inaccurate positioning near obstacles.

[0003] In existing technologies, the navigation and positioning system, planning and decision-making system, and flight control system of UAVs often operate independently without communication. Especially for UAVs flying beyond visual line of sight, if the positioning performance of the navigation system and the maneuverability of the flight control system are quantitatively considered during motion planning, safety risks caused by inaccurate positioning or difficulty in tracking the planned route can be avoided. Although some existing route planning algorithms have been studied to improve computational efficiency by introducing jump point search or sparse A* search, or to use sampling-based methods (such as RRT), evolutionary algorithms (such as GA, PSO), and artificial intelligence-based methods for UAV route planning, these methods either do not fully consider motion constraints, or cannot guarantee path optimality due to randomness, and generally lack quantitative consideration of the positioning error of the navigation system, a key safety factor. Summary of the Invention

[0004] In view of this, the present invention aims to overcome the shortcomings of the above-mentioned problems in the prior art and proposes a UAV route planning method and system based on an improved three-dimensional A* algorithm.

[0005] To achieve the above objectives, the technical solution of the present invention is implemented as follows: The first aspect of this invention provides a UAV route planning method based on an improved 3D A* algorithm, comprising the following steps: S1. Load the 3D raster map and create the OPEN and CLOSED lists; S2. Calculate the starting heuristic value and add it to the OPEN list; S3. Take the node with the smallest f value from the OPEN list and use it as the current node; S4. Determine if the current node is the target point. If it is, backtrack the path. Otherwise, proceed to step S5. S5. Add the current node to the CLOSED list; S6. Generate candidate neighborhood nodes based on the 5×5×5 neighborhood and turning angle constraints; S7. Traverse the candidate neighboring nodes and update the OPEN list; S8. Repeat steps S3-S7 until a path is found or the OPEN list is empty.

[0006] Furthermore, in step S1, before loading the three-dimensional grid map, a three-dimensional grid map is constructed based on the known three-dimensional environment information, and each grid in the three-dimensional grid map is mapped as a node, wherein the grid corresponding to the obstacle is identified as an occupied node, and the grid corresponding to the free space is identified as an unoccupied node.

[0007] Furthermore, the process of generating candidate neighbor nodes in step S6 is as follows: Iterate through all neighboring nodes within a 5×5×5 radius of the current node; Filter neighboring nodes that meet the following conditions as candidate neighboring nodes, including nodes not in the CLOSED list, nodes that are not occupied, and nodes that meet the drone turning angle limit.

[0008] Furthermore, in step S6, the process of determining whether the candidate neighboring nodes meet the UAV turning angle limit is as follows: For each neighboring node of the current node, calculate the horizontal turning angle θ and vertical turning angle φ of that neighboring node relative to its corresponding parent node, as shown in the following expressions. , , The current node number is denoted as n Current node n The coordinates are as The corresponding parent node coordinates are The coordinates of the neighboring nodes to be determined for expansion are marked as follows: , If the horizontal turning angle θ of a neighboring node exceeds the preset horizontal turning angle constraint, or the vertical turning angle φ exceeds the preset vertical turning angle constraint, then the neighboring node is removed from the candidate neighboring nodes. If both the horizontal turning angle θ and the vertical turning angle φ of a neighboring node meet the corresponding turning angle constraints, then the neighboring node is retained in the candidate neighboring nodes.

[0009] Furthermore, in step S2, the calculation of the starting heuristic value includes: The actual path cost at the starting point g = 0; The heuristic function h=getHeu() uses a weighted Euclidean distance that takes into account the positioning error of the UAV navigation system and the number of occupied grid cells in the local environment. The total cost is f = g + h.

[0010] Furthermore, the process of updating the OPEN list in step S7 is as follows: Iterate through each neighbor node in the set of candidate neighbor nodes of the current node; determine whether the neighbor node is in the OPEN list; If the neighboring node is in the OPEN list, then determine whether the actual cost from the starting point to the neighboring node calculated with the current node as the parent node is less than the actual cost from the starting point to the neighboring node itself that the neighboring node has already stored. If it is less, then update the parent node of the neighboring node, the actual cost from the starting point to the neighboring node, and recalculate the total cost f, where f = g + h, g is the actual cost, and h is the heuristic function. Then update the OPEN list. If the condition is not met, then do not do anything. If the neighboring node is not in the OPEN list, calculate the actual cost from the starting point to the neighboring node, the heuristic function, and the total cost f, and add the neighboring node to the OPEN list.

[0011] Furthermore, the expression for the heuristic function h is: , in, , , These are weighting coefficients. Based on the current node n The heuristic term for the Euclidean distance from the destination of the route; Based on the current node n Heuristics on the number of grid cells occupied within a local environment; This is a heuristic term for the positioning error of the navigation system, and This refers to the actual radius of the drone. It is the at least 95% confidence interval of the positioning error of a navigation system that follows a Gaussian distribution. For the current node n With the most recently occupied grid l distance, This represents the result of expanding the equivalent radius of the UAV based on the positioning error of the navigation system.

[0012] A second aspect of this invention provides an unmanned aerial vehicle (UAV) route planning system based on an improved 3D A* algorithm, comprising: The map loading and data structure initialization module is used to load 3D raster maps and create OPEN and CLOSED lists. The starting point initialization and heuristic value calculation module is used to calculate the starting point heuristic value and add it to the OPEN list; The node selection module is used to select the node with the smallest f value from the OPEN list as the current node; The path determination module is used to determine whether the current node is the target point. If it is, the path is backtracked; otherwise, the node exploration module is entered. The node exploration module is used to add the current node to the CLOSED list; The node expansion module is used to generate candidate neighborhood nodes based on 5×5×5 neighborhood and turning angle constraints. The list update module is used to traverse candidate nodes and update the OPEN list; The iteration control module is used to control the repeated execution of node selection, path determination, node exploration, node expansion, and list update steps.

[0013] Compared with the prior art, the present invention has the following advantages: The UAV route planning method based on the improved 3D A* algorithm described in this invention generates routes that conform to the maneuverability conditions of the UAV. By introducing steering angle constraints and expanding the search neighborhood, the generated paths avoid sharp turns, better reflect the actual motion capabilities of the UAV, and facilitate subsequent short-distance real-time motion optimization and flight control system tracking. Safety is significantly improved by quantifying the positioning uncertainty of the navigation system into a safety cost through an improved heuristic function, enabling the algorithm to actively avoid high-risk areas near obstacles and providing sufficient safety redundancy for the UAV even when navigation performance degrades. Search efficiency is optimized by eliminating nodes that do not meet motion constraints and guiding the search towards safe areas, reducing the number of nodes that need to be expanded, thereby improving search efficiency. It is highly practical, as this method is based on offline planning using static maps, ensuring reliable computation and allowing direct integration into the UAV's flight management system, providing reliable route planning for autonomous flight in complex environments. Attached Figure Description

[0014] The accompanying drawings, which form part of this invention, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an undue limitation of the invention. In the drawings: Figure 1 This is a schematic diagram of the UAV route planning method based on the improved 3D A* algorithm described in Embodiment 1 of the present invention; Figure 2 This is a flowchart of the UAV route planning method based on the improved 3D A* algorithm described in Embodiment 1 of the present invention. Figure 3 This is a schematic diagram of the 5x5x5 neighborhood range described in Embodiment 1 of the present invention. Detailed Implementation

[0015] It should be noted that, unless otherwise specified, the embodiments and features described in the present invention can be combined with each other.

[0016] In the description of this invention, it should be understood that these descriptions are merely exemplary and not intended to limit the scope of this application. In the following detailed description, numerous specific details are set forth to provide a thorough understanding of the embodiments of this application for ease of explanation. However, it will be apparent that one or more embodiments may be practiced without these specific details. Furthermore, descriptions of well-known structures and techniques are omitted in the following description to avoid unnecessarily obscuring the concepts of this application.

[0017] The terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the scope of this application. The terms “comprising,” “including,” etc., as used herein indicate the presence of the stated features, steps, operations, and / or components, but do not exclude the presence or addition of one or more other features, steps, operations, or components.

[0018] All terms used herein (including technical and scientific terms) have the meanings commonly understood by those skilled in the art, unless otherwise defined. It should be noted that the terms used herein are to be interpreted in a manner consistent with the context of this specification, and not in an idealized or overly rigid way.

[0019] The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0020] Example 1 like Figure 1 and Figure 2 As shown, the UAV path planning method based on the improved 3D A* algorithm includes the following steps: S1. Load the 3D raster map and create the OPEN and CLOSED lists; S2. Calculate the starting heuristic value and add it to the OPEN list; S3. Take the node with the smallest f value from the OPEN list and use it as the current node; S4. Determine if the current node is the target point. If it is, backtrack the path (i.e., backtrack the parent node from the target point to the starting point and arrange them in reverse order to obtain the final path). Otherwise, proceed to step S5. S5. Add the current node to the CLOSED list; S6. Generate candidate neighborhood nodes based on the 5×5×5 neighborhood and turning angle constraints; S7. Traverse the candidate neighboring nodes and update the OPEN list; S8. Repeat steps S3-S7 until a path is found or the OPEN list is empty.

[0021] In step S1, before loading the 3D grid map, a 3D grid map is constructed based on the known 3D environment information. Each grid in the 3D grid map is mapped to a node. The grid corresponding to obstacles is identified as an occupied node, such as buildings, terrain obstacles, etc., and the grid corresponding to free space is identified as an unoccupied node, that is, the area where the UAV can fly.

[0022] The process of generating candidate neighbor nodes in step S6 is as follows: Iterate through all neighboring nodes within a 5×5×5 radius of the current node; Filter neighboring nodes that meet the following conditions as candidate neighboring nodes, including nodes not in the CLOSED list, nodes that are not occupied, and nodes that meet the drone turning angle limit.

[0023] In step S6, the process of determining whether the candidate neighboring nodes meet the UAV turning angle limit is as follows: For each neighboring node of the current node, calculate the horizontal turning angle θ and vertical turning angle φ of that neighboring node relative to its corresponding parent node, as shown in the following expressions. , , The current node number that needs to be expanded is denoted as . n Current node n Coordinates are The corresponding parent node coordinates are The coordinates of the neighboring nodes to be determined for expansion (i.e., whether the neighboring nodes are retained in the set of candidate neighbors) are denoted as... , If the horizontal turning angle θ of a neighboring node exceeds the preset horizontal turning angle constraint (e.g., ±60°), or the vertical turning angle φ exceeds the preset vertical turning angle constraint (e.g., ±45°), then the neighboring node will be removed from the candidate neighboring nodes and will not be considered. If both the horizontal turning angle θ and the vertical turning angle φ of a neighboring node meet the corresponding turning angle constraints, then the neighboring node will be retained in the candidate neighboring nodes.

[0024] In step S2, the calculation of the starting heuristic value includes: The actual path cost at the starting point g = 0; The heuristic function h=getHeu(), i.e., the heuristic cost estimation, adopts a weighted Euclidean distance that takes into account the positioning error of the UAV navigation system and the number of occupied grid cells in the local environment. The total cost is f = g + h.

[0025] The process of updating the OPEN list in step S7 is as follows: Iterate through each neighbor node in the set of candidate neighbor nodes of the current node; determine whether the neighbor node is in the OPEN list; If the neighboring node is in the OPEN list, then it is determined whether the actual cost from the starting point to the neighboring node calculated with the current node as the parent node is less than the actual cost from the starting point to the neighboring node itself that the neighboring node has already stored. If it is less, then the parent node of the neighboring node, the actual cost from the starting point to the neighboring node, and the total cost f are recalculated, where f = g + h, g is the actual cost, and h is the heuristic function. Then the OPEN list is updated. If the condition is not met, no action is taken. In other words, if the neighboring node is already in the OPEN list, it is checked whether the current path is better. If it is, then the parent node and cost information of the neighboring node are updated. If the neighboring node is not in the OPEN list, calculate the actual cost from the starting point to the neighboring node, the heuristic function, and the total cost f, and add the neighboring node to the OPEN list.

[0026] The expression for the heuristic function h is: , in, , , These are weighting coefficients. Based on the current node n The heuristic term for the Euclidean distance to the destination of the route is responsible for guiding the route to converge toward the target destination; Based on the current node n The heuristic of the number of occupied grid cells within a certain local environment prompts the route planning algorithm to select areas with low obstacle density, thereby reducing both the risk of collision with obstacles and the probability of satellite signal obstruction. This is a heuristic term for the positioning error of the navigation system, and This refers to the actual radius of the drone. It is the at least 95% confidence interval of the positioning error of a navigation system that follows a Gaussian distribution. For the current node n With the most recently occupied grid l distance, The result of expanding the equivalent radius of the UAV based on the positioning error of the navigation system, and the ratio of its distance to the nearest occupied grid, reflects the security risk caused by the navigation positioning error; the smaller the heuristic term of the navigation system positioning error, the smaller the security risk caused by the navigation system positioning error.

[0027] Below is an example of a drone performing a task navigating complex urban terrain; the 3D scene information is randomly generated by a computer. For example... Figure 2 As shown, the process is as follows: 1. Initialization: First, load the 3D scene information and generate a 3D occupied grid map. Set the coordinates of the task start and destination. Initialize the OPEN list of the A* algorithm priority sequence and the CLOSED list to save nodes that will not be expanded. Store the coordinates of all occupied nodes in the CLOSED list. Set the UAV maneuverability parameters (calculate the horizontal and vertical heading angle constraints for single-step flight based on the size of a single grid map), the effective range of the UAV's onboard environmental perception system, and the UAV navigation and positioning system model and parameters. 2. Add the starting point coordinates to the OPEN list and calculate the cost function. The path cost g from the starting point to the current node n is 0, based on the heuristic term between the current node and the route endpoint. The Euclidean distance between the current node and the destination coordinates, and the obstacle density heuristic term. Calculate the number of occupied grid cells within the UAV's onboard perception system, and the heuristic term for the navigation system's positioning error. The example uses a navigation system model combining satellite navigation and inertial navigation. First, based on satellite ephemeris, the time of the UAV's flight mission, the UAV's coordinates, and obstacle obstruction, the position of visible satellites above the UAV is calculated. The pseudorange data of the visible satellites is then simulated. Combined with the simulated inertial navigation data, the Kalman filter algorithm is used to simulate the positioning result generated by the combined navigation system and the estimated standard deviation of the positioning error. 3. Repeat this step until the destination coordinates appear in the OPEN list. Extract the node with the smallest cost *f* from the OPEN list, designate it as the current node, and add it to the CLOSED list. Iterate through all neighboring nodes in the OPEN list. If the actual cost *g* of traveling to a neighboring node with the current node as its parent is less than the total cost from the starting point to that neighboring node, update the value of *g* and update the current node to become its parent node. Figure 3 The extended range shown is calculated one by one to determine whether these pre-extended neighboring nodes are unoccupied nodes and whether they meet the UAV maneuverability constraints. The cost value of the neighboring nodes that meet the conditions is calculated and added to the OPEN list. 4. Starting from the parent node of the destination coordinates (i.e., the target point), backtrack to the starting point to obtain the coordinates of all waypoints along the planned route. Because the navigation system's positioning accuracy in this example is significantly affected by terrain obstruction, resulting in large variations in positioning errors at different locations, it's impossible to address the navigation positioning error issue in route planning using hard constraints (i.e., a fixed equivalent expansion radius). Using a cost function as a soft constraint to handle positioning errors would lead to safety risks in the generated route if the weights are not set appropriately. This example uses multiple cost function weight ratios, performs route planning multiple times, and compares the generated results with the combined route length and the overall route safety factor assessment to select the most suitable route for execution.

[0028] Example 2 A UAV route planning system based on an improved 3D A* algorithm includes: The map loading and data structure initialization module is used to load 3D raster maps and create OPEN and CLOSED lists. The starting point initialization and heuristic value calculation module is used to calculate the starting point heuristic value and add it to the OPEN list; The node selection module is used to select the node with the smallest f value from the OPEN list as the current node; The path determination module is used to determine whether the current node is the target point. If it is, the path is backtracked; otherwise, the node exploration module is entered. The node exploration module is used to add the current node to the CLOSED list; The node expansion module is used to generate candidate neighborhood nodes based on 5×5×5 neighborhood and turning angle constraints. The list update module is used to traverse candidate nodes and update the OPEN list; The iteration control module is used to control the repeated execution of node selection, path determination, node exploration, node expansion, and list update steps.

[0029] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A UAV route planning method based on an improved 3D A* algorithm, characterized in that, Includes the following steps: S1. Load the 3D raster map and create the OPEN and CLOSED lists; S2. Calculate the starting heuristic value and add it to the OPEN list; S3. Take the node with the smallest f value from the OPEN list and use it as the current node; S4. Determine if the current node is the target point. If it is, backtrack the path. Otherwise, proceed to step S5. S5. Add the current node to the CLOSED list; S6. Generate candidate neighborhood nodes based on the 5×5×5 neighborhood and turning angle constraints; S7. Traverse the candidate neighboring nodes and update the OPEN list; S8. Repeat steps S3-S7 until a path is found or the OPEN list is empty.

2. The UAV route planning method based on the improved 3D A* algorithm according to claim 1, characterized in that: In step S1, before loading the three-dimensional grid map, a three-dimensional grid map is constructed based on the known three-dimensional environment information. Each grid in the three-dimensional grid map is mapped to a node, where the grid corresponding to the obstacle is identified as an occupied node, and the grid corresponding to the free space is identified as an unoccupied node.

3. The UAV route planning method based on the improved 3D A* algorithm according to claim 2, characterized in that, The process of generating candidate neighbor nodes in step S6 is as follows: Iterate through all neighboring nodes within a 5×5×5 radius of the current node; Filter neighboring nodes that meet the following conditions as candidate neighboring nodes, including nodes not in the CLOSED list, nodes that are not occupied, and nodes that meet the drone turning angle limit.

4. The UAV route planning method based on the improved 3D A* algorithm according to claim 3, characterized in that, In step S6, the process of determining whether the candidate neighboring nodes meet the UAV turning angle limit is as follows: For each neighboring node of the current node, calculate the horizontal turning angle θ and vertical turning angle φ of that neighboring node relative to its corresponding parent node, as shown in the following expressions. Here, the current node's index is denoted as n, the coordinates of the current node n are [x(n), y(n), z(n)], the corresponding parent node's coordinates are [x(n-1), y(n-1), z(n-1)], and the coordinates of the neighboring nodes to be determined for expansion are [x(m), y(m), z(m)]. If the horizontal turning angle θ of a neighboring node exceeds the preset horizontal turning angle constraint, or the vertical turning angle φ exceeds the preset vertical turning angle constraint, then the neighboring node is removed from the candidate neighboring nodes. If both the horizontal turning angle θ and the vertical turning angle φ of a neighboring node meet the corresponding turning angle constraints, then the neighboring node is retained in the candidate neighboring nodes.

5. The UAV route planning method based on the improved 3D A* algorithm according to claim 1, characterized in that: The process of updating the OPEN list in step S7 is as follows: Iterate through each neighbor node in the set of candidate neighbor nodes of the current node; determine whether the neighbor node is in the OPEN list; If the neighboring node is in the OPEN list, then determine whether the actual cost from the starting point to the neighboring node calculated with the current node as the parent node is less than the actual cost from the starting point to the neighboring node itself that the neighboring node has already stored. If it is less, then update the parent node of the neighboring node, the actual cost from the starting point to the neighboring node, and recalculate the total cost f, where f = g + h, g is the actual cost, and h is the heuristic function. Then update the OPEN list. If the condition is not met, then do not do anything. If the neighboring node is not in the OPEN list, calculate the actual cost from the starting point to the neighboring node, the heuristic function, and the total cost f, and add the neighboring node to the OPEN list.

6. The UAV route planning method based on the improved 3D A* algorithm according to claim 5, characterized in that, The expression for the heuristic function h is: h(n) = w1 · h distance (n) + w2 · h obstacle (n) + w3 · h avError (n), Where w1, w2, and w3 are weighting coefficients. h distance (n) is a heuristic term based on the Euclidean distance between the current node n and the destination of the route; H obstacle (n) is a heuristic term based on the number of grid cells occupied by the current node n in the local environment. This is a heuristic term for the positioning error of the navigation system, and Radius UAV This refers to the actual radius of the drone. It is a 95% confidence interval for the positioning error of a navigation system that follows a Gaussian distribution, where Distance(n, l) is the distance between the current node n and the nearest occupied grid cell l. This represents the result of expanding the equivalent radius of the UAV based on the positioning error of the navigation system.

7. A UAV route planning system based on an improved 3D A* algorithm, used to implement the UAV route planning method based on an improved 3D A* algorithm as described in any one of claims 1-6, characterized in that, include: The map loading and data structure initialization module is used to load 3D raster maps and create OPEN and CLOSED lists. The starting point initialization and heuristic value calculation module is used to calculate the starting point heuristic value and add it to the OPEN list; The node selection module is used to select the node with the smallest f value from the OPEN list as the current node; The path determination module is used to determine whether the current node is the target point. If it is, the path is backtracked; otherwise, the node exploration module is entered. The node exploration module is used to add the current node to the CLOSED list; The node expansion module is used to generate candidate neighborhood nodes based on 5×5×5 neighborhood and turning angle constraints. The list update module is used to traverse candidate nodes and update the OPEN list; The iteration control module is used to control the repeated execution of node selection, path determination, node exploration, node expansion, and list update steps.

Images