Cooperative control method of swarm patrol fire-fighting UAVs based on arrival time consistency

Abstract

The present invention proposes a collaborative control method for a cluster of patrol fire-fighting drones based on arrival time consistency. An adaptive adjustment mechanism and a time compensation strategy are introduced in the collaborative process. The flight trajectory is corrected in real time through node coordination to ensure that when the state of the fire point or the environmental information changes, it can quickly converge to the same disposal time, thereby improving the system's synchronous disposal accuracy and overall collaborative efficiency in a complex fire-fighting mission environment. This method provides a new control strategy for a cluster of patrol fire-fighting drones, enabling the cluster of patrol fire-fighting drones to form and maintain stable formation flight, and realize the synchronous disposal of multiple patrol fire-fighting drones for the same fire point.

Description

Technical Field

[0001] The present invention relates to the field of aircraft control technology, and in particular to a collaborative control method for cluster patrol fire-fighting UAVs based on arrival time consistency. Background Art

[0002] Patrol firefighting drones are a new type of unmanned platform capable of hovering over a target area for extended periods and, upon detecting a fire, precisely disposing of the ignition point. They play a positive role in scenarios such as forest firefighting. By carrying firefighting payloads, patrol firefighting drones can quickly address fires deep in the jungle, where firefighters struggle to reach them in time. These drones have long flight times and high maneuverability, allowing them to dynamically adjust their flight plans during mission execution. However, the firefighting payload and execution capabilities of a single patrol firefighting drone are limited, making it difficult for them to complete complex missions and multiple firefighting objectives alone. Therefore, it is necessary to consider combining individual patrol firefighting drones into a swarm collaborative mission model to improve the overall effectiveness of the system.

[0003] Swarm patrol requires multiple drones to form and maintain stable formation flight, ensuring efficient firefighting mission execution in complex environments. The core of formation control lies in the dynamic adjustment of the relative positions and attitudes of each drone, and each drone must always maintain the coordination and integrity of the formation. Existing formation control strategies are mainly divided into two types: centralized and distributed. The former relies on a single control node for global planning and is susceptible to node failure. The latter achieves autonomous adjustment of trajectory and attitude through local information exchange between multiple drones, enhancing the adaptability and robustness of the drone formation. For the full-process collaborative control of swarm patrol firefighting drones, further consideration is needed to match and integrate formation control with collaborative task algorithms, thereby efficiently connecting the front and back control stages of the drones and improving the drone swarm's firefighting mission execution capabilities.

[0004] When multiple drones collaborate on firefighting missions, they must adhere to arrival time consistency constraints to achieve synchronized response to the same fire point and avoid weakening the firefighting mission due to time differences. For example, when responding to a localized fire, the payload of a single firefighting drone cannot meet the firefighting mission requirements, and multiple firefighting drones are needed to synchronize the fire scene to prevent it from rekindling. To meet this requirement, existing technologies often use time window control or time synchronization path planning algorithms. The former coordinates the timing of drones arriving at the fire point by setting an allowable time error range, but it is difficult to ensure precise synchronization in highly dynamic environments. The latter shortens the arrival time error by dynamically adjusting the flight trajectory of each drone, but its high computational complexity makes it difficult to apply to large-scale drone swarms. Summary of the Invention

[0005] To address the challenges of existing technologies, this paper proposes a collaborative control method for swarms of patrol firefighting drones based on arrival time consistency. This method introduces an adaptive adjustment mechanism and a time compensation strategy into the collaborative process. Through node coordination, flight trajectories are corrected in real time, ensuring rapid convergence to the same response time when the fire point status or environmental information changes. This improves the system's synchronous response accuracy and overall collaborative effectiveness in complex firefighting mission environments. This method provides a new control strategy for swarms of patrol firefighting drones, enabling them to form and maintain stable formation flight, enabling multiple patrol firefighting drones to synchronously respond to the same fire point.

[0006] The technical solution of the present invention is:

[0007] A collaborative control method for a swarm of patrol firefighting drones based on arrival time consistency, characterized by comprising the following steps:

[0008] Step 1: Establish a six-degree-of-freedom dynamic model of the patrol firefighting UAV;

[0009] A control model for a patrol firefighting drone is designed based on a cascade proportional-integral controller with a given desired attitude angle. The control model consists of a longitudinal pitch control channel and a lateral roll control channel. In the longitudinal pitch control channel, a pitch rate feedback loop is introduced into the pitch angle control loop; in the lateral roll control channel, a roll rate feedback loop is introduced into the roll angle control loop.

[0010] Using the adjacency matrix A = [ a ij ] Characterizes the communication relationship between each patrol fire-fighting drone in the patrol fire-fighting drone cluster; the adjacency matrix is The fully connected matrix of order n represents the number of patrol firefighting drones contained in the patrol firefighting drone cluster, and the elements in the adjacency matrix are Represents the connection weight between the i-th patrol fire-fighting drone and the j-th patrol fire-fighting drone;

[0011] Step 2: Divide the mission phase of the patrol firefighting drone cluster into the cluster patrol phase and the coordinated disposal phase;

[0012] During the cluster patrol phase, a distributed formation control algorithm based on a virtual navigator is used to design a control strategy for patrol fire-fighting drones. Each patrol fire-fighting drone calculates the desired control command in real time based on the designated formation, virtual navigator information, and the flight status of itself and adjacent patrol fire-fighting drones. The desired control command is input into the patrol fire-fighting drone's ontological control model to calculate the control quantity of the patrol fire-fighting drone, and the patrol fire-fighting drone cluster is controlled to form a designated air formation, maintain the formation, and fly along the desired route.

[0013] Step 3: Perform conditional judgment based on the relative distance between the virtual navigator and the fire point being handled. When the relative distance reaches or is less than the set threshold, switch the control strategy of the patrol fire-fighting drone from the control strategy of the cluster patrol phase to the control strategy of the coordinated disposal phase;

[0014] Step 4: In the collaborative disposal stage, a distributed collaborative control algorithm with time consistency constraints is used to design a control strategy for the patrol fire-fighting drone cluster. Each patrol fire-fighting drone dynamically adjusts its flight state according to its own flight state and the position information of adjacent patrol fire-fighting drones, and solves the expected control instructions in real time. The expected control instructions are input into the ontological control model of the patrol fire-fighting drone to solve the control quantity of the patrol fire-fighting drone, so as to control the disposal time of each patrol fire-fighting drone in the patrol fire-fighting drone cluster to the same fire point to be synchronized.

[0015] Furthermore, in step 1, the controller structure of the longitudinal pitch control channel is:

[0016]

[0017] The controller structure of the lateral roll control channel is:

[0018]

[0019] In the formula is the control command value of the equivalent elevator deflection angle of the i-th patrol firefighting UAV, is the control command value of the equivalent aileron deflection angle of the i-th patrol firefighting UAV; is the trim value of the equivalent elevator angle of the i-th patrol firefighting UAV; is the trim value of the equivalent aileron deflection angle of the i-th patrol firefighting UAV; is the expected pitch rate of the i-th patrol firefighting UAV, is the expected roll angular rate of the i-th patrol firefighting UAV; is the roll angular rate of the i-th patrol firefighting UAV, is the pitch angle rate of the i-th patrol firefighting UAV; is the expected pitch angle of the i-th patrol firefighting drone, is the expected roll angle of the i-th patrol firefighting UAV; is the pitch angle of the i-th patrol firefighting drone, is the roll angle of the i-th patrol firefighting UAV; is the proportional gain value related to the pitch angle rate, is the proportional gain value related to the pitch angle, is the proportional gain value related to the roll angular rate, is the proportional gain value related to the roll angle; is the integral gain value related to the pitch angle rate, is the integral gain value related to the pitch angle, is the integral gain value related to the roll angular rate, is the integral gain value related to the roll angle.

[0020] Furthermore, in step 2, the desired control instruction includes the desired pitch angle , expected thrust and the desired roll angle , respectively, are obtained by the following formulas:

[0021] i i , d = α z [ ( z i v − z l v − D z i ) + c z ( w i v − w l v ) ] + α z ∑ j = 1 , j ≠ i n A [ ( z i v − z j v − D z i + D z j ) + c z ( w i v − w j v ) ]

[0022] T i , d = − α x [ ( x i v − x l v − D x i ) + c x ( u i v − u l v ) ] − α x ∑ j = 1 , j ≠ i n A [ ( x i v − x j v − D x i + D x j ) + c x ( u i v − u j v ) ]

