CN114564038A - Improved active disturbance rejection based trajectory tracking control system for quad-rotor unmanned aerial vehicle - Google Patents

Abstract

The invention discloses a trajectory tracking control system of a quad-rotor unmanned aerial vehicle based on improved active disturbance rejection. The method comprises the following steps: the device comprises a modeling unit, a parameter setting unit, a control unit, a reference rotating speed calculation unit and a track tracking control unit; and the control unit comprises a position outer ring module and an attitude inner ring module. In the invention, the trajectory tracking control of the unmanned aerial vehicle is divided into inner and outer ring control, the outer ring is position control, the inner ring is attitude control, the outer ring adopts nonlinear active disturbance rejection control, and the inner ring adopts model prediction control in the control process, so that the resistance of the unmanned aerial vehicle to the inner disturbance and the outer disturbance is improved. Because the active disturbance rejection control has the defect of phase lag, the phase compensator is adopted to compensate the phase lagging the outer ring so as to improve the control precision of the outer ring controller, and therefore the unmanned aerial vehicle can be ensured to trace and fly according to the given track with high precision.

Description

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

technical field

The invention relates to the technical field of trajectory control of wing unmanned aerial vehicles, in particular to a quadrotor unmanned aerial vehicle trajectory tracking control system based on improved active disturbance rejection.

Background technique

The quadrotor UAV has a crisscross frame structure, the rack has four arms, and a motor is installed at the end of each arm, and the position and position of the quadrotor UAV can be adjusted by adjusting the rotation speed of the four motors. attitude. Compared with other aircrafts, quadrotor UAVs have the advantages of low cost, high reliability, vertical take-off and landing, hovering, etc., and are widely used in aerial photography, plant protection, fire protection, power inspection and other fields. However, due to the complex working environment of UAVs, and the characteristics of high nonlinearity, strong coupling, and under-actuated UAVs, UAVs face many difficulties in the process of accurate trajectory tracking and control.

SUMMARY OF THE INVENTION

The purpose of the present invention is to provide a trajectory tracking control system for a quadrotor UAV based on an improved active disturbance rejection, so as to ensure that the UAV can track and fly with high precision according to a given trajectory.

For achieving the above object, the present invention provides following scheme:

A quadrotor UAV trajectory tracking control system based on improved active disturbance rejection, comprising:

The modeling unit is used to mathematically model and linearize the quadrotor UAV to obtain a linear model of the UAV;

The parameter setting unit is used to specify the x-direction reference trajectory, the y-direction reference trajectory, the z-direction reference trajectory and the reference yaw angle of the quadrotor UAV;

The control unit includes 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 based on the linear model of the UAV, and calculate the pitch angle according to the y-direction reference trajectory the roll angle, and calculate the first virtual input according to the z-direction reference trajectory; the attitude inner loop module is used to calculate the roll angle, the pitch angle and the reference based on the linear model of the UAV yaw angle calculation of the second virtual input, the third virtual input and the fourth virtual input;

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;

The trajectory tracking control unit is used for calculating the position and attitude of the UAV by using the kinematic model and the dynamic model of the UAV according to the reference rotation speed, so as to realize the trajectory tracking control of the quadrotor UAV.

Optionally, the expression of the UAV linear model is as follows:

为横滚角加速度，

为俯仰角加速度，

为偏航角加速度，

为x方向的加速度，

为y方向的加速度，

为z方向的加速度，U₁、U₂、U₃、U₄分别为第一虚拟输入、第二虚拟输入、第三虚拟输入、第四虚拟输入。where m is the mass of the drone, I _x , I _y , and I _z are the rotational inertia of the drone in the x, y, and z directions, g is the acceleration of gravity, φ is the roll angle, θ is the pitch angle,

is the roll angular acceleration,

is the pitch angle 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 U ₁ , U ₂ , U ₃ , and U ₄ are the first virtual input, the second virtual input, the third virtual input, and the fourth virtual input, respectively.

Optionally, the position outer ring module includes:

The x-direction position controller is configured to calculate the pitch angle according to the x-direction reference trajectory based on the UAV linear model;

a y-direction position controller for calculating a roll angle according to the y-direction reference trajectory based on the UAV linear model;

