Quadrotor unmanned aerial vehicle wind-aware robust tracking and prediction safety filtering method

Abstract

The application provides a quad-rotor unmanned aerial vehicle wind perception robust tracking and prediction safety filtering method. The method belongs to the technical field of unmanned aerial vehicle flight control, data-driven model prediction control and safety constraint control. The method comprises the following steps: establishing a quad-rotor unmanned aerial vehicle dynamics model and safety constraints; constructing a time-space wind field model containing average wind, gust, turbulence, sea wave induced disturbance and distributed external cross wind wave; obtaining online disturbance information based on wind field query, disturbance observation and uncertainty estimation; training and calling a Gaussian process residual model to modify a nominal prediction model; after a wind perception model prediction controller outputs a candidate control input, the candidate control input is subjected to safety authentication and necessary modification through a risk window predictor, a distributed external monitor, a prediction safety filter, a reference governor and a backup recovery strategy, so that three-dimensional trajectory tracking and safety constraint protection in a complex wind environment are realized.

Description

Technical Field

[0001] This invention relates to the fields of UAV flight control, wind field disturbance modeling, data-driven model predictive control, and safety constraint control, and particularly to a wind-sensing robust tracking and predictive safety filtering method for quadrotor UAVs. The method can be used for low-altitude inspection, maritime inspection, emergency search and rescue, surveying, target tracking, formation flying, and UAV safety track maintenance in complex wind environments. Background Technology

[0002] With the rapid development of applications such as low-altitude economy, marine engineering, urban inspection and emergency rescue, quadcopter drones are widely used for trajectory tracking and information collection tasks in complex environments due to their advantages such as vertical take-off and landing, strong hovering ability, flexible deployment and low mission cost. In actual flight, drones typically need to fly along a given three-dimensional reference trajectory and maintain a small tracking error within limited thrust, speed, tilt angle and spatial safety boundaries.

[0003] However, wind fields near the ground, sea surface, and over complex terrain exhibit significant spatiotemporal variations, often manifesting as a superposition of multiple disturbances, including mean wind profiles, gusts, Dryden turbulence, Ochi-Shin sea surface turbulence, wave-induced airflow, and abrupt changes in cross-domain wind fields. These disturbances can cause quadcopter UAVs to experience problems such as velocity deviations, attitude coupling errors, control input saturation, and approaching safety boundaries. Especially under conditions of strong winds, turbulence, or distributed external wind fields, controllers relying solely on nominal dynamic models struggle to compensate for residual dynamics in a timely manner, and even learned controllers may generate unreliable control inputs due to training distribution shifts.

[0004] Existing trajectory tracking control methods for quadrotor UAVs mainly include PID / PD cascade control, robust control, sliding mode control, extended state observer compensation, model predictive control, and predictive control based on data-driven residual learning. Among these, PID / PD cascade control is easy to implement, but it typically relies on fixed gain and an approximately linearized model, making it difficult to compensate for time-varying external forces under strong gusts, turbulence, or wave-induced disturbances. While robust control and sliding mode control can enhance disturbance rejection capabilities, their design often requires pre-setting conservative uncertainty boundaries and may lead to problems such as control chattering, increased input amplitude, or complex parameter tuning. Extended state observer and other disturbance compensation methods can estimate some external disturbances, but when wind field spatial variations are significant and disturbance statistical characteristics change rapidly, single-observation compensation cannot simultaneously ensure both prediction accuracy and constraint feasibility. Model predictive control can explicitly consider input, velocity, and spatial boundary constraints during optimization, but its safety depends on the accuracy of the predictive model and the disturbance boundary. When model residuals, learning errors, or external wind fields exist, insufficient constraint margins or even hard constraint violations may still occur. Residual learning based on methods such as Gaussian processes can improve prediction and tracking performance under wind disturbances, but if it is only used as a performance controller without independent online safety authentication, reference governance, and backup recovery mechanisms, it is difficult to guarantee that candidate control inputs continuously meet position, velocity, input, and terminal safety constraints throughout the complex wind field process. Therefore, this invention proposes a quadrotor UAV control method that combines spatiotemporal wind field modeling, disturbance estimation, data-driven residual prediction, robust model predictive control, and predictive safety filtering, enabling the UAV to balance trajectory tracking performance and safety constraint satisfaction in gusts, turbulence, full-wave wind fields, and some external wind fields. Summary of the Invention

[0005] The purpose of this invention is to address the problems encountered by quadrotor UAVs in three-dimensional trajectory tracking under spatiotemporal wind disturbances, such as strong wind disturbances, significant model errors, risk of distribution offset in data-driven control, easy triggering of thrust / velocity / position constraints, and insufficient feasibility of safety filtering under strong wind conditions. This invention proposes a wind-sensing robust tracking and predictive safety filtering method for quadrotor UAVs, which enables high-precision trajectory tracking and online safety constraint protection of UAVs under structured wind field pressure.

