Discrete-time trajectory tracking control method for small unmanned helicopter based on disturbance observer

Abstract

The present invention discloses a discrete-time trajectory tracking control method for a small unmanned helicopter based on a disturbance observer, comprising: establishing a nonlinear continuous model of a small unmanned helicopter; discretizing the nonlinear continuous model of the small unmanned helicopter to obtain a nonlinear discrete model of the small unmanned helicopter; designing a discrete-time disturbance observer to estimate the influence of external disturbances in the position loop and attitude loop of the small unmanned helicopter on the force and torque of the small unmanned helicopter; based on the nonlinear discrete model of the small unmanned helicopter and the discrete-time disturbance observer, respectively designing tracking controllers for the position loop, attitude loop and waving motion subsystem of the small unmanned helicopter; selecting control parameters according to the Lyapunov stability theory to ensure the boundedness of the system error signal. The present invention can ensure that the small unmanned helicopter can fly safely and stably in the presence of external disturbances, and realizes the robust anti-interference trajectory tracking flight control of the small unmanned helicopter under external disturbances.

Description

Technical Field

[0001] The invention relates to aircraft control technology, and in particular to a discrete time trajectory tracking flight control method for a small unmanned helicopter based on an interference observer. Background Art

[0002] Small unmanned helicopters are unmanned aerial vehicles that can take off and land vertically, hover in the air, fly in any direction, have small takeoff and landing sites, are small in size, and are flexible to operate. They are widely used in military and civilian fields such as reconnaissance, strike, tracking, target interception, atmospheric monitoring, and power monitoring. As a basic component of the UAV system, the flight control system is of great significance for ensuring flight control safety. At the same time, unmanned helicopters are easily affected by external interference airflow during the execution of missions. Therefore, studying efficient and robust flight control methods to ensure the flight safety of unmanned helicopters is an important direction for unmanned helicopter research.

[0003] Most of the existing unmanned helicopter flight control methods are based on the UAH nonlinear continuous model. With the continuous development of modern industrial production technology, the system model obtained based on real engineering abstraction has become increasingly complex. In order to meet actual control needs, more and more high-performance computers are used. When the controlled system is controlled by a computer, the continuous time signal needs to be converted into a discrete time signal. Therefore, the study of discrete time tracking control methods has practical significance for modern actual control needs. Summary of the invention

[0004] Purpose of the invention: The purpose of the present invention is to provide a discrete-time trajectory tracking flight control method for a small unmanned helicopter based on a disturbance observer, so as to improve the flight safety performance of the small unmanned helicopter under external disturbances.

[0005] Technical solution: The discrete-time trajectory tracking control method of a small unmanned helicopter based on a disturbance observer of the present invention comprises the following steps:

[0006] S1. Based on the idea of ​​mechanism modeling, a nonlinear continuous model of a small unmanned helicopter with external interference, including waving motion, is established;

[0007] S2. Based on the nonlinear continuous model of the small unmanned helicopter, the nonlinear continuous model of the small unmanned helicopter is discretized by the Euler approximation method to obtain a nonlinear discrete model of the small unmanned helicopter;

[0008] S3. Design a discrete-time disturbance observer to estimate the effect of external disturbances on the forces and torques of a small unmanned helicopter in its position and attitude loops.

[0009] S4. Based on the nonlinear discrete model of the small unmanned helicopter and the discrete-time disturbance observer, the tracking controllers of the position loop, attitude loop and waving motion subsystem of the small unmanned helicopter are designed respectively to realize the discrete-time trajectory tracking control of the small unmanned helicopter;

[0010] S5. Select the Lyapunov function of the control system and choose the control parameters according to the Lyapunov stability theory to ensure the boundedness of the system error signal.

[0011] Furthermore, the nonlinear continuous model of the small unmanned helicopter established in step S1 is:

[0012]

[0013]

[0014]

[0015]

[0016]

[0017]

[0018] Wherein, P is the position vector of the small unmanned helicopter in the inertial coordinate system; is the derivative of the position vector P; V is the velocity vector of the center of mass of the small unmanned helicopter in the inertial coordinate system; is the derivative of the velocity vector V; Λ is the attitude angle vector of the small unmanned helicopter in the body coordinate system; is the derivative of the attitude angle vector Λ; Ω is the angular velocity vector of the small unmanned helicopter in the body coordinate system; is the derivative of the angular velocity vector Ω; is the rotation matrix from the body coordinate system to the inertial coordinate system; is the parameter matrix; H is the attitude rotation matrix; J is the diagonal inertia matrix of the small unmanned helicopter; F and Σ are the resultant external force and torque acting on the small unmanned helicopter in the body coordinate system, m is the mass of the small unmanned helicopter; g is the gravitational acceleration; a is the longitudinal swing angle of the small unmanned helicopter; is the derivative of the longitudinal flapping angle a; b is the lateral flapping angle of the small unmanned helicopter; is the derivative of the lateral flapping angle b; τ e A is the time constant of the main rotor of a small unmanned helicopter; lon and B lat are the longitudinal and lateral gains of the main rotor cyclic pitch input respectively; T a and T bare the main rotor longitudinal cyclic pitch input and the main rotor lateral cyclic pitch input respectively; D v and D Ω are the external bounded unknown time-varying disturbances that affect the forces and moments of the unmanned helicopter in the system, and Interference vector D = [D v ,D Ω ] T , is the derivative of the interference vector D, Is a normal number.

[0019] Furthermore, the nonlinear discrete model of the small unmanned helicopter obtained in step S2 is:

[0020] P(k+1)=P(k)+T0V(k)

[0021]

[0022] Λ(k+1)=Λ(k)+T0H(k)Ω(k)

[0023] Ω(k+1)=Ω(k)+T0(-J -1 (Ω(k)×JΩ(k))+J -1 Σ(k))+T0D Ω (k)

[0024] =Ω(k)+T0(-J -1 (Ω(k)×JΩ(k))+A(T m (k))U1(k)+B(T m (k)))+T0D Ω (k)

[0025]

[0026]

[0027] Among them, P(k) and P(k+1) are the position vectors of the small unmanned helicopter at time k and time k+1 in the inertial coordinate system respectively; V(k) and V(k+1) are the velocity vectors of the center of mass of the small unmanned helicopter at time k and time k+1 in the inertial coordinate system respectively; T0 is the sampling period; m is the mass of the small unmanned helicopter; g is the acceleration of gravity; is the parameter matrix; is the rotation matrix from the body coordinate system to the inertial coordinate system at time k in the discrete state; F(k) and Σ(k) are the resultant external force and torque acting on the small unmanned helicopter at time k in the body coordinate system, respectively; D v (k) and D Ω(k) are the external bounded unknown time-varying disturbances that affect the forces and torques of the unmanned helicopter in the system at time k in the discrete state; Λ(k) and Λ(k+1) are the attitude angle vectors of the small unmanned helicopter at time k and time k+1 in the body coordinate system respectively; H(k) is the attitude rotation matrix at time k in the discrete state; Ω(k) and Ω(k+1) are the angular velocity vectors of the small unmanned helicopter at time k and time k+1 in the body coordinate system respectively; J is the diagonal inertia matrix of the small unmanned helicopter; A(T m (k)) is the parameter matrix of the reference input at time k in the discrete state; U1(k) is the state matrix of the longitudinal flapping angle, lateral flapping angle and tail rotor thrust of the unmanned helicopter at time k in the discrete state; B(T m T (k)) is the parameter matrix at time k in the discrete state; a(k) and a(k+1) are the longitudinal swing angles of the small unmanned helicopter at time k and time k+1 in the discrete state respectively; a (k) and T b (k) are the main rotor longitudinal cyclic pitch input and the main rotor lateral cyclic pitch input at time k and time k+1 in the discrete state respectively; b(k) and b(k+1) are the lateral flapping angles of the small unmanned helicopter at time k and time k+1 in the discrete state respectively; q(k) and p(k) are the pitch angular velocity and roll angle vector of the small unmanned helicopter at time k in the discrete state respectively; τ e A is the time constant of the main rotor of a small unmanned helicopter; lon and B lat They are the longitudinal and lateral gains of the main rotor cyclic pitch change input respectively.

