Unmanned aerial vehicle high-precision navigation method based on ADRC disturbance observation and MPC constraint optimization

Abstract

The application relates to the technical field of unmanned aerial vehicle (UAV) nonlinear control and autonomous navigation, and discloses a high-precision UAV navigation method based on ADRC disturbance observation and MPC constraint optimization, which comprises the following steps: inputting the obtained motion parameters and calculation time delay into an augmented random extended state observer to extract a desired drift term and a random diffusion term variance; combining the calculation time delay and the desired drift term to perform forward deduction to obtain a time delay feedforward prediction disturbance quantity; performing spatial contraction on thrust hard constraints based on state errors and diffusion term variances to generate a tube state optimization boundary; offline solving of a Hamilton Jacobi Bellman partial differential equation is performed to obtain an optimal value function to construct a fixed terminal penalty term and an invariant set constraint; and finally, a cost function is solved under the constraints of a state prediction equation, a tube state boundary and a terminal double constraint to extract a first control instruction to drive an actuator to form a closed loop. The application compensates for phase lag caused by system solving, avoids actuator saturation deadlock, and establishes global asymptotic stability in a random flow field.

Description

Technical Field

[0001] This invention relates to the field of nonlinear control and autonomous navigation technology for unmanned aerial vehicles (UAVs), and particularly to a high-precision navigation method for UAVs based on ADRC disturbance observation and MPC constraint optimization. Background Technology

[0002] During shipboard landings and low-altitude operations near the coast, shipborne UAVs are affected by factors such as ship wake and sea surface wind shear, resulting in flow fields that typically contain significant non-stationary random disturbance components. These disturbances exhibit broadband characteristics on a time scale, exhibiting both trending low-frequency drift and non-negligible high-frequency random fluctuations, and their statistical properties are difficult to accurately describe using deterministic models.

[0003] Existing combined control frameworks based on extended state observers and model predictive control typically treat the aforementioned disturbances as a unified lumped unknown input for online estimation and feedforward compensation. This approach reveals several deep-seated contradictions in strongly stochastic environments. There is an inherent trade-off between the disturbance tracking capability and the noise sensitivity of the extended state observer: increasing bandwidth helps track rapidly changing components in the disturbance, but simultaneously amplifies sensor noise and the inherent high-frequency random components of the flow field, resulting in difficult-to-filter random jitter in the observation output; reducing bandwidth, while suppressing noise, causes a delay in the estimation of transient disturbances. When the disturbance simultaneously contains broadband random components, a single observer structure lacks the ability to differentiate and utilize low-frequency trends and high-frequency random components.

[0004] Furthermore, the execution of quadratic programming solutions by airborne microprocessors inevitably introduces measurable computational delays. When the timescale of dynamic changes in the flow field is on the same order of magnitude as the computational delay, the stochastic evolution of the system state between the command application time and the state sampling time becomes non-negligible. In stochastic disturbance environments, the uncompensated evolution of disturbance components within this delay interval will substantially weaken the consistency between the prediction model and the real system, increasing the risk of control command mismatch. Summary of the Invention

[0005] To overcome the above shortcomings, this invention provides a high-precision navigation method for UAVs based on ADRC disturbance observation and MPC constraint optimization. It aims to improve the problem that traditional UAV navigation mostly adopts conventional predictive control, which causes trajectory instability and divergence due to the ignoring of onboard computing power consumption and strong random wind fields.

[0006] This invention provides the following technical solution: a high-precision navigation method for UAVs based on ADRC perturbation observation and MPC constraint optimization, comprising the following steps:

[0007] S1: Obtain the real-time spatial motion parameters of the UAV, the first control command of the previous control cycle, and the computational delay determined by the calculation time of the onboard microprocessor; and construct the dynamic model of the UAV.

[0008] The UAV dynamics model is an affine nonlinear model characterizing the motion characteristics of the UAV, expressed as follows: ;in, Let U be the state vector of the UAV. To control the input vector, The eigenvector field of the UAV dynamics model is given. This is the control input vector field for the UAV dynamics model;

[0009] S2, input the real-time spatial motion parameters and the first control command into an augmented stochastic extended state observer with Itō stochastic differential characteristics, and extract the variance of the expected drift term characterizing the low-frequency trend of the flow field and the random diffusion term characterizing the intensity of the high-frequency random fluctuations of the flow field.

[0010] S3, based on the calculated delay and the expected drift term, perform forward extrapolation calculation along the intrinsic drift vector field of the UAV dynamics model to obtain the delay feedforward prediction disturbance amount with the time label aligned to the prediction time;

[0011] S4. Based on the state error between the nominal state trajectory and the real-time spatial motion parameters and the variance of the random diffusion term, a tube-state cross-section radius function with a probability boundary is constructed; and based on this function, a spatial shrinkage calculation is performed on the thrust hard constraint set of the actuator to generate a tube-state optimization boundary stripped of the uncertainty tolerance space.

[0012] S5, Offline Phase: Based on the UAV dynamics model and the preset offline stochastic diffusion intensity, solve the approximate optimal control problem within the unshrunken thrust hard constraint set to obtain the optimal value function. Then, construct the terminal penalty term and terminal invariant set constraint of the objective cost function based on the optimal value function. The terminal invariant set constraint is a fixed control invariant set that does not change with the variance of the online stochastic diffusion term.

[0013] S6, substitute the time delay feedforward predicted disturbance into the UAV dynamics model to reconstruct the state prediction equation, solve the objective cost function under the constraints of the state prediction equation, the tube state optimization boundary, the fixed terminal penalty term and the terminal invariant set, extract the first control command of the control sequence to drive the actuator, and return to S1 to form a rolling optimization closed loop.

[0014] Furthermore, in S1, the computation delay is determined by the difference in hardware clock cycles between the task trigger timestamp and the instruction generation timestamp.

[0015] Furthermore, in S2, the augmented stochastic extended state observer is constructed as follows:

[0016] ;

