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

Abstract

The invention provides an unmanned aerial vehicle flight control method and system based on MPC-SO3 collaborative optimization, and electronic equipment, an MPC is used as an upper layer controller, an expected thrust is generated according to task requirements and environment information of an unmanned aerial vehicle by using the strong prediction and optimization capability of the MPC, a three-dimensional thrust output by the MPC is converted into an expected attitude, and SO3 is used as an attitude controller, so that the unmanned aerial vehicle flight control method and system based on the MPC-SO3 collaborative optimization are realized. And receiving expected attitude information generated by the MPC, accurately calculating the required control torque based on the geometrical characteristics of SO3, realizing rapid and accurate adjustment of the attitude of the unmanned aerial vehicle, inputting an attitude error as an adjustment factor into an MPC position loop controller, and dynamically adjusting a weight coefficient of an attitude item in the MPC. By adopting the layered and tightly coupled control architecture, the respective advantages of MPC and SO3 are fully played, the limitation of the prior art is overcome, the flight performance of the unmanned aerial vehicle in a complex environment is improved, and powerful technical support is provided for the application of the unmanned aerial vehicle in wider fields.

Description

Technical Field

[0001] The present invention relates to the field of unmanned aerial vehicles (UAVs), in particular to the field of UAV flight control, and more specifically to a UAV flight control method, system and electronic equipment based on MPC-SO3 collaborative optimization. Background Art

