Anti-disturbance and swing reduction control method for a fully state-constrained quadrotor UAV suspension system

Abstract

The present invention relates to the technical field of the control of a quadrotor UAV suspension system, and specifically relates to a disturbance rejection and swing reduction control method for a quadrotor UAV suspension system with full state constraints. Through Lagrangian modeling technology and Newton-Euler method, the constructed nonlinear model of the quadrotor UAV suspension system has stronger nonlinearity and coupling, and can be applied not only to the quadrotor UAV suspension system, but also to the quadrotor UAV system without a suspended object, with stronger applicability and more positive response; the method of dealing with disturbances by a compensation function observer can handle system uncertainty problems while dealing with disturbances, and has a wider range of applications; at the same time, considering the full state constraint problem, it has stronger robustness and wider inclusiveness.

Description

Technical Field

[0001] The present invention relates to the technical field of the control of a four-rotor UAV suspension system, and particularly relates to a disturbance rejection and swing reduction control method for a four-rotor UAV suspension system with all-state constraints. Background Art

[0002] In recent years, the application of four-rotor UAVs has become more and more extensive, such as logistics transportation, film shooting, power inspection, geographical survey, reconnaissance inspection, etc. Compared with traditional manned aircraft, four-rotor UAVs have the advantages of small size and easy operation, and also have a certain load transportation capacity.

[0003] A four-rotor suspension system suspends a four-rotor UAV and a suspended object through a hanging member such as a rope or a hook. The control purpose of the four-rotor UAV is to smoothly, safely and efficiently transport the suspended object to a specified target position through the four-rotor UAV. To achieve the control purpose, the four-rotor UAV is controlled by adjusting the output of the four-rotor UAV controller.

[0004] The four-rotor UAV suspension transportation system not only realizes aerial suspension and transportation, but also can completely isolate people from transported items, avoiding safety accidents caused by overweight suspended items during high-altitude operation in the atmosphere or ground transportation. This method does not need to consider the shape of the suspended object. Compared with the transportation method in which the load is fixed on the UAV fuselage, it avoids the problem of the mismatch between the load shape and the fuselage, and can greatly reduce the influence of the load on the moment of inertia of the UAV.

[0005] In some scenarios where a four-rotor UAV cannot land safely, such as when there is a steep slope on the ground or when carrying out mine clearance and explosive disposal work, the four-rotor UAV suspension transportation can safely drop the load from the air in a hovering state. The emergence of this device makes some operations that could only be carried out by manpower or mechanical equipment more efficient and convenient.

[0006] Although the four-rotor suspension transportation system has various advantages, there are still many challenges at present. First of all, the four-rotor suspension system itself is a complex non-linear system with multiple degrees of freedom, strong coupling and underactuation. During the transportation process, if the load swings violently, it will cause the UAV to be in an unstable state in the air. Especially in strong winds or complex environments, the swing amplitude may be more violent, threatening flight safety; at the same time, the load swing may affect the safety of the suspended object. Especially for fragile, dangerous or sensitive items, excessive swing may cause damage or failure. Secondly, different from fixed-wing UAVs, the system states such as the flight speed and attitude of rotor UAVs need to change within a reasonable range. Taking the roll angle as an example, if the roll angle of the rotor UAV is too large, it will directly lead to the overturning and even crashing of the system. Therefore, the problem of state constraints of rotor UAVs is also a practical problem faced during their flight process.

[0007] For example, the Chinese invention patent with the publication number CN110320925B provides a safety control method for a quadrotor aircraft based on a high-order disturbance observer to solve the problem that the quadrotor aircraft is vulnerable to external interference and has weak aircraft safety; however, this solution has the following three deficiencies:

[0008] First, the models studied are different. The model studied in the above solution is a simple quadrotor UAV system without non-linear and coupling models.

[0009] Second, the disturbance processing methods are different. The method of the high-order disturbance observer used in this solution has a more cumbersome design and proof process.

[0010] Finally, this solution does not consider the problem of state constraints, and state constraints will affect the system performance, so it cannot meet the actual use requirements. Summary of the Invention

[0011] The purpose of the present invention is to provide a disturbance rejection and swing reduction control method for a fully state-constrained quadrotor UAV suspension system to solve the technical problem of poor disturbance rejection and swing reduction effect of current quadrotor UAVs.

[0012] The solution of the present invention to the above technical problems:

[0013] A disturbance rejection and swing reduction control method for a fully state-constrained quadrotor UAV suspension system includes the following steps:

[0014] S1. Establish a non-linear model of the quadrotor UAV suspension system containing unknown disturbances according to the Lagrangian modeling method and the Newton-Euler equation;

[0015] S2. Generate a reference trajectory signal of the non-linear model of the quadrotor UAV suspension system by using an online trajectory planning method;

[0016] S3. Estimate the unknown disturbance in step S1 through a compensation function observer to obtain an estimated value of the disturbance term;

[0017] S4. Construct a robust adaptive swing reduction controller for the quadrotor UAV according to the barrier Lyapunov function, and compensate the robust adaptive swing reduction controller according to the estimated value obtained in step S3 to obtain an output value of the robust adaptive swing reduction controller;

[0018] S5. Realize disturbance rejection and swing reduction of the quadrotor UAV suspension system according to the reference trajectory signal obtained in step S2 and the output value obtained in step S4.

[0019] Further defined, step S1 is specifically:

[0020] Based on the Lagrangian modeling method and Newton-Euler equations, a nonlinear model of a quadrotor UAV suspension system with unknown disturbances is established:

[0021]

[0022] where \(A(\rho)\) is the symmetric inertia matrix, is the Coriolis force matrix, and \(C(\rho)\) is the gravity vector matrix;