[0006] This invention is achieved through the following technical solution: This invention proposes a wind-sensing robust tracking and prediction safety filtering method for quadcopter UAVs, the method comprising: Step 1: Establish a dynamic model and simulation / verification environment for the quadcopter UAV, and define the UAV's state vector, control vector, wind disturbance vector, as well as position, velocity, input, tilt angle, and terminal safety constraints; Step 2: Construct a spatiotemporal wind field environment model, generate mean wind, gusts, turbulence, sea surface wave-induced disturbances and distributed cross-wind and wave disturbances, and provide a wind field query interface; Step 3: Construct a wind-to-equivalent disturbance conversion module, a disturbance observer, a residual error buffer, and an uncertainty estimation module to generate disturbance estimation results containing the mean and boundary values; Step 4: Construct a residual dynamics dataset based on the UAV's historical state, control input, reference trajectory, and disturbance estimation, and train a Gaussian process residual prediction model; Step 5: Construct a wind-sensing data-driven model predictive controller, introduce disturbance estimation and Gaussian process residual prediction into the predictive model, and implement robust constraint tightening based on the uncertainty boundary; Step 6: Construct a risk window predictor and an out-of-distribution monitor, and determine whether to enable safety filtering based on constraint margin, wind field intensity, model uncertainty, and predicted risk. Step 7: Construct a predictive safety filter to perform safety authentication on candidate control inputs; if a candidate control input cannot be authenticated, solve for the corrected control input or backup control trajectory that satisfies the constraints. Step 8: Construct a reference governance and backup recovery mechanism to adjust the reference advancement speed, reduce the aggressiveness of tracking, and guide the system into the endpoint security set when security risks increase; Step 9: Execute closed-loop control and record trajectory error, control input, constraint margin, filter activation rate, backup feasibility rate, terminal safety set arrival rate, and number of hard constraint violations for comparative verification under different wind field scenarios.

[0007] Further, in step 1, the state vector, control vector, and wind disturbance vector of the quadcopter UAV are defined as follows: x=[p T ,v T ,η T ,ω T ] T u=[T,τ ,τθ,τψ] T dw=[fx,fy,fz,τx,τy,τz] T Where p = [x, y, z] T Let v be the position in the inertial frame of reference, where v = [vx, vy, vz]. T For the velocity of the inertial frame, η = [ ,θ,ψ] T For roll, pitch, and yaw, ω = [p, q, r] T τ is the angular velocity of the engine system; T is the total thrust; τ is the angular velocity of the engine system. τθ and τψ are the three-axis control torques; fx, fy, and fz are the equivalent wind disturbance torques; τx, τy, and τz are the equivalent wind disturbance torques. The safety constraints include position boundary constraints, velocity upper limit constraints, input constraints, thrust saturation constraints, torque saturation constraints, maximum tilt angle constraints, and terminal safety set constraints.

[0008] Furthermore, step 2 specifically includes: Step 2.1: Construct an average wind model and output the average wind speed based on flight altitude and environmental parameters; Step 2.2: Construct a gust model and generate local time-varying gusts based on gust amplitude, duration, and propagation direction; Step 2.3: Construct a turbulence model and generate time-dependent turbulence components based on the Dryden spectrum or spectral harmonic superposition method; Step 2.4: Construct a sea surface wave-induced disturbance model and generate wave-induced airflow components for flight over the water surface based on the intensity of sea surface wind and waves; Step 2.5: Construct an externally distributed cross-wind and wave scenario to simulate strong cross-winds, strong gusts, and wave coupling disturbances outside the training distribution; Step 2.6: Superimpose the mean wind, gusts, turbulence and wave-induced disturbances to form a spatiotemporal wind field W(p,t), and provide wind speed queries to the controller, observer and simulation platform through the unified interface get_wind(p,t).

[0009] Furthermore, step 3 specifically includes: Step 3.1: Based on the current position, speed, and attitude of the UAV, call the spatiotemporal wind field interface to obtain the wind speed at the current position and time; Step 3.2: Based on the relative wind speed and the force model of the UAV, convert the wind speed into equivalent perturbation force in the translational direction and equivalent perturbation moment in the attitude direction; Step 3.3: Construct a disturbance observer based on the state measurements, control input, and nominal dynamic model, and estimate the disturbance mean online. ; Step 3.4: Utilize the residual error cache to store recent prediction errors, and calculate the perturbation covariance Σd and the perturbation boundary. and quantile boundary q d ; Step 3.5, ,Σd, and q d The results are combined into a perturbation estimate and then passed to the model prediction controller and the prediction safety filter, respectively.

[0010] Furthermore, step 4 specifically includes: Step 4.1: Collect the state, control input, reference trajectory, wind field intensity, and disturbance estimation results during the closed-loop flight of the UAV; Step 4.2: Construct residual dynamic labels based on the difference between the state differences at adjacent time points and the difference between the nominal predicted state; Step 4.3: Using the state, control input, reference information, and disturbance estimation results as input samples, and the residual dynamics label as output samples, train the Gaussian process residual prediction model. Step 4.4: During the online control process, the Gaussian process residual prediction model outputs the residual mean and residual variance. The residual mean is used to correct the prediction model, and the residual variance is used for constraint tightening and safety risk assessment.

[0011] Furthermore, step 5 specifically includes: Step 5.1: In the model predictive control time domain, based on the nominal dynamic model, the mean of disturbance estimation and the mean of Gaussian process residuals are introduced to form a wind sensing prediction model. Step 5.2: Establish a rolling optimization cost function that includes position error, velocity error, terminal error, and control input amplitude; Step 5.3: Tighten the position, velocity, and input constraints based on the disturbance boundary, residual variance, and safety margin to obtain a robust constraint set; Step 5.4: Solve the wind sensing model predictive control problem in each control cycle and output candidate control inputs. .

[0012] Furthermore, step 6 specifically includes: Step 6.1: Calculate the position constraint margin, velocity constraint margin, input constraint margin, and terminal set margin based on the current state and predicted trajectory; Step 6.2: Construct a risk window indicator based on the trend of constraint margin changes over the next few steps; Step 6.3: Construct off-distribution monitoring indicators based on wind field intensity, residual variance, disturbance boundary, and statistical characteristics of training distribution; Step 6.4: When the risk window indicator or out-of-distribution monitoring indicator exceeds the preset threshold, the predictive security filter is triggered to enter the active authentication state.

