Path planning method for three-dimensional high-drift angle UAV based on improved greedy algorithm

Abstract

This invention belongs to the field of aircraft path planning and navigation control technology. It discloses a three-dimensional high-drift angle unmanned aerial vehicle (UAV) path planning method based on an improved greedy algorithm. The method constructs a three-dimensional terrain model of the mission area; constructs a path energy consumption assessment model; plans an initial three-dimensional flight path using the improved greedy algorithm; and evaluates and optimizes the initial three-dimensional flight path to obtain an optimized three-dimensional flight path. By introducing an improved greedy algorithm, constructing a cost function based on energy consumption estimates and pitch angle changes, and performing path search and optimization, the method addresses common problems in existing path planning methods, such as "overly idealistic assumptions about path feasibility," "ignoring the actual difficulty of high-drift angle flight," and "distorted simplified energy consumption models."

Description

Technical Field

[0001] The present invention belongs to the field of aircraft path planning and navigation control technology, and relates to a three-dimensional path planning method suitable for optimizing the path of an unmanned aerial vehicle (UAV) in a complex terrain environment, and in particular to a three-dimensional high-offset angle UAV path planning method based on an improved greedy algorithm. Background Art

[0002] In the execution of tasks in complex terrain areas, drones have been widely used in scenarios such as surveying and mapping, emergency rescue, and geographic information collection due to their high maneuverability and rapid deployment capabilities.

[0003] Patent application CN202411338741.0 discloses a drone path optimization method based on a biomimetic growth optimization process. By establishing a perception model, the drone can sense and identify surrounding obstacles. By establishing a control model, the drone can select eight basic flight directions on a two-dimensional plane to form an obstacle avoidance strategy. Path planning combines artificial potential fields with biomimetic growth mechanisms, enabling the drone to perceive obstacles and optimize its navigation path in real time during flight. This can improve the drone's autonomous obstacle avoidance capabilities and mission execution efficiency in complex environments. However, existing drone path planning technology still has significant shortcomings when facing environments with dramatic three-dimensional fluctuations, such as mountains and canyons. Current mainstream path planning methods are mostly based on two-dimensional maps or simplified three-dimensional modeling, which cannot accurately depict the impact of terrain changes on flight attitude. As a result, the generated paths often have problems such as excessive deviation angles, flight infeasibility, or excessive energy consumption in actual flight.

[0004] Furthermore, most existing methods assume that drones can turn or climb at any angle, ignoring the physical limitations of aircraft hardware in terms of pitch angle, turning radius, and power output. This poses the risk of flight instability or loss of control during actual implementation. Furthermore, existing energy consumption models generally use linear estimates or simplified formulas, failing to dynamically assess energy consumption based on flight status and aerodynamic factors, making it difficult to support precise energy efficiency optimization in complex terrain.

[0005] Therefore, there is an urgent need for a UAV path planning method that can take into account both path feasibility and optimal energy consumption in complex three-dimensional environments. Summary of the Invention

[0006] The purpose of the present invention is to address the above-mentioned technical problems existing in the prior art and provide a three-dimensional high-offset angle optimized UAV path planning method based on an improved greedy algorithm. The method can effectively constrain the pitch angle of the UAV in the path in complex three-dimensional terrain environments, such as mountains, canyons, hills and other areas with significant elevation changes; and also plan a low-energy-consumption, high-stability, and practical flight-executable three-dimensional path through dynamic energy consumption evaluation, thereby solving the problems of existing path planning technologies such as infeasible paths in complex terrains, excessive energy consumption, and unstable flight attitude.

[0007] In order to achieve the above objectives, the present invention adopts the following technical solutions.

[0008] The present invention provides a three-dimensional high-drift angle UAV path planning method based on an improved greedy algorithm, which comprises the following steps:

[0009] S1, constructing a 3D terrain model of the mission area: constructing the corresponding 3D terrain grid based on the digital elevation model data of the mission area;

[0010] S2, building a path energy consumption evaluation model;

[0011] S3, using the improved greedy algorithm to plan the initial three-dimensional flight path: between the set flight start and end points, the cost function used by the improved greedy algorithm is used to select the node with the minimum cost to form the initial three-dimensional flight path;

[0012] The cost function Cij used by the improved greedy algorithm is:

[0013] ;

[0014] Among them, Eij is the estimated energy consumption from the current node ni to the neighboring node nj; is the pitch angle change between the two nodes; λ is the angle penalty coefficient;

[0015] S4, evaluating and optimizing the initial three-dimensional flight path to obtain an optimized three-dimensional flight path.

[0016] In the above step S1, the task area is gridded with equal intervals based on the digital elevation model data of the task area. The specific operation is: the digital elevation model data of the task area is filled with null values and edge clipped to establish a three-dimensional terrain grid consistent with the real geographical features.

[0017] In the above step S2, the path energy consumption evaluation model includes energy consumption in the acceleration flight phase, energy consumption in the deceleration flight phase, and energy consumption in the constant speed flight phase;

[0018] Energy consumption during acceleration flight phase E accThe calculation formula is:

[0019] ;

[0020] in, P acc (t) is the flight power P during the acceleration phase; T acc To speed up the flight time;

[0021] Energy consumption during deceleration flight phase E dec The calculation formula is:

[0022] ;

[0023] in, P dec (t) is the flight power P during the deceleration flight phase; T dec The deceleration flight time;

[0024] Energy consumption during constant speed flight E V The calculation formula is:

[0025] ;

[0026] in, P V is the flight power P during the constant speed flight phase; T V is the constant speed flight time;

[0027] The flight power P is calculated according to the following formula:

[0028] ;

