Unmanned aerial vehicle three-dimensional route planning method and system based on terrain matching and threat source avoidance, and storage medium

Abstract

The invention discloses an unmanned aerial vehicle three-dimensional route planning method and system based on terrain matching and threat source avoidance, and a storage medium, and relates to the technical field of unmanned aerial vehicle route planning. The method comprises the steps of inputting an initial node and a target node; initializing a regional environment and a search tree, and inserting the initial node into the search tree; the regional environment comprises a planning region and a threat region; planning a random node range in the planning area and generating a random node, and determining a node closest to the random node from the search tree as a nearest node; expanding from the nearest node to the random node to generate a new node; judging whether a connecting line between the new node and the nearest node passes through the threat area, if so, regenerating a random node, and if not, inserting the new node into the search tree; and judging whether the new node is the target node, if so, backtracking to generate a route path, and if not, continuously generating a random node until the new node is the target node, and backtracking to generate the route path. According to the technical scheme, the calculation time can be obviously shortened, the route planning distance can be reduced, the requirements of rapid obstacle avoidance and safe flight of the unmanned aerial vehicle can be met, and the three-dimensional path planning method is reliable and effective.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicle (UAV) route planning, and in particular to a method, system and storage medium for three-dimensional UAV route planning based on terrain matching and threat source avoidance. Background Art

[0002] With the rapid development of artificial intelligence, control, and aerospace technologies, intelligent unmanned systems are appearing in large numbers in the highly competitive modern information warfare. As a crucial component of future warfare, drones are shouldering a crucial role. A key research direction in the field of unmanned systems is how to implement real-time route planning and automatic obstacle avoidance for drones in rapidly changing battlefield environments and complex, unknown environments. Drone route planning involves automatically planning an optimal route from a starting point to a destination with minimal total cost, taking into account various constraints, including battlefield radar and air defense threats, drone performance, flight distance, flight time, and energy consumption. Most existing drone route planning algorithms suffer from slow response times, overly simplified route models, and poor real-time performance, resulting in routes that fail to ensure safe flight. Therefore, implementing automatic obstacle avoidance route planning for drones in complex, unknown environments remains a significant challenge for current technologies.

[0003] Experts and scholars at home and abroad have conducted extensive research on UAV route planning and have proposed a series of route planning algorithms. For example, some experts have proposed an improved Rapidly Exploring Random Tree (RRT) algorithm, which incorporates dynamic step sizes and an adaptive target gravity optimization strategy to achieve real-time, rapid route planning in two-dimensional space. Some experts have considered combining the particle swarm optimization algorithm with the penguin search optimization algorithm for UAV reconnaissance applications. This algorithm is relatively simple to compute and has strong versatility, but its drawback is that it is prone to getting stuck in local optimal solutions. Furthermore, some scholars have proposed using the ant colony algorithm to solve route planning for automatic obstacle avoidance in unknown battlefield environments. Although this algorithm has strong versatility, it still suffers from slow search speed and even difficulty in obtaining the optimal route when the problem dimension is large. Therefore, newly designed routes need to possess real-time planning and automatic obstacle avoidance capabilities, which are crucial for ensuring safe flight of UAVs in uncertain environments, reducing threat factors, and improving mission execution efficiency.

[0004] Although the standard RRT UAV route planning algorithm is widely used and has a certain degree of autonomous route planning capabilities, it still has some problems in actual route planning applications:

[0005] (1) The generation of search tree nodes in the standard RRT algorithm is highly random, which causes the search tree to produce a large number of invalid nodes and invalid branches, resulting in a significant reduction in computational efficiency and a lengthy search path.

[0006] (2) The standard RRT algorithm generally determines the nearest point by calculating the Euclidean distance between a random point and a tree node. However, during flight, a UAV cannot make large-angle turns due to its own maneuvering conditions. The method of determining the nearest point only by Euclidean distance often results in the planned route not meeting the UAV flight maneuverability requirements.

[0007] (3) The standard RRT algorithm selects a fixed step size to expand new nodes, but this fixed step size search method generally does not meet actual needs and affects the node expansion speed. A larger step size makes it easier for the search tree to expand to the target point quickly, but it is easy to encounter obstacles. Although a smaller step size has strong flexibility and strong obstacle avoidance ability, it affects the calculation speed. Summary of the Invention

