Quad-rotor unmanned aerial vehicle trajectory tracking anti-interference control method and system

Abstract

The invention particularly relates to a four-rotor unmanned aerial vehicle trajectory tracking anti-interference control method and system. The four-rotor unmanned aerial vehicle trajectory tracking anti-interference control method comprises the following steps: establishing a four-rotor unmanned aerial vehicle double-loop control framework and a kinetic model, solving an error amount of a preset performance characteristic, constructing an extended state observer, regarding disturbance as a system state, estimating system uncertainty and external disturbance on line, and generating a compensation amount; a sliding mode controller is designed by taking sliding mode control as a core and combining an extended state observer, a preset performance error amount, a non-singular fast terminal sliding mode surface and an approaching rate, and high-precision trajectory tracking and anti-interference control are realized. According to the four-rotor unmanned aerial vehicle trajectory tracking anti-interference control method and system, high-precision trajectory tracking is achieved, the problem of collision or out-of-control caused by too large position errors can be remarkably reduced, the robustness to complex interference such as sudden change wind speed is remarkably improved, and the dynamic response time is shortened; and the method has low requirements on boundary conditions and is suitable for various small four-rotor unmanned aerial vehicles.

Description

Technical Field

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

[0002] A quadrotor UAV is an aircraft that relies on four rotors for lift. Due to its simple structure, maneuverability, small size, and ease of operation, it has been widely used in both military and civilian fields. Its applications include, but are not limited to, pesticide spraying, on-site reconnaissance, disaster relief, and aerial photography and mapping. With the rapid development of UAV technology, quadrotor UAVs have become a research hotspot in the aviation field, and their market demand and application scope continue to expand. However, the complexity of quadrotor UAV systems poses a significant challenge to their control technology.

[0003] Quadrotor unmanned aerial vehicle (UAV) systems exhibit multivariable, underactuated, and strongly coupled nonlinear characteristics. Their dynamic models involve complex motions with multiple degrees of freedom, including translation and rotation. These characteristics make UAVs susceptible to external disturbances and internal uncertainties in complex environments (such as strong winds, sudden load changes, or actuator failures), leading to decreased trajectory tracking accuracy or even system instability. Traditional control methods, such as proportional-integral-derivative (PID) control, demonstrate a certain degree of stability in many applications due to their simple structure. However, standard linear PID control struggles to meet the requirements of high accuracy and robustness in highly nonlinear and strongly coupled systems. Especially in the presence of external disturbances (such as wind) or actuator failures (such as decreased motor efficiency), the performance of PID controllers deteriorates significantly, failing to effectively cope with changes in the dynamic environment.

[0004] In recent years, researchers have attempted to improve the performance of quadcopter UAVs by refining control methods, such as adaptive PID, nonlinear PID, fuzzy control, and inversion control. These methods have improved the system's adaptability to some extent, but limitations remain. Adaptive PID can partially cope with system changes by adjusting parameters online, but its ability to suppress complex disturbances is limited; nonlinear PID and fuzzy control rely on empirical rules and lack systematic design methods; while inversion control can handle nonlinear systems, it lacks robustness in the face of uncertainties and actuator failures. Furthermore, traditional sliding mode control (SMC) has been widely studied due to its strong ability to suppress disturbances and uncertainties, but conventional sliding mode control suffers from slow convergence speed, potential chattering, and singularity issues, which limit its application in high-precision trajectory tracking tasks.

[0005] To address the aforementioned issues, researchers have proposed various improvement schemes, including higher-order sliding mode control, neural network-based control, and extended state observer (ESO)-based control. For example, higher-order sliding mode control reduces chattering by introducing higher-order derivatives, but its computational complexity is high; neural network-based control methods improve adaptability by learning system dynamics, but require significant computational resources; while conventional extended state observers (ESOs) can reduce system energy consumption by observing disturbances, their dynamic response and steady-state performance are insufficient when facing sudden external disturbances (such as sudden wind speed changes or load variations), and position and attitude errors are difficult to converge quickly. Although these methods have achieved certain results in specific scenarios, they still have shortcomings in comprehensively addressing both sudden external disturbances and the need for rapid convergence.

[0006] To address the issues of large system errors, insufficient robustness, and poor dynamic performance in existing quadrotor UAV trajectory tracking control methods under complex environments such as sudden wind disturbances and load changes, this invention proposes a quadrotor UAV trajectory tracking disturbance rejection control method and system. Summary of the Invention

[0007] To overcome the shortcomings of the prior art, this invention provides a simple and efficient method and system for trajectory tracking and anti-disturbance control of quadcopter unmanned aerial vehicles.

[0008] This invention is achieved through the following technical solution:

[0009] A method for trajectory tracking and disturbance rejection control of a quadcopter unmanned aerial vehicle (UAV) includes the following steps:

[0010] Step S1: Establish the dual-loop control framework and dynamic model of the quadcopter UAV.

[0011] By establishing a spatial coordinate system, a body coordinate system, an external disturbance model, and an actuator fault model, attitude dynamics model and position dynamics model of a second-order nonlinear system of a quadrotor UAV are established.

[0012] The quadcopter drone is modeled as a second-order nonlinear system with uncertainties. The channels of the quadcopter are decoupled, and the disturbance is further decomposed into each channel.

[0013] A wind disturbance composite model was constructed, and the wind disturbance was input into the attitude dynamics model and position dynamics model of the second-order nonlinear system of the quadrotor UAV, and observed through an extended state observer;

[0014] Step S2: Calculate the error of the preset performance characteristics.

[0015] Based on the preset performance theory, a performance constraint function is designed, and the system error is mapped into an unconstrained space. The error quantity with preset performance characteristics after transformation is further controlled.

