Unmanned aerial vehicle flight path planning method based on improved artificial potential field method

Abstract

The invention relates to the technical field of unmanned aerial vehicle route planning, in particular to an unmanned aerial vehicle route planning method based on an improved artificial potential field method. The potential field function of the artificial potential field method is not changed, the local minimum point is escaped by adding the moving virtual target point, and the method has the advantages of being simple, small in calculation amount and good in real-time performance.

Description

Technical Field

[0001] The invention relates to the technical field of unmanned aerial vehicle (UAV) trajectory planning, and in particular to a UAV trajectory planning method based on an improved artificial potential field method. Background Art

[0002] The trajectory planning method of a UAV refers to the method by which a UAV avoids obstacles, threat areas, etc. and reaches the target point according to a certain algorithm logic. Common UAV trajectory planning methods include dynamic window method, A* algorithm, artificial potential field method, etc. Compared with other algorithms, the artificial potential field method has the advantages of small computational complexity and high real-time performance. The artificial potential field method assumes that the target point exerts gravity on the UAV, and the magnitude of the gravity is proportional to the distance from the UAV to the target point; obstacles, threat areas, etc. exert repulsion on the UAV, and the closer to the obstacle or threat area, the greater the repulsion. The combined force of gravity and repulsion on the UAV is the next direction of the UAV. By changing the gravity coefficient, repulsion coefficient, or gravity and repulsion potential field functions, different trajectory planning effects can be easily achieved. However, the traditional artificial potential field method has the problem of local minimum points. When the angle between the gravity and repulsion directions of the UAV is close to 180°, it is easy for the UAV to fall into local oscillation and wandering, and cannot reach the target. The paper "Research on Path Planning in Unknown Environments with Improved Artificial Potential Field Method" solves the problem of local minimum points by adding virtual target points. However, this method will cancel the virtual target points when it detects that the local minimum point has been jumped out, which may cause a large sudden change in the direction of gravity and a problem of sharp corners. Summary of the invention

[0003] The purpose of the present invention is to propose a UAV trajectory planning method based on an improved artificial potential field method to solve the problem that the traditional artificial potential field method is prone to fall into local minima in UAV path planning.

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

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

[0006] Step 2: Determine whether the drone has reached the target point. If so, complete the track planning and end; if not, proceed to step 3;

[0007] Step 3: Determine whether the drone is trapped in a local minimum point. If so, proceed to step 4; if not, proceed to step 5;

[0008] In a possible embodiment, in step 3, the specific method for determining whether the drone is trapped in a local minimum point includes: if the drone repeatedly wanders in an area with a maximum distance of D within a time period t, the drone is considered to be trapped in a local minimum point; D is not greater than 0.1×U×t, and U is the speed of the drone.

[0009] Step 4: Generate a virtual target point based on the current position coordinates of the drone and the target point coordinates, and set the virtual target point movement speed and trajectory; the virtual target point is generated in the following way: in the plane where the drone velocity vector and the target point are located, the target point X G Take the current position of the drone X U As the center, with the distance from the drone to the target point as the radius, rotate clockwise or counterclockwise at an angle α to point X G ', with point X G ' as a virtual target point; the value range of angle α is 90 to 180°; the speed of the virtual target point is set to k times the speed of the drone, k is not less than π; the virtual target point moves toward the target point along a preset trajectory;

[0010] By setting a virtual target point, the drone can be guided to jump out of the local minimum point; the speed of the virtual target point is set to k times the speed of the drone, which can ensure that the drone cannot catch up with the virtual target point before reaching the target point; the virtual target point moves toward the target point along a preset trajectory, which can guide the drone to finally move towards the target point.

[0011] In a possible embodiment, the preset trajectory includes but is not limited to a straight line and an arc.

[0012] In a possible embodiment, the value of k is 4.

[0013] Step 5: Construct a gravity function, and calculate the gravity on the drone based on the gravity function according to the drone coordinates, target point coordinates or virtual target point coordinates;

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

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