[0029] in, P 0 is blade power; V is the flight speed; R is the rotor radius; is the rotor angular velocity; k is the thrust-to-weight ratio; W is the weight of the aircraft; F y It is the resultant thrust required by the aircraft in the pitch flight direction; is the air density; A is the rotor area; v 0 is the rotor induced speed; is the pitch angle of the flight path end point relative to the starting point;

[0030] v The calculation formula is as follows:

[0031] ;

[0032] k The calculation formula is as follows:

[0033] ;

[0034] F y The calculation formula is as follows:

[0035] ;

[0036] in, C d is the drag coefficient of the aircraft in the windward direction;

[0037] The calculation formula is as follows:

[0038] ;

[0039] in, 、 are the three-dimensional coordinates of the starting point and end point of the flight path respectively;

[0040] In step S3, energy consumption estimation ;

[0041] Pitch angle change ;

[0042] in, 、 Node n i and nodes n j The three-dimensional coordinates of .

[0043] Furthermore, in step S3, if the pitch angle changes between all adjacent nodes of the current node exceed the set maximum flight pitch angle, a candidate position is determined based on the current node and the maximum flight pitch angle, and it is judged whether the candidate position intersects with the three-dimensional terrain model boundary of the task area constructed in step S1. If no intersection occurs, the candidate position is the next node position; if an intersection occurs, a first fallback operation is performed, specifically: fall back from the historical path to the previous node that meets the angle continuity, and re-search for feasible alternative path nodes in the neighborhood of the node according to the improved greedy algorithm, and re-select the alternative path branch.

[0044] Furthermore, in step S3, a local window strategy is introduced to perform local path optimization. The local window size is K, and the path between two adjacent nodes is considered a one-step path segment. After every K steps, the overall direction or overall posture of the K-step path segment is determined. If the overall direction deviates from the main direction of the flight path from the start to the end point or the overall posture becomes unstable, a second fallback operation is performed. If the overall direction of the K-step path segment does not deviate from the main direction and the overall posture is stable, the next K-step path segment is constructed.

[0045] The method for judging whether the overall direction of a K-step path segment deviates from the main direction is to define the cosine similarity between the path direction in the local window and the global target direction from the start point to the end point. for:

[0046] ;

[0047] in, V l For the l The path direction vector of the step path segment, V goal is the global direction vector from the start point to the end point; if S dir < , it is judged that the overall direction of the K-step path segment deviates from the main direction, otherwise it is judged that the overall direction of the K-step path segment does not deviate from the main direction, is the direction consistency threshold;

[0048] The overall attitude instability judgment method of the K-step path segment is: calculate the absolute value and the pitch angle change in the local window :

[0049] ;

[0050] in, For the l Pitch angle changes of the step path segment; For the l +1 step pitch angle change of the path segment; if , it is determined that the overall posture of the K-step path segment is unstable, otherwise it is determined that the overall posture of the K-step path segment is stable. The cumulative change threshold of the pitch angle.

[0051] The specific implementation method of the second fallback operation is: fall back to the first node of the K-step path segment, re-search for feasible alternative path nodes in the neighborhood of this node according to the improved greedy algorithm, build a new K-step path segment and re-perform local path optimization.

[0052] The second fallback operation also includes adjusting the angle penalty term coefficient (for example, increasing the angle penalty term coefficient) to suppress single-direction deviation.

[0053] In step S4 above, the initial three-dimensional flight path is traversed and each path segment is evaluated. When the current path segment meets one of the following conditions, a third fallback operation is performed to optimize the flight path:

[0054] (1) Take three consecutive nodes as a continuous path segment, and the angle change of any continuous path segment is Exceeding the pitch angle change threshold;

[0055] (2) The estimated cumulative energy consumption of the current three-dimensional flight path exceeds the UAV’s endurance limit (i.e., the UAV’s battery capacity);

[0056] (3) Multiple consecutive path segments have gradually increasing positive pitch angles, that is, steep slopes appear on the local path;

[0057] (4) Take three consecutive nodes as a continuous path segment, and the heading angle change of any continuous path segment is The heading stability threshold is exceeded, which means that the path direction continuity is destroyed.

[0058] For the above case (1), for any three consecutive points in the path 、 、 The two adjacent path segments are composed of two paths, each defining the pitch angle of the two path segments. and angle change as follows:

[0059] ;

[0060] ;

[0061] ;

[0062] Will The pitch angle change threshold allowed for the drone Compare; if , then it is judged that the current path has a risk of sudden change or flight instability, and the node To Node The path segment is marked as infeasible and triggers the third fallback operation.

[0063] For the above situation (2), this step synchronously calls the path segment energy consumption evaluation model defined in step S2 to evaluate the energy consumption between path segments. Accumulate, if the accumulated energy consumption Exceeding the drone's endurance limit , and the path is also determined to be non-executable.

[0064] For the above situation (3), for example, if 3-5 consecutive path segments are all gradually increasing positive pitch angles, the overall attitude instability judgment method of the previous K-step path segment can also be directly called.

[0065] For the above case (4), for any three consecutive points in the path 、 、 The horizontal projection direction vectors of the two adjacent path segments are calculated separately:

[0066] ;

[0067] ;

[0068] Calculate the angle between the two vectors, that is, the heading angle change:

[0069] ;

[0070] in, For nodes To Node The horizontal projection direction vector of the path segment; For nodes To Node The horizontal projection direction vector of the path segment; is the heading angle change between adjacent path segments; if , If the heading stability threshold is set, the path direction continuity is determined to be broken, and the node To Node The path segment is marked as infeasible and triggers the third fallback operation.

