CN111026160A - Trajectory tracking control method for quad-rotor unmanned aerial vehicle - Google Patents

Abstract

The invention belongs to the field of robot control, and in particular relates to a trajectory tracking control method for a quadrotor unmanned aerial vehicle. The first angle and its corresponding angular velocity and angular acceleration, the first angle is the roll angle and the pitch angle, the loop disturbance estimation error converges to zero within a fixed time; the attitude angle control loop is used to estimate the loop disturbance in real time, and Based on the disturbance estimator, the torque control quantities of the UAV rotating around the x, y, and z axes of the body coordinate system are respectively calculated, and the second angle is the yaw angle, and the error of the loop disturbance estimation converges to zero within a fixed time; The resultant force control amount and the torque control amount control the trajectory tracking of the UAV, in which the angle, angular velocity error, and position and velocity errors of the UAV converge to zero within a fixed time. The invention realizes the high-precision trajectory tracking control of the unmanned aerial vehicle at a fixed time.

Description

A kind of quadrotor UAV trajectory tracking control method

technical field

The invention belongs to the field of robot control, and more particularly, relates to a trajectory tracking control method of a quadrotor unmanned aerial vehicle.

Background technique

With the development of modern control theory and research on rotary-wing drone technology, rotary-wing drones can achieve excellent characteristics such as stable hovering, fixed-point take-off and landing. As the mainstream development direction of future air logistics and transportation, rotary-wing drones encourage A large number of researchers are engaged in the research work on the related topics of the control system of the rotary-wing UAV.

However, in practical applications for single-rotor UAVs, there are often difficulties in control, high stability and poor stability. In view of the simplicity of stability in the application field, researchers have also begun to turn their research objects to quad-rotor UAV systems. On the other hand, due to the existence of more input variables, quadrotors are often more flexible, easier to control, and more stable than single-rotor UAVs, and can complete certain demanding tasks, such as aerial photography, at the same time while flying. Therefore, the quadrotor UAV is more in line with practical application needs.

In practical applications, the quadrotor UAV system may be required to complete the trajectory tracking task at a fixed time, and the current mainstream control methods cannot achieve fixed-time trajectory tracking. In order to meet the needs of practical applications, the fixed-time tracking method is particularly important. In addition, like most mechanical rigid body structures, the dynamic characteristics of the quadrotor UAV can be expressed by a mathematical model represented by its mechanical parameters, but the premise is that the structure of the UAV is known and the mechanical parameters are known. In fact, due to the influence of working conditions and external disturbances, some mechanical parameters of the UAV cannot be accurately measured during the working process. Therefore, it is usually necessary to consider parameter uncertainty when mathematical modeling of UAVs.

Therefore, combined with the above, it is of great significance to consider the disturbance and model parameter uncertainty in the finite-time trajectory tracking control of the quadrotor UAV system in the mission space.

SUMMARY OF THE INVENTION

The present invention provides a four-rotor UAV trajectory tracking control method, which is used to solve the problem of practical application in the existing four-rotor UAV trajectory tracking control because mechanical parameters cannot be accurately obtained during modeling and the trajectory tracking time cannot be fixed. The technical problem that the high-precision tracking control of the UAV in time and trajectory cannot be realized.

The technical solution of the present invention to solve the above-mentioned technical problems is as follows: a trajectory tracking control method of a quadrotor unmanned aerial vehicle, comprising:

Based on the desired position and its corresponding velocity and acceleration, as well as the actual position collected in real time and its corresponding velocity and angle, a position-velocity control loop is used to estimate the disturbance of the loop in real time, and based on the estimated amount of the disturbance, calculate the UAV The resultant force control amount and the reference first angle and its corresponding angular velocity and angular acceleration, wherein, the first angle is the roll angle and the pitch angle, and the disturbance estimation error of the loop converges to zero within a fixed time;

Based on the reference first angle and its corresponding angular velocity and angular acceleration, the expected second angle and its corresponding angular velocity and angular acceleration, and the actual angle and its corresponding angular velocity, an attitude angle control loop is used to estimate the disturbance of the loop in real time, And based on the estimated amount of the disturbance, the torque control amount of the UAV rotating around the x, y, and z axes of the body coordinate system is calculated respectively, wherein the second angle is the yaw angle, and the disturbance estimation error of the loop is fixed. converges to zero in time;

Based on the resultant force control amount and the torque control amount, the trajectory tracking of the UAV is controlled, wherein the angle, angular velocity errors, and position and velocity errors of the UAV converge to zero within a fixed time.

