Unmanned aerial vehicle trajectory tracking control method based on PSO-STEKF and NMPC in strong interference environment

Abstract

This invention belongs to the field of intelligent control and state estimation technology for unmanned aerial vehicles (UAVs), and discloses a UAV trajectory tracking control method based on PSO-STEKF and NMPC under strong interference environments. The method constructs a nonlinear discrete state-space model of the UAV, establishes the nonlinear model predictive control (NMPC) objective cost function and control input constraints; utilizes the particle swarm optimization algorithm (PSO) to optimize the fading factor in the strong tracking extended Kalman filter (STEKF) online, obtaining a posterior state estimate; uses the posterior state estimate as the initial state of the NMPC optimization problem, and continuously solves for the optimal control input sequence before applying the first control input. This method can improve the state estimation accuracy, trajectory tracking performance, and system robustness under strong interference environments.

Description

Technical Field

[0001] This invention relates to the field of UAV flight control and state estimation technology, specifically to a UAV trajectory tracking control method based on PSO-STEKF and NMPC under strong interference environment. Background Technology

[0002] With the development of aviation technology, unmanned aerial vehicles (UAVs), represented by quadcopter UAVs, have promising application prospects in fields such as military reconnaissance, target surveillance, precision agriculture, and disaster prevention and mitigation. However, UAV systems typically exhibit characteristics such as nonlinearity, strong coupling, and underactuation. In actual flight, they are also affected by uncertainties such as wind disturbances and sensor noise, which increases the difficulty of accurate state acquisition and stable trajectory tracking.

[0003] In existing technologies, nonlinear model predictive control (NMPC) has been applied to UAV trajectory tracking control due to its ability to handle system constraints and multivariable coupling problems. However, the control effect of NMPC usually depends on the accuracy of state feedback information. When sensor measurements are significantly affected by noise and external disturbances, the state estimation error may increase, thereby affecting prediction accuracy and trajectory tracking performance.

[0004] To improve the accuracy of state estimation, the Extended Kalman Filter (EKF) is often used for state estimation of nonlinear systems. However, its estimation performance may be affected when the system state changes rapidly or when external disturbances are strong. Strong tracking filtering, by introducing a fading factor to adjust the prediction covariance, can improve the responsiveness to state changes to some extent. However, the fading factor in related methods usually relies on empirical settings or preset rules, and its adaptability still has room for further improvement in complex time-varying disturbance environments.

[0005] Therefore, how to improve the adaptability and accuracy of UAV state estimation in a strong interference environment, and how to achieve high-precision trajectory tracking control based on the state estimation results, is a technical problem that needs to be solved in this field. Summary of the Invention

[0006] This invention aims to overcome the problem that the fading factor in traditional strong-tracking extended Kalman filtering mainly relies on empirical setting and lacks online adaptive adjustment capability, which limits the accuracy of state estimation. By combining the online optimization mechanism of fading factor of particle swarm optimization with the rolling optimization mechanism of nonlinear model predictive control, this invention provides a UAV trajectory tracking control method based on particle swarm optimization strong-tracking extended Kalman filtering and nonlinear model predictive control under strong interference environment, so as to improve the accuracy of UAV state estimation, trajectory tracking performance and system robustness under strong interference environment.

[0007] To achieve the above objectives, this invention provides a UAV trajectory tracking and control method based on PSO-STEKF and NMPC under strong interference environments, comprising the following steps:

[0008] S1. Construct a nonlinear dynamic model of the UAV and discretize the nonlinear dynamic model to obtain a nonlinear discrete state-space model; establish the target cost function and control input constraints of the nonlinear model predictive control NMPC based on the trajectory tracking task.

[0009] S2. Initialize the parameters of the Particle Swarm Optimization Algorithm (PSO), the Strong Tracking Extended Kalman Filter (STEKF), and the NMPC, and initialize the posterior state estimate, posterior covariance matrix, and residual covariance matrix estimate.

[0010] S3, at each sampling time Calculate the one-step predicted state estimate based on the discrete state transition function. Measurements based on the current sampling time Calculate the measurement residuals using the predicted state estimate from the first step:

[0011] ;

[0012] Based on the measurement residual and the estimated value of the residual covariance matrix Construction and fading factor Relevant fitness functions:

[0013] ;

