Multi-unmanned aerial vehicle formation global path planning method based on sampling

Abstract

A multi-unmanned aerial vehicle formation global path planning method based on sampling comprises the following steps: S1, constructing a global grid map, and giving a formation initial state and a formation target state; s2, adding a formation initial state to a first random tree as a root node, and adding a formation target state to a second random tree as a root node; s3, sampling to obtain a new sampling node, and adding 1 to the number of iterations; s4, bidirectionally expanding two random trees from the new sampling node; s5, judging whether the two extended random trees are connected or not, and if the two extended random trees are connected, optimizing cost values of nodes in the two random trees through a region pruning method; s6, judging whether an operation termination condition is met or not, if yes, entering S7, and if not, executing S3; and S7, if the two random trees are connected, determining that the path solving succeeds, and obtaining a path planning result, otherwise, determining that the path solving fails. According to the method, the solving speed and the solving quality of sampling-based multi-unmanned aerial vehicle formation global path planning in a complex environment are effectively improved.

Description

Technical Field

[0001] The present invention relates to the technical field of multi-UAV path planning, and in particular to a sampling-based global path planning method for a multi-UAV formation. Background Art

[0002] In recent years, multi-UAV formation technology has been developing rapidly. With its advantages such as wide field of view, large payload and strong elastic adaptability, multi-UAV formation can complete tasks that are unattainable for a single UAV. However, most of the current formation technologies are considered in ideal open environments, which greatly limits its scope of application.

[0003] When multiple UAVs fly in complex environments, global path planning is required to guide subsequent local path planning, otherwise they are prone to fall into local optimality. However, compared with the global path planning of a single UAV, the global path planning at the formation level has a higher dimension and it is difficult to obtain a faster solution rate and higher solution quality, which will greatly reduce the work efficiency of the multi-UAV formation. Based on the above difficulties, how to design an efficient global path planning method for multi-UAV formations is of great significance to improving the work efficiency of multiple UAVs in complex environments. Summary of the invention

[0004] The present invention provides a sampling-based global path planning method for a multi-UAV formation to solve the technical problems mentioned in the background technology.

[0005] To achieve the above object, the technical solution of the present invention is achieved as follows:

[0006] The present invention provides a sampling-based global path planning method for a multi-UAV formation, comprising the following steps:

[0007] S1. Build a global grid map, given the formation starting state S, formation target state G, and build the desired formation of multiple UAVs;

[0008] S2, add the formation starting state S to the first random tree T1 as the root node, add the formation target state G to the second random tree T2 as the root node, and set the number of iterations k to 0;

[0009] S3, obtain a new sampling node qne by sampling based on the obstacle-guided Gaussian sampling method w , and increase the number of iterations k by 1;

[0010] S4, according to the global grid map and the expected formation, resample the node q new Bidirectionally expand the first random tree T1 and the second random tree T2;

[0011] S5, judging whether the two random trees are connected after the expansion, if they are connected, optimizing the cost values ​​of the nodes in the two random trees by using the region pruning method;

[0012] S6, determine whether the termination condition is met, if yes, go to S7, otherwise go to S3;

[0013] S7. If the first random tree T1 and the second random tree T2 are connected, the path solving is successful and the final path planning result is obtained; otherwise, the path solving fails.

[0014] Furthermore, the S1 specifically includes the following steps:

[0015] S11, construct a global grid map, given the formation starting state S = [x s ,y s , z s , 1.0] T , formation target state G = [x g ,y g , z g , 1.0] T , maximum number of trials M, neighbor radius R, maximum formation size s max 、Minimum team size S min , maximum number of iterations N and maximum running time T max , where [x s ,y s , z s ] T ∈ Indicates the center position of the formation at the starting state of the formation, The center position of the formation representing the formation target state; T is the transpose of the matrix;

[0016] S12, construct the expected formation of n drones [p des，1 T , p des，2 T , ..., p des，n T ] T , where p des，i ∈ represents the expected position of the i-th UAV in the expected formation.

[0017] Furthermore, the S3 specifically includes the following steps:

[0018] S31, obtain random sampling node q′ by random sampling new , randomly sample node q′ new The formation center position is sampled in the global grid map, and the node q′ is randomly sampled. new The size of the formation is [smin ,s max ] range of sampling;

