Unmanned aerial vehicle path planning method fusing improved APF algorithm and improved RRT algorithm

Abstract

The invention belongs to the field of path planning, and discloses an unmanned aerial vehicle path planning method fusing an improved APF algorithm and an improved RRT algorithm, and the method comprises the steps: 1, building a repulsive force function based on the spatial positions of an unmanned aerial vehicle and an obstacle through the improved APF algorithm, and constructing a repulsive force potential field; 2, establishing a gravitational function based on a target position through an improved APF algorithm, and constructing a gravitational potential field; 3, superposing the repulsive force potential field obtained in the step 1 and the gravitational force potential field obtained in the step 2 to obtain a total artificially synthesized potential field; 4, the unmanned aerial vehicle moves in the steepest descending direction of the potential field according to the artificially synthesized potential field; and when the unmanned aerial vehicle is trapped in the local minimum value area, switching to the improved RRT algorithm to escape from the local minimum value area according to the temporary sub-target escape strategy, and switching to the improved APF algorithm to perform path planning until the unmanned aerial vehicle arrives at the target when the unmanned aerial vehicle escapes from the local minimum value area. The local optimization problem can be effectively improved, the path search complexity is reduced, and the unmanned aerial vehicle path planning requirement can be well met.

Description

Technical Field

[0001] The present invention belongs to the field of artificial intelligence path planning, and particularly relates to a UAV path planning method that combines and improves the APF algorithm and the RRT algorithm. Background Technique

[0002] What is provided in this part is only background information related to the present disclosure, and it is not necessarily prior art.

[0003] Unmanned aerial vehicles (UAVs) have characteristics such as miniaturization, low cost, and zero casualties, and are widely used in military and civilian fields. The path planning problem is one of the bottleneck problems restricting the current application of UAVs. For the path planning problem of UAVs, factors such as collision between UAVs, obstacle avoidance, and time efficiency need to be considered.

[0004] Currently, the mainstream path planning methods can be divided into three categories: traditional planning algorithms, intelligent planning algorithms, and sampling-based planning algorithms. Traditional planning algorithms are widely used in path planning due to their simplicity and rapidity; intelligent planning is often computationally complex and slow; sampling-based planning algorithms improve the search efficiency due to their probabilistic completeness, but the paths found are often not the optimal paths. Summary of the Invention

[0005] Aiming at the deficiencies of the prior art, the present invention discloses a UAV path planning method that combines and improves the APF and RRT algorithms. The improved APF algorithm is used for path planning and to solve the problem of unreachable targets, and the improved RRT algorithm is used to escape from local minimum regions, which is applicable to solving the UAV path planning problem.

[0006] The present invention provides a path planning algorithm that combines and improves the APF and RRT algorithms, including the following steps:

[0007] Step 1: Establish a repulsive force function based on the spatial positions of the UAV and obstacles through the improved APF algorithm to construct a repulsive potential field.

[0008] Step 2: Establish an attractive force function based on the target position through the improved APF algorithm to construct an attractive potential field.

[0009] Step 3: Superimpose the repulsive potential field obtained in Step 1 and the attractive potential field obtained in Step 2 to obtain a total artificial synthetic potential field.

[0010] Step 4: The UAV moves along the steepest descent direction of the potential field according to the artificial synthetic potential field obtained in Step 3; when the UAV falls into a local minimum region, it switches to the improved RRT algorithm to escape from the local minimum region according to the temporary sub-goal escape strategy, and then switches back to the improved APF algorithm for path planning after escaping from the local minimum region until it reaches the target.

[0011] Furthermore, step 1 is based on a repulsive force function of the three-dimensional spatial positions of the drone and the obstacles and an attractive force function of the three-dimensional spatial position of the target, and includes: the three-dimensional spatial position of the drone, the three-dimensional spatial position of the target object, establishing a repulsive force function based on the three-dimensional spatial positions of the obstacles and an attractive force function based on the three-dimensional spatial position of the target. Step 1 includes the following steps:

[0012] Step 1-1: Model the working space and abstract it to obtain a mathematical model.

[0013] The spatial position of the drone is denoted as q = [x, y, z] T , where x, y, z represent the position coordinates of the drone in the three-dimensional space O xyz , the direction of O x is the due east direction in the horizontal plane, the direction of O z is perpendicular to the ground and points directly upward, and the direction of O y is perpendicular to O x in the horizontal plane and satisfies the right-hand rule.

[0014] The three-dimensional spatial position of the obstacle is denoted as where represents the position coordinates of the j-th obstacle in the three-dimensional space O xyz , j = 1, 2, 3,..., N represents the total number of obstacles in the space, where x g , y g , z g represent the position coordinates of the center point of the target area in the three-dimensional space O xyz .

[0015] Step 1-2: Construct a modified repulsive force potential field function through the improved APF algorithm according to the mathematical model obtained in step 1-1

[0016] The improved APF algorithm adjusts the influence of the repulsive force of the original repulsive force potential field function U g (X) by introducing the relative distance ρ(X, X rep ) between the drone and the target and the repulsive force factor n, and constructs a modified repulsive force potential field function whose mathematical expression is as follows:

[0017]

[0018] In the formula,

[0019] k rep refers to the proportional gain factor of the repulsive force potential field; X refers to the position coordinates of the drone; X 0 refers to the position coordinates of the obstacle; X g refers to the position coordinates of the target point; ρ0 denotes the maximum influence distance of the obstacle repulsive potential field; ρ(X, X 0 ) denotes the relative distance between the UAV and the obstacle, ρ(X, X 0 ) = ||X 0 - X||; ρ(X, X g ) denotes the relative distance between the UAV and the target point, ρ(X, X g ) = ||X g - X||; n denotes the correction repulsive factor, and its value range is 0 ≤ n < 1.

[0020] Among them, when n = 0, there is no correction of the repulsive potential field at this time. When n is relatively large, as the target distance shortens, the UAV may no longer be affected by the repulsive force, but this may lead to the failure of obstacle avoidance. Therefore, the value of the correction repulsive factor should be gradually increased from small to adjust the correction repulsive potential field.

