Differential flat control method for quadrotor UAV based on optimized LQR

Abstract

The present invention discloses a differential flat control method for a quad-rotor UAV based on optimized LQR, the steps of which include: the system receives the control reference of the UAV from the planning end, and obtains the physical constraint information of the UAV from the actual environment; constructs a constraint function based on the LQR algorithm to limit the maximum value of the control instruction acceleration; adds constraint conditions to determine the feasible solution range of the optimization problem; limits the acceleration instruction according to the UAV thrust margin calculated in real time, solves the optimal acceleration output under the constraint conditions, and achieves the control target of the UAV in a complex environment while satisfying the physical constraint information of the UAV. The method of the present invention combines the reference target and the physical constraint, and introduces the optimization algorithm of NMPC into the acceleration vector calculation of the differential flat controller based on the advantages of excellent differential flat control performance and low computing power overhead, so as to ensure that the UAV can achieve efficient and safe movement in a complex environment.

Description

Technical Field

[0001] The present invention relates to the technical field of quadrotor UAV motion control, and particularly relates to a differential flatness control method for quadrotor UAVs based on optimized LQR. Background Art

[0002] In the control of quadrotor UAVs, it is a common and mature control method to perform proportional-integral-differential (PID) or linear quadratic regulator (LQR) control after linearizing the quadrotor model. Linearizing the complex nonlinear dynamic model into a simple linear model can significantly reduce the difficulty of controller design. However, these two control methods also have many limitations. First, the effectiveness of the linearized model highly depends on the approximation assumption near the operating point, that is, the yaw angle, pitch angle, and roll angle of the UAV need to be kept within a small range. When the UAV performs large-angle flight operations, such as rapid rolling or violent maneuvers, this linearization assumption often fails, resulting in a large deviation between the model and the actual system dynamics. In addition, linear controllers are inherently difficult to handle complex aerodynamic disturbances and significant nonlinear dynamic characteristics during high-speed flight, such as the coupling effect between rotors and the nonlinear variation of air resistance. These factors will reduce the robustness of the controller, thereby affecting the stability and response performance of the system.

[0003] In the prior art, based on the differential flatness controller (DFBC), significant improvement in trajectory tracking performance has been shown during high-speed flight. The differential flatness property is a dynamic system attribute that allows the system to directly calculate the control input from the desired trajectory without solving complex dynamic equations. In the control of quadrotor UAVs, the differential flatness control method directly calculates the attitude, angular velocity, and acceleration from the given trajectory to achieve high-precision tracking of the predetermined trajectory. However, since this method does not inherently rely on optimization solutions, it cannot guarantee the optimality of the control input or the satisfaction of the constraint conditions. Therefore, additional optimization strategies may be required to ensure the optimality of the control input when facing operation constraints.

[0004] Another control algorithm is model predictive control (NMPC), which can predict the future states of multiple time steps and calculate the corresponding control inputs. Thanks to the progress of hardware and nonlinear optimization solvers in recent years, nonlinear model predictive control based on full-state dynamics can meet the real-time requirements. NMPC can make full use of the performance of the UAV and consider actuator saturation constraints. However, it is still very challenging to run NMPC on hardware with limited computing power. In addition, NMPC may face numerical convergence problems when the system delay is large or the model error is significant, manifested as a high instability or collapse rate. It can be seen that NMPC is more suitable for high-dynamic task scenarios that require full exploration of platform performance and handling of dynamically infeasible trajectories.

[0005] Therefore, how to introduce the optimization algorithm of NMPC into the calculation of the acceleration vector of the differential flat controller of a quadrotor UAV, while reducing the computing power overhead and enabling the quadrotor UAV to adaptively obtain the optimal control effect in different complex environments, remains to be further studied. Summary of the Invention

[0006] The problem to be solved by the present invention is to provide a differential flat control method for a quadrotor UAV based on optimized LQR, which introduces the NMPC optimization algorithm into the calculation of the acceleration vector of the differential flat controller, while reducing the computing power overhead and enabling the quadrotor UAV to adaptively obtain the optimal control effect in different complex environments.

