Three-dimensional trajectory tracking control method for four-rotor unmanned aerial vehicle facing sea surface wind wave disturbance

Abstract

The application provides a four-rotor unmanned aerial vehicle three-dimensional trajectory tracking control method for sea surface wind wave disturbance, and belongs to the field of trajectory tracking control. The method establishes a four-rotor six-degree-of-freedom dynamics simulation model and adopts a fourth-order Runge-Kutta method to update the state; a multi-unmanned aerial vehicle three-dimensional reference trajectory generation mechanism is constructed; a six-dimensional comprehensive disturbance model containing wave-induced vertical force, gust horizontal force and attitude disturbance torque is established; a PID control law based on position / velocity error is designed in the outer loop, the desired acceleration is generated by combining gain gradual entry and integral anti-windup; an attitude controller is designed to output three-axis torque, and disturbance observation compensation is introduced to improve the anti-disturbance performance; comparative simulation is carried out under different sea state levels, and three-dimensional tracking error and control input are output to verify the control effect.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) flight control and trajectory tracking control technology, and in particular to a three-dimensional trajectory tracking control method for quadrotor UAVs facing sea surface wind and wave disturbances. The method can be used for track maintenance and formation flight control in missions such as maritime inspection, search and rescue, surveying, and marine environmental monitoring. Background Technology

[0002] With the rapid development of the marine economy and marine engineering, the demand for unmanned and intelligent equipment is constantly increasing for tasks such as maritime inspection, maritime search and rescue, marine resource monitoring, and patrol of offshore platforms and waterways. Quadcopter UAVs have advantages such as vertical takeoff and landing, high maneuverability, flexible deployment, and relatively low cost. They can carry payloads such as optoelectronic, radar, and communication relay, and can replace manual labor in complex maritime scenarios to perform tasks such as near-shore / ocean target inspection, emergency search and information collection, and have broad application prospects.

[0003] In the process of quadcopter UAVs performing maritime missions, three-dimensional trajectory tracking and attitude stabilization control are key technologies for achieving mission reliability. Compared with the land environment, the sea surface is often subject to multiple external disturbances such as gusts, turbulence, and wave-induced airflow fluctuations, and these disturbances exhibit time-varying and random characteristics. At the same time, the quadcopter system itself has characteristics such as strong coupling, underactuation, and nonlinearity, and is subject to physical constraints such as thrust / torque saturation, maximum tilt angle, and speed limit. Under these conditions, the UAV needs to achieve high-precision trajectory tracking while ensuring a safe envelope, especially at the end of the mission (such as point deployment, close-range target observation, and landing on a maritime platform), where centimeter-level or sub-decimeter-level position errors are often required. This places higher demands on the robustness and feasibility of the control method.

[0004] Commonly used methods include cascaded control based on PID / PD, feedforward-feedback combined control, sliding mode control, and model predictive control. Traditional single-loop or weakly coupled control methods are prone to problems such as difficulty in reducing tracking errors, significant terminal residuals, and steady-state deviations due to control saturation when faced with strong wind and wave disturbances or high dynamic characteristics of the reference trajectory. Furthermore, control strategies that do not fully consider the influence of yaw angle on horizontal acceleration and attitude mapping may introduce additional coupling errors under heading changes or formation flight conditions. While robust controls such as sliding mode control possess a certain degree of disturbance rejection capability, they are prone to chattering and affecting tracking smoothness if disturbance estimation compensation and reasonable boundary layer design are lacking. Although model predictive control can explicitly handle constraints, its implementation complexity and online computational overhead are high, hindering rapid engineering deployment. Meanwhile, marine environmental disturbances exhibit phased changes; without disturbance management and error assessment alignment mechanisms for terminal accuracy acceptance, a "threshold-bound" phenomenon in terminal errors may occur, affecting the stable achievement of high-precision indicators. Therefore, this invention proposes a dual-loop robust trajectory tracking control method for quadrotor UAVs that addresses the combined disturbances of wind and waves on the sea surface, takes into account feasibility constraints, and possesses high-precision terminal convergence capability, in order to improve the trajectory tracking accuracy and stability in complex marine environments. Summary of the Invention

[0005] The purpose of this invention is to address the problems existing in the three-dimensional trajectory tracking of quadrotor UAVs under sea surface wind and wave disturbances, such as strong external disturbances, significant system nonlinearity and coupling, limited feasibility due to thrust / torque saturation and tilt angle constraints, difficulty in eliminating terminal error residuals, and easy oscillation or slow convergence under strong disturbances. This invention proposes a three-dimensional trajectory tracking control method for quadrotor UAVs under sea surface wind and wave disturbances, so as to achieve stable flight and high-precision trajectory tracking of UAVs under different sea conditions.