[0023] ϕ i , d = − α y [ ( y i v − y l v − D y i ) + c y ( v i v − v l v ) ] − α y ∑ j = 1 , j ≠ i n A [ ( y i v − y j v − D y i + D y j ) + c y ( v i v − v j v ) ]

[0024] in is the first parameter of longitudinal proportional differential control, is the second parameter of the longitudinal proportional differential control, is the first parameter of forward proportional differential control, is the second parameter of the forward proportional derivative control, is the first parameter of lateral proportional differential control, The second parameter of lateral proportional differential control; represents the coordinates of the i-th patrol firefighting drone in the x-axis direction, y-axis direction and z-axis direction of the virtual navigator coordinate system respectively; represents the speed of the i-th patrol firefighting UAV in the x-axis, y-axis and z-axis directions of the virtual navigator coordinate system respectively; In turn, they represent the coordinates of the virtual navigator in the x-axis, y-axis, and z-axis directions of the virtual navigator's coordinate system. represents the speed of the virtual navigator in the x-axis direction, y-axis direction and z-axis direction of the virtual navigator's coordinate system respectively; represents the position coordinate deviation of the i-th patrol firefighting UAV relative to the virtual navigator in the x-axis direction, y-axis direction and z-axis direction of the virtual navigator's coordinate system respectively; represents the coordinates of the jth patrol firefighting UAV in the x-axis, y-axis, and z-axis directions of the virtual navigator coordinate system respectively; represents the speed of the j-th patrol firefighting UAV in the x-axis, y-axis and z-axis directions of the virtual navigator coordinate system respectively; represents the position coordinate deviation of the j-th patrol firefighting UAV relative to the virtual navigator in the x-axis direction, y-axis direction and z-axis direction of the virtual navigator coordinate system.

[0025] Furthermore, in step 2, the formation is determined by the position coordinate deviation of the patrolling fire-fighting drones in the patrolling fire-fighting drone cluster relative to the virtual navigator.

[0026] Furthermore, in step 2, the virtual navigator information is the spatial position coordinates of the virtual navigator in the global coordinate system. P l = [ x l , y l , z l ] T , global velocity vector V l = [ u l , v l , w l ] T and global flight direction ,in In turn, they represent the coordinates of the virtual navigator in the x-axis, y-axis, and z-axis directions of the global coordinate system; represents the speed of the virtual navigator in the x-axis, y-axis and z-axis directions of the global coordinate system respectively;

[0027] For the i-th patrol firefighting drone, the virtual space position in the virtual navigator coordinate system is expressed as P i v = [ x i v , y i v , z i v ] T , the virtual velocity vector is expressed as V i v = [ u i v , v i v , w i v ] T , the virtual heading angle is expressed as :

[0028] [ x i v y i v z i v ] = R ( ψ l ) ⋅ [ x i − x l y i − y l z i − z l ]

[0029] [ u i v v i v w i v ] = R ( ψ l ) ⋅ [ u i − u l v i − v l w i − w l ]

[0030]

[0031] in represents the coordinates of the i-th patrol firefighting drone in the x-axis, y-axis, and z-axis directions of the global coordinate system, represents the speed of the i-th patrol firefighting drone in the x-axis, y-axis and z-axis directions of the global coordinate system, represents the global heading angle of the i-th patrol firefighting UAV in the global coordinate system; is the transformation matrix from the global coordinate system to the virtual navigator coordinate system.

[0032] Furthermore, in step 4, the desired control instruction includes the desired pitch angle , expected thrust and the desired roll angle , respectively, are obtained by the following formulas:

[0033]

[0034]

[0035]

[0036] in Indicates the quality of patrol firefighting drones, In order The acceleration components of the i-th patrol firefighting drone in the x-axis, y-axis and z-axis directions of its own body axis system are: is the normal acceleration vector of the i-th patrol firefighting UAV in its own body axis system.

[0037] Further, The normal acceleration vector of the i-th patrol firefighting drone in the global coordinate system is It is obtained by transforming the coordinates from the global coordinate system to the body axis system of the i-th patrol firefighting drone.

[0038] Furthermore, the normal acceleration vector of the i-th patrol firefighting drone in the global coordinate system is According to the formula

[0039]

[0040] Get, among them is the sum of the acceleration values ​​of the distributed cooperative control algorithm of the i-th patrol firefighting drone, is the normal acceleration unit vector of the i-th patrol firefighting drone:

[0041]

[0042] N i a = − ( I 3 − V i a ∥ V i a ∥ ⋅ ( V i a ) T ∥ V i a ∥ ) ⋅ R i a ∥ R i a ∥ ∥ [ ( I 3 − V i a ∥ V i a ∥ ⋅ ( V i a ) T ∥ V i a ∥ ) ⋅ R i a ∥ R i a ∥ ] ∥

[0043] in is the acceleration value of the proportional guidance part of the i-th patrol firefighting UAV, is the acceleration value of the consistency cooperative control part of the i-th patrol firefighting UAV, is the velocity vector of the i-th patrol firefighting UAV in the target coordinate system, is the target vector between the i-th patrol fire-fighting UAV and the fire point being handled: , is the spatial position of the i-th patrol firefighting UAV in the target coordinate system, is the spatial position of the fire point to be treated in the target coordinate system, is a 3rd order unit vector.

[0044] Furthermore, the acceleration value of the proportional guidance part of the i-th patrol firefighting drone is The calculation formula is:

[0045]

[0046] in represents the proportional guidance coefficient, is the speed of the i-th patrol firefighting drone in the target coordinate system, ; is the angle between the velocity vector of the i-th patrol firefighting drone and the target vector:

[0047]

[0048] is the distance between the i-th patrol firefighting drone and the fire point being dealt with, ;

[0049] The acceleration value of the consistency cooperative control part of the i-th patrol firefighting drone The calculation formula is:

[0050]

[0051] in To coordinate the control of the first parameter for consistency, To coordinate the control of the second parameter for consistency, To coordinate the control of the third parameter for consistency, is the distance between the jth patrol firefighting drone and the fire point being dealt with, is the rate of change of the distance between the jth patrol firefighting drone and the fire point being handled, is the speed of the j-th patrol firefighting UAV in the target coordinate system, is the rate of change of the distance between the i-th patrol fire-fighting drone and the fire point being handled.

[0052] Beneficial effects:

[0053] This paper proposes a collaborative control method for swarm patrol firefighting drones based on arrival time consistency. Based on the differences in requirements before and after the execution of patrol firefighting drone missions, different control stages are divided. For the patrol flight stage and collaborative disposal stage of patrol firefighting drones, a distributed formation control algorithm based on a virtual navigator and a distributed collaborative control algorithm with time consistency constraints are designed, respectively. By converting and connecting the two control strategies and generating manipulated variables using a highly robust ontological control model, the swarm patrol firefighting drones achieve efficient control effects when performing various complex tasks. The specific effects are as follows:

[0054] (1) The longitudinal pitch control and lateral roll control of the patrol firefighting UAV are precisely adjusted through a cascade proportional-integral controller. By introducing an attitude angular rate feedback mechanism into the attitude control loop, the response speed and control accuracy of the control system are significantly improved, the control robustness of the UAV is enhanced, and the influence of external interference and nonlinear factors of the body is effectively suppressed, thereby ensuring the control stability of the patrol firefighting UAV cluster during formation flight and coordinated disposal.

[0055] (2) A distributed formation control algorithm based on a virtual navigator was used to design an early-stage formation control strategy for patrol firefighting drones. By constructing a virtual navigator, a unified reference point was provided for the patrol firefighting drone cluster, enabling each patrol firefighting drone to instantly track the preset flight path and maintain a stable formation according to the instructions of the virtual navigator. At the same time, the algorithm achieved local information interaction and autonomous adjustment between adjacent patrol firefighting drones, avoiding the single point of failure problem caused by centralized control and significantly improving the adaptability of the patrol firefighting drone cluster.

[0056] (3) A distributed cooperative control algorithm with time consistency constraints was used to design a late-stage cooperative control strategy for patrol firefighting drones. By introducing consistent cooperative control based on the classic proportional guidance algorithm, each patrol firefighting drone dynamically adjusts its normal acceleration and flight trajectory based on its own state and the state of adjacent patrol firefighting drones. This ensures that multiple patrol firefighting drones can simultaneously handle fire points, avoiding the impact of long time errors on the handling effect, and greatly improving the success rate of cluster missions.

