Robust control method suitable for four-rotor load hoisting and delivery

Abstract

The invention discloses a robust control method suitable for four-rotor load lifting and delivery, is applied to the field of automatic control of aircrafts, and aims to solve the problem of contradiction between transient response and steady-state performance of an interference estimator of conventional parameters. The invention provides control based on a time-varying uncertainty and interference estimator (TVUDE), compared with a classical uncertainty and interference estimator (UDE), the control has a time-varying interference estimation bandwidth, and due to complementary constraints, the bandwidth is optimized for performance tradeoff between interference suppression and measurement noise amplification; the time-varying interference estimator estimates and compensates time-varying interference faced by the system in the transportation process in real time; the trajectory tracking controller obtains a control signal based on an interference signal matrix output by the time-varying interference estimator, and the control signal acts on an attitude controller in the bottom layer controller to generate a torque signal required by four rotors, so that accurate trajectory tracking of a target track by the four-rotor unmanned aerial vehicle is realized. And the stability of the quad-rotor unmanned aerial vehicle at the moment of throwing the load is ensured.

Description

Technical Field

[0001] The invention belongs to the field of aircraft automatic control, and in particular relates to a robust control technology for a four-rotor lifting system. Background Art

[0002] In today's society, quadcopters are widely used in many fields due to their flexibility and compactness. At first, they were mostly used for aerial photography, but now they have been used in the agricultural field for high-altitude remote sensing detection and pesticide spraying, and in the construction field for building structure detection and unmanned on-site monitoring. In particular, due to the flexible characteristics of quadcopters, they are used in the field of logistics and transportation. In some mountainous areas, if some natural disasters such as earthquakes occur, they can more conveniently transport some emergency medical supplies and rescue supplies. Even if the transportation in the mountainous areas is not convenient, they can also use the characteristics of vertical take-off and landing to efficiently complete the rescue mission.

[0003] In order to complete the rescue mission more efficiently and to maximize the mobility of the quadcopter, for the rescue materials that need to be transported, the present invention adopts the method of lifting the packed materials, i.e., the load, under the quadcopter by a cable; at the same time, the cable tie point under the quadcopter should be the center of gravity of the quadcopter, so that when the quadcopter is disturbed by the load shaking, it can be ensured that the quadcopter will not generate additional interference torque due to the load shaking. Even so, when the quadcopter performs some rescue missions in mountainous areas, it will also encounter some problems and difficulties. Most basically, the wind in the mountainous area may be strong, which will seriously affect the stability of the quadcopter, and the shaking of the suspended load will also affect the stability of the quadcopter. Furthermore, after the quadcopter reaches the point where the load is thrown, the quadcopter needs to throw the materials to the target point by itself to ensure that the rescue time is not delayed. During this process, at the moment of payload throwing, the falling of the payload is a greater interference to the quadrotor drone, which may cause instability in the vertical direction of the quadrotor drone, that is, a larger position offset. If there are some environmental constraints in this process, the quadrotor drone may explode, which may cause certain economic losses and casualties. Summary of the invention

[0004] To solve the above technical problems, the present invention proposes a robust control method suitable for quad-rotor load lifting and delivery. The control algorithm of the present invention includes a trajectory tracking controller and a time-varying interference estimator. The trajectory tracking controller enables the quad-rotor UAV to achieve high-precision trajectory tracking of the target track in a complex and changeable environment. The time-varying interference estimator estimates and compensates for the time-varying interference faced by the system during transportation in real time, so that the quad-rotor can fly smoothly during load transportation and delivery.

[0005] The technical solution adopted by the present invention is: a robust control method suitable for four-rotor load lifting and delivery, the four-rotor UAV comprises a position controller and a bottom-level controller; the output of the position controller is used as the input of the bottom-level controller, and the bottom-level controller generates the torque signal required by the four-rotor; the position controller comprises: a nominal controller and a time-varying interference controller; the input of the nominal controller comprises a reference position state vector, a reference speed state vector, and the real-time position state vector and speed state vector of the four-rotor, and the output of the nominal controller is subtracted from the output result of the time-varying interference controller to obtain the output result of the position controller;