[0014] in, Let Jacobian matrix be the measurement function. To measure the noise covariance matrix, Represent the Frobenius norm; use the PSO to evaluate the fading factor within a preset feasible region. Perform an online optimization search to obtain the optimal fading factor. ;

[0015] S4. The optimal fading factor The STEKF one-step prediction covariance matrix update process is introduced to obtain the one-step prediction covariance matrix. Based on the updated one-step prediction covariance matrix, the current state of the UAV is predicted and the measurements are updated to obtain the posterior state estimate. ;

[0016] S5. The posterior state estimate is... As the initial state of the NMPC optimization problem at the current sampling time, based on the nonlinear discrete state-space model, the objective cost function, and the control input constraints, the nonlinear programming problem in the prediction time domain is solved to obtain the optimal control input sequence.

[0017] S6. Apply the first control input in the optimal control input sequence to the UAV, and repeat steps S3 to S5 in the rolling time domain framework until the trajectory tracking task is completed.

[0018] Furthermore, the estimated value of the residual covariance matrix in step S3 Obtained through an online recursive method based on the forgetting factor, satisfying:

[0019] ;

[0020] in, It is a forgetting factor, and ; For the first Measurement residuals at each sampling time.

[0021] Furthermore, the one-step prediction covariance matrix described in step S4 satisfies:

[0022] ;

[0023] in, Let Jacobian matrix be the state function. Let be the posterior covariance matrix at the previous sampling time. Let be the process noise covariance matrix.

[0024] Furthermore, the position variable of each particle in the PSO represents the fading factor. The candidate values ​​are determined, and a boundary constraint is applied to the particle position after each position update to keep the particle position within the preset feasible region of the fading factor.

[0025] Furthermore, in the PSO, the first The velocity and position of each particle are updated according to the following formula:

[0026] ,

[0027] ;

[0028] in, For the first The particle in the first Position at the next iteration For the corresponding speed, For the optimal position of an individual, To be the globally optimal position For inertial weights, and As a learning factor, and For interval Random numbers within.

[0029] Furthermore, the inertial weight A linear decreasing strategy is adopted, and the iteration termination condition of the PSO is to reach a preset maximum number of iterations, or the change of the fitness function value in a number of consecutive iterations is less than a preset threshold.

[0030] Furthermore, the objective cost function of NMPC described in step S1 satisfies:

[0031] ;

[0032] in, To predict the time domain, For the first Step-by-step prediction output, As a reference trajectory signal, For the first Predictive control input, This is the state tracking error weight matrix. To control the input weight matrix.

[0033] Furthermore, the control input constraints include control input amplitude constraints, which satisfy:

[0034] ;

[0035] in, and These are the lower and upper bound vectors controlling the input, respectively. To predict the time domain, To control the time domain, the vector inequality states that it holds true for every component of the vector and satisfies the following condition. ;

[0036] when At that time, ,satisfy: ;

[0037] Furthermore, at each sampling time, only the first control input in the optimal control input sequence is applied to the UAV.

[0038] Furthermore, in step S1, the nonlinear dynamic model is discretized using the fourth-order Runge-Kutta method; the UAV is a quadcopter UAV, and its state vector includes at least position, attitude angle, linear velocity and body angular velocity.

[0039] Furthermore, the strong interference environment includes at least one of the following: Dryden turbulence, time-varying wind field disturbance, and enhanced measurement noise; the measurement function The corresponding measurements include at least the position and attitude angles of the UAV.

[0040] Compared with the prior art, the present invention has at least the following beneficial effects:

[0041] 1. This invention optimizes the fading factor in a strong-tracking extended Kalman filter online using a particle swarm optimization algorithm. This enables the fading factor to be adaptively adjusted based on real-time measured residuals and residual covariance matrix estimates, thereby improving the problem of the fading factor relying on empirical settings in traditional strong-tracking extended Kalman filters. This enhances the filter's response to strong interference, sudden state changes, and model uncertainties, and is beneficial for improving state estimation accuracy and filtering stability.

[0042] 2. This invention uses the posterior state estimate of the strong tracking extended Kalman filter output after online optimization as the initial state of the nonlinear model predictive control optimization problem, enabling the nonlinear model predictive control to perform rolling optimization based on more accurate system state information, thereby improving the accuracy and anti-interference capability of UAV trajectory tracking control under strong interference environment.

