Trajectory tracking control method for quadrotor unmanned aerial vehicle based on nmpc

Abstract

This invention relates to the field of unmanned aerial vehicle (UAV) control technology and includes the following steps: S1, establishing a discrete nonlinear dynamic model with disturbances based on quaternions; S2, designing a lightweight nonlinear model predictive controller based on real-time iteration of sequential quadratic programming; S3, constructing a lightweight MLP disturbance observer to achieve real-time estimation of six-dimensional disturbances; and S4, designing a position-torque dual-layer feedforward compensation strategy. This invention significantly reduces the online computation of NMPC by combining the SQP_RTI real-time iterative framework with a hot-start and matrix pre-storage mechanism, enabling millisecond-level real-time operation on the airborne platform. It employs a 26-dimensional feature fusion MLP network to achieve joint estimation of disturbance forces and torques, eliminating dependence on precise model parameters. The dual-layer separate feedforward design, which corrects the reference trajectory at the position layer and directly compensates for motor speed at the torque layer, avoids coupling interference from a single compensation method, achieving high-precision trajectory tracking even under external disturbances such as wind fields and sudden load changes.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle control technology, specifically a trajectory tracking control method for quadcopter UAVs based on NMPC. Background Technology

[0002] With the rapid development of the low-altitude economy, quadcopter drones, with their advantages of simple structure, high maneuverability, and low cost, have been widely used in fields such as power line inspection, surveying and mapping, logistics distribution, and emergency rescue. Trajectory tracking control, as the core technology of autonomous drone flight, directly determines the quality of operations and flight safety through its tracking accuracy, response speed, and robustness. In actual flight environments, quadcopter drones exhibit strong coupling, underactuated, and nonlinear dynamic characteristics, making them susceptible to uncertainties such as gusts, load changes, sensor noise, and model parameter perturbations, leading to decreased trajectory tracking accuracy and attitude instability. Furthermore, drone control algorithms are typically deployed on embedded flight control platforms such as Pixhawk, which have limited computing resources and storage capacity, placing stringent requirements on the real-time performance and lightweight design of the algorithms.

[0003] Model predictive control (MPC) has become the mainstream solution for high-precision trajectory tracking of UAVs due to its ability to explicitly handle constraints, rolling optimization, and outstanding multivariate coordination capabilities. Nonlinear model predictive control (NMPC) employs nonlinear dynamics modeling, offering superior tracking accuracy and adaptability compared to linear MPC. However, it requires repeated online solutions to nonlinear programming (NLP) problems, resulting in high computational complexity and time consumption. This makes it difficult to achieve high-frequency real-time control on embedded platforms, and control lag can easily lead to trajectory drift and oscillation. To improve the real-time performance of NMPC, existing technologies often employ a real-time iterative strategy (RTI-NMPC), transforming nonlinear programming into a single quadratic programming (QP) iteration based on the SQP_RTI framework, thus reducing solution time to some extent. However, this method still has shortcomings: first, the predictive model does not consider the impact of unknown disturbances, leading to a significant decrease in tracking accuracy under complex conditions such as gusts and variable loads; second, the Jacobian matrix and constraint matrix are repeatedly calculated online, resulting in redundant overhead; and third, it does not combine disturbance observation and feedforward compensation, making its disturbance resistance capability dependent on model accuracy and lacking robustness.

[0004] In terms of disturbance suppression, methods such as Extended State Observer (ESO) and Kalman filtering have been used for disturbance estimation and compensation. However, these model-driven observers rely on precise dynamic equations and have limited ability to fit nonlinear, time-varying, and strongly coupled disturbances. Although machine learning-based disturbance observation methods have strong nonlinear approximation capabilities, existing solutions generally suffer from complex network structures, large number of parameters, high inference time, and difficulty in embedded deployment, making them unable to be efficiently integrated with lightweight NMPC. In addition, existing control methods that integrate NMPC and neural networks often directly embed disturbances into the optimization problem, leading to an increase in decision variables and constraint complexity, further increasing the computational burden. Some methods use offline training and online table lookup, which improves speed but has weak generalization ability and poor dynamic adaptability, making it difficult to meet the complex and ever-changing actual flight requirements.

[0005] In summary, current trajectory tracking control for quadrotor UAVs faces three major bottlenecks: first, traditional online NMPC solutions are computationally intensive, making them unsuitable for embedded high-frequency real-time control; second, model-driven disturbance observers lack sufficient accuracy in estimating complex nonlinear disturbances; and third, there is a lack of integrated control schemes that efficiently combine lightweight NMPC with data-driven disturbance observation, balancing real-time performance, accuracy, and robustness. Therefore, researching a lightweight, high-frequency, real-time, and highly disturbance-resistant NMPC trajectory tracking control method is of significant engineering value for improving the autonomous operation capabilities of UAVs in complex environments. Summary of the Invention