[0023] \(\rho\) is the state vector of the quadrotor UAV suspension system, \(\rho = [x, y, z, \varphi\ s , \theta\ s \ T , where \(x\), \(y\), and \(z\) respectively represent the coordinate values of the quadrotor UAV on the corresponding coordinate axes of the ground coordinate system, \(\varphi\ s is the lateral swing angle of the suspended object around the \(x\)-axis, and \(\theta\ s is the longitudinal swing angle of the suspended object around the \(y\)-axis;

[0024] \(F\ s = [N\ s + d1, 0, 0]\ T , where \(N\ s = -R\ s e\ s T\ s + (m\ Q + m\ L )ge\ s , \(e\ s = [0, 0, 1]\ T , \(R\ s represents the rotation matrix from the body coordinate system to the ground coordinate system, \(T\ s represents the total lift force, \(d1\) represents the unknown disturbance of the position loop, \(m\ Q represents the mass of the quadrotor UAV, \(m\ L represents the mass of the suspended object, and \(g\) is the acceleration due to gravity;

[0025] \(Q\ s = [\varphi, \theta, \psi]\ T represents the actual attitude angle vector of the quadrotor UAV, \(\varphi\) is the actual roll angle of the quadrotor UAV, \(\theta\) is the actual pitch angle of the quadrotor UAV, and \(\psi\) is the actual yaw angle of the quadrotor UAV;

[0026] \(\Omega\ s = [p, q, r]\ T represents the attitude angle rate vector of the quadrotor UAV, \(p\) represents the roll rate vector of the quadrotor UAV in the body coordinate system, \(q\) represents the pitch rate vector of the quadrotor UAV in the body coordinate system, and \(r\) represents the yaw angle rate vector of the quadrotor UAV in the body coordinate system;

[0027] \(H\ srepresents the attitude kinematics matrix, J s =diag{J sx ,J sy ,J sz} represents the moment of inertia matrix, J sx , J sy and J sz are the moments of inertia of the quadrotor drone around the x-axis, y-axis, and z-axis respectively; M s represents the control torque vector, and d2 represents the unknown disturbance of the attitude loop.

[0028] It is further defined that the flight state of the quadrotor drone satisfies the following constraints:

[0029]

[0030] Where i′=1, 2, 3, Q si′ Q s The i′th element of si′ Ω s The i′th element of , u is the derivative of x, v is the derivative of y, w is the derivative of z, and are all constants.

[0031] It is further defined that step S2 comprises the following steps:

[0032] S21. The flight speed of the quadcopter is set to be constant. The terminal position of the quadcopter flying at a constant speed is described as:

[0033] x a (t) = p x t,y a (t) = p y t, z a (t) = p z t

[0034] Among them, p x 、p y and p z are constants to be designed, x a ,y a and z a are the components of the target position of the quadrotor drone in the ground coordinate system corresponding to the coordinate axis direction, and t represents time;

[0035] S22. Calculate the acceleration of the reference trajectory of the quadrotor drone according to the target position of the quadrotor drone obtained in step S21:

[0036]

[0037]

[0038]

[0039] Among them, x d , y d and z d are the components of the reference trajectory of the quadrotor UAV on the corresponding coordinate axes in the ground coordinate system, is the acceleration of x d ; is the acceleration of y d ; is the acceleration of z d ;

[0040] ρ a (·), ρ b (·) and ρ c (·) are the anti-swing components to be designed along the corresponding coordinate axes of the suspension system of the quadrotor UAV in the ground coordinate system, φ s is the actual lateral swing angle of the suspended object, θ s is the actual longitudinal swing angle of the suspended object;

[0041] S23. Construct a positive definite Lyapunov function V0(t):

[0042]

[0043] Among them, μ L represents the length between the center of mass of the suspended object and the center of mass of the quadrotor UAV, g is the acceleration due to gravity, φ s is the actual lateral swing angle of the suspended object, θ s is the actual longitudinal swing angle of the suspended object;

[0044] S24. According to the non-linear model of the quadrotor UAV suspension system established in step S1, the derivative of the positive definite Lyapunov function V0 is calculated as:

[0045]

[0046] Among them,

[0047] S25. Set ρ a (·) = β1δ f1 , ρ b (·) = β2δ f2 , ρ c (·) = β3δ f3 , and calculate the reference trajectory of the quadrotor UAV according to steps S21 - S24 as:

[0048]

[0049]

[0050]

[0051] Among them, β1, β2, and β3 are all positive constants to be designed; τ is a constant.

[0052] Further limited, the step S3 includes the following steps:

[0053] S31. By constructing a position loop compensation function observer, estimate the unknown disturbance d1 of the position loop. The position loop compensation function observer is:

[0054]

[0055]

[0056]

[0057]

[0058] Among them, x1 = ρ, x3 = A -1 (x1)d 1o , d 1o = [d1, 0, 0] T , N so = [N s , 0, 0] T , the estimated value of the unknown disturbance d1 of the position loop is 's first three elements, is the estimated value of x1, is the estimated value of x2, is the estimated value of x3, x0 is an intermediate variable, h v1 and h v2 are both positive constants to be designed, L1 and L2 are both positive definite matrices to be designed, e1 is the estimation error of x1, e2 is the estimation error of x2, e3 is the estimation error of x3,

[0059] S32. By constructing an attitude loop compensation function observer, estimate the unknown disturbance d2 of the attitude loop. The attitude loop compensation function observer is:

[0060]

[0061]

[0062]

[0063]

[0064] Where x4 = Q s , x5=Ω s , is the estimated value of x4, is the estimated value of x5, is the estimated value of x6, x k is an intermediate variable, and the estimated value of the unknown disturbance d2 in the attitude loop is h v4 and h v5 are all positive constants to be designed, L4 and L5 are all positive definite matrices to be designed, e4 is the estimated error of x4, e5 is the estimated error of x5, e6 is the estimated error of x6.

[0065] It is further defined that step S4 comprises the following steps:

[0066] S41. Constructing the virtual control law of the position loop of a quadrotor drone based on the new obstacle Lyapunov function technology sd :

[0067]

[0068] λ1=x1-ρ sd

[0069] T h1 =diag{α 11 ,α 12 ,α 13 ,α 14 ,α 15}

[0070]

[0071] Where x1 = ρ, ρ sd =[x d ,y d ,z d ,0,0] T , ρ sd is the reference signal of the quadrotor UAV suspension system, ξ1=diag{ξ 11 ,ξ 12 ,ξ 13 ,ξ 14 ,ξ 15} is the positive definite matrix to be designed, σ 1mis a positive constant to be designed, N1 = diag{N 11 , N 12 , N 13 , N 14 , N 15} is a matrix to be designed, is the i-th element in matrix N1, λ 1i is the i-th element in λ1 = x1 - ρ sd . is a positive constant to be designed and satisfies γ 1i is the generalized boundary of element λ 1i , T h1 is a matrix to be designed, α 1i is the i-th element in matrix T h1 ;

[0072] S42. Based on the new barrier Lyapunov function technique, construct a robust position loop controller for a quadrotor UAV:

[0073]

[0074] λ2 = x2 - v sd

[0075] where x1 = ρ, x3 = A -1 (x1)d 1o , is the estimated value of x3, A(x1) is the symmetric inertia matrix, B(x1, x2) is the Coriolis force matrix, C(x1) is the gravity vector matrix; ξ2 = diag{ξ 21 , ξ 22 , ξ 23 , ξ 24 , ξ 25} is a positive definite matrix to be designed, σ 2m and v s1 are positive constants to be designed, N2 = diag{N 21 , N 22 , N 23 , N 24 , N 25} is a matrix to be designed, is the i-th element in matrix N2, λ 2i is the i-th element in λ2 = x2 - v sd , is a positive constant to be designed and satisfies γ 2i is the generalized boundary of λ 2i , T h2 = diag{α 21 , α22 , α 23 , α 24 , α 25} is the i-th element in matrix T h2 , representing the output of the position loop filter ;

[0076] S43. Based on the position loop robust controller obtained in step S42, the total lift T s , desired roll angle φ d and desired pitch angle θ d of the quadrotor UAV are obtained respectively:

[0077]

[0078]

[0079]

[0080] where N so = [N s1 , N s2 , N s3 , N s4 , N s5 T , N s = -R s e s T s + (m Q + m L )ge s = [N s1 , N s2 , N s3 T , R s represents the rotation matrix from the body coordinate system to the ground coordinate system, e s = [0, 0, 1] T , m L is the mass of the suspended object, g is the acceleration due to gravity, ψ d is the given desired yaw angle of the quadrotor UAV, φ d is the desired roll angle of the quadrotor UAV, θ d is the desired pitch angle of the quadrotor UAV,

[0081] It is further specified that step S4 further includes the following steps:

[0082] ​​S44. Based on the new barrier Lyapunov function technology, construct the virtual control law Ω of the attitude loop of the quadrotor UAV sd :

[0083]

[0084] λ3 = x4 - Q sd

[0085] T h3 = diag{α 31 , α 32 , α 33}

[0086]

[0087] where x4 = Q s , ξ3 = diag{ξ 31 , ξ 32 , ξ 33} is a positive definite matrix to be designed, σ 3m is a positive constant to be designed, represents the output of the filter; N3 = diag{N 31 , N 32 , N 33} is a matrix to be designed, is the i'-th element of the matrix N3, γ 3i′ is the i'-th element of λ3 = x4 - Q sd , is a positive constant to be designed and satisfies γ 3i′ is the generalized boundary of the element λ 3i′ , Q sd = [φ d , θ d , ψ d T represents the desired attitude angle vector of the quadrotor UAV;

[0088] S45. Based on the new barrier Lyapunov function technology, construct the robust controller of the attitude loop of the quadrotor UAV:

[0089]

[0090] λ4 = x5 - Ω sd

[0091] where x5 = Ω s , is the estimated value of x6, ξ4 = diag{ξ 41 , ξ 42 , ξ​43} is a positive definite matrix to be designed, N4 = diag{N 41 , N 42 , N 43} is a matrix to be designed, σ 4m and ν s2 are both positive constants to be designed, is the i'-th element in matrix N4, λ 4i′ is the i'-th element of λ4 = x5 - Ω sd , is a positive constant to be designed and satisfies γ 4i′ is the generalized boundary of λ 4i′ , T h4 = diag{α 41 , α 42 , α 43} is a matrix to be designed, is the i'-th element in matrix T h4 , represents the output of the attitude loop filter.

[0092] An electronic device includes a memory and a processor. The memory is used to store a computer program, and the processor runs the computer program to cause the electronic device to execute the anti-disturbance and anti-sway control method for a fully state-constrained quadrotor UAV suspension system according to the above.

[0093] A computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, it implements the anti-disturbance and anti-sway control method for a fully state-constrained quadrotor UAV suspension system according to the above.

[0094] A computer program product includes a computer program, and when the computer program is executed by a processor, it implements the anti-disturbance and anti-sway control method for a fully state-constrained quadrotor UAV suspension system according to the above.

[0095] The beneficial effects of the present invention are as follows:

[0096] Through Lagrangian modeling technology and Newton-Euler method, the constructed nonlinear model of the quadrotor UAV suspension system has stronger nonlinearity and coupling. It can be applied to both the quadrotor UAV suspension system and the quadrotor UAV system without a suspended object, with stronger applicability and more active response. The method of dealing with interference through a compensation function observer can handle system uncertainty problems while dealing with interference, and has a wider range of applications. At the same time, considering the fully state-constrained problem, it has stronger robustness and wider inclusiveness. Description of the Drawings

[0097] Figure 1This is the flowchart of the anti-disturbance and anti-swing control method for the suspension system of a fully state-constrained quadrotor UAV in the present invention;

[0098] Figure 2 This is a schematic diagram for comparing the changes in the lateral swing angle of the suspended object around the x-axis when the present invention is respectively used without a controller and with a conventional PID controller;

[0099] Figure 3 This is a schematic diagram for comparing the changes in the lateral swing angle of the suspended object around the y-axis when the present invention is respectively used without a controller and with a conventional PID controller;

[0100] Figure 4 This is a schematic diagram for comparing the tracking error curves of the roll angle of the UAV between the present invention and a conventional PID controller;

[0101] Figure 5 This is a schematic diagram for comparing the tracking error curves of the pitch angle of the UAV between the present invention and a conventional PID controller;

[0102] Figure 6 This is a schematic diagram for comparing the tracking error curves of the yaw angle of the UAV between the present invention and a conventional PID controller. Detailed implementation manners

[0103] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0104] Among them, it is assumed that the hanging part of the quadrotor UAV for hanging an object is rigid and has a constant length, and the swing amplitude of the swing angle of the suspended object is between (-90°, 90°).

[0105] Embodiment 1

[0106] Refer to Figure 1 , this embodiment provides an anti-disturbance and anti-swing control method for the suspension system of a fully state-constrained quadrotor UAV, including the following steps:

[0107] S1. Establish a nonlinear model of the quadrotor UAV suspension system including unknown disturbances;

[0108] S2. Generate a reference trajectory signal for the nonlinear model of the quadrotor UAV suspension system by using an online trajectory planning method;

[0109] S3. Estimate the unknown disturbances in step S1 through a compensation function observer to obtain an estimated value of the disturbance term;

[0110] S4. Construct a robust adaptive anti-sway controller for the quadrotor UAV based on the barrier Lyapunov function, and compensate the robust adaptive anti-sway controller according to the estimated value obtained in step S3 to obtain the output value of the robust adaptive anti-sway controller;

[0111] S5. Compensate according to the reference trajectory signal obtained in step S2 and the output value obtained in step S4 to achieve anti-disturbance and anti-sway of the quadrotor UAV suspension system.

[0112] Further explanation: Step S1 is specifically as follows:

[0113] Establish a nonlinear model of the quadrotor UAV suspension system containing unknown disturbances according to the Lagrangian modeling method and the Newton-Euler equation:

[0114]

[0115]

[0116]

[0117] Among them, A(ρ) is a symmetric inertia matrix, is the Coriolis force matrix, and C(ρ) is the gravity vector matrix;

[0118] ρ is the state vector of the quadrotor UAV suspension system, ρ = [x, y, z, φ s , θ s T , where x, y, and z respectively represent the coordinate values of the quadrotor UAV on the corresponding coordinate axes of the ground coordinate system, φ s is the lateral swing angle of the suspended object around the x-axis, and θ s is the longitudinal swing angle of the suspended object around the y-axis, is the first derivative of ρ, is the second derivative of ρ.

[0119] F s = [N s + d1, 0, 0] T , N s = -R s e s T s + (m Q + m L )ge s , e s = [0, 0, 1] T , R s represents the rotation matrix from the body coordinate system to the ground coordinate system, T s represents the total lift force, d1 represents the unknown disturbance of the position loop, m Q represents the mass of the quadrotor UAV, m​L represents the mass of the suspended object, and \(g\) is the acceleration due to gravity.

[0120] Q s = [φ, θ, ψ] T represents the actual attitude angle vector of the quadrotor UAV, where φ is the actual roll angle of the quadrotor UAV, θ is the actual pitch angle of the quadrotor UAV, and ψ is the actual yaw angle of the quadrotor UAV.

[0121] Ω s = [p, q, r] T represents the attitude angle rate vector of the quadrotor UAV, where \(p\) represents the roll rate vector of the quadrotor UAV in the body coordinate system, \(q\) represents the pitch rate vector of the quadrotor UAV in the body coordinate system, and \(r\) represents the yaw angle rate vector of the quadrotor UAV in the body coordinate system.

[0122] H s represents the attitude kinematic matrix, \(J\) s = diag{J sx , J sy , J sz} represents the inertia matrix, \(J\) sx 、J sy and \(J\) sz are the moments of inertia of the quadrotor UAV about the \(x\)-axis, \(y\)-axis, and \(z\)-axis respectively; \(M\) s represents the control torque vector, and \(d_2\) represents the unknown disturbance of the attitude loop.

[0123] Specifically, C(ρ) = [0 0 C 13 C 14 C 15 T .

[0124] Among them, \(A\) 11 = \(A\) 22 = \(A\) 33 = \(m\) Q + \(m\) L , C 13 = (\(m\) Q + \(m\) L )g, ​

[0125] Furthermore, the flight state of the quadrotor UAV satisfies the following constraints:

[0126]

[0127] where \(i' = 1, 2, 3\), \(Q\) si′ is the \(i'\)-th element of \(Q\), \(\Omega\) s is the \(i'\)-th element of \(\Omega\), \(u\) is the derivative of \(x\), \(v\) is the derivative of \(y\), \(w\) is the derivative of \(z\), si′ is \(\Omega\) s and and are both constants.

[0128] Meanwhile, the following assumptions are needed:

[0129] Assumption 1: There exist positive constants \(d\) q , \(d\) w , \(d\) e and \(d\) r satisfying the following inequalities:

[0130]

[0131] Assumption 2: For the desired trajectories \(x\) d , \(y\) d , \(z\) d and \(\psi\) d , there exists a positive constant satisfying and

[0132] where \(x\) d , \(y\) d and \(z\) d are the components of the reference trajectory of the quadrotor UAV on the corresponding coordinate axes in the ground coordinate system, and \(\psi\) d is the desired yaw angle of the quadrotor UAV.

[0133] Assumption 3: For the desired trajectory vector \(P\) d = \([x\) d , \(y\) d , \(z\) d , \(\psi\) d T , there exists a positive constant satisfying and In addition, for the desired trajectory vector \(P\) d = \([x\) d ​, y d , z d , ψ d T , there exists a positive constant such that where Y0 is a compact set.

[0134] Furthermore, step S2 includes the following steps:

[0135] S21. Set the flight speed of the quadrotor UAV to be constant. Then, the end position of the quadrotor UAV flying at a constant speed, i.e., the target position, is described as:

[0136] x a (t) = p x t, y a (t) = p y t, z a (t) = p z t

[0137] where p x , p y and p z are all constants to be designed, and x a , y a and z a are the components of the target position of the quadrotor UAV in the corresponding axis directions of the ground coordinate system respectively, and t represents time.

[0138] S22. According to the target position of the quadrotor UAV obtained in step S21, obtain the acceleration of the reference trajectory of the quadrotor UAV by taking the second derivative of the desired trajectory of the quadrotor UAV:

[0139]

[0140]

[0141]

[0142] where x d , y d and z d are the components of the reference trajectory of the quadrotor UAV on the corresponding coordinate axes in the ground coordinate system respectively, is the acceleration of x d , is the acceleration of y d , is the acceleration of z d .

[0143] ρ a (·), ρ b (·) and ρ c ​(·) are the anti-swing components to be designed along the corresponding coordinate axes of the quadrotor UAV suspension system in the ground coordinate system, φ s is the actual lateral swing angle of the suspended object, θ s is the actual longitudinal swing angle of the suspended object.

[0144] Combined with the control objective, the desired tracking task and can be converted into the following form: and

[0145] Obviously, the key to the validity of this conversion process is to design reasonable and effective functions ρ a (·), ρ b (·) and ρ c (·).

[0146] S23. Construct a positive definite Lyapunov function V0(t):

[0147]

[0148] where μ L represents the length between the centroid of the suspended object and the centroid of the quadrotor UAV, g is the acceleration due to gravity, φ s is the actual lateral swing angle of the suspended object, θ s is the actual longitudinal swing angle of the suspended object.

[0149] S24. According to the nonlinear model of the quadrotor UAV suspension system established in step S1, the derivative of the positive definite Lyapunov function V0 is calculated as:

[0150]

[0151] where, φ s is the actual lateral swing angle of the suspended object, θ s is the actual longitudinal swing angle of the suspended object.

[0152] S25. To ensure the convergence of the positive definite Lyapunov function V0(t),

[0153] set ρ a (·) = β1δ f1 , ρ b (·) = β2δ f2 , ρ c (·) = β3δ f3 , and calculate the reference trajectory of the quadrotor UAV according to steps S21 - S24 as:

[0154]

[0155]

[0156]

[0157] Among them, β1, β2, and β3 are all positive constants to be designed; τ is a constant.

[0158] Furthermore, by taking the second-order derivatives of x d , y d and z d respectively, we can obtain:

[0159]

[0160]

[0161]

[0162] Combining with the acceleration of the reference trajectory of the quadrotor UAV and substituting and into the derivative of the positive definite Lyapunov function V0 in step S24, we get:

[0163]

[0164] Among them, β m = min{β1, β2, β3},

[0165] Therefore, it can be seen that V0 is a monotonically decreasing function, and its convergence speed depends on the value of β m . According to Lyapunov theory, it can be seen that V0 will converge to 0 over time, that is, the swing angle of the suspended load is convergent. Therefore, the online trajectory planning generated by step S2 can effectively suppress the swing angle of the suspended object and achieve the effect of reducing the swing of the suspended object.

[0166] Further explanation, step S3 includes the following steps:

[0167] S31. By constructing a position loop compensation function observer, estimate the unknown disturbance d1 of the position loop. The position loop compensation function observer is:

[0168]

[0169]

[0170]

[0171]

[0172] Among them, x1 = ρ, x3 = A -1 (x1)d 1o , d 1o = [d1, 0, 0] T , N so = [N s , 0, 0] T , the estimated value of the unknown disturbance d1 of the position loop is the first three elements of is the estimated value of x1, is the estimated value of x2, is the estimated value of x3, x0 is an intermediate variable, h v1 and h v2 are both positive constants to be designed, L1 and L2 are both positive definite matrices to be designed, e1 is the estimation error of x1, e2 is the estimation error of x2, e3 is the estimation error of x3,

[0173] Specifically, define x1 = ρ and It can be obtained from the nonlinear model of the quadrotor UAV suspension system:

[0174]

[0175]

[0176] Furthermore, since there exists a positive constant A m such that ||A -1 (ρ)|| ≤ A m holds, so can be expressed as D1 = A -1 (x1)d 1o .

[0177] According to Assumption 2, it can be known that D1 is bounded, then let D1 = x3, and define Then it can be obtained:

[0178]

[0179]

[0180] Thus, according to the above formula, the position loop compensation function observer can be obtained as:

[0181]

[0182]

[0183]

[0184]

[0185] h v1 > 0 and h v2 > 0 are positive constants, L1 ∈ R 5×5 and L2 ∈ R 5×5 are designed positive definite matrices.

[0186] Define and Then, take the derivatives of e1, e2, and e3 with respect to time respectively to obtain:

[0187]

[0188]

[0189]

[0190] By definition it can be obtained that:

[0191]

[0192] where the subscripts 5×5 and 5×1 represent the dimensions of the corresponding matrices or vectors, 0 5×5 and I 5×5 represent the zero matrix and the identity matrix respectively. Since k d is bounded, thus k e is also bounded.

[0193] Here, appropriate parameters can be selected to ensure that ζ e is a Hurwitz matrix, that is, there exists a positive definite matrix P e , such that holds, where Q e is a positive definite matrix.

[0194] Choose the Lyapunov function as follows:

[0195]

[0196] According to and take the derivative of V1 to obtain:

[0197]

[0198] where, ω1 = λ min (Q e-I 15×15 ),k dm =||P e k e || 2 。

[0199] S32. Estimate the unknown disturbance d2 of the attitude loop by constructing an attitude loop compensation function observer. The attitude loop compensation function observer is:

[0200]

[0201]

[0202]

[0203]

[0204] where x4 = Q s ,x5 = Ω s , is the estimated value of x4, is the estimated value of x5, is the estimated value of x6, x k is an intermediate variable. The estimated value of the unknown disturbance d2 of the attitude loop is h v4 and h v5 are both positive constants to be designed, and L4 and L5 are both positive definite matrices to be designed. e4 is the estimation error of x4, e5 is the estimation error of x5, e6 is the estimation error of x6.

[0205] Specifically, define x4 = Q s and x5 = Ω s , then the attitude loop of the quadrotor UAV suspension system can be expressed as:

[0206]

[0207]

[0208] where,

[0209] Define D2 = x6 and obtain Therefore, the attitude loop compensation function observer can be obtained from the above formula as:

[0210]

[0211]

[0212]

[0213]

[0214] where \(L4\in R\) 5×5 and \(L5\in R\) 5×5 are positive definite matrices to be designed.

[0215] Define and Then, by taking the derivatives of \(e4\), \(e5\), and \(e6\), we get:

[0216]

[0217]

[0218]

[0219] Define We can obtain:

[0220]

[0221] where Similarly, appropriate parameters can be chosen to ensure that \(Z\) f is a Hurwitz matrix, that is, there exists a positive definite matrix \(P\) f such that holds, where \(Q\) f is the designed positive definite matrix.

[0222] Choose the Lyapunov function as follows:

[0223]

[0224] According to Taking the derivative of \(V4\) gives:

[0225]

[0226] where \(\omega2 = \lambda\) min \((Q\) f -I\) 9×9 - 2\(\Psi2I\) 9×9 ), \(\|P\) f k\) D \| = \(\Psi1\), \(\|P\) 2 Z\) f \| \(\leq \Psi2\), \(\lambda\) k is the minimum eigenvalue of the matrix \((Q\) min -I\) f - 2\(\Psi2I\) 9×9 -I\) 9×9 ), \(\Psi2\) is a positive constant, and \(\Psi1\) is a positive constant, \(I\)9×9 The subscript 9×9 represents the dimension of the corresponding matrix or vector.

[0227] Design the position-loop compensation function observer and the attitude-loop compensation function observer to estimate the unknown disturbances existing in the position loop and the attitude loop, obtain the estimated values of the disturbances, and compensate the controller.

[0228] Further explanation: Step S4 includes the following steps:

[0229] S41. Based on the new barrier Lyapunov function technique, construct the virtual control law v of the quadrotor UAV sd :

[0230]

[0231] λ1 = x1 - ρ sd

[0232] T h1 = diag{α 11 , α 12 , α 13 , α 14 , α 15}

[0233]

[0234] where x1 = ρ, ρ sd = [x d , y d , z d , 0, 0] T , ρ sd is the reference signal of the quadrotor UAV suspension system, ξ1 = diag{ξ 11 , ξ 12 , ξ 13 , ξ 14 , ξ 15} is a positive definite matrix to be designed, σ 1m is a positive constant to be designed, N1 = diag{N 11 , N 12 , N 13 , N 14 , N 15} is a matrix to be designed, is the i-th element in the matrix N1, λ 1i is the i-th element in λ1 = x1 - ρ sd , is a positive constant to be designed and satisfies γ 1i is the generalized boundary of the element λ 1i , T h1is the matrix to be designed, and α 1i is the i-th element in matrix T h1 .

[0235] Specifically, the robust adaptive anti-sway controller of the quadrotor UAV includes a position loop controller. Before constructing the robust adaptive anti-sway controller of the quadrotor UAV, the following lemmas are given first:

[0236] Lemma 1: For any constant h s > 0 and variable function h m ∈ R, satisfying the following two inequalities:

[0237]

[0238] 0 ≤ |h m | - h m tanh(h m / h s ) ≤ h n h s

[0239] h n = 0.2785

[0240] Lemma 2: For any positive function satisfying and variable function if there exists a positive number satisfying and then the following inequality holds:

[0241]

[0242] According to x1 = ρ, and the position tracking error λ1 and the velocity tracking error λ2 are defined as follows:

[0243] λ1 = x1 - ρ sd

[0244] λ2 = x2 - v sd

[0245] where ρ sd = [x d , y d , z d , 0, 0] T is the desired position signal of the quadrotor UAV, and v sd is the designed position virtual control law.

[0246] Differentiating λ1 = x1 - ρ sd gives To solve the problem of flight state constraints, a virtual control law v is designed sd For

[0247] According to and Calculate to obtain

[0248] Construct the Lyapunov function as follows:

[0249]

[0250] where, log(·) represents the natural logarithm.

[0251] Taking the derivative of V2 with respect to time gives:

[0252]

[0253] where, M1 = diag{M 11 , M 12 , M 13 , M 14 , M 15}, is the i-th element in the matrix M1,

[0254] From it can be deduced that -M 1i < -N 1i , so the following inequality holds:

[0255]

[0256] Meanwhile, by defining and According to Lemma 2, the following conclusion can be obtained:

[0257]

[0258]

[0259] where, is the i-th element in the matrix T m1 in.

[0260] According to Lemma 1, it can be obtained:

[0261]

[0262]

[0263] Furthermore, it can be obtained where h n= 0.2785.

[0264] For λ2 = x2 - v sd Take the derivative to obtain To obtain an available Let v sd Pass through the following first-order filter τ2(0) = v sd (0), where σ2 = diag{σ 21 , σ 22 , σ 23 , σ 24 , σ 25} is the designed positive definite matrix.

[0265] Define f2 = τ2 - v sd as the filter error, and take the derivative of f2 to get where is a vector of smooth functions on the compact set .

[0266] According to Assumption 3, it can be obtained that under the given initial conditions, there exists a positive Γ 2m such that ||Γ2(·)|| ≤ Γ 2m holds.

[0267] Thus, based on τ2(0) = v sd (0) and construct the position loop robust controller.

[0268] S42. Based on the new barrier Lyapunov function technique, construct the position loop robust controller of the quadrotor UAV:

[0269]

[0270] λ2 = x2 - v sd

[0271] where x1 = ρ, x3 = A -1 (x1)d 1o , is the estimate of x3, A(x1) is the symmetric inertia matrix, B(x1, x2) is the Coriolis force matrix, C(x1) is the gravity vector matrix; ξ2 = diag{ξ 21 , ξ 22 , ξ 23 , ξ 24 , ξ 25} is the positive definite matrix to be designed, σ 2m and v s1 are the positive constants to be designed, N2 = diag{N 21,N 22 ,N 23 ,N 24 ,N 25} is the matrix to be designed, is the i-th element in matrix N2, λ 2i is λ2 = x2 - v sd 's i-th element, is a positive constant to be designed and satisfies γ 2i is the generalized boundary of λ 2i , T h2 = diag{α 21 , α 22 , α 23 , α 24 , α 25}, is the i-th element in matrix T h2 , represents the output of the position loop filter.

[0272] S43, the position loop robust controller obtained according to step S42, respectively obtain the total lift force T of the quadrotor UAV s , the desired roll angle φ d and the desired pitch angle θ d :

[0273]

[0274]

[0275]

[0276] where, N so = [N s1 , N s2 , N s3 , N s4 , N s5 T , N s = -R s e s T s + (m Q + m L )ge s = [N s1 , N s2 , N s3 T , R s represents the rotation matrix from the body coordinate system to the ground coordinate system, e s = [0, 0, 1] T , m L ​​is the mass of the hanging object, g is the acceleration due to gravity, ψ d is the desired yaw angle given to the quadrotor UAV, φ d is the desired roll angle of the quadrotor UAV, θ d is the desired pitch angle of the quadrotor UAV,

[0277]

[0278]

[0279] Specifically, from the above two equations, we can obtain:

[0280]

[0281] Select the following Lyapunov function:

[0282]

[0283] Take the derivative of V3 with respect to time:

[0284]

[0285] where, M2 = diag{M 21 , M 22 , M 23 , M 24 , M 25}, is the i-th element in M2.

[0286] Similarly, from it can be seen that the following inequality based on Lemma 2 holds:

[0287]

[0288]

[0289] where, is the i-th element in T m2 .

[0290] According to and it can be obtained that:

[0291]

[0292] According to Lemma 1, it can be obtained that Therefore, it can be obtained that

[0293] From N so = [N s1 , N s2 , N s3 , N s4 , N s5 T and N s = -R s e s T s + (m Q + m L )ge s = [N s1 , N s2 , N s3 T , the total lift force T of the quadrotor UAV can be calculated s , the desired roll angle φ of the quadrotor UAV d and the desired pitch angle θ of the quadrotor UAV d .

[0294] The robust adaptive anti-sway controller of the quadrotor UAV also includes an attitude loop controller.

[0295] Furthermore, step S4 also includes the following steps:

[0296] S44. Based on the new barrier Lyapunov function technique, construct the attitude loop virtual control law Ω of the quadrotor UAV sd :

[0297]

[0298] λ3 = x4 - Q sd

[0299] T h3 = diag{α 31 , α 32 , α 33}

[0300]

[0301] where x4 = Q s , ξ3 = diag{ξ 31 , ξ 32 , ξ 33} is a positive definite matrix to be designed, σ 3m is a positive constant to be designed, represents the output of the filter; N3 = diag{N 31 , N 32 , N 33} is a matrix to be designed, ​​is the \(i'\)-th element in matrix \(N_3\), \(\gamma\) 3i′ is the \(i'\)-th element of \(\lambda_3 = x_4 - Q\) sd , is a positive constant to be designed and satisfies \(\gamma\) 3i′ is the generalized boundary of the element \(\lambda\) 3i′ , \(Q\) sd = [\(\varphi\) d , \(\theta\) d , \(\psi\) d T represents the desired attitude angle vector of the quadrotor UAV.

[0302] Specifically, according to the attitude loop of the quadrotor UAV, the attitude loop tracking errors \(\lambda_3\) and \(\lambda_4\) are defined as follows:

[0303] \(\lambda_3 = x_4 - Q\) sd , \(\lambda_4 = x_5 - \Omega\) sd

[0304] where, \(Q\) sd = [\(\varphi\) d , \(\theta\) d , \(\psi\) d T is the desired attitude angle, and \(\Omega\) sd is the designed virtual control law.

[0305] Taking the derivative of \(\lambda_3 = x_4 - Q\) sd yields Pass \(Q\) sd through the following first-order filter,[[]] \(\tau_3(0) = Q\) sd (0), where, \(\sigma_3 = diag\{\sigma\) 31 , \(\sigma\) 32 , \(\sigma\) 33} is a positive definite matrix to be designed.

[0306] Define \(f_3 = \tau_3 - Q\) sd and take the derivative of \(f_3\), we get where \(\Gamma_3(\cdot)\) is a smooth function vector on the compact set . Obviously, for the given initial conditions, there exists a positive \(\Gamma\) 3m such that \(\|\Gamma_3(\cdot)\| \leq \Gamma\) 3m holds.

[0307] Thus, design the virtual control law \(\Omega\) sd as

[0308] Substitute the virtual control law into and we can obtain

[0309] ​​Select the appropriate Lyapunov function as follows:

[0310]

[0311] Take the derivative of V5 with respect to time:

[0312]

[0313] where, M3 = diag{M 31 ,M 32 ,M 33},

[0314] Similarly, the following inequality holds:

[0315]

[0316]

[0317] where,

[0318] Therefore, it can be further obtained that:

[0319]

[0320] Take the derivative of λ4 = x5 - Ω sd , and it can be obtained that Let Ω sd pass through the following first-order filter:

[0321] τ4(0) = Ω sd (0)

[0322] where, σ4 = diag{σ 41 ,σ 42 ,σ 43} is the designed positive diagonal matrix.

[0323] Define f4 = τ4 - Ω sd , and by taking the derivative of f4, it can be obtained that:

[0324]

[0325] where, is a vector of smooth functions on the compact set .

[0326] It can be seen from Assumption 3 that for the given initial conditions, there exists a positive Γ 4m , such that ||Γ4(·)|| ≤ Γ 4m

[0327] Thus, based on and a robust attitude loop controller is constructed.

[0328] S45. Based on the new barrier Lyapunov function technique, construct a robust attitude loop controller for a quadrotor UAV:

[0329]

[0330] λ4 = x5 - Ω sd

[0331] where x5 = Ω s , is the estimated value of x6, ξ4 = diag{ξ 41 , ξ 42 , ξ 43} is a positive definite matrix to be designed, N4 = diag{N 41 , N 42 , N 43} is a matrix to be designed, σ 4m and ν s2 are both positive constants to be designed, is the i'-th element in matrix N4, λ 4i′ is the i'-th element of λ4 = x5 - Ω sd , is a positive constant to be designed and satisfies γ 4i′ is the generalized bound of λ 4i′ , T h4 = diag{α 41 , α 42 , α 43} is a matrix to be designed, is the i'-th element in matrix T h4 , represents the output of the attitude loop filter.

[0332] Substitute the robust attitude loop controller into to obtain:

[0333]

[0334] Select the Lyapunov function as Differentiate V6 to get:

[0335]

[0336] where M4 = diag{M 41 , M 42 , M 43},

[0337] Considering the following fact:

[0338]

[0339]

[0340] where Furthermore, we obtain

[0341] Referring to Figure 2 and Figure 3 , where φ s is the lateral swing angle of the suspended object around the x-axis, θ s is the longitudinal swing angle of the suspended object around the y-axis, φ sw represents the lateral swing angle change curve of the suspended object around the x-axis without the action of the controller, φ sd represents the lateral swing angle change curve of the suspended object around the x-axis under the action of the robust adaptive anti-swing controller constructed according to the above steps, φ sp represents the lateral swing angle change curve of the suspended object around the x-axis under the action of the traditional PID controller.

[0342] θ sw represents the longitudinal swing angle change curve of the suspended object around the y-axis without the action of the controller, θ sd represents the longitudinal swing angle change curve of the suspended object around the y-axis under the action of the robust adaptive anti-swing controller constructed according to the above steps, θ sp represents the longitudinal swing angle change curve of the suspended object around the y-axis under the action of the traditional PID controller.

[0343] It can be clearly seen by comparison that when no control is applied to the suspended object, the swing angles of the suspended object on the x-axis and y-axis will become larger and larger, which is not conducive to the stability of the system; while when control is applied to the suspended object, the anti-disturbance and anti-swing response of the suspended object is faster and more obvious through the anti-disturbance and anti-swing control method of the quadrotor UAV suspension system with full-state constraints.

[0344] Referring to Figure 4 , where λ 31 represents the tracking error curve of the roll angle of the UAV under the action of different controllers; λ 31d represents the tracking error curve of the roll angle of the UAV under the action of the anti-disturbance and anti-swing control method of the quadrotor UAV suspension system with full-state constraints, λ 31p represents the tracking error curve of the roll angle of the UAV under the action of the conventional PID controller.

[0345] Referring to Figure 5 , where λ 32Shows the tracking error curve of the pitch angle of the UAV under the action of different controllers; λ 32d Shows the tracking error curve of the pitch angle of the UAV under the action of the anti-disturbance and anti-sway control method of the fully state-constrained quadrotor UAV suspension system, λ 32p Shows the tracking error curve of the pitch angle of the UAV under the action of a conventional PID controller.

[0346] Reference Figure 6 where λ 33 Shows the tracking error curve of the yaw angle of the UAV under the action of different controllers; λ 33d Shows the tracking error curve of the yaw angle of the UAV under the action of the anti-disturbance and anti-sway control method of the fully state-constrained quadrotor UAV suspension system, λ 33p Shows the tracking error curve of the yaw angle of the UAV under the action of a conventional PID controller.

[0347] Obviously, compared with the traditional PID control method, for the anti-disturbance and anti-sway control method of the fully state-constrained quadrotor UAV suspension system, the convergence errors of the roll angle, attitude angle and yaw angle under the action of the controller using this method are smaller and the convergence speed is faster, meeting the actual usage requirements.

[0348] Embodiment 2

[0349] This embodiment also provides an electronic device, including a memory and a processor. The memory is used to store a computer program, and the processor runs the computer program to enable the electronic device to execute the anti-disturbance and anti-sway control method of the fully state-constrained quadrotor UAV suspension system according to Embodiment 1.

[0350] This embodiment also provides a computer-readable storage medium. The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, it implements the anti-disturbance and anti-sway control method of the fully state-constrained quadrotor UAV suspension system of Embodiment 1.

[0351] This embodiment also provides a computer program product. The computer program product includes a computer program, and when the computer program is executed by a processor, it implements the anti-disturbance and anti-sway control method of the fully state-constrained quadrotor UAV suspension system of Embodiment 1.

[0352] The above embodiments and the specific parameters in the embodiments are only for clearly expressing the verification process of the invention, and are not used to limit the patent protection scope of the present invention. The patent protection scope of the present invention still takes its claims as the criterion. All equivalent structural changes made by using the content of the specification and drawings of the present invention should equally be included in the protection scope of the present invention.

Claims

1. A fully state-constrained anti-disturbance and anti-sway control method for a quad-rotor UAV suspension system, characterized in that: The following steps are involved: S1. Based on the Lagrangian modeling method and Newton-Euler equation, a nonlinear model of the quadrotor UAV suspension system with unknown interference is established. S2. Generate a reference trajectory signal of the nonlinear model of the quadrotor UAV suspension system using an online trajectory planning method; S3, estimating the unknown interference in step S1 through a compensation function observer to obtain an estimated value of the interference term; The step S3 comprises the following steps: S31, estimating the unknown disturbance d1 of the position loop by constructing a position loop compensation function observer, wherein the position loop compensation function observer is: Where x1 = ρ, x3=A -1 (x1)d 1o , d 1o =[d1,0,0] T , N so =[N s ,0,0] T , the estimated value of the unknown disturbance d1 in the position loop is The first three elements of is the estimated value of x1, is the estimated value of x2, is the estimated value of x3, x0 is the intermediate variable, h v1 and h v2 are all positive constants to be designed, L1 and L2 are all positive definite matrices to be designed, e1 is the estimated error of x1, e2 is the estimated error of x2, e3 is the estimated error of x3, S32, estimating the unknown disturbance d2 of the attitude loop by constructing an attitude loop compensation function observer, wherein the attitude loop compensation function observer is: Where x4 = Q s , x5=Ω s , is the estimated value of x4, is the estimated value of x5, is the estimated value of x6, x k is an intermediate variable, and the estimated value of the unknown disturbance d2 in the attitude loop is h v4 and h v5 are all positive constants to be designed, L4 and L5 are all positive definite matrices to be designed, e4 is the estimated error of x4, e5 is the estimated error of x5, e6 is the estimated error of x6; S4, constructing a robust adaptive sway reduction controller for the quadrotor drone according to the obstacle Lyapunov function, compensating the robust adaptive sway reduction controller according to the estimated value obtained in step S3, and obtaining an output value of the robust adaptive sway reduction controller; S5. According to the reference trajectory signal obtained in step S2 and the output value obtained in step S4, anti-disturbance and swing reduction of the hanging system of the quadrotor UAV is realized.

2. The anti-disturbance and anti-sway control method for the fully state-constrained quad-rotor UAV suspension system according to claim 1 is characterized in that: The step S1 is specifically as follows: According to the Lagrangian modeling method and Newton-Euler equation, a nonlinear model of the quadrotor UAV suspension system with unknown interference is established: Among them, A(ρ) is the symmetric inertia matrix, is the Coriolis force matrix, C(ρ) is the gravity vector matrix; ρ is the state vector of the quadrotor UAV suspension system, ρ = [x, y, z, φ s ,θ s ] T , x, y and z represent the coordinate values ​​of the quadrotor drone on the corresponding coordinate axes of the ground coordinate system, φ s is the lateral swing angle of the hanging object around the x-axis, θ s is the longitudinal swing angle of the hanging object around the y-axis; F s =[N s +d1,0,0] T , N s =-R s e s T s +(m Q +m L )ge s , e s =[0,0,1] T , R s Represents the rotation matrix from the body coordinate system to the ground coordinate system, T s represents the total lift, d1 represents the unknown disturbance of the position loop, m Q Represents the mass of the quadrotor drone, m L It represents the mass of the hanging object, and g is the acceleration due to gravity; Q s =[φ,θ,ψ] T represents the actual attitude angle vector of the quadrotor drone, φ is the actual roll angle of the quadrotor drone, θ is the actual pitch angle of the quadrotor drone, and ψ is the actual yaw angle of the quadrotor drone; Ω s =[p,q,r] T represents the attitude angular rate vector of the quadrotor drone, p represents the roll rate vector of the quadrotor drone in the body coordinate system, q represents the pitch rate vector of the quadrotor drone in the body coordinate system, and r represents the yaw angular rate vector of the quadrotor drone in the body coordinate system; H s represents the attitude kinematics matrix, J s =diag{J sx ,J sy ,J sz } represents the moment of inertia matrix, J sx , J sy and J sz are the moments of inertia of the quadrotor drone around the x-axis, y-axis, and z-axis respectively; M s represents the control torque vector, and d2 represents the unknown disturbance of the attitude loop.

3. The anti-disturbance and anti-sway control method for the fully state-constrained quad-rotor UAV suspension system according to claim 2 is characterized in that: The flight state of the quadrotor drone satisfies the following constraints: Where i′=1, 2, 3, Q si′ Q s The i′th element of si′ Ω s The i′th element of , u is the derivative of x, v is the derivative of y, w is the derivative of z, and are all constants.

4. The anti-disturbance and anti-sway control method for the fully state-constrained quad-rotor UAV suspension system according to claim 3 is characterized in that: The step S2 comprises the following steps: S21. The flight speed of the quadcopter is set to be constant. The terminal position of the quadcopter flying at a constant speed is described as: x a (t)=p x t,y a (t)=p y t,z a (t)=p z t Among them, p x 、p y and p z are constants to be designed, x a ,y a and z a are the components of the target position of the quadrotor drone in the ground coordinate system corresponding to the coordinate axis direction, and t represents time; S22. Calculate the acceleration of the reference trajectory of the quadrotor drone according to the target position of the quadrotor drone obtained in step S21: Among them, x d ,y d and z d are the components of the reference trajectory of the quadrotor drone on the corresponding coordinate axes in the ground coordinate system, For x d The acceleration of for y d The acceleration of For z d acceleration; ρ a (·),ρ b (·) and ρ c (·) are the anti-sway components to be designed along the corresponding coordinate axes of the quadrotor UAV suspension system in the ground coordinate system, φ s is the actual lateral swing angle of the hanging object, θ s is the actual longitudinal swing angle of the hanging object; S23. Construct the positive definite Lyapunov function V0(t): Among them, μ L represents the length between the center of mass of the hanging mass and the center of mass of the quadrotor drone, g is the acceleration of gravity, φ s is the actual lateral swing angle of the hanging object, θ s is the actual longitudinal swing angle of the hanging object; S24, according to the nonlinear model of the quadrotor drone suspension system established in step S1, the derivative of the positive definite Lyapunov function V0 is calculated as: in, S25. Setting ρ a (·)=β1δ f1 , ρ b (·)=β2δ f2 , ρ c (·)=β3δ f3 , according to steps S21 to S24, the reference trajectory of the quadrotor drone is calculated as: Among them, β1, β2 and β3 are all positive constants to be designed; τ is a constant.

5. The anti-disturbance and anti-sway control method for the fully state-constrained quad-rotor UAV suspension system according to claim 4 is characterized in that: The step S4 comprises the following steps: S41. Constructing the virtual control law of the position loop of a quadrotor drone based on the new obstacle Lyapunov function technology sd : λ1=x1-ρ sd T h1 =diag{a 11 ,a 12 ,a 13 ,a 14 ,a 15 } Where x1 = ρ, ρ sd =[x d ,y d ,z d ,0,0] T , ρ sd is the reference signal of the quadrotor UAV suspension system, ξ1=diag{ξ 11 ,ξ 12 ,ξ 13 ,ξ 14 ,ξ 15 } is the positive definite matrix to be designed, σ 1m is the normal number to be designed, N1=diag{N 11 ,N 12 ,N 13 ,N 14 ,N 15 } is the matrix to be designed, is the i-th element in matrix N1, λ 1i λ1=x1-ρ sd The i-th element in is a normal number to be designed and satisfies γ 1i For element λ 1i The generalized boundary of T h1 is the matrix to be designed, α 1i is the matrix T h1 The i-th element in ; S42. Based on the new obstacle Lyapunov function technology, a robust controller for the position loop of a quadrotor drone is constructed: λ2=x2-v sd Where x1 = ρ, x3=A -1 (x1)d 1o , is the estimated value of x3, A(x1) is the symmetric inertia matrix, B(x1,x2) is the Coriolis force matrix, and C(x1) is the gravity vector matrix; ξ2 = diag{ξ 21 ,ξ 22 ,ξ 23 ,ξ 24 ,ξ 25 } is the positive definite matrix to be designed, σ 2m and v s1 is the normal number to be designed, N2=diag{N 21 ,N 22 ,N 23 ,N 24 ,N 25 } is the matrix to be designed, is the i-th element in matrix N2, λ 2i λ2=x2-v sd The i-th element of is a normal number to be designed and satisfies γ 2i is 2i The generalized boundary of T h2 =diag{α 21 ,α 22 ,α 23 ,α 24 ,α 25 }, is the matrix T h2 The i-th element in Represents the output of the position loop filter; S43, according to the position loop robust controller obtained in step S42, the total lift T of the quadrotor drone is obtained respectively. s 、Desired roll angle φ d and the desired pitch angle θ d : Among them, N so =[N s1 ,N s2 ,N s3 ,N s4 ,N s5 ] T , N s =-R s e s T s +(m Q +m L )ge s =[N s1 ,N s2 ,N s3 ] T , R s represents the rotation matrix from the body coordinate system to the ground coordinate system, e s =[0,0,1] T , m L is the mass of the hanging object, g is the acceleration due to gravity, ψ d is the expected yaw angle given by the quadrotor drone, φ d is the desired roll angle of the quadrotor drone, θ d is the desired pitch angle of the quadrotor drone, 6. The anti-disturbance and anti-sway control method for the fully state-constrained quad-rotor UAV suspension system according to claim 5 is characterized in that: The step S4 further comprises the following steps: S44. Based on the new obstacle Lyapunov function technology, construct the virtual control law of the attitude loop of the quadrotor drone Ω sd : λ3=x4-Q sd T h3 =diag{a 31 ,a 32 ,a 33 } Where x4 = Q s ,ξ3=diag{ξ 31 ,ξ 32 ,ξ 33 } is the positive definite matrix to be designed, σ 3m is the normal number to be designed, Represents the output of the filter; N3 = diag{N 31 ,N 32 ,N 33 } is the matrix to be designed, is the i′th element in matrix N3, γ 3i′ λ3=x4-Q sd The i′th element of is a normal number to be designed and satisfies γ 3i′ For element λ 3i′ The generalized boundary of Q sd =[φ d ,θ d ,ψ d ] T represents the desired attitude angle vector of the quadrotor drone; S45. Based on the new obstacle Lyapunov function technology, a robust controller for the attitude loop of a quadrotor drone is constructed: λ4=x5-Ω sd Where x5 = Ω s , is the estimated value of x6, ξ4=diag{ξ 41 ,ξ 42 ,ξ 43 } is the positive definite matrix to be designed, N4=diag{N 41 ,N 42 ,N 43 } is the matrix to be designed, σ 4m and ν s2 are all normal numbers to be designed, is the i′th element in matrix N4, λ 4i′ λ4=x5-Ω sd The i′th element of is a normal number to be designed and satisfies γ 4i′ is 4i′ The generalized boundary of T h4 =diag{α 41 ,α 42 ,α 43 } is the matrix to be designed, is the matrix T h4 The i′th element in Represents the output of the attitude loop filter.

7. An electronic device, characterized in that: It includes a memory and a processor, the memory is used to store a computer program, and the processor runs the computer program to enable the electronic device to execute the anti-disturbance and swing reduction control method for the fully-state-constrained quad-rotor unmanned aerial vehicle suspension system according to any one of claims 1 to 6.

8. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the anti-disturbance and swing reduction control method for a fully state-constrained quad-rotor unmanned aerial vehicle suspension system according to any one of claims 1 to 6 is implemented.

9. A computer program product, characterized in that The computer program product includes a computer program, and when the computer program is executed by a processor, the anti-disturbance and swing reduction control method for the fully state-constrained quad-rotor unmanned aerial vehicle suspension system according to any one of claims 1 to 6 is implemented.

Images