[0006] The input of the time-varying disturbance controller includes the initial value of the quadrotor velocity, the real-time velocity state vector of the quadrotor, and the output of the nominal controller;

[0007] The output of the time-varying interference estimator is expressed as:

[0008]

[0009] Where T(t) represents the parameters of the time-varying disturbance estimator, v(t) represents the real-time velocity state vector of the quadrotor, v(0) represents the initial velocity value of the quadrotor, and u 0 (τ) represents the output of the nominal controller;

[0010] The quadrotor load lifting and delivery process is divided into four stages, including: the initial stage of quadrotor lifting, the quadrotor lifting flight stage, the quadrotor delivery stage, and the quadrotor delivery completion stage; the T(t) values ​​of each stage are arranged from large to small as follows: the T(t) value of the initial stage of quadrotor lifting, the T(t) value of the quadrotor delivery completion stage, the T(t) value of the quadrotor lifting flight stage, and the T(t) value of the quadrotor delivery stage.

[0011] The trigonometric function is used to smoothly adjust the T(t) values ​​in adjacent stages.

[0012] The value range of T(t) in each stage is greater than 0.

[0013] Beneficial effects of the invention: In order to suppress disturbances with time-varying characteristics during quadrotor flight and payload delivery, the invention proposes a control based on a time-varying uncertainty and disturbance estimator (TVUDE). Compared with the classical uncertainty and disturbance estimator (UDE), the control has a time-varying disturbance estimation bandwidth. Due to the complementary constraint, the bandwidth is optimized for the performance trade-off between interference suppression and measurement noise amplification. The invention has the following advantages:

[0014] (1) It can estimate and compensate for the time-varying disturbances faced by the quadrotor payload lifting and delivery system;

[0015] (2) The present invention analyzes the influence of external interference and sensor measurement noise on system performance in principle, and this influence changes with the parameters, which is of great significance for parameter selection;

[0016] (3) The present invention designs corresponding controller parameters for the quad-rotor load lifting and delivery scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 This is the four-rotor lifting model targeted by the present invention;

[0018] Figure 2 The control block diagram proposed by the present invention;

[0019] Figure 3 Examples of parameters designed for the present invention;

[0020] Figure 4 It is the simulation result of the present invention on the x-axis and y-axis;

[0021] Figure 5 This is the simulation result of the present invention on the z-axis. DETAILED DESCRIPTION

[0022] To facilitate those skilled in the art to understand the technical content of the present invention, the present invention is further explained below with reference to the accompanying drawings.

[0023] The present invention proposes a robust control method suitable for quad-rotor load lifting and delivery, and the control algorithm consists of a trajectory tracking controller and a time-varying interference estimator. The trajectory tracking controller realizes the trajectory tracking of the target track by the quad-rotor UAV; the time-varying interference estimator estimates and compensates for the time-varying interference faced by the system during transportation in real time, so that the quad-rotor can fly smoothly during the load transportation and delivery process. The method of the present invention includes the following steps:

[0024] S1. To achieve robust control of the quad-rotor position controller, it is first necessary to establish a quad-rotor position controller model and a load swing interference model. Figure 1 As shown, based on Newton's laws of motion, the position controller model of the quadrotor is expressed as

[0025]

