Four-rotor unmanned aerial vehicle disturbance rejection control method based on model compensation

Abstract

The invention belongs to the technical field of aviation aircraft control, and particularly relates to a four-rotor unmanned aerial vehicle disturbance rejection control method based on model compensation, which comprises the following steps: S1, establishing a four-rotor unmanned aerial vehicle attitude dynamic model with unknown disturbance; s2, based on output feedback, designing a model compensation expansion state observer under a discrete time domain, and performing observation feedback on an attitude output signal and unknown disturbance; s3, designing an incremental model prediction controller according to the attitude dynamics model; the control method gives consideration to input limitation and input change rate limitation, can achieve a better effect with smaller control input, and can effectively reduce energy loss caused by frequent high-power maneuvering of the unmanned aerial vehicle. Compared with a traditional method for limiting input by adding a saturation function, the method achieves a better control effect with better input limitation. According to the control method, stable attitude control of the quad-rotor unmanned aerial vehicle in the discrete time domain can be realized under the condition of reasonably considering input constraints.

Description

Technical Field

[0001] The present invention relates to the field of aviation vehicle control technology, and in particular to an anti-disturbance control method for a quad-rotor unmanned aerial vehicle based on model compensation. Background Art

[0002] Quad-rotor drones are widely used in military and civilian fields due to their simple structure, vertical take-off and landing, and flexible flight. However, quad-rotor drones have the characteristics of nonlinearity, strong coupling, and poor anti-interference ability, which makes it very difficult to design a high-performance controller for quad-rotor drones.

[0003] Common methods for attitude anti-interference control of quad-rotor drones include adaptive control, anti-interference control, sliding mode control, backstepping control, etc. Most of these methods are based on continuous-time control methods, but the microprocessor of actual drones can only process discrete-time signals, which makes traditional control methods have great limitations. In addition, most traditional control algorithms find it difficult to reasonably restrict control inputs, which will also affect system performance and even cause system instability.

[0004] As the application scenarios of drones become increasingly extensive, the requirements for drone flight performance are also increasing. The need to design a quadrotor drone controller with strong anti-interference ability in the discrete time domain is becoming more urgent. Summary of the invention

[0005] 1. Technical issues to be solved

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

[0007] 2. Technical solution

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

[0009] A disturbance suppression control method for a quadrotor unmanned aerial vehicle based on model compensation comprises the following steps:

[0010] S1: Establishment of the attitude dynamics model of the quadrotor drone: The angles formed by the rotation around the X-axis, Y-axis and Z-axis in the body coordinate system are defined as the roll angle Φ, pitch angle θ and yaw angle ψ of the quadrotor drone, respectively. The attitude dynamics model of the quadrotor drone is established using the Euler equation;

[0011] S2: Design of Model Compensated Extended State Observer in Discrete Time Domain: A novel model compensated extended state observer, referred to as MCESO, is designed in discrete time domain to improve the accuracy of unknown disturbance estimation;

[0012] S3: Design of incremental model predictive controller.

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

[0014]

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

[0016] Furthermore, the kinetic model of the above three channels can be written as follows:

[0017]

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

[0019] Preferably, in said S2, taking the roll channel as an example, the model compensation tension state observer is designed, and the MCESO design method of the pitch and yaw channels is similar to the design method of the roll channel;

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

[0021]

[0022] Among them, e Φ is the roll angle observation error, p3 is the observed value of the unknown disturbance in the roll angle channel, due to the first term in the roll angle dynamics equation, It can be obtained from the system dynamics model, so no observation is required; the model compensation extended state observer MCESO of the rolling channel is designed as follows:

[0023]

[0024] Where p2 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, and 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 of the roll channel, we can obtain:

[0028]

[0029] Among them, k represents the sampling time, h represents the sampling period, and the extended state observer is compensated according to the discretized model, and the output of p3 is the observed value of the channel disturbance.

[0030] Preferably, in S3, since the disturbances in the attitude control system of the quadrotor drone have been compensated by the discretized model-compensated 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:

[0031]

[0032] Selecting state variables 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 posture change equation can also be expressed as

[0033] Subtract the state equation of the actual posture from the change equation of the expected posture to obtain

[0034]

[0035] Where θ s =θ-θ d ,u=UU d ;

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

[0037]

[0038] in

[0039]

[0040] 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 (IMPC) is designed; the control input at time k is the sum of the input at time k-1 and the input increment between time k, so the control increment u(k) = U(k)-U(k-1), and a new state variable ξ(k) = [Θ s (k), U(k-1)] T ; then the equation can be transformed into

[0041]

[0042] in

[0043]

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

[0045] 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