[0013] Furthermore, step 7 specifically includes: Step 7.1: Predict candidate control inputs for the controller output using the wind sensing model. Input to be authenticated; Step 7.2: In the prediction time domain, check whether the predicted trajectory corresponding to the candidate control input satisfies the tightened set constraints of position, velocity, input, and terminal safety. Step 7.3: If the candidate control input passes security authentication, then directly execute u(k) = ; Step 7.4: If the candidate control input fails the safety certification, then solve for the corrected control input that has the smallest deviation from the candidate control input and satisfies the safety constraints. ; Step 7.5: If corrective control is not feasible, then execute a backup control strategy, which includes deceleration, braking, hovering, returning to home, or a recovery trajectory converging toward the terminal safe set.

[0014] Furthermore, step 8 specifically includes: Step 8.1: Construct a reference governing mechanism and adjust the reference trajectory advancement ratio based on safety margin and safety filtering feasibility; Step 8.2: When the safety margin is sufficient, increase the reference trajectory advancement ratio so that the UAV can track normally according to the original reference trajectory; Step 8.3: When the safety margin decreases or the out-of-distribution index increases, reduce the reference trajectory advance ratio to slow down, partially freeze, or turn the reference trajectory to safely restore the target. Step 8.4: Once the UAV enters the terminal safety set or safety filtering is restored to a feasible state, gradually restore the reference trajectory advancement ratio.

[0015] Furthermore, step 9 specifically includes: Step 9.1: Repeat the closed-loop control under different scenarios; Step 9.2: Record the reference state, actual state, candidate control input, final execution input, constraint margin, and safety filter state for each control cycle; Step 9.3: Calculate the 3D position RMSE, peak position error, number of hard constraint violations, filter activation rate, backup feasibility rate, task progress ratio, and terminal safety set arrival rate. Step 9.4: Compare the nominal model predictive control, disturbance sensing model predictive control, Gaussian process enhanced model predictive control, and Gaussian process enhanced model predictive control with predictive safety filtering to verify the tracking performance and safety protection effect of the method.

[0016] The beneficial effects of this invention are: This invention proposes a wind-sensing robust tracking and predictive safety filtering method for quadrotor UAVs. First, a quadrotor UAV dynamic model and a set of safety constraints are established. Second, multiple spatiotemporal wind fields are constructed, including mean wind, gusts, Dryden turbulence, Ochi-Shin sea surface turbulence, wave-induced airflow, and out-of-distribution cross-wind waves. Then, a disturbance estimate usable for control is formed through wind-to-disturbance conversion, disturbance observation, residual buffering, and uncertainty estimation. Based on this, a Gaussian process residual model is trained and combined with model predictive control to form wind-sensing data-driven MPC. Furthermore, online safety authentication and necessary corrections are performed on candidate control inputs through a risk window predictor, out-of-distribution monitor, predictive safety filter, reference governance unit, and backup recovery strategy. Finally, tracking and safety indicators are output under various structured wind field scenarios to verify the control effect.

[0017] Compared to traditional nominal control or data-driven control alone, this invention can maintain good trajectory tracking accuracy while providing online protection for position, velocity, input, and terminal safety constraints. Experimental results show that under structured wind fields such as full-wave, gust, and turbulence, Gaussian process-enhanced MPC can reduce position RMSE compared to nominal MPC. In similar operating conditions, the introduction of predictive safety filtering significantly reduces the number of hard constraint violations and improves the terminal safety set arrival rate. For operating conditions with strong out-of-distribution disturbances such as hard cross-waves, this invention reveals and mitigates the decline in safety feasibility through out-of-distribution monitoring and reference management, making it suitable as an engineering implementation scheme for UAV safety control in complex wind environments. Attached Figure Description

[0018] 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.

[0019] Figure 1 This is a flowchart of the wind-sensing robust tracking and prediction security filtering method for quadrotor UAVs oriented towards spatiotemporal wind field disturbances according to the present invention.

[0020] Figure 2 This is a schematic diagram of the spatiotemporal wind field modeling and wind field query interface of the present invention.

[0021] Figure 3 This is a schematic diagram of the predictive controller structure of the wind-sensing Gaussian process enhancement model of the present invention.

[0022] Figure 4This is a schematic diagram illustrating the collaborative operation of the predictive security filter, risk window predictor, and reference regulator of the present invention.

[0023] Figure 5 This is a schematic diagram comparing the trajectory tracking error and safety constraint indicators of the present invention under different wind field scenarios. Detailed Implementation

[0024] 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.

[0025] This invention proposes a wind-sensing robust tracking and prediction safety filtering method for quadrotor UAVs. The method includes: establishing a quadrotor UAV dynamic model and safety constraints; constructing a spatiotemporal wind field model including mean wind, gusts, turbulence, sea wave-induced disturbances, and out-of-distribution cross wind waves; obtaining online disturbance information based on wind field queries, disturbance observations, and uncertainty estimations; training and calling a Gaussian process residual model to correct the nominal prediction model; and after the wind-sensing model predictive controller outputs candidate control inputs, performing safety authentication and necessary corrections on the candidate control inputs through a risk window predictor, an out-of-distribution monitor, a prediction safety filter, a reference regulator, and a backup recovery strategy, thereby achieving three-dimensional trajectory tracking and safety constraint protection in complex wind environments.

