A method for UAV trajectory planning based on an improved artificial potential field method

Abstract

This invention relates to the field of UAV trajectory planning technology, specifically a UAV trajectory planning method based on an improved artificial potential field method. Without changing the potential field function of the artificial potential field method, it achieves escape from local minima by adding moving virtual target points, offering advantages such as simplicity, low computational cost, and good real-time performance.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) trajectory planning technology, specifically to a UAV trajectory planning method based on an improved artificial potential field method. Background Technology

[0002] Drone trajectory planning refers to methods by which drones, following specific algorithms, avoid obstacles and threat zones to reach a target point. Common drone trajectory planning methods include the dynamic window method, the A* algorithm, and the artificial potential field method. Compared with other algorithms, the artificial potential field method has advantages such as low computational cost and high real-time performance. The artificial potential field method assumes that the target point exerts an attractive force on the drone, the magnitude of which is proportional to the distance between the drone and the target point; obstacles and threat zones exert a repulsive force on the drone, with the repulsive force increasing the closer the drone is to the obstacle or threat zone. The resultant force of the attractive and repulsive forces on the drone determines its next direction of movement. By changing the attractive and repulsive force coefficients or the attractive and repulsive potential field functions, different trajectory planning effects can be easily achieved. However, the traditional artificial potential field method suffers from the local minima problem. When the angle between the directions of the attractive and repulsive forces on the drone approaches 180°, the drone is prone to local oscillations and cannot reach the target. The paper "Research on Path Planning in Unknown Environments Using Improved Artificial Potential Field Method" solves the local minima problem by adding virtual target points. However, this method cancels the virtual target points when it detects that the local minima has been escaped, which may cause a sudden change in the direction of gravity and result in a sharp turning angle. Summary of the Invention

[0003] The purpose of this invention is to propose a UAV trajectory planning method based on an improved artificial potential field method, which solves the problem that the traditional artificial potential field method is prone to getting trapped in local minima in UAV path planning.

[0004] The technical solution of the present invention: In order to achieve the above-mentioned objective, according to the first aspect of the present invention, a method for UAV trajectory planning based on an improved artificial potential field method is proposed, which specifically includes the following steps:

[0005] Step 1: Define a planar map coordinate system Oxy with the plane where the drone and the target point are located as the coordinate plane and the drone's position as the origin; obtain the position coordinates of the drone, the center of the obstacle, and the target point in the map coordinate system Oxy, as well as the drone's current speed, speed direction angle, and maximum angular velocity;

[0006] Step 2: Determine if the drone has reached the target point. If yes, complete the flight path planning and end; otherwise, proceed to Step 3.

[0007] Step 3: Determine if the drone is stuck in a local minimum. If yes, proceed to Step 4; otherwise, proceed to Step 5.

[0008] In one possible embodiment, in step 3, the specific method for determining that the UAV has fallen into a local minimum includes: if the UAV repeatedly hovers in a region with a maximum distance of D within a time period t, then the UAV is considered to have fallen into a local minimum; D is not greater than 0.1×U×t, where U is the speed of the UAV.

[0009] Step 4: Based on the current position coordinates of the UAV and the coordinates of the target point, generate a virtual target point and set its movement speed and trajectory. The virtual target point is generated as follows: within the plane containing the UAV's velocity vector and the target point, the target point X... G Based on the current location X of the drone U Centered on the distance from the drone to the target point, rotate clockwise or counterclockwise by an angle α to point X. G ', with point X G 'Used as a virtual target point; the angle α ranges from 90 to 180°; the speed of the virtual target point is set to k times the speed of the UAV, where k is not less than π; the virtual target point moves toward the target point along a preset trajectory;'

[0010] Setting a virtual target point can guide the drone to jump out of a local minimum point; setting the speed of the virtual target point to k times the speed of the drone can ensure that the drone cannot catch up with the virtual target point before reaching the target point; the virtual target point moving towards the target point along a preset trajectory can guide the drone to eventually move towards the target point.

