Three-dimensional path planning method for heterogeneous fixed-wing unmanned aerial vehicle cluster

Abstract

The invention discloses a three-dimensional path planning method for a heterogeneous fixed-wing unmanned aerial vehicle cluster. The method comprises a double-layer structure algorithm. The algorithm is composed of a conflict sensing layer and a path planning layer. A three-dimensional conflict detection mechanism is used in the conflict perception layer to convert path conflicts between the fixed-wing unmanned aerial vehicles into path planning constraints used by the path planning layer; establishing a set of action set generation mechanism in order to meet performance constraints of the fixed-wing unmanned aerial vehicle in a path planning layer, and establishing a collision detection mechanism suitable for a three-dimensional grid map and a path screening mechanism to generate a path planning result; and a group of optimal paths suitable for the fixed-wing unmanned aerial vehicle cluster are obtained through iterative interactive calculation of the conflict sensing layer and the path planning layer.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicles, and in particular to a three-dimensional path planning method for a heterogeneous fixed-wing unmanned aerial vehicle cluster. Background Art

[0002] As the country vigorously advocates low-altitude economy, fixed-wing UAVs are increasingly used in various industries due to their long endurance and strong carrying capacity. In such an application context, there is an increasing demand for fixed-wing UAVs of different forms and performances to complete a task together, or for different subtasks in a task to be completed by fixed-wing UAVs of different forms and performances.

[0003] Intelligence and autonomy are the research focuses of the development of UAV technology in recent years. Path planning is an important part of realizing autonomous avoidance, navigation and control of UAV. In the past few years, algorithms based on bionic intelligence (such as particle swarm optimization algorithm and genetic algorithm) and algorithms based on artificial intelligence can handle the path planning problem of heterogeneous fixed-wing UAV clusters. However, with the increase in the number of fixed-wing UAVs, the calculation time becomes unacceptable, but it cannot necessarily obtain the optimal path for each UAV and the versatility of the environment is poor; distributed algorithms, such as model predictive control (MPC) and speed-based obstacle avoidance algorithms, often cause UAVs to be congested in the air and cannot obtain the optimal solution. However, at present, there is a lack of a path planning method that can plan the global optimal path for heterogeneous fixed-wing UAV clusters and is computationally stable, which limits the application of fixed-wing UAV clusters in practice. Therefore, path planning for heterogeneous fixed-wing UAV clusters can help fixed-wing UAVs obtain the shortest path, reduce the execution time of a single task, increase the number of tasks performed by a single fixed-wing UAV, avoid congestion of each UAV in the air, and improve the utilization of airspace. The path planning method for heterogeneous fixed-wing UAV clusters can give full play to the application potential of fixed-wing UAV clusters, comply with the country's call to develop the low-altitude economy, and promote the development of the low-altitude economy. Summary of the invention

[0004] In order to solve the problems of long settlement time, uncertainty of settlement results and global optimality of settlement results in the path planning problem of heterogeneous fixed-wing UAVs, the present invention proposes a two-layer structure algorithm for heterogeneous fixed-wing UAVs, so as to settle the optimal path for the path planning problem with a large number of heterogeneous fixed-wing UAVs in a relatively short time, and further explore the potential of fixed-wing UAV clusters for application in practical environments.

[0005] The present invention provides a three-dimensional path planning method for heterogeneous fixed-wing UAV clusters based on conflict-based continuous time search (Continuous Time Conflict-Based Search, CCBS), characterized in that the method uses a two-layer structure algorithm to decouple and solve the path planning problem of fixed-wing UAV clusters, and the two-layer structure algorithm includes a conflict perception layer and a path planning layer. Among them, the conflict perception layer determines the spatiotemporal conflict in the path given by the path planning layer, and converts the spatiotemporal conflict into a path planning constraint that can be recognized by the path planning layer; the path planning layer plans a path for the UAV under the given path planning constraints.

[0006] Conflict perception layer: Find the spatiotemporal conflicts between the paths of various fixed-wing UAVs and convert them into path planning constraints suitable for the path planning layer to solve the path planning of a single fixed-wing UAV;

