Non-planar agricultural six-rotor unmanned aerial vehicle control method based on high-order all-wheel-drive system

Abstract

The invention discloses a non-planar agricultural six-rotor unmanned aerial vehicle control method based on a high-order all-wheel-drive system, and relates to the field of unmanned aerial vehicle control, in particular to the non-planar agricultural six-rotor unmanned aerial vehicle control method. The objective of the invention is to solve the problem that an existing method is low in control accuracy of an all-wheel-driven non-planar six-rotor unmanned aerial vehicle. The method comprises the following steps: step 1, constructing an all-drive non-planar six-rotor unmanned aerial vehicle model; 2, constructing a system state variable matrix; 3, designing a controller based on the all-drive non-planar six-rotor unmanned aerial vehicle model and the system state variable matrix; the method specifically comprises the following steps: step three 1, converting the all-wheel-drive non-planar six-rotor unmanned aerial vehicle model constructed in the step one into a high-order all-wheel-drive system model based on a system state variable matrix; step 3, obtaining an error tracking model based on the high-order all-drive system model; and 3, designing a controller based on the error tracking model.

Description

Technical Field

[0001] The present invention relates to the field of unmanned aerial vehicle control, and in particular to a control method for a non-planar agricultural six-rotor unmanned aerial vehicle. Background Art

[0002] my country's grain output has been growing for many years, and agricultural production has entered a new stage of development, and new demands for farmland production have also emerged. In agricultural production, improving the quality of agricultural products and strengthening the early warning capabilities of agricultural disasters have become urgent needs. At the same time, my country's agricultural planting and management model has been continuously changing from a small peasant economy to a "big agriculture" of agricultural cooperatives, and large-scale production has put forward higher requirements for agricultural equipment.

[0003] In the process of pesticide spraying, the original manual spraying operation has many shortcomings, such as the need for manual entry into harsh environments and low spraying efficiency. However, the pesticide spraying system with drones as carriers can spray farmland comprehensively, accurately, and at all angles. In addition, agricultural drones also have the ability to deal with problems immediately. When problems are found in farmland, drones can reach the problem area in time to fertilize, kill insects, provide information feedback, and predict yields. As a result, agricultural mechanization and automation have emerged across the country, and drones have been selected for operations. The use of drones to spray pesticides to promote agricultural production has emerged. The use of agricultural drones is in line with the new direction of agricultural modernization, informatization, and precision.

[0004] In most farms, my country has begun to use agricultural aircraft for pesticide spraying operations. Many farms have introduced low-speed agricultural aircraft and successfully completed the task of pesticide spraying on large-scale farms. Agricultural fixed-wing aircraft have the advantages of large load capacity, high efficiency, and high speed, but due to the high flight speed and altitude, it is impossible to spray accurately and it is difficult to control the drift of droplets. Agricultural helicopters have the characteristics of hovering ability, slow flight speed, and good airflow disturbance effect on crops. The airflow generated by its main rotor makes the pesticide evenly distributed and improves the utilization rate of pesticides. However, helicopters rely on rotor rotation to increase altitude and adjust direction, resulting in high energy consumption and low efficiency. In addition, the high cost of repair and maintenance makes it a relatively expensive spraying equipment in the field of agricultural aviation.

[0005] In recent years, the research on multi-rotor aircraft in civil, agricultural and military fields has made great progress. Multi-rotor UAVs have a simple structure, and the only moving part is the rotor itself. Compared with fixed-wing UAVs, they have stronger maneuverability and hovering ability, and are less affected by time, location and land area. The price is also relatively low. Therefore, multi-rotor aircraft have been widely used in the process of spraying pesticides.

[0006] The design of non-planar six-rotor drones can compensate for gravity in any posture of the drone and achieve static hovering in any posture. Pesticide spraying drones need to adapt to the complex environment of farmland or orchards (such as narrow space and obstacles). In many cases, drones are required to fly into farmland or orchards at a specific angle and spray pesticides to specific positions of plants. The attitude control and position control of traditional drones are highly coupled, which means that traditional underdriven drones are difficult to achieve the function of static hovering in any posture. Non-planar six-rotor drones expand the application scope of future drone development. Non-planar six-rotor drones can achieve unrestricted omnidirectional movement of six degrees of freedom in the air and robust tracking of arbitrary trajectories in three-dimensional space when working. This omnidirectional drone has a unique advantage over traditional drones, which brings great convenience to the use of drones for spraying pesticides. Summary of the invention

[0007] The purpose of the present invention is to solve the problem of low control accuracy of existing methods for all-wheel drive non-planar six-rotor UAVs, and to propose a control method for non-planar agricultural six-rotor UAVs based on a high-order all-wheel drive system.

[0008] The specific process of the non-planar agricultural six-rotor UAV control method based on the high-order full-drive system is as follows:

[0009] Step 1: Build a full-drive non-planar six-rotor drone model;

[0010] Step 2: Construct the system state variable matrix;

[0011] Step 3: Design a controller based on the all-wheel drive non-planar six-rotor drone model and the system state variable matrix; the specific process is:

[0012] Step 31: Convert the all-wheel drive non-planar six-rotor drone model constructed in step 1 into a high-order all-wheel drive system model based on the system state variable matrix;

