A fault-tolerant formation control method for quadrotor drones with anti-interference capability

Abstract

The present invention discloses a fault-tolerant formation control method for a quad-rotor unmanned aerial vehicle with anti-interference capability, including establishing a cluster system of unmanned aerial vehicles, and establishing translational dynamics, rotational dynamics, motor dynamics and actuator fault models for each unmanned aerial vehicle; designing a nominal formation controller and combining a high-order sliding mode observer to estimate and compensate for actuator deviation faults and external time-varying disturbances to realize trajectory tracking control of each quad-rotor unmanned aerial vehicle on a virtual navigator; decoupling a position loop and an attitude loop, designing an attitude controller for the nominal system and combining a high-order sliding mode observer to solve the influence of external time-varying disturbances on the attitude loop to realize attitude stability of each quad-rotor unmanned aerial vehicle; calculating the lift coefficient of each rotor after an actuator fault based on the Newton correction method to realize online fault diagnosis, and calculating the speed of each motor according to the control efficiency matrix after the fault to realize safe and reliable formation control of the quad-rotor unmanned aerial vehicle after the actuator fault. The stability and safety of the quad-rotor unmanned aerial vehicle cluster flight are improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of quad-rotor unmanned aerial vehicle formation control, and in particular relates to a quad-rotor unmanned aerial vehicle fault-tolerant formation control method with anti-interference capability. Background Art

[0002] A quad-rotor drone is a type of drone system that can take off and land vertically, has high flexibility, and is highly stable. Given these excellent characteristics, it has been widely used in many fields, including smart agriculture, environmental monitoring, power inspection, rescue and search, and military reconnaissance. Compared with a single quad-rotor system, an unmanned cluster system composed of multiple quad-rotors not only has the advantages of a single quad-rotor drone, but also has the advantages of strong autonomy, high robustness, good timeliness, and high cost performance of a multi-drone system. Therefore, a quad-rotor cluster system can perform tasks that are more difficult and have a wider range. How to make a quad-rotor cluster system perform tasks efficiently and safely has become a hot topic for technicians in this field.

[0003] Formation control is one of the key technologies for the formation, maintenance, reconstruction and action of unmanned swarm systems. In formation control based on distributed control structure, each quadrotor only needs to exchange information with neighboring quadrotors to achieve overall formation control. It has the advantages of flexibility, fault tolerance, scalability, high communication efficiency, etc., which can improve the performance and robustness of the swarm system and adapt to the formation control tasks of the coordinated work of quadrotor unmanned swarms. In the control strategy, the virtual navigator is simple, efficient and can be flexibly expanded according to actual needs to achieve more complex formation tasks. Therefore, the distributed control structure combined with the virtual navigator control strategy becomes a better strategy choice in formation control.

[0004] With the continuous expansion of the application field of formation control of quadrotor drones, the environment for formation control to perform tasks is becoming more and more complex, which puts higher requirements on the safety and robustness of formation control. Due to the small size and light weight of quadrotor drones, they are easily subject to unknown interferences such as time-varying wind disturbances and atmospheric turbulence in complex environments, which brings challenges to the stability and safety of formation control. Under some serious interferences, the damage of individual quadrotors will cause the entire formation control to fail. During the execution of tasks, the probability of failure of quadrotor drones will be greatly increased due to the influence of the complex external environment and the long-term operation of the internal mechanical structure, and most of the failures are sudden failures, especially actuator failures (motor power loss, blade damage) often occur during the execution of tasks, seriously affecting the flight safety and mission completion of formation control. Based on the above problems, there is an urgent need for a fault-tolerant formation control method for quadrotor drones with anti-interference ability. Summary of the invention

[0005] In view of the above technical problems, the present invention provides a fault-tolerant formation control method for a quad-rotor UAV with anti-interference capability.

[0006] The technical solution adopted by the present invention to solve the technical problem is:

[0007] A fault-tolerant formation control method for a quad-rotor unmanned aerial vehicle with anti-interference capability, the method comprising the following steps:

[0008] S100: Establish a UAV cluster system consisting of multiple quad-rotor UAVs in three-dimensional space, and establish translational dynamics, rotational dynamics, motor dynamics and actuator fault models for each UAV, and then establish a complete dynamic model of the quad-rotor cluster. The control goal of the complete dynamic model is to design the thrust and torque so that each quad-rotor UAV meets the preset conditions;

[0009] S200: Design a nominal formation controller. Rewrite the translational dynamics model with deviation faults according to the translational dynamics and actuator fault model. Combined with the high-order sliding mode observer to estimate and compensate for the actuator deviation fault and external time-varying disturbances, design a robust controller for the position loop, and then obtain the static thrust of each quadrotor drone, so that each quadrotor drone can track the trajectory of the virtual navigator.

[0010] S300: Decouple the position loop and attitude loop to obtain the desired attitude of each quadrotor drone, design an attitude controller for the nominal system, rewrite the rotational dynamics model with deviation fault according to the rotational dynamics and actuator fault model, and combine the high-order sliding mode observer to solve the influence of actuator deviation fault and external time-varying disturbance on the attitude loop, design a robust controller of the attitude loop, so as to achieve the attitude stability of each quadrotor drone;

[0011] S400: Based on the complete dynamics model, the robust controller of the position loop and the robust controller of the attitude loop, the complete dynamics model is rewritten, and the lift coefficient of each rotor after the actuator failure is calculated based on the Newton correction method to realize online fault diagnosis. The speed of each motor is calculated according to the control efficiency matrix after the fault, so as to realize safe and reliable formation control of the quadrotor UAV after the actuator failure.

