Quadrotor unmanned aerial vehicle trajectory tracking control method based on fuzzy disturbance observer

Abstract

The invention discloses a four-rotor unmanned aerial vehicle trajectory tracking control method based on a fuzzy disturbance observer. The method comprises the steps that S1, a kinetic model of a four-rotor unmanned aerial vehicle is established; s2, establishing a fuzzy disturbance observer (FDO); s3, establishing a position subsystem tracking controller; and S4, establishing an attitude subsystem tracking controller. According to the four-rotor unmanned aerial vehicle trajectory tracking control technology designed by the invention, the trajectory tracking problem of the unmanned aerial vehicle under the conditions of unknown parameter uncertainty and external disturbance is comprehensively considered, and the uncertainty and external disturbance of system parameters are evaluated based on a fuzzy disturbance observer. Besides, a dynamic curved surface control method is designed, researched and developed to construct a trajectory tracking controller, so that the robustness of the quad-rotor unmanned aerial vehicle is improved, and the explosion problem of differential terms of the system is reduced. For the designed trajectory tracking controller, a stability proof is given, which indicates that the tracking error of the subsystem can be globally consistent and bounded.

Description

Technical Field

[0001] The invention relates to a four-rotor unmanned aerial vehicle trajectory tracking control method based on a fuzzy disturbance observer, and belongs to the technical field of unmanned aerial vehicle trajectory tracking. Background Art

[0002] With the development of unmanned systems, drones have become the main research object in important fields and are widely used in various fields such as disaster relief, unmanned combat, and forest fire prevention. As a vertical take-off and landing aircraft, the key technologies of quadrotor drones have been fully developed and have shown good application prospects in rescue, monitoring, inspection, and mapping. In addition, quadrotor drones have the advantages of small size, low flight altitude, high maneuverability, and strong maneuverability. However, as a four-input-six-output mechanical system, its insufficient drive characteristics make the design of the control system complicated. Among them, the linear control method is widely used in the design of its controller by linearizing the dynamic model of the quadrotor at certain specific equilibrium points; in order to achieve trajectory tracking control, a conventional feedback controller composed of a proportional differential controller and a proportional integral differential controller can be designed; in order to obtain better control performance and overcome the weaknesses of several linear control methods, some advanced nonlinear control methods have also been widely studied, for example. Trajectory tracking is achieved by combining inner / outer loops with backstepping and sliding mode technology.

[0003] Although the above method can achieve the expected tracking performance of the quadrotor, the controller parameters cannot be adjusted online. It should be noted that the dynamics of the quadrotor involves several uncertainties, such as parameter disturbances, unknown external disturbances, nonlinear factors, and unmodeled dynamics. Therefore, the impact of parameter uncertainty and external disturbances on the control of the quadrotor cannot be ignored. It is well known that sliding mode control is one of the important tools for dealing with uncertain systems. For example, in the design of a quadrotor drone with tilted propellers, in order to overcome the inherent under-actuation, four additional control inputs are used to drive the propeller pitch, thereby generating a full drive of the quadrotor position; the controller designed according to the sliding mode theory can drive the system to reach a steady state along the pre-designed sliding mode trajectory. In addition, the sliding mode controller is insensitive to system uncertainties and external disturbances and is highly robust.

[0004] In fact, drones are easily affected by environmental conditions, blade waves, wind and other disturbances during flight. These disturbances can reduce flight quality and even damage the stability of the drone. The performance of the flight control system of the quadcopter drone cannot cope with model uncertainties and external aerodynamic disturbances. Summary of the invention

[0005] Purpose of the invention: In view of the shortcomings of the prior art, the present invention provides a quadrotor UAV trajectory tracking control method based on a fuzzy disturbance observer. The present invention establishes a fuzzy disturbance observer and based on this establishes a position subsystem tracking controller and an attitude subsystem tracking control, thereby realizing the tracking control of the quadrotor UAV trajectory.

[0006] Technical solution: A quadrotor UAV trajectory tracking control method based on fuzzy disturbance observer includes the following steps:

[0007] S1: Establish the dynamic model of the quadrotor drone: Analyze the dynamic characteristics of the quadrotor drone and establish the dynamic model of the quadrotor drone;

[0008] S2: Establish fuzzy disturbance observer FDO: Combine fuzzy disturbance observation theory to establish a fuzzy disturbance observer suitable for quadrotor UAV to provide estimation of parameter uncertainty and external disturbance;

[0009] S3: Establishing the position subsystem tracking controller: Combining the fuzzy disturbance observer established in S2, correcting parameter uncertainty and external disturbances in real time, and establishing the position subsystem tracking controller using the dynamic surface control method;

[0010] S4: Establish attitude subsystem tracking controller: Combine the fuzzy disturbance observer established in S2 and use the dynamic surface control method to design the attitude subsystem tracking controller.

[0011] Preferably, the S1 is specifically:

[0012] S101: Establish a basic dynamics model for a quadrotor drone;

[0013] S102: Combine parameter uncertainty and external disturbance factors to establish a quadrotor UAV dynamics model.

[0014] Preferably, the S101 is specifically:

[0015] Define the body coordinate system Oxyz and the geocentric coordinate system O respectively, whose origin coincides with the center of mass of the aircraft e x e y e z e , the structure of the quadrotor drone is symmetrical relative to the planes Oxz and Oyz, so the inertia moment tensor only has diagonal inertia terms. Since the fuselage of the quadrotor drone is a rigid body, the Newton-Euler formula is used to derive nonlinear dynamics, that is, the basic dynamic model of the quadrotor drone is obtained:

[0016]

[0017] Where m is the mass of the quadrotor; J = diag{J x ,Jy ,J z} is the inertia matrix; V b =[u,v,ω] T and Ω=[p,q,r] T are the velocity and angular velocity in the body coordinate system respectively; F is the resultant external force including rotor lift and gravity; τ a is the external torque; J x is the inertia matrix in the x-axis direction, J y is the inertia matrix in the y-axis direction, J z is the inertia matrix in the z-axis direction, u is the velocity in the x-axis direction, v is the velocity in the y-axis direction, ω is the velocity in the z-axis direction, p is the angular velocity in the x-axis direction, q is the angular velocity in the y-axis direction, and r is the velocity in the z-axis direction.