A z-direction position controller is configured to calculate a first virtual input according to the z-direction reference trajectory based on the UAV linear model.

Optionally, the x-direction position controller, the y-direction position controller and the z-direction position controller all use an active disturbance rejection controller.

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

Optionally, the active disturbance rejection controller includes a tracking-differentiator, an expansion observer, a nonlinear control law and a phase compensator.

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

Optionally, the calculation formula of the reference speed is as follows:

Among them, ω ₁ , ω ₂ , ω ₃ , and ω ₄ 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 shaft to the center of mass of the UAV, U ₁ , U ₂ , U ₃ , and U ₄ are the first virtual input, the second virtual input, the third virtual input, and the fourth virtual input, respectively.

The present invention also provides a trajectory tracking control method for the quadrotor UAV based on the improved active disturbance rejection, comprising:

Mathematical modeling of quadrotor UAVs;

Obtain the x-direction reference trajectory, y-direction reference trajectory, z-direction reference trajectory and 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;

calculating a reference rotational speed according to the first virtual input, the second virtual input, the third virtual input and the fourth virtual input;

According to the reference rotation speed, the direction position and attitude position of the UAV are calculated based on the mathematical model of the quadrotor UAV, and the trajectory tracking control of the quadrotor UAV is realized.

According to the specific embodiments provided by the present invention, the present invention discloses the following technical effects:

The trajectory tracking control of the UAV in the present invention is divided into inner and outer loop control, the outer loop is the position control, and the inner loop is the attitude control. The UAV's resistance to internal and external disturbances is improved. Due to the defect of phase lag in ADRC, a phase compensator is used to compensate the lag phase of the outer loop to improve the control accuracy of the outer loop controller, so as to ensure the high precision of the UAV according to the given trajectory. tracking flight.

Description of drawings

In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the accompanying drawings required in the embodiments will be briefly introduced below. Obviously, the drawings in the following description are only some of the present invention. In the embodiments, for those of ordinary skill in the art, other drawings can also be obtained according to these drawings without creative labor.

1 is a block diagram showing the structure of a quadrotor unmanned aerial vehicle trajectory tracking control system based on improved active disturbance rejection according to an embodiment of the present invention;

Fig. 2 is the UAV control flow chart;

Figure 3 is a schematic diagram of the control unit;

Figure 4 is a schematic diagram of the ADRC position controller;

Figure 5 is the original signal phase compensator;

Figure 6 is a differential signal phase compensator.

Detailed ways

The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, but not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

The purpose of the present invention is to provide a trajectory tracking control system for a quadrotor UAV based on an improved active disturbance rejection, so as to ensure that the UAV can track and fly with high precision according to a given trajectory.

In order to make the above objects, features and advantages of the present invention more clearly understood, the present invention will be described in further detail below with reference to the accompanying drawings and specific embodiments.

As shown in FIG. 1, the present invention provides a quadrotor UAV trajectory tracking control system based on improved ADRR, including: a modeling unit 1, a parameter setting unit 2, a control unit 3, and a reference rotational speed calculation unit 4 and the trajectory tracking control unit 5.

Modeling unit 1 is used to mathematically model and linearize the quadrotor UAV to obtain a linear model of the UAV.

The nonlinear mathematical model of the quadrotor UAV is as follows:

为横滚角速度，

为俯仰角速度，

为偏航角速度，

为x方向的速度，

为y方向的速度，

为z方向的速度，

为横滚角加速度，

为俯仰角加速度，

为偏航角加速度，

为x方向的加速度，

为y方向的加速度，

为z方向的加速度，U₁、U₂、U₃、U₄分别为第一虚拟输入、第二虚拟输入、第三虚拟输入、第四虚拟输入。where m is the mass of the drone, I _x , I _y , and I _z are the moment of inertia of the drone in the x, y, and z directions, I _r is the moment of inertia of the rotor, g is the acceleration of gravity, and ω _sum is the four The algebraic sum of the propeller speeds,

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 angle 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 U ₁ , U ₂ , U ₃ , and U ₄ are the first virtual input, the second virtual input, the third virtual input, and the fourth virtual input, respectively.

Among them, ω ₁ , ω ₂ , ω ₃ , and ω ₄ 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.