The beneficial effects of the present invention are as follows: the method is a fixed-time trajectory tracking control method considering the disturbance and the uncertainty of the model parameters. By controlling the disturbance estimation error in the position velocity loop and the disturbance estimation error in the attitude angle control loop, both Convergence to zero in a fixed time, and then realize the position and speed control loop to make the angle and angular velocity error of the UAV converge to zero in a fixed time, and realize the attitude angle control loop to make the position and speed error of the UAV in a fixed time in a lonely time. converges to zero in time. In the specific implementation, the uncertainty of dynamic parameters and external disturbances in the quadrotor UAV model can be considered, and appropriate control parameters can be selected to meet the requirements for the working speed and working accuracy of the robot, so that the involved control methods can be applied in practice. The quadrotor UAV system can complete the trajectory tracking task in a fixed time, with good timeliness and stronger practicability, and realizes the high-precision tracking control of the UAV in time and trajectory in practical applications.

On the basis of the above technical solutions, the present invention can also be improved as follows.

Further, the position-velocity control loop includes a state estimator for making the disturbance estimation error of the loop converge to zero within the _first fixed time T1, expressed as:

为该环路的实际扰动D₁的估计量，在初始时刻t₀，该环路的实际扰动量D₁为零，L₁为该环路的实际扰动各分量变化率的上界；k_i,i＝1,2,3为控制增益常数；

v为无人机的实际速度矢量，v_d为无人机的期望速度矢量，g为重力加速度，u₁为所述合力控制量，e_z表示大地坐标系下z轴的单位矢量，m为无人机质量，R∈R^3×3为依次以机体坐标系的z、y、x顺序旋转的旋转矩阵，a_d为无人机的期望加速度矢量；in,

is the estimator of the actual disturbance D ₁ of the loop. At the initial time t ₀ , the actual disturbance D ₁ of the loop is zero, and L ₁ is the upper bound of the rate of change of the actual disturbance components of the loop; k _i , i=1,2,3 is the control gain constant;

v is the actual velocity vector of the drone, v _d is the expected velocity vector of the drone, g is the acceleration of gravity, u ₁ is the control amount of the resultant force, e _z represents the unit vector of the z-axis in the geodetic coordinate system, and m is the The mass of the drone, R∈R ^3×3 is the rotation matrix that rotates in the order of z, y, and x of the body coordinate system, and a _d is the expected acceleration vector of the drone;

Then the _first fixed time T1 is expressed as:

Further, the resultant force control amount u ₁ is expressed as:

Wherein, α _i >0, β _i >0, 0<n _i <m _i , 0<p _i <q _i , i=1, 2, and m _i , n _i , p _i , and q _i are all positive odd numbers; p is the actual position of the UAV, p _d is the desired position of the UAV, e _v is the speed error of the UAV, and S ₁ is the preset sliding surface of the loop.

Further, the calculation expression of the reference first angle is:

Among them, ψ is the yaw angle in the geodetic coordinate system.

Further, the position and velocity errors controlled by the position-velocity control loop converge to zero within the _fourth fixed time T4, and the expression is:

Further beneficial effects of the present invention are: the present invention adopts the state estimator and the control law in the above-mentioned position-speed control loop to perform synergistic action, and it can be known from the formula of the fourth fixed time that the position can be effectively realized only by effectively setting parameters according to actual needs. And the speed error converges to zero in a fixed time, which is very practical.

Further, the attitude angle control link includes an inter-state estimator for making the disturbance estimation error of the loop converge to zero within the second fixed time T ₂ , which is expressed as:

为WD₂的估计量，D₂为该环路的实际扰动，L₂为该环路的实际扰动各分量变化率的上界，在初始时刻t₀，该环路的实际扰动量D₂为零，W为坐标系变换矩阵，I为单位矩阵，Ω为机体坐标系下的实际角速度矢量，

为在大地坐标系下的无人机的期望角速度矢量，τ'为该环的滑模控制律；k_i,i＝4,5,6为控制增益常数，

in,

is the estimator of WD ₂ , D ₂ is the actual disturbance of the loop, L ₂ is the upper bound of the rate of change of each component of the actual disturbance of the loop, and at the initial time t ₀ , the actual disturbance of the loop D ₂ is zero, W is the coordinate system transformation matrix, I is the unit matrix, Ω is the actual angular velocity vector in the body coordinate system,

is the expected angular velocity vector of the UAV in the geodetic coordinate system, τ' is the sliding mode control law of the loop; k _i , i=4, 5, 6 are the control gain constants,

