A Cooperative Formation Control Method and System for UAV Swarms Based on Unknown Environments

Abstract

The present invention discloses a method and system for cooperative formation control of an unmanned aerial vehicle (UAV) cluster based on an unknown environment. The cooperative formation control method of the UAV cluster sets the expected formation positions according to the positions of tracking targets, and constructs the target points of each UAV by flexibly adjusting the specific positions in the formation according to its own position; after constructing the optimal path from the UAV position to the target point and smoothing the optimal path, the UAV is controlled by a model predictive controller to move along the trajectory obtained after processing the optimal path; during the movement of the UAV, the acceleration or altitude of the rear UAV is determined whether to be adjusted by setting the speed adjustment area and position adjustment area of the front UAV to avoid collisions, effectively avoiding potential collision risks and improving the flight safety of the UAV in a complex environment, especially outstanding in a dense or dynamically changing flight area.

Description

Technical Field

[0001] The present invention belongs to the technical field of autonomous planning of UAV swarms, and particularly relates to a method and system for collaborative formation control of UAV swarms based on unknown environments. Background Art

[0002] With the rapid development of UAV technology, the application of UAV swarms in executing complex tasks is becoming increasingly widespread, especially in unknown or complex environments. In such applications, UAV swarms require a high degree of autonomy and collaboration capabilities to effectively execute tasks. However, the current UAV group motion planning and collaboration technologies face several major challenges:

[0003] 1. Environmental uncertainty: In unknown environments, UAV swarms must be able to collect environmental data and react quickly, which poses high requirements for their perception systems and data processing capabilities. UAVs must achieve motion planning and obstacle avoidance without prior knowledge.

[0004] 2. Communication limitations: In many practical application scenarios, such as disaster areas or complex terrain areas, the communication between UAVs may be severely limited. The traditional UAV group collaboration mode that relies on centralized control may not work effectively in these environments because this mode requires a high-bandwidth and low-latency communication network.

[0005] 3. Collaboration efficiency: How to improve the collaboration efficiency of UAV groups when facing complex tasks is a current research hotspot. In particular, how to optimize the collaboration strategies and formation control of UAVs on the premise of ensuring flight safety to adapt to rapidly changing environmental conditions and task requirements.

[0006] In the field of UAV swarm collaborative planning, various methods have been proposed to address various challenges, and each method has its unique advantages and limitations. The centralized control method commands the actions of all UAVs through a central control node. This method enables highly consistent decision-making and simplifies the communication process among UAVs. However, the main weakness of this method is the risk of single-point failure. Once the central control node encounters problems, the entire system may become paralyzed. In addition, as the number of UAVs increases, the computational and communication loads of the central node will rise sharply, thus limiting the scalability of the system. The virtual structure method guides the movement and position arrangement of UAVs by defining a virtual structure framework, and UAVs fly according to the predefined virtual structure. This method helps maintain the order of the group and perform complex formation flight tasks. The virtual structure can be dynamically adjusted according to task requirements to adapt to environmental changes. However, this method may limit the reaction flexibility of UAVs in the face of emergencies, and complex algorithms are required for implementation to adjust and maintain the virtual structure, increasing the computational load of the system. Through the analysis of the advantages and disadvantages of existing methods, it can be seen that no single method can perfectly solve all problems, and each method has its own disadvantages, resulting in poor UAV swarm collaborative planning effects. Therefore, when designing a new UAV swarm cooperation system, it is necessary to overcome the above disadvantages to achieve optimal performance in specific application scenarios. Summary of the Invention

[0007] The object of the present invention is to provide a method and system for collaborative formation control of UAV swarms based on unknown environments.

[0008] In a first aspect, the present invention provides a method for collaborative formation control of UAV swarms based on unknown environments, which includes the following steps:

[0009] Step 1: Construct a three-dimensional occupancy grid map based on the environmental information around the UAV swarm, and obtain the distance from each free area to the nearest obstacle;

[0010] Step 2: Obtain the positions of the tracking target and each UAV, and construct target points for each UAV; use a path planning algorithm to construct the optimal path from the current position of each UAV to the target point;

[0011] Step 3: Control the UAVs to move according to the optimal path; during the movement of the UAVs, divide the adjacent UAVs in the UAV swarm into the leading UAV and the trailing UAV according to the distance from the UAV to the tracking target; respectively set the speed adjustment area and the position adjustment area for the leading UAV; the range of the speed adjustment area is larger than the range of the position adjustment area; if the trailing UAV is within the speed adjustment area, adjust the acceleration of the trailing UAV to avoid collision; if the trailing UAV is within the position adjustment area, adjust the height of the trailing UAV to avoid collision, and re-obtain the optimal path;