[0008] In response to the above problems, the purpose of the present invention is to improve and optimize the standard RRT algorithm based on a full understanding of the theory, and to propose a three-dimensional route planning method, system and storage medium for unmanned aerial vehicles based on terrain matching and threat source avoidance. By generating random nodes based on goal orientation and quadratic programming, the search tree expansion range and branch nodes are greatly reduced. At the same time, the search judgment strategy for the nearest node is optimized, redundant nodes are reduced, and the generated route is made smoother. In addition, the route planning speed of the algorithm is improved through an adaptive variable step size node expansion method. Therefore, the technical solution of the present invention can significantly shorten the calculation time, reduce the planned route distance, and meet the requirements of rapid obstacle avoidance and safe flight of unmanned aerial vehicles. It is a reliable and effective three-dimensional path planning method.

[0009] According to a first aspect of the technical solution of the present invention, a method for three-dimensional route planning of an unmanned aerial vehicle (UAV) based on terrain matching and threat source avoidance is provided, comprising the following steps:

[0010] S1: Input the initial node and target node;

[0011] S2: Initialize the regional environment and the search tree, and insert the initial node into the search tree; the regional environment includes the planning area and the threat area;

[0012] S3: planning a random node range in the planning area and generating a random node, and determining a node closest to the random node from the search tree as the nearest node;

[0013] S4: Expand from the nearest node toward the random node to generate a new node;

[0014] S5: Determine whether the line connecting the new node and the nearest node passes through the threat area, and if so, regenerate a random node; if not, insert the new node into the search tree;

[0015] S6: Determine whether the new node is the target node, if so, backtrack to generate the route path, if not, continue to generate random nodes until the new node is the target node, and backtrack to generate the route path.

[0016] Furthermore, in S3, planning a random node range within the planning area specifically includes:

[0017] The planning area is divided twice, and the range of random nodes is limited to a preset distance on both sides of the line connecting the starting node and the target node.

[0018] Furthermore, in S3, the specific method of generating random nodes is:

[0019] The random nodes are generated in an alternating generation manner, wherein a random node is first generated in a random manner, and then the target point is used as the next random node, and the alternating generation process is executed cyclically.

[0020] Furthermore, in S3, determining the node closest to the random node from the search tree as the nearest node specifically includes:

[0021] Define a route node as a seven-tuple:

[0022]

[0023] Among them, x, y, z represent the spatial coordinates of the route node, θ1 is the horizontal turning angle of the UAV, is the vertical climb angle or dive angle of the drone, id is the node number of the current node, and pre_id is the parent node number of the current node;

[0024] The shortest distance is expressed as:

[0025]

[0026] Where d is the Euclidean distance between two points, θ1 is the horizontal turning angle of the UAV, is the vertical climb angle or dive angle of the UAV, k1, k2, and k3 are the corresponding weight coefficients, and the value range is determined according to the degree of attention paid to the horizontal turning angle and the vertical climb angle.

[0027] Furthermore, the S3 further includes:

[0028] Determine whether the angle between the direction of the generated random node and the nearest node and the direction from the starting node to the target node is less than a preset threshold. If so, retain the random node; if greater than or equal to the preset threshold, regenerate a new random node.

[0029] Furthermore, it is determined whether the angle between the direction of the generated random node and the nearest node and the direction from the starting node to the target node is less than a preset threshold, specifically:

[0030] Assume that the generated random node is P rand , whose nearest node is P near , then from P near Towards P rand The vector is Determine the vector between the starting node and the target node Is the direction angle θ2 less than the preset threshold θ max , where θ max The value is determined according to the actual situation, preferably 45 degrees.

[0031] Furthermore, the S4 specifically includes:

[0032] Obtain a preset step size array, wherein the step size array contains multiple step size values of different sizes;

[0033] Selecting a maximum step value from the step array as an initial step, and expanding from the nearest node toward the random node to generate a candidate new node;

[0034] Determine whether the candidate new node meets the safety condition, if so, use the candidate new node as the new node, if not, select the next step value from the step array in descending order, and re-expand to generate a new candidate new node;

[0035] The above judgment and expansion process is executed cyclically until the new node that meets the safety condition is generated or all step values in the step array are traversed.

[0036] Furthermore, the step size array is [λ, 2λ, 3λ, ..., k max λ], λ is a fixed step size; preferably, k max The value range is greater than or equal to 3.

[0037] Furthermore, the safety condition is that no obstacles in the threat area are encountered.

[0038] According to a second aspect of the technical solution of the present invention, a three-dimensional route planning system for an unmanned aerial vehicle based on terrain matching and threat source avoidance is provided, the system comprising: a processor and a memory for storing executable instructions; wherein the processor is configured to execute the executable instructions to execute the three-dimensional route planning method for an unmanned aerial vehicle based on terrain matching and threat source avoidance as described in any of the above aspects.