[0007] Path planning layer: Obtain path planning constraints from the conflict perception layer, and combine the path planning constraints with the start and end point information of the fixed-wing UAV that needs to be path planned to perform path planning in the three-dimensional grid map.

[0008] The conflict perception layer is characterized by establishing a set of conflict perception and conflict conversion processes containing a three-dimensional conflict detection mechanism, and the steps are as follows:

[0009] S1. First, call the path planning layer to plan paths for each fixed-wing UAV without path planning constraints;

[0010] S2. Calculate the path results of each fixed-wing UAV to obtain the path cost and cost, and initialize the path results P and cost of each fixed-wing UAV as the root node n of the constraint tree (composed of many nodes, with parent node and child node relationships between nodes). R =<P,cost> ;

[0011] S3. Find a node n with the minimum cost from the constraint tree k = <P k ,cost k >

[0012] S4. At node n k In the process, the paths of fixed-wing drones are matched against each other using a three-dimensional conflict detection mechanism to find space-time conflicts. <i,j,a i ,a j ,t>(The numbers of the conflicting parties are i and j. When the conflict occurs at time t, each party is performing action a i and a j ) If no spatiotemporal conflict is found, the paths of each fixed-wing UAV in node n are considered to be the final result and the algorithm ends.

[0013] The core of the three-dimensional conflict mechanism is:

[0014]

[0015] in, and They represent the velocity vectors of the conflicting parties respectively, and Respectively represent the positions of the conflicting parties. and They represent the radius of the two conflicting parties respectively; the collision time t is calculated by solving the equation;

[0016] S5. Based on space-time conflict <i,j,a i ,a j ,t>, create two constraints for fixed-wing drones i and j respectively <i,a i ,t>(fixed-wing drone i cannot perform action a at time t i )and <j,a j ,t>(fixed-wing drone j cannot perform action a at time t j )

[0017] S6. Find all constraints for fixed-wing drone i from tree node n and combine them with the newly created constraints <i,a i ,t> is combined into a constraint set const i , in const i Call the path planning layer to plan a path for fixed-wing UAV i under the constraints of ; find all constraints for fixed-wing UAV j from the constraint tree and combine them with the newly created constraints <j,a j ,t> is combined into a constraint set const j , call the path planning layer to plan a path for the fixed-wing UAV j under the constraint of const;

[0018] S7. Create two new tree nodes n i and n j

[0019] n i Store the new path of fixed-wing drone i and the old paths of other individuals except its own path, the total length and cost of all paths i In addition, n i Inherit all constraints from tree node n and add new constraints <i,a i ,t>;

[0020] n jStore the new path of fixed-wing drone j and the old paths of other individuals except its own path, the total length and cost of all paths j In addition, n j Inherit all constraints from tree node n and add new constraints <j,a j ,t>;

[0021] And n i and n j Add to constraint tree;

[0022] S8. Return to S3.

[0023] The path planning layer is characterized in that a single fixed-wing UAV path planning algorithm including an action set generation mechanism, a conflict perception of a three-dimensional collision detection mechanism, and a path screening mechanism is established based on the SIPP (Safe Interval Path Planning) algorithm, and the steps are as follows:

[0024] S1. Obtain the number i, starting point s, end point g and constraint set const of the fixed-wing UAV that needs path planning from the conflict perception layer;

[0025] S2, initialize OPEN, calculate the f value of the starting point s (such as the f value calculation method defined by the A* algorithm), and put s into OPEN;

[0026] S3, select a node n with the smallest f value from OPEN (composed of the node's location and the node's f value, i.e.<pos,f> );

[0027] S4, check whether the position of node n is the same as the position of end point g; if they are the same, store the path represented by node n into path container P, otherwise jump to S6;

[0028] S5. When the number of paths stored in the path container P is the same as the number of safety intervals of the end point, the path screening mechanism is used to obtain the path p that meets the performance requirements of the fixed-wing UAV and has the minimum path cost. reslut , the algorithm outputs the path p reslUT , and the algorithm ends; otherwise jump to S6;