[0007] The present invention adopts the following technical solutions: A differential flat control method for a quadrotor UAV based on optimized LQR, comprising the following steps:

[0008] Step 1, data acquisition: The control system receives the control reference quantity of the UAV from the planning end and obtains the physical constraint information of the UAV from the actual environment; constructs the body coordinate system and rotation matrix of the quadrotor UAV, and calculates the expected acceleration of the UAV through the control reference quantity;

[0009] Step 2, construct a constraint function: The control system combines the control reference quantity and physical constraints to generate the control command of the UAV and calculates the acceleration of the control command; constructs a constraint function based on the LQR algorithm to limit the maximum value of the acceleration of the control command;

[0010] Step 3, add constraints: Add the physical constraint conditions of the UAV to determine the feasible solution range of the optimization problem; the constraint conditions are real-time linear restrictions, including obstacle avoidance constraints in path planning and restrictions of the dynamic model;

[0011] Step 4, solve for the optimal acceleration: According to the acceleration command limited by the thrust margin of the UAV calculated in real time, the control system uses the interior point algorithm to solve for the real-time optimal acceleration that meets the physical constraint conditions of the UAV, transfers the optimal acceleration to the differential flat controller for control, and outputs the expected attitude to the flight control software for tracking to achieve the control target of the UAV in a complex environment.

[0012] Preferably, in step 1, the control reference quantity provides the expected flight trajectory of the UAV, which is obtained through trajectory planning calculation from the planning end and includes the reference speed and reference acceleration of the UAV.

[0013] Define the reference coordinate system in the world coordinate, denoted as ; Define the body coordinate system of the UAV, denoted as , the body coordinate system is fixed to the quadrotor UAV, and the origin coincides with the position of the center of mass of the UAV coincide;

[0014] The quadrotor UAV is subject to the action of the gravitational acceleration g in the negative direction. The rotation of the body coordinate system relative to the world coordinate system is represented by the rotation matrix . , represents the rotation matrix from the body coordinate system to the world coordinate system;

[0015] Calculate the desired acceleration vector of the quadrotor UAV through the control reference quantity.

[0016] Preferably, in step 1, the physical constraint information, obtained from the actual environment, is the state variable of the UAV in the current environment, including: maximum speed, maximum acceleration, and motion ability limit. The specific method is as follows:

[0017] Step 1.1: Obtain the maximum lift generated by the rotors of the quadrotor UAV, subtract the gravity, and obtain the thrust margin that can currently provide agile flight for the UAV;

[0018] Step 1.2: By obtaining the current state of the UAV, map the thrust margin to the body coordinate system of the UAV to obtain the dynamic constraint of the thrust, which is used to add dynamic boundary conditions when solving the objective function, so that the solved objective value does not exceed the maximum physical limit.

[0019] Preferably, in step 2, considering the influence of gravitational acceleration and air resistance, calculate the acceleration of the UAV control command ;

[0020] Describe the rotation of the quadrotor in the z-x-y order, and define the intermediate variables and . From the acceleration and the reference yaw angle , calculate the three-axis orthogonal unit vectors corresponding to the aircraft attitude .

[0021] Preferably, in step 2, select the three-axis position and speed as the state variables, select the acceleration as the input variable, construct a constraint function based on the LQR algorithm, and describe the LQR optimization as:

[0022] ;

[0023] Among them, the objective function is used to minimize the state cost and input energy, represents the prediction horizon, represents the state variable at the th moment, denotes the optimized variable at the moment, denotes the state variable at the terminal moment; the subscripts Q and R represent matrices and , the matrix and are diagonal matrices, and the diagonal elements represent the weights assigned to the state variables or inputs. The weight values indicate the convergence speed of the corresponding states or inputs; The subscript of represents the terminal condition, and the matrix is a diagonal matrix related to the terminal state.

[0024] Preferably, in step 3, add the physical constraint conditions of the drone, and the expression is:

[0025] ;

[0026] ;

[0027] ;

[0028] ;