[0026] Specifically, in combination Figures 1-5 This invention proposes a wind-sensing robust tracking and prediction safety filtering method for quadcopter UAVs, the method comprising: Step 1: Establish a dynamic model and simulation / verification environment for the quadcopter UAV, and define the UAV's state vector, control vector, wind disturbance vector, as well as position, velocity, input, tilt angle, and terminal safety constraints; Step 2: Construct a spatiotemporal wind field environment model, generate mean wind, gusts, turbulence, sea surface wave-induced disturbances and distributed cross-wind and wave disturbances, and provide a wind field query interface; Step 3: Construct a wind-to-equivalent disturbance conversion module, a disturbance observer, a residual error buffer, and an uncertainty estimation module to generate disturbance estimation results containing the mean and boundary values; Step 4: Construct a residual dynamics dataset based on the UAV's historical state, control input, reference trajectory, and disturbance estimation, and train a Gaussian process residual prediction model; Step 5: Construct a wind-sensing data-driven model predictive controller, introduce disturbance estimation and Gaussian process residual prediction into the predictive model, and implement robust constraint tightening based on the uncertainty boundary; Step 6: Construct a risk window predictor and an out-of-distribution monitor, and determine whether to enable safety filtering based on constraint margin, wind field intensity, model uncertainty, and predicted risk. Step 7: Construct a predictive safety filter to perform safety authentication on candidate control inputs; if a candidate control input cannot be authenticated, solve for the corrected control input or backup control trajectory that satisfies the constraints. Step 8: Construct a reference governance and backup recovery mechanism to adjust the reference advancement speed, reduce the aggressiveness of tracking, and guide the system into the endpoint security set when security risks increase; Step 9: Execute closed-loop control and record trajectory error, control input, constraint margin, filter activation rate, backup feasibility rate, terminal safety set arrival rate, and number of hard constraint violations for comparative verification under different wind field scenarios.

[0027] Further, in step 1, the state vector, control vector, and wind disturbance vector of the quadcopter UAV are defined as follows: x=[p T ,v T ,η T ,ω T ] T u=[T,τ ,τθ,τψ] T dw=[fx,fy,fz,τx,τy,τz] T Where p = [x, y, z] T Let v be the position in the inertial frame of reference, where v = [vx, vy, vz]. T For the velocity of the inertial frame, η = [ ,θ,ψ] T For roll, pitch, and yaw, ω = [p, q, r] T τ is the angular velocity of the engine system; T is the total thrust; τ is the angular velocity of the engine system. τθ and τψ are the three-axis control torques; fx, fy, and fz are the equivalent wind disturbance torques; τx, τy, and τz are the equivalent wind disturbance torques. The safety constraints include position boundary constraints, velocity upper limit constraints, input constraints, thrust saturation constraints, torque saturation constraints, maximum tilt angle constraints, and terminal safety set constraints. These constraints simultaneously serve as the safety boundaries for both the model predictive controller and the predictive safety filter. The thrust saturation constraints, torque saturation constraints, and maximum tilt angle constraints are as follows: 0≤T≤Tmax,|τ·|≤τmax | |≤θmax,|θ|≤θmax And set a speed limit constraint: ∥v∥≤vmax.

[0028] Furthermore, step 2 specifically includes: Step 2.1: Construct an average wind model and output the average wind speed based on flight altitude and environmental parameters; Step 2.2: Construct a gust model and generate local time-varying gusts based on gust amplitude, duration, and propagation direction; Step 2.3: Construct a turbulence model and generate time-dependent turbulence components based on the Dryden spectrum or spectral harmonic superposition method; Step 2.4: Construct a sea surface wave-induced disturbance model and generate wave-induced airflow components for flight over the water surface based on the intensity of sea surface wind and waves; Step 2.5: Construct an externally distributed cross-wind and wave scenario to simulate strong cross-winds, strong gusts, and wave coupling disturbances outside the training distribution; Step 2.6: Superimpose the mean wind, gusts, turbulence and wave-induced disturbances to form a spatiotemporal wind field W(p,t), and provide wind speed queries to the controller, observer and simulation platform through the unified interface get_wind(p,t).

[0029] Furthermore, step 3 specifically includes: Step 3.1: Based on the current position, speed, and attitude of the UAV, call the spatiotemporal wind field interface to obtain the wind speed at the current position and time; Step 3.2: Based on the relative wind speed and the force model of the UAV, convert the wind speed into equivalent perturbation force in the translational direction and equivalent perturbation moment in the attitude direction; Step 3.3: Construct a disturbance observer based on the state measurements, control input, and nominal dynamic model, and estimate the disturbance mean online. ; Step 3.4: Utilize the residual error cache to store recent prediction errors, and calculate the perturbation covariance Σd and the perturbation boundary. and quantile boundary q d ; Step 3.5, ,Σd, and q d The results are combined into a perturbation estimate and then passed to the model prediction controller and the prediction safety filter, respectively.

[0030] Furthermore, step 4 specifically includes: Step 4.1: Collect the state, control input, reference trajectory, wind field intensity, and disturbance estimation results during the closed-loop flight of the UAV; Step 4.2: Construct residual dynamic labels based on the difference between the state differences at adjacent time points and the difference between the nominal predicted state; Step 4.3: Using the state, control input, reference information, and disturbance estimation results as input samples, and the residual dynamics label as output samples, train the Gaussian process residual prediction model. Step 4.4: During the online control process, the Gaussian process residual prediction model outputs the residual mean and residual variance. The residual mean is used to correct the prediction model, and the residual variance is used for constraint tightening and safety risk assessment.

[0031] Furthermore, step 5 specifically includes: Step 5.1: In the model predictive control time domain, based on the nominal dynamic model, the mean of disturbance estimation and the mean of Gaussian process residuals are introduced to form a wind sensing prediction model. Step 5.2: Establish a rolling optimization cost function that includes position error, velocity error, terminal error, and control input amplitude; Step 5.3: Tighten the position, velocity, and input constraints based on the disturbance boundary, residual variance, and safety margin to obtain a robust constraint set; Step 5.4: Solve the wind sensing model predictive control problem in each control cycle and output candidate control inputs. .

[0032] Furthermore, step 6 specifically includes: Step 6.1: Calculate the position constraint margin, velocity constraint margin, input constraint margin, and terminal set margin based on the current state and predicted trajectory; Step 6.2: Construct a risk window indicator based on the trend of constraint margin changes over the next few steps; Step 6.3: Construct off-distribution monitoring indicators based on wind field intensity, residual variance, disturbance boundary, and statistical characteristics of training distribution; Step 6.4: When the risk window indicator or out-of-distribution monitoring indicator exceeds the preset threshold, the predictive security filter is triggered to enter the active authentication state.