[0028] Furthermore, the design method of the discrete-time disturbance observer in step S3 is:

[0029] S31. Based on the position loop discrete time model of the external disturbance in the nonlinear discrete model of the small unmanned helicopter, the position loop disturbance observer is designed as follows:

[0030] σ1(k)=ρD1(k)-ρV(k)

[0031]

[0032] in, D1(k) = T0D v (k) is estimated, T0 is the sampling period, D v (k) is the bounded unknown time-varying disturbance in the system at time k in the discrete state; σ1(k) is the intermediate variable, is the estimate of σ1(k); ρ is a positive constant; V(k) is the velocity vector of the center of mass of the small unmanned helicopter at time k in the inertial coordinate system;

[0033] S32. According to the attitude loop discrete time model of the external disturbance existing in the nonlinear discrete model of the small unmanned helicopter, the attitude loop disturbance observer is designed in the following form:

[0034] σ2(k)=ρ2D2(k)-ρ2Ω(k)

[0035]

[0036] in, D2(k) = T0D Ω The estimate of (k), D Ω (k) is the bounded unknown time-varying disturbance in the system at time k in the discrete state; σ2(k) is the intermediate variable, is the estimate of σ2(k); ρ2 is a positive constant, and Ω(k) is the angular velocity vector of the small unmanned helicopter at time k in the body coordinate system.

[0037] Furthermore, the design method of the tracking controller of the position loop, attitude loop and waving motion subsystem of the small unmanned helicopter in step S4 is:

[0038] S41. Design a tracking controller for the position loop of a small unmanned helicopter;

[0039] According to the nonlinear discrete model of the small unmanned helicopter, the following small unmanned helicopter position loop subsystem is considered:

[0040] P(k+1)=P(k)+T0V(k)

[0041]

[0042] Among them, P(k) and P(k+1) are the position vectors of the small unmanned helicopter at time k and time k+1 in the inertial coordinate system respectively; V(k) and V(k+1) are the velocity vectors of the center of mass of the small unmanned helicopter at time k and time k+1 in the inertial coordinate system respectively; T0 is the sampling period; m is the mass of the small unmanned helicopter; g is the acceleration of gravity; is the parameter matrix; is the rotation matrix from the body coordinate system to the inertial coordinate system at time k in the discrete state; F(k) is the resultant external force acting on the small unmanned helicopter at time k in the body coordinate system; D v (k) is the bounded unknown time-varying disturbance in the system at time k in the discrete state;

[0043] Design of virtual control law for position tracking controller of small unmanned helicopter d (k) is:

[0044]

[0045] Among them, K P is the diagonal matrix to be designed; e P (k) is the position tracking error; P d (k+1) is the expected position vector of the small unmanned helicopter at time k+1 in the discrete state;

[0046] Design the position loop tracking control vector as:

[0047]

[0048] Among them, K V is the diagonal matrix to be designed; e V (k) is the speed tracking error; D1(k) = T0D v (k) Estimation; V d (k+1) is the expected center-of-mass velocity vector of the small unmanned helicopter at time k+1 in the discrete state;

[0049] S42. Design a tracking controller for the attitude loop of a small unmanned helicopter;

[0050] According to the nonlinear discrete model of a small unmanned helicopter, the following small unmanned helicopter attitude loop subsystem is considered:

[0051] Λ(k+1)=Λ(k)+T0H(k)Ω(k)

[0052] Ω(k+1)=Ω(k)+T0(-J -1 (Ω(k)×JΩ(k))+A(T m (k))U1(k)+B(T m (k)))+T0D Ω (k)

[0053] Among them, Λ(k) and Λ(k+1) are the attitude angle vectors of the small unmanned helicopter at time k and time k+1 in the body coordinate system respectively; H(k) is the attitude rotation matrix at time k in the discrete state; Ω(k) and Ω(k+1) are the angular velocity vectors of the small unmanned helicopter at time k and time k+1 in the body coordinate system respectively; J is the diagonal inertia matrix of the small unmanned helicopter; D Ω (k) is the bounded unknown time-varying disturbance in the system at time k in the discrete state;

[0054] Design of virtual control law for attitude loop tracking controller of small unmanned helicopter d (k) is:

[0055]

[0056] Among them, KΛ is the diagonal matrix to be designed; e Λ (k) is the attitude angle tracking error; Λ d (k+1) is the expected attitude angle vector of the small unmanned helicopter at time k+1 in the discrete state;

[0057] Design the attitude loop tracking control vector as:

[0058]

[0059] Among them, a d (k) is the expected longitudinal swing angle vector of the small unmanned helicopter at time k in the discrete state; b d (k) is the expected lateral flapping angle vector of the small unmanned helicopter at time k in the discrete state; T t (k) is the tail rotor thrust vector of the small unmanned helicopter at time k in the discrete state; A -1 (T m (k)) is the parameter matrix A(T) of the small unmanned helicopter at time k in the discrete state with respect to the reference input m (k)) inverse matrix; Ω d (k+1) is the expected angular velocity vector of the small unmanned helicopter at time k+1 in the discrete state; K Ω is the control parameter vector to be designed; e Ω (k) is the attitude angular velocity tracking error; D2(k) = T0D Ω (k) estimates;

[0060] S43. Design a tracking controller for the swing motion subsystem of a small unmanned helicopter;

[0061] According to the nonlinear discrete model of a small unmanned helicopter, the following small unmanned helicopter waving motion subsystem is considered:

[0062]

[0063]

[0064] Where a(k) and a(k+1) are the longitudinal swing angles of the small unmanned helicopter at time k and time k+1 in the discrete state respectively; T a (k) and T b (k) are the main rotor longitudinal cyclic pitch input and the main rotor lateral cyclic pitch input at time k and time k+1 in the discrete state respectively; b(k) and b(k+1) are the lateral flapping angles of the small unmanned helicopter at time k and time k+1 in the discrete state respectively; q(k) and p(k) are the pitch angular velocity and roll angle vector of the small unmanned helicopter at time k in the discrete state respectively; τ eA is the time constant of the main rotor of a small unmanned helicopter; lon and B lat are the longitudinal and lateral gains of the main rotor cyclic pitch input respectively;

[0065] The tracking controllers of the swinging motion subsystem are designed as follows:

[0066]

[0067]

[0068] Among them, a d (k+1) is the expected longitudinal swing angle vector of the small unmanned helicopter at time k+1 in the discrete state; b d (k+1) is the expected lateral swing angle vector of the small unmanned helicopter at time k+1 in the discrete state; k a is the longitudinal flapping angle control parameter vector to be designed; k b is the lateral flapping angle control parameter vector to be designed; e a (k) and e b (k) are the longitudinal flapping angle and lateral flapping angle tracking errors respectively.

[0069] Furthermore, in step S5, the Lyapunov function of the entire control system is established based on the Lyapunov function of the tracking controller of the position loop, attitude loop and waving motion subsystem of the small unmanned helicopter, and the expression is as follows:

[0070]

[0071] Among them, V z (k) is the Lyapunov function of the control system; V1(k) is the Lyapunov function of the position loop subsystem of the small unmanned helicopter; V2(k) is the Lyapunov function of the attitude loop subsystem of the small unmanned helicopter; V3(k) is the Lyapunov function of the waving motion subsystem of the small unmanned helicopter; is the Lyapunov function of the position loop disturbance observer; is the Lyapunov function of the attitude loop disturbance observer;