[0019] S32, check the random sampling node q′ new Is it valid? If not, set the number of trials τ to 0 and execute S33; otherwise, execute S35;

[0020] S33, sampling by Gaussian sampling method to obtain Gaussian sampling node q″ new , Gaussian sampling node q″ new The formation center is randomly sampled at the node q′ within the global grid map. new The formation center is the mean, with σ 2 Gaussian sampling is performed to obtain the variance, q″ new The size of the formation is [s min ,s max ] is obtained by random sampling within the range, and the number of trials τ is increased by 1;

[0021] S34, determine Gaussian sampling node q″ new Is it valid? If it is invalid and the number of trials τ does not exceed the maximum number of trials M, then jump to S33 to execute. If it is invalid and the number of trials τ exceeds the maximum number of trials M, then jump to S31 to execute. Otherwise, the Gaussian sampling node q″ new Update to randomly sampled node q′ new , and execute S35;

[0022] S35, randomly sample node q′ new As a new sampling node q new , and increase the number of iterations k by 1.

[0023] Furthermore, the S4 specifically includes the following steps:

[0024] S41, respectively obtain new sampling nodes q new In the first random tree T1 and the second random tree T2, the first neighbor node set Ψ1 and the second neighbor node set Ψ2, each neighbor node q in the two neighbor node sets neigh Satisfy neighbor node q neigh The formation center to the new sampling node q new The formation center distance is less than the neighbor radius R;

[0025] If the first neighbor node set Ψ1 and / or the second neighbor node set Ψ2 is empty, the corresponding random tree with the newly sampled node q new The node with the smallest distance to the formation center is added to the first neighbor node set Ψ1 and / or the second neighbor node set Ψ2 to ensure that the set is not empty;

[0026] S42, searching for node q in the first neighbor node set Ψ1 u，1 , satisfying node q u，1 To the new sampling node q new The connection is valid and passes through node q u，1 To the new sampling node q new The formation level cost δ1 is the smallest; similarly, find node q in the second neighbor node set Ψ2 u，2 , satisfying node q u，2 To the new sampling node q new The connection is valid and passed u2 to q new The formation level cost δ2 is the smallest; the connection is effective, that is, the connection between the two nodes does not collide with obstacles;

[0027] S43. If node q is not found u，1 and q u，2 , it means that the growth fails, end S4 and jump to S3; if only node q is found u，1 , then jump to S44, if only node q is found u，2 , then jump to S45, otherwise jump to S46;

[0028] S44, the new sampling node q new Add to the node set V1 of the first random tree T1, and connect the node relationship (q u，1 ,q new ) is added to the node connection relationship set E1 of the first random tree T1, and the new sampling node q new The cost η(q new )=δ1, then jump to S48;

[0029] S45, the new sampling node q new Add to the node set V2 of the second random tree T2, and connect the node relationship (q u，2 ,q new ) is added to the node connection relationship set E2 of the second random tree T2, and the new sampling node q new The cost η(q new )=δ2, then jump to S48;

[0030] S46, compare the formation level costs δ1 and δ2, if δ1≤δ2, then the new sampling node q new Add to the node set V1 of the first random tree T1, and connect the node relationship (q u，1 ,q new ) is added to the node connection relationship set E1 of the first random tree T1, and the new sampling node q new The cost η(q new )=δ1, and the node q u，2Mark as new sampling node q new Otherwise, a new sampling node q new Add to the node set V2 of the second random tree T2, and connect the node relationship (q u，2 ,q new ) is added to the node connection relationship set E2 of the second random tree T2, and the new sampling node q new The cost η(q new )=δ2, and the node q u，1 Mark as new sampling node q new The connection point;

[0031] S47, the new sampling node q new Add to the intersection point set Π, which means that the two random trees have been connected;

[0032] S48, at the new sampling node q new The reconnection operation is performed in the random tree where the new sampling node q new Other neighbor nodes q except its parent node neigh , judge η(q new )+c(q new ,q neigh )<η(q neigh ) is true, if true, then at the new sampling node q new Delete the node connection relationship (q p ,q neigh ), increase the node connection relationship (q new ,q neigh ), where q p Q neigh The original parent node of .

[0033] Furthermore, the calculation formula of the formation level cost δ1 in S42 is:

[0034] δ1=η(q u，1 )+c(q u1 ,q new )