[0013] Step 32: Obtain an error tracking model based on a high-order all-wheel drive system model;

[0014] Step 33: Design a controller based on the error tracking model.

[0015] The beneficial effects of the present invention are:

[0016] The present invention proposes a control method for a non-planar agricultural six-rotor UAV based on a high-order all-wheel drive system. In order to more conveniently and intuitively observe the layout of the UAV rotor, Figure 2 The three views after removing the pesticide spraying equipment are shown. Figure 2 , f 1 -f 6 They represent the directions in which the six rotors provide lift. Figure 2a is a top view of the drone, f 1XY -f 6XY They are the directions of the forces after projecting the lift provided by the six rotors onto the plane where the fuselage and the arms are located; Figure 2 The lift f provided by the rotor is clearly shown in Figure c. 2 It is not vertically upward, but has an angle γ with the vertical direction, and the inclined lift is decomposed into f in the horizontal direction and the vertical direction respectively. 2z and f 2xy ; This non-planar design with passive tilt rotor overcomes the under-actuated characteristics of common multi-rotor configurations; The full rank of the control allocation matrix indicates that this drone with non-planar design has six independent control variables, so it does not need to tilt while moving on the horizontal plane. In this case, the equipment for spraying pesticides does not require additional rotation and stabilization devices, which enables it to achieve a certain degree of modularization and simplification; When spraying pesticides, the drone will be easily out of control due to complex wind interference, and the drone response speed is required to be relatively high, so the present invention uses a finite time high-order full-drive system method to design an anti-interference controller to improve the robustness and rapid response of the system; Since the mass of the medicine loaded on the belly of the drone will decrease over time, the present invention takes into account the "variable mass" characteristic of the pesticide spraying drone; When stopping spraying pesticides, the mass of the pesticides can be regarded as part of the mass of the drone; In summary, the present invention improves the control accuracy of the full-drive non-planar six-rotor drone. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 It is a flow chart of the present invention;

[0018] Figure 2 The structure diagram of the six-rotor all-wheel drive drone used in the present invention, a is a top view, b is an oblique view, c is a main view, f is a 1xy is the projection of the lift of the first rotor of the drone onto the xoy plane of the drone coordinate system, f 2xy is the projection of the lift of the second rotor of the drone onto the xoy plane of the drone coordinate system, f 3xy is the projection of the lift of the third rotor of the drone onto the xoy plane of the drone coordinate system, f 4xy is the projection of the lift of the fourth rotor of the drone onto the xoy plane of the drone coordinate system, f 5xy is the projection of the lift of the fifth rotor of the drone onto the xoy plane of the drone coordinate system, f 6xy is the projection of the lift of the sixth rotor of the drone onto the xoy plane of the drone coordinate system, f 1 is the direction of the lift generated by the first rotor of the UAV, f 2 is the direction of the lift generated by the second rotor of the UAV, f 3is the direction of the lift generated by the third rotor of the UAV, f 4 is the direction of the lift generated by the fourth rotor of the UAV, f 5 is the direction of the lift generated by the fifth rotor of the UAV, f 6 is the direction of the lift generated by the sixth rotor of the drone, X B is the x-axis of the drone coordinate system, Y B is the y-axis of the drone coordinate system, Z B is the z-axis of the drone coordinate system, O is the center of the drone, and f 2z is the vector obtained by projecting the lift generated by the No. 2 rotor of the drone toward the z-axis direction, and γ is the angle between the rotor of each drone and the xoy plane of the drone coordinate system (taken as 30°);

[0019] Figure 3 It is the position tracking curve of the UAV, a is the position tracking curve in the x-axis direction, b is the position tracking curve in the y-axis direction, and c is the position tracking curve in the z-axis direction;

[0020] Figure 4 is the attitude angle diagram of the UAV, a is the roll angle tracking curve, b is the yaw angle tracking curve, and c is the pitch angle tracking curve;

[0021] Figure 5 is the UAV position tracking curve after the interference is applied, a is the tracking error in the x-axis direction after the interference is applied, b is the tracking error in the y-axis direction after the interference is applied, and c is the tracking error in the z-axis direction after the interference is applied. DETAILED DESCRIPTION

[0022] Specific implementation method 1: The specific process of the non-planar agricultural six-rotor drone control method based on the high-order full-drive system in this implementation method is as follows:

[0023] Step 1: Build a full-drive non-planar six-rotor drone model;

[0024] Step 2: Construct the system state variable matrix;

[0025] Step 3: Design a controller based on the all-wheel drive non-planar six-rotor drone model and the system state variable matrix; the specific process is:

[0026] Step 31: Based on the system state variable matrix, the all-wheel drive non-planar six-rotor UAV model (Formula 1) constructed in Step 1 is converted into a high-order all-wheel drive system model (Formula 13);

[0027] Step 32: Obtain the error tracking model (14) based on the high-order all-wheel drive system model (Equation 13);

[0028] Step 33: Design a controller based on the error tracking model (14).

[0029] Specific implementation method 2: This implementation method is different from the specific implementation method 1 in that the full-drive non-planar six-rotor drone model is constructed in step 1; the specific process is:

[0030] After theoretical derivation, the following set of equations can describe this all-wheel drive six-rotor drone;