[0033] Furthermore, step 7 specifically includes: Step 7.1: Predict candidate control inputs for the controller output using the wind sensing model. Input to be authenticated; Step 7.2: In the prediction time domain, check whether the predicted trajectory corresponding to the candidate control input satisfies the tightened set constraints of position, velocity, input, and terminal safety. Step 7.3: If the candidate control input passes security authentication, then directly execute u(k) = ; Step 7.4: If the candidate control input fails the safety certification, then solve for the corrected control input that has the smallest deviation from the candidate control input and satisfies the safety constraints. ; Step 7.5: If corrective control is not feasible, then execute a backup control strategy, which includes deceleration, braking, hovering, returning to home, or a recovery trajectory converging toward the terminal safe set.

[0034] Furthermore, step 8 specifically includes: Step 8.1: Construct a reference governing mechanism and adjust the reference trajectory advancement ratio based on safety margin and safety filtering feasibility; Step 8.2: When the safety margin is sufficient, increase the reference trajectory advancement ratio so that the UAV can track normally according to the original reference trajectory; Step 8.3: When the safety margin decreases or the out-of-distribution index increases, reduce the reference trajectory advance ratio to slow down, partially freeze, or turn the reference trajectory to safely restore the target. Step 8.4: Once the UAV enters the terminal safety set or safety filtering is restored to a feasible state, gradually restore the reference trajectory advancement ratio.

[0035] Furthermore, step 9 specifically includes: Step 9.1: Repeat the closed-loop control under different scenarios such as mean wind, gusts, turbulence, full-wave wind field, and distributed external cross wind waves; Step 9.2: Record the reference state, actual state, candidate control input, final execution input, constraint margin, and safety filter state for each control cycle; Step 9.3: Calculate the 3D position RMSE, peak position error, number of hard constraint violations, filter activation rate, backup feasibility rate, task progress ratio, and terminal safety set arrival rate. Step 9.4: Compare the nominal model predictive control, disturbance sensing model predictive control, Gaussian process enhanced model predictive control, and Gaussian process enhanced model predictive control with predictive safety filtering to verify the tracking performance and safety protection effect of the method.

[0036] Simulation verification: Figure 5 This simulation serves as verification of the method of the present invention. In the simulation, the quadcopter UAV adopts a six-degree-of-freedom model or a dynamic model compatible with an external multi-rotor simulation platform. The controller executes steps 1-9 cyclically according to a fixed sampling period. The reference trajectory adopts a three-dimensional position trajectory or a trackpoint sequence, and the wind field scenarios include full-wave, gust, and turbulence conditions. The comparison objects include nominal model predictive control and Gaussian process enhanced model predictive control with added predictive safety filtering.

[0037] In the tracking performance verification, the Gaussian process-enhanced model predictive control, by learning the nominal model residuals, can reduce position prediction errors and trajectory tracking errors under structured wind field disturbances. In the safety performance verification, the predictive safety filter performs online certification of candidate control inputs. When the predicted trajectory approaches the position, velocity, input, or terminal safety boundaries, the safety filter makes minor corrections to the candidate control inputs. When strong disturbances make correction infeasible, the reference regulator and backup recovery strategy reduce the reference propulsion speed and guide the UAV back to the safe set.

[0038] Simulation results show that the present invention can achieve both good trajectory tracking accuracy and high safety constraint satisfaction under gusts, turbulence, and full-wave wind fields. Compared with control schemes without online safety authentication, the present invention is more suitable for low-altitude inspection, maritime inspection, search and rescue, surveying, and formation flight missions that require both trajectory tracking accuracy and safety constraints under complex wind fields.

[0039] This invention enables quadcopter UAVs to achieve wind-sensing trajectory tracking and online safety protection in complex wind environments through the collaborative design of spatiotemporal wind field modeling, disturbance observation, Gaussian process residual prediction, robust model predictive control, risk window prediction, out-of-distribution monitoring, predictive safety filtering, reference management, and backup recovery. It features strong robustness, clear constraint handling, high modularity, and easy engineering expansion.

[0040] This invention proposes a robust tracking and predictive safety filtering method for quadrotor UAVs accommodating spatiotemporal wind field disturbances. It belongs to the technical fields of UAV flight control, data-driven model predictive control, and safety-constrained control. The method establishes a quadrotor UAV dynamic model and its position, velocity, input, tilt angle, and terminal safety constraints; constructs a spatiotemporal wind field model incorporating mean wind, gusts, turbulence, sea wave-induced disturbances, and out-of-distribution cross-wind waves, and provides real-time wind field queries through a unified interface; obtains online disturbance information based on wind-to-equivalent disturbance transformation, disturbance observation, residual error caching, and uncertainty estimation; trains a Gaussian process residual prediction model and introduces it into model predictive control to correct the nominal prediction model and implement robust constraint tightening based on uncertainty boundaries; constructs a risk window predictor and an out-of-distribution monitor to perform risk discrimination on candidate control inputs; performs online safety authentication of candidate control inputs through a predictive safety filter, and solves for corrected control inputs that satisfy constraints or calls up backup recovery trajectories when authentication is not possible; simultaneously, it utilizes a reference governance mechanism to reduce the reference propulsion speed under strong disturbance conditions and guide the UAV into the terminal safety set. This invention can balance trajectory tracking accuracy with position, speed, input and terminal safety constraints under complex spatiotemporal wind fields, and is suitable for UAV missions such as low-altitude inspection, maritime inspection, search and rescue, surveying and mapping and formation flight.

