Rotor unmanned aerial vehicle cooperative obstacle avoidance control method based on nonlinear model predictive control

Abstract

The present application belongs to the technical field of unmanned aerial vehicle system control and simulation, and relates to a rotor unmanned aerial vehicle cooperative obstacle avoidance control method based on nonlinear model predictive control. The present application realizes control and online quadratic optimization of lift and three-axis torque received by the unmanned aerial vehicle under the constraint conditions of controlled variables and state variables, and realizes obstacle avoidance trajectory tracking of the unmanned aerial vehicle. Specifically, the high-order system of the unmanned aerial vehicle is reduced in order to process the established high-order system of the rotor unmanned aerial vehicle into a low-order system, so as to ensure real-time performance of subsequent control; an obstacle avoidance path is planned, a feasible trajectory of the rotor unmanned aerial vehicle is planned in real time under the scene with multiple random static obstacles; and a trajectory tracking control is performed, wherein the controller adopts a nonlinear model predictive controller, the optimal control input of the current control period is determined by solving a quadratic programming problem with constraints and is applied to the nonlinear system, so as to realize accurate tracking of the feasible reference path.

Description

Technical Field

[0001] This invention belongs to the field of unmanned aerial vehicle (UAV) system control and simulation technology, and relates to a cooperative obstacle avoidance control method for rotary-wing UAVs based on nonlinear model predictive control. Background Technology

[0002] As a foundation for other applications of drones, real-time obstacle avoidance and trajectory tracking control play a crucial role in the execution of drone missions. With the development of drone technology and its increasingly wide operating range, more stringent requirements are being placed on the real-time tracking and control systems of drones.

[0003] The core functions of an unmanned aerial vehicle (UAV) tracking and control system can be summarized as: precise and rapid real-time position control of the UAV; and ensuring that the UAV's speed, acceleration, and other parameters remain within dynamic constraints. Model predictive control (MMC) is a proactive constraint-handling control method that can satisfy various variable constraints during UAV trajectory control. MMC requires an accurate predictive model to optimize the system's future dynamics. However, rotary-wing UAVs are high-order nonlinear systems. Directly using this nonlinear model as the predictive model would result in excessively slow online solutions to nonlinear optimization problems, making it difficult to meet the real-time requirements of the control system. Conversely, if a linearized model of this nonlinear system is used as the predictive model, the linearization error will prevent the predictive model from accurately reflecting the UAV's dynamics during motion, leading to a significant decrease in the accuracy of the control system.

[0004] Therefore, a controller with high real-time performance is needed to achieve path tracking control of UAVs, while satisfying the conditions of high tracking accuracy and not violating motion and attitude constraints. Summary of the Invention

[0005] To ensure accurate and rapid tracking of a provided reference obstacle avoidance path by a UAV in an environment with multiple random static obstacles, considering the UAV's dynamics, operational constraints, and dynamic limitations, this invention proposes a cooperative obstacle avoidance control method for rotary-wing UAVs based on nonlinear model predictive control. This invention uses a nonlinear high-order system model of the rotary-wing UAV and an artificial potential field method for path planning. The nonlinear high-order system is linearized in real-time and its order reduced in a distributed manner during each sampling period. The optimal obstacle avoidance path is updated in real-time during each control period, establishing a distributed reduced-order model predictive control system for the UAV, enabling the control system to track the UAV's dynamic trajectory. The linearized distributed reduced-order predictive model of the UAV system is adjusted according to the dynamic changes of the nonlinear model in each sampling period, avoiding the accumulation of linearization errors. This meets the control system's requirements for the accuracy and real-time performance of UAV trajectory planning, reducing online computation, ensuring real-time performance, and improving the control performance of the control system for the UAV's nonlinear system.

[0006] The technical solution of this invention: A cooperative obstacle avoidance control method for rotary-wing unmanned aerial vehicles based on nonlinear model predictive control includes: Downgrading of high-order systems for rotary-wing UAVs: The established high-order system of the rotary-wing UAV is processed to reduce it to a low-order system to ensure the real-time performance of subsequent control. Obstacle avoidance path planning: In scenarios with static obstacles, a feasible trajectory for the rotary-wing drone is planned in real time, which ensures that the drone completes the obstacle avoidance task while accurately reaching the target point. Trajectory tracking control: A nonlinear model predictive controller is used to determine the optimal control input for the current control cycle by solving a constrained quadratic programming problem and apply it to the nonlinear system to achieve accurate tracking of the feasible reference path for obstacle avoidance path planning.

[0007] The specific steps are as follows: S1. Reduce the order of a high-order rotorcraft UAV system using distributed control. The inputs to the high-order rotorcraft UAV system model are lift and rotational torque, and the outputs include position information and attitude angles. Utilizing the principle of distributed control, the high-order rotorcraft UAV system model is divided into two subsystems: motion equations and attitude equations. The coupling variables of the two subsystems are used as information exchange between the two distributed subsystems, resulting in a distributed reduced-order system.

[0008] S2. A feasible obstacle avoidance path is planned using the artificial potential field method. Real-time path planning requires the current position and attitude angle of the rotorcraft UAV to obtain the reference position and path for the next sampling period. The artificial potential field method establishes a potential field based on the current position of the rotorcraft UAV. The potential field is divided into a gravitational field about the target point and a repulsive field about the obstacle to obtain the optimal obstacle avoidance path.