[0017] in, For the augmented state vector, To augment the system state transition matrix, To augment the system input control matrix, The first control command, The observer feedback gain matrix, The system measurement output vector is composed of the real-time spatial motion parameters. To augment the system's nominal measurement output matrix, Here is the diffusion coefficient matrix. For standard Wiener process increments;

[0018] The first-order original moment is extracted from the continuous state estimation results by mean square limit filtering and used as the expected drift term, and the second-order central moment is extracted and used as the variance of the random diffusion term.

[0019] Furthermore, in S3, the forward inference calculation is implemented by expanding the multi-order continuous Lie derivatives along the intrinsic drift vector field:

[0020] ;

[0021] in, For time-delay feedforward prediction of disturbance amount, For the desired drift term, The calculation delay is... The pre-defined truncation order is used for the derivation. For the intrinsic drift vector field Lie derivative expansion operator.

[0022] Furthermore, in S4, the tubular cross-sectional radius function is constructed as follows:

[0023] ;

[0024] in, The set of tubular section radius functions characterizing the tolerance set of errors. The state error is... for 3D real space, superscript Indicates transpose. It is a positive definite Lyapunov matrix. For confidence level scalar, The variance of the random diffusion term;

[0025] The positive definite Lyapunov matrix The confidence scalar is obtained by solving the algebraic Riccati equation of the UAV dynamics model after linearization at the equilibrium point. The chi-square distribution quantiles are set according to the preset confidence probability.

[0026] Furthermore, in S4, the spatial shrinkage calculation is implemented using the Poincaré reduction operator:

[0027] ;

[0028] in, To find the optimal boundary for the generated tube state, For the set of thrust hard constraints, For the Poincaré reduction operator (Minkowski difference operation). The gain matrix is ​​a local linear feedback control. This is the set of functions for the radius of the tubular cross section.

[0029] Furthermore, in S5, the offline solution to the approximate optimal control problem is as follows: within the neighborhood of the equilibrium point of the UAV dynamics model, and based on the preset offline stochastic diffusion intensity, the solution to the Hamiltonian-Jacobi-Bellman partial differential equation associated with the UAV dynamics model and the preset offline stochastic diffusion intensity is approximated using a state-dependent Riccati equation iterative solution or a policy iterative method based on a neural network, thereby obtaining the optimal value function. .

[0030] Furthermore, in S5, the terminal invariant set constraint is constructed as follows:

[0031] ;

[0032] in, For terminal invariant set constraint space, For system status, for 3D real space, The optimal value function is... The specified convergence level set constant.

[0033] Furthermore, in S6, the objective cost function is constructed as follows:

[0034] ;

[0035] in, Let the objective cost function be... To predict the time-domain step size, To predict the state, As the target reference state, and respectively with and The weighted norm squared for the weights. To control the input, and These are the state trajectory tracking error weight matrix and the control energy consumption weight matrix, respectively. The optimal value function is... To predict the end state in the time domain;

[0036] The solution is then performed under the constraints of the tube-state optimization boundary and the fixed terminal invariant set.

[0037] The present invention has the following beneficial effects:

[0038] 1. This invention decouples the trend component and random intensity component in the online estimation result of flow field disturbance, enabling rapid feedforward compensation of low-frequency disturbances, while the statistical characteristics of high-frequency random fluctuations are independently extracted and used for dynamic adjustment of constraint boundaries. This avoids the contradiction between estimation accuracy and noise suppression under broadband random disturbances by traditional single-bandwidth observers, and significantly reduces the high-frequency jitter component in control commands.

[0039] 2. This invention explicitly incorporates the measured computational delay into the prediction feedforward link to compensate for the evolution of disturbances within the delay interval, thereby maintaining time alignment between the prediction model and the system state at the moment of command action. This effectively suppresses prediction mismatch caused by solution lag and improves the consistency of dynamic response and trajectory tracking accuracy of closed-loop systems under stochastic environments.

[0040] 3. Based on the real-time estimation results of the random intensity of disturbance, this invention dynamically shrinks the physical constraint boundary of the actuator. During periods of strong random impact, it automatically reserves sufficient execution margin for anti-disturbance correction and releases unnecessary conservative margin during periods of mild disturbance. Thus, while ensuring that the actuator is always in the safe depth working range, it expands the feasible range of control input and avoids actuator saturation failure under extreme conditions.

[0041] 4. This invention introduces terminal penalties and invariant set constraints based on stochastic system optimization theory in the offline stage, and combines them with online-generated tube-state constraints, enabling the solution sequence of finite-time online rolling optimization to possess theoretically provable convergence characteristics under closed-loop conditions. The fixed terminal constraints designed offline reduce online computational overhead, while the online tube-state constraints are used to compensate for the effects of random disturbances in real time. Together, they provide a rigorous guarantee for the long-term stable operation of the controlled system under the combined effects of random disturbances and physical constraints. Attached Figure Description

[0042] Figure 1 This is a flowchart of the high-precision navigation method for UAVs based on ADRC perturbation observation and MPC constraint optimization proposed in this invention;

[0043] Figure 2 This diagram illustrates the parameter acquisition and time delay calculation steps of the high-precision UAV navigation method based on ADRC perturbation observation and MPC constraint optimization proposed in this invention.

[0044] Figure 3 This diagram illustrates the perturbation decoupling and feature extraction steps of the high-precision UAV navigation method based on ADRC perturbation observation and MPC constraint optimization proposed in this invention.

[0045] Figure 4 This is a calculation diagram of the constraint boundary space shrinkage of the high-precision UAV navigation method based on ADRC perturbation observation and MPC constraint optimization proposed in this invention.

[0046] Figure 5 This is a closed-loop calculation diagram for minimizing the constrained target cost function in the high-precision UAV navigation method based on ADRC perturbation observation and MPC constraint optimization proposed in this invention. Detailed Implementation

[0047] The technical solutions in 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.

[0048] Example 1:

[0049] In the first embodiment of the present invention, the present invention provides a high-precision navigation method for unmanned aerial vehicles (UAVs) based on ADRC perturbation observation and MPC constraint optimization, such as... Figures 1-2 As shown, it includes the following steps:

[0050] S1: Obtain the real-time spatial motion parameters of the UAV, the first control command of the previous control cycle and the calculation delay, and construct the UAV dynamics model;

[0051] Furthermore, in S1, based on the real-time spatial motion parameters and the first control command, a UAV dynamic model is constructed. This UAV dynamic model is an affine nonlinear dynamic model and is expressed as:

[0052] ;

[0053] in, Let UAV state vector be... To control the input vector, The eigenvector field of the UAV dynamics model is given. The control input vector field of the UAV dynamics model is given.

[0054] Furthermore, in S1, acquiring the UAV's real-time spatial motion parameters, the first control command of the previous control cycle, and the calculation delay includes:

[0055] The underlying communication interface of the airborne flight control system is called to read the three-dimensional position and attitude data after multi-source sensor fusion and calculation to construct real-time spatial motion parameters;

[0056] Access the instruction cache register of the model predictive control solver and retrieve the first control instruction in the control instruction issuance sequence corresponding to the previous control cycle;

[0057] Extract the task trigger timestamp and instruction generation timestamp corresponding to the previous control cycle from the airborne microprocessor real-time operating system, calculate the hardware clock cycle difference between the task trigger timestamp and the instruction generation timestamp, and define the hardware clock cycle difference as the computation latency.

[0058] Specifically, regarding the technical characteristic of acquiring real-time spatial motion parameters of a UAV, the data input / output process is as follows: Raw electrical signals from the onboard multi-source sensors are received, fused using Kalman filtering within the flight control system, and then output as a column vector of real-time spatial motion parameters containing position and attitude information. This vector is then input into the subsequent augmented stochastic state observer. The constructed mathematical model of the real-time spatial motion parameters is expressed as follows:

[0059] ;

[0060] in This represents the real-time spatial motion parameter matrix at the current sampling moment; This indicates the lateral position coordinates of the UAV in three-dimensional space; This represents the longitudinal position coordinates of the UAV in three-dimensional space; This represents the vertical height coordinates of the drone in three-dimensional space; Indicates the roll angle of the drone; Indicates the pitch angle of the drone; This indicates the yaw angle of the drone.

[0061] Regarding the technical feature of obtaining the first control command of the previous control cycle, the data input / output process is as follows: Historical prediction queue data is retrieved from the instruction cache register of the model predictive control solver; the first data frame is extracted and used as the known driving input for state transition, then passed to the dynamic iterative equations of the augmented stochastic extended state observer. Its mathematical extraction logic is expressed as follows:

[0062] ;

[0063] in This represents the column vector of the first control command of the previous control cycle; This represents the finite-time control prediction sequence matrix output by the solver in the previous control cycle; The first column vector in the finite-time-domain control prediction sequence matrix represents the actual drive command issued to the underlying actuator.

[0064] Regarding the technical characteristic of computational latency, the data input / output process is as follows: The hardware clock ticks of the onboard microprocessor's real-time operating system are read, and the computational latency is output in differential scalar numerical form for subsequent forward time-dimensional extrapolation along the nonlinear dynamic vector field. The specific mathematical formula for calculating the clock cycle difference is:

[0065] ;

[0066] in This indicates the computational delay, and its engineering unit is set to milliseconds. This represents the hardware timestamp indicating when the model predictive control solver completes the sequence minimization optimization and generates instructions; This indicates the hardware timestamp when the airborne microprocessor receives a signal from the sensor and triggers a status update task.

[0067] By calling the underlying communication interface of the airborne flight control system to extract three-dimensional position and attitude data to form real-time spatial motion parameters, and synchronously accessing the instruction cache register to extract the first control instruction of the previous control cycle, and calculating the difference between the hardware clock cycle based on the mission trigger timestamp and the instruction generation timestamp to establish the calculation delay, the traditional flight control architecture has improved the observation model distortion and time label misalignment defects caused by ignoring the underlying solution time and control action lag when facing huge matrix operations. This avoids the cross-axis coupling dynamic instability and trajectory yaw crash problems caused by the phase lag of the closed-loop control system in complex non-guided flow field environments.

[0068] like Figure 3 As shown in Figure S2, the real-time spatial motion parameters and the first control command are input into the augmented stochastic expansion state observer to extract the variance of the expected drift term and the stochastic diffusion term.

[0069] Furthermore, in S2, the real-time spatial motion parameters and the first control command are input into the augmented stochastic extended state observer, including:

[0070] The lumped airflow disturbance is introduced as an independent augmented state dimension into the UAV nominal state space model; combined with the standard Wiener process increment and the diffusion coefficient matrix, an augmented stochastic extended state observer with Itō stochastic differential properties is constructed.

[0071] Real-time spatial motion parameters and the first control command are synchronously input into the state observation equations of the augmented stochastic extended state observer for continuous state estimation.

[0072] In S2, the variances of the expected drift term and the random diffusion term are extracted as follows:

[0073] Perform mean-square limiting filtering on the continuous state estimation results of the augmented stochastic extended state observer;

[0074] The statistical mean component, which characterizes the low-frequency deterministic trend of the flow field, is extracted from the results of the mean square limit filter operation, and the statistical mean component is defined as the expected drift term.

[0075] The second-order central moment, which characterizes the intensity of high-frequency random fluctuations in the flow field, is extracted from the results of the mean square limit filter operation, and is defined as the variance of the random diffusion term.

[0076] Specifically, the lumped airflow disturbance is extended as an independent dimension into the UAV's nominal state space model to construct an augmented system. Based on the Markov process physical characteristics of the environmental wind field, a state observation equation incorporating Itō stochastic differential characteristics is constructed by combining the standard Wiener process increment and diffusion coefficient matrix. The hardware interface directly inputs the real-time spatial motion parameters and the first control command obtained in the previous step into this equation to perform continuous iterative solution. The data input for this stage is the three-dimensional position and attitude measurement values ​​of the UAV body and the underlying drive command of the previous cycle, and the data output is a continuously augmented state estimation matrix containing flow field disturbance characteristics.