Then the second fixed time T ₂ is expressed as:

Further, the torque control quantities for the rotation of the UAV around the x, y, and z axes of the body coordinate system are respectively u ₂ , u ₃ , and u ₄ , which are expressed as: [u ₂ , u ₃ , u ₄ ] ^T = τ; τ=I ^-1 Wτ';

为在大地坐标系下的无人机的期望角加速度矢量，

为W的导数，

S₂为该环路的预设滑模面。Wherein, α _i > 0, β _i > 0, 0 < n _i <m _i , 0 < p _i <q _i , i=3, 4, and m _i , n _i , p _i , and q _i are all positive odd numbers; Θ is the actual angle vector of the UAV under the geodetic coordinate system, _Θd is the desired angle vector of the UAV under the geodetic coordinate system,

is the expected angular acceleration vector of the UAV in the geodetic coordinate system,

is the derivative of W,

S ₂ is the preset sliding surface of the loop.

Further, the angle and angular velocity error controlled by the attitude angle control loop converge to zero within the _third fixed time T3, and the expression _of T3 is:

A further beneficial effect of the present invention is that: the present invention adopts the state estimator and the control law in the above attitude angle control loop to perform synergistic action, and it can be known from the formula of the third fixed time that the angle can be effectively realized only by effectively setting parameters according to actual needs. , The angular velocity error converges to zero in a fixed time, and the practicability is strong.

Further, the total convergence time of the trajectory tracking is T≤max(T ₁ , T ₂ )+T ₃ +T ₄ .

The further beneficial effects of the present invention are: the angle and angular velocity errors converge first, then the position and velocity errors converge again, and the disturbance convergence times T ₁ and T ₂ are independent of the control input. Based on this, the determined total convergence time of the trajectory tracking, High precision and strong practicability.

The present invention also provides a storage medium in which instructions are stored, and when a computer reads the instructions, the computer is made to execute any of the above-mentioned four-rotor UAV trajectory tracking control methods.

Description of drawings

1 is a flowchart of a method for tracking and controlling a trajectory of a quadrotor unmanned aerial vehicle provided by an embodiment of the present invention;

2 is a schematic diagram of a quadrotor UAV trajectory tracking dual-loop control provided by an embodiment of the present invention;

3 is a model diagram of a quadrotor unmanned aerial vehicle provided by an embodiment of the present invention;

4 is a structural diagram of a position-speed control loop provided by an embodiment of the present invention;

5 is a structural diagram of an attitude angle control loop provided by an embodiment of the present invention;

6 is a comparison diagram between a disturbance estimate and an actual disturbance in a position-speed control loop and an attitude angle control loop provided by an embodiment of the present invention;

7 is a tracking trajectory diagram of a position and an attitude angle in a task space provided by an embodiment of the present invention;

8 is a schematic diagram of a control input of an unmanned aerial vehicle provided by an embodiment of the present invention;

FIG. 9 is a trajectory tracking diagram of a position and an attitude angle in a task space provided by an embodiment of the present invention.

Detailed ways

In order to make the objectives, technical solutions and advantages of the present invention clearer, the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention, but not to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

Example 1

A four-rotor UAV trajectory tracking control method 100, as shown in FIG. 1, includes:

Step 110: Based on the desired position and its corresponding speed and acceleration, as well as the actual position collected in real time and its corresponding speed and angle, a position-velocity control loop is used to estimate the disturbance of the loop in real time, and based on the estimated amount of the disturbance, calculate The resultant force control amount of the UAV and the reference first angle and its corresponding angular velocity and angular acceleration, where the first angle is the roll angle and the pitch angle, and the disturbance estimation error of the loop converges to zero within a fixed time;

Step 120, based on the reference first angle and its corresponding angular velocity and angular acceleration, the expected second angle and its corresponding angular velocity and angular acceleration and the actual angle and its corresponding angular velocity, adopt the attitude angle control loop to estimate the disturbance of the loop in real time , and based on the estimated amount of the disturbance, respectively calculate the torque control amount of the UAV rotating around the x, y, and z axes of the body coordinate system, where the second angle is the yaw angle, and the disturbance estimation error of the loop is within a fixed time inner converges to zero;

Step 130: Control the trajectory tracking of the UAV based on the resultant force control amount and the torque control amount, wherein the angle, angular velocity error, and position and velocity errors of the UAV converge to zero within a fixed time.