[0006] To address the technical challenges in existing quadrotor UAV trajectory tracking control, such as the large computational load of traditional nonlinear model predictive control online solution, insufficient real-time performance of embedded platforms, low estimation accuracy of model-driven disturbance observers for complex nonlinear disturbances, and the inability of control algorithms to simultaneously achieve high-frequency real-time performance, high-precision tracking, and strong disturbance rejection robustness, this invention proposes a quadrotor UAV trajectory tracking control method based on NMPC.

[0007] The trajectory tracking control method for quadrotor UAVs based on NMPC includes the following steps:

[0008] S1. Establish a discrete nonlinear dynamic model with disturbance based on quaternions, define the system state vector, control input vector and time-varying disturbance vector, construct the continuous nonlinear dynamic equation with disturbance assignment matrix, and discretize it to obtain a discrete prediction model.

[0009] S2. A lightweight nonlinear model predictive controller is designed based on a discrete predictive model. The optimal control problem of trajectory tracking is transformed into a quadratic programming subproblem by adopting a real-time iterative framework of sequential quadratic programming. The online solution is accelerated by a hot-start strategy and a matrix pre-storage mechanism, and the optimal control input sequence is output.

[0010] S3. Construct a fully connected MLP network as a lightweight perturbation observer. The input feature vector is used as the network input. After being processed layer by layer by the feature transformation layer, the first hidden layer, the second hidden layer and the Dropout regularization layer, the perturbation estimation vector is output by the output layer to realize the real-time estimation of perturbation force and perturbation moment.

[0011] S4. Perform position layer feedforward compensation and torque layer feedforward compensation respectively based on the disturbance estimation vector: The position layer calculates the position compensation amount to correct the reference trajectory position based on the estimated value of the three-axis disturbance force components, and the torque layer calculates the motor speed compensation amount through the inverse mapping of the mixing matrix based on the estimated value of the three-axis disturbance torque components; the corrected reference trajectory and the compensated motor speed are superimposed, and the final control command is output to complete the closed-loop tracking.

[0012] Further, step S1 includes the following steps:

[0013] S1.1, Define system variables:

[0014] Define the system state vector In the formula, For the three-dimensional position in the ground coordinate system, As a unit quaternion pose, For the velocity in the body coordinate system, Angular velocity in the body coordinate system;

[0015] Define control input vector In the formula, Each corresponds to one of the four motors of the quadcopter at a specific time. The rotational speed;

[0016] Define time-varying perturbation vector In the formula, These represent the actions acting on the body coordinate system. External disturbance components of the three axes These represent the actions acting on the body coordinate system. Three-axis disturbance torque components;

[0017] S1.2 Constructing a continuous dynamic model:

[0018] By combining the Newton-Euler equations with quaternion attitude transformation, a continuous nonlinear dynamic model with perturbations is constructed. In the formula, This is the perturbation distribution matrix, used to map the six-dimensional external perturbation to each state channel;

[0019] S1.3 Discretization process:

[0020] The continuous nonlinear dynamic model is discretized and numerically integrated using the explicit fourth-order Runge-Kutta method, with the control period set as . The discrete prediction model is obtained as follows:

[0021]

[0022] In the formula, This is the index for discrete time steps.

[0023] Further, step S2 includes the following steps:

[0024] S2.1 Constructing the optimal control problem for trajectory tracking:

[0025] Set the prediction time domain Control Time Domain Predicted total duration The objective function is constructed with the goal of minimizing the state tracking error and maximizing the smoothness of the control input in the prediction time domain. The constraints of the optimization problem include state boundary constraints, input boundary constraints, and equality constraints of the discrete prediction model obtained in step S1.

[0026] S2.2, Discretization of Nonlinear Programming:

[0027] The optimal control problem is transformed into a standard nonlinear programming problem using the direct multiple-shot method, and the prediction time domain is decomposed into... Each discrete time step;

[0028] S2.3, SQP_RTI Real-time Iterative Solution:

[0029] A real-time iterative framework of sequential quadratic programming is adopted, in which the quadratic programming subproblem is solved only once in each control cycle, and the interior point method solver of HPIPM is called to achieve the solution;

[0030] S2.4, Hot Start Acceleration Strategy:

[0031] The optimal control sequence and state trajectory obtained from the previous control cycle are shifted over time and used as the initial iteration values ​​for the current cycle optimization problem.

[0032] S2.5, Matrix pre-storage mechanism:

[0033] Offline pre-computes the sparse structure, memory space, and index information of the Jacobian matrix, Hessian matrix, and constraint matrix. In the online phase, only the matrix values ​​are updated, and the matrix structure is not reconstructed repeatedly.

[0034] Furthermore, the objective function in step S2.1 is:

[0035]

[0036] In the formula, This is the discrete time step index, with a value range of [value range missing]. Q, R, and P are the state weight matrix, input weight matrix, and terminal weight matrix, respectively. For the first The state tracking error of the step, where To predict the state vector, The reference state vector; For the first The control input error of the step, where To control the input vector, This is the reference input vector.

[0037] Furthermore, in step S3, the expression for the input feature vector is:

[0038]

[0039] In the formula, These represent the 13-dimensional system state, 3-dimensional linear acceleration estimation, 3-dimensional angular acceleration estimation, 3-dimensional reference trajectory, and 4-dimensional control input, respectively.

[0040] Furthermore, in step S3, the configuration of each layer of the fully connected MLP network is as follows:

[0041] Input layer: Receives a 26-dimensional normalized feature vector, with 26 neurons;

[0042] Feature transformation layer: Contains 128 neurons and uses the ReLU activation function to map the original high-dimensional input to a high-dimensional latent space, completing feature extraction and nonlinear mapping;

[0043] The first hidden layer contains 64 neurons and uses the ReLU activation function to further extract abstract features from the middle layer;

[0044] The second hidden layer contains 32 neurons and uses the ReLU activation function to compress the feature dimension while preserving key perturbation patterns.

[0045] Dropout regularization layer: The inactivation rate is set to 0.2, randomly dropping some neuron connections to suppress the risk of overfitting;

[0046] Output layer: Contains 6 neurons, uses a linear activation function, and directly outputs the six-dimensional perturbation estimate. In the formula, These are the estimated values ​​of the three-axis disturbance force components, respectively. These are the estimated values ​​of the three-axis disturbance torque components.

[0047] Further, in step S3, the MLP network is trained using a weighted mean square error loss function, the expression of which is:

[0048]

[0049] In the formula, This represents the set of all learnable parameters in an MLP network; The total number of training samples; For training sample index; The loss weighting coefficient for the disturbance force component; This is the loss weighting coefficient for the disturbance torque component; and The first The network-predicted perturbation vector and the actual perturbation label vector corresponding to each training sample; and The first The network predicted perturbation moment vector and the actual perturbation moment label vector corresponding to each training sample.

[0050] Furthermore, the location layer feedforward compensation described in step S4 specifically includes:

[0051] Calculate the position compensation amount to address the translational trajectory deviation caused by the disturbance force: In the formula, This is the position compensation gain matrix. For the quality of drones, This is the estimated vector of the three-axis perturbation force components output by the MLP perturbation observer;

[0052] Corrected reference location: In the formula, The original reference trajectory position vector;

[0053] The lightweight nonlinear model predictive controller constructed in step S2 uses the corrected reference trajectory To track the target and mitigate the effects of disturbances in advance.

[0054] Furthermore, the torque layer feedforward compensation described in step S4 specifically includes:

[0055] To compensate for attitude fluctuations caused by disturbance torque, the motor speed is adjusted.

[0056] Construct the torque compensation vector: In the formula, The outputs are respectively Three-axis disturbance torque component estimates;

[0057] Through the hybrid control matrix The motor speed compensation amount is obtained by inverse mapping:

[0058]

[0059] In the formula, This is the motor thrust coefficient. This is the reference rotational speed of a single motor in the hovering state of the drone;

[0060] Final control input: ,in, The optimal control sequence output by the lightweight nonlinear model predictive controller constructed in step S2 is in the... The value of the step.

[0061] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0062] 1. Breakthrough improvement in real-time performance: The average solution time is reduced and the control frequency is increased, fully meeting the high-frequency real-time control requirements of embedded flight control platforms such as Pixhawk and PX4.

[0063] 2. High trajectory tracking accuracy: The trajectory tracking error is stably controlled throughout the entire cycle, the z-axis error is reduced, and the attitude angle fluctuation amplitude is reduced.

[0064] 3. Strong anti-disturbance robustness: It can effectively suppress complex disturbances such as gusts, variable loads, sensor noise, and model parameter perturbations, and there is no trajectory drift or attitude oscillation under strong disturbance conditions.