[0026] where e 3 =[0,0,1] T Represents a three-dimensional vector. The symbol "T" in the upper right corner represents the transpose of a vector or matrix. The symbol in bold indicates that this is a vector; p = [x, y, z] T represents the position state vector of the quadrotor, is the second-order derivative of p, x, y, and z represent the Euclidean position of the quadrotor in the inertial frame; Represents the velocity state vector of the quadrotor. Each element in the velocity state vector are the first-order derivatives of the elements x, y, and z in the position state vector, is the first-order derivative of v; φ, θ, ψ represent the Euler angles of the quadrotor in the inertial system. Specifically, φ represents the roll angle of the quadrotor, θ represents the pitch angle of the quadrotor, and ψ represents the yaw angle of the quadrotor; R is the rotation matrix from the quadrotor system to the inertial system; F = [0,0,-F t ] T , F t is the total thrust; m is the mass of the quadrotor; g is the acceleration due to gravity; f = [f x ,f y ,f z ] T is the disturbance signal vector of the quadrotor position controller model, which includes model uncertainty, wind disturbance and load swing disturbance. The derivatives of the disturbance are all bounded.

[0027] F t ,φ,θ are the real inputs of the quadrotor position controller model. The control signal generated by the position controller will be transmitted to the lower-level attitude controller to generate the torque signal required by the quadrotor. When the cable hanging point on the quadrotor is at the center of mass of the quadrotor, the interference generated by the load swing will not generate additional interference torque. Therefore, the focus of this invention is the design of the position controller.

[0028] The interference signal f of the position controller model mainly comes from the cable tension F caused by the load swing. c , the tension can be given by Figure 1 The load swing model in

[0029]

[0030] in are the second-order derivatives of x, y, and z, i.e., the acceleration of the quadrotor, α and β are the swing angles of the load, and are the first-order inverses of α and β, respectively. and are the second-order derivatives of α and β respectively; F c is the cable tension; L is the cable length; m p is the load mass; F D =[F Dx ,F Dy ,F Dz ] T is the air resistance term of the load. Therefore, the interference signal on the quadrotor caused by the cable tension is

[0031] mf p =R p [0,0,-F c ] T (4)

[0032]

[0033] where f p is the interference signal to the quadrotor caused by the load swing; R p is the rotation matrix from the load system to the inertial system.

[0034] The present invention aims to achieve fine interference resistance under time-varying interference for the quadrotor hoisting system. Therefore, a robust controller must be designed that can:

[0035] First, the quadrotor can actively resist external disturbances, including unknown wind and load swing disturbances, so that the estimated error vector satisfies

[0036]

[0037] in, represents the estimation error vector; ||·|| ∞ represents the infinite norm; f is the interference signal vector represented in the quadrotor model; represents the interference signal vector estimated by the interference estimator; ε is an arbitrarily small positive number, i.e., the estimation error accuracy; t ε It represents the time when the infinite norm of the estimated error vector reaches the required arbitrarily small positive number ε. This formula indicates that when time t is greater than t ε When , the infinite norm of the estimated error vector will always be smaller than the arbitrarily small positive number ε, that is, the required estimation error accuracy is achieved.

[0038] Secondly, enable the quadrotor to track the target track

[0039]

[0040] in, represents the position tracking error vector; p d represents the reference position vector of the quadrotor; p represents the position state vector; δ is an arbitrarily small positive number, i.e., the position tracking error accuracy; t δ It represents the time when the infinite norm of the position tracking error vector reaches the required arbitrarily small positive number δ. Formula (7) shows that when time t is greater than or equal to t δ When , the infinite norm of the position tracking error vector will always be smaller than the arbitrarily small positive number δ, that is, the required position tracking error accuracy is achieved.

[0041] S2. After the quadrotor position controller model is established, in order to achieve accurate anti-interference and high-precision trajectory tracking for the time-varying interference problem faced by the quadrotor flight and payload delivery, based on model (1), the controller designed by the present invention is

[0042]

[0043] The virtual control input u d =[u dx ,u dy ,u dz ] T ,Right now Figure 2 The output of the position controller is shown in 0 is a nominal controller, which can be designed as

[0044]

[0045] in is the acceleration of the desired trajectory as the feedforward term of the controller; k p =diag(k px ,k py ,k pz ) and k d =diag(k dx ,k dy ,k dz ) is the feedback gain matrix; is the trajectory tracking error, is the velocity tracking error; is the interference signal matrix generated by the interference estimator. In the design of the classic UDE with constant parameters, the signal can be designed in the frequency domain from (1) and (8) as