[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. Although PID control is simple and easy to implement, its parameters are difficult to adjust in 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 a certain extent, it relies on empirical rules and lacks a rigorous mathematical theoretical foundation, resulting in 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 drone in a compact and non-singular manner. Attitude control methods based on SO3 have good geometric properties and can be designed directly on the rotation group. This avoids the gimbal lock problem that can occur when representing attitude using Euler angles, while also achieving higher accuracy and efficiency when handling large rotation angles.

[0004] Model Predictive Control (MPC) is an advanced control strategy that optimizes control inputs within a limited time domain to achieve desired system outputs by building a predictive model of the system. MPC comprehensively considers the system's dynamic characteristics, constraints, and control objectives, demonstrating strong robustness and adaptability. It can handle complex multivariable constrained optimization problems in real time, providing powerful decision support for UAV flight control.

[0005] While SO3 and MPC each have their own applications in UAV flight control, control methods that effectively combine the two are currently relatively rare. Existing UAV flight control systems mostly use a single control method, or, in a multi-layered control architecture, lack close coordination between controllers at each level, resulting in poor control effectiveness. For example, during rapid maneuvering in complex environments, SO3 attitude control alone may not be able to generate the desired attitude in a timely manner, while relying solely on MPC for trajectory planning makes it difficult to accurately achieve attitude adjustment. This disjointed control approach makes it difficult for UAVs to balance flight stability and flexibility when faced with complex mission requirements, limiting their application in high-performance flight missions.

[0006] As drones gain traction in complex and dynamic scenarios like logistics and distribution, precision inspections, and emergency rescue, the performance limitations of traditional control methods under high-speed maneuvers and strong disturbances are becoming increasingly apparent. These methods often suffer from issues like insufficient control accuracy and delayed system response, making it difficult to meet the high performance requirements in complex environments. For example, in areas like automated control, unmanned systems, and intelligent manufacturing, existing technologies are unable to adequately handle external interference, dynamic changes, and the real-time correction of system errors. These issues result in poor system accuracy and stability during mission execution, making them unable to meet the high standards required in practical applications.

[0007] The background of the present invention stems from the key technical bottleneck of current UAV control in complex dynamic environments. The decoupling design of position and attitude control in the traditional cascade control architecture can no longer meet the needs of high-maneuverability flight, resulting in system response lag and trajectory tracking inaccuracy. Although the existing model predictive control (MPC) method provides optimized trajectory planning capabilities, due to the lack of closed-loop feedback on real-time attitude errors, the prediction error will continue to accumulate under external disturbances. At the same time, the control strategy with fixed parameters is difficult to adapt to the needs of variable flight missions, and an adaptive control scheme that can achieve dynamic coordination of attitude and position is urgently needed. It is against this technical background that the present invention proposes an innovative solution to the dynamic coupling problem and insufficient environmental adaptability in UAV control. Summary of the Invention

[0008] In view of the above-mentioned shortcomings of the existing UAV flight control technology, the present invention provides a UAV flight control method and system based on MPC-SO3 collaborative optimization. By combining SO3 and MPC control methods and utilizing their powerful prediction and optimization capabilities, a collaborative control method for the position loop and attitude loop is realized according to the UAV's mission requirements and environmental information.

[0009] The first aspect of the present invention discloses a UAV flight control method based on MPC-SO3 collaborative optimization, the method comprising:

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

[0011] S2: Set the MPC position loop controller based on the current flight status information 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, constructing a desired attitude rotation matrix through an orthogonalization method to obtain a desired attitude, and obtaining a desired angular velocity from the desired attitude rotation matrix;

[0013] S4: The desired attitude and the desired angular velocity are used as 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: The attitude error is normalized. When the attitude error exceeds a set threshold, the attitude error is input as an adjustment factor into the MPC position loop controller, and the weight coefficient of the attitude term in the MPC is dynamically adjusted to implement a closed-loop collaborative adjustment mechanism between the MPC and the SO3 controller, thereby obtaining control information. The control information is used as the current flight status information, and the process returns to step S2. When the attitude error does not exceed the set threshold, step S6 is executed.

[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 quantities: Position, velocity and heading angle reference value 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 of the three-dimensional thrust f b,k＝0 =[0 0 mg] T ; Read the attitude quaternion from the flight controller and convert it to the attitude R at time K k .

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

[0019] Set up the discrete-time equations for the dynamics 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 is the posture at time k+1, f b，k is the thrust vector at time K in the body coordinate system, Δt is the control system sampling period, exp([·] × ) is the exponential map on the Lie group SO(3), g is the gravitational acceleration vector, and m is the system mass;

[0024] Set the objective function:

[0025]

[0026] where w p 、w v 、w R and w f They are the weight coefficients of position error, velocity error, attitude error and control input, e R Attitude error, N is the length of the prediction time domain;

[0027] Set the control constraints, specifically expressed as:

[0028]

[0029] where v k+i is the velocity and angular velocity predicted at time k after i steps in the future; v max is the maximum speed, f max is the maximum thrust, f min For minimum thrust.

[0030] According to the UAV flight control method based on MPC-SO3 collaborative optimization described in the first aspect of the present invention, step S3 specifically includes: converting the thrust vector output by the three-dimensional thrust instruction output by 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 , construct the desired posture R by orthogonalization method d , realizing the conversion from thrust vector to desired attitude rotation matrix.

[0031] According to the UAV flight control method based on MPC-SO3 collaborative optimization according to the first aspect of the present invention, the specific setting of the SO3 attitude controller in step S4 includes:

[0032] Set the attitude error:

[0033]

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

[0035] Among them, R d is the expected posture; R k is the posture at the current moment K; e R Attitude error; e ω Angular velocity error;

[0036] Set the feedforward angular velocity:

[0037] Extract the predicted thrust at the next moment from the optimal control sequence and use it to Construct the feedforward angular velocity w d_f , which is used to assist the SO3 controller to quickly respond to thrust direction changes, is set to:

[0038]

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

[0040] Set up 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. The 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 is the proportional gain, used to amplify or reduce the attitude error; k ω is the differential gain, and J is the moment of inertia matrix.

[0044] According to the UAV flight control method based on MPC-SO3 collaborative optimization according to the first aspect of the present invention, the normalization processing in step S5 includes:

[0045] Normalize the attitude error vector in SO3 space as follows:

[0046]

[0047] Among them, e R_max is the maximum allowable attitude error, e R,norm is the normalized error.

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

[0049] Use the Sigmoid function to perform smooth mapping, dynamically generate the weight coefficient of the current moment posture error term, and convert w R The value is passed into the objective function of MPC to update the posture item weight dynamically in the objective function:

[0050]

[0051] Where: w R_min Minimum posture weight; w R_max Maximum posture weight; k slope factor, used to control the speed of weight change, e norm is the normalization factor, s0 is the offset factor, and controls the error threshold that triggers the weight increase.

[0052] The second aspect of the present invention discloses a UAV flight control system based on MPC-SO3 collaborative optimization, the system including a processing unit, which is configured to: execute the 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, wherein a computer program is stored in the memory, 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 the present invention discloses a computer-readable storage medium, which stores the UAV flight control method based on MPC-SO3 collaborative optimization described in the first aspect.

[0055] In summary, the present invention proposes a UAV flight control method, system and electronic equipment based on MPC-SO3 collaborative optimization. Through 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 realizes high-precision and high-robustness autonomous flight of UAVs in complex dynamic scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] In order to more clearly illustrate the specific embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the specific embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0057] Figure 1 It is a flow chart of the UAV flight control method in the prior art;

