A method for cooperative search and safety control of UAV swarms based on MPC and CBF

Abstract

This invention belongs to the field of unmanned aerial vehicle (UAV) control technology, and relates to a UAV formation cooperative search and safety control method based on MPC and CBF. The method includes: determining the width W_ground and length H_ground of the ground for individual UAVs to identify; and determining the total search width W_search of UAV groups of different formation types based on W_ground and H_ground. i and the spacing between each drone; based on W_search i The search area is determined by H_ground; the search path is determined according to the search area of ​​each drone group type; the optimal control input of the leader in the drone group at each moment is determined based on the search path by the MPC controller; the optimal control input of the leader in the drone group at each moment is used as the input of the CBF controller, combined with safety constraints, to control the spatial position of the leader and followers in the drone group while avoiding collisions, thus achieving cooperative search. Its beneficial effect is that it avoids drone collisions while controlling the drone group to achieve cooperative search.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) control technology, and in particular to a method for UAV swarm cooperative search and safety control based on MPC and CBF. Background Technology

[0002] Swarm search is a key technology in UAV applications, widely used in emergency rescue, disaster monitoring, field search, environmental surveys, and military reconnaissance. With technological advancements, higher demands are being placed on search efficiency, accuracy, and collaborative capabilities. Swarm UAVs, with their advantages of flexible networking, multi-angle coverage, and efficient collaboration, can efficiently complete area search and target localization tasks in complex environments, demonstrating broad application prospects.

[0003] Formation control methods based on PID control maintain formation by adjusting the relative positions of drones. However, in dynamic environments and under multiple constraints, the control accuracy and response speed are insufficient. Some methods employ consensus control algorithms to achieve cooperative control of drone formations. Furthermore, distributed optimal control methods based on reinforcement learning can acquire optimal control inputs online without knowing the dynamics of the following drones, thus achieving stable control of the drone swarm.

[0004] CN208781073U discloses a drone safety control system, including a safety management module, a cloud server, a ground control terminal, and at least one drone body connected in sequence. The safety management module sends safety control commands to the cloud server, which in turn sends them to the corresponding drone via the ground control terminal, enabling the drone owner or safety management personnel to manage and control the drone. CN105302043A provides a drone safety control system and method that automatically controls, detects, judges, and executes safety measures through a computer, such as initiating an automatic landing procedure and controlling the aircraft to avoid obstacles when abnormal situations are detected, thereby reducing the occurrence of drone operation accidents.

[0005] Traditional PID control methods struggle to meet the required control accuracy and response speed under multiple constraints and dynamic environments, easily leading to formation instability and reduced search efficiency. Consistency control algorithms offer good flexibility, robustness, and adaptability, but their distributed structure suffers from poor communication capabilities and complex algorithm design. While reinforcement learning-based distributed optimal control methods have achieved stable control of UAV swarms to some extent, they require large amounts of training data, and their real-time performance and accuracy in complex environments remain to be verified.

[0006] Existing safety control methods primarily rely on manual intervention or pre-set safety control commands, making them ill-suited to dynamically changing environments and unforeseen circumstances. For example, the safety control system in CN208781073U requires the drone owner or safety manager to detect dangerous actions or unauthorized flights before taking action, resulting in a certain degree of lag. While the safety control method in CN105302043A can automatically execute some safety measures, its monitoring and judgment scope is relatively limited, and it may not be able to respond promptly to some complex faults or dangerous situations. Summary of the Invention

[0007] Technical problems to be solved

[0008] In view of the above-mentioned shortcomings and deficiencies of the prior art, the present invention provides a method for UAV formation cooperative search and safety control based on MPC and CBF, which solves the technical problem of how to perform safety control while realizing UAV formation cooperative search.

[0009] Technical solution

[0010] To achieve the above objectives, the main technical solutions adopted by the present invention include:

[0011] In a first aspect, the present invention provides a method for collaborative search and safety control of UAV formations based on MPC and CBF, comprising:

[0012] Determine the width of the ground that a single drone can identify. and length ;

[0013] according to and Determine the total search width of drone groups of different formation types And the spacing between each drone, where i∈R, is used to distinguish different formation types;

