Anti-disturbance unmanned aerial vehicle trajectory tracking control method, system, equipment and medium

Abstract

The invention provides an anti-disturbance unmanned aerial vehicle trajectory tracking control method, system and device and a medium. The method comprises the following steps: establishing a six-degree-of-freedom rigid body dynamic model of a four-rotor unmanned aerial vehicle; selecting an unmanned aerial vehicle feasible trajectory of the kinetic model as an expected trajectory, and introducing a coupling attitude; designing a position loop control law, generating a virtual control force based on a smooth terminal sliding mode control surface, and calculating an actual thrust; designing an attitude loop control law, representing an attitude error based on Lie group SO (3), constructing an attitude tracking controller with a stable global index, and outputting a control torque; constructing a four-order state observer to estimate the angular velocity and the angular acceleration of the coupling attitude, and replacing a true value with an estimated value to participate in control law calculation; thrust and control torque are converted into thrust input of the unmanned aerial vehicle, the unmanned aerial vehicle is driven to track an expected trajectory, and the problems that in trajectory tracking control of the four-rotor unmanned aerial vehicle, dependence on position acceleration measurement is high, noise is amplified through differential derivation, disturbance rejection is weak, and real-time calculation burden is large are solved.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) flight control technology, and in particular to a disturbance-resistant UAV trajectory tracking control method, system, device, and medium. Background Technology

[0002] Research on disturbance rejection control for rotary-wing UAVs has continued to develop driven by the need for adaptability to complex environments. Currently, mainstream technologies include Active Disturbance Rejection Control (ADRC), composite disturbance rejection methods, intelligent learning algorithm fusion, and novel disturbance rejection controllers. ADRC uses an Extended State Observer (ESO) to estimate and compensate for the total system disturbance (including model uncertainties and external disturbances) in real time, combining a Tracking Differentiator (TD) and Nonlinear Feedback (NLSEF) to improve dynamic response. For example, Linear Active Disturbance Rejection Control (LADRC) has demonstrated robustness under configuration changes in a spliced ​​tilt-rotor UAV, showing improved response speed and reduced overshoot compared to traditional PID. Composite disturbance rejection methods, targeting complex disturbances (such as periodic oscillations and random disturbances), employ parallel observers to estimate disturbances in different frequency bands, improving estimation accuracy. For example, in the pitch maneuver of a fixed-wing UAV, the extended state observer and nonlinear disturbance observer work together to reduce disturbance estimation errors. Intelligent learning algorithm fusion uses a Radial Basis Function Neural Network (RBFNN) to approximate unmodeled dynamics and jointly compensate with a disturbance observer. Experiments show that position tracking errors in helicopter sling control are reduced. Novel disturbance rejection controller design: such as sliding mode control, which draws on the concept of high-gain control to design control algorithms, and uses Lyapunov theory to ensure stability. The control algorithm is relatively simple to calculate and has good disturbance rejection effect with few system variables.

[0003] Current quadcopter UAV motion control architectures mostly adopt a dual-loop framework of "outer loop position control + inner loop attitude control". Position control generates a virtual control force Fv, which is then used to calculate the coupled attitude Rc. Attitude control needs to track the angular velocity Ωc and angular acceleration of Rc, and its calculation depends on the second derivative of the position acceleration. External environmental disturbances to the UAV mostly affect the position loop, while disturbances to the attitude loop are relatively small. General state feedback closed-loop control can handle disturbances to the attitude loop.

[0004] Existing methods are highly dependent on measurements. Coupling attitude rotation angular velocity and angular acceleration requires precise acquisition of position acceleration and its derivative. However, position acceleration measurements are often noisy, and differentiation amplifies the noise signal. Furthermore, the mathematical expression of directly calculated attitude control laws is complex, resulting in a heavy real-time computational burden. These methods are also sensitive to position acceleration signals and have weak disturbance rejection capabilities. Attitude loop modeling and controller design are mostly based on Euler angles. However, Euler angles cannot globally represent the UAV attitude and suffer from singularity and ineffectiveness issues at specific attitudes. Therefore, attitude controllers based on Euler angles cannot drive UAVs to perform large-scale attitude maneuvers.