[0012] Preferably, the inertial coordinate system I and the body coordinate system B are established, and S100 includes:

[0013] S110: According to Newton's second law, the translational dynamics equation of each quadrotor subsystem is:

[0014] (1)

[0015] In the formula, Indicates the number of the quadrotor drone in the unmanned swarm system. and Respectively represent the position and linear velocity of each quadrotor drone in the inertial coordinate system I, where They are the positions of each quadrotor drone in the x-axis, y-axis and z-axis directions, are the speeds of each quadrotor drone in the x-axis, y-axis and z-axis directions, is a rotation matrix defined by Euler angles, representing the transformation from body coordinates to world coordinates. is a constant gravitational acceleration, is a constant vector, represents the mass of each quadrotor drone, is the force in the body coordinate system, represents the total thrust on the drone, is the external disturbance of the translational dynamics of each quadrotor drone, where They are the disturbances to which each quadrotor drone is subjected in the x-axis, y-axis, and z-axis directions, including air resistance, time-varying wind disturbance, and other unmodeled parts;

[0016] S120: According to the Euler equation, the rotational dynamics equation of the attitude subsystem is written as:

[0017] (2)

[0018] In the formula, are the Euler angles, where They are the pitch angle, roll angle and yaw angle of each quadrotor drone. represents the angular velocity in the body coordinate system, where are the pitch velocity, roll velocity and yaw velocity of each quadrotor drone, is the inertia matrix, where They are the moments of inertia of each quadrotor drone around the x-axis, y-axis, and z-axis of the body coordinate system, represents the torque, where They are the torques of each quadrotor drone in the x-axis, y-axis and z-axis directions, represents the disturbances suffered by the attitude subsystem, including the unmodeled part and external wind disturbance, where They are the disturbances that each quadrotor drone experiences when pitching, rolling, and yawing;

[0019] S130: Based on the relationship between the lift, rolling moment, pitch moment and yaw moment generated by the four rotors of the quad-rotor drone and the motor speed, the control input model of each quad-rotor drone, i.e., the motor dynamics equation, is:

[0020] (3)

[0021] In the formula, is the control input vector, is the speed of each motor, where is the speed of motors 1, 2, 3, and 4 of each quadrotor drone, is the control effectiveness matrix, and its specific form is:

[0022] (4)

[0023] In the formula, is the lift coefficient of each motor of each drone, is the reaction torque coefficient of each UAV, is the lift-to-drag ratio coefficient;

[0024] S140: Establish the actuator fault model as follows:

[0025] (5)

[0026] In the formula, ,in is the lift failure coefficient of each motor; represents the control input of the nominal system under ideal conditions, It is an unknown actuator deviation fault;

[0027] Combining equations (3) and (5), the actuator fault model is rewritten as:

[0028] (6)

[0029] S150: Combining translational dynamics (1), rotational dynamics (2), motor dynamics (3) and actuator failure model (6), the complete dynamic model of the quadrotor cluster is expressed as:

[0030] (7)

[0031] In the formula, is the status of each quadrotor drone in the unmanned swarm, , in ,in, is the sine operator, is the cosine operator, .

[0032] Preferably, in formula (5), according to and Taking different values, actuator failures can be divided into the following types:

[0033] (1) When and When , it is a partial failure of the actuator, also called a multiplicative failure;

[0034] (2) When and When , it is an actuator deviation fault, also called additive fault;

[0035] (3) When and When , the actuator has both partial failure and deviation fault, which is called mixed fault;

[0036] (4) When and There is no actuator fault.

[0037] Preferably, the preset conditions in S100 are specifically:

[0038] (8)

[0039] In the formula, It is the number of the quadcopter drone, which is similar to the drone Adjacent, It is The location of the drone, Indicates drone and The relative positions between them determine the required formation mode. is the time-varying trajectory of the virtual navigator, yes The first-order differential of represents the time-varying speed of the virtual navigator, which is also the reference line speed of the formation.

[0040] Preferably, S200 includes:

[0041] S210: Design a nominal formation controller based on the consistency control protocol of the second-order system to achieve translational motion under ideal conditions. The controller form is:

[0042] (9)

[0043] In the formula, and is a positive constant control parameter matrix, where They are the position control parameters in the x-axis, y-axis and z-axis directions, They are the speed control parameters in the x-axis, y-axis and z-axis directions respectively. and They are and The first-order differential of yes The second-order differential of Is able to The dynamic neighborhood set of UAVs where four quadrotor UAVs interact with each other. It is The communication topology value between the quadrotor drone and the virtual navigator is If you can communicate with the pilot The value is 1, otherwise it is 0. is the adjacency matrix The elements in The value of is given by:

[0044] (10)

[0045] S220: Combining the translational dynamics (1) and the third case in the actuator fault model, the translational dynamics model with deviation fault is rewritten as:

[0046] (11)

[0047] In the formula, is the lumped disturbance, including the effect of actuator deviation fault and external time-varying disturbance;

[0048] S230: Design of high-order sliding mode observers to estimate and compensate for lumped disturbances , its specific form is:

[0049] (12)

[0050] In the formula, is the position estimation error of each quadrotor, and are the estimated values ​​of the speed of each quadrotor and the lumped disturbance, It is an auxiliary parameter to ensure Second order differentiable, is a positive constant;

