Unmanned aerial vehicle flight control method and system based on MPC-SO3 collaborative optimization and electronic device

Abstract

The application provides a UAV flight control method and system based on MPC-SO3 collaborative optimization and an electronic device, wherein MPC is used as an upper controller, the powerful prediction and optimization capability of the MPC is utilized, expected thrust is generated according to the task demand and environmental information of the UAV, the three-dimensional thrust output by the MPC is converted into expected attitude, SO3 is used as an attitude controller, expected attitude information generated by the MPC is received, the required control moment is accurately calculated based on the geometric characteristics of SO3, the rapid and accurate adjustment of the attitude of the UAV is realized, and the attitude error is input into a position loop controller of the MPC as an adjustment factor to dynamically adjust the weight coefficient of the attitude term in the MPC. The hierarchical and tightly coupled control architecture of the application gives full play to the respective advantages of MPC and SO3, overcomes the limitations of the prior art, and aims to improve the flight performance of the UAV in a complex environment, thereby providing strong technical support for the application of the UAV in a wider field.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicles (UAVs), particularly to the field of UAV flight control, and more specifically to a UAV flight control method, system, and electronic device based on MPC-SO3 collaborative optimization. Background Technology

[0002] With the rapid development of drone technology, it has been widely used in various fields such as aerial photography, logistics, agricultural plant protection, and military reconnaissance. The flight control system of a drone is its core component, directly determining its flight performance and stability. Traditional drone flight control methods mainly include PID control and fuzzy control. While PID control is simple and easy to implement, its parameter adjustment is difficult when facing complex flight environments and dynamic changes, making it difficult to simultaneously meet the requirements of speed and robustness. Although fuzzy control can handle nonlinear systems to some extent, it relies on empirical rules, lacks a rigorous mathematical theoretical foundation, and has limited control accuracy.

[0003] SO3 (Special Orthogonal Group of Dimension 3) is a mathematical tool for describing rotations in three-dimensional space. It can represent the attitude of a UAV in a compact and singular-free manner. Attitude control methods based on SO3 have good geometric properties, allowing control laws to be designed directly on the rotation group, avoiding the gimbal lock problem that may occur when representing attitude with Euler angles. At the same time, it has higher accuracy and efficiency when handling large-angle rotations.

[0004] Model predictive control (MPC) is an advanced control strategy that optimizes control inputs within a finite time domain by establishing a predictive model of the system to achieve the desired system output. MPC comprehensively considers the system's dynamic characteristics, constraints, and control objectives, exhibiting strong robustness and adaptability. It can handle complex multivariate constrained optimization problems in real time, providing powerful decision support for the flight control of unmanned aerial vehicles (UAVs).

[0005] While both SO3 and MPC have their applications in UAV flight control, methods that organically combine the two are still relatively rare. Most existing UAV flight control systems employ a single control method, or in multi-layered control architectures, the coordination between controllers at different layers is insufficient, resulting in poor control performance. For example, during rapid maneuvering in complex environments, relying solely on SO3 attitude control may fail to generate the appropriate desired attitude in a timely manner, while relying solely on MPC for trajectory planning struggles to achieve precise attitude adjustments. This fragmented control approach makes it difficult for UAVs to balance flight stability and flexibility when facing complex mission requirements, limiting their application in high-performance flight missions.

[0006] It is evident that with the increasing application of drones in complex and dynamic scenarios such as logistics delivery, precision inspection, and emergency rescue, the performance limitations of traditional control methods under high-speed maneuvers and strong disturbances are becoming increasingly apparent. These methods typically suffer from insufficient control precision and system response delays, making it difficult to meet the high performance requirements of complex environments. For example, in fields such as automation control, unmanned systems, and intelligent manufacturing, existing technologies cannot adequately handle external interference, dynamic changes, and real-time correction of system errors. These problems result in poor accuracy and stability of the system during task execution, failing to meet the high standards required in practical applications.