[0029] Among them, formulas (11b) and (11c) are the initial state constraint and the dynamic constraint respectively, represents the initial value of the state at which the optimizer starts iteration, denotes the value of the optimized variable at the moment, is the system matrix and is the input matrix;

[0030] Formulas (11d) and (11e) represent the amplitude limits of the state and the input respectively, , represent the upper and lower bounds of the state variable respectively, represent the upper and lower bounds of the optimized variable respectively.

[0031] Preferably, in step 4, limit the acceleration command according to the real-time calculated thrust margin of the drone. The specific method is as follows:

[0032] Obtain the lift and the maximum lift corresponding to the current throttle of the drone motor. When applying the dynamic constraint, convert the thrust margin of the drone into the thrust components in the three-axis directions, denoted as ;

[0033] Convert the thrust margin into the acceleration margin, and solve to obtain the current acceleration margin of the drone, which is used as the optimized variable The physical constraints added for solving the optimization function in formula (11e); the acceleration margin is substituted into formula (11e) to calculate the boundary conditions of the dynamic target function, and the optimized is obtained to ensure that the target value obtained by solving the target function does not exceed the maximum physical limit.

[0034] Preferably, in step 4, the interior point algorithm is specifically as follows:

[0035] The state equation and control input of the quadrotor system are modeled by CasADi software, the optimization objective function and constraint conditions are constructed, and the LQR problem is transformed into an optimization problem;

[0036] The interior point method solver provided by CasADi software is used to solve the optimization problem, and the control input is iteratively optimized in each control cycle until it converges to the optimal solution;

[0037] The state of the UAV control system is updated according to the optimal control input to ensure the trajectory tracking accuracy and system stability.

[0038] Compared with the prior art, the present invention adopts the above technical solutions and has the following technical effects:

[0039] 1. The differential flat control method for quadrotor UAVs based on optimized LQR of the present invention is based on the advantages of excellent differential flat control performance and low computing power overhead. An optimization algorithm based on NMPC is introduced into the calculation of the acceleration vector of the differential flat controller, which makes up for the problem that the traditional DFBC controller cannot obtain the optimal control output according to the environment in real time; since the calculation of the acceleration vector itself is linear, the constructed optimized LQR problem will not bring the problem of the linearization failure of the nonlinear model of the quadrotor UAV in the classical LQR problem, and at the same time, the solution of the linear model will not cause the problem of excessive computing power overhead of the optimization algorithm on the edge computing unit, greatly increasing the control tracking performance.

[0040] 2. The differential flat control method for quadrotor UAVs based on optimized LQR of the present invention introduces the optimization problem into the differential flat control algorithm. By combining the reference target and physical constraints, the quadrotor UAV can adaptively obtain the optimal control effect in different complex environments and achieve efficient and safe movement. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] Figure 1 is the flow chart of the differential flat control method based on optimized LQR of the present invention;

[0042] Figure 2 is the flow chart of obtaining the physical constraints of the quadrotor UAV of the present invention;

[0043] Figure 3Schematic diagram of the coordinate system definition and propeller serial number definition of the present invention;

[0044] Figure 4 Comparison diagram of the trajectory tracking effect of the embodiment of the present invention;

[0045] Figure 5 Comparison diagram of the root mean square error of trajectory tracking of the embodiment of the present invention. Detailed implementation manners

[0046] In order to make the objectives, technical solutions and advantages of the present invention clearer, the technical solutions of the application will be further elaborated in detail below with reference to the accompanying drawings. The described embodiments are only a part of the embodiments involved in the present invention. All non-innovative embodiments of other researchers in the field on this embodiment fall within the protection scope of the present invention. At the same time, for the step numbers in the embodiments of the present invention, they are only set for the convenience of elaboration and explanation, and no limitation is imposed on the order between the steps. The execution order of each step in the embodiments can be adaptively adjusted according to the understanding of those skilled in the art.

[0047] In an embodiment of the present invention, a differential flatness control method for a quadrotor UAV based on optimized LQR is as Figure 1 shown, and includes the following steps:

[0048] Step 1, data acquisition: The control system receives the control reference quantities of the UAV from the planning end, including the reference speed or reference acceleration, and these reference quantities provide the expected flight trajectory for the UAV. At the same time, the control system obtains the physical constraint information of the UAV from the environment, and these constraints may include the maximum acceleration, maximum speed, and other limitations of the motion capabilities. These information are used as inputs together to provide a basis for the construction of the subsequent constraint function.

[0049] Step 2, construct the constraint function: The constraint function is the core part of the optimization problem, which combines the control reference quantities and physical constraints to ensure that the generated control instructions can both achieve the goal and meet the physical limitations of the UAV in practical applications. For example, in a high-speed flight mission, the constraint function will limit the maximum value of the acceleration to avoid exceeding the hardware capabilities of the UAV.

[0050] Step 3, add constraints: After constructing the constraint function, the control system further adds constraint conditions, which clarify the feasible solution range of the optimization problem. Among them, the added constraints are real-time linear limitations, such as the obstacle avoidance constraints in path planning or the limitations of the dynamic model.

[0051] Step 4, Solve for the optimal acceleration: The control system uses the interior point method as the optimization algorithm to solve for the optimal acceleration output under the constraint conditions; the interior point method is a linear / nonlinear optimization method that can efficiently handle optimization problems with complex constraints, and the finally generated acceleration output not only satisfies the physical constraints but also effectively achieves the control objectives of the UAV.

[0052] In this embodiment, obtaining the physical constraints of the UAV from the environment, such as Figure 2 shown, the main steps include:

[0053] First, obtain the maximum lift that the UAV rotor can generate, subtract the gravity, and obtain the thrust margin that can provide agile flight currently;

[0054] Secondly, by obtaining the current UAV state, map the thrust margin to the UAV body coordinate system to obtain the dynamic constraints of the thrust.

[0055] In this embodiment, vectors are represented by bold lowercase letters, and matrices are represented by bold uppercase letters. Unless otherwise specified, the rest of the symbols represent scalars.

[0056] Adopt an orthogonal basis Define a reference coordinate system represented by the world coordinate system At the same time, adopt an orthogonal basis Define a body coordinate system represented by the body coordinate system These two coordinate systems are both described in the world coordinate.

[0057] The body coordinate system is fixed to the quadrotor UAV, and its origin coincides with the center of mass of the UAV. As Figure 3 shown, the quadrotor UAV is affected by the gravitational acceleration g acting in the negative direction.

[0058] Furthermore, in this embodiment, the symbol represents the position of the center of mass of the UAV, and its corresponding derivatives (velocity , acceleration , jerk , snap ) are used for description. The rotation of the body coordinate system relative to the world coordinate system is represented by the rotation matrix , and can also be represented by the quaternion . When describing the rotation, represents the rotation matrix from the body coordinate system to the world coordinate system.

[0059] In this embodiment, the subscript is used to represent the reference value obtained from the reference trajectory, and the subscript Used to represent the expected value calculated by the outer loop controller, with subscript Used to represent the control instruction value directly passed to the underlying controller for calculation.

[0060] In traditional outer loop control, the expected acceleration vector of the quadrotor UAV Is calculated from the position error, velocity error, and reference trajectory, and the expression is as follows:

[0061] ;

[0062] Where, Are the acceleration, position, and velocity values directly obtained from the reference trajectory, and these values are usually calculated through trajectory planning; Are the current state variables obtained from the sensor or estimator respectively, Is the proportionality coefficient used to determine the influence degree of the error on the acceleration.

[0063] Furthermore, the expected acceleration Ignores the influence of gravitational acceleration and air resistance, so it cannot be directly used for attitude calculation or underlying control execution. The acceleration Used to directly calculate the attitude or underlying control instruction is expressed as:

[0064] ;

[0065] Where, Represents the influence of the gravitational acceleration g acting on the quadrotor UAV along the negative Direction, Represents the transpose of the rotation matrix , Represents the drag coefficient during the flight of the UAV.

[0066] In this embodiment, the rotation of the quadrotor is described in the order of z - x - y. Therefore, the intermediate variables And Are defined and expressed as:

[0067] ;

[0068] Where, Is the reference yaw angle obtained from the reference trajectory.

[0069] Furthermore, in the body coordinate system, , And Must be orthogonal to each other and all be unit vectors. Under this constraint, the rotation matrix Of the UAV control instruction is used as:

[0070] ;

[0071] It is expressed as:

[0072] ;

[0073] Among them, represents the aircraft attitude calculated from the acceleration and the reference yaw angle corresponding to the three-axis orthogonal unit vectors.

[0074] LQR is an optimal control algorithm aiming to minimize the quadratic cost function and achieve a balance between system performance and control energy. In this embodiment, the control input is further calculated by solving a set of linear equations to ensure the stability and optimality of the system response.

[0075] To apply LQR to control the acceleration, the three-axis position and velocity are selected as the state variables, and the acceleration is selected as the input variable, as shown in Formulas (9) and (10):

[0076] ;

[0077] Among them, represents the position variable of the center of mass of the quadrotor in the world coordinate system, represents the velocity variable of the center of mass of the quadrotor in the world coordinate system, represents the acceleration variable of the center of mass of the quadrotor in the world coordinate system; respectively represent the state variables and optimization variables of the optimization function .

[0078] Therefore, the LQR optimization is described as:

[0079] ;

[0080] Among them, the objective function in Formula (11a) is used to minimize the state cost and input energy, and the prediction horizon will affect the accuracy of the predicted output. A larger value can provide more accurate prediction, but will increase the computational cost at the same time; represents the state variable at the th moment, represents the optimization variable at the th moment, represents the state variable at the terminal moment.

[0081] Matrix and is a diagonal matrix, whose diagonal elements represent the weights assigned to the state variables or inputs. A larger weight value indicates that the corresponding state or input can converge faster; the matrix is a diagonal matrix related to the terminal state.

[0082] The initial state constraint and the dynamic constraint are represented in equations (11b) and (11c) respectively, and equations (11d) and (11e) represent the amplitude limits of the state and input.

[0083] According to equations 3 and 4, the system matrix and the input matrix are expressed as:

[0084] ;

[0085] where represents the period of the controller, and the missing elements in the matrix are 0.

[0086] Furthermore, in flight missions in high-dynamic or complex environments, there are often problems such as excessive acceleration commands causing thrust saturation or insufficient power of the UAV. The core of this problem lies in the difficulty for the UAV to meet its physical constraints in high-dynamic or complex environments.

[0087] During flight, if the acceleration command is too large, it may cause the required thrust to exceed the maximum output capacity of the motor and propeller, resulting in a thrust saturation problem. Thrust saturation will cause the actual output force to be unable to meet the control requirements, thereby causing deviations in the attitude and trajectory of the aircraft, and even potentially leading to out-of-control situations.

[0088] Therefore, by imposing dynamic constraints, the acceleration command can be limited according to the real-time calculated thrust margin, avoiding the problem of trajectory deviation caused by thrust saturation.

[0089] The thrust margin is the additional force that can provide agile flight after the four rotors of the UAV rotate to counteract gravity. It is a scalar, but when imposing dynamic constraints, it needs to be converted into the thrust components in the three-axis directions, denoted as .

[0090] ;

[0091] Convert the thrust margin into an acceleration margin:

[0092] ;

[0093] Add the calculated range constraint of to the constraint equation (11e) of the optimization problem to ensure that the solved objective value does not exceed the maximum physical limit, thereby improving the accuracy of trajectory tracking.

[0094] Finally, the obtained optimal acceleration is passed to the differential flatness controller for control, and the desired attitude is output and transmitted to the Pixhawk flight control software for tracking, so as to achieve the control objective of the UAV in a complex environment.

[0095] Furthermore, the LQR method with constraints added in the present invention is compared with the classical controller in terms of trajectory tracking on a real aircraft, as Figure 4 and Figure 5 shown.

[0096] Figure 4 shows the "8" - shaped trajectory tracking effect within a 2*2 range. It can be seen that the LQR algorithm with constraints added proposed in the present invention has better tracking performance than the traditional DFBC algorithm, and the trajectory curve is more consistent with the desired curve.

