Multi-unmanned aerial vehicle cooperative obstacle avoidance control method based on pigeon flock hierarchical leading intelligent mechanism

Abstract

The invention discloses a multi-unmanned aerial vehicle cooperative obstacle avoidance control method based on a pigeon flock hierarchical leading intelligent mechanism, and the method comprises the steps: 1, building a virtual leader and follower unmanned aerial vehicle model, and building an unmanned aerial vehicle group communication topology; 2, initializing the position, speed, attitude and other state information of the unmanned aerial vehicle, initializing the obstacle position distribution and threat radius in the environment, and initializing the detection area of the unmanned aerial vehicle; and the like are totally eight steps. According to the method, an intelligent decision-making process that a high level guides a low level to perform collision avoidance when a pigeon flock faces natural enemy predation is used for reference, the anti-collision decision-making authority of a virtual leader and a following level unmanned aerial vehicle is stipulated, a leader obstacle avoidance and trajectory tracking switching controller is designed, a collision avoidance control protocol based on a logarithmic potential field gradient is established, and a collision avoidance control system is established. The invention provides a pigeon-flock-level-decision-imitating master-slave cooperative obstacle avoidance control method developed for a four-rotor unmanned aerial vehicle cluster in an obstacle distribution environment, and the method is adaptive to random obstacle distribution.

Description

Technical Field

[0001] The present invention relates to a multi-UAV collaborative obstacle avoidance control method based on a pigeon flock hierarchical leading intelligent mechanism, and in particular to a multi-UAV collaborative obstacle avoidance control method based on a pigeon flock hierarchical leading intelligent mechanism for a quad-rotor UAV cluster in an obstacle environment, belonging to the technical field of UAV cluster collaborative control. Background Art

[0002] In a restricted battlefield environment where enemy fire control equipment and obstacles are randomly distributed, they pose a significant threat to our defenses. This requires drones performing stealth missions, such as reconnaissance and detection, to form swarms of coordinated flights while also possessing the ability to identify and avoid threats distributed across the battlefield in real time. Because of the dynamic coordination involved among multiple drones, these drones must simultaneously implement collision avoidance control, encompassing both obstacle avoidance and inter-drone collision avoidance, through distributed perception and decision-making. While ensuring collision avoidance, these drones must also maintain the pre-set formation to prevent any drone from escaping the swarm due to the repulsive effects of the collision avoidance protocol, thereby preventing any drone from being lost to enemy attack and resulting in damage. Many studies have employed attractive / repulsive potential fields to achieve dynamic coordination within swarms of drones. However, when the distance between drones reaches a critical value, swarms of drones are prone to becoming stationary and falling behind. Therefore, recent research on swarm collision avoidance has primarily employed a control framework that incorporates collision avoidance strategies based on consistent formation coordination.

[0003] Currently, common approaches for achieving collision-free motion in obstacle-prone environments for drone swarms include pre-planning, real-time dynamic planning, and hybrid planning frameworks. Pre-planning for a single unmanned platform is typically based on heuristic search or fast random search algorithms. These typically require access to a priori maps or the drone platform's wide-area, time-sensitive perception capabilities. They also place high demands on real-time information about obstacles or threats within the local area. When pre-planning is expanded from a single drone to multiple drones, collision-free constraints on inter-drone planned paths must also be considered. Many representative works model multi-drone trajectory generation as a convex optimization problem, which addresses the pre-planning requirements for multi-drone swarms in dynamic environments. However, this comes with challenges such as high onboard computational costs or complex optimization modeling and solution. Other work focuses on formation collision avoidance control, often considering pre-set reference trajectories that pass along obstacle edges. This significantly reduces the end-to-end platform deployment capabilities of collision avoidance algorithms in unknown environments and weakens their robustness to random obstacle avoidance tasks.

[0004] Pigeon flocks are typical birds in nature that have the habit of homing in groups. When moving in groups, pigeon flocks have a certain hierarchical interaction structure. When facing predator attacks or potential threats, they can transmit and interact information according to the social hierarchy derived from a certain foraging order, thereby achieving a coordinated stress response of the group.

[0005] To sum up, when drone swarms perform autonomous collaboration in obstacle environments, there are problems such as poor generalization of the global planner's computational load and application scenarios. In response to the above-mentioned problems that need to be solved urgently, the present invention introduces a hierarchical leading intelligent decision-making mechanism of pigeon swarms, establishes a collision avoidance control protocol based on the logarithmic potential field gradient, and proposes a multi-drone collaborative obstacle avoidance control method with a hierarchical leading intelligent mechanism of a four-rotor drone cluster in an obstacle environment, which can meet the adaptability to random obstacle distribution. Summary of the Invention

[0006] The purpose of this invention is to provide a multi-UAV collaborative obstacle avoidance control method based on an intelligent mechanism of hierarchical pigeon leadership, enabling collision-free autonomous collaborative safety control of multiple UAVs in complex obstacle mission areas. Inspired by the social hierarchical leadership system followed by pigeons when facing threatening targets, this method mimics the intelligent control behavior of hierarchical pigeon leadership and establishes a logarithmic repulsive potential field for environmental obstacle avoidance based on the leader / follower topology, thereby establishing a threat avoidance control protocol based on the potential field gradient. The present invention adopts a rotation-translation cascade control structure to solve the coordinated control problem of a cluster of four-rotor UAVs with under-actuated characteristics. For establishing an adaptive attitude control protocol in the rotation control loop, it has the advantage of not having to introduce attitude angle (quaternion) and attitude angular rate measurement feedback; for the translation control loop, a finite-time consistency formation collaborative control protocol based on terminal sliding mode is proposed, which can enable a cluster of UAVs whose interaction topology is an undirected graph to meet the consistent convergence of translation formation control within the time bound related to the initial state value; further, a pigeon-like hierarchical leading obstacle avoidance control protocol is introduced into the proposed cluster formation collaborative control protocol, imitating the pigeon-like leading and following foraging decision-making hierarchy, designing a hierarchical guidance obstacle avoidance control logic, and comprehensively forming a multi-UAV collaborative obstacle avoidance control framework based on the pigeon-like hierarchical leading intelligent mechanism.

[0007] The present invention sets a virtual leader at the center of the drone cluster, and the other drones move around the virtual leader node in a specific preset formation configuration. The present invention assumes the virtual leader as an intelligent entity with strong maneuverability, and the other nodes are regarded as follower-level drones and follow the vertical take-off and landing dynamics equations of quadcopter aircraft. The threat avoidance potential field is designed and it is stipulated that the virtual leader has higher decision-making authority and only needs to avoid obstacles distributed in the environment, while the follower-level drones have lower decision-making authority and need to consider avoiding the virtual leader, neighboring drones and obstacle areas within their detection range at the same time.

[0008] A multi-UAV collaborative obstacle avoidance control method based on a pigeon flock hierarchical leading intelligent mechanism, the method steps are as follows:

[0009] Step 1: Create virtual leader and follower drone models and establish the drone swarm communication topology;

[0010] Step 2: Initialize the drone's status information, including: position, speed, attitude, etc., initialize the obstacle location distribution and threat radius in the environment, and initialize the drone's detection area;

[0011] Step 3: Solve the first-order / second-order derivatives of the translation control input through a high-order sliding mode differentiator, and then substitute them into the attitude rotation command differential equation to solve the attitude angular rate command and its first-order derivative;

[0012] Step 4: Solve the attitude driving torque based on the adaptive attitude control law and the auxiliary error system, and update the quaternion, attitude angular rate and attitude rotation matrix through the attitude differential equation;

[0013] Step 5: Design a collaborative formation control protocol: Solve the cluster formation tracking position and velocity errors, and use them to determine the terminal sliding mode. Update the adaptive control gain using the adaptive law, and substitute the terminal sliding mode, adaptive control term, and attitude control coupling term into the translation control input.

[0014] Step 6: Imitate the hierarchical leadership decision-making model of a flock of pigeons and design a hierarchical obstacle avoidance decision-making logic under the leader-follower topology. This proposes a logarithmic threat avoidance potential field with repulsive effectiveness and designs a reference trajectory tracking and on-the-fly obstacle avoidance switching control protocol for the virtual leader.

[0015] Step 7: Compare the following drones to individuals of lower social levels in a flock of pigeons. Taking into account dynamic collision avoidance with neighboring drones and avoidance of static obstacles in the environment, repulsive potential fields are designed for inter-drone collision avoidance and dynamic obstacle avoidance, respectively. The gradient of the repulsive potential field is calculated to establish a safe collision avoidance control protocol. This is integrated with the collaborative formation control protocol in Step 5 to propose a multi-drone collaborative safe collision avoidance control method.

[0016] Step 8: Substitute the translation control variable used for multi-UAV collaborative formation collision avoidance and the attitude rotation matrix obtained in step 4 into the translation dynamics equation to update the UAV position and velocity.

[0017] Furthermore, the step 1 is specifically as follows: Considering the multi-UAV collaborative obstacle avoidance control based on the leader / follower framework, it is assumed that the leadership hierarchy includes a virtual leader with strong maneuverability, and the virtual leader is set to follow the dynamic model of the second-order agent:

[0018]

[0019] Where, and represent the position, velocity, and acceleration of the virtual leader, respectively;

[0020] The following hierarchical drone is set as a quadrotor dynamics model of vertical take-off and landing. Considering N following hierarchical drones, the attitude rotation dynamics model of the following hierarchical drone is established as follows:

[0021]

[0022] Where, represents a quaternion, and Represent the three-axis attitude angular rate and attitude rotation matrix respectively, Λ i represents the constant moment of inertia matrix;

[0023] Assume that the attitude rotation error matrix is ​​R(Q e,i )=(R(Q c,i )) T R(Q i ), and the control input of the translational motion equation is u i =T i R(Q c,i )e3 / m i , the translational dynamics model of the following hierarchical drone is established as:

[0024]

[0025] The topological connection from the virtual leader to the follower drones in the drone cluster is set to a unidirectional topology, and the weight matrix is ​​expressed as B = diag{b 10 ,b 20 ,...,b N0}, where b i0 >0 means that the virtual leader is unidirectionally connected to the nodes in the i-th follower level, and the follower level is connected in an undirected topology, that is, it satisfies w ij =w ji , if w ij =1 means that node j can obtain status information from node i.

[0026] Furthermore, the step 2 is as follows: Initialize the virtual leader position Initialize the follow-level drone position Set the follower level UAV i to form the desired formation configuration δ around the virtual leader i ; Initialize the initial attitude state of drone i, including the initial quaternion value Q i (t0)=[θ i (t0),(q i (t0))T ] T , initial value of attitude angular rate ω i (t0), respectively for the instruction quaternion Q c,i (t0)=[θ c,i (t0),(q c,i (t0)) T ] T and auxiliary instruction quaternion Q β,i (t0)=[θ β,i (t0),(q β,i (t0)) T ] T Initialize; initialize the detection radius R of the drone in the cluster d , initialize the three-dimensional position o of the static obstacle in the environment k , and set the threat radius of obstacle k to

[0027] Furthermore, the step three is specifically as follows: for the control input after iterative update, the first-order derivative and the second-order derivative thereof are solved by a high-order sliding mode differentiator as follows:

[0028]

[0029] Where, and is a positive constant coefficient, use and Represent the translation control input u i The first derivative of and the second-order derivative The observed values ​​of can be used to replace the first-order derivative and second-order derivative of the control input respectively;

[0030] The required R(Q c,i ),u i , and Substitution Solve the attitude angular rate command ω through inverse operation c,i and its derivatives

[0031] Furthermore, the step 4 is specifically as follows: the auxiliary attitude angular rate instruction, the quaternion instruction vector Q are updated respectively through the following three differential equations c,i And the auxiliary quaternion instruction vector Q β,i for:

[0032]

[0033] Where λ i >1 / 2 is the adaptive law gain, is the driving torque component for attitude consistent tracking control obtained in the previous time step;

[0034] Using the quaternion instruction Q c,i =[θ c,i ,(q c,i ) T ] T and auxiliary quaternion instruction Q β,i =[θ β,i ,(q β,i ) T ] T , the attitude rotation matrix is ​​solved based on the quaternion using the following formula:

[0035]

[0036] Using the attitude rotation instruction matrix R(Q c,i ) and R(Q β,i ), define and calculate the attitude tracking error, auxiliary attitude error, attitude angular rate error and auxiliary attitude angular rate error as:

[0037]

[0038] Based on Rodrigues formula And the quaternion deviation relationship Calculate the quaternion deviation Q between drone nodes i and j ij , and extract its component q ij Used to design driving torque;

[0039] According to the attitude angular rate instruction ω calculated in step 3 c,i and its first-order derivative The driving torque for attitude control is calculated as:

[0040]

[0041] Where, is the nonlinear compensation driving torque, is the posture consistency collaborative driving torque.

[0042] Furthermore, the step 5 is specifically as follows: according to the position p of the following level drone updated in real time i and speed v i , calculate the cluster formation tracking error as:

[0043]

[0044] The nonlinear terminal sliding mode vector is designed using the cluster formation tracking error:

[0045]

[0046] Where, k s >0 is the gain coefficient,

[0047] sgn(x) is the sign function;

[0048] The update equation for designing the adaptive control gain using the 1-norm of the terminal sliding mode vector is:

[0049]

[0050] Where, λ>0, μ>0 are constant gain coefficients in the adaptive law, ||s i ||1=∑ j＝x,y,z |s i,j |;

[0051] The finite-time formation cooperative control protocol for following hierarchical UAVs is designed using the nonlinear terminal sliding mode and adaptive gain:

[0052]