[0007] This invention stems from the critical technological bottlenecks in current UAV control in complex dynamic environments. The decoupled design of position and attitude control in traditional cascade control architectures can no longer meet the demands of high-maneuverability flight, leading to system response lag and trajectory tracking inaccuracies. While existing model predictive control (MPC) methods provide optimized trajectory planning capabilities, the lack of closed-loop feedback for real-time attitude errors allows prediction errors to accumulate continuously under external disturbances. Simultaneously, fixed-parameter control strategies struggle to adapt to the demands of varied flight missions, necessitating an adaptive control scheme that enables dynamic coordination of attitude and position. This invention, against this technological backdrop, proposes an innovative solution to address the dynamic coupling problem and insufficient environmental adaptability in UAV control. Summary of the Invention

[0008] In view of the shortcomings of existing UAV flight control technologies, this invention provides a UAV flight control method and system based on MPC-SO3 collaborative optimization. By combining SO3 and MPC, the control method utilizes their powerful prediction and optimization capabilities to achieve collaborative control of the position loop and attitude loop according to the UAV's mission requirements and environmental information.

[0009] The first aspect of this invention discloses a flight control method for unmanned aerial vehicles (UAVs) based on MPC-SO3 collaborative optimization, the method comprising:

[0010] S1: Collect the current flight status information of the UAV, including current position, speed and attitude data, and obtain the desired trajectory position and speed;

[0011] S2: Based on the current flight status information, set the MPC position loop controller and output three-dimensional thrust commands;

[0012] S3: Using the three-dimensional thrust command output by the MPC position loop controller and the input desired heading angle, construct the desired attitude rotation matrix through the orthogonalization method to obtain the desired attitude, and obtain the desired angular velocity from the desired attitude rotation matrix;

[0013] S4: The desired attitude and the desired angular velocity are used as the target inputs of the SO3 attitude controller to obtain the current attitude, compare the current attitude with the desired attitude to generate an attitude error, and combine the feedforward angular velocity compensation to obtain the three-dimensional torque of the UAV.

[0014] S5: After normalizing the attitude error, when the attitude error exceeds the set threshold, the attitude error is input as an adjustment factor into the MPC position loop controller to dynamically adjust the weight coefficient of the attitude term in the MPC, realize the closed-loop collaborative adjustment mechanism between the MPC and the SO3 controller, obtain control information, use the control information as the current flight status information, and return to step S2; when the attitude error does not exceed the set threshold, execute step S6;

[0015] S6: Send the three-dimensional torque of the UAV obtained in step S4 to the UAV.

[0016] According to the first aspect of the present invention, the UAV flight control method based on MPC-SO3 collaborative optimization is characterized in that step S1 specifically includes:

[0017] Input the following reference values ​​and state variables: position, velocity, and heading angle reference values ​​at time K: p k ref v k ref Φ k ref Position and velocity reference values ​​at time k+1: p k+1 ref v k+1 ref Current state values ​​of position, velocity, and angular velocity at time K: p k v k w k Current state values ​​of position, velocity, and angular velocity at time K+1: p k+1 v k+1 w k+1 Set the initial value f of the three-dimensional thrust. b,k＝0 = [0 0 mg] T Read the attitude quaternion from the flight controller and convert it into the attitude R at time K. k .

[0018] According to the first aspect of the UAV flight control method based on MPC-SO3 cooperative optimization, the MPC position loop controller structure in step S2 specifically includes:

[0019] Set the discrete-time equations for the dynamic model:

[0020] p k+1 =p k +vk ·Δt

[0021]

[0022] R k+1 =R k ·exp([w k ·Δt] × )

[0023] Where R k+1 It is the attitude at time k+1, f b，k Let be the thrust vector at time K in the body coordinate system, and Δt be the sampling period of the control system, exp([·] × ) is the exponential mapping on the Lie group SO(3), g is the gravitational acceleration vector, and m is the system mass;

[0024] Set the target function:

[0025]

