Multi-aircraft formation anti-collision method based on distributed model predictive control and formation control

Abstract

The present invention relates to the technical field of formation control of fixed-wing multi-UAV systems, and particularly relates to a multi-aircraft formation anti-collision method based on distributed model predictive control and formation control, which solves the problem of how to enable a UAV swarm to have stable and high-precision cooperative formation flight control capabilities. The special feature of this method is as follows: Step 1: Assemble the UAV swarm, input the information of the assembly point or waypoint, and perform formation anti-collision settings based on distributed model predictive control; Step 2: Cruise control; during cruising, if the formation needs to be reconstructed, execute Step 3; if the formation does not need to be reconstructed, execute Step 4; Step 3: Based on the requirements of the reconstructed formation, perform formation anti-collision settings based on distributed model predictive control again; Step 4: Perform formation maintenance control; Step 5: Determine whether the target point has been reached or the task has been completed; if so, end the task; if not, return to Step 2 and loop through Steps 2 to 5 until the target point is reached or the task is completed.

Description

Technical Field

[0001] The present invention relates to the technical field of formation control of fixed-wing multi-UAV systems, and particularly relates to a multi-aircraft formation anti-collision method based on distributed model predictive control and formation control. Background Art

[0002] Nowadays, with the wide application of UAVs in the fields of military, agriculture, science and technology, etc., more in-depth research has been carried out on the related technologies of UAVs. Due to the characteristics of long endurance and high speed of UAVs, they can be used for tasks such as reconnaissance and mapping in military applications; in civilian applications, they can be used for the collection of meteorology and geology, and the most commonly used is the aerial photography task. The characteristics of UAVs with multiple uses make them gradually become the research content valued by scholars in the aviation field.

[0003] UAVs can be classified into several categories according to their different structures. Among them, the applications and research related to fixed-wing UAVs have received the key attention of scholars in various countries. On the one hand, this is because fixed-wing UAVs have the characteristics of simple structure, long endurance and strong maneuverability; on the other hand, because fixed-wing UAVs have a long development time, there is a richer theory as a support, and there is a very abundant knowledge that can be referred to and learned from. Fixed-wing UAVs have a variety of application fields. In order to give full play to the role of a single UAV as much as possible, realize the control, decision-making and management of multi-UAV coordinated formation flight, so as to improve the efficiency of UAVs to complete tasks, especially in the execution of search, rescue, mapping, etc., to be able to complete tasks more effectively, broaden the scope of use of UAVs, and achieve the purpose of safely and highly reliably executing various tasks, a core problem in the field of multi-UAV systems is the motion planning problem among multiple intelligent agents. Only by reasonably planning the motion among multiple intelligent agents can the UAV cluster have a stable cruise control ability, accurate formation control ability and reliable formation anti-collision ability when performing tasks. In this way, when multiple UAVs are combined to complete a specified task, the limited single UAVs can be combined to complete the task, and multiple low-cost UAVs can achieve complementary functions through the method of clustering, which can avoid resource waste and improve efficiency.

[0004] Therefore, how to realize a multi-UAV system with formation cooperative control, obstacle avoidance and cruise formation control capabilities, so that the UAV swarm has a stable and highly accurate cooperative formation flight control ability, is an urgent problem to be solved in our research. Starting from this problem, the present invention strives to make breakthroughs and innovations in the theory and application of cooperative control research. Summary of the Invention

[0005] The object of the present invention is to provide a multi-aircraft formation anti-collision method based on distributed model predictive control and formation control, so as to solve the technical problem of how to implement a multi-UAV system with formation cooperative control, obstacle avoidance and cruise formation control capabilities, and enable the UAV swarm to have a stable and highly accurate cooperative formation flight control ability.

[0006] The technical solution adopted by the present invention is that the multi-aircraft formation anti-collision method based on distributed model predictive control and formation control is characterized in that it includes the following steps:

[0007] Step 1: First, based on the mission planning information and UAV platform factors, assemble the UAV swarm, input the information of the assembly point or the waypoint, and then perform formation anti-collision settings based on distributed model predictive control;

[0008] Step 2: Perform cruise control based on the PID algorithm; during the cruise process, if the formation needs to be reconstructed, execute the following Step 3; if the formation does not need to be reconstructed, skip the following Step 3 and execute the following Step 4;

[0009] Step 3: Based on the requirements of reconstructing the formation described in Step 2, perform formation anti-collision settings based on distributed model predictive control again;

[0010] Step 4: Perform formation maintenance control based on Leader-Follower;

[0011] Step 5: Determine whether the target point has been reached or the task has been completed; if so, end the task; if not, return to the above Step 2 and loop through Steps 2 to 5 until the target point is reached or the task is completed, and then end the task.

[0012] Furthermore, the control process of performing cruise control based on the PID algorithm described in Step 2 includes the following steps:

[0013] Step 1: The system designs a cooperative flight control law using the second-order consensus algorithm The represents the flight acceleration of UAV i;

[0014] Step 1.1: The system designs using the second-order consensus algorithm The respectively represent the three component accelerations of UAV i in the x, y, and z directions in the ground coordinate system; specifically:

[0015] The system uses the second-order consensus algorithm to design the following for the three position and acceleration channels of UAV i respectively as shown in Equation (1.1):

[0016]

[0017] In Equation (1.1): Ni Denote the neighbors of UAV \(i\) in the distributed network; \(a\) ij Denote the weighted coefficient considering the calculation between different UAVs; \(x\) diF , \(y\) diF , \(z\) diF Respectively represent the \(x\)-coordinate, \(y\)-coordinate, and \(z\)-coordinate of the reference point of the \(i\)-th UAV; \(x\) djF , \(y\) djF , \(z\) djF Respectively represent the \(x\)-coordinate, \(y\)-coordinate, and \(z\)-coordinate of the reference point of the \(j\)-th UAV; \(\gamma\) represents the weight coefficient of the state quantity, usually greater than 0; Respectively represent the three relative velocity components of UAV \(i\) relative to UAV \(j\) along the \(x\), \(y\), and \(z\) directions in the ground coordinate system;

