A differential flatness-based sparse nonlinear model predictive control method

Abstract

The application belongs to the field of trajectory tracking control of quadrotor unmanned aerial vehicles, and specifically provides a sparse nonlinear model predictive control method (FS-NMPC) based on differential flatness, to overcome the problem of insufficient control performance of the existing nonlinear model predictive control method (NMPC), and the application has good tracking accuracy and higher calculation efficiency. The application discretizes the differential flatness model of the quadrotor unmanned aerial vehicle through sparse discrete points in the prediction view, so that the number of optimization variables and matrix equations in the nonlinear model predictive control is greatly reduced; in summary, the application has significant advantages in calculation efficiency and good tracking performance.

Description

Technical Field

[0001] This invention belongs to the field of trajectory tracking control for quadrotor UAVs, and specifically provides a sparse nonlinear model predictive control (FS-NMPC) method based on differential flatness for trajectory tracking of quadrotor UAVs, so as to reduce the use of computing resources. Background Technology

[0002] Quadrotor drones (UAVs) play a vital role in agriculture, rescue, inspection, transportation, and entertainment, leading to increasing attention and demands for UAVs to independently perform flight missions. A quadcopter drone's control system typically consists of an upper decision-making module and a lower decision-making control module. The upper module plans the flight trajectory, while the lower control module controls the flight. However, UAV systems are inherently unstable and nonlinear systems, and are underactuated systems. These characteristics undoubtedly weaken their ability to execute flight missions more accurately and stably. Therefore, developing a high-performance flight controller remains a challenge.

[0003] Linear Model Predictive Control (LMPC), as a control method based on optimization, has good control performance and is widely used in flight control. However, in LMPC controllers, uniform discrete points are used to discretely predict the horizon, generating many variables and equation constraints that need to be optimized, resulting in high computational cost. In addition, since UAVs are nonlinear systems, using linearized models at discrete points will lead to a decrease in the tracking accuracy of UAVs.

[0004] Building upon this foundation, nonlinear model predictive control (NMPC) was proposed as an extension of the LMPC controller. Its main characteristic is the use of a nonlinear model for discrete points, a feature that helps improve the control accuracy and trajectory tracking performance of UAVs. However, NMPC also uses uniform discretization, leading to a large number of optimization variables and constraints, similar to LMPC. Furthermore, the quadrotor UAV model is a high-order, multi-state system, further increasing the computational burden on the controller. Therefore, it is essential to develop a high-precision and computationally efficient nonlinear model predictive control method for trajectory tracking of quadrotor UAVs. Summary of the Invention

[0005] The purpose of this invention is to provide a sparse nonlinear model predictive control method (FS-NMPC) based on differential flatness for trajectory tracking of quadrotor UAVs, in order to overcome the problem of insufficient control performance of existing nonlinear model predictive control methods (NMPC). This invention has good tracking accuracy and higher computational efficiency.

[0006] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0007] A sparse nonlinear model predictive control method based on differential flatness, characterized by comprising the following steps:

[0008] Set the physical parameters of the quadcopter drone, the initial state of the quadcopter drone, the reference trajectory tracked by the quadcopter drone, and the total flight time of the quadcopter drone;

[0009] Set the number of discrete points for the sparse nonlinear model predictive control, and calculate the sparse discrete points and differential matrix within the prediction horizon.

[0010] Based on the sparse discrete points and the differential matrix discretization of the differential flat model of the quadrotor UAV, the matrix equation of the control variables of the quadrotor UAV is obtained, and inequality constraints are set on the matrix equation.

[0011] The cost function of the sparse nonlinear model predictive control is set, and the sequential quadratic programming (SQP) solver is used to solve the cost function which is subject to matrix equation constraints and inequality constraints, so as to obtain all control variables within the prediction horizon.

[0012] The control command is calculated by using the Lagrange interpolation basis function and the solution result, and then applied to the quadrotor UAV to realize sparse nonlinear model predictive control for trajectory tracking of the quadrotor UAV.