[0018] Since the conversion between the body coordinate system and the geocentric coordinate system can be completed through the coordinate transformation matrix, the UAV dynamics model in the geocentric coordinate system can be expressed as:

[0019]

[0020] Where P = [x, y, z] T and They represent the position and velocity vectors in the geocentric coordinate system respectively; f 1 =-gz e is the system function, g 1 =1 / m is the system function, M 1 =TRz e is the system function, g is the acceleration due to gravity; is the total thrust generated by the four rotors, b is the thrust coefficient, ω i (i=1,2,3,4) is the speed of the rotor; d 1 =[d 11 ,d 12 ,d 13 ] T is a composite disturbance, including the modeling dynamic error d 11 , internal uncertainty 12 and external disturbance d 13 ; z e =(0,0,1) T is the unit vector of the geocentric coordinate system; R is the transformation matrix between the geocentric coordinate system and the body coordinate system, expressed as:

[0021]

[0022] Among them, θ is the roll angle, ψ is the pitch angle, and φ is the yaw angle;

[0023] According to the transformation matrix relationship between the geocentric coordinate system and the body coordinate system, the transformation relationship between the attitude angles θ, ψ, φ and the flight angular rates p, q, r around the body axis is expressed as:

[0024]

[0025] where Θ = [φ,θ,ψ] T is the Euler angle vector, and the transformation matrix Φ(Θ) is expressed as:

[0026]

[0027] Among them, det(Φ)=1 / cosθ. At this time, when the pitch angle of the drone satisfies θ≠(2k-1)π / 2, k∈Z, Φ(Θ) is a reversible matrix, where k=...,-2,-1,0,1,2,... are integers and Z is a set of integers.

[0028] Preferably, the S102 is specifically:

[0029] According to (2) and (6), considering the uncertainty of the aircraft model, its dynamic model can be expressed as:

[0030]

[0031] Where ΔJ∈R 3×3 represents the uncertainty of the inertia matrix caused by the change of the UAV load, Δg = [Δg 1 ,Δg 2 ,Δg 3 ] T is the unknown aerodynamic disturbance, where Δg 1 is the aerodynamic disturbance of the rolling channel, Δg 2 is the aerodynamic disturbance of the pitch channel, Δg 3 is the aerodynamic disturbance in the yaw channel, is the gyro torque, J r is the moment of inertia;

[0032] Control matrix equation τ a Produced by four rotors:

[0033]

[0034] Where l is the distance from the rotor to the center of mass, and k is the drag coefficient. Rearranging (8) yields:

[0035]

[0036] Among them, f 2 =-J -1 [Ω×JΩ]-J -1 G a,g 2 =J -1 ,M 2 =τ a And d 2 =[d 21 ,d 22 ,d 23 ] T is a composite disturbance, including the modeling dynamic error d 21 , internal uncertainty 22 and external disturbance d 23 , where f 2 is the system function matrix, g 2 is the system function matrix, M 2 is the system control matrix equation.

[0037] Preferably, S2 is specifically:

[0038] S201: Establish a fuzzy disturbance observer based on fuzzy disturbance observation theory;

[0039] S202: Verify whether the fuzzy disturbance observer of S201 can achieve consistent stability.

[0040] Preferably, the S201 is specifically:

[0041] Create a fuzzy disturbance observer:

[0042]

[0043] Where σ is the observer design parameter matrix, is the system estimation function, f is the matrix [f 1 ,f 2 ], g is the matrix [g 1 ,g 2 ];

[0044] The adaptive law design of the i-th adjustment parameter vector is:

[0045]

[0046] Where Z = [Z 1 ,Z 2 ,...,Z 6 ] T is the internal state of the fuzzy disturbance observer, σ=diag[σ i ] 6×6 and is the estimated value of the system d(x,u), l = xZ is the observation error of the system, κ 1i >0 is the default parameter, κ 2i>0 is the preset parameter, then the disturbance observer error can achieve consistent stability, which shows that l i Can converge to a small region near zero Inside; is the i-th adaptive law of the observer, ξ i is the i-th fuzzy basis function vector, σ i is the observer design parameter, l i is the i-th observation error, L i is the positive coefficient of the i-th Lyapunov function, L i is the constant coefficient of the i-th Lyapunov function;

[0047] Combining the approximation theory of fuzzy systems, unknown disturbances in is the optimal parameter vector, is the estimation error of the fuzzy system; The specific expression of is:

[0048]

[0049] Based on the approximation theory of fuzzy systems, the estimated error ε of the disturbance observer is confirmed i and the optimal parameter vector is bounded, which means that there is a constant Make and Established.

[0050] Preferably, the S202 is specifically:

[0051] According to l = xZ and equation (13), the dynamic change of the disturbance observer error is expressed as:

[0052]

[0053] in, Through the Lyapunov function calculation, we can get:

[0054] V 0 =V 1 +...+V 6 , (15)

[0055] The i-th (i=1,2,...,6) element is represented as:

[0056]

[0057] Among them, V i is the i-th Lyapunov function;

[0058] Expanding (16) and (14) yields:

[0059]

[0060] Substituting (12) into (17) yields:

[0061]

[0062] It turns out that:

[0063]

[0064] Finally, it is deduced that:

[0065]

[0066] ≤-L i V i +U i ,

[0067] Among them, L i =min{σ i ,κ 2i}, Among them, L i is the positive coefficient of the i-th Lyapunov function; from (20), we can derive:

[0068]

[0069] Where, 0<L≤min{L 1 ,...,L i ,...,L 6}, This shows that the established fuzzy disturbance observer can achieve consistent stability;

[0070] Transform formula (21) to obtain From formula (19) to (21), we can further obtain Where t is time, t 0 is the initial time, U i is the normal constant of the i-th Lyapunov function; by setting the observer parameters, l i It can converge to a small range close to the equilibrium point.

[0071] Preferably, S3 is specifically:

[0072] Define the position command vector as P d =[x d ,y d ,z d ] T , the position error is P 1=PP d ; First, choose the Lyapunov alternative equation as:

[0073]

[0074] Among them, K 1 is a symmetric positive definite parameter, W 1 is the Lyapunov candidate function, take The derivative of the result is:

[0075]

[0076] in, is a virtual control signal, which can be Calculated; V d is the desired aircraft speed;

[0077] The speed error is defined as In this case, we get Choose the Lyapunov-like function as:

[0078]

[0079] W 2 is the Lyapunov candidate function, and the derivative of the above formula with respect to time can be obtained:

[0080]

[0081] In the formula, u T =TRz e / m is used as a virtual control signal;

[0082] In order to prove that W 2 <0, use fuzzy disturbance observer to estimate the composite disturbance:

[0083]

[0084] Among them, K 2 is a symmetric positive definite function, E∈R 3×3 is the identity matrix, U=(U 1 ,U 2 ,U 3 ) T It can be expressed as:

[0085]

[0086] Among them, U is the matrix function of the system controller, U 1 ,U 2 ,U 3 They are the controllers for the x, y, and z directions of the aircraft respectively;

[0087] Substituting (26) into (24) yields:

[0088]

[0089] Among them, P 1 is the position error, P 2 is the velocity error, therefore, the position tracking subsystem can be stabilized;

[0090] Here, equation (26) is equivalent to:

[0091]

[0092] It can be seen from this that:

[0093] U 1 cosθcosψ+U 2 cosθsinψ-(U 3 +g)sinθ=0, (30)

[0094] U 1 (sinθcosψsinφ-sinψcosφ)+U 2 (cosψcosφ+sinθsinψsinφ)+(U 3 +g)cosθsinφ=0,(31)

[0095] U 1 (sinθcosψcosφ+sinψsinφ)+U 2 (cosψsinφ+sinθsinψcosφ)+(U 3 +g)cosθcosφ=T / m(32)

[0096] According to (30) and (31), the pitch and roll attitude dynamic equations are obtained:

[0097]

[0098] Among them, θ d is the desired roll angle, ψ d is the desired pitch angle, φ d For the desired yaw angle, the total desired lift generated by the four rotors is obtained by using (32) as follows:

[0099] T d =m[U 1 (sinθcosψcosφ+sinψsinφ)+U 2 (cosψsinφ+sinθsinψcosφ)+(U 3 +g)cosθcosφ](35).

[0100] Preferably, the S4 is specifically:

[0101] Assume that the norm Φ of the transformation matrix is ​​bounded, that is, there is a positive constant Φ L and Φ U Satisfy 0<Φ L ≤||Φ||≤Φ U ; Φ L is the norm lower bound of the matrix function Φ, Φ U is the norm upper bound of the matrix function Φ,

[0102] First, define the attitude angle tracking error as Ω 3 =θ-Ω d and Ω 3 is the attitude angle error, Ω d is the desired attitude angle; secondly, considering the angular rate Ω as the virtual control input of the attitude angle θ, the control law can be obtained:

[0103]

[0104] in, is the derivative of the desired attitude angle, K 3 is a symmetric and positive definite parameter matrix;

[0105] A first-order low-pass filter is used to process Ω d and its derivatives

[0106]

[0107] Define the angular rate tracking error Ξ 4 =Ω-Ω d , the filter error vector η is the filtering constant, then we get B=-Ω d , B is the negative desired attitude angle, take Ω 3 The derivative of can be obtained:

[0108]

[0109] Lyapunov candidate function selection:

[0110]

[0111] W 3 is the Lyapunov candidate function, for W 3 The derivative is:

[0112]

[0113] Assume that there are higher-order derivatives of the attitude angle command, and the parameter B and its related terms are defined on a compact set; in this case, all components of B have their maximum value H i >0,i=1,2,3 satisfies |B i |≤H i , that is, B T B≤||H|| 2 ;

[0114] From Young's inequality equation we can get:

[0115]

[0116] Since there is a positive constant Φ L and Φ U Satisfy 0<Φ L ≤||Φ||≤Φ U , substituting (41) into (40) yields:

[0117]

[0118] in, Φ U is the norm upper bound of the matrix function Φ; similarly, we can get Ξ 4 The derivative of :

[0119]

[0120] Among them, f 2 is the attitude subsystem matrix function, g 2 is the attitude subsystem matrix function,

[0121] In order to avoid the differential explosion problem of virtual control input, a dynamic surface strategy is constructed and the sliding surface S = cΩ is defined 3 +Ξ 4 , where c>0 is a preset parameter, and taking the derivative of S yields:

[0122]

[0123] The actual control input τ a Designed in the following form:

[0124]

[0125] Among them, K 4 is a symmetric positive definite parameter; the Lyapunov candidate function is chosen as:

[0126]

[0127] Furthermore, W 4 is the Lyapunov candidate function, W4 The derivative of satisfies:

[0128]

[0129] Among them, cS T ΦS≤cΦ U ||S|| 2 ,

[0130] Therefore, it can be deduced that:

[0131] The coefficient of

[0132] Therefore, by choosing a suitable control parameter K 3 ,K 4 ,c,η can make C m >0; In addition, d 1 ≈d 2 , and Q=||H|| 2 is a bounded constant; therefore, equation (48) is simplified to:

[0133]

[0134] Solving inequality (49) yields:

[0135]

[0136] Therefore, it is concluded that the tracking error, filtering error, adaptive estimation error and observation error of the attitude loop are all bounded; that is, the attitude subsystem can achieve ultimate boundedness.

[0137] Beneficial effects: The quadcopter UAV trajectory tracking control technology designed by the present invention comprehensively considers the trajectory tracking problem of the UAV under the condition of unknown parameter uncertainty and external disturbance, and evaluates the uncertainty of system parameters and external disturbance based on the fuzzy disturbance observer. In addition, a dynamic surface control method is designed and developed to construct a trajectory tracking controller, thereby ensuring the improvement of the robustness of the quadcopter UAV and reducing the explosion problem of the system differential term. For the designed trajectory tracking controller, a stability proof is given, indicating that the tracking error of the subsystem can be globally uniformly bounded. BRIEF DESCRIPTION OF THE DRAWINGS

[0138] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying creative work.

[0139] Figure 1 is a flow chart of the method of the present invention;

[0140] Figure 2 This is a structural principle diagram of the quad-rotor drone of the present invention;