[0041] Example This invention proposes a wind-sensing robust tracking and prediction safety filtering method for quadcopter UAVs, the method comprising the following steps: Step 1: Establish a dynamic model and simulation / verification environment for the quadcopter UAV, and define the UAV's state vector, control vector, wind disturbance vector, as well as position, velocity, input, tilt angle, and terminal safety constraints; Step 2: Construct a spatiotemporal wind field environment model, generate mean wind, gusts, turbulence, sea surface wave-induced disturbances and distributed cross-wind and wave disturbances, and provide a wind field query interface; Step 3: Construct a wind-to-equivalent disturbance conversion module, a disturbance observer, a residual error buffer, and an uncertainty estimation module to generate disturbance estimation results containing the mean and boundary values; Step 4: Construct a residual dynamics dataset based on the UAV's historical state, control input, reference trajectory, and disturbance estimation, and train a Gaussian process residual prediction model; Step 5: Construct a wind-sensing data-driven model predictive controller, introduce disturbance estimation and Gaussian process residual prediction into the predictive model, and implement robust constraint tightening based on the uncertainty boundary; Step 6: Construct a risk window predictor and an out-of-distribution monitor, and determine whether to enable safety filtering based on constraint margin, wind field intensity, model uncertainty, and predicted risk. Step 7: Construct a predictive safety filter to perform safety authentication on candidate control inputs; if a candidate control input cannot be authenticated, solve for the corrected control input or backup control trajectory that satisfies the constraints. Step 8: Construct a reference governance and backup recovery mechanism to adjust the reference advancement speed, reduce the aggressiveness of tracking, and guide the system into the endpoint security set when security risks increase; Step 9: Execute closed-loop control and record trajectory error, control input, constraint margin, filter activation rate, backup feasibility rate, terminal safety set arrival rate, and number of hard constraint violations for comparative verification under different wind field scenarios.

[0042] Further, in step 1, the state vector of the quadcopter UAV is defined as follows: x=[p T ,v T ,η T ,ω T ] T Where p = [x, y, z] T Let v be the position in the inertial frame of reference, where v = [vx, vy, vz]. T For the velocity of the inertial frame, η = [ ,θ,ψ] T For roll, pitch, and yaw, ω = [p, q, r] T The system angular velocity; the control vector is defined as... u=[T,τ ,τθ,τψ] T Where T is the total thrust, and τ τθ and τψ are the three-axis control torques, respectively; the wind disturbance vector is defined as... dw=[fx,fy,fz,τx,τy,τz] T Where fx, fy, and fz are equivalent wind disturbance forces, and τx, τy, and τz are equivalent wind disturbance torques.

[0043] Further, in step 1, the safety constraints include position boundary constraints p∈X, velocity constraints ∥v∥≤vmax, input constraints u∈U, thrust constraints 0≤T≤Tmax, torque constraints |τ·|≤τmax, and maximum tilt angle constraints | The constraints |≤θmax, |θ|≤θmax, and the terminal security set constraint xN∈XS are used both for rolling optimization of model predictive control and for control authentication of predictive security filters.

[0044] Furthermore, step 2 specifically includes: Step 2.1: Construct an average wind model, using a height-dependent wind speed profile to describe the change of low-altitude wind speed with altitude, and output the average wind speed W mean(p,t) at a specified spatial location p and time t. Step 2.2: Construct a gust model and generate a local time-varying gust Wgust(p,t) using a subtracted cosine gust and wind speed-related duration mode. Step 2.3: Construct the Dryden turbulence model by generating a time-dependent turbulence component W dryden(p,t) through the superposition of colored noise or spectral harmonics. Step 2.4: Construct a proxy model for Ochi-Shin turbulence and wave-induced airflow over the sea surface to generate wave-induced wind disturbance W wave(p,t) during flight over the water surface; Step 2.5: Construct an externally distributed cross-wind and wave scenario to simulate strong cross-winds, strong gusts, and wave coupling disturbances outside the training distribution; Step 2.6: Superimpose the above wind field components to form a spatiotemporal wind field. W(p,t)=W mean(p,t)+W gust(p,t)+W dryden(p,t)+W wave(p,t) It provides real-time wind speed queries to the controller, observer, and external six-degree-of-freedom simulation platform through a unified interface get_wind(p,t).

[0045] Furthermore, step 3 specifically includes: Step 3.1: Based on the relative wind speed, UAV attitude, and drag model, convert the wind speed W(p,t) into the equivalent disturbance force in the UAV translation equation; Step 3.2: Estimate the mean of the external disturbance using the disturbance observer based on state measurements, control inputs, and the nominal dynamic model. ; Step 3.3: Utilize the residual error cache to store recent prediction errors, and calculate the perturbation covariance Σd and the perturbation boundary. and quantile boundary q d ; Step 3.4, ,Σd, and q d It is encapsulated as a disturbance estimation result for use by both the model predictive controller and the safety filter.

[0046] Furthermore, step 4 specifically includes: Step 4.1: Collect closed-loop or open-loop flight data to form a sample. ; Step 4.2: Calculate the residual dynamic labels based on the state difference between adjacent time steps and the nominal prediction results.

[0047] in, For the measured or estimated state at step k, For the control input at step k, For the k-th step perturbation estimation, For the nominal discrete dynamics model, This refers to the actual state obtained through measurement or simulation at the next moment. Labels for residual dynamics used to train residual models; Step 4.3: Use a Gaussian process model to learn the input. To residual The mapping yields the mean residual. and residual variance ; Step 4.4: Use the Gaussian process prediction results to compensate for the velocity or acceleration prediction errors in the model predictive control, and use the variance to tighten the robust constraints.

[0048] Furthermore, step 5 specifically includes: Step 5.1: Establish a wind sensing prediction model within the prediction time domain N.

[0049] ; Step 5.2: Construct the tracking cost function

[0050] Where N is the prediction time domain length, and Let be the reference position and reference velocity at step k+i, respectively. , , , This is the weight matrix.