[0046]

[0047] in F(s), V(s) and U d (s) is the corresponding time domain signal f, v, u d G(s) is a transformation matrix designed as

[0048]

[0049] Where T xc ,T yc ,T zc >0 is the parameter of UDE, which determines the interference estimation bandwidth of UDE. By combining (10) and (11), we get the time domain expression of UDE interference estimation as

[0050]

[0051] Where T c =diag(T xc ,T yc ,T zc ), express The first-order derivative of f(t) is the time-domain signal representation of f. Since the parameters in the time-varying UDE are functions of time, it is a challenge to adopt the frequency-domain design principles similar to those of the classical UDE. Therefore, we will change the relationship described in (12) by introducing time-varying parameters, as shown below:

[0052]

[0053] Where T(t) = diag(T x (t),T y (t),T z (t)) represents the parameter matrix of TVUDE, and the elements in T(t) are greater than 0 at any time. Therefore, the present invention uses the relationship described by formula (13) to obtain the final interference estimation signal matrix In addition, the elements in T(t) are smooth, continuous and bounded and their derivatives are bounded. Combining (1), (8), and (13), we can obtain

[0054]

[0055] Then, using integration by parts, we get

[0056]

[0057] For ease of writing, subsequent time-related variables will be abbreviated to the variable itself, for example, T(t) is abbreviated to T. In (15), since we do not know the initial value of the velocity v(0), T(0) -1 v(0) will cause a peak in the interference estimate, which is called the peak phenomenon, and it can be seen that T(0) will affect the size of this peak.

[0058] The control signal u generated by the position loop d It will be transmitted to the lower-level controller to generate the thrust and torque required for quadrotor flight. The overall control block diagram is as follows: Figure 2 shown.

[0059] S3. Based on the above control design, we analyze the stability and performance of the system. We divide the system into a tracking error system and an estimation error system, and prove their stability using Lyapunov function and inequality scaling method respectively. We define the state tracking error and the disturbance estimation error of the time-varying UDE as

[0060]

[0061] The derivative of formula (17) is

[0062]

[0063] Therefore, the estimated error system is

[0064]

[0065] Then we combine formulas (8)(9)(16)(17) to obtain the tracking error system:

[0066]

[0067] in, and for and The first derivative of 3*3 Represents a 3*3 zero matrix, and all elements in the matrix are 0; I 3*3 Represents a 3*3 identity matrix, that is, only the elements on the diagonal are 1.

[0068] Then we analyze the tracking error system based on the Lyapunov function method. Yes, the tracking error system becomes

[0069]

[0070] We choose the Lyapunov function as

[0071]

[0072] When k p When all elements in are greater than 0, V is positive definite. Then, we can derive formula (22) along formula (21) to obtain

[0073]

[0074] When k d When all elements in are greater than 0, is semi-negative definite. Therefore, we can get the error system state and is bounded. Based on formula (21), we can get

[0075]

[0076] but The derivative of

[0077]

[0078] because and is bounded so is bounded, according to Barbara's lemma, Then we prove that system (21) is asymptotically stable according to the Russell invariant set theorem. When it is zero, it is easy to get So we can get In general, if and only if and hour, Therefore, it can be seen that when k p and k d When are all positive definite, the system (21) is asymptotically stable, which also means that the tracking error system (20) is asymptotically stable.

[0079] Next we prove that the estimation error system is bounded. By solving formula (19), we can get

[0080]

[0081]

[0082] Where Φ(t,0)=diag(Φ x (t,0),Φ y (t,0),Φ z (t,0)). Obviously, The solution is divided into two parts by the plus sign. Here we analyze the third dimension z-axis of formula (26) as an example (the analysis of x and y axes is the same). For the part before the plus sign, it is obvious that when T z >0, It converges at any time. For the part after the plus sign, we can use the inequality to scale and get

[0083]