[0006] This invention is achieved through the following technical solution: This invention proposes a three-dimensional trajectory tracking and control method for quadcopter unmanned aerial vehicles (UAVs) facing sea surface wind and wave disturbances. The method includes the following steps:

[0007] Step 1: Establish a quadcopter UAV simulation environment, define the UAV state vector, control vector and external disturbance vector, and set thrust / torque saturation constraints, maximum tilt angle constraints and speed limit constraints;

[0008] Step 2: Construct a 3D reference trajectory generator to generate a reference state and amplify the reference angular velocity to suppress the initial transient.

[0009] Step 3: Construct a comprehensive disturbance model of sea surface wind and waves to generate six-dimensional disturbances;

[0010] Step 4: The outer loop generates the desired inertial frame acceleration command based on the "reference-actual" position / velocity error, and performs anti-saturation processing on the integral term and performs gradual scheduling on the outer loop gain;

[0011] Step 5: Map the desired inertial frame acceleration command to the desired roll angle, pitch angle and thrust command under the given desired yaw angle, and apply the maximum roll angle constraint and amplitude limit;

[0012] Step 6: The inner loop outputs a three-axis torque command based on the desired attitude and the current attitude / angular velocity. The inner loop is a sliding mode controller or an attitude PD controller.

[0013] Step 7: Estimate the torque disturbance based on the disturbance observer and compensate for the triaxial torque command;

[0014] Step 8: Record the reference and pre-update states at the same simulation time, calculate the time alignment error, output the 3D position RMSE, terminal error and quantile index, and compare and verify the control effect under different sea state levels.

[0015] Furthermore, in step 1, a six-degree-of-freedom dynamic model of the quadcopter UAV is established, and the fourth-order Runge-Kutta method is used to discretize and update the UAV state; the state vector, control vector, and external disturbance vector are defined as follows:

[0016]

[0017]

[0018] in Position of the inertial frame, Euler angles, For the velocity in the inertial frame, The angular velocity of the machine system; For thrust, For three-axis control torque; External force disturbance This is an external torque disturbance.

[0019] Furthermore, in step 1, constraints are set, including thrust / torque saturation constraints:

[0020]

[0021] And the maximum tilt angle constraint:

[0022]

[0023] And set a speed limit constraint:

[0024] .

[0025] Furthermore, step 2 specifically includes:

[0026] Define the reference state as

[0027]

[0028] Set a horizontal circular trajectory and achieve multi-machine formation by offsetting the center of the circle; for the first Set up the center of the drone ,radius Target angular velocity ,generate:

[0029]

[0030]

[0031] Design an angular velocity asymptotic strategy to reduce initial transients during the asymptotic time. Inside

[0032]

[0033] exist hour

[0034]

[0035] Set the desired yaw angle along the tangential direction:

[0036] .

[0037] Furthermore, step 3 specifically includes:

[0038] Construct wave-induced vertical perturbations:

[0039]

[0040] Construct horizontal disturbances of gusts:

[0041]

[0042]

[0043] in It is a random variable with zero mean;

[0044] Construct attitude disturbance moment:

[0045]

[0046] and apply limit;

[0047] The disturbance amplitude is scaled according to the sea state level.

[0048] Furthermore, step 4 specifically includes:

[0049] Extract current position and velocity Reference position and velocity The error is defined as

[0050]

[0051] Constructing the integral error and applying anti-saturation measures:

[0052]

[0053] Construct the feedforward acceleration from the reference velocity difference:

[0054]

[0055] Set the outer loop gain asymptotic coefficient to reduce initial stage oscillations and improve terminal accuracy:

[0056]

[0057] Command to generate desired inertial frame acceleration:

[0058] .

[0059] Furthermore, step 5 specifically includes:

[0060] Based on the maximum tilt angle Calculate the upper limit of horizontal acceleration:

[0061]

[0062] And on Limiting the amplitude:

[0063] right Apply preset upper and lower limits to suppress high oscillations;

[0064] Synthetic total acceleration vector

[0065]

[0066] And calculate the thrust command:

[0067]

[0068] At the desired yaw angle The following yaw-sensing attitude analysis mapping is performed to obtain the desired roll and pitch angles:

[0069]

[0070]

[0071] right Perform symmetrical amplitude limiting and let :

[0072]

[0073] Simultaneously construct the desired yaw rate:

[0074] .

[0075] Furthermore, step 6 specifically includes:

[0076] Construction attitude error and angular velocity error:

[0077]

[0078]

[0079] When attitude PD control is used, the torque command is:

[0080]

[0081] And apply saturation constraints ;

[0082] When using sliding mode attitude control, construct the sliding surface:

[0083]

[0084] Construct equivalent terms:

[0085]

[0086] Constructing a toggle item:

[0087]

[0088] Output torque:

[0089]

[0090] and apply limit.