[0016] Step S3: Construct the Extended State Observer (ESO)

[0017] By using an extended state observer to treat disturbances as system states, system uncertainties and external disturbances are estimated online, and compensation quantities are generated.

[0018] Step S4: Construct a preset performance-sliding mode controller

[0019] Based on sliding mode control, and combined with extended state observer, preset performance error, non-singular fast terminal sliding surface and approach rate, a sliding mode controller is designed to achieve high-precision trajectory tracking and disturbance rejection control.

[0020] In step S1, two basic coordinate systems are used: the body coordinate system and the spatial coordinate system. The motion states in the body coordinate system and the spatial coordinate system are transformed to each other, thereby describing the behavior of the quadcopter UAV in global space. The transformation matrix between the body coordinate system and the spatial coordinate system is as follows:

[0021]

[0022] Where B represents the body coordinate system and E represents the spatial coordinate system. Represents the rotation matrix from the body coordinate system to the spatial coordinate system;

[0023] In a spatial coordinate system, the position of the quadcopter UAV is obtained by the Newton-Euler theorem: p = [x, y, z]. T satisfy:

[0024]

[0025] Where m is the mass of the drone. For the acceleration of the drone, F g =[0,0,-mg] T F represents Earth's gravity. t The thrust of the UAV in the spatial coordinate system;

[0026] After coordinate transformation, the position dynamics model of the quadcopter UAV is obtained as follows:

[0027]

[0028] in, and These represent the linear velocities of the quadrotor UAV along the three coordinate axes in the spatial coordinate system; correspondingly, and These represent the accelerations along the three coordinate axes, k. x k y With k zThese represent the air damping coefficients along the three coordinate axes; ψ, θ, and φ represent the roll, pitch, and yaw angles, respectively; u f The lift generated by the propeller;

[0029] The attitude dynamics equations of a quadrotor UAV based on rigid body rotational dynamics are expressed as follows:

[0030]

[0031] Among them, I x I y with I z It is the moment of inertia; and These are the angular accelerations for roll, pitch, and yaw, respectively. and K1, K2, and K3 represent the angular velocities corresponding to the roll, pitch, and yaw angles, respectively; K4, K5, and K6 represent the damping coefficients in the roll, pitch, and yaw directions, respectively.

[0032] In step S1, the wind disturbance composite model is constructed by combining the basic wind disturbance model, the gust wind disturbance model, the gradual wind disturbance model, and the random wind disturbance model under low-altitude conditions; the mathematical expressions of the above four models are as follows:

[0033] (1) Basic wind disturbance model:

[0034] The basic wind refers to a constant or slowly changing wind, determined by the environmental wind field, and can be represented by the following mathematical model:

[0035] W steady =W0(1+αsin(ωt))

[0036] Where W0 is the reference wind speed, α is the rate of change of wind speed, and ω is the low-frequency rate of change, which is generally less than 0.1 rad / s; (2) Gust wind disturbance model:

[0037] Gusts are rapid changes in wind speed over a short period of time. A 1-cos gust model based on a slope is used:

[0038]

[0039] W gust (t)=W g ,t>T g

[0040] Among them, W g T represents the maximum wind speed. g The duration is given by t, where t is the duration of the gust.

[0041] (3) Gradual wind disturbance model:

[0042] Gradual wind is wind whose speed changes linearly over time, represented by a ramp signal:

[0043]

[0044] Where k is the rate of change of wind speed, t1 and t2 are the start and end times of the gradual wind change, and v max That is the maximum wind speed;

[0045] (4) Random wind disturbance model:

[0046] Random wind is wind whose speed and direction change randomly, and is represented by a random signal (such as Gaussian white noise):

[0047] W random (t)=σ·η(t)

[0048] Where σ is the standard deviation of wind speed, and η(t) is Gaussian white noise;

[0049] The decoupled mathematical model of the position dynamics and attitude dynamics of the quadrotor UAV is expressed as follows:

[0050]

[0051]

[0052] For ease of subsequent analysis, the position dynamics model and attitude dynamics model are redefined as follows, where ξ 1i Represents speed, ξ 2i Represents acceleration, u i f represents the controller output. i (ξ) is a known nonlinear term, g i d is the controller coefficient. i This represents unknown uncertainties and external disturbances;

[0053]

[0054] The expressions for each parameter are as follows:

[0055] [ξ 1x ,ξ 1y ,ξ 1z ,ξ 1θ ,ξ 1φ ,ξ 1ψ ] T =[x,y,z,θ,φ,ψ] T

[0056]

[0057] [u x ,u y ,uz ,u θ ,u φ ,u ψ ] T =[a1u f ,a2u f ,a3u f ,τ θ ,τ φ ,τ ψ ] T

[0058] a1=cos(φ)sin(θ)cos(ψ)+sin(φ)sin(ψ)

[0059] a2=cos(φ)sin(θ)sin(ψ)-sin(φ)cos(ψ)

[0060] a3=cos(φ)cos(θ)

[0061] Given the nonlinear term f i The expression is:

[0062]

[0063] Controller system g i The expression is:

[0064]

[0065] In step S2, the error quantity of the second-order nonlinear system of the quadcopter UAV is defined as e. i =ξ 1i -ξ r,i , where ξ 1i ξ represents the actual position or angle of a quadcopter drone in various directions within a spatial coordinate system. r,i This refers to the reference position or desired angle of the quadcopter drone.

[0066] To ensure error e i To satisfy the preset transient and steady-state performance, a performance function is introduced:

[0067]