[0018] The motion speed of UAV \(i\) relative to UAV \(j\) Specifically, it is shown in the following formula (1.2):

[0019]

[0020] In formula (1.2): \(V\) xi , \(V\) yi , \(V\) zi Respectively represent the velocity components of the velocity \(V\) of UAV \(i\) i along the \(x\), \(y\), and \(z\) directions in the ground coordinate system; \(V\) xj , \(V\) yj , \(V\) zj Respectively represent the velocity components of the velocity \(V\) of UAV \(j\) j along the \(x\), \(y\), and \(z\) directions in the ground coordinate system;

[0021] The position coordination variable \(\rho\) iF is different at the beginning of the formation. As time goes on, when \(t o \infty\), \(\rho\) iF \( o \rho\) jF . The above formula (1.1) gives the three component acceleration commands in the ground coordinate system, and controlling can achieve the formation shape maintenance.

[0022] Step 1.2: Define the cooperative flight control rate As shown in the following formula (1.3):

[0023]

[0024] The second-order consensus algorithm control includes both position information and velocity information.

[0025] Step 2: Convert the cooperative flight control rate designed in Step 1 into flight control commands The represents the magnitude of the acceleration of the flight of UAV i; the represents the pitch angular velocity; the represents the yaw angular velocity; specifically:

[0026] The control method is shown as the following formula (1.4):

[0027]

[0028] According to the relationship between the pitch angle and the velocity component as shown in the following formula (1.5), and the relationship between the yaw angle and the velocity component as shown in the following formula (1.6):

[0029]

[0030] In formula (1.5), θ i represents the pitch angle of UAV i;

[0031]

[0032] In formula (1.6), Ψ i represents the yaw angle of UAV i;

[0033] Then

[0034]

[0035]

[0036] Step 3: Define a new cooperative flight control command v i ;

[0037] The new cooperative flight control command v i is shown as the following formula (1.9):

[0038]

[0039] In formula (1.9), represents the new magnitude of the acceleration of the flight of UAV i, represents the new pitch angular velocity of the flight of UAV i, represents the new yaw angular velocity of the flight of UAV i;

[0040] The specific form is shown as the following formula (1.10):

[0041]

[0042] In formula (1.10), represents the average velocity;

[0043] The specific form is shown in the following formula (1.11):

[0044]

[0045] In formula (1.11), represents the track reference pitch angle;

[0046] The specific form is shown in the following formula (1.12):

[0047]

[0048] In formula (1.12), represents the track reference yaw angle;

[0049] Step 4: Taking the new cooperative flight control instruction v i defined in Step 3 as the input, calculate the error between the actual trajectory and the reference trajectory of the UAV. Based on the PID algorithm, the trajectory controller designs the PID control law u i ; specifically:

[0050] The PID control law is designed by dividing it into longitudinal and lateral channels. Assuming that the coupling between the longitudinal and lateral channels of the UAV is small, the design of the control augmentation system is usually carried out separately for the longitudinal and lateral directions without cross-linking. The PID control law u i is designed according to the following formula (1.13):

[0051]

[0052] In formula (1.13), K p , K I , K D are the proportional, integral, and differential coefficient matrices respectively, represents the deviation.

[0053] Step 5: Based on the PID control law designed in Step 4, adjust the control instruction input value of the cooperative flight control system, thereby controlling the flight trajectory and attitude of the UAV.

[0054] Furthermore, the specific steps of the formation keeping control based on Leader-Follower described in Step 4 are as follows:

[0055] Step A: Establish a relative motion relationship model between the leader aircraft and the follower aircraft, and obtain the state variables of the leader aircraft and the follower aircraft system based on this relative motion relationship model; specifically:

[0056] The dynamic equation of the UAV centroid motion in the track coordinate system is shown as the following formula (2.1):

[0057]

[0058] In formula (2.1): represents the acceleration; represents the track tilt angular velocity; represents the track azimuth angular velocity; T represents the engine thrust; D represents the resistance force received by the UAV; m represents the mass of the UAV and its payload; g represents the acceleration due to gravity; μ represents the track tilt angle; L represents the lift force received by the UAV; C represents the side force received by the UAV; represents the track azimuth angle; φ represents the roll angle; V represents the UAV speed;

[0059] In formula (2.1), L, D, and C are calculated according to the following formula (2.2):

[0060]

[0061] In formula (2.2): C L represents the lift coefficient; ρ represents the atmospheric density; S represents the wing area; C Lα represents the derivative of the lift coefficient with respect to the angle of attack; α represents the UAV angle of attack; α0 represents the initial angle of attack; C D0 represents the drag coefficient; K represents the transfer coefficient; C Yβ represents the lateral aerodynamic force coefficient; β represents the UAV sideslip angle;

[0062] According to the above formula (2.1), the distance of the lead aircraft relative to the wingman is converted into the wingman coordinate system, and the relative motion relationship model between the lead aircraft and the wingman is established, as shown in the following formula (2.3):

[0063]

[0064] In formula (2.3): x d represents the component of the relative distance of the leader in the follower coordinate system on the x-axis; y d represents the component of the relative distance of the leader in the follower coordinate system on the y-axis; z d represents the component of the relative distance of the leader in the follower coordinate system on the z-axis; L w represents the wingman lift; C w represents the wingman side force; m w represents the wingman mass; V w represents the speed of the follower in the ground coordinate system; μ w represents the track tilt angle of the follower in the ground coordinate system; φw represents the roll angle of the follower itself; V L represents the velocity of the leader in the ground coordinate system; μ L represents the track inclination angle of the leader in the ground coordinate system; represents the relative azimuth error between the leader and the follower;

[0065] The lift L, drag D, and side force C acting on the UAV are functions of the UAV velocity V, angle of attack α, and sideslip angle β. Therefore, the state variables of the system are shown in the following equation (2.4):

[0066]

[0067] In equation (2.4): represents the track azimuth angle of the wingman; represents the track azimuth angle of the lead aircraft;

[0068] Step B: Based on the state variables of the system obtained in Step A, design a formation control law based on Leader-Follower;

