An unmanned aerial vehicle path planning method based on potential force guided BQ-RRT* algorithm

Abstract

The application discloses a UAV path planning method based on potential force guiding BQ-RRT* algorithm and relates to the technical field of UAV decision-making. In the application, a reselection parent node mode with a depth of 2 is introduced in the process of reselecting a parent node of an RRT* algorithm, the range of reselecting a parent node is enlarged, a parent node outside a circle domain is considered as a parent node of a new node, the path cost is reduced, and a smoother trajectory is planned; an end point and a random point respectively attract a random tree, an obstacle closest to the random tree repels the random tree, and a new node grows along the direction of the combined force of the three forces, and the improved potential force is used to reduce the iteration times; random trees are respectively established at a start point and the end point, the two random trees grow towards each other, and a bidirectional search mode shortens the convergence time.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) decision-making technology, and in particular to a UAV trajectory planning method based on the potential force-guided BQ-RRT* algorithm. Background Technology

[0002] Path planning is a crucial component of UAV technology. It refers to planning a collision-free flight path from the start point to the destination, taking into account the timeliness of UAVs. However, due to the inherent performance limitations of UAVs and numerous constraints in path planning, including local radar threats, flight path costs, runtime, and iteration counts, successfully planning a flight path that evades radar threats and completes the intended mission presents new challenges. Currently, UAV flight environments are more complex. If path planning algorithms cannot promptly plan a flight path with superior performance indicators, the UAV may be detected by enemy radar, leading to its attack. To improve the planning performance of path planning algorithms and enable UAVs to meet real-time requirements, it is generally necessary for UAVs to quickly plan their optimal flight path. This necessitates more efficient path planning algorithms. Therefore, research on path planning plays a crucial role, and sampling-based path planning algorithms are of significant importance due to their speed in various environments. However, the paths planned by these algorithms are highly random and lack asymptotic optimization. To address the shortcomings of the RRT algorithm, the Approximate Optimal Fast Search Random Tree (RRT*) algorithm is proposed. Compared to the RRT algorithm, RRT* adds a process of reselecting the parent node and rewiring after the algorithm generates a new node. When the number of iterations approaches infinity, RRT* will definitely find the optimal solution, but it has the problems of long running time and high track cost. Summary of the Invention

[0003] To address the shortcomings of existing technologies, this invention proposes a UAV trajectory planning method based on the latent force-guided BQ-RRT* algorithm. It improves the unidirectional search method to a bidirectional search method, introduces a parent node reselection method with a depth of 2, and incorporates the growth of an improved latent force-guided bidirectional random search tree, thereby realizing UAV trajectory planning and solving the trajectory planning problem of UAVs in performing flight missions.

[0004] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0005] This invention proposes a UAV trajectory planning method based on the potential force-guided BQ-RRT* algorithm, comprising the following steps:

[0006] Step 1: Initialize the iteration count iter, the UAV flight environment S, and set the starting point q for UAV trajectory planning. start , endpoint q goalStep size L; the flight environment S includes the wingable area S search Obstacle area S obs ;

[0007] Step 2: Determine if the iteration count (iter) has reached the maximum iteration count. If not, proceed to Step 3; otherwise, the UAV trajectory planning ends.

[0008] Step 3: In flight environment S, respectively using q start and q goal As the root node, create a starting tree T1 and an ending tree T2. The starting tree T1 contains only one node q. start The terminal tree T2 contains only one node q. goal Then, for each node in the tree, create a position-based index, a parent node index, and a parent node index;

[0009] Step 4: In the flyable area S search Inside, the starting tree T1 randomly generates random sampling points q. rand ;

[0010] Step 5: Select the distance q in the starting tree T1 rand The nearest node q nearest The initial node q nearest For q start ;

[0011] Step 6: Use an improved latent force function to make q goal and q rand For q respectively nearest Generates potential gravity, causing the obstacle to be affected by q nearest Generate a potential repulsive force, and generate a new node q along the direction of the resultant force of the potential attraction and potential repulsion forces according to a given step size L. new The improved potential force function includes an improved potential gravitational field function and an improved potential repulsive field function.

[0012] The improved potential gravitational field function is:

[0013]

[0014]

[0015] U att (q)=U att1 (q)+U att2 (q) (3)

[0016] Where, q rand For random sampling points; q goal The endpoint; q nearest For distance q rand The nearest node; Uatt1 (q) is q goal For q nearest The potential gravitational field function; U att2 (q) is q rand For q nearest The potential gravitational field function; k a Is the endpoint q goal The gravitational field gain constant, k b It is q rand The gravitational field gain constant, ρ g (q nearest ,q goal ) is q nearest and q goal The distance between them, ρ r (q nearest ,q rand ) is q nearest and q rand The distance between them; U att (q) is U att1 (q) and U att2 The resultant force of (q).

