Load state estimation and control method for tandem unmanned aerial vehicle suspended load system

Abstract

The invention relates to a tandem unmanned aerial vehicle suspended load system load state estimation and control method, which comprises the following steps of: establishing a dynamical model of an unmanned aerial vehicle suspended load system based on a Newton's law of motion, including dynamical models based on two different state variables respectively; an observer based on a Kalman filter is constructed based on the dynamical model corresponding to the first state variable, cable tension acting on the load is measured through a multi-axial force sensor and serves as known input of the observer, and load state estimation is achieved; and converting the load state estimation result under the first state variable into a second state variable, and constructing a nonlinear backstepping controller with a saturation mechanism in combination with a kinetic model corresponding to the second state variable, thereby realizing trajectory tracking control of the unmanned aerial vehicle suspended load system. Compared with the prior art, the change characteristics of the load state can be accurately captured, the complexity caused by sensor installation and wiring is avoided, and the method has the advantages of being high in real-time performance, high in control precision and the like.

Description

Technical Field

[0001] The present invention relates to the field of state estimation and control of aerial cargo transportation, and in particular to a load state estimation and control method for a tandem unmanned aerial vehicle (UAV) hoisting system based on a force sensor. Background Art

[0002] In recent years, multi-rotor drones (UAVs) have gained widespread application in material transportation due to their enhanced flight maneuverability, more precise control capabilities, and vertical take-off and landing capabilities. Compared to manned rotorcraft, multi-rotor UAVs offer significant advantages in terms of labor risk and transportation costs. There are two primary methods for attaching the payload to the drone during material transportation: directly attaching the payload to the drone, or slinging the payload from below via a tether. Generally speaking, the former imposes significant restrictions on the payload's shape, size, and mass, making the drone particularly cumbersome when transporting large components. While the latter increases the system's degrees of freedom and may increase underactuation, it imposes fewer restrictions on the payload's shape, size, and mass. Especially when transporting large components, multi-rotor UAVs can coordinate and lift the payload without compromising the drone's maneuverability. Therefore, the latter presents a broader development prospect.

[0003] Compared to existing fixed-type transport systems for rotary-wing UAVs, the lifting systems for rotary-wing UAVs have relatively looser restrictions on payload weight and shape, and offer a wider range of application scenarios. However, adopting a lifting method also presents some new challenges, one of which is how to effectively monitor the payload's status. Currently, various methods for monitoring payload status have been developed both domestically and internationally. Some studies use visual sensors to detect visual tags attached to the payload, connect cables to universal joints equipped with angle encoders, or use inertial measurement units to detect the relative posture between the payload and the UAV. For example, CN117709120A discloses a method, device, and medium for estimating the payload status of a rotary-wing UAV lifting system. The method comprises: Step S1, establishing a dynamic model of the rotary-wing UAV lifting system based on Newton's laws of motion; Step S2, identifying the tags attached to the payload using a real-time vision-based tag detection algorithm, and outputting the tag's coordinates within the UAV system, i.e., visual localization results, based on the camera model and cable length constraints; Step S3, combining the visual localization results output from Step S2 with the rotary-wing UAV lifting system dynamics model from Step S1, and employing a hybrid extended Kalman filter to estimate the payload status in real time. Although these methods can solve the problem to a certain extent, they still have some limitations: the solution of attaching sensors to the load will affect the movement of the load, and the solution using vision cannot overcome the influence of sunlight when working outdoors, and the load swings beyond the camera's field of view, causing the algorithm to fail. Summary of the Invention

[0004] The purpose of the present invention is to provide a load state estimation and control method for a tandem UAV lifting system based on a force sensor. The three-dimensional force sensor is used to realize real-time and high-precision estimation of the load state when the rotary-wing UAV is lifting cargo outdoors, and to implement nonlinear backstepping control that takes into account disturbance compensation and saturation characteristics.

[0005] The purpose of the present invention can be achieved by the following technical solutions:

[0006] The present invention provides a load state estimation and control method for a tandem UAV hoisting system based on a force sensor, comprising the following steps:

[0007] S1, establishing a dynamic model of the UAV lifting system based on Newton's laws of motion, wherein the dynamic model includes dynamic models based on two different state variables;

[0008] S2, constructing an observer based on a Kalman filter based on the dynamic model corresponding to the first state variable, using a multi-axis force sensor to measure the cable tension acting on the load and using the measured tension as a known input of the observer to achieve load state estimation;