[0072] Combining the Lyapunov function expressions of each control law, the Lyapunov function of the control system is converted into the following form:

[0073]

[0074] Among them, C1, C2, C3, C4, C a , C b and β represent parameter vectors, ρ, ρ2, and N represent positive constants; e Pj (k) and eVj (k) are the position tracking errors e P (k) and velocity tracking error e V (k) the jth variable; e Λj (k) and e Ωj (k) are the attitude angle tracking errors e Λ (k) and attitude angular velocity tracking error e Ω (k) the jth variable; is the position loop error vector estimation error The jth variable of is the attitude loop error vector estimation error The jth variable of a (k) and e b (k) are the longitudinal flapping angle and lateral flapping angle tracking errors respectively; is the upper bound of the position loop error vector D1(k); is the upper bound of the attitude loop error vector D2(k); is the position loop disturbance observer parameter; C D2 are the parameters of the attitude loop disturbance observer;

[0075] The control parameters are selected to make β greater than zero, ensuring that the signal of the control system is uniformly bounded.

[0076] The discrete-time trajectory tracking control system of a small unmanned helicopter based on a disturbance observer of the present invention comprises:

[0077] Model building module, which is used to build a nonlinear continuous model of a small unmanned helicopter with external interference, including waving motion, based on the idea of ​​mechanism modeling;

[0078] A discretization module discretizes the nonlinear continuous model of the small unmanned helicopter by using the Euler approximation method to obtain a nonlinear discrete model of the small unmanned helicopter;

[0079] A discrete-time disturbance observer is used to estimate the effects of external disturbances on the forces and torques of a small unmanned helicopter in its position and attitude loops;

[0080] A tracking controller is used to track and control the position loop, attitude loop and waving motion subsystem of the small unmanned helicopter according to the designed tracking controllers of the position loop, attitude loop and waving motion subsystem of the small unmanned helicopter respectively;

[0081] The effectiveness analysis module selects the Lyapunov function of the control system and chooses the control parameters according to the Lyapunov stability theory to ensure the boundedness of the system error signal.

[0082] Preferably, the discrete-time disturbance observer comprises a position loop disturbance observer and an attitude loop disturbance observer;

[0083] Virtual control law of position loop tracking controller for small unmanned helicopter d (k) is:

[0084]

[0085] Where T0 is the sampling period; P(k) is the position vector of the small unmanned helicopter at time k in the inertial coordinate system; K P is the diagonal matrix to be designed; e P (k) is the position tracking error; P d (k+1) is the expected position vector of the small unmanned helicopter at time k+1 in the discrete state;

[0086] The position loop tracking control vector is:

[0087]

[0088] Among them, m is the mass of the small unmanned helicopter; g is the acceleration due to gravity; is the parameter matrix; is the rotation matrix from the body coordinate system to the inertial coordinate system at time k in the discrete state; F(k) is the resultant external force acting on the small unmanned helicopter at time k in the body coordinate system; V(k) is the velocity vector of the center of mass of the small unmanned helicopter at time k in the inertial coordinate system; K V is the diagonal matrix to be designed; e V (k) is the speed tracking error; D1(k) = T0D v (k) Estimation; V d (k+1) is the expected center-of-mass velocity vector of the small unmanned helicopter at time k+1 in the discrete state;

[0089] Virtual control law of attitude loop tracking controller for small unmanned helicopter d (k) is:

[0090]

[0091] Among them, H -1 (k) is the inverse matrix of the posture rotation matrix H(k) at time k in the discrete state; Λ d (k+1) is the expected attitude angle vector of the small unmanned helicopter at time k+1 in the discrete state; Λ(k) is the attitude angle vector of the small unmanned helicopter at time k in the body coordinate system; K Λ is the diagonal matrix to be designed; e Λ (k) is the attitude angle tracking error;

[0092] The attitude loop tracking control vector is:

[0093]

[0094] Among them, a d (k) is the expected longitudinal swing angle vector of the small unmanned helicopter at time k in the discrete state; b d (k) is the expected lateral flapping angle vector of the small unmanned helicopter at time k in the discrete state; T t (k) is the tail rotor thrust vector of the small unmanned helicopter at time k in the discrete state; A -1 (T m (k)) is the parameter matrix A(T) of the small unmanned helicopter at time k in the discrete state with respect to the reference input m (k)) inverse matrix; Ω d (k+1) is the expected angular velocity vector of the small unmanned helicopter at time k+1 in the discrete state; K Ω is the control parameter vector to be designed; B(T m (k)) is the parameter matrix at time k in the discrete state; Ω(k) is the angular velocity vector of the small unmanned helicopter at time k in the body coordinate system; J is the diagonal inertia matrix of the small unmanned helicopter; e Ω (k) is the attitude angular velocity tracking error; D2(k) = T0D Ω (k) estimate; D Ω (k) is the bounded unknown time-varying disturbance in the system at time k in the discrete state;

[0095] The tracking controller of the swing motion subsystem is:

[0096]

[0097]

[0098] Among them, T a (k) and T b (k) are the main rotor longitudinal cyclic pitch input and the main rotor lateral cyclic pitch input at time k and time k+1 in the discrete state respectively; τ e A is the time constant of the main rotor of a small unmanned helicopter; lon and B lat are the longitudinal and lateral gains of the main rotor cyclic pitch input respectively; a d (k+1) is the expected longitudinal swing angle vector of the small unmanned helicopter at time k+1 in the discrete state; b d(k+1) is the expected lateral flapping angle vector of the small unmanned helicopter at time k+1 in the discrete state; a(k) is the longitudinal flapping angle of the small unmanned helicopter at time k in the discrete state; b(k) is the lateral flapping angle of the small unmanned helicopter at time k in the discrete state; q(k) and p(k) are the pitch angular velocity and roll angular vector of the small unmanned helicopter at time k in the discrete state respectively; k a is the longitudinal flapping angle control parameter vector to be designed; k b is the lateral flapping angle control parameter vector to be designed; e a (k) and e b (k) are the longitudinal flapping angle and lateral flapping angle tracking errors respectively.

[0099] A device of the present invention includes a memory and a processor, wherein:

[0100] A memory for storing computer programs that can be run on the processor;

[0101] The processor is used to execute the steps of the discrete-time trajectory tracking control method of a small unmanned helicopter based on a disturbance observer when running the computer program.

[0102] A storage medium of the present invention stores a computer program, which, when executed by at least one processor, implements the steps of the discrete-time trajectory tracking control method for a small unmanned helicopter based on a disturbance observer.

[0103] Beneficial effects: Compared with the prior art, the present invention has the following significant advantages: the discrete data generated by the nonlinear discrete model of the small unmanned helicopter can be directly used for modern high-performance computer data storage and calculation processing; the discrete-time nonlinear control scheme can be applied to the digital controller required for actual nonlinear control; the position loop interference observer and the attitude loop interference observer can effectively estimate the influence of external unknown interference on the force and torque of the unmanned helicopter in a discrete state, so as to facilitate the use of the estimated value of the interference to compensate and reduce the influence of the interference in the subsequent control process; the tracking controller of the position loop, attitude loop and waving motion subsystem of the small unmanned helicopter can realize the small unmanned helicopter in a discrete state. Effective tracking of the expected position reference signal and the expected yaw angle signal of the small unmanned helicopter under external unknown interference realizes the robust anti-disturbance tracking flight control of the small unmanned helicopter under external interference. BRIEF DESCRIPTION OF THE DRAWINGS

[0104] Figure 1 It is a control flow chart of the present invention. DETAILED DESCRIPTION

