Anti-interference trajectory tracking control method for quad-rotor unmanned aerial vehicle based on observer and tracking differentiator

Abstract

The invention relates to a four-rotor unmanned aerial vehicle anti-interference trajectory tracking control method based on an observer and a tracking differentiator, and belongs to the field of aerial robot control. According to the method, various aerodynamic disturbances in the environment are estimated in real time by using the nonlinear disturbance observer; then, external interference estimated in real time is used as an interference compensation item of a sliding mode controller to obtain control laws of a four-rotor position subsystem and a four-rotor attitude subsystem, and meanwhile, an improved tracking differentiator is used for providing a smoother expected attitude signal; and finally, proving accurate estimation of the nonlinear disturbance observer on external disturbance through a global consistent final bounded stability theory, and proving the stability of the four-rotor control system through a Lyapunov method. A novel high-frequency switching function established in a control law is combined with an improved tracking differentiator, so that the buffeting problem of sliding mode control is effectively suppressed, and a nonlinear interference observer improves the anti-interference capability of four rotors; the control input buffeting is reduced while the robustness of the four rotors is improved.

Description

Technical Field

[0001] The invention relates to an anti-interference trajectory tracking control method for a quad-rotor unmanned aerial vehicle based on an observer and a tracking differentiator, and belongs to the technical field of aerial robot control. Background Art

[0002] With the continuous development of control science and aeronautical engineering, the application of quadrotor drones in inspection, rescue, communication and transportation has received more and more attention. At the same time, the strong robustness of its trajectory tracking control has become the focus of current research.

[0003] Although quadrotor drones have the advantages of hovering in the air, simple operation, and free steering, they are highly coupled, underactuated, and nonlinear time-varying systems with model uncertainty. Therefore, efficient and reliable control algorithms are required to achieve trajectory tracking control of quadrotors, and their fast convergence, strong robustness, and high tracking accuracy are the key to performing flight missions.

[0004] The technology of this invention is funded by the Yunnan Provincial Major Science and Technology Special Project (202302AD080002); the Yunnan Provincial Key Laboratory of Computer Technology Application Open Fund (CB22144S073A); and the "Xingdian Talent Support Plan" Industrial Innovation Talent Project (Yunnan Development and Reform Personnel

[2019] No. 1096). Summary of the invention

[0005] The present invention provides a quadrotor unmanned aerial vehicle anti-interference trajectory tracking control method based on an observer and a tracking differentiator, which is used to solve the disturbance problems of various aerodynamic disturbances and parameter changes in the flight environment of the quadrotor unmanned aerial vehicle. The present invention can observe various external aerodynamic disturbances of the quadrotor during flight, deal with parameter uncertainty, and then overcome disturbances to achieve robust control of trajectory tracking; at the same time, the present invention greatly reduces the jitter degree of the sliding mode control input by smoothing and transitioning the desired attitude signal.

[0006] The technical solution of the present invention is: a quadrotor unmanned aerial vehicle anti-interference trajectory tracking control method based on an observer and a tracking differentiator, and the specific steps of the method are as follows:

[0007] Step 1, establish a nonlinear dynamic model of a quadrotor drone subject to external aerodynamic disturbance;

[0008] Step 2: Based on the nonlinear dynamics model of the quadrotor UAV, a nonlinear disturbance observer is designed to estimate the external aerodynamic disturbance of the quadrotor in terms of position direction and attitude angle;

[0009] Step 3: Use the external aerodynamic disturbance estimated in real time by the nonlinear disturbance observer as the disturbance compensation term of the position sliding mode controller, and solve the control law of the position direction based on the sliding mode control theory;

[0010] Step 4, decouple the control law based on the position direction to obtain the desired roll angle and pitch angle, and use the improved tracking differentiator to smoothly transition and differentiate the desired attitude angle; the improved tracking differentiator adopts the combination of the classic second-order differentiator and the tracking differentiator;

[0011] Step 5. Use the external aerodynamic disturbance estimated in real time by the nonlinear disturbance observer as the disturbance compensation term of the attitude sliding mode controller, and solve the control law of the attitude angle based on the sliding mode control theory.

[0012] As a further solution of the present invention, the Step 1 specifically includes:

[0013] The position direction of the quadrotor in the inertial coordinate system is x, y, z, and the angles of rotation around the x, y, z axes in different directions are φ, θ, ψ, which represent the roll angle, pitch angle, and yaw angle respectively; the four control inputs u of the quadrotor drone are i (i=1,2,3,4) determines the attitude and direction of the drone, where u1 determines the position of direction z by controlling the lift of the drone propeller, u2 further determines the position of direction y by controlling the size of the roll angle φ, u3 further determines the position of direction x by controlling the size of the pitch angle θ, and u4 determines the yaw angle ψ by controlling the torque of the drone propeller;

[0014] The nonlinear dynamic model of the quadrotor drone subject to external aerodynamic disturbance is established as:

[0015]

[0016] Equations (1) and (2) represent the dynamic models of the position subsystem and attitude subsystem of the quadrotor, respectively, where: represent the first and second order differentials of P respectively, They represent the first-order differential and the second-order differential of A respectively. In the following formulas (1)-(19) of the present invention, the corresponding letter heads appearing in the same cases as in formulas (1) and (2) represent the first-order differential and the second-order differential of the corresponding letters; wherein sA and cA represent the sine function sin(A) and the cosine function cos(A) respectively; m is the mass of the quadrotor drone; g=9.81m / s 2 is the gravitational acceleration constant; I x ,I y ,I z K represents the moment of inertia of the quadrotor when it rotates around the x, y, and z coordinate axes of the body; x ,K y ,K z Indicates the air resistance coefficient, K φ ,K θ ,K ψRepresents the air resistance moment coefficient; d P =[d x ,d y ,d z ] T , d A =[d φ ,d θ ,d ψ ] T A total of 6 dimensions of interference represent the lumped interference terms of the quadrotor position dynamics model and attitude dynamics model respectively; let P x1 =[x,y,z] T , A x1 =[φ,θ,ψ] T

[0017]

[0018] Models (1) and (2) can be simplified as:

[0019]

[0020] in, represents the first-order differential, U P =[u x ,u y ,u z ] T ,u x ,u y ,u z Represent the control laws in the x, y, and z directions respectively.

[0021] As a further solution of the present invention, Step 2 includes the following:

[0022] According to equations (3) and (4), a nonlinear disturbance observer is designed, and the nonlinear disturbance observer is used to estimate the external aerodynamic disturbance of the quadrotor in the position direction and attitude angle respectively:

[0023]

[0024] Equations (5) and (6) represent the nonlinear disturbance observers of the position subsystem and attitude subsystem of the quadrotor, respectively. Here, Z P ,L P and Z A ,L A Represent the state and gain of the position disturbance observer and attitude disturbance observer respectively, They represent the first-order differentials of the position disturbance observer and attitude disturbance observer states, respectively, where Z P ,Z A ∈R 3 ,L P=L A =diag(l,l,l),l>0, Respectively represent the total interference term d P ,d A The estimate of P ,d A They represent the external aerodynamic disturbances of the quadrotor in the x, y, z position directions and φ, θ, ψ attitude angles respectively.

[0025] As a further solution of the present invention, in Step 3, the control law of the position direction obtained based on the sliding mode control theory specifically includes:

[0026] In the position control system, P d =[x d ,y d ,z d ] T Represents P x1 =[x,y,z] T The desired position of the sliding surface e is defined as the tracking error in the x, y, and z directions. P ,s P ,P∈(x,y,z) is as follows:

[0027]

[0028] where c x ,c y ,c z >0, is the parameter of the sliding surface, and the continuous high-frequency switching function (8) is introduced into the sliding mode reaching law to replace the sign function k·sign(s), k, ε>0 is a constant, s represents the sliding surface;

[0029]

[0030] The control law in the x direction is obtained by using the designed sliding surface formula (7) and the reaching law after replacing the high-frequency switching function for the controlled object formula (1):

[0031]

[0032] Here K x ,λ x ,ε x >0 is the control law parameter, The total disturbance d in the x direction is given by the position disturbance observer in equation (5). x estimated compensation;

[0033] Similarly, the control laws u in the y and z directions are obtained respectively. y ,u z :

[0034]

[0035] Here K y ,λ y ,ε y >0,k z ,λ z ,ε z >0 is the control law parameter, The position disturbance observer of formula (5) is the total disturbance d in the y and z directions respectively. y ,d z Estimated compensation.

[0036] As a further solution of the present invention, in Step 4, the desired roll angle and pitch angle are obtained by decoupling the control law based on the position direction, and the desired attitude angle is smoothly transitioned and differentiated by using an improved tracking differentiator, specifically including:

[0037] According to U P =[u x ,u y ,u z ] T The expression of and the control laws in the x, y, and z directions are decoupled to obtain the desired roll angle φ d and the desired pitch angle θ d , the expected yaw angle ψ d It is given directly;

[0038]

[0039] Further get the control input u1 expression:

[0040]

[0041] Next, we use the expected attitude angle φ d ,θ d ,ψ d The improved tracking differentiator for smooth transition and differential solution is designed as:

[0042]

[0043] Here x j (·), j = 1, 2, 3, 4 are the state variables of the improved tracking differentiator, h is the sampling period, k t is the current discrete moment, a, b>0 are adjustable parameters, U f=fhan(x1-v,x2,r0,h0) represents the optimal synthetic fast control function (15), which ensures the fastest convergence of x1→v without overshoot, where v represents the reference signal, r0 and h0 are constants, which are adjusted separately according to the required transition speed and smoothness; where x1, x2, and x4 represent the signal after smoothing the reference signal v, the first-order differential smoothing signal, and the second-order differential smoothing signal, respectively, and x t represents the function fsg(x t ,d) variables;

[0044]

[0045] As a further solution of the present invention, in Step 5, the control law of the attitude angle obtained by solving the control law based on the sliding mode control theory specifically includes:

[0046] In the attitude control system, A d =[φ d ,θ d ,ψ d ] T Representative A x1 =[φ,θ,ψ] T The desired attitude angle defines the tracking errors in the three directions of φ, θ, and ψ and the sliding surface e A ,s A ,A∈(φ,θ,ψ) is as follows:

[0047]

[0048] where c φ ,c θ ,c ψ >0, is the parameter of sliding surface;

[0049] For the controlled object (2), the designed sliding surface (16) and the reaching law after high-frequency switching function replacement are used to solve the control law u4 of the yaw angle ψ:

[0050]

[0051] Here K ψ ,λ ψ ,ε ψ >0 is the reaching law parameter, is the lumped disturbance d of the attitude disturbance observer on the attitude angle ψ in equation (6) ψ estimated compensation;

[0052] Similarly, the control laws u2 and u3 at angles φ and θ are obtained respectively:

[0053]

[0054] Here K φ ,λ φ ,ε ψ >0,k θ ,λ θ ,ε θ >0 is the control law parameter, The attitude disturbance observer of formula (6) is respectively φ ,d θ Estimated compensation.

[0055] The beneficial effects of the present invention are as follows: the present invention improves the robustness of the quadrotor UAV in dealing with various external aerodynamic disturbances, while reducing the inherent jitter problem of sliding mode control; compared with conventional sliding mode control, it has stronger robustness and lower control input jitter; compared with terminal sliding mode control, it avoids the singularity problem, reduces the complexity of controller design and achieves a convergence speed similar to that of non-singular fast terminal sliding mode control; and has high tracking accuracy when dealing with nonlinear high-frequency large disturbances. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] Figure 1 It is a schematic diagram of the structure of the quad-rotor UAV of the present invention;

[0057] Figure 2 It is a structural diagram of the control system of the present invention;

[0058] Figure 3 It is a diagram showing the estimation result of the nonlinear disturbance observer of the present invention for external disturbance;

[0059] Figure 4 It is a comparison diagram of the trajectory tracking effect of the present invention;

[0060] Figure 5 It is the expected posture tracking effect diagram of the present invention;

[0061] Figure 6 It is a recording diagram of the control input jitter degree of the present invention;

[0062] Figure 7 It is a comparison diagram of the 3D space trajectory tracking effect of the present invention. DETAILED DESCRIPTION

[0063] Example 1: Figure 1 The structure diagram of the quadcopter shown in the figure, the body coordinate system B = (o b ,x b ,y b ,z b ) and the inertial coordinate system E=(o e ,x e ,y e ,z e) point in the same direction; propellers 1 and 3 rotate counterclockwise, and propellers 2 and 4 rotate clockwise. The attitude adjustment or trajectory tracking of the UAV is achieved by changing the control input to control the rotation speed of the four propellers;

[0064] The present invention builds a simulation model in the MATLAB / Simulink environment to simulate the control process of the quad-rotor drone tracking the target trajectory. Figure 2 The control system structure diagram of the present invention is shown, wherein NDO is the abbreviation of nonlinear disturbance observer, and TD is the abbreviation of tracking differentiator.