[0084] in Due to T z is bounded, we assume that T zmin ≤T z ≤T zmax , so we can get

[0085]

[0086] So we can get

[0087]

[0088] Further

[0089]

[0090] Therefore, we know that for the part after the plus sign, all axes are bounded in finite time. So we have is bounded.

[0091] Based on formulas (27)(30)(31), we can analyze the relationship between the estimation error and the parameter T z The relationship is

[0092]

[0093] So we can get is a monotonically increasing function, so the estimated error Will follow T z Since the tracking error system (20) and the estimation error system (19) are in series, reducing T z It will also improve the performance of the tracking error system.

[0094] The above analysis shows that the estimation error system is asymptotically stable and we need to reduce T to improve the tracking performance in each time period, but this will increase the peak phenomenon in formula (15), which requires us to make a trade-off between different performances.

[0095] Next, we will analyze the second performance trade-off, that is, the performance trade-off under the coexistence of measurement noise and external interference. In practical applications, airborne sensors are used to measure the position and velocity of the quadrotor. However, according to practical experience, sensor measurements are always accompanied by noise. Measurement noise affects the effectiveness of the controller and the performance of the system, especially when the controller parameters change. Therefore, we will analyze the system performance under the influence of sensor measurement noise. To distinguish it from the symbols in the above design, we use an asterisk "*" in the upper right corner to indicate that the signal is affected by measurement noise.

[0096] We assume that the system state input to the controller includes noise and is therefore different from the true state, i.e.

[0097] p * =p+n 1 ,v * =v+n 2 (33)

[0098] where n 1 =[n 1x ,n 1y ,n 1z ] T and n 2 =[n 2x ,n 2y ,n 2z ] T is the measurement noise vector. Due to the influence of measurement noise, controllers (8) and (9) will be rewritten as

[0099]

[0100] Combining formula (33) and formula (34), we can get

[0101]

[0102] Under the influence of measurement noise, formula (13) is rewritten as

[0103]

[0104] And because Therefore, the interference estimation signal is rewritten as

[0105]

[0106] Solving formula (37) yields

[0107]

[0108] Substituting formula (38) into formula (35) yields

[0109]

[0110] So the tracking error system becomes

[0111]

[0112] Therefore, it can be seen from (40) that reducing the parameter T will increase the impact of measurement noise on tracking performance.

[0113] S4. Based on the above analysis, we designed the corresponding parameter T according to the application scenario of the present invention and our control objectives. The parameter design rules are as follows:

[0114] Phase 1: We selected a larger parameter at the initial stage, i.e., time zero, to mitigate the impact of the peak phenomenon at the initial time in formula (15). This parameter is Figure 3 In the first stage shown, this parameter has a larger value than that in the second stage;

[0115] Phase 2: We need to select appropriate parameters to balance the effects of load swing disturbance and measurement noise during quadrotor flight, as shown in the analysis of formula (40), and keep the parameters at a constant value. Figure 3 The value of the second stage is shown. In this parameter design rule, this value is lower than that of the first stage and higher than that of the third stage;

[0116] Phase 3: In our scenario, the quadrotor transports the payload to the target location. During payload delivery, the payload is a serious external interference to the quadrotor, as shown in formula (32): will become larger. Therefore, we need to reduce the parameter to improve the system robustness during payload delivery. This parameter is Figure 3 The value of the third stage is shown. Since the payload delivery is a serious external disturbance to the quadrotor, this value will be smaller than the value of the second stage.

[0117] Phase 4: After the payload is delivered, we gradually increase the parameter to reduce the impact of measurement noise on system performance (40), and then keep the parameter at a constant value. Figure 3 The values ​​shown in the fourth stage are generally greater than those in the second stage.

[0118] To ensure a smooth transition of parameter T, we use trigonometric functions to adjust T between its different values. z The approximate changes are as follows Figure 3 shown. Figure 3 T z The change expression is as follows

[0119]