[0053] Where, is the acceleration control item of the virtual leader, are two control gains, h q >h p >0 is an exponential term, satisfying the relationship 0<h p / h q <1,sgn(s i )=[sgn(s i,x ),sgn(s i,y ),sgn(s i,z )] T is the symbolic function vector.

[0054] Furthermore, the sixth step is specifically as follows: inspired by the hierarchical leadership decision-making model of a pigeon flock, the leadership level is stipulated to have a higher decision-making authority, that is, the virtual leader only needs to consider prioritizing tracking of the preset expected reference trajectory and completing the avoidance of obstacles encountered on the path, without having to deal with collision avoidance for the follower-level drones with lower decision-making authority;

[0055] The repulsive potential field function designed for the virtual leader to safely avoid collisions with obstacles in the environment is:

[0056]

[0057] Where, is the gain coefficient, R d , R s and They represent the detection radius, safety collision avoidance radius, and circular threat area radius of the kth obstacle of the virtual leader respectively;

[0058] The virtual leader is designed to track the preset reference trajectory. At the same time, if the neighboring aircraft detects an obstacle and performs collision avoidance control, the switching control protocol is as follows:

[0059]

[0060] In the formula, the definition is the set of threat area ranges of obstacle k, and is defined as is the detection area range set of UAV node i, It represents the set of obstacles within the detection range of the virtual leader. and are position tracking control gain and velocity tracking control gain respectively, represents the collision avoidance potential field function Find the gradient of the virtual leader position p0;

[0061] Substitute u0 into the virtual leader's second-order integrator kinematic equations to update its velocity and position.

[0062] Furthermore, the step seven is specifically as follows: the follower-level drones are compared to individuals in the lower social level of a pigeon flock, and collision avoidance is comprehensively considered for the virtual leader and neighbor drones within the detection range, as well as avoidance of static obstacles in the environment. The neighbor drones and static obstacles detected by the follower drone node i are summarized into a set, which can be expressed as:

[0063]

[0064] Where, represents the set of neighboring drones within the detection range of the following level drone node i, represents the set of obstacles within the detection range of the following level drone node i;

[0065] The repulsive potential field functions designed for follower-level UAVs for inter-machine collision avoidance and obstacle avoidance are:

[0066]

[0067] The gradient of the repulsive potential field function is calculated, and the inter-machine collision avoidance and obstacle avoidance control protocol acting on the following level UAV node i is designed as follows:

[0068]

[0069] Combining the inter-drone collision avoidance and obstacle avoidance control protocol with the cooperative formation control protocol proposed in step 5, the formation collision avoidance control algorithm for following hierarchical drones is proposed as follows:

[0070]

[0071] Furthermore, the specific steps of step eight are as follows: the collaborative formation anti-collision translation control protocol for following hierarchical drones established in step seven and the attitude control driving torque calculated in step four are substituted into the drone dynamics equation established in step one, the attitude, velocity and position of the drone are updated, and step three is performed to iterate the next time step.

[0072] Advantages and effects:

[0073] The present invention proposes a multi-UAV collaborative obstacle avoidance control method based on a pigeon flock hierarchical leadership intelligent mechanism, which is used for autonomous collaborative obstacle avoidance control tasks of multiple UAVs in obstacle environments. In nature, pigeon flocks react to the stress of predation by natural enemies, forming a certain leader-follower hierarchical topology, in which the higher level leads the lower level in a top-down order to execute the avoidance decision-making process. The present invention intends to draw on this typical property of pigeon flocks, introduce a virtual leader as the benchmark for cluster movement, set a leader-follower topology, and stipulate the collision avoidance decision-making authority of the virtual leader and follower-level UAVs to meet the requirements of safe and autonomous collaboration in obstacle environments.

[0074] The multi-UAV collaborative obstacle avoidance framework proposed in this invention has the following advantages:

[0075] First, the present invention applies a hierarchical collision avoidance decision mechanism, which only requires an arbitrary reference trajectory to be given in advance, without the need to sample and generate a collision-free pre-planned trajectory. The drone cluster can effectively avoid obstacles in the mission environment while maintaining the cluster's collaborative formation configuration.

[0076] Secondly, the present invention can be applied to situations where obstacles are distributed on the expected reference path. By designing a leader obstacle avoidance and trajectory tracking switching controller, the cluster can be quickly reconstructed into a preset formation configuration after completing obstacle avoidance.

[0077] Furthermore, inspired by the social hierarchy of intelligent pigeons, where the upper levels lead the lower levels, the responsibilities of the virtual leader and follower drones are clearly divided. The virtual leader prioritizes tracking the reference trajectory, switching to obstacle avoidance control upon detecting an obstacle, and resuming tracking the desired trajectory after moving away from the obstacle. The follower drones are primarily responsible for collision avoidance control against the virtual leader, neighboring drones, and static obstacles that enter their detection range. This coordinated collision avoidance control logic is clear and easy to implement. BRIEF DESCRIPTION OF THE DRAWINGS

[0078] Figure 1 This is a principle block diagram of the multi-UAV collaborative obstacle avoidance control method based on the pigeon flock hierarchical leading intelligent mechanism.

[0079] Figure 2 This is the flow chart of the multi-UAV collaborative obstacle avoidance algorithm.

[0080] Figure 3a 3D trajectory diagram of multi-UAV collaborative obstacle avoidance perspective 1.

[0081] Figure 3b 2. 3D trajectory diagram of multi-UAV collaborative obstacle avoidance.

[0082] Figure 3c 3D trajectory diagram for multi-UAV collaborative obstacle avoidance.

[0083] Figure 4 is the position tracking error curve.

[0084] Figure 5 is the speed tracking error curve.

[0085] Figure 6 is the quaternion tracking error curve.

[0086] Figure 7 is the attitude angular rate tracking error curve

[0087] Figure 8 This is the change curve of the closest relative distance between machines.

[0088] Figure 9 This is the change curve of the closest relative distance between the UAV and the obstacle.

[0089] The numbers and symbols in the figure are explained as follows:

[0090] p i,x : The longitudinal position coordinate of drone node i along the x-axis, see Figure 3a 、 3b , 3c;

[0091] p i,y : The horizontal position coordinate of drone node i along the y-axis, see Figure 3a 、 3b , 3c;

[0092] p i,z : The position coordinates of drone node i along the z-axis height direction, see Figure 3a 、 3b , 3c;

[0093] s: seconds (time unit), see Figures 4 to 9 ;

[0094] m: meter (distance unit), see Figure 3a 、 3b , 3c to Figure 4 ;

[0095] The tracking error of the UAV node i along the x-axis is defined as See Figure 4 ; The tracking error of the UAV node i along the y-axis is defined as See Figure 4 ; The tracking error of the UAV node i along the z-axis is defined as See Figure 4 ;

[0096] The tracking error of the UAV node i along the x-axis is defined as See Figure 5 ;

[0097] The tracking error of the UAV node i along the y-axis is defined as See Figure 5 ;