[0014] according to and Define the search areas separately;

[0015] The search path is determined based on the search area of ​​each drone formation type;

[0016] The MPC controller determines the optimal control input for the navigator in the drone group at each moment based on the search path;

[0017] By using the optimal control input of the navigator in the drone swarm at each moment as the input of the CBF controller, and combining it with safety constraints, the spatial positions of the navigator and followers in the drone swarm can be controlled while avoiding collisions, thus achieving collaborative search.

[0018] Optionally, determine the width of the ground that a single drone can identify. and length Specifically, it includes:

[0019] Based on the known diagonal field of view (DFOV) of the UAV camera, with a camera image width of W pixels and a height of H pixels, determine the UAV's diagonal pixel DIAG, horizontal field of view (HFOV), and vertical field of view (VFOV). ; ; ;

[0020] Based on HFOV, VFOV, and the drone's current altitude z, determine the width of the ground that the drone can identify. and length ,in, ; .

[0021] Optionally, according to and Determine the total search width of drone groups of different formation types And the spacing between each drone, specifically including:

[0022] according to and Determine the total search width of the linear formation of drone groups The distance between each drone is ;

[0023] according to and Determine the total search width of the triangular formation of UAVs. The distance between the follower and the leader is The distance between followers is ;

[0024] according to and Determine the total search width of the V-formation drone group The distance between the follower and the leader is The distance between followers is .

[0025] Optionally, according to and The search areas are defined separately, including:

[0026] Expand the search area into a rectangle, and define the coordinates of the four vertices of the initial search area rectangle as follows: , , , Where AB and CD are the widths of the rectangle, and AD and BC are the lengths of the rectangle;

[0027] The vertex coordinates of the search area for the linear formation are determined as follows: , , , ;

[0028] The coordinates of the vertices of the triangular formation search area are determined as follows: , , , ,in, Let AB be the angle between AB and the positive y-axis. ;

[0029] The vertex coordinates of the V-shaped formation search area are determined as follows: , , , ;

[0030] The total width of the search area for all three formations is .

[0031] Optionally, the search path is determined separately for each drone formation type's search area, specifically including:

[0032] Rotate the search area counterclockwise around point A by an angle. After rotation, AB is parallel to the positive y-axis, and the resulting rectangle... The vertex coordinates are , , , ;

[0033] From the bottom right corner of the rectangular search area A serpentine search path begins to form: Calculation Rounding up, we get the number of target points on one side, 'count'. Therefore, the total number of target points in the serpentine search path is 2count. Coordinates are The coordinates of the first target point are The coordinates of the second target point are The coordinates of the third target point are Calculate 2count target points and stop the calculation to obtain a serpentine search path;

[0034] Rotate the serpentine search path clockwise around point A by an angle. The final search path is obtained, where the coordinates of a target point before rotation are... Then the coordinates of the target point after rotation are

[0035] .

[0036] Optionally, the MPC controller determines the optimal control input for the navigator in the UAV group at each moment based on the search path, specifically including:

[0037] Define the position of the drone at a certain moment as The target location is Predict the control inputs and states for the next N steps. The control inputs for the next N steps are defined as follows: If the sampling period is T, then the state is:

[0038] ;

[0039] ;

[0040] ...

[0041] ;

[0042] The objective function of the MPC controller is:

[0043] ;

[0044] Where q is the coefficient of the deviation term, Here, r is the coefficient of the deviation term in the final step, and r is the coefficient of the control input.

[0045] Substituting the state into the objective function, we get:

[0046] ;

[0047] The solution is obtained based on the method of solving the optimization problem. ;

[0048] The MPC controller takes the control input from the first step. Using the control input from the first step as the current control input, the same optimization calculation will be performed in the next step, resulting in the final state. Converging at the target position This allows us to obtain the optimal control input for the navigator at each moment, thereby controlling the navigator's spatial position.

[0049] Optionally, the method further includes:

[0050] Assuming the value returned by the navigator's position sensor is The target location of a certain follower aircraft in a certain formation is The formation's coordinated search for control over the follower aims to pinpoint the follower's location. Control to .