[0091] Furthermore, step 7 specifically includes:

[0092] The measured angular acceleration is obtained from the difference:

[0093]

[0094] Predicting angular acceleration from the equivalent term:

[0095]

[0096] Update torque disturbance estimate and limit:

[0097] .

[0098] Furthermore, step 8 specifically includes:

[0099] Record every moment Reference Compared to the previous state The time alignment error is calculated, and the position error is:

[0100]

[0101] Calculate the RMSE of the 3D position:

[0102]

[0103] And statistical analysis of terminal error and quantile index;

[0104] Repeat steps 3-8 under different sea state levels to output error curves, thrust input curves, and velocity profile curves, thereby achieving robustness comparison and verification.

[0105] The beneficial effects of this invention are:

[0106] This invention proposes a three-dimensional trajectory tracking control method for a quadrotor UAV accommodating sea surface wind and wave disturbances. First, a six-DOF dynamic simulation model of the quadrotor UAV is established, and the UAV's state is discretized and updated using the fourth-order Runge-Kutta method. Second, a three-dimensional reference trajectory generation mechanism is constructed, including a horizontal circular trajectory and an angular velocity asymptotic strategy, and multi-UAV formation reference is achieved through center offset. Then, a six-dimensional comprehensive disturbance model of the complex sea surface environment is established, introducing wave-induced vertical periodic force, gust horizontal force, and attitude disturbance torque. Based on this, the outer ring is adjusted according to position and... The velocity error is designed using a control law with proportional-derivative-integral terms. The desired inertial frame acceleration is generated by combining gain asymptotic and integral anti-saturation strategies. Then, based on the desired yaw angle, an acceleration-attitude analytical mapping is performed to obtain the desired roll angle, pitch angle, and thrust commands, and tilt angle and realizability limits are applied. The inner loop uses attitude sliding mode control or attitude PD control to output three-axis torque, and disturbance observation compensation is introduced to improve disturbance rejection robustness. Finally, comparative simulations are performed under different sea state levels, outputting three-dimensional tracking error, thrust, and velocity profiles to verify the control effect. This invention, through a dual-loop structure, yaw perception mapping, and disturbance observation compensation, improves trajectory tracking accuracy and stability under wind and wave disturbances. It features strong robustness and ease of engineering implementation, making it suitable for high-precision trajectory tracking tasks of UAVs in marine environments. Attached Figure Description

[0107] 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 only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0108] Figure 1 This is a flowchart of the three-dimensional trajectory tracking control method for a quadcopter UAV facing sea surface wind and wave disturbances as described in this invention.

[0109] Figure 2 This is a schematic diagram of the flight trajectory error of the UAV under operating condition A of the present invention.

[0110] Figure 3 This is a schematic diagram of the flight trajectory error of the UAV under working condition B of the present invention.

[0111] Figure 4 This is a schematic diagram of the flight trajectory error of the UAV under operating condition C of the present invention. Detailed Implementation

[0112] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0113] This invention proposes a three-dimensional trajectory tracking control method for quadrotor unmanned aerial vehicles (UAVs) facing sea surface wind and wave disturbances. The method includes: establishing a six-degree-of-freedom dynamic simulation model of the quadrotor and updating the state using the fourth-order Runge-Kutta method; constructing a multi-UAV three-dimensional reference trajectory generation mechanism; establishing a six-dimensional comprehensive disturbance model including wave-induced vertical force, gust horizontal force, and attitude disturbance torque; designing an outer-loop PID control law based on position / velocity error, combining gain asymptotic and integral anti-saturation to generate the desired acceleration; designing an inner-loop attitude controller to output three-axis torque, introducing disturbance observation compensation to improve disturbance rejection performance; and conducting comparative simulations under different sea state levels, outputting three-dimensional tracking error and control input to verify the control effect.

[0114] Combination Figures 1-4 This invention proposes a three-dimensional trajectory tracking and control method for quadcopter unmanned aerial vehicles (UAVs) facing sea surface wind and wave disturbances. The method includes the following steps:

[0115] Step 1: Establish a quadcopter UAV simulation environment, define the UAV state vector, control vector and external disturbance vector, and set thrust / torque saturation constraints, maximum tilt angle constraints and speed limit constraints;

[0116] Step 2: Construct a 3D reference trajectory generator to generate a reference state and amplify the reference angular velocity to suppress the initial transient.

[0117] Step 3: Construct a comprehensive disturbance model of sea surface wind and waves to generate six-dimensional disturbances;

[0118] Step 4: The outer loop generates the desired inertial frame acceleration command based on the "reference-actual" position / velocity error, and performs anti-saturation processing on the integral term and performs gradual scheduling on the outer loop gain;

