Predefined time trajectory tracking control method for quadrotor UAV under multi-source disturbance

Abstract

The present invention discloses a predefined time trajectory tracking control method for a quad-rotor drone under multi-source disturbances, and the steps include: step one, establishing a mathematical model of a quad-rotor drone containing multi-source disturbances; step two, designing a position controller; step three, designing an attitude controller; step four, performing stability analysis. This control method is used to deal with the tracking control problem of a quad-rotor drone under multi-source disturbances. The position loop designs a nonlinear disturbance observer to compensate for the disturbance, and designs a sliding surface to track the position; the attitude loop designs a predefined time non-singular terminal sliding mode controller to reduce the chattering caused by the sliding surface, and uses the convergence law to stabilize the attitude tracking error within a predefined time. The effectiveness of the present invention is verified by theoretical analysis and numerical simulation.

Description

Technical Field

[0001] The invention relates to a predefined time trajectory tracking control method for a quad-rotor unmanned aerial vehicle under multi-source disturbances, and belongs to the technical field of unmanned aerial vehicle intelligent control. Background Art

[0002] In recent years, with the rapid development of micro-mechanical sensors, composite materials, visual positioning and navigation technologies, drones have gradually become more intelligent and lightweight. Due to their versatility and ability to perform tasks autonomously or remotely, drones have obvious advantages in operating in dangerous and complex environments.

[0003] As a typical under-actuated, nonlinear and strongly coupled complex system, the flight process of the quadcopter drone will also be affected by multiple sources of interference such as internal startup parameter perturbations, friction and other unmodeled dynamic external gust interferences, and environmental uncertainty factors. When it works in a complex environment, it must have the characteristics of handling nonlinear dynamics and fast convergence to achieve anti-interference control. The existing control algorithms for the dynamic convergence characteristics of the system include finite time control, fixed time control and predefined time control. The criterion of finite time stability mainly relies on the homogeneous system theory and the finite time Lyapunov stability theory. Its disadvantage is that the upper limit of its stable time depends on the initial conditions of the system; fixed time control can overcome this disadvantage of finite time control. Its stable time is bounded, and the upper limit is independent of the initial state of the system, but the estimation of the stable time limit is uncertain. Therefore, how to achieve the quadcopter drone to handle nonlinear dynamics and fast convergence when working in a complex environment under multi-source disturbances is still a technical problem that needs to be solved urgently. Summary of the invention

[0004] In view of the problems existing in the above-mentioned prior art, the present invention provides a predefined time trajectory tracking control method for a quadrotor unmanned aerial vehicle under multi-source disturbances. The control method can achieve high tracking accuracy and rapid convergence, improve the anti-interference performance of the quadrotor unmanned aerial vehicle, control the state error to zero within a predefined time, and quickly achieve stabilization of the flight attitude.

[0005] In order to achieve the above object, the present invention provides a predefined time trajectory tracking control method for a quad-rotor unmanned aerial vehicle under multi-source disturbance, comprising the following steps:

[0006] S1. Establish a mathematical model of a quadrotor drone. The steps are as follows:

[0007] S1. Establish a mathematical model of a quadrotor drone with multi-source disturbances. The steps are as follows:

[0008] S1.1. Define the angles formed by the rotation around the X-axis, Y-axis and Z-axis in the body coordinate system as the roll angle φ, pitch angle θ and yaw angle ψ of the quadrotor drone, respectively. In the formula, the range of the roll angle φ and the pitch angle θ are both (-0.5π, 0.5π), and the range of the yaw angle ψ is (-π, π); x, y, z represent the horizontal position of the center of mass of the quadrotor drone in the inertial coordinate system; the ground coordinate system is E(O E X E Y E Z E ), the body coordinate system is B(O B X B Y B Z B ).

[0009] S1.2. Based on the Newton-Euler equation, a dynamic model of a quadrotor drone with multi-source disturbances is established:

[0010]

[0011] In the formula, m represents the total mass of the quadrotor drone, g represents the acceleration of gravity, and I x ,I y ,I z represents the moment of inertia of the aircraft around the x, y, and z axes, l is the distance from the rotor center to the center of mass of the drone, [d x ,d y ,d z ,d φ ,d θ ,d ψ ] T represents the total disturbance from multiple sources including wind disturbance, airflow disturbance and parameter perturbation, [k x ,k y ,k z ,k φ ,k θ ,k ψ ] T are the aerodynamic coefficients of the drone, [U1,U2,U3,U4] T is the control input of the system, and U1=f1+f2+f3+f4, U2=l(f4-f2), U3=l(f3-f1), U4=l(f4+f2-f3-f1), where [f1,f2,f3,f4] T The lift generated by the rotors is the actual control input of the quadrotor UAV system;