[0071] Furthermore, the specific implementation of the third fallback operation is: fall back to the previous path node on the initial three-dimensional flight path ,in is the step length of the backoff window; in the neighborhood of this node, according to the improved greedy algorithm in the previous step S3, a new path branch is searched for feasible alternative path nodes again to construct a new path branch; the new branch is connected to the main path to form a new complete path sequence. The new complete path sequence is re-evaluated and optimized. If the backoff fails multiple times in a row, the current three-dimensional flight path is considered unable to reach the target point and the current round of planning is terminated; wait for the user to reset the starting and ending points or the intermediate points. In order to prevent the path generation efficiency from decreasing due to frequent backoff, this step introduces a backoff limit parameter , a path is allowed to execute at most Rollback and reconstruction operations are performed. If the number of rollback and reconstruction operations exceeds the limit, the path will be forcibly terminated and effective.

[0072] In addition, during the path structure adjustment process of the third fallback operation, when a new complete path sequence is formed, the energy consumption growth trend of the newly added path segment can also be judged. If the energy consumption of the newly added path segment is significantly higher than that of the original path (the increase exceeds the set energy consumption ratio threshold), the angle penalty coefficient will be adjusted; by adjusting the angle penalty coefficient in the cost function , giving priority to the energy consumption control target and suppressing the impact of angle constraints on path deviation.

[0073] The above-mentioned three-dimensional high-offset angle UAV path planning method based on the improved greedy algorithm also includes:

[0074] S5, smoothing the optimized three-dimensional flight path and outputting the final three-dimensional flight path.

[0075] In the above step S5, the optimized three-dimensional flight path is smoothed by the mean filtering method. Assuming the sliding window size is K', the mean filtering calculation formula is:

[0076] ;

[0077] in, are the smoothed path node coordinates, is the original path node; when K'=5, the window range covers two adjacent nodes before and after the current node, and the local jitter is suppressed by the mean operation while retaining the terrain feature trend.

[0078] Compared with the existing technology, the three-dimensional high-offset angle UAV path planning method based on the improved greedy algorithm provided by the present invention has the following beneficial effects:

[0079] (1) This invention combines key technologies such as terrain modeling, flight attitude restriction, and dynamic energy consumption modeling. By introducing an improved greedy algorithm, a cost function is constructed based on energy consumption estimation and pitch angle change to perform path search and optimization. This solves the common problems in existing path planning methods, such as "overly ideal path feasibility assumptions", "ignoring the actual flight difficulty at high deviation angles", and "distorted simplified energy consumption models".

[0080] (2) The present invention effectively improves the attitude stability of the aircraft in complex terrain by real-time calculation and control of the pitch angle in the path;

[0081] (3) The present invention constructs a three-stage energy consumption model to accurately reflect the energy loss characteristics of different stages during flight;

[0082] (4) The present invention improves robustness and execution efficiency in actual complex environments through path fallback and dynamic adjustment mechanisms;

[0083] (5) The path generated by the present invention can be directly sent to the flight control system for execution, which has good practical deployment value and is suitable for complex mission environments such as plateau inspection, border reconnaissance, and disaster response. BRIEF DESCRIPTION OF THE DRAWINGS

[0084] Figure 1 A schematic diagram of the flow chart of the three-dimensional high-drift angle UAV path planning method based on the improved greedy algorithm provided by the present invention;

[0085] Figure 2 A schematic diagram of a digital elevation model (DEM) generated for the Jiuzhaigou area used in the present invention;

[0086] Figure 3 This is a schematic diagram of a three-dimensional high-precision landform model based on the Jiuzhaigou terrain of the present invention;

[0087] Figure 4 A comparison chart of the path generation effects of the present invention and the traditional path planning method. DETAILED DESCRIPTION

[0088] In order to make the purpose, technical solution and beneficial effects of the present invention clearer and more specific, the present invention is further described in detail below with reference to the accompanying drawings and implementation examples. It should be understood that the specific embodiments described herein are only for illustrating the present invention and are not intended to limit the scope of protection of the present invention.

[0089] This paper proposes a three-dimensional high-offset angle UAV path planning method based on an improved greedy algorithm. This method primarily addresses existing issues such as unreachable paths, excessive energy consumption, and excessive pitch angles for UAVs operating in complex mountainous or rugged terrain. Unlike existing simplified two-dimensional or contour surface models, this method reconstructs the actual terrain surface in three-dimensional space based on a high-precision digital elevation model (DEM). It incorporates energy consumption models for various flight states, such as climb, turn, and level flight, and introduces a dynamic high-offset angle screening mechanism. This results in a generated path with greater practical feasibility and energy efficiency.

[0090] The present invention provides a three-dimensional high-offset angle UAV path planning method based on an improved greedy algorithm, such as Figure 1 As shown, it includes the following steps:

[0091] S1, construct a three-dimensional terrain model of the mission area; based on the digital elevation model data of the mission area, construct the corresponding three-dimensional terrain grid.

[0092] This step extracts terrain elevation information for the designated mission area (e.g., typical mountainous terrain like Jiuzhaigou) by reading the mission's Digital Elevation Model (DEM) data. By performing null-filling and edge clipping on the DEM data, the mission area is gridded to create a 3D terrain grid consistent with the actual geography. This serves as the foundation for subsequent path planning, accurately reproducing elevation differences and steep slopes within the area.

[0093] This embodiment takes the Jiuzhaigou flight mission area as an example to explain in detail the process of constructing a three-dimensional terrain model of the mission area.