[0077] Its state-observation equation mathematical model, which possesses the stochastic differential properties of Itoh, is expressed as:

[0078] ;

[0079] in Represents the differential component of the augmented random state vector; This represents the nominal state transition matrix of the augmented system; This represents the nominal input control matrix of the augmented system; This represents the column vector of the first control command of the previous control cycle; This represents the state observer feedback gain matrix; This represents a column vector of system measurement outputs consisting of real-time spatial motion parameters; This represents the nominal measurement output matrix of the augmented system; This represents the diffusion coefficient matrix corresponding to the random flow field. This represents the increment matrix of a standard Wiener process, used to characterize processes with a mathematical expectation of zero and a variance equal to the corresponding time increment. Gaussian white noise.

[0080] For the continuously augmented state estimation matrix generated by the above operations, the observer's underlying computational unit performs mean-square limit filtering to decouple mathematical features. Specifically, the data flow logic is as follows: the final-dimensional state estimation component representing the lumped disturbance is extracted separately from the mean-square limit filtering result, and its first-order raw moment and second-order central moment are calculated respectively. The first-order raw moment calculation result is output as the statistical mean component representing the low-frequency deterministic trend of the flow field, i.e., the expected drift term; the second-order central moment calculation result is output as a parameter representing the intensity of high-frequency random fluctuations in the flow field, i.e., the variance of the random diffusion term.

[0081] Its feature decoupling and separation mathematical model is expressed as:

[0082] ;

[0083] ;

[0084] in This represents the expected drift term, which characterizes the low-frequency deterministic trend of the flow field. Operators that represent the calculation of mathematical expectation; This represents the first state in the augmented random state vector corresponding to the lumped airflow disturbance. Continuous state estimation components; The variance of the random diffusion term, representing the intensity of high-frequency random fluctuations in the flow field; This represents the operator for calculating the mathematical variance.

[0085] In the discrete-time implementation, the above expectation operation uses a fixed window length up to the current time step. The sampled values ​​within the range are obtained by performing an unbiased estimation, i.e. ,in To augment the state dimensional components in the th dimension The estimated value at each sampling time.

[0086] In the discrete-time implementation of this invention, the mean square limiting filter operation specifically refers to applying a fixed window length up to the current time step. The continuous state estimation sample values ​​are statistically calculated to approximate the first-order raw moment and the second-order central moment using unbiased estimation.

[0087] By introducing a standard Wiener process incremental reconstruction of the observer's dynamic model and superimposing mean-square limit filtering data processing operations, the physical limitations of conventional extended state observers when assuming external disturbances are deterministic and differentiable signals are overcome. The above operations precisely decompose the complex asymmetric random flow field into low-frequency expected parameters for feedforward compensation and high-frequency variance parameters for constructing pipe-state constraints. This solves the problem of high-frequency distortion of the observation matrix and signal noise amplification caused by the misjudgment of high-frequency random fluctuations as real state evolution in strong Markov white noise wind fields by traditional active disturbance rejection algorithms. At the underlying logic level, this ensures the high fidelity of the feedforward data received in subsequent forward inference calculations and the stability of system decoupling.

[0088] S3, based on the calculated time delay and expected drift term, perform forward extrapolation calculations along the intrinsic drift vector field of the UAV dynamics model to obtain the time delay feedforward prediction disturbance amount aligned with the time label to the prediction time.

[0089] Furthermore, in S3, based on the calculated time delay and expected drift term, forward inference calculation is performed along the intrinsic drift vector field to obtain the time delay feedforward prediction perturbation, including:

[0090] Extract the intrinsic drift vector field from the UAV dynamics model. The intrinsic drift vector field is the drift vector field in the UAV dynamics model that does not contain control input terms.

[0091] Construct a multi-order continuous Lie derivative expansion operator along the direction of the eigenvector field;

[0092] By using the expected drift term as the basic scalar function, and combining the computational delay with multi-order continuous Lie derivative expansion operators, a differential manifold polynomial expansion is performed in the time dimension to solve for the time delay feedforward prediction perturbation that aligns the time label to the prediction time.

[0093] Specifically, the data input terminal receives the pre-calculated delay and expected drift term, and the data output terminal generates a delay feedforward prediction disturbance with time lead compensation characteristics. The control system's underlying computing unit extracts the UAV dynamic model, which is an affine nonlinear dynamic model and can be expressed as follows: The affine form. In this step, the input control variables in the model are stripped away. Influence matrix items Only parameters related to the current system state are retained separately. Related eigenvector field .

[0094] Based on the acquired intrinsic drift vector field, the microprocessor's underlying algorithm space constructs a multi-order continuous Lie derivative expansion operator for multi-dimensional spatial feature mapping. The operational mechanism sets the desired drift term of the decoupled output as a fundamental scalar function, combines it with the extracted computational delay as the time step for forward recursion, and performs a time-dimensional differential manifold polynomial expansion along the direction of the intrinsic drift vector field. Through a polynomial series accumulation mechanism, the system extrapolates the evolution and mapping values ​​of the desired drift term at future moments, directly calculating the delay feedforward prediction disturbance amount that ensures the time label is perfectly aligned to the actual execution time of the control command.

[0095] The mathematical model for the above differential manifold polynomial expansion derivation is expressed as follows:

[0096] ;

[0097] in This represents the delay feedforward prediction perturbation amount that aligns the time stamp to the prediction execution time. This represents the desired drift term in the output of the flow field decoupling and stripping process; This represents the computational delay parameter extracted in the preceding steps; The order index scalar represents the polynomial expansion operator; This represents the limit value of the deduction cutoff order set by the system, and its engineering-level value is set to a positive integer three; The first eigenvector field is constructed along the direction of the eigenvector field. Continuous Lie derivative expansion operator; This represents the result of factorial operation on the order index scalar.