[0043] 3. This invention combines particle swarm optimization with strong tracking extended Kalman filtering and nonlinear model predictive control, so that the state estimation results and the trajectory tracking control process work together: more accurate state estimation helps to improve the solution quality of predictive control, while the optimized control input helps to improve the dynamic response of the system, thereby improving the subsequent state estimation and control effect, and thus improving the overall control performance of the system.

[0044] 4. The nonlinear model predictive control of the present invention can combine the nonlinear dynamic model of the UAV and explicitly consider practical constraints such as control input amplitude constraints. At the same time, it adopts a rolling time domain strategy to realize closed-loop feedback control, thereby improving the physical feasibility of the control output and the system's adaptability to external disturbances and dynamic response performance.

[0045] It is important to note that in this application, "strong interference environment" does not refer solely to the magnitude of external disturbance amplitude, but rather to a complex interference scenario where the statistical characteristics of the disturbance exhibit non-stationary, time-varying, or even abrupt features. In such scenarios, the fading factor strategy employed by traditional strong-tracking extended Kalman filtering, based on preset rules or fixed values, struggles to match the rapid evolution of the measurement residual statistical characteristics in real time, leading to a significant degradation in state estimation performance. This application introduces a particle swarm optimization algorithm to optimize the fading factor online. Its core significance lies in endowing the filtering algorithm with the ability to identify and optimally match the aforementioned unknown and time-varying disturbance statistical characteristics in real time, thereby achieving a fundamental improvement in adaptive robustness. Attached Figure Description

[0046] Figure 1 This is an overall flowchart of a UAV trajectory tracking and control method under strong interference environment according to the present invention.

[0047] Figure 2 This is a block diagram illustrating the principle of PSO online optimization of the STEKF fading factor in this invention.

[0048] Figure 3 This is the NMPC rolling optimization control block diagram based on posterior state estimates in this invention.

[0049] Figure 4 This is a schematic diagram of the structure of the UAV trajectory tracking control system in an embodiment of the present invention.

[0050] Figure 5 This is a comparison diagram of the position error of the UAV under the spiral reference trajectory in an embodiment of the present invention.

[0051] Figure 6 This is a comparison diagram of the attitude error of the UAV under a spiral reference trajectory in an embodiment of the present invention. Detailed Implementation

[0052] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. It should be understood that the following embodiments are for illustrative purposes only and are not intended to limit the scope of protection of the present invention. All modifications, equivalent substitutions, or improvements made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

[0053] Example 1

[0054] This embodiment provides a UAV trajectory tracking and control method based on PSO-STEKF and NMPC under strong interference environment, such as Figure 1 As shown. The following explanation uses a quadcopter drone as an example. It should be noted that the quadcopter drone is only a preferred embodiment of the present invention, and the method of the present invention is also applicable to other drone platforms with nonlinear dynamic characteristics.

[0055] In some embodiments, the method of the present invention can be run in the onboard flight control computer of an unmanned aerial vehicle (UAV). The UAV collects flight status information through an inertial measurement unit (IMU), a global positioning module (GPS), attitude sensors, etc. The onboard flight control computer executes the method of the present invention to obtain control input, and then converts the control input into drive commands for each rotor motor through a control distribution module to achieve closed-loop control of the UAV body. The method includes the following steps.

[0056] S1: Construct a nonlinear dynamic model of the UAV and discretize the nonlinear dynamic model to obtain a nonlinear discrete state-space model; establish the target cost function and control input constraints of the nonlinear model predictive control NMPC based on the trajectory tracking task.

[0057] In this embodiment, the quadcopter UAV is simplified into a rigid body model with constant mass, geometric symmetry, and uniform mass distribution. A ground inertial coordinate system is selected. and body coordinate system The body coordinate system is fixed to the UAV's center of mass. The UAV's attitude is expressed using Euler angles. The angular velocity of the machine system is denoted as:

[0058]

[0059] The rotational speeds of the four rotors are respectively The lift and counter-torque generated by a single rotor are as follows:

[0060]

[0061] in, The lift coefficient, This is the torque coefficient.

[0062] Define the control input vector as follows:

[0063] in, For total lift, For rolling torque, For pitching moment, This is the yaw moment. For the quadcopter configuration used in this embodiment, we have:

[0064]

[0065]

[0066]

[0067]

[0068] in, This is the length of the rotating arm.

[0069] Control input vector With the square vector of the four rotor speeds Satisfy the control allocation relationship:

[0070]

[0071] In actual execution, the control and allocation module determines the allocation method based on the obtained... The target speed of each motor is calculated and then converted into the corresponding ESC PWM drive signal.

[0072] In the inertial coordinate system, the translational motion equations of the quadrotor UAV are:

[0073] in, For position vectors, For the quality of drones, For gravity, For the expression of total thrust in an inertial frame, The driving force is wind disturbance.

[0074] In the body coordinate system, the rotational dynamics equation is:

[0075] in, , Here is the moment of inertia matrix.

[0076] The Euler rate of change and the system angular velocity satisfy the standard attitude kinematic transformation relationship:

[0077]

[0078] in, Let be the Euler angle kinematic transformation matrix.

[0079] This embodiment uses the Dryden turbulence model to generate wind disturbance dynamics. It should be noted that the Dryden turbulence model is only a preferred disturbance modeling method, and the present invention is also applicable to other strong interference scenarios such as time-varying wind field disturbances and measurement noise enhancement.

[0080] Define the state vector as follows:

[0081] in, Let be the linear velocity component. Then the continuous-time state-space model can be written as:

[0082]

[0083] in, It is a continuous-time nonlinear state function. This refers to noise in a continuous time process.

[0084] The continuous-time model is discretized using the fourth-order Runge-Kutta method, with the sampling period set as follows:

[0085]

[0086] For the state equation Its recursive formula is:

[0087]

[0088]

[0089]

[0090]

[0091]

[0092] This leads to the nonlinear discrete state-space model:

[0093]

[0094]

[0095] in, For discrete state transition functions, For measurement functions, , , The process noise covariance matrix is... This is for measuring the noise covariance matrix. As an example, the discretized process noise covariance matrix... Based on the continuous-time process noise covariance matrix and sampling period Through numerical integration or approximation (e.g.) )get.

[0096] In this embodiment, assuming that the position and attitude angles can be directly measured, the measurement function is taken as:

[0097]

[0098] Accordingly, the Jacobian matrix of the measurement function is defined as:

[0099] Since the measurement function in this embodiment is of linear selection form, the measurement Jacobian matrix is ​​a constant selection matrix, denoted as: ;

[0100] in, It is a 6th order identity matrix. for Zero matrix.

[0101] In this embodiment, the physical parameters of the UAV are set as follows:

[0102] ,

[0103] ,

[0104] ,

[0105] ,

[0106]

[0107] ,

[0108]

[0109] The process noise covariance matrix is ​​set as follows:

[0110]

[0111] The measurement noise covariance matrix is ​​set as follows:

[0112] Establish the NMPC objective cost function based on the trajectory tracking task:

[0113]

[0114] in, To predict the time domain, To control the time domain and satisfy ;

[0115] For the first Step-by-step prediction output, As the reference trajectory vector, For the first Predictive control input, This is the state tracking error weight matrix. To control the input weight matrix; and , .

[0116] when At that time, Within the range, the control input remains the last optimized control variable, i.e. .

[0117] In this embodiment, the prediction time domain is: Control time domain: In this embodiment, the control time domain and the prediction time domain are the same. In other embodiments, the control time domain... It can be taken as less than the prediction time domain. The value of is adjusted to reduce the computational load of online optimization.

[0118] The weight matrix is ​​taken as follows:

[0119]

[0120]

[0121] The control input constraints include amplitude constraints, which must satisfy:

[0122] ;

[0123] in, , ; To predict the time domain, To control the time domain and satisfy .when At that time, ,satisfy: ;

[0124] That is, within the prediction interval following the control time domain, the control input remains the last optimized control variable. In some implementations, the control input amplitude constraint can be determined jointly by the rotor speed range, power supply voltage limit, and actuator drive capability.

[0125] In other implementations, a terminal state penalty term may be added to the NMPC objective cost function, and / or terminal state constraints may be set to further improve closed-loop stability.

