A Disturbance Rejection Control Method for Quadrotor UAVs Based on Model Compensation

Abstract

The present invention belongs to the technical field of aircraft control, and specifically relates to a disturbance rejection control method for a quadrotor UAV based on model compensation, which includes the following steps: S1: Establish a quadrotor UAV attitude dynamics model with unknown disturbances; S2: Based on output feedback, design a discrete-time domain model compensation extended state observer to observe and feedback the attitude output signal and unknown disturbances; S3: According to the attitude dynamics model, design an incremental model predictive controller; The control method takes into account both input limitations and input rate-of-change limitations, can achieve better effects with smaller control inputs, and can effectively reduce the energy consumption caused by frequent high-power maneuvers of the UAV. The present invention achieves a better control effect than the traditional method of limiting the input by adding a saturation function with more optimal input limitations. The control method of the present invention can realize the attitude stabilization control of a quadrotor UAV in the discrete-time domain while reasonably considering input constraints.

Description

Technical Field

[0001] The present invention relates to the technical field of aircraft control, and particularly relates to a disturbance rejection control method for a quadrotor UAV based on model compensation. Background Art

[0002] Quadrotor UAVs have been widely used in military and civilian fields due to their advantages such as simple structure, vertical takeoff and landing, and flexible flight. However, quadrotor UAVs have characteristics such as nonlinearity, strong coupling, and poor anti-interference ability, which makes it very difficult to design a high-performance controller for quadrotor UAVs.

[0003] For the attitude disturbance rejection control of quadrotor UAVs, common methods include adaptive control, active disturbance rejection control, sliding mode control, backstepping control, etc. Most of these methods are based on continuous-time control methods, but the microprocessors of actual UAVs can only process discrete-time signals, which makes traditional control methods have great limitations. In addition, most traditional control algorithms are difficult to reasonably limit the control input, which will also affect the system performance and even cause the system to become unstable.

[0004] With the increasingly wide application scenarios of UAVs, the requirements for the flight performance of UAVs are also increasing day by day, and the demand for designing a quadrotor UAV controller with strong anti-interference ability in the discrete time domain is becoming more and more urgent. Summary of the Invention

[0005] 1. Technical Problems to be Solved

[0006] The purpose of the present invention is to solve the problem that most of the existing UAV anti-interference control algorithms are difficult to reasonably limit the control input, and at the same time make the control algorithm better applicable to the embedded system of actual UAVs, and propose a disturbance rejection control method for a quadrotor UAV based on model compensation.

[0007] 2. Technical Solutions

[0008] In order to achieve the above purpose, the present invention adopts the following technical solutions:

[0009] A disturbance rejection control method for a quadrotor UAV based on model compensation, comprising the following steps:

[0010] S1: Establishment of the attitude dynamics model of the quadrotor UAV: Define the angles formed by rotating around the X-axis, Y-axis, and Z-axis in the body coordinate system as the roll angle Φ, pitch angle θ, and yaw angle ψ of the quadrotor UAV respectively, and use Euler's equations to establish the attitude dynamics model of the quadrotor UAV;

[0011] S2: Design of Model-Compensated Extended State Observer in Discrete Time Domain: Design a new type of model-compensated extended state observer in discrete time domain, abbreviated as MCESO, to improve the accuracy of estimating unknown disturbances;

[0012] S3: Design of Incremental Model Predictive Controller.

[0013] Preferably, the attitude dynamics model of the quadrotor UAV in S1 is as follows:

[0014]

[0015] where Θ = [Φ, θ, ψ] T represents three attitude angles; U x , U y , U z are the control input quantities in the roll, pitch, and yaw channels respectively; d i (i = Φ, θ, ψ) represents the unknown external disturbances received by the quadrotor UAV;

[0016] Furthermore, the dynamic models of the above three channels can be written in the following form:

[0017]

[0018] where p, q, and r are the roll, pitch, and yaw angular velocities respectively.