[0098] The tracking error of the UAV node i along the z-axis is defined as See Figure 5 ;

[0099] m / s: meter per second (unit of speed), see Figure 5 ;

[0100] θ e,i : scalar component of quaternion tracking error, see Figure 6 ;

[0101] q e,i : The three-dimensional vector components of the quaternion tracking error, see Figure 6 ;

[0102] q e,i (1),q e,i (2),q e,i (3): Each dimension in the vector component of the quaternion tracking error, see Figure 6 ;

[0103] The attitude angular rate tracking error component rotating around the x-axis, see Figure 7 ;

[0104] The tracking error component of the attitude angular rate rotating around the y-axis is shown in Figure 7 ;

[0105] The tracking error component of the attitude angular rate rotating around the z-axis is shown in Figure 7 ;

[0106] rad / s: radians per second (unit of angular rate), see Figure 7 ;

[0107] The closest distance between drone node i and its neighboring drones is shown in Figure 8 ;

[0108] The closest distance between drone node i and the obstacle within its detection range is shown in Figure 9 . DETAILED DESCRIPTION

[0109] Aiming at the problem of multi-UAV autonomous collaborative obstacle avoidance control in obstacle environment, this paper invented a multi-UAV collaborative obstacle avoidance control method based on pigeon group hierarchical leading intelligent mechanism. The control principle block diagram is as follows: Figure 1 As shown, the implementation steps of the method proposed in the present invention mainly include the initialization of drone status and obstacle information, the design of an adaptive attitude control protocol, the design of a finite-time formation cooperative control protocol based on terminal sliding mode, the design of an obstacle avoidance control protocol based on logarithmic potential field gradient, and the design of a drone group obstacle avoidance control protocol led by a pigeon flock hierarchy, totaling eight steps. The specific implementation steps are as follows:

[0110] Step 1: Create virtual leader and follower drone models and establish the drone swarm communication topology.

[0111] Specifically, as the reference trajectory benchmark value for multiple drones to form a cluster tracking, the present invention sets the virtual leader to satisfy the following second-order intelligent agent dynamics model:

[0112]

[0113] Where p0=[p 0,x ,p 0,y ,p 0,z ] T ,v0=[v 0,x ,v 0,y ,v 0,z ] T and u0=[u 0,x ,u 0,y ,u 0,z ] T They represent the three-dimensional position, velocity and acceleration control quantities of the virtual leader respectively.

[0114] Considering N follower drones surrounding a virtual leader, the present invention models the drone attitude loop dynamics as Equation (2) and the position loop dynamics as Equation (3). Therefore, the attitude rotation differential equation of the i-th follower drone can be expressed as:

[0115]

[0116] In the formula, for the i-th UAV, represents a quaternion, and are the constant component and three-dimensional vector component of the quaternion, and Represent the three-axis attitude angular rate and attitude rotation matrix respectively, Λ i represents the constant moment of inertia matrix, Represents the quaternion equation conversion coefficient matrix, (a) × represents the antisymmetric matrix of vector a. Based on this, the translation differential equation of the i-th follower drone can be expressed as:

[0117]

[0118] Where, and Represent the three-dimensional position and velocity of UAV i, m i and T i Represents mass and thrust respectively, g is the gravitational acceleration constant, e3=[0,0,1] T represents a unit vector.

[0119] The present invention sets the communication topology of the UAVs in the cluster as a leader-follower architecture, and its topology is recorded as and They represent the set of all nodes in the leader-follower topology and the set of nodes in the follower level, respectively. A single virtual leader is located in the leader level and is denoted as node 0. and ε represent the set of edges connecting all nodes and the set of edges following the level nodes respectively.<j,i> ∈ε means that the status information of node i can be transmitted to node j and obtained. In the present invention, the leader level can only transmit information to the follower level in one direction.

[0120] Use B=diag{b 10 ,b 20 ,...,b N0} represents the information transmission weight from the leader layer to the follower layer, and b i0 >0 indicates that the virtual leader is connected to the i-th follower node in a one-way manner; the follower level is set to be connected in an undirected topology, which can transmit information in both directions, and the topological weight w ij =1 means<j,i> ∈ε holds, otherwise w ij = 0 means that the edge from node i to node j is not connected and satisfies w ij =w ji .

[0121] Step 2: Initialize the drone's position, speed, attitude and other status information, initialize the obstacle position distribution and threat radius in the environment, and initialize the drone's detection area.

[0122] Specifically: Initialize the virtual leader position p0(t0) = [p 0,x (t0),p 0,y (t0),p 0,z (t0)] T and the position p of the follower drone i i (t0)=[p i,x (t0),p i,y (t0),p i,z (t0)] T , i=1,2,...N;

[0123] Initialize the desired formation configuration δ formed by follower UAV i around the virtual leader i . Define and initialize the position tracking error of UAV i Initialization velocity tracking error Initialize cluster formation tracking error and And calculate the initial terminal sliding mode vector as

[0124] Initialize the initial attitude state of drone i, including the initial value of quaternion Initial value of attitude angular rate ω i (t0), respectively for the instruction quaternion and auxiliary instruction quaternion Initialize and calculate quaternion tracking error and auxiliary quaternion tracking error Then, we can solve R(Q e,i )and Initial value of the auxiliary attitude angular rate error β i (t0).

[0125] Initialize the detection area of ​​UAV i to where R d is the preset UAV detection radius; the obstacle position distributed in the initialization environment is o k =[o k,x ,o k,y ,o k,z ] T , the obstacles are assumed to be circular areas, and the threat radius of the kth obstacle is set to The threat area range is

[0126] Step 3: Use a high-order sliding mode differentiator to solve the first-order / second-order derivatives of the translation control input, and then substitute them into the attitude rotation command differential equation to solve the attitude angular rate command and its first-order derivative.

[0127] Specifically: Introducing the attitude rotation tracking error R(Q e,i )=(R(Q c,i )) T R(Q i ), substituting this term into equation (3) to transform the velocity differential equation, the translational dynamics equation of the follower UAV node i can be re-expressed as:

[0128]

[0129] The present invention controls the input u i The design goal is to compensate for the coupling term T i R(Q c,i )e3 / m i The control input u iteratively updated in step 7 is modified by a high-order sliding mode differentiator. i Perform filtering and solve the first-order derivative and the second-order derivative The expression is:

[0130]

[0131] Where, and is a positive constant coefficient, where and are the translation control input u i The first derivative of and the second-order derivative The observed estimate of the control input u i The estimation error of is defined as R(Q c,i ),u i , and Substitution The attitude rotation command differential equation represented by can be reversely solved to obtain the attitude angular rate command ω c,i and its derivatives

[0132] Step 4: Based on the adaptive attitude control law and auxiliary error system, design and solve the attitude drive torque control input τ i , and update the quaternion Q through the attitude differential equation (2) i and attitude angular rate ω i .

