Double-cascade MPC quadrotor unmanned aerial vehicle attitude control method based on robust infinity

Abstract

The invention relates to a dual-cascade MPC quadrotor unmanned aerial vehicle attitude control method based on robust infinity, and designs a technology combining robust infinity control and dual-cascade model predictive control (MPC). According to the method, the attitude control precision and stability of the unmanned aerial vehicle under random airflow disturbance are improved by designing a robust infinite controller, an angle ring MPC controller and an angular velocity ring MPC controller. The robust infinite controller ensures the initial stability of the system under the disturbance condition, the angle ring MPC optimizes the attitude angle, and the angular velocity ring MPC accurately controls the angular velocity. Compared with traditional PID and ADRC controllers, the method has remarkable advantages in disturbance response speed, stability and tracking precision.

Description

Technical Field

[0001] The invention belongs to the technical field of unmanned aerial vehicle control, and in particular relates to a dual-cascade MPC four-rotor unmanned aerial vehicle attitude control method based on robust infinite. Background Art

[0002] With the rapid development of drone technology, quad-rotor drones have been widely used in military, agriculture, logistics and other fields. However, in actual flight, quad-rotor drones often encounter various uncertain environmental factors, among which random airflow interference is a particularly common and challenging problem. Random airflow will affect the flight stability and control accuracy of the drone.

[0003] In order to study the impact of wind disturbance on UAV control, the current UAV control methods mainly include PID, sliding mode and backstepping control. Some researchers have also proposed a neural network method to compensate for the uncertainty caused by disturbances. Although researchers have continuously made technological innovations in UAV control algorithms, when the random airflow is too large or too complex, the tracking of UAVs always has problems of inaccurate tracking and poor attitude control stability. Summary of the invention

[0004] The purpose of the present invention is to overcome the shortcomings and deficiencies of the prior art and propose a dual-cascade MPC quadrotor UAV attitude control method based on robust infinite, which solves the problems of low tracking accuracy and stable attitude control of the UAV when the UAV encounters random airflow interference.

[0005] In order to achieve the above object, the technical solution provided by the present invention is as follows: A dual-cascade MPC control method based on robust infinite control includes the following steps: Step 1: Determine the continuous state space equation of the attitude angle channel of the quadrotor drone as follows: in, is the state vector of the quadcopter, and the output value in the equation Represents the pitch angle, roll angle and yaw angle of the quadcopter. represent the parameters of the system state space equation, It is the control input of the attitude angle of the quadrotor drone; Step 2: Design a robust infinite controller to perform preliminary control on the attitude angle of the UAV and use the control result as the input of the MPC controller; Step 3: Discretize the state space equation of the attitude angle channel of the quadrotor drone to obtain the discretized state space model of the attitude angle of the quadrotor drone: in, They represent the state variables, control input variables and control output variables of the attitude angle nominal system respectively; Step 4: Control input variables for the continuous-time state model and discrete-time state model in step 1 The conversion relationship between the two is determined as follows: Among them, T s and Represents the sampling time of attitude angle and control input matrix; Step 5: Use the matrix K of the robust infinite controller as the initial control rate to ensure that the initial state of the system is stable within a certain range; Step 6: Design the angle loop MPC controller, optimize the objective function by discretizing the attitude angle dynamics model, and calculate the optimal control input; Step 7: Determine the kinematic equation of the drone's attitude angular velocity: Among them, θ, φ and They represent the pitch angle, roll angle, and yaw angle of the drone when it is moving. and denote the pitch angle velocity, roll angle velocity and yaw angle velocity respectively, and Approximately p, Approximately q, Approximately r; Step 8: Design the angular velocity loop MPC controller and combine the velocity change of the attitude angle to achieve precise control of the attitude angle.

[0006] Furthermore, in step 2, the robust infinite controller has a workflow including the following steps: Step 2.1: Solve the generalized algebraic Riccati equation to obtain the gain matrix of the robust infinite controller. The form of the Riccati equation is as follows: A T X+XA-XBR -1 B T X+Q=0 Where: X is the solution of the Riccati equation. R and Q are weight matrices, and A and B represent the parameters of the system state equation respectively; Step 2.2: Use u k =-Kx k To implement the constraint, where x k is the solution of the Riccati equation.