[0035] In the formula, η(.) represents the first cost function; c(.) represents the state transition cost function; and η(q u，1 ) and c(q u1 ,q new ) satisfies the following relationship:

[0036]

[0037] Among them, p u1，i 、p new，i Represents node qu1 and the new sampling node q new The corresponding position of UAV i; p u1，i 、p new，i They are calculated by the following formulas:

[0038]

[0039]

[0040] Among them, s u1 、s new The nodes q u1 and the new sampling node q new The size of the formation, X u1 , X new The nodes q u1 and the new sampling node q new The center position of the formation.

[0041] Furthermore, the calculation formula of the formation level cost δ2 in S42 is:

[0042] δ2=η(q u，2 )+c(q u2 ,q new )

[0043] In the formula, c(q u2 ,q new ) satisfies the following relationship:

[0044]

[0045] Among them, p u2，i Represents node q u2 The corresponding position of UAV i; p u2，i Calculated by the following formula:

[0046]

[0047] Among them, s u2 For node q u2 The size of the formation, X u2 For node q u2 The center position of the formation.

[0048] Furthermore, the S5 specifically includes the following steps:

[0049] S51, determine whether the two random trees are connected after growth, if not, end S5 and jump to S6;

[0050] S52, define the update set Q, and add the new sampling node q new Add to the update set Q;

[0051] S53. Take out node q from update set Q cur ;

[0052] S54, detection node q cur Is there a grandparent node q g If not, execute S56, otherwise determine the grandfather node q g To node q cur Whether the connection is valid, if not valid, execute S56; if valid, execute S55;

[0053] S55. Delete q from the node connection relationship set E1 or the node connection relationship set E2. cur Its parent node q p The connection relationship (q p ,q cur ), add connection relationship (q g ,q cur );At this time q g Updated to q cur The parent node of ; then jump to S54;

[0054] S56, the new sampling node q new All child nodes of are added to the update set Q;

[0055] S57. If the update set Q is not empty, jump to S53; otherwise, end S5 and execute S6.

[0056] Furthermore, the termination condition in S6 is whether the number of iterations k exceeds the maximum number of iterations N or the algorithm reaches the maximum running time T. max .

[0057] Furthermore, the S7 specifically includes the following steps:

[0058] S71, judging whether the first random tree T1 and the second random tree T2 are connected. If the connection is not completed, it means that the path solving fails, and exit S7; if the connection is successful, enter S72;

[0059] S72. Find node q in the intersection point set Π. f , so that through node q f The path cost L f Minimum;

[0060] S73, from node q f Go to node q respectively f The parent node and its connection point are traced back to obtain a formation global path connecting the formation starting state and the formation target state as the solution result.

[0061] Furthermore, the node q in S72 f and the path cost L f The calculation formula is:

[0062]

[0063] L f =η(q f )+η(q f ′)+c(q f ,q f ′)

[0064] Among them, q σ ′ is the node q σ The connection point, in addition, node q σ It is also the function η(q σ )+η(q σ ′)+c(q σ ,q σ ′) is the independent variable. f ′ is the node q f connection point.

[0065] Beneficial effects of the present invention:

[0066] 1. The present invention defines the node states and cost functions of the random tree, so that the sampling-based method can be applied to the global path planning problem of multiple UAV formations.

[0067] 2. The present invention adopts a bidirectional sampling method and a Gaussian sampling method based on obstacle guidance, so that the sampling-based method can still maintain fast convergence characteristics in complex environments, greatly reducing the solution time of path planning.

[0068] 3. The present invention proposes a region pruning method, which can reduce the redundant connections of nodes in the random tree, reduce the cost of the nodes, optimize the structure of the random tree, and improve the solution quality of path planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0069] Figure 1 is a flow chart of the present invention;

[0070] Figure 2 is the result of the global formation path planning of multiple UAVs in an embodiment of the present invention;

[0071] Figure 3 This is a comparison chart of the path planning results of the global formation of multiple UAVs in an embodiment of the present invention. DETAILED DESCRIPTION

[0072] In order to facilitate the understanding of the present invention, the present invention will be described more fully below with reference to the relevant drawings. Preferred embodiments of the present invention are provided in the drawings. However, the present invention can be implemented in many other different forms and is not limited to the embodiments described herein. On the contrary, the purpose of providing these embodiments is to make the understanding of the disclosure of the present invention more thorough and comprehensive.