[0005] Therefore, there is an urgent need to propose a disturbance-resistant UAV trajectory tracking control method to solve the problems in existing quadrotor UAV trajectory tracking control, such as strong dependence on position acceleration measurement, weak disturbance resistance due to noise amplification from differential calculation and high real-time computation burden, singularity in attitude control based on Euler angles which prevents large-range attitude maneuvers, and actuator chatter caused by traditional sliding mode control. Summary of the Invention

[0006] To overcome the problems existing in the related technologies, this disclosure provides a disturbance-resistant UAV trajectory tracking control method, system, device and medium to solve the technical problems in the existing quadrotor UAV trajectory tracking control, such as strong dependence on position acceleration measurement, weak disturbance resistance due to noise amplification from differential calculation and large real-time calculation burden, singularity in attitude control based on Euler angles which prevents large-range attitude maneuvers, and the tendency of traditional sliding mode control to cause actuator chatter.

[0007] This specification provides one or more embodiments of a disturbance-resistant unmanned aerial vehicle (UAV) trajectory tracking control method, comprising the following steps: A six-degree-of-freedom rigid body dynamics model of a quadcopter UAV is established, including position dynamics equations and attitude dynamics equations; A feasible trajectory of the UAV that conforms to the ideal dynamics model is selected as the desired trajectory, and a coupled attitude is introduced, which is constructed by the direction vector of the virtual control force. Design a position loop control law, generate virtual control force based on a smooth terminal sliding diaphragm control surface, and calculate the actual thrust; Design an attitude loop control law, based on the Lie group SO(3) to represent the attitude error, construct a globally exponentially stable attitude tracking controller, and output the control torque; A fourth-order state observer is constructed to estimate the angular velocity and angular acceleration of the coupled attitude, and the estimated values ​​are used to replace the true values ​​in the calculation of the control law. The thrust and control torque are converted into thrust input for the quadcopter drone, driving the drone to track the desired trajectory.

[0008] Preferably, the six-degree-of-freedom rigid body dynamics model is expressed as: ; ; Where r represents the position of the center of mass in the inertial frame of reference. Let e3 represent the velocity of the center of mass in the inertial frame, and let d represent the unit vector of the inertial frame. v This represents the bounded disturbance along the position path in the inertial frame of reference, where m represents the mass of the object. Represents gravitational acceleration. f Represents total control thrust. Represents the attitude matrix. Represents the attitude angular velocity vector. I Represents the rotational inertia matrix of the UAV. To control the torque.

[0009] Preferably, the coupling attitude is represented as: ; in, This represents the coupling term between position and attitude. Indicates the desired posture, These represent the three directions of the desired posture. , The direction representing the coupled attitude The rotational angular velocity representing the coupled attitude. Angular acceleration representing the coupled attitude.

[0010] Preferably, the smooth terminal glial control surface is defined as: ; in, This indicates the positional error of the drone. Indicates the speed error of the drone. This represents a smooth switching function. Represents a symbolic function. This represents the gain parameter for the gliding surface control. .

[0011] Preferably, the control torque in the attitude loop control law is expressed as: ; in, Indicates control gain. This represents the moment of inertia of the drone. The rotational angular velocity represents the coupled attitude. The angular acceleration representing the coupled attitude. Indicates attitude error. Indicates the error in rotational angular velocity. yes The spinor is represented by, Represents the Morse function The gradient.

[0012] Preferably, the fourth-order state observer is represented as follows: ; in, Indicates the state of the observer. The parameters representing the observer, Represents the angular velocity of the coupled attitude rotation The estimated value, Represents the coupled attitude. Represents the angular acceleration of the coupled attitude rotation. The estimated value.

