Trajectory tracking and anti-swing control method for quadrotor-double pendulum system based on deep coupling of dynamics

Abstract

The application belongs to the technical field of quad-rotor double-swing unmanned aerial vehicle (UAV) swing elimination control, and provides a trajectory tracking and swing elimination control method for a quad-rotor double-swing system based on dynamic deep coupling. First, a dynamic model of the quad-rotor double-swing system is established, and a driving part in the dynamic model is simplified. Second, an energy coupling and nonlinear coupling controller is designed for trajectory tracking and swing elimination. Third, an uncertainty and disturbance estimator is designed for compensating model errors and external disturbances. Finally, the stability of a closed-loop system for tracking a time-varying reference trajectory is analyzed by using a Lyapunov method, and the trajectory tracking and swing elimination capability of the controller is analyzed by using a LaSalle invariance set principle. The application can realize trajectory tracking of a UAV body and elimination of double-swing swing of a load in the quad-rotor double-swing system.

Description

Technical Field

[0001] This invention relates to the field of anti-sway control technology for quadrotor dual-pendulum unmanned aerial vehicles (UAVs), and particularly to a trajectory tracking and anti-sway control method for quadrotor dual-pendulum systems based on deep dynamic coupling. Background Technology

[0002] In recent years, quadcopter drones have been increasingly widely used in logistics, disaster relief, and material delivery. In practical operations, drones suspend loads via flexible rigging, often forming complex double-pendulum dynamic systems due to the multi-stage connections between the hooks and loads. This system exhibits strong coupling, high nonlinearity, and underactuated characteristics, posing a significant challenge in dynamic control. However, the stable operation of double-pendulum systems in complex outdoor environments faces severe challenges. First, the double-pendulum load exhibits multi-frequency oscillations, and the energy excitation between the hooks and loads can easily lead to drone attitude instability and flight accidents. Second, external disturbances such as outdoor gusts severely affect the trajectory tracking accuracy of the aircraft and the anti-pendulum effect of the load. Existing research largely focuses on simple single-pendulum anti-pendulum methods, while effective solutions are still lacking for analyzing the complex energy coupling laws within the double-pendulum system and addressing the overall robustness of the system under external disturbances.

[0003] Therefore, in-depth analysis of the energy exchange mechanism of the dual-pendulum system and design of a robust control scheme that can balance fuselage positioning and load oscillation and has strong anti-interference capabilities are of great significance for improving the accuracy and safety of UAV hoisting operations.

[0004] See patent application CN202510881361.X, which discloses an adaptive control method and system for a two-stage pendulum rotorcraft hoisting system. The main components include: establishing a dynamic model of the two-stage pendulum rotorcraft hoisting system; taking the simultaneous driving of the four rotors to the desired position and suppression of the hook and load swing as the control objective; for outer-loop control, introducing generalized hook and load position signals and constructing an energy storage function to dissipate through the generalized hook and load position signals; converting the outer-loop tracking error into a generalized load position error to obtain the shaped total storage function; designing an adaptive law to estimate the load mass online, obtaining the reconstructed energy function of the outer-loop control, and thus obtaining the outer-loop controller to achieve control of the system displacement and load swing angle.

[0005] Therefore, this patent application significantly enhances the dynamic coupling characteristics between the quadrotor, hook, and load by employing a generalized displacement method for the hook and load and reconstructing the energy storage function, thereby effectively improving the system's transient response characteristics. However, this patent application essentially belongs to an enhanced energy coupling control scheme. Based on this scheme, the following problems still exist in the trajectory tracking and pendulum swing elimination process of the quadrotor dual-pendulum UAV:

[0006] (1) It is often difficult to accurately handle the strong nonlinear coupling of the double pendulum system, resulting in a slow pendulum elimination process or residual oscillations;

[0007] (2) When dealing with model mismatch and external interference, high-gain control is often relied upon, which places extremely high demands on the response speed and accuracy of the actuator. Summary of the Invention

[0008] The purpose of this invention is to provide a trajectory tracking and anti-sway control method for a quadcopter dual-pendulum system based on deep dynamic coupling. This method can significantly improve the anti-sway speed of the system while ensuring trajectory tracking accuracy. It not only greatly enhances the robustness of the system in complex environments, but also effectively reduces the stringent requirements on the performance indicators of the actuators.

[0009] The technical solution adopted by this invention to solve its technical problem is as follows:

[0010] A trajectory tracking and anti-slip control method for a quadrotor dual-pendulum system based on deep dynamic coupling includes the following steps:

[0011] A dynamic model of the quadrotor double pendulum system is established, and the driving part in the dynamic model is simplified.

[0012] Design a controller that combines energy coupling and nonlinear coupling for trajectory tracking and sway elimination;

[0013] Design an uncertainty and disturbance estimator to compensate for model errors and external disturbances;

[0014] The stability of a closed-loop system tracking a time-varying reference trajectory is analyzed using the Lyapunov method, and the trajectory tracking and oscillation elimination capabilities of the controller are analyzed using the Russell invariant set principle.

[0015] In some embodiments, the quadcopter dual-pendulum system is a quadcopter dual-pendulum unmanned aerial vehicle (UAV);