[0019] Preferably, in S2, taking the roll channel as an example, design a model-compensated extended state observer. The design methods of MCESO for the pitch and yaw channels are similar to that of the roll channel;

[0020] Define the error observation value and disturbance observation value of the above roll angle subsystem as:

[0021]

[0022] where e Φ is the roll angle observation error, p 3 is the observation value of the unknown disturbance in the roll angle channel. Since the first term in the roll angle dynamics equation, can be obtained from the system dynamics model, so it does not need to be observed. The design of the model-compensated extended state observer MCESO for the roll channel is as follows:

[0023]

[0024] where p 2 is the roll angular rate; U x represents the control input; b ф represents the control gain, which is equal to 1 / I x here; βф1 , β ф2 and β ф3 are parameters to be adjusted. The fal function is defined as

[0025]

[0026] where δ represents the length of the linear region of the fal function, α is a constant between 0 and 1, and e represents the tracking error;

[0027] Discretizing the model compensation extended state observer for the roll channel, we get:

[0028]

[0029] where k represents the sampling time and h represents the sampling period. Then, according to the discretized model compensation extended state observer, p 3 the output is the observed value of the disturbance quantity in this channel.

[0030] Preferably, since the disturbances in the attitude control system of the quadrotor UAV have been compensated by the discretized model compensation extended state observer in S3, they are not considered in the design of the controller. Therefore, the attitude dynamics of this part of the quadrotor UAV can be expressed as follows:

[0031]