[0097] Figure 5 shows the root - mean - square error (RMSE) of the calculated trajectory tracking. It can be seen that the root - mean - square error of the LQR method with constraints added proposed in the present invention is lower than the tracking error of the traditional DFBC controller, and the tracking effect is better.

[0098] It can be seen that the differential flatness control method for quadrotor UAVs based on optimized LQR proposed in the present invention combines the reference target and physical constraints. Based on the advantages of excellent differential flatness control performance and small computing power overhead, the optimization algorithm of NMPC is introduced into the calculation of the acceleration vector of the differential flatness controller, which can ensure the efficient and safe movement of the quadrotor UAV in a complex environment.

[0099] The above - mentioned are only the preferred embodiments of the present invention. It should be pointed out that for those of ordinary skill in the art, without departing from the principle of the present invention, several improvements and modifications can be made, and these improvements and modifications should also be regarded as the protection scope of the present invention.

Claims

1. A differential flat control method for a quadrotor drone based on optimized LQR, characterized in that: The steps include: Step 1: Data collection: The control system receives the control reference of the UAV from the planning end and obtains the physical constraint information of the UAV from the actual environment; constructs the body coordinate system and rotation matrix of the quadcopter UAV, and calculates the expected acceleration of the UAV through the control reference; Step 2: Construct constraint function: The control system generates the control command of the UAV by combining the control reference and the physical constraint, and calculates the control command acceleration; constructs the constraint function based on the LQR algorithm to limit the maximum value of the control command acceleration; Considering the influence of gravity acceleration and air resistance, calculate the acceleration of the drone control command , the expression is: ； in, is the expected acceleration of the UAV; To control the reference speed in the reference quantity, Indicates that the quadcopter is subject to negative The influence of gravitational acceleration g in the direction, Represents the rotation matrix The transpose of Indicates the drag coefficient of the drone during flight; Use zxy sequence to describe the rotation of the quadrotor and define intermediate variables and , the expression is: ； in, is the reference yaw angle obtained from the reference trajectory; Rotation matrix in body coordinate system The coordinates of the drone body , and They are mutually orthogonal and are all unit vectors, and the expression is: ； Rotation matrix of drone control instructions , the expression is: ； in, Expressed by acceleration and the reference yaw angle The calculated aircraft attitude The corresponding three-axis orthogonal unit vectors; The constraint function is constructed based on the LQR algorithm as follows: Step 2.1, select the three-axis position and speed As state variable, choose acceleration As input variables, the expression is: ； in, Represents the position variable of the center of mass of the quadrotor drone in the world coordinate system, represents the velocity variable of the center of mass of the quadrotor drone in the world coordinate system, Represents the acceleration variable of the center of mass of the quadrotor drone in the world coordinate system; Represent the constraint functions state variables and optimization variables; Step 2.2: Create a constraint function and describe the LQR optimization as: ； Among them, the objective function For minimizing state cost and input energy, represents the prediction time domain, Indicates The state variables at time, Indicates The optimization variable at time, State variables representing terminal moments; Subscript Q , R Representation Matrix and ,matrix and It is a diagonal matrix. The diagonal elements represent the weights assigned to the state variables or inputs. The weight values ​​indicate the convergence speed of the corresponding state or input. Subscript Represents the terminal status, the matrix is the diagonal matrix associated with the terminal states; Step 3, adding constraints: adding physical constraints of the drone to determine the range of feasible solutions to the optimization problem; the constraints are real-time linear constraints, including obstacle avoidance constraints in path planning and constraints of the dynamics model; Step 4. Solve for the optimal acceleration: Based on the real-time calculated UAV thrust margin limit acceleration instruction, the control system uses the interior point algorithm to solve the real-time optimal acceleration that meets the physical constraints of the UAV, and pass the optimal acceleration to the differential flat controller for control. The output of the desired attitude is passed to the flight control software for tracking, thereby achieving the control target of the UAV in a complex environment.