[0051] S240: Design the robust controller of the position loop as:

[0052] (13)

[0053] In the formula, is a low-pass filter, where is the cutoff frequency of the filter;

[0054] S250: The complete controller for the position loop is:

[0055] (14)

[0056] S260: The static thrust input for each quadrotor drone is as follows:

[0057] (15)

[0058] In the formula yes The third dimension of the vector.

[0059] Preferably, S300 includes:

[0060] S310: Decouple the position subsystem and attitude subsystem and select any yaw angle , the desired roll angle and pitch angle It can be obtained by the following formula:

[0061] (16)

[0062] In the formula, and The controllers are The first and second dimension data of

[0063] S320: Define the reference posture of each quadrotor drone as:

[0064] (17)

[0065] The attitude error is designed as:

[0066] (18)

[0067] In the formula, They are pitch, roll and yaw angle errors respectively;

[0068] S330: Design an ideal nominal attitude controller for the attitude subsystem to achieve attitude stability of each quadrotor. The specific form of the attitude controller is:

[0069] (19)

[0070] In the formula, and are positive constant control gain matrices, where are the attitude control parameters of pitch angle, roll angle and yaw angle, They are the pitch angular velocity, roll angular velocity and yaw angular velocity control parameters respectively;

[0071] S340: Combining the rotational dynamics and actuator fault models for the third case, the rotational dynamics model with actuator deviation fault is rewritten as:

[0072] (20)

[0073] In the formula, is a disturbance, where is the second to fourth dimension data of the actuator deviation, is external time-varying interference;

[0074] S350: Design a high-order sliding mode observer to estimate and compensate for lumped disturbances, which is of the form:

[0075] (twenty one)

[0076] In the formula, is the estimated error of the attitude, and are the estimates of the angular velocity and the lumped disturbance, respectively, is an auxiliary parameter to ensure It is second-order differentiable, All are normal numbers;

[0077] S360: The robust controller design of the attitude loop is:

[0078] (twenty two)

[0079] In the formula, is a low-pass filter, where is the filter cutoff frequency.

[0080] Preferably, S400 includes:

[0081] S410: According to the complete dynamics model (7), the robust controller of the position subsystem (13) and the robust control of the attitude subsystem (22), the complete dynamics model is rewritten as:

[0082] (twenty three)

[0083] In the formula, is the lumped disturbance term;

[0084] S420: The lift coefficient of each UAV is estimated online through real-time information, and the actuator fault is diagnosed. The dynamics (23) is rewritten as:

[0085] (twenty four)

[0086] In the formula , The specific form is as follows:

[0087] (25)

[0088] S430: Define the objective function as follows:

[0089] (26)

[0090] about The first and second order derivatives are:

[0091] (27)

[0092] In the formula, is the number of iterations;

[0093] S440: Judgment Is it established? is the allowable error. If it holds, stop the calculation. If it does not hold, let the gradient direction be:

[0094] (28)

[0095] In the formula, ,in is a suitable positive constant, which is used to ensure is positive definite; therefore, the specific form of the gradient direction is:

[0096] (29)

[0097] S450: From Departure, along the direction Do a one-dimensional search to find the corresponding learning step size make The fastest descent, one-dimensional search method is:

[0098] (30)

[0099] S460: Order , and iterate formulas (27)-(30) until , and get the corresponding The value can be obtained, so that the lift coefficient of each rotor actuator partial failure can be obtained, and the control efficiency distribution matrix after the actuator partial failure can be obtained as the matrix , finally, the expected speed of each motor of each quadrotor is:

[0100] (31).

[0101] The above-mentioned fault-tolerant control method for quadrotor UAV formation with anti-interference ability introduces a high-order sliding mode observer in the position loop and attitude loop to estimate unknown time-varying disturbances and actuator deviation faults, thereby improving the robustness and safety of the unmanned cluster system; in view of the actuator failure failure that each quadrotor in the unmanned cluster system may encounter, a fault-tolerant control strategy based on the Newton correction method is proposed to diagnose the actuator failure failure in real time online, so that the system has good fault tolerance and ensures the safe and stable operation of the entire system. The proposed formation controller is simple, robust, stable, and has good steady-state and dynamic performance. BRIEF DESCRIPTION OF THE DRAWINGS

[0102] Figure 1 This is a flow chart of a fault-tolerant control method for a quad-rotor UAV formation with anti-interference capability in one embodiment of the present invention;

[0103] Figure 2 It is a schematic diagram of a fault-tolerant control method for a quad-rotor UAV formation with anti-interference capability in one embodiment of the present invention;

[0104] Figure 3 is a topological diagram of the relationship between a virtual navigator and a follower in one embodiment of the present invention;

[0105] Figure 4 is a schematic diagram of the trajectory of the formation process of the quadrotor unmanned cluster system in simulation in one embodiment of the present invention;

[0106] Figure 5 is a schematic diagram of formation error of a quadrotor unmanned cluster system in simulation in one embodiment of the present invention;

[0107] Figure 6 is a schematic diagram of the attitude error of the quadrotor unmanned cluster system in simulation in one embodiment of the present invention;

[0108] Figure 7 is the observer aggregate disturbance observation error of the quadrotor unmanned cluster system in the simulation in one embodiment of the present invention;

[0109] Figure 8 is the actuator fault estimation error of the quadrotor unmanned cluster system in the simulation in one embodiment of the present invention;

