A Trajectory Tracking Control System for Quadrotor UAVs Based on Improved Active Disturbance Rejection

Abstract

The present invention discloses a trajectory tracking control system for a quadrotor UAV based on improved active disturbance rejection. It includes: a modeling unit, a parameter setting unit, a control unit, a reference rotational speed calculation unit, and a trajectory tracking control unit; the control unit includes a position outer loop module and an attitude inner loop module. In the present invention, the trajectory tracking control of the UAV is divided into inner and outer loop controls. The outer loop is position control, and the inner loop is attitude control. In the control process, the outer loop adopts nonlinear active disturbance rejection control, and the inner loop adopts model predictive control, which improves the resistance ability of the UAV to internal and external disturbances. Due to the defect of phase lag in active disturbance rejection control, a phase compensator is adopted to compensate for the lagging phase of the outer loop, so as to improve the control accuracy of the outer loop controller, thereby ensuring that the UAV can track and fly with high precision according to the given trajectory.

Description

Technical Field

[0001] The present invention relates to the technical field of trajectory control of wingless drones, and particularly to a trajectory tracking control system for a quadrotor drone based on improved active disturbance rejection. Background Art

[0002] A quadrotor drone has a cross-shaped frame structure. The frame has four arms, and a motor is installed at the end of each arm. By adjusting the rotational speeds of the four motors, the position and attitude of the quadrotor drone can be adjusted. Compared with other aircraft, the quadrotor drone has the advantages of low cost, high reliability, vertical takeoff and landing ability, and hovering ability, and is widely used in fields such as aerial photography, plant protection, fire fighting, and power line inspection. However, due to the complex working environment of the drone and the characteristics of high nonlinearity, strong coupling, and underactuation of the drone itself, the drone faces many difficulties in the process of precise trajectory tracking control. Summary of the Invention

[0003] The purpose of the present invention is to provide a trajectory tracking control system for a quadrotor drone based on improved active disturbance rejection to ensure that the drone tracks and flies with high precision according to a given trajectory.

[0004] To achieve the above purpose, the present invention provides the following solutions:

[0005] A trajectory tracking control system for a quadrotor drone based on improved active disturbance rejection, comprising:

[0006] A modeling unit for performing mathematical modeling on the quadrotor drone and linearizing it to obtain a drone linear model;

[0007] A parameter setting unit for setting the x-direction reference trajectory, y-direction reference trajectory, z-direction reference trajectory, and reference yaw angle of the quadrotor drone;

[0008] A control unit including a position outer loop module and an attitude inner loop module. The position outer loop module is used to calculate the pitch angle according to the x-direction reference trajectory, calculate the roll angle according to the y-direction reference trajectory, and calculate a first virtual input according to the z-direction reference trajectory based on the drone linear model; the attitude inner loop module is used to calculate a second virtual input, a third virtual input, and a fourth virtual input based on the drone linear model according to the roll angle, the pitch angle, and the reference yaw angle.

[0009] A reference rotational speed calculation unit for calculating the reference rotational speed according to the first virtual input, the second virtual input, the third virtual input, and the fourth virtual input;

[0010] A trajectory tracking control unit, which is used to calculate the position and attitude of the drone according to the reference rotational speed by using the kinematic model and dynamic model of the drone, so as to realize the trajectory tracking control of the quadrotor drone.

[0011] Optionally, the expression of the drone linear model is as follows:

[0012]

[0013] where m is the mass of the drone, I x 、I y 、I z are the moments of inertia of the drone in the x, y, and z directions, g is the acceleration due to gravity, φ is the roll angle, θ is the pitch angle, is the roll angular acceleration, is the pitch angular acceleration, is the yaw angular acceleration, is the acceleration in the x direction, is the acceleration in the y direction, is the acceleration in the z direction, and U1, U2, U3, and U4 are the first virtual input, the second virtual input, the third virtual input, and the fourth virtual input respectively.

[0014] Optionally, the position outer loop module includes:

[0015] An x-direction position controller, which is used to calculate the pitch angle based on the drone linear model according to the x-direction reference trajectory;

[0016] A y-direction position controller, which is used to calculate the roll angle based on the drone linear model according to the y-direction reference trajectory;

[0017] A z-direction position controller, which is used to calculate the first virtual input based on the drone linear model according to the z-direction reference trajectory.

[0018] Optionally, the x-direction position controller, the y-direction position controller, and the z-direction position controller all adopt active disturbance rejection controllers.