[0007] Further, in step 6, the angle loop MPC controller has a workflow including the following steps: Step 6.1: Take the difference of the state variables at two adjacent moments to obtain the differential equation of the attitude angle of the quadrotor drone at two adjacent moments: Step 6.2: Determine the discrete state space equation incremental model as: Step 6.3: The control increment form of the attitude angle state increment is obtained through the differential equation of the attitude angle of the drone at two adjacent moments: Step 6.4: Determine the incremental form of the differential equations for the quadrotor drone: Step 6.5: For the nominal model of the attitude angles, select the target optimization function: Step 6.6: Obtain the optimal control input sequence for the attitude angle at the Kth moment: Step 6.7: Determine the system input Δu at time K ζ (k) value, select the optimal input sequence The first element of the system input expression is: Step 6.8: Finally, the actual control input expression of the attitude angle at time t is obtained: Δu ζ (k) = K ζmpc E ζ (k+1|k)=Δu f (k)+Δu s (k)

[0008] Further, in step 8, the angular velocity loop MPC controller has a workflow including the following steps: Step 8.1: Establish the state space equation of the attitude angle control system with disturbance as follows: y=Cx+Du in: where x = [p, q, r, φ, θ, ψ] T , u=[τ φ ,τ θ ,τ ψ ] T , σ i (i=u,v,w) is the turbulence intensity in the x,y,z directions, L i (i=u,v,w) is the turbulence scale in the x,y,z directions, and V is the flight speed of the UAV; Step 8.2: The design of the subsequent MPC angular velocity loop controller is the same as the angle loop setting.

[0009] The present invention proposes a dual-cascade MPC control method based on robust infinite control, which has the following beneficial effects compared with the prior art: 1. Improve system stability: By setting the MPC angle loop as the main controller and the angular velocity loop as the auxiliary controller, the oscillation caused by real-time attitude adjustment is reduced, so that the drone can restore balance more quickly after encountering disturbances, thereby enhancing the robustness and stability of the system; 2. Reduce computational complexity: Robust infinite control introduces angle loop preprocessing data, which reduces the MPC controller's demand for real-time data processing, reduces computational complexity, and improves the efficiency of the control system; 3. Optimize control accuracy: Through the MPC optimization algorithm, the deviation between the prediction error and the control target is minimized, ensuring the high accuracy of the UAV attitude control, especially showing better control effect when dealing with complex environments or disturbances. BRIEF DESCRIPTION OF THE DRAWINGS

[0010] Figure 1 A flow chart of a dual-cascade MPC quadrotor UAV attitude control method based on robust infinite control proposed by the present invention; Figure 2 This is a schematic diagram of the attitude control structure of the dual-cascade MPC quadrotor UAV based on robust infinity proposed by the present invention; Figure 3 is the random airflow disturbance curve in Experimental Example 1; Figure 4 It is the X-axis trajectory tracking curve in Experimental Example 1; Figure 5 It is the Y-axis trajectory tracking curve in Experimental Example 1; Figure 6 It is the Z-axis trajectory tracking curve in Experimental Example 1; Figure 7 is the pitch angle trajectory tracking curve in Experimental Example 1; Figure 8 is the rolling angle trajectory tracking curve in Experimental Example 1; Fig. 9is the yaw angle trajectory tracking curve in Experimental Example 1; Fig.10 is the yaw angle root mean square error curve in Experimental Example 1; Fig.11 is the root mean square error curve of the pitch angle in Experimental Example 1; Fig.12 is the root mean square error curve of the rolling angle in Experimental Example 1; Fig.13 is the yaw angle velocity curve in Experimental Example 1; Fig.14 is the rolling angle angular velocity curve in Experimental Example 1; Fig.15 is the pitch angle angular velocity curve in Experimental Example 1. DETAILED DESCRIPTION

[0011] The present application is described in detail below in conjunction with specific embodiments. The following embodiments will help those skilled in the art to further understand the present application, but do not limit the present application in any form. It should be noted that, for those of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present application. These belong to the protection scope of the present application.

