A three-dimensional path planning method for unmanned aerial vehicles based on gradient guidance and adaptive cost

Abstract

The present application relates to the technical field of unmanned aerial vehicle path planning, and particularly relates to a three-dimensional path planning method for unmanned aerial vehicles based on gradient guidance and adaptive cost, which comprises the following steps: performing three-dimensional rasterization on the flight space of the unmanned aerial vehicle, performing three-dimensional Euclidean distance transformation on each node in the free space to obtain a three-dimensional distance field; calculating the gradient of each node in the three-dimensional distance field, and generating an obstacle-avoiding direction vector for each node based on the gradient; constructing a three-dimensional gradient guidance term; dynamically adjusting the adaptive gradient weight factor in the three-dimensional gradient guidance term; constructing an adaptive balance parameter and dynamically adjusting the adaptive balance parameter; constructing a three-dimensional total cost function; and expanding the nodes using the minimum principle of the three-dimensional total cost function to generate a continuous flight trajectory. The present application introduces an obstacle distance field and a gradient guidance mechanism to intelligently adjust the search direction, thereby improving the safety and computational efficiency of path planning.

Description

Technical Field

[0001] This invention relates to the field of UAV path planning and intelligent navigation control technology in complex indoor environments, and particularly to a UAV three-dimensional path planning method based on gradient guidance and adaptive cost. Background Technology

[0002] The A* algorithm is a classic heuristic search algorithm. This algorithm finds the optimal path from the starting point to the ending point by estimating a function, as shown in the following formula: By weighing the actual costs and heuristic costs To find the optimal path.

[0003] The A* algorithm is widely used in path planning for robots and drones, but in complex environments or areas with dense obstacles, the traditional A* algorithm has many shortcomings:

[0004] (1) The path is close to the obstacle, with low safety margin, which can easily lead to collision risk.

[0005] (2) The number of access nodes is large, the search is redundant, the computation efficiency is low, and the path smoothness is insufficient.

[0006] (3) The paths generated by the A* algorithm are usually in the form of broken lines with abrupt turns. Summary of the Invention

[0007] The purpose of this invention is to address the problems of path alignment with obstacles and low efficiency in complex environments inherent in the traditional A* algorithm, and to propose a UAV 3D path planning method based on gradient guidance and adaptive cost. By introducing an obstacle distance field and a gradient guidance mechanism, intelligent adjustment of the search direction is achieved, thereby improving the safety and computational efficiency of path planning.

[0008] To achieve the above objectives, the technical solution provided by this invention is as follows:

[0009] A UAV 3D path planning method based on gradient guidance and adaptive cost includes:

[0010] The environmental spatial information of the UAV flight space is processed into three-dimensional rasterization to obtain the obstacle area and free space of the UAV flight space. A three-dimensional Euclidean distance transformation is performed on each node in the free space to obtain a three-dimensional distance field. The three-dimensional distance field includes the shortest distance from each node to the nearest obstacle surface.

[0011] Calculate the gradient of each node in the 3D distance field. The gradient direction points away from the obstacle. Generate the obstacle-away direction vector for each node based on the gradient.

[0012] Based on the angle between the obstacle-moving direction vector and the unit direction vector pointing from the current node to the target node, a three-dimensional gradient guidance term is constructed.

[0013] The adaptive gradient weight factor in the three-dimensional gradient guidance term is dynamically adjusted based on the three-dimensional distance field.

[0014] An adaptive balance parameter is constructed based on the obstacle density in the neighborhood of the drone's current location, and the adaptive balance parameter is dynamically adjusted.

[0015] A three-dimensional total cost function is constructed based on a three-dimensional gradient guiding term, an adaptive gradient weight factor, and an adaptive balance parameter.

[0016] The node expansion is performed using the principle of minimizing the three-dimensional total cost function to generate continuous flight trajectories.

[0017] Furthermore, the construction of the three-dimensional gradient guidance term based on the angle between the obstacle-moving direction vector and the unit direction vector pointing from the current node to the target node includes:

[0018] Based on the position of the target node and the position of the current node, calculate the unit direction vector from the current node to the target node;

[0019] The three-dimensional gradient guiding term is constructed as follows:

[0020]

[0021] in, Indicates the first Three-dimensional gradient guiding terms for each node, Indicates the first Adaptive gradient weight factors for each node, For the first The angle between the obstacle-away direction vector and the unit direction vector at each node.

[0022] Furthermore, the adaptive gradient weight factor is expressed by the following formula:

[0023]

[0024] in, Indicates the first Adaptive gradient weight factors for each node, This represents the maximum weight of the gradient guidance effect when the object is close to an obstacle. This represents the minimum baseline weight when far from the obstacle. Indicates the environmental sensitivity coefficient. Indicates the first The Euclidean distance from each node to the nearest obstacle.

[0025] Furthermore, the construction of the adaptive balance parameters is expressed by the following formula:

[0026]

[0027] in, Indicates the adaptive balance parameter. This indicates the obstacle density within a preset radius around the drone's current location. This is the adjustment coefficient.

[0028] Furthermore, the construction of the three-dimensional total cost function is expressed by the following formula:

[0029]

[0030] in, Indicates the first The total three-dimensional cost of each node. Indicates from the starting point to the... The three-dimensional cumulative cost of each node, Indicates from the first The three-dimensional Euclidean heuristic cost from each node to the target node. Indicates the first Three-dimensional gradient guiding terms for each node, This represents the adaptive balance parameter.

[0031] Furthermore, the node expansion based on the principle of minimizing the three-dimensional total cost function to generate continuous flight trajectories includes:

[0032] Initialize the open list, which is implemented using a priority queue, and add the starting node to the open list;

[0033] Select the node with the minimum total cost in three dimensions from the open list as the current node;

[0034] Determine if the current node is the target node. If it is, backtrack the path. If not, expand the adjacent nodes of the current node.

[0035] Calculate the total 3D cost for each adjacent node and update the open list based on the total 3D cost until the target node is reached;

[0036] The discrete path node sequence obtained by backtracking is smoothed and time-parameterized to generate a continuous flight trajectory that conforms to the kinematic constraints of the UAV.

[0037] Furthermore, Gaussian smoothing is applied to the obstacle boundaries in the obstacle area.

[0038] Compared with the prior art, the significant advantages of this invention are:

[0039] 1. The gradient guidance mechanism automatically moves the path away from obstacles, increasing the safety margin.

[0040] By establishing a Euclidean distance field in a 3D environment and calculating its gradient vector, the algorithm can perceive the spatial distribution direction of obstacles. A gradient guiding term is introduced. It automatically deviates the search direction from high-risk areas of obstacles, thus effectively avoiding the phenomenon of the path running close to the obstacle boundary.

[0041] 2. The adaptive cost function can dynamically adjust the search strategy according to the environment density.

[0042] The exponential decay model based on the distance field can automatically adjust the gradient term's influence according to the distance from the node to the obstacle: the weight increases when the node is close to the obstacle, enhancing obstacle avoidance; the weight decreases when the node is far from the obstacle, improving search efficiency.

[0043] 3. Introduce adaptive balance parameters This achieves a dynamic balance between heuristic search and security constraints.

[0044] Balance parameters Based on local obstacle density Real-time adjustments enable an adaptive transition between heuristic and conservative approaches. This is particularly relevant when dealing with complex environments. To reduce the size, the algorithm prioritizes safety; when the space is open, As the algorithm increases, it tends to perform more efficient searches.

[0045] 4. The three-dimensional improved cost function enhances the overall planning efficiency and feasibility.

[0046] The total cost function of this invention comprehensively considers the actual cost, heuristic distance, and gradient direction cost, and significantly reduces invalid node visits while maintaining global optimality. Attached Figure Description

[0047] Figure 1 This is a flowchart illustrating the overall process of a UAV 3D path planning method based on gradient guidance and adaptive cost according to the present invention.

[0048] Figure 2 This is a schematic diagram of the obstacle gradient direction relationship of the present invention;

[0049] Figure 3 This is a schematic diagram of the obstacle distance field relationship of the present invention;

[0050] Figure 4 This is a schematic diagram of the path planning results of the present invention;

[0051] Figure 5 This is a schematic diagram of a 3D map model based on ROS established in this invention;

[0052] Figure 6 This is a schematic diagram of the path planning results based on ROS 3D map according to the present invention. Detailed Implementation

[0053] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0054] This invention provides a UAV 3D path planning method based on gradient guidance and adaptive cost, such as... Figure 1 As shown, the main steps include:

[0055] Step S1: Model or discretize the UAV flight space. Specifically, acquire environmental spatial information and perform 3D rasterization processing, with each voxel unit corresponding to a passable region (free space) or obstacle region in the space. Perform a 3D Euclidean distance transformation on the free space to obtain the 3D range field. .in, This represents the shortest Euclidean distance from all nodes in free space to the nearest obstacle surface. For the search node... Its corresponding position is Define the distance value at the node as The obstacle boundaries in the obstacle region are smoothed using Gaussian smoothing.