[0119] Step 5: Map the desired inertial frame acceleration command to the desired roll angle, pitch angle and thrust command under the given desired yaw angle, and apply the maximum tilt angle constraint and the feasibility limit.

[0120] Step 6: The inner loop outputs a three-axis torque command based on the desired attitude and the current attitude / angular velocity. The inner loop is a sliding mode controller or an attitude PD controller.

[0121] Step 7: Estimate the torque disturbance based on the disturbance observer and compensate for the triaxial torque command;

[0122] Step 8: Record the reference and pre-update states at the same simulation time, calculate the time alignment error, output the 3D position RMSE, terminal error and quantile index, and compare and verify the control effect under different sea state levels.

[0123] Furthermore, in step 1, a six-degree-of-freedom dynamic model of the quadcopter UAV is established, and the fourth-order Runge-Kutta method is used to discretize and update the UAV state; the state vector, control vector, and external disturbance vector are defined as follows:

[0124]

[0125]

[0126] in Position of the inertial frame, Euler angles are the roll / pitch / yaw angles. For the velocity in the inertial frame, The angular velocity of the machine system; For thrust, For three-axis control torque; External force disturbance This is an external torque disturbance.

[0127] Furthermore, in step 1, translational dynamics and rotational dynamics are defined: the thrust along the machine system... In the axial direction, let the rotation matrix (from the machine frame to the inertial frame) be...

[0128]

[0129] in The rotation matrix from the machine system to the inertial system. These are roll, pitch, and yaw angles, respectively.

[0130] The translation equation is:

[0131]

[0132] in For quality, It is the acceleration due to gravity. For resistance, Other external force components in the inertial frame.

[0133] The equation of rotation is

[0134]

[0135] Among them, the angular velocity of the machine system Matrix of inertia Control torque Disturbance torque ;

[0136] Furthermore, in step 1, the fourth-order Runge-Kutta method is used for state discretization and updating, assuming the continuous dynamics are... ,in For state, control, and disturbance, then

[0137]

[0138] in

[0139]

[0140]

[0141] For in ( f is calculated at point ) In order to be in f is calculated at the point. In order to be in f is calculated at the point. In order to be in f is calculated at the point. Controls and disturbances used in step k;

[0142] Furthermore, in step 1, constraints are set, including thrust / torque saturation constraints:

[0143]

[0144] And the maximum tilt angle constraint:

[0145]

[0146] And set a speed limit constraint:

[0147] .

[0148] in, Indicates the maximum tilt angle. Indicates the maximum speed. This is the upper limit of thrust. This represents the upper limit of the torque amplitude for each axis;

[0149] Furthermore, step 2 specifically includes:

[0150] Define the reference state as

[0151]

[0152] in, As a reference state vector (desired / given trajectory), The reference position is in three-axis coordinates in an inertial frame. For reference yaw angle, The reference velocity has three axial components in the inertial frame;

[0153] Set a horizontal circular trajectory and achieve multi-machine formation by offsetting the center of the circle; for the first Set up the center of the drone ,radius Target angular velocity ,generate:

[0154]

[0155]

[0156] Design an angular velocity asymptotic strategy to reduce initial transients during the asymptotic time. Inside

[0157]

[0158] exist hour

[0159]

[0160] Set the desired yaw angle along the tangential direction:

[0161] .

[0162] in Let ω be the asymptotic time of angular velocity. The instantaneous angular velocity during the asymptotic phase. The phase angle is a function of time;

[0163] Furthermore, step 3 specifically includes:

[0164] Define a six-dimensional perturbation vector:

[0165]

[0166] in Let x be the components of the equivalent external force disturbance in the x, y, z directions of the inertial frame. The components of the equivalent disturbance torque along the x, y, and z axes of the machine system;

[0167] Construct wave-induced vertical disturbance (low-frequency sine wave):

[0168]

[0169] in The amplitude of the vertical force disturbance. The frequency of the vertical disturbance caused by the wave. Let be the phase offset of the i-th UAV;

[0170] Constructing horizontal gust disturbances (mid-frequency sinusoidal superimposed random noise):

[0171]

[0172]

[0173] in It is a random variable with zero mean; The amplitude of the equivalent external force for horizontal gusts. The angular frequency of the gust disturbance.

[0174] This is the phase scaling factor, used to adjust the phase of the y-axis perturbation. Relationship, Noise intensity / standard deviation coefficient;

[0175] Construct attitude disturbance moment:

[0176]

[0177] and apply limit;

[0178] in As a reference for the amplitude of the disturbance torque, Let be the angular frequency of the triaxial disturbance torque. and This is the phase scaling factor;

[0179] The disturbance amplitude is scaled according to the sea state level.

[0180] Furthermore, step 4 specifically includes:

[0181] Extract current position and velocity Reference position and velocity The error is defined as

[0182]