[0026] Where w p w v w R and w f These are the weighting coefficients for position error, velocity error, attitude error, and control input, respectively. R Attitude error, where N is the prediction time domain length;

[0027] Set control constraints, specifically as follows:

[0028]

[0029] Where v k+i Let v be the velocity and angular velocity predicted i steps ahead at time k; max For the maximum speed, f max For maximum thrust, f min This is the minimum thrust.

[0030] According to the first aspect of the present invention, the UAV flight control method based on MPC-SO3 cooperative optimization, step S3 specifically includes: converting the thrust vector output by the three-dimensional thrust command from the MPC position loop controller into the desired thrust direction calculated in the inertial coordinate system, and combining it with the desired heading angle Φ. k ref The desired pose R is constructed using the orthogonalization method. d This enables the transformation from the thrust vector to the desired attitude rotation matrix.

[0031] According to the first aspect of the UAV flight control method based on MPC-SO3 cooperative optimization, the specific settings of the SO3 attitude controller in step S4 include:

[0032] Set attitude error:

[0033]

[0034] e ω =w k -R k T R d w d

[0035] Among them, R d For the desired posture; R k The pose at time K; e R Attitude error; e ω Angular velocity error;

[0036] Set the feedforward angular velocity:

[0037] Extract the predicted thrust for the next time step from the optimal control sequence, for use Construct the feedforward angular velocity w d_f The setting for assisting the SO3 controller in quickly responding to changes in thrust direction is as follows:

[0038]

[0039] in, Let be the thrust output by the MPC controller at time k; The MPC controller predicts the thrust at time K+1 for time k; W d_f It is the feedforward angular velocity;

[0040] Set the control law:

[0041] The control law is obtained by constructing a composite attitude controller using the attitude error feedback term and the feedforward angular velocity term, and its specific form is as follows:

[0042] τ=-k R e R -k ω e ω +J·w d_f

[0043] Where τ is the three-dimensional control torque; k R This is the proportional gain, used to amplify or reduce attitude errors; k ω Let J be the differential gain, and J be the moment of inertia matrix.

[0044] According to the first aspect of the UAV flight control method based on MPC-SO3 cooperative optimization, the normalization process in step S5 includes:

[0045] The attitude error vector in SO3 space is normalized as follows:

[0046]

[0047] Among them, e R_max e is the maximum permissible attitude error R,norm This represents the normalization error.

[0048] According to the first aspect of the UAV flight control method based on MPC-SO3 collaborative optimization, the dynamic adjustment of the weight coefficients of the attitude term in MPC in step S5 specifically includes:

[0049] The Sigmoid function is used to smooth the mapping, dynamically generating the weight coefficients of the attitude error term at the current time step, and w R The values ​​are passed into the objective function of MPC for updating, and the attitude term weights are dynamically updated in the objective function:

[0050]

[0051] Where: w R_min Minimum pose weights; w R_max Maximum attitude weights; k-slope factor, used to control the rate of weight change, e norm s0 is the normalization factor, s0 is the offset factor, and s0 is the error threshold that controls the trigger weight increase.

[0052] The second aspect of the present invention discloses an unmanned aerial vehicle (UAV) flight control system based on MPC-SO3 collaborative optimization, the system including a processing unit configured to execute steps for implementing the UAV flight control method based on MPC-SO3 collaborative optimization described in the first aspect.

[0053] The third aspect of the present invention discloses an electronic device, including a memory and a processor. The memory stores a computer program, and the processor is used to execute the program in the memory to implement the UAV flight control method based on MPC-SO3 collaborative optimization described in the first aspect.

[0054] The fourth aspect of this invention discloses a computer-readable storage medium that stores the UAV flight control method based on MPC-SO3 collaborative optimization described in the first aspect.