[0012] Example 1 The present application discloses a dual-cascade MPC control method based on robust infinite control, comprising the following steps: Step 1: Determine the continuous state space equation of the attitude angle channel of the quadrotor drone as follows: in, is the state vector of the quadcopter, and the output value in the equation Represents the pitch angle, roll angle and yaw angle of the quadcopter. represent the parameters of the system state space equation, It is the control input of the attitude angle of the quadrotor drone; Step 2: Design a robust infinite controller to perform preliminary control on the attitude angle of the UAV and use the control result as the input of the MPC controller; Step 3: Discretize the state space equation of the attitude angle channel of the quadrotor drone to obtain the discretized state space model of the attitude angle of the quadrotor drone: in, They represent the state variables, control input variables and control output variables of the attitude angle nominal system respectively; Step 4: Control input variables for the continuous-time state model and discrete-time state model in step 1 The conversion relationship between the two is determined as follows: Among them, T s and Represents the sampling time of attitude angle and control input matrix; Step 5: Use the matrix K of the robust infinite controller as the initial control rate to ensure that the initial state of the system is stable within a certain range; Step 6: Design the angle loop MPC controller, optimize the objective function by discretizing the attitude angle dynamics model, and calculate the optimal control input; Step 7: Determine the kinematic equation of the drone's attitude angular velocity: Among them, θ, φ and They represent the pitch angle, roll angle, and yaw angle of the drone when it is moving. and denote the pitch angle velocity, roll angle velocity and yaw angle velocity respectively, and Approximately p, Approximately q, Approximately r; Step 8: Design the angular velocity loop MPC controller and combine the velocity change of the attitude angle to achieve precise control of the attitude angle.

[0013] Furthermore, in step 2, the robust infinite controller has a workflow including the following steps: Step 2.1: Solve the generalized algebraic Riccati equation to obtain the gain matrix of the robust infinite controller. The form of the Riccati equation is as follows: A T X+XA-XBR -1 B T X+Q=0 Where: X is the solution of the Riccati equation. R and Q are weight matrices, and A and B represent the parameters of the system state equation. Step 2.2: Use u k =-Kx k To implement the constraint, where x k is the solution of the Riccati equation.

[0014] Further, in step 6, the angle loop MPC controller has a workflow including the following steps: Step 6.1: Take the difference of the state variables at two adjacent moments to obtain the differential equation of the attitude angle of the quadrotor drone at two adjacent moments: Step 6.2: Determine the discrete state space equation incremental model as: Step 6.3: The control increment form of the attitude angle state increment is obtained through the differential equation of the attitude angle of the drone at two adjacent moments: Step 6.4: Determine the incremental form of the differential equations for the quadrotor drone: Step 6.5: For the nominal model of the attitude angles, select the target optimization function: Step 6.6: Obtain the optimal control input sequence for the attitude angle at the Kth moment: Step 6.7: Determine the system input Δu at time K ζ (k) value, select the optimal input sequence The first element of the system input expression is: Step 6.8: Finally, the actual control input expression of the attitude angle at time t is obtained: Δu ζ (k) = K ζmpc E ζ (k+1|k)=Δu f (k)+Δu s (k)

[0015] Further, in step 8, the angular velocity loop MPC controller has a workflow including the following steps: Step 8.1: Establish the state space equation of the attitude angle control system with disturbance as follows: y=Cx+Du in: where x = [p, q, r, φ, θ, ψ] T , u=[τ φ ,τ θ ,τ ψ ] T , σ i (i=u,v,w) is the turbulence intensity in the x,y,z directions, L i(i=u,v,w) is the turbulence scale in the x,y,z directions, and V is the flight speed of the UAV; Step 8.2: The design of the subsequent MPC angular velocity loop controller is the same as the angle loop setting.

[0016] The following simulation is performed on a dual-cascade MPC control method based on robust infinite control disclosed in this application to prove the effectiveness and feasibility of the control strategy, as follows: The design of the various parameters of the drone in the simulation experiment is listed as follows: the mass of the quadrotor drone m = 0.698 kg, the distance from the rotor center to the center of mass of the quadrotor drone d = 0.171 m, and the gravity acceleration g = 9.8 m / s -2 , lift coefficient C T =7.6184×10 (-8) ×(60 / (2×pi)) 2 N·s -2 , X / Y moment of inertia J xx / J yy =0.0034Kg·m 2 , anti-torque coefficient C m =2.6839×10 (-9) ×(60 / (2×pi)) 2 N·m·s -2 、Rotor moment of inertia J m =1.302×10 (-6) Kg·m 2 and Z-axis moment of inertia J zz =0.0060Kg·m 2 .