[0011] In one possible embodiment, the preset trajectory includes, but is not limited to, straight lines and arcs.

[0012] In one possible implementation, k is 4.

[0013] Step 5: Construct the gravity function and calculate the gravitational force on the UAV based on the UAV coordinates, target point coordinates, or virtual target point coordinates;

[0014] In one possible embodiment, the gravitational function formula is:

[0015] F a =k a ·d(X U -X G (1)

[0016] In the formula, k a It is the gravitational coefficient, X U This is the current position coordinate of the drone; if a virtual target point exists, X... G These are the coordinates of the virtual target point. If the virtual target point does not exist, X... G The target point's position coordinates, d(X) U -X G() is a vector, with a direction derived from X. U Point to X G The module length is X U and X G The distance between them.

[0017] Step 6: Construct the repulsion function. Calculate the repulsion force on the UAV based on the UAV coordinates, obstacle center coordinates, target point coordinates, or virtual target point coordinates.

[0018] In one possible embodiment, the repulsion function formula is:

[0019]

[0020] In the formula, k r It is the repulsion coefficient, X U This is the current position coordinate of the drone, X. Ti d(X) represents the coordinates of the center of the i-th obstacle within the UAV's field of view. Ti -X U () is a vector, with a direction derived from X. Ti Point to X U The module length is X Ti and X U The distance between them is denoted as |X Ti -X U |,|X U -X G |For X U and X G The distance between them, R i X is the radius of influence of the i-th obstacle; if a virtual target point exists, X G These are the coordinates of the virtual target point. If the virtual target point does not exist, X... G These are the coordinates of the target point.

[0021] Step 7: Calculate the net force acting on the drone based on the gravitational force calculated in Step 5 and the repulsive force calculated in Step 6;

[0022] In one possible embodiment, the formula for calculating the net force F acting on the drone is:

[0023] F = F a +F r (3).

[0024] Step 8: Calculate the drone's turning angle θ a The calculation formula is:

[0025] θ a =max(min(θ) F -θ t θ max ), -θmax ) (4)

[0026] In the formula, θ F θ is the angle between the resultant force F and the Ox axis of the map coordinate system Oxy. t θ is the current velocity direction angle of the drone. max This is the maximum angular velocity of the drone; calculate the velocity direction angle θ of the drone's next motion. t+1 t refers to the current time; the calculation formula is:

[0027] θ t+1 =θ t +θ a (5)

[0028] Step 9: If a virtual target point exists, update the position coordinates of the virtual target point based on its velocity and preset trajectory using the method described in Step 4;

[0029] Step 10: Based on the drone's speed magnitude and speed direction angle, update the drone's next position, and then return to step 2.

[0030] In one possible embodiment, the next position of the drone is calculated as follows:

[0031] X U (t+1)=X U +U·[cos(θ t+1 ),sin(θ t+1 (6)

[0032] In the formula, X U (t+1) is the next position of the drone, and U is the speed of the drone.

[0033] The advantages of this invention are: 1. The method described in this invention does not change the potential field function of the artificial potential field method. It achieves escape from the local minimum point by adding a moving virtual target point. It has the advantages of simple method, small amount of calculation and good real-time performance.

[0034] 2. Compared with existing technologies, it has the following advantages:

[0035] (1) The method for generating virtual target points is simpler and reduces the amount of computation;

[0036] (2) The virtual target point is in motion, which reduces the probability that the UAV will fall into a local minimum again;

[0037] (3) The virtual target point gradually guides the UAV to move toward the target point, avoiding large-scale turning maneuvers of the UAV. Attached Figure Description

[0038] Figure 1This is a flowchart illustrating a UAV trajectory planning method based on an improved artificial potential field method as described in this application.

[0039] Figure 2 This is a schematic diagram illustrating how the method described in this application creates a virtual target point when the UAV gets stuck in a local minimum.

[0040] Figure 3 The effect of the method described in this application on trajectory planning when the UAV is trapped in a local minimum point;

[0041] Figure 4 This demonstrates the trajectory planning effect of the method described in this application when the UAV does not get stuck in a local minimum. Detailed Implementation

[0042] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0043] The features and illustrative embodiments of various aspects of the present invention will now be described in detail. Numerous specific details are set forth in the following detailed description to provide a thorough understanding of the invention. However, it will be apparent to those skilled in the art that the invention may be practiced without requiring some of these specific details. The following description of embodiments is merely intended to provide a better understanding of the invention by illustrating examples of the invention. The invention is by no means limited to any specific setups and methods set forth below, but covers any improvements, substitutions, and modifications to structures, methods, and devices without departing from the spirit of the invention. Well-known structures and techniques are not shown in the drawings and the following description to avoid unnecessarily obscuring the invention.

[0044] It should be noted that, unless otherwise specified, the embodiments of the present invention and the features thereof can be combined with each other, and the various embodiments can be referenced and cited in each other. The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0045] Example 1