[0105] The present invention will be further described below in conjunction with the accompanying drawings.

[0106] like Figure 1 As shown, the discrete-time trajectory tracking flight control method for a small unmanned helicopter based on a disturbance observer of the present invention comprises the following steps:

[0107] (1) Based on the idea of ​​mechanism modeling, a nonlinear continuous model of a small unmanned helicopter with external interference, including waving motion, is established;

[0108] Specifically, the nonlinear continuous model of a small unmanned helicopter is as follows:

[0109]

[0110]

[0111]

[0112]

[0113] Where P = [x, y, z] T is the position vector of the small unmanned helicopter in the inertial coordinate system; is the derivative of the position vector P; V = [u, v, w] T is the velocity vector of the center of mass of the small unmanned helicopter in the inertial coordinate system; is the derivative of the velocity vector V; Λ=[φ,θ,ψ] T is the attitude angle vector of the small unmanned helicopter in the body coordinate system; is the derivative of the attitude angle vector Λ; Ω=[p,q,r] T is the angular velocity vector of the small unmanned helicopter in the body coordinate system; is the derivative of the angular velocity vector Ω; is the rotation matrix from the body coordinate system to the inertial coordinate system, and its specific definition is shown in formula (2); is the parameter matrix; H is the attitude rotation matrix, which is specifically defined as shown in formula (3); J = diag{J xx ,J yy ,J zz} is the diagonal inertia matrix of the small unmanned helicopter; F = [F x ,F y ,F z ] T andΣ=[Σ x ,Σ y ,Σ z ] T are the resultant external force and torque acting on the small unmanned helicopter in the body coordinate system, respectively. Their specific definitions are shown in formula (4), where T mr and Ttr are the main rotor thrust and the tail rotor thrust, is the main rotor anti-torque, C m , C k and D k They represent the main rotor stiffness coefficient and torque coefficient respectively, L z Indicates the relative distance between the main rotor center and the body mass center on the Z axis, H x and H z are the relative distances between the tail rotor center and the body mass center on the X-axis and Z-axis respectively; m is the mass of the small unmanned helicopter; g is the acceleration of gravity; a is the longitudinal swing angle of the small unmanned helicopter; is the derivative of the longitudinal flapping angle a; b is the lateral flapping angle of the small unmanned helicopter; is the derivative of the lateral flapping angle b; τ e A is the time constant of the main rotor of a small unmanned helicopter; lon and B lat are the longitudinal and lateral gains of the main rotor cyclic pitch input respectively; T a and T b are the main rotor longitudinal cyclic pitch input and the main rotor lateral cyclic pitch input respectively; D v =[d u ,d v ,d w ] T and D Ω =[d p ,d q ,d r ] T are the external bounded unknown time-varying disturbances that affect the forces and moments of the unmanned helicopter in the system, and Interference vector D = [D v ,D Ω ] T , is the derivative of the interference vector D, Is a normal number.

[0114] (2) Based on the nonlinear continuous model of the small unmanned helicopter, the nonlinear continuous model of the small unmanned helicopter is discretized by the Euler approximation method to obtain a nonlinear discrete model of the small unmanned helicopter;

[0115] Specifically, the nonlinear discrete model of a small unmanned helicopter is as follows:

[0116]

[0117] Where P(k) = [x(k), y(k), z(k)] Tis the position vector of the small unmanned helicopter at time k in the inertial coordinate system; P(k+1) is the position vector of the small unmanned helicopter at time k+1 in the inertial coordinate system; V(k)=[u(k),v(k),w(k)] T is the velocity vector of the center of mass of the small unmanned helicopter at time k in the inertial coordinate system; V(k+1) is the velocity vector of the center of mass of the small unmanned helicopter at time k+1 in the inertial coordinate system; Λ(k)=[φ(k),θ(k),ψ(k)] T is the attitude angle vector of the small unmanned helicopter at time k in the body coordinate system; Λ(k+1) is the attitude angle vector of the small unmanned helicopter at time k+1 in the body coordinate system; Ω(k)=[p(k),q(k),r(k)] T is the angular velocity vector of the small unmanned helicopter at time k in the body coordinate system; Ω(k+1) is the angular velocity vector of the small unmanned helicopter at time k+1 in the body coordinate system; a(k) and a(k+1) are the longitudinal swing angles of the small unmanned helicopter at time k and time k+1 in the discrete state respectively; T0 is the sampling period;

[0118]

[0119] is the rotation matrix from the body coordinate system to the inertial coordinate system at time k in the discrete state; is the resultant external force acting on the small unmanned helicopter at time k in the body coordinate system; D v (k) = [d u (k),d v (k),d w (k)] T is the bounded unknown time-varying disturbance in the system at time k in the discrete state; is the posture rotation matrix at time k in the discrete state; is the total external torque acting on the small unmanned helicopter at time k in the body coordinate system, D Ω (k) is the bounded unknown time-varying disturbance in the system at time k in the discrete state;

[0120] Assumption (1): Assume that external disturbance D(k) = [D v (k),D Ω (k)] T , and D(k+1)=[D v (k+1),D Ω (k+1)] T It is also bounded.

[0121] Assumption (2): Assume that the expected reference signal P d (k) = [xd (k),y d (k),z d (k)] T and ψ d (k), P d (k+1)=[x d (k+1),y d (k+1),z d (k+1)] T and ψ d (k+1) and signal P d (k+2)=[x d (k+2),y d (k+2),z d (k+2)] T and ψ d (k+2) are all bounded.

[0122] (3) Design a discrete-time disturbance observer to estimate the effects of external disturbances on the forces and torques of a small unmanned helicopter in its position and attitude loops;

[0123] Specifically, the discrete-time disturbance observer is designed according to the following steps:

[0124] (3.1) According to the discrete time model of the position loop with external interference in equation (5):

[0125]

[0126] The disturbance observer is designed as follows:

[0127]

[0128] in, D1(k) = T0D v (k) = [T0d u (k),T0d v (k),T0d w (k)] T ; σ1(k) is an intermediate variable, is an estimate of σ1(k); ρ is a positive constant; V(k)=[u(k),v(k),w(k)] T is the velocity vector of the center of mass of the small unmanned helicopter at time k in the inertial coordinate system.

[0129] According to formula (6) and formula (7), we can get

[0130]

[0131] In order to facilitate the estimation of the intermediate variable σ1(k), the following expression is defined:

[0132]

[0133] According to formula (7), we have

[0134]

[0135] in, represents the estimated error of the position loop error vector D1(k), Represents the estimation error of the intermediate variable of the position loop.

[0136] According to formula (10), we can get:

[0137]

[0138] in, and They represent the position loop error vector estimation error and the position loop intermediate variable estimation error The j-th variable of , where j=1,2,3.

[0139] Combining equation (8), equation (9) and assumption (1), we can get:

[0140]

[0141] in, is the upper bound of the position loop error vector D1(k).

[0142] Select the Lyapunov function as According to equations (7), (8) and (12), we have

[0143]

[0144] Therefore, according to formula (13), the estimation error of the discrete-time disturbance observer can be obtained: is bounded.

[0145] (3.2) According to the discrete time model of the attitude loop with external interference in equation (5):

[0146] Ω(k+1)=Ω(k)+T0(-J -1 (Ω(k)×JΩ(k))+A(T m (k))U1(k)+B(T m (k)))+T0D Ω (k)(14) Design the disturbance observer as follows:

[0147]

[0148] in, D2(k) = T0D Ω (k) = [T0d p (k),T0d q (k),T0d r (k)] T ; σ2(k) is an intermediate variable, is an estimate of σ2(k); ρ2 is a positive constant; Ω(k)=[p(k),q(k),r(k)] T is the angular velocity vector of the small unmanned helicopter at time k in the body coordinate system.