[0019] Optionally, the attitude inner loop module adopts a model predictive controller.

[0020] Optionally, the active disturbance rejection controller includes a tracking differentiator, an extended observer, a nonlinear control law, and a phase compensator.

[0021] Optionally, the phase compensator adopts a phase lead compensator combined with a fal function filter.

[0022] Optionally, the calculation formula of the reference rotational speed is as follows:

[0023]

[0024] Among them, ω1, ω2, ω3, and ω4 are the reference rotational speeds of the four propellers respectively, b is the lift coefficient of the rotor, d is the drag coefficient of the rotor, l is the distance from the rotor axis to the center of mass of the UAV, and U1, U2, U3, and U4 are the first virtual input, the second virtual input, the third virtual input, and the fourth virtual input respectively.

[0025] The present invention also provides a trajectory tracking control method for a quadrotor UAV based on improved active disturbance rejection, including:

[0026] Conduct mathematical modeling on the quadrotor UAV;

[0027] Obtain the reference trajectory in the x direction, the reference trajectory in the y direction, the reference trajectory in the z direction, and the reference yaw angle of the quadrotor UAV;

[0028] According to the reference trajectory in the x direction, the reference trajectory in the y direction, and the reference trajectory in the z direction, use an active disturbance rejection controller to calculate the pitch angle, roll angle, and the first virtual input respectively;

[0029] According to the roll angle, the pitch angle, and the reference yaw angle, use a model predictive controller to calculate the second virtual input, the third virtual input, and the fourth virtual input respectively;

[0030] Calculate the reference rotational speed according to the first virtual input, the second virtual input, the third virtual input, and the fourth virtual input;

[0031] According to the reference rotational speed, calculate the direction position and attitude position of the UAV based on the quadrotor UAV mathematical model, and realize the trajectory tracking control of the quadrotor UAV.

[0032] According to the specific embodiments provided by the present invention, the following technical effects are disclosed by the present invention:

[0033] In the present invention, the trajectory tracking control of the UAV is divided into inner and outer loop controls. The outer loop is position control, and the inner loop is attitude control. In the control process, the outer loop uses nonlinear active disturbance rejection control, and the inner loop uses model predictive control, which improves the resistance ability of the UAV to internal and external disturbances. Due to the defect of phase lag in active disturbance rejection control, a phase compensator is used to compensate for the lagged phase of the outer loop to improve the control accuracy of the outer loop controller, so as to ensure that the UAV can track and fly with high precision according to the given trajectory. Description of the Drawings

[0034] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the accompanying drawings required in the embodiments. Obviously, the accompanying drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other accompanying drawings can also be obtained based on these drawings.

[0035] Figure 1 This is the structural block diagram of the trajectory tracking control system of a quadrotor UAV based on improved active disturbance rejection control (ADRC).

[0036] Figure 2 This is the control flowchart of the UAV.

[0037] Figure 3 This is the schematic diagram of the control unit.

[0038] Figure 4 This is the schematic diagram of the ADRC position controller.

[0039] Figure 5 This is the original signal phase compensator.

[0040] Figure 6 This is the differential signal phase compensator. Specific embodiments

[0041] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, rather than all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts belong to the scope of protection of the present invention.

[0042] The purpose of the present invention is to provide a trajectory tracking control system for a quadrotor UAV based on improved active disturbance rejection control to ensure that the UAV tracks and flies with high precision according to a given trajectory.

[0043] To make the above objects, features, and advantages of the present invention more obvious and understandable, the present invention will be further described in detail below in conjunction with the accompanying drawings and specific embodiments.

[0044] As Figure 1 shown, a trajectory tracking control system for a quadrotor UAV based on improved active disturbance rejection control provided by the present invention includes: a modeling unit 1, a parameter setting unit 2, a control unit 3, a reference rotational speed calculation unit 4, and a trajectory tracking control unit 5.

[0045] The modeling unit 1 is used to perform mathematical modeling on the quadrotor UAV and perform linearization processing to obtain a UAV linear model.

[0046] The non - linear mathematical model of a quad - rotor UAV is as follows:

[0047]