[0016] In the formula, k a is the gravitational coefficient, X U is the current position coordinate of the drone; if there is a virtual target point, X G is the virtual target point position coordinate. If there is no virtual target point, X G is the target point position coordinate, d(X U -X G) is a vector, and its direction is from X U Point to X G , the module length is X U and X G The distance between.

[0017] Step 6: Construct a repulsion function, and calculate the repulsion force on the drone based on the repulsion function according to the drone coordinates, obstacle center coordinates, target point coordinates or virtual target point coordinates;

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

[0019]

[0020] In the formula, k r is the repulsion coefficient, X U is the current position coordinate of the drone, X Ti is the coordinate of the center position of the ith obstacle in the UAV’s field of view, d(X Ti -X U ) is a vector, and its direction is from X Ti Point to X U , the module length is X Ti and X U The distance between them is represented by |X Ti -X U |,|X U -X G | for X U and X G The distance between i is the influence radius of the ith obstacle; if there is a virtual target point, X G is the virtual target point position coordinate. If there is no virtual target point, X G are the target point location coordinates.

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

[0022] In a possible embodiment, the calculation formula of the resultant force F acting on the drone is:

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

[0024] Step 8: Calculate the steering angle θ of the drone 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 speed angle of the drone, θ max is the maximum angular velocity of the drone; calculate the speed direction angle θ of the drone's next movement t+1 , t refers to the current moment; the calculation formula is:

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

[0028] Step 9: If there is a virtual target point, based on the speed of the virtual target point and the preset trajectory, the method in step 4 is used to update the position coordinates of the virtual target point;

[0029] Step 10: Based on the speed and direction of the drone, update the next position of the drone, 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 the present invention are: 1. The method of the present invention does not change the potential field function of the artificial potential field method, and escapes from the local minimum point by adding a moving virtual target point, which has the advantages of simple method, small calculation amount and good real-time performance;

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

[0035] (1) The method for generating virtual target points is simpler, reducing the amount of calculation;

[0036] (2) The virtual target point is in motion, which reduces the probability of the drone falling into the local minimum point again;

[0037] (3) The virtual target point gradually guides the UAV to move toward the target point, preventing the UAV from making large turns. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] Figure 1A schematic diagram of the flow of a UAV trajectory planning method based on an improved artificial potential field method according to the method described in this application;

[0039] Figure 2 A schematic diagram of creating a virtual target point when the UAV falls into a local minimum point according to the method of the present application;

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

[0041] Figure 4 This is the trajectory planning effect of the method described in this application when the UAV does not fall into a local minimum point. DETAILED DESCRIPTION

[0042] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0043] The features and illustrative embodiments of various aspects of the present invention will be described in detail below. In the detailed description below, many specific details are proposed to provide a comprehensive understanding of the present invention. However, it is obvious to those skilled in the art that the present invention can be implemented without the need for some of these specific details. The following description of the embodiments is only to provide a better understanding of the present invention by illustrating examples of the present invention. The present invention is by no means limited to any specific settings and methods proposed below, but covers any improvements, replacements and modifications of structures, methods, devices without departing from the spirit of the present invention. In the accompanying drawings and the following description, known structures and technologies are not shown to avoid unnecessary ambiguity in the present invention.

[0044] It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments can be combined with each other, and the embodiments can refer to and quote each other. The present invention will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0045] Example 1

[0046] Figure 1 This is a flow chart of a UAV trajectory planning method based on an improved artificial potential field method according to the method described in this application.

[0047] The following steps are involved:

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

[0049] Step 2: Determine whether the drone has reached the target point. If so, complete the track planning and end; if not, proceed to step 3;

[0050] Step 3: Determine whether the drone is trapped in a local minimum point. If so, proceed to step 4; if not, proceed to step 5;