[0058] Figure 2 It is a flow chart of the UAV flight control method based on MPC-SO3 collaborative optimization of the present invention. DETAILED DESCRIPTION

[0059] To make the objectives, technical solutions, and advantages of the embodiments of the present invention more clear, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts shall fall within the scope of protection of the present invention.

[0060] like Figure 1 FIG. 1 is a flight control method in the prior art, which includes the following steps:

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

[0062] Step S2: Setting the position control loop;

[0063] Step S3: Setting the attitude control loop;

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

[0065] It can be seen that the core defects of the existing technology lie in the information isolation of the decoupled architecture and the error accumulation of open-loop prediction, which leads to control lag, trajectory inaccuracy and insufficient adaptability of drones in dynamic environments.

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

[0067] In current drone control architectures, the attitude controller and position controller utilize a cascaded design, operating as independent modules. Due to a lack of effective information transfer and coordination, this decoupled design prevents the dynamic response of attitude control from aligning with the real-time requirements of trajectory tracking. In highly dynamic scenarios like sharp turns and obstacle avoidance, the attitude controller cannot adapt promptly to the demands of the position controller, resulting in delayed control response and ultimately causing flight oscillation and accumulated errors.

[0068] (2) Attitude error - trajectory prediction inaccuracy caused by the lack of closed-loop feedback of 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 a drone is subject to external disturbances or internal deviations, MPC still calculates control variables based on theoretical attitudes, leading to the 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 collaborative optimization proposed by the present invention includes the following steps:

[0071] Step S1: Data input and processing

[0072] Utilizing the drone's onboard IMU (Inertial Measurement Unit), GPS, and other sensors, the system acquires real-time drone status information, including key data such as position, speed, attitude, and angular velocity. It also obtains reference information such as the desired trajectory position and speed, providing basic data support for drone control and navigation.

[0073] Step S2: MPC position loop control

[0074] 1) Kinetic model construction

[0075] Establish coupled state equations that include position, velocity, attitude angle, and their rates of change. Represent the UAV's dynamic model as a linear or nonlinear state-space model in the form of 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, which simultaneously considers position tracking error, velocity error, and attitude error. The attitude error is explicitly introduced into the cost function, and collaborative optimization is achieved through weight matrix coordination.

[0078] 3) Physical constraints

[0079] Physical constraints such as maximum speed and maximum three-dimensional thrust are directly embedded in the MPC framework. Through constraint optimization, overshoot or unstable behavior caused by ignoring constraints in traditional MPC control is avoided, ensuring that the system operates within a safe range and improving system reliability and safety.

[0080] 4) Solve rolling time domain optimization: Use real-time iterative optimization algorithm to quickly calculate the optimal control sequence, only execute the first control quantity, and re-roll optimization in the next cycle.

[0081] Step S3: Thrust attitude conversion

[0082] Using the three-dimensional thrust command output by the MPC module and the input target heading angle information, a complete desired attitude rotation matrix is ​​constructed through the orthogonalization method. The 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 directional control accuracy and response speed of the UAV during complex trajectory flight.

[0083] Step S4: SO3 attitude loop control

[0084] Based on geometric control theory, the attitude controller is designed to directly calculate attitude errors in three-dimensional rotational 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 for the aircraft, driving the drone 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, thereby generating the required roll, pitch and yaw torque to achieve attitude control of the drone.

[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 control. The attitude error is normalized and then passed as an adjustment factor to the MPC optimizer, dynamically adjusting the weight coefficient of the attitude term in the MPC. This in turn dynamically adjusts the attitude error weight in the cost function, thus achieving a closed-loop collaborative adjustment mechanism between the MPC and SO(3) controllers.

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

[0089] (1) Data input

[0090] The input includes the following reference values ​​and status quantities

[0091] 1) Reference values ​​of position, velocity and heading angle at K+ time: 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 to the attitude R at time K k .

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

[0099] (1) Kinetic model