[0149] According to formula (14) and formula (15), we can get

[0150]

[0151] In order to facilitate the estimation of the intermediate variable σ2(k) of the attitude loop, the following expression is defined:

[0152]

[0153] According to formula (15), we have

[0154]

[0155] in, is the estimated error of the attitude loop error vector D2(k), is the estimation error of the intermediate variables of the attitude loop.

[0156] According to formula (18), we can get:

[0157]

[0158] in, and Respectively represent the estimated errors of the attitude loop error vector and the estimated error of the intermediate variables of the attitude loop The j-th variable of , where j=1,2,3.

[0159] Combining equation (16), equation (17) and assumption (1), we can obtain:

[0160]

[0161] in, is the upper bound of the attitude loop error vector D2(k).

[0162] Select the Lyapunov function as According to formula (11) and formula (12), we have

[0163]

[0164] Therefore, according to formula (21), the estimation error of the discrete-time disturbance observer can be obtained: is bounded.

[0165] (4) Based on the nonlinear discrete model of the small unmanned helicopter and the discrete-time disturbance observer, the tracking controllers of the position loop, attitude loop and waving motion subsystem of the small unmanned helicopter are designed respectively to realize the discrete-time trajectory tracking control of the small unmanned helicopter;

[0166] Specifically, the following steps are included:

[0167] (4.1) According to equation (5), consider the following small unmanned helicopter position loop subsystem:

[0168]

[0169] Define the position tracking error e P (k) and velocity tracking error e V (k), respectively expressed as follows:

[0170] e P (k)=P(k)-P d (k) (23)

[0171] e V (k)=V(k)-V d (k) (24)

[0172] Among them, P d =[x d (k),y d (k),z d (k)] T is the desired position reference signal, V d (k)=[u d (k),v d (k),w d (k)] T is the intermediate virtual control law to be designed.

[0173] According to formula (22) and formula (23), we can get

[0174] e P (k+1)=P(k+1)-P d (k+1)=P(k)+T0V d (k)+T0eV (k)-P d (k+1) (25)

[0175] Design virtual control law V d (k) is:

[0176]

[0177] Among them, K P =diag[k p1 ,k p2 ,k p3 ] is a diagonal matrix with design.

[0178] Substituting formula (26) into formula (25), we can obtain:

[0179] e Pj (k+1)=-k Pj e Pj (k)+T0e Vj (k) (27)

[0180] Among them, e Pj (k) and e Vj (k) are respectively e P (k) and e V (k) is the j-th variable.

[0181] According to formula (22) and formula (24), we can get:

[0182]

