Four-rotor unmanned aerial vehicle formation anti-collision and obstacle avoidance control method

Abstract

The invention belongs to the field of unmanned aerial vehicle formation control method design, and particularly relates to a four-rotor unmanned aerial vehicle formation anti-collision and obstacle avoidance control method which is suitable for switching a communication network. Through a target trajectory method, the unmanned aerial vehicle capable of communicating with the virtual navigator estimates the state of the navigator to obtain the target trajectory of the unmanned aerial vehicle, and other unmanned aerial vehicles obtain respective target trajectories through a communication topological structure. A potential energy function and a neural network fast terminal sliding mode control technology are utilized, self-adaptive neural network terminal sliding mode controllers are designed for the position subsystem and the attitude subsystem respectively, and on the premise that the stability of the system is guaranteed, the unmanned aerial vehicle rapidly flies to a formation preset position within finite time, rapidly forms a formation and keeps high tracking precision.

Description

Technical Field

[0001] The present invention belongs to the field of UAV formation control method design, and specifically relates to a four-rotor UAV formation collision avoidance and obstacle avoidance control method. Background Art

[0002] With the rapid development and widespread application of drone technology, quadcopters, with their unique flight stability and flexibility, have shown great potential in aerial photography, reconnaissance, agriculture, military, and other fields. However, achieving collision avoidance and obstacle avoidance during drone formation flight to ensure safe flight has become a pressing technical challenge.

[0003] Formation flying maintains a formation according to pre-set intervals, distances, and altitude differences. Due to various factors, such as formation maneuvers, formation adjustments, and changes, it's impossible to maintain absolute consistency in the intervals, distances, and altitude differences between drones during formation flying. Improper handling can easily lead to collisions. During flight, aerodynamic coupling occurs between the drones in the formation, and this is also affected by uncertainties such as flight parameter measurement errors.

[0004] Due to the coupling of relative motion and the nonlinear variations in dynamic torque, UAV formation control is a complex nonlinear dynamic control process, requiring the design of a high-performance controller to meet the requirements of formation flight. Therefore, designing a collision-free controller for UAV formation control plays a crucial role in ensuring safe multi-UAV formation flight. Furthermore, using centralized information exchange to achieve formation control requires an enormous amount of information, and the complexity of processing this information scales geometrically with the number of UAVs in the formation. However, a decentralized approach cannot guarantee collision-free formation control between UAVs. Only a distributed approach can address both information exchange and collision avoidance. Summary of the Invention

[0005] (1) Technical issues to be resolved

[0006] The technical problem to be solved by the present invention is: how to ensure that no collision occurs between quadrotor drones during formation.

[0007] (2) Technical solution

[0008] To solve the above technical problems, the present invention provides a quadrotor UAV formation collision avoidance and obstacle avoidance control method, which includes the following steps:

[0009] Step 1: Establish a quadrotor UAV formation cooperative control motion model and decouple the UAV formation control system into a position subsystem and an attitude subsystem;

[0010] Step 2: Decompose the control target based on the target trajectory method;

[0011] Step 3: Calculate the formation tracking error based on the switching topology and design a switching topology distributed formation controller;

[0012] Step 4: Design a collision avoidance controller based on the artificial potential field method;

[0013] Step 5: Using the neural network fast terminal sliding mode control technology, an adaptive neural network terminal sliding mode controller is designed for the position outer loop and the attitude inner loop respectively to achieve fast and stable tracking of the formation trajectory.

[0014] Among them, in step 1, the quadcopter UAV formation collaborative control motion model is:

[0015]

[0016] Where x i ,y i ,z i is the three-dimensional position coordinate of the i-th UAV, is the three-dimensional velocity, is the three-dimensional acceleration; φ i ,θ i , ψ i are the roll angle, pitch angle, and yaw angle of the i-th UAV respectively; are the roll angle, pitch angle, and yaw angular velocity of the i-th UAV respectively; are the roll angle, pitch angle, and yaw acceleration of the i-th UAV respectively; m i represents the mass of the i-th quadrotor drone, g is the acceleration of gravity; I i,1 ,I i,2 ,I i,3 is the moment of inertia of the UAV, K i,1 ,…K i,6 represents the damping coefficient; l i * is the distance from each rotor tip to the center of gravity of the drone; u i,1 ,u i,2 ,u i,3 ,u i,4 There are 4 control inputs;

[0017] Decoupling the UAV formation control system into the position subsystem and attitude subsystem, we can obtain:

[0018] (1) The position subsystem is

[0019]

[0020] (2) The attitude subsystem is

[0021]