[0065] The anti-interference trajectory tracking control method of a quadrotor drone based on an observer and a tracking differentiator, the specific steps of the method are as follows:

[0066] Step 1, establish a nonlinear dynamic model of a quadrotor drone subject to external aerodynamic disturbance;

[0067] The Step 1 specifically includes:

[0068] The position direction of the quadrotor in the inertial coordinate system is x, y, z, and the angles of rotation around the x, y, z axes in different directions are φ, θ, ψ, which represent the roll angle, pitch angle, and yaw angle respectively; the four control inputs u of the quadrotor drone are i (i=1,2,3,4) determines the attitude and direction of the drone, where u1 determines the position of direction z by controlling the lift of the drone propeller, u2 further determines the position of direction y by controlling the size of the roll angle φ, u3 further determines the position of direction x by controlling the size of the pitch angle θ, and u4 determines the yaw angle ψ by controlling the torque of the drone propeller;

[0069] The parameter settings of the quad-rotor drone of the present invention are shown in Table 1:

[0070] Table 1: Quadrotor UAV parameters

[0071] name parameter value unit quality m 1.23 kg Gravity acceleration g 9.81 <![CDATA[ms 2 ]]> Arm length l 0.20 m Moment of inertia about the x, y, and z axes <![CDATA[I x ,I y ,I z ]]> <![CDATA[(0.91,0.96,1.89)·10 -2 ]]> <![CDATA[kg·m 2 ]]> Air resistance coefficient <![CDATA[K x ,K y ,K z ]]> <![CDATA[(1.00,1.20,1.90)·10 -2 ]]> N·sm Air resistance moment coefficient <![CDATA[K φ ,K θ ,K ψ ]]> <![CDATA[(0.22,0.24,0.31)·10 -2 ]]> N·m·srad

[0072] The nonlinear dynamic model of the quadrotor drone subject to external aerodynamic disturbance is established as:

[0073]

[0074] Equations (1) and (2) represent the dynamic models of the position subsystem and attitude subsystem of the quadrotor, respectively, where: represent the first and second order differentials of P respectively, Represent the first and second order differentials of A respectively; where sA and cA represent the sine function sin(A) and cosine function cos(A) respectively; m is the mass of the quadcopter; g = 9.81 m / s 2is the gravitational acceleration constant; I x ,I y ,I z K represents the moment of inertia of the quadrotor when it rotates around the x, y, and z coordinate axes of the body; x ,K y ,K z Indicates the air resistance coefficient, K φ ,K θ ,K ψ Represents the air resistance moment coefficient; d P =[d x ,d y ,d z ] T , d A =[d φ ,d θ ,d ψ ] T A total of 6 dimensions of interference represent the lumped interference terms of the quadrotor position dynamics model and attitude dynamics model respectively; let P x1 =[x,y,z] T , A x1 =[φ,θ,ψ] T

[0075]

[0076] Models (1) and (2) can be simplified as:

[0077]

[0078] in, represents the first-order differential, U P =[u x ,u y ,u z ] T ,u x ,u y ,u z Represent the control laws in the x, y, and z directions respectively.

[0079] Step 2: Based on the nonlinear dynamics model of the quadrotor UAV, a nonlinear disturbance observer is designed to estimate the external aerodynamic disturbance of the quadrotor in terms of position direction and attitude angle;

[0080] As a further solution of the present invention, Step 2 includes the following:

[0081] According to equations (3) and (4), a nonlinear disturbance observer is designed, and the nonlinear disturbance observer is used to estimate the external aerodynamic disturbance of the quadrotor in the position direction and attitude angle respectively:

[0082]

[0083] Equations (5) and (6) represent the nonlinear disturbance observers of the position subsystem and attitude subsystem of the quadrotor, respectively. Here, Z P ,L P and Z A ,L A Represent the state and gain of the position disturbance observer and attitude disturbance observer respectively, They represent the first-order differentials of the position disturbance observer and attitude disturbance observer states, respectively, where Z P ,Z A ∈R 3 ,L P =L A =diag(l,l,l),l>0, Respectively represent the total interference term d P ,d A The estimate of P ,d A They represent the external aerodynamic disturbances of the quadrotor in the x, y, z position directions and φ, θ, ψ attitude angles respectively. The parameters are selected: the gain of the nonlinear disturbance observer is l = 200.

[0084] The stability of the disturbance observer is proved using the globally consistent eventually bounded stability theory:

[0085] Taking the nonlinear disturbance observation of the position subsystem as an example to prove its stability, the disturbance estimation error is defined as Assuming external interference L d >0, that is, the perturbation differential is bounded.

[0086]

[0087] make Taking its time derivative we get:

[0088]

[0089] Right now Solving this differential inequality yields:

[0090] because So we get

[0091]

[0092] Therefore, the interference estimation error is globally consistent and ultimately bounded and stable when l ≥ L d and is large enough, the location aggregate interference estimate The aggregate disturbance d of the position subsystem can be tracked asymptoticallyP Similarly, the stability of the nonlinear disturbance observation of the attitude subsystem can be proved.

[0093] Step 3: Use the external aerodynamic disturbance estimated in real time by the nonlinear disturbance observer as the disturbance compensation term of the position sliding mode controller, and solve the control law of the position direction based on the sliding mode control theory;

[0094] As a further solution of the present invention, in Step 3, the control law of the position direction obtained based on the sliding mode control theory specifically includes:

[0095] In the position control system, P d =[x d ,y d ,z d ] T Represents P x1 =[x,y,z] T The desired position of the sliding surface e is defined as the tracking error in the x, y, and z directions. P ,s P ,P∈(x,y,z) is as follows:

[0096]

[0097] where c x ,c y ,c z >0, is the parameter of the sliding surface, and the continuous high-frequency switching function (8) is introduced into the sliding mode reaching law to replace the sign function k·sign(s), k, ε>0 is a constant, s represents the sliding surface;

[0098]

[0099] The control law in the x direction is obtained by using the designed sliding surface formula (7) and the reaching law after replacing the high-frequency switching function for the controlled object formula (1):

[0100]

[0101] Here K x ,λ x ,ε x >0 is the control law parameter, The total disturbance d in the x direction is given by the position disturbance observer in equation (5). x estimated compensation;

[0102] Similarly, the control laws u in the y and z directions are obtained respectively. y ,u z :

[0103]

[0104] Here K y ,λ y ,ε y >0,k z ,λ z ,ε z >0 is the control law parameter, The position disturbance observer of formula (5) is the total disturbance d in the y and z directions respectively. y ,d z estimated compensation;

[0105] Among them, select parameters: position controller parameter k x =k y =k z =6,ε x =ε y =ε z =8,λ x =λ y =λ z =1, c x =c y =c z =3;

[0106] The stability of the position controller is proved using Lyapunov theory:

[0107] Lemma 1: For a smooth multivariable dynamic system, V is a strictly positive definite and continuously differentiable Lyapunov function, let is any set of connected compact initial conditions given by a dynamic system. Assume that along any trajectory of the system, x:R + →R n , starting from C, the differential inequality:

[0108]

[0109] For any t ≥ 0, x(0)∈C satisfies β>0, which is a fixed positive constant and α is an adjustable positive parameter. Under these conditions, all trajectories of the dynamic system starting from C will remain bounded for a sufficiently large time.

[0110] Take the x direction as an example to prove the tracking error e x is stable. Choose the Lyapunov function Based on formula (1) and formula (7), V x Taking the time derivative we get:

[0111]

[0112] Here are the controller parameters From Lemma 1, we can see that the coefficient k of the switching functionx It only needs to be no less than the interference estimation error to ensure e x The stability of the tracking error in the y and z directions can be proved by the same logic. y ,e z is stable.

[0113] Step 4, decouple the control law based on the position direction to obtain the desired roll angle and pitch angle, and use the improved tracking differentiator to smoothly transition and differentiate the desired attitude angle; the improved tracking differentiator adopts the combination of the classic second-order differentiator and the tracking differentiator;

[0114] As a further solution of the present invention, in Step 4, the desired roll angle and pitch angle are obtained by decoupling the control law based on the position direction, and the desired attitude angle is smoothly transitioned and differentiated by using an improved tracking differentiator, specifically including:

[0115] According to U P =[u x ,u y ,u z ] T The expression of and the control laws in the x, y, and z directions are decoupled to obtain the desired roll angle φ d and the desired pitch angle θ d , the expected yaw angle ψ d It is given directly;

[0116]

[0117] Further get the control input u1 expression:

[0118]

[0119] Next, we use the expected attitude angle φ d ,θ d ,ψ d The improved tracking differentiator for smooth transition and differential solution is designed as:

[0120]