[0110] Fig. 9 It is the thrust and torque input of the four-rotor unmanned cluster system in the simulation in one embodiment of the present invention. DETAILED DESCRIPTION

[0111] In order to enable those skilled in the art to better understand the technical solution of the present invention, the present invention is further described in detail below in conjunction with the accompanying drawings.

[0112] In one embodiment, Figure 1 As shown, a fault-tolerant formation control method for a quad-rotor drone with anti-interference capability comprises the following steps:

[0113] S100: Establish a UAV cluster system consisting of multiple quad-rotor UAVs in three-dimensional space, and establish translational dynamics, rotational dynamics, motor dynamics and actuator fault models for each UAV, and then establish a complete dynamic model of the quad-rotor cluster. The control goal of the complete dynamic model is to design the thrust and torque so that each quad-rotor UAV meets the preset conditions;

[0114] S200: Design a nominal formation controller. Rewrite the translational dynamics model with deviation faults according to the translational dynamics and actuator fault model. Combined with the high-order sliding mode observer to estimate and compensate for the actuator deviation fault and external time-varying disturbances, design a robust controller for the position loop, and then obtain the static thrust of each quadrotor drone, so that each quadrotor drone can track the trajectory of the virtual navigator.

[0115] S300: Decouple the position loop and attitude loop to obtain the desired attitude of each quadrotor drone, design an attitude controller for the nominal system, rewrite the rotational dynamics model with deviation fault according to the rotational dynamics and actuator fault model, and combine the high-order sliding mode observer to solve the influence of actuator deviation fault and external time-varying disturbance on the attitude loop, design a robust controller of the attitude loop, so as to achieve the attitude stability of each quadrotor drone;

[0116] S400: Based on the complete dynamics model, the robust controller of the position loop and the robust controller of the attitude loop, the complete dynamics model is rewritten, and the lift coefficient of each rotor after the actuator failure is calculated based on the Newton correction method to realize online fault diagnosis. The speed of each motor is calculated according to the control efficiency matrix after the fault, so as to realize safe and reliable formation control of the quadrotor UAV after the actuator failure.

[0117] The above-mentioned fault-tolerant control method for quadrotor UAV formation with anti-interference ability introduces a high-order sliding mode observer in the position loop and attitude loop to estimate unknown time-varying disturbances and actuator deviation faults, thereby improving the robustness and safety of the unmanned cluster system; in view of the actuator failure failure that each quadrotor in the unmanned cluster system may encounter, a fault-tolerant control strategy based on the Newton correction method is proposed to diagnose the actuator failure failure in real time online, so that the system has good fault tolerance and ensures the safe and stable operation of the entire system. The proposed formation controller is simple, robust, stable, and has good steady-state and dynamic performance.

[0118] In one embodiment, the inertial coordinate system I and the body coordinate system B are established, and S100 includes:

[0119] S110: According to Newton's second law, the translational dynamics equation of each quadrotor subsystem is:

[0120] (1)

[0121] In the formula, Indicates the number of the quadrotor drone in the unmanned swarm system. and Respectively represent the position and linear velocity of each quadrotor drone in the inertial coordinate system I, where They are the positions of each quadrotor drone in the x-axis, y-axis and z-axis directions, are the speeds of each quadrotor drone in the x-axis, y-axis and z-axis directions, is a rotation matrix defined by Euler angles, representing the transformation from body coordinates to world coordinates. is a constant gravitational acceleration, is a constant vector, represents the mass of each quadrotor drone, is the force in the body coordinate system, represents the total thrust on the drone, is the external disturbance of the translational dynamics of each quadrotor drone, where They are the disturbances to which each quadrotor drone is subjected in the x-axis, y-axis, and z-axis directions, including air resistance, time-varying wind disturbance, and other unmodeled parts;

[0122] S120: According to the Euler equation, the rotational dynamics equation of the attitude subsystem is written as:

[0123] (2)

[0124] In the formula, are the Euler angles, where They are the pitch angle, roll angle and yaw angle of each quadrotor drone. represents the angular velocity in the body coordinate system, where are the pitch velocity, roll velocity and yaw velocity of each quadrotor drone, is the inertia matrix, where They are the moments of inertia of each quadrotor drone around the x-axis, y-axis, and z-axis of the body coordinate system, represents the torque, where They are the torques of each quadrotor drone in the x-axis, y-axis and z-axis directions, represents the disturbances suffered by the attitude subsystem, including the unmodeled part and external wind disturbance, where They are the disturbances that each quadrotor drone experiences when pitching, rolling, and yawing;

[0125] S130: Based on the relationship between the lift, rolling moment, pitch moment and yaw moment generated by the four rotors of the quad-rotor drone and the motor speed, the control input model of each quad-rotor drone, i.e., the motor dynamics equation, is:

[0126] (3)

[0127] In the formula, is the control input vector, is the speed of each motor, where is the speed of motors 1, 2, 3, and 4 of each quadrotor drone, is the control effectiveness matrix, and its specific form is:

[0128] (4)

[0129] In the formula, is the lift coefficient of each motor of each drone, is the reaction torque coefficient of each UAV, is the lift-to-drag ratio coefficient;

[0130] S140: Establish the actuator fault model as follows:

[0131] (5)

[0132] In the formula, ,in is the lift failure coefficient of each motor; represents the control input of the nominal system under ideal conditions, It is an unknown actuator deviation fault;

