A method for controlling the attitude of a quadrotor drone

Abstract

The present invention belongs to the technical field of quad-rotor unmanned aerial vehicles, and specifically relates to a method for attitude control of a quad-rotor unmanned aerial vehicle, comprising the following steps: Step 1: Establishing an attitude dynamics model of a quad-rotor unmanned aerial vehicle; Step 2: Establishing the motion equation and state equation of the control system of the quad-rotor unmanned aerial vehicle, using an improved linear expansion state observer to estimate multiple state variables of the quad-rotor unmanned aerial vehicle in real time, obtaining their corresponding estimated values, and compensating for the total disturbance of the unmanned aerial vehicle system; Step 3: According to the sliding film control theory and the Lyapunov function, based on the above improved linear expansion state observer, defining the control law of the quad-rotor unmanned aerial vehicle control system; Step 4: Establishing a third-order differential tracker to optimize the control law of the quad-rotor unmanned aerial vehicle control system. The present invention has the advantages of good dynamic performance, strong anti-interference ability and good robustness for the unmanned aerial vehicle system.

Description

Technical Field

[0001] The present invention belongs to the technical field of quad-rotor unmanned aerial vehicles, and in particular relates to a posture control method for a quad-rotor unmanned aerial vehicle. Background Art

[0002] Quadrotors have the advantages of stable flight, strong maneuverability, vertical take-off and landing, free hovering, narrow space flight and easy portability, and have become one of the research hotspots in recent years. Since quadrotors are a typical nonlinear, strongly coupled, multivariable underactuated system, in order to ensure its good control and flight effects, it is of great significance to study the flight control algorithm and improve the control performance and robustness of the aircraft.

[0003] At present, most of the research on quadrotor flight control algorithms is about attitude stability control methods, including linear control algorithms such as PID control algorithm, linear quadratic regulator (LQR) control algorithm, etc., and nonlinear control algorithms such as feedback linearization control algorithm, backstepping method, sliding mode variable structure control algorithm, H∞ control algorithm, fuzzy control algorithm, etc.

[0004] The existing control algorithms all have a certain degree of robustness, but their anti-interference ability is weak. For example, the more classic and commonly used PID control algorithm does not have an ideal control effect under strong interference. When a quadcopter performs trajectory tracking flight in a real external environment, it will be subject to multiple external interferences, as well as the uncertainty of its own parameters and internal noise interference, requiring the flight control system to have strong adaptability and anti-interference capabilities. However, in most cases, the interference to the quadcopter cannot be measured directly, or the measurement cost is too high, making interference suppression difficult to achieve. In addition, in actual operation, the traditional LADRC has the defect that it is difficult to obtain the proportional differential gain value, which will cause the controller to be unable to provide sufficient interference compensation.

[0005] In summary, the anti-interference ability and robustness of the attitude control method of a quadrotor UAV can no longer meet the needs of a quadrotor aircraft. To this end, in order to solve the interference suppression problem of a quadrotor aircraft, the present invention proposes an attitude control method for a quadrotor UAV. Summary of the invention

[0006] The purpose of the present invention is to provide a posture control method for a quad-rotor UAV, which has the advantages of good dynamic performance, strong anti-interference ability and good robustness for the UAV system and meets the needs of quad-rotor aircraft.

[0007] The technical solution adopted by the present invention is as follows:

[0008] A method for controlling the attitude of a quad-rotor drone comprises the following steps:

[0009] Step 1: Establish the attitude dynamics model of the quadrotor drone;

[0010] Step 2: Establish the motion equation and state equation of the quadrotor UAV control system, use the improved linear expansion state observer to estimate multiple state variables of the quadrotor UAV in real time, obtain their corresponding estimated values, and compensate for the total disturbance of the UAV system;

[0011] Step 3: According to the sliding film control theory and Lyapunov function, based on the above improved linear extended state observer, define the control law of the quadrotor UAV control system;

[0012] Step 4: Establish a third-order differential tracker to optimize the control law of the quadrotor UAV control system.

[0013] 2. The attitude control method of a quad-rotor drone according to claim 1, characterized in that: the step 1 comprises the following steps:

[0014] First, a dynamic model is established based on the dynamic characteristics of the quadrotor drone and the Newton-Euler equation:

[0015]

[0016] Among them, (x, y, z) are the spatial coordinates of the quadrotor drone, is the second-order derivative of x with respect to time, that is, the acceleration in the x direction. Similarly, is the acceleration in the y direction, is the acceleration in the z direction; m is the mass of the quadrotor drone, b is the lift coefficient, g is the acceleration of gravity, ω is the rotor speed, φ, θ, ψ are the roll angle, pitch angle, and heading angle, respectively. are the angular velocities of the body around the x-axis, y-axis, and z-axis, respectively. are the angular accelerations of the body around the x-axis, y-axis, and z-axis, respectively. x , I y , I z ) are the body's rotational inertia in the xx, yy, and zz directions, l represents the distance from the body center to the motor rotor, and d represents the anti-torque coefficient; the unmodeled parts of the spatial three-axis x, y, and z channels and the roll angle φ, pitch angle θ, and heading angle ψ channels, as well as model uncertainty information, are represented by Δx, Δy, Δz, Δφ, Δθ, and Δψ, respectively;

[0017] Then build the power model of the drone:

[0018]

[0019] in, is the corresponding motor speed, b is the lift coefficient, l represents the distance from the center of the fuselage to the motor rotor, U1 is the total lift T brought by the four motor-driven propellers, U2, U3 and U4 are the x, y and z axis torques, and d represents the anti-torque coefficient.

[0020] Furthermore, the step 2 includes the following steps:

[0021] First, we design an improved linear state observer and combine equations (1) and (2) in step 1 to obtain:

[0022]

[0023] in, is the total disturbance of each channel in the inner loop, (Δφ, Δθ, Δψ) represent the uncertain disturbance terms in the system respectively;

[0024] According to formula (3), the motion equation of the second-order control system of the quadrotor drone can be expressed as:

[0025]

[0026] Among them, y is the output, u is the input, f is the system disturbance, and b0 is the system control gain;

[0027] Define x1=y, x3=f, x3 is the expanded state variable of the second-order control system, and we can get:

[0028]

[0029] Among them, x1, x2, x3 are state variables. From equation (5), the state space equation can be obtained as follows:

[0030]

[0031] in, C = [1,0,0];

[0032] The corresponding state observer is:

[0033]

[0034] Where, L = [β1 β2 β3] T is the error feedback gain matrix of the linear extended state observer (LESO); and the characteristic equation of the gain of the linear extended state observer is shown in equation (8):

[0035] λ=s 3 +β1s 2+β2s+β3=(s+ω0) 3 (8)

[0036] Then the gain matrix is:

[0037]

[0038] Then the LESO formula is:

[0039]

[0040] Among them, z1, z2, z3 are the estimated values ​​of x1, x2, x3, and β1β2β3 are the observer gains;

[0041] In order to make the control system more stable, the above linear extended state observer (LESO) can be improved. The transfer function can be obtained from equation (8):

[0042] G(s)=β3 / (s 3 +β1s 2 +β2s+β3) (11)

[0043] This third-order system is approximated as a second-order system in the mid-frequency band for analysis:

[0044]

[0045] From formula (12), we can see that compared with β1 and β2, the design of β3 has a greater impact on system performance; therefore, the improvement of β3 is as follows:

[0046] β3=β4(1+β5s) (13)

[0047] Among them, β4 and β5 are conventional parameters, which are proportional derivatives of the improved design of β3. The values ​​of β4 and β5 need to be adjusted to obtain appropriate values;

[0048] From the above formula, the new transfer function is:

[0049]

[0050] Furthermore, the step three includes the following steps:

[0051] First, according to the improved extended state observer, a sliding mode attitude controller based on the state observer is designed. Combined with the above formula, the observation error is defined as: e1 = z1-y, then ILESO is:

[0052]

[0053] Let the expected value of y be y d , the designed sliding surface is:

[0054]

[0055] Where e represents the difference between the system output y and the expected output;

[0056] Assume the Lyapunov function of the system is:

[0057]