[0051] In step 3, the specific method for judging whether the drone is trapped in a local minimum point includes: if the drone repeatedly wanders in an area with a maximum distance of D within a time period of 10 seconds, the drone is considered to be trapped in a local minimum point; D is not greater than 0.1×U×10, and U is the speed of the drone;

[0052] Step 4: Generate a virtual target point based on the current position coordinates of the drone and the target point coordinates, and set the movement speed and trajectory of the virtual target point; the method for generating the virtual target point is as shown in the attached figure. Figure 2 As shown, specifically: in the plane where the UAV velocity vector and the target point are located, the target point X G Take the current position of the drone X U With the distance from the drone to the target point as the radius, rotate 90° clockwise to point X G ', with point X G ' 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 toward the target point along a preset arc trajectory;

[0053] The beneficial effects of setting a virtual target point are as follows: by setting a virtual target point, the drone can be guided to jump out of the local minimum point; the speed of the virtual target point is set to 4 times the speed of the drone, which can ensure that the drone cannot catch up with the virtual target point before reaching the target point; the virtual target point moves toward the target point along a preset trajectory, which can guide the drone to finally move toward the target point;

[0054] Step 5: Construct a gravity function, and calculate the gravity on the drone based on the gravity function according to the drone coordinates, target point coordinates or virtual target point coordinates;

[0055] The gravitational function formula is:

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

[0057] In the formula, k a is the gravitational coefficient, k a =1;X U is the current position coordinate of the drone; if there is a virtual target point, X G is the virtual target point position coordinate. If there is no virtual target point, X G is the target point position coordinate, d(X U -X G ) is a vector, and its direction is from X U Point to X G , the module length is X U and X G The distance between.

[0058] Step 6: Construct a repulsion function, and calculate the repulsion force on the drone based on the repulsion function according to the drone coordinates, obstacle center coordinates, target point coordinates or virtual target point coordinates;

[0059] The repulsion function formula is:

[0060]

[0061] In the formula, k r is the repulsion coefficient, k r =1000000;X U is the current position coordinate of the drone, X Ti is the coordinate of the center position of the ith obstacle in the UAV’s field of view, d(X Ti -X U ) is a vector, and its direction is from X Ti Point to X U , the module length is X Ti and X U The distance between them is represented by |X Ti -X U |,|X U -X G | for X U and X G The distance between i is the influence radius of the ith obstacle, R i =1000; if there is a virtual target point, X G is the virtual target point position coordinate. If there is no virtual target point, X G are the target point location coordinates.

[0062] Step 7: Based on the gravity F calculated in step 5 a and the repulsive force F calculated in step 6 r , calculate the resultant force F on the drone, the calculation formula for F is:

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

[0064] Step 8: Calculate the steering angle θ of the drone a , the calculation formula is:

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

[0066] 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 speed angle of the drone, θ max is the maximum angular velocity of the drone, θ max =π; calculate the speed direction angle θ of the next movement of the drone t+1 , t refers to the current moment; the calculation formula is:

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

[0068] Step 9: If there is a virtual target point, based on the speed of the virtual target point and the preset trajectory, the method in step 4 is used to update the position coordinates of the virtual target point;

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

[0070] The next position of the drone is calculated as:

[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, U is the speed of the drone, U=100.

[0073] Use MATLAB to simulate the above steps and get Figure 3 From the trajectory planning results shown, it can be seen that when the UAV is trapped in the local minimum point, it successfully escapes from the local minimum point and reaches the target under the guidance of the virtual target point.

[0074] Example 2

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

[0076] Step 2: Determine whether the drone has reached the target point. If so, complete the track planning and end; if not, proceed to step 3;

[0077] Step 3: Determine whether the drone is trapped in a local minimum point. If so, proceed to step 4; if not, proceed to step 5;

[0078] In step 3, the specific method for judging whether the drone is trapped in a local minimum point includes: if the drone repeatedly wanders in an area with a maximum distance of D within a time period of 10 seconds, the drone is considered to be trapped in a local minimum point; D is not greater than 0.1×U×10, and U is the speed of the drone;

[0079] Step 4: Generate a virtual target point based on the current position coordinates of the drone and the target point coordinates, and set the movement speed and trajectory of the virtual target point; the method for generating the virtual target point is as shown in the attached figure. Figure 2 As shown, specifically: in the plane where the UAV velocity vector and the target point are located, the target point X G Take the current position of the drone X U With the distance from the drone to the target point as the radius, rotate 90° clockwise to point X G ', with point X G ' 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 toward the target point along a preset arc trajectory;

[0080] The beneficial effects of setting a virtual target point are as follows: by setting a virtual target point, the drone can be guided to jump out of the local minimum point; the speed of the virtual target point is set to 4 times the speed of the drone, which can ensure that the drone cannot catch up with the virtual target point before reaching the target point; the virtual target point moves toward the target point along a preset trajectory, which can guide the drone to finally move toward the target point;

[0081] Step 5: Construct a gravity function, and calculate the gravity on the drone based on the gravity function according to the drone coordinates, target point coordinates or virtual target point coordinates;

[0082] The gravitational function formula is:

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

[0084] In the formula, k a is the gravitational coefficient, k a =1;X U is the current position coordinate of the drone; if there is a virtual target point, X G is the virtual target point position coordinate. If there is no virtual target point, X G is the target point position coordinate, d(X U -X G ) is a vector, and its direction is from X U Point to X G , the module length is X U and X G The distance between.

[0085] Step 6: Construct a repulsion function, and calculate the repulsion force on the drone based on the repulsion function according to the drone coordinates, obstacle center coordinates, target point coordinates or virtual target point coordinates;

[0086] The repulsion function formula is:

[0087]

[0088] In the formula, k r is the repulsion coefficient, k r =1000000;X U is the current position coordinate of the drone, X Ti is the coordinate of the center position of the ith obstacle in the UAV’s field of view, d(X Ti -X U ) is a vector, and its direction is from X Ti Point to X U , the module length is X Ti and X U The distance between them is represented by |X Ti -X U |,|X U -X G | for X U and X G The distance between i is the influence radius of the ith obstacle, R i =1000; if there is a virtual target point, X G is the virtual target point position coordinate. If there is no virtual target point, X G are the target point location coordinates.

[0089] Step 7: Based on the gravity F calculated in step 5 a and the repulsive force F calculated in step 6 r , calculate the resultant force F on the drone, the calculation formula for F is:

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

[0091] Step 8: Calculate the steering angle θ of the drone a , the calculation formula is:

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

[0093] 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 speed angle of the drone, θ max is the maximum angular velocity of the drone, θ max =π; calculate the speed direction angle θ of the next movement of the drone t+1 , t refers to the current moment; the calculation formula is:

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

[0095] Step 9: If there is a virtual target point, based on the speed of the virtual target point and the preset trajectory, the method in step 4 is used to update the position coordinates of the virtual target point;

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

[0097] The next position of the drone is calculated as:

[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, U is the speed of the drone, U=100.

[0100] Use MATLAB to simulate the above steps and get Figure 4 From the trajectory planning results shown, it can be seen that when the UAV is not trapped in the local minimum point, the method described in the present application does not create a virtual target point, and the UAV avoids obstacles and reaches the target point.

Claims

1. A UAV trajectory planning method based on an improved artificial potential field method, characterized in that: The specific steps include: Step 1: Define the plane map coordinate system Oxy with the plane where the drone and the target point are located as the coordinate plane and the drone position as the coordinate origin; obtain the position coordinates of the drone, obstacle center and target point in the map coordinate system Oxy, as well as the current speed, speed direction angle and maximum angular velocity of the drone; Step 2: Determine whether the drone has reached the target point. If so, complete the track planning and end; if not, proceed to step 3; Step 3: Determine whether the drone is trapped in a local minimum point. If so, proceed to step 4; if not, proceed to step 5; Step 4: Generate a virtual target point based on the current position coordinates of the drone and the target point coordinates, and set the virtual target point movement speed and trajectory; the virtual target point is generated in the following way: in the plane where the drone velocity vector and the target point are located, the target point X G Take the current position of the drone X U As the center, with the distance from the drone to the target point as the radius, rotate clockwise or counterclockwise at an angle α to point X G ', with point X G ' as a virtual target point; the value range of angle α is 90 to 180°; the speed of the virtual target point is set to k times the speed of the drone, k is not less than π; the virtual target point moves toward the target point along a preset trajectory; Step 5: Construct a gravity function, and calculate the gravity on the drone based on the gravity function according to the drone coordinates, target point coordinates or virtual target point coordinates; Step 6: Construct a repulsion function, and calculate the repulsion force on the drone based on the repulsion function according to the drone coordinates, obstacle center coordinates, target point coordinates or virtual target point coordinates; Step 7: Calculate the resultant force on the drone based on the gravitational force calculated in step 5 and the repulsive force calculated in step 6; Step 8: Calculate the steering angle θ of the drone 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 speed angle of the drone, θ max is the maximum angular velocity of the drone; calculate the speed direction angle θ of the drone's next movement t+1 , t refers to the current moment; the calculation formula is: i t+1 =θ t +θ a (5) Step 9: If there is a virtual target point, based on the speed of the virtual target point and the preset trajectory, the method in step 4 is used to update the position coordinates of the virtual target point; Step 10: Based on the speed and direction of the drone, update the next position of the drone, and then return to step 2.

2. The unmanned aerial vehicle trajectory planning method based on the improved artificial potential field method according to claim 1 is characterized in that: In step 3, the specific method for determining whether the drone is trapped in a local minimum point includes: if the drone repeatedly wanders in an area with a maximum distance of D within a time period t, the drone is considered to be trapped in a local minimum point; D is not greater than 0.1×U×t, and U is the speed of the drone.

3. The unmanned aerial vehicle trajectory planning method based on the improved artificial potential field method according to claim 1 is characterized in that: In step 4, the preset trajectory includes but is not limited to a straight line and an arc.

4. The unmanned aerial vehicle trajectory planning method based on the improved artificial potential field method according to claim 1 is characterized in that: In step 4, the value of k is 4.

5. The unmanned aerial vehicle trajectory planning method based on the improved artificial potential field method according to claim 1 is characterized in that: In step 5, the gravity function formula is: F a =k a ·d(X U -X G ) (1) In the formula, k a is the gravitational coefficient, X U is the current position coordinate of the drone; if there is a virtual target point, X G is the virtual target point position coordinate. If there is no virtual target point, X G is the target point position coordinate, d(X U -X G ) is a vector, and its direction is from X U Point to X G , the module length is X U and X G The distance between.

6. The unmanned aerial vehicle trajectory planning method based on the improved artificial potential field method according to claim 1 is characterized in that: In step 6, the repulsion function formula is: In the formula, k r is the repulsion coefficient, X U is the current position coordinate of the drone, X Ti is the coordinate of the center position of the ith obstacle in the UAV’s field of view, d(X Ti -X U ) is a vector, and its direction is from X Ti Point to X U , the module length is X Ti and X U The distance between them is represented by |X Ti -X U |,|X U -X G | for X U and X G The distance between i is the influence radius of the ith obstacle; if there is a virtual target point, X G is the virtual target point position coordinate. If there is no virtual target point, X G are the target point location coordinates.

7. The unmanned aerial vehicle trajectory planning method based on the improved artificial potential field method according to claim 1 is characterized in that: In step 7, the calculation formula of the resultant force F applied to the drone is: F=F a +F r (3)。 8. The unmanned aerial vehicle trajectory planning method based on the improved artificial potential field method according to claim 1 is 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) Where, X U (t+1) is the next position of the drone, and U is the speed of the drone.

Images