[0133] Combining equations (3) and (5), the actuator fault model is rewritten as:

[0134] (6)

[0135] S150: Combining translational dynamics (1), rotational dynamics (2), motor dynamics (3) and actuator failure model (6), the complete dynamic model of the quadrotor cluster is expressed as:

[0136] (7)

[0137] In the formula, is the status of each quadrotor drone in the unmanned swarm, , in ,in, is the sine operator, is the cosine operator, .

[0138] In one embodiment, in formula (5), according to and Taking different values, actuator failures can be divided into the following types:

[0139] (1) When and When , it is a partial failure of the actuator, also called a multiplicative failure;

[0140] (2) When and When , it is an actuator deviation fault, also called additive fault;

[0141] (3) When and When , the actuator has both partial failure and deviation fault, which is called mixed fault;

[0142] (4) When and There is no actuator fault.

[0143] Specifically, the directed interaction topology between the quadrotor unmanned swarm systems is as follows: Figure 3 As shown in the figure, the actual quadrotor drones form a square formation at the four corners. Starting from the upper left corner, they are quadrotor drone 1, quadrotor drone 2, quadrotor drone 3, quadrotor drone 4 in a counterclockwise direction, and the virtual navigator drone is in the middle. The solid line is the directed interaction topology between the actual quadrotor drones, and the dotted line is the directed interaction topology between the virtual navigator and the actual quadrotor drone. Compared with the undirected interaction topology, the directed interaction topology consumes only half of the communication and energy resources, and is more suitable for quadrotor drones with limited communication and energy in practical applications.

[0144] based on Figure 3 The control goal of the unmanned swarm system with a directed interactive topology is to design thrust and torque Make each quadrotor drone meet the preset conditions.

[0145] In one embodiment, the preset conditions in S100 are specifically:

[0146] (8)

[0147] In the formula, It is the number of the quadcopter drone, which is similar to the drone Adjacent, It is The location of the drone, Indicates drone and The relative positions between them determine the required formation mode. is the time-varying trajectory of the virtual navigator, yes The first-order differential of represents the time-varying speed of the virtual navigator, which is also the reference line speed of the formation.

[0148] In one embodiment, S200 includes:

[0149] S210: Design a nominal formation controller based on the consistency control protocol of the second-order system to achieve translational motion under ideal conditions. The controller form is:

[0150] (9)

[0151] In the formula, and is a positive constant control parameter matrix, where They are the position control parameters in the x-axis, y-axis and z-axis directions, They are the speed control parameters in the x-axis, y-axis and z-axis directions respectively. and They are and The first-order differential of yes The second-order differential of Is able to The dynamic neighborhood set of UAVs where four quadrotor UAVs interact with each other. It is The communication topology value between the quadrotor drone and the virtual navigator is If you can communicate with the pilot The value is 1, otherwise it is 0. is the adjacency matrix The elements in The value of is given by:

[0152] (10)

[0153] S220: Combining the translational dynamics (1) and the third case in the actuator fault model, the translational dynamics model with deviation fault is rewritten as:

[0154] (11)

[0155] In the formula, is the lumped disturbance, including the effect of actuator deviation fault and external time-varying disturbance;

[0156] S230: Design of high-order sliding mode observers to estimate and compensate for lumped disturbances , its specific form is:

[0157] (12)

[0158] In the formula, is the position estimation error of each quadrotor, and are the estimated values ​​of the speed of each quadrotor and the lumped disturbance, It is an auxiliary parameter to ensure Second order differentiable, is a positive constant;

[0159] S240: Design the robust controller of the position loop as:

[0160] (13)

[0161] In the formula, is a low-pass filter, where is the cutoff frequency of the filter;

[0162] S250: The complete controller for the position loop is:

[0163] (14)

[0164] S260: The static thrust input for each quadrotor drone is as follows:

[0165] (15)

[0166] In the formula yes The third dimension of the vector.

[0167] In one embodiment, S300 includes:

[0168] S310: Decouple the position subsystem and attitude subsystem and select any yaw angle , the desired roll angle and pitch angle It can be obtained by the following formula:

[0169] (16)

[0170] In the formula, and The controllers are The first and second dimension data of

[0171] S320: Define the reference posture of each quadrotor drone as:

[0172] (17)

[0173] The attitude error is designed as:

[0174] (18)

[0175] In the formula, They are pitch, roll and yaw angle errors respectively;

[0176] S330: Design an ideal nominal attitude controller for the attitude subsystem to achieve attitude stability of each quadrotor. The specific form of the attitude controller is:

[0177] (19)

[0178] In the formula, and are positive constant control gain matrices, where are the attitude control parameters of pitch angle, roll angle and yaw angle, They are the pitch angular velocity, roll angular velocity and yaw angular velocity control parameters respectively;

[0179] S340: Combining the rotational dynamics and actuator fault models for the third case, the rotational dynamics model with actuator deviation fault is rewritten as:

[0180] (20)

[0181] In the formula, is a disturbance, where is the second to fourth dimension data of the actuator deviation, is external time-varying interference;

[0182] S350: Design a high-order sliding mode observer to estimate and compensate for lumped disturbances, which is of the form:

[0183] (twenty one)

[0184] In the formula, is the estimated error of the attitude, and are the estimates of the angular velocity and the lumped disturbance, respectively, is an auxiliary parameter to ensure It is second-order differentiable, All are normal numbers;

