1 Introduction

In recent years, the cooperative control of multi-UAV systems has become a research hotspot in the field of aviation control [1,2,3]. Among them, cooperative formation control is also one of the important research contents in the field of cooperative control [4,5,6], and it is widely used in the military and civilian fields. However, with the increasing complexity of the modern battlefield environments and combat task requirements, it has become particularly urgent to complete certain combat tasks by multi-UAV, which will make UAVs face greater challenges when performing complex tasks. For example, UAVs can not only prevent self-collision but can also safely evade various obstacles during flight. The UAV can restore the expected formation after avoiding obstacles, reach the target position stably, and finally achieve the consensus of the position and velocity, to ensure that the UAV completes various tasks through cooperation [7, 8]. Therefore, it is of great significance to study the problem of cooperative formation avoidance and consensus control of multi-UAV systems. This can not only enrich the cooperative control theory of multi-UAV systems but also provide a more theoretical basis for the autonomous control technology of UAVs.

Formation obstacle avoidance refers to the formation control behavior in which multiple UAVs can autonomously and safely avoid various obstacles on the route to meet the requirements of the battlefield environment constraints, and finally reach the target point smoothly [9]. Commonly used obstacle avoidance control algorithms include optimization method [10, 11], neural network method [12, 13], genetic algorithm [14, 15] and so on. This paper will not go into details because there are already detailed discussions about these methods in the references. In recent years, the wide application of artificial potential field (APF) [16] and consensus-based control algorithm [17] in cooperative formation obstacle avoidance and consensus control has also aroused the extremely high research interest of scholars. Among them, the APF is a typical obstacle avoidance control algorithm. For example, Zhao et al. used an IAPF and formation division method to combine to make high-speed UAVs more flexible to avoid obstacles [18]. Seyyed et al. added an adjustment agent to the artificial potential field function to improve the efficiency of avoiding fixed obstacles and reach the target position with the best path [19]. Hu et al. proposed a distributed collision avoidance and obstacle avoidance control method based on IAPF and Voronoi constraints to enable UAVs to avoid collisions while avoiding obstacles smoothly [20]. Chang et al. proposed an IAPF based on the rotation vector, which can plan a collision-free and efficient obstacle avoidance trajectory for the UAV [21]. Although the potential field functions designed in the above references can achieve the effect of avoiding obstacles, the gains in the potential field function are all in the form of fixed coefficients, and multiple debugging is required to determine more reasonable parameters, which increases the difficulty of debugging. The repulsive potential field is only used to avoid static obstacles, but it does not mean that the potential field is also applicable to environments with static and dynamic obstacles. Although APF has the advantages of simple calculation and strong real-time performance, it also has defects such as unreachable targets, local minima, and self-collision. Aiming at the problem of the unreachable target. Wang et al. proposed a formation control strategy combining virtual leader and APF, which realized that multi-UAV formation can reach the target position safely without collision [22]. Dai et al. modified the attractive potential field function and the repulsive potential field function based on the segmented idea to make the UAV reach the target point smoothly [23]. Aiming at the problems of self-collision and local minima. Zhang et al. proposed a "lead follower" control strategy based on APF to solve the problem of self-collision and local minima of multi-UAV and enable UAV to effectively avoids space obstacles [24]. Sun et al. proposed a method with distance factor and jump strategy to solve the problem of collision [25]. The potential field function between the UAVs designed in the above references does not consider the influence of the communication weight, which does not conform to the principle of distributed control. The theory of consensus is also widely used in the field of UAVs. The so-called consensus means that in the absence of global communication, several individuals in the spatial distribution achieve the same state through local mutual coupling [26]. Dai et al. used the idea of segmentation to improve the attractive potential field function and the repulsive potential field function, introduced the rotation force and the potential field force between the UAVs, and corrected the state error of the UAV based on the consensus theory, and designed the obstacle avoidance control law of cooperative formation to achieve state consensus and trajectory planning [27]. Wen et al. realized the multi-UAV formation obstacle avoidance control by combining the strategy of Leader–Follower, the APF, and the consensus algorithm [28]. Belkacem K et al. proposed a new distributed consensus algorithm based on the consensus theory for the cooperative control of the second-order multi-UAV system to realize the formation avoidance and tracking control of UAVs [29]. He XY et al. designed a distributed adaptive consensus algorithm combined with the potential field method with distance constraints to enable the UAV to achieve tracking control in the anti-collision situation [30]. The above-mentioned references adopt the method of combining IAPF and consensus theory to achieve the consensus of state and obstacle avoidance, but the potential field function between the UAVs still does not consider the influence of communication weight, and the formation recovery of UAVs after obstacle avoidance is relatively slow.

According to the above discussion and analysis, it can be seen that the research of multi-UAV formation obstacle avoidance and consensus control has made certain progress, and the method of combining APF and consensus theory has been widely used in the field of formation avoidance control, but existing research still exists several questions. For example, the UAV is not well able to adapt to the complex environment with static and dynamic obstacles because the gain in the designed potential field function in [18,19,20,21] is still in the form of a fixed coefficient. UAVs cannot avoid dynamic obstacles since the movement velocity of dynamic obstacles is not considered in the improved repulsive potential field functions of these references. The potential field function between the UAVs designed in [22,23,24,25] is not completely consistent with the topological relationship of the communication network because it does not consider the influence of the communication weight. And most of the references do not comprehensively consider the problem of UAV obstacle avoidance and the consensus of the state after obstacle avoidance. The potential field function between the UAVs designed in [27,28,29,30] still does not consider the influence of the communication weight, and each UAV cannot quickly return to the original desired formation after avoiding obstacles. In response to the above problems, a new potential field function is defined, and a consensus protocol is designed based on a distributed formation control structure. The consensus control of multi-UAV system cooperative formation obstacle avoidance is studied based on the idea of combining IAPF and consensus theory.

The main contributions of this study are as follows:

  1. 1.

    The fixed coefficient form of the gain in the potential field function is improved into a coordination factor form that can be automatically adjusted in real-time. The attractive potential field function containing the coordination gains factor is defined, and the influence factor of the repulsive force is introduced into the repulsive potential field function to prevent the UAV from falling into a local minimum and ensure that the UAV reaches the target point smoothly.

  2. 2.

    The coordination gains factor of the repulsive force based on the movement velocity of the dynamic obstacle, planning angle, and distance deviation is introduced into the repulsive potential field function to improve the adaptability of the UAV to static and dynamic obstacle environments. And the potential field function between the UAVs with communication weight is defined to improve the convergence efficiency.

  3. 3.

    A distributed cooperative formation obstacle avoidance control algorithm based on IAPF and consensus theory is proposed to achieve control objectives such as rapid obstacle avoidance, the consensus of the velocity of UAV, relative distance, and relative height between UAVs.

    The rest of the paper is organized as follows. In Sect. 2, the UAV particle models and system dynamic models are established, and the related knowledge of graph theory is briefly described. In Sect. 3, the obstacle avoidance control algorithm of cooperative formation is designed, and the principles of the IAPF and improved consensus protocol are discussed in detail. In Sect. 4, the stable convergence of the algorithm is proved by the Lyapunov function. In Sect. 5, simulation is carried out to compare and analyze the simulation results to further verify the effectiveness of the algorithm. In Sect. 6, the research content is summarized and the future research work has prospected in this paper.

2 Modeling and Problem Description

The research of UAV formation consensus control usually includes three basic elements of dynamic individuals, communication network topology, and consensus protocol [31]. The particle model of the UAV and the system dynamic model based on the second-order integrator will be established, and the graph theory knowledge used to establish the communication network topology will also be introduced in this section. The design method of cooperative formation obstacle avoidance and consensus control protocol will be discussed in Sect. 3.

2.1 The Particle Model of the UAV

In this paper, the dynamic individual is the UAV, and its motion model in space is mostly a complex six-degree-of-freedom model. The center of mass motion equations of the UAV have been described in detail in Ref. [32], and this paper will not repeat it. To conveniently analyze the problems of formation avoidance, formation recovery, and the consensus of relative position and velocity, the control of attitude is ignored, and each UAV is regarded as a single agent, and the simplified UAV mathematical model is described in matrix form as shown in Eq. (1):

$$M_{ui} = \left[ {x_{ui} \;\;v_{ui} \;\;F_{usi} \;d_{ui\min } \;d_{ui\max } \;d_{uij} \;v_{ui\max } } \right],$$
(1)

