Three-dimensional trajectory planning method based on bidirectional APF-RRT*-CPO algorithm

Abstract

The present invention relates to a three-dimensional trajectory planning method based on a bidirectional APF-RRT*-CPO algorithm, which includes the following steps: constructing a three-dimensional map, creating two random trees T1 and T2; adopting bidirectional search, generating sampling points on the random trees T1 and T2 respectively, finding the nodes closest to the sampling points, and using the potential field force of the adaptive APF algorithm to guide the generation of new nodes; re-wiring, finding the closest connected nodes, and adding the non-colliding nodes to the random trees; connecting all the nodes of the random trees to generate an initial trajectory; taking the trajectory points of the initial trajectory to involve the temporary starting point and the ending point, searching for the shortest distance between the nodes, calculating the total cost function, and generating an optimal trajectory according to the minimum cost position. The method of the present invention has a short trajectory search time and a fast convergence speed, and greatly speeds up the UAV trajectory planning in a complex three-dimensional environment.

Description

Technical Field

[0001] The invention relates to a three-dimensional track planning method based on a bidirectional APF-RRT*-CPO algorithm. Background Art

[0002] With the advancement of technology and the reduction of costs, the advantages of drones in the civilian field are becoming more and more prominent. For example, they are widely used in aerial photography, environmental monitoring, agricultural plant protection, power inspection, forest fire prevention, emergency communications, and drone logistics. The trajectory planning of drones is an important part of drone mission planning. Combining the performance indicators and characteristics of the drone itself, adopting certain trajectory planning methods, and formulating optimal or suboptimal paths are important guarantees for the safe flight of drones. Especially in complex mountainous environments, high-quality trajectories will greatly reduce the risk of drone damage, thereby improving the efficiency of drone missions. Therefore, the trajectory planning of drones in complex mountainous environments has important practical significance.

[0003] In the existing technology, traditional RRT* algorithm, Dijkstra algorithm, etc. will generate a large number of nodes in complex environments, with high computational complexity and long running time. Intelligent optimization algorithms such as genetic algorithm and ant colony algorithm are widely used in the trajectory planning of UAVs. However, in complex environments, intelligent optimization algorithms are prone to fall into local optimality and cannot accurately find a trajectory for safe flight of UAVs. It has certain limitations. Summary of the invention

[0004] The purpose of the present invention is to provide a three-dimensional trajectory planning method based on a bidirectional APF-RRT*-CPO algorithm, which can improve the trajectory planning search speed and trajectory feasibility of an unmanned aerial vehicle in a complex three-dimensional environment.

[0005] The present invention adopts the following technical solution:

[0006] A three-dimensional trajectory planning method based on a bidirectional APF-RRT*-CPO algorithm comprises the following steps:

[0007] (1) Build a 3D map and set the performance constraints of the drone; create two random trees T1 and T2;

[0008] (2) Use bidirectional search; random trees T1 and T2 generate sampling points q respectively rand1 ,q rand2 , and then find the q closest to the sampling point near1 ,q near2 , adaptive APF generates new node q new1 ,q new2 ;

[0009] (3) With the new node q new1 ,q new2As the center, re-find the distance to the new node q new1 ,q new2 A node with a smaller connection distance, if it exists and is connected to the new node q new1 ,q new2 If there is no obstacle between the two connections, q new1 Add T1 and change q new2 Add T2; otherwise, do not change the original track and continue to execute downward;

[0010] (4) Determine whether there are directly connected nodes in T1 and T2. If so, continue with step (5). If not, repeat step (2).

[0011] (5) Connect all nodes in T1 and T2 to obtain the initial track;

[0012] (6) Initialize CPO algorithm parameters;

[0013] (7) Set the temporary starting point and end point based on the initial track point, use the position update formula to update the track point, and find the shortest distance between the two nodes;

[0014] (8) Calculate the total cost function to calculate the total cost of the UAV track and find the track with the minimum cost;

[0015] (9) Based on the minimum cost position information, connect and generate the optimal trajectory.

[0016] Furthermore, in step (1), T1 is q start As the root node, T2 is q goal is the root node and marks the index of the root node.