[0069] Step B.1: Set x c , y c , z c as the distance that the wingman in the UAV formation should maintain from the lead aircraft as required; Based on the state variables of the system obtained in Step A, obtain the error vector; The error terms in the error vector include position error, velocity error, track inclination angle error, track azimuth angle error, and track roll angle error; The error vector is shown in the following equation (2.5):

[0070]

[0071] Step B.2: Based on the ultimate goal of formation control being to make the target errors e x , e y and e z be zero, and assuming the sideslip angle β is zero, based on the control quantities of the lead aircraft and the wingman in the formation shown in the following equation (2.6) and the error vector shown in equation (2.5) in Step B.1, give the control law for the control variables of the wingman as shown in the following equation (2.7), and then the control quantity of the formation is shown in the following equation (2.8);

[0072] The control quantities of the lead aircraft and the wingman in the formation are shown in the following equation (2.6):

[0073]

[0074] The control law for the control variables of the wingman is shown in the following equation (2.7):

[0075]

[0076] In Equation (2.7): K ZP and K ZI are the proportional and integral coefficients for altitude control respectively; K μP and K μI are the proportional and integral coefficients for track tilt angle control respectively; K XP and K XI are the proportional and integral coefficients for longitude control respectively; K VP and K VI are the proportional and integral coefficients for speed control respectively; K YP and K YI are the proportional and integral coefficients for dimension control respectively; K φP and K φI are the proportional and integral coefficients for roll angle control respectively;

[0077] The control quantity of the formation is shown in the following Equation (2.8):

[0078] U c = [α T φ] T (2.8).

[0079] Furthermore, the design process of the formation collision avoidance setting based on distributed model predictive control described in Step 1 and Step 3 includes the following steps:

[0080] Step a: Establish a formation prediction model;

[0081] Step b: According to the formation prediction model established in Step a, represent all the surrounding UAVs that may collide in a mathematical form as constraint penalty terms;

[0082] Step c: Use the penalty terms described in Step b to construct an optimization function to minimize the overall penalty when the UAVs meet the requirements, and then solve for the control quantity;

[0083] Step d: Resolve the control quantity obtained in Step c into trajectory control, and the formation collision avoidance setting is completed.

[0084] Furthermore, the specific establishment process of Step a: Establish a formation prediction model is as follows:

[0085] Use a second-order integral dynamics model to represent the UAV, and its discrete dynamics equation is shown in the following Equation (3.1):

[0086]

[0087] In Equation (3.1): p i [k + 1], v i[k + 1] represent the position and velocity at the (k + 1)-th moment; p i [k], v i [k], a i [k] represent the position, velocity, and acceleration at the k-th moment; h represents the unit time parameter;

[0088] The state of the UAV is expressed as the following formula (3.2):

[0089]

[0090] In formula (3.2), represents the predicted information of the position at moment k t + k; represents the predicted information of the velocity at moment k t + k; k t represents any moment;

[0091] Using formula (3.2), the position and velocity can be expressed as an overall state, and the prediction model step equation for K steps is shown as the following formula (3.3):

[0092] P i = A0X 0，i + ΛU i (3.3);

[0093] In formula (3.3): Λ is defined as the following formula (3.4):

[0094]

[0095] In formulas (3.3) and (3.4): Ψ = [I3 03], A0 = [(ΨA) T +(ΨA 2 ) T ...(ΨA K ) T T; X 0，i represents the initial position; P i represents the position prediction sequence for K steps; U i represents the input sequence;

[0096] The penalty term in step b includes the desired trajectory error penalty term, control cost penalty term, control input change penalty term, and collision constraint penalty term; the mathematical forms of the penalty terms are as follows respectively:

[0097] 2) Desired trajectory error penalty term

[0098]

[0099] The error two-norm between the trajectory from the starting moment to the K moment and the desired trajectory in the entire prediction step is represented by the above formula (3.5). Transforming formula (3.5) into the quadratic programming form is shown in the following formula (3.6):

[0100]

[0101] In formula (3.6): J e,i represents the desired trajectory error penalty term; P d,i represents the destination; represents a positive definite diagonal matrix the weight coefficient of the error for each step;

[0102] 2) Control cost penalty term:

[0103]

[0104] In formula (3.7): represents the coefficient of the penalty term; R represents the penalty coefficient of the control cost;

[0105] 3) Control input change penalty term:

[0106]

[0107] In formula (3.8):

[0108]

[0109]

[0110]

[0111] S represents the error penalty coefficient of the control change;

[0112] 4) Collision constraint penalty term

[0113] The addition of the collision constraint term allows the UAV to avoid collisions in a timely manner when there is a risk of collision:

[0114]

[0115] In formula (3.9):

[0116]

[0117]

[0118] Step c: Using the penalty terms described in step b to construct an optimization function to minimize the overall penalty of the UAV under the satisfied requirements, and then the specific construction process for solving the control amount is as follows:

[0119]

[0120] In Equation (3.10), U i represents the control quantity; A in , b in represent the inequality constraints of the control vector.

[0121] The beneficial effects of the present invention are as follows:

[0122] (1) In the present invention, cruise control is based on the PID algorithm, formation anti-collision is set based on distributed model predictive control, and formation keeping control is based on Leader-Follower. Model predictive control, as a modern control method, is a control method with simple calculation, strong robustness, good interference suppression ability, and high control accuracy. Using it as a basic control method can improve the control efficiency of the UAV formation. However, the real-time problem of its rolling optimization has become a bottleneck in the practical application of predictive control. For the UAV formation control problem, the present invention establishes a relative motion equation, and then designs a formation controller using the method of multi-model based predictive control, transforming the non-linear rolling optimization problem into a linear quadratic optimization problem, improving the real-time performance of non-linear prediction. At the same time, formation control is a multi-aircraft motion planning problem. The UAV motion route calculated only by the consensus algorithm does not consider anti-collision. The present invention combines the consensus algorithm with the distributed model predictive control algorithm. First, cruise control is designed through the consensus algorithm, and then the distributed model predictive control algorithm is used. All surrounding UAVs that may collide are expressed in a mathematical form as constraint constraints, so as to solve the UAV motion planning solution that meets multi-objective requirements, and then control the entire formation. The entire framework is a complete motion planning process, flying out the real state by the fixed-wing autopilot, and then the state is fed back to the consensus and distributed model predictive control algorithms for real-time planning to improve the control efficiency. Therefore, the present invention solves the technical problem of how to realize a multi-UAV system with formation cooperative control, obstacle avoidance, and cruise formation control capabilities, enabling the UAV swarm to have stable and high-precision cooperative formation flight control capabilities.