[0058] Combining equation (3), equation (15) and equation (16), we can get

[0059]

[0060] Where, for the φ and θ channels: I = I x ; For the ψ channel: I = I z ;

[0061] In order to satisfy the sliding mode existence and approximation conditions, the following switching control law is selected:

[0062]

[0063] According to sliding mode control theory, reducing η and increasing k can accelerate the convergence process;

[0064] Among them, η and k are constant parameters;

[0065] Then, from equations (18) and (19), the synovial control law based on the auxiliary system can be derived as:

[0066]

[0067] Then formula (18) can be transformed into:

[0068]

[0069] in,

[0070] Furthermore, the step 4 includes the following steps:

[0071] In the presence of noise and interference, for the control law shown in equation (20), a third-order TD is designed according to the above control law to realize the derivative calculation of its input signal;

[0072] The third-order TD in step 4 is expressed as:

[0073]

[0074] Among them, R and δ are the parameters to be adjusted; u d is the input signal to be tracked; They are the tracking value of the input instruction, the derivative of the input instruction, and the second-order derivative of the input instruction;

[0075] Therefore, the control law of the attitude loop becomes:

[0076]

[0077] Among them, u9t) is the output value of the SMC controller and also the control law of the attitude loop. is the observed value of S.

[0078] The technical effects achieved by the present invention are:

[0079] The invention discloses a method for attitude control of a quad-rotor unmanned aerial vehicle (UAV) and designs a sliding mode attitude controller (SMC) with strong robustness. On the basis of the SMC, an improved linear state observer (ILESO) with proportional differential gain is introduced, which not only performs external observation and compensation on interference, but also reduces peak time and improves the response speed of the system, so that the system response speed is faster. By designing a third-order differential tracker, auxiliary sliding mode control and ILESO, not only the accuracy and response speed of the UAV attitude control system are improved, but also the system's resistance to noise and interference is enhanced, so that the UAV can maintain stable and precise attitude control under various flight conditions. The method has the advantages of good dynamic performance, strong anti-interference ability and good robustness for the UAV system, and meets the needs of quad-rotor aircraft. BRIEF DESCRIPTION OF THE DRAWINGS

[0080] Figure 1 is a flow chart of the present invention;

[0081] Figure 2 It is a structural block diagram of the controller of the present invention. DETAILED DESCRIPTION

[0082] In order to make the purpose and advantages of the present invention more clearly understood, the present invention is specifically described below in conjunction with embodiments. It should be understood that the following text is only used to describe one or several specific embodiments of the present invention, and does not strictly limit the scope of protection of the specific claims of the present invention.

[0083] Aiming at the interference suppression problem of quadrotor aircraft, the present invention proposes an adaptive sliding mode control (SMC) based on improved linear state observer (ILESO).

[0084] First, in order to solve the problem of jitter caused by inaccurate models and interference to the aircraft, the present invention designs a sliding mode control with strong robustness to solve the nonlinear, under-actuated and strong coupling problems of the quadrotor aircraft. On the basis of SMC, an improved linear state observer (ILESO) is introduced to observe and compensate for interference externally, improve the control accuracy and anti-interference ability of the control system, and solve the problem that when the sliding mode is used as the control strategy, the controller generates strong sliding mode jitter under strong external interference, making it difficult to perform self-stabilization control of the aircraft and causing flight accidents.

[0085] In addition, based on ILESO, this technical solution designs a control law for the UAV based on SMC-ILESO attitude control according to the synovial film control theory and Lyapunov function, thereby optimizing the system model and simplifying the design of the controller.

[0086] In order to achieve high-precision SMC-ILESO control, the necessary derivative information is provided. For more precise control, this technical solution designs a third-order differential tracker to assist the synovial control and ILESO, which improves the UAV control system's resistance to noise and interference. And in this way, it helps the controller to more accurately adjust the UAV's attitude to achieve the predetermined flight path and attitude, so that the UAV control system can more accurately predict and respond to attitude changes.

[0087] like Figure 1-2 As shown, a method for attitude control of a quad-rotor drone comprises the following steps:

[0088] Step 1: Establish the attitude dynamics model of the quadrotor drone;

[0089] Specifically, step one includes the following steps:

[0090] First, a dynamic model is established based on the dynamic characteristics of the quadrotor drone and the Newton-Euler equation:

[0091]

[0092] Among them, (x, y, z) are the spatial coordinates of the quadrotor drone, is the second-order derivative of x with respect to time, that is, the acceleration in the x direction. Similarly, is the acceleration in the y direction, is the acceleration in the z direction; m is the mass of the quadrotor drone, b is the lift coefficient, g is the acceleration of gravity, ω is the rotor speed, φ, θ, ψ are the roll angle, pitch angle, and heading angle, respectively. are the angular velocities of the body around the x-axis, y-axis, and z-axis, respectively. are the angular accelerations of the body around the x-axis, y-axis, and z-axis, respectively.x , I y , I z ) are the body’s moments of inertia in the xx, yy, and zz directions, l represents the distance from the body center to the motor rotor, and d represents the back-torque coefficient; the unmodeled parts of the six channels including the three spatial axes x, y, and z and the roll angle φ, pitch angle θ, and heading angle ψ, as well as model uncertainty information, are represented by Δx, Δy, Δz, Δφ, Δθ, and Δψ, respectively.

[0093] Then, build the power model of the drone:

[0094]

[0095] in, is the corresponding motor speed, b is the lift coefficient, l represents the distance from the center of the fuselage to the motor rotor, U1 is the total lift T brought by the four motor-driven propellers, U2, U3 and U4 are the x, y and z axis torques, and d represents the anti-torque coefficient.

[0096] Step 2: Establish the motion equation and state equation of the quadrotor UAV control system, use the improved linear expansion state observer to estimate multiple state variables of the quadrotor UAV in real time, obtain their corresponding estimated values, and compensate for the total disturbance of the UAV system;

[0097] Specifically, step 2 includes the following steps:

[0098] First, we design an improved linear state observer. Combining equation (1) and equation (2), we can get:

[0099]

[0100] in, is the total interference of each channel in the inner loop, and (Δφ, Δθ, Δψ) represent the uncertain disturbance terms inside the system.

[0101] According to formula (3), the motion equation of the second-order control system of the quadrotor drone can be expressed as:

[0102]

[0103] Among them, y is the output, u is the input, f is the system disturbance, and b0 is the system control gain.

[0104] Redefine x1=y, x3=f, x3 is the expanded state variable of the second-order control system, and we can get:

[0105]

[0106] Among them, x1, x2, x3 are state variables. From equation (5), the state space equation can be obtained as follows:

[0107]

[0108] in, C = [1,0,0];

[0109] The corresponding state observer is:

[0110]

[0111] Where, L = [β1 β2 β3] T is the error feedback gain matrix of the linear extended state observer (LESO). The characteristic equation of the gain of the linear extended state observer is shown in equation (8):

[0112] λ=s 3 +β1s 2 +β2s+β3=(s+ω0) 3 (8)

[0113] Then the gain matrix is:

[0114]

[0115] Then the LESO formula is:

[0116]

[0117] Among them, z1, z2, z3 are the estimated values ​​of x1, x2, x3, and β1β2β3 are the observer gains.

[0118] In order to make the control system more stable, the above linear extended state observer (LESO) can be improved. The transfer function can be obtained from equation (8):

[0119] G(s)=β3 / (s 3 +β1s 2 +β2s+β3) (11)

[0120] This transfer function is a third-order system. It is difficult to analyze theoretically, and the intermediate frequency band of the frequency characteristics is similar to a second-order system. Therefore, this third-order system is approximated as a second-order system in the intermediate frequency band for analysis:

[0121]

[0122] From equation (12), we can see that compared with β1 and β2, the design of β3 has a greater impact on system performance. Therefore, this paper improves β3 and designs a LADRC based on proportional-differential terms. The improvement of β3 is as follows:

[0123] β3=β4(1+β5s) (13)

[0124] Among them, β4 and β5 are conventional parameters, which are proportional derivatives of the improved design of β3. The values ​​of β4 and β5 need to be adjusted to obtain appropriate values;