[0094] The digital elevation model of Jiuzhaigou area is as follows: Figure 2 As shown, the DEM data used is a .tif raster file that records the elevation information of each geographic location on the surface, with values in meters. The first band of the DEM data is read using the rasterio library and loaded into memory in matrix form to form an initial elevation data array. Because some areas contain invalid pixels or null pixels, these values are first uniformly processed and replaced with NaN values to automatically remove discontinuous data points during subsequent visualization and path calculation. Next, to improve computational efficiency and focus on the typical terrain features of the flight area, this embodiment selects a regional submatrix starting at the specified coordinates from the original DEM data, extracts a 500×500 pixel window as the planning range, and performs spatial clipping of the elevation data. During the clipping process, the selected area is checked to see if it exceeds the DEM boundary. If so, a warning is issued to prevent out-of-bounds reading. To handle residual NaN values, the system fills the gaps by adding a bias constant (e.g., 10 meters) to the original elevation minimum value to ensure a continuous 3D grid with no holes. On this basis, the system uses numpy to generate the horizontal and vertical coordinate vectors of the corresponding area and constructs the corresponding two-dimensional grid coordinate matrix, so that the elevation data has a clear x, y, and z three-dimensional position index relationship in space. The final three-dimensional terrain model is a continuous spatial surface, where x and y represent the terrain projection plane position and z is the actual elevation value. To verify the correctness of the three-dimensional model construction and data consistency, the system can call the plotly library to render the terrain as a three-dimensional image in the form of a Surface, such as Figure 3As shown, it can display the actual undulating terrain in the area and provide an environmental input basis for pitch angle control and energy consumption evaluation in path planning. The above-mentioned three-dimensional terrain model can support users to view the terrain contour at any angle and intuitively observe key landform features such as valleys, slope changes, and elevation trends. As the basic geographical support input for path planning, the accuracy of this model directly affects the rationality of subsequent path node generation and the accuracy of flight feasibility judgment. It can be seen that the high-precision DEM modeling mechanism provided by this embodiment not only retains the spatial characteristics of complex mountainous terrain, but also has good clipping flexibility and coordinate consistency, and can be widely adapted to any area with undulating terrain.

[0095] Based on the constructed three-dimensional terrain model of the mission area and combined with the coordinates of the flight path nodes, the vertical height difference and horizontal distance between each two adjacent nodes can be calculated, and the pitch angle of the path segment can be calculated through the inverse tangent function for subsequent path feasibility assessment and constraint judgment.

[0096] S2, construct the path energy consumption evaluation model.

[0097] After building the three-dimensional terrain model, this step needs to set flight physical constraints based on the UAV's flight performance parameters and mission scenarios, and on this basis, build a multi-stage, multi-state fusion path energy consumption evaluation model.

[0098] The path energy consumption assessment model comprehensively considers factors such as flight attitude, speed, acceleration, aerodynamic drag, and rotor characteristics, and strives to achieve high-precision estimation of the energy consumption of path segments in complex terrain.

[0099] Based on the selected UAV platform, initialization parameters include flight-related parameters such as aircraft mass, battery capacity, rotor area, air density, maximum flight speed, maximum pitch angle, maximum steering angle, rotor angular velocity, and an induced velocity estimation function. Any segment of a path can be considered to consist of an acceleration segment, a constant speed segment, and a deceleration segment. For each segment, the elevation difference and horizontal projection distance between its three-dimensional starting and ending points are first determined to calculate its pitch angle.

[0100] During flight path planning, the starting point, endpoint, and several intermediate points must be determined. Furthermore, key factors involved in flight path planning include the pitch angle of the endpoint relative to the starting point, the rotor induced speed, the thrust-to-weight ratio, and the flight power model.

[0101] The pitch angle of the flight path point relative to the starting point ( ) Calculation: Pitch angle is an important factor in the flight path. According to the three-dimensional coordinates of the flight start and end point, the pitch angle It can be calculated by the following formula:

[0102] ;

[0103] in, 、 are the three-dimensional coordinates of the starting point and end point of the flight path respectively.

[0104] rotor induced speed v The calculation formula is as follows:

[0105] ;

[0106] in, W is the weight of the aircraft; is the air density; A is the rotor area (the effective area of the rotor, that is, the circular projected area swept by the rotor when it rotates).

[0107] Thrust-to-weight ratio ( k ): Thrust-to-weight ratio affects the lift and propulsion efficiency of the aircraft. The calculation formula is as follows:

[0108] ;

[0109] in, F y is the synthetic thrust required by the aircraft in the pitch flight direction, F y The calculation formula is as follows:

[0110] ;

[0111] in, V is the flight speed; C d is the drag coefficient of the aircraft in the windward direction.

[0112] Flight power: The calculation of flight power takes into account the propulsion force, rotor induced speed and flight speed changes. P It can be calculated by the following formula:

[0113] ;

[0114] in, P 0 is blade power; is the rotor angular velocity; R is the rotor radius; v 0 is the rotor induced speed; The term represents the power consumption of thrust in the vertical direction when climbing or descending.

[0115] In order to further optimize the flight path, this embodiment proposes an optimization method based on accurate energy consumption estimation. E(Including energy consumption during the acceleration flight phase E acc , Energy consumption during constant speed flight E V and energy consumption during deceleration flight phase E dec ) combined with path planning can more accurately reflect the actual energy consumption of the aircraft in different flight phases.

[0116] Energy consumption during acceleration flight phase E acc The calculation formula is:

[0117] ;

[0118] in, P acc (t) is the flight power during the acceleration flight phase P ; T acc To speed up the flight time;

[0119] Assuming that the acceleration and deceleration phases are symmetrical, the energy consumption during the deceleration flight phase is E dec The calculation formula is:

[0120] ;

[0121] in, P dec (t) is the flight power during the deceleration flight phase P ; T dec The deceleration flight time;

[0122] Energy consumption during constant speed flight E V The calculation formula is:

[0123] ;

[0124] in, P V Flight power during constant speed flight P ; T V is the constant speed flight time.

[0125] Through this three-stage energy consumption model, the present invention can significantly improve the authenticity and feasibility of path energy consumption assessment, avoiding the traditional crude method of approximating energy consumption based on distance or height difference, so that the final generated path is not only "theoretically feasible" but also has engineering executable value in terms of "energy consumption economy".