[0013] This specification provides one or more embodiments of a disturbance-resistant unmanned aerial vehicle (UAV) trajectory tracking control system, including a model building module, a coupled attitude generation module, a position control module, an attitude control module, a state observation module, and a tracking module; The model building module is used to build a six-degree-of-freedom rigid body dynamics model of a quadcopter UAV, including position dynamics equations and attitude dynamics equations. The coupled attitude generation module is used to select a feasible trajectory of the UAV that conforms to the ideal dynamics model as the desired trajectory, and introduce a coupled attitude, which is constructed from the direction vector of the virtual control force. The position control module is used to design the position loop control law, generate virtual control force based on the smooth terminal sliding diaphragm control surface, and calculate the actual thrust; The attitude control module is used to design the attitude loop control law, and construct a globally exponentially stable attitude tracking controller based on the Lie group SO(3) to represent the attitude error, and output the control torque. The state observation module is used to construct a fourth-order state observer to estimate the angular velocity and angular acceleration of the coupled attitude, and use the estimated values ​​to replace the true values ​​in the calculation of the control law; The tracking module is used to convert the thrust and the control torque into thrust input for the quadcopter UAV, driving the UAV to track the desired trajectory.

[0014] Preferably, the six-degree-of-freedom rigid body dynamics model is expressed as: ; ; Where r represents the position of the center of mass in the inertial frame of reference. Let e3 represent the velocity of the center of mass in the inertial frame, and let d represent the unit vector of the inertial frame. v This represents the bounded disturbance along the position path in the inertial frame of reference, where m represents the mass of the object. Represents gravitational acceleration. f Represents total control thrust. Represents the attitude matrix. I represents the attitude angular velocity vector, and I represents the UAV rotational inertia matrix. To control the torque.

[0015] This specification provides one or more embodiments of a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the disturbance-resistant unmanned aerial vehicle trajectory tracking control method described above.

[0016] This specification provides one or more embodiments of a computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of the disturbance-resistant unmanned aerial vehicle trajectory tracking control method described above.

[0017] This disclosure provides a disturbance-resistant UAV trajectory tracking control method, system, device, and medium. Its advantages lie in establishing a six-degree-of-freedom rigid body dynamics model of a quadcopter UAV, including position dynamics equations and attitude dynamics equations, accurately characterizing the physical laws of the UAV's position and attitude motion. This provides a rigorous mathematical foundation for subsequent control law design, desired trajectory selection, and disturbance analysis, avoiding control deviations caused by model simplification and ensuring that the control strategy matches the actual motion characteristics of the UAV. A feasible UAV trajectory conforming to the ideal dynamics model is selected as the desired trajectory, and a coupled attitude is introduced. The coupled attitude is constructed from the direction vector of the virtual control force. The feasible trajectory ensures that the desired motion conforms to the UAV's dynamic constraints, avoiding tracking failure due to the trajectory exceeding the actuator's capabilities. The coupled attitude is established... The position loop and attitude loop are decoupled, transforming the position tracking requirement into a clear attitude control objective, eliminating the interference of strong coupling between position and attitude on control accuracy, and creating conditions for subsequent sub-loop control. The position loop control law is designed, generating virtual control force based on the smooth terminal sliding mode control surface, and calculating the actual thrust. The smooth terminal sliding mode control surface can drive the position tracking error to converge within a finite time, improving the trajectory tracking response speed. The smoothness characteristic replaces the discontinuous switching of the traditional sliding mode, completely avoiding actuator chatter, protecting the hardware and improving control stability. At the same time, the robustness of the sliding mode suppresses unknown disturbances in the position loop, ensuring position tracking accuracy. The attitude loop control law is designed, based on the Lie group SO(3) to represent the attitude error, constructing a globally exponentially stable attitude tracking controller, and outputting control torque. The Lie group SO(3) It can globally represent the attitude of the UAV, completely eliminating the singularity problem of Euler angle attitude representation, supporting a wide range of attitude maneuvers for the UAV. The global exponentially stable controller ensures that the attitude tracking error converges at an exponential rate, balancing the speed and stability of attitude control, and avoiding attitude divergence or excessive overshoot. A fourth-order state observer is constructed to estimate the angular velocity and angular acceleration of the coupled attitude. The estimated values ​​are used to replace the true values ​​in the control law calculation, without relying on position acceleration measurement and differential differentiation, avoiding the problem of measurement noise being amplified by differentiation, and reducing the dependence on high-precision sensors. The estimated values ​​have high real-time performance and accuracy. It can effectively eliminate the adverse effects of position measurement noise on attitude loop control, reduce the real-time calculation burden, and improve the robustness of attitude control to disturbances. It converts the thrust and control torque into thrust input for the quadcopter UAV, driving the UAV to track the desired trajectory. Through precise input allocation, the control commands of the position loop and attitude loop are converted into the actual thrust of the four rotors, ensuring that the control signal is efficiently transmitted to the actuator. Ultimately, it enables the UAV to achieve high-precision and stable tracking of a feasible desired trajectory in an unknown disturbance environment, taking into account tracking accuracy, response speed, and hardware security. Attached Figure Description