[0051] Optionally, the optimal control input of the navigator in the drone swarm at each moment is used as the input of the CBF controller. Combined with safety constraints, the spatial positions of the navigator and followers in the drone swarm are controlled to avoid collisions, thereby achieving cooperative search. Specifically, this includes:

[0052] Define the current position of a certain drone as The nominal controller speed is obtained based on the optimal control input of the MPC controller. The location of the obstacle is The safe distance that the drone maintains from the obstacle is ;

[0053] Define a safety set C to ensure that the distance between the drone and the obstacle is always greater than or equal to r:

[0054] ;

[0055] The control barrier function is:

[0056] ;

[0057] To ensure that the system state always remains within the safe set, the following CBF condition must be satisfied:

[0058] ;

[0059] in, yes ,Pick , h is the control barrier function. To control the derivative of the barrier function with respect to time;

[0060] Solving the above equation, we get:

[0061] ;

[0062] The following optimization problem is obtained:

[0063] min

[0064] st ;

[0065] Solving the optimization problem yields the UAV control input. .

[0066] In a second aspect, the present invention provides a computer-readable storage medium having a computer program stored thereon, wherein the program, when executed, implements the method for collaborative search and safety control of unmanned aerial vehicle formations based on MPC and CBF as described in any of the first aspects above.

[0067] Thirdly, the present invention provides a storage device, including a storage medium and a processor, wherein the storage medium stores a computer program, and when the program is executed by the processor, it implements the method for collaborative search and safety control of UAV formation based on MPC and CBF as described in any of the first aspects above.

[0068] Beneficial effects

[0069] The beneficial effects of this invention are as follows: This invention provides a UAV formation cooperative search and safety control method based on MPC and CBF. According to different formation types, it designs a highly adaptable search path planning method, which can effectively avoid omissions and repeated searches in the search area, improving search efficiency. Employing Model Predictive Control (MPC) as the control strategy, it achieves precise control of UAV positions by predicting future states and optimizing control inputs. This not only enables rapid response to the dynamic changes of the leader but also allows followers to maintain a stable formation, enabling UAVs to more accurately track target paths and adjust their positions in a timely manner during the search process, reducing deviations and oscillations. Introducing Control Obstacle Function (CBF) to design a distributed safety controller, by defining a safety set and control obstacle function and combining it with optimization algorithms to solve for the control inputs, allows UAVs to maintain flight within a safe area while achieving the search target. In this way, each UAV treats other UAVs as obstacles, thereby achieving safe control of the entire formation. To address the characteristics of coordinated search in formation, the control strategy between the follower and the navigator was optimized. By adjusting parameters of the MPC controller, such as prediction step size and control input weights, the follower can quickly respond to the dynamic changes of the navigator, thereby maintaining a stable formation. Furthermore, by optimizing search path planning, improving control accuracy and response speed, and introducing safety control mechanisms, the efficiency and safety of UAV coordinated search in formation are enhanced, enabling it to better adapt to the needs of search missions in complex environments. Attached Figure Description

[0070] Figure 1 A flowchart illustrating a method for collaborative search and safety control of UAV formations based on MPC and CBF, provided for an embodiment of the present invention;

[0071] Figure 2 This is a schematic diagram of the spacing between different formation types of UAVs provided in the embodiments of the present invention;

[0072] Figure 3 The search path planning diagram provided in the embodiments of the present invention;

[0073] Figure 4 This is a schematic diagram of the navigator search path provided in an embodiment of the present invention;

[0074] Figure 5 This is a schematic diagram illustrating the effect of the MPC controller provided in an embodiment of the present invention;

[0075] Figure 6-12 This is a schematic diagram illustrating the effect of the CBF safety control method provided in this embodiment of the invention;

[0076] Figure 13-15 The diagram shows the collaborative search effect of drones 1, 2, and 3 in a line formation, as well as the corresponding two-dimensional position diagram of the drones in the line formation, provided for embodiments of the present invention.

[0077] Figure 16-18 The diagram shows the cooperative search effect of the triangular formation drones 1, 2, and 3 provided in the embodiments of the present invention, as well as the corresponding two-dimensional position diagram of the triangular formation drones.