[0022] in, are the position and attitude angle vector p of the i-th UAV respectively i , χ i The first-order derivative represents the velocity of the position subsystem and the angular velocity of the attitude subsystem, the position vector p i =[x i ,y i ,z i ] T , attitude angle vector χ i =[φ i ,θ i ,ψ i ] T , v p,i , v χ,i are the velocity of the position subsystem and the angular velocity of the attitude subsystem of the i-th UAV respectively; v p,i , v χ,i The differential signal represents the acceleration of the position subsystem and the angular acceleration of the attitude subsystem;

[0023] f p,i (·),f χ,i (·) represent the nonlinear functions of the position subsystem and attitude subsystem of the i-th UAV, respectively, and are expressed as

[0024]

[0025] u p,i ,u χ,i are the controller vector parameters to be designed for the position subsystem and attitude subsystem of the i-th UAV, expressed as u p,i =[u x,i ,u y,i ,u z,i ] T ,u χ,i =[u i,2 ,u i,3 ,u i,4 ] T , due to the coupling relationship between the position subsystem and the attitude subsystem, the auxiliary three-dimensional position controller parameter u x,i ,u y,i ,u z,i Expressed as:

[0026] u x,i =(cosφ i sinθ i cosψ i +sinφ i sinψ i )·u i,1 ,

[0027] u y,i =(cosφ i sinθ i sinψ i -sinφ i cosψ i )·u i,1 ,

[0028] u z,i =(cosφ i cosθ i )·u i,1 .

[0029] The target trajectory method in step 2 decomposes the control target into two parts:

[0030] The control objective is, when t→∞,

[0031]

[0032] ||v p,i -v o ||≤ε v

[0033] |ψ i -ψ o |≤ε ψ

[0034]

[0035] Among them, p o =[x o ,y o ,z o ] T is the desired position trajectory vector, x o ,y o ,z o are the expected coordinates in three directions respectively, is the desired velocity vector, represents the position offset between the actual trajectory and the expected trajectory of the i-th UAV, is the position offset in three directions; ψ o is the desired yaw trajectory; ε p >0,ε v >0,ε ψ >0 indicates error; r co >0 represents the safe distance of each UAV, j represents the UAV adjacent to the i-th UAV, ||p i -p j ||≥2r coIndicates that the relative distance between the i-th UAV and the j-th UAV must be greater than or equal to twice the safety distance;

[0036] Due to the coupling relationship between the position subsystem and the attitude subsystem, it is necessary to use the target trajectory method to decouple the complex nonlinear coupling relationship between the position subsystem and the attitude subsystem to obtain the control law; since each UAV can dynamically adjust its own target trajectory based on the neighbor information, the target trajectory of the position, velocity and yaw angle of the i-th UAV can be defined as and They are the position target trajectories in three directions respectively, are the velocity target trajectories in three directions; therefore, the control objective is decomposed into two parts:

[0037] (1) The drone’s target trajectory tracks the virtual leader signal:

[0038]

[0039] (2) The actual trajectory of the UAV is consistent with the target trajectory:

[0040]

[0041] In step 3, based on the quadrotor UAV formation cooperative control motion model, the formation communication switching topology is established according to graph theory, the formation tracking error is defined, and the switching topology distributed formation controller is designed:

[0042] The formation control protocol is as follows

[0043]

[0044] is the controller parameter vector to be designed, The controller parameters in three directions are composed, and g(·) represents the controller parameters to be designed is included The vector function of , the formation controller parameters are designed as:

[0045]

[0046] in, v o differential variables; intermediate variables k η k p is a positive constant;

[0047] The target trajectory tracking error of the i-th UAV is defined as:

[0048]

[0049] Where N represents the number of drones, e p,i -e p,j represents the comprehensive error between the target trajectory and the actual trajectory of the i-th UAV and the j-th UAV, e p,i -p o represents the error vector between the i-th UAV and the desired trajectory; a ij represents the connection weight between UAV i and UAV j, b i represents the connection weight between the i-th UAV and the virtual navigator, and the expected trajectory is the route of the virtual navigator.

[0050] In step 4, a collision avoidance controller based on the artificial potential field method is designed. The process includes: the safe distance between drones needs to be considered during formation tracking; if this restriction is not met, drones may collide; therefore, a controller with a collision avoidance mechanism needs to be designed, and the potential energy function is designed as follows:

[0051]

[0052] Among them, R de Indicates the radius of the detection area. When the distance between two drones is far, they will appear to be attracted to each other, reducing the distance between them. When the distance between them is less than the detection distance R de When the distance between the drones is 0, a repulsive force will be generated between the drones, pushing them away from each other until a state of equilibrium is reached.