[0012] Step Four: Repeat Step Two and Step Three to complete the cooperative formation tracking of the UAV for the tracking target.

[0013] Preferably, in Step Three, the adjusted acceleration a back (k) is obtained as follows:

[0014]

[0015] where is a proportionality constant; k is the sampling time during the movement; d(k) is the distance between the leading aircraft and the trailing aircraft; is the minimum safe distance between UAVs; P front (k) is the position of the leading aircraft; P back (k) is the position of the trailing aircraft.

[0016] Preferably, in Step Three, select the maximum value among the desired altitude, the minimum flight altitude, and the minimum obstacle avoidance altitude of the UAV as the adjusted altitude ; the method for obtaining the desired altitude is as follows:

[0017]

[0018] where h back (k - 1) is the altitude of the trailing aircraft at the previous moment; k is the sampling time during the movement; is a proportionality constant; d(k) is the distance between the leading aircraft and the trailing aircraft; is the minimum safe distance between UAVs;

[0019] The minimum obstacle avoidance altitude is the sum of the safe distance and the altitude of the obstacle corresponding to the position of the UAV.

[0020] Preferably, in Step Two, the method for obtaining the target point of the UAV is as follows:

[0021] Obtain the ideal formation positions of each UAV respectively; if the distance between the current position of a UAV and its ideal formation position is greater than the preset tolerance factor, then each UAV uses the ideal formation position as the target point; otherwise, update the ideal formation positions of each UAV to reduce the distance between the original ideal formation position and the tracking target, and use the updated ideal formation position as the target point.

[0022] Preferably, the method for obtaining the ideal formation position of each UAV in Step Two is as follows:

[0023] Set multiple expected positions according to the position of the tracking target; each UAV corresponds to an expected position, and the closer the UAV is to the tracking target, the closer its corresponding expected position is to the tracking target; use the expected position corresponding to the UAV as the ideal formation position of the UAV.

[0024] Preferably, in the second step, after obtaining the optimal path, use the B-spline method to smooth and optimize the optimal path, and use the path points on the optimal path as the control points of the B-spline curve; obtain the final trajectory according to the optimized curve after smoothing optimization; the UAV moves along the final trajectory obtained according to the optimal path.

[0025] Preferably, in the second step, use the A* algorithm to obtain the optimal path from the current position of the UAV to the target point.

[0026] Preferably, in the second step, use the thinning algorithm to clip the constructed optimal path.

[0027] In a second aspect, the present invention provides a UAV swarm cooperative formation control system based on an unknown environment, which includes multiple UAVs; each UAV is equipped with a sensor module, a model predictive controller, and a distributed cooperative control module; the sensor module is used to detect the states of the UAV and the tracking target; the model predictive controller is used to generate the final trajectory and control the UAV to move along the final trajectory; the distributed cooperative control module avoids trajectory collisions between different UAVs by adjusting the altitude and acceleration of the UAVs; this UAV swarm cooperative formation control system is used for the above-mentioned UAV swarm cooperative formation control method.

[0028] Preferably, the model predictive controller includes a path planning module, a trajectory optimization module, and a motion control module; the path planning module is used to plan the path of the UAV to the target point; the trajectory optimization module is used to smooth the path planned by the path planning module; the motion control module is used to control the UAV to move along the trajectory obtained by the trajectory optimization module.

[0029] The beneficial effects of the present invention are:

[0030] 1. By obtaining the states of adjacent UAVs and dynamically adjusting the flight trajectory and speed, the present invention effectively avoids potential collision risks and improves the flight safety of UAVs in complex environments, especially in dense or dynamically changing flight areas; at the same time, by optimizing the cooperation strategy between UAVs, the UAVs can flexibly adjust their specific positions in the formation according to their own positions without presetting fixed positions, enabling the UAVs to perform tasks more coordinately. When performing complex tasks such as search and rescue and environmental monitoring, the UAV swarm can cover a larger area faster and maintain high resource utilization and task completion rate, realizing the efficient cooperation of the UAV group in an unknown environment.

[0031] 2. Without comprehensive environmental information, the present invention realizes efficient planning and cooperation of a group through local perception communication, solves the problems of motion planning and cooperative control in complex or unpredictable environments, enables the unmanned aerial vehicles (UAVs) to avoid collisions while quickly adapting to environmental changes, and optimizes the flight trajectories and formation structures in real time, thereby improving the overall performance and efficiency of the UAV swarm when performing complex tasks.