[0098] By extracting the intrinsic drift vector field and introducing forward extrapolation of the continuous Lie derivative expansion operator, the problem of control command lag failure caused by the time-consuming hardware solution calculation in traditional flight control architecture is directly solved. A time-advance compensation mechanism based on the underlying nonlinear dynamic geometry is introduced to proactively fill the blind spot of external environmental wind field disturbance evolution bias during the optimization of complex matrices by the predictive control solver. This avoids the loss of control phase margin and flight trajectory divergence caused by the airborne microprocessor forcibly substituting outdated feedforward disturbance data into the current system state equations, thus establishing the physical validity of the feedforward compensation signal from a time alignment perspective.

[0099] like Figure 4 As shown in Figure S4, the state error and random diffusion term variance between the nominal state trajectory and the real-time spatial motion parameters are combined to construct the tube-state cross-section radius function; based on the tube-state cross-section radius function, the space shrinkage calculation is performed on the thrust hard constraint set of the actuator to generate the tube-state optimization boundary.

[0100] Furthermore, in S4, by combining the state error between the nominal state trajectory and the real-time spatial motion parameters, as well as the variance of the random diffusion term, the tube-state cross-section radius function is constructed, including:

[0101] Extract the nominal state trajectory during sequence optimization;

[0102] Calculate the corresponding dimensional difference between the nominal state trajectory and the real-time spatial motion parameters to generate the state error;

[0103] A positive definite Lyapunov matrix and a predefined confidence scalar are introduced;

[0104] Using the product of the confidence scalar and the variance of the random diffusion term as the upper bound of the probability boundary, and combining the state error and the positive definite Lyapunov matrix, a tube-shaped cross-sectional radius function representing the error tolerance set is constructed.

[0105] In S4, the space shrinkage calculation is performed on the thrust hard constraint set of the actuator based on the tube cross-section radius function to generate the tube optimization boundary, including:

[0106] Extract the physical drive limit range parameters of the UAV rotor motor to construct a set of thrust hard constraints;

[0107] The control input mapping for the tube cross-section radius function is performed based on the local linear feedback control gain matrix;

[0108] By using the Poincaré reduction operator to perform a set subtraction operation on the tube cross-section radius function after mapping the thrust hard constraint set and the control input, a tube optimization boundary stripped of uncertainty tolerance space (i.e., the control input allowable set after robust shrinkage) is generated.

[0109] Specifically, the core task is to transform the random uncertainty observed at the front end into a specific contraction amount of the constraint boundary. Its data inputs are the nominal state trajectory, real-time spatial motion parameters, and variance of the random diffusion term, and the data output is the tube-state optimization boundary used for subsequent cost function optimization.

[0110] In constructing the tube-state cross-sectional radius function, the system first extracts the nominal state trajectory generated by model predictive control rolling optimization (in the first control cycle, the nominal state trajectory can be initialized as a preset reference trajectory or a constant trajectory of the current real-time spatial motion parameters), and subtracts it from the real-time spatial motion parameters obtained by the underlying sensing system to obtain a multi-dimensional state error vector. Subsequently, a positive definite Lyapunov matrix is ​​introduced to characterize the energy decay characteristics of the system error, and a confidence scalar is set and multiplied by the variance of the random diffusion term to form a threshold upper limit for the probability boundary. Combining these parameters, a set of cross-sectional radii with clear physical geometric meaning is constructed.

[0111] The mathematical model for the relationship between the state error and the radius function of the pipe section is expressed as follows:

[0112] ;

[0113] ;

[0114] in This represents the column vector of the UAV's state error at the current moment; This represents the real-time spatial motion parameter matrix at the current moment; This represents the nominal state trajectory matrix under the ideal, undisturbed state at the current moment; The set of probabilistically robust tubular cross-section radius functions represents an ellipsoidal cross-section in multidimensional space; express 3D real space; The transpose matrix representing the column vector of state errors; This represents the system's pre-defined positive definite Lyapunov matrix; This represents a manually set confidence level scalar used to adjust the conservatism of the constraints; This represents the variance of the random diffusion term output from the pre-decoupling stripping.

[0115] In this step, the positive definite Lyapunov matrix The method for obtaining the matrix is ​​as follows: the UAV dynamics model is Jacobian linearized near the hovering or landing equilibrium point to obtain the linear time-invariant system matrix. Select a weight matrix with positive diagonal elements. Solve the continuous-time algebraic Riccati equation:

[0116] ;

[0117] The unique symmetric positive definite solution matrix is ​​obtained, which serves as the positive definite Lyapunov matrix. .

[0118] The confidence scalar The setting rule is: based on the preset confidence probability that the tube should fall into the tube. The search freedom is The upper quantile of the chi-square distribution, i.e., let ,in Let be the dimension of the state. Therefore, by the inequality... The defined ellipsoidal domain constitutes a probability A confidence invariant set containing the actual state trajectory.

[0119] The above method of setting the confidence scalar using the chi-square distribution quantile is based on the following premise: after the flow field disturbance driven by the random diffusion term propagates through the UAV closed-loop dynamics system, the state error within the neighborhood of the equilibrium point... It approximately follows a Gaussian distribution with zero mean. Under this premise, the quadratic form... Obeying the degree of freedom The chi-square distribution, with the confidence probability of the state error formed by the ellipsoidal domain defined by the inequality being... The invariant set. When the actual perturbation characteristics deviate from this premise, the confidence probability can be increased. The set value is used to adjust the conservatism of the constraint in order to maintain the robustness of the tubular boundary in actual operation.

[0120] In the process of generating the tube-state optimization boundary, the control system extracts the maximum and minimum speed thresholds of the four rotor motors at the bottom layer and maps them to physical drive limit range parameters, thereby constructing the initial set of thrust hard constraints. Next, a local linear feedback control gain matrix is ​​introduced to project the tube-state cross-sectional radius function in the state space onto the control input space. Finally, the Poincaré reduction operator is executed to subtract the tolerance geometry space mapped from the initial thrust hard constraint set, directly outputting the final optimization boundary after safe contraction.