[0032] Select the state variables The control input U = [U x , U y , U z T Then the state equation of the attitude subsystem can be written as: The given desired attitude change equation can also be expressed as

[0033] Subtracting the state equation of the actual attitude from the desired attitude change equation gives

[0034]

[0035] where θ s = θ - θ d , u = U - U d ;

[0036] Set the sampling time to T and the output variable to η(k). Then discretize the equation and obtain the following expression:

[0037]

[0038] where

[0039] ​

[0040] The stability of the control input is an important requirement for controller design. To facilitate the adjustment of the control input increment, an incremental model predictive controller (abbreviated as IMPC) is designed; the control input at time k is the sum of the input at time k-1 and the input increment during the time period between time k-1 and time k. Therefore, the control increment u(k) = U(k) - U(k-1). Define a new state variable ξ(k) = [Θ s (k), U(k-1)] T ; then the equation can be transformed into

[0041]

[0042] where

[0043]

[0044] C(k) = [I 0].

[0045] Set the control horizon to N p , and set the prediction horizon to N c (Np≥Nc), then perform state prediction and output prediction to obtain the predicted output sequence

[0046] Γ(k) = F(k)ξ(k) + H(k)ΔU(k). (11)

[0047] In the formula

[0048] ΔU(k) = [u(k|k)u(k+1|k)…u(k+N c |k)] T ,

[0049] Γ(k) = [η(k+1|k)…η(k+N c |k)…η(k+N p |k)] T ,

[0050]

[0051] For a quadrotor UAV, to achieve fast and stable tracking of attitude angle changes in actual control, it is crucial to minimize the prediction error and the change of the control input. Therefore, the objective function is defined as

[0052]

[0053] where Q and R are the output weight matrix and input weight matrix of the system respectively;

[0054] Given the limitations of the control input and its rate of change of the quadrotor UAV, the constraint on u must be established in the following way:

[0055]

[0056] wherein, u min and u max represent the lower and upper limits of the input rate respectively; U max and U min represent the lower and upper limits of the input respectively; and

[0057]

[0058] Then, solve the above optimization problem to obtain the control input increment u(k) at step k. Finally, calculate the control input U(k) at time step k as U(k) = U(k - 1) + u(k).

[0059] 3. Beneficial Effects

[0060] Compared with the prior art, the advantages of the present invention are as follows:

[0061] (1) In the present invention, the control method can achieve attitude stabilization control of a quadrotor UAV in the discrete time domain while reasonably considering input constraints.

[0062] (2) In the present invention, the control method takes into account both input limitations and input rate limitations, can achieve better effects with smaller control inputs, and can effectively reduce the energy consumption caused by frequent high-power maneuvers of the UAV. The present invention achieves better control effects than the method of restricting inputs by traditional saturation functions with more optimal input limitations.

[0063] (3) In the present invention, the control method can make the attitude tracking of the quadrotor UAV faster than that of the traditional PID control algorithm, and the present invention has the advantage of smaller overshoot compared with the traditional method. Even for the response to a step signal, no overshoot can be achieved, and the anti-interference ability of the method of the present invention is significantly better than that of the traditional algorithm.

[0064] (4) In the present invention, the control method is transformed from the continuous time domain to the discrete domain, which better meets the requirements of the actual UAV embedded flight control system. Description of the Drawings

[0065] Figure 1 is the coordinate system relationship diagram of the quadrotor UAV proposed by the present invention;

[0066] Figure 2 is the control block diagram of an anti-disturbance control method for a quadrotor UAV based on model compensation proposed by the present invention;

[0067] Figure 3A comparison chart of the input response curves of a disturbance rejection control method for a quadrotor UAV based on model compensation and a PID control method with input saturation limitation proposed by the present invention;

[0068] Figure 4 A comparison chart of the control input curves of a disturbance rejection control method for a quadrotor UAV based on model compensation and a PID control method with input saturation limitation proposed by the present invention. Specific implementation manners

[0069] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all of the embodiments.

[0070] Embodiment 1:

[0071] A disturbance rejection control method for a quadrotor UAV based on model compensation includes the following steps:

[0072] S1: Establishment of the attitude dynamics model of the quadrotor UAV: Define the angles formed by rotating around the X-axis, Y-axis, and Z-axis in the body coordinate system as the roll angle Φ, pitch angle θ, and yaw angle ψ of the quadrotor UAV respectively. Using Euler's equation, establish the attitude dynamics model of the quadrotor UAV as follows:

[0073]

[0074] where Θ = [Φ, θ, ψ] T represents the three attitude angles; U x , U y , U z are the control input quantities in the roll, pitch, and yaw channels respectively; d i (i = Φ, θ, ψ) represents the unknown external disturbance received by the quadrotor UAV.

[0075] Furthermore, the dynamic models of the above three channels can be written in the following form:

[0076]

[0077] where p, q, and r are the roll, pitch, and yaw angular velocities respectively.

[0078] S2: Design of the model compensation extended state observer in the discrete time domain: A new type of model compensation extended state observer (abbreviation: MCESO) is designed in the discrete time domain to improve the accuracy of estimating the unknown disturbance. Taking the roll channel as an example below, design the model compensation extended state observer. The design method of the MCESO for the pitch and yaw channels is similar to that of the roll channel.

[0079] Define the error observation value of the above-mentioned roll angle subsystem and the disturbance observation value as

[0080]

[0081] where, e Φ is the roll angle observation error, and p 3 is the observation value of the unknown disturbance in the roll angle channel. Since the first term in the roll angle dynamics equation, can be obtained from the system dynamics model, it does not need to be observed. Therefore, the model compensation extended state observer (MCESO) for the roll channel is designed as follows:

[0082]

[0083] where p 2 is the roll rate; U x represents the control input; b ф represents the control gain, which is equal to 1 / I x here; β ф1 , β ф2 and β ф3 are parameters to be adjusted. The fal function is defined as:

[0084]

[0085] where, δ represents the length of the linear region of the fal function, α is a constant between 0 and 1, and e represents the tracking error.

[0086] Discretize the model compensation extended state observer for the roll channel, and we can get:

[0087]

[0088] where, k represents the sampling time, h represents the sampling period, then according to the discretized model compensation extended state observer, p 3 the output is the observation value of the disturbance in this channel.

[0089] S3: Design of the incremental model predictive controller: Since the disturbances in the attitude control system of the quadrotor UAV have been compensated by the discretized model compensation extended state observer, they can be not considered in the design of the controller. Therefore, the attitude dynamics of this part of the quadrotor UAV can be expressed as follows:

[0090]

[0091] Select the state variables The control input U = [U x , U y , U z T ​The state equation of the attitude subsystem can be written as: The given desired attitude change equation can also be expressed as

[0092] Subtracting the state equation of the actual attitude from the change equation of the desired attitude gives:

[0093]

[0094] where θ s = θ - θ d , u = U - U d .

[0095] Set the sampling time to T and the output variable to η(k). Then discretize the equation and obtain the following expression:

[0096]

[0097] where

[0098]

[0099] The stability of the control input is an important requirement for controller design. To facilitate the adjustment of the control input increment, an incremental model predictive controller (abbreviated as IMPC) is designed. The control input at time k is the sum of the input at time k - 1 and the input increment during the time period between time k - 1 and time k. Therefore, the control increment u(k) = U(k) - U(k - 1). Define a new state variable ξ(k) = [Θ s (k), U(k - 1)] T . Then the equation can be transformed into

[0100]

[0101] where

[0102]

[0103] C(k) = [I 0].[[]END]]

[0104] Set the control time domain to N p , and set the prediction time domain to N c (Np ≥ Nc). Then perform state prediction and output prediction to obtain the predicted output sequence:

[0105] Γ(k) = F(k)ξ(k) + H(k)ΔU(k). (11)

[0106] where

[0107] ΔU(k) = [u(k|k) u(k + 1|k) … u(k + N c |k)]T ,

[0108] Γ(k) = [η(k + 1|k)…η(k + N c |k)…η(k + N p |k)] T ,

[0109]

[0110] For a quadrotor UAV, to achieve fast and stable tracking of attitude angle changes in actual control, it is crucial to minimize the prediction error and the change of control input. Therefore, the objective function is defined as:

[0111]

[0112] where Q and R are the output weight matrix and input weight matrix of the system respectively.

[0113] Given the limitations of the control input and its rate of change of the quadrotor UAV, the constraint on u must be established in the following way:

[0114]

[0115] where, u min and u max represent the lower and upper limits of the input rate respectively; U max and U min represent the lower and upper limits of the input respectively; and

[0116]

[0117] Then, solve the above optimization problem to obtain the control input increment u(k) at step k. Finally, calculate the control input at time step k as U(k) = U(k - 1) + u(k).

[0118] In this embodiment, to achieve disturbance rejection, the control algorithm will increase additional control input. Therefore, when setting the control input limit for the quadrotor UAV, not only the input limit of the incremental model predictive control should be considered, but also the additional input caused by disturbance rejection should be considered. However, since the requirements for setting the control input limit in the incremental model predictive control algorithm proposed in the present invention are relatively strict, for example, the rate of change of the control quantity per unit time is also added, the range of the final control quantity is often much smaller than the input limit. Therefore, when the disturbance is relatively small, the increase in the control quantity caused by the disturbance can be ignored.

[0119] In this embodiment, the control method can achieve attitude stabilization control of the quadrotor UAV in the discrete time domain while reasonably considering the input constraints.

[0120] Embodiment 2:

[0121] In this embodiment, the control method disclosed in this application is simulated and verified to further prove the feasibility and effectiveness of the control method. It is assumed that the upper and lower limits of the control input of the quadrotor UAV are [5N, 5N, 5N] T , [-5N, -5N, -5N] T , and disturbances in the form of f = 0.01sin(t) are added to all three channels. The ideal attitude tracking signals are shown in Equations (14), (15), and (16) respectively:

[0122] Roll channel:

[0123]

[0124] Pitch channel:

[0125]

[0126] Yaw channel:

[0127]

[0128] In this embodiment, in order to verify the effectiveness of the tracking control method of the present invention, for the dynamic model of the quadrotor UAV attitude system, based on the Figure 1 shown quadrotor UAV, a disturbance rejection control method for quadrotor UAV based on model compensation as Figure 2 shown is constructed. Next, its control system design and simulation verification comparison will be carried out with the traditional PID control method with input constraints.

[0129] Table 1. Quadrotor UAV model parameters

[0130]

[0131] Table 2. Model compensation extended state observer parameters

[0132]

[0133] Table 3. Incremental model predictive controller parameters

[0134]

[0135] In this embodiment, Figure 3 are respectively the attitude tracking response curves of the three channels of the quadrotor UAV, namely the roll angle, pitch angle, and yaw angle. As can be seen from Figure 3 , a disturbance rejection control method for quadrotor UAV based on model compensation of the present invention can achieve more stable control of the UAV with a smaller control input. Figure 3It is shown that the method of the present invention can make the attitude tracking of the quadrotor UAV nearly 5 - 7 s faster than the traditional PID control algorithm. Moreover, compared with the traditional method, the present invention has the advantage of smaller overshoot, and even can achieve no overshoot for the response to step signals. More importantly, the method of the present invention has significantly better anti - interference ability than the traditional algorithm.

[0136] In this embodiment, Figure 4 They are respectively the control input change curve graphs of the three channels of the quadrotor UAV, namely the roll angle, pitch angle, and yaw angle channels. From Figure 4 it can be seen that the anti - disturbance control method of the quadrotor UAV based on model compensation of the present invention reaches a stable control input significantly smaller than the traditional PID control algorithm. Although both have input limitations, the traditional method of adding a saturation function to limit the input is obviously not very reasonable. The present invention takes into account both input limitations and input change rate limitations, can achieve better effects with smaller control inputs, and can effectively reduce the energy consumption caused by the frequent high - power maneuvers of the UAV. The present invention achieves better control effects than the traditional method of adding a saturation function to limit the input with more optimal input limitations.

[0137] The above - mentioned is only the preferred specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art within the technical scope disclosed by the present invention, according to the technical solution and inventive concept of the present invention, makes equivalent substitutions or changes, and all should be covered within the protection scope of the present invention.

Claims

1. A method for anti-disturbance control of a quadrotor drone based on model compensation, characterized in that: The following steps are involved: S1: Establishment of the attitude dynamics model of the quadrotor drone: Define the rotation around the body coordinate system X axis, Y Axis and Z The angles formed by the axis rotation are the roll angles of the quadrotor drone. Φ , Pitch angle θ and yaw angle ψ ,Using the Euler equation, the attitude dynamics model of the quadrotor drone is established; S2: Design of model-compensated extended state observer in discrete time domain: A novel model-compensated extended state observer is designed in discrete time domain. The observer performs compensation according to the attitude dynamics equation of the quadrotor UAV to improve the accuracy of unknown disturbance estimation. S3: Design of incremental model predictive controller; The attitude dynamics model of the quadrotor drone in S1 is as follows: (1) in Θ = [Φ, θ, ψ]T Indicates three attitude angles; U x , U y , U z are the control input quantities in the roll, pitch, and yaw channels, respectively; d i (i=Φ,θ,ψ) Indicates the external unknown disturbance received by the quadrotor drone; I x , I x , I x Four-rotor drones x, y, z Moments of inertia about the three coordinate axes; Furthermore, the kinetic model of the above three channels can be written as follows: (2) in, p , q , r are the roll, pitch and yaw angular velocities respectively; The model compensation extended state observer in the discrete domain in S2 satisfies the following formula: (3) in k,k+1 is the sampling time, h is the sampling step size; p 1 ，q 1 and r 1 They are the angular velocities of the roll, pitch, and yaw channels respectively; p 2 , q 2 and r 2 They are the angular accelerations of the roll, pitch, and yaw channels respectively; p 3 ,q 3 and r 3 They are the position disturbance estimates of the roll, pitch, and yaw channels respectively; U i (i=x,y,z) , e j and b j (j=φ,θ,ψ) They are the control input, tracking error and control quantity gain of the three channels; ꞵ j1 , ꞵ j2 and ꞵ j3 ( j=φ,θ,ψ ), δ , α 1 and α 2 is the parameter to be adjusted; The observation error and observation disturbance of the roll angle subsystem are: (4) in, e Φ is the roll angle observation error, p 3 is the observed value of the unknown disturbance in the roll angle channel, Since the first term in the roll angle dynamics equation, , can be obtained from the system dynamics model, so no observation is required; the model compensation expansion state observer of the rolling channel is designed as follows: (5) in p 2 is the roll angular rate; U x represents control input; b ф represents the control gain, which is equal to 1 / I x ; ꞵ ф1 , ꞵ ф2 and ꞵ ф3 is the parameter to be adjusted, fal The function is defined as: (6) in, δ express fal The length of the linear region of the function, α yes 0 and 1 The constant between e represents the tracking error; Discretizing the model compensation extended state observer of the roll channel, we can obtain: (7) in, k represents the sampling time, h represents the sampling period, then the extended state observer is compensated according to the discretized model, p 3 The output is the observed value of the channel disturbance. The design method of the model-compensated extended state observer for the pitch and yaw channels is consistent with that for the roll channel.

2. The anti-disturbance control method for a quad-rotor drone based on model compensation according to claim 1 is characterized in that: In S3, since the disturbances in the attitude control system of the quadrotor drone have been compensated by the discretized model compensation extended state observer, they are not considered in the design of the controller. Therefore, the attitude dynamics of this part of the quadrotor drone can be expressed as follows: (8) Selecting state variables , control input Then the state equation of the attitude subsystem can be written as: , the given expected posture change equation can also be expressed as ; θd is the ideal angle tracking value, Ud Ideal control input. Subtract the state equation of the actual posture from the change equation of the expected posture to obtain: (9) In the formula, ϴ is the actual state quantity, ϴd is a given ideal state quantity, ϴ s is the deviation between the actual angle and the ideal angle, that is ϴ s = ϴ - ϴd ,; u = U - Ud ; Set the sampling time to T , the output variable is η(k) , then discretize equation (8) and get the following expression: (10) in ϴ s is the deviation between the actual angle and the ideal angle, that is ϴ s = ϴ - ϴd ,; u is the difference between the actual value and the ideal value of the controlled quantity, that is, u = U - Ud ; A 0 (k) and B 0 (K) is the transfer function matrix of the state equation, （11） in T is the sampling time; The stability of the control input is an important requirement for controller design. In order to facilitate the adjustment of the control input increment, an incremental model predictive controller is designed; time k The control input is the moment k-1 Input and time k-1 arrive k The sum of the input increments for the time period, thus controlling the input increment u ( k )= U ( k )- U ( k-1 ), define a new state variable ξ ( k )=[ Θ s ( k ), U ( k- 1 )]T; then Equation (9) can be transformed into （12） in ξ(k)= [ θs(k),U(k−1) ] T is the system state matrix, η ( k )= I 3×3 Θ s ( k ) is the system output matrix, which represents the actual angle change, and A(k) , B(k) and C(k) They are Set the control time domain to N p , set the prediction time domain to N c (Np≥Nc) , and then perform state prediction and output prediction to obtain the predicted output sequence: （13） In the formula For a quadrotor drone, it is crucial to achieve fast and stable tracking of attitude angle changes in actual control and minimize the prediction error and control input changes. Therefore, the objective function is defined as: （14） in, η ( k+i )express k+i The difference between the given state quantity and the actual state quantity at a certain moment, η d ( k+i )yes k+i The ideal difference between the given state quantity and the actual state quantity at the moment is a three-dimensional 0 vector here; Q and R They are the output weight matrix and input weight matrix of the system respectively; In view of the limitations of the control input and its rate of change of the quadrotor drone, the control input in the control time domain must be established in the following way U c and the change in control input within the prediction time domain ΔU Constraints: （15） in, umin and umax Respectively represent the lower and upper limits of the input rate; Umin and Umax denote the lower and upper limits of the input respectively; and （16） Then, solve the above optimization problem and get the steps k The control input increment at u ( k ), and finally, calculate the time step k Control input U (k)= U ( k-1 )+ u ( k ).

Images