[0123] (2) Using the multi-aircraft formation anti-collision method based on distributed model predictive control and formation control of the present invention for formation control can improve the stability and control accuracy of multi-aircraft cooperative formation cruise control, formation control, and formation anti-collision. Description of the Drawings

[0124] Figure 1 is the overall system block diagram of the embodiment of the present invention;

[0125] Figure 2 is the schematic diagram of the control process of cruise control in the embodiment of the present invention;

[0126] Figure 3 It is the PID algorithm diagram during cruise control in the embodiments of the present invention;

[0127] Figure 4 It is a schematic diagram of the control process for formation keeping control based on Leader-Follower in the embodiments of the present invention;

[0128] Figure 5 It is a schematic diagram of the relationship between the position vectors of the leader Leader and the follower Follower in the ground coordinate system;

[0129] Figure 6 It is a schematic diagram of the framework for combining the consensus algorithm Consensus and the distributed model predictive control algorithm DMPC for control in the embodiments of the present invention. Detailed implementation manners

[0130] The present invention will be described in detail below with reference to the accompanying drawings and specific implementation manners.

[0131] See Figure 1 , the multi-aircraft formation anti-collision method based on distributed model predictive control and formation control of the present invention includes the following steps:

[0132] Step 1: First, based on the mission planning information and UAV platform factors, assemble the UAV swarm, input the information of the assembly point or waypoint, and then perform the formation anti-collision setting based on distributed model predictive control;

[0133] Step 2: Perform cruise control based on the PID algorithm; during the cruise process, if formation reconstruction is required, execute the following Step 3; if formation reconstruction is not required, skip the following Step 3 and execute the following Step 4;

[0134] Step 3: Based on the requirements of formation reconstruction in Step 2, perform the formation anti-collision setting based on distributed model predictive control again;

[0135] Step 4: Perform formation keeping control based on Leader-Follower;

[0136] Step 5: Determine whether the target point is reached or the task is completed; if so, end the task; if not, return to the above Step 2 and loop through Steps 2 to 5 until the target point is reached or the task is completed, and then end the task.

[0137] From the above task description, it can be seen that the execution process of the multi-aircraft formation anti-collision method based on distributed model predictive control and formation control of the present invention includes three aspects of control problems: cruise control, formation control, and formation anti-collision.

[0138] [1] Cruise control

[0139] See Figure 2 , the multi-UAV cruise control includes two parts, a cooperative flight control system and a cooperative trajectory control system. The cooperative flight control system is the inner loop, which controls the flight attitude; the cooperative trajectory control system is the outer loop, which controls the flight trajectory, and the output of the outer loop is used as the input of the inner loop. The cooperative trajectory control system calculates the corresponding trajectory commands such as pitch angle, yaw angle, and speed according to the predetermined route, and then transmits them to the cooperative flight control system. After receiving the commands, the flight control system calculates and controls the deflection of the control surface to track the trajectory control commands. The PID control law is designed separately for the longitudinal and lateral channels. Assuming that the coupling between the longitudinal and lateral channels of the UAV is small, the design of the control augmentation system is usually carried out separately for the longitudinal and lateral directions without cross-linking. In this form, the inner loop composed of the UAV model, the flight control system, and the servo is stable.

[0140] In this embodiment, the control process of the cruise control based on the PID algorithm in the above step two includes the following steps:

[0141] Step 1: The system designs the cooperative flight control law using the second-order consensus algorithm The above represents the flight acceleration of UAV i;

[0142] Step 1.1: The system designs using the second-order consensus algorithm respectively represent the three component accelerations of UAV i along the x, y, and z directions in the ground coordinate system; specifically:

[0143] The system uses the second-order consensus algorithm to design the three position and acceleration channels of UAV i as shown in the following formula (1.1):

[0144]

[0145] In formula (1.1): N i represents the neighbors of UAV i in the distributed network; a ij represents the weighted coefficient considered in the calculation between different UAVs; x diF , y diF , z diF respectively represent the x coordinate, y coordinate, and z coordinate of the reference point of the i-th UAV; x djF , y djF , z djF respectively represent the x coordinate, y coordinate, and z coordinate of the reference point of the j-th UAV; γ represents the weight coefficient of the state quantity, usually greater than 0; respectively represent the three relative velocity components of UAV i relative to UAV j along the x, y, and z directions in the ground coordinate system;

[0146] The motion speed of UAV i relative to UAV j Specifically, it is shown as the following formula (1.2):

[0147]

[0148] In formula (1.2): V xi , V yi , V zi respectively represent the velocity components of the UAV i's velocity V i along the x, y, and z directions in the ground coordinate system; V xj , V yj , V zj respectively represent the velocity components of the UAV j's velocity V j along the x, y, and z directions in the ground coordinate system;

[0149] The position coordination variable ρ iF is different at the beginning of the formation. As time goes on, when t → ∞, ρ iF → ρ jF . The above formula (1.1) gives three component acceleration commands in the ground coordinate system, and controlling can achieve the formation keeping.

[0150] Step 1.2: Define the cooperative flight control law as shown in the following formula (1.3):

[0151]

[0152] The second-order consensus algorithm control includes both position information and velocity information.

[0153] Step 2: Convert the above cooperative flight control law designed in Step 1 into flight control commands The above represents the magnitude of the acceleration of the UAV i in flight; the above represents the pitch angular velocity; the above represents the yaw angular velocity; specifically:

[0154] The control method of the above is shown in the following formula (1.4):

[0155]

[0156] According to the relationship between the pitch angle and the velocity components as shown in the following formula (1.5), and the relationship between the yaw angle and the velocity components as shown in the following formula (1.6):

[0157]

[0158] In Equation (1.5), θ i represents the pitch angle of UAV i;

[0159]