[0012] S1.3. To simplify the control system design, a virtual control quantity is introduced into the position subsystem:

[0013]

[0014] S1.4. Inversely solve formula (2) and obtain:

[0015]

[0016] In the formula, φ d is the desired roll angle, θ d is the desired pitch angle θ d , U1 is the controller input.

[0017] S1.5. Decouple the quadrotor drone into attitude loop and position loop, and control them separately. The state equation of the position subsystem is:

[0018]

[0019] Where X1 = [x, y, z] T , D X =[d x ,d y ,d z ] T is the bounded interference of multi-source disturbance, G(x)=[1,1,1] T , U=[u x ,u y ,u z ] T is the virtual control law;

[0020] S1.6. The state equation of the attitude subsystem is:

[0021]

[0022] Where, X3 = [φ,θ,ψ] T , D Φ =[d φ ,d θ ,d ψ ] T ,U Φ =[U2,U3,U4] T ,

[0023] S2. Position controller design, the steps are as follows:

[0024] S2.1. The dynamic error of the position system is defined as:

[0025] e X =[e1,e3,e5] T =[xx d ,yy d ,zz d ]T (6)

[0026] Where, e1, e3, e5 are the position tracking errors of the position subsystem;

[0027] Derivative of formula (6) yields:

[0028]

[0029] Where, e2, e4, e6 are the speed tracking errors of the position subsystem;

[0030] S2.2. Design the position subsystem using a sliding mode controller and define the sliding surface of the position system:

[0031]

[0032] Where C1 = [c1, c2, c3] T And c1, c2, c3>0, S X =[s x ,s y ,s z ] T ,s x ,s y ,s z Represent the sliding surfaces of X-axis, Y-axis and Z-axis respectively;

[0033] S2.3. Design of nonlinear disturbance observer:

[0034]

[0035] In the formula, Yes D X Estimated value of; χ=[χ1,χ2,χ3] T is the state vector of the disturbance observer; ε = [ε1, ε2, ε3] T is the bandwidth of the observer, and defines the estimation error and its derivative

[0036]

[0037] In the formula,

[0038] S2.4. Design of position controller

[0039]

[0040] Where η x ,η y ,η z The values ​​of h are all greater than 0; x ,h y ,h zThe values ​​of are all greater than 0;

[0041] S3. Design of attitude controller, the steps are as follows:

[0042] S3.1. Define the tracking error of the attitude angle

[0043] e Φ =[e φ ,e θ ,e ψ ] T =[φ-φ d ,θ-θ d ,ψ-ψ d ] T (12)

[0044] Then the derivative of the tracking error is:

[0045]

[0046] S3.2. Design of non-singular terminal sliding mode control surface with predefined time:

[0047]

[0048] Where, 0<ρ Φ <1 and ρ Φ =[ρ φ ,ρ θ ,ρ ψ ] T , parameter α Φ1 and α Φ2 Set to:

[0049]

[0050] Where n φ ,n θ ,n ψ The values ​​of m are all greater than 0; φ ,m θ ,m ψ The values ​​of are all greater than 0; T c >0;

[0051] Differentiating formula (14) yields:

[0052]

[0053] In the formula, Λ Φ1 =diag{|e φ | -ρ ,|e θ | -ρ ,|e ψ | -ρ},ΛΦ2 =diag{|e φ | ρ ,|e θ | ρ ,|e ψ | ρ};

[0054] S3.3. Design of non-singular sliding mode controller with predefined time:

[0055]

[0056] Where η Φ =[η φ ,η θ ,η ψ ] T ,η φ ,η θ ,η ψ The values ​​of θ are all greater than 0, σ>0, and the parameter h Φ1 and h Φ2 Set to:

[0057]

[0058] Saturation function sat σ (·) is defined as sat σ (Λ1) = diag{sat σ (Λ 11 ),…,sat σ (Λ 1n )}, and sat σ (Λ 1j )for:

[0059]

[0060] Where, j = φ, θ, ψ;

[0061] S4. Stability analysis:

[0062] S4.1. Sliding mode function The derivative is:

[0063]

[0064] Define the Lyapunov function V1:

[0065]

[0066] Differentiating V1 gives:

[0067]

[0068] From formula (22), we know that the Lyapunov function V1 is semi-negative definite, that is, the position system is asymptotically stable and the system converges to 0 in a finite time;

