Unmanned aerial vehicle cluster collaborative formation control method and system based on unknown environment

Abstract

The invention discloses an unmanned aerial vehicle cluster cooperative formation control method and system based on an unknown environment, and the method comprises the steps: setting a formation expectation position according to a tracking target position, and flexibly adjusting a specific position in a formation according to the position of the unmanned aerial vehicle, so as to construct a target point of each unmanned aerial vehicle; an optimal path from the position of an unmanned aerial vehicle to a target point is constructed, and after the optimal path is smoothed, the unmanned aerial vehicle is controlled by a model prediction controller to move along a track after the optimal path is processed; in the moving process of the unmanned aerial vehicle, whether the acceleration or height of the rear vehicle is adjusted or not is determined by setting the speed adjusting area and the position adjusting area of the front vehicle to avoid collision, so that the potential collision risk is effectively avoided, the flight safety of the unmanned aerial vehicle in a complex environment is improved, and particularly, the performance is 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 unmanned aerial vehicle clusters, and specifically relates to a method and system for cooperative formation control of unmanned aerial vehicle clusters based on an unknown environment. Background Art

[0002] With the rapid development of drone technology, drone swarms are increasingly being used to perform complex tasks, especially in unknown or complex environments. In such applications, drone swarms require a high degree of autonomy and collaboration to effectively perform tasks. However, current drone swarm motion planning and collaboration technologies face several major challenges:

[0003] 1. Environmental Unknownness: In an unknown environment, drone swarms must be able to collect environmental data and respond quickly, which places high demands on their perception systems and data processing capabilities. Drones 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, communication between drones may be severely limited. The traditional drone swarm collaboration model that relies on centralized control may not work effectively in these environments because this model requires a high-bandwidth and low-latency communication network.

[0005] 3. Collaboration efficiency: How to improve the coordination efficiency of drone groups when facing complex tasks is a hot topic in current research, especially how to optimize the coordination strategy and formation control of drones to adapt to rapidly changing environmental conditions and mission requirements while ensuring flight safety.

[0006] In the field of UAV swarm collaborative planning, a variety of methods have been proposed to address various challenges, each with its own unique advantages and limitations. The centralized control method commands the actions of all UAVs through a central control node. This method makes decisions highly consistent and simplifies the communication process between UAVs. However, the main weakness of this method is the risk of single point failure. Once the central control node has a problem, the entire system may be paralyzed. In addition, as the number of UAVs increases, the computational and communication loads of the central node will rise sharply, limiting the scalability of the system. The virtual structure method guides the movement and position arrangement of UAVs by defining a virtual structural framework, and the UAVs fly according to the predetermined virtual structure. This method helps maintain the orderliness of the group and perform complex formation flight tasks. The virtual structure can be dynamically adjusted according to mission requirements to adapt to environmental changes. However, this method may limit the flexibility of UAV response in the face of emergencies, and complex algorithms are required to adjust and maintain the virtual structure during implementation, which increases the computational load of the system. Through the analysis of the advantages and disadvantages of the existing methods, it can be seen that no single method can perfectly solve all problems, and each method has its own shortcomings, resulting in poor results in UAV swarm collaborative planning. Therefore, it is necessary to design a new UAV swarm collaboration system to overcome the above shortcomings and achieve optimal performance in specific application scenarios. Summary of the invention

[0007] The purpose of the present invention is to provide a method and system for controlling a swarm of UAVs in an unknown environment.

[0008] In a first aspect, the present invention provides a method for controlling a drone cluster collaborative formation based on an unknown environment, which comprises the following steps:

[0009] 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;

[0010] Step 2: Obtain the position of the tracking target and each drone, and construct 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;

[0011] 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;

[0012] Step 4: Repeat steps 2 and 3 to complete the coordinated formation tracking of the target by the drones.

[0013] Preferably, in step 3, the adjusted acceleration a back The method for obtaining (k) is as follows:

[0014]

[0015] 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.

[0016] Preferably, in step 3, the maximum height is selected from the desired height of the drone, the minimum flight height, and the minimum obstacle avoidance height as the adjusted height. ; The desired height The way to obtain is as follows:

[0017]

