Differential flatness-based sparse nonlinear model prediction control method

Abstract

The invention belongs to the field of trajectory tracking control of a quad-rotor unmanned aerial vehicle, and particularly provides a sparse nonlinear model predictive control method (FS-NMPC) based on differential flatness, which is used for overcoming the problem of insufficient control performance of an existing nonlinear model predictive control method (NMPC), and has higher calculation efficiency while having good tracking precision. According to the method, the differential flat model of the quad-rotor unmanned aerial vehicle is discretized in the prediction field of vision through sparse discrete points, so that the number of optimization variables and matrix equations in nonlinear model prediction control is greatly reduced; in conclusion, the method has remarkable advantages in the aspect of calculation efficiency, and has good tracking performance.

Description

Technical Field

[0001] The present invention belongs to the field of trajectory tracking control of quadrotor unmanned aerial vehicles, and specifically provides a differentially flat sparse nonlinear model predictive control (FS-NMPC) method for trajectory tracking of quadrotor unmanned aerial vehicles to reduce the use of computing resources. Background Art

[0002] Quadrotor UAVs (UAVs) play an important role in agriculture, rescue, inspection, transportation, and entertainment. Therefore, more and more people are paying attention to quadrotor UAVs and require UAVs to independently complete some flight missions. The control system of a quadrotor UAV usually consists of an upper decision-making module and a lower decision-making control module. The upper module is responsible for planning the flight trajectory, and the lower control module controls the flight. However, the UAV system itself is an unstable nonlinear system, and the UAV is an under-driven system. These characteristics undoubtedly weaken the UAV's ability to perform flight missions more accurately and stably. Therefore, it is still challenging to develop a high-performance flight controller.