[0078] Figure 19-21 The diagram shows the collaborative search effect of V-formation drones 1, 2, and 3, as well as the corresponding two-dimensional position diagram of the V-formation drones, provided for embodiments of the present invention. Detailed Implementation

[0079] To better explain and facilitate understanding of the present invention, it will be described in detail below with reference to the accompanying drawings and specific embodiments. While exemplary embodiments of the invention are shown in the drawings, it should be understood that the invention can be implemented in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided to enable a clearer and more thorough understanding of the invention and to fully convey the scope of the invention to those skilled in the art.

[0080] Firstly, referring to Figure 1 This embodiment provides a method for collaborative search and safety control of UAV swarms based on MPC and CBF, including:

[0081] S1 determines the width and length of the ground that a single drone can identify.

[0082] Define the width as , length is .

[0083] Optionally, determine the width of the ground that a single drone can identify. and length Specifically, it includes:

[0084] Based on the known diagonal field of view (DFOV) of the UAV camera, with a camera image width of W pixels and a height of H pixels, determine the UAV's diagonal pixel DIAG, horizontal field of view (HFOV), and vertical field of view (VFOV). ; ; ;

[0085] Based on HFOV, VFOV, and the drone's current altitude z, determine the width of the ground that the drone can identify. and length ,in, ; .

[0086] Based on the obtained width and length of the ground area identified by the drone, the area of ​​the ground area identified by the drone is: .

[0087] S2 determines the total search width of different formation types of drone groups and the spacing between each drone based on the width and length.

[0088] Among them, the total search width of drone groups of different formation types is defined as follows: , i∈R, are used to distinguish different formation types.

[0089] To maximize search efficiency, this invention determines the navigator's search width based on the formation type, avoiding redundant searches. The search area is set as a rectangle (non-rectangular areas can be expanded into rectangles), and the drone camera, search direction, and edges of the search area are always parallel. The navigator's search width is determined based on the formation type.

[0090] Optionally, according to and Determine the total search width of drone groups of different formation types And the spacing between each drone, specifically including:

[0091] To maximize search width and improve search efficiency, the total search width of the linear formation of drones was [not specified]. The distance between each drone is ;

[0092] To maximize the search width and improve search efficiency, the total search width of the triangular formation of UAVs is... The distance between the follower and the leader is The distance between followers is The followers form an isosceles triangle with the navigator on both sides;

[0093] To maximize the search width and improve search efficiency, the total search width of the V-formation drone group was [not specified]. The distance between the follower and the leader is The distance between followers is The followers form an isosceles triangle with the navigator on both sides, and the V-shaped formation is the rotated triangular formation.

[0094] S3 determines the search area based on the total width and length of the search for different formation types of drone groups.

[0095] Before determining the search path, the search area needs to be defined first. This invention expands all search areas into rectangles while minimizing the increase in search area. This invention uses the navigator's takeoff point as the origin and due east as the boundary. The axis is north. Establish a Cartesian coordinate system using axes.

[0096] Optionally, according to and The search areas are defined separately, including:

[0097] Expand the search area into a rectangle, and define the coordinates of the four vertices of the initial search area rectangle as follows: , , , Where AB and CD are the widths of the rectangle, and AD and BC are the lengths of the rectangle;

[0098] A linear formation will not miss any areas during normal search. The coordinates of the vertices of the search area for the linear formation are determined as follows: , , , ;

[0099] During normal searching, a triangular formation may miss some areas. Therefore, this invention extends the search area both upwards and downwards from the initial given search area. The coordinates of the vertices of the triangular formation search area are determined as follows: , , , ,in, Let AB be the angle between AB and the positive y-axis. ;

[0100] During a normal search, a portion of the area may be missed in a V-shaped formation. Therefore, this invention extends the search area both upwards and downwards from the initially given search area. The vertex coordinates of the V-shaped formation search area are determined as follows: , , , ;

[0101] The total width of the search area for all three formations is ,like Figure 2 As shown

[0102] S4 determines the search path based on the search area of ​​each drone formation type.

[0103] Optionally, the search path is determined separately for each drone formation type's search area, specifically including:

[0104] like Figure 3 As shown, the search area is rotated counterclockwise around point A by an angle. After rotation, AB is parallel to the positive y-axis, and the resulting rectangle... The vertex coordinates are , , , ;