It should be noted that this method first considers the disturbance, and models the dynamics and kinematics of the UAV; then converts the trajectory tracking control of the UAV into a double-loop control, the outer loop is the position and velocity control loop, and the inner loop is the attitude Angle loop control; as shown in Figure 2, the position and speed control loop inputs the desired position and speed and the actual position and speed, and outputs part of the control input to the motor and the reference desired angle and angular speed for the attitude angle control loop; Attitude The angle control loop inputs the desired angle and angular velocity as well as the actual angle and angular velocity, and outputs the control input to another part of the motor. Next, the inner and outer loop controller design is carried out, including: position velocity controller design, including fixed-time state estimator and fixed-time sliding mode controller design, in which the state estimator can accurately estimate the position and velocity control loop after a fixed time. disturbance, after accurately compensating the disturbance of the position-velocity control loop, the desired position-velocity trajectory can be tracked after another fixed time under the control of the proposed sliding mode control law; the attitude angle controller design includes a fixed-time state estimator and Fixed-time sliding mode controller design, in which the state estimator can accurately estimate the disturbance of the attitude angle control loop after a fixed time, after accurately compensating for the disturbance of the attitude angle control loop, and another fixed time after the proposed sliding mode control law The desired angle and angular velocity can be tracked under the control of . Among them, the time when the estimation error of the state estimator converges to 0 is uniquely determined by its parameters; after compensating the disturbance, the time when the sliding mode controller controls the position velocity to converge to the desired position velocity value is uniquely determined by its parameters. Based on the system design, the trajectory tracking control of the above quadrotor UAV is carried out. Among them, the roll angle, pitch angle and yaw angle are all angles in the geodetic coordinate system.

This method is a fixed-time trajectory tracking control method that considers the disturbance and the uncertainty of the model parameters. By controlling the disturbance estimation error in the position velocity loop and the disturbance estimation error in the attitude angle control loop, both converge to zero within a fixed time. Then, the position and velocity control loop can make the angle and angular velocity error of the UAV converge to zero in a fixed time, and the attitude angle control loop can make the position and velocity error of the UAV converge to zero in a fixed time in a lonely time. In the specific implementation, the uncertainty of dynamic parameters and external disturbances in the quadrotor UAV model can be considered, and appropriate control parameters can be selected to meet the requirements for the working speed and working accuracy of the robot, so that the involved control methods can be applied in practice. The four-rotor UAV system can complete the trajectory tracking task in a fixed time, with good timeliness and stronger practicability.

In order to ensure the correctness of the results of the above methods, the above methods are established on the basis of the following three conditions:

即位置速度控制环的扰动初始值为0，而且其一次导存在且有界；(1) The disturbance of the position-speed control loop satisfies:

That is, the initial value of the disturbance of the position-velocity control loop is 0, and its first derivative exists and is bounded;

即姿态角控制环的扰动初始值为0，而且其一次导存在且有界；(2) The disturbance of the attitude angle control loop satisfies:

That is, the initial value of the disturbance of the attitude angle control loop is 0, and its first derivative exists and is bounded;

(3) The rotation angle of the quadrotor UAV meets the requirements

The dynamics and kinematics modeling models are:

The quadrotor UAV model diagram shown in Figure 3, in the figure, E={ _{ex , e y ,} e _z } represents the geodetic coordinate system, B={b _x , b _y , b _z } represents the UAV Body coordinate system, p=[x, y, z] ^T ∈ R ³ represents the position of the UAV in the world coordinate system, v=[v _x , v _y , v _z ] ^T ∈ R ³ is the UAV The speed in the world coordinate system, Θ=[φ,θ,ψ] ^T ∈R ³ is the Euler angle of the drone in the world coordinate system, Ω=[ω _x ,ω _y ,ω _z ] ^T ∈ R ³ is the angular velocity of the drone in the fuselage coordinate system, g, m are the gravitational acceleration and the mass of the drone, respectively, e _z is the unit vector in the z direction, D ₁ = [d _x , d _y , d _z ] ^T ∈R ³ ,D ₂ =[d _φ ,d _θ ,d _ψ ] ^T ∈ R ³ is disturbance; I=diag(I _xx ,I _yy ,I _zz )∈R ^3×3 is inertia matrix; τ=[u ₂ , u ₃ , u ₄ ] ^T ∈ R ³ represents the system input torque, u ₁ represents the total lift of the system input UAV; R ∈ R ^3×3 , W ∈ R ^3×3 represents the rotation matrix _, C = cos( ·), S _· =sin(·), in addition:

该表达式即为当前跟踪目标，p_d＝[x_d,y_d,z_d]^T∈R³，v_d＝[v_{x_d},v_{y_d},v_{z_d}]^T∈R³，a_d＝[a_{x_d},a_{y_d},a_{z_d}]^T∈R³分别表示跟踪目标的位置状态、速度、加速度。The mathematical expression for tracking the target is:

This expression is the current tracking target, p _d =[x _d ,y _d ,z _d ] ^T ∈R ³ ,v _d =[v _{x_d} ,v _{y_d} , _{v z_d} ] ^T ∈R ³ ,ad =[a _{x_d} , a _{y_d} , a _{z_d} ] ^T ∈ R ³ represent the position state, velocity, and acceleration of the tracking target, respectively.

The physical parameters of the quadrotor UAV are shown in Table 1 below:

Table 1 Physical parameters of quadrotor UAV

Combined with the position, velocity and acceleration of the target, according to Newton's second law, the position and velocity control equation can be obtained:

是扰动的估计值。由此可以得到

令

则u₁,

都可以确定，

where T4 is the upper bound _on the convergence time, a constant determined by the design parameters,

is the estimated value of the disturbance. From this it can be obtained

make

Then u ₁ ,

can be sure,

The angle of the target Θ _d = [φ _d , θ _d , ψ _d ] ^T ∈ R ³ , we can get:

a_{Ω_d}＝[a_{ωx_d},a_{ωy_d},a_{ωz_d}]^T∈R³目标角加速度，a_{Ω_d}是控制器设计参数决定的，θ为横滚角度，φ为俯仰角度，ψ为偏航角度。ψ _d can be designed by yourself, and is generally set to 0; the target angular velocity can be obtained by taking the derivative of Θ _ds

a _{Ω_d} = [a _{ωx_d} , a _{ωy_d} , a _{ωz_d} ] ^T ∈ R ³ target angular acceleration, a _{Ω_d} is determined by the controller design parameters, θ is the roll angle, φ is the pitch angle, and ψ is the yaw angle.

Combined with the angle, angular velocity and acceleration of the target, the control equation of the attitude angle control loop can be obtained:

是扰动的估计值。由此可以得到τ，则u₂,u₃,u₄都可以确定。where T3 is the upper bound _on the convergence time, a constant determined by the design parameters,

is the estimated value of the disturbance. From this, τ can be obtained, then u ₂ , u ₃ , and u ₄ can be determined.

Preferably, the above-mentioned position-velocity control loop includes a state estimator for making the disturbance estimation error of the loop converge to zero within the _first fixed time T1, expressed as:

为该环路的实际扰动D₁的估计量，在初始时刻t₀，该环路的实际扰动量D₁为零，L₁为该环路的实际扰动各分量变化率的上界；k_i,i＝1,2,3为控制增益常数；

v为无人机的实际速度矢量，v_d为无人机的期望速度矢量，g为重力加速度，u₁为所述合力控制量，e_z表示大地坐标系下z轴的单位矢量，m为无人机质量，R∈R^3×3为依次以机体坐标系的z、y、x顺序旋转的旋转矩阵，a_d为无人机的期望加速度矢量；则上述第一固定时间T₁表示为：in,

is the estimator of the actual disturbance D ₁ of the loop. At the initial time t ₀ , the actual disturbance D ₁ of the loop is zero, and L ₁ is the upper bound of the rate of change of the actual disturbance components of the loop; k _i , i=1,2,3 is the control gain constant;

v is the actual velocity vector of the drone, v _d is the expected velocity vector of the drone, g is the acceleration of gravity, u ₁ is the control amount of the resultant force, e _z represents the unit vector of the z-axis in the geodetic coordinate system, and m is the The mass of the drone, R∈R3 ^×3 is the rotation matrix that rotates in the order of z, y, and x of the body coordinate system, and a _d is the expected acceleration vector of the drone; then the above-mentioned _first fixed time T1 is expressed as :

Preferably, the above-mentioned resultant force control amount u ₁ is expressed as:

Wherein, α _i >0, β _i >0, 0<n _i <m _i , 0<p _i <q _i , i=1, 2, and m _i , n _i , p _i , and q _i are all positive odd numbers; p is the actual position of the UAV, p _d is the desired position of the UAV, e _v is the speed error of the UAV, and S ₁ is the preset sliding surface of the loop.

Preferably, the position and speed errors controlled by the above-mentioned position-speed control loop converge to zero within the _fourth fixed time T4, and the expression is:

As shown in Figure 4, the structure of the position-velocity control loop includes a state estimator and a control law. In the figure:

Preferably, the above-mentioned attitude angle control link includes an inter-state estimator for making the disturbance estimation error of the loop converge to zero within the second fixed time T ₂ , which is expressed as:

为WD₂的估计量，D₂为该环路的实际扰动，L₂为该环路的实际扰动各分量变化率的上界，在初始时刻t₀，该环路的实际扰动量D₂为零，W为坐标系变换矩阵，I为单位矩阵，Ω为机体坐标系下的实际角速度矢量，

为在大地坐标系下的无人机的期望角速度矢量，τ'为该环的滑模控制律；k_i,i＝4,5,6为控制增益常数，

in,

is the estimator of WD ₂ , D ₂ is the actual disturbance of the loop, L ₂ is the upper bound of the rate of change of each component of the actual disturbance of the loop, and at the initial time t ₀ , the actual disturbance of the loop D ₂ is zero, W is the coordinate system transformation matrix, I is the unit matrix, Ω is the actual angular velocity vector in the body coordinate system,

is the expected angular velocity vector of the UAV in the geodetic coordinate system, τ' is the sliding mode control law of the loop; k _i , i=4, 5, 6 are the control gain constants,

Then the above-mentioned second fixed time T ₂ is expressed as:

The selection of parameters of each estimator can be seen in Table 2 below, and the disturbance settings are:

d _i = 0.2sin(5t), i = x, y, z, φ, θ, ψ;

Table 2 Parameters of the estimator

parameter name parameter value parameter name parameter value k₁ 3 k₄ 3 k₂ 0.5 k₅ 0.5 k₃ 4 k₆ 4 μ₁ 0.5 μ₂ 0.5 λ₁ 2 λ₂ 2

Preferably, the torque control quantities for the rotation of the UAV around the x, y, and z axes of the body coordinate system are respectively u ₂ , u ₃ , and u ₄ , which are expressed as: [u ₂ , u ₃ , u ₄ ] ^T = τ; τ=I ^-1 Wτ';

为在大地坐标系下的无人机的期望角加速度矢量，

为W的导数，

S₂为该环路的预设滑模面。Wherein, α _i > 0, β _i > 0, 0 < n _i <m _i , 0 < p _i <q _i , i=3, 4, and m _i , n _i , p _i , and q _i are all positive odd numbers; Θ is the actual angle vector of the UAV under the geodetic coordinate system, _Θd is the desired angle vector of the UAV under the geodetic coordinate system,

is the expected angular acceleration vector of the UAV in the geodetic coordinate system,

is the derivative of W,

S ₂ is the preset sliding surface of the loop.

Preferably, the angle and angular velocity errors controlled by the above attitude angle control loop converge to zero within the _third fixed time T3, and the expression _of T3 is:

Preferably, the total convergence time of the above-mentioned trajectory tracking is T≤max(T ₁ , T ₂ )+T ₃ +T ₄ .

As shown in Figure 5, the structure diagram of the attitude angle control loop, including the state estimator and control law, in the figure:

The selection of control parameters in each control law can be seen in Table 3 below, and the target trajectory is set as:

z _d (t)=0.25, ψ _d (t)=0;

Table 3 Control parameters

In order to better illustrate the present invention, it is now proved that the state estimator converges in a fixed time, as follows:

对位置速度控制环状态估计器求导，得：

Define the state observer estimation error

Derivating the position-velocity control loop state estimator, we get:

Divided into the following two cases, the state estimator is analyzed, and the following analysis results are obtained:

得：(1) When ||Δ ₁ ||≠0, both sides are left-multiplied at the same time

have to:

所述的状态估计器数学表达式满足：

The mathematical expression of the state estimator satisfies:

Let s=||Δ ₁ ||,

求导可得：Construct Lyapunov function

Guidance can be obtained:

So it is gradually stable.

V将在

时收敛到1；When V>1,

V will be in

converges to 1 when

V将在

时收敛到0。When V≤1,

V will be in

converges to 0.

后，V,s,Δ₁都会收敛于0，因为

经过t₁+t₂时间后

so over time

, V, s, Δ ₁ will converge to 0, because

After t ₁ +t ₂ time

收敛到0的时间

其中，

经过时间T₁，(2) When ||Δ ₁ ||=0,

time to converge to 0

in,

After time T ₁ ,

状态观测器估计值误差

收敛到0。

State Observer Estimate Error

converges to 0.

收敛到0。Similarly, the estimated value error of the state observer after the total time T ₂

converges to 0.