[0017] The simulation results are as follows: In the figure, target is the target tracking curve, proportional-integral-derivative controller (PID) and active disturbance rejection controller (ADRC) are control experimental curves, and cascade MPC control (H-MMPC) based on robust infinite control is the attitude control algorithm designed by the present invention. Figure 3-Figure 9 It can be concluded that after the disturbance occurs, the UAV has a small disturbance around 2.9s, 31.7s and 71.7s respectively. However, by comparison, the disturbance amplitude and fluctuation duration of the H-MMPC controller are smaller than those of the ADRC and PID controllers, but the overall difference is not very large. The reason is that the control algorithms used in the position loop are the traditional MPC controller, ADRC controller and PID controller.

[0018] The design of the H-MMPC controller in the attitude loop is significantly better than ADRC and PID, and its robustness is also better than the traditional ADRC and PID control algorithms. It can be clearly seen in the pitch angle-time curve that the H-MMPC controller has less amplitude and jitter times, smaller jitter amplitude, and shorter convergence time. This phenomenon is more obvious in the roll angle-time curve, and the amplitude fluctuation is basically maintained at a maximum of 0.005 fluctuations, which can be basically ignored for drones. However, in the yaw angle-time curve, the traditional ADRC and PID curves have a small jitter at the beginning, while the H-MMPC controller can stably follow the target curve value, and the tracking effect is very ideal. In general, different control methods have different response speeds when adjusting the target angle. It can be observed that the H-MMPC response speed is always the fastest when disturbance occurs, and in terms of stability, H-MMPC and ADRC can always approach the target curve most of the time, showing better stability. However, PID fluctuates greatly when disturbance occurs, and the stability shown is relatively poor.

[0019] Fig.10 , 11 1 and 12 are the root mean square error curves of pitch angle, roll angle and yaw angle. As can be seen from the figure, the curves show error fluctuations around 11 seconds, 30 seconds, 51 seconds and 91 seconds. This is because a disturbance is added to the drone around these time points to simulate the disturbance of random airflow. However, the drone attitude is quickly stabilized through controller adjustment. After each peak of the error value, the error drops rapidly and tends to zero, indicating that the control algorithm proposed in the present invention has a strong recovery ability. Except for the fluctuation after the disturbance is applied, the overall error level remains low, which further illustrates the good stability of the control algorithm.

[0020] Fig.13 , 14 15 are the angular velocity curves of pitch angle, roll angle and yaw angle. The attitude angle of the quadcopter is used as an auxiliary loop control of attitude control, and its overall speed change trend always changes with the change of disturbance. When the random airflow disturbance increases, the angular velocity of the attitude angle will always change, and its change trend is different from that of the attitude angle. When the drone control is stable, the angular velocity of the attitude angle is always at zero value and will not fluctuate.

[0021] The technical means disclosed in the scheme of the present invention are not limited to the technical means disclosed in the above-mentioned implementation mode, but also include technical solutions composed of any combination of the above-mentioned technical features. It should be pointed out that for ordinary technicians in this technical field, without departing from the principle of the present invention, several improvements and modifications can be made, which are also considered as the protection scope of the present invention.

Claims

1. A dual-cascade MPC quadrotor UAV attitude control method based on robust infinite loop is used for the flight attitude control of the UAV. The core design of this method is to set the model predictive control (MPC) angle loop as the main controller, which is responsible for the main adjustment of the UAV attitude, and the MPC angular velocity loop is used as the auxiliary controller. The control cycle of the MPC angle loop is relatively long, and attitude adjustment is given priority, while the MPC angular velocity loop is used to quickly respond to angular velocity changes with a shorter control cycle. The attitude controller is based on model predictive control and minimizes the deviation between the prediction error and the control target through an optimization algorithm. In order to reduce the computational complexity of the MPC controller and enhance the robustness of the system, robust infinite control is introduced in the MPC angle loop. The robust control first preprocesses the data to reduce the sensitivity of the system to external interference, and then passes the processed data to the MPC angle loop controller, thereby improving the stability and accuracy of attitude control.