[0057] Additional aspects and advantages of the present invention will be set forth in part in the description which follows and, in part, will be obvious from the description which follows, or may be learned by practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] The above and / or additional aspects and advantages of the present invention will become apparent and readily understood from the following description of the embodiments with reference to the accompanying drawings, in which:

[0059] Figure 1 This is a schematic diagram of the collaborative control algorithm architecture of the swarm patrol fire-fighting drones proposed in this invention;

[0060] Figure 2 Implement a flow chart for the cluster formation and collaborative control method of multiple patrol firefighting drones;

[0061] Figure 3 This is the attitude longitudinal step command response diagram of the cascade proportional-integral control system of the patrol fire-fighting UAV;

[0062] Figure 4This is the attitude lateral step command response diagram of the cascade proportional-integral control system of the patrol fire-fighting UAV;

[0063] Figure 5 Provide early formation flight trajectory tracking diagram for patrol firefighting drone cluster control;

[0064] Figure 6 This is a diagram of the position components of three patrol firefighting drones during formation flight;

[0065] Figure 7 This is a diagram of the velocity components in various directions during the formation flight of three patrol firefighting drones;

[0066] Figure 8 Trajectory tracking diagram for the coordinated handling of patrol firefighting drone swarms using the classic proportional guidance law;

[0067] Figure 9 A tracking diagram of the collaborative handling trajectory of a patrol firefighting UAV cluster using the collaborative control algorithm of the present invention in the later stage of control;

[0068] Figure 10 This is a diagram of the position components of three patrol firefighting drones during the coordinated disposal process;

[0069] Figure 11 This is a diagram of the velocity components in various directions during the coordinated disposal process of three patrol firefighting drones;

[0070] Figure 12 This is a diagram showing the relative distances between the three patrol firefighting drones during the coordinated disposal process;

[0071] Figure 13 This is a diagram of the global heading angles of each aircraft during the coordinated disposal process of three patrol firefighting drones. DETAILED DESCRIPTION

[0072] The following describes in detail embodiments of the present invention. The embodiments are exemplary and intended to explain the present invention, but are not to be construed as limiting the present invention.

[0073] This embodiment addresses the problem of inconsistent mission types in the early and late stages of current swarm patrol firefighting drones, which leads to incomplete control strategy design and a lack of effective arrival time consistency constraint control. This embodiment proposes a collaborative control method for swarm patrol firefighting drones based on arrival time consistency. This method divides the entire control process into a swarm patrol phase and a collaborative disposal phase, based on the specific mission types faced by the patrol firefighting drones in the early and late stages. The swarm patrol phase utilizes a distributed formation control algorithm based on a virtual navigator, enabling the patrol firefighting drones to form a specific aerial formation. Each patrol firefighting drone maintains formation flight and tracks a preset desired route. The collaborative disposal phase employs a distributed collaborative control algorithm with time consistency constraints, enabling multiple patrol firefighting drones to coordinate and simultaneously address the same fire point. Furthermore, this method constructs a control model for the patrol firefighting drone based on a cascade proportional-integral controller with a given desired attitude angle, thus providing the patrol firefighting drone with a complete and flexible trajectory control strategy and attitude control method.

[0074] This embodiment takes multiple small flying wing UAVs as an example to design a complete control system and introduce the algorithm proposed in the present invention in detail.

[0075] The specific steps include:

[0076] Step 1: Establish a six-degree-of-freedom dynamic model of the patrol fire-fighting UAV. According to the time-scale separation principle, the attitude control system of the patrol fire-fighting UAV is divided into attitude angle control and attitude angular rate control. The main control model of the patrol fire-fighting UAV is designed based on a cascade proportional-integral controller with a given desired attitude angle. The communication relationship between each patrol fire-fighting UAV in the patrol fire-fighting UAV cluster is represented by the adjacency matrix, thereby constructing a patrol fire-fighting UAV cluster system with distributed information exchange capabilities.

[0077] The patrol firefighting drone used in this embodiment is a simple flying wing configuration, featuring a low-aspect-ratio triangular main wing, a flat, box-shaped fuselage, external fairing end plates, and a tail propeller. Control of the patrol firefighting drone is accomplished via a set of elevons, two control surfaces of which are mounted on the left and right trailing edges of the main wing. Elevation control is achieved by deflecting the elevons in the same direction, creating an equivalent elevator angle. Roll control is achieved by deflecting the elevons differentially, creating an equivalent aileron angle. This achieves hybrid control via the elevons. The actual elevon angle is calculated by linearly superposing the equivalent elevator angle and the equivalent aileron angle, enabling attitude adjustment and control of the drone.

[0078] Based on the conventional aerodynamic data interpolation calculation method in this field, the aerodynamic calculation model of the small flying wing layout UAV is established. According to the principle of vertical and horizontal separation and linearization of the UAV body and the control aerodynamic force, the aerodynamic force and torque calculation formula of the i-th patrol fire-fighting UAV in the patrol fire-fighting UAV cluster is obtained as follows:

[0079]

[0080] Where, 、 and They correspond to the resistance, side force and lift of the i-th patrol firefighting drone respectively. 、 and They correspond to the rolling moment, pitching moment and yaw moment of the i-th patrol firefighting UAV respectively, and They correspond to the angle of attack and sideslip angle of the i-th patrol firefighting drone, and They represent the equivalent elevator deflection angle and the equivalent aileron deflection angle of the i-th patrol firefighting UAV respectively, 、 and They correspond to the roll angular rate, pitch angular rate, and yaw angular rate of the i-th patrol firefighting drone respectively; represents dynamic pressure, Indicates the wing reference area of ​​the patrol fire-fighting drone, Indicates the length of the exhibition, represents the mean aerodynamic chord length; The aerodynamic derivative of the angle of attack with respect to the drag coefficient, and They correspond to the control derivatives of the equivalent elevator deflection angle relative to the drag coefficient and the control derivatives of the equivalent aileron deflection angle relative to the drag coefficient respectively; represents the aerodynamic derivative of the sideslip angle with respect to the side force coefficient, The control derivative of the equivalent aileron deflection angle relative to the side force coefficient; The aerodynamic derivative of the angle of attack with respect to the lift coefficient, and They correspond to the control derivatives of the equivalent elevator deflection angle relative to the lift coefficient and the control derivatives of the equivalent aileron deflection angle relative to the lift coefficient respectively; 、 and They correspond to the aerodynamic derivative of the sideslip angle relative to the rolling moment coefficient, the aerodynamic derivative of the roll angular rate relative to the rolling moment coefficient, and the aerodynamic derivative of the yaw angular rate relative to the rolling moment coefficient, respectively. The control derivative of the equivalent aileron deflection angle relative to the rolling moment coefficient; and They correspond to the aerodynamic derivatives of the angle of attack relative to the pitching moment coefficient and the aerodynamic derivatives of the pitching angular rate relative to the pitching moment coefficient, respectively. The control derivative of the elevator deflection angle with respect to the pitching moment coefficient; 、 and They correspond to the aerodynamic derivative of the sideslip angle relative to the yaw moment coefficient, the aerodynamic derivative of the yaw angular rate relative to the yaw moment coefficient, and the aerodynamic derivative of the roll angular rate relative to the yaw moment coefficient, respectively. The aerodynamic force and moment calculation formula is well known in the art.

[0081] Based on the above aerodynamic force and torque model, a six-degree-of-freedom dynamic model of the i-th patrol fire-fighting UAV is established, including the force equation model of the i-th patrol fire-fighting UAV:

[0082]

[0083] And the torque equation model of the i-th patrol fire-fighting drone is:

[0084]

[0085] In the formula, the subscript i indicates that the modeling object is the i-th patrol fire-fighting drone in the patrol fire-fighting drone cluster. 、 and represents the external forces on the i-th patrol firefighting drone in the x-axis direction, y-axis direction and z-axis direction respectively; 、 and Denote the external moments of the i-th patrol firefighting drone around the x-axis, y-axis, and z-axis respectively; 、 、 represents the velocity components of the i-th patrol firefighting drone in the x-axis direction, y-axis direction, and z-axis direction respectively; 、 、 represents the roll rate, pitch rate, and yaw rate of the i-th patrol firefighting UAV respectively; Indicates the quality of patrol firefighting drones; 、 、 represents the principal moments of inertia of the i-th patrol firefighting UAV in the x-axis direction, y-axis direction, and z-axis direction respectively; The inertia product of the i-th patrol firefighting drone is represented by The six-degree-of-freedom dynamic model is well known in the art.