[0013] Furthermore, the physical parameters of a quadcopter drone include: the mass of the quadcopter drone and the three-axis rotational inertia of the quadcopter drone.

[0014] Furthermore, the initial state of the quadcopter drone includes: the initial value of the quadcopter drone's spatial position, the initial value of the quadcopter drone's three-axis velocities, the initial value of the Euler angles, and the initial value of the quadcopter drone's angular velocity.

[0015] Furthermore, the reference trajectory tracked by the quadcopter drone includes: the spatial position of the reference trajectory and the yaw angle of the quadcopter drone on the reference trajectory.

[0016] Furthermore, the specific process of calculating sparse discrete points and the differential matrix within the prediction horizon is as follows:

[0017] The roots of the first derivative of the Legendre orthogonal polynomial are calculated using Newton's iteration method, and are expressed as:

[0018] τ m,step+1 =τ m,step -Δτ m,step

[0019]

[0020] τ m,0=cos(πm / N), m=0,1,…,N

[0021] Where, τ m,0 Let τ represent the initial value of the m-th LGL point in Newton's iterative method. m,step τ m,step+1 Let Δτ represent the solution of the m-th LGL point in the step-th and step+1-th iterations. m,step ρ represents the increment of the m-th LGL point in the step-th iteration; N represents the number of discrete points, ρ N (·) and ρ (N-1) (·) denotes a term in a Legendre orthogonal polynomial;

[0022] When Δτ m,step When the value is less than a preset threshold, the iteration stops, resulting in N+1 LGL points. These LGL points are then treated as sparse discrete points, represented as: [τ0, τ1, τ2, ..., τ N-1 ,τ N ];

[0023] By discretizing the horizon using LGL points, the approximate state variables of the quadrotor UAV are obtained, expressed as:

[0024]

[0025] Where, τ k Let s represent the k-th LGL point, k = 0, 1, ..., N; N (τ k ) represents the approximate state variables of a quadcopter drone. s N (τ k The first derivative of τ, s(τ) m ) indicates that the quadcopter drone is in τ m The state variable at time D; km Let the element in the k-th row and m-th column of the differential matrix D be represented as:

[0026]

[0027] Furthermore, the specific process of discretizing the differential flat model of the quadcopter UAV is as follows:

[0028] The differential flat model of a quadcopter UAV is represented as:

[0029]