[0133] Specifically: Based on the results in step 3, first update the auxiliary attitude angular rate command, quaternion command vector Q respectively through the following three differential equations c,i And the auxiliary quaternion instruction vector Q β,i for:

[0134]

[0135] in, is the driving torque term designed in the previous round for posture consistent tracking, λ i >1 / 2 is the adaptive law gain, Υ(Q c,i )=[-q c,i ,θ c,i I3-(q c,i ) × ] T and Υ(Q β,i )=[-q β,i ,θ β,i I3-(q β,i ) × ] T are the two transformation coefficient matrices of the quaternion differential equation.

[0136] By R(Q c,i )=((θ c,i ) 2 -(q c,i ) T q c,i )I3+2q c,i (q c,i ) T -2θ c,i (q c,i ) × Establish

[0137] Based on Q c,i =[θ c,i ,(q c,i ) T ] T The quaternion instruction is used to solve the relationship between the attitude rotation instruction matrix. Similarly, according to Q β,i =[θ β,i ,(q β,i ) T ] T Auxiliary quaternion command solves the auxiliary attitude command matrix R(Q β,i ). Therefore, in formula (6), update Q c,i and Q β,i Based on the conversion relationship from quaternion to attitude rotation matrix, R(Q c,i ) and R(Q β,i ).

[0138] The following expression is used to update the attitude tracking error matrix R(Q e,i ), auxiliary posture tracking error matrix Attitude angular rate error ω e,i And auxiliary attitude angular rate error for:

[0139]

[0140] Leverage relationships Based on the Rodrigues formula

[0141] Solve the quaternion attitude deviation (i.e. between drone node i and node j) Q ij And obtain the component q ij , substituting into formula (8) we can get the driving torque τ i .

[0142] The present invention further calculates the driving torque for attitude control based on the attitude angular rate command and its first-order derivative obtained in step 3 and by using equations (6) and (7) to update the obtained state quantity:

[0143]

[0144] Where, is the nonlinear compensation driving torque, is the coordinated driving torque for consistent posture, q e,i is the quaternion error The last three-dimensional vector in ,q ij The solution is given in the following paragraphs, R(Q e,i ), and β i They are attitude rotation error, auxiliary attitude rotation error and auxiliary attitude instruction respectively.

[0145] The obtained driving torque τ i Substituting into the attitude rotation dynamics equation (2), the attitude angular rate ω can be updated through the differential equation i , quaternion Q i and the attitude rotation matrix R(Q i ).

[0146] Step 5: Formation collaborative control algorithm design: Solve the cluster formation tracking position and velocity errors, and use this to calculate the terminal sliding mode. Update the adaptive control gain through the adaptive law, and substitute the terminal sliding mode, adaptive control term, and attitude control coupling term into the translation control input.

[0147] Specifically: Based on the iterative update of the position p of the UAV i in step 8, the translational dynamics equation (4) i, speed v i , the cluster formation tracking error is obtained as:

[0148]

[0149] The nonlinear terminal sliding mode is designed using the cluster formation tracking error:

[0150]

[0151] Where k s >0 is the gain coefficient, sgn(x) is the sign function, then in formula (10) It can be expressed as:

[0152]

[0153] In order to meet the requirements of stable formation of cluster system, the present invention adopts the 1-norm of terminal sliding mode to design adaptive control gain It can be expressed as:

[0154]

[0155] Where, λ>0, μ>0 are constant gain coefficients in the adaptive law, ||s i ||1=∑j=x,y,z|s i,j | is the 1-norm of the terminal sliding mode vector.

[0156] The present invention integrates the results calculated in equations (9) to (12) to design a terminal sliding mode-based follower hierarchical UAV finite time formation collaborative control protocol as follows:

[0157]

[0158] Where, is the acceleration control item of the virtual leader, h q >h p >0 are two control gains and exponential terms respectively, 0<h p / h q <1 is used to satisfy the finite time convergence, is the adaptive control gain obtained by solving equation (12), sgn(s i )=[sgn(s i,x ),sgn(s i,y ),sgn(s i,z )] T represents a symbolic function vector.

[0159] Step 6: Imitate the hierarchical leadership decision-making model of a flock of pigeons, design a hierarchical obstacle avoidance decision logic under the leader-follower topology, propose a logarithmic threat avoidance potential field with repulsive effectiveness, and design an on-the-spot obstacle avoidance and reference trajectory reconstruction switching control protocol for the virtual leader.

[0160] Specifically: The present invention is inspired by the hierarchical leadership decision-making model of social groups in a flock of pigeons, and stipulates that the leadership level has higher decision-making power. Therefore, the virtual leader only needs to consider avoiding obstacles in the mission area, and does not need to perform collision avoidance processing on the drones in the following level. The following drones need to simultaneously handle the avoidance control tasks of the virtual leader, neighboring drones and obstacles in the environment.

[0161] In order to achieve collision avoidance of obstacles or threat areas, the present invention designs a repulsive potential field function for safe collision avoidance control for the virtual leader as follows:

[0162]

[0163] Where, represents the anti-collision potential function that needs to be applied to the virtual leader when the k-th obstacle is detected, is the gain coefficient, p0 represents the virtual leader position, o k represents the position vector of the kth obstacle, R d , R s and They represent the detection radius of the virtual leader, the safe collision avoidance radius, and the circular threat area radius of the kth obstacle respectively.

[0164] The present invention sets the virtual leader to track the preset reference trajectory first The control mode is switched according to whether an obstacle is detected. The control protocol designed for the virtual leader is:

[0165]

[0166] Where, and They represent the desired acceleration control input for generating the preset reference trajectory, as well as the trajectory tracking control term and obstacle avoidance control term designed for the virtual leader, as well as denote the ideal reference trajectory position and velocity vector of the virtual leader, represents the set of obstacles within the detection range of the virtual leader, as well as are position tracking control gain and velocity tracking control gain respectively, represents the collision avoidance potential field function Find the gradient with respect to p0.

[0167] Substitute the virtual leader control quantity u0 obtained by equation (15) into the kinematic equation expressed by equation (1) and integrate to obtain the virtual leader velocity v0 and position p0.

[0168] Step 7: Compare the following-level drones to individuals in the lower social levels of a pigeon flock. Taking into account the dynamic collision avoidance of neighboring drones and the avoidance of static obstacles in the environment, repulsive potential fields are designed for inter-drone collision avoidance and dynamic obstacle avoidance respectively. The gradient of the repulsive potential field is calculated to establish a safe collision avoidance control protocol. This is integrated with the formation collaborative control algorithm in step 5 to propose a multi-drone collaborative safe collision avoidance control method.

[0169] Specifically, the present invention regards the virtual leader as an individual at a higher level in a pigeon flock, and regards the follower drones as nodes at a lower decision-making level in the pigeon flock. The neighboring drones and static obstacles detected by the follower drone node i can be summarized as a set and expressed as:

[0170]