[0032] 3. The present invention smooths the optimal path through a spline curve, can effectively generate smooth flight trajectories, and is more reliable and safe when performing complex tasks; at the same time, by trimming the optimal path obtained from path planning, the computational amount in the process of smoothing by the spline curve is reduced, and the processing speed is improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] Figure 1 is the overall flowchart of the present invention.

[0034] Figure 2 is the schematic diagram of autonomous decision-making formation in the present invention.

[0035] Figure 3 is the schematic diagram of the UAV control process in the present invention.

[0036] Figure 4 is the schematic diagram of the constructed collision detection area in the present invention.

[0037] Figure 5 is the schematic diagram of the comparison of cooperative free-crossing flight trajectories between the present invention and the prior art in Scenario 1; among them, (a) is the schematic diagram of the trajectory planned by the EGO-Swarm cooperative planning system; (b) is the schematic diagram of the trajectory planned by the LSC-Planner cooperative planning system; (c) is the schematic diagram of the trajectory planned by the present invention.

[0038] Figure 6 is the schematic diagram of the comparison of cooperative free-crossing flight trajectories between the present invention and the prior art in Scenario 2; among them, (a) is the schematic diagram of the trajectory planned by the EGO-Swarm cooperative planning system; (b) is the schematic diagram of the trajectory planned by the LSC-Planner cooperative planning system; (c) is the schematic diagram of the trajectory planned by the present invention.

[0039] Figure 7 is the schematic diagram of the comparison of cooperative formation-crossing flight trajectories between the present invention and the EGO-Swarm cooperative planning system; among them, (a) is the schematic diagram of the trajectory planned by the EGO-Swarm cooperative planning system; (b) is the schematic diagram of the trajectory planned by the present invention.

[0040] Figure 8 is the schematic diagram of the cooperative dynamic formation-crossing flight trajectory of the present invention.

[0041] Figure 9 This is a schematic diagram for comparing the cooperative formation dynamic target tracking flight trajectories of the present invention and the EGO-Swarm cooperative planning system; among them, (a) is the schematic diagram of the trajectory planned by the EGO-Swarm cooperative planning system; (b) is the schematic diagram of the trajectory planned by the present invention.

[0042] Figure 10 It is a schematic diagram of the flight trajectories of different UAVs collaborating to cross an experiment.

[0043] Figure 11 It is a schematic diagram of the flight trajectories of different UAVs collaborating in formation to cross an experiment.

[0044] Figure 12 It is a schematic diagram of the flight trajectories of different UAVs collaborating in formation to track dynamic targets in an experiment. Detailed implementation manners

[0045] The present invention will be further described below with reference to the accompanying drawings.

[0046] As Figure 1 shown, a UAV swarm cooperative formation control method based on an unknown environment adopts a UAV swarm cooperative control system including multiple UAVs; each UAV is equipped with a sensor module, a model predictive controller (MPC), and a distributed cooperative control module; the sensor module includes a depth camera, an inertial measurement unit, an ultra-wideband positioning module, and an optical flow sensor; the depth camera is used to obtain environmental information; the inertial measurement unit is used to measure the motion state of the UAV; the ultra-wideband positioning module is used to perform real-time positioning on the UAV; the optical flow sensor is used to obtain the altitude information of the UAV; the model predictive controller includes a path planning module, a trajectory optimization module, and a motion control module; the path planning module is used to plan the path of the UAV to the target point; the trajectory optimization module is used to smooth the path planned by the path planning module; the motion control module is used to control the UAV to move along the trajectory obtained by the trajectory optimization module; the distributed cooperative control module is used to avoid trajectory collisions between different UAVs.

[0047] The UAV swarm cooperative control method includes the following steps:

[0048] Step 1: Construct a map

[0049] Use the depth camera carried by the UAV to obtain the point cloud data in the environment, construct a three-dimensional occupancy grid map, and generate an Euclidean signed distance field (ESDF) map to represent the distance from each free area to the nearest obstacle.

[0050] Step 2: Construct target points based on the autonomous formation strategy

[0051] As Figure 2As shown, obtain the current position P of each UAV c and the expected position ; form an expected formation by expanding formation points outward from the formation center, and move the formation center to the tracking target position to obtain the expected position , and its expression is:

[0052] (1)

[0053] where P center is the position of the formation center, ; is the current position of the th UAV; n is the number of UAVs; is the displacement deviation of the formation point relative to the formation center; is the preset safety observation distance adjustment factor between the formation center and the tracking target; is the displacement vector from the formation center to the target point.