[0009] S3, converts the load state estimation result under the first state variable to the second state variable, and constructs a nonlinear backstepping controller with a saturation mechanism in combination with the dynamic model corresponding to the second state variable to realize trajectory tracking control of the UAV hanging system, wherein the nonlinear backstepping controller includes three parts: load position control, cable direction control and longitudinal attitude control.

[0010] In the UAV hoisting system, the tandem UAV and the payload meet the following assumptions when the fixed rope length is long:

[0011] The drone is a simple single rigid body, with its geometric center coinciding with its center of gravity;

[0012] The load is a single mass point, regardless of its shape;

[0013] The rope is massless, inelastic and taut, with the center of mass of the drone and a point load at either end.

[0014] The dynamic model of the UAV lifting system includes three coordinate systems:

[0015] A body coordinate system, which is fixed to the UAV body and is represented by B;

[0016] A force sensor coordinate system, which is fixed to the force sensor, the coordinate system is denoted as S, and is set to coincide with the body coordinate system;

[0017] Inertial coordinate system, denoted as I.

[0018] In the UAV hoisting system, the dynamic model of the UAV is expressed as:

[0019]

[0020] in, is the angular velocity of the tandem aircraft in the body coordinate system, R∈SO(3) is the rotation matrix from the drone body coordinate system to the world coordinate system, and S is an operator. For two three-dimensional vectors m and n, S(m)n=m×n, is the time derivative of R;

[0021] The system's overall state is defined using two different state variables η and x:

[0022]

[0023] in, and Represent the position and speed of the tandem UAV, and represent the position and speed of the load, respectively. and represents the unknown system dynamics, is the angular velocity of the cable, Denotes the direction vector of the cable, D η 、D x are the domains of η and x,

[0024] The dynamic model corresponding to the first state variable η is expressed as:

[0025]

[0026] in, is the acceleration due to gravity, and denote the total thrust of the rotor on the tandem UAV and the cable tension acting on the load, respectively. The superscript · denotes the derivative, and e3 = [0 0 1] T , and They represent the mass of the tandem UAV and the payload respectively. The mass of the UAV and the payload is unknown. The unknown parts are included in b Q and b L In, use and represents a known nominal term, then:

[0027]

[0028] Among them, dQ and d L are the time-varying external disturbances on the tandem aircraft and the payload, respectively;

[0029] The total thrust f is always aligned with the z-axis of the drone's coordinate system, and:

[0030] f=-Tr3

[0031] in, Indicates the z-axis direction of the body coordinate system, It is thrust;

[0032] Cable tension F C Always aligned with the cable direction, and:

[0033] F C =-T L q

[0034] in, The tension in the cable, represents the direction vector of the cable;

[0035] After deduction, the dynamic model corresponding to the second state variable x is expressed as:

[0036]

[0037] Among them, P q =qq T , l is the radius of the circular motion of the load relative to the center of mass of the longitudinal column.

[0038] The observer is expressed as:

[0039]

[0040] Among them, A, B, and C are the system matrix, input matrix, and output matrix of the linear system respectively. is the Kalman gain, is the error covariance matrix, and is an adjustable matrix, is the estimated value of the first state variable η(t), u(t)=[F C T f T ge3 T ] T , In the adjustable matrix A disturbance term is added to track the time-varying disturbance.

[0041] The converting of the load state estimation result under the first state variable to the second state variable specifically comprises the following steps:

[0042] Define the transformation as:

[0043]

[0044] in, The superscript ∧ indicates an estimated value rather than a true value.

[0045] The transformation is about The partial derivative of is:

[0046]

[0047] in

[0048] definition

[0049] The cable tension is then expressed as:

[0050]

[0051] definition in is the estimated cable tension, defined as

[0052] Then the load state under the second state variable x is estimated to be:

[0053]

[0054] Given a vector Its time derivative is divided into two parts: in is the nominal term when the estimation error is zero, is an additional term caused by the estimation error, then in as well as

[0055] The load position control is specifically as follows:

[0056] set up is the desired trajectory of the load, and the load position and velocity errors are defined as and The coupling error is defined as e: = k1(z p +βz v ), where l1>0 and β>0 are control gains;

[0057] The first candidate Lyapunov function is defined as in is the saturation gain, and the time derivative of V1 is:

[0058]

[0059] in σ is a saturation function, i.e. σ(x) = tanh(x);

[0060] Combined with the load state estimation under the second state variable x, we get:

[0061]

[0062] in, is the controlled gain, is the saturation gain,

[0063] definition Rewrite the derivative of V1 as:

[0064]

[0065] The required thrust f d Parallel to The component of is defined as: The desired cable direction is defined as but: The cable direction control is specifically as follows:

[0066] Given a dependency Vector Its time derivative is expressed as:

[0067]

[0068] The cable direction error is defined as The second Lyapunov function is defined as in is the matching parameter, The time derivative of V2 is:

[0069] q d The time derivative of is written as: in, Then simplifying it, we get:

[0070]

[0071] in is the control gain, W2:=W1+k q ||z q || 2 , The required cable angular velocity is designed as:

[0072]

[0073] Its time derivative is given by:

[0074]

[0075] Define cable angular velocity error Will Written as:

[0076]

[0077] z ω The time derivative of is given by:

[0078]

[0079] in

[0080] The third Lyapunov function is defined as in is a matching parameter, then the time derivative of V3 is derived as:

[0081]

[0082] Where W3:=W2+k ω ||z ω || 2 , is the control gain,

[0083] Based on the time derivative of the third Lyapunov function, the required thrust f is determined d perpendicular to The weight is:

[0084]

[0085] Parallel to and perpendicular to The thrust components of the combined thrust f d for:

[0086]

[0087] but Rewritten as:

[0088]

[0089] The tandem posture control is specifically as follows:

[0090] The desired total motor thrust, desired tandem posture, and total motor thrust are designed as follows:

[0091]

[0092] but in, I is the identity matrix,

[0093]

[0094] r 3d The time derivative of is given by: in, Represents r 3d The nominal term of the time derivative of Represents r 3d Additional terms of the time derivative of ;

[0095] The attitude error is defined as Then the final Lyapunov function is defined as

[0096]

[0097] in is the matching parameter,

[0098] The time derivative of V is derived as:

[0099]

[0100] in, is the control gain, and

[0101] The tandem UAV uses angular velocity as the control input and eliminates Indefinite term in:

[0102]

[0103] Among them, Ω3 represents the third component of Ω, Used to control the tandem yaw angle ψ and the required yaw angle ψ d Stay in sync.

[0104] Compared with the prior art, the present invention has the following beneficial effects:

[0105] 1. By combining the rope tension measured by the force sensor with the system dynamics model, the Kalman filter-based observer designed in this invention can accurately capture the changing characteristics of the load state, has high robustness and adaptability, and can effectively cope with external disturbances and complex operating scenarios during the lifting process.

[0106] 2. The measurement method based on three-dimensional force sensors adopted in the present invention does not rely on complex visual sensors or external sensor support. It can achieve real-time estimation of the load state directly through force sensor feedback, avoiding the complexity brought by sensor installation and wiring, and reducing the impact of sensor installation on the dynamic characteristics of the lifting system.

[0107] Third, the control algorithm designed in this invention incorporates disturbance compensation and saturation characteristics. The nonlinear controller, designed using backstepping, efficiently and dynamically compensates for external disturbances in real time, ensuring that control inputs operate within the hardware's capabilities. This method offers strong real-time performance and meets the precise control requirements of UAV hoist systems in dynamic and complex scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0109] Figure 2 The coordinate system and vector definition diagram for the tandem UAV hoisting system;

[0110] Figure 3 A diagram showing simulation results in one embodiment. DETAILED DESCRIPTION

[0111] The present invention is described in detail below with reference to the accompanying drawings and specific embodiments. This embodiment is implemented based on the technical solution of the present invention, and provides a detailed implementation method and specific operation process, but the protection scope of the present invention is not limited to the following embodiments.

[0112] Example 1

[0113] This embodiment provides a load state estimation and control method for a tandem UAV hoisting system based on a force sensor. Figure 1 As shown, the following steps are included:

[0114] S1, establish the dynamic model of the UAV lifting system based on Newton's laws of motion, including dynamic models based on two different state variables;

[0115] S2, constructing an observer based on a Kalman filter based on the dynamic model corresponding to the first state variable, using a multi-axis force sensor to measure the cable tension acting on the load and using the measured tension as a known input of the observer to achieve load state estimation;

[0116] S3, converts the load state estimation result under the first state variable to the second state variable, and constructs a nonlinear backstepping controller with a saturation mechanism in combination with the dynamic model corresponding to the second state variable to achieve trajectory tracking control of the UAV hanging system.

[0117] In this invention, the following assumptions are made for the tandem drone and payload when the fixed rope is long:

[0118] 1) The drone is a simple single rigid body, and its geometric center coincides with its center of gravity.

[0119] 2) The load is a single mass point and its shape is not considered.

[0120] 3) The rope is massless, inelastic and taut, with the center of mass of the drone and the point load at both ends.

[0121] First, the dynamic model of the UAV lifting system is established.