[0029] S6. Generate an action set A that meets the performance constraints of the fixed-wing UAV through an action set generation mechanism. M}, use the action set A to obtain the adjacent node set N of node n = {n1,n2,…,n m};

[0030] S7, the connection between each node in the node set N and the node n is subjected to the three-dimensional collision detection mechanism, and the nodes related to the connection that collided are discarded from N, and the nodes in the set N that are still valid after the detection form a new node set N′={n1,n2,…,n k};

[0031] S8. Calculate the f value of each node in N′ and store it in OPEN; the algorithm returns to S3.

[0032] The action set generation mechanism is characterized in that it generates a set of actions that meet the performance constraints of the fixed-wing UAV (the fixed-wing UAV needs to meet a certain track angle γ and yaw angle before and after performing the action) according to the current position of the fixed-wing UAV. ), the mechanism is to construct an action generation function Action_generation to generate an action set A that meets the requirements of fixed-wing drones:

[0033]

[0034] Among them, pos k is the current position γ′ of the fixed-wing UAV k and They are the track angle and yaw angle of the fixed-wing UAV before and after performing the action.

[0035] The three-dimensional collision detection mechanism is characterized in that it is suitable for determining whether the grid that the fixed-wing UAV path passes through in the three-dimensional grid is not allowed to be reached. The mechanism is to construct a collision detection function Collision_detection to detect whether the path collides with the path planning environment:

[0036]

[0037] Among them, p is a path segment of the fixed-wing UAV, is a 3D grid map, and const is a static obstacle or a dynamic obstacle in the map. If the function result is true, it means that p has passed through a grid that is not allowed to pass through in the map, otherwise it is considered that p has no collision with the map.

[0038] The path screening mechanism is characterized by judging whether the path set π contains illegal actions (i.e., waiting for action a in place). wait And the flight speed v is less than the minimum flight speed v min ), whose mechanism is to construct a path screening function Path_check to screen out paths suitable for fixed-wing drones:

[0039]

[0040] in, is the wth path of fixed-wing drone k. If the function result is true, it means It can be submitted to the conflict perception layer as the planning result of the path planning layer, otherwise other paths in the path set π are selected for detection.

[0041] The advantages and beneficial effects of the present invention are as follows: a two-layer structure algorithm consisting of a conflict perception layer and a path planning layer is used to decouple the path planning problem of a drone cluster, thereby improving the speed and efficiency of calculation and ensuring the certainty of the calculation result; in the path planning layer, a path planning algorithm based on SIPP introduces performance constraints of fixed-wing drones and uses a three-dimensional collision detection function to make the path planning algorithm suitable for planning paths for heterogeneous fixed-wing drones of different drone sizes and speeds; a path screening mechanism is introduced in the path planning layer, so that the path planning layer can screen out the most suitable path for the fixed-wing drone under the premise of complete calculation. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Figure 1 Schematic diagram of the path planning method architecture of the present invention.

[0043] Figure 2 This is a schematic diagram of the first calculation result of the path of the present invention.

[0044] Figure 3 It is a schematic diagram of the path planning result of the present invention.

[0045] Figure 4 It is a schematic diagram of the constraint tree of the present invention. DETAILED DESCRIPTION

[0046] In order to better describe the operation process of the two-layer architecture algorithm and the role of the specific mechanism in the algorithm, the present invention will be explained in conjunction with a specific implementation example.

[0047] Based on the CCBS framework, this paper develops a three-dimensional path planning method for heterogeneous fixed-wing UAV clusters. The method consists of a conflict perception layer and a path planning layer. The algorithm architecture is as follows: Figure 1 shown.

[0048] Conflict perception layer: Find the spatiotemporal conflicts between the paths of various fixed-wing UAVs and convert them into path planning constraints suitable for the path planning layer to solve the path planning of a single fixed-wing UAV;

[0049] Path planning layer: Obtain path planning constraints from the conflict perception layer, and combine the path planning constraints with the start and end point information of the fixed-wing UAV that needs to be path planned to perform path planning in the three-dimensional grid map.