[0018] To more clearly illustrate the technical solutions in one or more embodiments of this specification or in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments recorded in this specification. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0019] Figure 1 A flowchart illustrating an anti-disturbance unmanned aerial vehicle trajectory tracking control method provided for one or more embodiments of this specification; Figure 2 A schematic diagram of a disturbance-resistant unmanned aerial vehicle (UAV) trajectory tracking control system provided for one or more embodiments of this specification; Figure 3 This is a schematic diagram of the structure of a computer device provided for one or more embodiments of this specification. Detailed Implementation

[0020] To enable those skilled in the art to better understand the technical solutions in one or more embodiments of this specification, the technical solutions in one or more embodiments of this specification will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of this specification, and not all of the embodiments. Based on one or more embodiments of this specification, all other embodiments obtained by those skilled in the art without creative effort should fall within the protection scope of this invention.

[0021] The present invention will now be described in detail with reference to specific embodiments and accompanying drawings.

[0022] Method Implementation Examples According to embodiments of the present invention, a disturbance-resistant unmanned aerial vehicle (UAV) trajectory tracking control method is provided, such as... Figure 1 The diagram shown is a flowchart illustrating the disturbance-resistant UAV trajectory tracking control method provided in this embodiment. The disturbance-resistant UAV trajectory tracking control method according to this embodiment includes the following steps: S110. Establish a six-degree-of-freedom rigid body dynamics model for a quadcopter UAV. The six-degree-of-freedom rigid body has four independent control inputs, including position dynamics equations and attitude dynamics equations: The six-degree-of-freedom rigid body dynamics model is expressed as: ; ; Where r represents the position of the center of mass in the inertial frame of reference. Let e3 represent the velocity of the center of mass in the inertial frame, and let d represent the unit vector of the inertial frame. v This represents the bounded disturbance along the position path in the inertial frame of reference, where m represents the mass of the object. Represents gravitational acceleration. f Represents total control thrust. Represents the attitude matrix. I represents the attitude angular velocity vector, and I represents the UAV rotational inertia matrix. To control the torque.

[0023] S120. Select the feasible trajectory of the UAV that conforms to the ideal dynamics model as the desired trajectory, defined as follows: ; ; in, Let be the basis vectors of the desired body coordinate system, and the desired attitude can also be expressed as: , This is the coupling term between the position loop and the attitude loop. (Note: The original text contains some formatting errors and inconsistencies. A more accurate translation would require the full context.) For virtual control force. Coupling term. This will generate a coupled attitude, denoted as... .

[0024] Coupled attitude is introduced, which is constructed from the direction vector of the virtual control force. The coupled attitude system is represented as follows: ; in, These represent the rotational angular velocity and angular acceleration of the coupled attitude, respectively. Obviously, however, if converges to Then the coupled attitude It will also converge to the expected posture. .

[0025] Position tracking error and translation velocity tracking error are denoted as follows: The tracking error of the attitude loop is defined as... .

[0026] S130. Design the position loop control law, generate virtual control force based on the smooth terminal sliding diaphragm control surface, and calculate the actual thrust.