[0121] The mathematical model for calculating its spatial contraction is expressed as follows:

[0122] ;

[0123] in This represents the set of tubular optimization boundary geometry generated after spatial contraction. This represents the set of initial thrust hard constraints composed of the physical drive limit range parameters of the rotor motor; This represents the Poincaré reduction operator, used to perform the Minkowski difference operation between two convex sets; This represents the local linear feedback control gain matrix used to compensate for local errors. This represents the set of probabilistically robust tubular cross-sectional radius functions constructed in the preceding sequence.

[0124] By extracting the hard constraint limits of the UAV motors and constructing a probabilistic robust tube-state cross-section radius function in conjunction with state errors, and then performing set subtraction, this improves the control defect of traditional model predictive control, which approaches the physical limits of the predicted trajectory when dealing with strong gusts, leading to hard saturation of the actuators. The aforementioned tolerance stripping operation forcibly transforms the uncertain random environmental noise variance into a rigid shrinkage of the control algorithm boundary, forcibly reserving a physical thrust margin for the underlying rotor motors for attitude self-stabilization adjustment. This avoids the UAV from losing its self-recovery capability due to motor thrust depletion when encountering sudden flow field impacts, thus preventing rollover and crashes caused by the UAV losing its self-recovery capability. This establishes an absolute safety boundary for trajectory tracking at the hardware execution level.

[0125] S5, offline construction of fixed terminal penalty terms and terminal invariant set constraints;

[0126] This step is performed offline before the UAV system goes online. The data input for this step is the UAV dynamics model and a preset offline random diffusion intensity, which can be set as the statistical upper bound or nominal value of the disturbance intensity under the expected operating environment; the output is a fixed-form terminal penalty term and terminal invariant set constraint, which can be directly called by the subsequent online model predictive control.

[0127] The control system constructs a continuous-time optimal control objective functional, which includes a state penalty term characterizing the trajectory tracking error and a control input penalty term characterizing the motor hardware energy consumption. This is done within the original, unconsolidated set of thrust hard constraints. Internally, the underlying computational unit establishes Hamiltonian-Jacobi-Bellman (HJB) partial differential equations. The mathematical expression of its core partial differential equation is:

[0128] ;

[0129] in, For control input; This represents the original set of thrust hard constraints. This refers to the system status; , This is the weight matrix; Let be the value function to be solved; , This refers to the system dynamics term; It is a fixed diffusion coefficient matrix set according to the preset random diffusion intensity.

[0130] The offline solution uses fixed physical limits as the constraint boundary. The diffusion coefficient is a preset fixed value. All are independent of the dynamically generated pipe-state optimization boundary during the online phase. and the variance of the random diffusion term estimated in real time .

[0131] For the above partial differential equations, an offline approximation solution is obtained by using the state-dependent Riccati equation (SDRE) iterative method or the neural network strategy iterative method to obtain an approximate optimal value function that satisfies the local asymptotic stability condition of the system. After that, The algebraic expression is directly assigned to the end time of the prediction time domain and is defined as a fixed terminal penalty term at the end time of the objective cost function.

[0132] The specified convergence level set constant is set based on the inherent convergence properties of the extracted approximate optimal value function. Based on this, fixed terminal state boundary conditions are generated to construct the terminal invariant set constraints. The mathematical model of these invariant set constraints is expressed as follows:

[0133] ;

[0134] The resulting terminal penalty item and terminal invariant set It is completely fixed and will be pre-set as a fixed parameter in the online MPC solver, and will be directly called in each subsequent rolling optimization of S6, independent of the variance of the real-time random diffusion term estimated online. Change.

[0135] It should be noted that, since the online tube-state optimization boundary has been dynamically shrunk according to the real-time random diffusion intensity, it ensures that the system state can still be effectively driven into the fixed terminal invariant set within the prediction time domain under strong disturbances. This achieves the synergy between online tube-state robust constraints and offline fixed terminal stability constraints. The above design, while ensuring the theoretical stability of the system, concentrates the main computational overhead in the offline stage.

[0136] like Figure 5 As shown in S6, the time delay feedforward predicted disturbance is substituted into the UAV dynamics model to reconstruct the state prediction equation. Under the constraints of the state prediction equation, the tube state optimization boundary, the terminal penalty term and the terminal invariant set, the objective cost function is solved. The first control command of the control sequence is extracted to drive the actuator, and the step of obtaining real-time spatial motion parameters is returned, forming a rolling optimization closed loop.

[0137] Furthermore, in S6, the time-delay feedforward predicted disturbance is substituted into the UAV dynamics model to reconstruct the state prediction equation. Under the constraints of the state prediction equation, the control state optimization boundary, the terminal penalty term, and the terminal invariant set, the objective cost function is solved. The first control command of the control sequence is extracted to drive the actuator, and the steps of obtaining real-time spatial motion parameters are returned, forming a rolling optimization closed loop including:

[0138] The time-delay feedforward prediction disturbance is substituted as an independent feedforward compensation parameter into the evolution matrix of the discretized dynamic model to generate the state prediction equation.

[0139] The terminal penalty term is added to the trajectory tracking error cost and control energy consumption cost during operation to construct a complete discretized objective cost function;

[0140] The state prediction equation is set as the system equality constraint condition for solving the sequential quadratic programming problem, and the tube state optimization boundary is set as the control input inequality constraint condition.

[0141] Under the combined constraints of system equality constraints, control input inequality constraints, and terminal invariant set constraints, a sequence minimization optimization calculation is performed on the objective cost function to output the optimal control sequence in the finite time domain.

[0142] The first data frame of the optimal control sequence in the finite time domain is extracted and sent to the actuator as the first control command, and the real-time spatial motion parameter acquisition command of the next control cycle is triggered to complete the rolling optimization closed loop.