[0100] The discrete-time dynamics model is used to predict future states in model predictive control and is a dynamic constraint that must be satisfied during the optimization process. The model describes the evolution of the position, velocity, and attitude of the UAV during each sampling period. The discrete-time equation 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 is the posture at time k+1. b，k is the thrust vector at time K in the body coordinate system, Δt is the sampling period of the control system, exp([·]× ) is the exponential map 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 is used to minimize the trajectory tracking error and control consumption of the UAV in the prediction time domain to achieve accurate and smooth control performance. This objective function comprehensively considers the deviation of position, velocity, attitude and thrust input, and balances control accuracy and energy consumption. It is specifically expressed as:

[0107]

[0108] where w p 、w v 、w R and w f are the weight coefficients of position error, velocity error, attitude error and control input respectively. N is the length of prediction time domain.

[0109] (3) Control constraints

[0110] Control constraints are used to limit the UAV state and control inputs to the physically feasible range to ensure flight safety and control stability, including speed and three-dimensional thrust amplitude limits. Specifically expressed as:

[0111]

[0112] where v k+i is the velocity and angular velocity predicted at time k after i steps in the future; v max is the maximum speed, f max is the maximum thrust, f min For minimum thrust.

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

[0114] The optimization problem is to solve the future control input sequence at each control moment to minimize the objective function while satisfying the system's dynamic equations and control constraints ((1) and (3) above). The optimal sequence in the prediction time domain is solved by the numerical optimization solver (ACADO). The control execution strategy adopts the rolling optimization method, and each optimization only executes the first step of control input at the current moment The system then updates its status and enters the next moment to re-solve the optimization problem.