[0048] Where m is the mass of the UAV, I x 、I y 、I z are the moments of inertia of the UAV in the x, y, and z directions, I r is the moment of inertia of the rotor, g is the acceleration due to gravity, ω sum is the algebraic sum of the rotational speeds of the four propellers, is the roll angular velocity, is the pitch angular velocity, is the yaw angular velocity, is the velocity in the x - direction, is the velocity in the y - direction, is the velocity in the z - direction, is the roll angular acceleration, is the pitch angular acceleration, is the yaw angular acceleration, is the acceleration in the x - direction, is the acceleration in the y - direction, is the acceleration in the z - direction, and U1, U2, U3, and U4 are the first virtual input, the second virtual input, the third virtual input, and the fourth virtual input respectively.

[0049]

[0050] Among them, ω1, ω2, ω3, and ω4 are the rotational speeds of the four propellers respectively, b is the lift coefficient of the rotor, d is the drag coefficient of the rotor, and l is the distance from the rotor shaft to the center of mass of the UAV.

[0051] Since the non - linear model is too complex, in order to facilitate the design of the control unit 3, the model is linearized according to the small - angle assumption:

[0052]

[0053] After obtaining the linear model of the UAV, the control unit 3 of the UAV can be designed according to the linear model. It can be known from the 4th equation of formula (3) that the position in the x - direction and the pitch angle θ are coupled, and from the 5th equation that the position in the y - direction and the roll angle Φ are coupled, while there is no coupling relationship between the position in the z - direction and the yaw angle ψ. Therefore, when designing the control unit 3 of the present invention, the coupled x and θ, y and Φ should be jointly controlled, and z and ψ should be separately controlled. The control unit 3 is as shown in Figure 3 shown, and the control flow is as shown in Figure 2 shown.

[0054] The control unit 3 mainly consists of a position outer loop module and an attitude inner loop module. The position outer loop module includes an x-direction position controller, a y-direction position controller, and a z-direction position controller. The parameter setting unit 2 sets the reference trajectory x in the x direction ref and the reference trajectory y in the y direction ref . After passing through the x-direction position controller and the y-direction position controller respectively, the reference pitch angle θ ref and the reference roll angle φ ref can be calculated. The parameter setting unit 2 sets the reference trajectory z in the z direction ref . After passing through the z-direction position controller, the virtual input U1 can be calculated. The reference roll angle φ ref , the reference pitch angle θ ref generated after passing through the position controller, and the reference yaw angle ψ set by the parameter setting unit 2 ref . After passing through the attitude inner loop module, the virtual inputs U2, U3, and U4 can be calculated respectively. The four virtual inputs U1, U2, U3, and U4 pass through the control allocator, and the reference rotational speeds of the four propellers of the UAV can be obtained through the reference rotational speed calculation unit 4. The trajectory tracking control unit 5 uses the kinematic model and dynamic model of the UAV to calculate the positions (x, y, z) in three directions and the attitude (φ, θ, ψ) of the UAV according to the rotational speeds of the propellers. Feeding the calculated positions and attitudes back to the position controller and the attitude inner loop module respectively can obtain the closed-loop control of the UAV.

[0055] The position controller adopts an active disturbance rejection control (ADRC) controller. Since it is not convenient to design the ADRC controller by considering all positions together, the position controllers in each direction need to be designed separately, so there are three position controllers. For attitude control, since model predictive control is adopted, it is very convenient to consider the three attitudes together (forming a matrix) when designing the controller, so there is only one attitude controller. According to the selected controller, the schematic diagram of the control unit as shown in Figure 3 can be obtained.

[0056] Figure 3 is the same in essence as Figure 2 , except that when designing the attitude controller, the three attitudes are jointly considered and designed into an MPC attitude controller. Although the position in the x direction of the UAV is coupled with the pitch angle θ, the position in the y direction is coupled with the roll angle Φ, and the position in the z direction is decoupled from the yaw angle, for the position controller, the ADRC controller is adopted, and the design process of the controller is the same, only the inputs and outputs of the controller are different. Therefore, when introducing the position controller, the present invention takes the position controller in the x direction as an example.

[0057] 1. The design of the x-direction position controller is as follows:

[0058] The outer - loop position controller mainly adopts active disturbance rejection control (ADRC). The structure of the controller is as follows Figure 4 shown. The ADRC controller consists of three parts: the tracking - differentiator (TD), the extended state observer (ESO), and the non - linear state error feedback (NLSEF). Due to the defect of phase lag in active disturbance rejection control, a phase compensator is added to the structure of the ADRC controller to compensate for the lagging phase.