[0031] Select any rotor in the six-rotor drone as the first rotor, and arrange the second rotor, the third rotor, the fourth rotor, the fifth rotor, and the sixth rotor in sequence based on the first rotor;

[0032] Construct the coordinate system of the drone: take the center of the six-rotor drone as the origin o, the direction of the third rotor pointing to the fifth rotor as the x-axis, the direction of the second rotor as the y-axis, and the z-axis perpendicular to the x and y planes;

[0033] The model of the all-wheel drive non-planar six-rotor drone is expressed as:

[0034]

[0035] In the formula, represents the second-order derivative of the drone's position along the x-axis, represents the second-order derivative of the drone’s position along the y-axis, represents the second-order derivative of the drone's position along the z-axis, Represents the second-order derivative of the UAV's roll angle, Represents the second-order derivative of the pitch angle of the drone, Represents the second-order derivative of the yaw angle of the drone; a, b, α p , α q , α r Both represent coefficients, d 1 d 2 d 3 d 4 d 5 d 6 Indicates the interference to the drone; u 1 、u 2 、u 3 、u 4 、u 5 、u 6 represents the control matrix of the UAV, u 1 、u 2 、u 3 、u 4 、u 5 、u 6 The speed of the six rotors of the drone The specific relationship is shown in formula (2); g represents the gravitational acceleration of the earth.

[0036] The other steps and parameters are the same as those in the first embodiment.

[0037] Specific implementation method three: This implementation method is different from specific implementation methods one or two in that the control matrix of the drone is expressed as:

[0038]

[0039] In the formula, Indicates the square of the first rotor speed of the drone. Indicates the square of the second rotor speed of the drone. Indicates the square of the rotation speed of the third rotor of the drone. Indicates the square of the speed of the fourth rotor of the drone. Indicates the square of the fifth rotor speed of the drone. It represents the square of the rotation speed of the sixth rotor of the UAV. The superscript T indicates the transpose.

[0040] The other steps and parameters are the same as those in the first or second embodiment.

[0041] Specific implementation method 4: This implementation method is different from any one of the specific implementation methods 1 to 3 in that the coefficient m represents the mass of the drone, δm represents the mass of the drone’s payload, and k f is the lift coefficient, S γ represents sinγ, where γ represents the angle between the UAV rotor and the xoy plane of the UAV coordinate system;

[0042] coefficient

[0043] coefficient l represents the rotor radius, C γ represents cosγ, k τ Represents the anti-torque coefficient, I x represents the moment of inertia of the drone along the x-axis;

[0044] coefficient I y Represents the UAV's moment of inertia along the y-axis;

[0045] coefficient I z Represents the UAV's moment of inertia along the z-axis.

[0046] The drone can be controlled by adjusting the rotation speed of the rotor.

[0047] Similarly, if the control matrix required at a certain moment is calculated during the control process, the rotational speed of the four rotors at that moment can also be solved, thereby achieving control of the drone.

[0048] After calculating the determinant of the coefficient matrix, the value is 2, so it can be seen that The coefficient matrix of is full rank, that is, the UAV can reach any state by adjusting the speed of the six rotors.

[0049] The other steps and parameters are the same as those in Specific Embodiments 1 to 3.

[0050] Specific implementation method 5: This implementation method is different from any one of the specific implementation methods 1 to 4 in that the system state variable matrix is ​​constructed in step 2; the specific process is:

[0051] Step 21. Define the matrix M i ∈R m×m , constant i=0,1,2,...,n-1, n=2; R represents the set of real numbers; m represents the mass of the drone;

[0052] M 0 、M 1 Respectively represent a real matrix with m rows and m columns;

[0053] Based on the matrix M i Construct the matrix M (0～(n-1)) ; The expression is:

[0054] M (0～(n-1)) =[M 0 M 1 … M n-1 ] (4)

[0055] Where M 0 represents a real matrix with m rows and m columns, M 1 represents a real matrix with m rows and m columns, M n-1 Represents a real matrix with m rows and m columns;

[0056] Based on the matrix M (0～(n-1)) Construct the matrix Γ(M (0～(n-1)) ); the expression is:

[0057]

[0058] Where I represents the unit matrix;

[0059] Step 22: Define the function as shown below:

[0060] sig σ (ζ)=sign(ζ)|ζ| σ (10)

[0061] In the formula, sig σ (ζ) represents the σ-order index of sig(ζ), |ζ| represents the absolute value of ζ, and |ζ|σ represents the σ-order index of |ζ|; σ represents an arbitrary constant, σ≥0;

[0062] ζ represents the system state variable; the system is an all-wheel drive non-planar six-rotor drone;

[0063] where sign(ζ) is expressed as:

[0064]

[0065] For any constant σ, the system state variable ζ, d|ζ| σ+1 / dζ=(σ+1)sig σ (ζ), d[sig σ+1 (ζ)] / dζ=(σ+1)|ζ| σ ;

[0066] In the formula, |ζ| σ+1 represents the σ+1 order index of |ζ|; sig σ+1 (ζ) represents the σ+1 order exponent of sig(ζ);

[0067] Step 23: Construct the system state variable matrix ζ based on the system state variable ζ (0～n) .