[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 , construct the complete desired posture rotation matrix R through the orthogonalization method d , realizing the conversion from thrust vector to attitude target.

[0117] (1) Expected acceleration a d Transform 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 body's xy axis

[0122] Use the desired heading angle Φ k ref Construct the desired xy plane direction vector X C (Towards)

[0123]

[0124] Use Gram-Schmidt to construct an orthogonal basis:

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

[0126]

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

[0128] x B =y B ×z B

[0129] (3) Expected posture rotation matrix R d

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

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

[0132]

[0133] in is the speed at which the rotation matrix Rd changes with time, indicating 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 at the next moment 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 to quickly respond to changes in thrust direction.

[0140]

[0141] (4) Adding a feedforward term to the control law

[0142] This control law uses the attitude error feedback term and the feedforward angular velocity term to construct a composite attitude controller. The specific form is as follows:

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

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

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

[0146] It is responsible for implementing attitude tracking control and feeding back attitude errors to MPC, and constructing a closed-loop coordination mechanism between MPC and SO(3) controller, which can allocate attitude weights according to flight status.

[0147] (1) Normalized attitude error

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

[0149]

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

[0151] (2) Dynamic weight function

[0152] Use the Sigmoid function to perform smooth mapping, dynamically generate the weight coefficient of the current moment posture error term, and convert w R The value is passed into the objective function of MPC to update the posture item weight in the objective function, thereby realizing real-time adjustment of the MPC optimization process by SO(3) feedback.

[0153]

[0154] Where: w R_min Minimum pose weight;

[0155] w R_max Maximum posture weight;

[0156] k slope factor, used to control the speed of weight change

[0157] s0 offset factor, which controls the error threshold that triggers 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 a computer program is stored in the memory, 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, which stores a computer program for implementing 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. This leads to a lag in attitude dynamic response, particularly during complex flight missions, impacting flight stability and accuracy. This paper proposes a coupled coordinated control scheme that eliminates information isolation by real-time linkage between attitude and position control, enabling attitude control to respond in real time to the dynamic demands of position control. This reduces response delays and improves the system's dynamic response capabilities.

[0164] 2. Solve the problem of trajectory prediction error amplification caused by the lack of attitude error feedback

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

[0166] Please note that the technical features of the above embodiments can be combined arbitrarily. In order to make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification. The above embodiments only express several implementation methods of the present application. The description is relatively specific and detailed, but it cannot be understood as a limitation on the scope of the invention patent. It should be pointed out that for ordinary technicians in this field, without departing from the concept of this application, several variations and improvements can be made, which all fall within the scope of protection of this application. Therefore, the scope of protection of the patent in this application shall be based on the attached claims.

Claims

1. A UAV flight control method based on MPC-SO3 collaborative optimization, characterized in that: The method comprises: S1: Collect the current flight status information of the UAV, including the current position, speed and attitude data, and obtain the desired trajectory position and speed; S2: Set the MPC position loop controller based on the current flight status information 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, constructing a desired attitude rotation matrix through an orthogonalization method to obtain a desired attitude, and obtaining a desired angular velocity from the desired attitude rotation matrix; S4: The desired attitude and the desired angular velocity are used as 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; S5: The attitude error is normalized. When the attitude error exceeds a set threshold, the attitude error is input as an adjustment factor into the MPC position loop controller, and the weight coefficient of the attitude term in the MPC is dynamically adjusted to implement a closed-loop collaborative adjustment mechanism between the MPC and the SO3 controller, thereby obtaining control information. The control information is used as the current flight status information, and the process returns to step S2. When the attitude error does not exceed the set threshold, step S6 is executed. S6: Send the three-dimensional torque of the UAV obtained in step S4 to the UAV.

2. The UAV flight control method based on MPC-SO3 collaborative optimization according to claim 1 is characterized in that: The step S1 specifically includes: Input the following reference values ​​and state quantities: Position, velocity and heading angle reference value 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 of the three-dimensional thrust f b,k＝0 =[00mg] T ; Read the attitude quaternion from the flight controller and convert it to the attitude R at time K k .

3. The UAV flight control method based on MPC-SO3 collaborative optimization according to claim 2 is characterized in that: The MPC position loop controller structure in step S2 specifically includes: Set up the discrete-time equations for the dynamics model: p k+1 =p k +v k ·Δt R k+1 =R k ·exp([w k ·Δt] × ) where R k+1 is the posture at time k+1, f b，k is the thrust vector at time K in the body coordinate system, Δt is the control system sampling period, exp([·] × ) is the exponential map on the Lie group SO(3), g is the gravitational acceleration vector, and m is the system mass; Set the objective function: where w p 、w v 、w R and w f They are the weight coefficients of position error, velocity error, attitude error and control input, e R Attitude error, N is the length of the prediction time domain; Set the control constraints, specifically expressed as: where v k+i is the velocity and angular velocity predicted at time k after i steps in the future; v max is the maximum speed, f max is the maximum thrust, f min For minimum thrust.

4. The UAV flight control method based on MPC-SO3 collaborative optimization according to claim 1 is characterized in that: The step S3 specifically includes: converting the thrust vector output by the three-dimensional thrust instruction output by 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 , construct the desired posture R by orthogonalization method d , realizing the conversion from thrust vector to desired attitude rotation matrix.

5. The UAV flight control method based on MPC-SO3 collaborative optimization according to claim 1 is characterized in that: The specific configuration of the SO3 attitude controller in step S4 includes: Set the attitude error: e ω =w k -R k T R d w d Among them, R d is the expected posture; R k is the posture at the current moment K; e R Attitude error; e ω Angular velocity error; Set the feedforward angular velocity: Extract the predicted thrust at the next moment from the optimal control sequence and use it to Construct the feedforward angular velocity w d_f , which is used to assist the SO3 controller to quickly respond to thrust direction changes, is set to: in, is the thrust output by the MPC controller at time k; The MPC controller at time k predicts the thrust at time K+1; W d_f is the feedforward angular velocity; Set up 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. The specific form is as follows: τ=-k R and R -k ω and ω +J·w d_f Where τ is the three-dimensional control torque; k R is the proportional gain, used to amplify or reduce the attitude error; k ω is the differential gain, and J is the moment of inertia matrix.

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

7. The UAV flight control method based on MPC-SO3 collaborative optimization according to claim 1 is characterized in that: The dynamic adjustment of the weight coefficient of the posture item in the MPC in step S5 specifically includes: Use the Sigmoid function to perform smooth mapping, dynamically generate the weight coefficient of the current moment posture error term, and convert w R The value is passed into the objective function of MPC to update the posture item weight dynamically in the objective function: Where: w R_min Minimum posture weight; w R_max Maximum posture weight; k slope factor, used to control the speed of weight change, e norm is the normalization factor, s0 is the offset factor, and controls the error threshold that triggers the weight increase.

8. A UAV flight control system based on MPC-SO3 collaborative optimization, characterized by: The system includes a processing unit configured to execute steps for implementing the UAV flight control method based on MPC-SO3 collaborative optimization according to any one of claims 1 to 7.

9. An electronic device, characterized in that: It includes 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 according to any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that The storage medium stores the UAV flight control method based on MPC-SO3 collaborative optimization according to any one of claims 1 to 7.

Images