[0171] Where, Indicates the detection range of the following drone for collision avoidance. Indicates the detection range of the following level drone for obstacle avoidance. represents the neighbor node set of drone node i without considering the detection range, It represents the set of neighbors that node i can detect information from after considering the detection range, and the neighbor set Contains a virtual leader node, represents the set of obstacles within the detection range of node i.

[0172] Based on the set given by formula (16), the present invention establishes a repulsive potential field function for collision avoidance for the follower drone node i:

[0173]

[0174] Where, is the collision avoidance potential field between UAV node i and node j, where j can be j=0, 1, N and satisfy j≠i, is the avoidance potential field between UAV node i and obstacle k.

[0175] After calculating the gradients of the potential field functions of Equations (17) and (18), the control protocols for inter-machine collision avoidance and obstacle avoidance (i.e., safety collision avoidance control protocols) acting on UAV node i are:

[0176]

[0177] For the follower UAV node i, the present invention comprehensively considers its anti-collision safety principle facing the virtual leader and neighbor follower UAVs, as well as its necessary conditions such as avoidance of static obstacles in the mission area, integrates the safety anti-collision control protocol established by equations (19) and (20) in step 7 and the cooperative formation control protocol proposed in step 5, and finally proposes a formation anti-collision control algorithm for follower UAVs:

[0178]

[0179] Step 8: Substitute the translation control quantity for multi-UAV cooperative formation collision avoidance (i.e., the control input obtained by equation (21)) and the attitude rotation matrix obtained in step 4 into the UAV translation dynamics equation expressed by equation (4). By integration, the velocity v of the follower UAV node i can be obtained. i and position p i At this point, the update of the rotation equation and translation equation is completed, and we go to step 3 to start the next round of iteration.

[0180] The following is a specific example to verify the effectiveness of the multi-UAV collaborative obstacle avoidance control method based on the pigeon flock hierarchical leading intelligent mechanism proposed in the present invention. In this example, a UAV cluster in a three-dimensional space is set to include 1 virtual leader and 6 follower UAVs. The hardware simulation environment of this example is an Intel i7-10875H processor with a main frequency of 2.30GHz and 32GB of memory, and the software simulation environment is Matlab 2020a version. The multi-UAV collaborative obstacle avoidance control method based on the pigeon flock hierarchical leading intelligent mechanism, the principle block diagram of its control method structure is as follows Figure 1 As shown in the flowchart of the implementation of this method Figure 2 As shown, the specific implementation steps of this simulation example are as follows:

[0181] Step 1: Create virtual leader and follower drone models and establish the drone swarm communication topology.

[0182] In this example, a virtual leader and six follower drones are set. The topology from the virtual leader to the follower drones is set to be a one-way connection, and the connection weight is B = diag{1,0,0,0,1,0}, where b 10 =b 50 = 1 and not connected to other nodes. The topology of the following hierarchical drone is a bidirectional connection with a connection weight of w 13 =w 23 =w 24 =w 31 =w 35 =w 36 =w 42 =w 43 =w 53 =w63 =1, and the connection weights between other nodes are all 0.

[0183] Step 2: Initialize the drone's position and speed information. Set the initial position of the virtual leader to p0(t0) = [-12.0208, -12.0208, 0] T m, the initial velocity is v0(t0)=[4.7206,-4.7206,3] T m / s, set the initial position of each drone in the following layer to p1(t0)=[-15,-15,0] T m, p2(t0)=[-10,-5,1] T m, p3(t0) = [-5, -5, 0] T m, p4(t0)=[-15,-5,1] T m, p5(t0) = [-10, -15, 0] T m, p6(t0) = [-13, -13, 0] T m, set the initial speed of the drone to v1(t0)=v2(t0)=v3(t0)=v4(t0)=v5(t0)=v6(t0)=[2,2,0] T m / s, the trajectory curve that the virtual leader expects to track is The mass of the drone is m i =0.35kg, set the formation offset of each drone in the follower layer around the virtual leader to:

[0184]

[0185] For the attitude dynamics equation, the moment of inertia matrix of each follower drone is set as:

[0186]

[0187] Initialize the drone's attitude information. Set the drone's initial quaternion to Q1 = [0.9110, 0.3, -0.2, 0.2] T ,

[0188] Q2=[0.9274,-0.1,0.2,0.3] T , Q3=[0.8185,0.1,-0.4,0.4] T , Q4=[0.8185,-0.4,-0.1,0.4] T ,

[0189] Q5=[0.9274,0.2,-0.1,0.3] T , Q6=[0.9110,0.3,-0.2,0.2]T , the initial attitude angular rate is ω1=[-0.5,0.5,-0.45] T rad / s,ω2=[0.5,-0.3,0.1] T rad / s,ω3=[0.1,0.6,-0.1] T rad / s,ω4=[0.4,0.4,-0.5] T rad / s,ω5=[0.4,-0.4,0.5] T rad / s, ω6=[-0.5,0.5,-0.45] T rad / s, set the initial quaternion instruction and auxiliary quaternion instruction to Q c,i =[1,0,0,0] T and Q β,i =[1,0,0,0] T , the initial auxiliary attitude angular rate command is β i =[5,5,5] T .

[0190] The obstacle position distribution and threat radius in the initialization environment are:

[0191] o1=[6.9447,10.6350,24.0000] T m,

[0192] o3=[-18.6350,5.9447,82.0000] T m,

[0193] o5=[3.9447,8.6350,117.0000] T m,

[0194] o7=[-14.9447,-19.6350,141.0000] T m,

[0195] Step 3: Use a high-order sliding mode differentiator to solve the first-order / second-order derivatives of the translation control input, and then substitute them into the attitude rotation command differential equation to solve the attitude angular rate command and its first-order derivative.

[0196] Setting the differentiator parameters Estimating the control input u using a high-order sliding mode differentiator i and its first-order derivative Second-order derivative use and To express the estimated value of the control input and its first / second order derivative, use Indicates u i The estimation error of the high-order sliding mode differentiator can be expressed as:

[0197]

[0198] Step 4: Design an adaptive attitude control protocol and auxiliary error system, design the driving torque as the control input of the attitude dynamics equation, and substitute it into the attitude dynamics equation to update the quaternion and attitude angular rate.

[0199] Set the attitude assistance system adaptive law adjustment coefficient to λ i =20, design auxiliary attitude angular rate instruction β i , quaternion command vector Q c,i And the auxiliary quaternion instruction vector Q β,i The update law is:

[0200]

[0201] Design the posture rotation matrix R(Q e,i ), auxiliary attitude rotation matrix Attitude angular rate error ω e,i , auxiliary attitude angular rate error The update equation is:

[0202]

[0203] Set the attitude control gain to k p,i =100,k p,ij =15, the driving torque control input of the designed attitude rotation equation is:

[0204]