[0009] S3. Design a nonlinear model predictive controller to achieve trajectory tracking control. The data stored in the controller includes: the obstacle avoidance path obtained in step S2, the constraints of the control system, and the initial values. The nonlinear model predictive controller is designed based on a real nonlinear system. Based on each sampling period, the high-order system of the rotorcraft UAV is linearized and then the distributed order reduction in step S1 is performed sequentially to obtain a linearized distributed order-reduced system. The nonlinear model predictive controller constructs an objective function based on factors such as the difference between the expected steady state of the system in the current sampling period and the state of the linearized distributed order-reduced system, the difference between the input and the expected input, and the deviation between the expected steady-state output trajectory and the reference path. Using the position and attitude constraints of the rotorcraft UAV and the actual input limitations as optimization constraints, the optimal control input vector for the current control period is obtained by solving a constrained quadratic programming problem online, and then applied to the actual system to complete the control of this sampling period, achieving accurate tracking of the feasible reference path for obstacle avoidance path planning.

[0010] Furthermore, the specific steps of S1 are as follows: S1.1 For the high-order nonlinear state-space model of rotary-wing UAVs: in, The state vector includes the three-dimensional position coordinates and velocity of the rotary-wing UAV, as well as the attitude angles and angular velocities of rotation in the three-dimensional coordinate system. n The dimension of the state vector; The input vector includes the total lift of the rotary-wing UAV and the rotational torque on the three coordinate axes. m The system input vector dimension; The output vector includes the position coordinates of the rotary-wing UAV. q The system input vector dimension; and These are the nonlinear state function and the output function, respectively. and These are the nonlinear state function and output function, which are separated into their respective state variables; The input matrix for the state equations, This is the direct transfer matrix of the output equation.

[0011] S1.2, a higher-order nonlinear state-space model based on S1.1, based on points. Linearization is performed to obtain a linearized higher-order system: in: In the formula, For the linearization point of a higher-order nonlinear system, and The state matrix and output matrix of the high-order nonlinear system at the linearization point, respectively; State points The linearized state error and output error; Controllable It can be observed.

[0012] Therefore, based on the above linearized higher-order model, we can obtain... Discrete higher-order linearized systems with linearized points: in: , , , , In the formula, The sampling period of a high-order discrete system. Sampling time, It is the identity matrix. and These are the state matrix and input matrix of the discrete system, respectively. and These are the output matrix and direct transfer matrix of the discrete system, respectively. and These are the state error and output error of the discrete system, respectively. Controllable The observation period is the same as the subsequent control period, which is 1. .

[0013] S1.3. Based on the discrete higher-order system in S1.2, the state matrix of the higher-order discrete system can be decomposed into the following form: , , , , in, and For subsystem The state matrix, and For subsystem The output matrix, and For subsystem Subsystem The interaction influence matrix and , and These are the linearized state errors of the two subsystems, respectively. and These are the state vector dimensions of the UAV motion subsystem and the state vector dimensions of the UAV attitude subsystem, respectively. and These are the linearized state errors of the two subsystems, respectively. and These are the output vector dimensions of the UAV motion subsystem and the UAV attitude subsystem, respectively.

[0014] To ensure the real-time performance of subsequent control, the discrete high-order system needs to be reduced in order through a distributed process. The reduced-order distributed system is as follows: in, , This is the state vector of the UAV motion subsystem, containing the UAV's three-dimensional coordinates, position, and velocity variables. Let be the state vector of the UAV attitude subsystem, containing the rotation angle and angular velocity variables of the UAV's three-dimensional coordinates, and ; This is the input control vector for the motion model, specifically the total lift force acting on the UAV. Let be the dimension of the input vector of the UAV motion subsystem; This is the input control vector for the attitude model, specifically the rotational torque of the UAV along the three coordinate axes. Let be the dimension of the input vector of the UAV attitude subsystem, and ; This is the output vector of the UAV motion subsystem. Let be the output vector of the UAV attitude subsystem, and .

[0015] Furthermore, the specific steps of S2 are as follows: S2.1 The path direction extension of the reference path is achieved by dividing the yaw angle. and pitch angle To achieve: , in, To divide the number, The unit extension angle for the yaw angle. This is a unit expansion angle for the pitch angle. It has the same expansion step size for every possible angular direction. R Assume the current position coordinates are... Then the coordinates of the possible candidate expansion positions for the next step. for: in, Therefore, the set of coordinates of the possible candidate expansion positions for the next step is defined as follows: S2.2 For the obstacle avoidance part of path planning, the artificial potential field method is used to determine the direction of movement of the next path, as follows: in, Let S2.1 be any point among the possible candidate points obtained. Virtual guide point For the first k Time Candidate Point The attraction The gravitational coefficient, For the first j The obstacle is for the first k Candidate points for real-time drones repulsive force, The net external force acting on the current point; For the first j The obstacle is for the first k Candidate points for real-time drones distance, This acts as the boundary of the repulsive field. The repulsive field generated by the obstacle only has an effect on the drone when the distance between the obstacle and the drone is less than this value. is the repulsion coefficient.

[0016] S2.3 After multiple drones achieve obstacle avoidance, the drone coordination phase begins, i.e., when the drones reach the virtual guidance point. Subsequently, multiple drones are required to synchronously approach the target point. At this point, the cooperative reference path is generated as follows: in, The cooperative radius of the drone, i.e., the distance from the target point. Less than At this time, the drones enter the collaborative phase; For drones The cycle number at which the coordination phase begins. For drones exist Cooperative reference path points at any time For drones Extended reference path yaw angle for entering the coordination phase.