[0065] 4. Extreme Embedded Adaptation: Both the MLP network and the LiNMPC controller are lightweight designs with small parameters and low computational overhead, and can be directly ported to PX4 firmware.

[0066] 5. High flight safety: The control output is smooth and vibration-free, and the motor speed is always limited within the safe range, eliminating the risk of instability due to over-range operation.

[0067] 6. Wide applicability: It can be directly applied to scenarios such as power line inspection, logistics distribution, and emergency rescue, and can also be extended to other multi-rotor drones and small mobile robot platforms. Attached Figure Description

[0068] Figure 1 This is a diagram of the overall control architecture of the quadcopter UAV trajectory tracking control method of the present invention.

[0069] Figure 2 This is a flowchart of the lightweight solution process for LiNMPC in this invention.

[0070] Figure 3 This is a network structure diagram of the MLP perturbation observer of the present invention.

[0071] Figure 4 This is a schematic diagram of the hierarchical feedforward compensation principle of the present invention.

[0072] Figure 5 This is a schematic diagram of the hardware-in-the-loop experimental platform according to an embodiment of the present invention.

[0073] Figure 6 This is a 3D trajectory output diagram of an embodiment of the present invention.

[0074] Figure 7 This is a schematic diagram of the actuator (motor) output according to an embodiment of the present invention.

[0075] Figure 8 This is a schematic diagram of the position channel output according to an embodiment of the present invention.

[0076] Figure 9 This is a schematic diagram of attitude output according to an embodiment of the present invention. Detailed Implementation

[0077] To better understand the above-mentioned objectives, features, and advantages of the present invention, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. It should be noted that, unless otherwise specified, the embodiments and features described in these embodiments can be combined with each other.

[0078] This invention proposes a trajectory tracking control method for quadrotor UAVs based on NMPC. It is implemented through core technologies such as perturbation quaternion dynamics modeling, lightweight nonlinear model predictive control (LiNMPC) design, MLP perturbation observer construction, hierarchical feedforward compensation, and closed-loop real-time control. The overall process is described in [reference needed]. Figure 1 The specific technical solution is as follows.

[0079] S1. Establish a discrete nonlinear dynamic model with perturbation based on quaternions.

[0080] Combination Figure 1 To avoid the singularity problem in Euler angle attitude description and to adapt to the high-frequency calculation requirements of embedded platforms, this invention uses quaternions to construct a complete dynamic model of the UAV and explicitly introduces external disturbance terms to provide an accurate model foundation for subsequent predictive control and disturbance observation.

[0081] S1.1 Define system variables.

[0082] Define a 13-dimensional system state vector: In the formula, For the three-dimensional position in the ground coordinate system, As a unit quaternion pose, For the velocity in the body coordinate system, Angular velocity in the body coordinate system;

[0083] Define a 4-dimensional control input vector: In the formula, Each corresponds to one of the four motors of the quadcopter at a specific time. The rotational speed;

[0084] Define a 6-dimensional time-varying perturbation vector: In the formula, These represent the actions acting on the body coordinate system. External disturbance components of the three axes These represent the actions acting on the body coordinate system. The disturbance torque components of the three axes; the disturbance satisfies the bounded, slow time-varying characteristics.

[0085] S1.2 Construct a continuous dynamics model.

[0086] By combining the Newton-Euler equations with quaternion attitude transformation, a continuous nonlinear dynamic model with perturbations is constructed: In the formula, This is the perturbation distribution matrix, used to map six-dimensional external perturbations to each state channel.

[0087] S1.3 Discretization process.

[0088] The above continuous model is discretized and numerically integrated using the explicit fourth-order Runge-Kutta method (RK4), with the control period set as . The discrete prediction model is obtained as follows: In the formula, This provides an index for discrete time steps; the model offers accurate state predictions for lightweight NMPC.

[0089] S2. Design a lightweight nonlinear model predictive controller (LiNMPC) based on real-time iteration of sequential quadratic programming (SQP_RTI).

[0090] This invention, based on the Real-Time Iterative NMPC (RTI-NMPC) framework, employs a dual lightweight optimization strategy of warm-start and matrix pre-storage mechanism. Without compromising control accuracy, it reduces the online solution time to a level acceptable for embedded platforms, achieving high-frequency real-time control. The process is as follows: Figure 2 As shown.

[0091] S2.1 Constructing the optimal control problem for trajectory tracking.