[0205] Step 5: Solve the position and velocity errors of the cluster formation tracking, and use this to find the terminal sliding mode. Update the adaptive control gain through the adaptive law, and substitute the terminal sliding mode, adaptive control term, and attitude control coupling term into the translation control input.

[0206] The cluster formation tracking error is solved as:

[0207]

[0208] Set by the nonlinear sliding mode function vector

[0209] The terminal sliding surface is further designed based on the formation tracking error:

[0210]

[0211] Set the adaptive law coefficients to λ = 0.006 and μ = 250.0, and design the adaptive law to update the control gain as:

[0212]

[0213] Define the gravitational acceleration constant as g = 9.80663 m / s 2 , control coefficient h p =5,h q =9, set the finite time cluster cooperative formation control protocol based on terminal sliding mode as:

[0214]

[0215] Step 6: Imitate the hierarchical leadership decision-making model of a flock of pigeons, design the obstacle avoidance decision logic under the leader-follower topology, propose a logarithmic threat avoidance potential field with repulsive effectiveness, and design an on-the-spot obstacle avoidance and reference trajectory reconstruction switching control protocol for the virtual leader.

[0216] The repulsive potential field function designed for safe collision avoidance control of the virtual leader is:

[0217]

[0218] Set the virtual leader to prioritize tracking the preset reference trajectory If an obstacle is detected on the way, the control mode is switched to the improvisation obstacle avoidance control, that is, the gradient of the potential field function is calculated to generate the anti-collision repulsion control force. The control gain of the virtual leader in the trajectory tracking mode is set to The collision avoidance control gain in the collision avoidance mode is Set the detection radius of the drone to R d =8.0, the safety radius of the drone is R s =6.0, used to generate the desired reference trajectory The desired acceleration control input is preset to Design the control protocol of the virtual leader as follows:

[0219]

[0220] By integrating the control variable u0 through the second-order integrator dynamic equation of the virtual leader, its position and velocity vectors p0 and v0 can be obtained.

[0221] Step 7: Compare the following drones to individuals of lower social levels in a flock of pigeons, design repulsive potential fields for inter-drone collision avoidance and dynamic obstacle avoidance respectively, and then calculate the potential field gradient to establish a safe collision avoidance control protocol. Combined with the formation collaborative control algorithm in step 5, a multi-drone collaborative safe collision avoidance control method is proposed.

[0222] The follower drone is regarded as a node at a lower decision-making level in the pigeon flock, and the detection range of the follower drone for inter-machine collision avoidance is set to Set the detection range of the follow-level drone for obstacle avoidance The neighboring drones and static obstacles detected by follower drone node i are summarized into a set and can be expressed as:

[0223]

[0224] Based on the establishment of a neighboring drone anti-collision set within the detection range for the following level drone node i and the static obstacle anti-collision set within the detection range Set the collision avoidance control gain to The repulsive potential field function designed for follower UAV node i for inter-machine collision avoidance is:

[0225]

[0226] Set the collision avoidance control gain to The repulsive potential field function designed for obstacle avoidance is:

[0227]

[0228] Anti-collision potential field function The control protocol for avoiding collisions between drones at node i can be designed to find the gradient:

[0229]

[0230] Collision avoidance potential field function Find the gradient and design the obstacle avoidance control protocol for the following hierarchical drone node i as follows:

[0231]

[0232] For the following-level UAV node i, taking into account its safety collision avoidance principles for the virtual leader and neighboring following-level UAVs, as well as the avoidance of static obstacles in the mission area, the formation cooperative control protocol around the virtual leader designed in step 5 and the collision avoidance and obstacle avoidance control protocol between the following-level UAVs designed in step 7 are integrated. The cluster cooperative collision avoidance safety control protocol designed for the following-level UAV node i is:

[0233]

[0234] Step 8: Substitute the cluster cooperative anti-collision safety control quantity into the translational dynamics differential equation, update the position and velocity of the UAV, and complete the state update of multi-UAV cooperative in the obstacle environment. The three-dimensional trajectory of multi-UAV cooperative obstacle avoidance is obtained as follows Figure 3a 、 3b , 3c, the expected position tracking error curve around the expected reference trajectory is as follows Figure 4 As shown, the expected speed tracking error curve is as follows Figure 5 As shown, the quaternion and attitude angular rate tracking error curves in attitude dynamics are respectively as follows: Figure 6 and Figure 7 As shown. The relative distance between each node in the cluster, including the virtual leader and the follower drones, and its nearest neighbor node at each moment is given, such as Figure 8 As shown, through Calculate the relative distance between each node in the cluster and the nearest obstacle at each moment, such as Figure 9 After completing the update and plotting for each time step, go to step 3 to start the next iteration.

Claims

1. A multi-UAV collaborative obstacle avoidance control method based on a pigeon flock hierarchical leadership intelligent mechanism, characterized by: The steps of this method are as follows: Step 1: Create virtual leader and follower drone models and establish the drone swarm communication topology; Step 2: Initialize the drone's status information, including: position, speed, attitude, initialization of obstacle location distribution and threat radius in the environment, and initialization of the drone's detection area; Step 3: Solve the first-order / second-order derivatives of the translation control input through a high-order sliding mode differentiator, and then substitute them into the attitude rotation command differential equation to solve the attitude angular rate command and its first-order derivative; Step 4: Solve the attitude driving torque based on the adaptive attitude control law and the auxiliary error system, and update the quaternion, attitude angular rate and attitude rotation matrix through the attitude differential equation; Step 5: Design a collaborative formation control protocol: Solve the cluster formation tracking position and velocity errors, and use them to determine the terminal sliding mode. Update the adaptive control gain using the adaptive law, and substitute the terminal sliding mode, adaptive control term, and attitude control coupling term into the translation control input. Step 6: Imitate the hierarchical leadership decision-making model of a flock of pigeons and design a hierarchical obstacle avoidance decision-making logic under the leader-follower topology. This proposes a logarithmic threat avoidance potential field with repulsive effectiveness and designs a reference trajectory tracking and on-the-fly obstacle avoidance switching control protocol for the virtual leader. Step 7: Compare the following drones to individuals of lower social levels in a flock of pigeons. Taking into account dynamic collision avoidance with neighboring drones and avoidance of static obstacles in the environment, repulsive potential fields are designed for inter-drone collision avoidance and dynamic obstacle avoidance, respectively. The gradient of the repulsive potential field is calculated to establish a safe collision avoidance control protocol. This is integrated with the collaborative formation control protocol in Step 5 to propose a multi-drone collaborative safe collision avoidance control method. Step 8: Substitute the translation control variable used for multi-UAV collaborative formation collision avoidance and the attitude rotation matrix obtained in step 4 into the translation dynamics equation to update the UAV position and velocity.