[0126] S2: Initialize the parameters of the Particle Swarm Optimization (PSO) algorithm, the Strong Tracking Extended Kalman Filter (STEKF) algorithm, and the NMPC algorithm, and initialize the posterior state estimates, posterior covariance matrix, and residual covariance matrix estimates.

[0127] The particle swarm optimization algorithm parameters are set as follows: number of particles Maximum number of iterations Learning factor

[0128] The inertia weight adopts a linear decreasing strategy:

[0129] in, For the first The inertia weight in the next iteration, here This indicates the number of PSO iterations.

[0130] The search interval for the fading factor is: The particle velocity range is: ;in, , .

[0131] The initial particle position and initial velocity are generated as follows:

[0132] ,

[0133] ;

[0134] in, For the first The initial position of each particle. For the first The initial velocity of each particle. For interval Uniformly distributed random numbers within. , , represent the lower and upper bounds of the search interval for the fading factor, respectively.

[0135] The initial state estimate is set as follows:

[0136] The initial posterior covariance matrix is ​​set as follows:

[0137]

[0138] The initial residual covariance estimates are set as follows:

[0139] in, Indicates the first Estimates of the residual covariance matrix at each sampling time point This represents the measurement noise covariance matrix at the initial time.

[0140] Initialize the NMPC reference trajectory. This embodiment uses a spiral reference trajectory, wherein... Representing continuous-time variables:

[0141]

[0142]

[0143]

[0144] The corresponding reference trajectory vector is taken as:

[0145] .

[0146] The latter three dimensions represent reference values ​​for roll angle, pitch angle, and yaw angle, respectively, all of which are set to zero in this embodiment.

[0147] It should be noted that the spiral reference trajectory is only an example, and the present invention is also applicable to various reference trajectories such as circles and straight lines.

[0148] S3: At each sampling time Calculate the one-step predicted state estimate based on the discrete state transition function. Measurements based on the current sampling time Calculate the measurement residuals using the predicted state estimate from the first step; construct and reduce factors based on the measurement residuals and the residual covariance matrix estimate. The relevant fitness function is used, and the PSO is applied to the fading factor within a preset feasible region. Perform an online optimization search to obtain the optimal fading factor. ;

[0149] At each sampling time First, the predicted state is calculated based on the discrete state transition function:

[0150]

[0151] Simultaneously calculate the Jacobian matrix of the state function:

[0152] ;

[0153] in, Discrete state transition function For state variables The Jacobian matrix, and in , Calculation at point.

[0154] This embodiment uses numerical differentiation to calculate. The perturbation step size is:

[0155]

[0156] The measurement residual is:

[0157]

[0158] in, This represents the measurement residual at the current sampling time.

[0159] Online recursive estimation of the residual covariance matrix based on the forgetting factor:

[0160] ;

[0161] in, As the forgetting factor, in this embodiment, we take: =0.95;

[0162] The above recursive method weights and fuses historical and current residual information, so that the estimated residual covariance matrix can better reflect the current interference environment and measurement changes.

[0163] To enhance the tracking capability of the strong tracking extended Kalman filter for strong disturbances and state abrupt changes, this embodiment constructs a consistency fitness function between the residual covariance and the theoretical innovation covariance based on the requirement that the residual sequence approximately satisfies the orthogonality principle. This function is used to optimize the fading factor online and improve the filter's response capability to strong disturbances and state abrupt changes.

[0164] Construction and fading factor Relevant fitness functions:

[0165]

[0166] in, For measurement function Jacobian matrix, To measure the noise covariance matrix, Denotes the Frobenius norm. To the fading factor The relevant one-step prediction of the covariance matrix satisfies:

[0167]

[0168] Therefore, the online optimization problem of the fading factor can be expressed as:

[0169]

[0170] In this embodiment, the position variable of each particle in PSO In a physical sense, it represents the fading factor. One of the candidate values ​​is:

[0171] Using PSO Perform online optimization. For the first... For each particle, the velocity and position update formulas are as follows:

[0172]

[0173]

[0174] in, For the first The particle in the first Speed ​​at the next iteration For location, For the optimal position of an individual, To be the globally optimal position For interval Random numbers within.

[0175] Apply boundary constraints to the updated particle velocities to keep them within the feasible region of particle velocities. Inside. Can be represented as:

[0176]