[0105] From the bottom right corner of the rectangular search area A serpentine search path begins to form: Calculation Rounding up, we get the number of target points on one side, 'count'. Therefore, the total number of target points in the serpentine search path is 2count. Coordinates are The coordinates of the first target point are To ensure a more stable search process, frequent switching of target points should be avoided; therefore, only the points reaching both ends of the rectangle should be searched. The coordinates of the second target point are... Since the search width of all three formation navigators is... Then the third target point is above the second target point. The coordinates of the third target point are: Calculate 2count target points and stop the calculation to obtain a serpentine search path;

[0106] Rotate the serpentine search path clockwise around point A by an angle. The final search path is obtained, where the coordinates of a target point before rotation are... Then the coordinates of the target point after rotation are

[0107] .

[0108] At this point, the navigator's serpentine search path calculation is complete. Different formation types correspond to different search paths. The navigator searches along the search path, while the followers maintain their distance from the navigator through the MPC controller.

[0109] S5, based on the MPC controller, determines the optimal control input for the navigator in the drone group at each moment according to the search path.

[0110] The dynamic equations of the unmanned aerial vehicle are:

[0111] ;

[0112] Since the dynamics are the same in all dimensions, the control process of one dimension will be discussed below.

[0113] Discretizing the model along the x-axis yields:

[0114] ;

[0115] Right now:

[0116] ;

[0117] Optionally, the MPC controller determines the optimal control input for the navigator in the UAV group at each moment based on the search path, specifically including:

[0118] Define the position of the drone at a certain moment as The target location is Predict the control inputs and states for the next N steps. The control inputs for the next N steps are defined as follows: If the sampling period is T, then the state is:

[0119] ;

[0120] ;

[0121] ...

[0122] ;

[0123] The objective function of the MPC controller is designed below. On the one hand, it aims to make the state x approximate the target position as closely as possible. On the other hand, it is desirable to reduce oscillations when the deviation is small, while also reducing control input to save energy. In summary, the objective function of the MPC controller is:

[0124] ;

[0125] Where q is the coefficient of the deviation term, Here, r is the coefficient of the deviation term in the final step, and r is the coefficient of the control input.

[0126] Substituting the state into the objective function, we get:

[0127] ;

[0128] The solution is obtained based on the method of solving the optimization problem. ;

[0129] In the objective function , Given T, q 'r' are parameters to be adjusted, or they can be considered known. Therefore, the objective function only controls the input. If the problem is unknown, it can be solved using optimization methods such as gradient descent or Newton's method. .

[0130] The MPC controller takes the control input from the first step. Using the control input from the current moment as the basis for the next moment's optimization calculation, the same control input from the first moment is used again. This is called rolling optimization control. Final state. Converging at the target position This allows us to obtain the optimal control input for the navigator at each moment, thereby controlling the navigator's spatial position, and by adjusting the parameter q. r can achieve fast convergence and reduce oscillations.

[0131] In collaborative search by UAV formation, the navigator searches along the search path. The three-dimensional position control of the navigator as it reaches the target point utilizes the previously designed MPC controller. Next, we analyze how the followers maintain their distance from the navigator to achieve collaborative search in formation.

[0132] Optionally, the method further includes:

[0133] Assuming the value returned by the navigator's position sensor is The target location of a certain follower aircraft in a certain formation is The formation's coordinated search for control over the follower aims to pinpoint the follower's location. Control to Looking only at the x-dimensional dimension (the other two dimensions are similar), we will soon... Control convergence to Compared to typical formation control, which often involves... Control to Both control methods ultimately produce the same deviation and control input.

[0134] In the process of achieving formation coordination, the three-dimensional position control of the followers also adopts the previously designed MPC controller. However, it should be noted that because the leader's position changes dynamically, the response speed of the followers in achieving formation coordination is much faster than that of the leader. Specifically, in the process of adjusting the parameters, the following can be controlled by reducing the position of the MPC controller in the followers. , Come on, increase control input and speed up the follower's response.