[0017] The improved potential repulsive field function is:

[0018]

[0019] Among them, U rep (q) represents the obstacle pair q nearest The potential repulsive field function; k r It is the repulsive field gain constant; n is a positive integer, and ρ0 is q nearest The minimum safe distance from the obstacle, ρ(q) nearest ,q obs ) is q nearest The Euclidean distance between it and the nearest obstacle.

[0020] The expressions for the potential attractive force, potential repulsive force, and resultant force are as follows:

[0021] F att1 (q)=k a ρ g (q nearest ,q goal (5)

[0022] F att2 (q)=k b ρ r (q nearest ,q rand (6)

[0023] F att (q)=F att1(q)+F att2 (q) (7)

[0024]

[0025]

[0026]

[0027] F total =F rep (q)+F att (q) (11)

[0028] Among them, F att1 (q) represents the endpoint pair q nearest The potential gravitational force generated; F att2 (q) is q rand For q nearest The potential gravitational force generated; F att (q) represents the endpoint and q rand For q nearest The resultant force of the potential gravitational force generated; Frep(q) is the resultant force of the potential gravitational force on q. nearest The resultant force of the potential repulsive force generated; F rep1 For obstacle pair q nearest The force, F rep2 It is q nearest Pointing to q goal The force, F total For combined efforts; It is q nearest The direction vector between the obstacle and the obstacle. It is q nearest and q goal The direction vector.

[0029] The new node q new for:

[0030]

[0031] Among them, (x n y n ) is q new The coordinates; (xc, y c ) is q nearest The coordinates of L; x It is the step size component in the horizontal direction, L y It is the step size component in the vertical direction;

[0032]

[0033] Where L is the step size of the UAV; k is the step size coefficient; F x and Fy They represent the resultant force F respectively. total Components of force in the x-axis and y-axis directions.

[0034] Step 7: Detect q nearest and q new Are there any obstacles between them, if q nearest and q new If an obstacle exists, the node collides with the obstacle. Repeat steps 4-6 to find a new node; otherwise, output the current node q. new ;

[0035] The detection q nearest and q new The method to determine whether there are obstacles between them is: using q nearest As the starting point for detection, with q new As the endpoint of the detection, q nearest and q new The distance between them is divided into j segments on average; each time a detection node q is generated, equation (14) is used. collision Position, calculate q collision Distance q collision The Euclidean distance to the center of the nearest obstacle is given by q. If this Euclidean distance is less than the radius of the circular obstacle, then q... nearest and q new If an obstacle exists, it will collide with the obstacle; if this Euclidean distance is greater than the radius of the circular obstacle, then q nearest and q new There are no obstacles between them;

[0036]

[0037] Where m is the number of growth steps, which is an integer, initially 0, and increases by m each time until it stops at j; r is the detection step size; and θ is the value of q. nearest to q new The angle between the direction and the x-axis, x qnearest and y qnearest They represent q respectively nearest x and y coordinates collision and y collision They represent the obtained q respectively collision x-axis and y-axis.

[0038] Step 8: Reselect q new The parent node and its relation to q new connect;

[0039] The selection q new The method for determining the parent node is: using q new Draw a circular region with center R and radius R. If q start Within the circular region, q will be...start As the parent node, if q start If it is not within the circular region, then divide the circular region by q. nearest Nodes outside the circle domain are selected as candidate parent nodes, and nodes outside the circle domain with a depth of 2 are also selected as candidate parent nodes, thus obtaining a set of candidate parent nodes. The candidate parent node that minimizes the path cost is then selected as q. new The parent node; the parent node with a depth of 2 is the parent node of the candidate parent node within the circular domain.

[0040] Step 9: Using q new Draw a circular region with center R and radius R, and then divide the region into three parts, excluding q. nearest Nodes outside the circle are selected as candidate child nodes, and child nodes outside the circle with a depth of 2 are also selected as candidate child nodes, thus obtaining a set of candidate child nodes. The candidate child node that minimizes the path cost is then selected as q. new Remove the child node and disconnect the child node from the current parent node, then connect the child node to q. new Connect; if there are no selectable child nodes, proceed directly to step 10; the child node with a depth of 2 is a child node of the candidate child node within the circular domain;

[0041] Step 10: Determine whether T1 and T2 are connected. If they are not connected, T2 is expanded using the same method as T1 in steps 4 to 9. After T2 is expanded, one iteration is completed. Otherwise, a flight path consisting of multiple points is generated, and the path planning ends. The method for determining whether T1 and T2 are connected is as follows: check whether the distance between the new nodes of T1 and T2 is less than a specified threshold. If it is not less than the threshold, they are not connected; otherwise, they are connected.

[0042] Compared with the prior art, the technical solution adopted in this invention has the following technical effects:

[0043] 1. In the process of reselecting parent nodes in the RRT* algorithm, a reselection method with a depth of 2 is introduced, which expands the scope of reselection of parent nodes, considers parent nodes outside the circle as the parent nodes of the new node, reduces the path cost, and plans a smoother trajectory.

[0044] 2. Improved potential forces are used to reduce the number of iterations. The target point and random points attract the random tree, while the obstacle closest to the random tree repels it. New nodes grow along the direction of the resultant force of the three forces.

[0045] 3. The bidirectional search method shortens the convergence time. Random trees are built at the starting point and the target point respectively, and the two random trees grow towards each other, which improves the calculation speed. The improved trajectory planning algorithm solves the problems of too many redundant points, too many iterations, and too long paths in the RRT* algorithm, thus improving the search efficiency of the algorithm. The proposed fusion algorithm optimizes the path length and reduces the convergence time. This method is easy to implement and has good operability. Attached Figure Description

[0046] Figure 1 This is a flowchart of a UAV trajectory planning method based on the potential force-guided BQ-RRT* algorithm in an embodiment of the present invention;

[0047] Figure 2 This is a schematic diagram of a UAV trajectory planning method based on the potential force-guided BQ-RRT* algorithm in an embodiment of the present invention.

[0048] Wherein, (a) represents a simple environment diagram; (b) represents a more complex environment diagram; and (c) represents a complex environment diagram.

[0049] Figure 3 In the growth process of the starting tree T1 in this embodiment of the invention, q nearest Force diagram;

[0050] Figure 4 This is a schematic diagram illustrating the reselection and parent node search in an embodiment of the present invention;

[0051] Figure 5 This is a schematic diagram of reselection and child node searching in an embodiment of the present invention;

[0052] Figure 6 This is a schematic diagram of bidirectional search in an embodiment of the present invention;

[0053] Figure 7 This is a schematic diagram of the generated flight path in an embodiment of the present invention;

[0054] Figure 8 These are simple environmental simulation comparison diagrams from embodiments of the present invention;

[0055] Wherein, (a) represents a schematic diagram of RRT* in a simple environment simulation comparison; (b) represents a schematic diagram of Q-RRT* in a simple environment simulation comparison; (c) represents a schematic diagram of P-RRT* in a simple environment simulation comparison; (d) represents a schematic diagram of B-RRT* in a simple environment simulation comparison; and (e) represents a schematic diagram of the present invention in a simple environment simulation comparison.

[0056] Figure 9 This is a simplified environmental simulation data diagram in this embodiment;

[0057] Wherein, (a) represents the average path cost of each algorithm in a simple environment; (b) represents the average running time of each algorithm in a simple environment; and (c) represents the average number of iterations of each algorithm in a simple environment.

[0058] Figure 10 This is a comparison diagram of a more complex environment simulation in this embodiment;

[0059] Wherein, (a) represents a schematic diagram of RRT* in a simulation comparison of a more complex environment; (b) represents a schematic diagram of Q-RRT* in a simulation comparison of a more complex environment; (c) represents a schematic diagram of P-RRT* in a simulation comparison of a more complex environment; (d) represents a schematic diagram of B-RRT* in a simulation comparison of a more complex environment; and (e) represents a schematic diagram of the present invention in a simulation comparison of a more complex environment.

[0060] Figure 11 This is a simulation data diagram of a more complex environment in this embodiment;

[0061] Wherein, (a) represents the average path cost of each algorithm in a more complex environment; (b) represents the average running time of each algorithm in a more complex environment; and (c) represents the average number of iterations of each algorithm in a more complex environment.

[0062] Figure 12 This is a comparison diagram of the complex environment simulation in this embodiment;

[0063] Wherein, (a) represents a schematic diagram of RRT* in the simulation comparison of complex environments; (b) represents a schematic diagram of Q-RRT* in the simulation comparison of complex environments; (c) represents a schematic diagram of P-RRT* in the simulation comparison of complex environments; (d) represents a schematic diagram of B-RRT* in the simulation comparison of complex environments; and (e) represents a schematic diagram of the present invention in the simulation comparison of complex environments.

[0064] Figure 13 This is a simulation data diagram of the complex environment in this embodiment;

[0065] (a) represents the average path cost of each algorithm in a complex environment; (b) represents the average running time of each algorithm in a complex environment; and (c) represents the average number of iterations of each algorithm in a complex environment. Detailed Implementation

[0066] The present invention will be further described below with reference to the accompanying drawings and specific embodiments;

[0067] This invention proposes a UAV trajectory planning method based on the latent force-guided BQ-RRT* algorithm, such as... Figure 1 As shown, the method includes the following steps:

[0068] Step 1: Initialize the iteration count iter, the UAV flight environment S, and set the starting point q for UAV trajectory planning. start , endpoint q goal Step size L; the flight environment S includes the wingable area S search Obstacle area S obs ;

[0069] In this embodiment, the UAV trajectory planning map is set as a two-dimensional space of 100km*100km, such as Figure 2 As shown, the circle represents the obstacle area, which the drone cannot pass through; the starting point of the drone's flight path planning is set to (1,1), and the ending point is set to (90,90).

[0070] Step 2: Determine if the iteration count (iter) has reached the maximum iteration count. If not, proceed to Step 3; otherwise, the UAV trajectory planning ends.

[0071] Step 3: In flight environment S, respectively using q start and q goal As the root node, create a starting tree T1 and an ending tree T2. The starting tree T1 contains only one node q. start The terminal tree T2 contains only one node q. goal Then, for each node in the tree, create a position-based index, a parent node index, and a parent node index;

[0072] Step 4: In the flyable area S search Inside, the starting tree T1 randomly generates random sampling points q. rand ;

[0073] Step 5: Select the distance q in the starting tree T1 rand The nearest node q nearest The initial node q nearest For q start ;

[0074] Step 6: Use an improved latent force function to make q goal and q rand For q respectively nearest Generates potential gravity, causing the obstacle to be affected by q nearest Generate a potential repulsive force, and generate a new node q along the direction of the resultant force of the potential attraction and potential repulsion forces according to a given step size L. new The improved potential force function includes an improved potential gravitational field function and an improved potential repulsive field function.

[0075] When a drone approaches a target point, the presence of surrounding obstacles causes the drone to experience a greater repulsive force than the target point's attractive force, resulting in the drone being unable to reach the target. Therefore, this invention proposes an improved latent gravitational field function U.att1 (q) As shown in equation (1), U att2 (q) is shown in equation (2):

[0076]

[0077]

[0078] U att (q)=U att1 (q)+U att2 (q) (3)

[0079] Where, q rand For random sampling points; q goal The endpoint; q nearest For distance q rand The nearest node; U att1 (q) is q goal For q nearest The potential gravitational field function; U att2 (q) is q rand For q nearest The potential gravitational field function; k a Is the endpoint q goal The gravitational field gain constant, k b It is q rand The gravitational field gain constant, ρ g (q nearest ,q goal ) is q nearest and q goal The distance between them, ρ r (q nearest ,q rand ) is q nearest and q rand The distance between them; U att (q) is U att1 (q) and U att2 The resultant force of (q).

[0080] The improved potential repulsive field function is shown in equation (4):

[0081]

[0082] Among them, U rep (q) represents the obstacle pair q nearest The potential repulsive field function; k r It is the repulsive field gain constant; n is a positive integer, and ρ0 is q nearest The minimum safe distance from the obstacle, ρ(q) nearest ,q obs ) is q nearestThe Euclidean distance between the drone and the nearest obstacle is such that when the drone approaches the obstacle, the repulsive force generated by the obstacle will change with ρ. g (q nearest ,q goal The force decreases and becomes smaller, so as to avoid the repulsive force from being greater than the attractive force;

[0083] The starting point tree T1 uses a function to generate random points, randomly generating a sampling point q within the UAV's flight area. rand And find the distance q in the search tree. rand The nearest node q nearest q is generated using an improved latent force. new q nearest The force diagram is as follows Figure 3 As shown, the endpoint is q nearest Generates gravity F att1 As shown in equation (5), in q rand Generate pairs of q on nodes nearest Potential appeal F att2 As shown in equation (6), F att1 and F att2 The resultant force is shown in equation (7); the obstacle affects q nearest Generates repulsive force F rep Different obstacles affect q nearest The repulsive force, as shown in equation (8), is obtained according to the parallelogram law, F. att1 F att2 F rep The resultant force F total As shown in equation (11); q new Along the direction of the resultant force, generate a new node q with a given step size L. new ;

[0084] F att1 (q)=k a ρ g (q nearest ,q goal (5)

[0085] F att2 (q)=k b ρ r (q nearest ,q rand (6)

[0086] F att (q)=F att1 (q)+F att2 (q) (7)

[0087]

[0088]

[0089]

[0090] F total =F rep (q)+F att (q) (11)

[0091] Among them, F att1 (q) represents the endpoint pair q nearest The potential gravitational force generated; F att2 (q) is q rand For q nearest The potential gravitational force generated; F att (q) represents the endpoint and q rand For q nearest The resultant force of the potential gravitational force generated; Frep(q) is the resultant force of the potential gravitational force on q. nearest The resultant force of the potential repulsive force generated; F rep1 For obstacle pair q nearest The force, F rep2 It is q nearest Pointing to q goal The force, F total For combined efforts; It is q nearest The direction vector between the obstacle and the obstacle It is q nearest and q goal The direction vector; when the drone reaches the vicinity of the target point, the repulsive force of the obstacle on the random tree becomes very small, ensuring that the drone can reach the target point; in solving q new At that time, q needs to be calculated separately. rand and q goal For q nearest The gravitational force of the obstacle is used to calculate the effect of the obstacle on q. nearest The repulsive force, then the resultant force F total Decompose it into forces in the x-axis and y-axis directions, respectively represented by F. x F y Indicate; set q nearest The coordinates are (x c y c The step size coefficient is determined by q. nearest The components of the net force acting on the UAV along the x and y axes determine the step size coefficient of the UAV.

[0092]

[0093] Where L is the step size of the drone, L x It is the step size component in the horizontal direction, L y q is the step size component in the vertical direction, where k is the step size coefficient. newLet q be the coordinates of q. new The coordinates are (x n y n ), then q new The coordinate equations are:

[0094]

[0095] Step 7: Detect q nearest and q new Are there any obstacles between them, if q nearest and q new If an obstacle exists, the node collides with the obstacle. Repeat steps 4-6 to find a new node; otherwise, output the current node q. new ;

[0096] The detection q nearest and q new The method to determine whether there are obstacles between them is: using q nearest As the starting point for detection, with q new As the endpoint of the detection, q nearest and q new The distance between them is divided into j segments on average; each time a detection node q is generated, it is determined by equation (14). collision Position, calculate q collision Distance q collision The Euclidean distance to the center of the nearest obstacle is given by q. If this Euclidean distance is less than the radius of the circular obstacle, then q... nearest and q new If an obstacle exists, it will collide with the obstacle; if this Euclidean distance is greater than the radius of the circular obstacle, then q nearest and q new There are no obstacles between them;

[0097]

[0098] Where m is the number of growth steps, which is an integer, initially 0, and increases by m each time until it stops at j; r is the detection step size; and θ is the value of q. nearest to q new The angle between the direction and the x-axis, x qnearest and y qnearest They represent q respectively nearest x and y coordinates collision and y collision They represent the obtained q respectively collision x-axis and y-axis.

[0099] Step 8: Reselect q new The parent node and its relation to q new connect;

[0100] The selection qnew The method for determining the parent node is: using q new Draw a circular region with center R and radius R. If q start Within the circular region, q will be... start As the parent node, if q start If it is not within the circular region, then divide the circular region by q. nearest Nodes outside the circle domain are selected as candidate parent nodes, and nodes outside the circle domain with a depth of 2 are also selected as candidate parent nodes, thus obtaining a set of candidate parent nodes. The candidate parent node that minimizes the path cost is then selected as q. new The parent node; the parent node with depth 2 is the parent node of the candidate parent node within the circular region; such as Figure 4 The diagram illustrates the process of finding a new parent node. The depth in the diagram is 2. After finding a new node, not only are parent nodes within the circular region considered, but also nodes outside the circular region at a depth of 2 are considered as candidate parent nodes. First, starting with q... new The set of nodes within a circular region centered at a radius of R, and the set of the first two nodes within that circular region, are used as candidate parent node sets, i.e., the set of nodes outside the circular region with a depth of 2. Each candidate parent node in the candidate node set is then used as q. new The parent node is used to compare the path costs from the new node through the candidate parent nodes to the starting point, and the candidate parent node that minimizes the path cost is selected as q. new New parent node, such as Figure 4 q ancestry As the parent node of the new node, the path cost is greatly reduced.

[0101] Step 9: Using q new Draw a circular region with center R and radius R, and then divide the region into three parts, excluding q. nearest Nodes outside the circle are selected as candidate child nodes, and child nodes outside the circle with a depth of 2 are also selected as candidate child nodes, thus obtaining a set of candidate child nodes. The candidate child node that minimizes the path cost is then selected as q. new Remove the child node and disconnect the child node from the current parent node, then connect the child node to q. new Connect; if there are no selectable child nodes, proceed directly to step 10; the child node with a depth of 2 is a child node of the candidate child node within the circular domain;

[0102] like Figure 5 As shown, after the algorithm reselects a parent node for the new node, it then uses the new node q... new The new node is rerouted both inside and outside a circular region centered at R and with radius R. This process will be based on the new node q. new Within a circular region centered at R and of radius R, each node in the candidate node set of the two preceding nodes of each node within the circular region is denoted as a child node of the new node. children Calculate the value of q for each node in each candidate node set.new As the parent node to q start Given the path cost, find the child node that minimizes the path cost and designate it as the child node of the new node, then disconnect the child node from the current parent node.

[0103] Step 10: Determine whether T1 and T2 are connected. If they are not connected, T2 is expanded using the same method as T1 in steps 4 to 9. After T2 is expanded, one iteration is completed. Otherwise, a flight path consisting of multiple points is generated, and the path planning ends. The method for determining whether T1 and T2 are connected is as follows: check whether the distance between the new nodes of T1 and T2 is less than a specified threshold. If it is not less than the threshold, they are not connected; otherwise, they are connected.

[0104] A schematic diagram of the growth process of two random trees is shown below. Figure 6 As shown; after T1 generates a new node, calculate q of the starting tree. new With the new node q of the endpoint tree new The distance between 'D(q) new ,q new The function checks whether the number of random trees is less than a specified threshold, i.e., whether the two random trees are connected. In the first iteration, after the starting tree is generated, the starting point q of the ending tree is set. goal As a new node in the endpoint tree, if D is not less than the specified threshold, then output node q in step 5. new Reselecting the parent node, not only in the case of q new Within a circular region centered at R, we find candidate parent nodes for the new node. We also consider nodes outside the circular region at a depth of 2 as candidate parent nodes. These candidate parent nodes are denoted as q. ancestry After finding a new parent node, a new node q is found both within the circular region and outside the circular region of depth 2. new As a child node of the parent node, reduce the path cost from this child node to the starting point; after finding the child node, q new It is added to the first random search tree T1, and the growth order is exchanged. T2 executes steps 4-9 to grow. If D is less than the specified threshold, then T1 and T2 are connected.

[0105] A flight path consisting of multiple points, such as Figure 7 As shown, this solves the problem of excessive turning angles during drone flight;

[0106] To further illustrate this invention in detail, simulation results of each algorithm in a simple environment are as follows: Figure 8 As shown, the dark lines represent the feasible flight paths of the UAV planned by the algorithm; the feasible flight paths of the UAV are indicated by the dark lines. Figure 8 In (a), due to the high randomness and blindness of RRT*, there are many honor points and many inflection points in the space. Figure 8(b) shows a simulation diagram of Q-RRT* at a depth of 2. Compared to RRT*, it has fewer inflection points in the planned point path and some nodes share the same parent node, resulting in a better path. Figure 8 In (c), RRT* and artificial potential field methods are introduced to make the path smoother. Figure 8 In (d), the B-RRT* random tree expansion process has fewer redundant points compared to RRT*. Regarding... Figure 8 (e) Due to the simple environment, the improved algorithm has the advantages of low track cost, few honor points, and low blindness, which can meet the real-time requirements of UAVs.

[0107] Simulation data for each algorithm in a simple environment includes the average runtime, average path length, and average number of iterations from 30 experiments. The average path length of the 30 experiments in this invention is 126.5908, the average runtime is 1.6879, and the average number of iterations is 58. Compared to the P-RRT* algorithm, the average length of the generated track is reduced by 4.10%, and the average number of iterations is reduced by 57.97%. Figure 9 As can be clearly seen, although the number of iterations in this invention is essentially the same as the P-RRT* algorithm, the path is smoother and the running time is shorter, such as Figure 8 As shown in (e). The results show that, in a simple environment, the convergence time of the present invention is greatly reduced, and the track cost is significantly lowered.

[0108] Table 1. Average data from 30 experiments in a simple environment

[0109]

[0110] In more complex environments, the simulation results are as follows: Figure 10 As shown in Table 2, compared to simple environments, these five algorithms exhibit more redundancy points during the algorithm growth process in more complex environments. The trajectory planned by this invention is smoother, has fewer nodes, and lower trajectory cost. Thirty experiments were conducted on each algorithm in a more complex environment, and the algorithm's running time, generated path length, and number of iterations were recorded. Experimental data are shown in Table 2 and... Figure 11 In China. Through the study of Figure 11 Analysis shows that, compared to the P-RRT* algorithm, the path planned by this invention has a lower cost. Table 2 shows that the average path length of this invention is 126.2841 km, the average running time is 2.8186 s, and the average number of iterations is 140. Compared to P-RRT*, the average running time is reduced by 3.90%. Combined data comparison indicates that this invention can obtain a better path.

[0111] Table 2. Average values ​​of 30 experiments in a relatively complex environment.

[0112]

[0113] In complex environments where obstacles are unevenly distributed and vary in size, this invention still has more significant advantages, such as... Figure 12 As shown in (e), compared with the trajectories planned by other algorithms, the trajectory planned by this invention has fewer inflection points and a smoother path. Analysis of Table 3 shows that the average path length for generating the initial solution in complex environments is 127.0492 km, the average running time is 5.8129 seconds, and the average number of iterations is 81. Compared with the P-RRT* algorithm, the average path length is 3.28%. Figure 13 As can be seen, the trajectory planned by this invention is significantly shorter, and its average running time and number of iterations are similar to those of P-RRT*. Data shows that the performance indicators of this invention are good among the five algorithms. Therefore, the trajectory planned by this invention is more in line with practical requirements.

[0114] Table 3. Average values ​​of 30 experimental data in complex environments

[0115]

[0116] This invention takes into account the impact of high path cost and slow convergence time, and realizes dynamic path planning research with short algorithm running time and low path cost during UAV flight. Therefore, this method can generate flight paths with lower path cost more quickly and timely, improving the search efficiency of the algorithm. Based on the above analysis, it can be seen that the proposed method can make UAV flight more realistic, that is, the method designed in this invention is effective.

Claims

1. A method for UAV trajectory planning based on the potential force-guided BQ-RRT* algorithm, characterized in that, Includes the following steps: Step 1: Initialize the iteration count iter, the UAV flight environment S, and set the starting point q for UAV trajectory planning. start , endpoint q goal Step size L; the flight environment S includes the wingable area S search Obstacle area S obs ; Step 2: Determine if the iteration count (iter) has reached the maximum iteration count. If not, proceed to Step 3; otherwise, the UAV trajectory planning ends. Step 3: In flight environment S, respectively using q start and q goal As the root node, create a starting tree T1 and an ending tree T2. The starting tree T1 contains only one node q. start The terminal tree T2 contains only one node q. goal Then, for each node in the tree, create a position-based index, a parent node index, and a parent node index; Step 4: In the flyable area S search Inside, the starting tree T1 randomly generates random sampling points q. rand ; Step 5: Select the distance q in the starting tree T1 rand The nearest node q nearest The initial node q nearest For q start ; Step 6: Use an improved latent force function to make q goal and q rand For q respectively nearest Generates potential gravity, causing the obstacle to be affected by q nearest Generate a potential repulsive force, and generate a new node q along the direction of the resultant force of the potential attraction and potential repulsion forces according to a given step size L. new The improved potential force function includes an improved potential gravitational field function and an improved potential repulsive field function. Step 7: Detect q nearest and q new Are there any obstacles between them, if q nearest and q new If an obstacle exists, the node collides with the obstacle. Repeat steps 4-6 to find a new node; otherwise, output the current node q. new ; Step 8: Reselect q new The parent node and its relation to q new connect; Step 9: Using q new Draw a circular region with center R and radius R, and then divide the region into three parts, excluding q. nearest Nodes outside the circle are selected as candidate child nodes, and child nodes outside the circle with a depth of 2 are also selected as candidate child nodes, thus obtaining a set of candidate child nodes. The candidate child node that minimizes the path cost is then selected as q. new Remove the child node and disconnect the child node from the current parent node, then connect the child node to q. new Connect; if there are no selectable child nodes, proceed directly to step 10; the child node with a depth of 2 is a child node of the candidate child node within the circular domain; Step 10: Determine whether T1 and T2 are connected. If they are not connected, T2 is expanded in the same way as T1 in steps 4 to 9. After T2 is expanded, one iteration is completed. Otherwise, a flight path consisting of multiple points is generated, and the path planning ends. The method for determining whether T1 and T2 are connected is: detect whether the distance between the new nodes of T1 and T2 is less than a specified threshold. If it is not less than the threshold, they are not connected; otherwise, they are connected.

2. The UAV trajectory planning method based on the latent force-guided BQ-RRT* algorithm according to claim 1, characterized in that, The improved potential gravitational field function mentioned in step 6 is: U att (q)=U att 1(q)+U att 2(q) (3) Where, q rand For random sampling points; q goal The endpoint; q nearest For distance q rand The nearest node; U att1 (q) is q goal For q nearest The potential gravitational field function; U att2 (q) is q rand For q nearest The potential gravitational field function; k a Is the endpoint q goal The gravitational field gain constant, k b It is q rand The gravitational field gain constant, ρ g (q nearest ,q goal ) is q nearest and q goal The distance between them, ρ r (q nearest ,q rand ) is q nearest and q rand The distance between them; U att (q) is U att1 (q) and U att2 The resultant force of (q).

3. The UAV trajectory planning method based on the latent force-guided BQ-RRT* algorithm according to claim 1, characterized in that, The improved potential repulsive field function mentioned in step 6 is: Among them, U rep (q) represents the obstacle pair q nearest The potential repulsive field function; k r It is the repulsive field gain constant; n is a positive integer, and ρ0 is q nearest The minimum safe distance from the obstacle, ρ(q) nearest ,q obs ) is q nearest The Euclidean distance between it and the nearest obstacle.

4. The UAV trajectory planning method based on the latent force-guided BQ-RRT* algorithm according to claim 1, characterized in that, The expressions for the potential attraction, potential repulsion, and resultant force mentioned in step 6 are as follows: F att1 (q)=k a ρ g (q nearest ,q goal ) (5) F att2 (q)=k b ρ r (q nearest ,q rand ) (6) F att (q)=F att1 (q)+F att2 (q) (7) F total =F rep (q)+F att (q) (11) Among them, F att1 (q) represents the endpoint pair q nearest The potential gravitational force generated; F att2 (q) is q rand For q nearest The potential gravitational force generated; F att (q) represents the endpoint and q rand For q nearest The resultant force of the potential gravitational force generated; Frep(q) is the resultant force of the potential gravitational force on q. nearest The resultant force of the potential repulsive force generated; F rep1 For obstacle pair q nearest The force, F rep2 It is q nearest Pointing to q goal The force, F total For combined efforts; It is q nearest The direction vector between the obstacle and the obstacle It is q nearest and q goal The direction vector.

5. The UAV trajectory planning method based on the potential force-guided BQ-RRT* algorithm according to claim 1, characterized in that, The new node q mentioned in step 6 new for: Among them, (x n y n ) is q new The coordinates; (xc, y c ) is q nearest The coordinates of L; x The step size component in the horizontal direction, L y It is the step size component in the vertical direction; Where L is the step size of the UAV; k is the step size coefficient; F x and F y They represent the resultant force F respectively. total Components of force in the x-axis and y-axis directions.

6. The UAV trajectory planning method based on the latent force-guided BQ-RRT* algorithm according to claim 1, characterized in that, The detection q described in step 7 nearest and q new The method to determine whether there are obstacles between them is: using q nearest As the starting point for detection, with q new As the endpoint of the detection, q nearest and q new The distance between them is divided into j equal segments; Each time, the detection node q is generated by equation (14). collision Position, calculate q collision Distance q collision The Euclidean distance to the center of the nearest obstacle is given by q. If this Euclidean distance is less than the radius of the circular obstacle, then q... nearest and q new If an obstacle exists, it will collide with the obstacle; if this Euclidean distance is greater than the radius of the circular obstacle, then q nearest and q new There are no obstacles between them; Where m is the number of growth steps, which is an integer, initially 0, and increases by m each time until it stops at j; r is the detection step size; and θ is the value of q. nearest to q new The angle between the direction and the x-axis, x qnearest and y qnearest They represent q respectively nearest x and y coordinates collision and y collision They represent the obtained q respectively collision x-axis and y-axis.

7. The UAV trajectory planning method based on the potential force-guided BQ-RRT* algorithm according to claim 1, characterized in that, Step 8 describes selecting q new The method for determining the parent node is: using q new Draw a circular region with center R and radius R. If q start Within the circular region, q will be... start As the parent node, if q start If it is not within the circular region, then divide the region by q. nearest Nodes outside the circle region are selected as candidate parent nodes, and nodes outside the circle region with a depth of 2 are also selected as candidate parent nodes, thus obtaining a set of candidate parent nodes. The candidate parent node that minimizes the path cost is then selected as q. new The parent node; the parent node with a depth of 2 is the parent node of the candidate parent node within the circular domain.

Images