UAV trajectory tracking control method based on function differentiation and adaptive variable gain

Abstract

A UAV trajectory tracking control method based on function differentiation and adaptive variable gain includes the following steps: 1. Establishing a quadrotor UAV tracking control method model; 2. Designing a differential tracker that combines an improved sigmoid function with a sliding mode terminal attractor; 3. Designing a novel third-order adaptive variable gain finite-time dilation observer based on the control model; 4. Designing an active disturbance rejection tracking controller, one part of which is composed of an observer to compensate for the total system disturbance, and the other part of which is a fast sliding mode controller to track the position and attitude of the quadrotor; 5. Proving the closed-loop stability of the entire trajectory tracking control method based on the Lyapunov function. This method can compensate for unknown disturbances and model uncertainties in the quadrotor UAV trajectory tracking control method, accelerate system convergence, and improve the performance of the tracking control method.

Description

Technical Field

[0001] The present invention belongs to the field of quadrotor unmanned aerial vehicle (UAV) trajectory tracking control, and in particular relates to a UAV trajectory tracking control method based on function differentiation and adaptive variable gain. Background Art

[0002] Quadrotor drones, due to their lightweight and flexible design, are widely used in military reconnaissance, disaster rescue, mail delivery, terrain surveying, pesticide spraying, and other fields. Quadrotor dynamics systems exhibit characteristics such as nonlinearity, strong coupling, modeling uncertainty, and underactuation. During actual flight, uncertain external interference, such as turbulent wind fields, positioning drift, variable loads, and motor failures, exists. These inherent characteristics and external interference pose significant challenges to quadrotor trajectory tracking control.

[0003] To achieve fast and accurate trajectory tracking control for quadrotor drones under external disturbances, researchers have proposed algorithms such as PID control, backstepping control, and sliding-mode variable structure control. PID control is suitable for linear, known-model conditions, requiring parameter retuning when model parameters change. Backstepping control uses multiple intermediate variables and derivatives of the system state to achieve desired values to ensure overall system performance, but it suffers from a "dimensionality explosion" problem as system complexity increases. Sliding-mode variable structure control allows the system state to move purposefully along a predetermined "sliding mode" trajectory, offering strong robustness to disturbances and unmodeled dynamics, but it suffers from chattering. Active disturbance rejection control, through the design of a tracking differentiator, an expanded observer, and a feedback controller, effectively suppresses model unknowns and external disturbances. It has been widely used in robotic control, permanent magnet motor control, and drone control. The classic second-order, fastest-switching tracking differentiator exhibits significant chattering during signal tracking and slow convergence far from equilibrium, making it difficult to accurately obtain differential information from tracking signals containing interference noise. However, linear extended observers are prone to spike effects, leading to large initial controller values. Furthermore, the stability of active disturbance rejection control (ADRC) is crucial for both verification and parameter tuning in practical applications. Research on ADRC has become a hot topic in the field of tracking control.

[0004] The existing technologies are as follows:

[0005] Patent Name: Adaptive Trajectory Tracking Controller for Quadcopter UAV Based on Sliding Mode Control and Perturbation and Design Method thereof, Patent Number: CN202110841286.6. This invention proposes an adaptive trajectory tracking controller for quadcopter UAV based on sliding mode control and perturbation and its design method. Based on the nonlinear mechanical model of the quadcopter UAV and the attitude angle target and flight position target of the quadcopter trajectory tracking, the controller uses a sliding mode variable structure control method to obtain the system's attitude control input function. At the same time, the system is predicted and the predicted value is used instead of the actual value to provide adaptive control compensation in advance. The controller also uses a sliding mode variable structure control method to obtain the system's position control input function. At the same time, the system is predicted and the predicted value is used instead of the actual value to provide adaptive control compensation in advance. Based on the expected yaw angle and virtual control input, the expected values of the quadcopter's roll angle and pitch angle are inversely solved as the reference input of the inner loop. This method effectively improves the trajectory tracking efficiency and tracking accuracy of the drone and ensures the stability of the adaptive trajectory tracking controller.