[0069] S4.2. The stability analysis of the attitude loop includes two stages, namely the arrival stage and the sliding stage. In the arrival stage, it is proved that when the predefined time non-singular terminal sliding mode controller is used, the system stabilizes to 0 within the predefined time; in the sliding stage, it is proved that when the system reaches the sliding surface of the predefined time non-singular sliding mode controller, the attitude angle tracking error stabilizes to 0 within the predefined time;

[0070] S4.3. At the arrival stage, select the Lyapunov function V2:

[0071]

[0072] Differentiating V2 gives:

[0073]

[0074] Substituting formula (17) into formula (24), we get:

[0075]

[0076] Substituting formula (18) into formula (25), we get:

[0077]

[0078] V1 is within the predetermined time T≤T c is zero, thus the non-singular terminal sliding surface S Φ Within a predefined time T≤T c Stable at 0;

[0079] S4.4. During the sliding phase, when the non-singular terminal sliding surface is reached at a predefined time, then:

[0080]

[0081] Select Lyapunov function V3:

[0082]

[0083] Differentiating V3 gives:

[0084]

[0085] Substituting formula (15) into formula (29), we get:

[0086]

[0087] From formula (30), we know that V3 is within the predefined time T≤T c The inner convergence to zero, the attitude angle tracking error of the quadrotor drone e Φ and At a predefined time T≤T c Converges to zero.

[0088] Furthermore, in the predefined time non-singular terminal sliding mode controller formula, when e φ →0, Λ φ →∞, introduce the saturation function sat σ (·) is used to limit the amplitude of the singularity function Λ1, thereby ensuring that the attitude tracking error converges to 0 within a predefined time.

[0089] In view of the influence of external disturbances on the quadrotor UAV system, the present invention compensates for the influence of external disturbances on the position loop by designing a sliding mode controller and introducing a disturbance observer; in the attitude loop design, a predefined time non-singular terminal sliding mode controller is designed to ensure that the attitude tracking error can converge within the predefined time. The control method of the present invention not only ensures the stability of the control law, but also realizes high-precision tracking of the reference trajectory within different predefined times, improves the anti-interference performance of the quadrotor UAV, controls the state error to zero within the predefined time, and quickly realizes the stability of the flight attitude. The simulation results show that the control method of the present invention has good effectiveness. BRIEF DESCRIPTION OF THE DRAWINGS

[0090] Figure 1 It is a coordinate system relationship diagram of the quadrotor drone of the present invention;

[0091] Figure 2 It is a comparison chart of the position tracking response curve of the UAV under the sliding mode control method and the given desired position;

[0092] Figure 3 The non-singular terminal sliding mode control method of the embodiment of the present invention has a predefined time of T c =2 when the posture tracking response curve;

[0093] Figure 4 The non-singular terminal sliding mode control method of the embodiment of the present invention has a predefined time of T c =1 when the posture tracking response curve;

[0094] Figure 5 is a posture tracking response curve diagram of the non-singular terminal sliding mode control method of an embodiment of the present invention when the predefined time is Tc=0.5;

[0095] Figure 6It is a comparison chart of the attitude angle tracking response curves of the non-singular terminal sliding mode control method of the present invention with a predefined time, the PD control method based on compensation, the traditional sliding mode control method and the UAV under a given desired attitude;

[0096] Figure 7 It is a comparison chart of the sliding mode control method of the present invention, the compensation-based PD control method and the position tracking response curve of the UAV under a given desired posture;

[0097] Figure 8 It is a comparison chart of the UAV controller response curves under the predefined time non-singular terminal sliding mode control method of the present invention, the compensation-based PD control method and the traditional sliding mode control method. DETAILED DESCRIPTION

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

[0099] Embodiment: In order to verify the effectiveness of the tracking control method of the present invention, for the attitude system model of the quad-rotor drone, based on the following Figure 1 The proposed predefined time non-singular terminal sliding mode control method is simulated and verified under different predefined times.

[0100] The present invention builds a simulation model in the Matlab / Simulink environment to simulate the trajectory tracking of three attitude angles and three positions of the quad-rotor UAV system. The relevant parameter settings are shown in Table 1 and Table 2:

[0101] Table 1 Quadrotor UAV model parameters

[0102] m=1.8kg g=9.8N / kg l＝0.22 <![CDATA[I x =0.0211]]> <![CDATA[I y =0.0211]]> <![CDATA[I z =0.1104]]> <![CDATA[k1=0.01]]> <![CDATA[k2=0.01]]> <![CDATA[k3=0.01]]> <![CDATA[k4=0.012]]> <![CDATA[k5=0.012]]> <![CDATA[k6=0.012]]>