Since the nonlinear model is too complicated, in order to facilitate the design of the control unit 3, the model is linearized according to the small angle assumption:

After the linear model of the UAV is obtained, the control unit 3 of the UAV can be designed according to the linear model. From the fourth equation of formula (3), we can know that the position in the x direction is coupled with the pitch angle θ. From the fifth equation, we can know that the position in the y direction is coupled with the roll angle Φ, and the position in the z direction is coupled with the yaw angle. There is no coupling relationship between the angles ψ. Therefore, when designing the control unit 3 in the present invention, the coupled x and θ, y and Φ should be controlled jointly, and z and ψ should be controlled independently. The control unit 3 is shown in FIG. 3 , and the control flow is shown in FIG. 2 .

The control unit 3 is mainly composed 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 gives the reference trajectory x _ref in the x direction and the reference trajectory y _ref in the y direction. After passing through the x-direction position controller and the y-direction position controller respectively, the reference pitch angle θ _ref and the reference transverse direction can be calculated. Roll angle φ _ref . The virtual input U ₁ can be obtained by calculating the reference trajectory z _ref in the z direction given by the parameter setting unit 2 after passing through the z direction position controller. After passing through the position controller, the reference roll angle φ _ref , the reference pitch angle θ _ref and the reference yaw angle ψ _ref given by the parameter setting unit 2 can be respectively calculated to obtain the virtual input U ₂ after passing through the attitude inner loop module, U ₃ , U ₄ . After the four virtual inputs U ₁ , U ₂ , U ₃ and U ₄ pass through the control distributor, the reference rotational speed of the four propellers of the UAV can be obtained through the reference rotational speed calculation unit 4 , and the trajectory tracking control unit 5 adopts the motion of the UAV. The scientific model and dynamic model calculate the position (x, y, z) of the UAV in three directions and the attitude (φ, θ, ψ) of the UAV according to the rotational speed of the propeller. The closed-loop control of the UAV can be obtained by feeding back the calculated position and attitude to the position controller and the attitude inner loop module respectively.

The position controller adopts an Active Disturbance Rejection (ADRC) controller. The ADRC controller is not convenient to design all positions together, so the position controllers in each direction need to be designed separately, so there are three position controllers. For attitude control, due to the use of model predictive control, it is convenient to consider three attitudes at the same time when designing the controller (the three attitudes are formed into a matrix), so there is only one attitude controller. The schematic diagram of the control unit as shown in 3 can be obtained according to the selected controller.

The essence of Figure 3 is the same as that of Figure 2, except that when designing the attitude controller, the three attitudes are jointly considered and an MPC attitude controller is designed. Although the position of the UAV in the x direction 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, but for the position controller, ADRC controllers are used in all of them, and the design process of the controllers is the same, except that the input and output of the controllers are different. Therefore, when introducing the position controller, the present invention takes the position controller in the x direction as an example.

1. The x-direction position controller is designed as follows:

The outer loop position controller mainly adopts Active Disturbance Rejection Control (ADRC). Linear Control Law (NLSEF). Because ADRC has the defect of phase lag, a phase compensator is added to the structure of ADRC controller to compensate the lag phase.

As shown in the schematic diagram of the position controller in Figure 4, after the reference position x _ref in the x direction is input to TD, the original signal x ₁ of the reference position and the differential signal x ₂ of the reference position will be obtained. After x ₁ and x ₂ pass through the original signal phase compensator and the differential signal phase compensator, the compensated original signal and the compensated differential signal will be obtained. The ESO will observe the states z ₁ , z ₂ and the total disturbance z ₃ of the UAV according to the UAV position x calculated by the UAV mathematical model and the reference pitch angle θ _ref calculated by the position controller. The difference between x ₁ and z ₁ can get the error e ₁ of the original signal, and the difference between x ₂ and z ₂ can get the error e ₂ of the differential signal. After e ₁ and e ₂ are combined and calculated by NLSEF, the control quantity u ₀ can be obtained. The reference input θ _ref of the attitude controller can be finally obtained after compensating the observed disturbance z ₃ by the amount of z 3 .

Next, the design methods of TD, phase compensator, ESO and NLSEF mathematical models are introduced in turn.

引入扰动后可以得到x方向的控制模型：First select the equation in the x direction in equation (3)