[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 the LMPC controller, uniform discrete points are used for discrete prediction horizons, resulting in many variables and equation constraints that need to be optimized, resulting in high computational complexity. In addition, since the UAV is a nonlinear system, the use of a linearized model at discrete points will lead to a decrease in the tracking accuracy of the UAV.

[0004] On this basis, as an extension of the LMPC controller, nonlinear model predictive control (NMPC) was proposed. Its main feature is that the discrete points use nonlinear models, which helps to improve the control accuracy and trajectory tracking performance of the UAV. However, NMPC also uses uniform discretization, which results in a large number of optimization variables and constraints like LMPC. In addition, the quadrotor UAV model is a high-order, multi-state system, which further increases the calculation amount of the controller. Therefore, it is necessary to develop a nonlinear model predictive control method for quadrotor UAV trajectory tracking with high accuracy and computational efficiency. Summary of the invention

[0005] The purpose of the present invention is to provide a differentially flat sparse nonlinear model predictive control method (FS-NMPC) for quadrotor UAV trajectory tracking, so as to overcome the problem of insufficient control performance of existing nonlinear model predictive control methods (NMPC). The present invention has good tracking accuracy and higher computational efficiency.

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

[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 quadrotor drone, the initial state of the quadrotor drone, the reference trajectory tracked by the quadrotor drone, and the total flight time of the quadrotor drone;

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

[0010] The differential flat model of the quadrotor UAV is discretized according to the sparse discrete points and differential matrix, the matrix equation of the control variables of the quadrotor UAV is obtained, and inequality constraints are set for the matrix equation;

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

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

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

[0014] Furthermore, the initial state of the quadrotor drone includes: an initial value of the spatial position of the quadrotor drone, an initial value of the three-axis speed of the quadrotor drone, an initial value of the Euler angle, and an initial value of the angular velocity of the quadrotor drone.

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

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

[0017] The roots of the first-order derivatives of Legendre orthogonal polynomials are calculated using Newton's iteration method, expressed as:

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

[0019]

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

[0021] Among them, τ m,0 represents the initial value of the mth LGL point in the Newton iteration method, τ m,step , τ m,step+1 represents the solution of the step-th and step+1-th iterations of the m-th LGL point, Δτ m,step represents the increment of the mth LGL point in the step iteration; N represents the number of discrete points, ρ N (·) and ρ (N-1) (·) represents the terms of Legendre orthogonal polynomials;

[0022] When Δτ m,step When it is less than the preset threshold, the iteration stops and N+1 LGL points are obtained. The LGL points are used as sparse discrete points and expressed as: [τ0,τ1,τ2,...,τ N-1 ,τ N ];

[0023] Through the discrete prediction horizon of the LGL point, the approximate state variables of the quadrotor drone are obtained, which can be expressed as:

[0024]

[0025] Among them, τ k represents the kth LGL point, k = 0, 1, ..., N; s N (τ k ) represents the approximate state variables of the quadrotor drone, Indicates N (τ k ), s(τ m ) indicates that the quadcopter is in τ m The state variables at the moment D km represents the element in the kth row and mth column of the differential matrix D, which is expressed as:

[0026]

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

[0028] The differential flat model of the quadrotor drone is expressed as:

[0029]

[0030] Among them, [x, y, z] represents the spatial position of the quadrotor drone. represents the Euler angle, [p,q,r] represents the angular velocity of the quadrotor drone; represents the control variable, f represents the thrust generated by the four propellers of the quadcopter, Represents the three-axis torque generated by the four propellers of the quadcopter; [I xx ,I yy ,I zz ] represents the three-axis moment of inertia in the quadcopter body coordinate system, g represents the acceleration of gravity, m q Indicates the mass of the quadrotor drone;

[0031] The discrete values ​​of the three-axis acceleration and Euler angle at sparse discrete points are expressed by matrix equations:

[0032]

[0033] Among them, [x,y,z] represents the discretized vector of [x,y,z], express The discretized vector [t0,t f ] represents the range of the prediction horizon, t0 represents the starting time point, t f represents the end time point, D represents the differential matrix; the discrete value of the angular velocity is expressed 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 are expressed by the matrix equation:

[0037] θ=arctan(K1)

[0038]

[0039] The discrete values ​​of the roll angle are expressed by the matrix equation:

[0040] φ=-arctan(K2)

[0041] K2=sinθ.*

[0042]

[0043] The discrete value of the thrust of the quadrotor drone is expressed by the matrix equation:

[0044]

[0045] The coordinate system of the quadcopter drone is x b The discrete values ​​of the torque on the shaft are expressed by the matrix equation:

[0046]

[0047] y b The discrete values ​​of the torque on the shaft are expressed by the matrix equation:

[0048]

[0049] z b The discrete values ​​of the torque on the shaft are expressed by the matrix equation:

[0050]

[0051] The discrete values ​​of the control variables of the quadrotor drone are expressed as:

[0052]

[0053] Among them, u represents the discrete value of the control variable of the quadrotor drone.

[0054] Furthermore, the inequality constraint is expressed as:

[0055]

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

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

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

[0059]

[0060] Among them, J represents the cost function, μ represents the integral weight of the discrete cost function, and P w With Q w Respectively represent the weights of tracking error and control variable, u represents the discrete value of the control variable of the quadrotor drone; [e x ,e y ,e z ] represents the three-axis position tracking error, Represents the yaw angle tracking error.

[0061] Furthermore, the specific process of calculating the control command through the Lagrange interpolation basis function and the solution result is:

[0062] Convert sparse discrete points to prediction horizons, expressed as:

[0063]

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

[0065] The control command is expressed as:

[0066]

[0067] Among them, f cmd Indicates the thrust control command, and Represents x in the quadcopter body coordinate system b Axis, y b Axis and z b Axis torque control command; L m (·) represents the Lagrange interpolation basis function, T s To control the cycle; Represents the discrete value of the control variable of the quadrotor drone.

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

[0069] The present invention provides a sparse nonlinear model predictive control method based on differential flatness for trajectory tracking of a quadrotor unmanned aerial vehicle. The differential flat model of the quadrotor unmanned aerial vehicle is discretized within a prediction horizon through sparse discrete points, so that the number of optimization variables and matrix equations in the nonlinear model predictive control is greatly reduced. In summary, the present invention has significant advantages in computational efficiency and has good tracking performance. BRIEF DESCRIPTION OF THE DRAWINGS

[0070] Figure 1 It is a flow chart of the differential flattening based sparse nonlinear model predictive control (FS-NMPC) method in the present invention.

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

[0072] Figure 3 It is a tracking error comparison diagram of UAV trajectory tracking under PID, NMPC, and FS-NMPC control in an embodiment of the present invention.

[0073] Figure 4 This is a comparison chart of control commands for UAV trajectory tracking under PID, NMPC, and FS-NMPC control in an embodiment of the present invention. DETAILED DESCRIPTION

[0074] In order to make the purpose, technical solution and beneficial effects of the present invention more clearly understood, the specific implementation manner and working principle of the present invention are further described in detail below in conjunction with the accompanying drawings and embodiments.

[0075] This embodiment provides a sparse nonlinear model predictive control method (FS-NMPC) based on differential flatness for trajectory tracking of a quadrotor drone, and its process is as follows: Figure 1 As shown, the specific steps include:

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

[0077] Table 1

[0078]

[0079]

[0080] Among them, m q represents the mass of the quadrotor drone, I xx ,I yy with I zz It represents the moment of inertia of the three axes (x-axis, y-axis and z-axis) of the quadrotor drone, and g represents the acceleration due to gravity;

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

[0082] Table 2

[0083]

[0084] Among them, [x init ,y init ,z init ] represents the initial value of the spatial position of the quadrotor drone, [v x,init ,v y,init ,v z,init ] represents the initial value of the three-axis speed of the quadrotor drone. Represents the initial value of the Euler angle, represents the rotation angle from the inertial reference frame to the quadrotor system, that is, the Euler angle (φ, θ, Roll angle, pitch angle, yaw angle), [p init ,q init ,r init] represents the initial value of the angular velocity of the quadrotor drone;

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

[0086] Table 3

[0087]

[0088] Among them, x ref (t), y ref (t) and z ref (t) represents the spatial position of the reference trajectory, represents the yaw angle of the quadrotor drone on the reference trajectory;

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

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

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

[0092] First, the Lagrange interpolation basis function is used to sample and interpolate the N+1 sampling points within the prediction horizon, so as to approximate the state variables and control variables of the quadrotor drone, which can be specifically expressed as:

[0093]

[0094] Among them, s(t) and u(t) represent the state variables and control variables of the quadrotor drone, 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 represents the sampling time point, L m (t) represents the Lagrange interpolation basis function;

[0095] The above process is uniformly discrete, which will lead to severe vibration of the approximate state variables and control variables near the starting point and the end point of the prediction horizon. In order to improve the approximate quality of the Lagrange interpolation basis function for the state variables and control variables, this embodiment uses sparse LGL points instead of uniform discrete points. The LGL points are the roots of the first-order derivatives of the Legendre orthogonal polynomials. Since the domain of the LGL points is a closed interval [-1,1], the prediction horizon needs to be transformed. The transformation formula is:

[0096]

[0097] Among them, τ represents the variable of Legendre orthogonal polynomial, ranging from [-1,1]; [t0,t f ] indicates the range of the prediction horizon, t0 indicates the starting time point (greater than 0), t f Indicates the end time point (greater than 0);

[0098] The LGL point is obtained by calculating the root of the first-order 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] Among them, ρ -1 (τ) to ρ N+1 (τ) represents the terms of Legendre orthogonal polynomials in turn. Legendre orthogonal polynomial ρ is a recursive formula, and α represents the order of the recursive formula;

[0102] The roots of the first-order derivatives of Legendre orthogonal polynomials are calculated using Newton's iteration method, expressed as:

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

[0104]

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

[0106] Among them, τ m,0 represents the initial value of the mth LGL point in the Newton iteration method, τ m,step , τ m,step+1 represents the solution of the step-th and step+1-th iterations of the m-th LGL point, Δτ m,step represents the increment of the mth LGL point in the step iteration; it should be noted that when Δτ m,step Less than 10 -16 When , the Newton iteration method stops iterating;

[0107] The LGL points obtained by the above method are used to discretely predict the horizon. 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, which is specifically expressed as:

[0110]

[0111] Among them, τ k represents the kth LGL point, k=0,1,…,N; it should be noted that: represents the first-order derivative of A, represents the second-order derivative of A; D represents the differential matrix, which is a square matrix of N+1 dimensions, used to store the differential value of the variable at each LGL point, D km represents the element in the kth row and mth column of the differential matrix D, which is specifically expressed as:

[0112]

[0113] Step 6. Discretize the differential flat model of the quadrotor drone;

[0114] The differential flat model of the quadrotor drone only requires Four states are used to represent all other state variables and control variables. The differential flat model of the quadrotor drone is expressed as:

[0115]

[0116] Among them, [x, y, z] represents the spatial position of the quadrotor drone. represents the Euler angle, [p,q,r] represents the angular velocity of the quadcopter, which is equal to represents the control variable, f represents the thrust generated by the four propellers of the quadcopter, Represents the three-axis torque generated by the four propellers of the quadcopter; [I xx ,I yy ,I zz ] represents the three-axis moment of inertia in the quadcopter body coordinate system, g represents the acceleration of gravity, m q Indicates the mass of the quadrotor drone;

[0117] It can be seen that the differential flat model of the quadrotor UAV can effectively reduce the number of variables that need to be optimized in NMPC. The differential flat model of the UAV is discretized using the differential matrix to obtain the corresponding equation constraints. In order to simplify the expression, the symbol ".*" is defined to represent the multiplication of the corresponding elements in the front and back vectors, the symbol ". / " is defined to represent the division of the corresponding elements in the front and back vectors, and the symbol "σ .χ ” represents the power operation of each element in the vector σ;

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

[0119]

[0120] Among them, [x,y,z] represents the discretized vector of [x,y,z], express The discretized vectors of , the dimension of each vector is equal to N+1;

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

[0122]

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

[0124] At each LGL point, combined with the differential leveling model of the quadrotor drone, the discrete value of the pitch angle of the quadrotor drone can be expressed as:

[0125] θ=arctan(K1)

[0126]

[0127] The discrete value of the roll angle of the quadrotor drone can be expressed as:

[0128] φ=-arctan(K2)

[0129] K2=sinθ.*

[0130]

[0131] Thus, the discrete values ​​of the four input variables of the quadcopter are obtained. Substituting the discrete values ​​of the three-axis acceleration in the inertial coordinate system into the equation describing the thrust, the thrust of the quadcopter can be obtained, which is expressed as:

[0132]

[0133] Then x in the quadrotor drone body coordinate system is 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 quadrotor drone are expressed as:

[0140]

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

[0142]

[0143] Among them, [f min ,f max ] represents the upper and lower boundaries of the thrust, and Indicates the upper and lower boundaries of the three-axis torque in the body coordinate system, the subscript "min" marks the lower boundary, and the subscript "max" marks the upper boundary; in this embodiment, [f min ,f max ] is set to [0,30], the lower boundary of the three-axis torque Set to [-1,-1,-1], the upper boundary of the three-axis torque Set to [1,1,1];

[0144] Step 8. Set the cost function of sparse nonlinear model predictive control (FS-NMPC);

[0145] In this embodiment, the cost function of FS-NMPC consists of two parts, one is the error between the current state of the drone and the tracked target, and the other is the input of the drone; among them, the position tracking error of the quadrotor drone 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 angle tracking error of the quadrotor drone can be expressed as:

[0149]

[0150] in, represents the yaw angle tracking error, The discretized vector representing the yaw angle of the quadrotor drone, 0 represents the zero vector; since trajectory tracking can be achieved by changing the pitch angle and roll angle of the drone, the yaw angle of the drone is kept at 0;

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

[0152]

[0153] Among them, μ represents the integral weight of the discrete cost function, P w With Q w Respectively represent the weights of the tracking error and the control variable; in this embodiment, P w and Q w Set 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] Then the Jacobian matrix of the objective function is:

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