[0006] It uses sliding mode variable structure control to calculate the input functions of the position and attitude subsystems respectively, and designs a prediction mechanism, using the predicted values to adaptively compensate for the unknown UAV parameters to ensure the UAV's high-precision trajectory tracking capability in a disturbed environment. The method of this patent first addresses the problem of second-order derivatives requiring the desired trajectory in a fast non-singular controller, and designs a differential tracker that combines a sigmoid function and a sliding mode terminal operator to obtain smooth trajectory information. At the same time, for external interference, this patent designs a third-order adaptive variable gain expansion observer, which effectively estimates the interference value and then performs feedback compensation to achieve high-precision trajectory tracking performance. Therefore, the two patents are completely different in their methods of dealing with external disturbances.

[0007] Patent Name: Quadrotor UAV Control Method Based on Fuzzy Extended State Observer and Adaptive Sliding Mode, Patent Number: CN201610565104.6. The present invention establishes a quadrotor UAV system model, initializes the system state and controller parameters; designs a tracking differentiator; designs a nonlinear extended state observer; establishes fuzzy rules; designs a parameter adaptive law; and designs an adaptive sliding mode controller. The extended state observer is designed to estimate system model uncertainty and external disturbances. The pole placement method is used to determine the initial values of the extended state observer parameters. Fuzzy rules are introduced to perform online tuning of the extended state observer parameters. The parameter adaptive law is designed to obtain the ideal controller gain. An adaptive sliding mode controller is designed to ensure that the system tracking error quickly stabilizes and converges to zero, thereby achieving rapid and stable position tracking and attitude adjustment for the quadrotor UAV. The present invention improves system performance and achieves rapid and stable position tracking and attitude adjustment for the system.

[0008] It designs an extended state observer, estimates the uncertainty of the system model and external disturbances, determines the initial values of the extended state observer parameters through the pole configuration method, introduces fuzzy rules, and performs online tuning of the extended state observer parameters; designs parameter adaptive laws to obtain ideal controller gains; and designs an adaptive sliding mode controller to ensure that the system tracking error is quickly stabilized and converges to zero.

[0009] This patent designs a third-order adaptive variable gain expanded observer to estimate external disturbances. By performing a stability analysis on the designed expanded observer, the necessary conditions for stability are obtained, and then an adaptive variable gain function that meets the conditions is designed. There is no need to introduce fuzzy rules to adjust the gain of the expanded observer online. Secondly, the two patents also differ in the design of the sliding surface. The "quadcopter control method based on fuzzy expanded state observer and adaptive sliding mode" uses the simplest linear sliding surface, which can only ensure that the error converges to zero, but the convergence time cannot be guaranteed. The fast non-singular sliding surface used in this patent can ensure rapid convergence within a limited time, and has better performance for fast and high-precision trajectory tracking. Therefore, the two patents differ in the design of the expanded observer and the sliding surface.

[0010] Research on the trajectory tracking control of quadrotor UAVs with a new type of active disturbance rejection control can improve the estimation accuracy of external disturbances, the convergence speed of tracking control, and the control accuracy, and provide a high-performance control algorithm for the widespread application of quadrotor UAVs in complex environments such as military reconnaissance, disaster search and rescue, and terrain surveys. Summary of the Invention

[0011] In order to overcome the shortcomings of the existing technology, the present invention provides a UAV trajectory tracking control method based on function differentiation and adaptive variable gain. The method can compensate for the unknown interference and model uncertainty of the quadrotor UAV trajectory tracking control method, accelerate the system convergence speed, and make the control error converge to zero within a limited time, thereby improving the performance of the tracking control method.

[0012] To achieve the above object, the technical solution adopted by the present invention is:

[0013] The UAV trajectory tracking control method based on function differentiation and adaptive variable gain has the following specific steps:

[0014] (1) Establish a quadrotor UAV tracking control system model;

[0015] (2) Design and improve the differential tracker that combines the Sigmoid function and the sliding mode terminal attractor;

[0016] In step (2), designing a differential tracker combining an improved Sigmoid function and a sliding mode terminal attractor comprises the following steps:

[0017] (2.1) Design and improve the differential tracker that combines the Sigmoid function and the sliding mode terminal attractor. Its specific form is:

[0018]

[0019] Where, σ=p / q, p<q, and p, q are odd numbers, α>0, γ>0, R>0. v(t) is the input signal, and is the original signal and differential signal of the tracking signal v(t);

[0020] (3) Based on the control model, a new third-order adaptive variable-gain finite-time expansion observer is designed;

[0021] In step (3), designing a novel third-order adaptive finite-time dilated observer includes the following steps:

[0022] (3.1) Design a new third-order adaptive finite-time dilation observer, whose specific form is:

[0023]

[0024] The variables defining the extended observer are: control signal Z1, first-order derivative of the control signal Z2, and total disturbance observation Z3. The error variables of the observer are e1=x1-Z1, e2=x2-Z2, e3=D-Z3, and the parameters satisfy 0.5<λ1<1, λ2=2λ1-1, l1(t), l2(t), l3(t) are the designed time-varying gains, and the adaptive law Update, under the premise of meeting stability, L(t) is designed as

[0025] (4) Design an active disturbance rejection tracking controller, one part of which is composed of an observer to compensate for the total disturbance of the system, and the other part is designed as a fast sliding mode controller to track the position and attitude of the quadrotor;

[0026] (5) Based on the Lyapunov function, the closed-loop stability of the entire trajectory tracking control system is proved.

[0027] As a further improvement of the present invention, in step (1), establishing a quadrotor drone tracking control system model includes the following steps:

[0028] (1.1) Establishing the kinematic model of the quadrotor drone:

[0029] The rotation vector of the quadrotor in the body coordinate system They are roll angle, pitch angle and yaw angle respectively; is the angular velocity in the body coordinate system; the position vector of the quadrotor in the body coordinate system The linear speeds in the three directions are The kinematic model of the quadrotor drone is expressed as:

[0030]

[0031] Among them, R ω is the transformation matrix of attitude change rate and body rotation angular velocity, expressed as:

[0032]

[0033] Rotation matrix from the body coordinate system to the ground-fixed coordinate system Expressed as:

[0034]

[0035] (1.2) Establish the dynamic model of the quadrotor drone:

[0036] The posture dynamics model is established by the Euler equation as follows:

[0037]

[0038] in, is the moment of inertia matrix of the quadrotor; represents the gyroscopic torque; Represents the torque generated by the propeller on the body axis; × represents the vector cross product operation; is the interference torque;

[0039] Force analysis of the quadrotor is carried out according to Newton's second law:

[0040]

[0041] Among them, K f =diag(k x , k y , k z ) is the air resistance coefficient matrix; G = [0, 0, g] T is gravity; UT is the total pulling force acting on the four motors; D p (t)=[d x , d y , d z ] T For other interference resistance, is the pulling force of the i-th motor, U r is the pulling force of the four motors in the x, y, and z axes;

[0042] (1.3) Analysis of control system modeling uncertainty and external disturbances:

[0043] Define the system state variable as X 1 =[x, y, z] T , X3=[φ,θ,ψ] T , Due to external disturbances and model parameter identification errors, the dynamic model is transformed into a state space form with lumped disturbances;

[0044]

[0045] in is the measured value of the system model parameter, Δ(·) is the uncertainty value of the model parameter, B1=[cosψsinθcosφ+sinψsinφ,sinψsinθsinφ−cosψsinφ,cosθcosφ] T , Π1=diag(J y -J z , J z -J x , J x -J y ), ∏2=diag(-J r Ω r , -J r Ω r ,0), is a continuous smooth function, and the lumped disturbance is defined as

[0046]

[0047] (1.4) Nonlinear decoupling:

[0048] To solve the coupling problem, the position dummy variable V=[V x , V y , V z ] T as follows

[0049]

[0050] The expected Euler angle (φ d ,θ d ) and total thrust u T as follows

[0051]

[0052] As a further improvement of the present invention, in step (4), designing a non-singular fast sliding mode controller includes the following steps:

[0053] (4.1) Define the spatial state equation of the quadrotor tracking control system:

[0054] The system spatial state equation is obtained from the two formulas in step (1.3):

[0055]

[0056] For the position control subsystem, a sliding mode controller is designed. The position error and velocity error are defined as follows:

[0057]

[0058] For the attitude control subsystem, a sliding mode controller is designed. The attitude error and angular velocity error are defined as follows:

[0059]

[0060] (4.2) Design a fast non-singular terminal sliding mode controller:

[0061] The fast non-singular terminal sliding surface of the attitude subsystem is designed as

[0062]

[0063] The fast non-singular terminal sliding surface of the position subsystem is designed as

[0064]

[0065] Here b n is a positive constant, 1<β n <2, β n+1 <α n

[0066] Derivative of the sliding mode function yields

[0067]

[0068]

[0069] make Get the control law

[0070]

[0071]

[0072] As a further improvement of the present invention, in step (5), the closed-loop stability proof of the trajectory tracking control system includes the following steps:

[0073] (5.1) Taking the decoupled position subsystem X channel as an example, the Lyapunov function is defined as:

[0074]

[0075] Bring in UX , the time derivative of formula (21) is written as

[0076]

[0077] The Lyapunov function of the position subsystem is chosen as

[0078]

[0079] Therefore, the stability of the position system is guaranteed, and the trajectory tracking capability is ensured. The stability proof of the attitude subsystem is similar and will not be repeated here.

[0080] Compared with the prior art, the present invention has the following beneficial effects:

[0081] 1. The control quantity of this method adopts active disturbance rejection control design. Compared with the traditional method, the proposed method can achieve high-precision tracking performance of the quadrotor with known or unknown disturbances and fast convergence.

[0082] 2. This method designs a tracking differentiator based on an improved sigmoid function. Within the tracking differentiator structure, a novel second-order differential tracker combining a sigmoid function with a sliding-mode terminal attractor is designed to accelerate its global convergence rate and effectively reduce chattering caused by high-frequency signals.

[0083] 3. This method designs a finite-time dilation observer with time-varying gain, which can effectively solve the peaking effect caused by constant gain and converge the observation error to zero within the effective time. BRIEF DESCRIPTION OF THE DRAWINGS

[0084] Figure 1 This is a flow chart of the trajectory tracking control method of the quadrotor drone disclosed in the present invention;

[0085] Figure 2 This is a comparison chart of the effects of the method disclosed in the embodiment and other traditional methods. DETAILED DESCRIPTION

[0086] The present invention will be further described below with reference to the accompanying drawings and specific embodiments. It should be understood that the following specific embodiments are only used to illustrate the present invention and are not used to limit the scope of the present invention.

[0087] like Figure 1 As shown, the present invention discloses a UAV trajectory tracking control method based on function differentiation and adaptive variable gain, comprising the following steps:

[0088] Step 1: Establish a quadrotor UAV tracking control method model;

[0089] (1.1) Establishing the kinematic model of the quadrotor drone:

[0090] The rotation vector of the quadrotor in the body coordinate system They are roll angle, pitch angle and yaw angle respectively; is the angular velocity in the body coordinate system; the position vector of the quadrotor in the body coordinate system The linear speeds in the three directions are The kinematic model of the quadrotor drone is expressed as:

[0091]

[0092] Among them, R ω is the transformation matrix of attitude change rate and body rotation angular velocity, which can be expressed as:

[0093]

[0094] Rotation matrix from the body coordinate system to the ground-fixed coordinate system It can be expressed as:

[0095]

[0096] (1.2) Establish the dynamic model of the quadrotor drone:

[0097] The posture dynamics model is established by the Euler equation as follows:

[0098]

[0099] in, is the moment of inertia matrix of the quadrotor; represents the gyroscopic torque; Represents the torque generated by the propeller on the body axis; × represents the vector cross product operation; is the interference torque.

[0100] Force analysis of the quadrotor is carried out according to Newton's second law:

[0101]

[0102] Among them, K f =diag(k x , k y , k z ) is the air resistance coefficient matrix; G = [0, 0, g] T is gravity; U T is the total pulling force acting on the four motors; D p (t)=[d x , d y , d z ] T For other interference resistance. is the pulling force of the i-th motor, UT is the pulling force of the four motors in the x, y, and z axes.

[0103] (1.3) Analytical control methods modeling uncertainty and external disturbances:

[0104] Define the system state variable as X1 = [x, y, z] T , X3=[φ,θ,ψ] T , Due to external disturbances and model parameter identification errors, the dynamic model is transformed into a state-space form with lumped disturbances.

[0105]

[0106] in is the measured value of the system model parameter, Δ(·) is the uncertainty value of the model parameter, B1=[cosψsinθcosφ+sinψsinφ,sinψsinθsinφ−cosψsinφ,cosθcosφ] T , ∏1=diag(J y -J z , J z -J x , J x -J y ), ∏2=diag(-J r Ω r , -J r Ω r ,0), is a continuous smooth function. The lumped disturbance is defined as

[0107]

[0108] (1.4) Nonlinear decoupling:

[0109] To solve the coupling problem, the position dummy variable V=[V x , V y , V z ] T as follows

[0110]

[0111] The expected Euler angle (φ d ,θ d ) and total thrust u T as follows

[0112]