[0092] Set the prediction time domain Control Time Domain Predicted total duration The objective function is constructed with the optimization objectives of minimizing the state tracking error in the prediction time domain and maximizing the smoothness of the control input: In the formula, This is the discrete time step index, with a value range of [value range missing]. Q, R, and P are the state weight matrix, input weight matrix, and terminal weight matrix, respectively. For the first The state tracking error of the step, where To predict the state vector, The reference state vector; For the first The control input error of the step, where To control the input vector, The reference input vector;

[0093] The constraints of the optimization problem include: state boundary constraints (position, attitude, velocity, angular velocity), input boundary constraints (safe range of motor speed), and discrete dynamic equation constraints obtained in step S1.

[0094] S2.2 Discretization of Nonlinear Programming (NLP).

[0095] The optimal control problem is transformed into a standard NLP problem using the Direct Multiple Targets (DMS) method, which decomposes the prediction time domain into... Using discrete time steps and the discrete dynamic equations obtained in step S1 as equality constraints, the complexity of solving the optimization problem is greatly reduced.

[0096] S2.3 and SQP_RTI provide real-time iterative solutions.

[0097] The system employs a sequential quadratic programming real-time iteration (SQP_RTI) framework, which executes the quadratic programming (QP) subproblem solution only once per control cycle and calls the HPIPM high-performance interior-point solver to achieve millisecond-level fast solution.

[0098] S2.4, Hot Start Acceleration Strategy.

[0099] The optimal control sequence and state trajectory obtained from the previous control cycle are shifted over time and used as the initial iteration values ​​for the current cycle optimization problem. This makes the linearization point closer to the true optimal solution, reduces the number of QP iterations, and improves the convergence speed.

[0100] S2.5, Matrix pre-storage mechanism.

[0101] The sparse structure, memory space, and index information of the Jacobian matrix, Hessian matrix, and constraint matrix are pre-computed offline. In the online stage, only the matrix values ​​are updated, and the matrix structure is not reconstructed repeatedly, thus completely eliminating redundant computational overhead.

[0102] S2.6 Optimal parameter configuration.

[0103] The optimal parameter configuration for this FAM was determined through experimentation and optimization: number of prediction steps. Predicting the time domain Control cycle This configuration balances tracking accuracy, control smoothness, and real-time performance.

[0104] S3. Construct a lightweight MLP perturbation observer to achieve real-time estimation of six-dimensional perturbations.

[0105] This invention designs a dedicated lightweight fully connected MLP network that uses measurable information from UAVs as input. It achieves high-precision real-time estimation of six-dimensional perturbation forces / torques without relying on precise dynamic models, adapting to complex nonlinear perturbation scenarios. Its network structure diagram is shown below. Figure 3 As shown.

[0106] S3.1 Design of network input feature vectors.

[0107] Using a 26-dimensional high-dimensional input feature vector: In the formula, These represent the 13-dimensional system state, 3-dimensional linear acceleration estimation, 3-dimensional angular acceleration estimation, 3-dimensional reference trajectory, and 4-dimensional control input, respectively, fully covering disturbance-related information.

[0108] S3.2, Refined design of network structure.

[0109] The network adopts a five-layer architecture, with each layer configured as follows:

[0110] Input layer: Receives a 26-dimensional normalized feature vector, with 26 neurons;

[0111] Feature transformation layer: Contains 128 neurons and uses the ReLU activation function to map the original high-dimensional input to a high-dimensional latent space, completing feature extraction and nonlinear mapping;

[0112] The first hidden layer contains 64 neurons and uses the ReLU activation function to further extract abstract features from the middle layer;

[0113] The second hidden layer contains 32 neurons and uses the ReLU activation function to compress the feature dimension while preserving key perturbation patterns.

[0114] Dropout regularization layer: The inactivation rate is set to 0.2, randomly dropping some neuron connections to suppress the risk of overfitting;

[0115] Output layer: Contains 6 neurons, uses a linear activation function, and directly outputs the six-dimensional perturbation estimate. In the formula, These are the estimated values ​​of the three-axis disturbance force components, respectively. These are the estimated values ​​of the three-axis disturbance torque components.

[0116] The network has a total of only 12,102 parameters, and the time taken for a single inference is less than 0.1 ms, which meets the requirements for embedded real-time deployment.

[0117] S3.3 Training Strategy and Loss Function.