[0018] Among them, h back (k-1) is the height of the rear machine at the previous 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;

[0019] The minimum obstacle avoidance height is the sum of the safety distance and the obstacle height corresponding to the position of the drone.

[0020] Preferably, in step 2, the method for obtaining the target point of the drone is as follows:

[0021] 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 in 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 position of the formation of each UAV is updated to reduce the distance between the original ideal position of the formation and the tracking target, and the updated ideal position of the formation is used as the target point.

[0022] Preferably, in step 2, the method for obtaining the ideal formation position of each drone is as follows:

[0023] A plurality of expected positions are set 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; the expected position corresponding to the UAV is used as the ideal position of the UAV formation.

[0024] Preferably, 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 optimization curve after smooth optimization; and the UAV moves along the final trajectory obtained according to the optimal path.

[0025] Preferably, in step 2, an A* algorithm is used to obtain the optimal path from the current position of the drone to the target point.

[0026] Preferably, in step 2, a thinning algorithm is used to perform path pruning on the constructed optimal path.

[0027] In the second aspect, the present invention provides a drone cluster collaborative formation control system based on an unknown environment, which includes multiple drones; each drone is equipped with a sensor module, a model prediction controller and a distributed collaborative control module; the sensor module is used to detect the status of the drone and the tracking target; the model prediction controller is used to generate a final trajectory and control the drone to move along the final trajectory; the distributed collaborative control module avoids trajectory collisions between different drones by adjusting the height and acceleration of the drone; the drone cluster collaborative formation control system is used for the above-mentioned drone cluster collaborative formation control method.

[0028] Preferably, 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.

[0029] The present invention has the following beneficial effects:

[0030] 1. The present invention dynamically adjusts the flight trajectory and speed by acquiring the status of adjacent drones, thereby effectively avoiding potential collision risks and improving the flight safety of drones in complex environments, especially in dense or dynamically changing flight areas; at the same time, by optimizing the cooperation strategy between drones, drones can flexibly adjust their specific positions in the formation according to their own positions without presetting fixed positions, so that drones can perform tasks more coordinatedly. When performing complex tasks such as search and rescue, environmental monitoring, etc., drone clusters can cover larger areas faster and maintain efficient resource utilization and task completion rate, thereby realizing efficient cooperation of drone groups in unknown environments.

[0031] 2. In the absence of comprehensive environmental information, the present invention achieves efficient planning and collaboration of the group through local perception communication, solves the problems of motion planning and collaborative control in complex or unpredictable environments, and enables drones to quickly adapt to environmental changes while avoiding collisions, and optimize flight trajectories and formation structures in real time, thereby improving the overall performance and efficiency of drone clusters when performing complex tasks.

[0032] 3. The present invention uses spline curves to smooth the optimal path, which can effectively generate a smooth flight trajectory, making it more reliable and safer when performing complex tasks; at the same time, by cutting the optimal path obtained by path planning, the amount of calculation in the spline curve smoothing process is reduced, and the processing speed is improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] Figure 1 It is the overall flow chart of the present invention.

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

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

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

[0037] Figure 5 Schematic diagram comparing the collaborative free-travel flight trajectories of the present invention and the prior art in scenario 1; wherein, (a) is a schematic diagram of the trajectory planned by the EGO-Swarm collaborative planning system; (b) is a schematic diagram of the trajectory planned by the LSC-Planner collaborative planning system; and (c) is a schematic diagram of the trajectory planned by the present invention.

[0038] Figure 6 Schematic diagram comparing the collaborative free-travel flight trajectories of the present invention and the prior art in scenario 2; wherein, (a) is a schematic diagram of the trajectory planned by the EGO-Swarm collaborative planning system; (b) is a schematic diagram of the trajectory planned by the LSC-Planner collaborative planning system; and (c) is a schematic diagram of the trajectory planned by the present invention.

[0039] Figure 7 Schematic diagram comparing the collaborative formation crossing flight trajectories of the present invention and the EGO-Swarm collaborative planning system; wherein (a) is a schematic diagram of the trajectory planned by the EGO-Swarm collaborative planning system; and (b) is a schematic diagram of the trajectory planned by the present invention.