[0027] S140. Design the attitude loop control law. Based on the Lie group SO(3) to represent the attitude error, construct a globally exponentially stable attitude tracking controller and output the control torque.

[0028] S150. Construct a fourth-order state observer to estimate the angular velocity and angular acceleration of the coupled attitude, and use the estimated values ​​to replace the true values ​​in the control law calculation.

[0029] S160. The thrust and the control torque are converted into thrust input for the quadcopter drone, driving the drone to track the desired trajectory.

[0030] The method provided in this embodiment establishes a six-degree-of-freedom rigid body dynamics model of a quadcopter UAV, including position dynamics equations and attitude dynamics equations. This model accurately describes the physical laws governing the UAV's position and attitude motions, providing a rigorous mathematical foundation for subsequent control law design, desired trajectory selection, and disturbance analysis. It avoids control deviations caused by model simplification and ensures that the control strategy matches the actual motion characteristics of the UAV. A feasible UAV trajectory conforming to the ideal dynamics model is selected as the desired trajectory, and a coupled attitude is introduced. This coupled attitude is constructed from the direction vector of the virtual control force. The feasible trajectory ensures that the desired motion conforms to the UAV's dynamic constraints, preventing the trajectory from exceeding the actuator's capabilities and causing tracking failure. The coupled attitude establishes a decoupling bridge between the position loop and the attitude loop. The position tracking requirement is transformed into a clear attitude control objective, eliminating the interference of strong coupling between position and attitude on control accuracy and creating conditions for subsequent sub-loop control. The position loop control law is designed, and a virtual control force is generated based on the smooth terminal sliding mode control surface to calculate the actual thrust. The smooth terminal sliding mode control surface can drive the position tracking error to converge within a finite time, improving the trajectory tracking response speed. The smoothness characteristic replaces the discontinuous switching of the traditional sliding mode, completely avoiding actuator chatter, protecting the hardware and improving control stability. At the same time, the robustness of the sliding mode suppresses unknown disturbances in the position loop to ensure position tracking accuracy. The attitude loop control law is designed, and the attitude error is represented by the Lie group SO(3). A globally exponentially stable attitude tracking controller is constructed to output the control torque. The Lie group SO(3) is used to represent the attitude error. It can globally represent the attitude of the UAV, completely eliminating the singularity problem of Euler angle attitude representation, supporting a wide range of attitude maneuvers for the UAV. The global exponentially stable controller ensures that the attitude tracking error converges at an exponential rate, balancing the speed and stability of attitude control, and avoiding attitude divergence or excessive overshoot. A fourth-order state observer is constructed to estimate the angular velocity and angular acceleration of the coupled attitude. The estimated values ​​are used to replace the true values ​​in the control law calculation, without relying on position acceleration measurement and differential differentiation, avoiding the problem of measurement noise being amplified by differentiation, and reducing the dependence on high-precision sensors. The estimated values ​​have high real-time performance and accuracy. It can effectively eliminate the adverse effects of position measurement noise on attitude loop control, reduce the real-time calculation burden, and improve the robustness of attitude control to disturbances. It converts the thrust and control torque into thrust input for the quadcopter UAV, driving the UAV to track the desired trajectory. Through precise input allocation, the control commands of the position loop and attitude loop are converted into the actual thrust of the four rotors, ensuring that the control signal is efficiently transmitted to the actuator. Ultimately, it enables the UAV to achieve high-precision and stable tracking of a feasible desired trajectory in an unknown disturbance environment, taking into account tracking accuracy, response speed, and hardware security.

[0031] In one embodiment, the state feedback thrust along the z-axis of the rotorcraft UAV's body coordinate system... ,in, The virtual control force for the position loop is given by the following formula: ; The smooth terminal sliding control surface is defined as follows: ; in, This indicates the positional error of the drone. Indicates the speed error of the drone. This indicates the weight of the drone. This represents a smooth switching function. Represents a symbolic function. This represents the gain parameter for the gliding surface control.

[0032] , This represents the control gain parameter.