[0016] The position vector of the quadcopter dual-pendulum UAV in the inertial coordinate system is expressed as: In this system, These represent the masses of the quadcopter, the hook, and the load, respectively. It is the length of the cable connecting the quadcopter and the hook. It is the length of the cable connecting the hook and the load. These represent the swing angles of the hook and the load in three-dimensional space, respectively.

[0017] In a quadcopter dual-pendulum drone, the control input is defined. , This represents the rotation matrix of a quadcopter dual-pendulum UAV from its fixed coordinate system to its inertial coordinate system. yes The unit vector of the channel.

[0018] In some embodiments, the established dynamic model of the quadrotor double pendulum system is expressed as:

[0019] ;

[0020] ;

[0021] ;

[0022] ;

[0023] ;

[0024] ;

[0025] ;

[0026] in, and These are the speed and angular velocity of the quadcopter dual-pendulum drone, respectively. and These are the angular velocity and angular acceleration of the swing, respectively. These are quadcopter dual-pendulum drones Channel control input, These are the model errors and external interference of the quadcopter dual-pendulum UAV, respectively. Acceleration due to gravity; symbol They represent , , , ,in .

[0027] In some embodiments, the dynamic equations of the dynamic model are expressed as:

[0028] ;

[0029] in, Let be the state vector of the outer loop system, specifically defined as follows: , , , These are the inertial matrix, the centrifugal-Coriolis force matrix, the gravity vector, the resultant force on the position subsystem, and the external disturbance, respectively.

[0030] In some embodiments, simplifying the driving component in the dynamic model includes the following steps:

[0031] Define unit vectors pointing from the quadcopter to the hook and from the hook to the load, respectively. and Its specific expression is: ;

[0032] Define coupling terms ,in , , At this point, the dynamic model of the driving part in the coupled system containing the dynamic model is rewritten as follows:

[0033] ;

[0034] Using feedback linearization technology, virtual control input and lumped disturbance are defined, and they are expressed as follows:

[0035] ;

[0036] The dynamic model of the driving part in the coupled system is simplified and expressed as follows: ;

[0037] in, It represents the acceleration of the coupled system.

[0038] In some embodiments, the energy-coupled and nonlinearly-coupled controller of the design includes:

[0039] The tracking error is defined as follows:

[0040] ;

[0041] in, This indicates the tracking error of a quadcopter dual-pendulum drone;

[0042] The system error dynamics equation is rewritten as:

[0043] ;

[0044] The outer-loop pseudo-energy function expression for the quadrotor double pendulum system is established as follows:

[0045] ;

[0046] right Conducting discussions about time Taking the derivative, we get:

[0047] ;

[0048] in, Let represent the second derivative of the quadcopter reference signal, and ;

[0049] The composite signal and tracking error are designed as follows:

[0050] ;

[0051] in, and This is the undetermined gain, used to adjust the coupling relationship between the quadrotor's motion and oscillation response;

[0052] Rewrite the energy function as:

[0053] ;

[0054] in:

[0055] ;

[0056] ;

[0057] When the condition is met ,expression It is positive definite and integrable, therefore ,but It is a positive definite scalar function;

[0058] The control input of the double pendulum-controlled transportation system is designed based on the simplified dynamic model of the driving part, and it is expressed as follows:

[0059] ;

[0060] in, and These represent the feedback gain, It is a lumped disturbance estimate consisting of coupled model error and external disturbance, obtained through the uncertainty and disturbance estimator.

[0061] In some embodiments, the design uncertainty and disturbance estimator includes:

[0062] Based on the simplified dynamic model of the driving part, we obtain: ;

[0063] According to the UDE principle, assuming the initial state of the system is zero, the lumped disturbance estimate is expressed as:

[0064] ;

[0065] in, It is a strictly true low-pass filter matrix that can be designed to ensure the physical realizability of the estimator; therefore, the filter is designed as follows:

[0066] ;

[0067] in, These are adjustable UDE parameters used to determine the filter bandwidth;

[0068] Substituting the control input expression of the double-pendulum controlled transport system designed based on the simplified driving part's dynamic model into the lumped disturbance estimation expression, we obtain:

[0069] ;

[0070] and The explicit time-domain expression is:

[0071] ;

[0072] in .

[0073] In some embodiments, the stability analysis of the closed-loop system tracking a time-varying reference trajectory using the Lyapunov method includes the following steps:

[0074] Assumption: Expected trajectory The second derivative of is bounded, that is, for all as well as ,satisfy ;

[0075] According to the lumped disturbance estimation expression, the estimation error is defined as: ;

[0076] Where “*” represents the convolution operator, The estimation error of the lumped disturbance consists of the coupling model error and the external disturbance. The filter designed corresponding to the estimator is a strictly well-posed and stable filter with unity gain and zero phase shift within the lumped uncertainty frequency spectrum, and zero gain in other frequency bands. Established;

[0077] Choosing the composite Lyapunov function, it is expressed as: ;

[0078] in, and Representing the nominal energy of the task space, here we introduce... and As a nonnegative potential energy term constructed from geometric constraints, used to compensate for underdriven double pendulum dynamics, it is defined as follows:

[0079] ;

[0080] ;

[0081] in and Related to mass-length distribution and gain coupling;

[0082] right Taking the time derivative, we get:

[0083] ;

[0084] Substituting the defined virtual control input and the feedback linearization result of the concentrated disturbance with the design of the control input for the double-pendulum controlled transport system based on the simplified dynamic model of the driving part, we obtain:

[0085] ;

[0086] ;

[0087] Therefore, we get: .

[0088] In some embodiments, after analyzing the stability of a closed-loop system tracking a time-varying reference trajectory using the Lyapunov method, and before analyzing the trajectory tracking and oscillation elimination capabilities of the controller using the Lassel invariant set principle, the uniform boundedness of the closed-loop system is obtained. At this point, based on the Lassel invariant set principle, the outer-loop system state is analyzed within the set... The asymptotic convergence of the outer loop system state can be analyzed by finding the largest invariant set M within the inner loop.

[0089] In some embodiments, the analysis of the trajectory tracking and oscillation elimination capabilities of the controller using the Lassel invariant set principle includes the following steps:

[0090] Based on the selected composite Lyapunov function and It can be inferred that: ;

[0091] In addition, due to ,and It consists of continuous trigonometric functions, therefore we have Inside M, according to as well as We can directly deduce that: ;

[0092] After completing the cargo transport task along the predetermined trajectory, the quadcopter will maintain a designated state. At this point, the convergence of the position error is proved by contradiction, namely:

[0093] For identities Integrating both sides with respect to time, we get ,in It is an integral constant vector that needs to be determined in subsequent analysis;

[0094] from and Therefore, we can conclude that: ;

[0095] Combining formulas , , and ,get:

[0096] ;

[0097] Assumption After integration, we get: ,in, Given an undetermined constant vector, based on the formula obtained after integration, the conclusion is: ;

[0098] Will Substitution and with Comparative analysis leads to the following conclusion: ;

[0099] from It can be inferred that: ;in, It is an undetermined constant vector, according to By definition, we get: ;

[0100] Will In Substituting the channel equations into the control and From the linearized equation, we obtain: Assuming and and will The two expressions in the expression are combined to obtain: ;

[0101] Adopted and A similar method, namely integration followed by proof by contradiction, leads to the conclusion. ,at this time, Therefore, it is proven that: ;

[0102] By combining , , and The system will be rewritten as follows:

[0103] .

[0104] The beneficial effects of this invention are:

[0105] (1) By organically integrating nonlinear coupled dynamics into the control design, this invention realizes the real-time transfer and active dissipation of swing angle energy, which can significantly improve the system's swing speed while ensuring trajectory tracking accuracy.

[0106] (2) The present invention introduces an uncertainty estimator to model and compensate for internal and external disturbances in a unified manner in real time. This not only greatly improves the robustness of the system in complex environments, but also effectively reduces the stringent requirements on the performance indicators of the actuator, making it easier to implement in engineering practice where there are non-ideal factors.

[0107] (3) Based on Lyapunov stability theory, this invention constructs a complete tracking control architecture for time-varying trajectories and proves the global / semi-global consistent eventual boundedness (or asymptotic stability) of the system under time-varying reference signals. It solves the motion control problem of double pendulum system under high speed and time-varying path from a theoretical perspective. Attached Figure Description

[0108] Figure 1 This is a flowchart of the trajectory tracking and anti-slip control method for a quadrotor dual-pendulum system based on deep dynamic coupling in Embodiment 1 of the present invention;

[0109] Figure 2 This is a schematic diagram of the composition structure of the quadcopter double pendulum system used in Embodiment 1 of the present invention;

[0110] Figure 3 This is a schematic diagram of the control framework of the method proposed in Embodiment 1 of the present invention;

[0111] Figure 4 This is a schematic diagram of the simulation results of fixed-point oscillation elimination using the method proposed in Example 1 and existing methods in Embodiment 3 of the present invention;

[0112] Figure 5 This is a schematic diagram of the simulation results of spherical trajectory tracking using the method proposed in Example 1 and existing methods in Embodiment 4 of the present invention;

[0113] Figure 6 This is a schematic diagram of the simulation results comparing the robustness of fixed-point pendulum elimination using the method proposed in Example 1 of the present invention with that of existing methods in Example 5 of the present invention. Detailed Implementation

[0114] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. The components of the embodiments of the present invention described and shown in the accompanying drawings can generally be arranged and designed in various different configurations.

[0115] Example 1

[0116] This embodiment provides a trajectory tracking and anti-slip control method for a quadrotor dual-pendulum system based on deep dynamic coupling. The flowchart is shown below. Figure 1 The method may include the following steps:

[0117] S1. Establish a dynamic model of the quadrotor double pendulum system and simplify the driving part in the dynamic model;

[0118] S2. Design a controller that combines energy coupling and nonlinear coupling for trajectory tracking and sway elimination;

[0119] S3. Design an uncertainty and disturbance estimator to compensate for model errors and external disturbances;