[0121] Here x j (·), j = 1, 2, 3, 4 are the state variables of the improved tracking differentiator, h = 0.01s is the sampling period, k t is the current discrete moment, a, b>0 are adjustable parameters, U f=fhan(x1-v,x2,r0,h0) represents the optimal synthetic fast control function (15), which ensures the fastest convergence of x1→v without overshoot, where v represents the reference signal, r0 and h0 are constants, which are adjusted separately according to the required transition speed and smoothness; the improved tracking differentiator parameters are: a=30, b=10, r0=300, h=h0=0.01; where x1, x2, x4 represent the signal after smoothing the reference signal v, the first-order differential smoothing signal, and the second-order differential smoothing signal, respectively, and x t represents the function fsg(x t ,d) variables;

[0122]

[0123] Step 5. Use the external aerodynamic disturbance estimated in real time by the nonlinear disturbance observer as the disturbance compensation term of the attitude sliding mode controller, and solve the control law of the attitude angle based on the sliding mode control theory.

[0124] As a further solution of the present invention, in Step 5, the control law of the attitude angle obtained by solving the control law based on the sliding mode control theory specifically includes:

[0125] In the attitude control system, A d =[φ d ,θ d ,ψ d ] T Representative A x1 =[φ,θ,ψ] T The desired attitude angle defines the tracking errors in the three directions of φ, θ, and ψ and the sliding surface e A ,s A ,A∈(φ,θ,ψ) is as follows:

[0126]

[0127] where c φ ,c θ ,c ψ >0, is the parameter of sliding surface;

[0128] For the controlled object (2), the designed sliding surface (16) and the reaching law after high-frequency switching function replacement are used to solve the control law u4 of the yaw angle ψ:

[0129]

[0130] Here K ψ ,λ ψ ,ε ψ >0 is the reaching law parameter, is the lumped disturbance d of the attitude disturbance observer on the attitude angle ψ in equation (6)ψ estimated compensation;

[0131] Similarly, the control laws u2 and u3 at angles φ and θ are obtained respectively:

[0132]

[0133] Here K φ ,λ φ ,ε φ >0,k θ ,λ θ ,ε θ >0 is the control law parameter, The attitude disturbance observer of formula (6) is respectively φ ,d θ Estimation compensation; Specifically, select parameters: attitude controller parameter k φ =k θ =k ψ =5,ε φ =ε θ =ε ψ =1,λ φ =λ θ =20,λ ψ =0.8, c φ =c θ =30, c ψ =3.

[0134] In order to verify the effectiveness of the control strategy of the present invention, simulation experiments were carried out to compare the high-frequency sliding mode control (NDO-HFSMC) based on the strategy proposed in the present invention with the self-disturbance rejection control (SMADRC) based on sliding mode, the robust backstepping integral sliding mode control (RBISMC) and the non-singular fast terminal sliding mode control (DO-NFTSMC) based on observer.

[0135] Figure 3 The designed nonlinear disturbance observer estimates the external disturbance in six dimensions. The dotted line represents the actual external disturbance, and the solid line is the disturbance observer's estimate of the external disturbance. It can be seen from the figure that after 5 seconds, whether static disturbance or nonlinear dynamic disturbance occurs, the observer can quickly and accurately estimate the size of the external disturbance.

[0136] Figure 4Reference in the figure represents the target trajectory. After the 5-second interference, the control strategy NDO-HFSMC proposed in the present invention has the highest tracking accuracy compared with the other three control strategies. In order to accurately compare the differences between different control strategies, the root mean square error RMSE and the absolute value error ISE are used as evaluation indicators for comparison. The results are shown in Table 2. The control strategy proposed in the present invention shows the smallest error.

[0137] Table 2: Trajectory tracking error performance indicators after interference occurs

[0138]

[0139] Figure 5 Denotes the desired roll angle φ after decoupling d and the desired pitch angle θ d The change of the expected yaw angle ψ d It is given as π / 6. As can be seen from the figure, the designed attitude controller can accurately track the decoupled desired roll angle and pitch angle as well as the given desired yaw angle.

[0140] Figure 6 The size changes and jitter levels of the four control inputs u1, u2, u3, and u4 were recorded. In order to accurately measure the control input jitter levels of the four control strategies, the control input absolute incremental integral (IADU) was used as an accurate indicator for numerical comparison. The results are shown in Table 3. The control input jitter level of the control strategy NDO-HFSMC proposed in the present invention is the lowest, which can reduce the loss of the motor rotor and increase the service life of the actuator, which makes the present invention more practical. Figure 7 The tracking of the target trajectory (Reference) by the four control strategies is more intuitively reflected in the 3D space. It can be found from the figure that the control strategy proposed by the present invention shows extremely strong robustness and high tracking accuracy whether in the vertical ascent stage, spiral ascent stage or hovering stage.