[0183] Design the position loop to track the control vector (i.e. Figure 1 The position loop control law in ( ) is:

[0184]

[0185] Among them, K V =diag[k V1 ,k V2 ,k V3 ] is a diagonal matrix with design.

[0186] Substituting formula (29) into formula (28), we can obtain:

[0187]

[0188] Among them, e Vj (k) and They are e V (k) and The jth variable of .

[0189] Select the Lyapunov function as According to formula (27) and formula (30), we have

[0190]

[0191] in, represents the parameter vector, represents the parameter vector.

[0192] Pick Define the attitude angle reference vector as Λ d (k)=[φ d (k),θ d (k),ψ d (k)] T , where ψ d (k) is the given yaw angle signal. Solving equation (29), by calculating the attitude angle, we can obtain the main rotor control input T mr (k), expected roll angle signal φ d (k) and the desired pitch angle signal θ d (k), the specific expression is as follows:

[0193]

[0194]

[0195]

[0196] The specific process of attitude angle calculation is as follows:

[0197] Depend on We can get:

[0198]

[0199] Among them, T mr (k) represents the control input of the discrete system at time k; [F1(k), F2(k), F3(k)] represents the control vector of the discrete system at time k; [φ d (k),θ d (k),ψ d (k)] represents the expected attitude angle reference signal of the discrete system at time k.

[0200] According to formula (35), we can get:

[0201]

[0202] Multiply both sides of formula (36) by We can get:

[0203]

[0204] Will Substituting into formula (37), we can get:

[0205]

[0206] According to the first and second lines of formula (38), we can obtain:

[0207]

[0208]

[0209] according to And formula (39) and formula (40), we can get:

[0210]

[0211] (4.2) According to equation (5), consider the following small unmanned helicopter attitude loop subsystem:

[0212]

[0213] Define the attitude angle tracking error e Λ (k) and attitude angular velocity tracking error e Ω (k), respectively expressed as follows:

[0214] e Λ (k)=Λ(k)-Λ d (k) (43)

[0215] e Ω (k)=Ω(k)-Ω d (k) (44)

[0216] Among them, Λ d =[φ d (k),θ d (k),ψ d (k)] T is the desired attitude angle reference signal, φ d (k) and θ d (k) The reference signal is obtained by solving the position controller, ψ d is the given yaw angle reference signal, Ω d (k) = [p d (k),q d (k),r d (k)] T is the virtual control law to be designed.

[0217] According to formula (35) and formula (36), we can get

[0218] e Λ(k+1)=Λ(k+1)-Λ d (k+1)=Λ(k)+T0H(k)Ω d (k)+T0H(k)e Ω (k)-Λ d (k+1) (45)

[0219] Design virtual control law Ω d (k) is:

[0220]

[0221] Among them, K Λ =diag[k Λ1 ,k Λ2 ,k Λ3 ] is the diagonal matrix to be designed.

[0222] Substituting formula (39) into formula (38), we can obtain:

[0223] e Λj (k+1)=-k Λj e Λj (k)+T0H(k)e Ωj (k) (47)

[0224] Among them, e Λj (k) and e Ωj (k) are respectively e Λ (k) and e Ω (k) is the j-th variable.

[0225] According to formula (35) and formula (37), we can get:

[0226]

[0227] Design the attitude loop tracking control vector (i.e. Figure 1 The attitude loop control law shown in ( ) is:

[0228]

[0229] Substituting formula (42) into formula (41), we can obtain:

[0230]

[0231] Among them, e Λj (k) and They are e Λ (k) and The jth variable of .

[0232] Select the Lyapunov function as According to formula (40) and formula (43), we have

[0233]

[0234] in, represents the parameter vector, represents the parameter vector.

[0235] (4.3) According to equation (5), consider the following small unmanned helicopter waving motion subsystem:

[0236]

[0237] The longitudinal flapping angle and lateral flapping angle tracking errors are defined as follows:

[0238]

[0239] Among them, a d (k) and b d (k) are the desired longitudinal flapping angle reference signal and lateral flapping angle reference signal, respectively, which are given by the attitude loop control vector.

[0240] According to formula (45) and formula (46), we can get:

[0241]

[0242] Then the swing motion subsystem tracking controller (i.e. Figure 1 The longitudinal periodic control law and the transverse periodic control law shown in are designed as follows:

[0243]

[0244] Among them, k a and k b are the parameters to be designed.

[0245] Substituting formula (48) into formula (47), we can obtain:

[0246]

[0247] Select the Lyapunov function as According to formula (32) and formula (33), we can get:

[0248]

[0249] in, represents the parameter vector, represents the parameter vector.

[0250] (5) Select the Lyapunov function of the control system and choose the control parameters according to the Lyapunov stability theory to ensure the boundedness of the system error signal;

[0251] Specific:

[0252] The Lyapunov function of the entire closed-loop system is established based on the Lyapunov function of each controller. The expression is as follows:

[0253]

[0254] Combining the Lyapunov function expressions of each control law, the Lyapunov function of the closed-loop system is converted into the following form:

[0255]

[0256] Among them, C1, C2, C3, C4, C a , C b and β denote the parameter vector, ρ, ρ2 and N represent positive constants, is the position loop disturbance observer parameter; C D2 are the parameters of the attitude loop disturbance observer;

[0257] The control parameters are selected to make β greater than zero, ensuring that the error signal of the closed-loop system is uniformly bounded.

[0258] The discrete-time trajectory tracking control system of a small unmanned helicopter based on a disturbance observer of the present invention comprises:

[0259] Model building module, which is used to build a nonlinear continuous model of a small unmanned helicopter with external interference, including waving motion, based on the idea of ​​mechanism modeling;

[0260] A discretization module discretizes the nonlinear continuous model of the small unmanned helicopter by using the Euler approximation method to obtain a nonlinear discrete model of the small unmanned helicopter;

[0261] A discrete-time disturbance observer is used to estimate the effects of external disturbances on the forces and torques of a small unmanned helicopter in its position and attitude loops;

[0262] A tracking controller is used to track and control the position loop, attitude loop and waving motion subsystem of the small unmanned helicopter according to the designed tracking controllers of the position loop, attitude loop and waving motion subsystem of the small unmanned helicopter respectively;

[0263] The effectiveness analysis module selects the Lyapunov function of the control system and chooses the control parameters according to the Lyapunov stability theory to ensure the boundedness of the system error signal.

[0264] A device of the present invention includes a memory and a processor, wherein:

[0265] A memory for storing computer programs that can be run on the processor;

[0266] The processor is used to execute the steps of the discrete-time trajectory tracking control method of a small unmanned helicopter based on a disturbance observer when running the computer program, and achieve the same technical effect as the above method.

[0267] A storage medium of the present invention stores a computer program, which, when executed by at least one processor, implements the steps of the discrete-time trajectory tracking control method for a small unmanned helicopter based on an interference observer and achieves the same technical effect as the above method.

[0268] In summary, the present invention can ensure the safe and stable flight of a small unmanned helicopter in the presence of external interference, and realizes robust anti-interference trajectory tracking flight control of a small unmanned helicopter under external interference.

Claims

1. Discrete-time trajectory tracking control method for a small unmanned helicopter based on disturbance observer, It is characterized in that The following steps are involved: S1. Based on the idea of ​​mechanism modeling, a nonlinear continuous model of a small unmanned helicopter with external interference including waving motion is established. The expression is: Wherein, P is the position vector of the small unmanned helicopter in the inertial coordinate system; is the derivative of the position vector P; V is the velocity vector of the center of mass of the small unmanned helicopter in the inertial coordinate system; is the derivative of the velocity vector V; Λ is the attitude angle vector of the small unmanned helicopter in the body coordinate system; is the derivative of the attitude angle vector Λ; Ω is the angular velocity vector of the small unmanned helicopter in the body coordinate system; is the derivative of the angular velocity vector Ω; is the rotation matrix from the body coordinate system to the inertial coordinate system; θ 3 is the parameter matrix; H is the attitude rotation matrix; J is the diagonal inertia matrix of the small unmanned helicopter; F and Σ are the resultant external force and torque acting on the small unmanned helicopter in the body coordinate system, m is the mass of the small unmanned helicopter; g is the gravitational acceleration; a is the longitudinal swing angle of the small unmanned helicopter; is the derivative of the longitudinal flapping angle a; b is the lateral flapping angle of the small unmanned helicopter; is the derivative of the lateral flapping angle b; τ e A is the time constant of the main rotor of a small unmanned helicopter; lon and B lat are the longitudinal and lateral gains of the main rotor cyclic pitch input respectively; T a and T b are the main rotor longitudinal cyclic pitch input and the main rotor lateral cyclic pitch input respectively; D v and D Ω are the external bounded unknown time-varying disturbances that affect the forces and moments of the unmanned helicopter in the system, and Interference vector D = [D v ,D Ω ] T , is the derivative of the interference vector D, is a positive constant; S2. Based on the nonlinear continuous model of the small unmanned helicopter, the nonlinear continuous model of the small unmanned helicopter is discretized by the Euler approximation method to obtain the nonlinear discrete model of the small unmanned helicopter, which is expressed as: P(k+1)=P(k)+T 0 V(k) Λ(k+1)=Λ(k)+T 0 H(k)Ω(k) Ω(k+1)=Ω(k)+T 0 (-J -1 (Ω(k)×JΩ(k))+J -1 Σ(k))+T 0 D Ω (k) =Ω(k)+T 0 (-J -1 (Ω(k)×JΩ(k))+A(T m (k))U 1 (k)+B(T m (k)))+T 0 D Ω (k) Among them, P(k) and P(k+1) are the position vectors of the small unmanned helicopter at time k and time k+1 in the inertial coordinate system respectively; V(k) and V(k+1) are the velocity vectors of the center of mass of the small unmanned helicopter at time k and time k+1 in the inertial coordinate system respectively; T 0 is the sampling period; is the rotation matrix from the body coordinate system to the inertial coordinate system at time k in the discrete state; F(k) and Σ(k) are the resultant external force and torque acting on the small unmanned helicopter at time k in the body coordinate system, respectively; D v (k) and D Ω (k) are the external bounded unknown time-varying disturbances that affect the forces and torques of the unmanned helicopter in the system at time k in the discrete state; Λ(k) and Λ(k+1) are the attitude angle vectors of the small unmanned helicopter at time k and time k+1 in the body coordinate system respectively; H(k) is the attitude rotation matrix at time k in the discrete state; Ω(k) and Ω(k+1) are the angular velocity vectors of the small unmanned helicopter at time k and time k+1 in the body coordinate system respectively; A(T m (k)) is the parameter matrix of the reference input at time k in the discrete state; U 1 (k) is the state matrix of the longitudinal flapping angle, lateral flapping angle and tail rotor thrust of the unmanned helicopter at time k in the discrete state; B(T m (k)) is the parameter matrix at time k in the discrete state; a(k) and a(k+1) are the longitudinal swing angles of the small unmanned helicopter at time k and time k+1 in the discrete state, respectively; T a (k) and T b (k) are the main rotor longitudinal cyclic pitch input and the main rotor lateral cyclic pitch input at time k and time k+1 in the discrete state respectively; b(k) and b(k+1) are the lateral flapping angles of the small unmanned helicopter at time k and time k+1 in the discrete state respectively; q(k) and p(k) are the pitch angular velocity and roll angle vector of the small unmanned helicopter at time k in the discrete state respectively; S3. Design a discrete-time disturbance observer to estimate the effect of external disturbances on the force and torque of the small unmanned helicopter in the position loop and attitude loop of the small unmanned helicopter. The design method of the discrete-time disturbance observer is as follows: S31. Based on the position loop discrete time model of the external disturbance in the nonlinear discrete model of the small unmanned helicopter, the position loop disturbance observer is designed as follows: s 1 (k)=ρD 1 (k)-ρV(k) in, D 1 (k) = T 0 D v The estimate of (k), σ 1 (k) is the intermediate variable, is 1 (k) is an estimate; ρ is a positive constant; S32. According to the attitude loop discrete time model of the external disturbance existing in the nonlinear discrete model of the small unmanned helicopter, the attitude loop disturbance observer is designed in the following form: s 2 (k)=ρ 2 D 2 (k)-p 2 Ohm(k) in, D 2 (k) = T 0 D Ω (k) estimate; σ 2 (k) is the intermediate variable, is 2 (k) estimate; ρ 2 is a positive constant; S4. Based on the nonlinear discrete model of the small unmanned helicopter and the discrete-time disturbance observer, the tracking controllers of the position loop, attitude loop and waving motion subsystem of the small unmanned helicopter are designed respectively to realize the discrete-time trajectory tracking control of the small unmanned helicopter; the design method of the tracking controllers of the position loop, attitude loop and waving motion subsystem of the small unmanned helicopter is as follows: S41. Design a tracking controller for the position loop of a small unmanned helicopter; According to the nonlinear discrete model of the small unmanned helicopter, the following small unmanned helicopter position loop subsystem is considered: P(k+1)=P(k)+T 0 V(k) Design of virtual control law for position tracking controller of small unmanned helicopter d (k) is: Among them, K P is the diagonal matrix to be designed; e P (k) is the position tracking error; P d (k+1) is the expected position vector of the small unmanned helicopter at time k+1 in the discrete state; Design the position loop tracking control vector as: Among them, K V is the diagonal matrix to be designed; e V (k) is the speed tracking error; V d (k+1) is the expected center-of-mass velocity vector of the small unmanned helicopter at time k+1 in the discrete state; S42. Design a tracking controller for the attitude loop of a small unmanned helicopter; According to the nonlinear discrete model of a small unmanned helicopter, the following small unmanned helicopter attitude loop subsystem is considered: Λ(k+1)=Λ(k)+T 0 H(k)Ω(k) Ω(k+1)=Ω(k)+T 0 (-J -1 (Ω(k)×JΩ(k))+A(T m (k))U 1 (k)+B(T m (k)))+T 0 D Ω (k) Design of virtual control law for attitude loop tracking controller of small unmanned helicopter d (k) is: Among them, K Λ is the diagonal matrix to be designed; e Λ (k) is the attitude angle tracking error; Λ d (k+1) is the expected attitude angle vector of the small unmanned helicopter at time k+1 in the discrete state; Design the attitude loop tracking control vector as: Among them, a d (k) is the expected longitudinal swing angle vector of the small unmanned helicopter at time k in the discrete state; b d (k) is the expected lateral flapping angle vector of the small unmanned helicopter at time k in the discrete state; T t (k) is the tail rotor thrust vector of the small unmanned helicopter at time k in the discrete state; A -1 (T m (k)) is the parameter matrix A(T) of the small unmanned helicopter at time k in the discrete state with respect to the reference input m (k)) inverse matrix; Ω d (k+1) is the expected angular velocity vector of the small unmanned helicopter at time k+1 in the discrete state; K Ω is the control parameter vector to be designed; e Ω (k) is the attitude angular velocity tracking error; S43. Design a tracking controller for the swing motion subsystem of a small unmanned helicopter; According to the nonlinear discrete model of a small unmanned helicopter, the following small unmanned helicopter waving motion subsystem is considered: The tracking controllers of the swinging motion subsystem are designed as follows: Among them, a d (k+1) is the expected longitudinal swing angle vector of the small unmanned helicopter at time k+1 in the discrete state; b d (k+1) is the expected lateral swing angle vector of the small unmanned helicopter at time k+1 in the discrete state; k a is the longitudinal flapping angle control parameter vector to be designed; k b is the lateral flapping angle control parameter vector to be designed; e a (k) and e b (k) are the longitudinal flapping angle and lateral flapping angle tracking errors respectively; S5. Select the Lyapunov function of the control system, select the control parameters according to the Lyapunov stability theory, and ensure the boundedness of the system error signal; establish the Lyapunov function of the entire control system based on the Lyapunov function of the tracking controller of the position loop, attitude loop and waving motion subsystem of the small unmanned helicopter, and the expression is as follows: Among them, V z (k) is the Lyapunov function of the control system; V 1 (k) is the Lyapunov function of the position loop subsystem of the small unmanned helicopter; V 2 (k) is the Lyapunov function of the attitude loop subsystem of the small unmanned helicopter; V 3 (k) is the Lyapunov function of the swing motion subsystem of the small unmanned helicopter; is the Lyapunov function of the position loop disturbance observer; is the Lyapunov function of the attitude loop disturbance observer; Combining the Lyapunov function expressions of each control law, the Lyapunov function of the control system is converted into the following form: Among them, C 1 , C 2 , C 3 , C 4 , C a , C b and β represent parameter vectors, N represents a positive constant; e Pj (k) and e Vj (k) are the position tracking errors e P (k) and velocity tracking error e V (k) the jth variable; e Λj (k) and e Ωj (k) are the attitude angle tracking errors e Λ (k) and attitude angular velocity tracking error e Ω (k) the jth variable; is the position loop error vector estimation error The jth variable of is the attitude loop error vector estimation error The jth variable of is the position loop error vector D 1 (k) an upper bound; is the attitude loop error vector D 2 (k) an upper bound; is the position loop disturbance observer parameter; are the parameters of the attitude loop disturbance observer; The control parameters are selected to make β greater than zero, ensuring that the signal of the control system is uniformly bounded.

2. Discrete-time trajectory tracking control system for a small unmanned helicopter based on disturbance observer, It is characterized in that include: The model building module is used to establish a nonlinear continuous model of a small unmanned helicopter including waving motion with external interference based on the mechanism modeling idea. The expression is: Wherein, P is the position vector of the small unmanned helicopter in the inertial coordinate system; is the derivative of the position vector P; V is the velocity vector of the center of mass of the small unmanned helicopter in the inertial coordinate system; is the derivative of the velocity vector V; Λ is the attitude angle vector of the small unmanned helicopter in the body coordinate system; is the derivative of the attitude angle vector Λ; Ω is the angular velocity vector of the small unmanned helicopter in the body coordinate system; is the derivative of the angular velocity vector Ω; is the rotation matrix from the body coordinate system to the inertial coordinate system; θ 3 is the parameter matrix; H is the attitude rotation matrix; J is the diagonal inertia matrix of the small unmanned helicopter; F and Σ are the resultant external force and torque acting on the small unmanned helicopter in the body coordinate system, m is the mass of the small unmanned helicopter; g is the gravitational acceleration; a is the longitudinal swing angle of the small unmanned helicopter; is the derivative of the longitudinal flapping angle a; b is the lateral flapping angle of the small unmanned helicopter; is the derivative of the lateral flapping angle b; τ e A is the time constant of the main rotor of a small unmanned helicopter; lon and B lat are the longitudinal and lateral gains of the main rotor cyclic pitch input respectively; T a and T b are the main rotor longitudinal cyclic pitch input and the main rotor lateral cyclic pitch input respectively; D v and D Ω are the external bounded unknown time-varying disturbances that affect the forces and moments of the unmanned helicopter in the system, and Interference vector D = [D v ,D Ω ] T , is the derivative of the interference vector D, is a positive constant; The discretization module discretizes the nonlinear continuous model of the small unmanned helicopter through the Euler approximation method to obtain the nonlinear discrete model of the small unmanned helicopter, which is expressed as: P(k+1)=P(k)+T 0 V(k) Λ(k+1)=Λ(k)+T 0 H(k)Ω(k) Ω(k+1)=Ω(k)+T 0 (-J -1 (Ω(k)×JΩ(k))+J -1 Σ(k))+T 0 D Ω (k) =Ω(k)+T 0 (-J -1 (Ω(k)×JΩ(k))+A(T m (k))U 1 (k)+B(T m (k)))+T 0 D Ω (k) Among them, P(k) and P(k+1) are the position vectors of the small unmanned helicopter at time k and time k+1 in the inertial coordinate system respectively; V(k) and V(k+1) are the velocity vectors of the center of mass of the small unmanned helicopter at time k and time k+1 in the inertial coordinate system respectively; T 0 is the sampling period; is the rotation matrix from the body coordinate system to the inertial coordinate system at time k in the discrete state; F(k) and Σ(k) are the resultant external force and torque acting on the small unmanned helicopter at time k in the body coordinate system, respectively; D v (k) and D Ω (k) are the external bounded unknown time-varying disturbances that affect the forces and torques of the unmanned helicopter in the system at time k in the discrete state; Λ(k) and Λ(k+1) are the attitude angle vectors of the small unmanned helicopter at time k and time k+1 in the body coordinate system respectively; H(k) is the attitude rotation matrix at time k in the discrete state; Ω(k) and Ω(k+1) are the angular velocity vectors of the small unmanned helicopter at time k and time k+1 in the body coordinate system respectively; A(T m (k)) is the parameter matrix of the reference input at time k in the discrete state; U 1 (k) is the state matrix of the longitudinal flapping angle, lateral flapping angle and tail rotor thrust of the unmanned helicopter at time k in the discrete state; B(T m (k)) is the parameter matrix at time k in the discrete state; a(k) and a(k+1) are the longitudinal swing angles of the small unmanned helicopter at time k and time k+1 in the discrete state, respectively; T a (k) and T b (k) are the main rotor longitudinal cyclic pitch input and the main rotor lateral cyclic pitch input at time k and time k+1 in the discrete state respectively; b(k) and b(k+1) are the lateral flapping angles of the small unmanned helicopter at time k and time k+1 in the discrete state respectively; q(k) and p(k) are the pitch angular velocity and roll angle vector of the small unmanned helicopter at time k in the discrete state respectively; A discrete-time disturbance observer is used to estimate the effect of external disturbances on the force and torque of a small unmanned helicopter in the position loop and attitude loop of the small unmanned helicopter. The design method of the discrete-time disturbance observer is as follows: S31. Based on the position loop discrete time model of the external disturbance in the nonlinear discrete model of the small unmanned helicopter, the position loop disturbance observer is designed as follows: s 1 (k)=ρD 1 (k)-ρV(k) in, D 1 (k) = T 0 D v The estimate of (k), σ 1 (k) is the intermediate variable, is 1 (k) is an estimate; ρ is a positive constant; S32. According to the attitude loop discrete time model of the external disturbance existing in the nonlinear discrete model of the small unmanned helicopter, the attitude loop disturbance observer is designed in the following form: s 2 (k)=ρ 2 D 2 (k)-p 2 Ohm(k) in, D 2 (k) = T 0 D Ω (k) estimate; σ 2 (k) is the intermediate variable, is 2 (k) estimate; ρ 2 is a normal number; the tracking controller, according to the designed tracking controller of the position loop, attitude loop and waving motion subsystem of the small unmanned helicopter, respectively tracks and controls the position loop, attitude loop and waving motion subsystem of the small unmanned helicopter; the design method of the tracking controller of the position loop, attitude loop and waving motion subsystem of the small unmanned helicopter is: S41. Design a tracking controller for the position loop of a small unmanned helicopter; According to the nonlinear discrete model of the small unmanned helicopter, the following small unmanned helicopter position loop subsystem is considered: P(k+1)=P(k)+T 0 V(k) Design of virtual control law for position tracking controller of small unmanned helicopter d (k) is: Among them, K P is the diagonal matrix to be designed; e P (k) is the position tracking error; P d (k+1) is the expected position vector of the small unmanned helicopter at time k+1 in the discrete state; Design the position loop tracking control vector as: Among them, K V is the diagonal matrix to be designed; e V (k) is the speed tracking error; V d (k+1) is the expected center-of-mass velocity vector of the small unmanned helicopter at time k+1 in the discrete state; S42. Design a tracking controller for the attitude loop of a small unmanned helicopter; According to the nonlinear discrete model of a small unmanned helicopter, the following small unmanned helicopter attitude loop subsystem is considered: Λ(k+1)=Λ(k)+T 0 H(k)Ω(k) Ω(k+1)=Ω(k)+T 0 (-J -1 (Ω(k)×JΩ(k))+A(T m (k))U 1 (k)+B(T m (k)))+T 0 D Ω (k) Design of virtual control law for attitude loop tracking controller of small unmanned helicopter d (k) is: Among them, K Λ is the diagonal matrix to be designed; e Λ (k) is the attitude angle tracking error; Λ d (k+1) is the expected attitude angle vector of the small unmanned helicopter at time k+1 in the discrete state; Design the attitude loop tracking control vector as: Among them, a d (k) is the expected longitudinal swing angle vector of the small unmanned helicopter at time k in the discrete state; b d (k) is the expected lateral flapping angle vector of the small unmanned helicopter at time k in the discrete state; T t (k) is the tail rotor thrust vector of the small unmanned helicopter at time k in the discrete state; A -1 (T m (k)) is the parameter matrix A(T) of the small unmanned helicopter at time k in the discrete state with respect to the reference input m (k)) inverse matrix; Ω d (k+1) is the expected angular velocity vector of the small unmanned helicopter at time k+1 in the discrete state; K Ω is the control parameter vector to be designed; e Ω (k) is the attitude angular velocity tracking error; S43. Design a tracking controller for the swing motion subsystem of a small unmanned helicopter; According to the nonlinear discrete model of a small unmanned helicopter, the following small unmanned helicopter waving motion subsystem is considered: The tracking controllers of the swinging motion subsystem are designed as follows: Among them, a d (k+1) is the expected longitudinal swing angle vector of the small unmanned helicopter at time k+1 in the discrete state; b d (k+1) is the expected lateral swing angle vector of the small unmanned helicopter at time k+1 in the discrete state; k a is the longitudinal flapping angle control parameter vector to be designed; k b is the lateral flapping angle control parameter vector to be designed; e a (k) and e b (k) are the longitudinal flapping angle and lateral flapping angle tracking errors respectively; In the effectiveness analysis module, the Lyapunov function of the control system is selected, and the control parameters are selected according to the Lyapunov stability theory to ensure the boundedness of the system error signal; the Lyapunov function of the entire control system is established based on the Lyapunov function of the tracking controller of the position loop, attitude loop and waving motion subsystem of the small unmanned helicopter. The expression is as follows: Among them, V z (k) is the Lyapunov function of the control system; V 1 (k) is the Lyapunov function of the position loop subsystem of the small unmanned helicopter; V 2 (k) is the Lyapunov function of the attitude loop subsystem of the small unmanned helicopter; V 3 (k) is the Lyapunov function of the swing motion subsystem of the small unmanned helicopter; is the Lyapunov function of the position loop disturbance observer; is the Lyapunov function of the attitude loop disturbance observer; Combining the Lyapunov function expressions of each control law, the Lyapunov function of the control system is converted into the following form: Among them, C 1 , C 2 , C 3 , C 4 , C a , C b and β represent parameter vectors, N represents a positive constant; e Pj (k) and e Vj (k) are the position tracking errors e P (k) and velocity tracking error e V (k) the jth variable; e Λj (k) and e Ωj (k) are the attitude angle tracking errors e Λ (k) and attitude angular velocity tracking error e Ω (k) the jth variable; is the position loop error vector estimation error The jth variable of is the attitude loop error vector estimation error The jth variable of is the position loop error vector D 1 (k) an upper bound; is the attitude loop error vector D 2 (k) an upper bound; is the position loop disturbance observer parameter; are the parameters of the attitude loop disturbance observer; The control parameters are selected to make β greater than zero, ensuring that the signal of the control system is uniformly bounded.

3. A device, It is characterized in that comprising a memory and a processor, wherein: A memory for storing computer programs that can be executed on the processor; A processor is used to execute the steps of the discrete-time trajectory tracking control method for a small unmanned helicopter based on a disturbance observer as claimed in claim 1 when running the computer program.

4. A storage medium, It is characterized in that The storage medium stores a computer program, and when the computer program is executed by at least one processor, the steps of the discrete-time trajectory tracking control method for a small unmanned helicopter based on a disturbance observer as claimed in claim 1 are implemented.

Images