[0183] in As the reference position vector, As the reference velocity vector, This is the position error vector. This is the velocity error vector;

[0184] Constructing the integral error and applying anti-saturation measures:

[0185]

[0186] in Let be the integral error vector. To control / distance walking length, For points update, This is the integral limiting threshold. To prevent saturation limiting;

[0187] Construct the feedforward acceleration from the reference velocity difference:

[0188]

[0189] in The feedforward acceleration vector, This is the reference speed for the k-th step; This is the reference speed for the (k-1)th step;

[0190] Set the outer loop gain asymptotic coefficient to reduce initial stage oscillations and improve terminal accuracy:

[0191]

[0192] in The outer loop gain asymptotic coefficient. The gain asymptotic time constant;

[0193] Generate the desired inertial frame acceleration command (including feedforward):

[0194] .

[0195] in The desired inertial frame acceleration command output by the outer loop. For the outer loop proportional, differential, and integral gain, This is an element-wise (Hadamard) product;

[0196] Furthermore, step 5 specifically includes:

[0197] Based on the maximum tilt angle The upper limit of acceleration that can be achieved by calculating the horizontal plane:

[0198]

[0199] And on Limiting the amplitude:

[0200] And on Preset upper and lower limits are applied to suppress high oscillations.

[0201] in For the maximum tilt angle, g is the gravitational acceleration. The upper limit of acceleration amplitude can be achieved on the horizontal plane (xy). The three-axis components of the desired inertial frame acceleration command given by the outer loop, and clip is the clipping function;

[0202] Synthetic total acceleration vector

[0203]

[0204] And calculate the thrust command:

[0205]

[0206] in To synthesize the total acceleration vector "after gravity compensation", where m is the mass of the spacecraft, For the desired thrust command;

[0207] At the desired yaw angle The following yaw-sensing attitude analysis mapping is performed to obtain the desired roll and pitch angles:

[0208]

[0209]

[0210] in For reference / desired yaw angle, For the desired roll angle, Let arctan2() be the desired pitch angle, and arctan2() be the arctangent function in the four quadrants.

[0211] right Perform symmetrical amplitude limiting and let :

[0212]

[0213] Simultaneously construct the desired yaw rate (for inner loop tracking):

[0214] .

[0215] in For the desired yaw rate, The proportional gain of the yaw rate. This is the angle error normalization function;

[0216] Furthermore, step 6 specifically includes:

[0217] Construction attitude error and angular velocity error:

[0218]

[0219]

[0220] in ; Let be the attitude error vector. , , Let `wrap` be the desired roll, pitch, and yaw angles, and `wrap` be the angle normalization function. This is the angular velocity error vector. The desired angular velocity vector, Let w be the desired yaw rate, w be the current angular velocity of the aircraft system, and (p,q,r) be the angular velocity components of the aircraft system about the x, y, and z axes.

[0221] When attitude PD control is used, the torque command is:

[0222]

[0223] And apply saturation constraints ;

[0224] in This is the torque command vector. , For attitude scaling and differential gain vectors, For element-wise (Hadamard) product, This represents the upper limit of the torque amplitude for each axis;

[0225] When using sliding mode attitude control, construct the sliding surface:

[0226]

[0227] Construct equivalent terms (attitude PD equivalent terms):

[0228]

[0229] Constructing a switching term (using boundary layer to suppress chattering):

[0230]

[0231] Output torque:

[0232]

[0233] in Estimate the torque disturbance; and apply limit. This is the derivative of the attitude error with respect to time. Let be the attitude error vectors at step k and step (k-1), s be the sliding surface vector, and C be the coefficient matrix or vector of the error derivative terms in the sliding surface. This is an equivalent control item. is the switching term used to drive torque on the sliding surface, with a boundary layer to suppress chattering. K is the switching term gain.

[0234] Furthermore, step 7 specifically includes:

[0235] The measured angular acceleration is obtained from the difference:

[0236]

[0237] in The measured and estimated values ​​of the angular velocity at step k and step (k-1) are given. Sampling period / control step size The measured angular acceleration is obtained from the difference.

[0238] Predicting angular acceleration from the equivalent term:

[0239]

[0240] in ; The angular acceleration predicted by the equivalent control term, For the equivalent moment term, Let x, y, and z be the principal moments of inertia of the orbital system.

[0241] Update torque disturbance estimate and limit:

[0242] .

[0243] in Let be the estimated vector of the disturbance torque at step k. For perturbation observer gain, For angular acceleration residuals, This is the upper limit of the amplitude of the estimated disturbance moment.

[0244] Furthermore, step 8 specifically includes:

[0245] Record every moment Reference Compared to the previous state The time alignment error is calculated, and the position error is:

[0246]

[0247] in For the k-th discrete sampling time, Let k be the reference position vector. Let k be the position error scalar;