[0113] Step 2: Design a differential tracker that combines an improved Sigmoid function with a sliding mode terminal attractor;

[0114] (2.1) Design and improve the differential tracker that combines the Sigmoid function and the sliding mode terminal attractor. Its specific form is:

[0115]

[0116] Where, σ=p / q, p<q, and p, q are odd numbers, α>0, γ>0, R>0. v(t) is the input signal, and are the original signal and differential signal of the tracking signal v(t).

[0117] (3.1) Design a new third-order adaptive finite-time dilation observer, whose specific form is:

[0118]

[0119] The variables defining the extended observer are: control signal Z1, first-order derivative of the control signal Z2, and total disturbance observation Z3. The error variables of the observer are e1=x1-Z1, e2=x2-Z2, e3=D-Z3, and the parameters satisfy 0.5<λ1<1, λ2=2λ1-1, l1(t), l2(t), l3(t) are the designed time-varying gains, and the adaptive law Update, under the premise of meeting stability, L(t) is designed as

[0120]

[0121] Step 4: Design a non-singular fast sliding mode controller

[0122] (4.1) Define the spatial state equation of the quadrotor tracking control method:

[0123] From Equation (6) and Equation 7, the system spatial state equation can be obtained as follows:

[0124]

[0125] For the position control subsystem, a sliding mode controller is designed. The position error and velocity error are defined as follows:

[0126]

[0127] For the attitude control subsystem, a sliding mode controller is designed. The attitude error and angular velocity error are defined as follows:

[0128]

[0129] (4.2) Design a fast non-singular terminal sliding mode controller:

[0130] The fast non-singular terminal sliding surface of the attitude subsystem is designed as

[0131]

[0132] The fast non-singular terminal sliding surface of the position subsystem is designed as

[0133]

[0134] Here b n is a positive constant, 1<β n <2, β n+1 <α n

[0135] Derivative of the sliding mode function yields

[0136]

[0137]

[0138] make Get the control law

[0139]

[0140]

[0141] Step 5: Proof of closed-loop stability of trajectory tracking control method Taking the decoupled position subsystem X channel as an example, the Lyapunov function is defined as:

[0142]

[0143] Bring in U X , the time derivative of formula (21) can be written as

[0144]

[0145] The Lyapunov function of the position subsystem can be chosen to be

[0146]

[0147] Therefore, the stability of the position system is guaranteed, and the trajectory tracking capability is ensured. The stability proof of the attitude subsystem is similar and will not be repeated here.

[0148] To verify the trajectory tracking control performance of the quadrotor UAV disclosed in the present invention, the mass of the UAV is m = 1.65 kg, the distance from the rotor to the center of the UAV is l = 0.225 m, and the moment of inertia is [J x J y J z]=[0.01782 0.017820.03191]kg m 2 , the motor's moment of inertia is J r =0.00099 kg m 2 , the air damping coefficient matrix is [k x k y k z ]=[0.005567 0.0005567 0.005567]N(m / s) 2 , the moment damping coefficient matrix is [k φ k θ k ψ ]=[0.065790.06579 0.06579]Nm(rad / s)2, when the external interference is d x =0.2sin(0.2t)+0.1cos(0.3t), d y =0.4cos(0.5t)+0.2,d z =0.2cos(0.1t+π / 4)+0.1, compared with the traditional sliding mode steering control assisted driving system, the error is as follows Figure 2 As shown in the figure, "-▲" is the tracking curve of the traditional active disturbance rejection control, and "-*" is the tracking curve of the quadrotor based on the sigmoid differential tracker and variable gain observer disclosed in the present invention. The specific comparison effect is as follows:

[0149] The root mean square errors of the tracking control in the three directions of x, y, and z of the traditional method are 0.5441m, 0.3506m, and 0.9744m respectively; the root mean square errors of the tracking control in the three directions of x, y, and z of the method proposed in the present invention are 0.1002m, 0.3909m, and 0.3434m respectively.

[0150] The above description is merely a preferred embodiment of the present invention and does not constitute any other form of limitation to the present invention. Any modification or equivalent variation based on the technical essence of the present invention shall still fall within the scope of protection claimed by the present invention.

Claims