[0073] The terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, a feature defined as "first" or "second" may explicitly or implicitly include one or more of the features. In the description of the present invention, the meaning of "plurality" is two or more, unless otherwise clearly and specifically defined.

[0074] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as those commonly understood by those skilled in the art of the present invention. The terms used herein in the specification of the present invention are only for the purpose of describing specific embodiments and are not intended to limit the present invention. The term "and / or" used herein includes any and all combinations of one or more related listed items.

[0075] Reference Figure 1 The present application embodiment provides a sampling-based multi-UAV formation global path planning method, comprising the following steps:

[0076] S1. Build a global grid map, given the formation starting state S, formation target state G, and build the desired formation of multiple UAVs;

[0077] S2, add the formation starting state S to the first random tree T1 as the root node, add the formation target state G to the second random tree T2 as the root node, and set the number of iterations k to 0;

[0078] S3, obtain a new sampling node q by sampling based on the obstacle-guided Gaussian sampling method new , and increase the number of iterations k by 1;

[0079] S4, according to the global grid map and the expected formation, resample the node q new Bidirectionally expand the first random tree T1 and the second random tree T2;

[0080] S5, judging whether the two random trees are connected after the expansion, if they are connected, optimizing the cost values ​​of the nodes in the two random trees by using the region pruning method;

[0081] S6, determine whether the termination condition is met, if yes, go to S7, otherwise go to S3;

[0082] S7. If the first random tree T1 and the second random tree T2 are connected, the path solving is successful and the final path planning result is obtained; otherwise, the path solving fails.

[0083] In some embodiments, the S1 specifically includes the following steps:

[0084] S11, construct a global grid map, given the formation starting state S = [x s ,y s , z s , 1.0] T , formation target state G = [x g ,y g , z g , 1.0] T , maximum number of trials M, neighbor radius R, maximum formation size s max , minimum team size min , maximum number of iterations N and maximum running time T max ,in Indicates the center position of the formation at the starting state of the formation, The center position of the formation representing the formation target state; T is the transpose of the matrix;

[0085] S12, construct the expected formation of n drones [p des，1 T , p des，2 T , ..., p des，n T ] T ,in represents the expected position of the i-th UAV in the expected formation (1≤i≤n).

[0086] In some embodiments, the random tree T in S2 (the first random tree T1 and the second random tree T2 are collectively referred to as T) is defined as follows:

[0087] The random tree T is defined as T = (V, E), where V is the node set, is a node connection relationship set. The node q in V = [X T , s] T Can represent formation information, where Indicates the center position of the formation, represents the size of the formation. The effective definition of node q is: for any UAV i in the formation, it satisfies Where X free Indicates the collision-free area, obtained through the global grid map, p q，i is the position of drone i corresponding to node q, which is calculated by the following formula

[0088]

[0089] For the node connection relationship (u, v) ∈ E, it means that node u ∈ V is connected to node v ∈ V, and u is the parent node of v, and v is the child node of u. Except for the root node, each node in the random tree has only one parent node. The cost of the root node is 0, and the cost of other nodes can be calculated by the cost function Calculated and defined as follows

[0090] η(v)=η(u)+c(u,v)

[0091]

[0092] in is the state transition cost function, p u，i 、p v，i are the positions of drone i corresponding to u and v respectively.

[0093] The node connection relationship (u, v) in the random tree is effectively defined as: for any drone i, p in the formation u，i and p v，i The connecting lines do not collide with obstacles.

[0094] In some embodiments, S3 specifically includes the following steps:

[0095] S31, obtain random sampling node q′ by random sampling new , randomly sample node q′ new The formation center position is sampled in the global grid map, and the node q′ is randomly sampled. new The size of the formation is [s min ,s max ] range of sampling;

[0096] S32, check the random sampling node q′ new Is it valid? If not, set the number of trials τ to 0 and execute S33; otherwise, execute S35;

[0097] S33, sampling by Gaussian sampling method to obtain Gaussian sampling node q″ new , Gaussian sampling node q″ new The formation center is randomly sampled at the node q′ within the global grid map. new The formation center is the mean, with σ 2 Gaussian sampling is performed to obtain the variance, q″ new The size of the formation is [s min ,s max ] is obtained by random sampling within the range, and the number of trials τ is increased by 1;