[0103] Table 2 Controller parameters

[0104]

[0105]

[0106] Figure 2 This is the tracking response curve of the position subsystem of the traditional sliding mode control method. The red dotted line is the actual position, and the black solid line is the reference position. It can be seen from the figure that the position tracking can be completed within 1s.

[0107] The tracking response curve of the attitude subsystem of the non-singular terminal sliding mode control method of the present invention is as follows: Figures 3 to 5 As shown in the figure, the red dotted line is the actual position and the black solid line is the reference position. c =2, from Figure 3 It can be seen that posture tracking can be completed within 1.25s; when the predefined time T c =1, from Figure 4 It can be seen that posture tracking can be completed within 0.75s; when the predefined time T c = 0.5, from Figure 5 It can be seen that posture tracking can be completed within 0.45s; Figures 3 to 5 It can be seen that the non-singular predefined time terminal sliding mode controller proposed in the present invention can achieve tracking control of the attitude angle within a given time.

[0108] Figure 6 It is the attitude angle tracking response curve, where the black solid line is the reference attitude angle response curve, the red dotted line is the attitude response curve based on the PD compensation method, the green dotted line is the attitude response curve under the traditional sliding mode control method, and the blue dotted line is the attitude response curve of the predefined time non-singular terminal sliding mode control method proposed in the present invention. It can be seen from the figure that the predefined time non-singular terminal sliding mode control proposed in the present invention can complete attitude tracking within 0.5 seconds, and the control effect is more effective than other control algorithms.

[0109] Figure 7 is the position tracking response curve, where the black solid line is the reference attitude angle response curve, the red dotted line is the attitude response curve based on the PD compensation method, and the green dotted line is the attitude response curve under the sliding mode control method used in the present invention. It can be seen from the figure that the sliding mode control used in the present invention can complete attitude tracking within 0.6; Figure 8 is the response curve of the controller input over time. It can be seen from the figure that the predefined time non-singular terminal sliding mode position control input proposed in the present invention can converge to zero in a shorter time compared with other control methods, and the control inputs corresponding to pitch, roll and yaw can converge to a bounded range in a shorter time.

Claims

1. A predefined time trajectory tracking control method for a quadrotor drone under multi-source disturbances, characterized in that: The steps include: S1. Establish a mathematical model of a quadrotor drone with multi-source disturbances. The steps are as follows: S1.

1. Define the angles formed by the rotation around the X-axis, Y-axis and Z-axis in the body coordinate system as the roll angle φ, pitch angle θ and yaw angle ψ of the quadrotor drone, where the value ranges of the roll angle φ and pitch angle θ are both (-0.5π, 0.5π), and the value range of the yaw angle ψ is (-π, π); x, y, z represent the horizontal position of the center of mass of the quadrotor drone in the inertial coordinate system; the ground coordinate system is E(O E X E Y E Z E ), the body coordinate system is B(O B X B Y B Z B ); S1.

2. Based on the Newton-Euler equation, a dynamic model of a quadrotor drone with multi-source disturbances is established: Where m represents the total mass of the quadcopter, g represents the acceleration of gravity, and I x ,I y ,I z represents the moment of inertia of the body around the x, y, and z axes, l is the distance from the rotor center to the center of mass of the drone, [d x ,d y ,d z ,d φ ,d θ ,d ψ ] T represents the total disturbance from multiple sources including wind disturbance, airflow disturbance and parameter perturbation, [k x ,k y ,k z ,k φ ,k θ ,k ψ ] T is the aerodynamic coefficient of the drone, [U1,U2,U3,U4] T is the control input of the system, and U1=f1+f2+f3+f4, U2=l(f4-f2), U3=l(f3-f1), U4=l(f4+f2-f3-f1), where [f1,f2,f3,f4] T The lift generated by the rotors is the actual control input of the quadrotor UAV system; S1.

3. To simplify the control system design, a virtual control quantity is introduced into the position subsystem: S1.

4. Inversely solve formula (2) and obtain: Among them, φ d is the desired roll angle, θ d is the desired pitch angle θ d , U1 is the controller input; S1.

5. Decouple the quadrotor drone into attitude loop and position loop, and control them separately. The state equation of the position subsystem is: Where X1 = [x, y, z] T , D X =[d x ,d y ,d z ] T is the bounded interference of multi-source disturbance, G(x)=[1,1,1] T ,U=[u x ,u y ,u z ] T is the virtual control quantity; S1.