[0122] There are three coordinate systems in the dynamic model, such as Figure 2 As shown, including:

[0123] (1) Body coordinate system, which is fixed to the UAV body and is represented by B.

[0124] (2) The force sensor coordinate system is fixed to the force sensor. The coordinate system is represented by S. In the present invention, it is set to coincide with the body coordinate system without loss of generality.

[0125] (3) Inertial coordinate system, denoted as I.

[0126] The kinematic model of the UAV can be expressed as:

[0127]

[0128] in is the angular velocity of the tandem machine in the body coordinate system, R∈SO(3) is the rotation matrix from the drone body coordinate system to the world coordinate system, S is an operator, for two three-dimensional vectors m,n, S(m)n=m×n, is the time derivative of R. It is worth noting that the present invention assumes that the tandem UAV has a high-gain inner-loop controller that adjusts the torque vector to track the reference angular velocity, so the angular velocity is considered as a control input that can go in any direction.

[0129] The position and velocity of the column are respectively given by and The position and velocity of the load are represented by and The dynamics of the UAV lifting system is described by the following equation:

[0130]

[0131] in is the acceleration due to gravity, and denote the total thrust of the rotor on the tandem UAV and the cable tension acting on the load, respectively. The superscript · denotes the derivative, and e3 = [0 0 1] T The mass of the tandem UAV and the payload are expressed as and express. and Represents the unknown system dynamics. Note that in this invention, the mass of the drone and the payload is partially unknown, and the unknown parts are included in b Q and b L In. Use and Denotes a known nominal term. The vector and The system dynamics representing the unknown is shown below:

[0132]

[0133] where d Q and d L are the time-varying external disturbances on the tandem aircraft and the payload, respectively.

[0134] vector and The total thrust f is always aligned with the z-axis of the UAV coordinate system and is equal to

[0135] f=-Tr3 (8)

[0136] in Indicates the z-axis direction of the body coordinate system, It's thrust.

[0137] Cable tension f C Always aligned with the cable direction, equal to

[0138] F C =-T L q (9)

[0139] in The tension in the cable, The direction vector of the cable.

[0140] So far, about the cable tension F CThe information about the cable is limited because the dynamics of the cable orientation is unknown. In the following analysis, the system dynamics will be analyzed in more depth.

[0141] Since the cable is always taut, the load moves in a circle with a radius of l relative to the center of mass of the longitudinal column. Figure 2 As shown, there are

[0142] lq=p L -p Q (10)

[0143]

[0144] in is the angular velocity of the cable.

[0145] Taking the time derivatives on both sides, we can deduce from equations (10) and (11) that It is worth noting that due to and the circular motion of the load, there is and Combining these, there are

[0146]

[0147] Using equations (3) and (5), we have

[0148]

[0149] Note that F C is aligned with q, and the derivative of ω can be derived by multiplying S(q) on both sides of Equation (13) as follows:

[0150]

[0151] Substituting equations (12) and (14) into the sum of equations (3) and (5), we obtain

[0152]

[0153] Two different state variables η and x are used to define the overall state of the system:

[0154]

[0155] Among them, D η 、D x are the domains of η and x,

[0156] The dynamics of the system corresponding to the state variable η is given in Equations (2)-(5). The UAV hoisting system model corresponding to the state variable x can be rewritten as follows:

[0157]

[0158] Among them, P q =qq T .

[0159] Next, we design an observer based on Kalman filtering.

[0160] Despite having different state variables, the two states η and x are essentially equivalent because they both describe the same UAV loading system. And there is a one-to-one correspondence between them. From the perspective of observer design, a multi-axis force sensor can be used to measure F C , thus treating it as a known input to the observer. Furthermore, the simple structure of the η system allows for the implementation of a high-performance observer by leveraging a wide range of existing techniques in linear system theory. Therefore, this paper designs a Kalman filter-based observer for the η system and subsequently a nonlinear backstepping controller for the x system. Given that the position of the tandem UAV is known, the onboard triaxial force sensor plays a central role in estimating the load state.

[0161] Without loss of generality, we assume that the sensor coordinate system {S} is aligned with the body coordinate system {B}. Then, the measured cable tension is represented by {I} for

[0162]

[0163] in is the raw measurement of cable tension, expressed as {S}, assumed to be non-zero, and is the rotation matrix from {S} to {I}. Equations (9) and (10) inspire the position of the measured load relative to the longitudinal equation as follows:

[0164]

[0165] Based on equations (2)-(5), a continuous-time linear time-invariant (CT-LTI) system can be expressed as follows:

[0166]

[0167] y(t)=Cη(t) (25)