[0120] S4. The stability of the closed-loop system tracking the time-varying reference trajectory is analyzed using the Lyapunov method, and the trajectory tracking and oscillation elimination capabilities of the controller are analyzed using the Russell invariant set principle.

[0121] See Figure 2 This is a schematic diagram of the composition of the quadcopter double pendulum system used in this embodiment. Figure 2 In the inertial coordinate system, the position vector of a quadcopter UAV is represented as: In this system, These represent the masses of the quadcopter, hook, and load, respectively. It is the length of the cable connecting the quadcopter and the hook. It is the length of the cable connecting the hook and the load; These represent the swing angles of the hook and the load in three-dimensional space, respectively. In this embodiment, control inputs can be defined. , This represents the rotation matrix of the quadrotor UAV from its fixed coordinate system to its inertial coordinate system. yes Given the unit vector of the channel, the dynamic model of the established quadcopter double pendulum system can be expressed as:

[0122] ;

[0123] ;

[0124] ;

[0125] ;

[0126] ;

[0127] ;

[0128] ;

[0129] in, and These are the speed and angular velocity of the quadcopter dual-pendulum drone, respectively. and These are the angular velocity and angular acceleration of the oscillation, respectively. These are quadcopter dual-pendulum drones Channel control input, These are the model errors and external interference of the quadcopter dual-pendulum UAV, respectively. Acceleration due to gravity; symbol They represent , , , ,in .

[0130] Here, the dynamic equations of the dynamic model can be expressed as:

[0131] ;

[0132] in, Let be the state vector of the outer loop system, specifically defined as follows: , , , These are the inertial matrix, the centrifugal-Coriolis force matrix, the gravity vector, the resultant force on the position subsystem, and the external disturbance, respectively.

[0133] At this point, the dynamic model of the driving part in the above coupled system can be simplified, such as... Figure 2 As shown, unit vectors are defined from the quadcopter to the hook and from the hook to the load, respectively. and Its specific expression is: ;

[0134] Based on this, coupling terms can be defined. ,in , , At this point, the dynamic model of the driving part in the coupled system containing the dynamic model is rewritten as follows:

[0135] ;

[0136] In this embodiment, feedback linearization technology is used to define virtual control input and centralized disturbance, which are respectively represented as follows:

[0137] ;

[0138] Finally, the dynamic model of the driving part in the coupled system is simplified, and the simplified model is expressed as: ;

[0139] in, It represents the acceleration of the coupled system.

[0140] After simplifying the dynamic model of the drive component in the aforementioned coupled system, the outer-loop controller can be considered. (See [link]) Figure 3 The control architecture diagram shown here first designs controllers for energy coupling and nonlinear coupling (which correspond to respectively). Figure 3 The energy coupling signal generator and nonlinear controller (used for trajectory tracking and wobbling elimination) in this embodiment are designed to generate energy coupling signals and nonlinear coupling signals, and include:

[0141] The tracking error is defined as follows:

[0142] ;

[0143] in, This indicates the tracking error of a quadcopter dual-pendulum drone;

[0144] The system error dynamics equation is rewritten as:

[0145] ;

[0146] The outer-loop pseudo-energy function expression for the quadrotor double pendulum system is established as follows:

[0147] ;

[0148] right Conducting discussions about time Taking the derivative, we get:

[0149] ;

[0150] in, Let represent the second derivative of the quadcopter reference signal, and ;

[0151] Based on the kinematic geometry between the quadcopter and the load, when the swing angle is zero, the positions of the hook and the quadcopter satisfy a specific geometric relationship (their horizontal coordinates coincide). Therefore, in order to ensure The tracking error of the axis approaches zero, and this invention compensates for this geometric relationship when designing composite signals. Accordingly, the composite signal and tracking error are designed as follows:

[0152] ;

[0153] in, and This is the undetermined gain, used to adjust the coupling relationship between the quadrotor's motion and oscillation response;

[0154] Therefore, the energy function is rewritten as:

[0155] ;

[0156] in:

[0157] ;

[0158] ;

[0159] After formula transformation and analysis, we can obtain the result when the condition is satisfied. ,expression It is positive definite and integrable, therefore ,but It is a positive definite scalar function.

[0160] The control input of the double pendulum-controlled transportation system is designed based on the simplified dynamic model of the driving part, and it is expressed as follows:

[0161] ;

[0162] in, and These represent the feedback gain, It is a lumped disturbance estimate consisting of coupled model error and external disturbance, obtained through the Uncertainty and Disturbance Estimator (UDE).