[0141] Figure 3 This is a trajectory tracking diagram of the quadrotor drone of the present invention. DETAILED DESCRIPTION

[0142] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0143] In the description of the present invention, it is necessary to understand that the terms "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "outside", etc., indicating the orientation or position relationship are based on the orientation or position relationship shown in the drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation on the present invention.

[0144] In the present invention, unless otherwise clearly specified and limited, a first feature being "above" or "below" a second feature may include that the first and second features are in direct contact, or may include that the first and second features are not in direct contact but are in contact through another feature between them. Moreover, a first feature being "above", "above" and "above" a second feature includes that the first feature is directly above and obliquely above the second feature, or simply indicates that the first feature is higher in level than the second feature. A first feature being "below", "below" and "below" a second feature includes that the first feature is directly below and obliquely below the second feature, or simply indicates that the first feature is lower in level than the second feature.

[0145] like Figure 1 As shown in FIG. 1 , a quadrotor UAV trajectory tracking control method based on a fuzzy disturbance observer comprises the following steps:

[0146] S1: Establish the dynamic model of the quadrotor drone: Analyze the dynamic characteristics of the quadrotor drone and establish the dynamic model of the quadrotor drone;

[0147] S101: Establish a basic dynamics model for a quadrotor drone;

[0148] like Figure 2As shown in the figure, the body coordinate system Oxyz and the geocentric coordinate system O are defined respectively, whose origin coincides with the center of mass of the aircraft. e x e y e z e , the structure of the quadrotor drone is symmetrical relative to the planes Oxz and Oyz, so the inertia moment tensor only has diagonal inertia terms. Since the fuselage of the quadrotor drone is a rigid body, the Newton-Euler formula is used to derive nonlinear dynamics, that is, the basic dynamic model of the quadrotor drone is obtained:

[0149]

[0150] Where m is the mass of the quadrotor; J = diag{J x ,J y ,J z} is the inertia matrix; V b =[u,v,ω] T and Ω=[p,q,r] T are the velocity and angular velocity in the body coordinate system respectively; F is the resultant external force including rotor lift and gravity; τ a is the external torque; J x is the inertia matrix in the x-axis direction, J y is the inertia matrix in the y-axis direction, J z is the inertia matrix in the z-axis direction, u is the velocity in the x-axis direction, v is the velocity in the y-axis direction, ω is the velocity in the z-axis direction, p is the angular velocity in the x-axis direction, q is the angular velocity in the y-axis direction, and r is the velocity in the z-axis direction.

[0151] Since the conversion between the body coordinate system and the geocentric coordinate system can be completed through the coordinate transformation matrix, the UAV dynamics model in the geocentric coordinate system can be expressed as:

[0152]

[0153] Where P = [x, y, z] T and They represent the position and velocity vectors in the geocentric coordinate system respectively; f 1 =-gz e is the system function, g 1 =1 / m is the system function, M 1 =TRz e is the system function, g is the acceleration due to gravity; is the total thrust generated by the four rotors, b is the thrust coefficient, ω i (i=1,2,3,4) is the speed of the rotor; d 1 =[d 11 ,d 12 ,d 13 ]T is a composite disturbance, including the modeling dynamic error d 11 , internal uncertainty 12 and external disturbance d 13 ; z e =(0,0,1) T is the unit vector of the geocentric coordinate system; R is the transformation matrix between the geocentric coordinate system and the body coordinate system, expressed as:

[0154]

[0155] Among them, θ is the roll angle, ψ is the pitch angle, and φ is the yaw angle;

[0156] According to the transformation matrix relationship between the geocentric coordinate system and the body coordinate system, the transformation relationship between the attitude angles θ, ψ, φ and the flight angular rates p, q, r around the body axis is expressed as:

[0157]

[0158] where Θ = [φ,θ,ψ] T is the Euler angle vector, and the transformation matrix Φ(Θ) is expressed as:

[0159]

[0160] Among them, det(Φ)=1 / cosθ. At this time, when the pitch angle of the drone satisfies θ≠(2k-1)π / 2, k∈Z, Φ(Θ) is a reversible matrix, where k=...,-2,-1,0,1,2,... are integers and Z is a set of integers.

[0161] S102: Establish a quadrotor UAV dynamics model by combining parameter uncertainty and external disturbance factors;

[0162] According to (2) and (6), considering the uncertainty of the aircraft model, its dynamic model can be expressed as:

[0163]

[0164] Where ΔJ∈R 3×3 represents the uncertainty of the inertia matrix caused by the change of the UAV load, Δg = [Δg 1 ,Δg 2 ,Δg 3 ] T is the unknown aerodynamic disturbance, where Δg 1 is the aerodynamic disturbance of the rolling channel, Δg 2 is the aerodynamic disturbance of the pitch channel, Δg 3 is the aerodynamic disturbance in the yaw channel, is the gyro torque, J r is the moment of inertia;

[0165] Control matrix equation τ a Produced by four rotors:

[0166]

[0167] Where l is the distance from the rotor to the center of mass, and k is the drag coefficient. Rearranging (8) yields:

[0168]

[0169] Among them, f 2 =-J -1 [Ω×JΩ]-J -1 G a ,g 2 =J -1 ,M 2 =τ a And d 2 =[d 21 ,d 22 ,d 23 ] T is a composite disturbance, including the modeling dynamic error d 21 , internal uncertainty 22 and external disturbance d 23 , where f 2 is the system function matrix, g 2 is the system function matrix, M 2 is the system control matrix equation.

[0170] S2: Establish a fuzzy disturbance observer: Combine the fuzzy disturbance observation theory to establish a fuzzy disturbance observer suitable for quadrotor UAV to provide estimation of parameter uncertainty and external disturbance;

[0171] S201: Establish a fuzzy disturbance observer based on fuzzy disturbance observation theory;

[0172] The trajectory tracking control technology for quadrotor UAV based on fuzzy disturbance observer is designed and developed in combination with fuzzy disturbance observation theory, which can generate estimates of parameter uncertainty and external disturbances.

[0173] Create a fuzzy disturbance observer:

[0174]

[0175] Where σ is the observer design parameter matrix, is the system estimation function, f is the matrix [f 1 ,f 2 ], g is the matrix [g 1 ,g 2 ];