[0177] After each position update, a boundary constraint is applied to the particle position to keep it within the feasible region of the fading factor. The iteration termination condition is reaching the maximum number of iterations, or the change in the fitness function value over 5 consecutive iterations being less than [a certain value]. .

[0178] After the iteration is complete, output the optimal fading factor. .

[0179] S4: The optimal fading factor The STEKF one-step prediction covariance matrix update process is introduced to obtain the one-step prediction covariance matrix. Based on the updated one-step prediction covariance matrix, the current state of the UAV is predicted and the measurements are updated to obtain the posterior state estimate. ;

[0180] Will Substituting the one-step prediction covariance matrix, we get:

[0181]

[0182] Calculate the Kalman gain:

[0183]

[0184] Status update complete:

[0185]

[0186] Update the posterior covariance matrix:

[0187]

[0188] in, The identity matrix is ​​adapted to the dimension of the state.

[0189] This yields the posterior state estimate at the current sampling time. The posterior state estimate serves as the initial state of the NMPC optimization problem at the current sampling time.

[0190] S5: The posterior state estimate is... As the initial state of the NMPC optimization problem at the current sampling time, based on the nonlinear discrete state-space model, the objective cost function, and the control input constraints, the nonlinear programming problem in the prediction time domain is solved to obtain the optimal control input sequence.

[0191] The posterior state estimate obtained in step S4 is used as the initial state of the NMPC optimization problem at the current sampling time. That is, the initial state at the current optimization time is taken as: .

[0192] Based on this, a finite-time nonlinear programming problem is constructed:

[0193] ;

[0194] The constraints are:

[0195] ,

[0196] ,

[0197] ;

[0198] when At that time, have .

[0199] In this embodiment, the Sequential Quadratic Programming (SQP) algorithm is used to solve the nonlinear programming problem. After obtaining the optimal control input sequence, only the first control input is used. The control input is applied to the drone. The control distribution module then processes the control input according to the given control input. Calculate the target rotational speed of the four rotors and drive the corresponding motors to perform the operation.

[0200] S6: Apply the first control input in the optimal control input sequence to the UAV, and repeat steps S3 to S5 in the rolling time domain framework until the trajectory tracking task is completed;

[0201] Under the rolling time-domain framework, steps S3 to S6 are repeated at each sampling time until the trajectory tracking task is completed.

[0202] In a preferred verification method, the total runtime can be set to The sampling period is set to A spiral trajectory is used as a reference trajectory. To verify the effectiveness of the method of this invention, comparative simulations can be performed using the pure NMPC method, the NMPC+STEKF method, and the NMPC+PSO-STEKF method.

[0203] like Figure 5 The figure shows a comparison of the position errors of the UAV under a spiral reference trajectory. The comparison results show that, compared with the pure NMPC method and the NMPC+STEKF method, the NMPC+PSO-STEKF method described in this invention has smaller position tracking errors and faster convergence.

[0204] like Figure 6 The figure shows a comparison of attitude errors of the UAV under a spiral reference trajectory. The comparison results show that the NMPC+PSO-STEKF method described in this invention can further reduce the tracking errors of roll angle, pitch angle and yaw angle, and enhance the system's attitude maintenance capability in strong interference environments.

[0205] Under the above conditions, PSO-STEKF performs a state estimation at each sampling time, and NMPC performs a finite-time domain optimization at each sampling time, applying the first control input from the optimal control input sequence to the UAV. Therefore, the method described in this embodiment can improve the accuracy of state estimation and trajectory tracking performance under strong interference environments, and enhance the system's robustness to wind disturbance and measurement noise.

[0206] Example 2

[0207] This embodiment provides a drone trajectory tracking and control system for implementing the method described in Embodiment 1, such as... Figure 4 As shown, the system includes the UAV body, sensor module, airborne flight control computer, control distribution module, and actuator module.

[0208] The drone body is preferably a quadcopter drone. It should be noted that the quadcopter drone is only a preferred embodiment of this invention, and the drone trajectory tracking control system is also applicable to other drone platforms with nonlinear dynamic characteristics.

[0209] The sensor module is used to collect flight status information of the UAV. The flight status information includes at least position, attitude angles, and measurement information related to state estimation. Preferably, the sensor module includes an inertial measurement unit (IMU), a global positioning module (GPS), and an attitude sensor, used to acquire the UAV's position, attitude angles, angular velocity, and / or acceleration information.