[0160] In Equation (1.6), Ψ i represents the yaw angle of UAV i;

[0161] Then

[0162]

[0163]

[0164] Step 3: Define a new cooperative flight control command v i ;

[0165] The new cooperative flight control command v i is shown in the following Equation (1.9):

[0166]

[0167] In Equation (1.9), represents the magnitude of the new acceleration of UAV i in flight, represents the new pitch angular velocity of UAV i in flight, represents the new yaw angular velocity of UAV i in flight;

[0168] The specific form of the above is shown in the following Equation (1.10):

[0169]

[0170] In Equation (1.10), represents the average speed;

[0171] The specific form of the above is shown in the following Equation (1.11):

[0172]

[0173] In Equation (1.11), represents the track reference pitch angle;

[0174] The specific form of the above is shown in the following Equation (1.12):

[0175]

[0176] In Equation (1.12), represents the track reference yaw angle;

[0177] Step 4: Taking the above-mentioned newly defined cooperative flight control instruction v in Step 3 i as the input, calculate the error between the actual trajectory and the reference trajectory of the UAV. Refer to Figure 3 , and based on the PID algorithm, the trajectory controller designs the PID control law u on the premise that the above error meets the preset conditions i ; specifically:

[0178] The PID control law is designed by dividing it into longitudinal and lateral channels. Assuming that the coupling between the longitudinal and lateral channels of the UAV is small, the design of the control augmentation system is usually carried out separately for the longitudinal and lateral directions without cross-linking. The PID control law u i is designed according to the following formula (1.13):

[0179]

[0180] In formula (1.13), K p , K I , K D are the proportional, integral, and differential coefficient matrices respectively, and e yi represents the deviation.

[0181] Step 5: Based on the PID control law designed in Step 4, adjust the control instruction input value of the cooperative flight control system, so as to control the flight trajectory and attitude of the UAV.

[0182] [2] Formation control

[0183] Refer to Figure 4 , and the formation controller is the key for the UAV to perform formation flight. In the formation, the formation controller of the wingman uses various error signals to maintain the position of the wingman relative to the leader, while the formation controller of the leader is in an "idle" state and only takes effect when the role changes. Considering the engineering application, the present invention designs this formation controller according to the design principle of PID control and its application in the design of the formation controller, in combination with the relative motion model of the formation.

[0184] In formation flight, the leader leads the entire formation to fly towards the target point and is not responsible for maintaining the formation. The formation controller of the wingman maintains the formation according to the formation requirements. Therefore, the control process of the formation maintenance control based on Leader-Follower in the embodiment of the present invention is as Figure 4 shown.

[0185] In this embodiment, the specific steps of the formation maintenance control based on Leader-Follower in the above Step 4 are as follows:

[0186] Step A: Establish the relative motion relationship model between the leader and the wingman. Refer to Figure 5, the state variables of the lead aircraft and wingman system are obtained based on this relative motion relationship model; specifically:

[0187] The dynamic equation of the centroid motion of the UAV in the flight path coordinate system is shown in the following formula (2.1):

[0188]

[0189] In formula (2.1): represents the acceleration; represents the flight path tilt angular velocity; represents the flight path azimuth angular velocity; T represents the engine thrust; D represents the drag force on the UAV; m represents the mass of the UAV and its payload; g represents the acceleration due to gravity; μ represents the flight path tilt angle; L represents the lift force on the UAV; C represents the side force on the UAV; represents the flight path azimuth angle; φ represents the roll angle; V represents the UAV speed;

[0190] In formula (2.1), L, D, and C are calculated according to the following formula (2.2):

[0191]

[0192] In formula (2.2): C L represents the lift coefficient; ρ represents the atmospheric density; S represents the wing area; C Lα represents the derivative of the lift coefficient with respect to the angle of attack; α represents the angle of attack of the UAV; α0 represents the initial angle of attack; C D0 represents the drag coefficient; K represents the transfer coefficient; C Yβ represents the lateral aerodynamic force coefficient; β represents the sideslip angle of the UAV;

[0193] Based on the above formula (2.1), the distance of the lead aircraft relative to the wingman is converted into the wingman coordinate system, and the relative motion relationship model of the lead aircraft and the wingman is established, as shown in the following formula (2.3):

[0194]

[0195] In formula (2.3): x d represents the component of the relative distance of the leader in the follower coordinate system on the x-axis; y d represents the component of the relative distance of the leader in the follower coordinate system on the y-axis; z d represents the component of the relative distance of the leader in the follower coordinate system on the z-axis; L w represents the lift of the wingman; C w represents the side force of the wingman; m w represents the mass of the wingman; V wDenote the velocity of the follower in the ground coordinate system; μ w Denote the track inclination angle of the follower in the ground coordinate system; φ w Denote the roll angle of the follower itself; V L Denote the velocity of the leader in the ground coordinate system; μ L Denote the track inclination angle of the leader in the ground coordinate system; Denote the relative azimuth error between the leader and the follower;

[0196] The lift L, drag D, and side force C acting on the UAV are functions of the UAV velocity V, angle of attack α, and sideslip angle β; therefore, the state variables of the system are shown in Equation (2.4) as follows:

[0197]

[0198] In Equation (2.4): Denote the track azimuth angle of the wingman; Denote the track azimuth angle of the lead aircraft;

[0199] Step B: Based on the state variables of the above system obtained in Step A, design a formation control law based on Leader-Follower;

[0200] Step B.1: Set x c , y c , z c as the distance that the wingman and the lead aircraft in the UAV formation should maintain as required; based on the state variables of the above system obtained in Step A, obtain the error vector; the error terms in the above error vector include position error, velocity error, track inclination angle error, track azimuth angle error, and track roll angle error; the above error vector is shown in Equation (2.5) as follows:

[0201]

[0202] Step B.2: Based on the ultimate goal of formation control is to make the target errors e x , e y and e z be zero, and assuming the sideslip angle β is zero, based on the control quantities of the lead aircraft and the wingman in the formation shown in Equation (2.6) and the error vector shown in Equation (2.5) in Step B.1, give the control law for the control variables of the wingman as shown in Equation (2.7) below, and then the control quantity of the formation is shown in Equation (2.8) below;