After the disturbance is introduced, the control model in the x direction can be obtained:

为总扰动，选取状态变量

将式(4)转化为状态方程：in the formula

For the total disturbance, choose the state variable

Transform equation (4) into the equation of state:

In the formula, x ₁ is the position in the x direction, x ₂ is the differential of the position in the x direction, and y represents the output, that is, the position in the x direction.

1.1 Tracking-differentiator (TD) design:

When designing the tracking differentiator, the discrete fastest feedback system established according to the fastest synthesis function fhan(x ₁ , x ₂ , r ₀ , h ₀ ) is as follows

where fhan(x ₁ ,x ₂ ,r ₀ ,h ₀ ) is

Equation (6) represents the mathematical model of the tracking-differentiator (TD), where v represents the input of the tracking differentiator, which is the reference position x _ref in the x direction, and x ₁ and x ₂ are the outputs of the tracking differentiator, representing respectively The original signal of the reference signal extracted by the differentiator and the differential signal of the reference signal are tracked. r ₀ and h ₀ are the parameters of the TD model, and h represents the sampling period.

1.2 Phase Compensator Design

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

Figure 5 is the original signal phase compensator, the differential signal x ₂ of the reference signal in the x-direction output by TD (in Figure 4 ) passes through the fal function filter to obtain the filtered differential signal x ₁₁ , and then predicts the λ time forward and compensates On the original signal x ₁ extracted by TD (in FIG. 4 ), the compensated original signal X ₁ can be finally obtained. Figure 6 is the differential signal phase compensator. Since the differential signal of the signal needs to be used when compensating the signal, the differential signal of the differential signal needs to be used when compensating the differential signal, so the differential signal x extracted by TD (in Figure 4) ₂ It needs to go through the tracking-differentiator (TD1 in Figure 6) again to extract the original signal x ₂₁ of the differential signal of x _ref and the differential signal x ₂₂ of the differential signal, and x ₂₂ can be obtained after passing through the fal function filter The filtered differential signal x ₂₃ , x ₂₃ is forward predicted λ ₁ and compensated to x ₂₁ , and finally the differential signal X ₂ of x _ref after compensation can be obtained. The mathematical model of the phase compensator is introduced below. The discrete form of the original signal compensator model is:

The first three formulas in the formula are the formulas of the tracking-differentiator (TD in Figure 4), which have been introduced before and will not be introduced here. x ₁₁ represents the filtered differential signal, k, a, δ, γ is the phase compensator parameter, X ₁ represents the original signal after compensation, and λ represents the time of forward prediction.

The discrete form of the differential signal compensator model is:

The first three formulas in the formula are the formulas of TD in Figure 4, the fourth to sixth formulas are the formulas of TD1 in Figure 6, x ₂₁ represents the original signal of the differential signal of x _ref , and x ₂₂ represents the differential signal of x _ref The differential signal of , x ₂₃ represents the differential signal of the filtered differential signal of x _ref , X ₂ is the differential signal after compensation, λ ₁ is the forward predicted time, k ₁ , a ₁ , δ ₁ , γ ₁ are the phases Compensator parameters. r ₀₁ , h ₀₁ are the parameters of TD1 in FIG. 6 , and h ₁ is the sampling period.

1.3 Expansion Observer (ESO) Design

首先将总扰动扩张为一个新的状态变量x₃ according to

First expand the total disturbance to a new state variable x ₃

The new system after expansion is

Build a state observer for the new expanded system

It can be seen from the structure of the observer in Figure 3 that the observer has two inputs and three outputs. The two inputs are θ _ref and the position x in the x-direction, respectively. Through the two inputs, three outputs z ₁ can be observed, z ₂ , z ₃ . In the formula, ε ₁ represents the error between the state z ₁ observed by the observer and the position x in the x direction output by the UAV system, z ₁ and z ₂ are the state of the system observed by the observer, and z ₃ is the total value observed by the observer. Disturbance, β ₀₁ , β ₀₂ , β ₀₃ , δ are the parameters of the controller, fal(x, a, δ) is a nonlinear function, defined as follows:

In the formula, a is a constant of 0-1, and δ is a constant that affects the filtering effect.

Discretizing the state observer gives:

h represents the discretization time.

1.4 Nonlinear Control Law (NLSEF) Design

Based on the TD and the phase compensator, we can obtain the original signal and differential signal of the reference signal in the x-direction. By making a difference with the state observed by 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 a combined control law. The combination can be a linear combination or a non-linear combination. However, the control efficiency of the control law formed by the general nonlinear combination is better than that of the linear combination, so the nonlinear combination is used when designing the control law, and the function of the nonlinear combination is selected as u ₀ =β ₁ fal(e ₁ ,a ₁ ,δ) +β ₂ fal(e ₂ ,a ₂ ,δ). The control law is designed according to the selected nonlinear combination as follows:

where e ₁ is the original signal error, which is obtained by the difference between the original signal X1 and the state z ₁ observed by ESO after compensation by the original signal phase compensator; e ₂ is the differential signal error, which is compensated by the differential signal phase compensator to obtain the differential signal The difference between X ₂ and the state z ₂ observed by ESO is obtained. u ₀ is the control amount formed after the combination, and β ₁ , β ₂ , β ₃ , a ₁ , a ₂ , and δ are parameters.

After the control variable u ₀ is obtained, the observed disturbance z ₃ needs to be compensated, and then the control variable after compensation of the disturbance is obtained:

In the formula, θ _ref represents the control amount formed after compensating the disturbance, that is, the reference input of the pitch angle and attitude controller, u ₀ is the control amount calculated by the nonlinear combination, and b ₀ is the compensation factor.

2 Attitude Controller Design

Attitude control adopts Model Predictive Controller (MPC), and the three attitudes are considered together when designing the attitude controller. Select the attitude equation in formula (3)

Rewrite it in the form of a state space equation and discretize it

x(k+1)=A _k,t x(k)+B _k,t u(k) (17)

in the formula

where ΔT is the discrete sampling time. x represents the state of the system, that is, three attitudes (φ, θ, ψ), and u represents the input of the UAV system (U ₂ , U ₃ , U ₄ ), which is the output of attitude control. J _x , J _y and J _z represent the moment of inertia of the UAV in the x, y and z directions, respectively.

Expand x and u together into a new state ξ, set:

In the formula, k|t represents the quantity predicted at the k-th time at the current time t, ξ(k|t) represents the expansion state at the t-th time, and x(k|t) represents the original state (φ, θ) at the t-th time. , ψ), u(k-1|t) represents the input (U ₂ , U ₃ , U ₄ ) at the previous time at time t.

So a new state space equation can be obtained

是系统矩阵，

是输入矩阵，Δu是控制输入的增量，η是输出，

是输出矩阵：where ξ represents the state after expansion,

is the system matrix,

is the input matrix, Δu is the increment of the control input, η is the output,

is the output matrix:

in:

In order to simplify the calculation, the following assumptions are made

If the prediction time domain of the system is N _p and the control time domain is N _c , the future output Y(t) of the system is

Y(t)=ψ _t ξ(t|t)+ΘΔU(t) (22)

in

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 tracking ability of the UAV to the attitude should be taken into account. In order to ensure the stability of the flight, the change of the control signal should not be too large. At the same time, it is also necessary to ensure that the optimized target can be solved at every moment. A feasible solution , so a relaxation factor needs to be added to the optimization objective.

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

According to the above mathematical derivation, the quadratic programming method can be used to calculate the control increment ΔU in the three attitude directions (it is a matrix, including three direction control increments), and then the control increment and the control at the previous moment can be calculated through the control increment. Input u(k-1|t) to calculate the current control input (U ₂ , U ₃ , U ₄ ).

At the same time, the position controller in the z direction will calculate the control input U ₁ , U ₁ , U ₂ , U ₃ , and U ₄ according to the input reference position in the z direction, and the speed of the motor can be obtained after distribution by the controller.

Mathematical model pose solution

According to the virtual input, we can get the reference speed of the motor, and through the motor control, we can get the actual motor speed. According to the speed of the motor, the force and torque on the drone can be obtained:

表示地面坐标系到机体坐标系的旋转矩阵，T^B表示无人机受到的拉力，

表示无人机受到的空气阻力。M^B表示无人机受到的总力矩，

表示陀螺力矩，