[0248] Calculate the RMSE of the 3D position:

[0249]

[0250] And statistical analysis of terminal error and quantile index; among which To evaluate the number of sample points within an interval, RMSE is the root mean square error of the three-dimensional location.

[0251] Repeat steps 3-8 under different sea state levels to output the error curve, thrust input curve, and velocity profile curve, achieving robustness comparison and verification. N represents the number of sample points (dimensionless) within the evaluation interval, such as the total number of simulation steps or the number of discrete points within the selected window.

[0252] Simulation verification:

[0253] Figures 2-4 This is a simulation verification of the method of the present invention. In the simulation, the quadcopter UAV dynamics adopt a six-degree-of-freedom model, and the state vector is x= The control input is The simulation step size Δt = 0.01s, the total simulation duration T = 120s, and the number of UAVs is 2. The reference trajectory adopts a horizontal circular trajectory with an angular velocity asymptotic strategy. The radius of the circular trajectory is R = 30m, the target angular velocity ω = 1 / 3 rad / s, and the angular velocity asymptotic time is... Reference height It also achieves multi-drone formation reference by setting different center offsets; at the same time, it sets an upper limit constraint on the speed of the drones. To verify adaptability to complex sea environments, three sea state levels A / B / C were set: A represents the undisturbed baseline condition; B and C represent moderate / strong wind and wave conditions, with a six-dimensional comprehensive disturbance introduced. (Including wave-induced vertical periodic force, gust horizontal force, and attitude disturbance torque), and the disturbance amplitude is smoothly attenuated at the end of the simulation to verify the high-precision tracking performance at the end; under different sea states, parameters are configured through outer loop gain scaling factor, integral gain, and maximum tilt angle constraint to improve disturbance robustness and feasibility. Figure 2 As shown, under operating condition A, with external disturbances set to zero, the system primarily reflects the tracking performance of the controller itself. The "3D position tracking error of each UAV" curves reveal that the two UAVs exhibit a certain initial deviation in the early stages of the simulation. The position error rapidly decays and converges within a short time; after approximately ten seconds, the error stabilizes below the 0.05 m threshold. The enlarged view at the end (approximately 90–120 s) shows only minor fluctuations in error with no significant drift, indicating that the dual-loop control strategy exhibits rapid convergence, high steady-state accuracy (≤0.05 m), and good stability under disturbance-free conditions. Figure 3 As shown, under condition B, the 3D position tracking errors of the two UAVs exhibit significant transient deviations at the beginning of the simulation, but both decay rapidly and converge within a short time. After entering steady state, the position errors of the two UAVs remain at a very low level, consistently below the 0.05 m threshold, with only minor fluctuations. From the magnified window at the end (approximately 110–120 s), it can be seen that the UAVs… 1. The steady-state error is approximately on the order of 0.01–0.02 m for UAVs. 2. The steady-state error is slightly large and fluctuates slightly, but remains within the range of approximately 0.02–0.04 m. Meanwhile, the velocity error (dashed line) also decays rapidly in the initial stage and approaches zero, indicating that the control system can effectively suppress external influences and achieve high-precision and stable tracking performance under this sea state disturbance. Figure 4 As shown, compared with operating condition B, operating condition C experiences stronger external disturbances, resulting in more pronounced residual fluctuations and a more "viscous" convergence tail after the system reaches a steady state, but the overall accuracy still meets the 0.05 m accuracy target. It is noteworthy that the initial transient amplitudes of the two aircraft differ under operating condition C: UAV-2 has a larger initial peak value and a steeper convergence process, while UAV-1's peak value is relatively smaller but is also suppressed within a short time. This indicates that under strong wind and wave conditions, different platforms / initial states have different sensitivities to disturbances, but this control strategy provides consistent within-threshold accuracy for both aircraft after convergence. Compared with conventional trajectory tracking schemes that do not fully consider realizability constraints, yaw coupling mapping, and disturbance compensation, this invention has significant advantages in error convergence stability and terminal accuracy maintenance under wind and wave disturbances, making it suitable for applications requiring high-precision flight control, such as maritime patrol, search and rescue, and marine monitoring.