[0021] The above formula describes the correction repulsive potential field function Compared with the original repulsive potential field function U rep (X), the relative distance ρ(X, X g ) between the UAV and the target and the repulsive factor n are introduced to adjust the influence of the repulsive force and avoid the situation where the repulsive force is too large in the target area and the UAV cannot reach the target.

[0022] Step 1 - 3: Construct a repulsive force function through the correction repulsive potential field function obtained in Step 1 - 2 The expression of the said repulsive force function

[0023] The said repulsive force function is as follows:

[0024]

[0025] The above formula is a piecewise function. When ρ(X, X 0 ) > ρ 0 , it means that the distance from the obstacle exceeds the influence distance of the repulsive potential field, and the corresponding repulsive force is zero.

[0026] When the UAV is within the influence range of the repulsive potential field and is affected by the repulsive force of the obstacle at this time, the corresponding corrected repulsive force includes the following two component forces and The specific expressions are as follows:

[0027]

[0028] Furthermore, the value of the said correction factor n is as follows:

[0029] The influence of the value of the repulsive force correction factor \(n\) on the modified repulsive force function is discussed below. When the repulsive force correction factor \(n = 0\), the modified repulsive force component At this time, the repulsive force potential field and the repulsive force are not affected by the relative distance between the UAV and the target point, and there is no effect of modifying the repulsive force.

[0030] When the repulsive force factor \(n>0\), the repulsive force component That is, the repulsive force function is corrected by the relative distance between the UAV and the target point. According to the different values of the repulsive force correction factor \(n\), the following three cases can be discussed:

[0031] (1) When the repulsive force factor \(n>1\), the repulsive force potential field function is differentiable at the target \(x = x g . When the UAV approaches the target point, the relative distance \(\rho(X,X g )\) between the UAV and the target point gradually decreases, and the repulsive force function also gradually decreases and approaches zero. Due to the influence of the gravitational potential field, the UAV can reach the target point smoothly under the action of the combined potential field;

[0032] (2) When the repulsive force factor \(n = 1\), the relative distance \(\rho(X,X 0 )\leq\rho 0 and \(\rho(X,X 0 )\neq0\), the repulsive force functions are respectively:

[0033]

[0034] When the UAV approaches the target position, the relative distance \(\rho(X,X g )\to0\). At this time, according to the formula, the repulsive force component where \(c\) is a constant, then the resultant force on the UAV is greater than zero, and the direction is from the UAV to the target point. The UAV can still reach the target point smoothly;

[0035] (3) When the repulsive force correction factor \(0 < n < 1\), the repulsive force potential field function \(U rep (X)\) is not differentiable at the target position point \(x = x g . According to this functional formula, when the relative distance \(\rho(X,X 0 )<\rho 0 and \(\rho(X,X 0 )\neq0\), its repulsive force functions are respectively:

[0036]

[0037] When the UAV approaches the target position, that is, \(\rho(X,X g )\to0\), the first vector component of the repulsive force function And the second vector component Then the resultant force acting on the UAV is greater than zero, and the direction is from the UAV to the target point, and the UAV can reach the target point smoothly.

[0038] Further, Step 2 includes the following steps:

[0039] Step 2-1: Construct the gravitational potential field function U att (X) based on the target position by improving the APF algorithm.

[0040] In the path planning problem, the UAV can be regarded as a particle X = (x, y) in a two-dimensional space environment T , and its motion space is a two-dimensional space. In the traditional artificial potential field method, the gravitational potential field function U att generated by the target point is generally expressed as:

[0041]

[0042] In the formula

[0043] k att refers to the gravitational potential field proportional gain factor; X refers to the position coordinates of the UAV; X g refers to the position coordinates of the target point; m refers to the gravitational potential field factor; ρ(X, X g ) refers to the Euclidean distance between the UAV and the target, ρ(X, X g ) = ||X g - X||.

[0044] Step 2-2: Construct the gravitational function F att (X) through the gravitational potential field function U att (X) obtained in Step 2-1.

[0045] The gravitational potential field U att generates a gravitational force F att on the UAV, and the gravitational force F att is the negative gradient force of the gravitational potential field U att , representing the steepest descent direction of the gravitational potential field function U att , and is the derivative of the relative distance between the UAV and the target point with respect to the gravitational potential field U att , and its mathematical expression is:

[0046]

[0047] Further, Step 3 includes the following steps:

[0048] Step 3-1: Superimpose the repulsive potential field and the gravitational potential field to obtain the total artificial synthetic potential field U(X);

[0049] The expression of the resultant potential field function is as follows:

[0050] U(X) = U att (X) + U rep (X),

[0051] The expression of the resultant force function is as follows:

[0052]

[0053] Furthermore, the temporary sub-goal escape strategy described in step 4 is specifically described as follows:

[0054] Strategy 1: When it is detected that the UAV is in a local minimum state and there are no other obstacles distributed along and around the line connecting the current position of the UAV and the target, macroscopically, the UAV can directly reach the target point to complete the path planning. However, the UAV is trapped in the local minimum due to the balance of the repulsive force. In this case, set the temporary sub-goal to be equal to the final target point. By increasing the proportional gain factor of the gravitational potential field, strengthen the acting force of the gravitational potential field at the target point, break the balance between the gravitational force and the repulsive force, and drive the UAV to quickly escape from the local minimum state and move towards the target.

[0055] Strategy 2: When it is detected that the UAV is in a local minimum state and there is a single obstacle distributed along the line connecting the UAV and the target, the UAV can use its own sensors to detect the environment, detect the edge geometric information of the obstacle, and thus select one end of the edge that is closer. Set the temporary sub-goal, as shown in Figure 2 . The new temporary sub-goal will temporarily replace the global goal to generate gravity, and the corresponding gravity points from the UAV to the temporary sub-goal. At this time, it is no longer a balanced state between the repulsive force and the gravitational force, and the UAV is no longer in the local minimum state. At this time, the resultant force direction points to the temporary sub-goal. After the UAV moves to the position of the temporary sub-goal, switch the target state to the final target point and continue the path planning. According to the definition of the potential field, it will not fall into the old local minimum area again.

[0056] Strategy 3: When it is detected that the UAV is in a local minimum state and the UAV is between two obstacles, use the sensors carried by the UAV to detect the distances from the edges of the two obstacles, measure the included angle between the two obstacles, and calculate the distance of the obstacle edges. If this distance exceeds the set safe obstacle avoidance distance, it is considered that the UAV can reach the target position through the gap between the obstacles, and there is no need to escape from the local minimum and re-plan the path. On the premise of ensuring the safety of the UAV movement, by adjusting the proportional gain factor k att of the gravitational potential field or the gain factor k rep of the repulsive potential field, make the repulsive force and the gravitational force unbalanced, and directly move forward through the gap between the obstacles.

[0057] Strategy 4: When it is detected that the UAV is in a local minimum state and there are obstacles between the line connecting the UAV and the target point and the other side of the UAV. The UAV not only needs to avoid the obstacle directly in front, but also needs to consider the influence of the obstacle on one side. In this case, it is possible to choose the side with fewer or no obstacles to move forward, and adopt appropriate strategies to specify a temporary sub-goal point. The new temporary sub-goal temporarily replaces the global goal point to generate a gravitational potential field, and the corresponding gravity points from the UAV to the temporary sub-goal. After the UAV escapes from the local minimum, it switches the target state, and the UAV continues to perform path planning until it reaches the target state.

[0058] Strategy 5: When it is detected that the UAV is in a local minimum state and the UAV is trapped in a U-shaped obstacle. The U-shaped obstacle blocks other escape paths of the UAV, and then there is only one path available for retreat. At this time, the UAV needs to judge the edges of the surrounding obstacles, stop the normal path planning of the artificial potential field method, and perform a backward movement "against the potential field", that is, the opposite direction of the artificial potential field decrease. According to whether the edge detection of the obstacle can escape from the U-shaped obstacle area, it is possible to specify temporary sub-goals on both sides of the edge of the U-shaped obstacle, as Figure 4 shown. After the UAV escapes from the U-shaped obstacle area, in order to avoid falling into the U-shaped obstacle area again, it may be necessary to set temporary sub-goals multiple times. The number of settings can be selected from two to three.

[0059] Furthermore, the process of switching the RRT algorithm to escape from the local minimum area in step 4 includes the following steps:

[0060] Step S1: The extended random tree construction stage, which is the first and main stage of the RRT algorithm, is used to iteratively attempt to expand the search tree to a randomly selected new state in the system state space until the target state point is reached, and finally construct and generate an extended random tree.

[0061] Step S2: The path query stage, which is the second stage of the RRT algorithm. According to the extended tree constructed in the previous stage, an effective path containing the initial state point to the target state point is calculated. Essentially, it is a tree path query algorithm.

[0062] Furthermore, the construction process of the rapidly-exploring random tree is as Figure 5 shown, and the specific expansion steps are as follows:

[0063] Step S101, randomly sample a state point. Randomly sample and select a state point q free in the free state space C rand , q rand ∈C free and q rand ∈T k .

[0064] Step S102, calculate the metric nearest point. Traverse the extended random tree T k , T 1 = q init , according to the metric of the extended random tree, such as geometric distance, find the node q k in the extended random tree T rand that is metric nearest to the randomly sampled state point q near . In the case where the metric is the set distance, let (x, y) ∈ C free , and let Dis(x, y) represent the geometric distance between two state space points x and y. Then the geometric relationship between the randomly sampled state point q rand and the metric nearest state point q near is expressed as

[0065] Dis(q near , q rand ) ≤ Dis(q init , q rand ).

[0066] Step S103, calculate the new state point. After finding the metric nearest state point q near , obtain a new state point q near on the line connecting q rand and q new , and q new must satisfy the following conditions: q new ∈ C free and Dis(q near , q new ) = ξ. Where ξ > 0 is the fixed incremental distance of the rapidly-exploring random tree, which is called the expansion step size of the extended random tree. Here, based on the metric nearest state point q near , increment a step size distance in the direction of the randomly sampled state point q rand to obtain the new state point q new . At the same time, ensure that the new state point q new satisfies the global constraint of C through the collision detection algorithm of the unmanned aerial vehicle, that is, q new ∈ C free .

[0067] If there exists such a node q new and Dis(q new , q goal ) ≤ ξ or q new ∈ C goal , then it is determined that the extended random tree has reached the target state point C goal or the target area C goal , then directly add C goal to the extended random tree and end the expansion process of step S1 of the extended random tree.

[0068] If there is such a node q in S103 new , q new ∈C free but Dis(q new , q goal ) > ξ, then it is determined that the expansion process is not over yet. Then, add the leaf node q k to the expanded random tree T new . Let T k+1 represent the new expanded random tree. Then T k+1 = T k +q new . Then, re - execute step S1 for the new expanded random tree T k+1 .

[0069] The random point is generally within a range, and the free - growing point has strong randomness. If there is no such node q new in S103, that is or it is calculated according to the collision detection algorithm that q new is not in the free state space of the UAV, then it is necessary to return to step S101 to randomly sample and select another node q rand , and repeat the above expansion process.

[0070] In the third step of the expansion step, a new state point q new needs to be calculated according to the fixed step - size ξ step - size distance:[[]]

[0071]

[0072] In the formula

[0073] q new refers to the new node to be added;

[0074] q near refers to the node closest to the randomly sampled point in terms of metric;

[0075] ξ refers to the step - size of the expanded random tree;

[0076] q rand —— the randomly sampled point;

[0077] Furthermore, step S2 is specifically described as follows:

[0078] After completing the construction of the expanded tree, stop adding new state nodes. Starting from the target state point q goal , traverse the expanded random tree T k in reverse, and traverse its parent nodes in turn, iterating layer by layer until the initial state point q init is reached. In this way, a path from the planning start point q init to the planning end point qgoal The branch path, and this path satisfies the global constraints of the UAV.

[0079] Beneficial effects: The present invention utilizes an improved artificial potential field method to solve the multi-UAV path planning problem, effectively avoiding the problem of path planning falling into local minima, and improving the path planning efficiency; secondly, when the UAV falls into the local minimum area, it can combine with the improved RRT algorithm to escape from the local minimum area, thereby improving the time and computing power efficiency of escaping from the local minimum area. Brief Description of the Drawings

[0080] The following further specifically describes the present invention in conjunction with the drawings and specific embodiments, and the above and / or other advantages of the present invention will become clearer.

[0081] Figure 1 Flowchart of the fusion algorithm of the improved APF algorithm and the improved RRT algorithm.

[0082] Figure 2 Schematic diagram of a single-obstacle sub-goal.

[0083] Figure 3 Schematic diagram of connecting double-obstacle sub-goals.

[0084] Figure 4 Schematic diagram of a U-shaped obstacle sub-goal.

[0085] Figure 5 Schematic diagram of the construction process of the rapidly-exploring random tree of this solution.

[0086] Figure 6 Schematic diagram of the test results of the UAV simulation model and the environmental BITMAP map under the condition that the repulsive force correction factor is n = 0.9.

[0087] Figure 7 Schematic diagram of the experimental results of Strategy 2.

[0088] Figure 8 Schematic diagram of the experimental results of the APF-RRT algorithm.

[0089] Figure 9 Schematic diagram of the experimental results of the APF-RRT-APF algorithm. Specific Embodiments

[0090] The present invention provides a path planning algorithm that fuses an improved artificial potential field method and an improved RRT, including the following steps:

[0091] Step 1: Establish a repulsive force function based on the spatial positions of the UAV and the obstacles through the improved APF algorithm to construct a repulsive potential field.

[0092] Step 2: Establish a gravitational function based on the target position through an improved APF algorithm to construct a gravitational potential field.

[0093] Step 3: Superimpose the repulsive potential field obtained in Step 1 and the gravitational potential field obtained in Step 2 to obtain the total artificial synthetic potential field.

[0094] Step 4: The UAV moves along the steepest descent direction of the potential field according to the artificial synthetic potential field obtained in Step 3; when the UAV falls into the local minimum area, according to the temporary sub-goal escape strategy, it switches to the improved RRT algorithm to escape the local minimum area, and after escaping the local minimum area, it switches back to the improved APF algorithm for path planning until it reaches the target.

[0095] Furthermore, the repulsive force function based on the three-dimensional spatial positions of the UAV and the obstacles and the gravitational force function based on the three-dimensional spatial position of the target in Step 1 include: the three-dimensional spatial position of the UAV, the three-dimensional spatial position of the target object, and establish a repulsive force function based on the three-dimensional spatial position of the obstacle and a gravitational force function based on the three-dimensional spatial position of the target. Step 1 includes the following steps:

[0096] Step 1-1: Model the working space and abstract it to obtain a mathematical model.

[0097] The spatial position of the UAV is denoted as q = [x, y, z] T , where x, y, and z represent the position coordinates of the UAV in the three-dimensional space O xyz The direction of O x is the due east direction in the horizontal plane, the direction of O z is perpendicular to the ground and points directly upward, and the direction of O y is perpendicular to O x in the horizontal plane and satisfies the right-hand rule.

[0098] The three-dimensional spatial position of the obstacle is denoted as where represents the position coordinates of the j-th obstacle in the three-dimensional space O xyz , j = 1, 2, 3,..., N represents the total number of obstacles in the space, where x g , y g , z g represents the position coordinates of the center point of the target area in the three-dimensional space O xyz .

[0099] Step 1-2: Construct a modified repulsive potential field function through the improved APF algorithm according to the mathematical model obtained in Step 1-1

[0100] Construct a modified repulsive potential field function Its mathematical expression is as follows:

[0101]

[0102] In the formula,

[0103] k rep represents the proportional gain factor of the repulsive potential field; X represents the position coordinates of the UAV; X 0 represents the position coordinates of the obstacle; X g represents the position coordinates of the target point; ρ 0 represents the maximum influence distance of the obstacle repulsive potential field; ρ(X, X 0 ) represents the relative distance between the UAV and the obstacle, ρ(X, X 0 ) = ||X 0 - X||; ρ(X, X g ) represents the relative distance between the UAV and the target point, ρ(X, X g ) = ||X g - X||; n represents the correction repulsive factor, and its value range is 0 ≤ n < 1.

[0104] The above formula describes the corrected repulsive potential field function Compared with the original repulsive potential field function U rep (X), the relative distance ρ(X, X g ) between the UAV and the target and the repulsive factor n are introduced to adjust the influence of the repulsion and avoid the situation where the repulsion is too large in the target area, resulting in the UAV being unable to reach the target.

[0105] Step 1-3: Construct a repulsive force function using the corrected repulsive potential field function obtained in Step 1-2 The expression of the said repulsive force function

[0106] The said repulsive force function is as follows:

[0107]

[0108] The above formula is a piecewise function. When ρ(X, X 0 ) > ρ 0 , it means that the distance from the obstacle exceeds the influence distance of the repulsive potential field, and the corresponding repulsive force is zero.

[0109] When the UAV is within the influence range of the repulsive potential field and is affected by the repulsive force of the obstacle at this time, the corresponding corrected repulsive force includes the following two component forces.

[0110]

[0111] Furthermore, the value of the said correction factor n is as follows:

[0112] The influence of the value of the repulsive force correction factor \(n\) on the modified repulsive force function is discussed below. When the repulsive force correction factor \(n = 0\), the modified repulsive force component At this time, the repulsive force potential field and the repulsive force are not affected by the relative distance between the UAV and the target point, and there is no effect of modifying the repulsive force.

[0113] When the repulsive force factor \(n>0\), the repulsive force component That is, the repulsive force function is corrected by the relative distance between the UAV and the target point. According to the different values of the repulsive force correction factor \(n\), the following three cases can be discussed:

[0114] (1) When the repulsive force factor \(n>1\), the repulsive force potential field function is differentiable at the target \(x = x g \). When the UAV approaches the target point, the relative distance \(\rho(X,X g )\) between the UAV and the target point gradually decreases, and the repulsive force function also gradually decreases and approaches zero. Due to the influence of the gravitational potential field, the UAV can reach the target point smoothly under the action of the combined potential field;

[0115] (2) When the repulsive force factor \(n = 1\), the relative distance \(\rho(X,X 0 )\leq\rho 0 and \(\rho(X,X 0 )\neq0\), the repulsive force functions are respectively:

[0116]

[0117] When the UAV approaches the target position, the relative distance \(\rho(X,X g )\to0\). At this time, according to the formula, the repulsive force component where \(c\) is a constant, then the resultant force on the UAV is greater than zero, and the direction is from the UAV to the target point. The UAV can still reach the target point smoothly;

[0118] (3) When the repulsive force correction factor \(0 < n < 1\), the repulsive force potential field function \(U rep (X)\) is not differentiable at the target position point \(x = x g \). According to this functional formula, when the relative distance \(\rho(X,X 0 )<\rho 0 and \(\rho(X,X 0 )\neq0\), its repulsive force functions are respectively:

[0119]

[0120] When the UAV approaches the target position, that is, \(\rho(X,X g )\to0\), the first vector component of the repulsive force function And the second vector component Then the resultant force on the UAV is greater than zero, and the direction is from the UAV to the target point, and the UAV can reach the target point smoothly.

[0121] Further, step 2 includes the following steps:

[0122] Step 2-1: Construct the gravitational potential field function U att (X) based on the target position by improving the APF algorithm.

[0123] In the path planning problem, the UAV can be regarded as a particle X = (x, y) in a two-dimensional space environment T , and its motion space is a two-dimensional space. In the traditional artificial potential field method, the gravitational potential field function U ATT Generally, the expression is:

[0124]

[0125] In the formula

[0126] k att Refers to the gravitational potential field proportional gain factor; X refers to the position coordinates of the UAV; X g Refers to the position coordinates of the target point; m refers to the gravitational potential field factor; ρ(X, X g ) refers to the Euclidean distance between the UAV and the target, ρ(X, X g ) = ||X g - X||.

[0127] Step 2-2: Construct the gravitational function F att (X) through the gravitational potential field function U att (X) obtained in step 2-1.

[0128] The gravitational potential field U att Generates a gravitational force F att on the UAV, and the gravitational force F att is the negative gradient force of the gravitational potential field U att , representing the steepest descent direction of the gravitational potential field function U att , which is the derivative of the gravitational potential field U att with respect to the relative distance between the UAV and the target point, and its mathematical expression is:

[0129]

[0130] Further, step 3 includes the following steps:

[0131] Step 3-1: Superimpose the repulsive potential field and the gravitational potential field to obtain the total artificial synthetic potential field U(X);

[0132] The expression of the resultant potential field function is as follows:

[0133] U(X) = U att (X) + U rep (X),

[0134] The expression of the resultant force function is as follows:

[0135]

[0136] According to the modified repulsive potential field and the modified repulsive potential field function, a UAV simulation model and an environmental BITMAP map are adopted. The repulsive correction factor n = 0.9 is set to increase the repulsive correction effect, and other parameters are the same as those in the traditional potential field experiment. The test results are as Figure 6 shown. Under the modified repulsive potential field, the modified repulsion decreases as the relative distance between the UAV and the target point decreases, and the direction of the resultant repulsive force also gradually deflects towards the target point when the UAV approaches the target point, making the direction of the resultant force of the UAV deflect towards the target point, ensuring that the UAV can reach the target point. Therefore, the modified repulsive force function can solve the problem of unreachable targets in the traditional artificial potential field method, and the path planning efficiency is higher.

[0137] Furthermore, the temporary sub-goal escape strategy described in step 4 is specifically described as follows:

[0138] Strategy 1: When it is detected that the UAV is in a local minimum state and there are no other obstacles distributed along the line connecting the current position of the UAV and the target, macroscopically, the UAV can directly reach the target point to complete the path planning. However, the UAV is trapped in the local minimum due to the balance of attraction and repulsion. In this case, the temporary sub-goal is set to be the same as the final target point. By increasing the proportional gain factor of the gravitational potential field, the acting force of the gravitational potential field at the target point is strengthened, breaking the balance between gravity and repulsion, and driving the UAV to quickly escape from the local minimum state and move towards the target.

[0139] Strategy 2: When it is detected that the UAV is in a local minimum state and there is a single obstacle distributed along the line connecting the UAV and the target, the UAV can use its own sensors to detect the environment and detect the edge geometric information of the obstacle, so as to select one end of the edge with a shorter distance and set a temporary sub-goal, as Figure 2 shown. The new temporary sub-goal will temporarily replace the global goal to generate gravity, and the corresponding gravity points from the UAV to the temporary sub-goal. At this time, it is no longer the balanced state of repulsion and gravity, and the UAV is no longer in the local minimum state. At this time, the direction of the resultant force points to the temporary sub-goal. After the UAV moves to the position of the temporary sub-goal, the target state is switched to the final target point, and the path planning continues. According to the definition of the potential field, it will not fall into the old local minimum area again.

[0140] As Figure 2As shown in the figure, when it is detected that the UAV has fallen into a local minimum state and there is a single obstruction distributed along the line connecting the UAV and the target goal, the UAV uses its own sensors to detect the environment and probe the edge geometric information of the obstruction, so as to select one end of the edge with a shorter distance and set a temporary sub-goal goal. * The new temporary sub-goal will temporarily replace the global goal to generate gravity. The corresponding gravity points from the UAV to the temporary sub-goal. At this time, it is no longer the balanced state of repulsive force and gravity, and the UAV is no longer in the local minimum state. At this time, the resultant force direction points to the temporary sub-goal. After the UAV moves to the position of the temporary sub-goal, it switches to the target state as the final target point and continues path planning. According to the definition of the potential field, it will no longer fall into the old local minimum area.

[0141] In response to this situation, by increasing the positive gain gravity of the gravity potential field, the balance state between the gravity potential field and the repulsive force potential field is broken, and the UAV is driven to reach the end point. The experimental results are as Figure 7 shown.

[0142] Strategy 3: When it is detected that the UAV has fallen into a local minimum state and the UAV is between two obstructions, use the sensors carried by the UAV to detect the distances from the edges of the two obstructions, measure the included angle between the two obstructions, and calculate the distance of the obstruction edge. If this distance exceeds the set safe obstacle avoidance distance, it is considered that the UAV can reach the target position through the gap between the obstacles, and there is no need to escape the local minimum and replan the path again. On the premise of ensuring the safety of the UAV's movement, by adjusting the gravity potential field gain factor k att or the repulsive force potential field gain factor k rep , the repulsive force and gravity are no longer balanced, and the UAV directly passes through the gap between the obstacles and continues to move forward.

[0143] Strategy 4: When it is detected that the UAV has fallen into a local minimum state and there are obstructions between the line connecting the UAV and the target point and the other side of the UAV. The UAV not only needs to avoid the obstacle directly in front, but also needs to consider the influence of the obstacle on one side. In this case, it is possible to choose the side with fewer or no obstacles to move forward and adopt appropriate strategies to specify a temporary sub-goal point. The new temporary sub-goal temporarily replaces the global goal point to generate a gravity potential field. The corresponding gravity points from the UAV to the temporary sub-goal. After the UAV escapes from the local minimum, it switches the target state, and the UAV continues path planning until it reaches the target state.

[0144] As Figure 3 shown, select a temporary sub-goal point goal on the side without obstacles of the UAV * , and the new temporary sub-goal temporarily replaces the global goal point to generate a gravity potential field The corresponding gravitational force points from the UAV to the temporary sub-goal. Meanwhile, under the action of the original repulsive force F rep , the UAV escapes from the local minimum region towards the direction of the resultant force . After the UAV escapes from the local minimum, it switches the target state and continues path planning until it reaches the target state.

[0145] Strategy 5: When it is detected that the UAV is in the local minimum state and the UAV is trapped in a U-shaped obstacle. The U-shaped obstacle blocks other escape paths of the UAV, and then there is only one backward path available. At this time, the UAV needs to judge the edges of the surrounding obstacles, stop the normal path planning of the artificial potential field method, and perform backward movement "against the potential field". According to whether it detects the edge of the obstacle to escape from the U-shaped obstacle area, it can select to specify temporary sub-goals on both sides of the edge of the U-shaped obstacle, such as Figure 4 shown. After the UAV escapes from the U-shaped obstacle area, in order to avoid being trapped in the U-shaped obstacle area again, it may be necessary to set temporary sub-goals multiple times.

[0146] As Figure 4 shown, the U-shaped obstacle blocks other escape paths of the UAV, and only one backward path can be selected. At this time, the UAV needs to judge the edges of the surrounding obstacles, stop the normal path planning of the APF, perform backward movement "against the potential field", and select to specify temporary sub-goals goal on both sides of the edge of the U-shaped obstacle * . The new temporary sub-goal replaces the global goal to generate gravitational force Meanwhile, under the action of the original repulsive force F rep , the UAV escapes from the local minimum region towards the direction of the resultant force . To avoid being trapped in this U-shaped obstacle again, it is necessary to set temporary sub-goals multiple times.

[0147] Furthermore, switching the RRT algorithm to escape from the local minimum region in step 4 includes the following steps:

[0148] Step S1: The stage of expanding the random tree construction, which is the first and main stage of the RRT algorithm. It is used to iteratively try to expand the search tree to a randomly selected new state in the system state space until it reaches the target state point, and finally construct and generate an expanded random tree.

[0149] Step S2: The path query stage, which is the second stage of the RRT algorithm. According to the expanded tree constructed in the previous stage, it calculates an effective path containing the initial state point to the target state point, which is essentially a tree path query algorithm.

[0150] Furthermore, the construction process of the rapidly-exploring random tree is shown in the figure, and the specific expansion steps are as follows:

[0151] Step S101, randomly sample a state point. Randomly sample and select a state point q free from the free state space C rand , q rand ∈ C free and q rand ∈ T k ;

[0152] Step S102, calculate the metric nearest point. Traverse the extended random tree T k , T 1 = q init , and according to the metric of the extended random tree, such as geometric distance, find the node q k in the extended random tree T rand that is metric-nearest to the randomly sampled state point q near . In the case where the metric is the set distance, let (x, y) ∈ C free , and let Dis(x, y) represent the geometric distance between two state space points x and y. Then the geometric relationship between the randomly sampled state point q rand and the metric-nearest state point q near is expressed as

[0153] Dis(q near , q rand ) ≤ Dis(q init , q rand ).

[0154] Step S103, calculate the new state point. After finding the metric-nearest state point q near , find a new state point q near on the line connecting q rand and q new , and q new must satisfy the following conditions: q new ∈ C free and Dis(q near , q new ) = ξ. Where ξ > 0 is the fixed incremental distance of the rapidly-exploring random tree, called the expansion step size of the extended random tree. Here, based on the metric-nearest state point q near , increment a step distance in the direction of the randomly sampled state point q rand to obtain the new state point q new . At the same time, ensure that the new state point q new satisfies the global constraints of C through the collision detection algorithm of the UAV, that is, q new ∈ C free .

[0155] If there exists such a node q new in the third step and Dis(q new , qgoal ) ≤ ξ or q new ∈ C goal , it is determined that the extended random tree has reached the target state point q goal or the target area C goal , then directly add q goal to the extended random tree, and end the extension process of step S1 of the extended random tree.

[0156] If there is such a node q in the third step new , q new ∈ C free but Dis(q new , q goal ) > ξ, then it is determined that the extension process has not ended yet. Then add the leaf node q in the extended random tree T k , let T new represent the new extended random tree, then T k+1 = T k+1 + q k , and then re - execute step S1 for the new extended random tree T new . k+1

[0157] If there is no such a node q in the third step new , that is or calculated according to the collision detection algorithm that q new is not in the free state space of the UAV, then it is necessary to return to step S101 to randomly sample and select another node q rand , and repeat the above extension process.

[0158] In the third step of the extension step, a new state point q needs to be calculated according to the fixed step size ξ new :

[0159]

[0160] In the formula

[0161] q new refers to the new node to be added;

[0162] q near refers to the node closest to the random sampling point in terms of metric;

[0163] ξ refers to the step size of the extended random tree;

[0164] q rand —— random sampling point;

[0165] According to the above formula, let q new =(x 1 , y 1 ​), q rand = (x 2 , y 2 ), let v = (q rand - q near ), where v represents the distance between q near and q near defined by the Euclidean norm. Calculate the unit vector , multiply it by the step size ξ, and finally add q near , then the new extended leaf node q new = q near + ξu is calculated.

[0166] Furthermore, the specific description of step S2 is as follows:

[0167] After completing the construction of the extended tree, stop adding new state nodes. Starting from the target state point q goal , traverse the extended random tree T k in reverse, and traverse its parent nodes in turn, iterating layer by layer until the initial state point q init is reached. In this way, a branch path from the planning start point q init to the planning end point q goal is traversed, and this path satisfies the global constraints of the drone.

[0168] In this experiment, through the APF-RRT-APF fusion algorithm, when the improved APF algorithm is used to plan the path, when a local minimum is detected, it switches to the improved RRT algorithm. After escaping the minimum, it switches back to the improved APF algorithm. Figure 8 is the experimental result of the APF-RRT algorithm, Figure 9 is the experimental result of the APF-RRT-APF. The experimental results show that when escaping from the local minimum area and then switching back to the improved APF algorithm, the path planning efficiency is higher.

[0169] The present invention provides a method for path planning of a drone that combines an improved APF algorithm and an improved RRT algorithm. There are many methods and ways to specifically implement this technical solution. The above description is only the preferred embodiment of the present invention. It should be noted that for those of ordinary skill in the art, without departing from the principle of the present invention, several improvements and refinements can be made, and these improvements and refinements should also be regarded as the protection scope of the present invention. Each component not clearly defined in this embodiment can be implemented by existing technologies.

Claims

1. A UAV path planning method integrating improved APF algorithm and improved RRT algorithm, characterized in that: The steps include: Step 1: Establish a repulsive function based on the spatial position of the drone and obstacles by improving the APF algorithm to construct a repulsive potential field; Step 2: Establish a gravitational function based on the target position by improving the APF algorithm to construct the gravitational potential field; Step 3: Superimpose the repulsive potential field obtained in step 1 and the attractive potential field obtained in step 2 to obtain the total artificial synthetic potential field; Step 4: The UAV moves under the guidance of the artificial potential field obtained in step 3. The motion trajectory generated by the motion is the path planned by the improved APF algorithm. When it falls into the local minimum area, it switches to the improved RRT algorithm to escape the local minimum area. After escaping the local minimum area, it switches to the improved APF algorithm for path planning until it reaches the target point.

2. The UAV path planning method integrating the improved APF algorithm and the improved RRT algorithm according to claim 1 is characterized in that: Step 1 contains the following steps: Step 1-1: Model the workspace and abstract the mathematical model; The spatial position of the drone is recorded as q = [x, y, z] T , where x, y, z represent the UAV in the three-dimensional space O xyz The position coordinates, O x The direction is due east in the horizontal plane, O z The direction is perpendicular to the ground and points upward. y The direction is perpendicular to O in the horizontal plane. x and satisfies the right-hand rule; The three-dimensional spatial position of the obstacle is recorded as in Indicates the jth obstacle in the three-dimensional space O xyz The position coordinates of the object, j = 1, 2, 3, ..., N represents the total number of obstacles in the space, where x g ,y g , z g Indicates that the center point of the target area is in the three-dimensional space O xyz The location coordinates of Step 1-2: Construct a modified repulsive potential field function based on the mathematical model obtained in step 1-1 by improving the APF algorithm The improved APF algorithm introduces the relative distance ρ(X,X g ) and the repulsive factor n, adjust the original repulsive potential field function U rep (X) repulsive force, construct a modified repulsive potential field function Its mathematical expression is as follows: In the formula, k rep Refers to the proportional gain factor of the repulsive potential field; X refers to the position coordinates of the drone; X0 refers to the location coordinates of the obstacle; X g Refers to the location coordinates of the target point; ρ0 refers to the maximum influence distance of the obstacle repulsive potential field; ρ(X,X0) refers to the relative distance between the drone and the obstacle, ρ(X,X0)=||X0-X||; ρ(X,X g ) refers to the relative distance between the drone and the target point, ρ(X,X g )=||X g -X||; n refers to the modified repulsion factor, and its value range is 0≤n<1; Step 1-3: Modified repulsive potential field function obtained through step 1-2 Constructing the repulsion function Repulsion function The expression is: When ρ(X,X0)>ρ0, that is, the distance from the obstacle exceeds the influence distance of the repulsive potential field, the corresponding repulsive force is zero; When the drone is in the range of the repulsive potential field, it is affected by the repulsive force of the obstacle. The corresponding repulsive force includes the component force and The specific expression is:

3. The UAV path planning method integrating the improved APF algorithm and the improved RRT algorithm according to claim 2 is characterized in that: Step 2 contains the following steps: Step 2-1: Construct the gravitational potential field function U based on the target position by improving the APF algorithm att (X); Abstract the drone as a particle X=(x,y) in a two-dimensional space environment T , its motion space is two-dimensional space, and the gravitational potential field function U generated by the target point att The general expression is: In the formula k att Refers to the proportional gain factor of the gravitational potential field; X refers to the position coordinates of the drone; X g Refers to the location coordinates of the target point; m refers to the gravitational potential field factor; ρ(X,X g ) refers to the Euclidean distance between the drone and the target, ρ(X,X g )=||X g -X||; Step 2-2: The gravitational potential field function U obtained by step 2-1 att (X) Construct gravity function F att (X); Gravitational potential field U att Generates gravitational force F on the drone att , gravity F att is the gravitational potential field U att The negative gradient force represents the gravitational potential field function U att The fastest descending direction is the gravitational potential field U att The mathematical expression of the derivative of the relative distance between the drone and the target point is:

4. The UAV path planning method integrating the improved APF algorithm and the improved RRT algorithm according to claim 3 is characterized in that: Step 3 contains the following steps Step 3-1: Superimpose the repulsive potential field and the gravitational potential field to obtain the total artificial synthetic potential field U(X); U(X)=U att (X)+U rep (X); Step 3-2: Construct the resultant force function F(X) using the artificial synthetic potential field U(X) obtained in step 3-1. The expression is as follows:

5. The UAV path planning method integrating the improved APF algorithm and the improved RRT algorithm according to claim 4 is characterized in that: The temporary sub-target escape strategy described in step 4 includes the following strategies: Strategy 1: When the local minimum state is detected and there are no other obstacles on and around the line between the current position of the drone and the target: The drone falls into a local minimum due to the balance of attraction and repulsion. The temporary sub-target is set to be equal to the final target point. By increasing the proportional gain factor k of the gravitational potential field att , strengthen the force of the gravitational potential field of the target point, break the balance between gravity and repulsion, drive the drone out of the local minimum state and continue path planning; Strategy 2: When a local minimum state is detected and there is a single obstacle on the line between the drone and the target: The drone uses its own sensors to detect the environment, detect the edge geometry information of the obstacle, and selects a temporary sub-target at the edge that is closer. The new temporary sub-target temporarily replaces the global target to generate gravity. The corresponding gravity points from the drone to the temporary sub-target, breaking the balance between gravity and repulsion. The direction of the combined force points to the temporary sub-target. After the drone moves to the position of the temporary sub-target, it switches the target state to the final target point and continues path planning. Strategy 3: When the drone is detected to be in a local minimum state and is between two obstacles: The drone uses its own sensors to detect the distance from the edges of two obstacles, measures the angle between the two obstacles, and calculates the distance to the edge of the obstacle. If the distance exceeds the set safe obstacle avoidance distance, it is considered that the target position can be reached through the gap between the obstacles, and there is no need to escape the local minimum and plan the path again. Under the premise of ensuring the safety of the drone's movement, the gravitational potential field proportional gain factor k is adjusted. att Or the repulsive potential field gain factor k rep , breaking the balance between gravity and repulsion, and directly passing through the gaps between obstacles to continue path planning; Strategy 4: When the drone is detected to be in a local minimum state and obstacles exist on the line between the drone and the target point and on the other side of the drone: The drone not only needs to avoid obstacles in front of it, but also needs to consider the impact of obstacles on one side, choose the side with fewer obstacles or no obstacles to move forward, and adopt appropriate strategies to specify temporary sub-target points. The new temporary sub-target temporarily replaces the global target point to generate a gravitational potential field. The corresponding gravity points from the drone to the temporary sub-target. After the drone escapes from the local minimum, it switches to the target state, and the drone continues to plan the path until it reaches the target state. Strategy 5: When the drone is detected to be in a local minimum state and is trapped in a U-shaped obstacle; The U-shaped obstacle blocks the drone's other escape paths. At this time, the drone needs to determine the edges of the surrounding obstacles, stop the normal path planning of the APF algorithm, and move backward "against the trend field". According to whether the edge detection of the obstacle has left the U-shaped obstacle area, you can choose to specify temporary sub-targets on both sides of the edge of the U-shaped obstacle. After the drone escapes from the U-shaped obstacle area, set temporary sub-targets multiple times to avoid entering the U-shaped obstacle area again.

6. The UAV path planning method integrating the improved APF algorithm and the improved RRT algorithm according to claim 5 is characterized in that: Strategy 5 describes that the number of temporary sub-goals set by the drone after escaping the U-shaped obstacle area is 2-3 times.

7. The UAV path planning method integrating the improved APF algorithm and the improved RRT algorithm according to claim 5 or 6, characterized in that: Switching the RRT algorithm to escape the local minimum area includes the following steps: Step S1: Extended random tree construction, in the system state space, through iteration, each time trying to expand the search tree to a randomly selected new state until reaching the target state point, and finally constructing an extended random tree; Step S2: Path query, based on the expansion tree constructed in step S1, calculate the valid path from the initial state point to the target state point.

8. The UAV path planning method integrating the improved APF algorithm and the improved RRT algorithm according to claim 7 is characterized in that: Step S1 specifically includes the following steps: Step S101: Randomly sample state point q in the free state space C free Randomly sample a state point q rand ,q rand ∈C free And q rand ∈T k ; Step S102: Calculate the nearest point of the metric and traverse the extended random tree T k , where T1 = q init , according to the metric of the extended random tree, find the extended random tree T k and randomly sampled state point q rand Measuring the nearest node q near , when the metric is set distance, randomly sample state point q rand The closest state point q to the metric near The geometric relationship is expressed as Dis(q near ,q rand )≤Dis(q init ,q rand ), where (x,y)∈C free , Dis(x,y) represents the geometric distance between two state space points x and y; Step S103, calculate the new state point, based on the metric nearest state point q obtained in step S102 near Later, in q near With q rand Find the new state point q on the connection line new , and q new The following conditions must be met: new ∈C free And Dis(q near ,q new )=ξ, where ξ>0, is the fixed incremental distance of the fast expanding random tree; when measuring the nearest state point q near Based on the random sampling of state points q rand The direction of the step length is increased by one step distance to obtain the new state point q new , and at the same time ensure that the new state point q new Satisfy the global constraint of C, that is, q new ∈C free ; When the state point q new Satisfy Dis(q new ,q goal )≤ξorq new ∈C goal When , it is determined that the extended random tree has reached the target state point q goal or target area C goal , add q goal To expand the random tree, end step S1 to expand the random tree construction; When the state point q new Satisfy q new ∈C free At the same time Dis(q new ,q goal )>ξ, the expansion process is not yet completed, then the random tree T is expanded k Add leaf node q new , get a new extended random tree T k+1 , where T k+1 =T k +q new , for the new extended random tree T k+1 Re-execute step S1; If S103 does not have a node q that meets the conditions new ,Right now or Return to step S101 and randomly select another different node q rand .

9. The UAV path planning method integrating the improved APF algorithm and the improved RRT algorithm according to claim 8 is characterized in that: In step S103, the new state point q is calculated new The method is: New state point q new The calculation formula is: In the formula q new Refers to the new node to be added; q near Refers to the node closest to the random sampling point; ξ refers to the step size of the extended random tree; q rand Refers to random sampling points.

10. The UAV path planning method integrating the improved APF algorithm and the improved RRT algorithm according to claim 9 is characterized in that: The step S2 is specifically as follows: After the expansion tree obtained in step S1, from the target state point q goal Start by traversing the expanded random tree T in reverse k , traverse its parent nodes in turn until the initial state point q init So far, we get the starting point q init To the planned end point q goal of tree branches.

Images