[0126] S3, using the improved greedy algorithm to plan the initial three-dimensional flight path; between the set flight start point and end point, the cost function used by the improved greedy algorithm is used to select the node with the minimum cost to form the initial three-dimensional flight path.

[0127] The greedy strategy prioritizes the direction with the lowest energy consumption and the smallest path inclination at the current stage, gradually advancing to generate a complete path sequence. The greedy strategy in this invention differs from the traditional greedy algorithm based on the shortest Euclidean distance. Its cost assessment considers the energy consumption estimates between nodes, the terrain undulation trend, and the overall directional continuity of the path. It also uses a window mechanism to optimize and control local paths.

[0128] In this embodiment, the terrain area is first gridded and divided into an N×M node network, where each node n p,q Each location on the map (p, q represents the row and column indices of the grid, respectively) contains three-dimensional coordinates (x, y, z) and terrain elevation information. Based on this, the adjacency extension relationship is defined as an "eight-direction" structure, meaning that each node is connected in up to eight directions: above, below, left, right, and four diagonal directions.

[0129] Set the starting point to S and the end point to G. Starting from the starting point, traverse the neighbor nodes of the current node in each step and calculate the cost function based on the following formula C ij Make your selection:

[0130] ;

[0131] in, E ij From the current node n i To neighboring nodes n j Energy consumption estimates; is the pitch angle change between the two nodes; λ is the angle penalty coefficient.

[0132] Energy consumption valuation ;

[0133] Pitch angle change ;

[0134] in, 、 Node n i and nodes n j The three-dimensional coordinates of .

[0135] In this embodiment, in order to highlight the impact of pitch angle changes and simplify the energy consumption calculation process, only the constant speed flight situation is considered, the flight speed is set to a constant value, and the rotor angular velocity and blade power are also given. n i To neighboring nodes n j , calculate the cost function of all adjacent nodes of the current node according to the above formula C ij For complex paths or real-time path generation, changes in flight speed, rotor angular velocity, blade power, etc. during the acceleration flight phase, deceleration flight phase, and constant speed flight phase can be taken into account.

[0136] Calculate the cost of all adjacent nodes C ij The node with the lowest cost is selected as the next hop node of the current path, and then the next round of search begins. This process is repeated until the search path reaches the end point G.

[0137] To further improve the flyability and structural smoothness of the path, the improved greedy algorithm also has a built-in path retraction and correction mechanism: if the pitch angle changes between all adjacent nodes of the current node exceed the set maximum flight pitch angle, the candidate position is determined based on the current node and the maximum flight pitch angle, and it is judged whether the candidate position intersects with the three-dimensional terrain model boundary of the task area constructed in step S1. If no intersection occurs, the candidate position is the next node position; if an intersection occurs, the first retraction operation is performed, specifically: retract from the historical path to the last node that meets the angle continuity, and re-search for feasible alternative path nodes in the neighborhood of the node according to the improved greedy algorithm, and re-select the alternative path branch.

[0138] Furthermore, considering that paths in mountainous environments may fall into local minimum cost traps, the improved greedy algorithm also introduces a local window strategy for local path optimization to avoid the problem of the path continuously deviating in the same direction and accidentally entering the local optimal dilemma. Specifically, the local window size is set to K (based on actual engineering experience, the window size needs to balance response speed and computational efficiency, here K=5), and the path between two adjacent nodes is considered a one-step path segment. After every K steps, the overall direction or overall posture of the K-step path segment is judged. If the overall direction deviates from the main direction of the flight path from the start to the end point or the overall posture becomes unstable, a second fallback operation is performed. If the overall direction of the K-step path segment does not deviate from the main direction and the overall posture is stable, the next K-step path segment is constructed.

[0139] The method for judging whether the overall direction of the K-step path segment has deviated from the main direction is to define the cosine similarity between the path direction in the local window and the global target direction from the start point to the end point. for:

[0140] ;

[0141] in, V l For the l The path direction vector of the step path segment, V goal is the global direction vector from the start point to the end point; if S dir < , it is judged that the overall direction of the K-step path segment deviates from the main direction, otherwise it is judged that the overall direction of the K-step path segment does not deviate from the main direction, is the direction consistency threshold; in this embodiment , the angle between the corresponding path direction and the main direction is about 53°.

[0142] The overall attitude instability judgment method of the K-step path segment is: calculate the absolute value and the pitch angle change in the local window :

[0143] ;

[0144] in, For the l Pitch angle changes of the step path segment; For the l +1 step pitch angle change of the path segment; if , it is determined that the overall posture of the K-step path segment is unstable, otherwise it is determined that the overall posture of the K-step path segment is stable. is the cumulative change threshold of the pitch angle. , to prevent the pitch angle from fluctuating violently within a single window.

[0145] The second fallback operation is implemented by falling back to the first node of the K-step path segment, re-searching for feasible alternative nodes in the neighborhood of that node using the improved greedy algorithm, constructing a new K-step path segment, and re-performing local path optimization. The second fallback operation also includes adjusting the angle penalty coefficient (for example, increasing it) to suppress single-directional drift.

[0146] Ultimately, this step outputs an initial three-dimensional path node sequence from the starting point S to the end point G, and satisfies the following three conditions: the energy consumption of each path segment is at a local optimum in the full minimum path structure; the pitch angle between any adjacent path segments does not exceed the set physical limit; and the path continuity, directional consistency, and flight feasibility all meet the subsequent screening conditions in step S4.

[0147] The initial path generated in this step provides a structural basis for subsequent feasibility checks and optimization smoothing processing, and is the key input path for achieving "flyable path" and "acceptable energy consumption".