[0050] For the convenience of explanation, a two-dimensional scene is used for specific explanation. In a 5×5 two-dimensional grid environment, there are two fixed-wing drones 1 (hereinafter referred to as drone 1) and fixed-wing drone 2 (hereinafter referred to as drone 2) modeled as discs. A set of space-time collision-free paths is planned for them. The starting point of fixed-wing drone 1 is A5, the end point is E1, the speed is 1 grid / second, and the radius is 0.5 grid; the starting point of fixed-wing drone 2 is C5, the end point is C1, the speed is 0.5 grid / second, and the radius is 0.25 grid.

[0051] 1. The process of solving the problem of the conflict perception layer algorithm for this implementation example is as follows:

[0052] S1, calling the path planning layer to plan a path for UAV 1 and UAV 2 ignoring each other. The path result is as follows Figure 2 ;

[0053] S2. Create a constraint tree root node root for the conflict perception layer. The sum of the two path costs is 13.66s. Store the two paths and the total path cost in root and put root into the constraint tree.

[0054] S3, take out the tree node N0 with the minimum cost from the constraint tree (the first time the tree node is taken is the root node of the constraint tree);

[0055] S4. In node N0, the UAVs use a three-dimensional conflict detection mechanism to find space-time conflicts. If no space-time conflicts are found, the algorithm ends. In this implementation case, a space-time conflict <1,2,a1,a2,2.246> is found (a space-time conflict occurs on the path at t=2.246s during the process of UAV 1 from B4 to C3 and UAV 2 from C4 to C3);

[0056] S5. According to the spatiotemporal conflict <1,2,a1,a2,2.246>, two constraints <1,a1,2.246> (drone 1 cannot perform action a1 at 2.246s) and <2,a2,2.246> (drone 2 cannot perform action a2 at 2.246s) are created for drones 1 and 2 respectively.

[0057] S6. Find all constraints for drone 1 from node N0 and combine them with the newly created constraint <1, a1, 2.246> to form a constraint set const1 = {<1, a1, 2.246>}. ​​Under the constraint of const1, call the path planning layer to plan a path for drone 1; find all constraints for drone 2 from the constraint tree and combine them with the newly created constraint <2, a2, 2.246> to form a constraint set const2 = {<2, a2, 2.246>}. ​​Under the constraint of const2, call the path planning layer to plan a path for drone 2. The path result is as follows: Figure 3 As shown;

[0058] S7. Create two new tree nodes N 01 and N 02 , and N 01 and N 02 Add to the constraint tree, such as Figure 4 As shown;

[0059] N 01 Store the new path of drone 1, the old paths of other individuals except its own path, and the total cost of all paths. In addition, N 01 Inherit all constraints from tree node N0 and add a new constraint <1,a1,2.246>;

[0060] N 02 Store the new path of drone 2, the old paths of other individuals except its own path, and the total cost of all paths. In addition, N 02 Inherit all constraints from tree node N0 and add a new constraint <2,a2,2.246>;

[0061] S8. Return to S3.

[0062] 2. The path planning algorithm of the path planning layer is called multiple times in the conflict perception layer. The first call occurs in the initialization phase of the conflict perception layer, and then it is called when a new constraint tree node is created. In this implementation example, the path result of UAV 1 has changed before and after, and UAV 1 is used as the object for detailed description.

[0063] S1, obtain the number 1, starting point A5, end point E1 and constraint set const1 of the drone that needs path planning from the conflict perception layer;

[0064] S2, initialize OPEN, calculate the f value (5.66) of the starting point A5, and put A5 into OPEN;

[0065] S3, select a node n with the smallest f value from OPEN;

[0066] S4, check whether the positions of node n and end point E1 are the same; if they are the same, store the path represented by the node into path container P; otherwise jump to S6;

[0067] S5. If the number of paths in P is the same as the number of safety time intervals of the endpoint, the path screening mechanism is used to obtain the path of all drones 1 that does not include the waiting action, and the algorithm returns the path and ends; otherwise, jump to S6;

[0068] S6. Generate an action set A that meets the performance constraints of the fixed-wing UAV through an action set generation mechanism. m}, use the action set A to obtain the adjacent node set N of node n = {n1,n2,…,n m};