[0040] Figure 8 It is a schematic diagram of the collaborative dynamic formation crossing flight trajectory of the present invention.

[0041] Fig. 9 Schematic diagram comparing the dynamic target tracking flight trajectories of the collaborative formation of the present invention and the EGO-Swarm collaborative planning system; wherein (a) is a schematic diagram of the trajectory planned by the EGO-Swarm collaborative planning system; and (b) is a schematic diagram of the trajectory planned by the present invention.

[0042] Fig.10 Schematic diagram of the flight trajectories of different UAVs’ collaborative crossing experiments.

[0043] Fig.11 Schematic diagram of the flight trajectories of different UAV cooperative formation crossing experiments.

[0044] Fig.12 Schematic diagram of the flight trajectory of the experiment of different UAVs cooperating in formation to track dynamic targets. DETAILED DESCRIPTION

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

[0046] like Figure 1 As shown, a UAV cluster collaborative formation control method based on an unknown environment adopts a UAV cluster collaborative control system including multiple UAVs; each UAV is equipped with a sensor module, a model predictive controller (MPC) and a distributed collaborative 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 locate the UAV in real time; the optical flow sensor is used to obtain the height 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 from 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 collaborative control module is used to avoid trajectory collisions between different UAVs.

[0047] The UAV cluster collaborative control method comprises the following steps:

[0048] Step 1: Build a map

[0049] The point cloud data in the environment is acquired using the depth camera carried by the drone, a three-dimensional occupancy grid map is constructed, and a Euclidean signed distance field (ESDF) map is generated to represent the distance from each free area to the nearest obstacle.

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

[0051] like Figure 2As shown, get the current position P of each drone c and expected location ; Expand the formation points outward from the formation center to form the expected formation, move the formation center to the tracking target position, and get the expected position , whose expression is:

[0052] (1)

[0053] Among them, P center is the position of the formation center, ; For the The current position of the drone; n is the number of drones; is the displacement deviation of the formation point relative to the formation center; Adjustment factor for the preset safe observation distance between the formation center and the tracking target; is the displacement vector from the formation center to the target point.

[0054] According to the Euclidean distance to the tracking target, the current position P of the UAV is calculated. c and the expected position F t Arrange in ascending order, according to the current position P of the sorted drone c and the expected position F t Get the ideal formation position of each drone , and the acquisition method is as follows:

[0055] (2)

[0056] Among them, x i is the Euclidean distance from the UAV to the tracking target.

[0057] In the formation, the drones that are closer to the tracking target fly to the expected position that is farther away from the formation center; compared with the fixed decision formation strategy, it provides higher flexibility. At the same time, this method reduces the risk of mutual collision between drones during the formation process, thereby improving the safety and efficiency of the drone group formation.

[0058] In addition, the tolerance factor of the UAV formation is introduced If there is a relative Euclidean distance between the current position of the drone and its ideal position in the formation Greater than When the drone group is in the formation, it takes the ideal position of the formation as the target point to maintain the stability of the overall formation. Otherwise, the expected position is reset. To reduce the distance from the original expected position to the tracking target, its expression is:

[0059] (3)

[0060] Re-set the expected position Get the ideal position of the formation and take the ideal position of the formation as the target point.

[0061] In this embodiment, the tolerance factor The value range is 1m~3m.

[0062] By tolerance factor , so that when the overall formation is unstable, the drone group can use the ideal position of the formation as the target point to maintain the stability of the overall formation. After the overall formation is stable, the exploration efficiency can be improved by real-time planning of the flight trajectory. Tolerance factor of autonomous decision-making formation It reduces the dependence on the precise position of the formation, enhances the robustness of the formation, improves the scalability of the formation, is suitable for resource-constrained scenarios, and has higher flexibility.

[0063] Step 3: Path Planning

[0064] Based on the 3D kinodynamic-A* search method, the optimal path of the drone group from the current position to the target point is obtained; the algorithm takes into account the dynamic characteristics of the drone to ensure the feasibility and safety of the path. To the target state The minimum cost J*(T) is expressed as follows:

[0065] (4)