[0055] In summary, this invention proposes a UAV flight control method, system, and electronic equipment based on MPC-SO3 collaborative optimization. By using attitude-position closed-loop collaborative control and dynamic error feedback mechanism, it solves the inherent defects of traditional methods in response speed, prediction accuracy, and environmental adaptability, and ultimately achieves high-precision and robust autonomous flight of UAVs in complex dynamic scenarios. Attached Figure Description

[0056] To more clearly illustrate the specific embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the specific embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0057] Figure 1 This is a flowchart of an existing UAV flight control method;

[0058] Figure 2 This is a flowchart of the UAV flight control method based on MPC-SO3 collaborative optimization according to the present invention. Detailed Implementation

[0059] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0060] like Figure 1 The image shows a flight control method in the prior art, which includes the following steps:

[0061] Step S1: Reference trajectory and current state;

[0062] Step S2: Set up the position control loop;

[0063] Step S3: Set up the attitude control loop;

[0064] Step S4: Send the obtained control information to the drone.

[0065] It is evident that the core flaw of existing technologies lies in the information isolation of the decoupled architecture and the accumulation of errors in open-loop prediction, which leads to lag in control, inaccurate trajectory, and insufficient adaptability of UAVs in dynamic environments.

[0066] (1) System response lag caused by decoupling of attitude control and position control

[0067] In current UAV control architectures, the attitude controller and position controller employ a cascaded design, operating as independent modules. Due to a lack of effective information transmission and coordination, this decoupled design prevents the dynamic response of attitude control from keeping pace with the real-time requirements of trajectory tracking. In highly dynamic scenarios such as sharp turns and obstacle avoidance, the attitude controller cannot promptly adjust to the position controller's demands, resulting in a lag in control response and ultimately causing flight oscillations and accumulated errors.

[0068] (2) Attitude error - Trajectory prediction inaccuracy caused by lack of closed-loop feedback in position control.

[0069] Existing MPC controllers rely on idealized dynamic models for trajectory prediction, without fully considering the impact of real-time attitude errors on position control. When the UAV is affected by external disturbances or internal deviations, the MPC still calculates control quantities based on theoretical attitude, leading to a gradual accumulation of trajectory prediction errors, which in turn affects the accuracy and stability of the flight path.

[0070] Figure 2 The flowchart of the UAV flight control method based on MPC-SO3 cooperative optimization proposed in this invention includes the following steps:

[0071] Step S1: Data Input and Processing

[0072] By utilizing sensors such as the IMU (Inertial Measurement Unit) and GPS onboard the drone, real-time status information, including key data such as position, velocity, attitude, and angular velocity, can be acquired. Simultaneously, reference information such as the desired trajectory position and velocity can be obtained, providing fundamental data support for the drone's control and navigation.

[0073] Step S2: MPC Position Loop Control

[0074] 1) Construction of dynamic model

[0075] Establish coupled state equations that include position, velocity, attitude angles, and their rates of change. Represent the UAV's dynamic model as a linear or nonlinear state-space model, in the form x˙=f(x,u), where x is the state vector (including position, velocity, attitude, etc.) and u is the control input (three-dimensional thrust).

[0076] 2) Multi-objective function design

[0077] In the MPC framework, a multi-objective optimization function is designed, simultaneously considering position tracking error, velocity error, and attitude error. Attitude error is explicitly introduced into the cost function, and coordinated optimization is achieved through a weight matrix.

[0078] 3) Physical constraints

[0079] The MPC framework directly embeds physical constraints such as maximum speed and maximum three-dimensional thrust. Through constraint optimization, it avoids overshoot or instability caused by neglecting constraints in traditional MPC control, ensuring that the system operates within a safe range and improving system reliability and security.

[0080] 4) Solving the rolling time-domain optimization: The optimal control sequence is quickly calculated using a real-time iterative optimization algorithm. Only the first control variable is executed, and the rolling optimization is restarted in the next cycle.

[0081] Step S3: Thrust Attitude Conversion