As shown in Figure 6, the left picture corresponds to the position and velocity control loop, the right picture corresponds to the attitude angle control loop, the position velocity control loop and the attitude angle control loop disturbance estimator and the actual disturbance value, under the action of the designed state estimator , each component of the position and attitude angle perturbation estimators converges to its corresponding perturbation value at a fixed time.

可以保证位置速度是固定时间收敛的：The following proves that the designed control rate

The position velocity is guaranteed to converge in constant time:

Taking the derivative of the sliding surface S ₁ , we get:

的数学表达式和设计的

的表达式带入上式，可得：

其中

是估计误差。Incorporate dynamics and kinematics into models

Mathematical expressions and designs of

The expression of is brought into the above formula, we can get:

in

is the estimation error.

得：

According to the foregoing, elapsed time T ₁ ,

have to:

求导可得：Construct Lyapunov function

Guidance can be obtained:

V₁将在

后收敛到1。When V ₁ > 1,

_V1 will be in

then converges to 1.

V₁将在

后收敛到0。When V ₁ ≤ 1,

_V1 will be in

then converges to 0.

后，V₁,S₁都会收敛于0。so over time

After that, V ₁ , S ₁ will converge to 0.

令e_p＝p-p_d，e_v＝v-v_d，则

From S ₁ =0, we get

Let e _p =pp _d , e _v =vv _d , then

后，e_p和e_v都会收敛于0。Similarly elapsed time

After that, both e _p and e _v will converge to 0.

The convergence time of the position-velocity control loop is:

Similarly, the convergence time of the attitude angle control loop is:

Since the angle and angular velocity errors converge first, and then the position and velocity errors converge, and the disturbance convergence time T ₁ and T ₂ are independent of the control input, the total time for the trajectory tracking convergence is expressed as:

T≤max(T ₁ , T ₂ )+T ₃ +T ₄ .

As shown in Figure 7, the tracking trajectory of the position and attitude angle in the task space, under the action of the designed estimator-based fixed-time controller, the position and attitude angle in the task space are both within a fixed time (within 8 seconds) converges to its corresponding tracked target trajectory. Based on Figure 6 and Figure 7, the control input of the UAV is obtained, including the resultant force control amount and all torque control amounts, as shown in Figure 8, and finally the trajectory tracking diagram of the position and attitude angle in the task space is obtained, as shown in Figure 9, Figure 9 as a whole is very close to the simulation results in Figure 7. There is only a small error in the tracking of the position and attitude angle due to the accuracy of the sensor. Excluding the error caused by this factor, the position and attitude angle can be tracked within a fixed time (within 8 seconds). ) converges to its corresponding tracking target trajectory, and the result can support the control effect of the method of this embodiment.

Embodiment 2

A storage medium, where instructions are stored in the storage medium, and when a computer reads the instructions, the computer is made to execute the above-mentioned method for tracking and controlling the trajectory of a quadrotor unmanned aerial vehicle as described in the first embodiment.

The related technical solutions are the same as those in the first embodiment, and are not repeated here.

Those skilled in the art can easily understand that the above are only preferred embodiments of the present invention, and are not intended to limit the present invention. Any modifications, equivalent replacements and improvements made within the spirit and principles of the present invention, etc., All should be included within the protection scope of the present invention.

Claims (10)

1. a four-rotor unmanned aerial vehicle trajectory tracking control method, is characterized in that, comprises: Based on the desired position and its corresponding velocity and acceleration, as well as the actual position collected in real time and its corresponding velocity and angle, a position-velocity control loop is used to estimate the disturbance of the loop in real time, and based on the estimated amount of the disturbance, calculate the UAV The resultant force control amount and the reference first angle and its corresponding angular velocity and angular acceleration, wherein, the first angle is the roll angle and the pitch angle, and the disturbance estimation error of the loop converges to zero within a fixed time; Based on the reference first angle and its corresponding angular velocity and angular acceleration, the expected second angle and its corresponding angular velocity and angular acceleration, and the actual angle and its corresponding angular velocity, an attitude angle control loop is used to estimate the disturbance of the loop in real time, And based on the estimated amount of the disturbance, the torque control amount of the UAV rotating around the x, y, and z axes of the body coordinate system is calculated respectively, wherein the second angle is the yaw angle, and the disturbance estimation error of the loop is fixed. converges to zero in time; Based on the resultant force control amount and the torque control amount, the trajectory tracking of the UAV is controlled, wherein the angle, angular velocity errors, and position and velocity errors of the UAV converge to zero within a fixed time. 2. A kind of quadrotor unmanned aerial vehicle trajectory tracking control method according to claim 1, is characterized in that, described position speed control loop comprises and is used to make the disturbance estimation error of this loop within the first fixed time T ₁ A state estimator that converges to zero, expressed as:

in,

is the estimator of the actual disturbance D ₁ of the loop. At the initial time t ₀ , the actual disturbance D ₁ of the loop is zero, and L ₁ is the upper bound of the rate of change of the actual disturbance components of the loop; k _i , i=1,2,3 is the control gain constant;

k ₂ > 0, k ₃ > 4L ₁ , 0 < μ ₁ <1, λ ₁ > 1, v is the actual velocity vector of the drone, v _d is the expected velocity vector of the drone, g is the acceleration of gravity, u ₁ is the control amount of the resultant force, e _z represents the unit vector of the z-axis in the geodetic coordinate system, m is the mass of the drone, R∈R ^3×3 is the rotation in the order of z, y, and x of the body coordinate system. matrix, a _d is the expected acceleration vector of the UAV; Then the _first fixed time T1 is expressed as:

3. a kind of quadrotor unmanned aerial vehicle trajectory tracking control method according to claim 2, is characterized in that, described resultant force control quantity u ₁ is expressed as:

Wherein, α _i >0, β _i >0, 0<n _i <m _i , 0<p _i <q _i , i=1, 2, and m _i , n _i , p _i , and q _i are all positive odd numbers; p is the actual position of the UAV, p _d is the desired position of the UAV, e _v is the speed error of the UAV, and S ₁ is the preset sliding surface of the loop. 4. a kind of quadrotor unmanned aerial vehicle trajectory tracking control method according to claim 3, is characterized in that, described with reference to the calculation expression of the first angle is:

Among them, ψ is the yaw angle in the geodetic coordinate system. 5. a kind of quadrotor unmanned aerial vehicle trajectory tracking control method according to claim 4, is characterized in that, the position and speed error controlled by described position speed control loop converges to zero in the 4th fixed time T ₄ , expression The formula is:

6. a kind of quadrotor unmanned aerial vehicle trajectory tracking control method according to claim 5, is characterized in that, described attitude angle control link comprises for making the disturbance estimation error of this loop in the second fixed time T ₂ The inter-state estimator that converges to zero is expressed as:

in,

is the estimator of WD ₂ , D ₂ is the actual disturbance of the loop, L ₂ is the upper bound of the rate of change of each component of the actual disturbance of the loop, and at the initial time t ₀ , the actual disturbance of the loop D ₂ is zero, W is the coordinate system transformation matrix, I is the unit matrix, Ω is the actual angular velocity vector in the body coordinate system,

is the expected angular velocity vector of the UAV in the geodetic coordinate system, τ' is the sliding mode control law of the loop; k _i , i=4, 5, 6 are the control gain constants,

k ₅ >0, k ₆ >4L ₂ , 0<μ ₂ <1, λ ₂ >1; Then the second fixed time T ₂ is expressed as:

7. a kind of quadrotor unmanned aerial vehicle trajectory tracking control method according to claim 6, is characterized in that, described unmanned aerial vehicle is rotated around body coordinate system x, y, the moment control amount of z axis is respectively u ₂ , u ₃ , u ₄ , expressed as: [u ₂ , u ₃ , u ₄ ] ^T = τ; τ = I ^{- 1} Wτ';

Wherein, α _i > 0, β _i > 0, 0 < n _i <m _i , 0 < p _i <q _i , i=3, 4, and m _i , n _i , p _i , and q _i are all positive odd numbers; Θ is the actual angle vector of the UAV under the geodetic coordinate system, _Θd is the desired angle vector of the UAV under the geodetic coordinate system,

is the expected angular acceleration vector of the UAV in the geodetic coordinate system,

is the derivative of W,

S ₂ is the preset sliding surface of the loop. 8. a kind of quadrotor unmanned aerial vehicle trajectory tracking control method according to claim 7, is characterized in that, the angle that described attitude angle control loop controls, angular velocity error converges to zero in the 3rd fixed time T ₃ , T The expression for ₃ is:

9 . The trajectory tracking control method for a quadrotor UAV according to claim 8 , wherein the total convergence time of the trajectory tracking is T≦max(T ₁ , T ₂ )+T ₃ +T ₄ . 10 . 10. A storage medium, characterized in that, the storage medium stores instructions, and when a computer reads the instructions, the computer is made to execute one of the four methods according to any one of claims 1 to 9. Rotor UAV trajectory tracking control method.

Images