[0066] in, and is the coefficient; represents the time of the trajectory segment; and They are the speed information and position information of the current state respectively; and are the speed 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 , and the actual cost of the optimal path from the starting state to the current state , according to the minimum cost and actual cost Get the total cost function It is expressed as follows:

[0068] (5)

[0069] In order to improve the efficiency of A* algorithm path planning, target points are introduced in the planning process to optimize local path planning. Compared with global path planning, this method can greatly 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 retaining the overall path morphological characteristics. The processing method is as follows:

[0071] (6)

[0072] in, is a sequence of waypoints; Represents a recursive call to the path; P max is the distance to the path point With waypoints Connect the farthest waypoints; is the threshold, used to control the degree of path simplification; Indicates from arrive The direction vector of i Represents the position vector of the i-th path point.

[0073] Step 4: Trajectory Optimization

[0074] Construct a Gaussian potential field in the ESDF environment 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 path generated in step 2, and convert the clipped path points into B-spline curve control points, so that the final trajectory is smoother and meets the flight dynamics constraints of the drone. 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, the optimized curve is used as the final trajectory; otherwise, the optimized curve is adjusted away from the obstacle through 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 5: Motion Control

[0076] 5-1. If Figure 3 As shown in the figure, the final trajectory is taken as the desired state of the UAV, and the model predictive control (MPC) controller is used to control the UAV to fly stably according to the desired state. The state vector of the UAV is defined as X = [ x , y , z , ϕ , θ , ψ ] T , the control input vector is U = [ F 1 , F 2 , F 3 , F 4 ] T At sampling time The state space equation is as follows:

[0077] (7)

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

[0079] In order to control the drone 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] in, and are the state weight matrix and the input weight matrix respectively, ; is the terminal weight matrix, ; The sampling time expected state.

[0082] The thrust generated by the rotation of the drone's propellers Subject to voltage constraints, the cost function for optimizing the shortest distance between the drone and the final path is expressed as:

[0083] (9)

[0084] 5-2. Distributed collaborative control

[0085] The distributed cooperative control method (Back Field Neighbor Planning, BFNP) aims to improve the cooperation efficiency and safety of UAV swarms under communication-restricted conditions. The information exchange protocol between UAVs is set to ensure that each UAV can receive the position, speed and expected trajectory information of the adjacent UAVs in a timely manner; each UAV communicates with the adjacent UAVs through the UAV swarm network information to obtain each other's position information, thereby reducing the communication burden of the UAV swarm network. The core of this method is to optimize the information exchange strategy between UAVs so that each UAV only exchanges necessary information with its direct neighbors instead of communicating extensively with the entire group. This direct communication mode between neighbors significantly reduces the steps and delays of information transmission, improves the response 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 own flight trajectory and speed when necessary to avoid collision with neighboring UAVs, and quickly adapts to environmental changes to achieve more accurate and safe flight configuration in complex environments.

[0086] like Figure 4 As shown in the figure, adjacent drones are divided into the front and rear drones according to the Euclidean distance between the drone group and the tracking target; if the trajectories planned by the front and rear drones intersect during the exploration process, the two drones may face a collision risk. In order to avoid the risk of collision and maximize the exploration efficiency of the drones, only the rear drone is adjusted. A safety buffer zone is created by inflating the size of the drone based on the wheelbase information. For the front drone, a collision detection area is established, and the collision detection area is divided into a speed adjustment area and a position adjustment area; the distance thresholds of the speed adjustment area are set separately Distance threshold to the position adjustment area When the MPC controller controls the flight of the drone group, if the Euclidean distance d(k) between the front and rear drones is greater than the distance threshold and less than the distance threshold , there is no need to re-plan the optimized trajectory of the two drones. It is only necessary to control the acceleration of the rear drone to ensure a safe distance between the two drones to avoid collision, thereby improving the flight efficiency of the drones. back The expression of (k) is:

[0087] (10)

[0088] in, It is a proportional constant, used to control speed adjustment; is the minimum safe distance between drones; P front (k) is the front position; P back (k) is the rear position.

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

[0090] (11)

[0091] in, is a proportional constant used to control the range of height adjustment; h back (k) is the height of the rear aircraft at sampling time k.