[0118] The loss function uses weighted mean square error (WMSE), which assigns higher weights to the perturbation force and perturbation moment components, thereby enhancing the accuracy of attitude perturbation estimation. In the formula, This represents the set of all learnable parameters (weights and biases) in an MLP network; The total number of training samples; For training sample index; The loss weighting coefficient for the disturbance force component; This is the loss weighting coefficient for the disturbance torque component; and The first The network-predicted perturbation vector and the actual perturbation label vector corresponding to each training sample; and The first The network predicted perturbation moment vector and the actual perturbation moment label vector corresponding to each training sample.

[0119] The training set uses 100,000 sets of perturbation flight data generated by MuJoCo simulation. The Adam optimizer, exponentially decaying learning rate, L2 regularization and early stopping mechanism are used to ensure the network's generalization ability.

[0120] The trained MLP observer is directly embedded into the flight control firmware and operates synchronously with a fixed control cycle, outputting disturbance estimates in real time to provide data support for feedforward compensation.

[0121] S4. Design position-torque dual-layer feedforward compensation strategy.

[0122] This invention innovatively proposes a hierarchical feedforward compensation mechanism, which injects the disturbance information observed by the MLP into the position loop and the actuator loop respectively. Without increasing the complexity of NMPC optimization, it can achieve full-dimensional disturbance suppression. The flowchart of its hierarchical compensation law is shown below. Figure 4 .

[0123] S4.1, Location layer feedforward compensation.

[0124] Calculate the position compensation amount to address the translational trajectory deviation caused by the disturbance force: In the formula, This is the position compensation gain matrix. For drone quality;

[0125] Corrected reference location: In the formula, The original reference trajectory position vector;

[0126] The LiNMPC controller constructed in step S2 uses the corrected reference trajectory To track the target and mitigate the effects of disturbances in advance.

[0127] S4.2, Torque layer feedforward compensation.

[0128] To compensate for attitude fluctuations caused by disturbance torque, the motor speed is directly adjusted.

[0129] Construct the torque compensation vector: In the formula, The outputs are respectively Three-axis disturbance torque component estimates;

[0130] Through the hybrid control matrix The motor speed compensation amount is obtained by inverse mapping: In the formula, This is the motor thrust coefficient. This is the reference rotational speed of a single motor in the hovering state of the drone;

[0131] Final control input: ,in, The optimal control sequence output by the lightweight nonlinear model predictive controller constructed in step S2 is in the... The value of the step. The final control input can quickly suppress attitude disturbances and loop coupling effects.

[0132] S4.3 Compensation and Coordination Mechanism.

[0133] The position layer compensates for low-frequency translational disturbances, while the torque layer compensates for high-frequency attitude disturbances. The two layers work together to suppress disturbances across the entire frequency band, significantly improving the system's robustness.

[0134] Example:

[0135] Build a hardware simulation platform, such as Figure 5 As shown, the trajectory tracking effect of the NMPC-based quadcopter UAV trajectory tracking control method is output, such as... Figures 6-9 As shown.

[0136] Compared to the perturbation feedforward compensation method used in this invention, NDP-NMPC directly embeds perturbation information into the prediction model and constraint construction of NMPC, completing perturbation compensation and solving for the optimal control output within the online optimization problem. This method can improve the explicit handling capability of perturbations, but it also increases the coupling degree of the optimization problem and the complexity of online solution. This embodiment compares and analyzes the actual output performance of four algorithms—traditional NMPC, RTI-NMPC, NDP-NMPC, and MLP-LiNMPC (MLiNMPC)—under the same trajectory and perturbation conditions, as shown in the table below:

[0137]

[0138] Based on the data in the table above, the following conclusions can be drawn: In terms of real-time performance, MLiNMPC exhibits the best solution efficiency, with an average NMPC solution time of only 0.83 ms, a reduction of 61.4% compared to the traditional NMPC's 2.15 ms and a reduction of 46.8% compared to NDP-NMPC's 1.55 ms. Simultaneously, MLiNMPC's control frequency reaches 360.8 Hz, significantly higher than the traditional NMPC's 150.4 Hz and NDP-NMPC's 268.8 Hz, meeting the millisecond-level real-time control requirements of embedded platforms. These results demonstrate that the warm-start and matrix pre-storage mechanism employed in this invention effectively reduces online solution overhead. Furthermore, MLiNMPC employs an outer-loop hierarchical feedforward compensation method based on perturbation observation, eliminating the need to introduce additional perturbation constraint variables within the NMPC optimization problem, thus avoiding the increase in solution complexity caused by the expansion of the problem dimension.