[0059] As Figure 4 shown in the schematic diagram of the position controller, the reference position \(x\) in the \(x\) - direction ref After being input into the TD, the original signal \(x1\) of the reference position and the differential signal \(x2\) of the reference position can be obtained. After \(x1\) and \(x2\) pass through the original - signal phase compensator and the differential - signal phase compensator, the compensated original signal and the compensated differential signal can be obtained. The ESO will observe the state \(z1\), \(z2\) of the UAV and the total disturbance \(z3\) according to the UAV position \(x\) calculated by the UAV mathematical model and the reference pitch angle \(\theta\) calculated by the position controller. ref The error \(e1\) of the original signal can be obtained by subtracting \(z1\) from \(x1\), and the error \(e2\) of the differential signal can be obtained by subtracting \(z2\) from \(x2\). After \(e1\) and \(e2\) are combined and calculated by the NLSEF, the control quantity \(u0\) is obtained. After compensating the observed disturbance \(z3\) with the control quantity, the reference input \(\theta\) of the attitude controller can be finally obtained. ref .

[0060] Next, the design methods of the mathematical models of TD, phase compensator, ESO, and NLSEF will be introduced in turn.

[0061] First, select the equation in the \(x\) - direction in Equation (3) After introducing the disturbance, the control model in the \(x\) - direction can be obtained:

[0062]

[0063] In the formula is the total disturbance. Select the state variables Convert Equation (4) into a state equation:

[0064]

[0065] where \(x1\) is the position in the \(x\) - direction, \(x2\) is the differential of the position in the \(x\) - direction, and \(y\) represents the output, that is, the position in the \(x\) - direction.

[0066] 1.1 Design of the tracking - differentiator (TD):

[0067] When designing the tracking - differentiator, the discrete fastest feedback system established according to the fastest synthesis function fhan(x1,x2,r0,h0) is as follows

[0068]

[0069] where fhan(x1, x2, r0, h0) is

[0070]

[0071] Equation (6) represents the mathematical model of the tracking differentiator (TD). In the equation, v represents the input of the tracking differentiator, which is the reference position x in the x direction ref , x1 and x2 are the outputs of the tracking differentiator, representing the original signal and the differential signal of the reference signal extracted by the tracking differentiator respectively. r0 and h0 are the parameters of the TD model, and h represents the sampling period.

[0072] 1.2 Design of Phase Compensator

[0073] Since the tracking differentiator (TD) in the active disturbance rejection control will filter the tracked signal, resulting in a phase delay between the output signal and the input signal of the tracking differentiator (TD). Therefore, in order to improve the performance of trajectory tracking, it is necessary to perform phase compensation on the signal after passing through the tracking differentiator. A phase lead compensator combined with a fal function filter is used for phase compensation. The basic principle of the compensator is to predict the differential signal obtained after passing through the tracking differentiator forward by λ time to compensate the original signal. Similarly, there is also a certain phase delay in the differential signal, and the same method is used to compensate the differential signal. The schematic diagram of the compensator is as Figure 5 、 Figure 6 shown.

[0074] Figure 5 is the original signal phase compensator. The differential signal x2 of the reference signal in the x direction output by TD ( Figure 4 in) passes through the fal function filter to obtain the filtered differential signal x 11 , and then it is predicted forward by λ time and compensated to the original signal x1 extracted by TD ( Figure 4 in), and finally the compensated original signal X1 can be obtained. Figure 6 is the differential signal phase compensator. Since the differential signal of the signal is required when compensating the signal, the differential signal of the differential signal is required when compensating the differential signal. Therefore, the differential signal x2 extracted by TD ( Figure 4 in) needs to pass through another tracking differentiator ( Figure 6 TD1 in) to extract the original signal x ref of the differential signal of x 21 and the differential signal x 22 of the differential signal. x 22 passes through the fal function filter to obtain the filtered differential signal x 23 , x 23The compensated value after forward prediction λ1 is compensated to x 21 Finally, the compensated x can be obtained ref and its differential signal X2. The following introduces the mathematical model of the phase compensator. The discrete form of the original signal compensator model is:

[0075]

[0076] The first three formulas in the equation are the formulas of the tracking-differentiator ( Figure 4 TD in), which have been introduced before and will not be elaborated here. x 11 represents the filtered differential signal, k, a, δ, γ are the phase compensator parameters, X1 represents the compensated original signal, and λ represents the forward prediction time.

[0077] The discrete form of the differential signal compensator model is:

[0078]