[0086] The main control model of the patrol firefighting drone is specifically divided into a longitudinal pitch control channel and a lateral roll control channel. In this embodiment, both are designed based on a cascade proportional-integral controller with a given desired attitude angle. Pitch rate and roll rate feedback loops are introduced on the basis of the pitch angle and roll angle control loops, respectively, to increase the longitudinal and lateral heading control damping of the system and prevent adjustment oscillation. The controller structure of the longitudinal pitch control channel is as follows:

[0087]

[0088] The controller structure of the lateral roll control channel is:

[0089]

[0090] Where, 、 represents the control command value of the equivalent elevator angle and the control command value of the equivalent aileron angle of the i-th patrol firefighting UAV in sequence; 、 represents the trim value of the equivalent elevator angle and the trim value of the equivalent aileron angle of the i-th patrol firefighting UAV in turn; 、 Denote the expected pitch rate and roll rate of the i-th patrol firefighting UAV respectively; 、 Denote the expected pitch angle and expected roll angle of the i-th patrol firefighting UAV respectively; 、 represents the pitch angle and roll angle of the i-th patrol firefighting drone respectively; is the proportional gain value related to the pitch angle rate, is the proportional gain value related to the pitch angle, is the proportional gain value related to the roll angular rate, is the proportional gain value related to the roll angle; is the integral gain value related to the pitch angle rate, is the integral gain value related to the pitch angle, is the integral gain value related to the roll angular rate, is the integral gain value related to the roll angle.

[0091] In this invention, the communication relationship network between each patrol fire-fighting drone is represented in the form of an adjacency matrix, which is used to describe the information interaction of the patrol fire-fighting drone cluster. The adjacency matrix is ​​usually recorded as A = [ a ij ] , is a The matrix of order n represents the number of patrol firefighting drones contained in the patrol firefighting drone cluster. Each element in the matrix represents the connection weight between the i-th patrol firefighting drone and the j-th patrol firefighting drone. Furthermore, to ensure that the patrol firefighting drone cluster can successfully complete coordinated flight, the communication network defined by the adjacency matrix must have connectivity, ensuring that each patrol firefighting drone can obtain the status and location information of other patrol firefighting drones.

[0092] In this embodiment, three small flying wing patrol fire-fighting drones of the same type are used to form a patrol fire-fighting drone cluster. To characterize the communication interaction between the three patrol firefighting drones. The adjacency matrix is ​​a A square matrix of order, each element in the matrix represents the communication connection weight between the i-th patrol fire-fighting drone and the j-th patrol fire-fighting drone. The adjacency matrix in this embodiment is specifically defined as:

[0093] A = [ 0 1 1 1 0 1 1 1 0 ]

[0094] The adjacency matrix It is a fully connected array, which means that there is information exchange between the three patrol fire-fighting drones. Each patrol fire-fighting drone can transmit its own information to the other two patrol fire-fighting drones and receive information from the other two patrol fire-fighting drones. The communication connection weight always remains consistent.

[0095] Step 2: Based on the long-term hovering characteristics required for patrol firefighting drones to execute their initial missions, the initial control phase of patrol firefighting drones is defined as the swarm patrol phase. During this phase, a control strategy for patrol firefighting drones is designed based on a distributed formation control algorithm with a virtual navigator. Each patrol firefighting drone calculates the desired control commands in real time based on the designated formation, virtual navigator information, and the flight status of itself and adjacent patrol firefighting drones. Through the control models of each patrol firefighting drone, the swarm is formed into a designated aerial formation, maintaining the formation and flying along the desired route.

[0096] A "virtual navigator" is defined based on the desired instructions for the patrol firefighting drone swarm. This "virtual navigator" is not a real drone, but a virtual reference point that needs to be set in advance based on the ideal formation flight conditions. This virtual reference point is used to define the desired control parameters for the patrol firefighting drone swarm's formation flight, including: formation formation, global heading angle, desired trajectory, global cruising speed, etc. Formation control is performed using these desired control parameters as control instructions, ensuring that each patrol firefighting drone in the formation can coordinate and maintain a stable formation.

[0097] When using a "virtual navigator" for formation control, the first step is to establish a "virtual navigator coordinate system." This coordinate system provides a unified reference framework for the desired control parameters of each patrol firefighting drone. The origin of the virtual navigator coordinate system is set to the virtual navigator's current position, and the vector directions of each coordinate axis are defined based on the virtual navigator's heading angle. This allows the baseline control instructions for the patrol firefighting drone formation to be clearly set based on this coordinate system, and the desired control instructions for each patrol firefighting drone can be uniformly solved.

[0098] The spatial position coordinates of the virtual navigator in the global coordinate system are defined as P l = [ x l , y l , z l ] T , the global velocity vector is defined as V l = [ u l , v l , w l ] T , the global flight direction defines the global heading angle ,in In turn, they represent the coordinates of the virtual navigator in the x-axis, y-axis, and z-axis directions of the global coordinate system; They represent the speed of the virtual navigator in the x-axis, y-axis and z-axis directions of the global coordinate system respectively; these seven state variables serve as the benchmark control parameters for formation control and are pre-set before formation control is performed.

[0099] In this embodiment, the spatial position coordinates of the virtual navigator at the current moment in the global coordinate system are defined as P l = [ 1 0 2 t , 1 0 2 t , 1 0 0 ] T (m), the global velocity vector is defined as V l = [ 1 0 2 , 1 0 2 , 0 ] T (m / s), the global heading angle is defined as The expected global heading angle of the patrol firefighting drone cluster is is 45°, the expected global cruising speed value is 20m / s, the expected global cruising altitude The desired flight path is along the bisector of the angle between the positive x-axis and the positive y-axis of the global coordinate system. Two formations are defined: a "triangle" formation and a "line" formation.

[0100] Based on the virtual navigator coordinate system, the corresponding formation position matrix is ​​defined for each patrol firefighting drone according to the expected formation during the cluster flight process. For the i-th patrol firefighting drone in the formation, its formation position matrix is ​​recorded as F i = [ D x i , D y i , D z i ] T , which defines the spatial position deviation component of the i-th patrol firefighting UAV relative to the virtual navigator, where The coordinates of the i-th patrol firefighting drone relative to the virtual navigator in the x-axis, y-axis, and z-axis directions of the virtual navigator's coordinate system are represented in turn. The formation of the patrol firefighting drones in the patrol firefighting drone cluster is determined by the coordinates of the patrol firefighting drones relative to the virtual navigator.

[0101] In this embodiment, we first define the "triangle" formation of a cluster of three patrol firefighting drones, with the symbol subscript A as the symbol, then the formation position matrix of the first patrol firefighting drone is F A , 1 = [ 0 , 0 , 0 ] T , the formation position matrix of the second patrol firefighting drone is F A , 2 = [ − 80 , − 60 , 0 ] T , the formation position matrix of the third patrol firefighting drone is F A , 3 = [ − 80 , 60 , 0 ] T ; Secondly, define the "line" formation of the three patrol firefighting drones, with the symbol subscript B as the symbol, then the formation position matrix of the first patrol firefighting drone is F B , 1 = [ 0 , 0 , 0 ] T , the formation position matrix of the second patrol firefighting drone is F B , 2 = [ 0 , − 100 , 0 ] T ,The position matrix of the third patrol firefighting drone formation is F B , 3 = [ 0 , 100 , 0 ] T .

[0102] When solving the expected command parameters corresponding to each patrol firefighting drone in the cluster formation, it is necessary to calculate the spatial position of each patrol firefighting drone in the cluster formation in the global coordinate system at the current moment. P i = [ x i , y i , z i ] T , global velocity vector V i = [ u i , v i , w i ] T and global heading angle The state parameters are uniformly converted to the virtual navigator coordinate system for representation, and the state parameters of the virtual navigator itself also need to be converted to the virtual navigator coordinate system for representation. represents the coordinates of the i-th patrol firefighting drone in the x-axis, y-axis, and z-axis directions of the global coordinate system, represents the speed of the i-th patrol firefighting drone in the x-axis, y-axis and z-axis directions of the global coordinate system, Represents the global heading angle of the i-th patrol firefighting UAV in the global coordinate system.

[0103] Since in three-dimensional space, the global heading angle of the virtual navigator It mainly affects the rotation in the x- and y-axis directions, while the state parameters in the z-axis direction remain unchanged. According to the coordinate transformation relationship, the transformation matrix of the patrol firefighting UAV from the global coordinate system to the virtual navigator coordinate system can be expressed as:

[0104] R ( ψ l ) = [ c o s ( ψ l ) s i n ( ψ l ) 0 − s i n ( ψ l ) c o s ( ψ l ) 0 0 0 1 ]

[0105] According to the above coordinate transformation relationship, the state parameters of each patrol fire-fighting UAV are transformed into an axis system. For the i-th patrol fire-fighting UAV in the formation, its virtual space position after transformation in the virtual navigator coordinate system is expressed as: P i v = [ x i v , y i v , z i v ] T , the virtual velocity vector is expressed as V i v = [ u i v , v i v , w i v ] T , the virtual heading angle is expressed as , represents the coordinates of the i-th patrol firefighting drone in the x-axis, y-axis, and z-axis directions of the virtual navigator coordinate system, It represents the speed of the i-th patrol firefighting UAV in the x-axis direction, y-axis direction and z-axis direction of the virtual navigator coordinate system respectively. The specific conversion formula is expressed as:

[0106] [ x i v y i v z i v ] = R ( ψ l ) ⋅ [ x i − x l y i − y l z i − z l ]

[0107] [ u i v v i v w i v ] = R ( ψ l ) ⋅ [ u i − u l v i − v l w i − w l ]

[0108]

[0109] The state parameters of the virtual navigator itself also need to be converted to the virtual navigator coordinate system for unified calculation. According to the coordinate conversion relationship, its own heading angle is always expressed as 0 in the virtual navigator coordinate system, and the virtual space position of the virtual navigator itself in the virtual navigator coordinate system is expressed as P l v = [ x l v , y l v , z l v ] T , the virtual velocity vector is expressed as V l v = [ u l v , v l v , w l v ] T ,in In turn, they represent the coordinates of the virtual navigator in the x-axis, y-axis, and z-axis directions of the virtual navigator's coordinate system. represents the speed of the virtual navigator in the x-axis, y-axis and z-axis directions of the virtual navigator's coordinate system respectively; the specific conversion formula is expressed as:

[0110] [ x l v y l v z l v ] = R ( ψ l ) ⋅ [ x l y l z l ]

[0111] [ u l v v l v w l v ] = R ( ψ l ) ⋅ [ u l v l w l ]

[0112] In this embodiment, the spatial positions of the three patrol firefighting drones in the current cluster formation in the global coordinate system are pre-set. 、 and , the global velocity vector 、 and , global heading angle 、 and The state parameters are uniformly converted to the virtual navigator coordinate system for representation, and the state parameters of the virtual navigator itself need to be 、 and Convert to the virtual navigator coordinate system for representation. Since in three-dimensional space, the heading angle of the virtual navigator It mainly affects the rotation in the x- and y-axis directions, while the state parameters in the z-axis direction remain unchanged. According to the coordinate transformation relationship, the transformation matrix of the patrol firefighting UAV from the global coordinate system to the virtual navigator coordinate system can be expressed as:

[0113] R ( π 4 ) = [ c o s ( π 4 ) s i n ( π 4 ) 0 − s i n ( π 4 ) c o s ( π 4 ) 0 0 0 1 ]

[0114] According to the coordinate transformation relationship, the state parameters of each patrol fire-fighting UAV are transformed into the axis system, and the virtual space positions of the three patrol fire-fighting UAVs in the formation after transformation are: 、 and , the virtual velocity vector is 、 and , the virtual heading angle represents 、 and , the coordinate transformation formula is:

[0115] [ x i v y i v z i v ] = R ( π 4 ) ⋅ [ x i − x l y i − y l z i − z l ]

[0116] [ u i v v i v w i v ] = R ( π 4 ) ⋅ [ u i − u l v i − v l w i − w l ]

[0117]

[0118] The state parameters of the virtual navigator itself are also converted to the virtual navigator coordinate system for representation. The converted state parameters are and , the conversion formula is:

[0119] [ x l v y l v z l v ] = R ( π 4 ) ⋅ [ x l y l z l ]

[0120] [ u l v v l v w l v ] = R ( π 4 ) ⋅ [ u l v l w l ]