2. The method according to claim 1, wherein: The specific steps of step 1 are as follows: Consider the multi-UAV collaborative obstacle avoidance control based on the leader / follower framework. Assume that the leadership hierarchy includes a virtual leader with strong maneuverability. Set the virtual leader to follow the dynamic model of the second-order agent: Where, and represent the position, velocity, and acceleration of the virtual leader, respectively; The following hierarchical drone is set as a quadrotor dynamics model of vertical take-off and landing. Considering N following hierarchical drones, the attitude rotation dynamics model of the following hierarchical drone is established as follows: Where, represents a quaternion, and Represent the three-axis attitude angular rate and attitude rotation matrix respectively, Λ i represents the constant moment of inertia matrix; set up The attitude rotation error matrix is ​​R(Q e,i )=(R(Q c,i )) T R(Q i ), and the control input of the translational motion equation is u i =T i R(Q c,i )e3 / m i , the translational dynamics model of the following hierarchical drone is established as: The topological connection from the virtual leader to the follower drones in the drone cluster is set to a unidirectional topology, and the weight matrix is ​​expressed as B = diag{b 10 ,b 20 ,...,b N0 }, where b i0 >0 means that the virtual leader is unidirectionally connected to the nodes in the i-th follower level, and the follower level is connected in an undirected topology, that is, it satisfies w ij =w ji , if w ij =1 means that node j obtains the status information from node i.

3. The method according to claim 1, wherein: The second step is as follows: Initialize the virtual leader position Initialize the follow-level drone position Set the follower level UAV i to form the desired formation configuration δ around the virtual leader i ; Initialize the initial attitude state of drone i, including the initial quaternion value Q i (t0)=[θ i (t0),(q i (t0)) T ] T , initial value of attitude angular rate ω i (t0), respectively for the instruction quaternion Q c,i (t0)=[θ c,i (t0),(q c,i (t0)) T ] T and auxiliary instruction quaternion Q β,i (t0)=[θ β,i (t0),(q β,i (t0)) T ] T Initialize; initialize the detection radius R of the drone in the cluster d , initialize the three-dimensional position o of the static obstacle in the environment k , and set the threat radius of obstacle k to 4. The method according to claim 1, wherein: The specific step three is as follows: for the control input after iterative update, the first-order derivative and the second-order derivative are solved by a high-order sliding mode differentiator: Where, and is a positive constant coefficient, use and Represent the translation control input u i The first derivative of and the second-order derivative Observations of , i.e., the first and second derivatives used to replace the control input; The required R(Q c,i ),u i , and Substitution Solve the attitude angular rate command ω through inverse operation c,i and its derivatives 5. The method according to claim 1, wherein: The fourth step is as follows: Update the auxiliary attitude angular rate command, quaternion command vector Q respectively through the following three differential equations c,i And the auxiliary quaternion instruction vector Q β,i for: Where λ i >1 / 2 is the adaptive law gain, is the driving torque component for attitude consistent tracking control obtained in the previous time step; Using the quaternion instruction Q c,i =[θ c,i ,(q c,i ) T ] T and auxiliary quaternion instruction Q β,i =[θ β,i ,(q β,i ) T ] T , the attitude rotation matrix is ​​solved based on the quaternion using the following formula: Using the attitude rotation instruction matrix R(Q c,i ) and R(Q β,i ), define and calculate the attitude tracking error, auxiliary attitude error, attitude angular rate error and auxiliary attitude angular rate error as: Based on Rodrigues formula And the quaternion deviation relationship Calculate the quaternion deviation Q between drone nodes i and j ij , and extract its component q ij Used to design driving torque; According to the attitude angular rate instruction ω calculated in step 3 c,i and its first-order derivative The driving torque for attitude control is calculated as: Where, is the nonlinear compensation driving torque, is the posture consistency collaborative driving torque.

6. The method according to claim 1, wherein: The step 5 is as follows: according to the real-time updated position p of the following level drone i and speed v i , calculate the cluster formation tracking error as: The nonlinear terminal sliding mode vector is designed using the cluster formation tracking error: Where, k s >0 is the gain coefficient, sgn(x) is the sign function; The update equation for designing the adaptive control gain using the 1-norm of the terminal sliding mode vector is: Where, λ>0, μ>0 are constant gain coefficients in the adaptive law, ||s i ||1=∑ j＝x,y,z |s i,j |; The finite-time formation cooperative control protocol for following hierarchical UAVs is designed using the nonlinear terminal sliding mode and adaptive gain: Where, is the acceleration control item of the virtual leader, are two control gains, h q >h p >0 is an exponential term, satisfying the relationship 0<h p / h q <1,sgn(s i )=[sgn(s i,x ),sgn(s i,y ),sgn(s i,z )] T is the symbolic function vector.

7. The method according to claim 1, wherein: The sixth step is as follows: inspired by the hierarchical leadership decision-making model of pigeon flocks, the leadership level is stipulated to have a higher decision-making power. That is, the virtual leader only needs to consider prioritizing the tracking of the preset expected reference trajectory and avoid obstacles encountered on the path, without having to deal with collision avoidance for the follower drones with lower decision-making power. The repulsive potential field function designed for the virtual leader to safely avoid collisions with obstacles in the environment is: Where, is the gain coefficient, R d , R s and They represent the detection radius, safety collision avoidance radius, and circular threat area radius of the kth obstacle of the virtual leader respectively; The virtual leader is designed to track the preset reference trajectory. At the same time, if the neighboring aircraft detects an obstacle and performs collision avoidance control, the switching control protocol is as follows: In the formula, the definition is the set of threat area ranges of obstacle k, and is defined as is the detection area range set of UAV node i, It represents the set of obstacles within the detection range of the virtual leader. and are position tracking control gain and velocity tracking control gain respectively, represents the collision avoidance potential field function Find the gradient of the virtual leader position p0; Substitute u0 into the virtual leader's second-order integrator kinematic equations to update its velocity and position.

8. The method according to claim 1, wherein: The details of step seven are as follows: the follower-level drones are compared to individuals in the lower social level of a pigeon flock. The virtual leader and neighboring drones within the detection range are comprehensively considered for collision avoidance, and static obstacles in the environment are avoided. The neighboring drones and static obstacles detected by the follower drone node i are summarized into a set represented as: Where, represents the set of neighboring drones within the detection range of the following level drone node i, represents the set of obstacles within the detection range of the following level drone node i; The repulsive potential field functions designed for follower-level UAVs for inter-machine collision avoidance and obstacle avoidance are: The gradient of the repulsive potential field function is calculated, and the inter-machine collision avoidance and obstacle avoidance control protocol acting on the following level UAV node i is designed as follows: Combining the inter-drone collision avoidance and obstacle avoidance control protocol with the cooperative formation control protocol proposed in step 5, the formation collision avoidance control algorithm for following hierarchical drones is proposed as follows:

9. The method according to claim 1, wherein: The specific steps of step eight are as follows: the collaborative formation anti-collision translation control protocol for following hierarchical drones established in step seven and the attitude control driving torque calculated in step four are substituted into the drone dynamics equation established in step one, the attitude, velocity and position of the drone are updated, and step three is performed to iterate the next time step.

Images