[0135] In this invention, there is a close connection between the MPC controller and the CBF-based safety controller. The MPC controller is responsible for calculating the optimal control input for the UAV at each moment based on the preset path planning and target point, in order to achieve precise control of the UAV's position. These control inputs include the UAV's desired speed (i.e., nominal controller speed), which is transmitted as input to the CBF-based safety controller. The CBF controller, in turn, further considers safety constraints to ensure that the UAV avoids collisions with other UAVs or obstacles while achieving the control objective.

[0136] S6 uses the optimal control input of the navigator in the drone group at each moment as the input of the CBF controller. Combined with safety constraints, it controls the spatial position of the navigator and followers in the drone group while avoiding collisions, thus achieving collaborative search.

[0137] This invention designs a distributed safety controller based on the Control Barrier Function (CBF). Since the altitude of the UAV needs to be adjusted when performing specific tasks, the safety controller is designed in a two-dimensional plane, which can effectively ensure safety.

[0138] A control barrier function (CBF) is a method used to ensure the safety of a control system. Its core idea is to limit the control input of the system by defining a barrier function to prevent the system from entering an unsafe region.

[0139] Optionally, the optimal control input of the navigator in the drone swarm at each moment is used as the input of the CBF controller. Combined with safety constraints, the spatial positions of the navigator and followers in the drone swarm are controlled to avoid collisions, thereby achieving cooperative search. Specifically, this includes:

[0140] Define the current position of a certain drone as The nominal controller speed is obtained based on the optimal control input of the MPC controller. The location of the obstacle is The safe distance between the drone and the obstacle is r;

[0141] First, define a safety set that ensures the distance between the drone and the obstacle is always greater than or equal to r. Then the safety set is:

[0142] ;

[0143] Since radicals are inconvenient to calculate, the safe set is defined in a different way:

[0144] ;

[0145] The control barrier function is:

[0146] ;

[0147] To ensure that the system state always remains within the safe set, the following CBF condition must be satisfied:

[0148] ;

[0149] in, yes ,Pick , h is the control barrier function. To control the derivative of the barrier function with respect to time;

[0150] Solving the above equation, we get:

[0151] ;

[0152] UAV dynamics model:

[0153] ;

[0154] In addition to ensuring safety, the control objective must still be achieved. This requires getting as close as possible to the nominal controller speed. Then, the problem of achieving the control objective while ensuring the safety of the control system becomes the following optimization problem:

[0155] min ;

[0156] st ;

[0157] When solving the above optimization problem in each cycle, the current position of the UAV nominal controller speed obstacle location The safety distance r is a known value. These are adjustable parameters, only For the unknowns to be solved, after solving the above optimization problem in each cycle, we obtain... As the control input for the drone, it achieves the control objective as much as possible while ensuring safety.

[0158] In practical use of distributed security controllers, each drone needs to pass through the aforementioned security controller. During this process, other drones besides itself act as obstacles for that drone, thus achieving security control in drone swarm collaborative search.

[0159] The following specific embodiment further illustrates the UAV swarm cooperative search and safety control method based on MPC and CBF of the present invention:

[0160] 1. The effectiveness of drone search path planning

[0161] The vertex coordinates of the search area are set to (0,0), (100,-100), (200,0), and (100,100), and the width of the ground is identified by the drone. and length If the values ​​are 8 and 6 respectively, then the navigator search path is as follows: Figure 4 As shown. According to Figure 4 The Navigator search path covers the entire search area and avoids omissions and duplicate searches, resulting in high search efficiency.

[0162] 2. The effect of the MPC controller

[0163] The sampling time is set to 0.1s, and the prediction steps are 30. The initial state is 0, the target state is 20, and the effect of the MPC controller is as follows: Figure 5 As shown. According to Figure 5 The state converged from 0 to 20, achieving the control objective. Convergence was completed in the 75th step, which is 7.5 seconds, showing a relatively fast convergence speed.

[0164] 3. The effectiveness of the CBF-based safety control method