[0082] Using the three-dimensional thrust command output by the MPC module and the target heading angle information input, a complete desired attitude rotation matrix is ​​constructed through the orthogonalization method. This desired attitude will be used as the target input of the SO(3) attitude controller to realize the conversion from thrust command to attitude control command, effectively ensuring the direction control accuracy and response speed of the UAV during complex trajectory flight.

[0083] Step S4: SO3 Attitude Loop Control

[0084] An attitude controller is designed based on geometric control theory, directly calculating the attitude error in a three-dimensional rotation space. By comparing the current rotation matrix with the desired rotation matrix, an error vector reflecting the attitude deviation is generated. Combined with feedforward angular velocity compensation, a nonlinear control law is designed to output torque commands to the airframe, driving the UAV to quickly converge to the desired attitude.

[0085] When the flight controller receives a three-dimensional torque command, it will control the speed difference of each motor accordingly to generate the required roll, pitch and yaw torques, thereby achieving attitude control of the UAV.

[0086] Step S5: Real-time feedback and correction

[0087] The system constructs a closed-loop feedback path using attitude data collected in real time by the IMU and flight controller. After normalization, the attitude error is used as an adjustment factor and fed into the MPC optimizer to dynamically adjust the weight coefficients of the attitude term in the MPC. This, in turn, dynamically adjusts the attitude error weights in the cost function, thus realizing a closed-loop collaborative adjustment mechanism between the MPC and the SO(3) controller.

[0088] Specifically, the data input and processing in step S1 includes:

[0089] (1) Data input

[0090] The inputs include the following reference values ​​and state variables.

[0091] 1) Reference values ​​for position, velocity, and heading angle at time K+: p k ref v kref Φ k ref ;

[0092] 2) Position and velocity reference values ​​at time k+1: p k+1 ref v k+1 ref

[0093] 3) Current state values ​​of position, velocity, and angular velocity (machine system) at time K: p k v k w k ;

[0094] 4) Current state values ​​of position, velocity, and angular velocity (machine system) at time K+1: p k+1 v k+1 w k+1 ;

[0095] 5) Set the initial value f of the three-dimensional thrust. b,k＝0 = [00mg] T ;

[0096] (2) Data processing

[0097] Read the attitude quaternion from the flight controller and convert it into the attitude R at time K. k .

[0098] The MPC position loop control in step S2 specifically includes:

[0099] (1) Dynamic model

[0100] Discrete-time dynamics models are used to predict future states in model predictive control and are dynamic constraints that must be satisfied during the optimization process. This model characterizes the evolution of the UAV's position, velocity, and attitude within each sampling period. The discrete-time equations can be expressed as:

[0101] p k+1 =p k +v k ·Δt

[0102]

[0103] R k+1 =R k ·exp([w k ·Δt] × )

[0104] Where R k+1 It represents the attitude at time k+1. b，k Let be the thrust vector at time K in the body coordinate system, Δt be the sampling period of the control system, and exp([·]× ) is the exponential mapping on the Lie group SO(3), g is the gravitational acceleration vector, and m is the system mass.

[0105] (2) Objective function

[0106] The objective function aims to minimize the trajectory tracking error and control overhead of the UAV within the prediction time domain, achieving precise and smooth control performance. This objective function comprehensively considers deviations in position, velocity, attitude, and thrust inputs, balancing control accuracy with energy consumption, and is specifically expressed as follows:

[0107]

[0108] Where w p w v w R and w f These are the weighting coefficients for position error, velocity error, attitude error, and control input, respectively. N is the prediction time domain length.

[0109] (3) Control constraints

[0110] Control constraints are used to limit the UAV's state and control inputs within physically feasible limits to ensure flight safety and control stability. These constraints include speed and three-dimensional thrust amplitude limits, specifically expressed as follows:

[0111]

[0112] Where v k+i Let v be the velocity and angular velocity predicted i steps ahead at time k; max For the maximum speed, f max For maximum thrust, f min This is the minimum thrust.

[0113] (4) Define the optimization problem and solve it.