[0148] S4, evaluating and optimizing the initial three-dimensional flight path to obtain an optimized three-dimensional flight path.

[0149] This step aims to assess the feasibility of path energy consumption and flight angle variations. After the initial path is generated, to ensure it meets the physical constraints of the drone and is executable, key path metrics are evaluated segment by segment. These include key parameters such as path segment energy consumption, continuous flight pitch angle variation, and overall path structure continuity. This information triggers a fallback reconstruction mechanism to eliminate infeasible path segments.

[0150] In this step, the initial three-dimensional flight path is traversed and each path segment is evaluated. When the current path segment meets one of the following conditions, a third fallback operation is performed to optimize the flight path:

[0151] (1) Take three consecutive nodes as a continuous path segment, and the angle change of any continuous path segment is Exceeding the pitch angle change threshold;

[0152] (2) The estimated cumulative energy consumption of the current three-dimensional flight path exceeds the UAV’s endurance limit (i.e., the UAV’s battery capacity);

[0153] (3) Multiple consecutive path segments have gradually increasing positive pitch angles, that is, steep slopes appear on the local path;

[0154] (4) Take three consecutive nodes as a continuous path segment, and the heading angle change of any continuous path segment is The heading stability threshold is exceeded, which means that the path direction continuity is destroyed.

[0155] For the above case (1), for any three consecutive points in the path 、 、 The two adjacent path segments are composed of two paths, each defining the pitch angle of the two path segments. and angle change as follows:

[0156] ;

[0157] ;

[0158] ;

[0159] Will The pitch angle change threshold allowed for the drone Compare; if , then it is judged that the current path has a risk of sudden change or flight instability, and the node To Node The path segment is marked as infeasible and triggers the third fallback operation; otherwise, the adjacent path segment is feasible and the evaluation continues along the path.

[0160] For the above situation (2), this step calls the path segment energy consumption evaluation model defined in step S2 to evaluate the energy consumption of all path segments of the three-dimensional flight path. Accumulate, if the accumulated energy consumption Exceeding the drone's endurance limit , and the path is also determined to be non-executable.

[0161] For the above situation (3), the five consecutive path segments are all gradually increasing positive pitch angles, that is, a steep slope appears on the local path; or the overall attitude instability judgment method of the previous K-step path segment is directly called.

[0162] For the above case (4), for any three consecutive points in the path 、 、 The horizontal projection direction vectors of the two adjacent path segments are calculated separately:

[0163] ;

[0164] ;

[0165] Calculate the angle between the two vectors, that is, the heading angle change:

[0166] ;

[0167] in, For nodes To Node The horizontal projection direction vector of the path segment; For nodes To Node The horizontal projection direction vector of the path segment; is the heading angle change between adjacent path segments; if , If the heading stability threshold is set, the path direction continuity is determined to be broken, and the node To Node The path segment is marked as infeasible and triggers the third fallback operation; otherwise, the adjacent path segment is feasible and the evaluation continues along the path.

[0168] The specific implementation of the third fallback operation is: fall back to the previous path node on the initial three-dimensional flight path ,in is the step length of the backoff window; in the neighborhood of this node, according to the improved greedy algorithm in the previous step S3, a new path branch is searched for feasible alternative path nodes again to construct a new path branch; the new branch is connected to the main path to form a new complete path sequence. The new complete path sequence is re-evaluated and optimized. If the backoff fails multiple times in a row, the current three-dimensional flight path is considered unable to reach the target point and the current round of planning is terminated; wait for the user to reset the starting and ending points or the intermediate points. In order to prevent the path generation efficiency from decreasing due to frequent backoff, this step introduces a backoff limit parameter , a path is allowed to execute at most Rollback and reconstruction operations are performed. If the number of rollback and reconstruction operations exceeds the limit, the path will be forcibly terminated and effective.

[0169] In addition, during the path structure adjustment process of the third fallback operation, when a new complete path sequence is formed, the energy consumption growth trend of the newly added path segment can also be judged. If the energy consumption of the newly added path segment is significantly higher than that of the original path, and the increase exceeds the set energy consumption ratio threshold, the angle penalty coefficient will be adjusted; by adjusting the angle penalty coefficient in the cost function , giving priority to the energy consumption control target and suppressing the impact of angle constraints on path deviation.

[0170] This step ensures the physical feasibility of the output path in terms of flight angle and energy consumption through a fallback mechanism. The resulting path sequence serves as input for subsequent smoothing and visualization verification, ensuring that subsequent stages will not interrupt mission execution due to sudden path changes or excessive energy consumption.

[0171] S5, smoothing the optimized three-dimensional flight path and outputting the final three-dimensional flight path.

[0172] This step uses a sliding window to smooth the optimized three-dimensional flight path. Specifically, the optimized three-dimensional flight path is smoothed by the mean filter method, assuming that the sliding window size is K'=5 and the sliding window step size is 5; the mean filter calculation formula is:

[0173] ;

[0174] Among them, among them, are the smoothed path node coordinates, is the original path node. The window range covers the two adjacent nodes before and after the current node. The mean operation is used to suppress local jitter while preserving the terrain feature trend.

[0175] When the optimized path meets the global pitch angle limit, energy consumption control range and continuity conditions, the final three-dimensional flight path data is output, including the path sequence, the position information of each node and the segmented flight status (climb, turn, level flight, etc.).

[0176] The above five steps constitute the overall technical process of this invention. From 3D terrain modeling, flight constraint setting, initial path generation, angle constraint backoff screening, to final path verification output, this method builds a complete system for 3D UAV path planning suitable for high-rise and complex terrain. Compared to existing path planning methods, this invention fully integrates the two key indicators of "flyability" and "optimal energy consumption." This method is particularly effective in addressing the difficulties faced by UAVs in large-angle deflection and the dramatic fluctuations in energy consumption in complex mountainous environments. It possesses significant practical application value and technical innovations, which will be explained in detail below.