[0017] Furthermore, in step (2), when node q near1 When in an obstacle-free area, the target node q is used goal For node q near1 The gravitational force F att1 (q near1 ) Guide the growth direction of tree T1 to generate a new node q new1 .

[0018]

[0019] in, is the scale factor, Represents node q near1 To the target point q goal The Euclidean distance.

[0020] When node q near1 When in an obstacle area, use the target point q goal , sampling point q rand1 For node qnear1 The gravitational force F att2 (q near1 ) and the obstacle center point q obs For node q near1 Generate repulsive force F repi (q near1 ) of the resultant force F(q near1 ) to guide the growth direction of tree T1 to generate new node q new1 .

[0021]

[0022]

[0023]

[0024] in, is the scale factor, Represents node q near1 To the sampling point q rand1 The Euclidean distance of For node q near1 To obstacles The Euclidean distance of is the repulsive force scale factor; Obstacle node The radius of influence; is node q near1 To the obstacle q obsi Direction vector, i=1,2,…,m, where m is the number of obstacles.

[0025] Furthermore, in step (2), when node q near2 When in an obstacle-free area, use the starting point q start For node q near2 The gravitational force F att1 (q near2 ) Guide the growth direction of tree T2 to generate a new node q new2 ;

[0026]

[0027] in, Represents node q near2 To the starting point start The Euclidean distance.

[0028] When node q near2 When in an obstacle area, use the starting point q start , sampling point q rand2 For node q near2 The gravitational force F att2 (q near2) and the obstacle center point q obs For node q near2 Generate repulsive force F repi (q near2 ) of the resultant force F(q near2 ) to guide the growth direction of tree T2 to generate a new node q new2 ;

[0029]

[0030]

[0031]

[0032] in, Represents node q near2 To the sampling point q rand2 The Euclidean distance of For node qnear1 to the obstacle The Euclidean distance of is the repulsive force scale factor; Obstacle node The radius of influence; is node q near2 To the obstacle q obsi Direction vector.

[0033] Furthermore, in step (3), the obstacle detection method is: divide the node-to-node line into n+1 coordinate points, determine the Euclidean distance between the coordinate point and the obstacle center coordinate point, if this distance is less than the radius of the obstacle, that is, there is an obstacle between the nodes; if this distance is greater than the radius of the obstacle, there is no obstacle between the nodes.

[0034] Furthermore, in step (7), it is first determined whether the algorithm is in the exploration phase or the development phase, and then the position update formula is determined.

[0035] The method for determining whether the algorithm is in the exploration stage or the development stage is: first generate two random values ​​r1 and r2 between 0 and 1; if r1 is less than r2, the algorithm enters the algorithm exploration stage; if r1 is greater than r2, the algorithm enters the algorithm development stage.

[0036] Furthermore, when the algorithm enters the algorithm exploration phase, two random values ​​r3 and r4 between 0 and 1 are generated again; if r3 is less than r4, the position update adopts the position update formula 1; if r3 is greater than r4, the position update adopts the position update formula 2.

[0037] Position update formula 1:

[0038]

[0039]

[0040]

[0041] in, is the t-th iteration position of the i-th population individual, rand is a random value between 0 and 1, is the best individual position, is the position of a random individual in the population; i and is an integer random value greater than or equal to 1 and less than or equal to N; F is the inverse cumulative distribution number of the Cauchy distribution, p is the probability value between 0 and 1; γ is the setting parameter.

[0042] Position update formula 2:

[0043]

[0044]

[0045]

[0046] in, is the t-th iteration position of the i-th population individual; t is the number of iterations; rand is a random value between 0 and 1, is a binary vector, , , are the positions of three different random individuals in the population; popr, popr1, and popr2 are respectively integer random values ​​in the population that are greater than or equal to 1 and less than or equal to N; u and v are random numbers that obey the normal distribution.

[0047] Furthermore, when the algorithm enters the algorithm development stage, a custom threshold between 0 and 1 is set, and a random value r5 between 0 and 1 is generated again; if r5 is less than the custom threshold, the position update formula adopts position update formula 3; if r5 is greater than the custom threshold, the position update formula adopts position update formula 4.