[0139] In terms of trajectory tracking accuracy, the MLiNMPC exhibits superior performance in all three-axis position errors. Compared to traditional NMPCs, its... The average shaft position error decreased from 0.067m, 0.059m, and 0.021m to 0.060m, 0.058m, and 0.010m, respectively, corresponding to reductions of approximately 10.5%, 1.9%, and 49.3%. Meanwhile, MLiNMPC also significantly outperformed RTI-NMPC in roll angle error, reducing it from 34.782 degrees to 15.134 degrees, a reduction of approximately 56.5%.

[0140] In terms of robustness against disturbances, MLiNMPC also demonstrates a strong advantage. Under gust disturbance conditions, its maximum trajectory deviation and maximum attitude fluctuation are both at a lower level than the four algorithms, verifying that the method of this invention can more effectively suppress the impact of external disturbances on the system's translational and attitude responses.

[0141] In summary, the MLP-LiNMPC method proposed in this invention demonstrates superior overall performance in terms of real-time performance, trajectory tracking accuracy, and disturbance robustness through the synergistic effect of lightweight nonlinear model predictive control, MLP disturbance observation, and hierarchical feedforward compensation mechanism. Experimental results show that this method effectively improves the high-precision trajectory tracking capability of quadrotor UAVs under complex operating conditions without increasing optimization complexity, and has good engineering application value.

[0142] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A trajectory tracking and control method for a quadrotor UAV based on NMPC, characterized in that, Includes the following steps: S1. Establish a discrete nonlinear dynamic model with disturbance based on quaternions, define the system state vector, control input vector and time-varying disturbance vector, construct the continuous nonlinear dynamic equation with disturbance assignment matrix, and discretize it to obtain a discrete prediction model. S2. A lightweight nonlinear model predictive controller is designed based on a discrete predictive model. The optimal control problem of trajectory tracking is transformed into a quadratic programming subproblem by adopting a real-time iterative framework of sequential quadratic programming. The online solution is accelerated by a hot-start strategy and a matrix pre-storage mechanism, and the optimal control input sequence is output. S3. Construct a fully connected MLP network as a lightweight perturbation observer. The input feature vector is used as the network input. After being processed layer by layer by the feature transformation layer, the first hidden layer, the second hidden layer and the Dropout regularization layer, the perturbation estimation vector is output by the output layer to realize the real-time estimation of perturbation force and perturbation moment. S4. Perform position layer feedforward compensation and torque layer feedforward compensation respectively based on the disturbance estimation vector: The position layer calculates the position compensation amount to correct the reference trajectory position based on the estimated value of the three-axis disturbance force components, and the torque layer calculates the motor speed compensation amount through the inverse mapping of the mixing matrix based on the estimated value of the three-axis disturbance torque components; the corrected reference trajectory and the compensated motor speed are superimposed, and the final control command is output to complete the closed-loop tracking.

2. The trajectory tracking and control method for a quadrotor UAV based on NMPC according to claim 1, characterized in that, Step S1 includes the following steps: S1.1, Define system variables: Define the system state vector In the formula, For the three-dimensional position in the ground coordinate system, As a unit quaternion posture, For the velocity in the body coordinate system, Angular velocity in the body coordinate system; Define control input vector In the formula, Each corresponds to one of the four motors of the quadcopter at a specific time. The rotational speed; Define time-varying perturbation vector In the formula, These represent the actions acting on the body coordinate system. External disturbance components of the three axes These represent the actions acting on the body coordinate system. Three-axis disturbance torque components; S1.2 Constructing a continuous dynamic model: By combining the Newton-Euler equations with quaternion attitude transformation, a continuous nonlinear dynamic model with perturbations is constructed. In the formula, This is the perturbation distribution matrix, used to map the six-dimensional external perturbation to each state channel; S1.3 Discretization process: The continuous nonlinear dynamic model is discretized and numerically integrated using the explicit fourth-order Runge-Kutta method, with the control period set as . The discrete prediction model is obtained as follows: In the formula, This is the index for discrete time steps.