[0068] Where, ρ 0i Represents the initial performance bound and ρ 0i >|e i (0)|;ρ ∞i For steady-state performance bounds and ρ ∞i >0; β i Let be the attenuation rate, satisfying β i >0;

[0069] Performance function ρi (t) satisfies the following condition:

[0070] (1)ρ i (t) is monotonically decreasing because

[0071]

[0072] The construction error constraint is:

[0073] -ρ i (t)<e i (t)<ρ i (t)

[0074] The constrained error e i (t) can be reasonably transformed into an unconstrained variable λ. i :

[0075] e i (t)=ρ i (t)S(λ i )

[0076] Where S(λ) i S(λ) is a homeomorphic mapping function, which is a smooth, strictly monotonically increasing function, and satisfies -1 < S(λ). i If ) < 1, the expression is as follows:

[0077]

[0078] After performing the inverse transformation, we get:

[0079]

[0080] when e i (t) satisfies -ρ i (t)<e i (t)<ρ i When (t), we get And λ i ∈(-∞,∞), the error constraint can also be guaranteed, thus completing the transformation from the error-constrained space to the unconstrained space.

[0081] In step S3, the external disturbance d i Defined as extended state ξ 3i =d i Then, the extended second-order nonlinear system of the quadcopter UAV becomes the following form:

[0082]

[0083] Where, ξ 1i ξ indicates the position or angle of a quadcopter drone.2i u represents the speed or angular velocity of a quadcopter drone. i f represents the controller output. i (ξ) is a known nonlinear term, g i d is the controller coefficient. i Representing unknown uncertainty and external disturbances, y i Indicate the output equation;

[0084] Then, a linear extended state observer (ESO) is designed as follows:

[0085]

[0086] in, Representing ξ respectively 1i ,ξ 2i ,d i Observed values; e i For observation error; β 1i ,β 2i ,β 3i >0 represents the observer gain;

[0087] Define the estimation error:

[0088]

[0089] The dynamic equation for the estimation error is expressed in the following form:

[0090]

[0091] Rewritten in matrix form:

[0092]

[0093] By customizing the selection of β 1i ,β 2i ,β 3i The value of enables the extended state observer to effectively track the system's state variables and generate compensation for disturbances.

[0094] In step S4, a preset performance-non-singular terminal sliding surface is defined:

[0095]

[0096] Where α>0, β>0, g / h>p / q, 1<p / q<2, and p,q,g,h are positive odd numbers;

[0097] When s i When = 0, we get:

[0098]

[0099] Since 1 < p / q < 2, we can obtain 0 < q / p < 1, which effectively solves the problem of singular phenomena in the second-order nonlinear system of a quadcopter UAV under traditional sliding mode control when the error is 0.

[0100] In step S4, the control rate required for the sliding mode controller is further designed:

[0101] Control law design is divided into equivalent control law and switching control law:

[0102] u i =u eq,i +u sw,i

[0103] Among them, u eq,i Equivalent control rate, u sw,i The equivalent control is the switching control law;

[0104] To obtain the equivalent control law u by combining preset performance constraints and extended state observers. eq,i It is expressed as:

[0105]

[0106] To resist disturbances and ensure finite-time convergence, a switching control law u is designed. sw,i ,as follows:

[0107]

[0108] Where, k 3i >0,k 4i >0.

[0109] A quadcopter unmanned aerial vehicle (UAV) trajectory tracking and anti-disturbance control system, used to implement the above method, includes:

[0110] The dynamics modeling module is responsible for establishing the attitude dynamics model and position dynamics model of the second-order nonlinear system of the quadrotor UAV by establishing a spatial coordinate system, a body coordinate system, an external disturbance model, and an actuator failure model; modeling the quadrotor UAV as a second-order nonlinear system with uncertainties, decoupling the channels of the quadrotor, and further decomposing the disturbances into each channel;

[0111] The preset performance constraint and error transformation module is responsible for designing performance constraint functions based on preset performance theory, performing unconstrained spatial mapping of system errors, and further controlling the error quantity with preset performance characteristics after transformation.

[0112] The extended state observer module is responsible for treating disturbances as system states through the extended state observer, estimating system uncertainties and external disturbances online, and generating compensation quantities.

[0113] The attitude calculation and motor speed control module is responsible for designing a sliding mode controller based on sliding mode control, combined with an extended state observer, preset performance error, non-singular fast terminal sliding surface, and approach rate, to achieve high-precision trajectory tracking and disturbance rejection control.

[0114] A quadcopter unmanned aerial vehicle (UAV) trajectory tracking and anti-disturbance control device includes a memory and a processor; the memory is used to store a computer program, and the processor is used to implement the method when executing the computer program.

[0115] A readable storage medium storing a computer program that, when executed by a processor, implements the method.

[0116] The beneficial effects of this invention are: the quadcopter UAV trajectory tracking anti-disturbance control method and system, by strictly constraining the pose error through a preset performance function, achieves high-precision trajectory tracking, which can significantly reduce the problem of collision or loss of control caused by excessive position error of the quadcopter UAV due to sudden disturbances;

[0117] Meanwhile, by using an extended state observer to estimate and compensate for external disturbances and internal uncertainties in real time, the robustness to complex disturbances such as sudden wind speed changes is significantly improved.

[0118] Furthermore, non-singular terminal sliding mode control can ensure that the system converges quickly within a finite time, thus shortening the dynamic response time.

[0119] It has low requirements for boundary conditions, is suitable for a variety of small quadcopter UAVs, and can flexibly optimize dynamic response and steady-state accuracy by adjusting relevant parameters. Attached Figure Description