[0048] Position update formula 3:

[0049]

[0050]

[0051] .

[0052] Position update formula 4:

[0053]

[0054]

[0055]

[0056] in, is the convergence speed factor, is the best individual position, is the position of a random individual in the population, is the total cost function value.

[0057] Furthermore, the total cost function in step (8) is:

[0058]

[0059] in, are the penalty coefficients for track distance, turning angle, pitch angle and mountain threat respectively; n is the number of path points; L i is the track distance cost function; ψ i is the maximum turning angle cost function; θ i is the maximum pitch angle cost function; h i Constrained by the threat of mountains.

[0060] The beneficial effects of the present invention are as follows: the present invention uses the improved CPO algorithm to solve the problem that the track obtained by the RRT* algorithm is not smooth and has a long distance. The combination of algorithms can find the track faster and more accurately. The track search time is short and the convergence speed is fast. Compared with other algorithms in complex three-dimensional environments, it has great advantages and greatly speeds up the speed of UAV track planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] Figure 1 Flow chart of the method of the present invention.

[0062] Figure 2 This is a simulation diagram of the method of the present invention in a complex three-dimensional map environment.

[0063] Figure 3 This is a simulation diagram of the RRT algorithm in a complex three-dimensional map environment.

[0064] Figure 4 This is a simulation diagram of the RRT* algorithm in a complex three-dimensional map environment.

[0065] Figure 5 This is a simulation diagram of the APF-RRT* algorithm in a complex three-dimensional map environment. DETAILED DESCRIPTION

[0066] The following will be combined with the accompanying drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. It should be noted that the protection scope of the present invention is not limited to these embodiments. Any changes or equivalent substitutions that do not deviate from the concept of the present invention are included in the protection scope of the present invention.

[0067] The method flow chart of the present invention is shown in Figure 1.

[0068] Step 1: Build a 3D map, set up a complex obstacle environment, set up the performance constraints of the drone, and set the starting point of the drone. start , target point q goal . Set the algorithm step size L S .

[0069] The performance constraints of the UAV include the UAV's track distance, maximum turning angle, maximum pitch angle constraints, and mountain threat constraints.

[0070] Step 2: Create two random trees T1 and T2. T1 is based on q start As the root node, T2 is q goal is the root node and marks the index of the root node.

[0071] Step 3: Bidirectional search. Tree T1 randomly generates sampling points q rand1 , select the distance q in tree T1 rand1 The nearest node q near1 , the initial node q near1 for q start ; Tree T2 randomly generates random sampling points q rand2 , select the distance q in tree T2 rand2 The nearest node q near2 , the initial node q near2 for q goal ;q near is the distance from the sampling point q rand The nearest node.

[0072] Step 4: For node q near1 ,q near2 The nearby environment is judged, when the node q near1 or q near2 When in an obstacle-free area, eliminate sampling points q rand1 ,q rand2 For node q near1 ,q near2 The gravitational force generated is the target node q goal For node q near1 The generated gravity guidance tree T1 is based on the given step size L S Grow and generate new node q new1 , the direction of gravity points to the target point qgoal . Using the starting point q start For node q near2 The resulting gravitational guided tree T2 grows to generate a new node q according to the given step size new2 , the direction of gravity points to the starting point q start .

[0073] The gravitational formula is:

[0074]

[0075] In the formula, is the scale factor, Represents node q near1 To the target point q goal The Euclidean distance of Represents node q near2 To the starting point start The Euclidean distance.

[0076] When node q near1 ,q near2 When in an obstacle area, use the target point q goal , sampling point q rand1 For node q near1 The gravitational force generated and the obstacle center point q obs For node q near1 Generate a repulsive force to guide the growth direction of tree T1, using the starting point q start , sampling point q rand2 For node q near2 The gravitational force generated and the obstacle center point q obs For node q near2 Generate a repulsive force to guide the growth direction of tree T2, according to the given step length L s Grow along the direction of the combined force to generate a new node q new1 and q new2 , i=1,2,…,m, m is the number of obstacles.

[0077] Sampling point q rand1 For node q near1 The gravitational function that produces gravity is:

[0078]

[0079] In the formula, is the scale factor, Represents node q near1 To the sampling point q rand1 The Euclidean distance.

[0080] obstacle For node q nearThe repulsive force that produces the repulsive force is:

[0081]

[0082] In the formula, For node q near1 To obstacles The Euclidean distance of is the repulsive force scale factor; Obstacle node The radius of influence; is node q near1 To the obstacle q obsi Direction vector.

[0083] When node q near1 In an accessible area, i.e. When , the resultant force is: ;

[0084] When node q near1 In the obstacle area, When , the resultant force is: ;

[0085] m is the number of obstacles.

[0086] Sampling point q rand2 For node q near2 The gravitational function that produces gravity is:

[0087]

[0088] In the formula, is the scale factor, Represents node q near2 To the sampling point q rand2 The Euclidean distance.

[0089] obstacle For node q near The repulsive force that produces the repulsive force is:

[0090]

[0091] In the formula, For node q near1 To obstacles The Euclidean distance of is the repulsive force scale factor; Obstacle node The radius of influence; is node q near2 To the obstacle q obsi Direction vector; i=1,2,…,m, where m is the number of obstacles.

[0092] When node q near2 In an accessible area, i.e. When , the resultant force is: ;

[0093] When the node qnear2 is in the obstacle area, that is, When , the resultant force is: ;

[0094] m is the number of obstacles.

[0095] Step 5: Take the new node q new1 ,q new2 Re-search for a node with a smaller distance to the center. If such a node exists, and the new node q new1 ,q new2 If there is no obstacle between them, a new track will be established to connect point q new1 ,q new2 Add them to trees T1 and T2 respectively, otherwise the original track will not be changed and execution will continue downward.

[0096] Step 6: Determine whether there are nodes in the random trees T1 and T2 that can be directly connected (the distance between the two points is less than a set fixed value, and there are no obstacles between the connecting lines). If so, execute step 7; otherwise, return to step 3 for the next iteration.

[0097] The obstacle detection method is as follows: the obstacle detection divides the nodes and the lines connecting the nodes into n+1 coordinate points, and determines the Euclidean distance between the coordinate points and the coordinate point of the obstacle center. If the distance is less than the radius of the obstacle, there is an obstacle between the nodes; if the distance is greater than the radius of the obstacle, there is no obstacle between the nodes.

[0098] Step 7: Record all nodes in trees T1 and T2 as , and connect them to get an initial track. As a starting point, is the target point, is the initial path node.

[0099] Step 8: Initialize CPO algorithm parameters. Initialize the algorithm population N, the maximum number of iterations t max .

[0100] Step 9: Take path point q start is the temporary starting point of the algorithm optimization, and q2 is the temporary end point. start The CPO algorithm with an improved position update formula is used to update the position of the track points between ~q2 (i.e., a certain number of crested porcupine populations are generated and the positions of these populations are updated), and the best position is recorded to find the shortest distance between the two nodes.

[0101] The position update is performed using the following position update formula. First, it is determined that a certain stage of the algorithm has been entered, and then the corresponding position update formula is used to update the individual positions of the population, record the position information of the track points, and complete an iterative update of the track.

[0102] (a) Determine the algorithm is at a certain stage

[0103] First, generate two random values ​​r1 and r2 between 0 and 1, and then determine whether r1 is less than r2. If r1 is less than r2, the algorithm enters the algorithm exploration stage; if r1 is greater than r2, the algorithm enters the algorithm development stage.

[0104] (b) Selection of position update formula

[0105] I) Algorithm Exploration Phase

[0106] When the algorithm enters the algorithm exploration phase, two random values ​​r3 and r4 between 0 and 1 are generated again, and then it is determined whether r3 is less than r4. If r3 is less than r4, the position update adopts the position update formula 1, and if r3 is greater than r4, the position update adopts the position update formula 2.

[0107] Position update formula 1

[0108] The improved position update formula combined with the inverse cumulative distribution function of the Cauchy distribution is:

[0109]

[0110]

[0111]

