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

Abstract

The invention relates to an unmanned aerial vehicle flight path planning method based on an angle constraint artificial potential field method, and belongs to the field of unmanned aerial vehicle flight path planning. The method comprises the following steps: firstly, modeling a three-dimensional terrain and a building environment space, replacing different buildings with cylindrical obstacles, representing the environment as an artificial potential field, constructing a gravitational field by taking a target point as a center, and constructing a repulsive force field near each cylindrical obstacle; under the combined action of a gravitational field generated by the target point and a repulsive force field generated by the cylindrical obstacle, the unmanned aerial vehicle gradually approaches the target point in the descending direction of the combined potential field, and it is ensured that the unmanned aerial vehicle finally and accurately arrives at the target position while avoiding the obstacle. Then, calculation of the repulsive force between the unmanned aerial vehicle and the cylindrical obstacle is replaced, and when the unmanned aerial vehicle, the cylindrical obstacle and the target point are not collinear, the repulsive force between the unmanned aerial vehicle and the cylindrical obstacle is converted into the repulsive force between the unmanned aerial vehicle and the circle center of the top surface; and when the unmanned aerial vehicle, the cylindrical obstacle and the target point are collinear, the repulsive force between the unmanned aerial vehicle and the cylindrical obstacle is converted into the repulsive force between the unmanned aerial vehicle and the nearest threat point. Meanwhile, an angle mechanism is introduced, the magnitude and direction of repulsive force under the collinear condition are changed, and the unmanned aerial vehicle is prevented from falling into a local minimum value. And finally, the unmanned aerial vehicle accurately arrives at a target point along the shortest and safest route by optimizing the route planning. The method provided by the invention is more efficient in calculation efficiency, and also has stronger guarantee in security.

Description

Technical Field

[0001] The present invention belongs to the field of UAV trajectory planning, and relates to a UAV trajectory planning method based on an angle-constrained artificial potential field method. Background Art

[0002] Logistics and distribution play a crucial role in the modern economy. It not only affects the circulation efficiency of goods but also is directly related to the operating costs of enterprises, customer satisfaction, and market competitiveness. First of all, logistics and distribution are the core links of the supply chain, which ensure the smooth flow of goods from the production end to the hands of consumers. An efficient logistics system can significantly shorten the delivery time of products, meet the market's demand for rapid response, especially in the e-commerce industry and the fast-moving consumer goods field. The distribution link directly determines the timeliness and accuracy of goods. Any delay or error may lead to customer dissatisfaction and even affect the brand reputation. In addition, the optimization of logistics and distribution can help enterprises reduce transportation costs, improve inventory management efficiency, and reduce waste, thereby improving the overall operating efficiency. With the development of technology, intelligent logistics and automated distribution have gradually become the key means to improve logistics efficiency and reduce costs. UAVs can fly directly without being affected by road traffic, which enables packages to be delivered from warehouses or distribution centers to consumers faster, especially in busy urban or remote areas. For urgent or time-sensitive packages, using UAVs for delivery can greatly shorten the waiting time and meet the demand for fast delivery. For example, in scenarios that require rapid response such as medical supplies and emergency rescue materials, UAVs are the ideal choice.

[0003] To realize the application of UAVs in logistics and distribution, a key issue lies in trajectory planning. The research on this issue has continued for many years, and researchers have proposed various algorithms. Among them, the artificial potential field method, as a real-time online trajectory planning algorithm, can effectively meet the requirements of UAV three-dimensional trajectory planning. This method has the advantages of short calculation time, meeting the requirements of real-time control, and being able to generate safe and smooth planned paths. However, the artificial potential field method also has some deficiencies. In some cases, there are problems such as the target being unreachable, being easily trapped in local minima, and path oscillation. Summary of the Invention

[0004] Technical Problems to be Solved

[0005] In order to avoid the deficiencies of the prior art, the present invention proposes a UAV trajectory planning method based on an angle-constrained artificial potential field method to solve the UAV trajectory planning problem in logistics and distribution.