[0114] The optimization problem is to find the optimal sequence of future control inputs at each control time point that minimizes the objective function while satisfying the system's dynamic equations and control constraints (1) and (3) above). The optimal sequence in the predicted time domain is solved using a numerical optimization solver (ACADO). The control execution strategy employs a rolling optimization method, where each optimization only executes the first step of the control input at the current moment. The system then updates its status and proceeds to the next moment to solve the optimization problem again.

[0115] Step S3: Thrust attitude conversion specifically includes:

[0116] The thrust vector of the control output is converted into the desired thrust direction calculated in the inertial coordinate system, and combined with the desired heading angle Φ. kref The complete desired attitude rotation matrix R is constructed using the orthogonalization method. d This enables the conversion from thrust vector to attitude target.

[0117] (1) Expected acceleration a d Transformation from thrust vector to inertial coordinate system:

[0118]

[0119] (2) Calculate the desired thrust direction z B :

[0120]

[0121] (3) Use the desired angle to construct the xy axis of the machine body

[0122] Using the desired orientation angle Φ k ref Construct the desired xy-plane direction vector X C (Orientation)

[0123]

[0124] Constructing orthogonal bases using Gram-Schmidt:

[0125] y-axis direction (lateral vector of the body) B ):

[0126]

[0127] x-axis direction (body forward vector X) B ):

[0128] x B =y B ×z B

[0129] (3) Desired attitude rotation matrix R d

[0130] R d =[x B y B z B ]

[0131] (4) Calculate the desired angular velocity w d

[0132]

[0133] in Rd is the rate at which the rotation matrix changes over time, representing the rate of change of the rotation matrix; Tr is the trace.

[0134] Step S4SO(3) attitude loop control specifically includes:

[0135] (1) Definition of attitude error

[0136]

[0137] e w =w k -R k T R d w d

[0138] (2) Feedforward angular velocity

[0139] Extract the predicted thrust for the next time step from the optimal control sequence. Used to construct the feedforward angular velocity w d_f (Body coordinate system), the parameters can be used to assist the SO(3) controller in responding quickly to changes in thrust direction.

[0140]

[0141] (4) Add feedforward term to control law

[0142] This control law combines attitude error feedback and feedforward angular velocity terms to construct a composite attitude controller, which takes the following form:

[0143] τ=-k R e R -k ω e ω +J·w d_f

[0144] Where τ is the three-dimensional control torque; k R This is the proportional gain, used to amplify or reduce attitude errors; k ω Let J be the differential gain, and J be the moment of inertia matrix.

[0145] Step S5, real-time feedback and correction, specifically includes:

[0146] It is responsible for implementing attitude tracking control and feeding back the attitude error to the MPC. A closed-loop collaborative mechanism between the MPC and the SO(3) controller is constructed, which can allocate attitude weights according to the flight state.

[0147] (1) Normalized attitude error

[0148] The attitude error vector in SO(3) space is normalized to ensure that the error value is within a preset controllable range.

[0149]

[0150] Where e R_max This is the maximum set attitude error.

[0151] (2) Dynamic weighting function

[0152] The Sigmoid function is used to smooth the mapping, dynamically generating the weight coefficients of the attitude error term at the current time step, and w R The values ​​are passed into the objective function of MPC to update the attitude term weights dynamically in the objective function, thereby realizing the real-time adjustment of the MPC optimization process by SO(3) feedback.

[0153]

[0154] Where: w R_min Minimum pose weights;

[0155] w R_max Maximum pose weight;

[0156] The slope factor k is used to control the rate of change of the weights.

[0157] s0 offset factor controls the error threshold that triggers the weight increase.

[0158] The present invention also provides a UAV flight control system based on MPC-SO3 collaborative optimization, which is used to implement the aforementioned UAV flight control method based on MPC-SO3 collaborative optimization.

[0159] The present invention also provides an electronic device, including a memory and a processor, wherein the memory stores a computer program, and the processor is used to execute the program in the memory to implement the aforementioned UAV flight control method based on MPC-SO3 collaborative optimization.