[0098] S34, determine Gaussian sampling node q″ new Is it valid? If it is invalid and the number of trials τ does not exceed the maximum number of trials M, then jump to S33 to execute. If it is invalid and the number of trials τ exceeds the maximum number of trials M, then jump to S31 to execute. Otherwise, the Gaussian sampling node q″ new Update to randomly sampled node q′ new , and execute S35;

[0099] S35, randomly sample node q′ new As a new sampling node q new , and increase the number of iterations k by 1.

[0100] In some embodiments, the S4 specifically includes the following steps:

[0101] S41, respectively obtain new sampling nodes q new In the first random tree T1 and the second random tree T2, the first neighbor node set Ψ1 and the second neighbor node set Ψ2, each neighbor node q in the two neighbor node sets neigh Satisfy neighbor node q neigh The formation center to the new sampling node q new The formation center distance is less than the neighbor radius R;

[0102] If the first neighbor node set Ψ1 and / or the second neighbor node set Ψ2 is empty, the corresponding random tree with the newly sampled node q new The node with the smallest distance to the formation center is added to the first neighbor node set Ψ1 and / or the second neighbor node set Ψ2 to ensure that the set is not empty;

[0103] S42, searching for node q in the first neighbor node set Ψ1 u，1 , satisfying node q u，1 To the new sampling node q new The connection is valid and passes through node q u，1 To the new sampling node q new The formation level cost δ1 is the smallest; similarly, find node q in the second neighbor node set Ψ2 u，2 , satisfying node q u，2 To the new sampling node q new The connection is valid and passed u2 to q new The formation level cost δ2 is the smallest; the connection is effective, that is, the connection between the two nodes does not collide with obstacles;

[0104] S43. If node q is not found u，1 and q u，2, it means that the growth fails, end S4 and jump to S3; if only node q is found u，1 , then jump to S44, if only node q is found u，2 , then jump to S45, otherwise jump to S46;

[0105] S44, the new sampling node q new Add to the node set V1 of the first random tree T1, and connect the node relationship (q u，1 ,q new ) is added to the node connection relationship set E1 of the first random tree T1, and the new sampling node q new The cost η(q new )=δ1, then jump to S48;

[0106] S45, the new sampling node q new Add to the node set V2 of the second random tree T2, and connect the node relationship (q u，2 ,q new ) is added to the node connection relationship set E2 of the second random tree T2, and the new sampling node q new The cost η(q new )=δ2, then jump to S48;

[0107] S46, compare the formation level costs δ1 and δ2, if δ1≤δ2, then the new sampling node q new Add to the node set V1 of the first random tree T1, and connect the node relationship (q u，1 ,q new ) is added to the node connection relationship set E1 of the first random tree T1, and the new sampling node q new The cost η(q new )=δ1, and the node q u，2 Mark as new sampling node q new Otherwise, a new sampling node q new Add to the node set V2 of the second random tree T2, and connect the node relationship (q u，2 ,q new ) is added to the node connection relationship set E2 of the second random tree T2, and the new sampling node q new The cost η(q new )=δ2, and the node q u，1 Mark as new sampling node q new The connection point;

[0108] S47, the new sampling node q new Add to the intersection point set Π, which means that the two random trees have been connected;

[0109] S48, at the new sampling node qnew The reconnection operation is performed in the random tree where the new sampling node q new Other neighbor nodes q except its parent node neigh , judge η(q new )+c(q new ,q neigh )<η(q neigh ) is true, if true, then at the new sampling node q new Delete the node connection relationship (q p ,q neigh ), increase the node connection relationship (q new ,q neigh ), where q p for q neigh The original parent node of .

[0110] In some embodiments, the calculation formula of the formation level cost δ1 in S42 is:

[0111] δ1=η(q u，1 )+c(q u1 ,q new )

[0112] In the formula, η(.) represents the first cost function; c(.) represents the state transition cost function; and η(q u，1 ) and c(q u1 ,q new ) satisfies the following relationship:

[0113]

[0114] Among them, p u1，i 、p new，i Represents node q u1 and the new sampling node q new The corresponding position of UAV i; p u1，i 、p new，i They are calculated by the following formulas:

[0115]

[0116] Among them, s u1 、s new The nodes q u1 and the new sampling node q new The size of the formation, X u1 , X new The nodes q u1 and the new sampling node q new The center position of the formation.