[0069] The action set generation mechanism checks node n, its parent node n′, and its child node n in this instance. k The positional relationship between them;

[0070] Let e ​​be the edge between n and n′, and let e′ be the edge between n and n k The edge between

[0071] Check the angle between e and e' (in this example, the angle between the two sides cannot be greater than 45°)

[0072] S7, the connection between each node in the node set N and the node n is subjected to the three-dimensional collision detection mechanism, and the nodes related to the connection that collided are discarded from N, and the nodes in the set N that are still valid after the detection form a new node set N′={n1,n2,…,n k};

[0073] In the example, the process of UAV 1 moving from B4 to C3 (the line connecting the two points is defined as line segment E) invokes the three-dimensional collision detection mechanism (applicable to the two-dimensional environment);

[0074] The mechanism first checks whether the line E collides with a static obstacle in the environment (there is no static obstacle in this example);

[0075] Secondly, check with const1 (UAV 1 cannot move from B4 to C3 in 2.246s) whether there is a conflict with the constraint. In this case, the constraint is violated, so C3 is discarded from N. Figure 2 As shown;

[0076] S8. Calculate the f value of each node in N′ and store it in OPEN; the algorithm returns to S3.

Claims

1. A three-dimensional path planning method for heterogeneous fixed-wing UAV clusters, characterized in that: A two-layer structure algorithm is adopted, including a conflict perception layer and a path planning layer; Conflict perception layer: Find the spatiotemporal conflicts between the paths of various fixed-wing UAVs and convert them into path planning constraints suitable for the path planning layer to solve the path planning of a single fixed-wing UAV; Path planning layer: Obtain path planning constraints from the conflict perception layer, and combine the path planning constraints with the start and end point information of the fixed-wing UAV that needs to be path planned to perform path planning in the three-dimensional grid map.

2. A three-dimensional path planning method for heterogeneous fixed-wing UAV clusters according to claim 1, characterized in that: At the conflict perception layer, a conflict perception and conflict transformation process with a three-dimensional conflict detection mechanism is established. The steps are as follows: Step S1: First, call the path planning layer to plan paths for each fixed-wing UAV without path planning constraints; Step S2: Calculate the path results of each fixed-wing UAV to obtain the path cost and cost, and initialize the path result P and cost of each fixed-wing UAV as the root node n of the constraint tree. R =<P,cost> ; Step S3: Find a node n with the minimum cost from the constraint tree k = <P k ,cost k > Step S4: At node n k In the process, the paths of fixed-wing drones are matched against each other using a three-dimensional conflict detection mechanism to find space-time conflicts. <i,j,a i ,a j ,t>; The conflicting parties are numbered i and j respectively. When the conflict occurs at time t, each party is performing action a. i and a j ; If no time-space conflict is found, node n is considered k The paths of the individual fixed-wing drones in are the final results, and the algorithm ends; Step S5: Based on time-space conflict <i,j,a i ,a j ,t>, create two constraints for fixed-wing drones i and j respectively <i,a i ,t> and <j,a j ,t>; Step S6: From the tree node n k Find all constraints for fixed-wing drone i and combine them with the newly created constraints <i,a i ,t> is combined into a constraint set const i , in const i Call the path planning layer to plan a path for fixed-wing UAV i under the constraints of ; find all constraints for fixed-wing UAV j from the constraint tree and combine them with the newly created constraints <j,a j ,t> is combined into a constraint set const j , in const j Call the path planning layer to plan the path for the fixed-wing UAV j under the constraints of ; Step S7: Create two new tree nodes n i and n j ; and n i and n j Add to constraint tree; Step S8, return to step S3.

3. The three-dimensional path planning method for heterogeneous fixed-wing UAV clusters according to claim 2 is characterized in that: In step S4, the three-dimensional conflict mechanism is: in, and They represent the velocity vectors of the conflicting parties respectively, and Respectively represent the positions of the conflicting parties. and They respectively represent the radius of the two conflicting parties; the collision time t is calculated.