[0203] The control quantities of the lead aircraft and the wingman in the formation are shown in Equation (2.6) as follows:

[0204]

[0205] The control law of the control variables of the wingman is shown in the following formula (2.7):

[0206]

[0207] In formula (2.7): K ZP and K ZI are the proportional and integral coefficients of altitude control respectively; K μP and K μI are the proportional and integral coefficients of track tilt angle control respectively; K XP and K XI are the proportional and integral coefficients of longitude control respectively; K VP and K VI are the proportional and integral coefficients of speed control respectively; K YP and K YI are the proportional and integral coefficients of dimension control respectively; K φP and K φI are the proportional and integral coefficients of roll angle control respectively;

[0208] The control quantity of the formation is shown in the following formula (2.8):

[0209] U c =[α T φ] T (2.8).

[0210] [3] Formation anti-collision

[0211] During the flight of the UAV formation, the formation needs to be changed according to the mission requirements. During the corresponding transformation process, the UAVs cannot collide with each other, which is the basic requirement for formation flight. Since the UAV control system itself is a non-linear coupling system, coupled with the constraints of the complex combat environment, the technical requirements for the formation design, change and maintenance of formation flight are also constantly improving. Therefore, an effective control strategy needs to be proposed. Based on the above requirements, the present invention proposes a formation algorithm based on distributed model predictive control to solve the problem of anti-collision in UAV formation control.

[0212] As a modern control method, model predictive control is a control method with simple calculation, strong robustness, good anti-interference ability and high control accuracy. It can be used as a basic control method for reference to improve the control efficiency of UAV formation. However, the real-time problem of its rolling optimization has become a bottleneck in the practical application of predictive control. Aiming at the UAV formation control problem, a relative motion equation is established, and then a formation controller is designed by using the method of multi-model based predictive control, which transforms the non-linear rolling optimization problem into a linear quadratic optimization problem, improves the real-time performance of non-linear prediction, and realizes formation control.

[0213] Since formation control is a multi - aircraft motion planning problem, the UAV motion routes calculated by the consensus algorithm do not consider anti - collision. At this time, the distributed model predictive control algorithm is used to represent all the surrounding UAVs that may collide in a mathematical form as constraint conditions, so as to solve the UAV motion planning solution that meets multi - objective requirements.

[0214] Figure 6 It is a schematic diagram of the framework for combining the Consensus algorithm and the Distributed Model Predictive Control (DMPC) algorithm for control in the embodiments of the present invention. Among them, Consensus represents the consensus algorithm, DMPC represents the distributed model predictive control algorithm, and Fixed - wing Autopilot represents the fixed - wing autopilot. The whole framework is a complete motion planning process. The fixed - wing autopilot flies out the real state, and then the state is fed back to the consensus and distributed model predictive control algorithms for real - time planning to improve the control efficiency.

[0215] In this embodiment, the design process of the formation anti - collision setting based on the distributed model predictive control in the above step one and step three includes the following steps:

[0216] Step a: Establish a formation prediction model;

[0217] Step b: According to the formation prediction model established in step a, represent all the surrounding UAVs that may collide in a mathematical form as a constraint penalty term;

[0218] Step c: Use the penalty term in step b to construct an optimization function to minimize the overall penalty when the UAV meets the requirements, and then solve for the control quantity;

[0219] Step d: Calculate the control quantity solved in step c into trajectory control, and the formation anti - collision setting is completed.

[0220] In this embodiment, the specific establishment process of the above step a: establishing a formation prediction model is as follows:

[0221] Use the second - order integral dynamics model to represent the UAV, and its discrete dynamics equation is shown as the following formula (3.1):

[0222]

[0223] In formula (3.1): p i [k + 1], v i [k + 1] respectively represent the position and velocity at the (k + 1) - th moment; p i [k], v i [k], a i[k] represents the position, velocity, and acceleration at the k-th moment; h represents the unit time parameter;

[0224] The state of the UAV is expressed as the following formula (3.2):

[0225]

[0226] In formula (3.2), represents the predicted information of the position at time k t +k; represents the predicted information of the velocity at time k t +k; k t represents any moment;

[0227] Using formula (3.2), the position and velocity can be expressed as an overall state, and the prediction model step equation for K steps is shown as the following formula (3.3):

[0228] P i = A0X 0，i + ΛU i (3.3);

[0229] In formula (3.3): Λ is defined as the following formula (3.4):

[0230]

[0231] In formulas (3.3) and (3.4): Ψ = [i3 03], A0 = [(ΨA)T(ΨA 2 ) T …(ΨA K ) T T ; X 0，i represents the initial position; P i represents the position prediction sequence for K steps; U i represents the input sequence;

[0232] The penalty terms in step b above include the desired trajectory error penalty term, control cost penalty term, control input change penalty term, and collision constraint penalty term; the mathematical forms of the above penalty terms are as follows:

[0233] 3) Desired trajectory error penalty term

[0234]

[0235] The above formula (3.5) represents the two-norm of the error between the trajectory from the start time to the K-th moment and the desired trajectory in the entire prediction step. Transforming formula (3.5) into the quadratic programming form is shown as the following formula (3.6):

[0236] ​

[0237] In Equation (3.6): J e,i represents the desired trajectory error penalty term; P d,i represents the destination; represents a positive definite diagonal matrix the weight coefficient for the error at each step;

[0238] 2) Control cost penalty term:

[0239]

[0240] In Equation (3.7): represents the coefficient of the penalty term; R represents the penalty coefficient of the control cost;

[0241] 3) Control input change penalty term:

[0242]

[0243] In Equation (3.8):

[0244]

[0245]

[0246]

[0247] S represents the error penalty coefficient of the control change;

[0248] 4) Collision constraint penalty term

[0249] The addition of the collision constraint term allows the UAV to avoid collisions in a timely manner when there is a risk of collision:

[0250]

[0251] In Equation (3.9):

[0252]

[0253]

[0254] In the above step c: Use the above penalty terms in step b to construct an optimization function to minimize the overall penalty of the UAV under the satisfied requirements, and then the specific construction process of solving the control quantity is as follows:

[0255]