[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 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

[0052]

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

[0054] Given the constraints on the control inputs and their rates of change of the quadrotor, the constraints on u must be established as follows:

[0055]

[0056] Among them, u min and u max Respectively represent the lower and upper limits of the input rate; U max and U min denote the lower and upper limits of the input respectively; and

[0057]

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

[0059] 3. Beneficial effects

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

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

[0062] (2) In the present invention, the control method takes into account both input limitation and input change rate limitation, and can achieve better results with smaller control input, which can effectively reduce the energy loss caused by frequent high-power maneuvers of the UAV. The present invention achieves better control effects than the traditional method of limiting input with a saturation function by using a more optimal input limitation.

[0063] (3) In the present invention, the control method can make the attitude tracking of the quadrotor drone faster than the traditional PID control algorithm, and the present invention has the advantage of small overshoot compared with the traditional method, and can even achieve no overshoot in response to step signals, 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 needs of the actual UAV embedded flight control system. BRIEF DESCRIPTION OF THE DRAWINGS

[0065] Figure 1 This is a coordinate system relationship diagram of the quad-rotor drone proposed by the present invention;

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

[0067] Figure 3 A comparison diagram of input response curves of a quadrotor UAV anti-disturbance control method based on model compensation and a PID control method with input saturation limitation proposed in the present invention;

[0068] Figure 4 This is a control input curve comparison diagram of a quadrotor UAV anti-disturbance control method based on model compensation and a PID control method with input saturation limitation proposed in the present invention. DETAILED DESCRIPTION

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

[0070] Embodiment 1:

[0071] A disturbance suppression control method for a quadrotor unmanned aerial vehicle based on model compensation comprises the following steps:

[0072] S1: Establishment of the attitude dynamics model of the quadrotor drone: Define the angles formed by the rotation 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 drone. Use the Euler equation to establish the attitude dynamics model of the quadrotor drone as follows:

[0073]

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

[0075] Furthermore, the kinetic model of the above three channels can be written as follows:

[0076]

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

[0078] S2: Design of model-compensated extended state observer in discrete time domain: A new model-compensated extended state observer (MCESO) is designed in discrete time domain to improve the accuracy of unknown disturbance estimation. The roll channel is taken as an example to design a model-compensated extended state observer. The MCESO design method for pitch and yaw channels is similar to that for roll channel.

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

[0080]

[0081] Among them, e Φ is the roll angle observation error, and p3 is the observed value of the unknown disturbance in the roll angle channel. Since the first term in the roll angle dynamics equation, It can be obtained from the system dynamics model, so no observation is required. Therefore, the model compensated extended state observer (MCESO) of the roll channel is designed as follows:

[0082]

[0083] Where p2 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. 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] Discretizing the model compensation extended state observer of the roll channel, we can obtain:

[0087]

[0088] Among them, k represents the sampling time, h represents the sampling period, and the extended state observer is compensated according to the discretized model, and the output of p3 is the observed value of the channel disturbance.

[0089] S3: Incremental model predictive controller design: Since the disturbances in the attitude control system of the quadrotor drone have been compensated by the discretized model-compensated extended state observer, they can be ignored in the design of the controller. Therefore, this part of the quadrotor drone attitude dynamics can be expressed as follows:

[0090]

[0091] Selecting state variables 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 posture change equation can also be expressed as

[0092] Subtract the state equation of the actual posture from the change equation of the expected posture to obtain:

[0093]

[0094] Where θ s =θ-θ d ,u=UU d .

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

[0096]

[0097] in

[0098]

[0099] 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 (IMPC) is designed. The control input at time k is the sum of the input at time k-1 and the input increment for the time period between time k, so 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] in

[0102]

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

[0104] Set the control time domain to N p , 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] In the formula