[0143] Specifically, the data stream input end aggregates the time delay feedforward prediction perturbation generated by the preceding calculations, the tube-state optimization boundary, and the optimal value function with terminal constraints. The data stream output end generates physical drive commands that are directly sent to the UAV's underlying rotor motors, along with a hardware clock synchronization signal that triggers a new round of sensor data acquisition.

[0144] The onboard microprocessor discretizes the UAV system according to the set control sampling step size. The time-delay feedforward prediction disturbance is treated as an independent and deterministic feedforward compensation parameter and directly substituted into the discretized system evolution matrix to establish a state prediction equation for future time-domain extrapolation. The mathematical expression of the state prediction equation is constructed as follows:

[0145] ;

[0146] in Indicates the current sampling time Next for the future The system state prediction column vector for each step; This represents the nonlinear evolution mapping matrix of the UAV nominal system after discretization. Indicates the current sampling time Next for the future The state prediction column vector for each step; Indicates the current sampling time Next for the future The control input column vector for the step; This represents the time-delay feedforward prediction disturbance calculated using the Lie derivative.

[0147] The system mathematically sums the trajectory tracking error cost, control energy consumption cost, and terminal penalty term during operation to construct a complete discretized objective cost function. The mathematical model of this discretized objective cost function is expressed as follows:

[0148] ;

[0149] in This represents the discretized scalar value of the objective cost function for model predictive control. This indicates the finite prediction time step set by the system; This represents the off-step index scalar in the prediction time domain; This represents the column vector of state predictions corresponding to the walk; This represents the column vector of the set UAV target reference state; Represents the square of the weighted norm; This represents the set state trajectory tracking error weight matrix; This represents the squared weighted norm of the control energy; This represents the set control energy consumption weight matrix; This represents the optimal value function when substituted into the prediction time domain's final time as the terminal penalty term; This represents the column vector of the predicted final state of the UAV at the end of the prediction time domain.

[0150] The control algorithm unit sets the derived state prediction equation as the system equality constraint condition for the sequential quadratic programming solver, and sets the space-shrinking tube-state optimization boundary as the control input inequality constraint condition. Under the joint framework of the above equality constraints, inequality boundaries, and terminal invariant set constraints, the sequential quadratic programming solver performs a minimization optimization calculation on the objective cost function. The solver searches for a set of control input mappings within the multidimensional constraint space that minimizes the scalar value of the objective cost function, and outputs the optimal control sequence in the finite-time domain.

[0151] The instruction distribution logic intercepts the first data frame within the finite-time-domain optimal control sequence. The system converts this into the first control instruction and sends it to the underlying actuator to drive the rotor motor. At the instant this instruction takes effect at the physical level, the underlying hardware data bus triggers the real-time spatial motion parameter acquisition instruction for the next control cycle, driving the sensors and state observers to enter the next clock cycle.

[0152] By directly substituting the feedforward disturbance into the discrete prediction equation constraints, the lag in dynamic response of conventional closed-loop feedback control in the face of severe external nonlinear airflow is overcome. The sequential quadratic programming solver performs joint numerical calculations within strict pipe-state inequality boundaries and terminal convergence sets, curbing the control solution space divergence problem that easily occurs in conventional single-source disturbance rejection algorithms when facing multidimensional strongly coupled wind fields. Extracting the first instruction and triggering a new clock cycle via the hardware bus establishes the time-closed-loop property of finite-time-domain rolling optimization at the underlying logic level. The combination of these features ensures that the computationally limited airborne microprocessor can continuously output a safe driving matrix that conforms to physical limits in extremely variable and highly disturbed flow fields, preventing flight trajectory deviations and UAV crashes caused by control overshoot from the system architecture source.

[0153] Example 2:

[0154] In the scenario of autonomous landing of carrier-based UAVs in strong wake fields, the UAVs need to land accurately on the moving deck under adverse sea conditions. This process faces severe technical challenges caused by the complex environment and hardware constraints. First, a stochastic wake field with strong Markov characteristics arises above the aircraft carrier deck. Traditional deterministic observers are prone to high-frequency distortion and noise amplification when dealing with such non-smooth, non-differentiable, and extremely strong flow field disturbances, making it impossible to achieve accurate disturbance decoupling. Second, high-precision trajectory optimization algorithms involve massive matrix calculations. The resulting computational delay of the onboard microprocessor causes the issued commands to lag significantly behind the rapidly changing flow field state. This control phase deviation directly undermines the Lyapunov stability of the controlled system, leading to trajectory divergence. Finally, under extreme gusts of wind, frequent attitude adjustments can easily cause the underlying rotor motors to reach their thrust physical limits. The existing control architecture lacks dynamic tube-state constraint contraction and terminal cost fallback mechanisms for random disturbances, causing the motors to frequently fall into hard saturation deadlock, ultimately leading to cross-axis coupling instability and even a crash of the UAV. To address the aforementioned problems, this invention provides a high-precision navigation method for UAVs based on ADRC perturbation observation and MPC constraint optimization, the structure of which is as follows: Figure 1 As shown. The specific implementation process of this method is as follows:

[0155] By constructing an augmented stochastic extended state observer, the complex flow field is accurately separated into the variance of the expected drift term and the random diffusion term, avoiding the measurement errors caused by conventional observers when dealing with high-frequency noise. The system performs forward inference calculations along the dynamic vector field based on the extracted expected drift term, actively filling the blind spots of disturbance evolution during control command solution to eliminate phase lag caused by hardware. To defend against extreme gust impacts, the algorithm transforms the extracted random variance into a set of hard thrust constraints on the underlying actuators based on the pipe cross-section radius, performing geometric shrinkage to forcibly retain the physical thrust margin for attitude self-recovery. Subsequently, an offline approximate solution of the Hamiltonian-Jacobi-Bellman partial differential equations is used to establish a terminal invariant set catch-all constraint for prediction optimization. This constraint is independent of the online pipe boundary and is pre-set as a fixed parameter in the solver. Finally, under the joint constraints of the time delay feedforward compensation equation, the safe shrinking pipe optimization boundary, and the fixed terminal stability constraint, a minimization solution is performed, and the first command is extracted to form a closed loop. This control mechanism completely eliminates the uncertainty tolerance space in the underlying solution architecture, preventing the actuator from falling into hard saturation deadlock under extreme conditions. It establishes the global asymptotic stability and absolute hardware safety of the controlled system in extremely strong non-guided wind fields from a mathematical mechanism perspective.