[0254] This invention proposes a three-dimensional trajectory tracking control method for a quadrotor UAV accommodating sea surface wind and wave disturbances. First, a six-DOF dynamic simulation model of the quadrotor UAV is established, and the UAV's state is discretized and updated using the fourth-order Runge-Kutta method. Second, a three-dimensional reference trajectory generation mechanism is constructed, including a horizontal circular trajectory and an angular velocity asymptotic strategy, and multi-UAV formation reference is achieved through center offset. Then, a six-dimensional comprehensive disturbance model of the complex sea surface environment is established, introducing wave-induced vertical periodic force, gust horizontal force, and attitude disturbance torque. Based on this, the outer ring is adjusted according to position and... The velocity error is designed using a control law with proportional-derivative-integral terms. The desired inertial frame acceleration is generated by combining gain asymptotic and integral anti-saturation strategies. Then, based on the desired yaw angle, an acceleration-attitude analytical mapping is performed to obtain the desired roll angle, pitch angle, and thrust commands, and tilt angle and realizability limits are applied. The inner loop uses attitude sliding mode control or attitude PD control to output three-axis torque, and disturbance observation compensation is introduced to improve disturbance rejection robustness. Finally, comparative simulations are performed under different sea state levels, outputting three-dimensional tracking error, thrust, and velocity profiles to verify the control effect. This invention, through a dual-loop structure, yaw perception mapping, and disturbance observation compensation, improves trajectory tracking accuracy and stability under wind and wave disturbances. It features strong robustness and ease of engineering implementation, making it suitable for high-precision trajectory tracking tasks of UAVs in marine environments.

[0255] The above provides a detailed description of the three-dimensional trajectory tracking control method for quadcopter UAVs facing sea surface wind and wave disturbances proposed in this invention. Specific examples have been used to illustrate the principles and implementation methods of this invention. The descriptions of the above embodiments are only for the purpose of helping to understand the method and core ideas of this invention. At the same time, for those skilled in the art, there will be changes in the specific implementation methods and application scope based on the ideas of this invention. Therefore, the content of this specification should not be construed as a limitation of this invention.

Claims

1. A method for three-dimensional trajectory tracking control of a quadrotor unmanned aerial vehicle in the presence of sea surface wind disturbance, characterized in that, The method includes the following steps: Step 1: Establish a quadcopter UAV simulation environment, define the UAV state vector, control vector and external disturbance vector, and set thrust / torque saturation constraints, maximum tilt angle constraints and speed limit constraints; Step 2: Construct a 3D reference trajectory generator to generate a reference state and amplify the reference angular velocity to suppress the initial transient. Step 3: Construct a comprehensive disturbance model of sea surface wind and waves to generate six-dimensional disturbances; Step 4: The outer loop generates the desired inertial frame acceleration command based on the "reference-actual" position / velocity error, and performs anti-saturation processing on the integral term and performs gradual scheduling on the outer loop gain; Step 5: Map the desired inertial frame acceleration command to the desired roll angle, pitch angle and thrust command under the given desired yaw angle, and apply the maximum roll angle constraint and amplitude limit; Step 6: The inner loop outputs a three-axis torque command based on the desired attitude and the current attitude / angular velocity. The inner loop is a sliding mode controller or an attitude PD controller. Step 7: Estimate the torque disturbance based on the disturbance observer and compensate for the triaxial torque command; Step 8: Record the reference and pre-update states at the same simulation time, calculate the time alignment error, output the 3D position RMSE, terminal error and quantile index, and compare and verify the control effect under different sea state levels.

2. The method according to claim 1, characterized in that, In step 1, a six-degree-of-freedom dynamic model of the quadrotor UAV is established, and the fourth-order Runge-Kutta method is used to discretize and update the UAV state; the state vector, control vector, and external disturbance vector are defined as follows: in Position of the inertial frame, Euler angles, For the velocity in the inertial frame, The angular velocity of the machine system; For thrust, For three-axis control torque; External force disturbance This is an external torque disturbance.

3. The method according to claim 2, characterized in that, In step 1, set the constraints, including thrust / torque saturation constraints: And the maximum tilt angle constraint: And set a speed limit constraint: ； in, Indicates the maximum tilt angle. Indicates the maximum speed. This is the upper limit of thrust. This represents the upper limit of the torque amplitude for each axis.

4. The method according to claim 1, characterized in that, Step 2 specifically involves: Define the reference state as in, As the reference state vector, The reference position is in three-axis coordinates in an inertial frame. For reference yaw angle, The reference velocity has three axial components in the inertial frame; Set a horizontal circular trajectory and achieve multi-machine formation by offsetting the center of the circle; for the first Set up the center of the drone ,radius Target angular velocity ,generate: Design an angular velocity asymptotic strategy to reduce initial transients during the asymptotic time. Inside exist hour Set the desired yaw angle along the tangential direction: ； in Let ω be the asymptotic time of angular velocity. The instantaneous angular velocity during the asymptotic phase. It is a function of the phase angle over time.