[0210] The airborne flight control computer is communicatively connected to the sensor module, and is used to receive the flight status information and execute the UAV trajectory tracking control method based on PSO-STEKF and NMPC under strong interference environment described in Embodiment 1, so as to output control input. The airborne flight control computer may include a processor and a memory, the memory storing computer program instructions, and the processor calling the computer program instructions to achieve the following functions:

[0211] A nonlinear dynamic model of the UAV is constructed, and the nonlinear dynamic model is discretized to obtain a nonlinear discrete state-space model; based on the trajectory tracking task, a nonlinear model is established to predict the target cost function and control input constraints of the NMPC.

[0212] Initialize the parameters of the Particle Swarm Optimization (PSO) algorithm, the Strong Tracking Extended Kalman Filter (STEKF) algorithm, and the NMPC algorithm, and initialize the posterior state estimates, posterior covariance matrix, and residual covariance matrix estimates.

[0213] At each sampling time, a one-step predicted state estimate is calculated based on the discrete state transition function; the measurement residual is calculated based on the measurement value at the current sampling time and the one-step predicted state estimate; a fitness function related to the fading factor is constructed based on the measurement residual and the residual covariance matrix estimate, and the PSO is used to perform online optimization search on the fading factor within a preset feasible region to obtain the optimal fading factor;

[0214] The optimal fading factor is introduced into the one-step prediction covariance matrix update process of the STEKF to obtain the one-step prediction covariance matrix. Based on the updated one-step prediction covariance matrix, the current state of the UAV is predicted and the measurement is updated to obtain the posterior state estimate.

[0215] Using the posterior state estimate as the initial state of the NMPC optimization problem at the current sampling time, and based on the nonlinear discrete state-space model, the objective cost function, and the control input constraints, the nonlinear programming problem in the prediction time domain is solved to obtain the optimal control input sequence.

[0216] The first control input in the optimal control input sequence is output to the control allocation module, and state estimation and predictive control are performed cyclically in the rolling time domain framework to complete the trajectory tracking task.

[0217] In some embodiments, the airborne flight control computer may further include a model building and discretization unit, a parameter initialization unit, an online optimization unit for fading factors, a state estimation unit, and a predictive control solution unit. These units can be implemented using hardware circuits, software functional modules, or a combination of hardware and software.

[0218] The control allocation module is connected to the airborne flight control computer and is used to calculate the target rotational speed of each rotor according to the control input vector output by the airborne flight control computer, and generate the corresponding drive allocation result.

[0219] The actuator module is connected to the control allocation module and is used to drive the UAV to perform flight maneuvers according to the drive allocation result. Preferably, the actuator module includes an electronic speed controller and rotor motors. The electronic speed controller controls the operation of each rotor motor according to the drive signal corresponding to the target rotation speed, thereby realizing the attitude adjustment and trajectory tracking control of the UAV.

[0220] In this embodiment, the sensor module, the airborne flight control computer, the control allocation module, and the actuator module sequentially constitute a closed-loop control link. The measurement information output by the sensor module serves as the input to the PSO-STEKF, the posterior state estimate output by the PSO-STEKF serves as the current initial state of the NMPC optimizer, and the first control input in the optimal control input sequence output by the NMPC is applied to the UAV body via the control allocation module and the actuator module to achieve closed-loop trajectory tracking control under strong interference environments.

[0221] The UAV trajectory tracking control system provided in this embodiment integrates the PSO online optimization mechanism, the STEKF state estimation mechanism, and the NMPC rolling optimization control mechanism, which can improve the state estimation accuracy and trajectory tracking performance under strong interference environment, and enhance the system's robustness to wind disturbance and measurement noise.

[0222] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit them. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can still be made to the technical solutions of the present invention. For example, the method can be applied to UAVs of other configurations such as fixed-wing and compound-wing, or the PSO optimizer can be replaced with other swarm intelligence optimization algorithms (such as genetic algorithms, differential evolution algorithms, etc.) and applied to the online optimization of fading factors. However, these modifications or equivalent substitutions shall not cause the modified technical solutions to deviate from the spirit and scope of the technical solutions of the present invention.

Claims