[0120] To more clearly illustrate the technical solutions in the embodiments of the present invention or 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 some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0121] Appendix Figure 1 This is a schematic diagram of the dual-ring control structure of the quadcopter UAV of the present invention.

[0122] Appendix Figure 2 This is a schematic diagram of the coordinate system transformation of the quadcopter UAV of the present invention.

[0123] Appendix Figure 3 This is a schematic diagram of the three-dimensional tracking curve of the quadcopter UAV of the present invention.

[0124] Appendix Figure 4This is a schematic diagram of the x-direction tracking response comparison curves of the present invention.

[0125] Appendix Figure 5 This is a schematic diagram of the tracking response comparison curves in the y-direction of the present invention.

[0126] Appendix Figure 6 This is a schematic diagram of the z-direction tracking response comparison curves of the present invention.

[0127] Appendix Figure 7 This is a schematic diagram of the error envelope in the x, y, and z directions of the present invention.

[0128] Appendix Figure 8 This is a schematic diagram of the wind disturbance tracking curve of the extended state observer of the present invention.

[0129] Appendix Figure 9 This is a schematic diagram of the speed response of the quadcopter motor of the present invention. Detailed Implementation

[0130] To enable those skilled in the art to better understand the technical solutions of this invention, the technical solutions in the embodiments of this invention will be clearly and completely described below in conjunction with the embodiments of this invention. Obviously, the described embodiments are merely some embodiments of this invention, and not all embodiments. Based on the embodiments of this invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of this invention.

[0131] The trajectory tracking and disturbance rejection control method for this quadcopter UAV includes the following steps:

[0132] Step S1: Establish the dual-loop control framework and dynamic model of the quadcopter UAV.

[0133] By establishing a spatial coordinate system, a body coordinate system, an external disturbance model, and an actuator fault model, attitude dynamics model and position dynamics model of a second-order nonlinear system of a quadrotor UAV are established.

[0134] The quadcopter drone is modeled as a second-order nonlinear system with uncertainties. The channels of the quadcopter are decoupled, and the disturbance is further decomposed into each channel.

[0135] A wind disturbance composite model was constructed, and the wind disturbance was input into the attitude dynamics model and position dynamics model of the second-order nonlinear system of the quadrotor UAV, and observed through an extended state observer;

[0136] Step S2: Calculate the error of the preset performance characteristics.

[0137] Based on the preset performance theory, a performance constraint function is designed, and the system error is mapped into an unconstrained space. The error quantity with preset performance characteristics after transformation is further controlled.

[0138] Step S3: Construct the Extended State Observer (ESO)

[0139] By using an extended state observer to treat disturbances as system states, system uncertainties and external disturbances are estimated online, and compensation quantities are generated.

[0140] Step S4: Construct a preset performance-sliding mode controller

[0141] Based on sliding mode control, and combined with extended state observer, preset performance error, non-singular fast terminal sliding surface and approach rate, a sliding mode controller is designed to achieve high-precision trajectory tracking and disturbance rejection control.

[0142] In step S1, to accurately describe the dynamic model of the quadcopter UAV, a reasonable coordinate system needs to be established to describe the position, velocity, angle, and other information of the quadcopter UAV. Two basic coordinate systems are used: the body coordinate system B (OB, XB, YB, ZB) and the spatial coordinate system E (OE, XE, YE, ZE). The target position and trajectory of the quadcopter UAV are based on global space, so it is necessary to convert the motion state in the body coordinate system to that in the spatial coordinate system to more intuitively describe the behavior of the quadcopter UAV in global space. The transformation matrix between the two is as follows:

[0143]

[0144] Where B represents the body coordinate system and E represents the spatial coordinate system. Represents the rotation matrix from the body coordinate system to the spatial coordinate system;

[0145] In a spatial coordinate system, the position of the quadcopter UAV is obtained by the Newton-Euler theorem: p = [x, y, z]. T satisfy:

[0146]

[0147] Where m is the mass of the drone. For the acceleration of the drone, F g =[0,0,-mg] T F represents Earth's gravity. t The thrust of the UAV in the spatial coordinate system;

[0148] After coordinate transformation, the position dynamics model of the quadcopter UAV is obtained as follows:

[0149]

[0150] in, and The linear velocity of the quadcopter; and Represents acceleration; ψ, θ, and φ represent roll, pitch, and yaw angles, respectively; k x k y With k z Represents the air damping coefficient; u f The lift generated by the propeller;

[0151] The attitude dynamics equations of a quadrotor UAV based on rigid body rotational dynamics are expressed as follows:

[0152]

[0153] Among them, I x I y with I z It is the moment of inertia; and For roll, pitch, and yaw, angular acceleration; and K represents the angular velocity corresponding to the roll, pitch, and yaw angles; K4, K5, and K6 represent the damping coefficients.

[0154] In step S1, the wind disturbance composite model is constructed by combining the basic wind disturbance model, the gust wind disturbance model, the gradual wind disturbance model, and the random wind disturbance model under low-altitude conditions; the mathematical expressions of the above four models are as follows:

[0155] (1) Basic wind disturbance model:

[0156] The basic wind refers to a constant or slowly changing wind, determined by the environmental wind field, and can be represented by the following mathematical model:

[0157] W steady =W0(1+αsin(ωt))

[0158] Where W0 is the reference wind speed, α is the rate of change of wind speed, and ω is the low-frequency rate of change, which is generally less than 0.1 rad / s;

[0159] (2) Gust wind disturbance model:

[0160] Gusts are rapid changes in wind speed over a short period of time. A 1-cos gust model based on a slope is used:

[0161]

[0162] W gust (t)=W g ,t>T g