[0176] The adaptive law design of the i-th adjustment parameter vector is:

[0177]

[0178] Where Z = [Z 1 ,Z 2 ,...,Z 6 ] T is the internal state of the fuzzy disturbance observer, σ=diag[σ i ] 6×6 and is the estimated value of the system d(x,u), l = xZ is the observation error of the system, κ 1i >0 is the default parameter, κ 2i >0 is the preset parameter, then the disturbance observer error can achieve consistent stability, which shows that l i Can converge to a small region near zero Inside; is the i-th adaptive law of the observer, ξ i is the i-th fuzzy basis function vector, σ i is the observer design parameter, l i is the i-th observation error, L i is the positive coefficient of the i-th Lyapunov function, L i is the constant coefficient of the i-th Lyapunov function;

[0179] Combining the approximation theory of fuzzy systems, unknown disturbances in is the optimal parameter vector, is the estimation error of the fuzzy system; The specific expression of is:

[0180]

[0181] Based on the approximation theory of fuzzy systems, the estimated error ε of the disturbance observer is confirmed i and the optimal parameter vector is bounded, which means that there is a constant Make and Established.

[0182] S202: verify whether the fuzzy disturbance observer of S201 can achieve consistent stability;

[0183] According to l = xZ and equation (13), the dynamic change of the disturbance observer error is expressed as:

[0184]

[0185] in, Through the Lyapunov function calculation, we can get:

[0186] V 0 =V 1 +...+V 6 , (15)

[0187] The i-th (i=1,2,...,6) element is represented as:

[0188]

[0189] Among them, V i is the i-th Lyapunov function;

[0190] Expanding (16) and (14) yields:

[0191]

[0192] Substituting (12) into (17) yields:

[0193]

[0194] It turns out that:

[0195]

[0196] Finally, it is deduced that:

[0197]

[0198] Among them, L i =min{σ i ,κ 2i}, Among them, L i is the positive coefficient of the i-th Lyapunov function; from (20), we can derive:

[0199]

[0200] Where, 0<L≤min{L 1 ,...,L i ,...,L 6}, This shows that the established fuzzy disturbance observer can achieve consistent stability;

[0201] Transform formula (21) to obtain From formula (19) to (21), we can further obtain Where t is time, t 0is the initial time, U i is the normal constant of the i-th Lyapunov function; by setting the observer parameters, l i It can converge to a small range close to the equilibrium point.

[0202] Based on the time scale separation theory, we can construct controllers for the fast loop and the slow loop separately. When designing the slow loop controller, we assume that the fast loop is straight-through, and then separate the attitude control loop and the trajectory control loop. Specifically, the former handles attitude motion through virtual control surfaces, while the latter supports trajectory control through attitude motion.

[0203] S3: Establishing the position subsystem tracking controller: Combining the fuzzy disturbance observer established in S2, correcting parameter uncertainty and external disturbances in real time, and establishing the position subsystem tracking controller using the dynamic surface control method;

[0204] In S3, a position subsystem tracking controller combined with a fuzzy disturbance observer is designed. Specifically, an attitude controller based on a dynamic surface framework is developed, and the fuzzy disturbance observer proposed in S2 is applied to the controller design process to correct parameter uncertainties and external disturbances in real time, ensuring the stability and performance of the UAV.

[0205] Define the position command vector as P d =[x d ,y d ,z d ] T , the position error is P 1 =PP d ; First, choose the Lyapunov alternative equation as:

[0206]

[0207] Among them, K 1 is a symmetric positive definite parameter, W 1 is the Lyapunov candidate function, take The derivative of the result is:

[0208]

[0209] in, is a virtual control signal, which can be Calculated; V d is the desired aircraft speed;

[0210] The speed error is defined as In this case, we get Choose the Lyapunov-like function as:

[0211]

[0212] W 2 is the Lyapunov candidate function, and the derivative of the above formula with respect to time can be obtained:

[0213]

[0214] In the formula, u T =TRz e / m is used as a virtual control signal;

[0215] In order to prove that W 2 <0, use fuzzy disturbance observer to estimate the composite disturbance:

[0216]

[0217] Among them, K 2 is a symmetric positive definite function, E∈R 3×3 is the identity matrix, U=(U 1 ,U 2 ,U 3 ) T It can be expressed as:

[0218]

[0219] Among them, U is the matrix function of the system controller, U 1 ,U 2 ,U 3 They are the controllers for the x, y, and z directions of the aircraft respectively;

[0220] Substituting (26) into (24) yields:

[0221]

[0222] Among them, P 1 is the position error, P 2 is the velocity error, therefore, the position tracking subsystem can be stabilized;

[0223] Here, equation (26) is equivalent to:

[0224]

[0225] It can be seen from this that:

[0226] U 1 cosθcosψ+U 2 cosθsinψ-(U 3 +g)sinθ=0, (30)

[0227] U 1 (sinθcosψsinφ-sinψcosφ)+U 2(cosψcosφ+sinθsinψsinφ)+(U 3 +g)cosθsinφ=0,(31)

[0228] U 1 (sinθcosψcosφ+sinψsinφ)+U 2 (cosψsinφ+sinθsinψcosφ)+(U 3 +g)cosθcosφ=T / m(32)

[0229] According to (30) and (31), the pitch and roll attitude dynamic equations are obtained:

[0230]

[0231] Among them, θ d is the desired roll angle, ψ d is the desired pitch angle, φ d For the desired yaw angle, the total desired lift generated by the four rotors is obtained by using (32) as follows:

[0232] T d =m[U 1 (sinθcosψcosφ + sinψsinφ)+U 2 (cosψsinφ+sinθsinψcosφ)+(U 3 +g)cosθcosφ] (35)

[0233] S4: Establish attitude subsystem tracking controller: Combine the fuzzy disturbance observer established in S2 and use the dynamic surface control method to design the attitude subsystem tracking controller;

[0234] S4 combines the attitude subsystem controller with a fuzzy disturbance observer to realize the development of a trajectory tracking controller, improve the robustness of the UAV, and mitigate the explosion problem of the differential term.