[0053] The partial derivative of

[0054]

[0055] The formation controller with collision avoidance function is designed as follows:

[0056]

[0057] Wherein, in said step 5, the adaptive neural network terminal sliding mode controller is designed, and the process includes:

[0058] Since the analysis process in the x, y, and z directions is similar, To analyze the location subsystem;

[0059] For the position subsystem, the sliding surface is selected as

[0060]

[0061] in, μ i is an adjustable parameter and satisfies β ω >0, 1<μi <2; is the tracking error between the target trajectory and the actual motion trajectory of the i-th UAV, is the differential signal;

[0062] The sliding mode function s of the i-th UAV ω,i The time derivative of is:

[0063]

[0064] Position auxiliary controller parameters Can be designed as

[0065]

[0066] Among them, the intermediate variable Input parameters The optimal weights for the neural network Estimates, Represents the basis vector function of the neural network; K1, K2, δ i is a positive constant to be designed, and 0<δ i <1; sign(·) represents the sign function;

[0067] Based on sin 2 (·)+cos 2 (·)=1, the position controller can be obtained as

[0068]

[0069] Take π = {φ, θ, ψ} to analyze the attitude subsystem, the attitude angle controller u i,l (l={2,3,4}) can be designed as:

[0070]

[0071] Among them, the intermediate variable Input parameters The optimal weights for the neural network Estimate of Θ π,i (·) represents the basis vector function of the neural network; K1, K2, δ i is a positive constant to be designed, and 0<δ i <1; sign(·) represents the sign function.

[0072] (3) Beneficial effects

[0073] Compared with the prior art, the present invention has the following beneficial effects:

[0074] The present invention provides a multi-UAV formation collision and obstacle avoidance method based on an improved artificial potential field method, capable of achieving multi-UAV formation obstacle avoidance while simultaneously adjusting and transforming the formation. A directed spanning tree is used to optimize the topology of the UAV communication network, conserving communication bandwidth resources and improving the efficiency of information exchange while ensuring the connectivity of the entire UAV communication network. Using potential energy functions and neural network fast terminal sliding mode control technology, adaptive neural network terminal sliding mode controllers are designed for the position outer loop and attitude inner loop, respectively. This avoids collisions during formation flight and formation transformation, and enables rapid and stable tracking of the formation trajectory. BRIEF DESCRIPTION OF THE DRAWINGS

[0075] Figure 1 This is a flow chart of a quadrotor UAV formation collision avoidance and obstacle avoidance control method according to the present invention;

[0076] Figure 2 The coordinate system of the quadrotor drone involved in the present invention;

[0077] Figure 3 This invention relates to a quad-rotor UAV formation control block diagram;

[0078] Figure 4 This is a schematic diagram of the communication relationship between drones involved in the present invention;

[0079] Figure 5 This is a schematic diagram of the communication distance relationship between four-rotor drones involved in the present invention;

[0080] Figure 6 The present invention relates to a 3D formation trajectory diagram of a quad-rotor drone;

[0081] Figure 7a-7c The present invention relates to a quad-rotor UAV position tracking error curve diagram; wherein, Figure 7a is the x-direction position tracking error of the UAV; Figure 7b is the UAV y-direction position tracking error; Figure 7c is the UAV z-direction position tracking error.

[0082] Figure 8a-8c The present invention relates to a quad-rotor UAV speed tracking error curve. Figure 8a is the x-direction velocity tracking error of the UAV; Figure 8b is the UAV y-direction velocity tracking error; Figure 8c is the UAV z-direction velocity tracking error. DETAILED DESCRIPTION

[0083] In order to make the purpose, content, and advantages of the present invention more clear, the specific implementation methods of the present invention are further described in detail below with reference to the accompanying drawings and examples.

[0084] Based on the artificial potential field method and neural network fast terminal sliding mode control technology, the present invention designs adaptive collision avoidance controllers for the position subsystem and attitude subsystem respectively, avoiding collision problems during formation flying and formation transformation, and realizing fast and stable tracking of the formation trajectory.

[0085] To solve the above technical problems, the present invention provides a quadrotor UAV formation collision avoidance and obstacle avoidance control method, which includes the following steps:

[0086] Step 1: Establish a quadrotor UAV formation cooperative control motion model and decouple the UAV formation control system into a position subsystem and an attitude subsystem;

[0087] Step 2: Decompose the control target based on the target trajectory method;

[0088] Step 3: Calculate the formation tracking error based on the switching topology and design a switching topology distributed formation controller;

[0089] Step 4: Design a collision avoidance controller based on the artificial potential field method;