[0160] The present invention also provides a computer-readable storage medium storing a computer program that implements the aforementioned UAV flight control method based on MPC-SO3 collaborative optimization.

[0161] In summary, the UAV flight control method based on MPC-SO3 collaborative optimization proposed in this invention can achieve the following effects:

[0162] 1. Solve the response lag problem caused by the decoupling design of attitude control and position control.

[0163] Existing decoupled designs result in inconsistent attitude and position control, particularly in complex flight missions, where attitude dynamic response lags, impacting flight stability and accuracy. This invention proposes a coupled and coordinated control scheme that eliminates information isolation through real-time linkage between attitude and position control, enabling real-time response of attitude control to dynamic position control demands. This reduces response latency and improves the system's dynamic response capability.

[0164] 2. Address the issue of amplified trajectory prediction errors caused by missing attitude error feedback.

[0165] Because existing MPC controllers fail to consider the impact of real-time attitude errors on position control, trajectory prediction errors accumulate and amplify. This invention proposes an improved MPC control scheme that introduces closed-loop feedback of attitude errors into the control loop to correct trajectory predictions in real time, enhancing the accuracy and robustness of the control system and ensuring trajectory tracking accuracy and system stability for UAVs in complex environments. Based on the method of this invention, UAVs achieve higher trajectory tracking accuracy in complex environments: coupling optimization significantly reduces position / attitude errors; stronger robustness: adapting to dynamic environments based on predictive capabilities and constraint handling; and greater safety: ensuring system stability based on explicit constraints and avoiding actuator saturation risks.

[0166] Please note that the technical features of the above embodiments can be combined arbitrarily. For the sake of brevity, not all possible combinations of the technical features in the above embodiments have been described. However, as long as the combination of these technical features does not contradict each other, it should be considered within the scope of this specification. The above embodiments only illustrate several implementation methods of this application, and their descriptions are relatively specific and detailed, but they should not be construed as limiting the scope of the invention patent. It should be pointed out that for those skilled in the art, several modifications and improvements can be made without departing from the concept of this application, and these all fall within the protection scope of this application. Therefore, the protection scope of this patent application should be determined by the appended claims.

Claims

1. A method for flight control of a UAV based on MPC-SO3 collaborative optimization, characterized in that, The method includes: S1: Collect the current flight status information of the UAV, including current position, speed and attitude data, and obtain the desired trajectory position and speed; S2: Based on the current flight status information, set the MPC position loop controller and output three-dimensional thrust commands; S3: Using the three-dimensional thrust command output by the MPC position loop controller and the input desired heading angle, construct the desired attitude rotation matrix through the orthogonalization method to obtain the desired attitude, and obtain the desired angular velocity from the desired attitude rotation matrix; S4: Using the desired attitude and the desired angular velocity as the target inputs of the SO3 attitude controller, the current attitude is obtained, the current attitude is compared with the desired attitude to generate an attitude error, and combined with feedforward angular velocity compensation, the three-dimensional torque of the UAV is obtained. S5: After normalizing the attitude error, when the attitude error exceeds the set threshold, the attitude error is input as an adjustment factor into the MPC position loop controller to dynamically adjust the weight coefficient of the attitude term in the MPC, realize the closed-loop collaborative adjustment mechanism between the MPC and the SO3 controller, obtain control information, use the control information as the current flight status information, and return to step S2; when the attitude error does not exceed the set threshold, execute step S6; S6: Send the three-dimensional torque of the UAV obtained in step S4 to the UAV; Specifically, the settings for the SO3 attitude controller in step S4 include: Set attitude error: in, R d The desired posture; R k The pose at time K; e R Attitude error; e ω Angular velocity error; Set the feedforward angular velocity: Extract the predicted thrust for the next time step from the optimal control sequence, for use Constructing feedforward angular velocity w d_f The setting for assisting the SO3 controller in quickly responding to changes in thrust direction is as follows: in, Let be the thrust output by the MPC controller at time k; The MPC controller predicts the thrust at time K+1 for time k; W d_f For feedforward angular velocity, To control the sampling period of the system; Set the control law: The control law is obtained by constructing a composite attitude controller using the attitude error feedback term and the feedforward angular velocity term, and its specific form is as follows: in, τ For three-dimensional control torque; k R This is the proportional gain, used to amplify or reduce attitude errors; For differential gain, J Here is the rotational inertia matrix; The dynamic adjustment of the weight coefficients of the attitude term in MPC in step S5 specifically includes: use Sigmoid The function performs a smooth mapping, dynamically generating the weight coefficients of the attitude error term at the current moment, and... w R The values ​​are passed into the objective function of MPC for updating, and the attitude term weights are dynamically updated in the objective function: in: w R_min Minimum pose weights; w R_max Maximum pose weight; k The slope factor is used to control the rate of change of the weights. This refers to the normalization factor obtained after normalizing the attitude error vector in space. s 0 Offset factor controls the error threshold that triggers the weight increase.