[0235] Assume that the norm Φ of the transformation matrix is ​​bounded, that is, there is a positive constant Φ L and Φ U Satisfy 0<Φ L ≤||Φ||≤Φ U ; Φ L is the norm lower bound of the matrix function Φ, Φ U is the norm upper bound of the matrix function Φ,

[0236] First, define the attitude angle tracking error as Ω 3 =θ-Ω d and Ω 3 is the attitude angle error, Ωd is the desired attitude angle; secondly, considering the angular rate Ω as the virtual control input of the attitude angle θ, the control law can be obtained:

[0237]

[0238] in, is the derivative of the desired attitude angle, K 3 is a symmetric and positive definite parameter matrix;

[0239] A first-order low-pass filter is used to process Ω d and its derivatives

[0240]

[0241] Define the angular rate tracking error Ξ 4 =Ω-Ω d , the filter error vector η is the filtering constant, then we get B=-Ω d , B is the negative desired attitude angle, take Ω 3 The derivative of can be obtained:

[0242]

[0243] Lyapunov candidate function selection:

[0244]

[0245] W 3 is the Lyapunov candidate function, for W 3 The derivative is:

[0246]

[0247] Assume that there are higher-order derivatives of the attitude angle command, and the parameter B and its related terms are defined on a compact set; in this case, all components of B have their maximum value H i >0,i=1,2,3 satisfies |B i |≤H i , that is, B T B≤||H|| 2 ;

[0248] From Young's inequality equation we can get:

[0249]

[0250] Since there is a positive constant Φ L and Φ U Satisfy 0<Φ L ≤||Φ||≤ΦU , substituting (41) into (40) yields:

[0251]

[0252] in, Φ U is the norm upper bound of the matrix function Φ; similarly, we can get Ξ 4 The derivative of :

[0253]

[0254] Among them, f 2 is the attitude subsystem matrix function, g 2 is the attitude subsystem matrix function,

[0255] In order to avoid the differential explosion problem of virtual control input, a dynamic surface strategy is constructed and the sliding surface S = cΩ is defined 3 +Ξ 4 , where c>0 is a preset parameter, and the derivative of S yields:

[0256]

[0257] The actual control input τ a Designed in the following form:

[0258]

[0259] Among them, K 4 is a symmetric positive definite parameter; the Lyapunov candidate function is chosen as:

[0260]

[0261] Furthermore, W 4 is the Lyapunov candidate function, W 4 The derivative of satisfies:

[0262]

[0263] Among them, cS T ΦS≤cΦ U ||S|| 2 ,

[0264] Therefore, it can be deduced that:

[0265]

[0266] In the formula, C m >0 is the coefficient of formula (48);

[0267] Therefore, by choosing a suitable control parameter K3 ,K 4 ,c,η can make C m >0; In addition, d 1 ≈d 2 , and Q=||H|| 2 is a bounded constant; therefore, equation (48) is simplified to:

[0268]

[0269] Solving inequality (49) yields:

[0270]

[0271] Therefore, it is concluded that the tracking error, filtering error, adaptive estimation error and observation error of the attitude loop are all bounded; that is, the attitude subsystem can achieve ultimate boundedness.

[0272] MATLAB simulation verification and experimental implementation of quadrotor drone:

[0273] The following provides a simulation example and experimental verification of a quadrotor UAV subject to unknown external disturbances and parameter uncertainties, aiming to demonstrate the effectiveness of the proposed trajectory tracking control technology scheme based on fuzzy disturbance observer.

[0274] In order to prove the effectiveness and accuracy of the proposed quadrotor UAV trajectory tracking control technology based on fuzzy disturbance observer, a quadrotor UAV virtual simulation system was built, and simulation experiments and result analysis were completed on this system. The simulation parameters of the simulation experiment system were set as follows: m = 1.2 kg, g = 9.81 m / s 2 ,J x =2.4e -3 kg·m 2 ,J y =2.2e -3 kg·m 2 ,J z =2.6e -3 kg·m 2 ,l=0.3m,in addition,b=185.624e -7 N·s 2 The reference trajectory of the quadrotor drone is set as: P d =[4cos(t),4sin(t),0.5+t / (2π)] T ; The initial position is set to: [x, y, z] = [4, 0, 0.5] T ; The initial attitude angle is set to: [φ,θ,ψ] = [0,0,0] T(rad); the initial angular rate is set to: [p,q,r]T=[0,0,0]Trad / s[p,q,r]=[0,0,0] T (rad / s); the trajectory tracking controller parameters are set to: c = 0.8, K 1 =2.5,K 2 =1.6,K 3 =4,K 4 =3; the filtering time constant is: η = 0.06. In addition, the FDO parameter is set to: σ = diag (0.15) ∈ R 6×6 ,κ 1 =[0.2,0.15,0.15,0.1,0.1,0.1] T ,κ 2 =[1.6,1.6,1.6,1.6,1.6,1.6] T In order to illustrate the robust performance of the developed trajectory tracking controller, a disturbance d 1 =[0.2sin(2t),0.2cos(0.5t),0.5sin(t)] T ,d 2 =[0.2sin(t),0.5cos(0.5t),0.2sin(2t)] T .

[0275] Through the simulation experiment of the quadcopter in three-dimensional space, the simulation results and controller performance can be analyzed more intuitively. Figure 3 It can be clearly seen that the quadrotor drone is able to track the command trajectory signal. In addition, from the simulation results, it can be seen that the tracking error of the position control loop using the designed fuzzy disturbance observer is smaller than the tracking error of the commonly used control loop; the numerical results show the effectiveness of the proposed trajectory tracking algorithm.

[0276] In this specification, each embodiment is described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the embodiments can be referred to each other. For the device disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant parts can be referred to the method part.

[0277] The above description of the disclosed embodiments enables one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A quadrotor UAV trajectory tracking control method based on fuzzy disturbance observer, characterized in that: The following steps are involved: S1: Establish the dynamic model of the quadrotor drone: Analyze the dynamic characteristics of the quadrotor drone and establish the dynamic model of the quadrotor drone; S2: Establish a fuzzy disturbance observer: Combine the fuzzy disturbance observation theory to establish a fuzzy disturbance observer suitable for quadrotor UAV to provide estimation of parameter uncertainty and external disturbance; S3: Establishing the position subsystem tracking controller: Combining the fuzzy disturbance observer established in S2, correcting parameter uncertainty and external disturbances in real time, and establishing the position subsystem tracking controller using the dynamic surface control method; S4: Establish attitude subsystem tracking controller: Combine the fuzzy disturbance observer established in S2 and use the dynamic surface control method to design the attitude subsystem tracking controller.