[0039] According to a third aspect of the technical solution of the present invention, a computer-readable storage medium is provided, on which a computer program is stored, wherein when the computer program is executed by a processor, the method for three-dimensional route planning of a UAV based on terrain matching and threat source avoidance as described in any of the above aspects is implemented.

[0040] Beneficial effects of the present invention:

[0041] (1) The alternating generation of target-oriented random points is adopted to force the search tree to move toward the target point, thus accelerating the convergence of the algorithm. Secondary planning is performed on the threat area to reduce the expansion range of random nodes. The node expansion direction is optimized, node branches are reduced, and the calculation speed is improved.

[0042] (2) The standard RRT algorithm generally determines the nearest point by calculating the Euclidean distance between a random point and a tree node. The improved RRT algorithm of the technical solution of the present invention adds two constraint parameters, the horizontal turning angle and the vertical climb angle or dive angle of the drone, as the basis for searching and judging the nearest point, making the planned route path smoother and more in line with the requirements of the physical characteristics of drone flight.

[0043] (3) The node expansion method based on adaptive variable step size is used for node expansion, which greatly improves the adaptability and calculation speed of route planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the structures shown in these drawings without paying any creative work.

[0045] Figure 1 This is a flow chart of a three-dimensional route planning method for a UAV based on terrain matching and threat source avoidance according to the present invention;

[0046] Figure 2 This is a schematic diagram of secondary planning in the three-dimensional route planning method for unmanned aerial vehicles based on terrain matching and threat source avoidance of the present invention;

[0047] Figure 3This is a schematic diagram of the search tree expansion direction in the three-dimensional route planning method for unmanned aerial vehicles based on terrain matching and threat source avoidance of the present invention;

[0048] Figure 4 is a schematic diagram of a threat area in an embodiment of the present invention;

[0049] Figure 5 1 is a trajectory planning plan diagram of the standard RRT algorithm and the improved RRT algorithm of the present invention in an embodiment of the present invention;

[0050] Figure 6 It is the trajectory planning algorithm result of the standard RRT algorithm in the embodiment of the present invention and the improved RRT algorithm of the present invention.

[0051] The purpose, features and advantages of the present invention will be further described with reference to the accompanying drawings and in conjunction with the embodiments. DETAILED DESCRIPTION

[0052] Exemplary embodiments will be described in detail herein, with examples illustrated in the accompanying drawings. In the following description, when referring to the drawings, identical numerals in different figures represent identical or similar elements, unless otherwise indicated. The embodiments described in the following exemplary embodiments are not intended to represent all possible embodiments consistent with the present disclosure. Rather, they are merely examples of apparatus and methods consistent with certain aspects of the present disclosure, as detailed in the appended claims.

[0053] The terms "first," "second," and the like in the description and claims of the present disclosure are used to distinguish similar objects and are not necessarily used to describe a particular order or precedence. It should be understood that the terms used in this manner are interchangeable where appropriate, such that the embodiments of the present disclosure described herein can, for example, be implemented in orders other than those illustrated or described herein.

[0054] In addition, the terms "comprises" and "having" and any variations thereof are intended to cover a non-exclusive inclusion. For example, a process, method, system, product or apparatus that includes a series of steps or elements is not necessarily limited to those steps or elements expressly listed but may include other steps or elements not expressly listed or inherent to such process, method, product or apparatus.

[0055] Multiple includes two or more.

[0056] It should be understood that the term "and / or" used in this disclosure simply describes the relationship between associated objects, indicating that three possible relationships exist. For example, "A and / or B" can represent: A exists alone, A and B exist simultaneously, or B exists alone. This reduces manpower input and facilitates business automation, offering universal, efficient, and high-precision features.

[0057] The present invention proposes a random node generation strategy based on goal guidance and quadratic programming. The optimization is mainly reflected in: alternating generation of random points based on goal guidance; quadratic programming track interval to reduce node expansion range; node expansion direction optimization to reduce node branches.

[0058] (1) In the standard RRT algorithm, random nodes are randomly generated in the entire space, which has strong randomness and poor convergence. Therefore, an alternating method of generating random points can be adopted. That is, in the first step, random points are generated in a traditional way. In the second step, the target point is used as a random point. Then, random points are generated alternately to force the search tree to move toward the target point and accelerate the convergence of the algorithm.