[0163] Among them, W g T represents the maximum wind speed. gThe duration is given by t, where t is the duration of the gust.

[0164] (3) Gradual wind disturbance model:

[0165] Gradual wind is wind whose speed changes linearly over time, represented by a ramp signal:

[0166]

[0167] Where k is the rate of change of wind speed, t1 and t2 are the start and end times of the gradual wind change, and v max That is the maximum wind speed;

[0168] (4) Random wind disturbance model:

[0169] Random wind is wind whose speed and direction change randomly, and is represented by a random signal (such as Gaussian white noise):

[0170] W random (t)=σ·η(t)

[0171] Where σ is the standard deviation of wind speed, and η(t) is Gaussian white noise;

[0172] When wind disturbance acts on a quadrotor UAV, it can be decomposed in a spatial coordinate system into forces acting on the three coordinate axes. Regardless of the type of wind, the impact on the quadrotor UAV ultimately manifests as forces and moments; therefore, the disturbing forces and moments generated by the wind need to be considered in the quadrotor's control system. The decoupled mathematical model of the quadrotor UAV's position dynamics and attitude dynamics is expressed as follows:

[0173]

[0174] For ease of subsequent analysis, the position dynamics model and attitude dynamics model are redefined as follows, where ξ 1i Represents speed, ξ 2i Represents acceleration, u i f represents the controller output. i (ξ) is a known nonlinear term, g i d is the controller coefficient. i This represents unknown uncertainties and external disturbances;

[0175]

[0176] The expressions for each parameter are as follows:

[0177] [ξ 1x ,ξ 1y ,ξ 1z ,ξ 1θ ,ξ 1φ ,ξ 1ψ ]T =[x,y,z,θ,φ,ψ] T

[0178]

[0179] [u x ,u y ,u z ,u θ ,u φ ,u ψ ] T =[a1u f ,a2u f ,a3u f ,τ θ ,τ φ ,τ ψ ] T

[0180] a1=cos(φ)sin(θ)cos(ψ)+sin(φ)sin(ψ)

[0181] a2=cos(φ)sin(θ)sin(ψ)-sin(φ)cos(ψ)

[0182] a3=cos(φ)cos(θ)

[0183] Given the nonlinear term f i The expression is:

[0184]

[0185] Controller system g i The expression is:

[0186]

[0187] In step S2, the error quantity of the second-order nonlinear system of the quadcopter UAV is defined as e. i =ξ 1i -ξ r,i ξ 1i ξ represents the actual position or angle of a quadcopter drone in various directions within a spatial coordinate system. r,i This refers to the reference position or desired angle of the quadcopter drone.

[0188] To ensure error e i To satisfy the preset transient and steady-state performance, a performance function is introduced:

[0189]

[0190] Where, ρ 0i Represents the initial performance bound and ρ 0i>|e i (0)|;ρ ∞i For steady-state performance bounds and ρ ∞i >0; β i Let be the attenuation rate, satisfying β i >0;

[0191] Performance function ρ i (t) satisfies the following condition:

[0192] (1)ρ i (t) is monotonically decreasing because

[0193] (2)

[0194] The construction error constraint is:

[0195]

[0196] The maximum value of the desired tracking error is adjusted by adjusting βρ0, and the maximum value of the error is adjusted by adjusting βρ. ∞ The error range at steady state is adjusted by regulating the convergence rate of the error through the parameter β;

[0197] In this design, β = 1 is appropriately selected:

[0198] -ρ i (t)<e i (t)<ρ i (t)

[0199] The preset performance constraints contain inequalities, so to facilitate the subsequent quadrotor control rate design, the constrained e needs to be... i (t) can be reasonably transformed into an unconstrained variable λ. i :

[0200] e i (t)=ρ i (t)S(λ i )

[0201] Where S(λ) i It is designed to be in the following form:

[0202]

[0203] S(λ i Also known as the homeomorphism function, it is a smooth, strictly monotonically increasing function that satisfies -1 < S(λ). i If ) < 1, then after performing an inverse transformation, we get:

[0204]

[0205] when e i (t) satisfies -ρ i (t)<e i (t)<ρ i When (t), we get And λ i ∈(-∞,∞), the error constraint can also be guaranteed, thus completing the transformation from the error-constrained space to the unconstrained space.

[0206] In step S3, the external disturbance d i Defined as extended state ξ 3i =d i Taking the i-th channel of the original system model as an example;

[0207]

[0208] Where: ξ 1i ξ indicates the position or angle of a quadcopter drone. 2i f represents the speed or angular velocity of a quadcopter drone. i (ξ) represents the known nonlinear dynamics of the system (which may include coupling terms, gravity, Coriolis forces, etc.), g i (ξ) represents the control gain (usually invertible and known), u i Indicates control input, d i (t) represents external disturbances (including wind disturbances, unmodeled dynamics, etc.);

[0209] Disturbance d i (t) is extended to a new state, let ξ 3i =d i (t); Assuming the disturbance changes slowly, or its rate of change is bounded and unknown, it can be written as:

[0210]

[0211] Here, h(t) is an unknown function, but it is usually assumed that |h(t)| is bounded.

[0212] Therefore, the extended second-order nonlinear system of the quadcopter UAV becomes a third-order system, in the following form:

[0213]

[0214] Where, ξ 1i ξ indicates the position or angle of a quadcopter drone. 2i u represents the speed or angular velocity of a quadcopter drone. i f represents the controller output. i (ξ) is a known nonlinear term, g i d is the controller coefficient. i Representing unknown uncertainty and external disturbances, yi Indicate the output equation;

[0215] For quadcopter drones, the physical meanings of the above four items are as follows:

[0216] Assuming i = 1, 2, 3, 4 correspond to the four control channels of the quadcopter UAV (e.g., x-position, y-position, z-position, yaw angle), the meaning of each term in the equation is as follows:

[0217] First equation: Kinematic relationship: The derivative of position / angle equals velocity / angular velocity;

[0218] Second equation: f i (ξ)+g i (ξ)u i +ξ 3i The dynamic relationship is expressed as: acceleration = known nonlinear term + control term + disturbance state ξ. 3i ;

[0219] Third equation: Representing disturbance dynamics: Modeling an external disturbance as a state whose rate of change is unknown but bounded;

[0220] Output equation: y i =ξ 1i This indicates the system output, which is usually the position or angle (which can be directly measured);

[0221] After this extension, the total disturbance ξ of the second-order nonlinear system of the quadcopter UAV 3i It becomes a state variable, which can be estimated in real time using an extended state observer;

[0222] Then, a linear extended state observer (ESO) is designed as follows:

[0223]

[0224] in, Representing ξ respectively 1i ,ξ 2i ,d i Observed values; e i For observation error; β 1i ,β 2i ,β 3i >0 represents the observer gain;

[0225] Define the estimation error:

[0226]

[0227] The dynamic equation for the estimation error is expressed in the following form:

[0228]

[0229] Rewritten in matrix form:

[0230]

[0231] By customizing the selection of β 1i ,β 2i ,β 3i The value of enables the extended state observer to effectively track the system's state variables and generate compensation for disturbances.

[0232] In step S4, a preset performance-non-singular terminal sliding surface is defined:

[0233]

[0234] Where α>0, β>0, g / h>p / q, 1<p / q<2, and p,q,g,h are positive odd numbers;

[0235] When s i When = 0, we get:

[0236]

[0237] Because the traditional terminal sliding surface form is Where β > 0, 0 < p / q < 1;

[0238] To analyze the stability of the system, it is necessary to differentiate with respect to s to design the control law. The final control law equation for the terminal sliding mode is generally in the form of:

[0239]

[0240] When 0 < p / q < 1, the error λ will be controlled to tend to 0. Therefore, when the error approaches 0, since -1 < p / q - 1 < 0, This can lead to singularity problems.

[0241] Here, 1 < p / q < 2, which effectively solves the problem of singular phenomena occurring in the second-order nonlinear system of a quadcopter UAV under traditional terminal sliding mode control when the error is 0.

[0242] In step S4, the control rate required for the sliding mode controller is further designed:

[0243] Control law design is divided into equivalent control law and switching control law:

[0244] u i =u eq,i +u sw,i

[0245] Among them, u eq,iEquivalent control rate, u sw,i The equivalent control is the switching control law;

[0246] To obtain the equivalent control law u by combining preset performance constraints and extended state observers. eq,i It is expressed as:

[0247]

[0248] To resist disturbances and ensure finite-time convergence, a switching control law u is designed. sw,i ,as follows:

[0249]

[0250] Where, k 3i >0,k 4i >0.

[0251] The quadcopter UAV trajectory tracking and disturbance rejection control system is used to implement the above method, including:

[0252] The dynamics modeling module is responsible for establishing the attitude dynamics model and position dynamics model of the second-order nonlinear system of the quadrotor UAV by establishing a spatial coordinate system, a body coordinate system, an external disturbance model, and an actuator failure model; modeling the quadrotor UAV as a second-order nonlinear system with uncertainties, decoupling the channels of the quadrotor, and further decomposing the disturbances into each channel;

[0253] The preset performance constraint and error transformation module is responsible for designing performance constraint functions based on preset performance theory, performing unconstrained spatial mapping of system errors, and further controlling the error quantity with preset performance characteristics after transformation.

[0254] The extended state observer module is responsible for treating disturbances as system states through the extended state observer, estimating system uncertainties and external disturbances online, and generating compensation quantities.

[0255] The attitude calculation and motor speed control module is responsible for designing a sliding mode controller based on sliding mode control, combined with an extended state observer, preset performance error, non-singular fast terminal sliding surface, and approach rate, to achieve high-precision trajectory tracking and disturbance rejection control.

[0256] Example

[0257] To verify the effectiveness of the proposed control method, a Matlab simulation platform and a reverse comparison simulation experiment were used to compare the simulation results with the extended state observer-nonsingular fast termination sliding mode control method and the nonsingular fast termination sliding mode control method under wind disturbance conditions. The parameters of the quadcopter UAV in the simulation are shown in Table 1, referencing the DJI Mini 2 and similarly sized UAVs. The sliding mode controller parameters are shown in Table 2, and the wind disturbance parameters are shown in Table 3. The simulation results are attached. Figure 3 To be continued Figure 8 As shown.

[0258] Simulation experiments verified that the control method of this invention exhibits significant advantages in both anti-interference performance and dynamic response. The traditional NFTSMC scheme, under sudden wind disturbances, shows a position tracking error (e.g., a Z-direction error of 0.281m) significantly exceeding the allowable error limit. While the NFTSMC-ESO scheme reduces the error (e.g., 0.0123m in the Z-direction) by estimating the disturbance through ESO, it still fails to meet the preset error requirements. In contrast, as shown in the attached... Figure 4 - Appendix Figure 7 As shown, this method, combining error transformation and preset performance constraints, can strictly limit all errors within the preset limits even under strong wind disturbances of 10–15 seconds, demonstrating extremely strong robustness. (According to the appendix...) Figure 3 The 3D trajectory tracking results show that the proposed scheme can quickly suppress drift caused by wind disturbance, with no overshoot in attitude and position errors and minimal steady-state deviation. Furthermore, based on single-machine speed simulation results... Figure 9 The dynamic adjustment of control input and motor speed verified the real-time performance of the algorithm; lift, torque, and motor speed all responded quickly to sudden wind disturbances. The integration of PPC and NFTSMC solved the overshoot problem of traditional sliding mode control through error transformation, and by adding... Figure 8 It can be observed that the enhanced compensation through ESO further improves the system's ability to estimate complex disturbances. This solution provides a reliable solution for the precise control of UAVs in complex environments, and is particularly suitable for applications with high precision requirements such as logistics and inspection, achieving a balance between bounded error, anti-interference capability, and dynamic response.