where \(x_{ui} = (x_{uix} ,x_{uiy} ,x_{uiz} )\) is the space position coordinate of the UAV, \(v_{ui} = (v_{uix} ,v_{uiy} ,v_{uiz} )\) is the movement velocity of the UAV, \(F_{usi} = (F_{usix} ,F_{usiy} ,F_{usiz} )\) is the resultant force received by the UAV, \(d_{ui\min }\) and \(d_{ui\max }\) are the minimum and maximum values of the UAV's visual distances, respectively, \(d_{uij}\) is the expected distance between the two UAVs that can exchange information. Since the IAPF can calculate the movement velocity of the UAV in real-time, it is considered that there is a certain velocity range, and \(v_{ui\max }\) is set as the maximum velocity of the UAV, \(i,j \in \{ 1,2, \ldots ,n\}\).

This model describes the relationship between the resultant force received by the UAV in the IAPF and the movement velocity and space position. In each iteration, the resultant force is obtained to calculate the acceleration and velocity corresponding to the next moment, and the position information at the next moment based on the position information at the current moment is treated as a new starting position. The calculation mechanism is continuously looped to plan the flight path for the UAV in real-time.

2.2 The Dynamic Model of Multi-UAV System

As far as the state is concerned, the formation consensus means that the relative position and velocity of UAVs converge and tend to be consistent, therefore, a dynamic model of the multi-UAV system based on the second-order integrator is established. Without loss of generality, this paper considers a formation system with UAVs, where the dynamic equation of the i-th UAV is shown in Eq. (2):