[0090] Step 5: Using the neural network fast terminal sliding mode control technology, an adaptive neural network terminal sliding mode controller is designed for the position outer loop and the attitude inner loop respectively to achieve fast and stable tracking of the formation trajectory.

[0091] Among them, in step 1, the quadcopter UAV formation collaborative control motion model is:

[0092]

[0093] Where x i ,y i ,z i is the three-dimensional position coordinate of the i-th UAV, is the three-dimensional velocity, is the three-dimensional acceleration; φ i ,θ i , ψ i are the roll angle, pitch angle, and yaw angle of the i-th UAV respectively; are the roll angle, pitch angle, and yaw angular velocity of the i-th UAV respectively; are the roll angle, pitch angle, and yaw acceleration of the i-th UAV respectively; m i represents the mass of the i-th quadrotor drone, g is the acceleration of gravity; I i,1 ,I i,2 ,I i,3 is the moment of inertia of the UAV, K i,1 ,…K i,6 represents the damping coefficient; l i *is the distance from each rotor tip to the center of gravity of the drone; u i,1 ,u i,2 ,u i,3 ,u i,4 There are 4 control inputs;

[0094] Decoupling the UAV formation control system into the position subsystem and attitude subsystem, we can obtain:

[0095] (1) The position subsystem is

[0096]

[0097] (2) The attitude subsystem is

[0098]

[0099] in, are the position and attitude angle vector p of the i-th UAV respectively i , χ i The first-order derivative represents the velocity of the position subsystem and the angular velocity of the attitude subsystem, the position vector p i =[x i ,y i ,z i ] T , attitude angle vector χ i =[φ i ,θ i ,ψ i ] T , v p,i , v χ,i are the velocity of the position subsystem and the angular velocity of the attitude subsystem of the i-th UAV respectively; v p,i , v χ,i The differential signal represents the acceleration of the position subsystem and the angular acceleration of the attitude subsystem; f p,i (·),f χ,i (·) represent the nonlinear functions of the position subsystem and attitude subsystem of the i-th UAV, respectively, and are expressed as

[0100]

[0101] u p,i ,u χ,i are the controller vector parameters to be designed for the position subsystem and attitude subsystem of the i-th UAV, expressed as u p,i =[u x,i ,u y,i ,u z,i ] T ,u χ,i =[u i,2 ,ui,3 ,u i,4 ] T , due to the coupling relationship between the position subsystem and the attitude subsystem, the auxiliary three-dimensional position controller parameter u x,i ,u y,i ,u z,i Expressed as:

[0102] u x,i =(cosφ i sinθ i cosψ i +sinφ i sinψ i )·u i,1 ,

[0103] u y,i =(cosφ i sinθ i sinψ i -sinφ i cosψ i )·u i,1 ,

[0104] u z,i =(cosφ i cosθ i )·u i,1 .

[0105] The target trajectory method in step 2 decomposes the control target into two parts:

[0106] The control objective is, when t→∞,

[0107]

[0108] ||v p,i -v o ||≤ε v

[0109] |ψ i -ψ o |≤ε ψ

[0110]

[0111] Among them, p o =[x o ,y o ,z o ] T is the desired position trajectory vector, x o ,y o ,z o are the expected coordinates in three directions respectively, is the desired velocity vector, represents the position offset between the actual trajectory and the expected trajectory of the i-th UAV, is the position offset in three directions; ψ o is the desired yaw trajectory; ε p >0,ε v >0,ε ψ >0 indicates error; r co >0 represents the safe distance of each UAV, j represents the UAV adjacent to the i-th UAV, ||p i -p j ||≥2r co Indicates that the relative distance between the i-th UAV and the j-th UAV must be greater than or equal to twice the safety distance;

[0112] Due to the coupling relationship between the position subsystem and the attitude subsystem, it is necessary to use the target trajectory method to decouple the complex nonlinear coupling relationship between the position subsystem and the attitude subsystem to obtain the control law; since each UAV can dynamically adjust its own target trajectory based on the neighbor information, the target trajectory of the position, velocity and yaw angle of the i-th UAV can be defined as and They are the position target trajectories in three directions respectively, are the velocity target trajectories in three directions; therefore, the control objective is decomposed into two parts:

[0113] (1) The drone’s target trajectory tracks the virtual leader signal:

[0114]

[0115] (2) The actual trajectory of the UAV is consistent with the target trajectory:

[0116]

[0117] In step 3, based on the quadrotor UAV formation cooperative control motion model, the formation communication switching topology is established according to graph theory, the formation tracking error is defined, and the switching topology distributed formation controller is designed:

[0118] The formation control protocol is as follows

[0119]