2. The differential flat control method of a quadrotor drone based on optimized LQR according to claim 1, characterized in that: In step 1, the control reference quantity provides the expected flight trajectory for the UAV, which is obtained from the planning end through trajectory planning calculation, including the reference speed and reference acceleration of the UAV; The reference coordinate system is defined in world coordinates and is expressed as ; Define the drone's body coordinate system, expressed as The body coordinate system is fixed to the quadrotor drone, and the origin and the center of mass of the drone are coincide; The quadcopter drone is subject to The gravitational acceleration g in the direction of the body coordinate system is rotated relative to the world coordinate system by the rotation matrix express, .

3. The differential flat control method of a quadrotor drone based on optimized LQR according to claim 2 is characterized in that: In step 1, the physical constraint information of the drone is obtained from the actual environment. The physical constraint information is the state variables of the drone in the current environment, including: maximum speed, maximum acceleration, and motion capacity limit. The specific method is as follows: Step 1.1, obtain the maximum lift generated by the quadcopter rotor, subtract gravity, and obtain the current thrust margin for the drone flight; Step 1.2: By obtaining the current state of the UAV, the thrust margin is mapped to the UAV body coordinate system to obtain the dynamic constraint of the thrust, which is used to add dynamic boundary conditions when solving the objective function so that the target value does not exceed the maximum physical limit.

4. The differential flat control method of a quadrotor drone based on optimized LQR according to claim 2 is characterized in that: In step 1, the desired acceleration vector of the quadrotor drone is calculated by controlling the reference quantity, and the expression is: ； in, is the expected acceleration of the UAV; To control the reference speed in the reference quantity, To control the reference acceleration in the reference quantity, for location; are the current position and speed of the UAV obtained by the sensor or estimator respectively; A proportionality factor that determines how much the error affects acceleration.

5. The differential flat control method for a quadrotor drone based on optimized LQR according to claim 1, characterized in that: In step 3, add the physical constraints of the drone, the expression is: ； ； ； ； Among them, formula (11b) and formula (11c) are the initial state constraint and dynamic constraint respectively. Indicates the initial value of the state at the start of the optimizer iteration. Indicates The optimization variable value at time is the system matrix, is the input matrix; Formula (11d) and Formula (11e) represent the state and input limits respectively, , Respectively represent the upper and lower bounds of the state variable, They represent the upper and lower bounds of the optimization variable respectively.

6. The differential flat control method for a quadrotor drone based on optimized LQR according to claim 5 is characterized in that: System Matrix and the input matrix , the expression is: ； in, Represents the period of the controller, and the missing elements in the matrix are 0.

7. The differential flat control method for a quadrotor drone based on optimized LQR according to claim 6 is characterized in that: In step 4, the acceleration command is limited according to the real-time calculated UAV thrust margin. The specific method is as follows: Get the lift and maximum lift corresponding to the current throttle of the drone motor. When applying dynamic constraints, convert the drone thrust margin into thrust components in the three-axis direction, recorded as : ； in, Indicates the current throttle value of the drone. Indicates the lift size corresponding to the current throttle. It indicates the maximum combined lift provided by the motor manufacturer when the motor runs at the maximum speed. Represents the rotation matrix from the body coordinate system to the world coordinate system; Convert the thrust margin to acceleration margin: ； in, Indicates the current acceleration margin of the drone; represents the mass of the quadrotor; The acceleration margin obtained is , put it into formula (11e), calculate the dynamic boundary conditions of the objective function, and get the optimized , which is used to ensure that the target value obtained when solving the objective function does not exceed the maximum physical limit.

8. The differential flat control method for a quadrotor drone based on optimized LQR according to claim 1, characterized in that: In step 4, the interior point algorithm is as follows: The state equation and control input of the quadrotor system are modeled by CasADi software, the optimization objective function and constraints are constructed, and the LQR problem is transformed into an optimization problem. The optimization problem is solved using the interior point solver provided by the CasADi software, and the control input is iteratively optimized in each control cycle until convergence to the optimal solution; Update the drone control system status according to the optimal control input to ensure trajectory tracking accuracy and system stability.

Images