[0006] Technical Solution

[0007] A UAV trajectory planning method based on an angle-constrained artificial potential field method, characterized in that the steps are as follows:

[0008] Step 1: Model the three-dimensional terrain and built environment space. Replace different buildings with cylindrical obstacles, represent the environment as an artificial potential field, construct a gravitational field centered on the target point, and construct a repulsive field near each cylindrical obstacle;

[0009] Step 2: According to the established artificial potential field, calculate the resultant force currently acting on the UAV, and make the UAV move forward under the action of the resultant force. Set the initial situation of the UAV as non-collinear. At this time, the repulsive force between the UAV and the cylindrical obstacle is equivalent to the repulsive force between the UAV and the center of the top surface of the cylindrical obstacle. The resultant force acting on the UAV is calculated as follows:

[0010] F = F att + F rep

[0011] where F att is the gravitational force acting on the UAV, and F rep is the repulsive force acting on the UAV;

[0012] Step 3: The UAV continues to fly towards the target point. When the UAV faces the crisis of local minimum, that is, the UAV, the cylindrical obstacle, and the target point are collinear, calculate the position of the nearest threat point. At this time, the repulsive force between the UAV and the cylindrical obstacle is equivalent to the repulsive force between the UAV and the nearest threat point. The calculation of the nearest threat point is as follows:

[0013]

[0014] where X t =(x t , y t , z t ) are the coordinates of the nearest threat point, X c =(x c , y c , z c ) are the coordinates of the center of the cylindrical obstacle at the same height as the UAV, R c is the radius of the cylindrical obstacle, X = (x, y, z) are the coordinates of the UAV, and ρ(X, X c ) is the Euclidean distance between the UAV and the center of the cylindrical obstacle at the same height;

[0015] Step 4: According to the situation where the UAV, the cylindrical obstacle, and the target point are collinear, calculate a new repulsive force through the angle mechanism, and make the UAV move forward under the action of the gravitational force and the new repulsive force. The resultant force acting on the UAV is calculated as follows:

[0016] F = F att + F nrep

[0017] where Fnrep is the new repulsive force exerted on the drone under the action of the angle mechanism;

[0018] Step 5: Determine whether the drone has reached the target point by calculating the Euclidean distance d between the drone and the target point at the current moment; if the Euclidean distance d is less than a pre-set first threshold, it means that the drone has reached the target point and the operation ends; if the Euclidean distance d is greater than the pre-set first threshold, it means that the drone has not reached the target point, and repeat steps 3 to 5.

[0019] Furthermore, the technical solution of the present invention states that: the mathematical model of the artificial potential field in step 1 is specifically as follows:

[0020]

[0021]

[0022] where U att (x) and U rep (x) are the gravitational field and the repulsive field respectively, x and x g represent the spatial positions of the drone and the target point respectively, k att and k rep are the gravitational gain coefficient and the repulsive gain coefficient respectively, ρ0 represents the limit distance affected by the potential field, and ρ is the shortest distance from the drone to the obstacle.

[0023] Furthermore, the technical solution of the present invention states that: the gravitational force F att and the repulsive force F rep are respectively the negative gradients of the gravitational field and the repulsive field:

[0024] F att =-grad[U att (x)]=-k att (x - x g )

[0025]

[0026] Furthermore, the technical solution of the present invention states that: in step 4, a new repulsive force is calculated through the angle mechanism, and the relationship between the new repulsive force F nrep and the repulsive force F rep is specifically as follows:

[0027] F nrep = F rep / cosθ

[0028] θ = arctan[z / ρ(X, X t )]

[0029] where ρ(X, X t) is the distance between the UAV and the nearest threat point, and z is the second threshold.

[0030] Furthermore, in the technical solution of the present invention: the first threshold in step 5 is set to 0.1.

[0031] Furthermore, in the technical solution of the present invention: the second threshold is set to 20.

[0032] Beneficial effects