5. The method according to claim 1, characterized in that, Step 3 specifically includes: Construct wave-induced vertical perturbations: in The amplitude of the vertical force disturbance. The frequency of the vertical disturbance caused by the wave. Let i be the phase offset of the i-th UAV; Construct horizontal disturbances of gusts: in It is a random variable with zero mean; The amplitude of the equivalent external force for horizontal gusts. The angular frequency of the gust disturbance. This is the phase scaling factor, used to adjust the phase of the y-axis perturbation. Relationship, Noise intensity / standard deviation coefficient; Construct attitude disturbance moment: and apply Restrictions; among them As a reference for the amplitude of the disturbance torque, Let be the angular frequency of the triaxial disturbance torque. and This is the phase scaling factor; The disturbance amplitude is scaled according to the sea state level.

6. The method according to claim 1, characterized in that, Step 4 specifically includes: Extract current position and velocity Reference position and velocity The error is defined as in As the reference position vector, As the reference velocity vector, This is the position error vector. This is the velocity error vector; Constructing the integral error and applying anti-saturation measures: in Let be the integral error vector. To control / distance walking length, For points update, This is the integral limiting threshold. To prevent saturation limiting; Construct the feedforward acceleration from the reference velocity difference: in The feedforward acceleration vector, This is the reference speed for the k-th step; This is the reference speed for the (k-1)th step; Set the outer loop gain asymptotic coefficient to reduce initial stage oscillations and improve terminal accuracy: in The outer loop gain asymptotic coefficient. The gain asymptotic time constant; Command to generate desired inertial frame acceleration: ； in The desired inertial frame acceleration command output by the outer loop. For the outer loop proportional, differential, and integral gain, This is an element-wise product.

7. The method according to claim 1, characterized in that, Step 5 specifically includes: Based on the maximum tilt angle Calculate the upper limit of horizontal acceleration: And on Limiting the amplitude: right Apply preset upper and lower limits to suppress high oscillations; in For the maximum tilt angle, g is the acceleration due to gravity. This represents the upper limit of the xy acceleration amplitude on the horizontal plane. The three-axis components of the desired inertial frame acceleration command given by the outer loop, and clip is the clipping function; Synthetic total acceleration vector And calculate the thrust command: in To synthesize the total acceleration vector "after gravity compensation", where m is the mass of the spacecraft, For desired thrust command; At the desired yaw angle The following yaw-sensing attitude analysis mapping is performed to obtain the desired roll and pitch angles: in For reference yaw angle, For the desired roll angle, Let arctan2() be the desired pitch angle, and arctan2() be the arctangent function in the four quadrants. right Perform symmetrical amplitude limiting and let : Simultaneously construct the desired yaw rate: ； in For the desired yaw rate, This is the proportionality coefficient for yaw rate. This is the angle error normalization function.

8. The method according to claim 1, characterized in that, Step 6 specifically includes: Construction attitude error and angular velocity error: in ; Let be the attitude error vector. , , Let `wrap` be the desired roll, pitch, and yaw angles, and `wrap` be the angle normalization function. This is the angular velocity error vector. The desired angular velocity vector, Let w be the desired yaw rate, w be the current angular velocity of the aircraft system, and (p,q,r) be the angular velocity components of the aircraft system about the x, y, and z axes. When attitude PD control is used, the torque command is: And apply saturation constraints ;in This is the torque command vector. , For attitude scaling and differential gain vectors, For element-wise multiplication, This represents the upper limit of the torque amplitude for each axis; When using sliding mode attitude control, construct the sliding surface: Construct equivalent terms: Constructing a toggle item: Output torque: in Estimate the torque disturbance; and apply limit; This is the derivative of the attitude error with respect to time. Let be the attitude error vectors at step k and step (k-1), s be the sliding surface vector, and C be the coefficient matrix or vector of the error derivative terms in the sliding surface. Equivalent control item; is the switching term used to drive torque on the sliding surface, with a boundary layer to suppress chattering; K is the switching term gain.

9. The method according to claim 1, characterized in that, Step 7 specifically includes: The measured angular acceleration is obtained from the difference: in The measured and estimated values ​​of the angular velocity at step k and step (k-1) are given. Sampling period / control step size The measured angular acceleration is obtained from the difference. Predicting angular acceleration from the equivalent term: in ; The angular acceleration predicted by the equivalent control term, For equivalent moment terms, Let x, y, and z be the principal moments of inertia of the orbital system. Update torque disturbance estimate and limit: ； in Let be the estimated vector of the disturbance torque at step k. For perturbation observer gain, For angular acceleration residuals, This is the upper limit of the amplitude of the estimated disturbance moment.

10. The method according to claim 1, characterized in that, Step 8 specifically includes: Record every moment Reference Compared to the previous state The time alignment error is calculated, and the position error is: in For the k-th discrete sampling time, Let k be the reference position vector. Let k be the position error scalar; Calculate the RMSE of the 3D position: N represents the number of sample points within the evaluation interval; and the terminal error and quantile index are calculated. Repeat steps 3-8 under different sea state levels to output error curves, thrust input curves, and velocity profile curves, thereby achieving robustness comparison and verification.

Images