[0017] S2.4 Based on the extended path method set in S2.1, during the obstacle avoidance phase, the net external force in each extended direction is calculated using S2.2. F Then, the point with the largest resultant external force is selected from the remaining candidate expansion positions as the next expansion point, forming an expansion reference path; in the coordination phase, the respective expansion reference paths of the rotary-wing UAVs are formed through S2.3.

[0018] Furthermore, the specific steps for S3 are as follows: S3.1 Based on the extended reference path generated in S2, a nonlinear model predictive control (MMDC) method with real-time linearization is used to construct the controller. This involves implementing the linearization and discretization steps in S1 for each sampling period. To reduce steady-state error, the nonlinear MMDC adopts an incremental form. Therefore, the incremental form of the distributed linearized discrete system is as follows: in, These are two subsystems of a distributed linearized discrete system. For incremental subsystem i The state vector, For incremental subsystem i The input increment vector; These are the state matrix and input matrix of the incremental state equation, respectively. This is the error propagation matrix of the incremental state equation. For subsystem State vector pairs of subsystems Communication matrix; This is the output matrix of the incremental subsystem. The specific forms of the above vectors and matrices are as follows: , , , , Therefore, the state constraints of the original system and control input constraints , and Let the state constraint set and control input constraint set be the same for a high-order nonlinear system. Then, the state constraint adjustment for a distributed reduced-order incremental system is as follows: ,in: and Distributed reduction subsystem The set of state constraints and the set of control constraints; For distributed reduced-order incremental subsystems The set of state constraints.

[0019] S3.2. Based on S3.1 and S3.2, the equilibrium point of the distributed reduced-order incremental system is: in, For distributed reduced-order incremental systems at the linearization point The set of all steady-state points, For subsystem The steady-state point.

[0020] S3.3, based on S3.3, the prediction and control time domains are respectively... and The quadratic programming problem consisting of the online optimization objective function and its corresponding constraints is as follows: in, , For subsystem The model predicts the value function of the controller's optimization problem. For subsystem The output vector at the steady state point, Set values ​​for the output vector of the corresponding subsystem's reference obstacle avoidance path. For subsystem The weight coefficients of the optimization problem and , It is a positive definite weighting matrix. At sampling time... k Solve the subsystem i The quadratic programming problem P The optimal control increment of the next iteration At that time, it is necessary to communicate with related subsystems. j The state prediction sequence of the previous iteration was exchanged during communication. and control increment sequence For the previous sampling time k -1, Solving the subsystem i Optimal control quantity obtained from quadratic programming problem Then, the optimal control quantity at the current moment can be solved based on the incremental model in S3.1. .

[0021] S3.4. Based on the form of the objective function, the general form of the system's predicted output, input sequence, state sequence, and their relationships is as follows: in, , For subsystem exist k The initial value at time , and Subsystems Augmented vectors for all states and incremental controls in the prediction time domain; and They are respectively augmentation subsystems The state matrix and input matrix of the state equation, and For augmented subsystems and The communication coefficient matrix, For augmented subsystems The error propagation matrix of the state equations. Its specific form is as follows: , , , in, , , as a subsystem exist Time Prediction The state vector after the step; , , as a subsystem exist Time Prediction Control increment vector after step; superscript T It is the transpose of a matrix or vector.

[0022] Based on S3.4, it can be determined that the two distributed reduced-order subsystems have a coupled portion, therefore the two augmented subsystems are merged: in, To merge the state vectors of the system, To merge the control input increments of the system, For the system in The initial state at time 1. To incorporate the system's state errors, , , and The coefficient matrix of the merged system takes the following form: , , , , , , Based on the system state space obtained by the merging transformation, a decoupling transformation is performed to obtain the subsystem. i State-space prediction model: in, , , , , , , To decouple subsystems i The coefficient matrix of the state equations, and is , and This is obtained by dividing the data into blocks according to the dimensions of the two subsystems.