[0185] S360: The robust controller design of the attitude loop is:

[0186] (twenty two)

[0187] In the formula, is a low-pass filter, where is the filter cutoff frequency.

[0188] In one embodiment, S400 includes:

[0189] S410: According to the complete dynamics model (7), the robust controller of the position subsystem (13) and the robust control of the attitude subsystem (22), the complete dynamics model is rewritten as:

[0190] (twenty three)

[0191] In the formula, is the lumped disturbance term;

[0192] S420: The lift coefficient of each UAV is estimated online through real-time information, and the actuator fault is diagnosed. The dynamics (23) is rewritten as:

[0193] (twenty four)

[0194] In the formula , The specific form is as follows:

[0195] (25)

[0196] S430: Define the objective function as follows:

[0197] (26)

[0198] about The first and second order derivatives are:

[0199] (27)

[0200] In the formula, is the number of iterations;

[0201] S440: Judgment Is it established? is the allowable error. If it holds, stop the calculation. If it does not hold, let the gradient direction be:

[0202] (28)

[0203] In the formula, ,in is a suitable positive constant, which is used to ensure is positive definite; therefore, the specific form of the gradient direction is:

[0204] (29)

[0205] S450: From Departure, along the direction Do a one-dimensional search to find the corresponding learning step size make The fastest descent, one-dimensional search method is:

[0206] (30)

[0207] S460: Order , and iterate formulas (27)-(30) until , and get the corresponding The value can be obtained, so that the lift coefficient of each rotor actuator partial failure can be obtained, and the control efficiency distribution matrix after the actuator partial failure can be obtained as the matrix , finally, the expected speed of each motor of each quadrotor is:

[0208] (31).

[0209] Here, in order to better illustrate the effectiveness of the method of the present invention for the formation control of quadrotor drones under external disturbances and actuator failures, Figure 4-Figure 9 Here is a simulation description:

[0210] For the convenience of explanation, all the data appearing below are in international units by default. In the simulation, the present invention considers a group of four actual quad-rotor drones Quadrotor1-Quadrotor4 and a virtual leader Virtual leader to form an unmanned cluster system, whose directed network topology is as follows Figure 3 As shown. Among them: the mass of each quadcopter model is taken as mi=1, the acceleration of gravity is 9.8, and the moment of inertia ; The initial position of each quadrotor drone is set to: ; The initial linear velocity and angular velocity are zero; The desired formation of the system is determined by the relative displacement of the actual quadrotor drone and the virtual navigator quadrotor drone , , , It is determined that the reference linear speed of the virtual navigator is: ; The desired yaw angle is: , assuming that each UAV is subject to a time-varying disturbance , , the control parameters of the position loop nominal controller are set as: , the control parameters of the attitude loop nominal controller are set to The parameters of the high-order sliding mode observers for the position loop and attitude loop are set to , Newton's correction method is set , , assuming that the second and third actuators of quadrotor No. 1, the fourth actuator of quadrotor No. 3, and the third actuator of quadrotor No. 4 fail during flight, and the loss rate of lift coefficient is set to 50%, that is, , the lift coefficient of each quadrotor drone is selected as , then the corresponding actuator partial failure fault estimation error is defined as: The corresponding simulation results under these parameter configurations are as follows Figure 4-9 shown. Figure 4 The three-dimensional trajectory formed by the entire formation is shown. Figure 5 Shows the position formation error of four quadrotor drones in the simulation. Figure 6 The figure shows the attitude formation error of the quadrotor unmanned swarm system in the simulation. It can be seen that the attitude formation error converges quickly to near zero. Figure 7 It is the error of each quadrotor drone subjected to disturbance estimated by the high-order sliding mode observer. It can be seen that the observer error can quickly converge to near zero, indicating that the observer can quickly and accurately estimate deviation faults and time-varying disturbances. Figure 8 The error in the estimation of the partial failure of the actuator by the fault-tolerant method is shown. It can be seen that the size of the failure can be estimated quickly and accurately after the failure occurs. Fig. 9 The thrust and torque inputs of four quadrotor drones are shown. The simulation results show that the anti-disturbance formation fault-tolerant control algorithm of the present invention has good control performance.

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

[0212] (1) A dynamic model of a quadrotor swarm with time-varying disturbances, deviation faults, and partial actuator failures is established, which provides a theoretical basis for designing a formation control algorithm with both anti-disturbance and fault-tolerant capabilities.

[0213] (2) A high-order sliding mode observer is introduced in the position loop and attitude loop to estimate unknown time-varying disturbances and actuator deviation failures, thereby improving the robustness and safety of the unmanned swarm system.

[0214] (3) A fault-tolerant control strategy based on the Newton correction method is proposed to diagnose actuator failures in real time online, which enables the system to have good fault tolerance and ensures the safe and stable operation of the entire system.

[0215] The above is a detailed introduction to a fault-tolerant formation control method for a quad-rotor drone with anti-interference capability provided by the present invention. This article uses specific examples to illustrate the principles and implementation methods of the present invention. The description of the above embodiments is only used to help understand the core idea of ​​the present invention. It should be pointed out that for ordinary technicians in this technical field, without departing from the principles of the present invention, several improvements and modifications can be made to the present invention, and these improvements and modifications also fall within the scope of protection of the claims of the present invention.

Claims