[0165] The initial positions of the three drones are set to the three vertices of an equilateral triangle: (0, 0), (4, 0), and (2, 3.46). The target positions of the three drones are set to the midpoints of the opposite sides of the three vertices of the equilateral triangle: (3, 1.73), (1, 1.73), and (2, 0). The safe distance between the drones is... Adjustable parameters The effectiveness of the CBF-based security control method is as follows: Figures 6-12 As shown. Figures 6-12 As shown, the distance between the three drones was always greater than the safe distance, and the control objective was achieved. Furthermore, the control input comparison curves of the drones in the X and Y directions based on the CBF method and the nominal control method are shown below. Figures 6-12 As shown in the comparison results, when approaching other drones, the CBF method adjusts the nominal control input to achieve collision avoidance control; while when moving away from other drones, the CBF control input is basically the same as the nominal control input, ensuring control performance.

[0166] 4. The effect of drone swarm collaborative search

[0167] During the experimental phase, this invention utilized an industrial-grade drone with a maximum horizontal flight speed of 23 m / s and a maximum ascent speed of 6 m / s. It was equipped with four camera systems: a wide-angle camera, a zoom camera, a thermal imaging camera, and a laser rangefinder. The wide-angle camera offered a broad field of view, suitable for large-area searches.

[0168] The drone and remote controller communicate using a 2.4GHz radio frequency signal. This frequency band is globally universal and has many idle frequencies, which helps reduce interference. The remote controller and ground station communicate via Ethernet (switch), specifically using the TCP / IP protocol, to control the drone and acquire video streams.

[0169] The experiment used a UAV formation cooperative search method based on MPC and CBF. The current coordinates of the three UAVs and their projections on a two-dimensional plane were displayed on a visualization interface. These coordinates are three-dimensional coordinates relative to the navigator's takeoff point in a northeast-sky coordinate system. The search area was [missing information]. Then the angle between AB and the positive y-axis direction is... Based on the included angle and the width of the ground identified by the drone and length The relative coordinates of the follower to the navigator were calculated under different formation types, and the collaborative search effect of different formation types of drones was shown. Figures 13-21 As shown.

[0170] The effect of linear formation collaborative search is as follows Figures 13-15 As shown, Figure 13 The image on the left, from top to bottom, shows the collaborative search effect of drones 1, 2, and 3 in a straight line formation. Figure 13 The image on the right shows the position of a two-dimensional drone formation in a straight line; subsequent images follow the same pattern. The effect of a triangular formation in collaborative search is shown below. Figures 16-18 As shown, the effect of V-shaped formation cooperative search is as follows: Figures 19-21 As shown.

[0171] In a second aspect, embodiments of the present invention provide a computer-readable storage medium having a computer program stored thereon, wherein the program, when executed, implements the method for collaborative search and safety control of UAV formations based on MPC and CBF as described in any of the first aspects above.

[0172] Thirdly, embodiments of the present invention provide a storage device, including a storage medium and a processor, wherein the storage medium stores a computer program, and when the program is executed by the processor, it implements the method for collaborative search and safety control of UAV formation based on MPC and CBF as described in any of the first aspects above.

[0173] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0174] Obviously, those skilled in the art can make various modifications and variations to this invention without departing from its spirit and scope. Therefore, if these modifications and variations fall within the scope of the claims of this invention and their equivalents, then this invention should also include these modifications and variations.

[0175] Although embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those skilled in the art can make modifications, alterations, substitutions and variations to the above embodiments within the scope of the present invention.

Claims

1. A method for cooperative search and safety control of UAV formation based on MPC and CBF, characterized in that, Comprise: Determining the width and length of the ground by a single drone and length ; According to and determining the total width of the search of the group of unmanned vehicles of different formation types and the distance between each unmanned vehicle, wherein i∈R, to distinguish different formation types; According to and determine a search area, respectively; determining a search path for each formation type of UAV group respectively according to the search area; determining the optimal control input of the leader in the UAV group at each time according to the search path based on the MPC controller; taking the optimal control input of the leader in the UAV group at each time as the input of the CBF controller, combining the safety constraints, controlling the spatial positions of the leader and the follower in the UAV group while avoiding collision, and realizing cooperative search, specifically comprising: Definition of the current position of the UAV is , the nominal controller speed is obtained according to the optimal control input of the MPC controller , the obstacle position is , and the safety distance between the UAV and the obstacle is r; defining a safety set C to ensure that the distance between the UAV and the obstacle is always greater than or equal to r: ； the control barrier function is: ； To constrain the system state to always be within the safety set, the following CBF condition needs to be met: ； wherein is , take , h is a control barrier function, is a derivative of the control barrier function with respect to time; solving the above formula gives: ； the following optimization problem is obtained: min ; s.t. ; Solving an optimization problem to obtain a control input for the drone .