1. A method for trajectory tracking control of unmanned aerial vehicle (UAV) in strong interference environment based on PSO-STEKF and NMPC, characterized in that, Includes the following steps: S1. Construct a nonlinear dynamic model of the UAV and discretize the nonlinear dynamic model to obtain a nonlinear discrete state-space model; establish the target cost function and control input constraints of the nonlinear model predictive control NMPC based on the trajectory tracking task. S2. Initialize the parameters of the Particle Swarm Optimization Algorithm (PSO), the Strong Tracking Extended Kalman Filter (STEKF), and the NMPC, and initialize the posterior state estimate, posterior covariance matrix, and residual covariance matrix estimate. S3, at each sampling time Calculate the one-step predicted state estimate based on the discrete state transition function. Measurements based on the current sampling time Calculate the measurement residuals using the predicted state estimate from the first step: ； Based on the measurement residual and the estimated value of the residual covariance matrix Construction and fading factor Relevant fitness functions: ； in, Let Jacobian matrix be the measurement function. To measure the noise covariance matrix, Represent the Frobenius norm; use the PSO to evaluate the fading factor within a preset feasible region. Perform an online optimization search to obtain the optimal fading factor. ; S4. The optimal fading factor The STEKF one-step prediction covariance matrix update process is introduced to obtain the one-step prediction covariance matrix. Based on the updated one-step prediction covariance matrix, the current state of the UAV is predicted and the measurements are updated to obtain the posterior state estimate. ; S5. The posterior state estimate is... As the initial state of the NMPC optimization problem at the current sampling time, based on the nonlinear discrete state-space model, the objective cost function, and the control input constraints, the nonlinear programming problem in the prediction time domain is solved to obtain the optimal control input sequence. S6. Apply the first control input in the optimal control input sequence to the UAV, and repeat steps S3 to S5 in the rolling time domain framework until the trajectory tracking task is completed.

2. The method according to claim 1, characterized in that, The residual covariance matrix estimate in step S3 Obtained through an online recursive method based on the forgetting factor, satisfying: ； in, It is a forgetting factor, and ; For the first Measurement residuals at each sampling time.

3. The method according to claim 1 or 2, characterized in that, The one-step prediction covariance matrix described in step S4 satisfies: ； in, Let Jacobian matrix be the state function. Let be the posterior covariance matrix at the previous sampling time. Let be the process noise covariance matrix.

4. The method according to claim 1, characterized in that, The position variable of each particle in the PSO represents the fading factor. The candidate values ​​are determined, and a boundary constraint is applied to the particle position after each position update to keep the particle position within the preset feasible region of the fading factor.

5. The method according to claim 4, characterized in that, The PSO in the first The velocity and position of each particle are updated according to the following formula: ， ； in, For the first The particle in the first Position at the next iteration For the corresponding speed, For the optimal position of an individual, To be the globally optimal position For inertial weights, and As a learning factor, and For interval Random numbers within.

6. The method according to claim 5, characterized in that, The inertial weight A linear decreasing strategy is adopted, and the iteration termination condition of the PSO is to reach a preset maximum number of iterations, or the change of the fitness function value in a number of consecutive iterations is less than a preset threshold.

7. The method according to claim 1, characterized in that, The objective cost function of NMPC described in step S1 satisfies: ； in, To predict the time domain, For the first Step-by-step prediction output, As a reference trajectory signal, For the first Predictive control input, This is the state tracking error weight matrix. To control the input weight matrix.

8. The method according to claim 1 or 7, characterized in that, The control input constraints include control input amplitude constraints, which satisfy: ； in, and These are the lower and upper bound vectors controlling the input, respectively. To predict the time domain, To control the time domain, the vector inequality states that it holds true for every component of the vector and satisfies the following condition. ; when At that time, ,satisfy: ; Furthermore, at each sampling time, only the first control input in the optimal control input sequence is applied to the UAV.

9. The method according to claim 1, characterized in that, In step S1, the nonlinear dynamic model is discretized using the fourth-order Runge-Kutta method; the UAV is a quadcopter UAV, and its state vector includes at least position, attitude angle, linear velocity and system angular velocity.

10. The method according to claim 1, characterized in that, The strong interference environment includes at least one of the following: Dryden turbulence, time-varying wind field disturbance, and enhanced measurement noise; the measurement function The corresponding measurements include at least the position and attitude angles of the UAV.

Images