[0168] in

[0169]

[0170] Among them, A, B, and C are the system matrix, input matrix, and output matrix of the linear system respectively, and u(t)=[F CT f T ge3 T ] T ,

[0171] The present invention proposes to use a Kalman-based observer to first establish the consistent complete observability of the system, which is stronger than the standard observability:

[0172] For the system (24)-(25), the observability Gramian matrix is given by:

[0173]

[0174] Where δ is any real number greater than 0. The observability Gramian matrix is independent of time t. After inspection, all principal minors of the observability Gramian matrix are greater than 0. Since is real symmetric, and all its eigenvalues are positive real numbers. and Then, Therefore, the system is uniformly fully observable.

[0175] The Kalman filter-based observer proposed in the present invention is as follows:

[0176]

[0177] in, is the Kalman gain, is the error covariance matrix, and its update law is given by equation (34), and is the adjustable matrix in the experiment, is an estimate of η(t), whose update law is given by (32).

[0178] Note that the time derivative of the disturbance term is not included in Equation (24). This is because the time derivative of the disturbance term is unknown and therefore cannot be considered in the observer. However, by using the adjustable matrix By adding the corresponding terms in , the Kalman filter-based observer can still track the time-varying disturbance. Despite the model mismatch, the experimental results show that the estimation error remains within a small range near zero.

[0179] Now consider the time derivative of the disturbance. Then the system dynamics (24) can be rewritten as:

[0180]

[0181] in,

[0182]

[0183] Assume that the unknown system dynamics term b Q (t),b L (t) and its derivative with respect to time is bounded. Then for systems (35) and (25), the estimation errors given by the Kalman-based observers (32)-(33) are uniformly and ultimately bounded, as shown below:

[0184] The estimation error is defined as:

[0185]

[0186] According to the properties of the Kalman filter, when the derivative of the unknown dynamics term is 0, is an exponentially stable equilibrium point. Let the Lyapunov function be defined as From this we can see that the Lyapunov function is bounded above and below Among them, λ max ,λ min are the maximum and minimum eigenvalues of the matrix respectively. From (24), (25), (32) and (33), we can see that the derivative of the Lyapunov function is given by Given, where Then there are because have Now consider the systems (35) and (25). Assume For all t ≥ t0, for all in And it holds for a certain constant θ∈(0,1). So, for all And for some T>0, there is

[0187]

[0188] From formula (39), we can conclude that the estimation error of the observer is uniformly bounded. The proof is complete.

[0189] In order to convert the state estimate of the η system to the state estimate of the x system, the transformation is defined as:

[0190]

[0191] in, The superscript ∧ indicates an estimate rather than a true value. Note that this transformation is actually the same as mapping from the η system to the x system, i.e. x = T(η). The partial derivative of

[0192]

[0193] in

[0194] It has been proven that the estimation error of the Kalman filter-based observer is uniformly bounded. In order to ensure that the final control error remains bounded, the estimation error after transformation It is also proven to be uniformly eventually bounded:

[0195] for have holds for all t≥t0. This implies that and Since ||p L -p Q ||=l, so For all t≥t0. Assume l-2r>0, and consider It can be concluded that and is continuous on D. Let the Lipschitz constant be Using local Lipschitz continuity, for all t≥t0+T and for all have

[0196]

[0197] Further analysis is necessary to derive the dynamics of the estimated x-state, which is crucial for controller design. To simplify the notation, we now define

[0198] According to equations (5) and (19), the cable tension can be expressed as

[0199]

[0200] definition in is the estimated cable tension, defined as According to the time derivatives of (32) and (40), the time derivative of the estimated x state can be expressed as

[0201]

[0202] To simplify the notation, given a vector Its time derivative is divided into two parts: where ∈R is the nominal term when the estimation error is zero (i.e. ),and is an additional term due to the estimation error. Then we have in as well as

[0203] Finally, a nonlinear backstepping controller is designed which takes disturbance compensation into consideration and has saturation characteristics.

[0204] According to Equations (44)-(47), it can be observed that the system has highly coupled and highly underdriven nonlinear dynamics. To cope with such a complex system, the subsequent analysis of this embodiment will be divided into three parts: load position control, cable direction control, and tandem attitude control.

[0205] (1) Load position control

[0206] First, suppose is the desired trajectory of the load. The load position and velocity errors are defined as and The coupling error is defined as e: = k1(z p +βz v ), where k1>0 and β>0 are control gains. To reduce the load position and velocity errors to zero, the first Lyapunov function candidate is defined as in is the saturation gain. The time derivative of V1 is given by