[0112] in, is the t-th iteration position of the i-th population individual, rand is a random value between 0 and 1, is the best individual position, is the position of a random individual in the population. i and is an integer random value greater than or equal to 1 and less than or equal to N. F is the inverse cumulative distribution number of the Cauchy distribution, and p is the probability value between 0 and 1. γ is the setting parameter of 0.3.

[0113] Position update formula 2

[0114] The improved position update formula combined with Levy flight strategy is:

[0115]

[0116]

[0117]

[0118] in, is the t-th iteration position of the i-th individual in the population. t is the number of iterations. rand is a random value between 0 and 1. is a binary vector, , , are the positions of three different random individuals in the population. popr, popr1, and popr2 are integer random values ​​in the population that are greater than or equal to 1 and less than or equal to N. u and v are random numbers that follow a normal distribution, and β is 1.5.

[0119] II) Algorithm Development Phase

[0120] When the algorithm enters the algorithm development stage, a custom threshold (a random value between 0 and 1) is set, and a random value r5 is generated again between 0 and 1. The relationship between r5 and the custom threshold is determined: if r5 is less than the custom threshold, the position update formula uses position update formula 3; if r5 is greater than the custom threshold, the position update formula uses position update formula 4.

[0121] Position update formula 3:

[0122]

[0123]

[0124]

[0125] in, is the position of the ith individual at iteration t, rand is a random value between 0 and 1, popr1, popr2, and popr3 are integer random values ​​in the population that are greater than or equal to 1 and less than or equal to N, is a binary vector. t is the number of iterations, t max is the maximum number of iterations.

[0126] Position update formula 4:

[0127]

[0128]

[0129]

[0130] in, is the convergence speed factor. rand is a random value between 0 and 1. is the best individual position, is the position of a random individual in the population, is the position of the i-th individual at iteration t. is the total cost function value, t is the current iteration number, t max is the maximum number of iterations.

[0131] Step 10: Calculate the total cost of the drone’s track based on the recorded individual position information and the total cost function, and find the track with the minimum cost (the cost includes the drone’s track distance, maximum turning angle, maximum pitch angle, and collision cost). Record the position information with the minimum cost.

[0132] Track distance cost function:

[0133]

[0134] in, are the coordinate values ​​of two adjacent path points. To set the value yourself.

[0135] Maximum pitch angle cost function:

[0136]

[0137] in, are the coordinate values ​​of two adjacent path points. is the set maximum pitch angle.

[0138] Maximum turning angle cost function:

[0139]

[0140] in, , It is the projection of the i-th and i+1-th tracks on the horizontal plane. is the maximum turning angle set.

[0141] The planned trajectory should avoid crossing mountain obstacles. Therefore, if the generated trajectory is lower than the safe flight altitude above the corresponding terrain, the trajectory will be penalized to improve the safety of the drone. The mountain threat constraint function is described using the following formula:

[0142] Mountain Threat Constraints:

[0143]

[0144] In the formula, Used to indicate the threat level of mountains to drones. Represents the preset minimum safe flight altitude. Represents the degree of penalty for the track point. Indicates track point At terrain height. are the track point coordinates.

[0145] Total cost function:

[0146]

[0147] in, They are the penalty coefficients for track distance, turning angle, pitch angle and mountain threat respectively. . n is the number of path points. The smaller the total cost function value, the better.

[0148] Step 11: Connect the shortest path found. The node on the shortest path is used as the temporary starting point, and the next node q3 is used as the temporary end point, and continue to search for q start ~q3, and connect the shortest path. And so on, the next node of the temporary end point is taken as the temporary end point again, and finally the temporary end point is set at the real target point q goal , find the optimal trajectory.

[0149] Calculation Example

[0150] The complex environment of this experiment is a mountainous environment, and the obstacle is a mountain. The drone needs to avoid the mountain obstacle and reach the target point. The starting coordinates are set to [0,0,0], the end coordinates are set to [100,100,5], and the step length L required by the algorithm S =5km, the maximum range of the drone is 200km, and the yaw angle and pitch angle constraints of the drone are both [0°-40°]. The map uses a spatial range of [100km, 100km, 10km]. Initialize the algorithm population N=100, and the maximum number of iterations t max =300. =0.3, =0.2, =0.2, =0.3. The minimum safe flight altitude is +0.01km, Indicates track point At terrain height. = 10000. The threshold value is customized in the development stage as 0.7. The simulation results of the three-dimensional trajectory planning method for unmanned aerial vehicles provided by the present invention in a complex three-dimensional map environment are as follows: Figure 2 shown.