[0033] In one implementation, the control torque in the attitude loop control law is expressed as: ; in, Indicates control gain. This represents the moment of inertia of the drone. The rotational angular velocity represents the coupled attitude. The angular acceleration representing the coupled attitude. Indicates attitude error. Indicates the error in rotational angular velocity. yes The spinor is represented by, Represents the Morse function The gradient of a matrix is ​​often found using Morse functions such as the trace function. ).

[0034] In one embodiment, two mappings are defined as follows: and ,

[0035] Mapping Embedding the three-dimensional nonlinear manifold SO(3) into a high-dimensional Euclidean space In the middle. Mapping Nine-dimensional space Projected into a lower-dimensional space .

[0036] Construct a fourth-order state observer, represented as: ; in, The parameters representing the observer, Represents the angular velocity of the coupled attitude rotation The estimated value, Represents the coupled attitude. Represents the angular acceleration of the coupled attitude rotation. The estimated value.

[0037] Use estimated values and Replace the state values ​​of the coupled attitude in the control law and The torque is expressed as: .

[0038] System Implementation Examples According to embodiments of the present invention, a disturbance-resistant unmanned aerial vehicle (UAV) trajectory tracking control system is provided, such as... Figure 2 The diagram shown is a schematic diagram of the structure of the anti-disturbance UAV trajectory tracking control system provided in this embodiment. The anti-disturbance UAV trajectory tracking control system according to this embodiment includes a model building module 21, a coupled attitude generation module 22, a position control module 23, an attitude control module 24, a state observation module 25, and a tracking module 26.

[0039] Model building module 21 is used to build a six-degree-of-freedom rigid body dynamics model of a quadcopter UAV, including position dynamics equations and attitude dynamics equations.

[0040] The coupled attitude generation module 22 is used to select a feasible trajectory of the UAV that conforms to the ideal dynamic model as the desired trajectory and introduce a coupled attitude, which is constructed by the direction vector of the virtual control force.

[0041] The position control module 23 is used to design the position loop control law, generate virtual control force based on the smooth terminal sliding diaphragm control surface, and calculate the actual thrust.

[0042] The attitude control module 24 is used to design the attitude loop control law. Based on the Lie group SO(3) to represent the attitude error, a globally exponentially stable attitude tracking controller is constructed and the control torque is output.

[0043] The state observation module 25 is used to construct a fourth-order state observer to estimate the angular velocity and angular acceleration of the coupled attitude, and use the estimated values ​​to replace the true values ​​in the calculation of the control law.

[0044] The tracking module 26 is used to convert the thrust and the control torque into thrust input for the quadcopter UAV, driving the UAV to track the desired trajectory.