6. The state equation of the attitude subsystem is: Where, X3=[φ,θ,ψ] T , D Φ =[d φ ,d θ ,d ψ ] T ,U Φ =[U2,U3,U4] T , S2. Position controller design, the steps are as follows: S2.

1. The dynamic error of the position system is defined as: e X =[e1,e3,e5] T =[x-x d ,y-y d ,z-z d ] T (6) Among them, e1, e3, e5 are the position tracking errors of the position subsystem; Derivative of formula (6) yields: Among them, e2, e4, e6 are the speed tracking errors of the position subsystem; S2.

2. Design the position subsystem using a sliding mode controller and define the sliding surface of the position system: Where C1=[c1,c2,c3] T And the values ​​of c1, c2, c3 are all greater than 0, S X =[s x ,s y ,s z ] T ,s x ,s y ,s z Represent the sliding surfaces of X-axis, Y-axis and Z-axis respectively; S2.

3. Design of nonlinear disturbance observer: in, Yes D X Estimated value of; χ=[χ1,χ2,χ3] T is the state vector of the disturbance observer; ε = [ε1, ε2, ε3] T is the bandwidth of the observer, and defines the estimation error and its derivative in, S2.

4. Design of position controller Among them, η x ,η y ,η z The values ​​of h are all greater than 0; x ,h y ,h z The values ​​of are all greater than 0; S3. Design of attitude controller, the steps are as follows: S3.

1. Define the tracking error of the attitude angle: e Φ =[e φ ,e θ ,e ψ ] T =[φ-φ d ,θ-θ d ,ψ-ψ d ] T (12) Then the derivative of the tracking error is: S3.

2. Design of non-singular terminal sliding mode control surface with predefined time: Among them, 0<ρ Φ <1 and ρ Φ =[ρ φ ,ρ θ ,ρ ψ ] T , parameter α Φ1 and α Φ2 Set to: Among them, n φ ,n θ ,n ψ The values ​​of m are all greater than 0; φ ,m θ ,m ψ The values ​​of are all greater than 0; T c >0; Differentiating formula (14) yields: where, Λ Φ1 = diag{|e φ | -ρ ,|e θ | -ρ ,|e ψ | -ρ}, Λ Φ2 = diag{|e φ | ρ ,|e θ | ρ ,|e ψ | ρ}; S3.

3. Design of non-singular sliding mode controller with predefined time: Among them, η Φ =[η φ , η θ , η ψ ] T , η φ , η θ , η ψ The values ​​of are all greater than 0, σ>0, and the parameter h Φ1 and h Φ2 Set to: Saturation function sat σ (·) is defined as sat σ (Λ1) = diag{sat σ (Λ 11 ),…,sat σ (Λ 1n )}, and sat σ (Λ 1j )for: Where j = φ, θ, ψ; S4. Stability analysis: S4.

1. Sliding mode function The derivative is: Define the Lyapunov function V1: Differentiating V1 gives: From formula (22), we know that the Lyapunov function V1 is semi-negative definite, that is, the position system is asymptotically stable and the system converges to 0 in a finite time; S4.

2. The stability analysis of the attitude loop includes two stages, namely the arrival stage and the sliding stage. In the arrival stage, it is proved that when the predefined time non-singular terminal sliding mode controller is used, the system stabilizes to 0 within the predefined time; in the sliding stage, it is proved that when the system reaches the sliding surface of the predefined time non-singular sliding mode controller, the attitude angle tracking error stabilizes to 0 within the predefined time; S4.

3. At the arrival stage, select the Lyapunov function V2: Differentiating V2 gives: Substituting formula (17) into formula (24), we get: Substituting formula (18) into formula (25), we get: V1 is within the predetermined time T≤T c is zero, thus the non-singular terminal sliding surface S Φ Within a predefined time T≤T c Stable at 0; S4.

4. During the sliding phase, when the non-singular terminal sliding surface is reached at a predefined time, then: Select Lyapunov function V3: Differentiating V3 gives: Substituting formula (15) into formula (29), we get: From formula (30), we know that V3 is within the predefined time T≤T c The inner convergence to zero, the attitude angle tracking error of the quadrotor drone e Φ and At a predefined time T≤T c Converges to zero.

2. The method for tracking and controlling a quadrotor drone in a predefined time under multi-source disturbance according to claim 1, characterized in that: In the predefined time non-singular terminal sliding mode controller formula, when e φ →0, Λ φ →∞, introduce the saturation function sat σ (·) is used to limit the amplitude of the singularity function Λ1, thereby ensuring that the attitude tracking error converges to 0 within a predefined time.

Images