[0023] S3.6. Based on S3.4 and S3.5, the objective function for online optimization is described in the following simplified form: Among them, the simplified subsystem i Augmented state Weight matrix and Augmented steady state The definition is as follows: , , Take optimization vector After expansion, the pairs were merged and eliminated. After removing the constant term which has no effect, the online optimization objective function can be described in the following quadratic form: Based on the derivation, the parameter matrix of the optimization problem in the above equation is defined as follows: , , , in, and , The definitions are the same, both being ordinal numbers of the subsystems. Based on the derivation, the weight matrix of the optimization problem after simplification is (…). , ) and steady-state matrix ( The format is as follows: , , For the state constraint part of the quadratic optimization It can be expressed in the following form: in, and These are the upper and lower limits of the state variable, respectively. This state constraint constrains both the state of the original system and the control input of the original system.

[0024] Based on the objective function and constraints, linearization and distributed order reduction are performed for each sampling period. Online rolling optimization is achieved by solving a constrained quadratic programming problem online, constructing a real-time linearized model predictive controller. The optimal control increment sequence can be obtained from the optimized vector, and the first element of the sequence is taken. The control quantity compared to the previous control cycle Summing yields the optimal control input for the current control cycle. .

[0025] The beneficial effects of this invention are: (1) This invention performs distributed order reduction on high-order nonlinear systems by reducing the high-order system into two distributed low-order subsystems. Compared with the order reduction method of projection, the order reduction scheme proposed in this invention communicates the coupled angular velocities through communication between the two subsystems. While improving the real-time performance of the system, it also retains the dynamic information of all variables of the nonlinear system relatively completely, and avoids the loss of tracking accuracy to a certain extent.

[0026] (2) This invention utilizes the artificial potential field method and a collaborative approach for path planning. In the first half, the artificial potential field method is used for real-time obstacle avoidance. Within a certain range close to the target point, collaborative planning is performed to ensure the speed of obstacle avoidance and the synchronicity of collaboration. At the same time, the dynamic constraints of the UAV are also taken into account to ensure the possibility of the actual UAV performing actions during the obstacle avoidance process.

[0027] (3) The real-time linearized model predictive controller designed in this invention performs real-time linearization on the sampling points of each cycle. When the dynamics of the UAV change, the nonlinear high-order system will change, and the cumulative linearization error of the linearized system that only targets the equilibrium point will increase, thus reducing the accuracy of the controller. The real-time linearized model predictor reduces the accumulation of linearization error and ensures control accuracy; by using a linearized predictive model, the real-time performance of the system is guaranteed to the greatest extent. Attached Figure Description

[0028] Figure 1 This is the control principle diagram of the present invention.

[0029] Figure 2 This is a schematic diagram of drone obstacle avoidance and tracking under simulation condition 1.

[0030] Figure 3 This is a schematic diagram of drone obstacle avoidance and tracking under simulation condition 2.

[0031] Figure 4 The curves show the trajectory tracking changes of UAV 1 under different obstacle and initial position conditions when the target position is the same, i.e., simulation condition 1 and simulation condition 2.

[0032] Figure 5 The curves show the trajectory tracking changes of UAV No. 2 under different obstacle and initial position conditions when the target position is the same, i.e., simulation condition 1 and simulation condition 2.

[0033] Figure 6 When the target position is the same, under different obstacle conditions and initial positions, i.e., simulation condition 1 and simulation condition 2, the vertical lift of UAV No. 1 is... Change comparison curve.

[0034] Figure 7 When the target position is the same, under different obstacle conditions and initial positions, i.e., simulation condition 1 and simulation condition 2, the vertical lift of UAV No. 2 is... Change comparison curve.

[0035] Figure 8 When the target position is the same, under different obstacle conditions and initial positions, i.e., simulation condition 1 and simulation condition 2, the three-axis torque of UAV No. 1 is as follows: Change comparison curve.

[0036] Figure 9 This refers to the three-axis torques of UAV No. 2 under different obstacle and initial position conditions, i.e., simulation conditions 1 and 2, when the target position is the same. Change comparison curve.

[0037] Figure 10 This is a schematic diagram of drone obstacle avoidance and tracking under simulation condition 3.

[0038] Figure 11 The curves show the trajectory tracking changes of UAV No. 1 under different obstacle and target positions when the initial position is the same, i.e., simulation condition 1 and simulation condition 3.

[0039] Figure 12 The curves show the trajectory tracking changes of UAV No. 2 under different obstacle and target positions when the initial position is the same, i.e., simulation conditions 1 and 3.

[0040] Figure 13 When the initial position is the same, under different obstacle and target positions, i.e., simulation condition 1 and simulation condition 3, the vertical lift of UAV No. 1 is... Change comparison curve.

[0041] Figure 14 When the initial position is the same, under different obstacle and target positions, i.e., simulation condition 1 and simulation condition 3, the vertical lift of UAV No. 2 is... Change comparison curve.

[0042] Figure 15When the initial position is the same, under different obstacle and target positions, i.e., simulation condition 1 and simulation condition 3, the three-axis torque of UAV No. 1 is as follows: Change comparison curve.

[0043] Figure 16 This refers to the three-axis torques of UAV No. 2 under different obstacle and target positions, assuming the same initial position (i.e., simulation conditions 1 and 3). Change comparison curve.

[0044] Note: (1) Simulation condition 1: The initial position and target position of UAV No. 1 are respectively and The initial position and target position of UAV No. 2 are respectively and .

[0045] (2) Simulation condition 2: The initial position and target position of UAV No. 1 are respectively and The initial position and target position of UAV No. 2 are respectively and .

[0046] (3) Simulation condition 3: The initial position and target position of UAV No. 1 are respectively and The initial position and target position of UAV No. 2 are respectively and . Detailed Implementation

[0047] The specific embodiments of the present invention will be further described below with reference to the accompanying drawings and technical solutions.

[0048] This embodiment is a cooperative obstacle avoidance control method for rotary-wing UAVs based on nonlinear model predictive control. The basic principle is as follows: Figure 1 As shown, the following example uses a quadcopter drone for illustration, including the following steps: Step 1: Reduce the order of the high-order system using distributed control. The inputs to the quadcopter UAV are the lift and rotational torque, and the outputs include position, velocity, attitude angle, and attitude angular velocity. Utilizing distributed principles, the high-order system model of the UAV is divided into two subsystems, resulting in a distributed reduced-order system. The drone in this embodiment is a quadcopter drone, and its dynamic characteristics depend on the external forces and torques acting on it. For this high-order nonlinear system, the external forces acting on the drone are taken as... and rotational torque and The three-dimensional position coordinates of the UAV are taken as the input variables for the prediction model. These are the output variables of the prediction model.

[0049] The high-order nonlinear state-space model of an unmanned aerial vehicle (UAV) is denoted as: (1) in This is the state vector, whose elements are the position coordinates and velocity of the quadcopter UAV, as well as the rotation attitude angle and angular velocity. The input vector consists of the total lift of the UAV. and rotational torque in three-dimensional coordinates ; This is the output vector, whose elements are the position coordinates of the quadcopter drone. In this example... It is a non-linear output function. The nonlinear state function is shown below: (2) in, and These represent the three-dimensional position and velocity of the quadcopter drone. This is the rotation matrix that transforms the UAV's body coordinate system to the geodetic coordinate system. This matrix is ​​related to the UAV's attitude angles. For the quality of the drone itself, It is the acceleration due to gravity; and These are the attitude angle and angular velocity of the quadcopter drone, respectively. Let be the rotational inertia vector of the UAV in three-dimensional coordinates. The three-dimensional rotational torque of the quadcopter drone, The gyroscopic torque generated by the UAV during rotor rotation is represented in three-dimensional coordinates, and its specific form is as follows: (3) in, Let be the moment of inertia of the rotor. This represents the relative rotational speed of the rotor. In this example, the parameter values ​​are shown in the table below:

[0050] Based on formulas (2) and (3), at point Linearization is performed to obtain a linearized higher-order system: (4) in: (5) In the formula, State points The linearized state error and output error; Controllable It can be observed.

[0051] Therefore, based on formulas (4) and (5), we can obtain Linearized discrete model of points: (6) in: , , , , and Controllable It can be observed.

[0052] In this example, the discretized sampling period is used. =0.2 s The same as the subsequent control cycle.

[0053] Based on the above discrete high-order system, in order to ensure the real-time performance of subsequent control, it is necessary to perform distributed order reduction on the high-order system: (7) in, , , , , , in, At that time, the system shown in formula (6) is the motion subsystem of the quadcopter UAV; When, the system shown in formula (6) is the rotating subsystem of a quadcopter UAV. This is the state vector of the UAV motion model, containing the UAV's three-dimensional coordinates, position, and velocity variables. This is the state vector of the UAV attitude model, which includes the rotation angle and angular velocity of the UAV's three-dimensional coordinates; This is the input control vector for the motion model, whose elements are the total lift force acting on the UAV. This is the input control vector for the attitude model, whose elements are the rotational torques of the three coordinate axes of the UAV; and These are the linearized state errors of the two subsystems, respectively. and These are the linearized state errors of the two subsystems, respectively.

[0054] Step 2: In scenarios with multiple obstacles, the artificial potential field method is used to plan obstacle avoidance paths and obtain the reference path for subsequent UAV trajectory tracking and control.

[0055] The path direction extension of the reference path in this invention is achieved by dividing the yaw angle and pitch angle: , (8) It has the same expansion step size for each possible angle direction. R Assume the current position coordinates are... Then we can obtain the three-dimensional coordinates of the next candidate expansion position. : (9) in, Therefore, the set of coordinates of the possible candidate expansion positions for the next step is defined as follows: (10) In this example, take , .

[0056] For the non-cooperative planning part of path planning, the artificial potential field method is used to determine the direction of motion of the next path. The principle is as follows: (11) in, Virtual guide point For the current candidate point The attraction The gravitational coefficient, For the first j The obstacle is for the first k The repulsive force of candidate positions for real-time drones. For the first j The obstacle is for the first k Distance to candidate locations of real-time drones This acts as the boundary of the repulsive field. The repulsive field generated by the obstacle only has an effect on the drone when the distance between the obstacle and the drone is less than this value. This represents the repulsion coefficient. Based on the principle of the artificial potential field method, the resultant external force is calculated for each path point. .

[0057] In this example, , , .

[0058] After multiple drones successfully avoided obstacles, the drones entered the drone coordination phase, which occurs when a drone reaches a virtual guidance point. Subsequently, multiple drones are required to synchronously approach the target point. The cooperative reference path of the drones will be generated as follows: (12) Distance from target point Less than At this time, the drones enter the collaborative phase; For drones The cycle number at which the coordination phase begins. For drones exist Coordinated reference path points at any given time.

[0059] In this example, , , .

[0060] Step 3: Based on the linearization method in Step 1, a real-time linearized model predictive control method is adopted, that is, the linearization and discretization shown in Step 1 are performed for each sampling period; the obstacle avoidance path based on Step 2 is used as the reference path to be tracked subsequently. The real-time linearized model predictive controller in this example uses an incremental approach for prediction.

[0061] So, regarding the cycle , define the first k Within a cycle Increment for: (13) Assume the initial conditions of the current sampling period are So, the incremental distributed order reduction model for quadcopter drones: (14) in: , , , , Therefore, the state constraints of the original system and control input constraints It can be adjusted according to the distributed order reduction system. ,in: (15) The equilibrium point of the distributed reduced-order discrete system, based on the model of formula (14), is: (16) In this example, for the model predictive controller, the prediction time domain is taken. and control time domain The quadratic programming problem consisting of the online optimization objective function and its corresponding constraints is as follows: (17) in, Set the value for the subsystem output vector corresponding to the obstacle avoidance path obtained in step 2. and It is a positive definite weighting matrix. At sampling time... k Solve the subsystem i The above quadratic programming problem P The optimal control sequence for the next iteration At that time, it is necessary to communicate with related subsystems. j The state prediction sequence of the previous iteration was exchanged during communication. and control sequence For the previous sampling time k -1, Solving the subsystem i Optimal control quantity obtained from quadratic programming problem Then, the optimal control quantity at the current moment can be solved based on the incremental model in S3.1. .

[0062] In this example, the value is... .

[0063] Based on the form of the objective function, the general form of the system's predicted output, input sequence, state sequence, and their relationships can be integrated as follows: (18) The matrix form of the integrated incremental augmented subsystem shown in formula (17) is as follows: (19) (20) (twenty one) (twenty two) (twenty three) (twenty four) (25) Based on the incremental augmented subsystems of formulas (18)-(25), the two subsystems are merged and transformed: (26) in, , , , , , , .

[0064] Based on the system state space shown in formula (26) obtained by the merging transformation, a decoupling transformation is performed to obtain the subsystem. i State-space prediction model: (27) in, , , , , , , To decouple subsystems i The coefficient matrix of the state equations, and is , and This is obtained by dividing the data into blocks according to the dimensions of the two subsystems.

[0065] Combining formulas (16), (17), and (27), the objective function for online optimization can be described in the following simplified form: (28) The simplified matrix form of the online optimization objective function is as follows: (29) , , , (30) Take optimization vector According to formulas (28), (29) and (30), after expansion, the pairs are merged and eliminated. After removing the constant term which has no effect, the online optimization objective function can be described in the following quadratic form: (31) The matrix coefficients of the online optimization objective function (31) are as follows: , (32) , , (33) The weight matrix of the optimized problem after simplification is as follows: , , (34) For the state constraint part of the quadratic optimization It can be expressed in the following form: (35) (36) in, and These are the upper and lower limits of the state variable, respectively. This state constraint constrains both the state of the original system and the control input of the original system.

[0066] Based on the objective function, as shown in formula (31), and the constraints as shown in formulas (35) and (36), linearization and distributed order reduction are performed for each sampling period, as shown in formulas (4)-(7) of step 1. Online rolling optimization is achieved by solving the constrained quadratic programming problem online, and a real-time linearized model predictive controller is constructed. The optimal control increment sequence can then be obtained, and the first element of the sequence is taken. The control quantity compared to the previous control cycle Summing yields the optimal control input for the current control cycle. .

[0067] To further illustrate the effectiveness of this embodiment, the following simulation experiment verifies the effectiveness of the nonlinear model predictive control method for obstacle avoidance of rotary-wing UAVs in this invention.

[0068] To verify the UAV control system's ability to track a reference path and adapt to different obstacles and starting points, the lift of the UAV was used... and torque on the three coordinate axes Let be the controlled variable. Two sets of simulations are set with different obstacles. The initial positions of the two drones in the two simulations are initial position 1 and initial position 2 respectively. Initial position 2 and initial position 3 Initial position 4 Under these conditions, the drones with initial positions 1 and 3 in the two simulation groups are designated as drone number 1, and the drones with initial positions 2 and 4 are designated as drone number 2. The simulation runtime is set to 80 seconds. s The target points for UAVs 1 and 2 in the two sets of experiments were set as follows: , The drone trajectories of the two sets of experiments are shown in the diagram below. Figure 2 and Figure 3 As shown.

[0069] Through observation Figure 4 , Figure 5It can be seen that the positions of the two drones can follow the final position setting value, with a low oscillation frequency, small oscillation amplitude, and no steady-state error; in addition, the cooperative tracking effect of the drones under different initial points and obstacles all met expectations. Through observation... Figure 6 , Figure 7 , Figure 8 and Figure 9 It can be seen that the controlled variables, lift and torque, can change according to the position and attitude of the UAV, and the magnitude and frequency of the change are relatively stable. While adjusting the controlled variables in a timely manner according to the position and attitude of the UAV, large sudden changes are avoided. In addition, the controlled variables of the UAV can be controlled in a timely and accurate manner according to the state of the UAV under different initial points and obstacles.

[0070] To verify the UAV control system's ability to track a reference path and adapt to different obstacles and target points, the initial positions of both UAVs in the two simulations were set to... and Set the simulation runtime to 80 seconds. s Two sets of simulated UAV No. 1 were set with target points 1 and 2 respectively. and The target points 1 and 2 of UAV No. 2 are respectively and Two sets of simulated obstacles were set up, and the drone trajectories of the two experiments are shown in the figure below. Figure 2 and Figure 10 As shown.

[0071] Through observation Figure 11 , Figure 12 It can be seen that the positions of the two drones can follow the final position setting value, with a low oscillation frequency, small oscillation amplitude, and no steady-state error; in addition, the cooperative tracking effect of the drones under different target points and obstacles all met expectations. Through observation... Figure 13 , Figure 14 , Figure 15 and Figure 16 It can be seen that the controlled variables, lift and torque, can change according to the position and attitude of the UAV, and the magnitude and frequency of the change are relatively stable. In addition, the controlled variables of the UAV can be controlled according to the state of the UAV under different target points and obstacles, so as to achieve tracking control.

Claims

1. A cooperative obstacle avoidance control method for rotary-wing unmanned aerial vehicles based on nonlinear model predictive control, characterized in that, include: Downgrading of high-order systems for rotary-wing UAVs: The established high-order system of the rotary-wing UAV is processed to reduce it to a low-order system to ensure the real-time performance of subsequent control. Obstacle avoidance path planning: In scenarios with static obstacles, a feasible trajectory for the rotary-wing drone is planned in real time, which ensures that the drone completes the obstacle avoidance task while accurately reaching the target point. Trajectory tracking control: A nonlinear model predictive controller is used to determine the optimal control input for the current control cycle by solving a constrained quadratic programming problem and apply it to the nonlinear system to achieve accurate tracking of the feasible reference path for obstacle avoidance path planning.

2. The cooperative obstacle avoidance control method for rotary-wing unmanned aerial vehicles based on nonlinear model predictive control according to claim 1, characterized in that, The specific steps are as follows: S1. Reduce the order of the high-order system of the rotorcraft UAV through distributed control. The input of the high-order system model of the rotorcraft UAV is lift and rotation torque, and the output includes position information and attitude angle. Using the principle of distribution, the high-order system model of the rotorcraft UAV is divided into two subsystems: motion equations and attitude equations. The coupling variables of the two subsystems are used as information exchange between the two distributed subsystems to obtain a distributed reduced-order system. S2. Plan feasible obstacle avoidance paths using the artificial potential field method; Real-time path planning requires the current position and attitude angle of the rotary-wing UAV to obtain the reference position and path for the next sampling period. The artificial potential field method establishes a potential field based on the current position of the rotorcraft UAV. The potential field is divided into a gravitational field about the target point and a repulsive field about the obstacle to obtain the optimal obstacle avoidance path. S3. Design a nonlinear model predictive controller to achieve trajectory tracking control; The data stored in the controller includes: the obstacle avoidance path obtained in step S2, the constraints of the control system, and the initial values; The nonlinear model predictive controller is designed based on actual nonlinear systems. It performs linearization and distributed order reduction in step S1 on the high-order system of the rotorcraft UAV in each sampling period to obtain a linearized distributed order-reduced system. The nonlinear model predictive controller constructs an objective function based on the difference between the expected steady state of the system in the current sampling period and the state of the linearized distributed order-reduced system, the difference between the input and the expected input, and the deviation between the expected steady-state output trajectory and the reference path. Using the position and attitude constraints of the rotorcraft UAV and the actual input limitations as optimization constraints, the controller obtains the optimal control input vector for the current control period by solving a constrained quadratic programming problem online, and applies it to the actual system to complete the control of the current sampling period, thereby achieving accurate tracking of the feasible reference path for obstacle avoidance path planning.

3. The cooperative obstacle avoidance control method for rotary-wing unmanned aerial vehicles based on nonlinear model predictive control according to claim 2, characterized in that, The specific steps for S1 are as follows: S1.1 For the high-order nonlinear state-space model of rotary-wing UAVs: in, The state vector includes the three-dimensional position coordinates and velocity of the rotary-wing UAV, as well as the attitude angles and angular velocities of rotation in the three-dimensional coordinate system. n The dimension of the state vector; The input vector includes the total lift of the rotary-wing UAV and the rotational torque on the three coordinate axes. m The system input vector dimension; The output vector includes the position coordinates of the rotary-wing UAV. q The system input vector dimension; and These are the nonlinear state function and the output function, respectively. and These are the nonlinear state function and output function, which are separated into their respective state variables; The input matrix for the state equations, This is the direct transfer matrix of the output equation; S1.2, a higher-order nonlinear state-space model based on S1.1, based on points. Linearization is performed to obtain a linearized higher-order system: in: In the formula, For the linearization point of a higher-order nonlinear system, and The state matrix and output matrix of the high-order nonlinear system at the linearization point, respectively; State points The linearized state error and output error; Controllable Observable; Therefore, based on the above linearized higher-order model, we obtain Discrete higher-order linearized systems with linearized points: in: ， ， ， ， In the formula, The sampling period of a high-order discrete system. Sampling time, It is the identity matrix. and These are the state matrix and input matrix of the discrete system, respectively. and These are the output matrix and direct transfer matrix of the discrete system, respectively. and These are the state error and output error of the discrete system, respectively; and Controllable The observation period is the same as the subsequent control period, which is 1. ; S1.

3. Based on the discrete higher-order system in S1.2, the state matrix of the higher-order discrete system is decomposed into the following form: ， ， ， ， in, and For subsystem The state matrix, and For subsystem The output matrix, and For subsystem Subsystem The interaction influence matrix and , and These are the linearized state errors of the two subsystems, respectively. and These are the state vector dimensions of the UAV motion subsystem and the state vector dimensions of the UAV attitude subsystem, respectively. and These are the linearized state errors of the two subsystems, respectively. and These are the output vector dimensions of the UAV motion subsystem and the UAV attitude subsystem, respectively. Distributed order reduction is performed on a discrete high-order system, resulting in the following distributed system: in, , This is the state vector of the UAV motion subsystem, containing the UAV's three-dimensional coordinates, position, and velocity variables. Let be the state vector of the UAV attitude subsystem, containing the rotation angle and angular velocity variables of the UAV's three-dimensional coordinates, and ; This is the input control vector for the motion model, specifically the total lift force acting on the UAV. Let be the dimension of the input vector of the UAV motion subsystem; This is the input control vector for the attitude model, specifically the rotational torque of the UAV along the three coordinate axes. Let be the dimension of the input vector of the UAV attitude subsystem, and ; This is the output vector of the UAV motion subsystem. Let be the output vector of the UAV attitude subsystem, and .

4. The cooperative obstacle avoidance control method for rotary-wing unmanned aerial vehicles based on nonlinear model predictive control according to claim 2, characterized in that, The specific steps for S2 are as follows: S2.1 The path direction extension of the reference path is achieved by dividing the yaw angle. and pitch angle To achieve: ， in, To divide the number, The unit extension angle for the yaw angle. The unit expansion angle for pitch; with the same expansion step for every possible angular direction. R Assume the current position coordinates are Then the coordinates of the possible candidate expansion positions for the next step. for: in, Therefore, the set of coordinates of the possible candidate expansion positions for the next step is defined as follows: S2.2 For the obstacle avoidance part of path planning, the artificial potential field method is used to determine the direction of movement of the next path, as follows: in, Let S2.1 be any point among the possible candidate points obtained. Virtual guide point For the first k Time Candidate Point The attraction The gravitational coefficient, For the first j The obstacle is for the first k Candidate points for real-time drones repulsive force, The net external force acting on the current point; For the first j The obstacle is for the first k Candidate points for real-time drones distance, This acts as the boundary of the repulsive field. The repulsive field generated by the obstacle only has an effect on the drone when the distance between the obstacle and the drone is less than this value. It is the repulsion coefficient; S2.3 After multiple drones achieve obstacle avoidance, the drone coordination phase begins, i.e., when the drones reach the virtual guidance point. Subsequently, multiple drones are required to synchronously approach the target point; at this point, the cooperative reference path is generated as follows: in, The cooperative radius of the drone, i.e., the distance from the target point. Less than At this time, the drones enter the collaborative phase; For drones The cycle number at which the coordination phase begins. For drones exist Cooperative reference path points at any time For drones Extended reference path yaw angle for entering the coordination phase; S2.4 Based on the extended path method set in S2.1, during the obstacle avoidance phase, the net external force in each extended direction is calculated using S2.

2. F Then, the point with the largest resultant external force is selected from the remaining candidate expansion positions as the next expansion point, forming an expansion reference path; in the coordination phase, the respective expansion reference paths of the rotary-wing UAVs are formed through S2.

3.

5. The cooperative obstacle avoidance control method for rotary-wing unmanned aerial vehicles based on nonlinear model predictive control according to claim 2, characterized in that, The specific steps for S3 are as follows: S3.1 Based on the extended reference path generated in S2, a nonlinear model predictive control method with real-time linearization is used to construct a nonlinear model predictive controller. That is, linearization and discretization in S1 are implemented for each sampling period. To reduce steady-state error, the nonlinear model predictive controller adopts an incremental form. The incremental form of the distributed linearized discrete system is as follows: in, These are two subsystems of a distributed linearized discrete system. For incremental subsystem i The state vector, For incremental subsystem i The input increment vector; These are the state matrix and input matrix of the incremental state equation, respectively. This is the error propagation matrix of the incremental state equation. For subsystem State vector pairs of subsystems Communication matrix; This is the output matrix of the incremental subsystem; its specific form is as follows: ， ， ， ， Therefore, the state constraints of the original system and control input constraints , and Let the state constraint set and control input constraint set be the same for a high-order nonlinear system. Then, the state constraint adjustment for a distributed reduced-order incremental system is as follows: ,in: and Distributed reduction subsystem The set of state constraints and the set of control constraints; For distributed reduced-order incremental subsystems The set of state constraints; S3.

2. Based on S3.1 and S3.2, the equilibrium point of the distributed reduced-order incremental system is: in, For distributed reduced-order incremental systems at the linearization point The set of all steady-state points, For subsystem The steady-state point; S3.3, based on S3.3, the prediction and control time domains are respectively... and The quadratic programming problem consisting of the online optimization objective function and its corresponding constraints is as follows: in, , For subsystem The model predicts the value function of the controller's optimization problem. For subsystem The output vector at the steady state point, Set values ​​for the output vector of the corresponding subsystem's reference obstacle avoidance path. For subsystem The weight coefficients of the optimization problem and , It is a positive definite weighted matrix; at sampling time... k Solve the subsystem i The quadratic programming problem P The optimal control increment of the next iteration At that time, it is necessary to communicate with related subsystems. j The state prediction sequence of the previous iteration was exchanged during communication. and control increment sequence For the previous sampling time k -1, Solving the subsystem i Optimal control quantity obtained from quadratic programming problem Then, the optimal control quantity at the current moment can be solved based on the incremental model in S3.

1. ; S3.

4. Based on the form of the objective function, the system's predicted output, input sequence, state sequence, and their relationships are obtained as follows: in, , For subsystem exist k The initial value at time , and Subsystems Augmented vectors for all states and incremental controls in the prediction time domain; and They are respectively augmentation subsystems The state matrix and input matrix of the state equation, and For augmented subsystems and The communication coefficient matrix, For augmented subsystems The error propagation matrix of the state equation; its specific form is as follows: ， ， ， in, , , as a subsystem exist Time Prediction The state vector after the step; , , as a subsystem exist Time Prediction Control increment vector after step; superscript T Transpose of a matrix or vector; S3.

5. Based on S3.4, it is known that the two distributed reduced-order subsystems have a coupled part, so the two augmented subsystems are merged: in, To merge the state vectors of the system, To merge the control input increments of the system, For the system in The initial state at time 1. To incorporate the system's state errors, , , and The coefficient matrix of the merged system takes the following form: ， ， ， ， ， ， Based on the system state space obtained by the merging transformation, a decoupling transformation is performed to obtain the subsystem. i State-space prediction model: in, , , , , , , To decouple subsystems i The coefficient matrix of the state equation; S3.

6. Based on S3.4 and S3.5, the objective function for online optimization is described in the following simplified form: Among them, the simplified subsystem i Augmented state Weight matrix and Augmented steady state The definition is as follows: ， ， Take optimization vector After expansion, the pairs were merged and eliminated. After removing the constant term which has no effect, the online optimization objective function can be described in the following quadratic form: Based on the derivation, the parameter matrix of the optimization problem in the above equation is defined as follows: ， ， ， in, and , The definitions are the same, both being ordinal numbers of the subsystems; according to the derivation process, the weight matrix of the optimization problem after the above equation is rearranged ( , ) and steady-state matrix ( The format is as follows: ， ， For the state constraint part of the quadratic optimization It can be expressed in the following form: in, and These are the upper and lower bounds of the state variable, respectively; Based on the objective function and constraints, linearization and distributed order reduction are performed for each sampling period. Online rolling optimization is achieved by solving a constrained quadratic programming problem online, constructing a real-time linearized model predictive controller. The optimal control increment sequence can be obtained from the optimized vector, and the first element of the sequence is taken. The control quantity compared to the previous control cycle Summing yields the optimal control input for the current control cycle. .

Images