3. The trajectory tracking and control method for a quadrotor UAV based on NMPC according to claim 2, characterized in that, Step S2 includes the following steps: S2.1 Constructing the optimal control problem for trajectory tracking: Set the prediction time domain Control Time Domain Predicted total duration The objective function is constructed with the goal of minimizing the state tracking error and maximizing the smoothness of the control input in the prediction time domain. The constraints of the optimization problem include state boundary constraints, input boundary constraints, and equality constraints of the discrete prediction model obtained in step S1. S2.2, Discretization of Nonlinear Programming: The optimal control problem is transformed into a standard nonlinear programming problem using the direct multiple-shot method, and the prediction time domain is decomposed into... Each discrete time step; S2.3, SQP_RTI Real-time Iterative Solution: A real-time iterative framework of sequential quadratic programming is adopted, in which the quadratic programming subproblem is solved only once in each control cycle, and the interior point method solver of HPIPM is called to achieve the solution; S2.4, Hot Start Acceleration Strategy: The optimal control sequence and state trajectory obtained from the previous control cycle are shifted over time and used as the initial iteration values ​​for the current cycle optimization problem. S2.5, Matrix pre-storage mechanism: Offline pre-computes the sparse structure, memory space, and index information of the Jacobian matrix, Hessian matrix, and constraint matrix. In the online phase, only the matrix values ​​are updated, and the matrix structure is not reconstructed repeatedly.

4. The trajectory tracking and control method for a quadrotor UAV based on NMPC according to claim 3, characterized in that, The objective function in step S2.1 is: In the formula, This is the discrete time step index, with a value range of [value range missing]. Q, R, and P are the state weight matrix, input weight matrix, and terminal weight matrix, respectively. For the first The state tracking error of the step, where To predict the state vector, The reference state vector; For the first The control input error of the step, where To control the input vector, This is the reference input vector.

5. The trajectory tracking and control method for a quadrotor UAV based on NMPC according to claim 1, characterized in that, In step S3, the input feature vector expression is: In the formula, These represent the 13-dimensional system state, 3-dimensional linear acceleration estimation, 3-dimensional angular acceleration estimation, 3-dimensional reference trajectory, and 4-dimensional control input, respectively.

6. The trajectory tracking and control method for a quadrotor UAV based on NMPC according to claim 5, characterized in that, In step S3, the configuration of each layer of the fully connected MLP network is as follows: Input layer: Receives a 26-dimensional normalized feature vector, with 26 neurons; Feature transformation layer: Contains 128 neurons and uses the ReLU activation function to map the original high-dimensional input to a high-dimensional latent space, completing feature extraction and nonlinear mapping; The first hidden layer contains 64 neurons and uses the ReLU activation function to further extract abstract features from the middle layer; The second hidden layer contains 32 neurons and uses the ReLU activation function to compress the feature dimension while preserving key perturbation patterns. Dropout regularization layer: The inactivation rate is set to 0.2, randomly dropping some neuron connections to suppress the risk of overfitting; Output layer: Contains 6 neurons, uses a linear activation function, and directly outputs the six-dimensional perturbation estimate. In the formula, These are the estimated values ​​of the three-axis disturbance force components, respectively. These are the estimated values ​​of the three-axis disturbance torque components.

7. The trajectory tracking and control method for a quadrotor UAV based on NMPC according to claim 6, characterized in that, In step S3, the MLP network is trained using a weighted mean square error loss function, the expression of which is: In the formula, This represents the set of all learnable parameters in an MLP network; The total number of training samples; For training sample index; The loss weighting coefficient for the disturbance force component; This is the loss weighting coefficient for the disturbance torque component; and The first The network-predicted perturbation vector and the actual perturbation label vector corresponding to each training sample; and The first The network predicted perturbation moment vector and the actual perturbation moment label vector corresponding to each training sample.

8. The trajectory tracking and control method for a quadrotor UAV based on NMPC according to claim 1, characterized in that, The location layer feedforward compensation mentioned in step S4 specifically refers to: Calculate the position compensation amount to address the translational trajectory deviation caused by the disturbance force: In the formula, This is the position compensation gain matrix. For the quality of drones, This is the estimated vector of the three-axis perturbation force components output by the MLP perturbation observer; Corrected reference location: In the formula, The original reference trajectory position vector; The lightweight nonlinear model predictive controller constructed in step S2 uses the corrected reference trajectory To track the target and mitigate the effects of disturbances in advance.

9. The trajectory tracking and control method for a quadrotor UAV based on NMPC according to claim 8, characterized in that, The torque layer feedforward compensation mentioned in step S4 specifically refers to: To compensate for attitude fluctuations caused by disturbance torque, the motor speed is adjusted. Construct the torque compensation vector: In the formula, The outputs are respectively Three-axis disturbance torque component estimates; Through the hybrid control matrix The motor speed compensation amount is obtained by inverse mapping: In the formula, This is the motor thrust coefficient. This is the reference rotational speed of a single motor in the hovering state of the drone; Final control input: ,in, The optimal control sequence output by the lightweight nonlinear model predictive controller constructed in step S2 is in the... The value of the step.

Images