[0120] is the controller parameter vector to be designed, The controller parameters in three directions are composed, and g(·) represents the controller parameters to be designed is included The vector function of , the formation controller parameters are designed as:

[0121]

[0122] in, v o differential variables; intermediate variables k η k p is a positive constant;

[0123] The target trajectory tracking error of the i-th UAV is defined as:

[0124]

[0125] Where N represents the number of drones, e p,i -e p,j represents the comprehensive error between the target trajectory and the actual trajectory of the i-th UAV and the j-th UAV, e p,i -p o represents the error vector between the i-th UAV and the desired trajectory; a ij represents the connection weight between UAV i and UAV j, b i represents the connection weight between the i-th UAV and the virtual navigator, and the expected trajectory is the route of the virtual navigator.

[0126] In step 4, a collision avoidance controller based on the artificial potential field method is designed. The process includes: the safe distance between drones needs to be considered during formation tracking; if this restriction is not met, drones may collide; therefore, a controller with a collision avoidance mechanism needs to be designed, and the potential energy function is designed as follows:

[0127]

[0128] Among them, R de Indicates the radius of the detection area. When the distance between two drones is far, they will appear to be attracted to each other, reducing the distance between them. When the distance between them is less than the detection distance R de When the distance between the drones is 0, a repulsive force will be generated between the drones, pushing them away from each other until a state of equilibrium is reached.

[0129] The partial derivative of

[0130]

[0131] The formation controller with collision avoidance function is designed as follows:

[0132]

[0133] Wherein, in said step 5, the adaptive neural network terminal sliding mode controller is designed, and the process includes:

[0134] Since the analysis process in the x, y, and z directions is similar, To analyze the location subsystem;

[0135] For the position subsystem, the sliding surface is selected as

[0136]

[0137] in, μ i is an adjustable parameter and satisfies β ω >0, 1<μ i <2; is the tracking error between the target trajectory and the actual motion trajectory of the i-th UAV, is the differential signal;

[0138] The sliding mode function s of the i-th UAV ω,i The time derivative of is:

[0139]

[0140] Position auxiliary controller parameters Can be designed as

[0141]

[0142] Among them, the intermediate variable Input parameters The optimal weights for the neural network Estimates, Represents the basis vector function of the neural network; K1, K2, δ i is a positive constant to be designed, and 0<δ i <1; sign(·) represents the sign function;

[0143] Based on sin 2 (·)+cos 2 (·)=1, the position controller can be obtained as

[0144]

[0145] Take π = {φ, θ, ψ} to analyze the attitude subsystem, the attitude angle controller u i,l (l={2,3,4}) can be designed as:

[0146]

[0147] Among them, the intermediate variable Input parameters The optimal weights for the neural network Estimate of Θ π,i (·) represents the basis vector function of the neural network; K1, K2, δ i is a positive constant to be designed, and 0<δ i <1; sign(·) represents the sign function.

[0148] Example 1

[0149] This embodiment addresses the problems of unstable communication links that may occur during formation flight of quadrotor drones in complex environments, leading to changes in communication topology and possible collisions during formation transformation. An adaptive formation collision avoidance control method for multiple quadrotor drones based on an artificial potential field method and neural network sliding mode control technology is provided. A suitable Lyapunov function is constructed and a formation collision avoidance controller is designed for it. Simulation experiments are also conducted to test whether the performance requirements of the established goals can be met.

[0150] In order to make the above-mentioned objects, features and advantages of the present invention more easily understood, the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

[0151] like Figure 1 As shown, the quadrotor UAV formation collision avoidance and obstacle avoidance control method provided by the present invention includes:

[0152] Step 1: Establish a quadrotor UAV formation cooperative control motion model and decouple the UAV formation control system into a position subsystem and an attitude subsystem;

[0153] Step 2: Decompose the control target based on the target trajectory method;

[0154] Step 3: Calculate the formation tracking error based on the switching topology and design a switching topology distributed formation controller;

[0155] Step 4: Design a collision avoidance controller based on the artificial potential field method;

[0156] Step 5: Using the neural network fast terminal sliding mode control technology, an adaptive neural network terminal sliding mode controller is designed for the position outer loop and the attitude inner loop respectively to achieve fast and stable tracking of the formation trajectory.

[0157] like Figure 2 As shown in the figure, the coordinate system of the quadrotor drone involved in the present invention is based on Figure 2 The UAV inertial coordinate system and body coordinate system defined in [1] are used to establish the cooperative control motion model of the quadrotor UAV formation:

[0158]