[0163] In this embodiment, unlike the prior art document which ignores the processing of higher-order nonlinear terms, this embodiment introduces a term including the square of the angular velocity. ) and coupling speed error ( The product compensation mechanism explicitly compensates for the centrifugal force coupling generated by the system in high dynamic motion, enabling the controller to actively convert this into a driving force to suppress sway energy based on the current motion state, thereby achieving a faster sway reduction response speed than existing technologies.

[0164] Next, an uncertainty and disturbance estimator (which corresponds to) can be designed. Figure 3 The UDE disturbance estimator (used to compensate for model errors and external disturbances) in this embodiment includes:

[0165] Based on the simplified dynamic model of the driving part, we obtain: ;

[0166] According to the UDE principle, assuming the initial state of the system is zero, the lumped disturbance estimate is expressed as:

[0167] ;

[0168] in, It is a strictly true low-pass filter matrix that can be designed to ensure the physical realizability of the estimator; therefore, the filter is designed as follows:

[0169] ;

[0170] in, These are adjustable UDE parameters used to determine the filter bandwidth;

[0171] Substituting the control input expression of the double-pendulum controlled transport system designed based on the simplified driving part's dynamic model into the lumped disturbance estimation expression, we obtain:

[0172] ;

[0173] and The explicit time-domain expression is:

[0174] ;

[0175] in .

[0176] In designing the uncertainty and disturbance estimator, this embodiment can estimate and compensate for the equivalent acceleration disturbance online by fusing the desired acceleration command and the real-time speed of the quadcopter. Therefore, external wind disturbances, parameter perturbations (such as load mass and rope length changes), and unmodeled dynamics can be normalized into equivalent disturbances for real-time feedforward compensation. This effectively alleviates the controller's excessive reliance on system model parameters and significantly improves its anti-interference capability in non-ideal engineering environments. Therefore, compared to the prior art, the method proposed in this embodiment achieves higher trajectory tracking accuracy while reducing the performance requirements of the actuator.

[0177] Finally, this embodiment can analyze the stability of the closed-loop system tracking the time-varying reference trajectory using the Lyapunov method, and analyze the trajectory tracking and oscillation elimination capabilities of the controller using the Russell invariant set principle.

[0178] Specifically, in this embodiment, the stability analysis of the closed-loop system tracking the time-varying reference trajectory using the Lyapunov method includes the following steps:

[0179] Assumption: Expected trajectory The second derivative of is bounded, that is, for all as well as ,satisfy ;

[0180] According to the lumped disturbance estimation expression, the estimation error is defined as: ;

[0181] Where “*” represents the convolution operator, The estimation error of the lumped disturbance consists of the coupling model error and the external disturbance. The filter designed corresponding to the estimator is a strictly well-posed and stable filter with unity gain and zero phase shift within the lumped uncertainty frequency spectrum, and zero gain in other frequency bands. This is valid; in this embodiment, if the filter is designed to be a strictly true and stable filter with unity gain and zero phase shift in the spectral range with lumped uncertainty, and zero gain in other spectral ranges, then theoretically it is possible to achieve... While strictly meeting this design condition in practical applications is very difficult, it is not a problem—because its main characteristics can be satisfied through approximate design, and the controller itself has excellent robustness and can automatically compensate for parameter deviations. Although this narrows the stability region, it is sufficient for most engineering systems.

[0182] Therefore, in this embodiment, a composite Lyapunov function is selected, which is expressed as: ;

[0183] in, and Representing the nominal energy of the task space, here we introduce... and As a nonnegative potential energy term constructed from geometric constraints, used to compensate for underdriven double pendulum dynamics, it is defined as follows:

[0184] ;

[0185] ;

[0186] in and It is related to mass-length distribution and gain coupling.

[0187] The above expression introduces a symbolic function. ,and Defined in the assumption, for Taking the time derivative, we get:

[0188] ;

[0189] Substituting the defined virtual control input and the feedback linearization result of the concentrated disturbance with the design of the control input for the double-pendulum controlled transport system based on the simplified dynamic model of the driving part, we can obtain:

[0190] ;

[0191] ;

[0192] Therefore, we can obtain: .

[0193] Based on the above dynamic model, this embodiment constructs an energy-type Lyapunov function composed of the system's total kinetic energy, gravitational potential energy, and a potential energy term reflecting the oscillating coupling energy. It is used to establish an energy shaping mechanism. In this embodiment, unlike conventional feedback control, this embodiment can design an acceleration control law based on energy shaping and passive theory to actively reshape the energy distribution of the closed-loop system. By introducing a damping injection term, it ensures that the total energy of the system is forcibly dissipated along a preset trajectory direction.

[0194] In addition, this embodiment can also combine the LaSalle invariant set principle to rigorously prove the asymptotic stability of the closed-loop system with respect to the time-varying tracking trajectory under the action of the controller. This design ensures that when the system is dealing with complex dynamic paths, the oscillation energy can be quickly absorbed into the position tracking loop and converted into stable action, thereby eliminating residual oscillations.

[0195] Therefore, in this embodiment, after analyzing the stability of the closed-loop system tracking the time-varying reference trajectory using the Lyapunov method, and before analyzing the trajectory tracking and oscillation elimination capabilities of the controller using the Lassel invariant set principle, the uniform boundedness of the closed-loop system is obtained. At this point, based on the Lassel invariant set principle, the outer-loop system state is analyzed in the set... The asymptotic convergence of the outer loop system state can be analyzed by finding the largest invariant set M within the inner loop.

[0196] In this embodiment, analyzing the trajectory tracking and oscillation elimination capabilities of the controller using the Lassel invariant set principle may include the following steps:

[0197] Based on the selected composite Lyapunov function and It can be inferred that: ;

[0198] In addition, due to ,and It consists of continuous trigonometric functions, therefore we have Inside M, according to as well as We can directly deduce that: .

[0199] After completing its cargo transport mission along a predetermined trajectory, the quadcopter will maintain a designated state (hovering or landing at a designated location, or flying at a constant speed along a designated trajectory within a designated range, i.e.) This embodiment uses proof by contradiction to prove the convergence of the position error, that is:

[0200] For identities Integrating both sides with respect to time, we get ,in It is an integral constant vector that needs to be determined in subsequent analysis;

[0201] from and Therefore, we can conclude that: ;

[0202] Combining formulas , , and ,get:

[0203] ;

[0204] Assumption After integration, we get: ,in, It is an undetermined constant vector; however, the left side of the formula may change as... And it tends toward infinity, which is consistent with the aforementioned... This contradicts the boundedness conclusion. Therefore, based on the formula obtained after integration, this embodiment concludes that: ;

[0205] Will Substitution and with Comparative analysis leads to the following conclusion: ;

[0206] from It can be inferred that: ;in, It is an undetermined constant vector, according to By definition, we get: ;

[0207] Will In Substituting the channel equations into the control and From the linearized equation, we obtain: Assuming and and will The two expressions in the expression are combined to obtain: ;

[0208] Adopted and A similar method, namely integration followed by proof by contradiction, leads to the conclusion. ,at this time, Therefore, it is proven that: ;

[0209] By combining , , and The system will be further rewritten as follows:

[0210] .

[0211] In summary, this embodiment, based on the LaSalle invariant set principle and proof by contradiction, ensures that the tracking error and swing angle asymptotically converge to zero in the steady state, thereby achieving a theoretical closed-loop control of precise tracking and active suppression.

[0212] Example 2

[0213] Based on Example 1, this example is simulated in MATLAB / Simulink using the ode5 (Dormand-Prince) solver with fixed compensation and a step size of 0.001. The specific control parameters in the simulation are as follows:

[0214] ;

[0215] Select the controller parameters as follows:

[0216] ;

[0217] ;

[0218] The control methods for comparison include:

[0219] (1) Nonlinear anti-sway controller:

[0220] ;

[0221] in ;

[0222] (2) Enhanced energy coupling controller (a prior patent provided in the background art, which serves as a prior art document):

[0223] ;

[0224] in:

[0225] ;

[0226] ;

[0227] Control gain , and It is the same controller as that proposed in Example 1.

[0228] The enhanced energy coupling controller was not considered in the design. The unit component on the axis is not compensated for, causing it to essentially track... Therefore, this method is essentially in The reference signal cannot be tracked on the axis and will have a fixed error. In this embodiment, the unit vector that this method lacks is supplemented when comparing the two methods, thereby achieving... Tracking of the reference signal on the axis.

[0229] Example 3

[0230] Based on the method proposed in Example 1 and the simulation environment and control method provided in Example 2, this example sets up scenario 1. In scenario 1, the anti-sway performance of the method proposed in Example 1 and the two comparative methods in Example 2 are compared under hovering conditions.

[0231] In this embodiment, the initial position of the quadcopter drone is The initial swing angle is The simulation comparison results are as follows Figure 4 As shown, the first four pictures are the swing angles. The trajectory diagram, swing angle The trajectory diagram, swing angle The trajectory diagram, swing angle The trajectory diagram. The last three images are of a quadcopter dual-pendulum drone. Tracking error of the channel.

[0232] As can be seen from the first four simulation figures, the control strategy proposed in this embodiment exhibits superior sway suppression performance compared to the comparative controller. Specifically, throughout the entire control process, the sway angle amplitude under the controller in this embodiment remains at a lower level, and effective sway convergence is achieved within 2 seconds, while the comparative controller requires more than 5 seconds to achieve the same convergence effect. It is worth noting that sway suppression and trajectory tracking have an inherent coupling contradiction in terms of control objectives. To achieve rapid sway elimination, the controller in this embodiment makes necessary trade-offs in trajectory tracking accuracy. The last three simulation figures visually illustrate this performance trade-off process, verifying that the proposed scheme has a controllable impact on tracking accuracy while maintaining system stability.

[0233] Example 4

[0234] Based on the method proposed in Example 1 and the simulation environment and control method provided in Example 2, this example sets up scenario 2, in which the time-varying spherical trajectory tracking of the method proposed in Example 1 and the two comparison methods in Example 2 are compared.

[0235] In this embodiment, the initial position of the quadcopter drone is With the initial swing angle all set to 0, the UAV's tracking reference trajectory is as follows:

[0236] ;

[0237] in, Let be the radius of the ball's trajectory. The unit is rad / s, which is the angular velocity of the reference trajectory.

[0238] Simulation results are as follows Figure 5 As shown in the diagram. Based on the analysis of the last three simulation figures, the control strategy proposed in this embodiment demonstrates excellent trajectory tracking performance. During the dynamic tracking of a spherical trajectory, this embodiment strictly controls the tracking error to within 0.05m, while the error of the comparison controller under the same conditions is as high as approximately 0.30m, verifying the significant advantage of this embodiment in complex path tracking tasks. It is worth noting that since the quadcopter must maintain continuous acceleration changes when tracking a spherical trajectory, the inability of the system's swing angle to completely converge to zero is a reasonable physical characteristic. Even under this dynamic environment, the controller in this embodiment can still effectively limit the swing amplitude. Simulation data shows that the peak change of the swing angle after the controller in this embodiment stabilizes is...

[0239] ( , , , (in rad), which is significantly lower than the swing angle level of the comparison controller, demonstrating the superiority of this embodiment in suppressing motion residuals.

[0240] Example 5

[0241] Based on the method proposed in Example 1 and the simulation environment and control method provided in Example 2, this example sets up scenario 3, in which the robustness of the method proposed in Example 1 and the two comparison methods in Example 2 are compared.

[0242] This embodiment adds external interference to the quadcopter drone model based on scenario 1, specifically:

[0243] ;

[0244] in, This is the angular frequency of the interference. The simulation results are as follows: Figure 6 As shown, the performance of each control algorithm under external disturbances is intuitively demonstrated.

[0245] Through the Figure 6 Analysis shows that under the impact of external disturbances, the existing comparison method exhibits strong sensitivity, with the peak position tracking error exceeding 0.30m, thus reflecting its insufficient robustness. In contrast, the control framework proposed in this embodiment demonstrates excellent anti-interference capability. Even under the same disturbance conditions, the position error is always maintained within 0.10m, thus significantly improving the trajectory tracking accuracy and stability of the system.

[0246] Furthermore, based on the analysis of the magnified sub-figures in the first four figures, when the UAV performs trajectory tracking tasks, the fuselage under the comparison controller exhibits obvious oscillation residuals. The control framework proposed in this embodiment effectively reduces the dynamic response amplitude of the system through a more efficient oscillation suppression mechanism. Therefore, the experimental data in scenario 3 can further confirm that this embodiment has stronger robustness and control accuracy under complex dynamic environments and external disturbances.

[0247] The above are merely preferred embodiments of the present invention and are not intended to limit the present invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A trajectory tracking and anti-pendulum control method for a quadrotor dual-pendulum system based on deep dynamic coupling, characterized in that, Includes the following steps: A dynamic model of the quadrotor double pendulum system is established, and the driving part in the dynamic model is simplified. Design a controller that combines energy coupling and nonlinear coupling for trajectory tracking and sway elimination; Design an uncertainty and disturbance estimator to compensate for model errors and external disturbances; The stability of a closed-loop system tracking a time-varying reference trajectory is analyzed using the Lyapunov method, and the trajectory tracking and oscillation elimination capabilities of the controller are analyzed using the Russell invariant set principle.

2. The trajectory tracking and anti-oscillation control method for a quadrotor dual-pendulum system based on deep dynamic coupling according to claim 1, characterized in that, The quadcopter dual-pendulum system is a quadcopter dual-pendulum unmanned aerial vehicle; The position vector of the quadcopter dual-pendulum UAV in the inertial coordinate system is expressed as: In this system, These represent the masses of the quadcopter, the hook, and the load, respectively. It is the length of the cable connecting the quadcopter and the hook. It is the length of the cable connecting the hook and the load. These represent the swing angles of the hook and the load in three-dimensional space, respectively. In a quadcopter dual-pendulum drone, the control input is defined. , This represents the rotation matrix of a quadcopter dual-pendulum UAV from its fixed coordinate system to its inertial coordinate system. yes The unit vector of the channel.

3. The trajectory tracking and anti-slip control method for a quadrotor dual-pendulum system based on deep dynamic coupling according to claim 2, characterized in that, The established dynamic model of the quadrotor double pendulum system is expressed as follows: ； ； ； ； ； ； ； in, and These are the speed and angular velocity of the quadcopter dual-pendulum drone, respectively. and These are the angular velocity and angular acceleration of the swing, respectively. These are quadcopter dual-pendulum drones Channel control input, These are the model errors and external interference of the quadcopter dual-pendulum UAV, respectively. Acceleration due to gravity; symbol They represent , , , ,in .

4. The trajectory tracking and anti-oscillation control method for a quadrotor dual-pendulum system based on deep dynamic coupling according to claim 3, characterized in that, The dynamic equations of the aforementioned dynamic model are expressed as: ； in, Let be the state vector of the outer loop system, specifically defined as follows: ; , , These are the inertial matrix, the centrifugal-Coriolis force matrix, the gravity vector, the resultant force on the position subsystem, and the external disturbance, respectively.

5. The trajectory tracking and anti-slip control method for a quadrotor dual-pendulum system based on deep dynamic coupling according to claim 4, characterized in that, The simplification of the driving component in the dynamic model includes the following steps: Define unit vectors pointing from the quadcopter to the hook and from the hook to the load, respectively. and Its specific expression is: ; Define coupling terms ,in , , At this point, the dynamic model of the driving part in the coupled system containing the dynamic model is rewritten as follows: ; Using feedback linearization technology, virtual control input and lumped disturbance are defined, and they are expressed as follows: ； The dynamic model of the driving part in the coupled system is simplified and expressed as follows: ,in, It represents the acceleration of the coupled system.

6. The trajectory tracking and anti-oscillation control method for a quadrotor dual-pendulum system based on deep dynamic coupling according to claim 5, characterized in that, The energy coupling and nonlinear coupling controller designed includes: The tracking error is defined as follows: ； in, This indicates the tracking error of a quadcopter dual-pendulum drone; The system error dynamics equation is rewritten as: ； The outer-loop pseudo-energy function expression for the quadrotor double pendulum system is established as follows: ； right Conducting discussions about time Taking the derivative, we get: ； in, Let represent the second derivative of the quadcopter reference signal, and ; The composite signal and tracking error are designed as follows: ； in, and This is the undetermined gain, used to adjust the coupling relationship between the quadrotor's motion and oscillation response; The energy function is rewritten as: ； in: ； ； When the condition is met ,expression It is positive definite and integrable, therefore ,but It is a positive definite scalar function; The control input of the double pendulum-controlled transportation system is designed based on the simplified dynamic model of the driving part, and it is expressed as follows: ； in, and These represent the feedback gain, It is a lumped disturbance estimate consisting of coupled model error and external disturbance, obtained through the uncertainty and disturbance estimator.

7. The trajectory tracking and anti-oscillation control method for a quadrotor dual-pendulum system based on deep dynamic coupling according to claim 6, characterized in that, The design uncertainty and disturbance estimator includes: Based on the simplified dynamic model of the driving part, we obtain: ; According to the UDE principle, assuming the initial state of the system is zero, the lumped disturbance estimate is expressed as: ； in, It is a strictly true low-pass filter matrix that can be designed to ensure the physical realizability of the estimator; therefore, the filter is designed as follows: ； in, These are adjustable UDE parameters used to determine the filter bandwidth; Substituting the control input expression of the double-pendulum controlled transport system designed based on the simplified driving part's dynamic model into the lumped disturbance estimation expression, we obtain: ； and The explicit time-domain expression is: ； in .

8. The trajectory tracking and anti-oscillation control method for a quadrotor dual-pendulum system based on deep dynamic coupling according to claim 7, characterized in that, The stability analysis of the closed-loop system tracking a time-varying reference trajectory using the Lyapunov method includes the following steps: Assumption: Expected trajectory The second derivative of is bounded, that is, for all . as well as ,satisfy ; According to the lumped disturbance estimation expression, the estimation error is defined as: ; Where "*" represents the convolution operator. The estimation error of the lumped disturbance consists of the coupling model error and the external disturbance. The filter designed corresponding to the estimator is a strictly well-posed and stable filter with unity gain and zero phase shift within the lumped uncertainty frequency spectrum, and zero gain in other frequency bands. Established; Choosing the composite Lyapunov function, it is expressed as: ; in, and Representing the nominal energy of the task space, here we introduce... and As a nonnegative potential energy term constructed from geometric constraints, used to compensate for underdriven double pendulum dynamics, it is defined as follows: ； ； in and Related to mass-length distribution and gain coupling; right Taking the time derivative, we get: ； Substituting the defined virtual control input and the feedback linearization result of the concentrated disturbance with the design of the control input for the double-pendulum controlled transport system based on the simplified dynamic model of the driving part, we obtain: ； ； Therefore, we get: .

9. The trajectory tracking and anti-oscillation control method for a quadrotor dual-pendulum system based on deep dynamic coupling according to claim 8, characterized in that, After analyzing the stability of a closed-loop system tracking a time-varying reference trajectory using the Lyapunov method, and before analyzing the controller's trajectory tracking and oscillation elimination capabilities using the Lassel invariant set principle, the uniform boundedness of the closed-loop system is obtained. At this point, based on the Lassel invariant set principle, the state of the outer-loop system in the set is analyzed. The asymptotic convergence of the outer loop system state can be analyzed by finding the largest invariant set M within the inner loop.

10. The trajectory tracking and anti-slip control method for a quadrotor dual-pendulum system based on deep dynamic coupling according to claim 9, characterized in that, The analysis of the controller's trajectory tracking and oscillation elimination capabilities using the Lassel invariant set principle includes the following steps: Based on the selected composite Lyapunov function and It can be inferred that: ; In addition, due to ,and It consists of continuous trigonometric functions, therefore we have Inside M, according to as well as We can directly deduce that: ; After completing the cargo transport task along the predetermined trajectory, the quadcopter will maintain a designated state. At this point, the convergence of the position error is proved by contradiction, namely: For identities Integrating both sides with respect to time, we get ,in It is an integral constant vector that needs to be determined in subsequent analysis; from and Therefore, we can conclude that: ; Combining formulas , , and ,get: ; Assumption After integration, we get: ,in, Given an undetermined constant vector, based on the formula obtained after integration, the conclusion is: ; Will Substitution and with Comparative analysis leads to the following conclusion: ; from It can be inferred that: ;in, It is an undetermined constant vector, according to By definition, we get: ; Will In Substituting the channel equations into the control and From the linearized equation, we obtain: Assuming and and will The two expressions in the expression are combined to obtain: ; Adopted and A similar method, namely integration followed by proof by contradiction, leads to the conclusion. ,at this time, Therefore, it is proven that: ; By combining , , and The system will be rewritten as follows: 。

Images