[0045] The system provided in this embodiment includes a model building module 21 that establishes a six-degree-of-freedom rigid body dynamics model of a quadcopter UAV, including position dynamics equations and attitude dynamics equations. This model accurately describes the physical laws governing the UAV's position and attitude motions, providing a rigorous mathematical foundation for subsequent control law design, desired trajectory selection, and disturbance analysis. It avoids control deviations caused by model simplification and ensures that the control strategy matches the actual motion characteristics of the UAV. The coupled attitude generation module 22 selects a feasible UAV trajectory that conforms to the ideal dynamics model as the desired trajectory and introduces a coupled attitude. The coupled attitude is constructed from the direction vector of the virtual control force. The feasible trajectory ensures that the desired motion conforms to the UAV's dynamic constraints, preventing the trajectory from exceeding the actuator's capabilities and causing tracking failure. The coupled attitude establishes a decoupling bridge between the position loop and the attitude loop. The position tracking requirement is transformed into a clear attitude control objective, eliminating the interference of strong coupling between position and attitude on control accuracy and creating conditions for subsequent sub-loop control; the position control module 23 is used to design the position loop control law, generating virtual control force based on the smooth terminal sliding mode control surface, calculating the actual thrust, the smooth terminal sliding mode control surface can drive the position tracking error to converge within a finite time, improve the trajectory tracking response speed, the smoothness characteristic replaces the discontinuous switching of the traditional sliding mode, completely avoids the actuator chatter phenomenon, protects the hardware and improves control stability; at the same time, the robustness of the sliding mode suppresses unknown disturbances in the position loop, ensuring position tracking accuracy; the attitude control module 24 is used to design the attitude loop control law, based on the Lie group SO(3) to represent the attitude error, constructing a globally exponentially stable attitude tracking controller, outputting control torque, Lie group SO(3) It can globally represent the UAV attitude, completely eliminating the singularity problem of Euler angle attitude representation, supporting a wide range of UAV attitude maneuvers. The global exponentially stable controller ensures that the attitude tracking error converges at an exponential rate, balancing the speed and stability of attitude control, and avoiding attitude divergence or excessive overshoot. The state observation module 25 is used to construct a fourth-order state observer to estimate the angular velocity and angular acceleration of the coupled attitude. The estimated values ​​are used to replace the true values ​​in the control law calculation, without relying on position acceleration measurement and differential differentiation, avoiding the problem of measurement noise being amplified by differentiation, and reducing the dependence on high-precision sensors. The tracking module 26 is used for estimating the values. It boasts high real-time performance and accuracy, effectively eliminating the adverse effects of position measurement noise on attitude loop control, reducing the real-time computation burden, and improving the robustness of attitude control to disturbances. It converts the thrust and control torque into thrust input for the quadcopter UAV, driving the UAV to track the desired trajectory. Through precise input allocation, it transforms the control commands of the position loop and attitude loop into the actual thrust of the four rotors, ensuring that the control signals are efficiently transmitted to the actuators. Ultimately, it enables the UAV to achieve high-precision and stable tracking of a feasible desired trajectory in an unknown disturbance environment, balancing tracking accuracy, response speed, and hardware security.

[0046] The embodiments of the present invention are system embodiments corresponding to the above method embodiments. The specific operations of each module processing step can be understood by referring to the description of the method embodiments, and will not be repeated here.

[0047] like Figure 3 As shown, the present invention also provides a computer-readable storage medium having a computer program stored thereon. When the computer program is executed by a processor, it implements the disturbance-resistant UAV trajectory tracking control method in the above embodiments, or when the computer program is executed by a processor, it implements the disturbance-resistant UAV trajectory tracking control method in the above embodiments.

[0048] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium, and when executed, it can include the processes of the embodiments of the above methods. Any references to memory, storage, databases, or other media used in the embodiments provided in this application can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), dual data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), Rambus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM), etc.

[0049] The various embodiments in this specification are described in a progressive manner. Similar or identical parts between embodiments can be referred to mutually. Each embodiment focuses on describing the differences from other embodiments. In particular, for apparatus or system embodiments, since they are basically similar to method embodiments, the description is relatively simple; relevant parts can be referred to the descriptions in the method embodiments. The apparatus and system embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. Those skilled in the art can understand and implement this without creative effort.

[0050] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features. These modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention, and the contents not described in detail in the specification of the present invention are known to those skilled in the art.

Claims

1. A disturbance-resistant UAV trajectory tracking control method, characterized in that, Includes the following steps: A six-degree-of-freedom rigid body dynamics model of a quadcopter UAV is established, including position dynamics equations and attitude dynamics equations; A feasible trajectory of the UAV that conforms to the ideal dynamics model is selected as the desired trajectory, and a coupled attitude is introduced, which is constructed by the direction vector of the virtual control force. Design a position loop control law, generate virtual control force based on a smooth terminal sliding diaphragm control surface, and calculate the actual thrust; Design an attitude loop control law, based on the Lie group SO(3) to represent the attitude error, construct a globally exponentially stable attitude tracking controller, and output the control torque; A fourth-order state observer is constructed to estimate the angular velocity and angular acceleration of the coupled attitude, and the estimated values ​​are used to replace the true values ​​in the calculation of the control law. The thrust and control torque are converted into thrust input for the quadcopter drone, driving the drone to track the desired trajectory.