[0033] A UAV trajectory planning method based on the angle-constrained artificial potential field method proposed by the present invention replaces different buildings with cylindrical obstacles, and refines the cylindrical obstacles into the nearest threat point and the center of the top surface to handle different situations faced by the UAV, greatly simplifying the three-dimensional trajectory planning problem. At the same time, by introducing an angle mechanism, it solves the deficiencies of the traditional artificial potential field method such as the unreachability of the target point and the existence of local minima, further ensuring the safety of the UAV. In traffic congestion or emergency situations, compared with other trajectory planning methods, the UAV trajectory planning method based on the angle-constrained artificial potential field method has higher calculation efficiency and stronger safety, so it is of great significance to promote the application of UAVs in logistics and distribution. Description of the drawings

[0034] The drawings are only for the purpose of showing specific embodiments and are not considered to be a limitation of the present invention. Throughout the drawings, the same reference signs denote the same components.

[0035] Figure 1 is the flowchart of the UAV trajectory planning method based on the angle-constrained artificial potential field method of the present invention;

[0036] Figure 2 is the spatial position of the nearest threat point and the center of the top surface;

[0037] Figure 3 is the force analysis diagram of the UAV in a three-dimensional environment;

[0038] Figure 4 is the method diagram for generating a new repulsive force using the angle mechanism;

[0039] Figure 5 is the trajectory planned by the algorithm; Specific embodiments

[0040] In order to make the objectives, technical solutions and advantages of the present invention clearer, the present invention will be further described in detail below with reference to the drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0041] Please refer toFigure 1 , a UAV path planning method based on the angle-constrained artificial potential field method provided by the present invention, and the method content includes:

[0042] As Figure 2 shown, model the three-dimensional terrain and the building environment space, replace different buildings with cylindrical obstacles, and construct the spatial positions of the nearest threat points and the center of the top surface.

[0043] Establish an artificial potential field in the space, construct a gravitational field U att (x) centered on the target point, and construct a repulsive field U rep (x) near each cylindrical obstacle. The specific modeling method is as follows:

[0044]

[0045] In the formula, k att is the gravitational gain coefficient, and x and x g respectively represent the spatial positions of the UAV and the target point.

[0046]

[0047] In the formula, k rep is the repulsive gain coefficient, ρ0 represents the limit distance of the potential field influence, and ρ is the shortest distance from the UAV to the obstacle.

[0048] As Figure 3 shown, set the UAV to be in a non-collinear situation at the beginning. At this time, the repulsive force between the UAV and the cylindrical obstacle is equivalent to the repulsive force between the UAV and the center of the top surface of the cylindrical obstacle. According to the established artificial potential field, calculate the resultant force currently acting on the UAV, and make the UAV move forward under the action of the resultant force. The gravitational force F att and the repulsive force F rep are respectively the negative gradients of the gravitational field and the repulsive field:

[0049] F att =-grad[U att (x)]=-k att (x - x g )

[0050]

[0051] As Figure 4 shown, the UAV continues to fly towards the target point. When the UAV faces the crisis of local minimum, that is, the UAV, the cylindrical obstacle, and the target point are collinear, calculate a new repulsive force through the angle mechanism to change the current flight direction of the UAV. The relationship between the new repulsive force F nrep and the repulsive force F rep is specifically as follows:

[0052] Fnrep = F rep / cosθ

[0053] θ = arctan[z / ρ(X, X t )]

[0054] where ρ(X, X t ) is the distance between the UAV and the nearest threat point, and z is the second threshold value.

[0055] By calculating the Euclidean distance d between the UAV and the target point at the current moment, it is determined whether the UAV has reached the target point. The preset first threshold value is equal to 0.1. If the Euclidean distance d is less than the preset first threshold value, it indicates that the UAV has reached the target point and the algorithm program ends. If the Euclidean distance d is greater than the preset first threshold value, it indicates that the UAV has not reached the target point and the algorithm continues to run in a loop.

[0056] The preset second threshold value is equal to 20. The UAV reaches the preset second threshold value from the initial ground position, achieving a three-dimensional height crossing. The finally obtained planned flight path is as Figure 5 shown.