[0121] The distributed formation control algorithm employed in this step is primarily implemented through specific control designs for each patrol firefighting drone in the longitudinal (along the z-axis of the drone's coordinate system), forward (along the x-axis), and lateral (along the y-axis) directions within the virtual leader's coordinate system. This control algorithm incorporates feedback from the drone's position and velocity errors and applies proportional-differential control laws in each direction. It also considers interactions between adjacent patrol firefighting drones to generate desired control commands, which are then applied to the drone control model constructed in Step 1, ensuring that each drone maintains a consistent formation flight state.

[0122] The desired control command includes the desired pitch angle , expected thrust and the desired roll angle , corresponding to longitudinal, forward and lateral control respectively.

[0123] The main purpose of longitudinal control is to adjust the flight altitude of the patrol firefighting drone to ensure that it can maintain an ideal relative position deviation with the altitude of the virtual leader and other patrol firefighting drones in the formation, thereby achieving precise control of the longitudinal formation position. The control algorithm is based on the proportional-differential controller design and introduces the collaborative control terms of the relative position and speed between adjacent patrol firefighting drones. For the i-th patrol firefighting drone in the cluster formation, the output parameter generated by its longitudinal control algorithm is the desired pitch angle of the patrol firefighting drone , its formula is specifically expressed as:

[0124] i i , d = α z [ ( z i v − z l v − D z i ) + c z ( w i v − w l v ) ] + α z ∑ j = 1 , j ≠ i n A [ ( z i v − z j v − D z i + D z j ) + c z ( w i v − w j v ) ]

[0125] in is the first parameter of longitudinal proportional differential control, is the second parameter of the longitudinal proportional differential control, n is the number of patrol firefighting drones in the cluster, and are the coordinates and speed of the jth patrol firefighting drone in the z-axis direction of the virtual navigator coordinate system, is the position coordinate deviation of the j-th patrol firefighting UAV relative to the virtual navigator in the z-axis direction of the virtual navigator's coordinate system.

[0126] The forward control is used to adjust the relative position of the patrol firefighting drone in the x-axis direction, so that the patrol firefighting drone can follow the virtual navigator along the desired trajectory, keep the forward relative position in the formation stable, and maintain the forward relative position deviation of the formation. For the i-th patrol firefighting drone in the cluster formation, the expected thrust is The formula is:

[0127] T i , d = − α x [ ( x i v − x l v − D x i ) + c x ( u i v − u l v ) ] − α x ∑ j = 1 , j ≠ i n A [ ( x i v − x j v − D x i + D x j ) + c x ( u i v − u j v ) ]

[0128] in is the first parameter of forward proportional differential control, is the second parameter of the forward proportional differential control, n is the number of patrol firefighting drones in the cluster, and are the coordinates and speed of the jth patrol firefighting drone in the x-axis direction of the virtual navigator coordinate system, is the position coordinate deviation of the j-th patrol firefighting UAV relative to the virtual navigator in the x-axis direction of the virtual navigator's coordinate system.

[0129] The lateral control is used to adjust the relative position of the patrol firefighting UAV in the y-axis direction, so that the lateral relative position of the patrol firefighting UAV in the formation remains stable. For the i-th patrol firefighting UAV in the cluster formation, its expected roll angle is The formula is:

[0130] ϕ i , d = − α y [ ( y i v − y l v − D y i ) + c y ( v i v − v l v ) ] − α y ∑ j = 1 , j ≠ i n A [ ( y i v − y j v − D y i + D y j ) + c y ( v i v − v j v ) ]

[0131] in is the first parameter of lateral proportional differential control, is the second parameter of lateral proportional differential control, n is the number of patrol firefighting drones in the cluster, and are the coordinates and speed of the jth patrol firefighting drone in the y-axis direction of the virtual navigator coordinate system, is the position coordinate deviation of the j-th patrol firefighting UAV relative to the virtual navigator in the y-axis direction of the virtual navigator's coordinate system.

[0132] For the small flying wing layout UAV in the embodiment, at each moment in the formation control process, the above three formulas are used to obtain the desired pitch angle , expected thrust and the desired roll angle , where the desired pitch angle and the desired roll angle Input into the established patrol fire-fighting UAV control model to obtain the specific control quantity of the patrol fire-fighting UAV: ​​the equivalent elevator deflection angle control instruction value of the i-th patrol fire-fighting UAV and the equivalent aileron deflection angle control command value , and according to the expected thrust Get the corresponding throttle opening , thereby controlling the three patrol fire-fighting drones to form and maintain a stable formation flight to perform the preliminary cluster patrol mission.

[0133] Through the distributed formation control in step 2, the patrol firefighting drone swarm has formed a stable formation configuration at the moment of transition between the early cluster patrol and the later coordinated disposal control strategies. The global spatial position of each patrol firefighting drone is clear. Step 3 dynamically switches the control strategy based on the distance between the patrol firefighting drone formation position and the fire location, transitioning the patrol firefighting drone swarm into the later coordinated disposal phase.

[0134] Step 3: Based on the relative distance between the virtual navigator and the fire point being handled, the firefighting drone swarm is judged to switch from the swarm patrol phase to the coordinated handling phase. The specific implementation is as follows:

[0135] According to the specific mission plan, when the patrol firefighting drone enters the collaborative disposal stage from the cluster patrol stage, it is necessary to switch the corresponding control algorithm, converting the "distributed formation control algorithm based on virtual navigator" used in the early stage to the "distributed collaborative control algorithm with time consistency constraints". The switching of the control algorithm is based on the relative distance between the virtual navigator and the fire point being disposed of. Make conditional judgments, The spatial coordinates of the fire being addressed in the global coordinate system. When this relative distance reaches or falls below a set threshold, the control algorithm used by the patrol firefighting drones is instantly switched. Because the patrol firefighting drones have already formed a stable formation configuration during the initial control phase, and the global spatial positions of each drone are clear, the instantaneous switch in control algorithm does not affect their normal flight.

[0136] Step 4: Based on the multi-machine collaborative handling characteristics required in the later stages of patrol firefighting drone mission execution, the later stages of patrol firefighting drone control are defined as the collaborative handling phase. In this phase, a distributed collaborative control algorithm with time consistency constraints is used to design the post-control strategy for the patrol firefighting drone swarm. Each patrol firefighting drone dynamically adjusts its flight state based on its own flight status and the position information of adjacent patrol firefighting drones. The expected control commands are calculated in real time, synchronizing the handling time of each patrol firefighting drone at the same fire point, thereby improving the handling accuracy and capability of the drone swarm.

[0137] The collaborative control algorithm is based on the relative position relationship between each patrol fire-fighting drone and the fixed fire point. It is necessary to establish a "target coordinate system" as a unified basis for solving control instructions. The "target coordinate system" takes the spatial position of the fire point being handled as the coordinate origin. The vector direction of each coordinate axis is always consistent with the global coordinate system. The spatial position coordinates of the fire point being handled in the global coordinate system are P t = [ x t , y t , z t ] T ,in The coordinates of the fire point being handled in the x-axis, y-axis, and z-axis directions of the global coordinate system are represented in sequence. Then, the transformation relationship of the spatial position of the i-th patrol firefighting drone in the formation from the global coordinate system to the target coordinate system is:

[0138] [ x i a y i a z i a ] = [ x i − x t y i − y t z i − z t ]

[0139] In the formula, the superscript a represents the target coordinate system. Since the coordinate axis vector direction of the target coordinate system is the same as that of the global coordinate system, the velocity vector and attitude angle of the patrol firefighting UAV remain unchanged.

[0140] The state parameters of each patrol fire-fighting UAV and the fire point being handled are expressed based on the target coordinate system, and the spatial position of the i-th patrol fire-fighting UAV in the target coordinate system is P i a = [ x i a , y i a , z i a ] T , the velocity vector is V i a = [ u i a , v i a , w i a ] T The spatial position of the ignition point is P t a = [ x t a , y t a , z t a ] T = [ 0 , 0 , 0 ] ,in represents the coordinates of the i-th patrol firefighting drone in the x-axis, y-axis, and z-axis directions of the target coordinate system, represents the speed of the i-th patrol firefighting drone in the x-axis, y-axis and z-axis directions of the target coordinate system, The coordinates of the fire point being treated in the x-axis, y-axis and z-axis directions of the target coordinate system are represented in sequence.

[0141] In addition, the control state parameters of the patrol fire-fighting UAV are defined according to the proportional guidance relationship. The target vector between the i-th patrol fire-fighting UAV and the fire point being handled is: , then the modulus of the target vector between the i-th patrol firefighting drone and the fire point being handled is the distance between the two , the speed of the i-th patrol firefighting drone in the target coordinate system is .

[0142] Lead angle is the angle between the velocity vector and the target vector of the i-th patrol fire-fighting UAV, which is used to represent the line of sight deviation between the flight direction of the i-th patrol fire-fighting UAV and the fire point being handled. The formula for solving the lead angle of the i-th patrol fire-fighting UAV is:

[0143]

[0144] Machine change rate is the rate of change of the distance between the i-th patrol fire-fighting UAV and the fire point being handled. The formula for solving the change rate of the i-th patrol fire-fighting UAV is:

[0145]

[0146] The distributed collaborative control algorithm is based on proportional guidance control and introduces a consistent collaborative control part based on the status of adjacent patrol fire-fighting drones. The two control parameters are solved and synthesized, so that the collaborative disposal control strategy of patrol fire-fighting drones has time consistency constraints.

[0147] First, based on the flight state parameters of the patrol fire-fighting UAV itself, the proportional guidance part of the distributed cooperative control algorithm is solved according to the classical proportional guidance algorithm. The acceleration value of the proportional guidance part of the i-th patrol fire-fighting UAV is calculated. The calculation formula is:

[0148]

[0149] Where, Indicates the proportional guidance coefficient. Adjust the parameter according to the actual situation.

[0150] Secondly, in order to ensure that the patrol fire-fighting drone cluster can deal with the fire points at the same time, it is necessary to adjust the acceleration of the patrol fire-fighting drone through the consistency collaborative control part, so that each patrol fire-fighting drone can dynamically adjust its own flight state according to the flight state of the adjacent patrol fire-fighting drone. Based on the adjacency matrix defined in step 1 , and according to the flight state parameters of the patrol fire-fighting UAV and the adjacent patrol fire-fighting UAV, the coordinated control part of the distributed coordinated control algorithm is solved, and the acceleration value of the consistent coordinated control part of the i-th patrol fire-fighting UAV is calculated. The calculation formula is:

[0151]

[0152] Where, To coordinate the control of the first parameter for consistency, To coordinate the control of the second parameter for consistency, To coordinate and control the third parameter in a consistent manner, the parameter is adjusted and calibrated according to the actual situation; is the distance between the jth patrol firefighting drone and the fire point being dealt with, is the rate of change of the distance between the jth patrol firefighting drone and the fire point being handled, is the speed of the j-th patrol firefighting UAV in the target coordinate system.

[0153] Finally, the acceleration values ​​of the proportional guidance part and the consistency cooperative control part are combined to obtain the sum of the acceleration values ​​of the distributed cooperative control algorithm of the i-th patrol firefighting drone. , the calculation formula is:

[0154]

[0155] Since the acceleration command of the patrol fire-fighting UAV needs to have a clear direction, it is necessary to calculate the unit vector of the normal acceleration, so as to solve the normal acceleration vector. According to the geometric relationship between the velocity vector, the target vector and the normal acceleration of the patrol fire-fighting UAV, the normal acceleration unit vector of the i-th patrol fire-fighting UAV can be obtained as The calculation formula is:

[0156] N i a = − ( I 3 − V i a ∥ V i a ∥ ⋅ ( V i a ) T ∥ V i a ∥ ) ⋅ R i a ∥ R i a ∥ ∥ [ ( I 3 − V i a ∥ V i a ∥ ⋅ ( V i a ) T ∥ V i a ∥ ) ⋅ R i a ∥ R i a ∥ ] ∥

[0157] in is a 3rd order unit vector.

[0158] By multiplying the specific acceleration value obtained by the solution with the normal acceleration unit vector, we can get the normal acceleration vector required by the i-th patrol fire-fighting drone during the collaborative disposal flight. The calculation formula for the normal acceleration vector of the i-th patrol fire-fighting drone is:

[0159]

[0160] The normal acceleration vector of the i-th patrol firefighting drone is obtained .

[0161] The normal acceleration vector of the i-th patrol firefighting drone obtained above is expressed based on the target coordinate system. Since the directions of the coordinate axis vectors of the target coordinate system are always consistent with the global coordinate system, it can be considered that the normal acceleration vector is also expressed based on the global coordinate system. The normal acceleration vector in the global coordinate system needs to be converted to the drone body axis system for expression. For the i-th patrol firefighting drone, the coordinate conversion formula from the global coordinate system to the drone body axis system is:

[0162] T i B = [ c o s i i c o s ψ i c o s i i s i n ψ i − s i n i i s i n ϕ i s i n i i c o s ψ i − c o s ϕ i s i n ψ i s i n ϕ i s i n i i s i n ψ i + c o s ϕ i c o s ψ i s i n ϕ i c o s i i c o s ϕ i s i n i i c o s ψ i + s i n ϕ i s i n ψ i c o s ϕ i s i n i i s i n ψ i − s i n ϕ i c o s ψ i c o s ϕ i c o s i i ]

[0163] Where, 、 and represent the roll angle, pitch angle, and yaw angle of the i-th patrol firefighting drone respectively.

[0164] After coordinate transformation, the normal acceleration vector of the i-th patrol firefighting drone is expressed in its own body axis system as DCG, i B = [ a DCG, i B ( x ) , a DCG, i B ( y ) , a DCG, i B ( z ) ] ,in In order The acceleration components in the x-axis, y-axis and z-axis directions of the body axis system. Then, according to the acceleration component of the normal acceleration vector of the i-th patrol firefighting drone in the x-axis direction of its own body axis system, , the corresponding calculation results in the expected thrust , and solve the expected pitch angle and expected roll angle according to the acceleration components of the normal acceleration vector of the i-th patrol firefighting drone in the z-axis and y-axis directions of its own body axis system. The calculation formula is:

[0165]

[0166]

[0167] At each moment in the collaborative processing process, the above formula will be used to obtain the expected pitch angle and the desired roll angle , and expected thrust Input into the established patrol fire-fighting UAV control model to obtain the specific control quantity of the patrol fire-fighting UAV: ​​the equivalent elevator deflection angle control instruction value of the i-th patrol fire-fighting UAV and the equivalent aileron deflection angle control command value , and according to the expected thrust Get the corresponding throttle opening , enabling a cluster of patrol fire-fighting drones to deal with the same fire point at the same time.

[0168] In this embodiment, the global positions of the three patrol fire-fighting drones are clear at the initial moment when they enter the coordinated disposal stage from the cluster patrol stage. A stable "single-line" formation has been formed before the control strategy is switched. At the same time, the global position of the fire point being disposed of is fixed and known.

[0169] A "target coordinate system" is established based on the fire point being dealt with, which serves as the basis for solving the control instructions of the three patrol fire-fighting drones. The "target coordinate system" uses the spatial position of the fire point being dealt with as the coordinate origin, and the vector direction of each coordinate axis is always consistent with the global coordinate system.

[0170] In this embodiment, the collaborative control algorithm is designed based on the target coordinate system, and the spatial position of the treated fire point in the target coordinate system is expressed as P t a = [ 0 , 0 , 0 ] , the initial spatial position of the first patrol firefighting UAV in the target coordinate system is P 1 a = [ − 1 0 0 0 , − 4 0 0 , 1 0 0 ] T , the initial spatial position of the second patrol firefighting UAV in the target coordinate system is P 2 a = [ − 1 0 0 0 , − 3 0 0 , 1 0 0 ] T , the initial spatial position of the third patrol firefighting UAV in the target coordinate system is P 3 a = [ − 1 0 0 0 , − 5 0 0 , 1 0 0 ] T In addition, the control state parameters of the patrol fire-fighting UAV are defined according to the proportional guidance relationship, and the target vectors between the three patrol fire-fighting UAVs and the fire point being handled are: 、 and The distance between the three patrol firefighting drones and the fire point being handled is 、 and , the speed is 、 and Lead angle 、 and is the angle between the velocity vectors of the three patrol firefighting drones and their target vectors, which is used to represent the sight deviation between the flight directions of the three patrol firefighting drones and the fire points being handled, and the target change rate 、 and It is the rate of change of the distance between the three patrol fire-fighting drones and the fire point being dealt with.

[0171] The final form is Figure 2 The complete coordinated control method of a swarm of patrol firefighting drones based on arrival time consistency is shown. Next, a simulation is conducted on a swarm of patrol firefighting drones consisting of three small flying wing layout drones in this embodiment. The specific implementation is as follows:

[0172] First, the cascade proportional-integral controller based on the given desired attitude angle built in step 1 is simulated. This embodiment is specifically implemented by controlling the longitudinal pitch control channel +10° pitch angle step command control and the lateral roll control channel +5° roll angle step command control of the small flying wing layout UAV body control model, observing the step command tracking control effect, thereby verifying the effectiveness of the control system.

[0173] The simulation results are as follows Figure 3 and Figure 4As shown, the patrol firefighting drone has high longitudinal pitch control efficiency, while the lateral roll control efficiency is relatively low. However, the roll angle step adjustment time only takes about 1 second, which is sufficient to complete efficient attitude adjustment for the patrol firefighting drone. In addition, it should be noted that the flying wing layout used in this embodiment has inherent characteristics of inconsistent longitudinal and lateral stability. If a drone with a different layout is used, the main control effect will be different.

[0174] Secondly, the distributed formation control algorithm based on the virtual navigator designed in step 2 is simulated. The embodiment of the present invention specifically simulates the formation control of a cluster of three patrol fire-fighting drones. The simulation lasts for 100 seconds. Between 0 seconds and 45 seconds, a "triangle" formation is required to be formed and maintained, a transition is made between 45 seconds and 50 seconds, and a "single-line" formation is required to be formed and maintained between 50 seconds and 100 seconds.

[0175] The simulation results are as follows Figure 5 、 Figure 6 and Figure 7 As shown in the figure, the three patrol fire-fighting drones can form and maintain a stable desired formation. The position and speed control in all directions during the formation flight are reasonable, and the response to the formation switching instructions is sensitive, which fully verifies that the formation control algorithm can meet the control requirements in the early stage of control.

[0176] Finally, a simulation is performed on the distributed collaborative control algorithm with time consistency constraints designed in step 3. This embodiment specifically simulates the collaborative disposal of a cluster of three patrol fire-fighting drones. The initial relative positions of the three patrol fire-fighting drones are the "single-line" formation positions formed in step 2, and the final relative positions are the fixed positions of the disposed fire points (within 10 meters). The classical proportional guidance law and the collaborative control algorithm designed in the present invention are used for comparative simulation verification.

[0177] The simulation results using the classical proportional guidance law are as follows: Figure 8 As shown in the figure, the simulation results of the cooperative control algorithm designed by the present invention are as follows: Figure 9 、 Figure 10 、 Figure 11 、 Figure 12 and Figure 13As shown in the figure, the three patrol firefighting drones using the proportional guidance law terminated their operations at 51.60s, 53.36s, and 55.55s, respectively. The cooperative control algorithm terminated its operations at 55.22s, 55.43s, and 55.51s, respectively, with time-to-impact errors of 3.95s and 0.39s, respectively. The cooperative control algorithm reduced the time-to-impact error by approximately 90%, significantly improving the ability of multiple drones to simultaneously handle the same fire. Furthermore, the position and velocity control in all directions during the control process were reasonable, fully verifying that the cooperative control algorithm can meet the control requirements of the later stages of the control process.

[0178] Although the embodiments of the present invention have been shown and described above, it will be understood that the above embodiments are illustrative and are not to be construed as limitations on the present invention. A person skilled in the art may change, modify, replace and modify the above embodiments within the scope of the present invention without departing from the principles and purpose of the present invention.

Claims

1. A collaborative control method for cluster patrol firefighting drones based on arrival time consistency, characterized by: The following steps are involved: Step 1: Establish a six-degree-of-freedom dynamic model of the patrol fire-fighting UAV; The main control model of the patrol fire-fighting UAV is designed based on the cascade proportional-integral controller with a given desired attitude angle; the main control model is divided into a longitudinal pitch control channel and a lateral roll control channel; in the longitudinal pitch control channel, a pitch angle rate feedback loop is introduced into the pitch angle control loop; in the lateral roll control channel, a roll angle rate feedback loop is introduced into the roll angle control loop; Using the adjacency matrix Represents the communication relationship between each patrol fire-fighting drone in the patrol fire-fighting drone cluster; the adjacency matrix is The fully connected matrix of order n represents the number of patrol firefighting drones contained in the patrol firefighting drone cluster, and the elements in the adjacency matrix are Represents the connection weight between the i-th patrol fire-fighting drone and the j-th patrol fire-fighting drone; Step 2: Divide the mission phase of the patrol firefighting drone cluster into the cluster patrol phase and the coordinated disposal phase; In the cluster patrol stage, a distributed formation control algorithm based on a virtual navigator is used to design a control strategy for patrol fire-fighting UAVs. Each patrol fire-fighting UAV calculates the expected control command in real time according to the designated formation, virtual navigator information, and the flight status of the patrol fire-fighting UAV and the adjacent patrol fire-fighting UAVs. The expected control command is input into the ontology control model of the patrol fire-fighting UAV to calculate the control quantity of the patrol fire-fighting UAV, and the patrol fire-fighting UAV cluster is controlled to form a designated air formation, maintain the formation and fly along the expected route. Step 3: Perform conditional judgment based on the relative distance between the virtual navigator and the fire point to be dealt with. When the relative distance reaches or is less than the set threshold, switch the control strategy of the patrol fire-fighting drone from the control strategy of the cluster patrol stage to the control strategy of the coordinated disposal stage; Step 4: In the collaborative disposal stage, a distributed collaborative control algorithm with time consistency constraints is used to design the control strategy of the patrol fire-fighting drone cluster. Each patrol fire-fighting drone dynamically adjusts its flight state according to its own flight state and the position information of adjacent patrol fire-fighting drones, and solves the expected control instructions in real time. The expected control instructions are input into the ontology control model of the patrol fire-fighting drone to solve the control quantity of the patrol fire-fighting drone, so as to control the disposal time of each patrol fire-fighting drone in the patrol fire-fighting drone cluster to deal with the same fire point to be synchronized.

2. According to claim 1, a coordinated control method for cluster patrol fire-fighting drones based on arrival time consistency is characterized by: In step 1, the controller structure of the longitudinal pitch control channel is: The controller structure of the lateral rolling control channel is: In the formula is the control command value of the equivalent elevator deflection angle of the i-th patrol firefighting UAV, is the control command value of the equivalent aileron deflection angle of the i-th patrol firefighting UAV; is the trim value of the equivalent elevator deflection angle of the i-th patrol firefighting UAV; is the trim value of the equivalent aileron deflection angle of the i-th patrol firefighting UAV; is the expected pitch rate of the i-th patrol firefighting UAV, is the expected roll angular rate of the i-th patrol firefighting UAV; is the rolling angular rate of the i-th patrol firefighting UAV, is the pitch angle rate of the i-th patrol firefighting drone; is the expected pitch angle of the i-th patrol firefighting drone, is the expected roll angle of the i-th patrol firefighting UAV; is the pitch angle of the i-th patrol firefighting drone, is the rolling angle of the i-th patrol firefighting drone; is the proportional gain value related to the pitch angle rate, is the proportional gain value related to the pitch angle, is the proportional gain value related to the roll angular rate, is the proportional gain value related to the roll angle; is the integral gain value related to the pitch angle rate, is the integral gain value related to the pitch angle, is the integral gain value related to the roll angular rate, is the integral gain value related to the roll angle.

3. The collaborative control method of cluster patrol fire-fighting drones based on arrival time consistency according to claim 1 is characterized by: In step 2, the desired control instruction includes the desired pitch angle , expected thrust and the desired roll angle , respectively, are obtained by the following formulas: in is the first parameter of the longitudinal proportional differential control, is the second parameter of the longitudinal proportional differential control, is the first parameter of forward proportional derivative control, is the second parameter of the forward proportional derivative control, is the first parameter of lateral proportional differential control, The second parameter of lateral proportional differential control; represents the coordinates of the i-th patrol firefighting drone in the x-axis direction, y-axis direction and z-axis direction of the virtual navigator coordinate system respectively; represents the speed of the i-th patrol firefighting drone in the x-axis direction, y-axis direction and z-axis direction of the virtual navigator coordinate system respectively; represents the coordinates of the virtual navigator in the x-axis direction, y-axis direction and z-axis direction of the virtual navigator coordinate system, respectively. represents the speed of the virtual navigator in the x-axis direction, y-axis direction and z-axis direction of the virtual navigator coordinate system respectively; represents the position coordinate deviation of the i-th patrol firefighting UAV relative to the virtual navigator in the x-axis direction, y-axis direction and z-axis direction of the virtual navigator coordinate system; represents the coordinates of the jth patrol firefighting UAV in the x-axis direction, y-axis direction and z-axis direction of the virtual navigator coordinate system respectively; represents the speed of the jth patrol firefighting UAV in the x-axis direction, y-axis direction and z-axis direction of the virtual navigator coordinate system respectively; They represent the position coordinate deviations of the j-th patrol firefighting UAV relative to the virtual navigator in the x-axis, y-axis, and z-axis directions of the virtual navigator's coordinate system.

4. The collaborative control method of cluster patrol fire-fighting drones based on arrival time consistency according to claim 3 is characterized by: In step 2, the formation is determined by the position coordinate deviation of the patrolling fire-fighting drones in the patrolling fire-fighting drone cluster relative to the virtual navigator.

5. The collaborative control method of cluster patrol fire-fighting drones based on arrival time consistency according to claim 3 is characterized by: In step 2, the virtual navigator information is the spatial position coordinates of the virtual navigator in the global coordinate system. , global velocity vector and the global flight direction ,in Represent the coordinates of the virtual navigator in the x-axis direction, y-axis direction and z-axis direction of the global coordinate system respectively; It represents the speed of the virtual navigator in the x-axis direction, y-axis direction and z-axis direction of the global coordinate system respectively; For the i-th patrol firefighting drone, the virtual space position in the virtual navigator coordinate system is expressed as , the virtual velocity vector is expressed as , the virtual heading angle is expressed as : in represents the coordinates of the i-th patrol firefighting drone in the x-axis, y-axis, and z-axis directions of the global coordinate system, respectively. represents the speed of the i-th patrol firefighting drone in the x-axis, y-axis, and z-axis directions of the global coordinate system, respectively. represents the global heading angle of the i-th patrol firefighting UAV in the global coordinate system; is the transformation matrix from the global coordinate system to the virtual navigator coordinate system.

6. The collaborative control method of cluster patrol fire-fighting drones based on arrival time consistency according to claim 1 is characterized by: In step 4, the desired control instruction includes the desired pitch angle , expected thrust and the desired roll angle , respectively, are obtained by the following formulas: in Indicates the quality of patrol firefighting drones. In order The acceleration components of the i-th patrol firefighting drone in the x-axis, y-axis and z-axis directions of its own body axis system, is the normal acceleration vector of the i-th patrol firefighting UAV in its own body axis system.

7. The method for coordinated control of cluster patrol fire-fighting drones based on arrival time consistency according to claim 6 is characterized by: The normal acceleration vector of the i-th patrol firefighting drone in the global coordinate system is It is obtained by transforming the coordinates from the global coordinate system to the body axis system of the i-th patrol fire-fighting UAV.

8. The collaborative control method of cluster patrol fire-fighting drones based on arrival time consistency according to claim 7 is characterized by: The normal acceleration vector of the i-th patrol firefighting drone in the global coordinate system According to the formula Get, where is the sum of the acceleration values ​​of the distributed cooperative control algorithm of the i-th patrol firefighting drone, is the normal acceleration unit vector of the i-th patrol firefighting drone: in is the acceleration value of the proportional guidance part of the i-th patrol firefighting UAV, is the acceleration value of the consistency cooperative control part of the i-th patrol firefighting UAV, is the velocity vector of the i-th patrol firefighting UAV in the target coordinate system, is the target vector between the i-th patrol fire-fighting drone and the fire point being dealt with: , is the spatial position of the i-th patrol firefighting UAV in the target coordinate system, is the spatial position of the fire point to be treated in the target coordinate system, is a third-order unit vector.

9. The collaborative control method of cluster patrol fire-fighting drones based on arrival time consistency according to claim 8 is characterized by: The acceleration value of the proportional guidance part of the i-th patrol firefighting UAV The calculation formula is: in represents the proportional guidance coefficient, is the speed of the i-th patrol firefighting drone in the target coordinate system, ; is the angle between the velocity vector of the i-th patrol firefighting drone and the target vector: is the distance between the i-th patrol firefighting drone and the fire point being dealt with, ; The acceleration value of the consistent cooperative control part of the i-th patrol firefighting drone The calculation formula is: in The first parameter is coordinated for consistency control, To coordinate the control of the second parameter for consistency, To coordinate the control of the third parameter for consistency, is the distance between the jth patrol firefighting drone and the fire point being dealt with, is the rate of change of the distance between the jth patrol firefighting drone and the fire point being dealt with, is the speed of the jth patrol firefighting UAV in the target coordinate system, is the rate of change of the distance between the i-th patrol fire-fighting drone and the fire point being dealt with.

Images