[0141] In summary, the above are only specific embodiments of the present invention. The present invention is not limited to the above specific embodiments. Those skilled in the art can make various modifications or variations within the scope of the claims.

[0142] Table 3: IADU performance indicators of control signals

[0143]

[0144] The specific implementation modes of the present invention are described in detail above in conjunction with the accompanying drawings, but the present invention is not limited to the above implementation modes, and various changes can be made within the knowledge scope of ordinary technicians in this field without departing from the purpose of the present invention.

Claims

1. An anti-interference trajectory tracking control method for a quadrotor drone based on an observer and a tracking differentiator, characterized in that: The specific steps of the method are as follows: Step 1, establish a nonlinear dynamic model of a quadrotor drone subject to external aerodynamic disturbance; Step 2: Based on the nonlinear dynamics model of the quadrotor UAV, a nonlinear disturbance observer is designed to estimate the external aerodynamic disturbance of the quadrotor in terms of position direction and attitude angle; Step 3: Use the external aerodynamic disturbance estimated in real time by the nonlinear disturbance observer as the disturbance compensation term of the position sliding mode controller, and solve the control law of the position direction based on the sliding mode control theory; Step 4, decouple the control law based on the position direction to obtain the desired roll angle and pitch angle, and use the improved tracking differentiator to smoothly transition and differentiate the desired attitude angle; the improved tracking differentiator adopts the combination of the classic second-order differentiator and the tracking differentiator; Step 5. Use the external aerodynamic disturbance estimated in real time by the nonlinear disturbance observer as the disturbance compensation term of the attitude sliding mode controller, and solve the control law of the attitude angle based on the sliding mode control theory.

2. The anti-interference trajectory tracking control method of a quadrotor drone based on an observer and a tracking differentiator according to claim 1 is characterized in that: The Step 1 specifically includes: The position direction of the quadrotor in the inertial coordinate system is x, y, z, and the angles of rotation around the x, y, z axes in different directions are φ, θ, ψ, which represent the roll angle, pitch angle, and yaw angle respectively; the four control inputs u of the quadrotor drone are i (i=1,2,3,4) determines the attitude and direction of the drone, where u1 determines the position of direction z by controlling the lift of the drone propeller, u2 further determines the position of direction y by controlling the size of the roll angle φ, u3 further determines the position of direction x by controlling the size of the pitch angle θ, and u4 determines the yaw angle ψ by controlling the torque of the drone propeller; The nonlinear dynamic model of the quadrotor drone subject to external aerodynamic disturbance is established as: Equations (1) and (2) represent the dynamic models of the position subsystem and attitude subsystem of the quadrotor, respectively, where: represent the first and second order differentials of P respectively, Represent the first and second order differentials of A respectively; where sA and cA represent the sine function sin(A) and cosine function cos(A) respectively; m is the mass of the quadcopter; g = 9.81 m / s 2 is the gravitational acceleration constant; I x ,I y ,I z K represents the moment of inertia of the quadrotor when it rotates around the x, y, and z coordinate axes of the body; x ,K y ,K z Indicates the air resistance coefficient, K φ ,K θ ,K ψ Represents the air resistance moment coefficient; d P =[d x ,d y ,d z ] T , d A =[d φ ,d θ ,d ψ ] T A total of 6 dimensions of interference represent the lumped interference terms of the quadrotor position dynamics model and attitude dynamics model respectively; let P x1 =[x,y,z] T , A x1 =[φ,θ,ψ] T Models (1) and (2) can be simplified as: in, represents the first-order differential, U P =[u x ,u y ,u z ] T ,u x ,u y ,u z Represent the control laws in the x, y, and z directions respectively.