[0054] Ascendingly sort the current position P of the UAV c and the expected position F t respectively according to the Euclidean distance from the tracking target. According to the sorted current position P of the UAV c and the expected position F t obtain the ideal formation position of each UAV , and the obtaining method is as follows:

[0055] (2)

[0056] where x i is the Euclidean distance from the UAV to the tracking target.

[0057] In the formation, the UAVs closer to the tracking target fly to the expected positions farther from the formation center; it provides higher flexibility compared to the fixed decision formation strategy. At the same time, this method reduces the risk of collision between UAVs during the formation process, thereby improving the safety and efficiency of the UAV swarm formation.

[0058] In addition, introduce the tolerance factor of the UAV formation. If the relative Euclidean distance between the current position of the UAV and its ideal formation position is greater than , the UAV swarm takes the ideal formation position as the target point to maintain the stability of the overall formation. Otherwise, reset the expected position to reduce the distance from the original expected position to the tracking target, and its expression is:

[0059] (3)

[0060] According to the re-set expected position Obtain the ideal position of the formation and use the ideal position of the formation as the target point

[0061] In this embodiment, the tolerance factor has a value range of 1m to 3m

[0062] Through the tolerance factor , when the overall formation of the UAV swarm is unstable, the ideal position of the formation is used as the target point, so as to keep the overall formation stable. After the overall formation is stable, the exploration efficiency is improved by real-time planning of the flight trajectory. The tolerance factor of the autonomous decision-making formation reduces the dependence on the precise position of the formation, enhances the robustness of the formation, improves the scalability of the formation, is applicable to resource-constrained scenarios, and has higher flexibility

[0063] Step 3. Path planning

[0064] Based on the three-dimensional space kinematic constraint A* (3D kinodynamic-A-star) search method, obtain the optimal path of the UAV swarm from the current position to the target point; this algorithm considers the dynamic characteristics of the UAV to ensure the feasibility and safety of the path. By applying the Pontryagin minimum principle to calculate the minimum cost J*(T) of the UAV from the current state to the target state , which is expressed as follows

[0065] (4)

[0066] Among them and are coefficients represents the time of the trajectory segment and are the velocity information and position information of the current state respectively and are the velocity information and position information of the target state respectively; μ is the direction variable; (x, y, z) is the position of the UAV in three-dimensional space

[0067] Calculate the minimum time from the current state to the target state and its corresponding minimum cost , as well as the actual cost of the optimal path from the starting state to the current state. According to the minimum cost and the actual cost obtain the total cost function which is expressed as follows

[0068] (5)

[0069] To improve the efficiency of the A* algorithm path planning, a target point is introduced during the planning process to optimize the local path planning. Compared with the global path planning, this method can significantly reduce the search burden.

[0070] The Ramer-Douglas-Peucker (RDP) algorithm is used to clip the path planned by the A* algorithm. This algorithm reduces the number of path points while preserving the overall path shape characteristics. The processing method is as follows:

[0071] (6)

[0072] Among them, is the path point sequence; represents the recursive call to the path; P max is the path point and the path point is the path point with the farthest distance from the line connecting is the threshold value used to control the degree of path simplification; represents the direction vector from to ; P i represents the position vector of the i-th path point.

[0073] Step Four: Trajectory Optimization

[0074] Construct a Gaussian potential field in the ESDF environmental map to obtain the environmental information of the Gaussian potential field of the three-dimensional obstacle; apply the Gaussian Potential Field B-Spline (GPB) method to optimize the trajectory of the path generated in Step Two. Convert the clipped path points into B-spline curve control points to make the final trajectory smoother and meet the flight dynamics constraints of the UAV. Perform collision detection on the optimized curve. If there is no collision risk on the optimized curve, that is, the potential energy of the points on the optimized curve is lower than or equal to the minimum potential energy of the obstacle, then use the optimized curve as the final trajectory; otherwise, adjust the optimized curve away from the obstacle by the gradient descent method to obtain a safe and smooth B-spline curve as the final trajectory, which is more adaptable to complex environments and ensures safety.

[0075] Step Five: Motion Control

[0076] 5-1. As Figure 3 shown, taking the final trajectory as the desired state of the UAV, use a Model Predictive Control (MPC) controller to control the UAV to fly stably according to the desired state. Define the state vector of the UAV as X = [ x , y , z , ϕ , θ , ψ ] T , the control input vector is U = [ F 1 , F 2 , F 3 , F 4 ] T . At the sampling time , the state-space equation can be obtained as follows:

[0077] (7)

[0078] where, X(k) is the predicted state at the sampling time ; U(k) is the control input at the sampling time ; f is the discretized dynamic equation of the UAV.