表示螺旋桨在机体轴上产生的力矩，

表示气动力矩。In the formula, the superscript B represents the body coordinate system, F ^B represents the total pulling force of the drone, 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 of the drone,

Indicates the air resistance experienced by the drone. M ^B represents the total moment received by the UAV,

represents the gyroscopic moment,

represents the torque generated by the propeller on the body shaft,

Represents aerodynamic torque.

The solution process of the pulling force T ^B received by the UAV is as follows:

的求解过程如下：Air resistance on drones

The solution process is as follows:

In the formula, u, v, w represent the relative airflow velocity of the UAV in the three directions of x, y, and z, and c _d is the air resistance coefficient.

的计算过程如下：Gyroscopic moment

The calculation process is as follows:

表示第k个螺旋桨的角速度，ω_yb与ω_xb分别表示绕机体y轴旋转角速度与绕机体x轴旋转角速度。In the formula, J _RP represents the total moment of inertia of the motor rotor and the propeller rotating around the shaft, n _r represents the number of propellers, k represents the number of the propeller,

represents the angular velocity of the k-th propeller, and ω _yb and ω _xb represent the angular velocity of rotation around the y-axis of the body and the angular velocity of rotation around the x-axis of the body, respectively.

计算过程如下The torque generated by the propeller on the body shaft

The calculation process is as follows

The calculation process of aerodynamic torque is as follows:

In the formula, ω _x , ω _y , and ω _z represent the rotational speed of the relative air, which is approximately the angular velocity around the three axes of the body, and c _dm is the damping moment coefficient.

After calculating the force and moment of the UAV, the position and attitude of the UAV can be calculated according to the kinematic model and dynamic model of the UAV:

表示四元数，J表示转动惯量。In the formula, the superscript e represents the ground coordinate system, the superscript B represents the body coordinate system, p represents the position of the UAV, v represents the speed, ω ^b represents the angular velocity of the body rotation,

represents the quaternion, and J represents the moment of inertia.

The present invention also provides a trajectory tracking control method for the quadrotor UAV based on the improved active disturbance rejection, comprising:

Step 1. Mathematically model the quadrotor UAV.

Step 2. 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.

Step 3. 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.

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.

Step 5: Calculate a reference rotational speed according to the first virtual input, the second virtual input, the third virtual input and the fourth virtual input.

Step 6: According to the reference rotation speed, calculate the direction position and attitude position of the UAV based on the mathematical model of the quadrotor UAV, so as to realize the trajectory tracking control of the quadrotor UAV.

The present invention has the following advantages:

1. Compared with the traditional PID control, the control system proposed by the present invention has stronger ability to resist disturbance and higher robustness. Reason: When controlling the UAV, both the inner and outer loops use a robust controller. The active disturbance rejection control used in the outer loop can estimate the internal and external disturbances in real time and compensate them, which improves the UAV's ability to respond to internal and external disturbances. resistance.

2. Compared with the traditional ADRC trajectory tracking, the accuracy is higher. Reason: The traditional ADRC has the defect of phase lag in the control process. The control system proposed by the invention compensates the delayed phase and improves the accuracy of trajectory tracking.

In this paper, specific examples are used to illustrate the principles and implementations of the present invention. The descriptions of the above embodiments are only used to help understand the methods and core ideas of the present invention; meanwhile, for those skilled in the art, according to the present invention There will be changes in the specific implementation and application scope. In conclusion, the contents of this specification should not be construed as limiting the present invention.

Claims (9)

1. A quad-rotor unmanned aerial vehicle trajectory tracking control system based on improved active disturbance rejection, comprising:

the modeling unit is used for carrying out mathematical modeling and linearization processing on the quad-rotor unmanned aerial vehicle to obtain a linear model of the unmanned aerial vehicle;

the parameter giving unit is used for giving an x-direction reference track, a y-direction reference track, a z-direction reference track and a reference yaw angle of the quad-rotor unmanned aerial vehicle;

the control unit comprises a position outer ring module and an attitude inner ring module, wherein the position outer ring module is used for calculating a pitch angle according to the x-direction reference track, a roll angle according to the y-direction reference track and a first virtual input according to the z-direction reference track on the basis of the unmanned aerial vehicle linear model; the attitude inner loop module is used for calculating a second virtual input, a third virtual input and a fourth virtual input according to the roll angle, the pitch angle and the reference yaw angle based on the unmanned aerial vehicle linear model;