Claims

1. A high-precision navigation method for unmanned aerial vehicles (UAVs) based on ADRC perturbation observation and MPC constraint optimization, characterized in that, include: S1, acquire the real-time spatial motion parameters of the UAV, the first control command of the previous control cycle, and the computational delay determined by the calculation time of the onboard microprocessor; And construct a dynamic model of the drone; The UAV dynamics model is an affine nonlinear model characterizing the motion characteristics of the UAV, expressed as follows: ;in, Let U be the state vector of the UAV. To control the input vector, The eigenvector field of the UAV dynamics model is given. This is the control input vector field for the UAV dynamics model; S2, input the real-time spatial motion parameters and the first control command into an augmented stochastic extended state observer with Itō stochastic differential characteristics, and extract the variance of the expected drift term characterizing the low-frequency trend of the flow field and the random diffusion term characterizing the intensity of the high-frequency random fluctuations of the flow field. S3, based on the calculated delay and the expected drift term, perform forward extrapolation calculation along the intrinsic drift vector field of the UAV dynamics model to obtain the delay feedforward prediction disturbance amount with the time label aligned to the prediction time; S4. Based on the state error between the nominal state trajectory and the real-time spatial motion parameters and the variance of the random diffusion term, a tube-state cross-section radius function with a probability boundary is constructed; and based on this function, a spatial shrinkage calculation is performed on the thrust hard constraint set of the actuator to generate a tube-state optimization boundary stripped of the uncertainty tolerance space. S5, Offline Phase: Based on the UAV dynamics model and the preset offline stochastic diffusion intensity, solve the approximate optimal control problem within the unshrunken thrust hard constraint set to obtain the optimal value function. Then, construct the terminal penalty term and terminal invariant set constraint of the objective cost function based on the optimal value function. The terminal invariant set constraint is a fixed control invariant set that does not change with the variance of the online stochastic diffusion term. S6, substitute the time delay feedforward predicted disturbance into the UAV dynamics model to reconstruct the state prediction equation, solve the objective cost function under the constraints of the state prediction equation, the tube state optimization boundary, the fixed terminal penalty term and the terminal invariant set, extract the first control command of the control sequence to drive the actuator, and return to S1 to form a rolling optimization closed loop.

2. The method according to claim 1, characterized in that, In S1, the computation delay is determined by the difference in hardware clock cycles between the task trigger timestamp and the instruction generation timestamp.

3. The method according to claim 1, characterized in that, In S2, the augmented stochastic expansion state observer is constructed as follows: ； in, For the augmented state vector, To augment the system state transition matrix, To augment the system input control matrix, The first control command, The observer feedback gain matrix, The system measurement output vector is composed of the real-time spatial motion parameters. To augment the system's nominal measurement output matrix, Here is the diffusion coefficient matrix. For standard Wiener process increments; The first-order original moment is extracted from the continuous state estimation results by mean square limit filtering and used as the expected drift term, and the second-order central moment is extracted and used as the variance of the random diffusion term.

4. The method according to claim 1, characterized in that, In S3, the forward derivation calculation is implemented by expanding the multi-order continuous Lie derivatives along the intrinsic drift vector field: ； in, For time-delay feedforward prediction of disturbance amount, For the desired drift term, The calculation delay is... The pre-defined truncation order is used for the derivation. For the intrinsic drift vector field Lie derivative expansion operator.

5. The method according to claim 1, characterized in that, In S4, the tube-shaped cross-sectional radius function is constructed as follows: ； in, The set of tubular section radius functions characterizing the tolerance set of errors. The state error is... for 3D real space, superscript Indicates transpose. It is a positive definite Lyapunov matrix. For confidence level scalar, The variance of the random diffusion term; The positive definite Lyapunov matrix The confidence scalar is obtained by solving the algebraic Riccati equation of the UAV dynamics model after linearization at the equilibrium point. The chi-square distribution quantiles are set according to the preset confidence probability.

6. The method according to claim 5, characterized in that, In S4, the spatial shrinkage calculation is implemented using the Poincaré reduction operator: ； in, To find the optimal boundary for the generated tube state, For the set of thrust hard constraints, For the Poincaré reduction operator (Minkowski difference operation). The gain matrix is ​​a local linear feedback control. This is the set of functions for the radius of the tubular cross section.

7. The method according to claim 1, characterized in that, In S5, the offline solution to the approximate optimal control problem is as follows: Within the equilibrium point neighborhood of the UAV dynamics model, and based on the preset offline stochastic diffusion intensity, the solution to the Hamiltonian-Jacobi-Bellman partial differential equation associated with the UAV dynamics model and the preset offline stochastic diffusion intensity is approximated using a state-dependent Riccati equation iterative solution or a policy iterative method based on a neural network, thereby obtaining the optimal value function. .

8. The method according to claim 7, characterized in that, In S5, the terminal invariant set constraint is constructed as follows: ； in, For terminal invariant set constraint space, For system status, for 3D real space, The optimal value function is... The specified convergence level set constant.

9. The method according to claim 1, characterized in that, In S6, the objective cost function is constructed as follows: ； in, Let the objective cost function be... To predict the time-domain step size, To predict the state, As the target reference state, and respectively with and The weighted norm squared for the weights. To control the input, and These are the state trajectory tracking error weight matrix and the control energy consumption weight matrix, respectively. The optimal value function is... To predict the end state in the time domain; The solution is then performed under the constraints of the tube-state optimization boundary and the fixed terminal invariant set.

Images