[0079] To control the UAV to fly stably along the desired state, a quadratic function is used to calculate the distance error between the predicted state and the desired state, and the error minimization is used as the cost function. In the prediction time domain [0, N], the objective function J is defined as follows:

[0080] (8)

[0081] where, and are the state weight matrix and the input weight matrix respectively, ; is the terminal weight matrix, ; is the desired state at the sampling time .

[0082] Since the thrust generated by the rotation of the UAV propeller is limited by voltage, the cost function for optimizing the shortest distance between the UAV and the final path is expressed as:

[0083] (9)

[0084] 5-2. Distributed Cooperative Control

[0085] The distributed cooperative control method (Back Field Neighbor Planning, BFNP) aims to improve the collaboration efficiency and safety of UAV swarms under communication-constrained conditions. Set the information exchange protocol between UAVs to ensure that each UAV can receive the position, speed, and predicted trajectory information of adjacent UAVs in a timely manner; each UAV communicates with neighboring UAVs through the UAV swarm network information to obtain position information from each other, thus reducing the communication burden on the UAV swarm network. The core of this method lies in optimizing the information exchange strategy between UAVs, enabling each UAV to exchange necessary information only with its direct neighbors, rather than communicating extensively with the entire group. This direct communication mode between neighbors significantly reduces the steps and delays of information transmission, improving the reaction speed and the overall efficiency of the system. Through localized communication and collaboration, based on the collected neighbor information, each UAV calculates the potential collision area and adjusts its flight trajectory and speed when necessary to avoid collisions with neighboring UAVs and quickly adapt to environmental changes, achieving a more precise and safe flight configuration in complex environments.

[0086] As Figure 4 shown, according to the Euclidean distance between the UAV swarm and the tracking target, adjacent UAVs are divided into the leading UAV and the trailing UAV; if the trajectories planned by the leading UAV and the trailing UAV intersect during the exploration process, the two UAVs may face a collision risk. To avoid the collision risk and maximize the exploration efficiency of the UAVs, only the trailing UAV is adjusted. Create a safety buffer by inflating the UAV size based on the wheelbase information. For the leading UAV, establish a collision detection area and divide the collision detection area into a speed adjustment area and a position adjustment area; respectively set the distance threshold of the speed adjustment area and the distance threshold of the position adjustment area. During the flight of the UAV swarm controlled by the MPC controller, if the Euclidean distance d(k) between the leading UAV and the trailing UAV is greater than the distance threshold and less than the distance threshold , there is no need to re-plan the optimized trajectories of the two UAVs. Just control the acceleration of the trailing UAV to ensure the safe distance between the two UAVs to avoid collisions, thereby improving the flight efficiency of the UAVs. The expression of the acceleration a back (k) is:

[0087] (10)

[0088] where is a proportionality constant used to control speed adjustment; is the minimum safe distance between UAVs; P front (k) is the position of the leading UAV; P back (k) is the position of the trailing UAV.

[0089] If the distance d(k) is less than or equal to the distance threshold , it is considered that the distance between the two UAVs is too close, and there is a high risk of collision. The altitude of the rear UAV must be adjusted immediately to avoid collision. Obtain the expected altitude of the UAV at the current moment The expression for is:

[0090] (11)

[0091] where is a proportional constant used to control the range of altitude adjustment; h back (k) is the altitude of the rear UAV at sampling time k.

[0092] To ensure that the rear UAV regenerates the trajectory at a safe altitude, obstacle constraints and minimum altitude constraints are added to the UAV altitude adjustment function to obtain the adjusted altitude as follows:

[0093] (12)

[0094] where h obs (k) is the obstacle altitude corresponding to the position of the UAV at sampling time k; is the safe distance between the UAV and the obstacle; is the minimum flight altitude.

[0095] The rear UAV re-plans the path and optimizes the trajectory based on the adjusted altitude to plan the motion trajectory; the collision risk of the UAV swarm can be effectively reduced in a dense space through the position adjustment area method, a new trajectory can be re-planned safely in real time, and the flight efficiency and safety can be improved.

[0096] 5-3. Repeat steps 5-1 and 5-2 until all UAVs track their respective target points.

[0097] Step Six. Repeat steps two to five to complete the tracking of the tracking target by the UAV.

[0098] Step Seven. Simulation

[0099] The simulation experiment consists of three parts:

[0100] Task 1: Cooperative free crossing simulation to verify the cooperative safe flight ability between UAV swarms.