a reference rotation speed calculation unit configured to calculate a reference rotation speed according to the first virtual input, the second virtual input, the third virtual input, and the fourth virtual input;

and the trajectory tracking control unit is used for calculating the position and the posture of the unmanned aerial vehicle by adopting a kinematics model and a dynamics model of the unmanned aerial vehicle according to the reference rotating speed, so that trajectory tracking control of the quad-rotor unmanned aerial vehicle is realized.

2. The improved active disturbance rejection based quad-rotor drone trajectory tracking control system according to claim 1, wherein said drone linear model is expressed as follows:

where m is the unmanned aerial vehicle mass, I_x、I_y、I_zThe moment of inertia of the unmanned aerial vehicle in the directions of x, y and z, g is the gravity acceleration, phi is the roll angle, theta is the pitch angle,

in order to obtain the roll angle acceleration,

for the purpose of pitch angular acceleration,

in order to determine the yaw angular acceleration,

is the acceleration in the x-direction and,

is the acceleration in the y-direction and,

acceleration in the z direction, U₁、U₂、U₃、U₄Respectively a first virtual input, a second virtual input, a third virtual input, and a fourth virtual input.

3. The improved active disturbance rejection based quad-rotor drone trajectory tracking control system according to claim 1, wherein said position outer loop module comprises:

the x-direction position controller is used for calculating a pitch angle according to the x-direction reference track based on the unmanned aerial vehicle linear model;

the y-direction position controller is used for calculating a roll angle according to the y-direction reference track based on the unmanned aerial vehicle linear model;

and the z-direction position controller is used for calculating a first virtual input according to the z-direction reference track based on the unmanned aerial vehicle linear model.

4. The improved active-disturbance-rejection based quad-rotor unmanned aerial vehicle trajectory tracking control system of claim 3, wherein the x-direction position controller, the y-direction position controller, and the z-direction position controller are all active-disturbance-rejection controllers.

5. The improved active disturbance rejection based quad-rotor drone trajectory tracking control system according to claim 1, wherein said pose inner loop module employs a model predictive controller.

6. A quad-rotor drone trajectory tracking control system based on improved active disturbance rejection according to claim 4, wherein the active disturbance rejection controller includes a tracking-differentiator, an extended observer, a nonlinear control law and a phase compensator.

7. The improved active disturbance rejection based quad-rotor drone trajectory tracking control system according to claim 6, wherein said phase compensator employs a phase lead compensator incorporating a fal function filter.

8. The improved active disturbance rejection based quad-rotor drone trajectory tracking control system according to claim 1, wherein said reference rotation speed is calculated as follows:

wherein, ω is₁、ω₂、ω₃、ω₄Be the reference rotational speed of four propellers respectively, b is the lift coefficient of rotor, and d is the resistance coefficient of rotor, and l is the distance of rotor pivot to unmanned aerial vehicle barycenter, U₁、U₂、U₃、U₄Respectively a first virtual input, a second virtual input, a third virtual input, and a fourth virtual input.

9. A trajectory tracking control method for a quad-rotor unmanned aerial vehicle based on improved active disturbance rejection is characterized by comprising the following steps:

performing mathematical modeling on the quad-rotor unmanned aerial vehicle;

acquiring an x-direction reference track, a y-direction reference track, a z-direction reference track and a reference yaw angle of the quad-rotor unmanned aerial vehicle;

calculating a pitch angle, a roll angle and a first virtual input by adopting an active disturbance rejection controller according to the x-direction reference track, the y-direction reference track and the z-direction reference track;

respectively calculating a second virtual input, a third virtual input and a fourth virtual input by adopting a model prediction controller according to the roll angle, the pitch angle and the reference yaw angle;

calculating a reference rotation speed according to the first virtual input, the second virtual input, the third virtual input and the fourth virtual input;

according to refer to the rotational speed, calculate unmanned aerial vehicle's direction position and gesture position based on four rotor unmanned aerial vehicle mathematical model, realize four rotor unmanned aerial vehicle's trajectory tracking control.

Images