[0068] The other steps and parameters are the same as those in the specific implementation mode 1 to 1.

[0069] Specific implementation method 6: This implementation method is different from the specific implementation methods 1 to 5 in that the system state variable matrix ζ is constructed based on the system state variable ζ in step 23. (0～n) ; The expression is:

[0070]

[0071] In the formula, ζ (0～n) represents the system state variable matrix;

[0072] represents the first-order derivative of ζ, ζ (n) represents the nth-order derivative of ζ, n = 2;

[0073] ζ∈R r , R represents the set of real numbers, R r represents the r-dimensional real number set, r = 1;

[0074] The system is an all-wheel drive non-planar six-rotor drone;

[0075] The ζ (n) It is expressed as:

[0076] ζ (n) =f(ζ (0～(n-1)))+T d +L(ζ (0～(n-1)) )u (6)

[0077] In the formula, f(ζ (0～(n-1)) ) represents a continuous state function, f(ζ (0～(n-1)) )∈R r ;ζ (0～(n-1)) represents the system state variable matrix,

[0078] T d represents the external disturbance function of the system, T d ∈R r , the system is a fully-driven non-planar six-rotor UAV;

[0079] L(ζ (0～(n-1)) ) represents a continuous matrix function, and detL(ζ (0～(n-1)) )≠0, L(ζ (0～(n-1)) )∈R r×r , R r×r Represents a real matrix with r rows and r columns;

[0080] u represents the control quantity of the system, u∈R r , the system is a fully-driven non-planar six-rotor UAV;

[0081] The control quantity u of the system is written in the following form:

[0082] u=u 0 +u a (7)

[0083] In the formula, u 0 represents the nominal part of the controller; u a It represents the auxiliary controller, which enables the system to achieve faster state convergence, improve dynamic performance such as robustness and anti-interference ability;

[0084] Here, we choose finite-time control to improve the controller’s control performance for this hexacopter drone;

[0085] The nominal part u of the controller 0 It is expressed as follows:

[0086]

[0087] In the formula, u * It represents the robust controller, which is used to resist the external interference to the UAV;

[0088] ε represents a positive number, The norm of the disturbance to the system The square of represents the norm of the disturbance to the system, W L (M (0～(n-1)) ) represents the parameter matrix, and the superscript T represents the transposition; ζ *(n) represents the desired system state ζ * The nth derivative of ;

[0089] The most basic part of the controller 0 The purpose is to convert the linear term M (0～n-1) ζ (0～n-1) Assigned to the closed-loop systemζ (n) =f(ζ (0～(n-1)) ); ensure that the system state variable ζ converges to the ellipsoid region as shown below:

[0090]

[0091] In the formula, (ζ * ) (0～(n-1)) Represents the desired system state ζ * A matrix represents the ellipsoid region to which the desired system state converges, ζ * represents the desired system state, P(ζ-ζ * ) (0～(n-1)) represents a matrix, ((ζ-ζ * ) (0～(n-1)) ) T P(ζ-ζ * ) (0～(n-1)) The whole represents the ellipsoid region to which the desired system state converges, and κ are two arbitrarily given positive numbers;

[0092] The auxiliary controller u a It is expressed as follows:

[0093] According to the finite-time sliding mode control theory, when the auxiliary controller u a When designed in the following form, the system state variable ζ can converge to the sliding surface within a finite time, thus achieving finite-time control of the system;

[0094]

[0095] Among them, * represents the desired system state, The first derivative representing the desired system state;

[0096] σ 1 , σ 2 , k p , k d are all constants and satisfy 0<σ 1<1,σ 2 =2σ 1 / (1+σ 1 ), k p >0,k d >0;

[0097] In this way, we can track and control the state system of the drone within a limited time, that is, make the current state ζ reach the desired state ζ within a limited time. d .

[0098] represents sig(ζ * -ζ) 1 The order index, express σ 2 Order index.

[0099] The other steps and parameters are the same as those in Specific Implementations 1 to 5.

[0100] Specific implementation method 7: This implementation method is different from specific implementation methods 1 to 6 in that in step 31, the all-wheel drive non-planar six-rotor drone model (Formula 1) constructed in step 1 is converted into a high-order all-wheel drive system model (Formula 13); the expression is:

[0101]

[0102] Where ζ represents the system state variable, ζ==[x,y,z,φ,θ,ψ] T , x represents the position coordinate of the drone on the x-axis, y represents the position coordinate of the drone on the y-axis, z represents the position coordinate of the drone on the z-axis, φ represents the roll angle of the drone, θ represents the pitch angle of the drone, ψ represents the yaw angle of the drone, and the superscript T represents the transposition;

[0103] The system is an all-wheel drive non-planar six-rotor drone;

[0104] represents the second-order derivative of the system state variable, represents the second-order derivative of the drone's position along the x-axis, represents the second-order derivative of the drone’s position along the y-axis, represents the second-order derivative of the drone's position along the z-axis, Represents the second-order derivative of the UAV's roll angle, Represents the second-order derivative of the pitch angle of the drone, Represents the second derivative of the yaw angle of the drone;