3. The anti-interference trajectory tracking control method of a quadrotor drone based on an observer and a tracking differentiator according to claim 2 is characterized in that: The Step 2 includes the following: According to equations (3) and (4), a nonlinear disturbance observer is designed, and the nonlinear disturbance observer is used to estimate the external aerodynamic disturbance of the quadrotor in the position direction and attitude angle respectively: Equations (5) and (6) represent the nonlinear disturbance observers of the position subsystem and attitude subsystem of the quadrotor, respectively. Here, Z P ,L P and Z A ,L A Represent the state and gain of the position disturbance observer and attitude disturbance observer respectively, They represent the first-order differentials of the position disturbance observer and attitude disturbance observer states, respectively, where Z P ,Z A ∈R 3 ,L P =L A =diag(l,l,l),l>0, Respectively represent the total interference term d P ,d A The estimate of P ,d A They represent the external aerodynamic disturbances of the quadrotor in the x, y, z position directions and φ, θ, ψ attitude angles respectively.

4. The anti-interference trajectory tracking control method of a quadrotor drone based on an observer and a tracking differentiator according to claim 3 is characterized in that: In the Step 3, the control law of the position direction obtained based on the sliding mode control theory specifically includes: In the position control system, P d =[x d ,y d ,z d ] T Represents P x1 =[x,y,z] T The desired position of the sliding surface e is defined as the tracking error in the x, y, and z directions. P ,s P ,P∈(x,y,z)is as follows: where c x ,c y ,c z >0, is the parameter of the sliding surface, and the continuous high-frequency switching function (8) is introduced into the sliding mode reaching law to replace the sign function k·sign(s), k, ε>0 is a constant, s represents the sliding surface; The control law in the x direction is obtained by using the designed sliding surface formula (7) and the reaching law after replacing the high-frequency switching function for the controlled object formula (1): Here K x ,λ x ,ε x >0 is the control law parameter, The total disturbance d in the x direction is given by the position disturbance observer in equation (5). x estimated compensation; Similarly, the control laws u in the y and z directions are obtained respectively. y ,u z : Here K y ,λ y ,ε y >0,k z ,λ z ,ε z >0 is the control law parameter, The position disturbance observer of formula (5) is the total disturbance d in the y and z directions respectively. y ,d z Estimated compensation.

5. The anti-interference trajectory tracking control method of a quadrotor drone based on an observer and a tracking differentiator according to claim 4 is characterized in that: In the Step 4, the desired roll angle and pitch angle are obtained by decoupling the control law based on the position direction, and the desired attitude angle is smoothly transitioned and differentiated by using the improved tracking differentiator. Specifically, the following steps are performed: According to U P =[u x ,u y ,u z ] T The expression of and the control laws in the x, y, and z directions are decoupled to obtain the desired roll angle φ d and the desired pitch angle θ d , the expected yaw angle ψ d It is given directly; Further get the control input u1 expression: Next, we use the expected attitude angle φ d ,θ d ,ψ d The improved tracking differentiator for smooth transition and differential solution is designed as: Here x j (·), j = 1, 2, 3, 4 are the state variables of the improved tracking differentiator, h is the sampling period, k t is the current discrete moment, a, b>0 are adjustable parameters, U f =fhan(x1-v,x2,r0,h0) represents the optimal synthetic fast control function (15), which ensures the fastest convergence of x1→v without overshoot, where v represents the reference signal, r0 and h0 are constants, which are adjusted separately according to the required transition speed and smoothness; where x1, x2, and x4 represent the signal after smoothing the reference signal v, the first-order differential smoothing signal, and the second-order differential smoothing signal, respectively, and x t represents the function fsg(x t ,d) variables; 6. The anti-interference trajectory tracking control method of a quadrotor drone based on an observer and a tracking differentiator according to claim 2 is characterized in that: In the Step 5, the control law of the attitude angle obtained by solving the sliding mode control theory specifically includes: In the attitude control system, A d =[φ d ,θ d ,ψ d ] T Representative A x1 =[φ,θ,ψ] T The desired attitude angle defines the tracking errors in the three directions of φ, θ, and ψ and the sliding surface e A ,s A ,A∈(φ,θ,ψ) is as follows: where c φ ,c θ ,c ψ >0, is the parameter of sliding surface; For the controlled object (2), the designed sliding surface (16) and the reaching law after high-frequency switching function replacement are used to solve the control law u4 of the yaw angle ψ: Here K ψ ,λ ψ ,ε ψ >0 is the reaching law parameter, is the total disturbance d of the attitude disturbance observer on the attitude angle ψ in equation (6) ψ estimated compensation; Similarly, the control laws u2 and u3 at angles φ and θ are obtained respectively: Here K φ ,λ φ ,ε φ >0,k θ ,λ θ ,ε θ >0 is the control law parameter, The attitude disturbance observer of formula (6) is respectively φ ,d θ Estimated compensation.

Images