[0207]

[0208] in By substituting (44) and (45) into Add and subtract items k2‖γ‖ 2 and get

[0209]

[0210] in is the controlled gain, is the saturation gain, σ is a saturation function, that is, σ(x) = tanh(x). By definition Rewrite the derivative of V1:

[0211]

[0212] Note that the first three terms in the brackets of equation (52) are the same as Align. If n is equal to If the two terms are aligned, the term in the brackets can be eliminated by adjusting the thrust f. This observation motivates the following two intermediate goals: 1) Eliminate the term by manipulating the thrust f 2) Alignment With n.

[0213] To achieve the first goal, the required thrust f d Parallel to The component is defined as

[0214]

[0215] To achieve the second goal, the desired cable direction is defined as

[0216]

[0217] therefore

[0218]

[0219] (2) Cable direction control

[0220] This part aims to design f according to the rope direction error and rope angular velocity error d Perpendicular to The weight of To keep the notation simple, given a Vector Its time derivative is expressed as

[0221]

[0222] There are two reasons for using this expression: 1) to combine the current unknown term f with the nominal term Separation helps in designing the final because is a known term. 2) f and f d The error between will be processed in the tandem attitude control section to generate the tandem angular velocity control input.

[0223] The cable direction error is defined as To reduce the cable orientation error to zero, the second Lyapunov function is defined as in is the matching parameter. Please note that The time derivative of V2 is given by

[0224]

[0225] To continue, press qd The time derivative of is written as:

[0226]

[0227] It is worth noting that By substituting (52) and (58) into (57) and adding and subtracting one term in (57) get

[0228]

[0229] in is the control gain, W2:=W1+k q ||z q || 2 ,

[0230] The required cable angular velocity is designed as

[0231]

[0232] Its time derivative is given by

[0233]

[0234] By defining the cable angular velocity error Can be written as

[0235]

[0236] z ω The time derivative of is given by

[0237]

[0238] in The third Lyapunov function is defined as in is the matching parameter. By substituting Equation (63) into the time derivative of V3, we can obtain

[0239]

[0240] By adding or subtracting k in equation (64) ω ||z ω || 2 And note get

[0241]

[0242] Where W3:=W2+k ω||z ω || 2 , is the control gain, as well as Now the required thrust f can be designed d perpendicular to The amount

[0243]

[0244] Parallel to and perpendicular to The thrust components of the combined thrust f d It can be written as

[0245]

[0246] therefore, Can be written as

[0247]

[0248] (3) Tandem attitude control

[0249] If the thrust f can be adjusted arbitrarily, then the goal is to set f = f d However, since a tandem can only generate thrust along the z-axis of its body frame, a tandem attitude subsystem needs to be addressed.

[0250] By designing the expected total thrust of the motor, the expected tandem posture and the total thrust of the motor,

[0251]

[0252] It can be concluded that in, I is the identity matrix.

[0253] Therefore, there are

[0254]

[0255] r 3d The time derivative of is given by

[0256]

[0257] in, Represents r 3d The nominal term of the time derivative of Represents r 3d The additional term of the time derivative of .

[0258] The attitude error is defined as The final Lyapunov function is defined as

[0259]

[0260] in is the matching parameter. Please note that and So the time derivative of V can be written as

[0261]

[0262] It is worth noting that By subtracting and adding The time derivative of V can be expressed as

[0263]

[0264] in is the control gain, and

[0265] Tandem UAVs use angular velocity as control input, and now you can set Ω to eliminate Since S(e3) exists outside the brackets, the third component of Ω can be freely designed. In this case, define

[0266]

[0267] Where Ω3 represents the third component of Ω. In the present invention, it is designed as To make the tandem yaw angle ψ and the required yaw angle ψ d Stay in sync.

[0268] Note that throughout the controller design, it is assumed that And r3≠-r 3d , for all t≥t0. This assumption is reasonable because and r3=-r 3d The probability measure of occurrence is 0.

[0269] This embodiment completes the stability analysis of the closed-loop system.

[0270] Considering the UAV loading system described in (1), (18)-(21), the Kalman-based observer expressed in (32)-(34), the mapping law given in (40), and the feedback control laws given in (71) and (77), for any positive gain k1, β, k q , k ω as well as Tracking error p L -p d, QQ d ,ω-ω d and r3-r 3d It is always bounded in the end, as shown below:

[0271] Consider the Lyapunov function V given in Eq. (74) and define

[0272]

[0273] Clearly, V is positive definite with respect to z. Using the feedback control law given in (71) and (77), the time derivative of V can be written as

[0274]