[0256] In Equation (3.10), U i represents the control quantity; A in , bin Represents the inequality constraints of the control vector.

[0257] By using the multi-aircraft formation anti-collision method based on distributed model predictive control and formation control of the present invention for formation control, the stability and control accuracy of multi-aircraft cooperative formation cruise control, formation control, and formation anti-collision can be improved.

Claims

1. A multi-aircraft formation anti-collision method based on distributed model predictive control and formation control, characterized in that It includes the following steps: Step 1: First, based on the mission planning information and UAV platform factors, assemble a UAV swarm, input the information of the assembly point or waypoint, and then perform formation anti-collision settings based on distributed model predictive control; Step 2: Perform cruise control based on the PID algorithm; during the cruise, if formation reconstruction is required, execute the following Step 3; if formation reconstruction is not required, skip the following Step 3 and execute the following Step 4; Step 3: Based on the requirements of formation reconstruction described in Step 2, perform formation anti-collision settings based on distributed model predictive control again; Step 4: Perform formation maintenance control based on Leader-Follower; Step 5: Determine whether the target point has been reached or the mission has been completed; if so, end the mission; if not, return to the above Step 2 and loop through Steps 2 to 5 until the target point is reached or the mission is completed, and then end the mission; The control process of performing cruise control based on the PID algorithm described in Step 2 includes the following steps: Step 1: The system designs a cooperative flight control law using a second-order consensus algorithm The represents the flight acceleration of UAV i; Step 1.1: The system is designed using a second-order consensus algorithm The respectively represent the three component accelerations of the UAV i in the x, y, and z directions in the ground coordinate system; specifically: The system adopts a second-order consensus algorithm, and designs the three position and acceleration channels of UAV i as shown in the following formula (1.1): In Equation (1.1): N i represents the neighbors of UAV i in the distributed network; a ij represents the weighted coefficient considering the calculation between different UAVs; x diF , y diF , z diF respectively represent the x - coordinate, y - coordinate, and z - coordinate of the reference point of the i - th UAV; x djF , y djF , z djF respectively represent the x - coordinate, y - coordinate, and z - coordinate of the reference point of the j - th UAV; γ represents the weight coefficient of the state quantity, usually greater than 0; respectively represent the three relative velocity components of UAV i relative to UAV j along the x, y, and z directions in the ground coordinate system; The moving speed of UAV i relative to UAV j Specifically, it is shown in the following formula (1.2): In Equation (1.2): V xi , V yi , V zi respectively represent the velocity components of the drone i's velocity V i along the x, y, and z directions in the ground coordinate system; V xj , V yj , V zj respectively represent the velocity components of the drone j's velocity V j along the x, y, and z directions in the ground coordinate system; Position coordination variable ρ iF are different at the beginning of the formation. As time progresses, when t → ∞, ρ iF → ρ jF ; The above formula (1.1) gives the three component acceleration commands in the ground coordinate system, and controlling can achieve the formation shape maintenance; Step 1.2: Define the cooperative flight control law As shown in the following equation (1.3): The second-order consensus algorithm control includes both position information and velocity information; Step 2: Convert the collaborative flight control rate designed in Step 1 into a flight control command wherein the represents the magnitude of the acceleration of the flight of UAV i; the represents the pitch angular velocity; the represents the yaw angular velocity; specifically: The control method is as shown in the following formula (1.4): According to the relationship between the pitch angle and the velocity component as shown in the following formula (1.5), and the relationship between the yaw angle and the velocity component as shown in the following formula (1.6): In Equation (1.5), θ i represents the pitch angle of UAV i; In Equation (1.6), Ψ i represents the yaw angle of UAV i; Then Step 3: Define a new cooperative flight control command v i ; New collaborative flight control command v i As shown in the following formula (1.9): In Equation (1.9), represents the new magnitude of the acceleration of UAV i in flight, represents the new pitch angular velocity of UAV i in flight, represents the new yaw angular velocity of UAV i in flight; The said has the following specific form as shown in formula (1.10): In Equation (1.10), represents the average speed; The said has the specific form as shown in the following formula (1.11): In Equation (1.11), represents the track reference pitch angle; The said has the following specific form as shown in formula (1.12): In Equation (1.12), represents the track reference yaw angle; Step 4: Take the new cooperative flight control instruction v defined in Step 3 i as the input, calculate the error between the actual trajectory and the reference trajectory of the UAV, and based on the PID algorithm, the trajectory controller designs the PID control law u i ; specifically: The PID control law is designed separately for the longitudinal and lateral channels. Assuming that the coupling between the longitudinal and lateral channels of the UAV is small, the design of the control augmentation system is usually carried out separately for the longitudinal and lateral directions without cross-linking. The PID control law u i is designed according to the following formula (1.13): In Equation (1.13), K p , K I , K D are the proportional, integral, and derivative coefficient matrices respectively, represents the deviation; Step 5: Based on the PID control law designed in Step 4, adjust the control instruction input value of the cooperative flight control system, so as to control the flight trajectory and attitude of the UAV.