[0057] The above is only the specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art within the technical scope disclosed by the present invention can easily think of various equivalent modifications or substitutions, and these modifications or substitutions should all be covered within the protection scope of the present invention.

Claims

1. A UAV trajectory planning method based on angle-constrained artificial potential field method, characterized in that Here are the steps: Step 1: Model the cylindrical obstacle space, that is, represent the three-dimensional environment as an artificial potential field, construct a gravitational field with the target point as the center, and construct a repulsive field near each cylindrical obstacle; Step 2: Set the drone to be non-collinear at the beginning. At this time, the repulsive force between the drone and the cylindrical obstacle is equivalent to the repulsive force between the drone and the center of the top surface of the cylindrical obstacle. The resultant force on the drone is calculated as follows: F=F att +F rep Among them, F att is the gravitational force on the drone, F rep is the repulsive force on the drone; Step 3: The drone continues to fly towards the target point. When the drone faces the crisis of a local minimum, that is, the drone, the cylindrical obstacle and the target point are collinear, the position of the nearest threat point is calculated. At this time, the repulsive force between the drone and the cylindrical obstacle is equivalent to the repulsive force between the drone and the nearest threat point. The calculation of the nearest threat point is as follows: Among them, X t =(x t ,y t ,z t ) is the coordinate of the nearest threat point, X c =(x c ,y c ,z c ) is the coordinate of the center of the cylindrical obstacle at the same height as the drone, R c is the radius of the cylindrical obstacle, X = (x, y, z) is the coordinate of the drone, ρ(X, X c ) is the Euclidean distance between the drone and the center of the cylindrical obstacle at the same height; Step 4: According to the collinearity between the drone, the cylindrical obstacle and the target point, the new repulsive force is calculated through the angle mechanism, so that the drone moves forward under the action of gravity and the new repulsive force. The resultant force on the drone is calculated as follows: F=F att +F nrep Among them, F nrep is the new repulsive force that the drone is subjected to under the action of the angle mechanism; Step 5: By calculating the Euclidean distance d between the UAV and the target point at the current moment, determine whether the UAV has reached the target point; if the Euclidean distance d is less than the preset first threshold, it means that the UAV has reached the target point and the operation ends; if the Euclidean distance d is greater than the preset first threshold, it means that the UAV has not reached the target point, and steps 3 to 5 are repeated.

2. The unmanned aerial vehicle trajectory planning method based on the angle-constrained artificial potential field method according to claim 1 is characterized in that: The mathematical model of the artificial potential field described in step 1 is as follows: Among them, U att (x) and U rep (x) are the gravitational field and the repulsive field, respectively, x and x g Represent the spatial positions of the UAV and the target point, respectively, att and k rep are the gravitational gain coefficient and the repulsive gain coefficient respectively, ρ0 represents the limit distance affected by the potential field, and ρ is the shortest distance from the UAV to the obstacle.

3. The unmanned aerial vehicle trajectory planning method based on the angle-constrained artificial potential field method according to claim 2 is characterized in that: The gravitational force F on the drone att and repulsive force F rep They are the negative gradients of the gravitational field and the repulsive field respectively: F att =-grad[U att (x)]=-k att (x-x g ) 4. The method for unmanned aerial vehicle trajectory planning based on angle-constrained artificial potential field method according to claim 1, characterized in that: In step 4, the new repulsive force is calculated by the angle mechanism. The new repulsive force F nrep and repulsive force F rep The relationship between them is as follows: F nrep =F rep / cosθ θ=arctan[z / ρ(X,X t )] Among them, ρ(X,X t ) is the distance between the UAV and the nearest threat point, and z is the second threshold.

5. The unmanned aerial vehicle trajectory planning method based on the angle-constrained artificial potential field method according to claim 1 is characterized by: The first threshold in step 5 is set to 0.

1.

6. The method for unmanned aerial vehicle trajectory planning based on angle-constrained artificial potential field method according to claim 4 is characterized in that: The second threshold is set to 20.

Images