[0079] The first three formulas in the equation are Figure 4 the formulas of TD in Figure 6 and the fourth to sixth formulas are 21 the formulas of TD1 in ref x 22 represents the original signal of the differential signal of x ref x 23 represents the differential signal of the differential signal of x ref x 01 r 01 h Figure 6 is the parameter of TD1 in

[0080] 1.3 Design of Extended State Observer (ESO)

[0081] According to First, expand the total disturbance into a new state variable x3

[0082]

[0083] The new system after expansion is

[0084]

[0085] Establish a state observer for the expanded new system

[0086]

[0087] From Figure 3From the structure of the observer, it can be seen that the observer has two inputs and three outputs. The two inputs are θ ref and the position x in the x direction. Through these two inputs, three outputs z1, z2, and z3 can be observed. In the formula, ε1 represents the error between the state z1 observed by the observer and the position x in the x direction output by the UAV system. z1 and z2 are the states of the system observed by the observer, and z3 is the total disturbance observed by the observer. β 01 、β 02 、β 03 、δ are the parameters of the controller, and fal(x,a,δ) is a non-linear function, defined as follows:

[0088]

[0089] In the formula, a is a constant between 0 and 1, and δ is a constant that affects the filtering effect.

[0090] Discretizing the state observer gives:

[0091]

[0092] h represents the discretization time.

[0093] 1.4 Nonlinear control law (NLSEF) design

[0094] Based on TD and the phase compensator, we can obtain the original signal and the differential signal of the reference signal in the x direction. By subtracting the states observed by the ESO, we can obtain the error of the original signal and the error of the differential signal. The purpose of NLSEF is to combine the error signals to obtain the combined control law. The combination method can be a linear combination or a non-linear combination. However, generally, the control efficiency of the control law formed by non-linear combination is better than that of linear combination. Therefore, non-linear combination is adopted when designing the control law. The function of non-linear combination is selected as u0 = β1fal(e1,a1,δ)+β2fal(e2,a2,δ). The control law is designed as follows according to the selected non-linear combination:

[0095]

[0096] Among them, e1 is the error of the original signal, which is obtained by subtracting the state z1 observed by the ESO from the original signal X1 after compensation by the original signal phase compensator; e2 is the error of the differential signal, which is obtained by subtracting the state z2 observed by the ESO from the differential signal X2 after compensation by the differential signal phase compensator. u0 is the control quantity formed after combination, and β1, β2, β3, a1, a2, δ are parameters.

[0097] After obtaining the control quantity u0, it is necessary to compensate for the observed disturbance z3, and then obtain the control quantity after compensating for the disturbance:

[0098]

[0099] where θ ref represents the control quantity formed after compensating for the disturbance, which is also the reference input of the pitch angle attitude controller. u0 is the control quantity calculated by the nonlinear combination, and b0 is the compensation factor.

[0100] 2 Attitude Controller Design

[0101] The attitude control adopts a model predictive controller (MPC). When designing the attitude controller, the three attitudes are considered together for design. Select the attitude equation in formula (3)

[0102]

[0103] Rewrite it into the form of a state-space equation and discretize it

[0104] x(k + 1) = A k,t x(k) + B k,t u(k) (17)

[0105] where

[0106] where ΔT is the discrete sampling time. x represents the state of the system, that is, the three attitudes (φ, θ, ψ), and u represents the input of the UAV system (U2, U3, U4), which is also the output of the attitude control. J x , J y and J z represent the moments of inertia of the UAV in the x, y, and z directions respectively.

[0107] Combine x and u and expand them into a new state ξ, and set:

[0108]

[0109] where k|t represents the quantity predicted at the kth moment at the current moment t, ξ(k|t) represents the expanded state at the tth moment, x(k|t) represents the original state (φ, θ, ψ) at the tth moment, and u(k - 1|t) represents the input (U2, U3, U4) at the previous moment at the tth moment.

[0110] Therefore, a new state-space equation can be obtained

[0111]

[0112] where ξ represents the expanded state, is the system matrix, is the input matrix, Δu is the increment of the control input, η is the output, is the output matrix:

[0113] Where:

[0114]

[0115] The following assumptions are made to simplify the calculations

[0116]

[0117] If the prediction horizon of the system is N p , and the control horizon is N c , then the future output Y(t) of the system is

[0118] Y(t) = ψ t ξ(t|t) + ΘΔU(t) (22)

[0119] Where

[0120]