[0105] f(ζ (0～1)) represents a continuous state function, f(ζ (0～1) )=[0,0,-g,0,0,0] T , g represents the earth's gravitational acceleration, ζ (0～1) represents the 0th to 1st order derivative of ζ, ζ (0～1) =[ζ ζ] T , Represents the first-order derivative of the system state variable;

[0106] L(ζ (0～1) ) represents a continuous matrix function, Re represents a diagonal matrix, a, b, α p , α q , α r All represent coefficients;

[0107] T d represents the external disturbance function of the system, T d =[d 1 ,d 2 ,d 3 ,d 4 ,d 5 ,d 6 ] T , T d ∈R r ;

[0108] d 1 represents the disturbance of the coordinates of the drone’s position on the x-axis; d 2 represents the disturbance of the coordinates of the drone’s position on the y-axis; d 3 represents the disturbance of the coordinates of the drone’s position on the z-axis; d 4 represents the disturbance of the UAV's roll angle; d 5 represents the disturbance of the pitch angle of the UAV; d 6 Represents the disturbance of the UAV's yaw angle.

[0109] The other steps and parameters are the same as those in Specific Embodiments 1 to 6.

[0110] Specific implementation eight: This implementation is different from specific implementations one to seven in that the error tracking model is obtained based on the high-order all-wheel drive system model (Formula 13) in step three two; the specific process is:

[0111] Assume that the desired position of the hexacopter is ζ * =(x * ,y * ,z * ,φ * ,θ * ,ψ * ) T ,and and Is continuous and differentiable, let the deviation Δ = ζ-ζ * ;

[0112] In the formula, x * Indicates the desired coordinate of the drone on the x-axis, y * Indicates the desired coordinate of the drone on the y-axis, z * represents the desired coordinate of the drone on the z-axis, φ * represents the desired roll angle of the drone, θ * represents the desired pitch angle of the UAV, ψ * represents the desired yaw angle of the drone, represents the first-order derivative of the desired drone state, represents the second-order derivative of the desired UAV state;

[0113] The high-order all-wheel drive system model (Equation 13) is transformed into an error tracking model; the expression is:

[0114]

[0115] In the formula, represents the second-order derivative of the deviation Δ;

[0116] Δ (0～1) represents the deviation matrix, represents the first derivative of the deviation Δ;

[0117] f(Δ (0～1) ) represents a continuous state function, f(Δ (0～1) )=[0,0,-g,0,0,0] T ;

[0118] L(Δ (0～1) ) represents a continuous matrix function, Re represents a diagonal matrix, a, b, α p , α q , α r All represent coefficients.

[0119] The other steps and parameters are the same as those in Specific Embodiments 1 to 7.

[0120] Specific implementation method 9: This implementation method is different from the specific implementation methods 1 to 8 in that the controller is designed based on the error tracking model formula (14) in step 33; the specific process is:

[0121] The controller is shown below

[0122] u=u 0 +u a(15)

[0123]

[0124] where β 1 , β 2 , k p , k d are all constants, 0<β 1 <1, β 2 =2β 1 / (1+β 1 ), k p >0,k d >0;

[0125] M (0～1) represents the matrix, M (0～1) =[M 0 M 1 ] T ;

[0126] u * It represents the robust controller, which is used to resist the external interference to the UAV;

[0127] The norm of the disturbance to which the system is subjected The square of Represents the norm of the disturbance to which the system is subjected;

[0128] W L (M 0～1 ) represents the parameter matrix, and the superscript T represents the transpose;

[0129] represents the desired UAV system state ζ * The second derivative of

[0130] sig β1 (ζ * -ζ) represents the function sig(ζ * -ζ) 1 Order index;

[0131] Representation function Beta 2 Order index;

[0132] In this way, we can track and control the state system of the drone within a limited time, that is, make the current state (x, y, z, φ, θ, ψ) T To reach the desired state (x * ,y * ,z * ,φ * ,θ * ,ψ* ) T .

[0133] The other steps and parameters are the same as those in Specific Embodiments 1 to 8.

[0134] Specific implementation method 10: This implementation method is different from the specific implementation methods 1 to 9 in that the parameter matrix W L (M 0～(n-1) ) is obtained as follows:

[0135] 1) Set matrix P and matrix Q to satisfy detN(Q,P)≠0;

[0136] The matrix P represents a real matrix with nr rows and nr columns, the matrix Q represents a real matrix with r rows and nr columns, and detN(Q,P) represents the rank of the matrix N(Q,P);

[0137] The expression of the rank of the matrix N(Q,P) is:

[0138]

[0139] Where P n-1 represents the n-1 power of the matrix P;

[0140] 2) Based on the matrix N(Q,P) and matrix P, construct the matrix Γ(M 0～(n-1) ); the expression is:

[0141] Γ(M 0～(n-1) )=N(Q,P)PN -1 (Q,P) (19)

[0142] 3) Based on matrix N(Q,P), matrix P, and matrix Q, construct matrix M 0～(n-1) ; The expression is:

[0143] M 0～(n-1) =-QP n N -1 (Q,P) (20)

[0144] 4) The matrix Γ(M 0～(n-1) ), matrix M 0～(n-1) Substitute the Riccati equation and solve the matrix W(M 0～(n-1) );

[0145] The Riccati equation is Γ T (M 0～1 )W(M 0～(n-1) )+W(M 0～(n-1) )Γ(M 0～1 )≤-κW(M 0～(n-1) );