[0158]

[0159]

[0160] Step 9. Set the sequential quadratic programming (SQP) solver parameters for solving FS-NMPC. The SQP parameters are set to the default values.

[0161] Step 10. Set the initial value of the quadratic programming solver iteration. When SQP solves FS-NMPC for the first time, the initial value of the SQP iteration is set to all 1s; in the subsequent solution process, the initial value of the SQP iteration is set to the result of the previous solution;

[0162] Step 11. Set the initial conditions of the quadrotor drone;

[0163] In the actual flight of a quadrotor drone, the initial value of each state needs to be constrained. In the differential flat model of the drone, some states are hidden in the equations, but these state variables have been expressed in the discrete process; the discrete values ​​of the three-axis speed are given:

[0164]

[0165] Therefore, when FS-NMPC is solved cyclically, the initial value of each state can be set as:

[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 FS-NMPC for the first time, the initial conditions of the quadrotor are given by step 2. When SQP solves FS-NMPC subsequently, the initial conditions can be obtained by feedback from the sensors on the current state of the quadrotor;

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

[0172] Step 13. Calculate the control command through the Lagrange interpolation basis function and the solution result of the SQP solver;

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

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

[0175]

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

[0177] Then the control command for the quadrotor to generate thrust is:

[0178]

[0179] In the quadrotor body coordinate system, the control instructions for generating torque on the three axes are:

[0180]

[0181] Among them, T s is the control period. In this embodiment, T s Set to 0.1;

[0182] Step 14. Apply the control command obtained in step 13 to the quadrotor drone to implement sparse nonlinear model predictive control for quadrotor drone trajectory tracking;

[0183] At the same time, record the optimal solution obtained by solving FS-NMPC in this SQP. If the flight is not over, return to step 10 and use it as the initial value for the next SQP iteration. FS-NMPC is a control method with rolling optimization characteristics. In order to avoid the problem that the solver is sensitive to the initial values ​​of the optimization variables, 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: Then 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 are described below in conjunction with simulation tests.

[0185] At the end of the quadrotor flight, the tracking performance and computational efficiency are statistically analyzed. The position and yaw angle of the quadrotor are recorded in each control cycle of the quadrotor. The root mean square error (RMSE) is used as the tracking performance indicator. The calculation method of RMSE is expressed as:

[0186]

[0187] The time consumed by each SQP solution to FS-NMPC is recorded, and then the average MEAN of the consumed time is used to measure the computational efficiency of FS-NMPC.

[0188] This example compares the tracking performance of FS-NMPC with Well tuned PID and NMPC. Figure 2 The following is a three-dimensional comparison of the trajectory tracking of the UAV under PID, NMPC, and FS-NMPC control. Figure 3 The following is a comparison of the tracking errors of the UAV trajectory tracking under PID, NMPC, and FS-NMPC control. Figure 4 The following is a comparison chart of the control commands for UAV trajectory tracking under PID, NMPC, and FS-NMPC control; the detailed comparison results of the tracking error are shown in Table 4, and the detailed comparison results of the computational efficiency are shown in Table 5;

[0189] Table 4

[0190]

[0191] Table 5

[0192]

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

[0194] The above description is only a specific implementation mode of the present invention. Any feature disclosed in this specification, unless otherwise stated, can be replaced by other alternative features that are equivalent or have similar purposes; all the disclosed features, or all the steps in the methods or processes, except for mutually exclusive features and / or steps, can be combined in any way.

Claims

1. A sparse nonlinear model predictive control method based on differential flatness, characterized in that: The following steps are involved: Set the physical parameters of the quadrotor drone, the initial state of the quadrotor drone, the reference trajectory tracked by the quadrotor drone, and the total flight time of the quadrotor drone; Set the number of discrete points for sparse nonlinear model predictive control, and calculate the sparse discrete points and differential matrix within the prediction horizon; The differential flat model of the quadrotor UAV is discretized according to the sparse discrete points and differential matrix, the matrix equation of the quadrotor UAV model is obtained, and inequality constraints are set for the matrix equation; The cost function of sparse nonlinear model predictive control is set, and the sequential quadratic programming (SQP) solver is used to solve the cost function subject to matrix equation constraints and inequality constraints to obtain all control variables within the prediction horizon; The control command is calculated by Lagrange interpolation basis function and solution results, and the control command is applied to the quadrotor UAV to realize sparse nonlinear model predictive control for quadrotor UAV trajectory tracking.

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

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

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 quadrotor drone includes: the spatial position of the reference trajectory and the yaw angle of the quadrotor 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 calculating sparse discrete points and differential matrices within the prediction horizon is: The roots of the first-order derivatives of Legendre orthogonal polynomials are calculated using Newton's iteration method, expressed as: t m,step+1 =t m,step -Dt m,step t m,0 =cos(πm / N),m=0,1,…,N Among them, τ m,0 represents the initial value of the mth LGL point in the Newton iteration method, τ m,step , τ m,step+1 represents the solution of the step-th and step+1-th iterations of the m-th LGL point, Δτ m,step represents the increment of the mth LGL point in the step iteration; N represents the number of discrete points, ρ N (·) and ρ (N-1) (·) represents the terms of Legendre orthogonal polynomials; When Δτ m,step When it is less than the preset threshold, the iteration stops and N+1 LGL points are obtained. The LGL points are used as sparse discrete points and expressed as: [τ0,τ1,τ2,...,τ N-1 ,τ N ]; Through the discrete prediction horizon of the LGL point, the approximate state variables of the quadrotor drone are obtained, which can be expressed as: Among them, τ k represents the kth LGL point, k = 0, 1, ..., N; s N (τ k ) represents the approximate state variables of the quadrotor drone, Indicates N (τ k ), s(τ m ) indicates that the quadcopter is in τ m The state variables at the moment D km represents the element in the kth row and mth column of the differential matrix D, which is expressed as:

6. 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 the quadrotor drone is: The differential flat model of the quadrotor drone is expressed as: Among them, [x, y, z] represents the spatial position of the quadrotor drone. represents the Euler angle, [p,q,r] represents the angular velocity of the quadrotor drone; represents the control variable, f represents the thrust generated by the four propellers of the quadcopter, Represents the three-axis torque generated by the four propellers of the quadcopter; [I xx ,I yy ,I zz ] represents the three-axis moment of inertia in the quadcopter body coordinate system, g represents the acceleration of gravity, m q Indicates the mass of the quadrotor drone; The discrete values ​​of the three-axis acceleration and Euler angle at sparse discrete points are expressed by matrix equations: Among them, [x,y,z] represents the discretized vector of [x,y,z], express The discretized vector [t0,t f ] represents the range of the prediction horizon, t0 represents the starting time point, t f represents the termination time point, D represents the differential matrix; The discrete values ​​of angular velocity are expressed by the matrix equation: Where [p,q,r] represents the discretized vector of [p,q,r]; The discrete values ​​of the pitch angle are expressed by the matrix equation: θ=arctan(K1) The discrete values ​​of the roll angle are expressed by the matrix equation: φ=-arctan(K2) The discrete value of the thrust of the quadrotor drone is expressed by the matrix equation: The coordinate system of the quadcopter drone is x b The discrete values ​​of the torque on the shaft are expressed by the matrix equation: y b The discrete values ​​of the torque on the shaft are expressed by the matrix equation: z b The discrete values ​​of the torque on the shaft are expressed by the matrix equation: The discrete values ​​of the control variables of the quadrotor drone are expressed as: Among them, u represents the discrete value of the control variable of the quadrotor drone.

7. The sparse nonlinear model predictive control method based on differential flatness according to claim 6, characterized in that: The inequality constraints are expressed as: Among them, [f min ,f max ] represents the upper and lower boundaries of the thrust, and Indicates the upper and lower boundaries of the three-axis torque in the body coordinate system.

8. The sparse nonlinear model predictive control method based on differential flatness according to claim 1, characterized in that: In the process of using the sequential quadratic programming solver to solve the cost function subject to matrix equation constraints and inequality constraints, the parameters of the sequential quadratic programming (SQP) solver are set to the default values, the iteration initial value is set to all 1, and the iteration update value is the previous solution result.

9. 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: Among them, J represents the cost function, μ represents the integral weight of the discrete cost function, and P w With Q w They represent the weights of the tracking error and the control variable, respectively, and u represents the discrete value of the control variable of the quadrotor drone; [e x ,e y ,e z ] represents the three-axis position tracking error, Represents the yaw angle tracking error.

10. The sparse nonlinear model predictive control method based on differential flatness according to claim 1, characterized in that: The specific process of calculating the control command through the Lagrange interpolation basis function and the solution result is: Convert sparse discrete points to prediction horizons, expressed as: Among them, [τ0,τ1,...,τ N ] represents sparse discrete points, [t0,t1,...,t N ] represents the time point of the predicted horizon, [t0,t f ] represents the range of the prediction horizon, t0 represents the starting time point, t f Indicates the end time point; The control command is expressed as: Among them, f cmd Indicates the thrust control command, and Represents x in the quadcopter body coordinate system b Axis, y b Axis and z b Axis torque control command; L m (·) represents the Lagrange interpolation basis function, T s To control the cycle; Represents the discrete value of the control variable of the quadrotor drone.

Images