[0121] After obtaining the prediction equation, it is necessary to select an appropriate objective function to optimize the control increment. When selecting the objective function, the attitude tracking ability of the UAV should be considered. To ensure flight stability, the change of the control signal should not be too large. At the same time, it is necessary to ensure that the optimized objective can solve a feasible solution at each moment. Therefore, a relaxation factor needs to be added to the optimized objective.

[0122]

[0123] In the formula, η represents the actual attitude, and η ref represents the reference attitude. Q and R are weight matrices, ρ is the weight coefficient, and ε is the relaxation factor. The first term in the formula represents the tracking ability of the given trajectory, the second term is to ensure that the change of the control signal will not be too large, and the third term is the relaxation factor to ensure that the result can be solved during optimization.

[0124] According to the above mathematical derivation, the quadratic programming method can be used to calculate the control increment ΔU in the three attitude directions (which is a matrix containing the control increments in the three directions), and then the control input at the current moment (U2, U3, U4) can be calculated through the control increment and the control input u(k - 1|t) at the previous moment.

[0125] At the same time, the position controller in the z direction will calculate the control input U1 according to the input reference position in the z direction. After the controller distributes U1, U2, U3, and U4, the rotational speeds of the motors can be obtained.

[0126] Pose solution of the mathematical model

[0127] According to the virtual input, we can obtain the reference speed of the motor. Through motor control, the actual motor speed can be obtained. Based on the motor speed, the forces and torques acting on the UAV can be calculated:

[0128]

[0129] In the formula, the superscript B represents the body coordinate system, F B represents the total pulling force acting on the UAV, G = [00mg] T represents the gravity, represents the rotation matrix from the ground coordinate system to the body coordinate system, T B represents the pulling force acting on the UAV, represents the air resistance acting on the UAV. M B represents the total torque acting on the UAV, represents the gyroscopic torque, represents the torque generated by the propeller on the body axis, represents the aerodynamic torque.

[0130] The pulling force T acting on the UAV B The solution process is as follows:

[0131]

[0132] The air resistance acting on the UAV The solution process is as follows:

[0133]

[0134] In the formula, u, v, w represent the relative air flow velocities of the UAV in the x, y, and z directions, c d is the air resistance coefficient.

[0135] The gyroscopic torque The calculation process is as follows:

[0136]

[0137] In the formula, J RP represents the total moment of inertia of the motor rotor and the propeller rotating about the axis, n r represents the number of propellers, k represents the propeller number, represents the angular velocity of the k-th propeller, ω yb and ω xb respectively represent the angular velocity of rotation about the body y-axis and the angular velocity of rotation about the body x-axis.

[0138] The torque generated by the propeller on the body axis The calculation process is as follows

[0139]

[0140] The calculation process of the aerodynamic moment is as follows:

[0141]

[0142] In the formula, ω x , ω y , ω z represents the rotational speed relative to the air, which is approximately the angular velocity of rotation about the three axes of the airframe, and c dm is the damping moment coefficient.

[0143] After calculating the forces and moments acting on the UAV, the position and attitude of the UAV can be calculated according to the kinematic model and dynamic model of the UAV:

[0144]

[0145] In the formula, the superscript e represents the ground coordinate system, the superscript B represents the airframe coordinate system, p represents the position of the UAV, v represents the velocity, ω b represents the angular velocity of airframe rotation, represents the quaternion, and J represents the moment of inertia.

[0146] The present invention also provides a trajectory tracking control method for a quadrotor UAV based on improved active disturbance rejection, including:

[0147] Step 1, perform mathematical modeling on the quadrotor UAV.

[0148] Step 2, obtain the reference trajectories in the x direction, y direction, z direction of the quadrotor UAV and the reference yaw angle.

[0149] Step 3, according to the reference trajectories in the x direction, the reference trajectories in the y direction and the reference trajectories in the z direction, use an active disturbance rejection controller to calculate the pitch angle, roll angle and the first virtual input respectively.

[0150] Step 4, according to the roll angle, the pitch angle and the reference yaw angle, use a model predictive controller to calculate the second virtual input, the third virtual input and the fourth virtual input respectively.

[0151] Step 5, calculate the reference rotational speed according to the first virtual input, the second virtual input, the third virtual input and the fourth virtual input.

[0152] Step 6, according to the reference rotational speed, calculate the direction position and attitude position of the UAV based on the mathematical model of the quadrotor UAV, and realize the trajectory tracking control of the quadrotor UAV.

[0153] The present invention has the following advantages:

[0154] 1. Compared with the traditional PID control, the control system proposed by the present invention has stronger disturbance resistance and higher robustness. Reason: When controlling the drone, controllers with strong robustness are adopted for both the inner and outer loops. The active disturbance rejection control adopted for the outer loop can estimate the internal and external disturbances in real time and compensate them, improving the drone's resistance to internal and external disturbances.

[0155] 2. The trajectory tracking accuracy of the present invention is higher compared with the traditional ADRC. Reason: There is a defect of phase lag in the process of traditional ADRC control. The control system proposed by the present invention compensates for the lagging phase, improving the trajectory tracking accuracy.

[0156] In this article, specific examples are used to elaborate on the principle and implementation manner of the present invention. The description of the above embodiments is only used to help understand the method of the present invention and its core idea; at the same time, for those of ordinary skill in the art, according to the idea of the present invention, there will be changes in the specific implementation manner and application scope. In summary, the content of this specification should not be construed as a limitation to the present invention.

Claims

1. A trajectory tracking control system for a quadrotor UAV based on improved active disturbance rejection, characterized in that, including: a modeling unit configured to perform mathematical modeling on a quadrotor unmanned aerial vehicle (UAV) and linearize it to obtain a UAV linear model; a parameter setting unit configured to set an x-direction reference trajectory, a y-direction reference trajectory, a z-direction reference trajectory, and a reference yaw angle of the quadrotor UAV; a control unit including a position outer loop module and an attitude inner loop module, where the position outer loop module is configured to calculate a pitch angle based on the UAV linear model according to the x-direction reference trajectory, calculate a roll angle according to the y-direction reference trajectory, and calculate a first virtual input according to the z-direction reference trajectory; the attitude inner loop module is configured to calculate a second virtual input, a third virtual input, and a fourth virtual input based on the UAV linear model according to the roll angle, the pitch angle, and the reference yaw angle; a reference rotational speed calculation unit configured to calculate a reference rotational speed according to the first virtual input, the second virtual input, the third virtual input, and the fourth virtual input; a trajectory tracking control unit configured to calculate the position and attitude of the UAV according to the reference rotational speed by using the kinematic model and dynamic model of the UAV, so as to implement trajectory tracking control of the quadrotor UAV; wherein, the position outer loop module includes an x-direction position controller, a y-direction position controller, and a z-direction position controller; the x-direction position controller, the y-direction position controller, and the z-direction position controller all adopt an active disturbance rejection controller; the active disturbance rejection controller includes a tracking differentiator, an extended observer, a nonlinear control law, and a phase compensator; The reference signal x in the x direction of the tracking-differentiator output ref The differentiated signal x2 passes through the fal function filter to obtain the filtered differentiated signal x 11 , and then it is predicted forward by λ time and compensated to the original signal x1 extracted by the tracking-differentiator. Finally, the compensated original signal X1 can be obtained; the differentiated signal x2 extracted by the tracking-differentiator needs to pass through the tracking-differentiator once again to extract the original signal x ref of the differentiated signal 21 and the differentiated signal of the differentiated signal x 22 , x 22 After passing through the fal function filter, the filtered differentiated signal x can be obtained 23 , x 23 After forward prediction and compensation by λ1, it is compensated to x 21 to finally obtain the compensated differentiated signal X2; the attitude inner loop module adopts a model predictive controller, and when designing the attitude controller, three attitudes are considered together for design, and the attitude equation is selected as: the state space equation corresponding to the attitude equation is: where ξ represents the expanded state, is the system matrix, is the input matrix, Δu is the increment of the control input, and η is the output, is the output matrix; If the prediction horizon of the system is N p , and the control horizon is N c , then the future output Y(t) of the system is: Y(t) = ψ t ξ(t|t) + ΘΔU(t) after obtaining the prediction equation, it is necessary to select an appropriate objective function to optimize the control increment, and a relaxation factor needs to be added to the optimized objective; where η represents the actual attitude, and η ref represents the reference attitude, Q and R are weight matrices, ρ is the weight coefficient, and ε is the relaxation factor.