[0146] In the formula, κ represents an arbitrarily selected positive number, κ>0;

[0147] 5) Based on the matrix W(M 0～(n-1) ) Construct the matrix W L (M 0～(n-1) ); the expression is:

[0148]

[0149] In the formula, I r represents an r-row and r-column identity matrix.

[0150] The other steps and parameters are the same as those in Specific Embodiments 1 to 9.

[0151] The following examples are used to verify the beneficial effects of the present invention:

[0152] Embodiment 1:

[0153] In order to verify and demonstrate the effectiveness of the controller designed in the present invention for the finite-time tracking control method of a non-planar six-rotor UAV based on high-order all-wheel drive system theory, the following numerical simulation experiments were carried out.

[0154] The parameters of the drone are: mass m = 7.5 kg, δm = (3-0.02t·δ(t-1)) kg, where t is the simulation time, δ(t-1) is a step function, gravitational acceleration g = 9.8 N / kg, and lift coefficient k f =1.91×10 -3 , anti-torque coefficient k τ =4.21×10 -5 , rotor radius l = 0.45m, I x =0.363kg·m 2 , I y =0.363kg·m 2 , I z =0.651kg·m 2 , γ=30°. Other parameters in the controller are: κ=8.

[0155] Select position reference trajectory x * =5sin(0.5πt)m,y * =5cos(0.5πt)m,z * =0.5tm; attitude reference trajectory θ * =0.2cos(0.5πt)rad, The numerical experimental results are as follows Figure 3 , 4 As shown;

[0156] The system can achieve tracking relatively well in 0.5 seconds. The finite-time UAV controller designed based on the high-order all-wheel drive system can quickly make the UAV's position reach the desired effect, which reflects the rapidity of the controller's tracking and can fly to the designated location very accurately and quickly to perform tasks.

[0157] A random wind disturbance with an average speed of 6 m / s was applied to the drone along the x-axis and y-axis, and a random wind disturbance with an average speed of 3 m / s was applied along the z-axis. Another set of simulation experimental results and the tracking error after 0.5 seconds are obtained as follows: Figure 5 As shown;

[0158] Depend on Figure 5 It can be seen that after applying a certain amount of interference, the drone can still track the expected position curve. The interference causes the average offset of the drone along the x-axis and y-axis to be about 0.08m, and the maximum offset is about 0.12m; the average offset along the z-axis is about 0.02m, and the maximum offset is about 0.04m. This shows that this finite-time all-wheel drive controller has relatively strong robustness and can better resist external interference caused by external factors such as wind during the spraying of pesticides.

[0159] The present invention may also have many other embodiments. Without departing from the spirit and essence of the present invention, those skilled in the art may make various corresponding changes and modifications based on the present invention, but these corresponding changes and modifications should all fall within the scope of protection of the claims attached to the present invention.

Claims

1. A control method for a non-planar agricultural six-rotor UAV based on a high-order all-wheel drive system, characterized in that: The specific process of the method is: Step 1: Build a full-drive non-planar six-rotor drone model; Step 2: Construct the system state variable matrix; Step 3: Design a controller based on the all-wheel drive non-planar six-rotor drone model and the system state variable matrix; the specific process is: Step 31: Convert the all-wheel drive non-planar six-rotor drone model constructed in step 1 into a high-order all-wheel drive system model based on the system state variable matrix; Step 32: Obtain an error tracking model based on a high-order all-wheel drive system model; Step 33: Design a controller based on the error tracking model.

2. The control method of a non-planar agricultural six-rotor UAV based on a high-order all-wheel drive system according to claim 1, characterized in that: In the step 1, a full-drive non-planar six-rotor drone model is constructed; the specific process is as follows: Select any rotor in the six-rotor drone as the first rotor, and arrange the second rotor, the third rotor, the fourth rotor, the fifth rotor, and the sixth rotor in sequence based on the first rotor; Construct the coordinate system of the drone: take the center of the six-rotor drone as the origin o, the direction of the third rotor pointing to the fifth rotor as the x-axis, the direction of the second rotor as the y-axis, and the z-axis perpendicular to the x and y planes; The model of the all-wheel drive non-planar six-rotor drone is expressed as: In the formula, represents the second-order derivative of the drone's position along the x-axis, represents the second-order derivative of the drone’s position along the y-axis, represents the second-order derivative of the drone's position along the z-axis, Represents the second-order derivative of the UAV's roll angle, Represents the second-order derivative of the pitch angle of the drone, Represents the second-order derivative of the yaw angle of the drone; a, b, α p , α q , α r All represent coefficients, d1, d2, d3, d4, d5, and d6 represent the interference to the UAV; u1, u2, u3, u4, u5, and u6 represent the control matrix of the UAV; and g represents the gravitational acceleration of the earth.

3. The control method of a non-planar agricultural six-rotor UAV based on a high-order all-wheel drive system according to claim 2, characterized in that: The control matrix of the UAV is expressed as: In the formula, Indicates the square of the first rotor speed of the drone. Indicates the square of the second rotor speed of the drone. Indicates the square of the rotation speed of the third rotor of the drone. Indicates the square of the speed of the fourth rotor of the drone. Indicates the square of the fifth rotor speed of the drone. It represents the square of the rotation speed of the sixth rotor of the UAV. The superscript T indicates the transpose.