[0092] In order to ensure that the rear aircraft regenerates the trajectory at a safe height, add obstacle constraints and minimum height constraints to the drone height adjustment function to obtain the adjusted height. as follows:

[0093] (12)

[0094] Among them, h obs (k) is the obstacle height corresponding to the position of the UAV at sampling time k; The safe distance between the drone and obstacles; is the minimum flight altitude.

[0095] The rear machine is based on the adjusted height Re-plan the motion trajectory by re-performing path planning and trajectory optimization; the position adjustment area method can effectively reduce the collision risk of drone swarms in dense spaces, re-plan new trajectories in real time and safely, and improve flight efficiency and safety.

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

[0097] Step 6. Repeat steps 2 to 5 to complete the tracking of the target by the drone.

[0098] Step 7: Simulation

[0099] The simulation experiment consists of three parts:

[0100] Task 1: Collaborative free-flying simulation to verify the collaborative and safe flight capabilities of drone clusters.

[0101] Mission 2: Collaborative formation flying simulation to verify the autonomous and safe formation flying capability of the drone swarm.

[0102] Task 3: Collaborative formation dynamic target tracking simulation to verify the ability of drone swarms to autonomously collaborate and plan to track targets.

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

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

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

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

[0107] The present invention is used together with the LSC-Planner collaborative planning system and the EGO-Swarm collaborative planning system to perform collaborative free traversal in the above two scenarios. In scenario 1, the flight trajectory comparison results of 10 drones are as follows: Figure 5 As shown. Figure 5 It can be seen that compared with EGO-Swarm and LSC-Planner, the flight trajectory planned by the present invention is smoother and has a smaller curvature, thereby providing a more stable flight. The trajectory curvature of EGO-Swarm is larger, which may cause greater jitter during flight. The LSC-Planner trajectory is relatively long, which consumes more flight resources. In terms of 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] In the collaborative planning and crossing process, it is assumed that the collision radius of the drone is 0.1m. When the distance between two drones is less than 0.2m, a collision is considered to have occurred. The minimum distance between each drone and its nearest neighbor drone in the collaborative planning and crossing process of different methods is shown in Table 1.