[0159] Where x i ,y i ,z i is the three-dimensional position coordinate of the i-th UAV, is the first-order derivative of the three-dimensional position coordinate, is the second-order derivative of the three-dimensional position coordinate. i ,θ i , ψ i are the roll angle, pitch angle, and yaw angle of the i-th UAV respectively. are the first-order derivatives of the roll angle, pitch angle, and yaw angle of the i-th UAV, respectively. are the second-order derivatives of the roll angle, pitch angle, and yaw angle of the i-th UAV. i I represents the mass of the i-th quadrotor drone, and g is the acceleration of gravity. i,1 ,I i,2 ,I i,3 is the moment of inertia of the UAV, K i,1 ,…K i,6 Represents the damping coefficient. i * is the distance from each rotor tip to the center of gravity of the drone. i,1 ,u i,2 ,u i,3 ,u i,4 There are 4 control inputs.

[0160] Decoupling the UAV formation control system into position subsystem and attitude subsystem, we can get

[0161] (1) The position subsystem is

[0162]

[0163] (2) The attitude subsystem is

[0164]

[0165] like Figure 3 Figure 1 shows a block diagram of the quadrotor UAV formation control system. The controller design utilizes an inner and outer loop control structure to study the quadrotor UAV system in the presence of external interference. The entire closed-loop system is divided into an inner loop and an outer loop. Due to the coupling relationship between the position and attitude subsystems, a dual-loop control method is required to obtain the control law. Each UAV can dynamically adjust its target trajectory based on neighbor information. Therefore, the control objective is decomposed into two parts:

[0166] (1) The target trajectory of all UAVs follows the virtual leader signal and maintains the desired formation, i.e. and are the target trajectory of the position, velocity and yaw angle of the i-th UAV respectively. The position offset, p o ,v o ,ψ o are the desired position, velocity, and yaw angle trajectory of the UAV, respectively.

[0167] (2) The actual trajectory of the UAV is consistent with the target trajectory. The control law is designed so that where ε p,1 ,ε v,1 ,ε χ,1 is a positive constant. is the target trajectory of the i-th UAV attitude angle.

[0168] like Figure 4 The figure shows a schematic diagram of the communication relationship between drones involved in the present invention. In a drone formation mission, a drone formation control system consisting of 4 drone followers and 1 drone virtual navigator is considered. The figure shows 3 possible communication topologies. Communication topology Switching between G1 → G2 → G3 → G1 is performed every 2 seconds. The adjacency matrix of the communication topology is:

[0169]

[0170] The initial positions of the four drones are p1 = (-12, -13, 0), p2 = (-21, -4, 0), p3 = (26, 4, 0), and p4 = (12, 9, 0). The initial speed and initial attitude angle are 0. The distance between the drones in the formation is kept at 2 meters. The reference trajectory is: p o =[sin(0.5t),-cos(0.5t),t] T ,ψ o =0

[0171] like Figure 5 Figure 2 shows a schematic diagram of the safe distance between drones for communication. This safe distance between drones must be considered during formation tracking. Without this restriction, drones may collide. Therefore, it is necessary to design a controller that includes a collision avoidance mechanism. The potential energy function is designed as follows:

[0172]

[0173] where R de Indicates the radius of the detection area. When the distance between two drones is far, they will appear to be attracted to each other, reducing the distance between them. When the distance between them is less than the detection distance R deWhen the UAVs are in a collision-avoidance state, a repulsive force will be generated between them, pushing them away from each other until a state of equilibrium is reached. The formation control law with collision avoidance function is designed as follows:

[0174]

[0175] like Figure 6 As shown, in order to test the obstacle avoidance capability of the UAV formation, two obstacles are placed on the preset running track of the formation. The position coordinates of the obstacles are P and ob1 (5.0,-0.8,20) and P ob2 (-2.3, -1, 10). As can be seen from the figure, the drones, starting from any initial position, move along a predetermined trajectory while maintaining their formation. When encountering an obstacle within the formation's trajectory, the repulsive field allows them to bypass it, effectively avoiding it. The formation as a whole maintains its predetermined formation at the outset. When encountering an obstacle, the distance between drones increases due to obstacle avoidance maneuvers. After the formation passes the obstacle, the inter-drone potential field allows the drones to return to their predetermined distance.

[0176] like Figure 7a-Figure 8c The following figure shows the position and velocity tracking errors of each drone in the formation tracking the virtual leader. As can be seen from the figure, when the formation encounters an obstacle, the repulsive field causes the drone's speed toward the obstacle to decrease, and the repulsive force then propels the drone away from the obstacle. After passing the obstacle, the gravitational field causes the drone to correct its direction and velocity, ultimately aligning with the speed of the other drones in the formation. Simulations verify that the proposed drone formation collision avoidance control algorithm can accomplish the formation mission and effectively avoid obstacles.