[0101] Task 2: Cooperative formation crossing simulation to verify the autonomous and safe formation flight ability of the UAV swarm.

[0102] Task 3: Cooperative formation dynamic target tracking simulation to verify the ability of the UAV swarm to autonomously cooperate and plan to track the target.

[0103] In all tasks, the maximum speed of the drone is , and the maximum acceleration is .

[0104] 7-1. In the collaborative free flight simulation, two experimental scenarios were set up:

[0105] Scenario 1: In the dense environment map, 10 drones achieved safe flight through collaborative planning.

[0106] Scenario 2: In the unknown environment map, 6 drones efficiently and quickly traversed the unknown environment through collaborative planning.

[0107] The present invention was respectively used in collaboration with the LSC-Planner collaborative planning system and the EGO-Swarm collaborative planning system for collaborative free flight in the above two scenarios. In Scenario 1, the comparison results of the flight trajectories of 10 drones are as Figure 5 shown. It can be seen from Figure 5 that compared with EGO-Swarm and LSC-Planner, the flight trajectories planned by the present invention are smoother and have a smaller curvature, thus providing a more stable flight. The trajectory curvature of EGO-Swarm is relatively large, which may cause large jitters during flight. The LSC-Planner trajectory is relatively long, which consumes more flight resources. In terms of the total flight time, KGPB-BFNP only takes 6.43 seconds, while EGO-Swarm takes 8.41 seconds and LSC-Planner takes 14.6 seconds.

[0108] During the collaborative planning and traversal process, assuming the collision radius of the drone is 0.1 m, when the distance between two drones is less than 0.2 m, it is considered a collision. During the collaborative planning and traversal process of different methods, the minimum distance between each drone and its nearest neighbor drone is shown in Table 1.

[0109] Table 1 Minimum distance between the drone and its nearest neighbor drone (cm)

[0110] 1 2 3 4 5 6 7 8 9 10 EGO-Swarm 0.442 0.195 0.284 0.215 0.243 0.195 0.547 0.310 0.228 0.215 LSC-Planner 0.317 0.316 0.347 0.328 0.305 0.316 0.431 0.305 0.668 0.317 The present invention 0.361 0.403 0.369 0.671 0.392 0.400 0.361 0.382 0.392 0.411

[0111] As can be seen from Table 1, in EGO-Swarm, the minimum distance between adjacent drones is the distance of 0.195 m between Drone 2 and Drone 6, presenting a collision risk. In LSC-Planner, the minimum distance between adjacent drones is 0.305 m between Drone 5 and Drone 8, without any collision. In the present invention, the minimum distance between adjacent drones is the distance of 0.361 m between Drone 1 and Drone 7, without any collision. The present invention can better control the safety distance between drones in a dense environment, ensuring safe and collaborative flight.

[0112] The comparison results of the flight trajectories collaboratively planned by 6 drones are as Figure 6 shown. As Figure 6 can be seen, all drone swarms successfully reach the target points in the unknown environment under the control of the three collaborative planning systems. The flight trajectory planned by the present invention is smoother compared to EGO-Swarm and LSC-Planner. In terms of the total flight time, KGPB-BFNP has the best performance, only taking 26.82 seconds, while EGO-Swarm takes 39.79 seconds and LSC-Planner takes 64.1 seconds.

[0113] 7-2. Conduct a collaborative formation flight simulation using the present invention and the EGO-Swarm collaborative planning system; deploy 6 drones with randomly distributed initial positions. The entire drone swarm initially forms a hexagonal formation with a side length of 3 meters based on their respective position information, and then maintains this formation to cross the unknown environment map. The experimental results of the drone swarm formation crossing the unknown environment are as Figure 7 shown. For the fixed formation method of EGO-Swarm based on Laplacian graph theory, the total flight time is 39.53 seconds, and the average speed of the formation is 1.72 m / s. Although it can maintain a relatively stable formation in the initial stage, due to the increasing environmental complexity, the drones cannot balance the formation structure and safe trajectory planning, and with a large computational amount, some drones fail to plan the formation flight trajectory, and only a few drones successfully cross the environment. For the present invention based on the autonomous decision-making formation strategy, the total flight time is 38.41 seconds, and the average speed of the formation is 2.05 m / s. Compared with EGO-Swarm, the formation of the present invention does not require presetting the positions of the drones, but autonomously decides the nearest expected formation positions according to the currently obtained drone position information, thus forming a flexible formation, which shows a faster average speed and a tighter formation structure, and has higher efficiency in crossing the unknown environment.