[0046] Figure 1 This is a flowchart illustrating a UAV trajectory planning method based on an improved artificial potential field method as described in this application.

[0047] Includes the following steps:

[0048] Step 1: Define a planar map coordinate system Oxy with the plane where the drone and the target point are located as the coordinate plane and the drone's position as the origin; obtain the position coordinates of the drone, the center of the obstacle, and the target point in the map coordinate system Oxy, as well as the drone's current speed, speed direction angle, and maximum angular velocity;

[0049] Step 2: Determine if the drone has reached the target point. If yes, complete the flight path planning and end; otherwise, proceed to Step 3.

[0050] Step 3: Determine if the drone is stuck in a local minimum. If yes, proceed to Step 4; otherwise, proceed to Step 5.

[0051] In step 3, the specific method for determining whether the drone has fallen into a local minimum includes: if the drone repeatedly hovers in an area with a maximum distance of D within a time period of 10 seconds, then the drone is considered to have fallen into a local minimum; D is not greater than 0.1×U×10, where U is the speed of the drone;

[0052] Step 4: Based on the current position coordinates of the UAV and the target point coordinates, generate a virtual target point and set its movement speed and trajectory; the method for generating the virtual target point is shown in the attached figure. Figure 2 As shown, specifically: in the plane containing the UAV's velocity vector and the target point, the target point X... G Based on the current location X of the drone U Centered on point X, and using the distance from the drone to the target point as the radius, rotate 90° clockwise to point X. G ', with point X G 'Used as a virtual target point; the speed of the virtual target point is set to 4 times the speed of the drone; the virtual target point moves towards the target point in a preset arc trajectory;'

[0053] The benefits of setting a virtual target point are as follows: setting a virtual target point can guide the drone to jump out of the local minimum point; setting the speed of the virtual target point to 4 times the speed of the drone can ensure that the drone cannot catch up with the virtual target point before reaching the target point; the virtual target point moving towards the target point along a preset trajectory can guide the drone to eventually move towards the target point.

[0054] Step 5: Construct the gravity function and calculate the gravitational force on the UAV based on the UAV coordinates, target point coordinates, or virtual target point coordinates;

[0055] The formula for the gravitational function is:

[0056] F a =k a ·d(X U -X G (1)

[0057] In the formula, k a It is the gravitational coefficient, k a =1; X U This is the current position coordinate of the drone; if a virtual target point exists, X... G These are the coordinates of the virtual target point. If the virtual target point does not exist, X... G The target point's position coordinates, d(X) U -X G () is a vector, with a direction derived from X. U Point to X G The module length is X U and X G The distance between them.

[0058] Step 6: Construct the repulsion function. Calculate the repulsion force on the UAV based on the UAV coordinates, obstacle center coordinates, target point coordinates, or virtual target point coordinates.

[0059] The formula for the repulsive force function is:

[0060]

[0061] In the formula, k r It is the repulsion coefficient, k r =1,000,000; X U This is the current position coordinate of the drone, X. Ti d(X) represents the coordinates of the center of the i-th obstacle within the UAV's field of view. Ti -X U () is a vector, with a direction derived from X. Ti Point to X U The module length is X Ti and X U The distance between them is denoted as |X Ti -X U |,|X U -X G |For X U and X G The distance between them, R i R is the radius of influence of the i-th obstacle. i =1000; if a virtual target point exists, X G These are the coordinates of the virtual target point. If the virtual target point does not exist, X... G These are the coordinates of the target point.

[0062] Step 7: Calculate the gravitational force F based on Step 5. a And the repulsive force F calculated in step 6 r Calculate the net force F acting on the drone. The formula for F is:

[0063] F = F a +Fr (3)

[0064] Step 8: Calculate the drone's turning angle θ a The calculation formula is:

[0065] θ a =max(min(θ) F -θ t θ max ), -θ max ) (4)

[0066] In the formula, θ F It is the angle between the resultant force F and the Ox axis of the map coordinate system Oxy; θ t θ is the current velocity direction angle of the drone. max It is the maximum angular velocity of the drone, θ max =π; Calculate the velocity direction angle θ of the drone's next movement. t+1 t refers to the current time; the calculation formula is:

[0067] θ t+1 =θ t +θ a (5)

[0068] Step 9: If a virtual target point exists, update the position coordinates of the virtual target point based on its velocity and preset trajectory using the method described in Step 4;

[0069] Step 10: Based on the drone's speed magnitude and speed direction angle, update the drone's next position, and then return to step 2.

[0070] The method for calculating the drone's next position is as follows:

[0071] X U (t+1)=X U +U·[cos(θ t+1 ),sin(θ t+1 (6)

[0072] In the formula, X U (t+1) is the next position of the drone, and U is the speed of the drone, U = 100.

[0073] The above steps were simulated using MATLAB to obtain the results. Figure 3 The flight path planning results shown indicate that when the UAV gets stuck in a local minimum, it successfully escapes the local minimum under the guidance of the virtual target point and reaches the target.

[0074] Example 2

[0075] Step 1: Define a planar map coordinate system Oxy with the plane where the drone and the target point are located as the coordinate plane and the drone's position as the origin; obtain the position coordinates of the drone, the center of the obstacle, and the target point in the map coordinate system Oxy, as well as the drone's current speed, speed direction angle, and maximum angular velocity;

[0076] Step 2: Determine if the drone has reached the target point. If yes, complete the flight path planning and end; otherwise, proceed to Step 3.

[0077] Step 3: Determine if the drone is stuck in a local minimum. If yes, proceed to Step 4; otherwise, proceed to Step 5.

[0078] In step 3, the specific method for determining whether the drone has fallen into a local minimum includes: if the drone repeatedly hovers in an area with a maximum distance of D within a time period of 10 seconds, then the drone is considered to have fallen into a local minimum; D is not greater than 0.1×U×10, where U is the speed of the drone;

[0079] Step 4: Based on the current position coordinates of the UAV and the target point coordinates, generate a virtual target point and set its movement speed and trajectory; the method for generating the virtual target point is shown in the attached figure. Figure 2 As shown, specifically: in the plane containing the UAV's velocity vector and the target point, the target point X... G Based on the current location X of the drone U Centered on point X, and using the distance from the drone to the target point as the radius, rotate 90° clockwise to point X. G ', with point X G 'Used as a virtual target point; the speed of the virtual target point is set to 4 times the speed of the drone; the virtual target point moves towards the target point in a preset arc trajectory;'

[0080] The benefits of setting a virtual target point are as follows: setting a virtual target point can guide the drone to jump out of the local minimum point; setting the speed of the virtual target point to 4 times the speed of the drone can ensure that the drone cannot catch up with the virtual target point before reaching the target point; the virtual target point moving towards the target point along a preset trajectory can guide the drone to eventually move towards the target point.

[0081] Step 5: Construct the gravity function and calculate the gravitational force on the UAV based on the UAV coordinates, target point coordinates, or virtual target point coordinates;

[0082] The formula for the gravitational function is:

[0083] F a =k a ·d(X U -X G (1)

[0084] In the formula, k a It is the gravitational coefficient, k a =1; X U This is the current position coordinate of the drone; if a virtual target point exists, X... G These are the coordinates of the virtual target point. If the virtual target point does not exist, X... G The target point's position coordinates, d(X) U -X G () is a vector, with a direction derived from X. U Point to X G The module length is X U and X G The distance between them.

[0085] Step 6: Construct the repulsion function. Calculate the repulsion force on the UAV based on the UAV coordinates, obstacle center coordinates, target point coordinates, or virtual target point coordinates.

[0086] The formula for the repulsive force function is:

[0087]

[0088] In the formula, k r It is the repulsion coefficient, k r =1,000,000; X U This is the current position coordinate of the drone, X. Ti d(X) represents the coordinates of the center of the i-th obstacle within the UAV's field of view. Ti -X U () is a vector, with a direction derived from X. Ti Point to X U The module length is X Ti and X U The distance between them is denoted as |X Ti -X U |,|X U -X G |For X U and X G The distance between them, R i R is the radius of influence of the i-th obstacle. i =1000; if a virtual target point exists, X G These are the coordinates of the virtual target point. If the virtual target point does not exist, X... G These are the coordinates of the target point.

[0089] Step 7: Calculate the gravitational force F based on Step 5. a And the repulsive force F calculated in step 6 r Calculate the net force F acting on the drone. The formula for F is:

[0090] F = F a +Fr (3)

[0091] Step 8: Calculate the drone's turning angle θ a The calculation formula is:

[0092] θ a =max(min(θ) F -θ t θ max ), -θ max ) (4)

[0093] In the formula, θ F It is the angle between the resultant force F and the Ox axis of the map coordinate system Oxy; θ t θ is the current velocity direction angle of the drone. max It is the maximum angular velocity of the drone, θ max =π; Calculate the velocity direction angle θ of the drone's next movement. t+1 t refers to the current time; the calculation formula is:

[0094] θ t+1 =θ t +θ a (5)

[0095] Step 9: If a virtual target point exists, update the position coordinates of the virtual target point based on its velocity and preset trajectory using the method described in Step 4;

[0096] Step 10: Based on the drone's speed magnitude and speed direction angle, update the drone's next position, and then return to step 2.

[0097] The method for calculating the drone's next position is as follows:

[0098] X U (t+1)=X U +U·[cos(θ t+1 ),sin(θ t+1 (6)

[0099] In the formula, X U (t+1) is the next position of the drone, and U is the speed of the drone, U = 100.

[0100] The above steps were simulated using MATLAB to obtain the results. Figure 4 The flight path planning results shown indicate that when the UAV does not get stuck in a local minimum, the method described in this application does not create a virtual target point, and the UAV avoids obstacles to reach the target point.

Claims

1. A method for UAV trajectory planning based on an improved artificial potential field method, characterized in that, Specifically, the steps include the following: Step 1: Define a planar map coordinate system Oxy with the plane where the drone and the target point are located as the coordinate plane and the drone's position as the origin; obtain the position coordinates of the drone, the center of the obstacle, and the target point in the map coordinate system Oxy, as well as the drone's current speed, speed direction angle, and maximum angular velocity; Step 2: Determine if the drone has reached the target point. If yes, complete the flight path planning and end; otherwise, proceed to Step 3. Step 3: Determine if the drone is stuck in a local minimum. If yes, proceed to Step 4; otherwise, proceed to Step 5. Step 4: Based on the current position coordinates of the UAV and the coordinates of the target point, generate a virtual target point and set its movement speed and trajectory. The virtual target point is generated as follows: within the plane containing the UAV's velocity vector and the target point, the target point X... G Based on the current location X of the drone U Centered on the distance from the drone to the target point, rotate clockwise or counterclockwise by an angle α to point X. G ', with point X G 'Used as a virtual target point; the angle α ranges from 90 to 180°; the speed of the virtual target point is set to k times the speed of the UAV, where k is not less than π; the virtual target point moves toward the target point along a preset trajectory;' Step 5: Construct the gravity function and calculate the gravitational force on the UAV based on the UAV coordinates, target point coordinates, or virtual target point coordinates; Step 6: Construct the repulsion function. Calculate the repulsion force on the UAV based on the UAV coordinates, obstacle center coordinates, target point coordinates, or virtual target point coordinates. Step 7: Calculate the net force acting on the drone based on the gravitational force calculated in Step 5 and the repulsive force calculated in Step 6; Step 8: Calculate the drone's turning angle θ a , the calculation formula is: i a =max(min(θ F -θ t i max ),-θ max ) (4) In the formula, θ F θ is the angle between the resultant force F and the Ox axis of the map coordinate system Oxy. t θ is the current velocity direction angle of the drone. max This is the maximum angular velocity of the drone; calculate the velocity direction angle θ of the drone's next motion. t+1 t refers to the current time; the calculation formula is: i t+1 =θ t +θ a (5) Step 9: If a virtual target point exists, update the position coordinates of the virtual target point based on its velocity and preset trajectory using the method described in Step 4; Step 10: Based on the drone's speed magnitude and speed direction angle, update the drone's next position, and then return to step 2.

2. The UAV trajectory planning method based on the improved artificial potential field method according to claim 1, characterized in that, In step 3, the specific method for determining whether the UAV has fallen into a local minimum includes: if the UAV repeatedly hovers in a region with a maximum distance of D within a time period t, then the UAV is considered to have fallen into a local minimum; D is not greater than 0.1×U×t, where U is the speed of the UAV.

3. The UAV trajectory planning method based on the improved artificial potential field method according to claim 1, characterized in that, In step 4, the preset trajectory includes, but is not limited to, straight lines and arcs.

4. The UAV trajectory planning method based on the improved artificial potential field method according to claim 1, characterized in that, In step 4, k is set to 4.

5. The UAV trajectory planning method based on the improved artificial potential field method according to claim 1, characterized in that, In step 5, the formula for the gravitational function is: F a =k a ·d(X U -X G ) (1) In the formula, k a It is the gravitational coefficient, X U This is the current position coordinate of the drone; if a virtual target point exists, X... G These are the coordinates of the virtual target point. If the virtual target point does not exist, X... G The target point's position coordinates, d(X) U -X G () is a vector, with a direction derived from X. U Point to X G The module length is X U and X G The distance between them.

6. The UAV trajectory planning method based on the improved artificial potential field method according to claim 1, characterized in that, In step 6, the repulsive force function formula is: In the formula, k r It is the repulsion coefficient, X U This is the current position coordinate of the drone, X. Ti d(X) represents the coordinates of the center of the i-th obstacle within the UAV's field of view. Ti -X U () is a vector, with a direction derived from X. Ti Point to X U The module length is X Ti and X U The distance between them is denoted as |X Ti -X U |,|X U -X G |For X U and X G The distance between them, R i X is the radius of influence of the i-th obstacle; if a virtual target point exists, X G These are the coordinates of the virtual target point. If the virtual target point does not exist, X... G These are the coordinates of the target point.

7. The UAV trajectory planning method based on the improved artificial potential field method according to claim 1, characterized in that, In step 7, the formula for calculating the resultant force F acting on the UAV is: F=F a +F r (3)。 8. The UAV trajectory planning method based on the improved artificial potential field method according to claim 1, characterized in that, In step 10, the next position of the drone is calculated as follows: X U (t+1)=X U +U·[cos(θ t+1 ),sin(θ t+1 )] (6) In the formula, X U (t+1) is the next position of the drone, and U is the speed of the drone.

Images