[0056] Step S2: 3D gradient calculation and direction vector extraction.

[0057] Step S21: Calculate the gradient of the three-dimensional range field, using the following formula:

[0058]

[0059] in, Represents the gradient of the three-dimensional distance field. Represents the rate of change in the x-axis direction. Represents the rate of change in the y-axis direction. It represents the rate of change in the z-axis direction, with the gradient direction pointing away from the obstacle, and its magnitude reflects the intensity of the obstacle's influence on that point.

[0060] Step S22: Normalize the gradient to obtain the obstacle's moving away direction vector, as shown in the following formula:

[0061]

[0062] in, Represents the three-dimensional distance field at the th The gradient at each node Indicates the first The obstacle at each node moves away from the direction vector.

[0063] Step S23: Define the current node The unit direction vector pointing to the target node:

[0064]

[0065] in, Indicates the first The unit direction vector from each node to the target node. Indicates the location of the target node. Indicates the first The position of each node.

[0066] Step S3: Apply the 3D gradient guidance term To build.

[0067] Based on the angle between the obstacle gradient direction and the target direction, a three-dimensional gradient guidance term is introduced. This is used to measure the degree to which the path direction deviates from the obstacle safety zone, and the formula is as follows:

[0068]

[0069] in, Represents the three-dimensional gradient guiding term. Represents the adaptive gradient weight factor. for and The included angle.

[0070] When the gradient direction is consistent with the target direction hour, A smaller value indicates that the path is aligned with the obstacle boundary, resulting in lower cost; when the direction is opposite... hour, The cost is relatively high, so the algorithm increases the cost to guide the search away from obstacles.

[0071] Specifically, such as Figure 2 and Figure 3 As shown, the gradient direction is opposite to the obstacle boundary normal direction, and the gradient magnitude represents the intensity of the obstacle's influence; while the distance map illustrates the distance relationship between obstacles and free space in the environment, which is the basis for the algorithm to construct the gradient guidance mechanism.

[0072] Step S4: Adjust the adaptive gradient weight factor Dynamic adjustments are made.

[0073] To achieve adaptive adjustment of the gradient-guided term, an exponential decay model based on the three-dimensional distance field is introduced, as shown in the following formula:

[0074]

[0075] in, This represents the maximum weight of the gradient guidance effect when the object is near an obstacle. Represents the minimum baseline weight when far from the obstacle; This represents the environmental sensitivity coefficient, used to control the rate at which the weight decays with distance.