[0259] Table 1 Physical parameters of the quadcopter

[0260] Table 2 Controller Parameters

[0261]

[0262]

[0263] Note: The proportional gain k3 under the NFTSMC control scheme is 20, and other parameters are the same as in the table above.

[0264] Table 3 Wind Disturbance Parameters

[0265]

[0266] A quadcopter unmanned aerial vehicle (UAV) trajectory tracking and anti-disturbance control device includes a memory and a processor; the memory is used to store computer programs, and the processor is used to execute methods implemented when executing the computer programs.

[0267] A readable storage medium on which a computer program is stored, and a method for implementing the computer program when executed by a processor.

[0268] The foregoing has provided a detailed description of a quadcopter UAV trajectory tracking anti-disturbance control method and system according to an example of the present invention. This section uses specific examples to illustrate the principles and implementation methods of the invention. These examples are only for the purpose of helping to understand the core ideas of the present invention. All other embodiments obtained by those skilled in the art without creative effort, without departing from the principles of the present invention, should fall within the scope of protection of the present invention.

Claims

1. A method for trajectory tracking and disturbance rejection control of a quadcopter unmanned aerial vehicle, characterized in that: Includes the following steps: Step S1: Establish the dual-loop control framework and dynamic model of the quadcopter UAV. By establishing a spatial coordinate system, a body coordinate system, an external disturbance model, and an actuator fault model, attitude dynamics model and position dynamics model of a second-order nonlinear system of a quadrotor UAV are established. The quadcopter drone is modeled as a second-order nonlinear system with uncertainties. The channels of the quadcopter are decoupled, and the disturbance is further decomposed into each channel. A wind disturbance composite model was constructed, and the wind disturbance was input into the attitude dynamics model and position dynamics model of the second-order nonlinear system of the quadrotor UAV, and observed through an extended state observer; Step S2: Calculate the error of the preset performance characteristics. Based on the preset performance theory, a performance constraint function is designed, and the system error is mapped into an unconstrained space. The error quantity with preset performance characteristics after transformation is further controlled. Step S3: Construct the extended state observer By using an extended state observer to treat disturbances as system states, system uncertainties and external disturbances are estimated online, and compensation quantities are generated. Step S4: Construct a preset performance-sliding mode controller Based on sliding mode control, and combined with extended state observer, preset performance error, non-singular fast terminal sliding surface and approach rate, a sliding mode controller is designed to achieve high-precision trajectory tracking and disturbance rejection control.

2. The quadcopter UAV trajectory tracking and disturbance rejection control method according to claim 1, characterized in that: In step S1, two basic coordinate systems are used: the body coordinate system and the spatial coordinate system. The motion states in the body coordinate system and the spatial coordinate system are transformed to each other, thereby describing the behavior of the quadcopter UAV in global space. The transformation matrix between the body coordinate system and the spatial coordinate system is as follows: Where B represents the body coordinate system and E represents the spatial coordinate system. Represents the rotation matrix from the body coordinate system to the spatial coordinate system; In a spatial coordinate system, the position of the quadcopter UAV is obtained by the Newton-Euler theorem: p = [x, y, z]. T ; After coordinate transformation, the position dynamics model of the quadcopter UAV is obtained as follows: in, and These represent the linear velocities of the quadrotor UAV along the three coordinate axes in the spatial coordinate system; correspondingly, and These represent the accelerations along the three coordinate axes, k. x k y With k z These represent the air damping coefficients along the three coordinate axes; ψ, θ, and φ represent the roll, pitch, and yaw angles, respectively; u f The lift generated by the propeller; The attitude dynamics equations of a quadrotor UAV based on rigid body rotational dynamics are expressed as follows: Among them, I x I y with I z It is the moment of inertia; and These are the angular accelerations for roll, pitch, and yaw, respectively. and K1, K2, and K3 represent the angular velocities corresponding to the roll, pitch, and yaw angles, respectively; K4, K5, and K6 represent the damping coefficients in the roll, pitch, and yaw directions, respectively.

3. The quadcopter UAV trajectory tracking and disturbance rejection control method according to claim 2, characterized in that: In step S1, the wind disturbance composite model is constructed by combining the basic wind disturbance model, the gust wind disturbance model, the gradual wind disturbance model, and the random wind disturbance model under low-altitude conditions; the mathematical expressions of the above four models are as follows: (1) Basic wind disturbance model: The basic wind refers to a constant or slowly changing wind, determined by the environmental wind field, and can be represented by the following mathematical model: W steady =W0(1+αsin(ωt)) Where W0 is the reference wind speed, α is the rate of change of wind speed, and ω is the low-frequency rate of change, which is generally less than 0.1 rad / s; (2) Gust wind disturbance model: Gusts are rapid changes in wind speed over a short period of time. A 1-cos gust model based on a slope is used: W gust (t)=W g ,t>T g Among them, W g T represents the maximum wind speed. g The duration is given by t, where t is the duration of the gust. (3) Gradual wind disturbance model: Gradual wind is wind whose speed changes linearly over time, represented by a ramp signal: Where k is the rate of change of wind speed, t1 and t2 are the start and end times of the gradual wind change, and v max That is the maximum wind speed; (4) Random wind disturbance model: Random wind is wind whose speed and direction change randomly, and is represented by random signals: W random (t)=σ·η(t) Where σ is the standard deviation of wind speed, and η(t) is Gaussian white noise; The decoupled mathematical model of the position dynamics and attitude dynamics of the quadrotor UAV is expressed as follows:

4. The quadcopter UAV trajectory tracking and disturbance rejection control method according to claim 3, characterized in that: In step S2, the error of the quadcopter system is defined as e. i =ξ 1i -ξ r,i , where ξ 1i ξ represents the actual position or angle of a quadcopter drone in various directions within a spatial coordinate system. r,i This refers to the reference position or desired angle of the quadcopter drone. To ensure error e i To satisfy the preset transient and steady-state performance, a performance function is introduced: Where, ρ 0i Represents the initial performance bound and ρ 0i >|e i (0)|;ρ ∞i For steady-state performance bounds and ρ ∞i >0; β i Let be the attenuation rate, satisfying β i >0; Performance function ρ i (t) satisfies the following condition: (1)ρ i (t) is monotonically decreasing because (2) The construction error constraint is: -r i (t)<e i (t)<ρ i (t) The constrained error e i (t) can be reasonably transformed into an unconstrained variable λ. i : e i (t)=ρ i (t)S(λ i ) Wherein, S(λ) i S(λ) is a homeomorphic mapping function, which is a smooth, strictly monotonically increasing function, and satisfies -1 < S(λ). i If ) < 1, the expression is as follows: After performing the inverse transformation, we get: When e i (t) satisfies -ρ i (t)<e i (t)<ρ i When (t), we get And λ i ∈(-∞,∞), the error constraint can also be guaranteed, thus completing the transformation from the error-constrained space to the unconstrained space.

5. The quadcopter UAV trajectory tracking and disturbance rejection control method according to claim 4, characterized in that: In step S3, the external disturbance d i Defined as extended state ξ 3i =d i Then the extended second-order nonlinear system of the quadcopter drone becomes a third-order system: Where: ξ 1i ξ indicates the position or angle of a quadcopter drone. 2i f represents the speed or angular velocity of a quadcopter drone. i (ξ) represents the known nonlinear dynamics of the system, g i (ξ) represents the control gain, u i Indicates control input, d i Let y(t) represent the external disturbance, h(t) represent the bounded unknown function, and y(t) represent the unknown function. i This indicates the output equation, and the output result includes position or angle; After expansion, the total disturbance ξ of the second-order nonlinear system of the quadcopter UAV 3i It is a state variable estimated in real time using an extended state observer; The linear extended state observer is designed as follows: in, Representing ξ respectively 1i ,ξ 2i ,d i Observed values; e i For observation error; β 1i ,β 2i ,β 3i >0 represents the observer gain; Define the estimation error: The dynamic equation for the estimation error is expressed in the following form: Rewritten in matrix form: By customizing the selection of β 1i ,β 2i ,β 3i The value of allows the extended state observer to track the system's state variables and generate compensation for disturbances.

6. The quadcopter UAV trajectory tracking and disturbance rejection control method according to claim 5, characterized in that: In step S4, a preset performance-non-singular terminal sliding surface is defined: Where α>0, β>0, g / h>p / q, 1<p / q<2, and p,q,g,h are positive odd numbers; When s i When = 0, we get: In step S4, the control rate required for the sliding mode controller is further designed: Control law design is divided into equivalent control law and switching control law: in i =in eq,i +in sw,i Among them, u eq,i U represents the equivalent control rate. sw,i This indicates that the equivalent control is the switching control law; To obtain the equivalent control law u by combining preset performance constraints and extended state observers. eq,i It is expressed as: To resist disturbances and ensure finite-time convergence, a switching control law u is designed. sw,i ,as follows: Where, k 3i >0,k 4i >0.

7. A trajectory tracking and anti-interference control system for a quadcopter unmanned aerial vehicle (UAV), characterized in that: For implementing the method as described in any one of claims 1 to 6, comprising: The dynamics modeling module is responsible for establishing the attitude dynamics model and position dynamics model of the second-order nonlinear system of the quadrotor UAV by establishing a spatial coordinate system, a body coordinate system, an external disturbance model, and an actuator failure model; modeling the quadrotor UAV as a second-order nonlinear system with uncertainties, decoupling the channels of the quadrotor, and further decomposing the disturbances into each channel; The preset performance constraint and error transformation module is responsible for designing performance constraint functions based on preset performance theory, performing unconstrained spatial mapping of system errors, and further controlling the error quantity with preset performance characteristics after transformation. The extended state observer module is responsible for treating disturbances as system states through the extended state observer, estimating system uncertainties and external disturbances online, and generating compensation quantities. The attitude calculation and motor speed control module is responsible for designing a sliding mode controller based on sliding mode control, combined with an extended state observer, preset performance error, non-singular fast terminal sliding surface, and approach rate, to achieve high-precision trajectory tracking and disturbance rejection control.

8. A trajectory tracking and anti-disturbance control device for a quadcopter unmanned aerial vehicle, characterized in that: It includes a memory and a processor; the memory is used to store a computer program, and the processor is used to execute the computer program to implement the method as described in any one of claims 1 to 6.

9. A readable storage medium, characterized in that: The readable storage medium stores a computer program that, when executed by a processor, implements the method as described in any one of claims 1 to 6.

Images