[0177] The above is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the technical principles of the present invention. These improvements and modifications should also be regarded as the scope of protection of the present invention.

Claims

1. A quadrotor UAV formation collision avoidance and obstacle avoidance control method, characterized in that: The control method comprises the following steps: Step 1: Establish a quadrotor UAV formation cooperative control motion model and decouple the UAV formation control system into a position subsystem and an attitude subsystem; Step 2: Decompose the control target based on the target trajectory method; Step 3: Calculate the formation tracking error based on the switching topology and design a switching topology distributed formation controller; Step 4: Design a collision avoidance controller based on the artificial potential field method; Step 5: Using the neural network fast terminal sliding mode control technology, an adaptive neural network terminal sliding mode controller is designed for the position outer loop and the attitude inner loop respectively to achieve fast and stable tracking of the formation trajectory.

2. The quadrotor UAV formation collision avoidance and obstacle avoidance control method according to claim 1, characterized in that: In step 1, the quadcopter UAV formation collaborative control motion model is: Where x i ,y i ,z i is the three-dimensional position coordinate of the i-th UAV, is the three-dimensional velocity, is the three-dimensional acceleration; φ i ,θ i , ψ i are the roll angle, pitch angle, and yaw angle of the i-th UAV respectively; are the roll angle, pitch angle, and yaw angular velocity of the i-th UAV respectively; are the roll angle, pitch angle, and yaw acceleration of the i-th UAV respectively; m i represents the mass of the i-th quadrotor drone, g is the acceleration due to gravity; I i,1 ,I i,2 ,I i,3 is the moment of inertia of the UAV, K i,1 ,…K i,6 represents the damping coefficient; l i * is the distance from each rotor tip to the center of gravity of the drone; u i,1 ,u i,2 ,u i,3 ,u i,4 There are 4 control inputs; Decoupling the UAV formation control system into the position subsystem and attitude subsystem, we can obtain: (1) The position subsystem is (2) The attitude subsystem is in, are the position and attitude angle vector p of the i-th UAV respectively i , χ i The first-order derivative represents the velocity of the position subsystem and the angular velocity of the attitude subsystem, the position vector p i =[x i ,y i ,z i ] T , attitude angle vector χ i =[φ i ,θ i ,ψ i ] T , v p,i , v χ,i are the velocity of the position subsystem and the angular velocity of the attitude subsystem of the i-th UAV respectively; v p,i , v χ,i The differential signal represents the acceleration of the position subsystem and the angular acceleration of the attitude subsystem; f p,i (·),f χ,i (·) represent the nonlinear functions of the position subsystem and attitude subsystem of the i-th UAV, respectively, and are expressed as u p,i ,u χ,i are the controller vector parameters to be designed for the position subsystem and attitude subsystem of the i-th UAV, expressed as u p,i =[u x,i ,u y,i ,u z,i ] T ,u χ,i =[u i,2 ,u i,3 ,u i,4 ] T , due to the coupling relationship between the position subsystem and the attitude subsystem, the auxiliary three-dimensional position controller parameter u x,i ,u y,i ,u z,i Expressed as: the x,i =(cosφ i sinθ i soψ i +sinφ i sinψ i )·u i,1 , the y,i =(cosφ i sinθ i sinψ i -sinφ i soψ i )·u i,1 , u z,i =(cosφ i cosθ i )·u i,1 。 3. The quadrotor UAV formation collision avoidance and obstacle avoidance control method according to claim 2, characterized in that: The target trajectory method in step 2 decomposes the control target into two parts: The control objective is, when t→∞, ||v p,i -v o ||≤ε v |ψ i -ψ o |≤e ψ Among them, p o =[x o ,y o ,z o ] T is the desired position trajectory vector, x o ,y o ,z o are the expected coordinates in three directions respectively, is the desired velocity vector, represents the position offset between the actual trajectory and the expected trajectory of the i-th UAV, is the position offset in three directions; ψ o is the desired yaw trajectory; ε p >0,ε v >0,ε ψ >0 indicates error; r co >0 represents the safe distance of each UAV, j represents the UAV adjacent to the i-th UAV, ||p i -p j ||≥2r co Indicates that the relative distance between the i-th UAV and the j-th UAV must be greater than or equal to twice the safety distance; Due to the coupling relationship between the position subsystem and the attitude subsystem, it is necessary to use the target trajectory method to decouple the complex nonlinear coupling relationship between the position subsystem and the attitude subsystem to obtain the control law; since each UAV can dynamically adjust its own target trajectory based on the neighbor information, the target trajectory of the position, velocity and yaw angle of the i-th UAV can be defined as and They are the position target trajectories in three directions respectively, are the velocity target trajectories in three directions; therefore, the control objective is decomposed into two parts: (1) The drone’s target trajectory tracks the virtual leader signal: (2) The actual trajectory of the UAV is consistent with the target trajectory:

4. The quadrotor UAV formation collision avoidance and obstacle avoidance control method according to claim 3, characterized in that: In step 3, based on the quadrotor UAV formation cooperative control motion model, the formation communication switching topology is established according to graph theory, the formation tracking error is defined, and the switching topology distributed formation controller is designed: The formation control protocol is as follows is the controller parameter vector to be designed, The controller parameters in three directions are composed, and g(·) represents the controller parameters to be designed is included The vector function of , the formation controller parameters are designed as: in, v o differential variables; intermediate variables is a positive constant; The target trajectory tracking error of the i-th UAV is defined as: Where N represents the number of drones, e p,i -e p,j represents the comprehensive error between the target trajectory and the actual trajectory of the i-th UAV and the j-th UAV, e p,i -p o represents the error vector between the i-th UAV and the desired trajectory; a ij represents the connection weight between UAV i and UAV j, b i represents the connection weight between the i-th UAV and the virtual navigator, and the expected trajectory is the route of the virtual navigator.

5. The quadrotor UAV formation collision avoidance and obstacle avoidance control method according to claim 4, characterized in that: In step 4, a collision avoidance controller based on the artificial potential field method is designed. The process includes: the safe distance between drones needs to be considered during formation tracking; if this restriction is not met, drones may collide; therefore, a controller with a collision avoidance mechanism needs to be designed. The potential energy function is designed as follows: Among them, R de Indicates the radius of the detection area. When the distance between two drones is far, they will appear to be attracted to each other, reducing the distance between them. When the distance between them is less than the detection distance R de When the distance between the drones is 0, a repulsive force will be generated between the drones, pushing them away from each other until a state of equilibrium is reached. The partial derivative of The formation controller with collision avoidance function is designed as follows:

6. The quadrotor UAV formation collision avoidance and obstacle avoidance control method according to claim 5, characterized in that: In step 5, an adaptive neural network terminal sliding mode controller is designed, and the process includes: Since the analysis process in the x, y, and z directions is similar, To analyze the location subsystem; For the position subsystem, the sliding surface is selected as in, μ i is an adjustable parameter and satisfies β ω >0, 1<μ i <2; is the tracking error between the target trajectory and the actual motion trajectory of the i-th UAV, is the differential signal; The sliding mode function s of the i-th UAV ω,i The time derivative of is: Position auxiliary controller parameters Can be designed as Among them, the intermediate variable Input parameters The optimal weights for the neural network Estimates, Represents the basis vector function of the neural network; K1, K2, δ i is a positive constant to be designed, and 0<δ i <1; sign(·) represents the sign function; Based on sin 2 (·)+cos 2 (·)=1, the position controller can be obtained as Take π = {φ, θ, ψ} to analyze the attitude subsystem, the attitude angle controller u i,l (l={2,3,4}) can be designed as: Among them, the intermediate variable Input parameters The optimal weights for the neural network Estimate of Θ π,i (·) represents the basis vector function of the neural network; K1, K2, δ i is a positive constant to be designed, and 0<δ i <1; sign(·) represents the sign function.

7. The quadrotor UAV formation collision avoidance and obstacle avoidance control method according to claim 6, characterized in that: The method is based on the artificial potential field method and neural network fast terminal sliding mode control technology. Adaptive collision avoidance controllers are designed for the position subsystem and attitude subsystem respectively, which avoids collision problems during formation flying and formation change, and realizes fast and stable tracking of the formation trajectory.

8. The quadrotor UAV formation collision avoidance and obstacle avoidance control method according to claim 6, characterized in that: The method can realize obstacle avoidance for a multi-UAV formation while completing formation adjustment and transformation.

9. The quadrotor UAV formation collision avoidance and obstacle avoidance control method according to claim 6, characterized in that: The method uses a directed spanning tree to optimize the topology of the UAV communication network, saving communication bandwidth resources and improving the efficiency of information interaction while ensuring the connectivity of the entire UAV communication network.

10. The quadrotor UAV formation collision avoidance and obstacle avoidance control method according to claim 6, characterized in that: The method uses potential energy function and neural network fast terminal sliding mode control technology to design adaptive neural network terminal sliding mode controllers for the position outer loop and attitude inner loop respectively, avoiding collision problems during formation flying and formation transformation, and achieving fast and stable tracking of the formation trajectory.

Images