4. The control method of a non-planar agricultural six-rotor UAV based on a high-order all-wheel drive system according to claim 3, characterized in that: The coefficient m represents the mass of the drone, δm represents the mass of the drone’s payload, and k f is the lift coefficient, S γ represents sinγ, where γ represents the angle between the UAV rotor and the xoy plane of the UAV coordinate system; coefficient coefficient l represents the rotor radius, C γ represents cosγ, k τ Represents the anti-torque coefficient, I x represents the moment of inertia of the drone along the x-axis; coefficient I y Represents the UAV's moment of inertia along the y-axis; coefficient I z Represents the UAV's moment of inertia along the z-axis.

5. The control method of a non-planar agricultural six-rotor UAV based on a high-order all-wheel drive system according to claim 4, characterized in that: In the step 2, a system state variable matrix is ​​constructed; the specific process is: Step 21. Define the matrix M i ∈R m×m , constant i=0,1,2,...,n-1, n=2; R represents the set of real numbers; m represents the mass of the drone; M0 and M1 represent a real matrix with m rows and m columns respectively; Based on the matrix M i Construct the matrix M (0～(n-1)) ; The expression is: M (0～(n-1)) =[M0 M1…M n-1 ] (4) In the formula, M0 represents a real number matrix with m rows and m columns, M1 represents a real number matrix with m rows and m columns, and M n-1 Represents a real matrix with m rows and m columns; Based on the matrix M (0～(n-1)) Construct the matrix Γ(M (0～(n-1)) ); the expression is: Where I represents the unit matrix; Step 22: Define the function as shown below: sig σ (ζ)=sign(ζ)|ζ| σ (10) In the formula, sig σ (ζ) represents the σ-order index of sig(ζ), |ζ| represents the absolute value of ζ, and |ζ| σ represents the σ-order index of |ζ|; σ represents an arbitrary constant, σ≥0; ζ represents the system state variable; the system is an all-wheel drive non-planar six-rotor drone; where sign(ζ) is expressed as: For any constant σ, the system state variable ζ, d|ζ| σ+1 / dζ=(σ+1)sig σ (ζ), d[sig σ+1 (ζ)] / dζ=(σ+1)|ζ| σ ; In the formula, |ζ| σ+1 represents the σ+1 order index of |ζ|; sig σ+1 (ζ) represents the σ+1 order exponent of sig(ζ); Step 23: Construct the system state variable matrix ζ based on the system state variable ζ (0～n) .

6. The control method of a non-planar agricultural six-rotor UAV based on a high-order all-wheel drive system according to claim 5, characterized in that: In the step 23, the system state variable matrix ζ is constructed based on the system state variable ζ (0～n) ; The expression is: In the formula, ζ (0～n) represents the system state variable matrix; represents the first-order derivative of ζ, ζ (n) represents the nth-order derivative of ζ, n = 2; ζ∈R r , R represents the set of real numbers, R r represents the r-dimensional real number set, r = 1; The system is an all-wheel drive non-planar six-rotor drone; The ζ (n) It is expressed as: g (n) =f(ζ (0～(n-1)) )+T d +L(ζ (0～(n-1)) )u (6) In the formula, f(ζ (0～(n-1)) ) represents a continuous state function, f(ζ (0～(n-1)) )∈R r ;ζ (0～(n-1)) represents the system state variable matrix, T d represents the external disturbance function of the system, T d ∈R r , the system is a fully-driven non-planar six-rotor UAV; L(ζ (0～(n-1)) ) represents a continuous matrix function, and detL(ζ (0～(n-1)) )≠0, L(ζ (0～(n-1)) )∈R r×r , R r×r Represents a real matrix with r rows and r columns; u represents the control quantity of the system, u∈R r , the system is a fully-driven non-planar six-rotor UAV; The control quantity u of the system is written in the following form: u=u0+u a (7) Where u0 represents the nominal part of the controller; u a represents an auxiliary controller; The nominal part u0 of the controller is expressed as follows: In the formula, u * represents the robust controller; ε represents a positive number, The norm of the disturbance to the system The square of represents the norm of the disturbance to the system, W L (M (0～(n-1)) ) represents the parameter matrix, and the superscript T represents the transposition; ζ *(n) represents the desired system state ζ * The nth derivative of ; The auxiliary controller u a It is expressed as follows: Among them, * represents the desired system state, The first derivative representing the desired system state; σ1, σ2, k p , k d are all constants and satisfy 0<σ1<1,σ2=2σ1 / (1+σ1), k p >0,k d >0; represents sig(ζ * -ζ) of the σ1 order, express The σ2-order exponent of .