[0051] Step 5.3, according to , and Tighten the position, velocity, and input constraints to form robust feasible constraints; Step 5.4: Solve the model predictive control problem in a rolling manner and output candidate control inputs. This serves as the control input to be certified for subsequent predictive security filters.

[0052] Furthermore, step 6 specifically includes: Step 6.1: Calculate the position margin, velocity margin, input margin, and terminal safety set margin based on the current state and the predicted trajectory; Step 6.2: Develop risk window indicators based on wind field intensity indices, residual uncertainty, forecast constraint margin, and recent violation trends. ; Step 6.3: Calculate the deviation of the current wind field and residual statistics relative to the training distribution using the off-distribution monitor. ; Step 6.4, when or When the preset threshold is exceeded, or when the candidate control input causes the predicted trajectory to approach the safety boundary, the safety filter will be switched to active authentication mode.

[0053] Furthermore, step 7 specifically includes: Step 7.1: Using candidate control inputs Given initial values, construct a security authentication optimization problem within the prediction time domain; Step 7.2: If there exists a control sequence that satisfies the tightened position, velocity, input, and terminal constraints in the predicted state, then let u(k) = The safety filter does not modify the candidate control input; Step 7.3: If the candidate control input is not authenticable, then solve for the... Minimum deviation correction control input

[0054] The predicted trajectory satisfies the safety constraints and reaches the terminal safety set, where Candidate control inputs for the model predicts the controller output. The correction control input is used to predict the output of the safety filter. To control the modified weight matrix, .

[0055] Step 7.4: If the modification and optimization are feasible, then execute. If the correction and optimization are not feasible, an alternative control strategy is executed, which includes deceleration, braking, hovering, returning to home, or a recovery trajectory that converges toward a safe set.

[0056] Furthermore, step 8 specifically includes: Step 8.1: Set up the reference governing unit and adjust the reference trajectory advancement ratio according to the constraint margin and the feasibility of safety filtering. ; Step 8.2: When the safety margin is sufficient, Approaching 1, the drone proceeds normally along the original reference trajectory; Step 8.3: When the predicted risk increases or the out-of-distribution index increases, reduce... This causes the reference trajectory to slow down or freeze locally, reducing the controller's approach to dangerous boundaries; Step 8.4: During the standby recovery phase, guide the reference point to the terminal security set or local security hover point until the security filtering problem is resolved and feasible.

[0057] Furthermore, step 9 specifically includes: Step 9.1: Repeat steps 1-8 under wind field scenarios such as mean wind, gust, turbulence, full-wave, and hard cross-wave; Step 9.2: Statistically measure trajectory tracking RMSE, peak position error, control input amplitude, filter activation rate, backup feasibility rate, task progress ratio, terminal safety set arrival rate, and number of hard constraint violations; Step 9.3: Compare the nominal MPC, disturbance-aware MPC, Gaussian process enhancement MPC, and Gaussian process enhancement MPC combined with a predictive security filter to verify the tracking improvement and security protection effects.

[0058] The wind-sensing robust tracking and prediction safety filtering method for quadcopter UAVs proposed in this invention has been described in detail above. 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 wind-sensing robust tracking and prediction safety filtering method for quadrotor UAVs, characterized in that, The method includes: Step 1: Establish a dynamic model and simulation / verification environment for the quadcopter UAV, and define the UAV's state vector, control vector, wind disturbance vector, as well as position, velocity, input, tilt angle, and terminal safety constraints; Step 2: Construct a spatiotemporal wind field environment model, generate mean wind, gusts, turbulence, sea surface wave-induced disturbances and distributed cross-wind and wave disturbances, and provide a wind field query interface; Step 3: Construct a wind-to-equivalent disturbance conversion module, a disturbance observer, a residual error buffer, and an uncertainty estimation module to generate disturbance estimation results containing the mean and boundary values; Step 4: Construct a residual dynamics dataset based on the UAV's historical state, control input, reference trajectory, and disturbance estimation, and train a Gaussian process residual prediction model; Step 5: Construct a wind-sensing data-driven model predictive controller, introduce disturbance estimation and Gaussian process residual prediction into the predictive model, and implement robust constraint tightening based on the uncertainty boundary; Step 6: Construct a risk window predictor and an out-of-distribution monitor, and determine whether to enable safety filtering based on constraint margin, wind field intensity, model uncertainty, and predicted risk. Step 7: Construct a predictive safety filter to perform safety authentication on candidate control inputs; if a candidate control input cannot be authenticated, solve for the corrected control input or backup control trajectory that satisfies the constraints. Step 8: Construct a reference governance and backup recovery mechanism to adjust the reference advancement speed, reduce the aggressiveness of tracking, and guide the system into the endpoint security set when security risks increase; Step 9: Execute closed-loop control and record trajectory error, control input, constraint margin, filter activation rate, backup feasibility rate, terminal safety set arrival rate, and number of hard constraint violations for comparative verification under different wind field scenarios.