[0117] In some embodiments, the calculation formula of the formation level cost δ2 in S42 is:

[0118] δ2=η(q u，2 )+c(q u2 ,q new )

[0119] In the formula, c(q u2 ,q new ) satisfies the following relationship:

[0120]

[0121] Among them, p u2，i Represents node q u2 The corresponding position of UAV i; p u2，i Calculated by the following formula:

[0122]

[0123] Among them, s u2 For node q u2 The size of the formation, X u2 For node q u2 The center position of the formation.

[0124] In some embodiments, S5 specifically includes the following steps:

[0125] S51, determine whether the two random trees are connected after growth, if not, end S5 and jump to S6;

[0126] S52, define the update set Q, and add the new sampling node q new Add to the update set Q;

[0127] S53. Take out node q from update set Q cur ;

[0128] S54, detection node q cur Is there a grandparent node q g If not, execute S56, otherwise determine the grandfather node q g To node q cur Whether the connection is valid, if not valid, execute S56; if valid, execute S55;

[0129] S55. Delete q from the node connection relationship set E1 or the node connection relationship set E2. cur Its parent node q p The connection relationship (q p ,q cur ), add connection relationship (q g ,qcur );At this time q g Updated to q cur The parent node of ; then jump to S54;

[0130] S56, the new sampling node q new All child nodes of are added to the update set Q;

[0131] S57. If the update set Q is not empty, jump to S53; otherwise, end S5 and execute S6.

[0132] In some embodiments, the termination condition in S6 is whether the number of iterations k exceeds the maximum number of iterations N or the algorithm reaches the maximum running time T. max .

[0133] In some embodiments, the S7 specifically includes the following steps:

[0134] S71, judging whether the first random tree T1 and the second random tree T2 are connected. If the connection is not completed, it means that the path solving fails, and exit S7; if the connection is successful, enter S72;

[0135] S72. Find node q in the intersection point set Π. f , so that through node q f The path cost L f Minimum;

[0136] S73, from node q f Go to node q respectively f The parent node and its connection point are traced back to obtain a formation global path connecting the formation start state and the formation target state as the solution result.

[0137] In some embodiments, the node q in S72 f and the path cost L f The calculation formula is:

[0138]

[0139] L f =η(q f )+η(q f ′)+c(q f ,q f ′)

[0140] Among them, q σ ′ is the node q σ The connection point, in addition, node q σ It is also the function η(q σ )+η(q σ ′)+c(q σ,q σ ′) is the independent variable. f ′ is the node q f connection point.

[0141] The present invention completes the global path planning of multiple UAV formations on a complex map. The planned path results are as follows: Figure 2 In order to fully reflect the advantages of the Gaussian sampling method and the regional pruning method in the present invention, the method based on bidirectional sampling only (BRRT*), the method based on bidirectional sampling and Gaussian sampling (BRRT*-OGS), the method based on bidirectional sampling and regional pruning (BRRT*-RP) and the method based on bidirectional sampling, Gaussian sampling and regional pruning (BRRT*-OGS-RP) are compared, and the curves of the changes of the path length over time are shown as follows: Figure 3 As shown, it can be seen that the method proposed in the present invention effectively improves the solution speed and solution quality of the global path planning of multi-UAV formation based on sampling in complex environments.

[0142] The above is only a specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any technician familiar with the technical field can easily think of changes or substitutions within the technical scope disclosed by the present invention, which should be covered within the protection scope of the present invention. In addition, the technical solutions between the various embodiments of the present invention can be combined with each other, but it must be based on the ability of ordinary technicians in the field to implement. When the combination of technical solutions is contradictory or cannot be implemented, it should be considered that such a combination of technical solutions does not exist and is not within the protection scope of the present invention. Therefore, the protection scope of the present invention shall be based on the protection scope of the claims.

Claims

1. A sampling-based global path planning method for multi-UAV formation, characterized in that: The steps include: S1. Build a global grid map, given the formation starting state S, formation target state G, and build the desired formation of multiple UAVs; S2, add the formation starting state S to the first random tree T1 as the root node, add the formation target state G to the second random tree T2 as the root node, and set the number of iterations k to 0; S3, obtain a new sampling node qne by sampling based on the obstacle-guided Gaussian sampling method w , and increase the number of iterations k by 1; S4, according to the global grid map and the expected formation, resample the node q new Bidirectionally expand the first random tree T1 and the second random tree T2; S5, judging whether the two random trees are connected after the expansion, if they are connected, optimizing the cost values ​​of the nodes in the two random trees by using the region pruning method; S6, determine whether the termination condition is met, if yes, go to S7, otherwise go to S3; S7. If the first random tree T1 and the second random tree T2 are connected, the path solving is successful and the final path planning result is obtained; otherwise, the path solving fails.