[0177] In order to illustrate the significance of the effect of the present invention, the final three-dimensional flight path generated by the present invention is compared with the path generated by the traditional method (for each terrain point (x, y, z), along its normal direction Move vertically up 15 meters and generate a path composed of new points (x′, y′, z′) (this is the traditional path) for comparison. The results are as follows Figure 4 As shown in the figure, the path length, flight time, deviation angle, and total energy consumption are all superior to the traditional path. Its maximum vertical deviation is reduced by about 26.3%, significantly improving flight safety and endurance in complex terrain, and successfully avoiding steep terrain or abnormal slopes.

[0178] Therefore, the three-dimensional high-offset angle UAV path planning method based on the improved greedy algorithm provided by the present invention calculates the pitch angles between adjacent nodes according to the coordinate information of the path nodes in three-dimensional space; establishes an energy consumption model that combines factors such as aircraft attitude, rotor parameters, flight speed, and thrust-to-weight ratio; in the path planning process, the improved greedy algorithm is used to gradually generate a flight path, and in each step of path selection, the pitch angle and unit energy consumption of the current path segment are simultaneously evaluated, and the path with the lowest energy consumption that meets the pitch angle restriction conditions is preferentially selected among multiple candidate paths; when the pitch angle of the current path segment exceeds the preset maximum offset angle limit, the path height adjustment mechanism is triggered to reduce the angle; if the requirements are still not met after adjustment, the path fallback mechanism is activated, and the path is retraced to the last feasible node for re-path expansion; finally, a three-dimensional flight path that meets the aircraft hardware attitude restriction conditions, has strong path stability and low energy consumption is obtained. The path fallback mechanism manages the path history through the path state stack, recording the start and end nodes, pitch angle, energy consumption value and other information of each path segment. It is used to backtrack the path when the current path fails and avoid infinite loop search, thereby improving the global feasibility and convergence speed of the path planning algorithm.

[0179] The path planning method proposed in this invention can be used for both pre-mission global path generation and embedded in a flight control system for local path adjustment during flight. When a drone detects sudden changes in pitch angle, abnormal energy consumption, or dramatic terrain changes during flight, the system automatically triggers a local path reconstruction mechanism, re-evaluating the surrounding path at the current location and planning a new alternative path to ensure the safety and continuity of the flight mission. This invention implements a complete path optimization chain from terrain modeling, flight constraint setting, path generation, feasibility assessment, and final output. It offers excellent practicality, adaptability, and scalability, making it particularly suitable for typical mountain flight mission scenarios characterized by high terrain elevation and multiple obstacles.

[0180] The above are merely embodiments of the present invention and are not intended to limit the scope of the present invention. Any equivalent structures or equivalent process changes made using the contents of the present invention's description and drawings, or directly or indirectly applied to other related technical fields, are also included in the protection scope of the present invention.

Claims

1. A three-dimensional high-offset angle UAV path planning method based on an improved greedy algorithm, characterized in that: The following steps are involved: S1, constructing a 3D terrain model of the mission area: constructing the corresponding 3D terrain grid based on the digital elevation model data of the mission area; S2, constructing a path energy consumption evaluation model; the path energy consumption evaluation model includes energy consumption in the acceleration flight phase, energy consumption in the deceleration flight phase, and energy consumption in the constant speed flight phase; Energy consumption during acceleration flight phase E acc The calculation formula is: ； in, P acc (t) is the flight power during the acceleration phase P ; T acc To speed up the flight time; Energy consumption during deceleration flight phase E dec The calculation formula is: ； in, P dec (t) is the flight power during the deceleration flight phase P ; T dec The deceleration flight time; Energy consumption during constant speed flight E V The calculation formula is: ； in, P V Flight power during constant speed flight P ; T V is the constant speed flight time; Flight power P Calculated according to the following formula: ； in, P 0 is blade power; V is the flight speed; R is the rotor radius; is the rotor angular velocity; k is the thrust-to-weight ratio; W is the weight of the aircraft; F y It is the resultant thrust required by the aircraft in the pitch flight direction; is the air density; A is the rotor area; v 0 is the rotor induced speed; is the pitch angle of the flight path end point relative to the starting point; v The calculation formula is as follows: ； k The calculation formula is as follows: ； F y The calculation formula is as follows: ； in, C d is the drag coefficient of the aircraft in the windward direction; The calculation formula is as follows: ； in, 、 are the three-dimensional coordinates of the starting point and end point of the flight path respectively; S3, using the improved greedy algorithm to plan the initial three-dimensional flight path: between the set flight start and end points, the cost function used by the improved greedy algorithm is used to select the node with the minimum cost to form the initial three-dimensional flight path; The cost function used by the improved greedy algorithm C ij for: ； in, E ij From the current node n i To adjacent nodes n j Energy consumption estimates; is the pitch angle change between two adjacent nodes; λ is the angle penalty coefficient; Energy consumption valuation ; Pitch angle change ; in, 、 Node n i and nodes n j The three-dimensional coordinates of S4, evaluating and optimizing the initial three-dimensional flight path to obtain an optimized three-dimensional flight path.

2. The three-dimensional high-drift angle UAV path planning method based on the improved greedy algorithm according to claim 1 is characterized in that: In step S1, the task area is gridded with equal intervals based on the digital elevation model data of the task area.

3. The three-dimensional high-drift angle UAV path planning method based on the improved greedy algorithm according to claim 1 is characterized in that: If the pitch angle changes between all adjacent nodes of the current node exceed the set maximum flight pitch angle, a candidate position is determined based on the current node and the maximum flight pitch angle, and it is determined whether the candidate position intersects with the boundary of the three-dimensional terrain model of the task area constructed in step S1. If no intersection occurs, the candidate position is the next node position; if an intersection occurs, a first fallback operation is performed, specifically: fall back from the historical path to the last node that meets the angle continuity, and re-search for feasible alternative path nodes in the neighborhood of the node according to the improved greedy algorithm, and re-select the alternative path branch.

4. The three-dimensional high-drift angle UAV path planning method based on the improved greedy algorithm according to claim 1 is characterized in that: In step S3, a local window strategy is introduced to perform local path optimization; the local window size is K, and the path between two adjacent nodes is a one-step path segment; after every K steps, the overall direction or overall posture of the K-step path segment is judged. If the overall direction deviates from the main direction from the start point to the end point of the flight path or the overall posture is unstable, a second fallback operation is performed.

5. The three-dimensional high-drift angle UAV path planning method based on the improved greedy algorithm according to claim 4 is characterized in that: The method for judging whether the overall direction of a K-step path segment deviates from the main direction is to define the cosine similarity between the path direction in the local window and the global target direction from the start point to the end point. for: ； in, V l For the l The path direction vector of the step path segment, V goal is the global direction vector from the start point to the end point; if S dir < , it is judged that the overall direction of the K-step path segment deviates from the main direction, otherwise it is judged that the overall direction of the K-step path segment does not deviate from the main direction, is the direction consistency threshold; The overall attitude instability judgment method of the K-step path segment is: calculate the absolute value and the pitch angle change in the local window : ； in, For the l Pitch angle changes of the step path segment; For the l +1 step pitch angle change of the path segment; if , it is determined that the overall posture of the K-step path segment is unstable, otherwise it is determined that the overall posture of the K-step path segment is stable. The cumulative change threshold of the pitch angle.

6. The three-dimensional high-drift angle UAV path planning method based on the improved greedy algorithm according to claim 4 is characterized in that: The specific implementation method of the second fallback operation is: fall back to the first node of the K-step path segment, re-search for feasible alternative path nodes in the neighborhood of this node according to the improved greedy algorithm, build a new K-step path segment and re-perform local path optimization.

7. The three-dimensional high-drift angle UAV path planning method based on the improved greedy algorithm according to claim 6 is characterized in that: The second backoff operation also includes adjusting the angle penalty term coefficient.

8. The three-dimensional high-drift angle UAV path planning method based on the improved greedy algorithm according to claim 1 is characterized in that: In step S4, the initial three-dimensional flight path is traversed and each path segment is evaluated. When the current path segment meets one of the following conditions, a third fallback operation is performed to optimize the flight path: (1) Take three consecutive nodes as a continuous path segment, and the angle change of any continuous path segment is Exceeding the pitch angle change threshold; (2) The estimated cumulative energy consumption of the current three-dimensional flight path exceeds the UAV's endurance limit; (3) Multiple consecutive path segments have gradually increasing positive pitch angles; (4) Take three consecutive nodes as a continuous path segment, and the heading angle change of any continuous path segment is Heading stability threshold exceeded.

9. The three-dimensional high-drift angle UAV path planning method based on the improved greedy algorithm according to claim 8 is characterized in that: For case (1), for any three consecutive points in the path 、 、 The two adjacent path segments are composed of two paths, each defining the pitch angle of the two path segments. and angle change as follows: ； ； ； Will The pitch angle change threshold allowed for the drone Compare; if , then it is judged that the current path has a risk of sudden change or flight instability, and the node To Node The path segment is marked as infeasible and triggers the third fallback operation.

10. The three-dimensional high-drift angle UAV path planning method based on the improved greedy algorithm according to claim 8 is characterized in that: For case (4), for any three consecutive points in the path 、 、 The horizontal projection direction vectors of the two adjacent path segments are calculated separately: ； ； Calculate the angle between the two vectors, that is, the heading angle change: ； in, For nodes To Node The horizontal projection direction vector of the path segment; For nodes To Node The horizontal projection direction vector of the path segment; is the heading angle change between adjacent path segments; if , If the heading stability threshold is set, the path direction continuity is determined to be broken, and the node To Node The path segment is marked as infeasible and triggers the third fallback operation.

11. The three-dimensional high-drift angle UAV path planning method based on the improved greedy algorithm according to claim 8 is characterized in that: The specific implementation of the third fallback operation is: fall back to the previous path node on the initial three-dimensional flight path ,in is the step size of the backoff window; within the neighborhood of the node, re-search for feasible alternative path nodes according to the improved greedy algorithm in the previous step S3 to construct a new path branch; connect the new branch to the main path to form a new complete path sequence; re-evaluate and optimize the new complete path sequence.

12. The three-dimensional high-drift angle UAV path planning method based on the improved greedy algorithm according to claim 11 is characterized in that: In the third fallback operation, when a new complete path sequence is formed, the energy consumption growth trend of the newly added path segment is also judged; if the energy consumption of the newly added path segment is significantly higher than that of the original path, that is, the increase exceeds the set energy consumption ratio threshold, the angle penalty coefficient in the cost function is adjusted. .

13. The three-dimensional high-drift angle UAV path planning method based on the improved greedy algorithm according to any one of claims 1 to 12, characterized in that: Also includes: S5, smoothing the optimized three-dimensional flight path and outputting the final three-dimensional flight path.

14. The three-dimensional high-drift angle UAV path planning method based on the improved greedy algorithm according to claim 13 is characterized in that: The optimized three-dimensional flight path is smoothed by the mean filtering method. Assuming the sliding window size is K', the mean filtering calculation formula is: ； in, are the smoothed path node coordinates, is the original path node.

Images