1. A fault-tolerant formation control method for a quad-rotor drone with anti-interference capability, characterized in that: The method comprises the following steps: S100: Establish a UAV cluster system consisting of multiple quad-rotor UAVs in three-dimensional space, and establish translational dynamics, rotational dynamics, motor dynamics and actuator fault models for each UAV, and then establish a complete dynamic model of the quad-rotor cluster. The control goal of the complete dynamic model is to design the thrust and torque so that each quad-rotor UAV meets the preset conditions; S200: Design a nominal formation controller. Rewrite the translational dynamics model with deviation faults according to the translational dynamics and actuator fault model. Combined with the high-order sliding mode observer to estimate and compensate for the actuator deviation fault and external time-varying disturbances, design a robust controller for the position loop, and then obtain the static thrust of each quadrotor drone, so that each quadrotor drone can track the trajectory of the virtual navigator. When the fault in the actuator fault model is a mixed fault, that is, the actuator has both partial failure fault and deviation fault, combined with translational kinematics, the translational kinematics model with deviation fault is rewritten as: （11） In the formula, is a constant gravitational acceleration, is a constant vector, represents the total thrust on the drone, Indicates the number of the quadrotor drone in the unmanned swarm system. is the lumped disturbance, including the effect of actuator deviation fault and external time-varying disturbance, is the external disturbance of the translational dynamics of each quadrotor drone, where They are the disturbances to which each quadrotor drone is subjected in the x-axis, y-axis, and z-axis directions, including air resistance, time-varying wind disturbance, and other unmodeled parts; S300: Decouple the position loop and attitude loop to obtain the desired attitude of each quadrotor drone, design an attitude controller for the nominal system, rewrite the rotational dynamics model with deviation fault according to the rotational dynamics and actuator fault model, and combine the high-order sliding mode observer to solve the influence of actuator deviation fault and external time-varying disturbance on the attitude loop, design a robust controller of the attitude loop, so as to achieve the attitude stability of each quadrotor drone; When the fault in the actuator fault model is a mixed fault, that is, the actuator has both partial failure fault and deviation fault, combined with rotational dynamics, the rotational dynamics model with actuator deviation fault is rewritten as: （20） In the formula, is the inertia matrix, where are the moments of inertia of each quadrotor drone around the x-axis, y-axis, and z-axis of the body coordinate system, represents the torque, where They are the torques of each quadrotor drone in the x-axis, y-axis and z-axis directions, represents the angular velocity in the body coordinate system, where are the pitch velocity, roll velocity and yaw velocity of each quadrotor drone, is a disturbance, where is the second to fourth dimension data of the actuator deviation, represents the disturbances suffered by the attitude subsystem, including the unmodeled part and external wind disturbance, where They are the disturbances that each quadrotor drone experiences when pitching, rolling, and yawing; S400: Based on the complete dynamics model, the robust controller of the position loop and the robust controller of the attitude loop, the complete dynamics model is rewritten, the lift coefficient of each rotor after the actuator failure is calculated based on the Newton correction method to achieve online fault diagnosis, and the speed of each motor is calculated according to the control efficiency matrix after the failure, so as to achieve safe and reliable formation control of the quadrotor drone after the actuator failure; Among them, according to the complete dynamics model, the robust controller of the position loop and the robust controller of the attitude loop, the complete dynamics model is rewritten as: （23） In the formula, ,in is the lift failure coefficient of each motor, is the status of each quadrotor drone in the unmanned swarm, , ,in ,in, represents the mass of each quadrotor drone, is the sine operator, is the cosine operator, is the speed of each motor, is the speed of motors 1, 2, 3, and 4 of each quadrotor drone. is the control effectiveness matrix, is the lumped disturbance term.

2. The method according to claim 1, characterized in that Establishing the inertial coordinate system I and the body coordinate system B, S100 includes: S110: According to Newton's second law, the translational dynamics equation of each quadrotor subsystem is: (1) In the formula, Indicates the number of the quadrotor drone in the unmanned swarm system. and Respectively represent the position and linear velocity of each quadrotor drone in the inertial coordinate system I, where They are the positions of each quadrotor drone in the x-axis, y-axis and z-axis directions, are the speeds of each quadrotor drone in the x-axis, y-axis and z-axis directions, is a rotation matrix defined by Euler angles, representing the transformation from body coordinates to world coordinates. is a constant gravitational acceleration, is a constant vector, represents the mass of each quadrotor drone, is the force in the body coordinate system, represents the total thrust on the drone, is the external disturbance of the translational dynamics of each quadrotor drone, where They are the disturbances to which each quadrotor drone is subjected in the x-axis, y-axis, and z-axis directions, including air resistance, time-varying wind disturbance, and other unmodeled parts; S120: According to the Euler equation, the rotational dynamics equation of the attitude subsystem is written as: （2） In the formula, are the Euler angles, where They are the pitch angle, roll angle and yaw angle of each quadrotor drone. represents the angular velocity in the body coordinate system, where are the pitch velocity, roll velocity and yaw velocity of each quadrotor drone, is the inertia matrix, where are the moments of inertia of each quadrotor drone around the x-axis, y-axis, and z-axis of the body coordinate system, represents the torque, where They are the torques of each quadrotor drone in the x-axis, y-axis and z-axis directions, represents the disturbances suffered by the attitude subsystem, including the unmodeled part and external wind disturbance, where They are the disturbances that each quadrotor drone experiences when pitching, rolling, and yawing; S130: Based on the relationship between the lift, rolling moment, pitch moment and yaw moment generated by the four rotors of the quad-rotor drone and the motor speed, the control input model of each quad-rotor drone, i.e., the motor dynamics equation, is: （3） In the formula, is the control input vector, is the speed of each motor, where is the speed of motors 1, 2, 3, and 4 of each quadrotor drone. is the control effectiveness matrix, and its specific form is: （4） In the formula, is the lift coefficient of each motor of each drone, is the reaction torque coefficient of each UAV, is the lift-to-drag ratio coefficient; S140: Establish the actuator fault model as follows: （5） In the formula, ,in is the lift failure coefficient of each motor; represents the control input of the nominal system under ideal conditions, It is an unknown actuator deviation fault; Combining equations (3) and (5), the actuator fault model is rewritten as: （6） S150: Combining translational dynamics (1), rotational dynamics (2), motor dynamics (3) and actuator failure model (6), the complete dynamic model of the quadrotor cluster is expressed as: （7） In the formula, is the status of each quadrotor drone in the unmanned swarm, , in ,in, is the sine operator, is the cosine operator, .