[0275] in set up Make This holds true for all t≥t0. So we have

[0276]

[0277] The result in (80) means that z is eventually consistent with Directly, we have r3-r 3d Eventually uniformly bounded. and Eventually uniformly bounded, there is qq d and ω-ω d Eventually uniformly bounded. is eventually uniformly bounded, and It is bounded by design, so z v is eventually uniformly bounded, so is eventually uniformly bounded. By using a sufficiently large k s1 , The boundedness of implies the boundedness of e, so p L -p d It is ultimately uniformly bounded. The proof is complete.

[0278] Example 2

[0279] This example provides an application process of the UAV load state estimation and backstepping control method based on a force sensor in Example 1, including the following steps:

[0280] Step 1) Establish a dynamic model of the UAV lifting system based on Newton's laws of motion.

[0281] Step 2) Design a Kalman-like observer based on three-dimensional force sensor feedback and system model to observe the position and velocity of the load, and obtain the direction and angular velocity of the rope through the designed mapping relationship.

[0282] Step 3) Design a nonlinear backstepping controller with a saturation mechanism. Based on the observation values and expected trajectory obtained in the previous step, obtain the control input of the system to complete the control of the trajectory tracking problem of the UAV lifting system.

[0283] In this embodiment, a tandem UAV is used to carry a load. The observer and controller proposed in the present invention perform observation and control calculations in real time, so that the load tracks a predetermined trajectory. The trajectory of the load is given by the following formula:

[0284] p d =[2sin(2t)3cos(t)sin(t)-1] T

[0285] During the simulation, when the initial position error of the load is large, the UAV driven by the algorithm proposed in this invention quickly converges to the desired tracking trajectory. After a period of time, the actual trajectory basically coincides with the expected trajectory, as shown in Figure 2. Figure 3 As shown, the effectiveness of the observation and control algorithm proposed in this invention is verified.

[0286] The above describes in detail the preferred embodiments of the present invention. It should be understood that those skilled in the art can make numerous modifications and variations based on the concepts of the present invention without inventive effort. Therefore, any technical solutions that can be derived by those skilled in the art through logical analysis, reasoning, or limited experimentation based on the concepts of the present invention and the prior art should be within the scope of protection defined by the claims.

Claims

1. A load state estimation and control method for a tandem UAV hoisting system based on a force sensor, characterized in that: The following steps are involved: S1, establishing a dynamic model of the UAV lifting system based on Newton's laws of motion, wherein the dynamic model includes dynamic models based on two different state variables; S2, constructing an observer based on a Kalman filter based on the dynamic model corresponding to the first state variable, using a multi-axis force sensor to measure the cable tension acting on the load and using the measured tension as a known input of the observer to achieve load state estimation; S3, converts the load state estimation result under the first state variable to the second state variable, and constructs a nonlinear backstepping controller with a saturation mechanism in combination with the dynamic model corresponding to the second state variable to realize trajectory tracking control of the UAV hanging system, wherein the nonlinear backstepping controller includes three parts: load position control, cable direction control and longitudinal attitude control.

2. The load state estimation and control method of a tandem UAV hoisting system based on a force sensor according to claim 1 is characterized in that: In the UAV hoisting system, the tandem UAV and the payload meet the following assumptions when the fixed rope length is long: The drone is a simple single rigid body, with its geometric center coinciding with its center of gravity; The load is a single mass point, regardless of its shape; The rope is massless, inelastic and taut, with the center of mass of the drone and a point load at either end.

3. The load state estimation and control method of a tandem UAV hoisting system based on a force sensor according to claim 1 is characterized in that: The dynamic model of the UAV lifting system includes three coordinate systems: A body coordinate system, which is fixed to the UAV body and is represented by B; A force sensor coordinate system, which is fixed to the force sensor, the coordinate system is denoted as S, and is set to coincide with the body coordinate system; Inertial coordinate system, denoted as I.

