An unmanned aerial vehicle path planning method
By using the kd-tree algorithm and an extremely greedy approach in UAV trajectory planning, the problem that the traditional PRM algorithm cannot plan short paths in a short time is solved, achieving efficient optimization of collision-free trajectories. Simulation experiments verify the significant improvement in time and distance.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- XIAN UNIV OF POSTS & TELECOMM
- Filing Date
- 2023-05-26
- Publication Date
- 2026-05-29
AI Technical Summary
Existing technologies cannot plan a shorter flight path for drones that avoids collisions with obstacles in a short time, and traditional PRM algorithms are difficult to plan effectively in narrow passage environments.
By employing the kd-tree algorithm combined with an extremely greedy approach, a collision-free path to be optimized is formed by determining whether the path intersects with a straight line of obstacles. The path is then optimized by calculating weights, thus avoiding the creation of an undirected path graph and shortening the trajectory planning time.
While shortening the trajectory planning time, the path is optimized to obtain a shorter collision-free trajectory. Simulation experiments show that the planning time is significantly reduced and the path efficiency is improved.
Smart Images
Figure CN116642494B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of unmanned aerial vehicle (UAV) research, and specifically to a high-efficiency UAV trajectory planning method. Background Technology
[0002] Unmanned aerial vehicles (UAVs) play an increasingly important role in future integrated joint operations due to their advantages such as low detectability, low cost, lack of risk of casualties, simple takeoff and landing, flexible operation, diverse system configurations, and intelligent automatic control. UAV trajectory planning is a crucial aspect of UAV research. Its main task is to quickly plan a path for the UAV in complex obstacle environments that is as short as possible, with the largest possible turning radius, the fewest possible turns, and no collisions with obstacles.
[0003] Currently, common path planning algorithms include ant colony optimization (A* algorithm), artificial potential field method, PRM algorithm, and RRT algorithm. Specifically, ant colony optimization has significant drawbacks, such as long search time and susceptibility to getting trapped in local optima; A* algorithm has poor real-time performance, with high computational cost per node, resulting in long processing times, and its search efficiency decreases as the number of nodes increases; artificial potential field method suffers from the problems of goal unreachability and local minima; RRT algorithm cannot find the optimal path and often struggles to overcome narrow passages, requiring significant time overhead; PRM algorithm struggles to plan paths in narrow passage environments.
[0004] Traditional PRM algorithms are divided into an offline learning phase and an online query phase. Since the offline learning phase requires the establishment of an undirected path graph, it takes a lot of time to use the PRM algorithm to draw a path, and it is impossible to plan a shorter path for the drone in a short time. Summary of the Invention
[0005] To address the problems existing in the aforementioned fields, this invention proposes a UAV trajectory planning method that can solve the technical problem of not being able to plan a shorter trajectory for a UAV in a short time.
[0006] To address the aforementioned technical problems, this invention discloses a method for planning the trajectory of an unmanned aerial vehicle (UAV), comprising the following steps:
[0007] Obtain a set of points on the map that do not contain obstacles, and determine the start and end points. Add the determined start and end points to the point set. Based on the kd-tree algorithm, set the breadth m and depth k, search for the m neighboring points of the start point as the first key points, and search for m second key points based on each first key point. Repeat this process until the kth key point is found, obtaining multiple paths from the start point to the m first key points, from each first key point to its nearest neighbor in the second key points, and from each (k-1)th key point to its nearest neighbor in the kth key point. Use the kth key point of the shortest path as the new start point, and search again until the start point is determined to be the end point, forming a path to be optimized. Here, the values of k and m are natural numbers.
[0008] Determine if the shortest path intersects with the obstacle line. If not, use the k-th point of the path as the new starting point and keep the path. Then determine if the new starting point is the ending point. If it is, a collision-free path to be optimized is formed. If it is not the ending point, use an extremely greedy approach to determine if the line connecting the starting point and the ending point intersects with the obstacle line. If they intersect, remove the endpoints from the point set and return to the kd-tree algorithm. If they do not intersect, a collision-free path to be optimized is formed.
[0009] If the shortest path intersects the obstacle line, check if the second shortest path intersects the obstacle line. If they do, check if the current path's ranking in the path set is greater than the number of points in the first key point. If the current path's ranking in the path set is greater than the number of points in the second key point, remove the first key point from the point set, keep the starting point unchanged, and return to the kd-tree algorithm. If the current path's ranking in the path set is less than the number of points in the second key point, return to check if the second shortest path intersects the obstacle line.
[0010] The collision-free paths to be optimized are then optimized to obtain the shortest path, which is the collision-free optimal path.
[0011] Preferably, obtaining multiple paths from the starting point to the m first key points, from each first key point to its nearest neighbor in the second key points, and from each (k-1)th key point to its nearest neighbor in the kth key point includes the following steps:
[0012] Given a breadth m of 3 and a depth k of 2, search for the three nearest neighbors a, b, and c of the starting point s. Starting from a, b, and c respectively, find the three nearest neighbors of a, b, and c. The nearest neighbors of a are a1, a2, and a3; the nearest neighbors of b are b1, b2, and b3; and the nearest neighbors of c are c1, c2, and c3. a, b, and c are the first priority, and a1, a2, a3, b1, b2, b3, c1, c2, and c3 are the second priority. Finding the second priority ends the search with breadth m of 3 and depth k of 2. Calculate the distance from the starting point s to a, and the distance from a to... The paths from point s to point b, from point b to points b1, b2, b3, from point s to point c, and from point c to points c1, c2, c3 result in nine paths: d1, d2, d3, d4, d5, d6, d7, d8, d9. These nine paths are sorted from smallest to largest and placed in set D, where D = {d1, d2, d3, d4, d5, d6, d7, d8, d9}. Then, the second endpoint a1 of d1 is taken as the new starting point s, and each new starting point is placed in set Q. This process continues until the endpoint f is reached, forming a path.
[0013] Preferably, the adoption of an extremely greedy approach includes the following steps:
[0014] For each shortest path generated, the second key point of the path is connected to the end point. It is then determined whether the lines connecting the second key point and the end point intersect. If they do not intersect, the second key point and the end point are directly connected to form a path, thus completing the trajectory planning. If they intersect, the original trajectory planning method is used to continue the planning process.
[0015] When searching for a new starting point, the distance from the starting point s to the nearest neighbor is considered, as well as the distance from the nearest neighbor to the destination f. The weight from the starting point to the nearest neighbor is set as α, and the weight from the nearest neighbor to the destination is set as β. The path distance is calculated using the formula d1 = α * distance from s to a + β * distance from a to the destination f + α * distance from a to a1 + β * distance from a1 to the destination f. Here, the values of α and β satisfy α + β = 1.
[0016] Preferably, determining whether the shortest path intersects with a straight line of an obstacle includes the following steps:
[0017] S1: Determine whether the shortest path d1 intersects with the line formed by obstacles. If they do not intersect, no collision occurs, and the second endpoint of the path is used as the new starting point. Continue path planning according to the original trajectory planning method. Otherwise, a collision occurs, and the path is no longer selected.
[0018] S2: If a collision occurs, select the second shortest path d2, determine whether d2 intersects with the obstacle line, and proceed to step S1;
[0019] S3: If all paths in set D intersect with the lines formed by obstacles, remove the first key point from set H, then find the three nearest neighbors of the starting point s and re-form the path.
[0020] Preferably, optimizing the formed collision-free path to be optimized includes the following steps:
[0021] Connect the first starting point and the second starting point in set Q, and determine whether the line formed by the connection intersects with the line formed by the obstacles. If they intersect, keep the path between the first and second starting points, and continue to determine whether the line connecting the second and third starting points intersects with the line formed by the obstacles.
[0022] If they do not intersect, then check if the line connecting the first and third starting points intersects with the line formed by the obstacles. If they do not intersect, continue to check the first and fourth starting points. Similarly, when a starting point intersects with the line formed by the obstacles, connect the first starting point with the previous intersecting starting point, retain this path, and use this starting point as the new starting point to continue checking.
[0023] If they intersect, the path between the first and second starting points is retained. The line connecting the second and third starting points is then checked to see if it intersects with the line formed by the obstacles. This process continues until all starting points in set Q are checked, and the shortest path is obtained, which is the optimal path without collision.
[0024] Compared with the prior art, the present invention has the following beneficial effects:
[0025] This invention forms a collision-free path to be optimized by determining whether the shortest path intersects with a straight line of an obstacle. Employing an extremely greedy approach, it determines whether the line connecting the start and end points intersects with a straight line of an obstacle to form a collision-free path to be optimized. This eliminates the need to build an undirected path graph, saving significant time during trajectory planning. Simulation experiments comparing this method with the traditional PRM algorithm verify that the method can significantly shorten the time required for trajectory planning and achieve a shorter optimized trajectory distance. Attached Figure Description
[0026] Figure 1(a) is a flowchart of the overall method of the present invention;
[0027] Figure 1(b) is a flowchart of the path optimization method of the present invention;
[0028] Figure 2(a) shows the UAV trajectory planning diagram when the number of random points in the traditional PRM algorithm is 100;
[0029] Figure 2(b) is the UAV trajectory planning diagram when the number of random points in the algorithm of the present invention is 100;
[0030] Figure 3(a) shows the UAV trajectory planning diagram when the number of random points in the traditional PRM algorithm is 150;
[0031] Figure 3(b) is the UAV trajectory planning diagram when the number of random points in the algorithm of the present invention is 150;
[0032] Figure 4(a) shows the UAV trajectory planning diagram when the number of random points in the traditional PRM algorithm is 200;
[0033] Figure 4(b) shows the UAV trajectory planning diagram when the number of random points in the algorithm of the present invention is 200. Detailed Implementation
[0034] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to Figures 1-4. It should be understood that the terminology used in the present invention is only for describing particular implementation methods and is not intended to limit the present invention.
[0035] Example
[0036] This invention provides a method for planning the trajectory of an unmanned aerial vehicle (UAV), the specific steps of which are as follows:
[0037] As shown in Figure 1(a), several points are randomly scattered on a map with known obstacles to form a point set H. Points on the obstacles are deleted. Then, based on the kd-tree algorithm, with a breadth m of 3 and a depth k of 2, the three nearest neighbors a, b, and c of the starting point s are directly searched in the point set H. Then, taking each of the three nearest neighbors as a starting point, their three nearest neighbors are found: the nearest neighbors of a are a1, a2, and a3; the nearest neighbors of b are b1, b2, and b3; and the nearest neighbors of c are c1, c2, and c3. If a, b, and c are the first priority points, and a1, a2, a3, b1, b2, b3, c1, c2, and c3 are the second priority points, then the search ends once the second priority points are found. Calculate the distances from s to a, a to a1, a2, a3, s to b, b to b1, b2, b3, s to c, and c to c1, c2, c3 respectively. This yields nine paths d1, d2, d3, d4, d5, d6, d7, d8, and d9 (e.g., d1 = distance from s to a + distance from a to a1).
[0038] Sort these nine paths from smallest to largest and put them into set D. If D = {d1, d2, d3, d4, d5, d6, d7, d8, d9}, then take the second endpoint a1 of d1 as the new starting point s. Put each new starting point into set Q, and so on, until the endpoint f, to obtain the original track.
[0039] An extremely greedy algorithm is employed to optimize the original trajectory. This greediness involves two aspects: First, for each shortest path segment generated, the second key point is connected to the endpoint. The line connecting the second key point and the endpoint is checked for intersection. If they don't intersect, the second key point and the endpoint are directly connected, forming a path, thus completing the trajectory planning. If they intersect, the original trajectory planning method is continued. Second, when searching for a new starting point, the algorithm considers not only the distance from the starting point *s* to its nearest neighbors but also the distance from those neighbors to the endpoint. Ignoring the endpoint's distance would result in finding neighbors that are far from the endpoint, leading to a longer planned path.
[0040] Taking the weight α from the starting point to the nearest neighbor as 0.6 and the weight β from the nearest neighbor to the ending point as 0.4 as an example, the specific calculation method is: d1 = 0.6 * distance from s to a + 0.4 * distance from a to the ending point f + 0.6 * distance from a to a1 + 0.4 * distance from a1 to the ending point f.
[0041] To determine whether a collision will occur, the system determines whether the planned flight path intersects with the lines of obstacles.
[0042] S1: Determine whether the shortest path d1 intersects with the obstacle line. If they do not intersect, there is no collision. The second endpoint a1 of the path is used as the new starting point, and the planning continues according to the original path planning method. If they intersect, it indicates a collision, and the path is no longer selected.
[0043] S2: If a collision occurs, select the second shortest path d2, determine whether d2 intersects with the obstacle line, and proceed to step S1;
[0044] S3: If all paths in set D intersect with obstacle lines, remove the first priority from set H and re-plan the trajectory using the method described above.
[0045] The original trajectory was optimized. After collision detection was completed and an extremely greedy algorithm was introduced, the trajectory planned for the UAV no longer collided with obstacles, and the trajectory distance was improved. However, the obtained trajectory distance was not the shortest, and further optimization can be made, as shown in Figure 1(b):
[0046] Specifically, connect the first starting point and the second starting point in set Q, and determine whether the line formed by the connection intersects with the obstacle line. If they intersect, retain the path between the first and second starting points, and continue to determine the situation of the second and third starting points.
[0047] If they do not intersect, then determine whether the line connecting the first and third starting points intersects with the obstacle line. If they do not intersect, then continue to determine whether the first and fourth starting points intersect.
[0048] This process continues until a starting point intersects with an obstacle line. Then, connect the first starting point to the previous intersecting starting point, preserve this path, and use this starting point as the new starting point to continue the judgment (for example, if the line connecting the first and fourth starting points intersects with an obstacle line, connect the first and third starting points, preserve this path, and then start judging from the fourth starting point). If they intersect, preserve the path between the first and second starting points, and continue judging the case of the second and third starting points.
[0049] This process continues until all starting points in set Q have been determined, resulting in the shortest possible trajectory, which is the optimized trajectory.
[0050] To verify the algorithm mentioned in this paper, a simulation experiment was conducted using Python 3.10. The simulation used a 100×100 map with units set to centimeters (cm). Thick black lines in the scene represented obstacles, "..." indicated the route planned before optimization, and "---" indicated the optimized route, which is the final result. The drone's starting coordinates were set to [10,20], and the target point to [90,10]. Randomly selected points on the map were 100, 150, and 200, and the discussion focused on the cases with a breadth of 3 and a depth of 2.
[0051] Figures 2-4 show the comparison between the traditional PRM algorithm and the simulation experiment of this invention when the number of random points in the map is 100, 150, and 200. To further verify the accuracy of the algorithm, multiple experiments were conducted, and the average value was taken to obtain the track distance and the time used for track planning for the two algorithms, as shown in Table 1. This verifies the effectiveness of the method proposed in this invention, showing that the method proposed in this invention can significantly shorten the time used for track planning and optimize the track distance.
[0052] Table 1 shows the time and track length used by the two algorithms for different numbers of random points.
[0053]
[0054] The above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any equivalent substitutions or modifications made by those skilled in the art within the scope of the technology disclosed in the present invention, based on the technical solution and inventive concept of the present invention, should be covered within the scope of protection of the present invention.
[0055] Furthermore, unless otherwise stated, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains. All references to this specification are incorporated by way of citation to disclose and describe methods relating to those references. In the event of any conflict with any incorporated reference, the content of this specification shall prevail.
Claims
1. A method for planning the trajectory of an unmanned aerial vehicle (UAV), characterized in that, The steps include the following: Obtain the set of points on the map that do not contain obstacles, determine the start and end points, and add the determined start and end points to the point set; based on the kd-tree algorithm, set the breadth-first search... m and depth k Search starting point m The nth neighboring point is used as the nth neighboring point one The focus, and according to each of the first one Key Search m The second key point is to repeat this process until the first [target] is found. k The key point is to obtain the starting point. m Each first key point, multiple paths from each first key point to its nearest neighbor in the second key point, and each... k -1 The key point is to the first k Multiple paths to each of the nearest neighbors are considered; the path with the shortest distance is selected as the first... k The key point serves as a new starting point, and the search continues until the starting point is determined to be the destination, forming a path to be optimized. k,m The values of are all natural numbers; Determine if the shortest path intersects with a straight line containing an obstacle. If they do not intersect, determine the path's... k The key is to find the new starting point, keep the path, and determine whether the new starting point is the end point. If it is the end point, form a collision-free path to be optimized; if it is not the end point, use an extremely greedy approach to determine whether the line connecting the starting point and the end point intersects with the straight line of the obstacle. If they intersect, delete the endpoints on the path from the point set and return to the kd-tree algorithm; if they do not intersect, form a collision-free path to be optimized. If the shortest path intersects with the obstacle's straight line, check if the second shortest path intersects with the obstacle's straight line. If they intersect, check if the current path's ranking in the path set is greater than the [rank of the [path] in the [path] set]. one The number of key points, if the current path's ranking in the path set is greater than the first key point. k The number of key points will be the first one The key point is removed from the point set, keeping the starting point unchanged, and the kd-tree algorithm is returned; if the current path's ranking in the path set is less than the k-th node... k The number of key midpoints is used to determine whether the second shortest path intersects with a straight line containing an obstacle. The collision-free paths to be optimized are then optimized to obtain the shortest path, which is the collision-free optimal path. Wherein, the acquisition of the starting point to m Each first key point, multiple paths from each first key point to its nearest neighbor in the second key point, and each... k -1 The key point is to the first k The key points include multiple paths to each neighboring point, including the following steps: Set its breadth m The depth is 3. k Given a starting point s, search for the three nearest neighbors a, b, and c. Starting from a, b, and c respectively, find the three nearest neighbors of a, b, and c. The nearest neighbors of a are a1, a2, and a3; the nearest neighbors of b are b1, b2, and b3; and the nearest neighbors of c are c1, c2, and c3. Among these, a, b, and c are the first priority, and a1, a2, a3, b1, b2, b3, c1, c2, and c3 are the second priority. Finding the second priority ends the breadth-first search. m For 3 and depth k For a search of length 2, calculate the paths from the starting point s to a, a to a1, a2, a3, from the starting point s to b, b to b1, b2, b3, from the starting point s to c, and c to c1, c2, c3, resulting in nine paths: d1, d2, d3, d4, d5, d6, d7, d8, d9. Sort these nine paths in ascending order and place them in set D, where D = {d1, d2, d3, d4, d5, d6, d7, d8, d9}. Then, take the second endpoint a1 of d1 as the new starting point s. Place each new starting point into set Q, and so on, until the endpoint f, forming a path. The method employs an extremely greedy approach and includes the following steps: For each shortest path generated, the second key point of the path is connected to the end point. It is then determined whether the lines connecting the second key point and the end point intersect. If they do not intersect, the second key point and the end point are directly connected to form a path, thus completing the trajectory planning. If they intersect, the original trajectory planning method is used to continue the planning process. When searching for a new starting point, the distance from the starting point s to its nearest neighbors is considered, as well as the distance from the nearest neighbors to the ending point f. The weights from the starting point to its nearest neighbors are set to... The weight of the nearest neighbor to the destination is ; through the formula d1= *Distance from s to a + *Distance from a to the destination f + *Distance from a to a1+ The path distance value is calculated by taking the distance from a1 to the destination f; where, and The value satisfies ; Determining whether the shortest path intersects with a straight line of an obstacle includes the following steps: S1: Determine whether the shortest path d1 intersects with the line formed by obstacles. If they do not intersect, no collision occurs, and the second endpoint of the path is used as the new starting point. Continue path planning according to the original trajectory planning method. Otherwise, a collision occurs, and the path is no longer selected. S2: If a collision occurs, select the second shortest path d2, determine whether d2 intersects with the obstacle line, and proceed to step S1; S3: If all paths in set D intersect with the lines formed by obstacles, remove the first key point from set H, then find the three nearest neighbors of the starting point s to re-form multiple paths.
2. The UAV trajectory planning method according to claim 1, characterized in that, The optimization of the formed collision-free path to be optimized includes the following steps: Connect the first starting point and the second starting point in set Q, and determine whether the line formed by the connection intersects with the line formed by the obstacles. If they intersect, keep the path between the first and second starting points, and continue to determine whether the line connecting the second and third starting points intersects with the line formed by the obstacles. If they do not intersect, then check if the line connecting the first and third starting points intersects with the line formed by the obstacles. If they do not intersect, continue to check the first and fourth starting points. Similarly, when a starting point intersects with the line formed by the obstacles, connect the first starting point with the previous intersecting starting point, retain this path, and use this starting point as the new starting point to continue checking. If they intersect, the path between the first and second starting points is retained. The line connecting the second and third starting points is then checked to see if it intersects with the line formed by the obstacles. This process continues until all starting points in set Q are checked, and the shortest path is obtained, which is the optimal path without collision.