[0076] The exponential decay model states that when a node approaches an obstacle ( (smaller value) Larger gradients enhance obstacle avoidance performance; when a node is far from an obstacle ( (The value is relatively large) It automatically reduces the search area, making the search more focused on the target.

[0077] Step S5: Adjust the adaptive equilibrium parameters Dynamic adjustments are made.

[0078] In complex 3D environments, an adaptive balance parameter is introduced to achieve an adaptive balance between search efficiency and security. The formula is as follows:

[0079]

[0080] in, This indicates the obstacle density within a preset radius (e.g., a 3×3×3 voxel neighborhood) around the drone's current location; This is the adjustment coefficient, used for control. Sensitivity to changes in barrier density.

[0081] When the local obstacle density is high , When obstacles are sparse, the algorithm prioritizes path safety and cost conservation. , With increased efficiency, the algorithm places greater emphasis on heuristic search efficiency.

[0082] Step S6: Improve the design of the cost function.

[0083] The formula for the traditional A* cost model is as follows:

[0084]

[0085] Based on the traditional A* cost model, a gradient guiding term is introduced. With adaptive parameters An improved three-dimensional total cost function is constructed, as shown in the following formula:

[0086]

[0087] in, Indicates the first The total three-dimensional cost of each node. Indicates from the starting point to the... The three-dimensional cumulative cost of each node, Indicates from the first The three-dimensional Euclidean heuristic cost from each node to the target node. Indicates the first The 3D gradient guidance term for each node represents the direction penalty based on the gradient guidance term. This represents the adaptive balancing parameter, used to control heuristics and safety.

[0088] In the formula: →0: Prefers conservative cost-saving, stable search but lengthy path; →1: It is more heuristic and the search is more aggressive but more efficient; This refers to the adaptive gradient term described in the previous section.

[0089] like Figure 4 As shown, the path generated based on this cost model is smoother than the traditional A* algorithm path and can significantly avoid obstacle edge areas, thus improving the safety margin of the path.

[0090] Step S7: Based on the three-dimensional total cost function The node expansion follows the principle of minimum size, and a priority queue structure is used to sort and update candidate nodes, generating a continuous flight trajectory that conforms to the dynamic constraints of the UAV. The final output 3D path satisfies both obstacle avoidance safety and global optimality.

[0091] Specifically, an open list (Open) and a closed list (Closed) are initialized. The open list is implemented using a priority queue, with the starting node added to it. The node with the minimum total 3D cost is selected from the open list as the current node, and its neighboring nodes are expanded. Candidate nodes (the neighboring nodes of the current node) are sorted, inserted, and updated until the target node is reached. The discrete path node sequence obtained from the backtracking is smoothed and time-parameterized (or alternatively, fifth-order polynomial interpolation or minimum Jerk trajectory optimization can be used) to generate a continuous flight trajectory that conforms to the UAV's kinematic constraints (including velocity, acceleration, angular velocity, and continuity constraints).

[0092] like Figure 5 The image shows a 3D grid map of the UAV's flight environment. This map is obtained by discretizing the flight space in three dimensions, using grid cells to represent obstacles and passable areas in the environment. Different colored grids are used to distinguish spatial structure and altitude distribution, visually reflecting the spatial location of obstacles in the environment and providing environmental information for subsequent path planning.

[0093] like Figure 6 The image shows the flight path of the UAV generated on the aforementioned 3D grid map. The black curve in the image represents the planned and smoothed flight path. This path can avoid obstacles in complex environments and maintain continuous and smooth turning changes, indicating that the generated path conforms to the motion characteristics of the UAV in actual flight and is feasible.

[0094] The method of this invention can be implemented in a UAV flight control system. The system includes a processor and a memory, the memory storing a computer program that implements the above steps. When the program is executed by the processor, it achieves automatic path planning.

[0095] The total cost function of this invention comprehensively considers the actual cost, heuristic distance, and gradient direction cost, significantly reducing invalid node visits while maintaining global optimality. A jump-point algorithm (JPS) is introduced as a comparison, which reduces node expansion through jump-point pruning, but may result in longer paths and increased turns and angles. Simulation results are as follows:

[0096] (1) The comparison results of the number of nodes accessed and the running time of each algorithm in the 30m×30m grid map are shown in Table 1.

[0097] Table 1. Comparison of performance metrics of various algorithms for 30×30 raster maps

[0098]

[0099] The traditional A* algorithm visits 209 nodes and takes 6.10ms; this invention visits 122 nodes and takes 3.30ms, reducing the number of nodes by 41.6% and the time by 45.9%. Compared to the JPS algorithm (77 nodes, 3.40ms), this invention has a shorter running time and a smoother path: the path length of this invention is 43.36m, with 11 turns and an average turning angle of 49.09°, which is better than JPS's 43.70m, 13 turns, and 58.85°.

[0100] (2) The comparison results of the number of nodes accessed and the running time of each algorithm in the 40m×40m grid map are shown in Table 2.

[0101] Table 2 Comparison of performance metrics of various algorithms for 40×40 raster maps

[0102]

[0103] The traditional A* algorithm visits 674 nodes and takes 15.4ms; this invention visits only 158 nodes and takes 4.60ms, reducing the number of nodes by 76.6% and the time by 70.1%. Compared to JPS (103 nodes, 4.00ms), this invention has a shorter path and smoother turning: the path length of this invention is 57.25m and the average turning angle is 45.00°, which is significantly better than JPS's 69.60m and 82.06°.

[0104] (3) The comparison results of the number of nodes accessed and the running time of each algorithm in the 50m×50m grid map are shown in Table 3.

[0105] Table 3. Comparison of performance metrics of various algorithms for 50×50 raster maps

[0106]

[0107] The traditional A* algorithm visits 1223 nodes and takes 32.0 ms; this invention visits 214 nodes and takes 4.30 ms, reducing the number of nodes by 82.5% and the time by 86.6%. Compared to JPS (168 nodes, 4.70 ms), this invention has a shorter running time and lower path cost: the path length of this invention is 75.74 m, and the average turning angle is 46.45° (31 turns), which is better than JPS's 84.33 m and 75.00° (21 turns).

[0108] Simulation experiments have demonstrated that, under the same map scale, this invention can significantly reduce the number of accessed nodes and shorten the running time (approximately 41.6%–82.5% reduction in the number of accessed nodes and approximately 45.9%–86.6% reduction in running time compared to the traditional A* algorithm). Furthermore, while JPS can further reduce the number of accessed nodes, it suffers from increased path length, more pronounced turns, and larger average angles in multiple scenarios. This invention, while maintaining high search efficiency, yields shorter paths with smoother turns, making it more conducive to generating continuous flight trajectories that conform to the kinematic constraints of UAVs.

[0109] The embodiments described above are merely illustrative of several implementations of the present invention, and while the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the invention patent. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and these all fall within the protection scope of the present invention. Therefore, the protection scope of this invention patent should be determined by the appended claims.

Claims

1. A UAV 3D path planning method based on gradient guidance and adaptive cost, characterized in that, The UAV 3D path planning method based on gradient guidance and adaptive cost includes: The environmental spatial information of the UAV flight space is processed into three-dimensional rasterization to obtain the obstacle area and free space of the UAV flight space. A three-dimensional Euclidean distance transformation is performed on each node in the free space to obtain a three-dimensional distance field. The three-dimensional distance field includes the shortest distance from each node to the nearest obstacle surface. Calculate the gradient of each node in the 3D distance field. The gradient direction points away from the obstacle. Generate the obstacle-away direction vector for each node based on the gradient. Based on the angle between the obstacle-moving direction vector and the unit direction vector pointing from the current node to the target node, a three-dimensional gradient guidance term is constructed. The adaptive gradient weight factor in the three-dimensional gradient guidance term is dynamically adjusted based on the three-dimensional distance field. An adaptive balance parameter is constructed based on the obstacle density in the neighborhood of the drone's current location, and the adaptive balance parameter is dynamically adjusted. A three-dimensional total cost function is constructed based on a three-dimensional gradient guiding term, an adaptive gradient weight factor, and an adaptive balance parameter. The node expansion is performed using the principle of minimizing the three-dimensional total cost function to generate continuous flight trajectories; in, The construction of a three-dimensional gradient guidance term based on the angle between the obstacle-moving direction vector and the unit direction vector pointing from the current node to the target node includes: Based on the position of the target node and the position of the current node, calculate the unit direction vector from the current node to the target node; The three-dimensional gradient guiding term is constructed as follows: in, Indicates the first Three-dimensional gradient guiding terms for each node, Indicates the first Adaptive gradient weight factors for each node, For the first The angle between the obstacle's distance direction vector and the unit direction vector at each node; The adaptive gradient weight factor is expressed by the following formula: in, Indicates the first Adaptive gradient weight factors for each node, This represents the maximum weight of the gradient guidance effect when the object is close to an obstacle. This represents the minimum baseline weight when moving away from the obstacle. Indicates the environmental sensitivity coefficient. Indicates the first The Euclidean distance from each node to the nearest obstacle.

2. The UAV 3D path planning method based on gradient guidance and adaptive cost according to claim 1, characterized in that, The adaptive balance parameters are constructed as follows: in, Indicates the adaptive balance parameter. This indicates the obstacle density within a preset radius around the drone's current location. This is the adjustment coefficient.

3. The UAV 3D path planning method based on gradient guidance and adaptive cost according to claim 1, characterized in that, The constructed three-dimensional total cost function is expressed by the following formula: in, Indicates the first The total three-dimensional cost of each node. Indicates from the starting point to the... The three-dimensional cumulative cost of each node, Indicates from the first The three-dimensional Euclidean heuristic cost from each node to the target node. Indicates the first Three-dimensional gradient guiding terms for each node, This represents the adaptive balance parameter.

4. The UAV 3D path planning method based on gradient guidance and adaptive cost according to claim 1, characterized in that, The method of expanding nodes and generating continuous flight trajectories using the principle of minimizing the three-dimensional total cost function includes: Initialize the open list, which is implemented using a priority queue, and add the starting node to the open list; Select the node with the minimum total cost in three dimensions from the open list as the current node; Determine if the current node is the target node. If it is, backtrack the path. If not, expand the adjacent nodes of the current node. Calculate the total 3D cost for each adjacent node and update the open list based on the total 3D cost until the target node is reached; The discrete path node sequence obtained by backtracking is smoothed and time-parameterized to generate a continuous flight trajectory that conforms to the kinematic constraints of the UAV.

5. The UAV 3D path planning method based on gradient guidance and adaptive cost according to claim 1, characterized in that, Gaussian smoothing is applied to the obstacle boundaries in the obstacle region.

Images