2. The disturbance-resistant UAV trajectory tracking control method as described in claim 1, characterized in that, The six-degree-of-freedom rigid body dynamics model is expressed as follows: ； ； Where r represents the position of the center of mass in the inertial frame of reference. Let e3 represent the velocity of the center of mass in the inertial frame, e3 represent the unit vector of the inertial frame, dv represent the bounded disturbance along the position channel in the inertial frame, and m represent the mass of the object. f represents gravitational acceleration, and f represents the total control thrust. Represents the attitude matrix. I represents the attitude angular velocity vector, and I represents the UAV rotational inertia matrix. To control the torque.

3. The disturbance-resistant UAV trajectory tracking control method as described in claim 1, characterized in that, The coupling attitude is represented as: ； in, This represents the coupling term between position and attitude. Indicates the desired posture, These represent the three directions of the desired posture. , The direction representing the coupled attitude The rotational angular velocity representing the coupled attitude. Angular acceleration representing the coupled attitude.

4. The disturbance-resistant UAV trajectory tracking control method as described in claim 1, characterized in that, The smooth terminal sliding control surface is defined as: ； in, This indicates the positional error of the drone. Indicates the speed error of the drone. This represents a smooth switching function. Represents a symbolic function. This represents the control gain parameter for the slicker surface. .

5. The disturbance-resistant UAV trajectory tracking control method as described in claim 1, characterized in that, The control torque in the attitude loop control law is expressed as: ； in, Indicates control gain. This represents the moment of inertia of the drone. The rotational angular velocity represents the coupled attitude. The angular acceleration representing the coupled attitude. Indicates attitude error. Indicates the error in rotational angular velocity. yes The spinor is represented by, Represents the Morse function The gradient.

6. The disturbance-resistant UAV trajectory tracking control method as described in claim 1, characterized in that, The fourth-order state observer is represented as follows: ； in, Indicates the state of the observer. Represents mapping, The parameters representing the observer, Represents the angular velocity of the coupled attitude rotation The estimated value, Represents the coupled attitude. Represents the angular acceleration of the coupled attitude rotation. The estimated value.

7. A disturbance-resistant unmanned aerial vehicle (UAV) trajectory tracking control system, characterized in that, It includes a model building module, a coupled attitude generation module, a position control module, an attitude control module, a state observation module, and a tracking module; The model building module is used to build a six-degree-of-freedom rigid body dynamics model of a quadcopter UAV, including position dynamics equations and attitude dynamics equations. The coupled attitude generation module is used to select a feasible trajectory of the UAV that conforms to the ideal dynamics model as the desired trajectory, and introduce a coupled attitude, which is constructed from the direction vector of the virtual control force. The position control module is used to design the position loop control law, generate virtual control force based on the smooth terminal sliding diaphragm control surface, and calculate the actual thrust; The attitude control module is used to design the attitude loop control law, and construct a globally exponentially stable attitude tracking controller based on the Lie group SO(3) to represent the attitude error, and output the control torque. The state observation module is used to construct a fourth-order state observer to estimate the angular velocity and angular acceleration of the coupled attitude, and use the estimated values ​​to replace the true values ​​in the calculation of the control law; The tracking module is used to convert the thrust and the control torque into thrust input for the quadcopter UAV, driving the UAV to track the desired trajectory.

8. The disturbance-resistant UAV trajectory tracking control system as described in claim 7, characterized in that, The six-degree-of-freedom rigid body dynamics model is expressed as follows: ； ； Where r represents the position of the center of mass in the inertial frame of reference. Let e3 represent the velocity of the center of mass in the inertial frame, e3 represent the unit vector of the inertial frame, dv represent the bounded disturbance along the position channel in the inertial frame, and m represent the mass of the object. f represents gravitational acceleration, and f represents the total control thrust. Represents the attitude matrix. I represents the attitude angular velocity vector, and I represents the UAV rotational inertia matrix. To control the torque.

9. A computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the disturbance-resistant UAV trajectory tracking control method as described in any one of claims 1 to 4.

10. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by the processor, it implements the steps of the disturbance-resistant unmanned aerial vehicle trajectory tracking control method as described in any one of claims 1 to 4.

Images