[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 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:

[0111]

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

[0113] Given the constraints on the control inputs and their rates of change of the quadrotor, the constraints on u must be established as follows:

[0114]

[0115] Among them, u min and u max Respectively represent the lower and upper limits of the input rate; U max and U min denote the lower and upper limits of the input respectively; and

[0116]

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

[0118] In this embodiment, the control algorithm will add additional control inputs to achieve anti-disturbance. Therefore, when limiting the control input of the quadrotor drone, not only the input limit of the incremental model predictive control should be considered, but also the additional input added by anti-disturbance. However, since the incremental model predictive control algorithm proposed in the present invention has strict requirements on the control input limit, such as adding the rate of change of the control amount per unit time, the range of the final control amount is often much smaller than the input limit, so when the disturbance is relatively small, the increase in the control amount caused by the disturbance can be ignored.

[0119] In this embodiment, the control method can achieve attitude stabilization control of the quadrotor drone in discrete time domain while reasonably considering 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. Assume that the upper and lower limits of the control input of the quad-rotor drone are [5N, 5N, 5N] respectively. T ,[-5N,-5N,-5N] T, and interference in the form of f = 0.01sin(t) is 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, the attitude system dynamics model of the quad-rotor UAV is used as the basis for the following Figure 1 The quadcopter shown is constructed as Figure 2 The anti-disturbance control method of the quadrotor drone based on model compensation is shown in the figure. The control system design and simulation verification are compared with the traditional PID control method with input limitation.

[0129] Table 1. Quadrotor UAV model parameters

[0130]

[0131] Table 2. Model-compensated extended state observer parameters

[0132]

[0133] Table 3. Incremental model predictive controller parameters

[0134]

[0135] In this embodiment, Figure 3 The following are the attitude tracking response curves of the quadcopter drone with three channels: roll angle, pitch angle, and yaw angle. Figure 3 It can be seen that the anti-disturbance control method of a quad-rotor UAV based on model compensation of the present invention can achieve more stable control of the UAV with smaller control input. Figure 3 It is shown that the method of the present invention can make the attitude tracking of the quadcopter UAV faster by nearly 5-7 seconds than the traditional PID control algorithm, and the present invention has the advantage of small overshoot compared with the traditional method, and can even achieve no overshoot in response to step signals. More importantly, the anti-interference ability of the method of the present invention is significantly better than that of the traditional algorithm.

[0136] In this embodiment, Figure 4The following are the control input change curves of the three channels of the quad-rotor drone: roll angle, pitch angle, and yaw angle. Figure 4 It can be seen that the anti-disturbance control method of the four-rotor drone based on model compensation of the present invention achieves a stable control input that is significantly smaller than the traditional PID control algorithm. Although both of them perform input restrictions, the traditional method of limiting the input with a saturation function is obviously not very reasonable. The present invention takes into account both the input restriction and the input change rate restriction, and can achieve better results with a smaller control input, which can effectively reduce the energy loss caused by frequent high-power maneuvers of drones. The present invention achieves better control effects than the traditional method of limiting the input with a saturation function with better input restrictions.

[0137] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any technician familiar with the technical field can make equivalent replacements or changes according to the technical scheme and inventive concept of the present invention within the technical scope disclosed by the present invention, which should be covered by 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: The angles formed by the rotation around the X-axis, Y-axis and Z-axis in the body coordinate system are defined as the roll angle Φ, pitch angle θ and yaw angle ψ of the quadrotor drone, respectively. The attitude dynamics model of the quadrotor drone is established using the Euler equation; S2: Design of Model Compensated Extended State Observer in Discrete Time Domain: A novel model compensated extended state observer, referred to as MCESO, is designed in discrete time domain to improve the accuracy of unknown disturbance estimation; S3: Design of incremental model predictive controller.

2. The anti-disturbance control method for a quad-rotor drone based on model compensation according to claim 1 is characterized in that: The attitude dynamics model of the quadrotor drone in S1 is as follows: where Θ = [Φ, θ, ψ] T Represents three attitude angles; U x , U y , U z are the control inputs in the roll, pitch and yaw channels respectively; d i (i = Φ, θ, ψ) represents the external unknown disturbance received by the quadrotor drone; Furthermore, the kinetic model of the above three channels can be written as follows: Among them, p, q, and r are the roll, pitch, and yaw angular velocities, respectively.

3. The anti-disturbance control method for a quad-rotor drone based on model compensation according to claim 2 is characterized in that: In S2, the roll channel is taken as an example to design a model compensation tension state observer. The MCESO design method of the pitch and yaw channels is similar to that of the roll channel. The observation error and observation disturbance of the above roll angle subsystem are defined as: Among them, e Φ is the roll angle observation error, p3 is the observed value of the unknown disturbance in the roll angle channel, due to the first term in the roll angle dynamics equation, It can be obtained from the system dynamics model, so no observation is required; the model compensation extended state observer MCESO of the rolling channel is designed as follows: Where p2 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, and the fal function is defined as: 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; Discretizing the model compensation extended state observer of the roll channel, we can obtain: Among them, k represents the sampling time, h represents the sampling period, and the expanded state observer is compensated according to the discretized model, and the output of p3 is the observed value of the channel disturbance.

4. The anti-disturbance control method for a quad-rotor drone based on model compensation according to claim 3 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: Selecting state variables 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 posture change equation can also be expressed as Subtract the state equation of the actual posture from the change equation of the expected posture to obtain: In the formula θ s = θ-θ d ,u=UU d ; Set the sampling time to T, the output variable to η(k), and then discretize the equation to obtain the following expression: in 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 (IMPC) is designed; the control input at time k is the sum of the input at time k-1 and the input increment between time k, so the control increment u(k) = U(k)-U(k-1), and a new state variable ξ(k) = [Θ s (k), U(k-1)] T ; then the equation can be transformed into in C(k)=[I 0]. 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: Γ(k)=F(k)ξ(k)+H(k)ΔU(k). (11) In the formula ΔU(k)=[u(k|k)u(k+1|k)…u(k+N c |k)] T , Γ(k)=[η(k+1|k)…η(k+N c |k)…η(k+N p |k)] T , 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: Where Q and R are the output weight matrix and input weight matrix of the system respectively; Given the constraints on the control inputs and their rates of change of the quadrotor, the constraints on u must be established as follows: Among them, u min and u max Respectively represent the lower and upper limits of the input rate; U max and U min denote the lower and upper limits of the input respectively; and Then, the above optimization problem is solved to obtain the control input increment u(k) at step k. Finally, the control input U(k)=U(k-1)+u(k) of time step k is calculated.

Images