2. The method for attitude control of a quad-rotor UAV based on a robust infinite dual-cascade MPC according to claim 1 is characterized in that: The method may include the following steps: Step 1: Determine the continuous state space equation of the attitude angle channel of the quadrotor drone as follows: in, is the state vector of the quadcopter, and the output value in the equation Represents the pitch angle, roll angle and yaw angle of the quadrotor drone, matrix represent the parameters of the system state space equation, It is the control input of the attitude angle of the quadrotor drone; Step 2: Design a robust infinite controller to perform preliminary control on the attitude angle of the UAV and use the control result as the input of the MPC controller; Step 3: Discretize the state space equation of the attitude angle channel of the quadrotor drone to obtain the discretized state space model of the attitude angle of the quadrotor drone: in, They represent the state variables, control input variables and control output variables of the attitude angle nominal system respectively; Step 4: Control input variables for the continuous-time state model and discrete-time state model in step 1 The conversion relationship between the two is determined as follows: Among them, T s and Represents the sampling time of attitude angle and control input matrix; Step 5: Use the matrix K of the robust infinite controller as the initial control rate to ensure that the initial state of the system is stable within a certain range; Step 6: Design the angle loop MPC controller, optimize the objective function by discretizing the attitude angle dynamics model, and calculate the optimal control input; Step 7: Determine the kinematic equation of the drone's attitude angular velocity: Among them, θ, φ and They represent the pitch angle, roll angle, and yaw angle of the drone when it is moving. and denote the pitch angle velocity, roll angle velocity and yaw angle velocity respectively, and Approximately p, Approximately q, Approximately r; Step 8: Design the angular velocity loop MPC controller and combine the velocity change of the attitude angle to achieve precise control of the attitude angle.

3. The method for attitude control of a quad-rotor UAV based on a robust infinite dual-cascade MPC according to claim 2 is characterized in that: The robust infinite controller described in step 2 has the following steps in its working process: Step 2.1: Solve the generalized algebraic Riccati equation to obtain the gain matrix of the robust infinite controller. The form of the Riccati equation is as follows: A T X+XA-XBR -1 B T X+Q=0 Where: X is the solution of the Riccati equation, R and Q are weight matrices, A and B represent the parameters of the system state equation respectively; Step 2.2: Use u k =-Kx k To implement the constraint, where x k is the solution of the Riccati equation.

4. The method for attitude control of a quad-rotor UAV based on a robust infinite dual-cascade MPC according to claim 2 is characterized in that: The angle loop MPC controller described in step 6 has the following steps in its workflow: Step 6.1: Take the difference of the state variables at two adjacent moments to obtain the differential equation of the attitude angle of the quadrotor drone at two adjacent moments: Step 6.2: Determine the discrete state space equation incremental model as: Step 6.3: The control increment form of the attitude angle state increment is obtained through the differential equation of the attitude angle of the drone at two adjacent moments: Step 6.4: Determine the incremental form of the differential equations for the quadrotor drone: Step 6.5: For the nominal model of the attitude angles, select the target optimization function: Step 6.6: Obtain the optimal control input sequence for the attitude angle at the Kth moment: Step 6.7: Determine the system input Δu at time K ζ (k) value, select the optimal input sequence The first element of the system input expression is: Step 6.8: Finally, the actual control input expression of the attitude angle at time t is obtained: Δu ζ (k)=K ζmpc E ζ (k+1|k)=Δu f (k)+Δu s (k)。 5. The method for attitude control of a quad-rotor UAV based on a robust infinite dual-cascade MPC according to claim 2 is characterized in that: The angular velocity loop MPC controller described in step 8 has the following steps in its working process: Step 8.1: Establish the state space equation of the attitude angle control system with disturbance as follows: y=Cx+Du in: where x = [p, q, r, φ, θ, ψ] T , u=[τ φ ,τ θ ,τ ψ ] T , σ i (i=u,v,w) is the turbulence intensity in the x,y,z directions, L i (i=u,v,w) is the turbulence scale in the x,y,z directions, and V is the flight speed of the UAV; Step 8.2: The design of the subsequent MPC angular velocity loop controller is the same as the angle loop setting.

Images