$$\left\{ {\begin{array}{*{20}c} {\dot{x}_{ui} (t) = v_{ui} (t)} \\ {\dot{v}_{ui} (t) = u_{ui} (t)} \\ \end{array} } \right.\;\quad i \in \{ 1,2, \ldots ,n\},$$
(2)

where \(x_{ui} = (x_{uix} ,x_{uiy} ,x_{uiz} )\), \(v_{ui} = (v_{uix} ,v_{uiy} ,v_{uiz} )\) and \(u_{ui} = (u_{uix} ,u_{uiy} ,u_{uiz} )\) are the position status, velocity status, and control input of the i-th UAV, respectively. The state vector and the second-order basic consensus protocol of the formation system are shown in Eq. (3) and (4), respectively:

$$\left\{ \begin{gathered} x_{u} (t) = \left[ {x_{u1}^{T} (t)\;\;x_{u2}^{T} (t)\; \ldots \;x_{un}^{T} (t)} \right]^{T} \in {\mathbb{R}}^{n \times 3} \hfill \\ v_{u} (t) = \left[ {v_{u1}^{T} (t)\;\;v_{u2}^{T} (t)\; \ldots \;\;v_{un}^{T} (t)} \right]^{T} \in {\mathbb{R}}^{n \times 3} \hfill \\ \end{gathered} \right.,$$
(3)
$$u_{ui} (t) = - \sum\limits_{j = 1}^{n} {a_{ij} \left[ {\gamma_{1} \left( {x_{ui} (t) - x_{uj} (t)} \right) + \gamma_{2} \left( {v_{ui} (t) - v_{uj} (t)} \right)} \right]} ,$$
(4)

where \(a_{ij}\) is the communication weight between UAV\(i\) and UAV\(j\), \(\gamma_{1} > 0\) and \(\gamma_{2} > 0\) are the protocol parameters.

Definition 1

When each UAV satisfies Eq. (5) for any given initial state, it is said that the multi-UAV system gradually reaches consensus.

$$\left\{ {\begin{array}{*{20}c} {\mathop {\lim }\limits_{t \to \infty } \left\| {x_{ui} (t) - x_{uj} (t)} \right\| = 0} \\ {\mathop {\lim }\limits_{t \to \infty } \left\| {v_{ui} (t) - v_{uj} (t)} \right\| = 0} \\ \end{array} } \right.\;\quad i,j \in \{ 1,2, \ldots ,n\} .$$
(5)

It should be noted that when the consensus theory is applied to the control of multi-UAV formations, to avoid the center of mass of the UAVs overlapping at one point, the safety distance between UAVs should be considered and the position coordination variables should be improved. See Sect. 3.3 for details.

2.3 The Knowledge of Graph Theory

The information interaction process between the UAVs can be described by the knowledge of graph theory, that is, nodes represent UAVs, and the edges between nodes represent the information transfer relationship between the UAVs. Supposing \(G = \left\{ {V,E,A} \right\}\) is a weighted undirected graph, where \(V = \left\{ {v_{1} ,v_{2} , \ldots ,v_{n} } \right\}\) is a point set composed of nodes in the graph, \(E \subset V \times V\) is the edge set composed of all edges in the graph, \(A = [a_{ij} ] \in {\mathbb{R}}^{n \times n}\) is the weighted adjacency matrix of the graph, \(a_{ij}\) is the communication weight between nodes \(v_{i}\) and \(v_{j}\), \((v_{i} ,v_{j} ) \in E \Leftrightarrow (v_{j} ,v_{i} ) \in E\) is an undirected edge between \(v_{i}\) and \(v_{j}\) in the graph. If the UAV \(i\) can transmit information to the UAV \(j\), then \(v_{i}\) is called the parent node, \(v_{j}\) is the child node, that is, \(v_{i}\) is the neighbor of \(v_{j}\). The set of all the neighbors of the node \(v_{j}\) can be represented by \(N_{j} = \left\{ {v_{i} \in V,(v_{i} ,v_{j} ) \in E} \right\}\).

$$L = [l_{ij} ]_{n \times n} \in {\mathbb{R}}^{n \times n} \;{\text{is}}\;{\text{the}}\;{\text{Laplacian}}\;{\text{matrix}}\;{\text{of}}\;{\text{the}}\;{\text{graph,}}$$

where \(l_{ij} = - a_{ij} ,\forall i \ne j\),\(l_{ii} = \sum\nolimits_{j = 1,j \ne i}^{n} {a_{ij} }\). In the graph, the total number of all UAVs that transmit information to the UAV \(i\) is defined as the input degree, and the diagonal matrix \(D\) composed of the input degree is the degree matrix of the graph and \(L{ = }D - A\) is the Laplacian matrix of the graph.

3 Obstacle Avoidance Control Algorithm of Cooperative Formation

This section mainly designs the obstacle avoidance control algorithm of cooperative formation. Firstly, the basic principles of the traditional APF are summarized, and the reasons for its inadequacy in the field of formation control are analyzed. Secondly, an IAPF with three parts includes the attractive potential field, the repulsive potential field, and the potential field between the UAVs are designed. Then, the position variable and the velocity variable in the consensus protocol are improved, respectively, through the reference vector of the formation center and the expected velocity. Finally, an obstacle avoidance control algorithm of cooperative formation based on the IAPF and improved consensus protocol is proposed.

3.1 Traditional APF

Khatib proposed an APF based on the virtual field in Ref. [33]. In a 2D static obstacle environment, a UAV moves toward the target through the attractive force generated by the attractive potential field at the target point, and the UAV hinders itself from moving toward the obstacle through the repulsive force generated by the repulsive force field around the obstacle. The UAV finally reaches the target position driven by the resultant force. The principle of the traditional APF is shown in Fig. 1. the attractive potential field function, the function of the attractive force, repulsive potential field function, and the function of the repulsive force are shown in Eqs. (6)–(9), respectively:

$$U_{ut} \left( {x_{u} ,x_{t} } \right) = {0}{\text{.5}}\alpha_{ut} \left\| {x_{u} - x_{t} } \right\|^{{2}},$$
(6)
$$F_{ut} \left( {x_{u} ,x_{t} } \right) = - \nabla U_{ut} \left( {x_{u} ,x_{t} } \right) = - \alpha_{ut} \left\| {x_{u} - x_{t} } \right\|,$$
(7)
$$U_{uo} \left( {x_{u} ,x_{o} } \right) = \left\{ {\begin{array}{*{20}c} {0.5\beta_{uo} \left( {\frac{1}{{\left\| {x_{u} - x_{o} } \right\|}} - \frac{1}{{d_{o} }}} \right)^{2} ,\quad \;\left\| {x_{u} - x_{o} } \right\| \le d_{o} } \\ {0,\;\quad \left\| {x_{u} - x_{o} } \right\| > d_{o} \;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;} \\ \end{array} } \right.,$$
(8)
$$\begin{aligned} F_{uo} \left( {x_{u} ,x_{o} } \right) & = - \nabla U_{uo} \left( {x_{u} ,x_{o} } \right) \\ & = \left\{ {\begin{array}{*{20}c} \begin{gathered} \frac{{\beta_{uo} }}{{\left\| {x_{u} - x_{o} } \right\|^{{2}} }}\left( {\frac{1}{{\left\| {x_{u} - x_{o} } \right\|}} - \frac{1}{{d_{o} }}} \right),\; \hfill \\ \begin{array}{*{20}c} {} & {} & {} & {} \\ \end{array} \begin{array}{*{20}c} {} \\ \end{array} \begin{array}{*{20}c} {} \\ \end{array} \left\| {x_{u} - x_{o} } \right\| \le d_{o} \hfill \\ \end{gathered} \\ {0,\begin{array}{*{20}c} {} & {} & {} & {} \\ \end{array} \begin{array}{*{20}c} {} \\ \end{array} \left\| {x_{u} - x_{o} } \right\| > d_{o} \;} \\ \end{array} } \right. \\ \end{aligned}$$
(9)

where \(U_{ut} (x_{u} ,x_{t} )\) and \(U_{uo} (x_{u} ,x_{o} )\) are the attractive potential field and the repulsive potential field, respectively,\(\nabla U_{ut} (x_{u} ,x_{t} )\) and \(\nabla U_{uo} (x_{u} ,x_{o} )\) are the gradients of \(U_{ut} (x_{u} ,x_{t} )\) and \(U_{uo} (x_{u} ,x_{o} )\) on \(x_{u}\), respectively,\(F_{ut} (x_{u} ,x_{t} )\) and \(F_{uo} (x_{u} ,x_{o} )\) are the attractive force and the repulsive force, respectively, \(\left\| {x_{u} - x_{t} } \right\|\) is the distance between the UAV and the target point, \(\left\| {x_{u} - x_{o} } \right\|\) is the distance between the UAV and the obstacle, \(d_{o}\) is the influence distance of the obstacle repulsive potential field on the UAV, \(\alpha_{ut}\) and \(\beta_{uo}\) are the gains coefficients of the attractive force and the repulsive force, respectively.

Fig. 1
figure 1

Traditional APF

Full size image

The resultant force received by the UAV is shown in Eq. (10):

$$F_{u} \left( {x_{u} } \right) = F_{ut} \left( {x_{u} ,x_{t} } \right) + F_{uo} \left( {x_{u} ,x_{o} } \right),$$
(10)

where \(F_{ut} (x_{u} ,x_{t} )\), \(F_{uo} (x_{u} ,x_{o} )\) and \(F_{u} (x_{u} )\) are the attractive force, the repulsive force, and the resultant force, respectively.

There are several problems when the traditional APF is applied to UAV formation control:

  1. 1.

    The UAV will not be able to reach the target point because the repulsive force generated by all obstacles on the UAV may be much greater than the attractive force generated by the target point on the UAV when there are obstacles near the target point. Or the UAV falls into a local minimum because the resultant force is zero when the UAV is located in an area with dense obstacles.

  2. 2.

    UAVs can only avoid static obstacles but cannot avoid dynamic obstacles because the traditional repulsive potential field function does not consider information such as the velocity of dynamic obstacles.

  3. 3.

    UAVs are prone to collision with each other because they are only affected by the attractive force and the repulsive force and the interaction between UAVs is not considered when multiple UAVs fly at the same time. After avoiding obstacles, the UAV cannot quickly recover the expected formation and cannot ensure the consensus of the relative position and velocity because the communication weight is not considered in the potential field function.

3.2 IAPF

The multi-UAV is not only affected by the attractive force and the repulsive force in a 3D obstacle environment, but also by the potential field force between the UAVs. Supposing the resultant force received by the UAV is shown in Eq. (11):

$$F_{usi} \left( {x_{ui} } \right) = F_{uti} \left( {x_{ui} ,x_{t} } \right) + F_{uopi} \left( {x_{ui} ,x_{op} } \right) + F_{uij} \left( {x_{ui} ,x_{uj} } \right),$$
(11)

where \(F_{uti} (x_{ui} ,x_{t} )\), \(F_{uopi} (x_{ui} ,x_{op} )\), \(F_{uij} (x_{ui} ,x_{uj} )\) and \(F_{usi} (x_{ui} )\) are the attractive force, the repulsive force, the potential field force between the UAVs and the resultant force, respectively, and \(i,j \in \{ 1,2, \ldots ,n\}\).

3.2.1 The Attractive Potential Field of the Target

A coordination gain factor is introduced into the piecewise attractive potential field function to make each UAV reach the target point smoothly in a complex obstacle environment, and the factor is determined by the distance between the UAV and the target point and the expected distance deviation of the formation. Then the newly defined attractive potential field function and the function of the attractive force are shown in Eqs. (12) and (13), respectively:

$$U_{uti} \left( {x_{ui} ,x_{t} } \right) = \left\{ {\begin{array}{*{20}c} {0.5\alpha_{ut1} \left( {\rho_{uti} } \right)\left\| {x_{ui} - x_{t} } \right\|^{2} ,\left\| {x_{ui} - x_{t} } \right\| < d_{uij} } \\ {0.5\alpha_{ut2} \left( {\rho_{uti} } \right)\left\| {x_{ui} - x_{t} } \right\|^{2} ,\left\| {x_{ui} - x_{t} } \right\| \ge d_{uij} } \\ \end{array} } \right.,$$
(12)
$$\begin{aligned} F_{uti} \left( {x_{ui} ,x_{t} } \right) & = - \nabla U_{uti} \left( {x_{ui} ,x_{t} } \right) \\ & = \left\{ {\begin{array}{*{20}c} { - \alpha_{ut1} \left( {\rho_{uti} } \right)\left\| {x_{ui} - x_{t} } \right\|,\left\| {x_{ui} - x_{t} } \right\| < d_{uij} } \\ { - \alpha_{ut2} \left( {\rho_{uti} } \right)\left\| {x_{ui} - x_{t} } \right\|,\left\| {x_{ui} - x_{t} } \right\| \ge d_{uij} } \\ \end{array} } \right., \\ \end{aligned}$$
(13)

where:

$$\left\{ {\begin{array}{*{20}c} {\alpha_{{ut{1}}} \left( {\rho_{uti} } \right) = 1 + e^{{ - \frac{1}{{\rho_{uti} }}}} } \\ {\alpha_{ut2} \left( {\rho_{uti} } \right) = 1 + e^{{\frac{1}{{\rho_{uti} }}}} } \\ \end{array} } \right.,$$
(14)
$$\rho_{uti} = \left| {\left\| {x_{ui} - x_{t} } \right\| - d_{uij} } \right|,$$
(15)

where \(U_{uti} (x_{ui} ,x_{t} )\) is the attractive Potential Field of the target, \(\nabla U_{uti} (x_{ui} ,x_{t} )\) is the gradient of \(U_{uti} (x_{ui} ,x_{t} )\) on \(x_{ui}\), \(F_{uti} (x_{ui} ,x_{t} )\) is the attractive force of the target, \(\left\| {x_{ui} - x_{t} } \right\|\) is the distance between the UAV and the target point, \(d_{uij}\) is the expected distance between the two UAVs that can exchange information, \(\alpha_{{ut{1}}} (\rho_{uti} )\) and \(\alpha_{ut2} (\rho_{uti} )\) are both the coordination gains factor of the attractive force, \(\rho_{uti}\) is the deviation about the distance between the UAV and the target point and the expected distance, \(i,j \in \{ 1,2, \ldots ,n\}\).

The gains coefficients of the attractive force adjusted by UAV under certain obstacle environment is no longer applicable under other types of obstacle environment because the gain in the traditional attractive potential field and the improved attractive potential field of the existing reference is still in the form of a fixed coefficient. However, for the improved attractive potential field in this paper, both \(\alpha_{{ut{1}}} (\rho_{uti} )\) and \(\alpha_{ut2} (\rho_{uti} )\) can automatically adjust their size by \(\rho_{uti}\) adjusting the intensity of the attractive force. Adjusting \(\alpha_{{ut{1}}} (\rho_{uti} )\) and \(\alpha_{ut2} (\rho_{uti} )\) within the range of \(\left\| {x_{ui} - x_{t} } \right\| < d_{uij}\) and \(\left\| {x_{ui} - x_{t} } \right\| \ge d_{uij}\) can appropriately increase the attractive force to prevent the UAV from being unable to reach the target point due to too small attractive force when there are obstacles near the target point.

3.2.2 The Repulsive Potential Field of the Obstacle

The coordination gains factor of the repulsive force is introduced into the repulsive potential field function to improve the UAV's adaptability to static and dynamic obstacle environments or improve the efficiency of obstacle avoidance, and this factor is determined by the movement velocity of the obstacle, planning angle and distance deviation. The influence factor of the repulsive force is introduced to prevent the UAV from falling into a local minimum because the resultant force is zero, and this factor is determined by the distance between the UAV and the target point. The planning angle is calculated based on the distance between the UAV and the geometric center of the obstacle, the distance between the UAV and the target point, and the distance between the geometric center of the obstacle and the target point. Then the newly defined repulsive potential field function and the function of the repulsive force are shown in Eqs. (16) and (17), respectively, and the schematic diagram and calculation method of the planning angle are shown in Fig. 2 and Eq. (20), respectively:

$$U_{uopi} \left( {x_{ui} ,x_{op} } \right) = \left\{ {\begin{array}{*{20}c} \begin{gathered} 0.5\beta_{uo} \left( {\theta_{ui} ,v_{o} ,\rho_{uopi} } \right)\left( {\frac{1}{{\left\| {x_{ui} - x_{op} } \right\|}} - \frac{1}{{d_{o\max } }}} \right)^{2} \cdot \hfill \\ \left\| {x_{ui} - x_{t} } \right\|^{q} ,\left\| {x_{ui} - x_{op} } \right\| \in \left. {\left( {d_{o\min } ,d_{o\max } } \right.} \right] \hfill \\ + \infty ,\;\;\;\;\;\;\;\;\;\;\left\| {x_{ui} - x_{op} } \right\| \in \left[ {0,d_{o\min } } \right] \hfill \\ \end{gathered} \\ {0,\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;{\text{else}}\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;} \\ \end{array} } \right.,$$
(16)
$$\begin{aligned} F_{uopi} \left( {x_{ui} ,x_{op} } \right) & = - \nabla U_{uopi} \left( {x_{ui} ,x_{op} } \right) \\ & = \left\{ {\begin{array}{*{20}c} \begin{gathered} \beta_{uo} \left( {\theta_{ui} ,v_{o} ,\rho_{uopi} } \right)\left[ {\frac{1}{{\left\| {x_{ui} - x_{op} } \right\|^{{2}} }}\left( {\frac{1}{{\left\| {x_{ui} - x_{op} } \right\|}}} \right.} \right. \hfill \\ - \left. {\frac{1}{{d_{o\max } }}} \right)\left\| {x_{ui} - x_{t} } \right\|^{q} - 0.5q\left( {\frac{1}{{\left\| {x_{ui} - x_{op} } \right\|}}} \right. \hfill \\ - \left. {\frac{1}{{d_{o\max } }}} \right)^{2} \left. {\left\| {x_{ui} - x_{t} } \right\|^{(q - 1)} } \right],\; \hfill \\ \;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\left\| {x_{ui} - x_{op} } \right\| \in (d_{o\min } ,d_{o\max } ] \hfill \\ + \infty ,\;\;\;\;\;\;\;\;\;\;\;\left\| {x_{ui} - x_{op} } \right\| \in [0,d_{o\min } ] \hfill \\ \end{gathered} \\ {0,\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;{\text{else}},\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;} \\ \end{array} } \right. \\ \end{aligned}$$
(17)

where:

$$\beta_{uo} \left( {\theta_{ui} ,v_{o} ,\rho_{uopi} } \right) = \left\{ {\begin{array}{*{20}c} \begin{gathered} 1 + \theta_{ui} e^{{v_{o} - \rho_{uopi} }} , \hfill \\ \theta_{ui} > 0\;{\text{and}}\;v_{o} > 0 \hfill \\ {\text{and}}\;\left\| {x_{ui} - x_{op} } \right\| \in \left. {\left( {d_{o\min } ,d_{o\max } } \right.} \right] \hfill \\ \end{gathered} \\ {1 + \theta_{ui} e^{{ - \rho_{uopi} }} ,\begin{array}{*{20}c} {} & {} \\ \end{array} {\text{else}}\begin{array}{*{20}c} {} & {} & {} \\ \end{array} } \\ \end{array} } \right.,$$
(18)
$$\rho_{uopi} = \left\| {x_{ui} - x_{op} } \right\| - d_{o\min } ,$$
(19)
$$\theta_{ui} {\text{ = arcos}}\frac{{\left\| {x_{ui} - x_{t} } \right\|^{{2}} + \left\| {x_{ui} - x_{o} } \right\|^{{2}} - \left\| {x_{t} - x_{o} } \right\|^{{2}} }}{{2\left\| {x_{ui} - x_{t} } \right\| \cdot \left\| {x_{ui} - x_{o} } \right\|}}.$$
(20)
Fig. 2
figure 2

Planning angle

Full size image

where \(i \in \{ 1,2, \ldots ,n\}\),\(U_{uopi} (x_{ui} ,x_{op} )\) is the repulsive potential field, \(\nabla U_{uopi} (x_{ui} ,x_{op} )\) is the gradient of \(U_{uopi} (x_{ui} ,x_{op} )\) on \(x_{ui}\), \(F_{uopi} (x_{ui} ,x_{op} )\) is the repulsive force, \(x_{op}\) is the space position coordinate of the intersection point that the connection between the UAV and the geometric center of the obstacle with the obstacle surface, \(\left\| {x_{ui} - x_{op} } \right\|\) is the distance between the UAV and the intersection, \(\left\| {x_{ui} - x_{t} } \right\|^{q}\) is the influence factor of the repulsive force, \(\left\| {x_{ui} - x_{t} } \right\|\) is the distance between the UAV and the target point, \(\left\| {x_{t} - x_{o} } \right\|\) is the distance between the target point and the geometric center of the obstacle, \((d_{o\min } ,d_{o\max } ]\) and \([0,d_{o\min } ]\) are the action area of the repulsive potential field of obstacles, \(d_{o\min }\) and \(d_{o\max }\) are the minimum and maximum influence distances of the repulsive potential field of obstacles on the UAV, respectively, \(\beta_{uo} (\theta_{ui} ,v_{o} ,\rho_{uopi} )\) is The coordination gains factor of the repulsive force, \(\rho_{uopi}\) is the deviation between \(\left\| {x_{ui} - x_{op} } \right\|\) and \(d_{o\min }\), \(v_{o}\) is the movement velocity of the dynamic obstacle, \(\theta_{ui}\) is the planning angle, and \(q \in (0,1)\), \(i,j \in \{ 1,2, \ldots ,n\}\).

The improved gain of the repulsive force takes into account the movement velocity of dynamic obstacles, planning angle, and distance deviation. The size of \(\theta_{ui}\) will has a certain impact on the size of the repulsive force. Figure 2 shows that the UAV moves towards the target direction and keeps approaching the obstacle will increase \(\theta_{ui}\) when the UAV is within the influence of the repulsive potential field of obstacles. In other words, under the premise that the UAV is within the influence of the repulsive potential field, the larger the \(\theta_{ui}\) is, the closer the UAV is to the obstacle, at this time, the smaller \(\rho_{uopi}\) is, the larger the gains factor of the repulsive force is, which increases the repulsive force effect to a certain extent. Therefore, this can enhance the UAV's adaptability to static and dynamic obstacle environments and improve the efficiency of obstacle avoidance. The second factor in the first paragraph of the repulsive potential field function of Eq. (17) contains two items, and the first one will decrease as the distance between the UAV and the target point decreases, and even tends to zero. At this time, the second term increases relatively. This equation mainly plays a role in reducing the repulsive force. This equation can also reduce the repulsive force to a certain extent to prevent the UAV from falling into a local minimum due to the equal and opposite directions of the attractive force and the repulsive force.

3.2.3 The Potential Field Between the UAVs

Extending the design ideas of the attractive potential field function and the repulsive potential field function in the traditional APF to the design method of the potential field function between the UAVs to prevent UAV collision. The communication weight is introduced into the potential field function between the UAVs to make full use of communication between UAVs to quickly restore the originally expected formation after avoiding obstacles. Then the newly defined potential field function between the UAVs and the potential field force between the UAVs are shown in Eqs. (21) and (22), respectively:

$$U_{uij} \left( {x_{ui} ,x_{uj} } \right) = \left\{ {\begin{array}{*{20}c} {\sum\limits_{{j \in N_{i} }} {0.5a_{ij} \alpha_{uij} \left( {\rho_{uij} } \right)\left\| {x_{ui} - x_{uj} } \right\|^{2} } ,\begin{array}{*{20}c} {} & {} & {} \\ \end{array} } \\ {\;\left\| {x_{ui} - x_{uj} } \right\| > d_{uij} \;{\text{and}}\;\left\| {x_{ui} - x_{uj} } \right\| \in \left[ {d_{u\min } ,d_{u\max } } \right]} \\ {\sum\limits_{{j \in N_{i} }} {0.5a_{ij} \beta_{uij} \left( {\rho_{uij} } \right)\left( {\frac{1}{{\left\| {x_{ui} - x_{uj} } \right\|}} - \frac{1}{{d_{uij} }}} \right)^{2} } ,} \\ {\left\| {x_{ui} - x_{uj} } \right\| < d_{uij} \;{\text{and}}\;\;\left\| {x_{ui} - x_{uj} } \right\| \in \left[ {d_{u\min } ,d_{u\max } } \right]} \\ {0,\;\;\;\;\;\;\;\left\| {x_{ui} - x_{uj} } \right\| \notin \left[ {d_{u\min } ,d_{u\max } } \right]} \\ \end{array} } \right.$$
(21)
$$\begin{aligned} F_{uij} (x_{ui} ,x_{uj} ) & = - \nabla U_{uij} (x_{ui} ,x_{uj} ) \\ & = \left\{ {\begin{array}{*{20}c} \begin{gathered} - \sum\limits_{{j \in N_{i} }} {a_{ij} \alpha_{uij} (\rho_{uij} )\left\| {x_{ui} - x_{uj} } \right\|} , \hfill \\ \left\| {x_{ui} - x_{uj} } \right\| > d_{uij} \;{\text{and}}\;\left\| {x_{ui} - x_{uj} } \right\| \in \left[ {d_{u\min } ,d_{u\max } } \right] \hfill \\ \end{gathered} \\ {\sum\limits_{{j \in N_{i} }} {\frac{{a_{ij} \beta_{uij} (\rho_{uij} )}}{{\left\| {x_{ui} - x_{uj} } \right\|^{{2}} }}\left( {\frac{1}{{\left\| {x_{ui} - x_{uj} } \right\|}} - \frac{1}{{d_{uij} }}} \right),} } \\ {\;\left\| {x_{ui} - x_{uj} } \right\| < d_{uij} \;{\text{and}}\;\left\| {x_{ui} - x_{uj} } \right\| \in \left[ {d_{u\min } ,d_{u\max } } \right]} \\ {0,\;\;\;\;\;\;\;\;\;\left\| {x_{ui} - x_{uj} } \right\| \notin \left[ {d_{u\min } ,d_{u\max } } \right]} \\ \end{array} } \right. \\ \end{aligned}$$
(22)

where:

$$\left\{ {\begin{array}{*{20}c} {\alpha_{uij} (\rho_{uij} ) = 1 + e^{{\rho_{uij} }} } \\ {\beta_{uij} (\rho_{uij} ) = 1 + e^{{\rho_{uij} }} } \\ \end{array} } \right.,$$
(23)
$$\rho_{uij} = \left| {\left\| {x_{ui} - x_{uj} } \right\| - d_{uij} } \right|,$$
(24)

where \(\left\| {x_{ui} - x_{uj} } \right\|\) is the distance between the UAV \(i\) and its neighboring UAV \(j\), \(d_{uij}\) is the expected distance between the two UAVs that can exchange information.\(\alpha_{uij} (\rho_{uij} )\) and \(\beta_{uij} (\rho_{uij} )\) are the mutual reference gain factor and mutual exclusion gain factor, respectively, \(a_{ij}\) is the communication weight from UAV \(j\) to UAV \(i\), \(N_{i}\) is the set of all UAVs that interact with the UAV \(i\), if UAV \(i\) can receive information from UAV \(j\), then \(a_{ij} > 0\), otherwise \(a_{ij} = 0\), and for all \(i \in \{ 1,2, \ldots ,n\}\), \(a_{ii} = 0\) will be assumed, \(i,j \in \{ 1,2, \cdots ,n\}\).

It can be seen from Eq. (22) that there are both the attractive force and the repulsive force between the UAVs, and the potential field function between the UAVs with \(a_{ij}\) is more in line with the actual communication network topology. The attractive potential field between the UAVs plays a leading role when the distance between the UAVs is greater than the formation safety distance. At this time, the UAVs are preferentially attracted to UAVs with high communication weights, and the distance between the UAVs is appropriately reduced to avoid leaving the formation. The repulsive potential field between the UAVs plays a leading role when the distance between the UAVs is less than the formation safety distance. At this time, the UAVs preferentially repel each other with UAVs with high communication weights, and the distance between the UAVs is appropriately increased to prevent UAVs from colliding. The force between the UAVs is zero when the distance between the UAVs is equal to the expected distance, and the UAV formation tends to be stable.

3.3 Obstacle Avoidance Control Protocol of Cooperative Formation

Although the UAV can prevent self-collision by the IAPF, it can quickly restore its formation after avoiding obstacles safely, and reach the target point smoothly, but the IAPF alone cannot achieve the consensus of position information and velocity information about UAV. In response to this problem, considering that a certain expected distance must be maintained between the UAVs, the formation center reference vector represented by the formation expected distance and expected velocity is introduced in the basic consensus protocol [34] to improve the position variable and the velocity variable, respectively. That is, the deviation between the actual position of the UAV and the reference vector of the preset formation center is taken as the corresponding formation center, and the deviation between the actual velocity and the expected velocity is taken as the new velocity variable. The information difference of the formation center of the UAVs and the new information difference of velocity update the position and velocity information of the UAVs in real-time. The improved consensus protocol is shown in Eq. (25):

$$\begin{aligned} u_{ci} (t) & = - \sum\limits_{j = 1}^{n} {a_{ij} \left[ {\gamma_{1} \left( {\hat{x}_{ui} (t) - \hat{x}_{uj} (t)} \right)} \right.} \\ & + \gamma_{2} \left. {\left( {\hat{v}_{ui} (t) - \hat{v}_{uj} (t)} \right)\;} \right]\quad i,j \in \{ 1,2, \ldots ,n\} , \\ \end{aligned}$$
(25)

where:

$$\left\{ {\begin{array}{*{20}c} {\hat{x}_{ui} (t) = x_{ui} (t) - x^{\prime}_{ui} } \\ {\hat{v}_{ui} (t) = v_{ui} (t) - v^{\prime}_{ui} } \\ \end{array} .} \right.$$
(26)

The obstacle avoidance control protocol of cooperative formation designed by combining the IAPF and the improved consensus protocol is shown in Eq. (27):

$$\begin{aligned} u_{ui} (t) & = u_{ci} (t) + \gamma_{3} F_{usi} \left( {x_{ui} } \right) \\ & = - \sum\limits_{j = 1}^{n} {a_{ij} \left[ {\gamma_{1} \left( {\hat{x}_{ui} (t) - \hat{x}_{uj} (t)} \right) + \gamma_{2} \left( {\hat{v}_{ui} (t) - \hat{v}_{uj} (t)} \right)} \right]} \\ & + \gamma_{3} \left[ {F_{uti} \left( {x_{ui} ,x_{t} } \right){ + }\sum\limits_{o = 1}^{m} {F_{uopi} \left( {x_{ui} ,x_{op} } \right)} } \right. \\ & + \left. {\sum\limits_{{j \in \overline{N}_{i} (t)}} {F_{uij} \left( {x_{ui} ,x_{uj} } \right)} } \right]. \\ \end{aligned}$$
(27)

Putting Eq. (13), Eq. (17) and Eq. (22) into Eq. (27) can get Eq. (28):

$$\begin{aligned} u_{ui} (t) & = - \sum\limits_{j = 1}^{n} {a_{ij} \left[ {\gamma_{1} \left( {\hat{x}_{ui} (t) - \hat{x}_{uj} (t)} \right) + \gamma_{2} \left( {\hat{v}_{ui} (t) - \hat{v}_{uj} (t)} \right)} \right]} \\ & + \gamma_{3} \left[ { - \nabla U_{uti} \left( {x_{ui} ,x_{t} } \right) - \sum\limits_{o = 1}^{m} {\nabla U_{uopi} \left( {x_{ui} ,x_{op} } \right)} } \right. \\ & - \left. {\sum\limits_{{j \in \overline{N}_{i} (t)}} {\nabla U_{uij} \left( {x_{ui} ,x_{uj} } \right)} } \right]. \\ \end{aligned}$$
(28)

Substituting Eq. (28) into Eq. (2), the closed-loop dynamic equation of the formation obstacle avoidance control system is shown in Eq. (29).

$$\left\{ \begin{gathered} \dot{\hat{x}}_{ui} (t) = \hat{v}_{ui} (t) \hfill \\ \dot{\hat{v}}_{ui} (t) = - \sum\limits_{j = 1}^{n} {a_{ij} \left[ {\gamma_{1} \left( {\hat{x}_{ui} (t) - \hat{x}_{uj} (t)} \right) + \gamma_{2} \left( {\hat{v}_{ui} (t) - \hat{v}_{uj} (t)} \right)} \right]} \hfill \\ + \gamma_{3} \left[ { - \nabla U_{uti} \left( {x_{ui} ,x_{t} } \right) - \sum\limits_{o = 1}^{m} {\nabla U_{uopi} \left( {x_{ui} ,x_{op} } \right)} } \right. \hfill \\ - \left. {\sum\limits_{{j \in \overline{N}_{i} (t)}} {\nabla U_{uij} \left( {x_{ui} ,x_{uj} } \right)} } \right], \hfill \\ \end{gathered} \right.$$
(29)

where \(u_{ci} {\text{(t)}}\) is the improved consensus protocol, \(\hat{x}_{ui} (t)\) is the position information difference between the actual position \(x_{ui} (t)\) of the UAV \(i\) and its preset formation center reference vector \(x^{\prime}_{ui}\), \(\hat{v}_{ui} (t)\) is the velocity information difference between the actual velocity \(v_{ui} (t)\) of the UAV \(i\) and its expected velocity \(v^{\prime}_{ui}\), \(\gamma_{1}\) , \(\gamma_{2}\) and \(\gamma_{3}\) are all adjustable protocol parameters greater than 0, \(a_{ij}\) is the communication weight of the UAVs, which is used to describe the information interaction process between the UAVs. If information can be transferred between two UAVs, assumed that \(a_{ij} > 0\), otherwise \(a_{ij} = 0\), \(i,j \in \{ 1,2, \ldots ,n\}\).

Definition 2

If the formation system can always meet the conditions shown in Eq. (30) under any given initial state conditions, it is said that the UAVs can restore the originally expected formation after avoiding obstacles, and realize the consensus of velocity , relative distance and height between the UAVs.

$$\left\{ {\begin{array}{*{20}c} {\mathop {\lim }\limits_{t \to \infty } \left\| {\hat{x}_{ui} (t) - \hat{x}_{uj} (t)} \right\| = 0} \\ {\mathop {\lim }\limits_{t \to \infty } \left\| {\hat{v}_{ui} (t) - \hat{v}_{uj} (t)} \right\| = 0} \\ \end{array} } \right.\;\quad i,j \in \{ 1,2, \ldots ,n\} .$$
(30)

4 Stability and Convergence Analysis

The stability, convergence, and control performance of obstacle avoidance and collision avoidance of the formation control system will be analyzed in this section. Among them, the stability and convergence of the system are mainly proved by the characteristics of the solution of the closed-loop dynamic equation, the Lyapunov function defined by the total energy of the system, and the LaSalle invariant set principle. For obstacle avoidance and collision avoidance, it is mainly proved by contradiction.

The following assumptions are given to be able to better control the UAV formation flight:

  1. 1.

    The formation system composed of \(n\) UAVs performs formation flying under the action of Eq. (27). The UAVs will encounter \(m\) obstacles in the external environment during the flight.

  2. 2.

    2)Each UAV is equipped with onboard communication equipment and the communication between the UAVs is in good condition, without communication delay, data packet loss, etc.

  3. 3.

    There is a spanning tree in the fixed communication network topology, that is, any UAV in the formation system can at least obtain communication information through a neighbor UAV.

  4. 4.

    The total energy function of the formation system constructed by the potential energy function and the kinetic energy function is defined as \(V(t)\), and the total energy function is a finite value, there is \(V(t) \le a\).

The following theorem is now given:

Theorem 1

Under the action of Eq. (27), the UAV can not only prevent collisions between UAVs, but also safely avoid static and dynamic obstacles, quickly recover the expected formation after avoiding obstacles, and achieve the consensus of relative position and velocity.

Proof

Let \([x_{u} (0),v_{u} (0)]^{T}\) be the initial state, where \(x_{u} (0) = [x_{u1}^{T} (0)\;\;x_{u2}^{T} (0)\;\; \cdots \;\;x_{un}^{T} (0)]^{T}\) is the initial position state, and \(v_{u} (0) = [v_{u1}^{T} (0)\;\;v_{u2}^{T} (0)\;\; \cdots \;\;v_{un}^{T} (0)]^{T}\) is the initial velocity state. It is assumed that the vector form of the improved synergy variable and the improved potential field force are shown in Eq. (31) and Eq. (32), respectively:

$$\left\{ {\begin{array}{*{20}c} {\hat{x}_{u} (t) = \left[ {\hat{x}_{u1}^{T} (t)\begin{array}{*{20}c} {} \\ \end{array} \hat{x}_{u2}^{T} (t) \ldots \hat{x}_{un}^{T} (t)} \right]^{T} \in {\mathbb{R}}^{n \times 3} } \\ {\hat{v}_{u} (t) = \left[ {\hat{v}_{u1}^{T} (t)\begin{array}{*{20}c} {} \\ \end{array} \hat{v}_{u2}^{T} (t) \ldots \hat{v}_{un}^{T} (t)} \right]^{T} \in {\mathbb{R}}^{n \times 3} } \\ \end{array} } \right.,$$
(31)
$$F_{us} (t) = \left[ {F_{us1}^{T} (t)\begin{array}{*{20}c} {} \\ \end{array} F_{us2}^{T} (t) \ldots F_{usn}^{T} (t)} \right]^{T} \in {\mathbb{R}}^{n \times 3} .$$
(32)

Therefore, Eq. (27) can be rewritten as:

$$\dot{\hat{v}}_{u} (t) = - \gamma_{1} L\hat{x}_{u} (t) - \gamma_{2} L\hat{v}_{u} (t) + \gamma_{3} F_{us} (t).$$
(33)

The Laplacian matrix is shown in Eq. (34):

$$L = \left[ {\begin{array}{*{20}c} {\sum\limits_{j = 1}^{n} {a_{1j} } } & { - a_{12} } & \cdots & { - a_{1n} } \\ { - a_{21} } & {\sum\limits_{j = 1}^{n} {a_{2j} } } & \cdots & { - a_{2n} } \\ \vdots & \vdots & \ddots & \vdots \\ { - a_{n1} } & { - a_{n2} } & \cdots & {\sum\limits_{j = 1}^{n} {a_{nj} } } \\ \end{array} } \right].$$
(34)

Combined with Eq. (26), Eq. (33) can be expressed as:

$$\dot{\hat{v}}_{u} (t) = - \gamma_{1} Lx_{u} (t) - \gamma_{2} Lv_{u} (t) + \gamma_{1} Lx^{\prime}_{u} + \gamma_{2} Lv^{\prime}_{u} + \gamma_{3} F_{us} (t).$$
(35)

The Eq. (35) is expressed as a matrix form as shown in Eq. (36):

$$\left[ {\begin{array}{*{20}c} {\dot{x}_{u} (t)} \\ {\dot{v}_{u} (t)} \\ \end{array} } \right] = \left[ {\begin{array}{*{20}c} {0_{n} } & {I_{n} } \\ { - \gamma_{1} L} & { - \gamma_{2} L} \\ \end{array} } \right]\left[ {\begin{array}{*{20}c} {x_{u} (t)} \\ {v_{u} (t)} \\ \end{array} } \right] + \left[ {\begin{array}{*{20}c} {0_{n} } & {0_{n} } \\ {\gamma_{1} L} & {\gamma_{2} L} \\ \end{array} } \right]\left[ {\begin{array}{*{20}c} {x^{\prime}_{u} } \\ {v^{\prime}_{u} } \\ \end{array} } \right] + \left[ {\begin{array}{*{20}c} {0_{n} } \\ {F_{us} (t)} \\ \end{array} } \right],$$
(36)

where In is an n-dimensional identity matrix, and 0n is an n-dimensional zero vector.

Next, Eq. (36) is solved, and the stability and convergence of the system are discussed according to the characteristics of the solution. The solution of this equation corresponding to the homogeneous equation is:

$$\left[ {\begin{array}{*{20}c} {\dot{\hat{x}}_{u} (t)} \\ {\dot{\hat{v}}_{u} (t)} \\ \end{array} } \right] = \left[ {\begin{array}{*{20}c} {0_{n} } & {I_{n} } \\ { - \gamma_{1} L} & { - \gamma_{2} L} \\ \end{array} } \right]\left[ {\begin{array}{*{20}c} {x_{u} (t)} \\ {v_{u} (t)} \\ \end{array} } \right] + \;\left[ {\begin{array}{*{20}c} {0_{n} } \\ {F_{us} (t)} \\ \end{array} } \right].$$
(37)

Now the Lyapunov function including the potential energy and kinetic energy function of the formation system is established as shown in Eq. (38). Among them, the attractive potential energy can be set to zero because the UAV is always affected by the attractive potential field of the target to move towards the target point.

$$\begin{aligned} V(t) & = \frac{1}{2}\hat{v}_{u}^{T} (t)\hat{v}_{u} (t) + \frac{1}{2}\gamma_{1} \hat{x}_{u}^{T} (t)L\hat{x}_{u} (t) \\ & + \sum\limits_{i = 1}^{n} {\left[ {\gamma_{3} \sum\limits_{o = 1}^{m} {U_{uopi} \left( {\hat{x}_{ui} (t)} \right)} + \frac{1}{2}\gamma_{3} \sum\limits_{j = 1}^{n} {U_{uij} \left( {\hat{x}_{ui} (t)} \right)} } \right]} . \\ \end{aligned}$$
(38)

Taking the derivative of Eq. (38) can get Eq. (39):

$$\begin{aligned} \dot{V}(t) & = \hat{v}_{u}^{T} (t)\left[ {\dot{\hat{v}}_{u} (t) + \gamma_{1} L\hat{x}_{u} (t) + \gamma_{3} \sum\limits_{i = 1}^{n} {\sum\limits_{o = 1}^{m} {\dot{U}_{uopi} \left( {\hat{x}_{ui} (t)} \right)} } } \right. \\ & + \left. {\gamma_{3} \sum\limits_{i = 1}^{n} {\sum\limits_{j = 1}^{n} {\dot{U}_{uij} \left( {\hat{x}_{ui} (t)} \right)} } } \right] \\ & = \hat{v}_{u}^{T} (t)\left[ { - \gamma_{2} L\hat{v}_{u} (t) - \gamma_{1} L\hat{x}_{u} (t)} \right. \\ & - \gamma_{3} \sum\limits_{i = 1}^{n} {\sum\limits_{o = 1}^{m} {\nabla U_{uopi} \left( {\hat{x}_{ui} (t)} \right)} } - \gamma_{3} \sum\limits_{i = 1}^{n} {\sum\limits_{j = 1}^{n} {\nabla U_{uij} \left( {\hat{x}_{ui} (t)} \right)} } \\ & + \gamma_{1} L\hat{x}_{u} (t) + \gamma_{3} \sum\limits_{i = 1}^{n} {\sum\limits_{o = 1}^{m} {\nabla U_{uopi} \left( {\hat{x}_{ui} (t)} \right)} } \\ & + \left. {\gamma_{3} \sum\limits_{i = 1}^{n} {\sum\limits_{j = 1}^{n} {\nabla U_{uij} \left( {\hat{x}_{ui} (t)} \right)} } } \right] \\ & = - \gamma_{2} \hat{v}_{u}^{T} (t)L\hat{v}_{u} (t) \\ & = - \gamma_{2} \sum\limits_{i = 1}^{n} {\sum\limits_{j = 1}^{n} {a_{ij} \left[ {\hat{v}_{ui} (t)\left( {\hat{v}_{ui} (t) - \hat{v}_{uj} (t)} \right)} \right]} } \\ & = - \gamma_{2} \sum\limits_{i = 1}^{n} {\sum\limits_{j = 1}^{n} {a_{ij} \left[ {\left\| {\hat{v}_{ui} (t) - \frac{1}{2}\hat{v}_{ui} (t)} \right\|^{2} + \left\| {\frac{1}{2}\hat{v}_{uj} (t)} \right\|} \right]^{2} } } \\ & \le 0. \\ \end{aligned}$$
(39)

Knowing that \(\gamma_{2} > 0\), from Eq. (39), when \(t \to \infty\), there is \(\dot{V}(t) \le 0\), that is, \(V(t)\) is monotonically decreasing.

\(L_{V} (a) = \{ [\hat{x}_{u} (t),\hat{v}_{u} (t)]^{T} \left| {V(t) \le a} \right.\}\) is defined as a bounded set, if \([\hat{x}_{u} (0),\hat{v}_{u} (0)]^{T} \in L_{V} (a)\), then \(V(t) \le a\) holds for both \(\forall t\), and \(L_{V} (a)\) is the invariant set. According to the principle of the LaSalle invariant set [35], the formation system will eventually converge to the maximum invariant set \(\Omega = \{ [\hat{x}_{u} (t),\hat{v}_{u} (t)]^{T} \in L_{V} (a)\left| {\dot{V}(t) = 0} \right.\}\) of \(L_{V} (a)\) if the initial solution of the system is in \(L_{V} (a)\). If \(\dot{V}(t) = 0\), then there is \(\hat{v}_{u} (t) = 0\), and \(v_{u} (t) = v^{\prime}_{u}\) is easy to get, then there is \(\hat{v}_{u1} (t) = \hat{v}_{u2} (t) = \cdots = \hat{v}_{un} (t) = 0\), so there is \(\hat{x}_{u1} (t) = \hat{x}_{u2} (t) = \cdots = \hat{x}_{un} (t) = 0\). It can be seen that \(V(t)\) is positive semidefinite because \(L\) is positive semidefinite, and the total energy of the potential field is \(U_{u} (x^{\prime}_{u} ) \ge 0\), that is, \(U_{u} (x_{u} (t)) \ge 0\) is obtained. Therefore, the system can be stable according to the Lyapunov theorem.

For the convergence and consensus of the system. There is \(\dot{v}^{\prime}_{u} = 0\) because the initially expected velocity is a constant value, and the special solution of the corresponding non-homogeneous equation of Eq. (36) can be solved under this constraint:

$$\left[ {\begin{array}{*{20}c} {\hat{x}_{u} (t)} \\ {\hat{v}_{u} (t)} \\ \end{array} } \right] = \left[ {\begin{array}{*{20}c} {x^{\prime}_{u} } \\ {v^{\prime}_{u} } \\ \end{array} } \right].$$
(40)

Therefore, when \(t \to \infty\), and the control protocol parameters are reasonably selected, the solution of Eq. (36) will gradually converge to \([x^{\prime}_{u} ,v^{\prime}_{u} ]^{T}\). That is to say, each UAV can restore the originally expected formation after avoiding obstacles to achieve the convergence and consensus of the state.

The realization of collision avoidance between UAVs and obstacle avoidance can be proved by contradiction.

For the realization of collision avoidance between UAVs, supposing \([\hat{x}_{u} (0),\hat{v}_{u} (0)]^{T} \in L_{V} (a)\) is the initial state, when \(t_{1} > 0\), UAV \(i\) and UAV \(j\) collide, at this time \(U_{uij} (x_{ui} (t_{1} )) \to \infty\), then there are:

$$\begin{aligned} V(t_{1} ) & = U_{u} (\hat{x}_{u} (t_{1} )) + \frac{1}{2}\hat{v}_{u}^{T} (t_{1} )\hat{v}_{u} (t_{1} ) + \frac{1}{2}\gamma_{1} \hat{x}_{u}^{T} (t_{1} )L\hat{x}_{u} (t_{1} ) \\ & \ge U_{u} (\hat{x}_{u} (t_{1} )) \\ & \ge U_{uij} (\hat{x}_{ui} (t_{1} )) \\ & = U_{uij} (x_{ui} (t_{1} )) \to \infty . \\ \end{aligned}$$
(41)

From Eq. (41), it is contradictory to \(V(t) \le a\), so the original hypothesis is not true, that is, there will be no collision between UAVs in the system. In the same way, the UAV's ability to avoid obstacles can also be proved by contradiction, which is omitted here to save space.

In summary, under Eq. (27), multiple UAVs can adapt to static and dynamic obstacle environments. They can avoid obstacles safely while preventing collisions between UAVs. The UAV can quickly restore the originally expected formation and achieve the consensus of relative position and velocity after avoiding obstacles. The certification is completed.

5 Numerical Simulation

5.1 Establishment of Simulation Environment and Parameter Initialization

This paper intends to simulate and compare the traditional APF, the IAPF in this paper, the algorithm in [27], and the obstacle avoidance control algorithm of cooperative formation proposed in this paper in a 3D obstacle environment to verify the effectiveness and feasibility of the proposed algorithm. Static obstacles are simulated by 2 cylinders, 3 spheres and 2 hemispheres, respectively, and another sphere is used to simulate dynamic obstacles, and it moves from \({(120,25,95)}\) to \({(25,120,95)}\) at a velocity of \(v_{o} = 5\;{\text{m}}/{\text{s}}\). Four UAVs are pre-formed at the starting point of the expected formation based on a diamond. A fixed undirected connected graph is used to represent the communication network topology of the multi-UAV system as shown in Fig. 3. There is a two-way connection between the UAVs and the expected distance is 40 m, that is, \(d_{u12} = d_{u13} = d_{u24} = d_{u34} = d_{u23} = 40\;{\text{m}}\), then \(d_{u14} = 40\sqrt 3 \;{\text{m}}\), and the relative height between the UAVs can also be \(h_{u12} = h_{u13} = h_{u24} = h_{u34} = 20\sqrt 3 \;{\text{m}}\), \(h_{u23} = 0\;{\text{m}}\), \(h_{u14} = 40\sqrt 3 \;{\text{m}}\). If the two UAVs can exchange information, there is \(a_{ij} = 2\), otherwise \(a_{ij} = 0\), and for \(i \in \left\{ {1,2, \ldots ,n} \right\}\), \(a_{ii} = 0\) will be assumed, and the adjacency matrix is shown in Eq. (42). The preset formation center vector is set according to the expected distance of formation as shown in Eq. (43). The main parameters of the algorithm proposed in this paper are \(q = 0.5\), \(\gamma_{1} = 0.001\), \(\gamma_{2} = 0.5\), \(\gamma_{3} = 1\). The coordinates of the geometric center position of each obstacle are shown in Table 1. The starting position and initial velocity of each UAV and target position coordinates are shown in Table 2.

$$A{ = }\left[ {\begin{array}{*{20}c} 0 & 2 & 2 & 0 \\ 2 & 0 & 0 & 2 \\ 2 & 0 & 0 & 2 \\ 0 & 2 & 2 & 0 \\ \end{array} } \right],$$
(42)
$$\left\{ {\begin{array}{*{20}c} {x^{\prime}_{u1} = \left( {0,0,\frac{\sqrt 3 }{2}d_{uij} } \right)\begin{array}{*{20}c} {} \\ \end{array} \begin{array}{*{20}c} {} & {} & {} \\ \end{array} } \\ {x^{\prime}_{u2} = \left( { - \frac{\sqrt 2 }{4}d_{uij} , - \frac{\sqrt 2 }{4}d_{uij} ,0} \right)} \\ {x^{\prime}_{u3} = \left( {\frac{\sqrt 2 }{4}d_{uij} ,\frac{\sqrt 2 }{4}d_{uij} ,0} \right)\begin{array}{*{20}c} {} \\ \end{array} \begin{array}{*{20}c} {} \\ \end{array} } \\ {x^{\prime}_{u4} = \left( {0,0, - \frac{\sqrt 3 }{2}d_{uij} } \right).\begin{array}{*{20}c} {} \\ \end{array} \begin{array}{*{20}c} {} \\ \end{array} \begin{array}{*{20}c} {} \\ \end{array} \begin{array}{*{20}c} {} \\ \end{array} \begin{array}{*{20}c} {} \\ \end{array} } \\ \end{array} } \right.$$
(43)
Fig. 3
figure 3

Formation communication network topology

Full size image
Table 1 Obstacle position coordinates
Full size table
Table 2 The starting position and initial velocity of the UAV and the position of the target
Full size table

5.2 Simulation Results and Analysis

In the 3D obstacle environment model, the traditional APF (Denoted as \(A_{1}\)), the IAPF in this paper (Denoted as \(A_{2}\)), the control algorithm in [27] (Denoted as \(A_{3}\)) and the obstacle avoidance control algorithm for cooperative formation proposed in this paper (Denoted as \(A_{4}\)) are simulated from the aspects of formation obstacle avoidance track and state consensus, respectively, and the results are compared and analyzed to further verify the effectiveness and feasibility of \(A_{4}\).

The formation obstacle avoidance track of the diamond formation under the action of the above four algorithms are shown in Fig. 4a–d, respectively.

Fig. 4
figure 4

The formation obstacle avoidance track of the diamond formation under different algorithms

Full size image

It can be seen from Fig. 4a that the four UAVs can pass between the two cylinders, but they stagnate near the first sphere obstacle on the left when the diamond formation is flying under the action of the algorithm \(A_{1}\). A complete and feasible trajectory from the starting point to the target point was not planned, and the formation flight mission could not be completed. It can be seen from Fig. 4b that the four UAVs can not only avoid static obstacles but also dynamic obstacles. Finally, all UAVs can reach the target position when the diamond formation is flying under the action of the algorithm \(A_{2}\). However, the UAVs moved directly toward the target point and did not quickly recover the expected formation after avoiding obstacles. It can be seen from Fig. 4c that the four UAVs can avoid static obstacles, but the UAV4 directly passes through the dynamic obstacles and cannot avoid it because the dynamic obstacles are not considered in the repulsive potential field function when the diamond formation is flying under the action of the algorithm \(A_{3}\). Although all UAVs can eventually reach the target position, the speed of the UAVs returning to the originally expected formation is relatively slow. It can be seen from Fig. 4d that the UAV's adaptability to complex obstacle environments is enhanced because the potential field gains in the algorithm \(A_{4}\) are all factors that can be automatically adjusted in real-time. All four UAVs can safely evade static and dynamic obstacles. The UAVs can quickly return to the expected formation and maintain a stable formation after avoiding obstacles. Finally, the UAVs reach the target point smoothly without collisions between UAVs.

The corresponding state change curve is not drawn because the UAV cannot plan a complete trajectory under the effect of the traditional APF. The change curve of the relative distance and the relative height between the UAVs under the algorithm \(A_{{2}}\), algorithm \(A_{{3}}\), and algorithm \(A_{4}\) of the diamond formation with time \(t\) is shown in Fig. 5, where time \(t\) is the total step time.

Fig. 5
figure 5

The change curve of the relative position between UAVs

Full size image

It can be seen from Fig. 5 that the four UAVs can maintain the preset diamond formation before obstacle avoidance, and the relative distance and relative height between the UAVs can be consistent with the expected value when the diamond formation is flying under the action of the above three algorithms. The formations are scattered, and the relative position change curve has a certain range of fluctuations in the process of obstacle avoidance. However, the relative distance and relative height of the UAV did not quickly converge to the expected value after avoiding obstacles under the algorithm \(A_{{2}}\), which shows that the consensus of the relative position cannot be achieved by simply improving the APF. The relative distance and relative height of the UAV begin to converge to the expected value at about the 100th second after obstacle avoidance under the algorithm \(A_{3}\), which shows that the convergence speed of the relative position between the UAVs is slow when the communication weight is not considered in [27]. The relative distance and relative height of the UAV begin to converge to the expected value at about the 80th second after obstacle avoidance under the algorithm \(A_{4}\), which shows that the introduction of communication weights in the potential field function between UAVs can speed up the convergence speed of the relative position.

The change curve of the velocity component of the diamond formation under the algorithm \(A_{{2}}\), algorithm \(A_{{3}}\), and algorithm \(A_{4}\) with time \(t\) is shown in Fig. 6.

Fig. 6
figure 6

The change curve of the velocity component of the UAV

Full size image

It can be seen from Fig. 6 that the velocity components on each coordinate axis remain the same before obstacle avoidance when the diamond formation is flying under the action of the above three algorithms. During the obstacle avoidance process, the UAV's velocity component change curve has a certain range of fluctuations. Among them, the velocity curves under algorithms \(A_{{2}}\) and \(A_{4}\) fluctuate when the dynamic obstacle moves near the UAV, and the velocity change under this algorithm is more severe than other algorithms because the algorithm \(A_{4}\) takes into account the planning angle and distance deviation, which can improve the efficiency of obstacle avoidance. However, the velocity curve under the algorithm \(A_{3}\) does not show a certain degree of fluctuation, which shows that the algorithm in [27] cannot avoid dynamic obstacles. The velocity components under the algorithm \(A_{2}\) do not completely converge after each UAV avoids obstacles. The velocity component under the algorithm \(A_{3}\) starts to converge approximately at the 100th second.

The velocity component under the algorithm \(A_{4}\) starts to converge at about the 80th second, which shows that the algorithm in this paper converges faster than the algorithm in [27], which fully verifies the feasibility of the algorithm in this paper.

6 Conclusion and Outlook

The problems of cooperative formation avoidance and consensus control of multi-UAV systems in a 3D obstacle environment are mainly studied in this paper. The main conclusions are as follows:

  1. 1.

    The coordination gains factor of the attractive force is introduced into the segmented attractive potential field function, and the influence factor of the repulsive force is introduced into the repulsive potential field function to prevent the UAV from falling into a local minimum and ensure that the UAV reaches the target point smoothly.

  2. 2.

    The coordination gains factor based on the movement velocity of the dynamic obstacle, planning angle, and distance deviation is introduced into the repulsive potential field function to enhance the UAV's adaptability to complex obstacle environments and improve the efficiency of obstacle avoidance.

  3. 3.

    The communication weight is introduced into the potential field function between the UAVs, and a cooperative formation obstacle avoidance control algorithm based on the IAPF and the improved consensus protocol is designed. The algorithm can achieve multiple control objectives such as collision avoidance, obstacle avoidance, formation recovery, and the consensus of the position and velocity.

This paper assumes that the communication between UAVs is good, but there may be communication delays in the actual formation system. Therefore, it is also worth studying the consensus problems of multi-UAV formations with communication delays based on the algorithm of this paper in future work.