7. The control method of a non-planar agricultural six-rotor UAV based on a high-order all-wheel drive system according to claim 6, characterized in that: In step 31, the all-wheel drive non-planar six-rotor drone model constructed in step 1 is converted into a high-order all-wheel drive system model; the expression is: Where ζ represents the system state variable, ζ==[x,y,z,φ,θ,ψ] T , x represents the position coordinate of the drone on the x-axis, y represents the position coordinate of the drone on the y-axis, z represents the position coordinate of the drone on the z-axis, φ represents the roll angle of the drone, θ represents the pitch angle of the drone, ψ represents the yaw angle of the drone, and the superscript T represents the transposition; The system is an all-wheel drive non-planar six-rotor drone; represents the second-order derivative of the system state variable, represents the second-order derivative of the drone's position along the x-axis, represents the second-order derivative of the drone’s position along the y-axis, represents the second-order derivative of the drone's position along the z-axis, Represents the second-order derivative of the UAV's roll angle, Represents the second-order derivative of the pitch angle of the drone, Represents the second derivative of the yaw angle of the drone; f(ζ (0～1) ) represents a continuous state function, f(ζ (0～1) )=[0,0,-g,0,0,0] T , g represents the earth's gravitational acceleration, ζ (0～1) represents the 0th to 1st order derivative of ζ, Represents the first-order derivative of the system state variable; L(ζ (0～1) ) represents a continuous matrix function, Re represents a diagonal matrix, a, b, α p , α q , α r All represent coefficients; T d represents the external disturbance function of the system, T d =[d1,d2,d3,d4,d5,d6] T , T d ∈R r ; d1 represents the disturbance of the coordinates of the drone's position on the x-axis; d2 represents the disturbance of the coordinates of the drone's position on the y-axis; d3 represents the disturbance of the coordinates of the drone's position on the z-axis; d4 represents the disturbance of the drone's roll angle; d5 represents the disturbance of the drone's pitch angle; and d6 represents the disturbance of the drone's yaw angle.

8. The control method of a non-planar agricultural six-rotor UAV based on a high-order all-wheel drive system according to claim 7, characterized in that: In step 32, an error tracking model is obtained based on a high-order all-wheel drive system model; the specific process is as follows: Assume that the desired position of the hexacopter is ζ * =(x * ,y * ,z * ,φ * ,θ * ,ψ * ) T ,and and Is continuous and differentiable, let the deviation Δ = ζ-ζ * ; In the formula, x * Indicates the desired coordinate of the drone on the x-axis, y * Indicates the desired coordinate of the drone on the y-axis, z * represents the desired coordinate of the drone on the z-axis, φ * represents the desired roll angle of the drone, θ * represents the desired pitch angle of the UAV, ψ * represents the desired yaw angle of the drone, represents the first-order derivative of the desired drone state, represents the second-order derivative of the desired drone state; The high-order all-wheel drive system model is converted into an error tracking model; the expression is: In the formula, represents the second-order derivative of the deviation Δ; Δ (0～1) represents the deviation matrix, represents the first derivative of the deviation Δ; f(Δ (0～1) ) represents a continuous state function, f(Δ (0～1) )=[0,0,-g,0,0,0] T ; L(Δ (0～1) ) represents a continuous matrix function, Re represents a diagonal matrix, a, b, α p , α q , α r All represent coefficients.

9. The control method of a non-planar agricultural six-rotor UAV based on a high-order all-wheel drive system according to claim 8, characterized in that: In step 33, a controller is designed based on the error tracking model (14); the specific process is: The controller is shown below u=u0+u a (15) Among them, β1, β2, k p , k d are all constants, 0<β1<1, β2=2β1 / (1+β1), k p >0,k d >0; M (0～1) represents the matrix, M (0～1) =[M0 M1] T ; u * represents the robust controller; The norm of the disturbance to which the system is subjected The square of represents the norm of the disturbance to which the system is subjected; W L (M 0～1 ) represents the parameter matrix, and the superscript T represents the transpose; represents the desired UAV system state ζ * The second derivative of Represents the function sig(ζ * -ζ) β1-order index; Representation function The β2-order index of .

10. The control method of a non-planar agricultural six-rotor UAV based on a high-order all-wheel drive system according to claim 9, characterized in that: The parameter matrix W L (M 0～(n-1) ) is obtained as follows: 1) Set matrix P and matrix Q to satisfy detN(Q,P)≠0; The matrix P represents a real matrix with nr rows and nr columns, the matrix Q represents a real matrix with r rows and nr columns, and det N(Q,P) represents the rank of the matrix N(Q,P); The rank of the matrix N(Q,P) is expressed as: Where P n-1 represents the n-1 power of the matrix P; 2) Based on the matrix N(Q,P) and matrix P, construct the matrix Γ(M 0～(n-1) ); the expression is: Γ(M 0～(n-1) )=N(Q,P)PN -1 (Q,P) (19) 3) Based on matrix N(Q,P), matrix P, and matrix Q, construct matrix M 0～(n-1) ; The expression is: M 0～(n-1) =-QP n N -1 (Q,P) (20) 4) The matrix Γ(M 0～(n-1) ), matrix M 0～(n-1) Substitute the Riccati equation and solve the matrix W(M 0～(n-1) ); Riccati equation forC T (M 0～1 )W(M 0～(n-1) )+W(M 0～(n-1) )C(M 0～1 )≤-κW(M 0～(n-1) ); In the formula, κ represents an arbitrarily selected positive number, κ>0; 5) Based on the matrix W(M 0～(n-1) ) Construct the matrix W L (M 0～(n-1) ); the expression is: In the formula, I r represents an r-row and r-column identity matrix.

Images