2. The quadrotor UAV trajectory tracking control method based on fuzzy disturbance observer according to claim 1 is characterized in that: The S1 is specifically: S101: Establish a basic dynamics model for a quadrotor drone; S102: Combine parameter uncertainty and external disturbance factors to establish a quadrotor UAV dynamics model.

3. The trajectory tracking control method of a quadrotor drone based on a fuzzy disturbance observer according to claim 2 is characterized in that: The S101 is specifically: Define the body coordinate system Oxyz and the geocentric coordinate system O respectively, whose origin coincides with the center of mass of the aircraft e x e y e z e , the structure of the quadrotor drone is symmetrical relative to the planes Oxz and Oyz, so the inertia moment tensor only has diagonal inertia terms. Since the fuselage of the quadrotor drone is a rigid body, the Newton-Euler formula is used to derive nonlinear dynamics, that is, the basic dynamic model of the quadrotor drone is obtained: Where m is the mass of the quadrotor; J = diag{J x ,J y ,J z } is the inertia matrix; V b =[u,v,ω] T and Ω=[p,q,r] T are the velocity and angular velocity in the body coordinate system respectively; F is the resultant external force including rotor lift and gravity; τ a is the external torque; J x is the inertia matrix in the x-axis direction, J y is the inertia matrix in the y-axis direction, J z is the inertia matrix in the z-axis direction, u is the velocity in the x-axis direction, v is the velocity in the y-axis direction, ω is the velocity in the z-axis direction, p is the angular velocity in the x-axis direction, q is the angular velocity in the y-axis direction, and r is the velocity in the z-axis direction. Since the conversion between the body coordinate system and the geocentric coordinate system can be completed through the coordinate transformation matrix, the UAV dynamics model in the geocentric coordinate system can be expressed as: Where P = [x, y, z] T and Respectively represent the position and velocity vector in the geocentric coordinate system; f1 = -gz e is the system function, g1=1 / m is the system function, M1=TRz e is the system function, g is the acceleration due to gravity; is the total thrust generated by the four rotors, b is the thrust coefficient, ω i (i=1,2,3,4) is the speed of the rotor; d1=[d 11 ,d 12 ,d 13 ] T is a composite disturbance, including the modeling dynamic error d 11 , internal uncertainty 12 and external disturbance d 13 ; z e =(0,0,1) T is the unit vector of the geocentric coordinate system; R is the transformation matrix between the geocentric coordinate system and the body coordinate system, expressed as: Among them, θ is the roll angle, ψ is the pitch angle, and φ is the yaw angle; According to the transformation matrix relationship between the geocentric coordinate system and the body coordinate system, the transformation relationship between the attitude angles θ, ψ, φ and the flight angular rates p, q, r around the body axis is expressed as: where Θ = [φ,θ,ψ] T is the Euler angle vector, and the transformation matrix Φ(Θ) is expressed as: Among them, det(Φ)=1 / cosθ. At this time, when the pitch angle of the drone satisfies θ≠(2k-1)π / 2, k∈Z, Φ(Θ) is a reversible matrix, where k=...,-2,-1,0,1,2,... are integers and Z is a set of integers.

4. The trajectory tracking control method of a quadrotor drone based on a fuzzy disturbance observer according to claim 3 is characterized in that: The S102 is specifically: According to (2) and (6), considering the uncertainty of the aircraft model, its dynamic model can be expressed as: Where ΔJ∈R 3×3 Represents the uncertainty of the inertia matrix caused by the change of the UAV load, Δg=[Δg1,Δg2,Δg3] T is the unknown aerodynamic disturbance, where Δg1 is the aerodynamic disturbance of the roll channel, Δg2 is the aerodynamic disturbance of the pitch channel, and Δg3 is the aerodynamic disturbance of the yaw channel. is the gyro torque, J r is the moment of inertia; Control matrix equation τ a Produced by four rotors: Where l is the distance from the rotor to the center of mass, and k is the drag coefficient. Rearranging (8) yields: Where f2 = -J -1 [Ω×JΩ]-J -1 G a ,g2=J -1 ,M2=τ a And d2=[d 21 ,d 22 ,d 23 ] T is a composite disturbance, including the modeling dynamic error d 21 , internal uncertainty 22 and external disturbance d 23 , where f2 is the system function matrix, g2 is the system function matrix, and M2 is the system control matrix equation.

5. The trajectory tracking control method of a quadrotor drone based on a fuzzy disturbance observer according to claim 1, characterized in that: The S2 is specifically: S201: Establish a fuzzy disturbance observer based on fuzzy disturbance observation theory; S202: Verify whether the fuzzy disturbance observer of S201 can achieve consistent stability.

6. The quadrotor UAV trajectory tracking control method based on fuzzy disturbance observer according to claim 5 is characterized in that: The S201 is specifically: Create a fuzzy disturbance observer: Where σ is the observer design parameter matrix, is the system estimation function, f is the matrix [f1,f2], g is the matrix [g1,g2]; The adaptive law design of the i-th adjustment parameter vector is: Where Z = [Z1, Z2, ..., Z6] T is the internal state of the fuzzy disturbance observer, σ=diag[σ i ] 6×6 and σ i >0, is the estimated value of the system d(x,u), l = xZ is the observation error of the system, κ 1i >0 is the default parameter, κ 2i >0 is the preset parameter, then the disturbance observer error can achieve consistent stability, which shows that l i Can converge to a small region near zero Inside; is the i-th adaptive law of the observer, ξ i is the i-th fuzzy basis function vector, σ i is the observer design parameter, l i is the i-th observation error, L i is the positive coefficient of the i-th Lyapunov function, L i is the constant coefficient of the i-th Lyapunov function; Combining the approximation theory of fuzzy systems, unknown disturbances in is the optimal parameter vector, is the estimation error of the fuzzy system; The specific expression of is: Based on the approximation theory of fuzzy systems, the estimated error ε of the disturbance observer is confirmed i and the optimal parameter vector is bounded, which means that there is a constant Make and Established.