4. The method for estimating and controlling the load state of a tandem UAV hoisting system based on a force sensor according to claim 1, characterized in that: In the UAV hoisting system, the dynamic model of the UAV is expressed as: in, is the angular velocity of the tandem aircraft in the body coordinate system, R∈SO(3) is the rotation matrix from the drone body coordinate system to the world coordinate system, and S is an operator. For two three-dimensional vectors m and n, S(m)n=m×n, is the time derivative of R; The system's overall state is defined using two different state variables η and x: in, and Represent the position and speed of the tandem UAV, and represent the position and velocity of the load, and represents the unknown system dynamics, is the angular velocity of the cable, Denotes the direction vector of the cable, D η 、D x are the domains of η and x, 5. The method for estimating and controlling the load state of a tandem UAV hoisting system based on a force sensor according to claim 4, characterized in that: The dynamic model corresponding to the first state variable η is expressed as: in, is the acceleration due to gravity, and denote the total thrust of the rotor on the tandem UAV and the cable tension acting on the load, respectively. The superscript · denotes the derivative, and e3 = [0 0 1] T , and They represent the mass of the tandem UAV and the payload respectively. The mass of the UAV and the payload is unknown. The unknown parts are included in b Q and b L In, use and represents a known nominal term, then: Among them, d Q and d L are the time-varying external disturbances on the tandem aircraft and the payload, respectively; The total thrust f is always aligned with the z-axis of the drone's coordinate system, and: f=-Tr3 in, Indicates the z-axis direction of the body coordinate system, It is thrust; Cable tension F C Always aligned with the cable direction, and: F C =-T L q in, The tension in the cable, represents the direction vector of the cable; After deduction, the dynamic model corresponding to the second state variable x is expressed as: Among them, P q =qq T , l is the radius of the circular motion of the load relative to the center of mass of the longitudinal column.

6. The method for estimating and controlling the load state of a tandem UAV hoisting system based on a force sensor according to claim 5, characterized in that: The observer is expressed as: Among them, A, B, and C are the system matrix, input matrix, and output matrix of the linear system respectively. is the Kalman gain, is the error covariance matrix, and is an adjustable matrix, is the estimated value of the first state variable η(t), u(t)=[F C T f T ge3 T ] T , In the adjustable matrix A disturbance term is added to track the time-varying disturbance.

7. The method for estimating and controlling the load state of a tandem UAV hoisting system based on a force sensor according to claim 6, characterized in that: The converting of the load state estimation result under the first state variable to the second state variable specifically comprises the following steps: Define the transformation as: in, The superscript ∧ indicates an estimated value rather than a true value. The transformation is about The partial derivative of is: in definition The cable tension is then expressed as: definition in is the estimated cable tension, defined as Then the load state under the second state variable x is estimated to be: Given a vector Its time derivative is divided into two parts: in is the nominal term when the estimation error is zero, is an additional term caused by the estimation error, then in as well as 8. The method for estimating and controlling the load state of a tandem UAV hoisting system based on a force sensor according to claim 7, characterized in that: The load position control is specifically as follows: set up is the desired trajectory of the load, and the load position and velocity errors are defined as and The coupling error is defined as e: = k1(z p +βz v ), where k1>0 and β>0 are control gains; The first candidate Lyapunov function is defined as in is the saturation gain, and the time derivative of V1 is: in σ is a saturation function, i.e. σ(x) = tanh(x); Combined with the load state estimation under the second state variable x, we get: in, is the controlled gain, is the saturation gain, definition Rewrite the derivative of V1 as: The required thrust f d Parallel to The component of is defined as: The desired cable direction is defined as but:

9. The method for estimating and controlling the load state of a tandem UAV hoisting system based on a force sensor according to claim 8, characterized in that: The cable direction control is specifically as follows: Given a dependency Vector Its time derivative is expressed as: The cable direction error is defined as The second Lyapunov function is defined as in is the matching parameter, The time derivative of V2 is: q d The time derivative of is written as: in, Then simplifying it, we get: in is the control gain, W2:=W1+k q ||a q || 2 , The required cable angular velocity is designed as: Its time derivative is given by: Define cable angular velocity error Will Written as: z ω The time derivative of is given by: in The third Lyapunov function is defined as in is a matching parameter, then the time derivative of V3 is derived as: in, is the control gain, Based on the time derivative of the third Lyapunov function, the required thrust f is determined d perpendicular to The weight is: Parallel to and perpendicular to The thrust components of the combined thrust f d for: but Rewritten as:

10. The method for estimating and controlling the load state of a tandem UAV hoisting system based on a force sensor according to claim 9, characterized in that: The tandem posture control is specifically as follows: The desired total motor thrust, desired tandem posture, and total motor thrust are designed as follows: but in, I is the identity matrix, r 3d The time derivative of is given by: in, Represents r 3d The nominal term of the time derivative of Represents r 3d Additional terms of the time derivative of ; The attitude error is defined as Then the final Lyapunov function is defined as in is the matching parameter, The time derivative of V is derived as: in, is the control gain, and The tandem UAV uses angular velocity as the control input and eliminates Indefinite term in: Among them, Ω3 represents the third component of Ω, Used to control the tandem yaw angle ψ and the required yaw angle ψ d Stay in sync.

Images