2. The trajectory tracking control system of a quadrotor UAV based on improved active disturbance rejection according to claim 1, wherein the expression of the UAV linear model is as follows: where m is the mass of the unmanned aerial vehicle, and I x , I y , I z are the moments of inertia of the unmanned aerial vehicle in the x, y, and z directions, g is the acceleration due to gravity, φ is the roll angle, θ is the pitch angle, is the roll angular acceleration, is the pitch angular acceleration, is the yaw angular acceleration, is the acceleration in the x direction, is the acceleration in the y direction, is the acceleration in the z direction, and U1, U2, U3, and U4 are the first virtual input, the second virtual input, the third virtual input, and the fourth virtual input, respectively.

3. The trajectory tracking control system of a quadrotor UAV based on improved active disturbance rejection according to claim 1, characterized in that, the position outer loop module includes: an x-direction position controller configured to calculate a pitch angle based on the UAV linear model according to the x-direction reference trajectory; a y-direction position controller configured to calculate a roll angle based on the UAV linear model according to the y-direction reference trajectory; a z-direction position controller configured to calculate a first virtual input based on the UAV linear model according to the z-direction reference trajectory.

4. The trajectory tracking control system of a quadrotor UAV based on improved active disturbance rejection according to claim 1, wherein, The phase compensator adopts a phase lead compensator combined with a fal function filter.

5. The trajectory tracking control system of a quadrotor UAV based on improved active disturbance rejection according to claim 1, characterized in that, The calculation formula of the reference rotational speed is as follows: wherein, ω1, ω2, ω3, and ω4 are the reference rotational speeds of four propellers respectively, b is the lift coefficient of the rotor, d is the drag coefficient of the rotor, l is the distance from the rotor shaft to the center of mass of the UAV, and U1, U2, U3, and U4 are the first virtual input, the second virtual input, the third virtual input, and the fourth virtual input respectively.

6. A trajectory tracking control method for a quadrotor UAV based on improved active disturbance rejection, characterized in that including: performing mathematical modeling on a quadrotor UAV; obtaining an x-direction reference trajectory, a y-direction reference trajectory, a z-direction reference trajectory, and a reference yaw angle of the quadrotor UAV; According to the x-direction reference trajectory, the y-direction reference trajectory, and the z-direction reference trajectory, an active disturbance rejection controller is used to calculate the pitch angle, the roll angle, and the first virtual input respectively; According to the roll angle, the pitch angle, and the reference yaw angle, a model predictive controller is used to calculate the second virtual input, the third virtual input, and the fourth virtual input respectively; According to the first virtual input, the second virtual input, the third virtual input, and the fourth virtual input, a reference rotational speed is calculated; According to the reference rotational speed, the direction position and the attitude position of the quadrotor UAV are calculated based on the mathematical model of the quadrotor UAV, so as to realize the trajectory tracking control of the quadrotor UAV; Wherein, the position outer loop module includes an x-direction position controller, a y-direction position controller, and a z-direction position controller; the x-direction position controller, the y-direction position controller, and the z-direction position controller all adopt an active disturbance rejection controller; the active disturbance rejection controller includes a tracking differentiator, an extended observer, a nonlinear control law, and a phase compensator; the phase compensator includes an original signal phase compensator and a differential signal phase compensator; The reference signal x in the x direction output by the tracking differentiator ref The differentiated signal x2 passes through the fal function filter to obtain the filtered differentiated signal x 11 , and then it is predicted forward by λ time and compensated to the original signal x1 extracted by the tracking differentiator. Finally, the compensated original signal X1 can be obtained; the differentiated signal x2 extracted by the tracking differentiator needs to pass through the tracking differentiator again to extract the original signal x ref of the differentiated signal 21 and the differentiated signal of the differentiated signal x 22 , x 22 After passing through the fal function filter, the filtered differentiated signal x can be obtained 23 , x 23 After forward prediction and compensation by λ1, it is compensated to x 21 Finally, the compensated differentiated signal X2 can be obtained; The attitude inner loop module adopts a model predictive controller. When designing the attitude controller, the three attitudes are considered together for design, and the attitude equation is selected as: The state space equation corresponding to the attitude equation is: where ξ represents the expanded state, is the system matrix, is the input matrix, Δu is the increment of the control input, and η is the output, is the output matrix; If the prediction horizon of the system is N p , and the control horizon is N c , then the future output Y(t) of the system is: Y(t) = ψ t ξ(t|t) + ΘΔU(t) After obtaining the prediction equation, it is necessary to select an appropriate objective function to optimize the control increment, and a relaxation factor needs to be added to the optimized objective: where η represents the actual attitude, and η ref represents the reference attitude, Q and R are weight matrices, ρ is the weight coefficient, and ε is the relaxation factor.

Images