[0125] From the above formula, the new transfer function is:

[0126]

[0127] The transfer function of the improved disturbance observer adds zero to the closed loop. From the time domain analysis, the role of the zero point is to reduce the peak time and improve the response speed of the system; from the frequency domain analysis, it is equivalent to connecting a lead network in series to reduce the amplitude drop and phase lag.

[0128] Step 3: According to the sliding film control theory and Lyapunov function, based on the above improved linear extended state observer, define the control law of the quadrotor UAV control system;

[0129] Specifically, step three includes the following steps:

[0130] According to the improved extended state observer, a sliding mode attitude controller based on the state observer is designed. Combined with the above formula, the observation error is defined as: e1 = z1-y, then ILESO is:

[0131]

[0132] Let the expected value of y be y d , the designed sliding surface is:

[0133]

[0134] Where e represents the difference between the system output y and the expected output;

[0135] Assume the Lyapunov function of the system is:

[0136]

[0137] Combining equation (3), equation (15) and equation (16), we can get

[0138]

[0139] Where, for the φ and θ channels: I = I x ; For the ψ channel: I = I z , is the first-order derivative of V(t). Lyapunov function is a commonly used method to determine the stability of a system. The system is stable through this back-stepping control law.

[0140] In order to satisfy the sliding mode existence and approximation conditions, the following switching control law is selected:

[0141]

[0142] According to sliding mode control theory, reducing η and increasing k can accelerate the convergence process;

[0143] Among them, η and k are the same as the previous β4 and β5, which are constant parameters and are set by adjusting parameters.

[0144] Then, from equations (18) and (19), the synovial control law based on the auxiliary system can be derived as:

[0145]

[0146] Then formula (18) can be transformed into:

[0147]

[0148] in, represents η, represents any k.

[0149] It can be seen from equation (21) that when the input is equation (20), the system is stable.

[0150] Step 4: Establish a third-order differential tracker to optimize the control law of the quadrotor UAV control system.

[0151] Step 4 includes the following steps:

[0152] In the presence of noise and interference, for the control law shown in equation (20), in order to accurately estimate the derivative information of the system and improve the performance of the control system, it is necessary to design a third-order TD according to the above control law to realize the derivative calculation of its input signal. The third-order TD is as follows:

[0153]

[0154] Among them, R and δ are the parameters to be adjusted; u d is the input signal to be tracked; are the tracking value of the input command, the derivative of the input command, and the second-order derivative of the input command. Therefore, the control law of the attitude loop becomes:

[0155]

[0156] Among them, u(t) is the output value of the SMC controller and also the control law of the attitude loop. is the observed value of S.

[0157] The present invention designs a sliding mode attitude controller (SMC) with strong robustness, and introduces an improved linear state observer (ILESO) with proportional differential gain on the basis of SMC, which not only performs external observation and compensation for interference, but also reduces peak time and improves the response speed of the system, making the system respond faster. By designing a third-order differential tracker, auxiliary sliding mode control and ILESO, not only the accuracy and response speed of the UAV attitude control system are improved, but also the system's resistance to noise and interference is enhanced, so that the UAV can maintain stable and precise attitude control under various flight conditions.

[0158] In summary, for UAV systems, the present invention has the advantages of good dynamic performance, strong anti-interference ability and good robustness.

[0159] The above is only a preferred embodiment of the present invention. It should be noted that, for those skilled in the art, several improvements and modifications can be made without departing from the principles of the present invention, and these improvements and modifications should also be considered as the protection scope of the present invention. The structures, devices and operating methods not specifically described and explained in the present invention shall be implemented according to the conventional means in the art unless otherwise specified and limited.

Claims