[0059] (2) In the standard RRT algorithm, the area where nodes are generated is a large area. However, in the actual flight process of the UAV, due to the need to consider the length of the route, it is necessary to minimize large-scale detours as much as possible, generally within a certain horizontal distance d between the starting point and the target point. Therefore, this paper performs secondary planning on the planning area based on the scope of the actual threat area to reduce the invalid expansion range of random nodes, such as Figure 2 shown.

[0060] (3) In the standard RRT algorithm, the search tree will produce many useless branches, such as Figure 3 As we can see, if there is a vector from the starting point to the target point There are three general search tree expansion directions, which are related to vector acute angle Direction and vector Approximately right angles Direction and vector obtuse direction. It can be found that when the node is facing Direction and The search tree does not expand towards the target point, and more invalid branches are generated, which seriously affects the calculation speed. Therefore, it is necessary to limit the direction of random node generation. The restriction method in this paper is as follows:

[0061] Assume that the generated random point is P rand , whose nearest point is P near , then from P near Towards P rand The vector is Determine the vector from the starting point to the target point Is the direction angle θ2 less than a certain value θ max , that is, satisfying:

[0062]

[0063] Secondly, this paper optimizes the search and judgment strategy for the nearest node. In the standard RRT algorithm, the nearest point is generally determined by calculating the Euclidean distance between a random point and a tree node. However, during flight, a drone cannot make large-angle turns due to its own maneuvering conditions. Therefore, in order to make the planned route smoother and meet the requirements of the physical characteristics of drone flight, this paper optimizes the search and judgment of the nearest point, mainly from the following corrections:

[0064] In the traditional RRT algorithm, node parameters only include the three-dimensional spatial coordinates and the parent node. Considering the power constraints of the UAV, there are certain restrictions on up and down and left and right turns. Therefore, in order to make the UAV route smoother, this paper defines the route node as a seven-tuple:

[0065]

[0066] Among them, x, y, z represent the spatial coordinates of the route node, θ1 is the horizontal turning angle of the UAV, is the vertical climb angle or dive angle of the drone, id is the node number of the current node, and pre_id is the parent node number of the current node.

[0067] Therefore, the calculation of the shortest distance can be expressed as:

[0068]

[0069] Where d is the Euclidean distance between two points, θ1 is the horizontal turning angle of the UAV, is the vertical climb angle or dive angle of the UAV, k1, k2, and k3 are the corresponding weight coefficients, and the value range is determined according to the degree of attention paid to the horizontal turning angle and the vertical climb angle.

[0070] Finally, the present invention proposes a node expansion method with an adaptive variable step size, which improves the route planning speed of the algorithm. In the standard RRT algorithm, a fixed step size is used to expand new nodes, but this fixed step size search method generally does not meet actual needs. The size of the step size affects the expansion speed of the node. A larger step size makes it easy for the search tree to quickly expand to the target point, but it is easy to encounter obstacles. Although a smaller step size has strong flexibility and strong obstacle avoidance capabilities, it affects the calculation speed. Generally, a large step size is selected in areas with fewer obstacles and relatively open areas, and a smaller step size is selected in areas with dense obstacles. Therefore, in order to plan a safe and fast route, it is necessary to optimize the step size. This paper chooses to use an adaptive variable step size method for node expansion to improve the algorithm calculation speed while ensuring safety. The method is as follows:

[0071] (1) Set a fixed step size, defined as λ, and define a step size array [λ, 2λ, 3λ, ..., k maxλ];

[0072] (2) After generating random points and determining the nearest point, first follow the maximum step size k defined in the step size array max λ is expanded. If the new nodes generated by the expansion do not meet the requirements, they are decreased by a certain step size, that is, according to the following step sizes (k max -1)λ,(k max -2)λ,......,λ for cyclic expansion, otherwise select the current step size to expand the new node and terminate the loop.

[0073] Example

[0074] The present invention provides a three-dimensional route planning method for UAV based on terrain matching and threat source avoidance. The main steps of path search are:

[0075] (1) Input the initial point P0 and the target point P goal ;

[0076] (2) Initialize the regional environment and search tree, and insert the initial point P0 into the search tree;

[0077] (3) Secondary planning of random node range within the planning area, and then alternate generation of random points P in a certain order rand Or the target point P goal As a random point P rand , then traverse all nodes in the search tree to find the tree node P closest to the random node near ;

[0078] (4) Determine the random node P rand and the nearest node P near Whether the angle between the direction and the target direction meets the limit value θ max , if satisfied, go to step 5, otherwise, go to step 3.