[0114] In addition, apply the autonomous decision-making formation strategy of the present invention to dynamic formation transformation to cross the unknown environment, as Figure 8As shown. In the simulation, the UAV swarm formed formations in the shapes of letters such as "I", "A", "U", "S", "L", "I", etc. During the formation flight, the randomly distributed UAVs formed formations according to their numbers and entered the unknown environment. Using the autonomous decision-making formation strategy, the formation was changed every 10 s, and the average flight speed of the entire formation was 1.92 m / s. The experimental results show that during the entire flight process, the formation can smoothly perform formation transformation and spatial position adjustment, effectively verifying that the present invention improves the space utilization rate of the UAV swarm based on the autonomous decision-making formation strategy and enhances its adaptability in the unknown environment.

[0115] 7-3. Use the present invention and the EGO-Swarm collaborative planning system for collaborative formation dynamic target tracking; use 6 UAVs to randomly set the initial positions in the unknown environment map. A randomly moving target was set in the 3D space in this experiment, and its speed range in the X direction was: , in the Y direction: , in the Z direction: . To ensure the reasonable movement of the target in the environment map, the displacement range in the Z direction was limited to . The simulation tracking results are as Figure 9 shown. EGO-Swarm failed to complete the formation cooperation tracking task. The main reason was that it failed to balance the target tracking and formation maintenance processes; at 45 s, the formation could not calculate the optimal trajectory to continue moving forward, resulting in tracking failure. At the same time, EGO-Swarm needed to maintain a fixed formation for target tracking at the beginning of the task, which limited the ability of the UAV swarm to dynamically adjust the formation. The present invention successfully completed the formation cooperation tracking task; in the stage, the present invention controlled the UAVs to track the dynamic target at the maximum speed. During this stage, the UAV cluster needed to quickly shorten the distance from the target. During this period, the UAV speed was relatively high, indicating that the present invention can quickly respond to the change of the target position. As the distance between the UAV cluster and the target gradually decreased, the UAVs began to adjust to a hexagonal formation while continuously collaborating to track the dynamic target to ensure the stable execution of the tracking task.

[0116] Step eight. Actual experimental verification

[0117] To verify the applicability of the distributed KGPB-BFNP cluster collaborative planning system in a real environment, the experimental verification was carried out in an outdoor dense forest environment. Four small quadrotor drones with a wheelbase of 110 mm were used in the experiment, and YOLOv5 and an RGB camera were used to identify and track target information. Considering that GPS signals may be lost in an unknown environment, the drones used VINS-Fusion to achieve autonomous positioning; in addition, an ultra-wideband system was adopted for the exchange of position information between drones, so as to improve the reliability of the group in collaborative tasks. In the forest environment, the four drones started from the starting point (0, 0, 0), and the target point was set as (20, 0, 1). The experiment required that all drones reach the target point successfully while ensuring no collision. The planned trajectories and total flight times of each drone in the dense forest environment are as Figure 10 shown. From the flight trajectories of the drone swarm, it can be observed that the four drones can autonomously adjust their flight trajectories to avoid obstacles and other drones that are about to collide. In addition, the drone swarm completed the task efficiently and showed good obstacle avoidance ability in the dense forest environment. The experimental results verified that the present invention can achieve efficient group motion planning and has high safety.

[0118] The collaborative formation flight through experiment required the four drones to change their formations at different time points to successfully fly through the dense forest environment. The drone swarm started the task at 0 seconds. It adjusted to a triangular formation at 5 seconds. It adjusted to a quadrilateral formation at 10 seconds. During the subsequent flight, the drone swarm first expanded the formation scale and then began to shrink the formation, and finally reached the end point at 22 seconds. The flight data of the drone swarm during the dynamic formation change are as Figure 11 shown. During the entire flight through process, each drone can dynamically adjust its formation according to the task requirements to adapt to the flight through task in the dense forest environment, fully demonstrating the flexibility of formation change and the collaborative control ability.

[0119] The collaborative formation tracking of a dynamic target experiment required the four drones to collaboratively track a dynamic target in a quadrilateral formation in the dense forest environment. The trajectories and position information of the four drones collaboratively tracking the dynamic target in the quadrilateral formation are as Figure 12 shown. The tolerance of the side length of the 3D quadrilateral of the formation was set to not exceed 3 m. The blue dots represent the position information of the drone formation, and the red dots represent the position of the target being tracked. During the entire tracking process, the formation always maintained a three-dimensional quadrilateral structure and ensured that the longest side length was less than 3 m. During the dynamic target tracking process, the speed of the drones was adjusted according to the position of the tracking target. Especially in the area where the target moved fast, the speed of the drones would increase rapidly to ensure the overall collaboration of the formation and the accuracy of target tracking.