1. A method for controlling the attitude of a quad-rotor drone, characterized in that: The following steps are involved: Step 1: Establish the attitude dynamics model of the quadrotor drone; Step 2: Establish the motion equation and state equation of the quadrotor UAV control system, use the improved linear expansion state observer to estimate multiple state variables of the quadrotor UAV in real time, obtain their corresponding estimated values, and compensate for the total disturbance of the UAV system; Step 3: According to the sliding film control theory and Lyapunov function, based on the above improved linear extended state observer, define the control law of the quadrotor UAV control system; Step 4: Establish a third-order differential tracker to optimize the control law of the quadrotor UAV control system; The step 1 comprises the following steps: Firstly, a dynamic model is established based on the dynamic characteristics of the quadrotor drone and the Newton-Euler equations; Then build the power model of the drone; The kinetic model in step 1 is expressed as: in, is the spatial coordinate of the quadrotor drone, for The second-order derivative with respect to time is The acceleration in the direction of for The acceleration in the direction, for Acceleration in direction; For the quality of the quadcopter, is the lift coefficient, is the acceleration due to gravity, is the rotor speed, They are roll angle, pitch angle, and heading angle, The body is axis, axis, The angular velocity of the axis, The body is axis, axis, The angular acceleration of the axis, The body Direction moment of inertia, Represents the distance from the center of the machine body to the motor rotor, Represents the anti-torque coefficient; three-axis space Channel and roll angle , Pitch angle , heading angle The unmodeled part of the channel and the model uncertainty information are respectively express.

2. The attitude control method of a quad-rotor drone according to claim 1, characterized in that: The power model of the UAV in step 1 is expressed as: in, is the corresponding motor speed, is the lift coefficient, Represents the distance from the center of the machine body to the motor rotor, Total lift for the four motor-driven propellers , and for Three-axis torque, Represents the anti-torque coefficient.

3. The attitude control method of a quad-rotor drone according to claim 2, characterized in that: The step 2 comprises the following steps: First, we design an improved linear state observer. Combining equations (1) and (2) in step 1, we can get: in, , , , is the total interference of each channel in the inner loop, They represent the uncertain disturbance terms in the system respectively. According to equation (3), the motion equation of the second-order control system of the quadrotor drone can be expressed as: in, For output, For input, is the system disturbance, is the system control gain; definition is the extended state quantity of the second-order control system, we can get: in, is the state variable, and the state space equation can be obtained from equation (5): in, , ; ; ; The corresponding state observer is: in, is the error feedback gain matrix of the linear extended state observer; and the characteristic equation of the gain of the linear extended state observer is shown in equation (8): Then the gain matrix is: Then the linear extended state observer formula can be obtained as: in, for The estimated value of is the observer gain; and in order to make the control system more stable, the above linear extended state observer can be improved. From equation (8), the transfer function for: This third-order system is approximated as a second-order system in the mid-frequency band for analysis: in, is the complex frequency variable after Laplace transformation; from formula (12), we can see that compared to, The design has a greater impact on system performance; therefore, The improvements are as follows: in, are conventional parameters; from the above formula, the new transfer function is: .

4. The attitude control method of a quad-rotor drone according to claim 3, characterized in that: The step three comprises the following steps: First, according to the improved extended state observer, a sliding mode attitude controller based on the state observer is designed. Combined with the above formula, the observation error is defined as: ,but for: make The expected value of , the designed sliding surface is: in, Indicates system output The difference between the expected output and the expected output; suppose the Lyapunov function of the system is: Combining equations (3), (15) and (16), we can get Among them, for and aisle: ;for aisle: In order to satisfy the sliding mode existence and approximation conditions, the following switching control law is selected: According to sliding mode control theory, reducing η and increasing k can accelerate the convergence process; is a constant parameter; then, according to equations (18) and (19), the synovial control law based on the auxiliary system can be derived as: Then formula (18) can be transformed into: in, .

5. The attitude control method of a quad-rotor drone according to claim 4, characterized in that: The step 4 comprises the following steps: In the presence of noise and interference, for the control law shown in equation (20), the third-order To realize the derivative calculation of its input signal.

6. The attitude control method of a quad-rotor drone according to claim 5, characterized in that: The third level in step 4 It is expressed as: in, and is the parameter to be adjusted; is the input signal to be tracked; are the tracking value of the input command, the derivative of the input command, and the second-order derivative of the input command; therefore, the control law of the attitude loop becomes: in, yes The output value of the controller is also the control law of the attitude loop. yes Observed value of .

Images