[0030] Where [x,y,z] represents the spatial position of the quadcopter drone. Let [p, q, r] represent Euler angles, and [p, q, r] represent the angular velocity of the quadcopter drone. Let f represent the thrust generated by the four propellers of the quadcopter drone, where f is the control variable. This represents the three-axis torque generated by the four propellers of a quadcopter drone; [I xx ,I yy ,I zz [] represents the moment of inertia of the three axes in the body coordinate system of the quadrotor drone, g represents the acceleration due to gravity, and m q Indicates the mass of a quadcopter drone;

[0031] The discrete values ​​of triaxial acceleration and Euler angles at sparse discrete points can be expressed by matrix equations as follows:

[0032]

[0033] Where [x,y,z] represents the discretized vector of [x,y,z]. express Discretized vector; [t0,t f ] represents the range of the predicted horizon, t0 represents the starting time point, t f The termination time point is represented by D, and the differential matrix is ​​represented by the matrix equation:

[0034]

[0035] Where [p,q,r] represents the discretized vector of [p,q,r];

[0036] The discrete values ​​of the pitch angle can be represented by a matrix equation as follows:

[0037] θ = arctan(K1)

[0038]

[0039] The discrete values ​​of the roll angle can be represented by a matrix equation as follows:

[0040] φ = -arctan(K2)

[0041] K2 = sinθ.

[0042]

[0043] The discrete values ​​of the thrust of a quadcopter UAV can be expressed by a matrix equation as follows:

[0044]

[0045] In the coordinate system of the quadcopter UAV, x b The discrete values ​​of the torque on the shaft can be represented by a matrix equation as follows:

[0046]

[0047] y b The discrete values ​​of the torque on the shaft can be represented by a matrix equation as follows:

[0048]

[0049] z b The discrete values ​​of the torque on the shaft can be represented by a matrix equation as follows:

[0050]

[0051] The discrete values ​​of the control variables of a quadcopter UAV are represented as follows:

[0052]

[0053] Where u represents the discrete value of the control variable of the quadcopter UAV.

[0054] Furthermore, the inequality constraint can be expressed as:

[0055]

[0056] Among them, [f min ,f max [] indicates the upper and lower boundaries of the thrust. and This represents the upper and lower boundaries of the three-axis torque in the body coordinate system.

[0057] Furthermore, in the process of solving the cost function constrained by matrix equations and inequalities using a sequential quadratic programming solver, the parameters of the sequential quadratic programming (SQP) solver are set to default values, the initial iteration value is set to all 1s, and the iteration update value is the result of the previous solution.

[0058] Furthermore, the cost function of sparse nonlinear model predictive control is expressed as:

[0059]

[0060] Where J represents the cost function, μ represents the integral weight of the discrete cost function, and P w With Q w Let represent the weights of the tracking error and the control variables, respectively, and u represent the discrete values ​​of the control variables of the quadcopter UAV; [e x ,e y ,e z The symbol ] indicates the three-axis position tracking error. This indicates the yaw angle tracking error.

[0061] Furthermore, the specific process of calculating the control command using the Lagrange interpolation basis functions and the solution results is as follows:

[0062] The sparse discrete points are transformed into a prediction horizon, represented as:

[0063]

[0064] Where, [τ0,τ1,...,τ N ] represents sparse discrete points, [t0,t1,...,t N [t0,t] represents the time point for predicting the horizon. f ] represents the range of the predicted horizon, t0 represents the starting time point, t f Indicates the end time point;

[0065] The control command is then expressed as:

[0066]

[0067] Among them, f cmd Indicates thrust control command, and In the coordinate system of the quadcopter UAV, x represents the x-axis. b axis, y b axis and z b Torque control command for shaft; L m (·) denotes the Lagrange interpolation basis function, T s To control the cycle; This represents the discrete values ​​of the control variables of a quadcopter drone.

[0068] Based on the above technical solution, the beneficial effects of the present invention are as follows:

[0069] This invention provides a sparse nonlinear model predictive control method based on differential flatness for trajectory tracking of quadrotor UAVs. By discretizing the differential flatness model of the quadrotor UAV within the prediction horizon using sparse discrete points, the number of optimization variables and matrix equations in nonlinear model predictive control is significantly reduced. In summary, this invention has significant advantages in computational efficiency and good tracking performance. Attached Figure Description

[0070] Figure 1 This is a flowchart illustrating the sparse nonlinear model predictive control (FS-NMPC) method based on differential flatness in this invention.

[0071] Figure 2 This is a three-dimensional comparison diagram of UAV trajectory tracking under PID, NMPC, and FS-NMPC control in an embodiment of the present invention.

[0072] Figure 3 This is a comparison chart of tracking errors for UAV trajectory tracking under PID, NMPC, and FS-NMPC control in embodiments of the present invention.

[0073] Figure 4 This is a comparison diagram of control commands for UAV trajectory tracking under PID, NMPC, and FS-NMPC control in embodiments of the present invention. Detailed Implementation

[0074] To make the objectives, technical solutions, and beneficial effects of the present invention clearer, the specific implementation methods and working principles of the present invention will be further described in detail below with reference to the accompanying drawings and embodiments.

[0075] This embodiment provides a differentially flat sparse nonlinear model predictive control method (FS-NMPC) for trajectory tracking of a quadrotor UAV, the process of which is as follows: Figure 1 As shown, the specific steps include:

[0076] Step 1. Set the physical parameters of the quadcopter drone. The specific settings are shown in Table 1.

[0077] Table 1

[0078]

[0079]

[0080] Where, m q I represents the mass of a quadcopter drone. xx I yy with I zz The moment of inertia of the three axes (x-axis, y-axis, and z-axis) of the quadcopter drone is represented by g, and g represents the acceleration due to gravity.

[0081] Step 2. Set the initial state of the quadcopter drone. The specific settings are shown in Table 2.

[0082] Table 2

[0083]

[0084] Among them, [x init ,y init ,z init [v] represents the initial value of the spatial position of the quadcopter drone. x,init ,v y,init ,v z,init [] represents the initial values ​​of the three-axis velocities of the quadcopter drone. The initial value of the Euler angles is represented by φ, θ, and θ represents the rotation angle from the inertial reference frame to the quadrotor system. In order, the angles are roll, pitch, and yaw. init ,q init ,r init[] represents the initial value of the angular velocity of the quadcopter drone;

[0085] Step 3. Set the reference trajectory to be tracked by the quadcopter drone. The specific settings are shown in Table 3.

[0086] Table 3

[0087]

[0088] Where, x ref (t), y ref (t) and z ref (t) represents the spatial position of the reference trajectory. This indicates the yaw angle of the quadcopter drone on the reference trajectory;

[0089] Step 4. Set the number of discrete points N=7 for the sparse nonlinear model predictive control (FS-NMPC), and the total flight time T of the quadcopter UAV. end =30s;

[0090] Step 5. Calculate the sparse discrete points and the differential matrix;

[0091] The uniform discretization method generates a large number of variables that need to be optimized. Therefore, this embodiment uses sparse discretization to discretize the continuous state variables and control variables to avoid this drawback.

[0092] First, the N+1 sampling points within the predicted field of view are sampled and interpolated using the Lagrange interpolation basis function to approximate the state and control variables of the quadcopter UAV, specifically as follows:

[0093]

[0094] Where s(t) and u(t) represent the state variables and control variables of the quadcopter UAV, s N (t) and u N (t) represents the Nth-order approximation of the state variable and the control variable, t m With t n L represents the sampling time point. m (t) represents the Lagrange interpolation basis function;

[0095] The above process involves uniform discretization, which leads to severe oscillations in the approximate state and control variables near the start and end points of the prediction horizon. To improve the approximation quality of the Lagrange interpolation basis functions for the state and control variables, this embodiment uses sparse LGL points instead of uniformly discretized points. LGL points are the roots of the first derivative of the Legendre orthogonal polynomial. Since the domain of LGL points is the closed interval [-1, 1], a transformation of the prediction horizon is required. The transformation formula is:

[0096]

[0097] Where τ represents the variables of the Legendre orthogonal polynomial, with a range of [-1, 1]; [t0, t... f ] represents the range of the predicted horizon, t0 represents the starting time point (greater than 0), t f Indicates the termination time point (greater than 0);

[0098] The LGL points are obtained by calculating the roots of the first derivative of the Legendre orthogonal polynomial; the Legendre orthogonal polynomial is expressed as:

[0099] (α+1)ρ N+1 (τ)=(2α+1)τρ N (τ)-αρ N-1 (τ)

[0100] ρ0(τ)=1,ρ -1 (τ)=0,α=0,1,2…N

[0101] Where, ρ -1 (τ) to ρ N+1 (τ) represents the terms of the Legendre orthogonal polynomial, where the Legendre orthogonal polynomial ρ is a recursive formula, and α represents the order of the recursive formula.

[0102] The roots of the first derivative of the Legendre orthogonal polynomial are calculated using Newton's iteration method, and are expressed as:

[0103] τ m,step+1 =τ m,step -Δτ m,step

[0104]

[0105] τ m,0 =cos(πm / N), m=0,1,…,N

[0106] Where, τ m,0 Let τ represent the initial value of the m-th LGL point in Newton's iterative method. m,step τ m,step+1 Let Δτ represent the solution of the m-th LGL point in the step-th and step+1-th iterations. m,step This represents the increment of the m-th LGL point in the step-th iteration; it should be noted that when Δτ m,step Less than 10 -16 When this happens, Newton's iteration method stops iterating;

[0107] The LGL points obtained using the above method are used to discretize and predict the view frustum. The LGL points are represented by vectors as follows:

[0108] [τ0,τ1,τ2,...,τ N-1 ,τ N ]

[0109] By taking L m The derivative of (t) can be used to obtain the differential of the approximate state variable, specifically expressed as:

[0110]

[0111] Where, τ k This represents the k-th LGL point, where k = 0, 1, ..., N; it should be noted that: Denotes the first derivative of A. Let A denote the second derivative of A; D denotes the differential matrix, specifically an N+1 dimensional square matrix used to store the differential values ​​of the variables at each LGL point. km Let represent the element in the k-th row and m-th column of the differential matrix D, specifically as follows:

[0112]

[0113] Step 6. Discretize the differential flat model of the quadcopter UAV;

[0114] The differential flat model of the quadcopter drone only requires The differential flat model of a quadcopter UAV is represented by four states to indicate all other state and control variables:

[0115]

[0116] Where [x,y,z] represents the spatial position of the quadcopter drone. Let [p, q, r] represent the Euler angles, and [p, q, r] represent the angular velocity of the quadcopter drone, specifically equal to... Let f represent the thrust generated by the four propellers of the quadcopter drone, where f is the control variable. This represents the three-axis torque generated by the four propellers of a quadcopter drone; [I xx ,I yy ,I zz [] represents the moment of inertia of the three axes in the body coordinate system of the quadrotor drone, g represents the acceleration due to gravity, and m q Indicates the mass of a quadcopter drone;

[0117] Therefore, the differential flattening model of a quadcopter UAV can effectively reduce the number of variables that need to be optimized in NMPC. The differential flattening model of the UAV is discretized using a differential matrix to obtain the corresponding equation constraints. To simplify the expression, the symbol ".*" is defined to represent the multiplication of corresponding elements in the preceding and following vectors, the symbol ". / " is defined to represent the division of corresponding elements in the preceding and following vectors, and the symbol "σ" is defined to represent the division of corresponding elements in the preceding and following vectors. .χ " represents the exponentiation operation for each element in vector σ;

[0118] In the inertial coordinate system, the discrete values ​​of the triaxial acceleration and Euler angle angular acceleration at each LGL point can be represented by a matrix equation, specifically:

[0119]

[0120] Where [x,y,z] represents the discretized vector of [x,y,z]. express The discretized vectors, each with a dimension of N+1;

[0121] The discrete values ​​of angular velocity can be represented by a matrix equation, specifically:

[0122]

[0123] Where [p,q,r] represents the discretized vector of [p,q,r];

[0124] At each LGL point, combining the differential planarization model of the quadcopter UAV, the discrete values ​​of the pitch angle of the quadcopter UAV can be expressed as:

[0125] θ = arctan(K1)

[0126]

[0127] The discrete values ​​of the roll angle of a quadcopter drone can be expressed as:

[0128] φ = -arctan(K2)

[0129] K2 = sinθ.

[0130]

[0131] This yields the discrete values ​​of the four input variables of the quadrotor UAV. Substituting the discrete values ​​of the triaxial acceleration in the inertial coordinate system into the equation describing the thrust, the thrust of the quadrotor UAV can be obtained, expressed as:

[0132]

[0133] In the body coordinate system of the quadrotor UAV, x bThe torque on the shaft can be expressed as:

[0134]

[0135] y b The torque on the shaft is expressed as:

[0136]

[0137] z b The torque on the shaft is expressed as:

[0138]

[0139] Step 7. The control variables of the quadcopter UAV are expressed as follows:

[0140]

[0141] The control variables need to be constrained by the following set of inequalities:

[0142]

[0143] Among them, [f min ,f max [] indicates the upper and lower boundaries of the thrust. and This represents the upper and lower boundaries of the three-axis torque in the body coordinate system, with the subscript "min" marking the lower boundary and the subscript "max" marking the upper boundary; in this embodiment, [f min ,f max Setting it to [0,30] sets the lower boundary of the triaxial torque. Set to [-1, -1, -1], the upper boundary of the triaxial torque. Set to [1,1,1];

[0144] Step 8. Set the cost function for Sparse Nonlinear Model Predictive Control (FS-NMPC);

[0145] In this embodiment, the cost function of FS-NMPC consists of two parts: one part is the error between the current state of the UAV and the tracked target, and the other part is the input of the UAV; wherein, the position tracking error of the quadcopter UAV can be expressed as:

[0146] [e x ,e y ,e z ] = [xx ref yy ref ,zz ref ]

[0147] Among them, [e x ,ey ,e z ] represents the three-axis position tracking error, [x ref ,y ref ,z ref ] represents the discretized vector of the three-axis position of the reference trajectory;

[0148] The yaw tracking error of a quadcopter UAV can be expressed as:

[0149]

[0150] in, Indicates the yaw angle tracking error. This represents the discretized vector of the yaw angle of the quadcopter drone, with 0 indicating a zero vector. Since trajectory tracking can be achieved by changing the pitch and roll angles of the drone, the yaw angle of the drone is kept at 0.

[0151] Therefore, the cost function that needs to be optimized in FS-NMPC can be described as:

[0152]

[0153] Where μ represents the integral weight of the discrete cost function, and P w With Q w These represent the weights of the tracking error and the control variable, respectively; in this embodiment, P w and Q w Set them to [140, 140, 700, 400] respectively. T and [1,1,1,1] T The integral weight μ is specifically expressed as:

[0154] μ = [μ0, μ1, ..., μ] N ]

[0155]

[0156] The Jacobian matrix of the objective function is then:

[0157] Jac = [s jac ,u jac ]

[0158]

[0159]

[0160] Step 9. Set the parameters of the Sequence Quadratic Programming (SQP) solver used to solve FS-NMPC. The SQP parameters are set to the default values.

[0161] Step 10. Set the initial values ​​for the quadratic programming solver iterations. When SQP solves FS-NMPC for the first time, the initial values ​​for SQP iterations are set to all 1s; in subsequent solutions, the initial values ​​for SQP iterations are set to the results of the previous solution.

[0162] Step 11. Set the initial conditions for the quadcopter drone;

[0163] In the actual flight of a quadcopter drone, the initial values ​​of each state need to be constrained. In the differential flat model of the drone, some states are hidden in the equations, but these state variables are already represented in the discretization process; the discrete values ​​of the three-axis velocities are given:

[0164]

[0165] Therefore, when FS-NMPC is solved iteratively, the initial values ​​of each state can be set as follows:

[0166] [x(τ0),y(τ0),z(τ0)]=[x init ,y init ,z init ]

[0167] [v x (τ0),v y (τ0),v z (τ0)]=[v x,init ,v y,init ,v z,init ]

[0168]

[0169] [p(τ0),q(τ0),r(τ0)]=[p init ,q init ,r init ]

[0170] When SQP solves the FS-NMPC for the first time, the initial conditions for the quadcopter UAV are given in step 2. In subsequent SQP solutions to the FS-NMPC, the initial conditions can be obtained by sensing the current state of the quadcopter UAV from its sensors.

[0171] Step 12. Use the pre-configured SQP solver to solve FS-NMPC and obtain all control variables within the prediction horizon;

[0172] Step 13. Calculate the control command using the Lagrange interpolation basis functions and the solution results from the SQP solver;

[0173] NMPC can obtain the result of each optimization through a nonlinear programming solver. This result contains the values ​​of all control variables of the quadcopter at each discrete point. The discrete values ​​of the control variables at time t1 are usually used as the control input of the quadcopter. It should be noted that in NMPC, the time span between t0 and t1 is equivalent to the control period T of the quadcopter. s Because NMPC is uniformly discrete;

[0174] In this embodiment, for FS-NMPC, the span between discrete points is non-uniform; the time span between t0 and t1 is not equal to the control period T of the quadcopter UAV. s Therefore, the control command for the next cycle is obtained through the Lagrange interpolation function; the LGL points located within the closed interval are converted into the prediction horizon [t0,t]. f The conversion formula is:

[0175]

[0176] Among them, t f equal to t N ;

[0177] The control command for the quadcopter to generate thrust is:

[0178]

[0179] In the quadcopter's body coordinate system, the control commands that generate torque on the three axes are as follows:

[0180]

[0181] Among them, T s To control the cycle, in this embodiment, T s Set to 0.1;

[0182] Step 14. Apply the control command obtained in Step 13 to the quadcopter UAV to realize sparse nonlinear model predictive control for trajectory tracking of the quadcopter UAV.

[0183] Simultaneously, record the optimal solution obtained by SQP in solving FS-NMPC. If the flight has not ended, return to step 10 as the initial value for the next SQP iteration. FS-NMPC is a control method with rolling optimization characteristics. To avoid the solver being sensitive to the initial values ​​of the variables to be optimized, it is necessary to reasonably estimate the initial values ​​of the variables to be optimized. Assume that the optimal solution obtained by the solver at time t0 is: In the next optimization, the initial value of the variable to be optimized can be expressed as:

[0184] The beneficial effects of the present invention will be explained below with reference to simulation tests.

[0185] Tracking performance and computational efficiency are statistically analyzed at the end of the quadcopter UAV's flight. The position and yaw angle of the quadcopter UAV are recorded in each control cycle. The root mean square error (RMSE) is used as the tracking performance indicator. The RMSE is calculated as follows:

[0186]

[0187] The time consumed by SQP in solving FS-NMPC is recorded, and the average time (MEAN) is used to measure the computational efficiency of FS-NMPC.

[0188] This embodiment compares the tracking performance of FS-NMPC with Well-tuned PID and NMPC, such as... Figure 2 The image shows a 3D comparison of UAV trajectory tracking under PID, NMPC, and FS-NMPC control. Figure 3 The image shows a comparison of tracking errors for UAV trajectory tracking under PID, NMPC, and FS-NMPC control. Figure 4 The diagram shows a comparison of control commands for UAV trajectory tracking under PID, NMPC, and FS-NMPC control. Detailed comparison results of tracking errors are shown in Table 4, and detailed comparison results of computational efficiency are shown in Table 5.

[0189] Table 4

[0190]

[0191] Table 5

[0192]

[0193] As shown in the figures, the tracking performance of FS-NMPC provided in this invention is better than that of PID and very close to that of NMPC; furthermore, the computational efficiency of FS-NMPC provided in this invention is significantly higher than that of NMPC; in summary, the differential flat sparse nonlinear model predictive control method (FS-NMPC) provided in this invention is more suitable for trajectory tracking control of quadcopter UAVs.

[0194] The above description is merely a specific embodiment of the present invention. Any feature disclosed in this specification may be replaced by other equivalent or similar features unless otherwise specified. All disclosed features, or steps in all methods or processes, may be combined in any way except for mutually exclusive features and / or steps.

Claims

1. A sparse nonlinear model predictive control method based on differential flatness, characterized in that, Includes the following steps: Set the physical parameters of the quadcopter drone, the initial state of the quadcopter drone, the reference trajectory tracked by the quadcopter drone, and the total flight time of the quadcopter drone; The number of discrete points for the sparse nonlinear model predictive control is set, and the sparse discrete points and differential matrix are calculated within the prediction horizon. The specific process is as follows: The roots of the first derivative of the Legendre orthogonal polynomial are calculated using Newton's iteration method, and are expressed as: in, Indicates the first The initial values ​​of LGL points in Newton's iterative method, , Indicates the first The first LGL point, the second The solution of the next iteration Indicates the first The LGL point at the _ The increment of the next iteration; Indicates the number of discrete points. and Denotes the terms of a Legendre orthogonal polynomial; when When the value is less than a preset threshold, the iteration stops, and the result is obtained. LGL points, treated as sparse discrete points, are represented as follows: ; By discretizing the horizon using LGL points, the approximate state variables of the quadrotor UAV are obtained, expressed as: in, Indicates the first LGL points, ; This represents the approximate state variables of a quadcopter drone. express The first derivative, This indicates that quadcopter drones are in The state variable at any given time; Represents the differential matrix The Middle line, number The elements of a column are represented as: Based on the sparse discrete points and the differential matrix, the differential flat model of the quadrotor UAV is discretized, the matrix equation of the quadrotor UAV model is obtained, and inequality constraints are set on the matrix equation. The cost function of the sparse nonlinear model predictive control is set, and the cost function subject to matrix equation constraints and inequality constraints is solved by a sequential quadratic programming solver to obtain all control variables within the prediction horizon. The control command is calculated by using the Lagrange interpolation basis function and the solution result, and then applied to the quadrotor UAV to realize sparse nonlinear model predictive control for trajectory tracking of the quadrotor UAV.

2. The sparse nonlinear model predictive control method based on differential flatness according to claim 1, characterized in that, The physical parameters of a quadcopter drone include: the mass of the quadcopter drone and the three-axis rotational inertia of the quadcopter drone.

3. The sparse nonlinear model predictive control method based on differential flatness according to claim 1, characterized in that, The initial state of a quadcopter drone includes: the initial value of the quadcopter drone's spatial position, the initial value of the quadcopter drone's three-axis velocities, the initial value of the Euler angles, and the initial value of the quadcopter drone's angular velocity.

4. The sparse nonlinear model predictive control method based on differential flatness according to claim 1, characterized in that, The reference trajectory tracked by the quadcopter drone includes: the spatial position of the reference trajectory and the yaw angle of the quadcopter drone on the reference trajectory.

5. The sparse nonlinear model predictive control method based on differential flatness according to claim 1, characterized in that, The specific process of discretizing the differential flat model of a quadcopter UAV is as follows: The differential flat model of a quadcopter UAV is represented as: in, Indicates the spatial position of a quadcopter drone. Represents Euler angles. This indicates the angular velocity of the quadcopter drone; Indicates control variables, This represents the thrust generated by the four propellers of a quadcopter drone. This indicates the three-axis torque generated by the four propellers of a quadcopter drone; This represents the moment of inertia of the three axes in the body coordinate system of a quadcopter drone. Represents gravitational acceleration. Indicates the mass of a quadcopter drone; The discrete values ​​of triaxial acceleration and Euler angles at sparse discrete points can be expressed by matrix equations as follows: in, express Discretized vector, express Discretized vector; [ , [Indicates the predicted field of view] Indicates the start time point. Indicates the end time point, Represents the differential matrix; The discrete values ​​of angular velocity can be expressed by a matrix equation as follows: in, express Discretized vector; The discrete values ​​of the pitch angle can be represented by a matrix equation as follows: The discrete values ​​of the roll angle can be represented by a matrix equation as follows: The discrete values ​​of the thrust of a quadcopter UAV can be expressed by a matrix equation as follows: In the coordinate system of a quadcopter drone The discrete values ​​of the torque on the shaft can be represented by a matrix equation as follows: The discrete values ​​of the torque on the shaft can be represented by a matrix equation as follows: The discrete values ​​of the torque on the shaft can be represented by a matrix equation as follows: The discrete values ​​of the control variables of a quadcopter UAV are represented as follows: in, This represents the discrete values ​​of the control variables of a quadcopter drone.

6. The sparse nonlinear model predictive control method based on differential flatness according to claim 5, characterized in that, Inequality constraints are expressed as: in, Indicates the upper and lower boundaries of the thrust. and This represents the upper and lower boundaries of the three-axis torque in the body coordinate system.

7. The sparse nonlinear model predictive control method based on differential flatness according to claim 1, characterized in that, In the process of solving the cost function constrained by matrix equations and inequalities using a sequential quadratic programming solver, the parameters of the sequential quadratic programming solver are set to default values, the initial iteration value is set to all 1s, and the iteration update value is the result of the previous solution.

8. The sparse nonlinear model predictive control method based on differential flatness according to claim 1, characterized in that, The cost function of sparse nonlinear model predictive control is expressed as: in, Represents the cost function. The integral weights represent the discrete cost function. and These represent the weights of the tracking error and the control variable, respectively. Represents the discrete values ​​of the control variables of a quadcopter drone; Indicates the three-axis position tracking error. This indicates the yaw angle tracking error.

Images