1. A UAV trajectory tracking control method based on function differentiation and adaptive variable gain has the following specific steps, which are characterized by: The steps include: (1) Establish a quadrotor UAV tracking control system model; (2) Design and improve the differential tracker that combines the Sigmoid function and the sliding mode terminal attractor; In step (2), the design of a differential tracker combining an improved Sigmoid function and a sliding mode terminal attractor includes the following steps: (2.1) Design and improve the differential tracker that combines the Sigmoid function and the sliding mode terminal attractor. Its specific form is: ； in, , and is an odd number, is the input signal, and To track the signal The original signal and the differentiated signal; (3) Based on the control model, a new third-order adaptive variable-gain finite-time expansion observer is designed; In step (3), designing a novel third-order adaptive finite-time dilated observer includes the following steps: (3.1) Design a new third-order adaptive finite-time dilation observer, whose specific form is: ； Among them, the variables defining the extended observer are: control signal , the first-order derivative of the control signal and total interference observations , the error variable of the observer is , the parameters satisfy , is the designed time-varying gain, according to the adaptive law Update, under the premise of meeting stability, Designed for ， ； (4) Design an active disturbance rejection tracking controller, one part of which is to compensate for the total disturbance of the system by the observer, and the other part is to design a fast sliding mode controller to track the position and attitude of the quadrotor; (5) Based on the Lyapunov function, the closed-loop stability of the entire trajectory tracking control system is proved.

2. The UAV trajectory tracking control method based on function differentiation and adaptive variable gain according to claim 1 is characterized in that: In step (1), establishing a quadrotor drone tracking control system model includes the following steps: (1.1) Establishing the kinematic model of the quadrotor drone: The rotation vector of the quadrotor in the body coordinate system They are roll angle, pitch angle and yaw angle respectively; is the angular velocity in the body coordinate system; the position vector of the quadrotor in the body coordinate system , the linear velocities in the three directions are , the kinematic model of the quadrotor drone is expressed as: ； in, is the transformation matrix of attitude change rate and body rotation angular velocity, expressed as: ； Rotation matrix from the body coordinate system to the ground-fixed coordinate system Expressed as: ； (1.2) Establish a quadrotor UAV dynamics model: The posture dynamics model is established by the Euler equation as follows: ； in, is the moment of inertia matrix of the quadrotor; represents the gyroscopic torque; It represents the torque generated by the propeller on the fuselage shaft; Represents vector cross product operation; is the interference torque; Force analysis of the quadrotor is carried out according to Newton's second law: ； in, is the air resistance coefficient matrix; is gravity; is the total pulling force acting on the four motors; For other interference resistance, For the The pulling force of the motor, For four motors Three-axis tension; (1.3) Analysis of control system modeling uncertainty and external disturbances: Define the system state variables as , due to external disturbances and model parameter identification errors, the dynamic model is transformed into a state space form with lumped disturbances; ； in are the measured values of the system model parameters, are the uncertain values of the model parameters, , , , is a continuous smooth function, and the lumped disturbance is defined as ； (1.4) Nonlinear decoupling: To solve the coupling problem, position dummy variables are used as follows ； The expected Euler angle can be obtained and total thrust as follows 。 3. The UAV trajectory tracking control method based on function differentiation and adaptive variable gain according to claim 2 is characterized in that: In step (4), designing a non-singular fast sliding mode controller includes the following steps: (4.1) Define the spatial state equation of the quadrotor tracking control system: The system spatial state equation is obtained from the two formulas in step (1.3): ； For the position control subsystem, a sliding mode controller is designed. The position error and velocity error are defined as follows: ； For the attitude control subsystem, a sliding mode controller is designed. The attitude error and angular velocity error are defined as follows: ； (4.2) Design a fast non-singular terminal sliding mode controller: The fast non-singular terminal sliding surface of the attitude subsystem is designed as; ； The fast non-singular terminal sliding surface of the position subsystem is designed as ； here is a positive constant, ; Derivative of the sliding mode function yields ； ； make Get the control law; ； 。 4. The UAV trajectory tracking control method based on function differentiation and adaptive variable gain according to claim 1 is characterized in that: In step (5), the closed-loop stability proof of the trajectory tracking control system includes the following steps: (5.1) Taking the X channel of the decoupled position subsystem as an example, the Lyapunov function is defined as: ； Bring in , the derivative of the above formula with respect to time is written as; ； The Lyapunov function of the position subsystem is chosen as; ； Therefore, the stability of the position system is guaranteed, ensuring the trajectory tracking capability, and the stability of the attitude subsystem is proved to be similar.

Images