2. The UAV flight control method based on MPC-SO3 collaborative optimization according to claim 1, characterized in that, Step S1 specifically includes: Input the following reference values ​​and state variables: position, velocity, and heading angle reference values ​​at time K: p k ref 、v k ref Φ k ref Position and velocity reference values ​​at time k+1: p k+1 ref 、v k+1 ref ; K Current state values ​​of position, velocity, and angular velocity at any given moment: p k 、v k 、 w k； K+1 Current state values ​​of position, velocity, and angular velocity at any given moment: p k+1 、v k+1 、w k+1 Set the initial value of the three-dimensional thrust. Read the attitude quaternion from the flight controller and convert it to... K The posture of the moment R k .

3. The UAV flight control method based on MPC-SO3 collaborative optimization according to claim 2, characterized in that, The MPC position loop controller structure in step S2 specifically includes: Set the discrete-time equations for the dynamic model: in R k+1 It is time k+1 At all times, f b，k In the body coordinate system K The thrust vector at time t, For the sampling period of the control system, For the exponential mapping on the Lie group SO(3), g The vector of gravitational acceleration. m For system quality; Set the target function: in w p 、w v 、w R and w f These are the weighting coefficients for position error, velocity error, attitude error, and control input. e R Attitude error, N To predict the length of the time domain; Set control constraints, specifically as follows: in v k+i For the future predicted at time k i Velocity and angular velocity after the step; v max For maximum speed, f max For maximum thrust, f min This is the minimum thrust.

4. The UAV flight control method based on MPC-SO3 collaborative optimization according to claim 1, characterized in that, Step S3 specifically includes: converting the thrust vector output by the three-dimensional thrust command from the MPC position loop controller into the desired thrust direction calculated in the inertial coordinate system, and combining it with the desired heading angle. Φ k ref The desired pose is constructed using an orthogonalization method. R d This enables the transformation from the thrust vector to the desired attitude rotation matrix.

5. The UAV flight control method based on MPC-SO3 collaborative optimization according to claim 1, characterized in that, The normalization process in step S5 includes: The attitude error vector in SO3 space is normalized as follows: in, e R_max The maximum permissible attitude error, e R,norm This represents the normalization error.

6. A UAV flight control system based on MPC-SO3 collaborative optimization, characterized in that, The system includes a processing unit configured to perform steps for implementing the UAV flight control method based on MPC-SO3 collaborative optimization as described in any one of claims 1-5.

7. An electronic device, characterized in that: It includes a memory and a processor, wherein the memory stores a computer program, and the processor executes the program in the memory to implement the UAV flight control method based on MPC-SO3 collaborative optimization as described in any one of claims 1-5.

8. A computer-readable storage medium, characterized in that, The storage medium stores the UAV flight control method based on MPC-SO3 collaborative optimization as described in any one of claims 1-5.

Images