[0120] Among them, t t Indicates the time of load throwing, corresponding to T in the figure z The time it takes to reach the lowest point. For the x and y axes, the parameters are as follows. The parameters for the x and y axes are the same:

[0121]

[0122] Figure 4 In Matlab / Simulink simulation, the quadrotor tracks the same trajectory under different methods (TVUDE of the present invention, prior art UDE, prior art PID, prior art MOBADC) ( Figure 4 ), the results shown.

[0123] Figure 4 It is a top view showing the tracking of the quadrotor using the method of the present invention and the prior art in the x and y axes. It can be seen that the tracking errors of the method of the present invention and the prior art in the x and y axes are comparable. Figure 5 The tracking situation of the quadrotor using the method of the present invention and the prior art on the z-axis is shown. Obviously, the TVUDE method used in the present invention has a more outstanding performance on the z-axis, such as Figure 5 As shown in the enlarged picture, the position error of the method of the present invention when throwing the load is smaller than these existing technologies.

[0124] Those skilled in the art will appreciate that the embodiments described herein are intended to help readers understand the principles of the present invention, and should be understood that the scope of protection of the present invention is not limited to such specific statements and embodiments. For those skilled in the art, the present invention may have various changes and variations. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of the claims of the present invention.

Claims

1. A robust control method suitable for quadrotor load lifting and delivery, characterized in that: The quadrotor drone comprises a position controller and a bottom-level controller; the output of the position controller is used as the input of the bottom-level controller, and the bottom-level controller generates the torque signal required by the quadrotor; The position controller includes: a nominal controller and a time-varying interference controller; the input of the nominal controller includes a reference position state vector, a reference speed state vector, a real-time position state vector and a speed state vector of the quadrotor, and the output of the nominal controller is subtracted from the output result of the time-varying interference controller to obtain the output result of the position controller; The input of the time-varying disturbance controller includes the initial value of the quadrotor velocity, the real-time velocity state vector of the quadrotor, and the output of the nominal controller; The output of the time-varying interference estimator is expressed as: Where T(t) represents the parameters of the time-varying disturbance estimator, v(t) represents the real-time velocity state vector of the quadrotor, v(0) represents the initial value of the quadrotor velocity, and u0(τ) represents the output of the nominal controller.

2. A robust control method for quadrotor load lifting and delivery according to claim 1, characterized in that: The output of the nominal controller is expressed as: in, Yes d The second derivative of d represents the reference position state vector; k p and k d is the feedback gain matrix; is the trajectory tracking error, p represents the position state vector of the quadrotor, is the velocity tracking error, v d represents the reference velocity state vector, and v represents the velocity state vector of the quadrotor.

3. The robust control method for quadrotor load lifting and delivery according to claim 1, characterized in that: The value of T(t) is determined based on the different motion stages of the quadrotor; The quadrotor load lifting and delivery process is divided into four stages, including: the initial stage of quadrotor lifting, the quadrotor lifting flight stage, the quadrotor delivery stage, and the quadrotor delivery completion stage; the T(t) values ​​of each stage are arranged from large to small as follows: the T(t) value of the initial stage of quadrotor lifting, the T(t) value of the quadrotor delivery completion stage, the T(t) value of the quadrotor lifting flight stage, and the T(t) value of the quadrotor delivery stage.

4. A robust control method for quadrotor load lifting and delivery according to claim 3, characterized in that: The value of T(t) in each stage is greater than 0.

5. A robust control method for quadrotor load lifting and delivery according to claim 4, characterized in that: The trigonometric function is used to smoothly adjust the T(t) values ​​in adjacent stages.

6. A robust control method for quadrotor load lifting and delivery according to claim 5, characterized in that: The parameters of T(t) on the x-axis and y-axis are the same: Among them, T x (t) represents the parameter value of T(t) on the x-axis, T y (t) represents the parameter value of T(t) on the y-axis; T(t) is calculated on the z-axis as: Among them, T z represents the parameter value of T(t) on the z-axis, t t Indicates the time when the load is dropped.

Images