Claims

1. A method for controlling a swarm of drones in an unknown environment, characterized in that: The following steps are involved: Step 1: Construct a three-dimensional occupancy grid map based on the environmental information around the drone swarm, and obtain the distance from each free area to the nearest obstacle; Step 2: Get the position of the tracking target and each drone, and build the target point of each drone; Use the path planning algorithm to construct the optimal path from the current position of each drone to the target point; Step 3: Control the drone to move according to the optimal path; during the movement of the drone, divide the adjacent drones in the drone group into the front drone and the rear drone according to the distance between the drone and the tracking target; set the speed adjustment area and the position adjustment area of ​​the front drone respectively; the range of the speed adjustment area is larger than the range of the position adjustment area; if the rear drone is within the speed adjustment area, adjust the acceleration of the rear drone to avoid collision; if the rear drone is within the position adjustment area, adjust the height of the rear drone to avoid collision, and re-acquire the optimal path; In step 3, the adjusted acceleration a back The method for obtaining (k) is as follows: ； in, is a proportional constant; k is the sampling time during the motion process; d(k) is the distance between the front and rear machines; is the minimum safe distance between drones; P front (k) is the front position; P back (k) is the rear position; Step 4: Repeat steps 2 and 3 to complete the coordinated formation tracking of the target by the drones.

2. The method for controlling a drone swarm in an unknown environment according to claim 1, characterized in that: In step 3, the maximum height among the desired height, minimum flight height and minimum obstacle avoidance height of the drone is selected as the adjusted height. ; The desired height The way to obtain is as follows: ； Among them, h back (k) is the height of the rear machine at the last moment; k is the sampling time during the movement; is the proportional constant; d(k) is the distance between the front and rear aircraft; is the minimum safe distance between drones; The minimum obstacle avoidance height is the sum of the safety distance and the obstacle height corresponding to the position of the drone.

3. The method for controlling a drone swarm in an unknown environment according to claim 1, characterized in that: In the step 2, the method for obtaining the target point of the drone is as follows: The ideal position of each UAV in the formation is obtained respectively; if the distance between the current position of a UAV and its ideal position of the formation is greater than the preset tolerance factor, each UAV takes the ideal position of the formation as the target point; Otherwise, the ideal formation position of each UAV is updated to reduce the distance between the original ideal formation position and the tracking target, and the updated ideal formation position is used as the target point.

4. The method for controlling a drone swarm in an unknown environment according to claim 3, characterized in that: In the above step 2, the method for obtaining the ideal formation position of each drone is as follows: A plurality of expected positions are set according to the position and formation of the tracking target; each UAV corresponds to an expected position and the closer the UAV is to the tracking target, the closer its corresponding expected position is to the tracking target; the expected position corresponding to the UAV is used as the ideal position of the UAV formation.

5. The method for controlling a drone swarm in an unknown environment according to claim 1, characterized in that: In the step 2, after obtaining the optimal path, the optimal path is smoothly optimized using the B-spline method, and the path point pairs on the optimal path are used as B-spline curve control points; the final trajectory is obtained according to the optimized curve after smooth optimization; the UAV moves along the final trajectory obtained according to the optimal path.

6. The method for controlling a drone swarm in an unknown environment according to claim 1, characterized in that: In the step 2, the A* algorithm is used to obtain the optimal path from the current position of the drone to the target point.

7. The method for controlling a drone swarm in an unknown environment according to claim 1, characterized in that: In the step 2, a thinning algorithm is used to perform path pruning on the constructed optimal path.

8. A UAV swarm collaborative formation control system based on an unknown environment, comprising a plurality of UAVs; each UAV is equipped with a sensor module and a model predictive controller; the sensor module is used to detect the state of the UAV and the tracking target; the model predictive controller is used to generate a final trajectory and control the UAV to move along the final trajectory; the characteristics are: Each drone is also equipped with a distributed collaborative control module; the distributed collaborative control module avoids trajectory collisions between different drones by adjusting the altitude and acceleration of the drone; the drone cluster collaborative formation control system is used to execute the drone cluster collaborative formation control method described in claim 1.

9. The UAV swarm collaborative formation control system based on an unknown environment according to claim 8, characterized in that: The model prediction controller includes a path planning module, a trajectory optimization module and a motion control module; the path planning module is used to plan the path of the UAV to the target point; the trajectory optimization module is used to smooth the path planned by the path planning module; and the motion control module is used to control the UAV to move along the trajectory obtained by the trajectory optimization module.

Images