2. The multi-aircraft formation anti-collision method based on distributed model predictive control and formation control according to claim 1, characterized in that The specific steps of performing formation maintenance control based on Leader-Follower described in Step 4 are as follows: Step A: Establish a relative motion relationship model between the leader UAV and the follower UAV, and obtain the state variables of the leader UAV and the follower UAV system according to this relative motion relationship model; specifically: The dynamic equation of the centroid motion of the UAV in the track coordinate system is as shown in the following formula (2.1): In Equation (2.1): represents acceleration; represents the track tilt angular velocity; represents the track azimuth angular velocity; T represents the engine thrust; D represents the drag force on the UAV; m represents the mass of the UAV and its payload; g represents the acceleration due to gravity; μ represents the track tilt angle; L represents the lift force on the UAV; C represents the side force on the UAV; represents the track azimuth angle; φ represents the roll angle; V represents the UAV speed; In formula (2.1), L, D, and C are calculated according to the following formula (2.2): In Equation (2.2): C L represents the lift coefficient; ρ represents the atmospheric density; S represents the wing area; C Lα represents the derivative of the lift coefficient with respect to the angle of attack; α represents the angle of attack of the UAV; α0 represents the initial angle of attack; C D0 represents the drag coefficient; K represents the transfer coefficient; C Yβ represents the lateral aerodynamic force coefficient; β represents the sideslip angle of the UAV; According to the above formula (2.1), convert the distance between the leader UAV and the follower UAV into the follower UAV coordinate system, and establish a relative motion relationship model between the leader UAV and the follower UAV, as shown in the following formula (2.3): In Equation (2.3): x d represents the component of the relative distance of the leader in the follower coordinate system on the x-axis; y d represents the component of the relative distance of the leader in the follower coordinate system on the y-axis; z d represents the component of the relative distance of the leader in the follower coordinate system on the z-axis; L w represents the lift of the wingman; C w represents the side force of the wingman; m w represents the mass of the wingman; V w represents the velocity of the follower in the ground coordinate system; μ w represents the track inclination angle of the follower in the ground coordinate system; φ w represents the roll angle of the follower itself; V L represents the velocity of the leader in the ground coordinate system; μ L represents the track inclination angle of the leader in the ground coordinate system; represents the relative azimuth error between the leader and the follower; The lift L, drag D, and side force C received by the UAV are functions of the UAV speed V, angle of attack α, and sideslip angle β; therefore, the state variables of the system are as shown in the following formula (2.4): In Equation (2.4): represents the azimuth angle of the wingman's flight path; represents the azimuth angle of the leader's flight path; Step B: Based on the state variables of the system obtained in Step A, design a formation control law based on Leader-Follower; Step B.1: Set x c , y c , z c as the distances that the wing drones in the UAV formation should maintain from the lead drone as required; obtain the error vector based on the state variables of the system obtained in Step A; the error terms in the error vector include position error, velocity error, track inclination angle error, track azimuth angle error, and track roll angle error; the error vector is shown in the following formula (2.5): Step B.2: Based on the ultimate goal of formation control, which is to make the target errors e x , e y and e z equal to zero, and assuming that the sideslip angle β is zero, according to the control variables of the lead aircraft and the wingman in the formation shown in Equation (2.6) below and the error vector shown in Equation (2.5) in Step B.1, the control law for the control variables of the wingman is given as shown in Equation (2.7) below, and then the control quantity of the formation is shown in Equation (2.8) below; The control quantities of the leader UAV and the follower UAV in the formation are as shown in the following formula (2.6): The control law of the control variable of the follower UAV is as shown in the following formula (2.7): In Equation (2.7): K ZP and K ZI are the proportional and integral coefficients for altitude control, respectively; K μP and K μI are the proportional and integral coefficients for track tilt angle control, respectively; K XP and K XI are the proportional and integral coefficients for longitude control, respectively; K VP and K VI are the proportional and integral coefficients for speed control, respectively; K YP and K YI are the proportional and integral coefficients for dimension control, respectively; K φP and K φI are the proportional and integral coefficients for roll angle control, respectively; The control quantity of the formation is as shown in the following formula (2.8): U c = [αTφ] T (2.8).

3. The multi-aircraft formation anti-collision method based on distributed model predictive control and formation control according to claim 1 or 2, characterized in that The design process of the formation anti-collision settings based on distributed model predictive control described in Step 1 and Step 3 includes the following steps: Step a: Establish a formation prediction model; Step b: According to the formation prediction model established in Step a, represent all the UAVs that may collide around in a mathematical form as a constraint penalty term; Step c: Use the penalty term described in Step b to construct an optimization function to minimize the overall penalty when the UAVs meet the requirements, and then solve for the control quantity; Step d: Resolve the control quantity obtained in step c into trajectory control, and the formation anti-collision setting is completed.

4. The multi-aircraft formation anti-collision method based on distributed model predictive control and formation control according to claim 3, wherein: The specific establishment process of the formation prediction model in step a is as follows: Use the second-order integral dynamics model to represent the UAV, and its discrete dynamics equation is shown in the following formula (3.1): v i [k + 1] = v i [k] + ha i [k] (3.1); In Equation (3.1): p i [k + 1], v i [k + 1] represent the position and velocity at the (k + 1)-th moment respectively; p i [k], v i [k], a i [k] represent the position, velocity, and acceleration at the k-th moment respectively; h represents the unit time parameter; The state of the UAV is expressed as the following formula (3.2): In formula (3.2), represents the predicted information at time k t + k; represents the predicted information of the speed at time k t + k; k t represents any time; Using formula (3.2) to represent the position and speed as an overall state, the prediction model step equation for K steps is shown in the following formula (3.3): P i = A0X 0，i + ΛU i (3.3); In formula (3.3): Λ is defined as shown in the following formula (3.4): In equations (3.3) and (3.4): Ψ = [I3 03], A0 = [(ΨA) T (ΨA 2 ) T …(ΨA K ) T T ; X 0，i represents the initial position; P i represents the position prediction sequence at the K-step; U i represents the input sequence;​ The penalty terms in step b include the desired trajectory error penalty term, the control cost penalty term, the control input change penalty term, and the collision constraint penalty term; the mathematical forms of the penalty terms are as follows: 1) Desired trajectory error penalty term The above formula (3.5) represents the error two-norm of the trajectory and the desired trajectory from the start time to the Kth moment in the entire prediction step. Transforming formula (3.5) into the quadratic programming form is shown in the following formula (3.6): In Equation (3.6): J e,i represents the desired trajectory error penalty term; P d,i represents the destination; Indicates a positive definite diagonal matrix The weight coefficient of the error at each step; 2) Control cost penalty term: In formula (3.7): represents the coefficient of the penalty term; R represents the penalty coefficient for controlling costs; 3) Control input change penalty term: In formula (3.8): S represents the error penalty coefficient of control change; 4) Collision constraint penalty term The addition of the collision constraint term enables the UAV to avoid in time when there is a risk of collision: In formula (3.9): The specific construction process of step c: Use the penalty terms described in step b to construct an optimization function to minimize the overall penalty of the UAV when meeting the requirements, and then solve for the control quantity is as follows: In Equation (3.10), U i represents the control quantity; A in , b in represent the inequality constraints of the control vector.

Images