2. The multi-UAV formation global path planning method according to claim 1, characterized in that: The S1 specifically includes the following steps: S11, construct a global grid map, given the formation starting state S = [x s ,y s , z s , 1.0] T , formation target state G = [x g ,y g , z g , 1.0] T , maximum number of trials M, neighbor radius R, maximum formation size s max , minimum team size min , maximum number of iterations N and maximum running time T max ,in Indicates the center position of the formation at the starting state of the formation, The center position of the formation representing the formation target state; T is the transpose of the matrix; represents the set of real numbers; S12, construct the expected formation of n drones [p des，1 T , p des，2 T , ..., p des，n T ] T ,in represents the expected position of the i-th UAV in the expected formation.

3. The multi-UAV formation global path planning method according to claim 2 is characterized in that: The S3 specifically includes the following steps: S31, obtain random sampling node q′ by random sampling new , randomly sample node q′ new The formation center position is sampled in the global grid map, and the node q′ is randomly sampled. new The size of the formation is [s min ,s max ] range of sampling; S32, check the random sampling node q′ new Is it valid? If not, set the number of trials τ to 0 and execute S33; otherwise, execute S35; S33, sampling by Gaussian sampling method to obtain Gaussian sampling node q″ new , Gaussian sampling node q″ new The formation center is randomly sampled at the node q′ within the global grid map. new The formation center is the mean, with σ 2 Gaussian sampling is performed to obtain the variance, q″ new The size of the formation is [s min ,s max ] is obtained by random sampling within the range, and the number of trials T is increased by 1; S34, determine Gaussian sampling node q″ new Is it valid? If it is invalid and the number of trials τ does not exceed the maximum number of trials M, then jump to S33 to execute. If it is invalid and the number of trials τ exceeds the maximum number of trials M, then jump to S31 to execute. Otherwise, the Gaussian sampling node q″ new Update to randomly sampled node q′ new , and execute S35; S35, randomly sample node q′ new As a new sampling node q new , and increase the number of iterations k by 1.

4. The multi-UAV formation global path planning method according to claim 3 is characterized in that: The S4 specifically includes the following steps: S41, respectively obtain new sampling nodes q new In the first random tree T1 and the second random tree T2, the first neighbor node set Ψ1 and the second neighbor node set Ψ2, each neighbor node q in the two neighbor node sets neigh Satisfy neighbor node q neigh The formation center to the new sampling node q new The formation center distance is less than the neighbor radius R; If the first neighbor node set Ψ1 and / or the second neighbor node set Ψ2 is empty, the corresponding random tree with the newly sampled node q new The node with the smallest distance to the formation center is added to the first neighbor node set Ψ1 and / or the second neighbor node set Ψ2 to ensure that the set is not empty; S42, searching for node q in the first neighbor node set Ψ1 u,1 , satisfying node q u,1 To the new sampling node q new The connection is valid and passes through node q u，1 To the new sampling node q new The formation level cost δ1 is the smallest; similarly, find node q in the second neighbor node set Ψ2 u,2 , satisfying node q u,2 To the new sampling node q new The connection is valid and passed u2 to q new The formation level cost δ2 is the smallest; the connection is effective, that is, the connection between the two nodes does not collide with obstacles; S43. If node q is not found u,1 and q u,2 , it means that the growth fails, end S4, and jump to S3; If only node q is found u,1 , then jump to S44, if only node q is found u,2 , then jump to S45, otherwise jump to S46; S44, the new sampling node q new Add to the node set V1 of the first random tree T1, and connect the node relationship (q u，1 ,q new ) is added to the node connection relationship set E1 of the first random tree T1, and the new sampling node q new The cost η(q new )=δ1, then jump to S48; S45, the new sampling node q new Add to the node set V2 of the second random tree T2, and connect the node relationship (q u，2 ,q new ) is added to the node connection relationship set E2 of the second random tree T2, and the new sampling node q new The cost η(q new )=δ2, then jump to S48; S46, compare the formation level costs δ1 and δ2, if δ1≤δ2, then the new sampling node q new Add to the node set V1 of the first random tree T1, and connect the node relationship (q u，1 ,q new ) is added to the node connection relationship set E1 of the first random tree T1, and the new sampling node q new The cost η(q new )=δ1, and the node q u，2 Mark as new sampling node q new Otherwise, a new sampling node q new Add to the node set V2 of the second random tree T2, and connect the node relationship (q u，2 ,q new ) is added to the node connection relationship set E2 of the second random tree T2, and the new sampling node q new The cost η(q new )=δ2, and the node q u,1 Mark as new sampling node q new The connection point; S47, the new sampling node q new Add to the intersection point set Π, which means that the two random trees have been connected; S48, at the new sampling node q new The reconnection operation is performed in the random tree where the new sampling node q new Other neighbor nodes q except its parent node neigh , judge η(q new )+c(q new ,q neigh )<η(q neigh ) is true, if true, then at the new sampling node q new Delete the node connection relationship (q p ,q neigh ), increase the node connection relationship (q new ,q neigh ), where q p for q neigh The original parent node of .