[0109] Table 1 Minimum distance between a drone and its nearest neighbor (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 0.195m between drone 2 and drone 6, which poses a risk of collision. In LSC-Planner, the minimum distance between adjacent drones is 0.305m between drone 5 and drone 8, and no collision occurs. In the present invention, the minimum distance between adjacent drones is 0.361m between drone 1 and drone 7, and no collision occurs. The present invention can better control the safe distance between drones in a dense environment and ensure safe and cooperative flight.

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

[0113] 7-2. Use the present invention and the EGO-Swarm collaborative planning system to perform collaborative formation crossing simulation; deploy 6 drones with random 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 experimental results of the UAV swarm formation crossing the unknown environment are as follows: Figure 7 As shown in the figure, the total flight time of EGO-Swarm's fixed formation method based on Laplace graph theory is 39.53 seconds, and the average speed of the formation is 1.72m / s. Although a relatively stable formation can be maintained in the early stage, due to the increased complexity of the environment, the drones cannot take into account both the formation structure and safe trajectory planning. In addition, the large amount of calculations causes some drones to fail to plan the formation flight trajectory, and only a few drones successfully cross the environment. The total flight time of the autonomous decision-making formation strategy of the present invention is 38.41 seconds, and the average speed of the formation is 2.05m / s. Compared with EGO-Swarm, the formation of the present invention does not need to preset the position of the drone, but autonomously decides the nearest expected position of the formation based on the currently acquired drone position information, thereby forming a flexible formation. The formation exhibits a faster average speed and a tighter formation structure, and has higher efficiency in crossing unknown environments.

[0114] In addition, the autonomous decision-making formation strategy using the present invention is applied to dynamically transform the formation to traverse unknown environments, such as Figure 8As shown. In the simulation, the drone swarm formed a formation in the shape of letters such as "I", "A", "U", "S", "L", and "I". During the formation flight, the randomly distributed drones formed a formation according to the number and entered the unknown environment. Using the autonomous decision-making formation strategy, the formation was changed every 10 seconds, 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 changes and spatial position adjustments, which effectively verifies that the present invention improves the spatial utilization of the drone swarm based on the autonomous decision-making formation strategy and enhances its adaptability in unknown environments.

[0115] 7-3. Use the present invention and EGO-Swarm collaborative planning system to perform collaborative formation dynamic target tracking; use 6 drones in The initial position is randomly set in the unknown environment map. This experiment sets a randomly moving target in 3D space, and its speed range in the X direction is: , Y direction: , Z direction: In order to ensure the reasonable movement of the target in the environment map, the displacement range in the Z direction is limited to The simulation tracking results are as follows: Fig. 9 As shown in the figure, EGO-Swarm failed to complete the formation collaborative tracking task, mainly because it failed to balance the target tracking and formation maintenance process; at 45 seconds, the formation could not calculate the optimal trajectory to continue moving forward, resulting in tracking failure. At the same time, EGO-Swarm needs to maintain a fixed formation for target tracking at the beginning of the task, which limits the ability of the drone group to dynamically adjust the formation. The present invention successfully completed the formation collaborative tracking task; at In the first stage, the present invention controls the drones to track the dynamic target at the maximum speed. In this stage, the drone cluster needs to quickly shorten the distance to the target. The drone speed is high during this period, which shows that the present invention can quickly respond to changes in the target position. As the distance between the drone cluster and the target gradually decreases, the drones begin to adjust to a hexagonal formation, while continuing to collaborate to track the dynamic target to ensure the stable execution of the tracking task.

[0116] Step 8: Actual experimental verification

[0117] In order 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 110mm were used in the experiment, and YOLOv5 and RGB cameras were used to identify and track target information. Taking into account the possible loss of GPS signals in unknown environments, the drones use VINS-Fusion to achieve autonomous positioning; in addition, an ultra-wideband system is used to communicate location information between drones, thereby improving the reliability of the group in collaborative tasks. In a forest environment, four drones start from the starting point (0, 0, 0), and the target point is set to (20, 0, 1). The experiment requires that all drones must successfully reach the target point while ensuring that no collisions occur. The planned trajectory and total flight time of each drone in the drone swarm in a dense forest environment are shown below. Fig.10 As shown in the figure, it can be observed from the flight trajectory of the drone group 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 group completed the task efficiently and showed good obstacle avoidance ability in a dense forest environment. The experimental results verify that the present invention can achieve efficient group motion planning and has high safety.

[0118] The collaborative formation crossing experiment requires four drones to change formations at different time points to successfully cross a dense forest environment. The drone swarm starts the mission at 0 seconds. At 5 seconds, it adjusts to a triangle formation. At 10 seconds, it adjusts to a quadrilateral formation. In the subsequent flight, the drone swarm first expands the size of the formation, then begins to reduce the formation, and finally reaches the end point at 22 seconds. The flight data of the drone swarm in dynamic formation change is shown in Figure 1. Fig.11 As shown, during the entire crossing process, each UAV can dynamically adjust the formation according to the mission requirements to adapt to the crossing mission in the dense forest environment, fully demonstrating the flexibility of formation changes and collaborative control capabilities.

[0119] The collaborative formation tracking dynamic target experiment requires four UAVs to collaboratively track dynamic targets in a dense forest environment in a quadrilateral formation. The trajectory and position information of the four UAVs in the quadrilateral formation collaboratively tracking the dynamic target are as follows: Fig.12 As shown in the figure, the 3D quadrilateral side length tolerance of the formation is set to no more than 3m. The blue dots indicate the location information of the drone formation, and the red dots indicate the location of the tracked target. Throughout the tracking process, the formation always maintains a three-dimensional quadrilateral structure, and ensures that the longest side length is less than 3m. During dynamic target tracking, the speed of the drone is adjusted with the position of the tracked target. Especially in areas where the target moves faster, the speed of the drone will increase rapidly to ensure the overall coordination 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; 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 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.

3. 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.

4. 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.

5. The method for controlling a drone swarm in an unknown environment according to claim 4, 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.

6. 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.

7. 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.

8. 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.

9. 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.

10. The UAV swarm collaborative formation control system based on an unknown environment according to claim 9, 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