[0079] (5) From the nearest node P near Towards The direction is expanded according to the adaptive variable step size λ to generate a new node P new ;

[0080] (6) Determine the generated new node P new and the nearest node P near Whether the line connecting passes through the threat area, if so, go to step 3, otherwise go to step 7;

[0081] (7) The new node P new Insert into the search tree and determine the new node P new Is it the target point P? goal , if yes, go to step 8, otherwise go to step 3;

[0082] (8) From the target point P goal Backtrack to the starting point P0 and add the backtracked nodes to the route path one by one.

[0083] In order to verify the effectiveness of the improved RRT algorithm in this paper, a simulation experiment was conducted on it. The simulation platform was based on Matlab2016b for relevant development and calculation. Since UAVs will inevitably encounter various obstacles during route planning, in order to make the planned route more in line with the actual application scenario, various threat sources that UAVs may encounter are modeled, mainly including terrain threats, radar threats, severe weather threats and no-fly zones, such as Figure 4 shown.

[0084] The three-dimensional planning area range is 100km, 100km, and 10km. The starting point of the drone is (10, 10, 0.1), and the target node is (95, 95, 1). The default unit is km. The weight coefficients of Euclidean distance, horizontal angle, and vertical angle are 1, 10, and 10 respectively. The fixed step size is 1km. The standard RRT algorithm and the improved RRT algorithm are used to plan the route in the same planning space. The planning results of the two algorithms are as follows: Figure 5 、 Figure 6 shown.

[0085] Depend on Figure 4 It can be seen that the search tree calculated by the standard RRT algorithm has many nodes, complex shapes, and large and long branch extensions. Some of the generated routes are not feasible. The search tree calculated by the improved RRT algorithm has greatly reduced nodes and few and short branches. Figure 6 It can be found that the route calculated by the improved RRT algorithm is smoother and can effectively avoid various threat areas.

[0086] In order to further quantitatively analyze the improved RRT algorithm, this paper chooses to conduct 10 simulation experiments and take the average value of 10 times for comparison. The quantitative result data statistics are shown in Table 1.

[0087] Table 1 Comparison of quantitative results of the two algorithms

[0088]

[0089] As shown in Table 1, the improved RRT algorithm can effectively reduce the scale of the search tree, accelerate algorithm convergence, improve calculation speed, and reduce flight route distance. By comparing the number of route nodes and flight distance, it can be found that the path nodes generated by the variable step size strategy are small and the path segments are long. Compared with the standard RRT algorithm, the calculation time of the improved RRT algorithm is about 260 times faster, and the route distance is shortened by 24.9%. This shows that the improved RRT algorithm is efficient and feasible.

[0090] In summary, the present invention provides a three-dimensional route planning method for unmanned aerial vehicles (UAVs) based on terrain matching and threat source avoidance. By making improvements based on the standard RRT algorithm, the method first optimizes the random node generation strategy based on goal orientation and quadratic programming to reduce the node expansion range and node branching. Secondly, the search and judgment strategy for the nearest node is optimized to make the UAV's route smoother. Then, a node expansion strategy based on adaptive variable step size is proposed. While ensuring safety, the algorithm calculation speed is improved. Finally, simulation experiments are conducted to analyze and verify the results. The results show that the improved RRT algorithm based on terrain matching and threat source avoidance can significantly shorten the calculation time, reduce the planned route distance, and meet the requirements of rapid obstacle avoidance and safe flight of UAVs. It is a reliable and effective three-dimensional path planning algorithm with certain feasibility.

[0091] It should be noted that, in this document, the terms "comprises," "includes," or any other variations thereof are intended to encompass non-exclusive inclusion, such that a process, method, article, or apparatus comprising a series of elements includes not only those elements but also other elements not explicitly listed, or elements inherent to such process, method, article, or apparatus. In the absence of further limitations, an element defined by the phrase "comprising a ..." does not exclude the presence of other identical elements in the process, method, article, or apparatus comprising the element.

[0092] The serial numbers of the above embodiments of the present invention are for description only and do not represent the advantages or disadvantages of the embodiments.

[0093] Through the description of the above embodiments, those skilled in the art can clearly understand that the above implementation method can be implemented by means of software plus the necessary general hardware platform, or of course by hardware, but in many cases the former is a better implementation method. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, can be embodied in the form of a software product. The computer software product is stored in a storage medium (such as ROM / RAM, magnetic disk, optical disk), and includes a number of instructions for enabling a terminal (which can be a mobile phone, computer, server, air conditioner, or network device, etc.) to execute the methods described in each embodiment of the present invention.