5. The multi-UAV formation global path planning method according to claim 4, characterized in that: The calculation formula of the formation level cost δ1 in S42 is: δ1=η(q u,1 )+c(q u1 ,q new ) In the formula, η(.) represents the first cost function; c(.) represents the state transition cost function; and η(q u,1 ) and c(q u1 ,q new ) satisfies the following relationship: Among them, p u1，i 、p new，i Represents node q u1 and the new sampling node qn e w corresponds to the position of UAV i; p u1,i 、p new，i They are calculated by the following formulas: Among them, s u1 、s new The nodes q u1 and the new sampling node q new The size of the formation, X u1 , X new The nodes q u1 and the new sampling node q new The center position of the formation.

6. The global path planning method for a multi-UAV formation according to claim 5, characterized in that: The calculation formula of the formation level cost δ2 in S42 is: δ2=η(q u,2 )+c(q u2 ,q new ) In the formula, c(q u2 ,q new ) satisfies the following relationship: Among them, p u2,i Represents node q u2 The corresponding position of UAV i; p u2,i Calculated by the following formula: Among them, s u2 For node q u2 The size of the formation, X u2 For node q u2 The center position of the formation.

7. The global path planning method for a multi-UAV formation according to claim 4, characterized in that: The S5 specifically includes the following steps: S51, determine whether the two random trees are connected after growth, if not, end S5 and jump to S6; S52, define the update set Q, and add the new sampling node q new Add to the update set Q; S53. Take out node q from update set Q cur ; S54, detection node q cur Is there a grandparent node q g If not, execute S56, otherwise determine the grandfather node q g To node q cur Whether the connection is valid, if not valid, execute S56; if valid, execute S55; S55. Delete q from the node connection relationship set E1 or the node connection relationship set E2. cur Its parent node q p The connection relationship (q p ,q cur ), add connection relationship (q g ,q cur ); at this time q g Updated to q cur The parent node of ; then jump to S54; S56, the new sampling node q new All child nodes of are added to the update set Q; S57. If the update set Q is not empty, jump to S53; otherwise, end S5 and execute S6.

8. The global path planning method for a multi-UAV formation according to claim 5, characterized in that: The termination condition in S6 is whether the number of iterations k exceeds the maximum number of iterations N or the algorithm reaches the maximum running time T max .

9. The multi-UAV formation global path planning method according to claim 5, characterized in that: The S7 specifically includes the following steps: S71, judging whether the first random tree T1 and the second random tree T2 are connected. If the connection is not completed, it means that the path solving fails, and exit S7; if the connection is successful, enter S72; S72. Find node q in the intersection point set Π. f , so that through node q f The path cost L f Minimum; S73, from node q f Go to node q respectively f The parent node and its connection point are traced back to obtain a formation global path connecting the formation start state and the formation target state as the solution result.

10. The global path planning method for a multi-UAV formation according to claim 9, characterized in that: The node q in S72 f and the path cost L f The calculation formula is: 50 f =η(q f )+η(q f ′)+c(q f ,q f ′) Among them, q σ ′ is the node q σ The connection point, q f ′ is the node q f connection point.

Images