3. The method according to claim 2, characterized in that In formula (5), according to and Taking different values, actuator faults are divided into the following types: (1) When and When , it is a partial failure of the actuator, also called a multiplicative failure; (2) When and When , it is an actuator deviation fault, also called additive fault; (3) When and When , the actuator has both partial failure and deviation fault, which is called mixed fault; (4) When and There is no actuator fault.

4. The method according to claim 3, characterized in that The preset conditions in S100 are specifically: （8） In the formula, It is the number of the quadcopter drone, which is similar to the drone Adjacent, It is The location of the drone, Indicates drone and The relative positions between them determine the required formation mode. is the time-varying trajectory of the virtual navigator, yes The first-order differential of represents the time-varying speed of the virtual navigator, which is also the reference line speed of the formation.

5. The method according to claim 4, characterized in that S200 includes: S210: Design a nominal formation controller based on the consistency control protocol of the second-order system to achieve translational motion under ideal conditions. The controller form is: （9） In the formula, and is a positive constant control parameter matrix, where They are the position control parameters in the x-axis, y-axis and z-axis directions, They are the speed control parameters in the x-axis, y-axis and z-axis directions respectively. and They are and The first-order differential of yes The second-order differential of Is able to The dynamic neighborhood set of UAVs where four quadrotor UAVs interact with each other. It is The communication topology value between the quadrotor drone and the virtual navigator is If you can communicate with the pilot The value is 1, otherwise it is 0. is the adjacency matrix The elements in The value of is given by: （10） S220: Design of high-order sliding mode observers to estimate and compensate for lumped disturbances , its specific form is: (12) In the formula, is the position estimation error of each quadrotor, and are the estimated values ​​of the speed of each quadrotor and the lumped disturbance, It is an auxiliary parameter to ensure Second order differentiable, is a positive constant; S230: Design the robust controller of the position loop as: （13） In the formula, is a low-pass filter, where is the cutoff frequency of the filter; S240: The complete controller for the position loop is: （14） S250: The static thrust input for each quadrotor drone is as follows: (15) In the formula yes The third dimension of the vector.

6. The method according to claim 5, characterized in that S300 includes: S310: Decouple the position subsystem and attitude subsystem and select any yaw angle , the desired roll angle and pitch angle It can be obtained by the following formula: (16) In the formula, and The controllers are The first and second dimension data of S320: Define the reference posture of each quadrotor drone as: （17） The attitude error is designed as: （18） In the formula, They are pitch, roll and yaw angle errors respectively; S330: Design an ideal nominal attitude controller for the attitude subsystem to achieve attitude stability of each quadrotor. The specific form of the attitude controller is: （19） In the formula, and are positive constant control gain matrices, where are the attitude control parameters of pitch angle, roll angle and yaw angle, They are the pitch angular velocity, roll angular velocity and yaw angular velocity control parameters respectively; S340: Design a high-order sliding mode observer to estimate and compensate for lumped disturbances, which is of the form: (21) In the formula, is the estimated error of the attitude, and are the estimates of the angular velocity and the lumped disturbance, respectively, is an auxiliary parameter to ensure It is second-order differentiable, All are normal numbers; S350: The robust controller design of the attitude loop is: （22） In the formula, is a low-pass filter, where is the filter cutoff frequency.

7. The method according to claim 6, characterized in that S400 includes: S410: The lift coefficient of each UAV is estimated online through real-time information, and the actuator fault is diagnosed. The dynamics (23) is rewritten as: （24） In the formula , The specific form is as follows: （25） S420: Define the objective function as follows: （26） about The first and second order derivatives are: （27） In the formula, is the number of iterations; S430: Judgment Is it established? is the allowable error. If it holds, stop the calculation. If it does not hold, let the gradient direction be: （28） In the formula, ,in is a suitable positive constant, which is used to ensure is positive definite; therefore, the specific form of the gradient direction is: （29） S440: From Departure, along the direction Do a one-dimensional search to find the corresponding learning step size make The fastest descent, one-dimensional search method is: （30） S450: Order , and iterate formulas (27)-(30) until , and get the corresponding The value can be obtained, so that the lift coefficient of each rotor actuator partial failure can be obtained, and the control efficiency distribution matrix after the actuator partial failure can be obtained as the matrix , finally, the expected speed of each motor of each quadrotor is: （31）。

Images