4. The three-dimensional path planning method for heterogeneous fixed-wing UAV clusters according to claim 2 is characterized in that: In step S5, the fixed-wing UAV i cannot perform action a at time t i ; Fixed-wing drone j cannot perform action a at time t j .

5. The three-dimensional path planning method for heterogeneous fixed-wing UAV clusters according to claim 2 is characterized in that: In step S7, n i Store the new path of fixed-wing drone i and the old paths of other individuals except its own path, the total length and cost of all paths i ;n j Store the new path of fixed-wing drone j and the old paths of other individuals except its own path, the total length and cost of all paths j .

6. A three-dimensional path planning method for heterogeneous fixed-wing UAV clusters according to claim 5, characterized in that: n i Inherit all constraints from tree node n and add new constraints <i,a i ,t>;n j Inherit all constraints from tree node n and add new constraints <j,a j ,t>.

7. The three-dimensional path planning method for heterogeneous fixed-wing UAV clusters according to claim 1 is characterized in that: At the path planning layer, a single fixed-wing UAV path planning algorithm with action set generation mechanism, conflict perception of three-dimensional collision detection mechanism and path screening mechanism was established based on the SIPP algorithm. The steps are as follows: Step 1: Obtain the number i, starting point s, end point g and constraint set const of the fixed-wing UAV that needs path planning from the conflict perception layer; Step 2: Initialize OPEN, calculate the f value of the starting point s, and put s into OPEN; Step 3: Select a node n with the smallest f value from OPEN, which is composed of the node's location and the node's f value, that is,<pos,f> ; Step 4: Check whether the position of node n is the same as the position of end point g; if they are the same, store the path represented by node n into path container P; otherwise, jump to step 6; Step 5: If the number of paths in P is the same as the number of safe time intervals of the endpoint, the path screening mechanism is used to obtain the path p that meets the performance requirements of the fixed-wing UAV and has the minimum path cost. reslut , the algorithm converts path p reslut Output as the algorithm result, and the algorithm ends; otherwise jump to step 7; Step 6: Generate an action set A = {a1, a2, ..., a m }, use the action set A to obtain the adjacent node set N of node n = {n1,n2,…,n m }; Step 7: Use the three-dimensional collision detection mechanism to remove the nodes related to the collision from N. The nodes in N that are still valid after the detection form a new node set N′={n1,n2,…,n k }; Step 8: Calculate the f value of each node in N′ and store it in OPEN; Return to step three.

8. The three-dimensional path planning method for heterogeneous fixed-wing UAV clusters according to claim 7 is characterized in that: In step 5, the path screening mechanism: determines whether the path set π contains illegal actions (waiting for action a in place) wait And the flight speed v is less than the minimum flight speed v min ), build a path screening function Path_check to filter out paths suitable for fixed-wing drones: in, is the wth path of fixed-wing drone k; if the function result is true, it means The planning result of the path planning layer is submitted to the conflict perception layer, otherwise other paths in the path set π are selected for detection.

9. The three-dimensional path planning method for heterogeneous fixed-wing UAV clusters according to claim 7 is characterized in that: In step 6, the action set generation mechanism: generates an action set A that meets the performance constraints of the fixed-wing drone according to the current position of the fixed-wing drone, and constructs an action generation function Action_generation to generate an action set A that meets the performance constraints of the fixed-wing drone: Among them, pos k is the current position γ′ of the fixed-wing UAV k and They are the track angle and yaw angle of the fixed-wing UAV before and after performing the action.

10. The three-dimensional path planning method for heterogeneous fixed-wing UAV clusters according to claim 7, characterized in that: In step seven, the three-dimensional collision detection mechanism is used to determine whether the grid that the fixed-wing drone path passes through in the three-dimensional grid is not allowed to reach. A collision detection function Collision_detection is constructed to detect whether the path collides with the path planning environment: Among them, p is a path segment of the fixed-wing UAV, is a three-dimensional grid map, and const is a static obstacle or a dynamic obstacle in the map; if the function result is true, it means that p has passed through a grid that is not allowed to pass through in the map, otherwise it is deemed that p has no collision with the map.

Images