7. The quadrotor unmanned aerial vehicle trajectory tracking control method based on fuzzy disturbance observer according to claim 6 is characterized in that: The S202 is specifically as follows: According to l = xZ and equation (13), the dynamic change of the disturbance observer error is expressed as: in, Through the Lyapunov function calculation, we can get: V0=V1+...+V6, (15) The i-th (i=1,2,...,6) element is represented as: Among them, V i is the i-th Lyapunov function; Expanding (16) and (14) yields: Substituting (12) into (17) yields: It turns out that: Finally, it is deduced that: Among them, L i =min{σ i ,κ 2i }, Among them, L i is the positive coefficient of the i-th Lyapunov function; from (20), we can derive: Where, 0<L≤min{L1,...,L i ,...,L6}, This shows that the established fuzzy disturbance observer can achieve consistent stability; Transform formula (21) to obtain From formula (19) to (21), we can further obtain Among them, t is time, t0 is the initial time, U i is the normal constant of the i-th Lyapunov function; by setting the observer parameters, l i It can converge to a small range close to the equilibrium point.

8. The trajectory tracking control method of a quadrotor drone based on a fuzzy disturbance observer according to claim 1, characterized in that: The S3 is specifically: Define the position command vector as P d =[x d ,y d ,z d ] T , the position error is P1=PP d ; First, choose the Lyapunov alternative equation as: Among them, K1 is a symmetric positive definite parameter, W1 is a Lyapunov candidate function, and The derivative of the result is: in, is a virtual control signal, which can be Calculated; V d is the desired aircraft speed; The speed error is defined as In this case, we get The selected Lyapunov-like function is: W2 is the Lyapunov candidate function. Deriving the above formula with respect to time yields: In the formula, u T =TRz e / m is used as a virtual control signal; In order to prove that W2<0, the fuzzy disturbance observer is used to estimate the composite disturbance: Among them, K2 is a symmetric positive definite function, E∈R 3×3 is the identity matrix, U = (U1, U2, U3) T It can be expressed as: Among them, U is the matrix function of the system controller, U1, U2, and U3 are the controllers of the aircraft in the x, y, and z directions respectively; Substituting (26) into (24) yields: Among them, P1 is the position error and P2 is the speed error. Therefore, the position tracking subsystem can achieve stability; Here, equation (26) is equivalent to: It can be seen from this that: U1cosθcosψ+U2cosθsinψ-(U3+g)sinθ=0, (30) U1(sinθcosψsinφ-sinψcosφ)+U2(cosψcosφ+sinθsinψsinφ)+(U3+g)cosθsinφ=0, (31) U1(sinθcosψcosφ+sinψsinφ)+U2(cosψsinφ +sinθsinψcosφ)+(U3+g)cosθcosφ=T / m (32) According to (30) and (31), the pitch and roll attitude dynamic equations are obtained: Among them, θ d is the desired roll angle, ψ d is the desired pitch angle, φ d For the desired yaw angle, the total desired lift generated by the four rotors is obtained by using (32) as follows: T d =m[U1(sinθcosψcosφ + sinψsinφ)+U2(cosψsinφ+sinθsinψcosφ)+(U3+g)cosθcosφ]。 (35)。 9. The trajectory tracking control method of a quadrotor drone based on a fuzzy disturbance observer according to claim 1, characterized in that: The S4 is specifically: Assume that the norm Φ of the transformation matrix is ​​bounded, that is, there is a positive constant Φ L and Φ U Satisfy 0<Φ L ≤||Φ||≤Φ U ; Φ L is the norm lower bound of the matrix function Φ, Φ U is the norm upper bound of the matrix function Φ, First, define the attitude angle tracking error as Ω3 = θ-Ω d and Ω3 is the attitude angle error, Ω d is the desired attitude angle; secondly, considering the angular rate Ω as the virtual control input of the attitude angle θ, the control law can be obtained: in, is the derivative of the desired attitude angle, K3 is a symmetric and positive definite parameter matrix; A first-order low-pass filter is used to process Ω d and its derivatives Define angular rate tracking error Ξ4 = Ω - Ω d , the filter error vector η is the filter constant, then we get B=-Ω d , B is the negative desired attitude angle, taking the derivative of Ω3 we get: Lyapunov candidate function selection: W3 is the Lyapunov candidate function, and the derivative of W3 is: Assume that there are higher-order derivatives of the attitude angle command, and the parameter B and its related terms are defined on a compact set; in this case, all components of B have their maximum value H i >0,i=1,2,3 satisfies |B i |≤H i , that is, B T B≤||H|| 2 ; From Young's inequality equation we can get: Since there is a positive constant Φ L and Φ U Satisfy 0<Φ L ≤||Φ||≤Φ U , substituting (41) into (40) yields: in, Φ U is the norm upper bound of the matrix function Φ; similarly, the derivative of Ξ4 can be obtained: Among them, f2 is the matrix function of the attitude subsystem, g2 is the matrix function of the attitude subsystem, In order to avoid the differential explosion problem of virtual control input, a dynamic surface strategy is constructed, and the sliding surface S = cΩ3 + Ξ4 is defined, where c>0 is a preset parameter. The derivative of S can be obtained: The actual control input τ a Designed in the following form: Among them, K4 is a symmetric positive definite parameter; the Lyapunov candidate function is chosen as: Furthermore, W4 is a Lyapunov candidate function, and the derivative of W4 satisfies: Among them, cS T ΦS≤cΦ U ||S|| 2 , Therefore, it can be deduced that: In the formula, C m >0 is the coefficient of formula (48); Therefore, by selecting appropriate control parameters K3, K4, c, η, C m >0; In addition, we need to consider the case where d1≈d2, and Q=||H|| 2 is a bounded constant; therefore, equation (48) is simplified to: Solving inequality (49) yields: Therefore, it is concluded that the tracking error, filtering error, adaptive estimation error and observation error of the attitude loop are all bounded; that is, the attitude subsystem can achieve ultimate boundedness.

Images