[0151] Comparative Example

[0152] According to the conditions given in the calculation example, the RRT algorithm, RRT* algorithm and APF-RRT* algorithm are used to perform simulation calculations respectively. The results are as follows: Figure 3~Figure 5By comparing the simulation diagrams, it can be seen that the track of the present invention is the smoothest, has no bends, and has the shortest distance, which meets the performance constraints of the drone. The generated track is more feasible.

[0153] Table 1 shows the average values ​​of 50 simulation data of four different algorithms in a complex three-dimensional map environment, including the average track length, the average number of iterations and the path generation time. It can be seen that the present invention greatly shortens the path generation time, and the path generation time is 0.38s. Compared with the traditional RRT algorithm, the path generation time is reduced by 94.30%, compared with the traditional RRT* algorithm, the path generation time is reduced by 91.12%, and compared with the APF-RRT* algorithm, the path generation time is reduced by 76.39%. It is more real-time.

[0154] Table 1 Comparison results of four algorithms

[0155]

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

Claims

1. A three-dimensional trajectory planning method based on a bidirectional APF-RRT*-CPO algorithm, characterized in that: It includes the following steps: (1) Build a 3D map and set the performance constraints of the UAV; create two random trees T1 and T2; T1 with q start As the root node, T2 is q goal is the root node, and marks the index for the root node; (2) Using bidirectional search; random trees T1 and T2 generate sampling points q respectively rand1 ,q rand2 , and then find the q closest to the sampling point near1 ,q near2 , adaptive APF generates new node q new1 ,q new2 ; When node q near1 When in an obstacle-free area, the target node q is used goal For node q near1 The gravitational force F att1 (q near1 ) Guide the growth direction of tree T1 to generate a new node q new1 ; F att1 (q near1 )=k a ρ(q near1 ,q goal ) Among them, k a is the scale factor, ρ(q near1 ,q goal ) represents node q near1 To the target point q goal The Euclidean distance of When node q near1 When in an obstacle area, use the target point q goal , sampling point q rand1 For node q near1 The gravitational force F att2 (q near1 ) and the obstacle center point q obs For node q near1 Generate repulsive force F repi (q near1 ) of the resultant force F(q near1 ) to guide the growth direction of tree T1 to generate a new node q new1 ; F att2 (q near1 )=k b ρ(q near1 ,q rand1 ) Among them, k b is the scale factor, ρ(q near1 ,q rand1 ) represents node q near1 To the sampling point q rand1 The Euclidean distance of near1 ,q obsi ) is the node q near1 To the obstacle q obsi The Euclidean distance of rep is the repulsion scale factor; ρ0 is the obstacle node q obsi The radius of influence; is node q near1 To the obstacle q obsi Direction vector, i = 1, 2, ..., m, where m is the number of obstacles; When node q near2 When in an obstacle-free area, use the starting point q start For node q near2 The gravitational force F att1 (q near2 ) Guide the growth direction of tree T2 to generate a new node q new2 ; F att1 (q near2 )=k a ρ(q near2 ,q start ) Among them, ρ(q near2 ,q start ) represents node q near2 To the starting point start The Euclidean distance of When node q near2 When in an obstacle area, use the starting point q start , sampling point q rand2 For node q near2 The gravitational force F att2 (q near2 ) and the obstacle center point q obs For node q near2 Generate repulsive force F repi (q near2 ) of the resultant force F(q near2 ) to guide the growth direction of tree T2 to generate a new node q new2 ; F att2 (q near2 )=k b ρ(q near2 ,q rand2 ) Among them, ρ(q near2 ,q rand2 ) represents node q near2 To the sampling point q rand2 The Euclidean distance of near2 ,q obsi ) is the distance from node qnear1 to obstacle q obsi The Euclidean distance of rep is the repulsion scale factor; ρ0 is the obstacle node q obsi The radius of influence; is node q near2 To the obstacle q obsi Direction vector; (3) With the new node q new1 ,q new2 As the center, re-find the distance to the new node q new1 ,q new2 A node with a smaller connection distance, if it exists and is connected to the new node q new1 ,q new2 If there is no obstacle between the two connections, then q new1 Add T1 and change q new2 Add T2; otherwise, do not change the original track and continue to execute downward; (4) Determine whether there are directly connected nodes in T1 and T2. If yes, proceed to step (5). If not, repeat step (2). (5) Connect all nodes in T1 and T2 to obtain the initial track; (6) Initialize CPO algorithm parameters; (7) Set the temporary starting point and end point with the track points of the initial track, update the track points using the position update formula, and find the shortest distance between the two nodes; (8) Calculate the total cost function to calculate the total cost of the UAV track and find the track with the minimum cost; (9) Based on the minimum cost position information, connect and generate the optimal trajectory.