2. The method according to claim 1, characterized in that, In step 1, the state vector, control vector, and wind disturbance vector of the quadcopter UAV are defined as follows: x=[p T ,v T ,or T ,oh T ] T u=[T,τ [,tth,tψ] T dw=[fx,fy,fz,τx,τy,τz] T Where p = [x, y, z] T Let v be the position in the inertial frame of reference, where v = [vx, vy, vz]. T For the velocity of the inertial frame, η = [ ,θ,ψ] T For roll, pitch, and yaw, ω = [p`, q, r] T τ is the angular velocity of the engine system; T is the total thrust; τ is the angular velocity of the engine system. τθ and τψ are the three-axis control torques; fx, fy, and fz are the equivalent wind disturbance torques; τx, τy, and τz are the equivalent wind disturbance torques. The safety constraints include position boundary constraints, velocity upper limit constraints, input constraints, thrust saturation constraints, torque saturation constraints, maximum tilt angle constraints, and terminal safety set constraints.

3. The method according to claim 2, characterized in that, Step 2 specifically involves: Step 2.1: Construct an average wind model and output the average wind speed based on flight altitude and environmental parameters; Step 2.2: Construct a gust model and generate local time-varying gusts based on gust amplitude, duration, and propagation direction; Step 2.3: Construct a turbulence model and generate time-dependent turbulence components based on the Dryden spectrum or spectral harmonic superposition method; Step 2.4: Construct a sea surface wave-induced disturbance model and generate wave-induced airflow components for flight over the water surface based on the intensity of sea surface wind and waves; Step 2.5: Construct an externally distributed cross-wind and wave scenario to simulate strong cross-winds, strong gusts, and wave coupling disturbances outside the training distribution; Step 2.6: Superimpose the mean wind, gusts, turbulence and wave-induced disturbances to form a spatiotemporal wind field W(p,t), and provide wind speed queries to the controller, observer and simulation platform through the unified interface get_wind(p,t).

4. The method according to claim 1, characterized in that, Step 3 specifically includes: Step 3.1: Based on the current position, speed, and attitude of the UAV, call the spatiotemporal wind field interface to obtain the wind speed at the current position and time; Step 3.2: Based on the relative wind speed and the force model of the UAV, convert the wind speed into equivalent perturbation force in the translational direction and equivalent perturbation moment in the attitude direction; Step 3.3: Construct a disturbance observer based on the state measurements, control input, and nominal dynamic model, and estimate the disturbance mean online. ; Step 3.4: Utilize the residual error cache to store recent prediction errors, and calculate the perturbation covariance Σd and the perturbation boundary. and quantile boundary q d ; Step 3.5, ,Σd, and q d The results are combined into a perturbation estimate and then passed to the model prediction controller and the prediction safety filter, respectively.

5. The method according to claim 1, characterized in that, Step 4 specifically includes: Step 4.1: Collect the state, control input, reference trajectory, wind field intensity, and disturbance estimation results during the closed-loop flight of the UAV; Step 4.2: Construct residual dynamic labels based on the difference between the state differences at adjacent time points and the difference between the nominal predicted state; Step 4.3: Using the state, control input, reference information, and disturbance estimation results as input samples, and the residual dynamics label as output samples, train the Gaussian process residual prediction model. Step 4.4: During the online control process, the Gaussian process residual prediction model outputs the residual mean and residual variance. The residual mean is used to correct the prediction model, and the residual variance is used for constraint tightening and safety risk assessment.

6. The method according to claim 1, characterized in that, Step 5 specifically includes: Step 5.1: In the model predictive control time domain, based on the nominal dynamic model, the mean of disturbance estimation and the mean of Gaussian process residuals are introduced to form a wind sensing prediction model. Step 5.2: Establish a rolling optimization cost function that includes position error, velocity error, terminal error, and control input amplitude; Step 5.3: Tighten the position, velocity, and input constraints based on the disturbance boundary, residual variance, and safety margin to obtain a robust constraint set; Step 5.4: Solve the wind sensing model predictive control problem in each control cycle and output candidate control inputs. .

7. The method according to claim 1, characterized in that, Step 6 specifically includes: Step 6.1: Calculate the position constraint margin, velocity constraint margin, input constraint margin, and terminal set margin based on the current state and predicted trajectory; Step 6.2: Construct a risk window indicator based on the trend of constraint margin changes over the next few steps; Step 6.3: Construct off-distribution monitoring indicators based on wind field intensity, residual variance, disturbance boundary, and statistical characteristics of training distribution; Step 6.4: When the risk window indicator or out-of-distribution monitoring indicator exceeds the preset threshold, the predictive security filter is triggered to enter the active authentication state.

8. The method according to claim 1, characterized in that, Step 7 specifically includes: Step 7.1: Predict candidate control inputs for the controller output using the wind sensing model. Input to be authenticated; Step 7.2: In the prediction time domain, check whether the predicted trajectory corresponding to the candidate control input satisfies the tightened set constraints of position, velocity, input, and terminal safety. Step 7.3: If the candidate control input passes security authentication, then directly execute u(k) = ; Step 7.4: If the candidate control input fails the safety certification, then solve for the corrected control input that has the smallest deviation from the candidate control input and satisfies the safety constraints. ; Step 7.5: If corrective control is not feasible, then execute a backup control strategy, which includes deceleration, braking, hovering, returning to home, or a recovery trajectory converging toward the terminal safe set.

9. The method according to claim 1, characterized in that, Step 8 specifically includes: Step 8.1: Construct a reference governing mechanism and adjust the reference trajectory advancement ratio based on safety margin and safety filtering feasibility; Step 8.2: When the safety margin is sufficient, increase the reference trajectory advancement ratio so that the UAV can track normally according to the original reference trajectory; Step 8.3: When the safety margin decreases or the out-of-distribution index increases, reduce the reference trajectory advance ratio to slow down, partially freeze, or turn the reference trajectory to safely restore the target. Step 8.4: Once the UAV enters the terminal safety set or safety filtering is restored to a feasible state, gradually restore the reference trajectory advancement ratio.

10. The method according to claim 1, characterized in that, Step 9 specifically includes: Step 9.1: Repeat the closed-loop control under different scenarios; Step 9.2: Record the reference state, actual state, candidate control input, final execution input, constraint margin, and safety filter state for each control cycle; Step 9.3: Calculate the 3D position RMSE, peak position error, number of hard constraint violations, filter activation rate, backup feasibility rate, task progress ratio, and terminal security set arrival rate. Step 9.4: Compare the nominal model predictive control, disturbance sensing model predictive control, Gaussian process enhanced model predictive control, and Gaussian process enhanced model predictive control with predictive safety filtering to verify the tracking performance and safety protection effect of the method.

Images