[0094] The embodiments of the present invention are described above in conjunction with the accompanying drawings, but the present invention is not limited to the above-mentioned specific implementation methods. The above-mentioned specific implementation methods are merely illustrative and not restrictive. Under the guidance of the present invention, ordinary technicians in this field can also make many forms without departing from the scope of protection of the present invention and the claims, all of which are protected by the present invention.

Claims

1. A three-dimensional route planning method for unmanned aerial vehicles based on terrain matching and threat source avoidance, characterized in that: The following steps are involved: S1: Input the initial node and target node; S2: Initialize the regional environment and the search tree, and insert the initial node into the search tree; the regional environment includes the planning area and the threat area; S3: planning a random node range in the planning area and generating a random node, and determining a node closest to the random node from the search tree as the nearest node; S4: Expand from the nearest node toward the random node to generate a new node; S5: Determine whether the line connecting the new node and the nearest node passes through the threat area, and if so, regenerate a random node; if not, insert the new node into the search tree; S6: Determine whether the new node is the target node, if so, backtrack to generate the route path, if not, continue to generate random nodes until the new node is the target node, and backtrack to generate the route path.

2. The method for three-dimensional route planning of a UAV according to claim 1, characterized in that: In S3, planning a random node range in the planning area specifically includes: The planning area is divided twice, and the range of random nodes is limited to a preset distance on both sides of the line connecting the starting node and the target node.

3. The method for three-dimensional route planning of a UAV according to claim 1, characterized in that: In S3, the specific method of generating random nodes is: The random nodes are generated in an alternating generation manner, wherein a random node is first generated in a random manner, and then the target node is used as the next random node, and the alternating generation process is executed cyclically.

4. The method for three-dimensional route planning of a UAV according to claim 1, characterized in that: In S3, determining the node closest to the random node from the search tree as the closest node specifically includes: Define a route node as a seven-tuple: Among them, x, y, z represent the spatial coordinates of the route node, θ1 is the horizontal turning angle of the UAV, is the vertical climb angle or dive angle of the drone, id is the node number of the current node, and pre_id is the parent node number of the current node; The shortest distance is expressed as: Where d is the Euclidean distance between two points, θ1 is the horizontal turning angle of the UAV, is the vertical climb angle or dive angle of the UAV, and k1, k2, and k2 are the corresponding weight coefficients.

5. The method for three-dimensional route planning of a UAV according to claim 1, characterized in that: Said S3 further comprises: Determine whether the angle between the direction of the generated random node and the nearest node and the direction from the starting node to the target node is less than a preset threshold. If so, retain the random node; if greater than or equal to the preset threshold, regenerate a new random node.

6. The method for three-dimensional route planning of a UAV according to claim 5, characterized in that: Determine whether the angle between the direction of the generated random node and the nearest node and the direction from the starting node to the target node is less than a preset threshold, specifically: Assume that the generated random node is P rand , whose nearest node is P near , then from P near Towards P rand The vector is Determine the vector between the starting node and the target node Is the direction angle θ2 less than the preset threshold θ max .

7. The method for three-dimensional route planning of a UAV according to claim 1, characterized in that: The S4 specifically includes: Obtain a preset step size array, wherein the step size array contains multiple step size values of different sizes; Selecting a maximum step value from the step array as an initial step, and expanding from the nearest node toward the random node to generate a candidate new node; Determine whether the candidate new node meets the safety condition, if so, use the candidate new node as the new node, if not, select the next step value from the step array in descending order, and re-expand to generate a new candidate new node; The above judgment and expansion process is executed cyclically until the new node that meets the safety condition is generated or all step values in the step array are traversed.

8. The method for three-dimensional route planning of a UAV according to claim 1, characterized in that: The step size array is [λ, 2λ, 3λ, ..., k max λ], λ is a fixed step size, k max The value range is greater than or equal to 3.

9. A three-dimensional route planning system for unmanned aerial vehicles based on terrain matching and threat source avoidance, the system comprising: A processor and a memory for storing executable instructions; characterized in that the processor is configured to execute the executable instructions to execute the three-dimensional route planning method for a drone based on terrain matching and threat source avoidance as described in any one of claims 1 to 8.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the method for three-dimensional route planning of a UAV based on terrain matching and threat source avoidance according to any one of claims 1 to 8 is implemented.

Images