2. A three-dimensional trajectory planning method based on a bidirectional APF-RRT*-CPO algorithm according to claim 1, characterized in that: In step (3), the obstacle detection method is: divide the node and the node connection into n+1 coordinate points, determine the Euclidean distance between the coordinate point and the obstacle center coordinate point, if this distance is less than the radius of the obstacle, that is, there is an obstacle between the nodes; if this distance is greater than the radius of the obstacle, there is no obstacle between the nodes.

3. A three-dimensional trajectory planning method based on a bidirectional APF-RRT*-CPO algorithm according to claim 2, characterized in that: In step (7), first determine whether the algorithm is in the exploration stage or the development stage, and then determine the position update formula; The method for determining whether the algorithm is in the exploration stage or the development stage is: first generate two random values ​​r1 and r2 between 0 and 1; if r1 is less than r2, the algorithm enters the algorithm exploration stage; if r1 is greater than r2, the algorithm enters the algorithm development stage.

4. A three-dimensional trajectory planning method based on a bidirectional APF-RRT*-CPO algorithm according to claim 3, characterized in that: When the algorithm enters the algorithm exploration phase, two random values ​​r3 and r4 between 0 and 1 are generated again; if r3 is less than r4, the position update adopts the position update formula 1; if r3 is greater than r4, the position update adopts the position update formula 2; Position update formula 1: in, is the t-th iteration position of the i-th population individual, rand is a random value between 0 and 1, is the best individual position, is the position of a random individual in the population; i and popr are integer random values ​​greater than or equal to 1 and less than or equal to N; F is the inverse cumulative distribution number of the Cauchy distribution, p is the probability value between 0 and 1; γ is the setting parameter; Position update formula 2: s=u / |v| 1 / β in, is the t-th iteration position of the i-th population individual; t is the number of iterations; rand is a random value between 0 and 1, U1 is a binary vector, are the positions of three different random individuals in the population; popr, popr1, and popr2 are respectively integer random values ​​in the population that are greater than or equal to 1 and less than or equal to N; u and v are random numbers that obey the normal distribution.

5. A three-dimensional trajectory planning method based on a bidirectional APF-RRT*-CPO algorithm according to claim 4, characterized in that: When the algorithm enters the algorithm development stage, a custom threshold between 0 and 1 is set, and a random value r5 between 0 and 1 is generated again; if r5 is less than the custom threshold, the position update formula adopts the position update formula 3; if r5 is greater than the custom threshold, the position update formula adopts the position update formula 4; Position update formula 3: Position update formula 4: Among them, α is the convergence speed factor, is the best individual position, is the position of a random individual in the population, and f(·) is the total cost function value.

6. A three-dimensional trajectory planning method based on a bidirectional APF-RRT*-CPO algorithm according to claim 5, characterized in that: The total cost function in step (8) is: Total cost function: Among them, ω1, ω2, ω3, ω4 are the penalty coefficients of track distance, turning angle, pitch angle and mountain threat respectively; n is the number of path points; L i is the track distance cost function; ψ i is the maximum turning angle cost function; θ i is the maximum pitch angle cost function; h i Constrained by the threat of mountains.

Images