2. The method of claim 1, wherein, Determining a width of a ground surface by a single drone and length , comprising in particular: According to a known diagonal field of view angle DFOV of the UAV camera, the camera image is W pixels wide and H pixels high, the diagonal pixel DIAG, the horizontal field of view angle HFOV and the vertical field of view angle VFOV of the UAV are determined, wherein, ; ; ; According to the HFOV, the VFOV and the current height z of the UAV, the width and the length of the ground recognized by the UAV are determined and the length wherein ; .

3. The method of claim 2, wherein, According to and determining the total width of the search of the group of unmanned vehicles of different formation types and the distance between each unmanned vehicle, specifically comprising: According to and determine a single file formation of unmanned aircraft groups to search the total width , each unmanned aircraft spacing is ; According to and determining the total width of the triangular formation of UAVs , the follower and leader spacing is , the follower spacing is ; According to and determining the total width of the V-shaped formation of UAVs , the follower and leader spacing is , the follower spacing is .

4. The method of claim 3, wherein, According to and determining the search area respectively, specifically comprising: The search area is extended to a rectangle, and the four vertex coordinates of the initial search area rectangle are defined as , , , , wherein AB and CD are the width of the rectangle, and AD and BC are the length of the rectangle. The vertex coordinates of the one-dimensional formation searching region are determined as , , , ; The vertex coordinates of the triangular formation search area are determined as , , , wherein, is the included angle between AB and the positive direction of the y-axis, ; The vertex coordinates of the V-shaped formation search area are determined as 、 、 、 ; The total width of the three platoon search areas is .

5. The method of claim 4, wherein, determining a search path for each formation type of UAV group respectively according to the search area, specifically comprising: Rotating the search area counterclockwise around point A by an angle , after rotation AB is parallel to the positive direction of the y-axis, the coordinates of the vertices of the rotated rectangle are , , , ; from the lower right corner of the rectangular search area Start forming a snake search path: calculate Rounding up, get the number of unilateral target points count, then the total number of snake search path target points is 2count, The coordinates are The first target point coordinates are , the second target point coordinates are , the third target point coordinates are , calculate 2count target points stop calculation, get a snake search path; rotating the serpentine search path clockwise around point A by an angle to obtain a final search path, wherein if a target point has coordinates before rotation, the target point has coordinates after rotation 。 6. The method of claim 5, wherein, determining the optimal control input of the leader in the UAV group at each time according to the search path based on the MPC controller, specifically comprising: Define the position of the UAV at a certain time as , the target position as , the control input and state of the future N steps are predicted, the control input of the future N steps is defined as , and the sampling period is T, then the state is: ； ； …… ； the objective function of the MPC controller is: ； where q is the coefficient of the bias term, is the coefficient of the last step bias term, and r is the coefficient of the control input; the state is brought into the objective function to obtain: ； Based on the method of solving optimization problems ; MPC controller takes the first step control input As the control input at the current time, the next time, the same optimization calculation is performed, and the first step control input is still taken, and the final state Converge to the target position is The optimal control input of the navigator at each time is obtained to control the spatial position of the navigator.

7. The method of claim 6, wherein, The method further comprises: Assume that the value fed back by the position sensor of the leader is , the target position of a follower in a certain formation is , then the control target of the formation cooperative search for the follower is to control the position of the follower to .

8. A computer-readable storage medium having stored thereon a computer program, characterized in that The computer program is executed by the processor to realize the MPC and CBF based cooperative search and safety control method of the UAV formation according to any one of claims 1 to 7.

9. A storage device comprising a storage medium and a processor, the storage medium storing a computer program, characterized in that, The processor executes the computer program to realize the MPC and CBF based cooperative search and safety control method of the UAV formation according to any one of claims 1 to 7.

Images