In this section presents, an extended artificial potential field method is presented for a leader–follower formation of UAVs with obstacle avoidance in 3D environment.
Control algorithm for each member-UAV
The motion of the member-UAV is driven by the total artificial force that consists of two components as follows:
$$\begin{aligned} u_n = \frac{{{f_{nc}} + {f_n}}}{m} \end{aligned}$$
(4)
where m is the mass of the nth UAV. The first component \(f_{nc}\) is an attractive force to control the UAV to reach the spherical surface whose center is leader UAV. \(f_{nc}\) can be described as:
$$\begin{aligned} {f_{nc}} = \left( {{f_{{x_{nc}}}},{f_{{y_{nc}}}},{f_{{z_{nc}}}}} \right) \end{aligned}$$
(5)
where
$$\begin{aligned} f_{x_{nc}}&= -{k_s}\left( {{x_n} - {x_l}} \right) \left( {{{\left( {{x_n} - {x_l}} \right) }^2}+ {{\left( {{y_n}-{y_l}} \right) }^2}+{{\left( {{z_n}-{z_l}} \right) }^2}-{{r_a}^2}} \right) \nonumber \\ f_{y_{nc}}&= - {k_s}\left( {{y_n} -{y_l}} \right) \left( {{{\left( {{x_n} - {x_l}} \right) }^2} + {{\left( {{y_n}-{y_l}} \right) }^2}+{{\left( {{z_n}-{z_l}} \right) }^2}-{{r_a}^2}} \right) \nonumber \\ f_{z_{nc}}&= - {k_s}\left( {{z_n} - {z_l}} \right) \left( {{{\left( {{x_n} - {x_l}} \right) }^2} + {{\left( {{y_n} - {y_l}} \right) }^2} + {{\left( {{z_n} - {z_l}} \right) }^2} - {{r_a}^2}} \right) \end{aligned}$$
(6)
where \((x_l,y_l,z_l)\) is the coordinate of the leader UAV. \(k_s\) is the gain coefficient.
The second component \(f_n\), which is the resultant force of repulsive forces of the UAVs, is created to arrange the unmanned vehicles evenly distributed on the spherical surface. We define leader-UAV as the center at \(\left( {{x_l},{y_l},{z_l}} \right)\) and all UAVs have been negatively or positively charged. The repulsive forces affect on the unmanned vehicles which have identical electric charge. The control force that is defined in (5) keeping the UAVs on the spherical surface whose radius is \(r_a\) and center is \(\left( {{x_l},{y_l},{z_l}} \right)\). The unmanned vehicle reaches the equilibrium point when the resultant of repulsive forces tangent to the spherical surface acting on an UAV is zero. It means that the distances between UAVs are equal. Then, the goal is achieved. The repulsive force between two UAVs is defined as follows:
$$\begin{aligned} {f_{ni}} = {k_r}\frac{{{q_n}{q_i}}}{{r_{ni}^2}} \end{aligned}$$
(7)
where \({{q_n}}\) is electric quantity of the nth unmanned vehicle, \({{q_i}}\) is electric quantity of the ith unmanned vehicle, \({k_r}\) is the repulse constant coefficient, \({r_{ni}}\) is the distance between the ith and the nth unmanned vehicle. The amount of unmanned vehicle is N. Hence, the resultant of the repulsive forces from the other unmanned vehicles on the nth unmanned vehicle is given as follows:
$$\begin{aligned} {f_n} = {k_r}{q_n}\sum \limits _{i = 1,i \ne n}^N {\frac{{{q_i}}}{{r_{ni}^2}}} \end{aligned}$$
(8)
In Fig. 2, P is the initial position of UAV. \(P'\) is the position of UAV after move. \(Q_1\) is projection of P on the \(X_0-O-Y_0\) plane. \(Q_2\) is projection of \(P'\) on the \(X_0-O-Y_0\) plane. \(Q_3\) is projection of \(P^{\prime }\) on the \(Z_0-O-Q_1\) plane. By decomposing the resultant force in three directions, component forces are in the x-axis direction, y-axis direction and the z-axis direction, respectively. It is shown in Fig. 2 as follows:
$$\begin{aligned} {f_{x_{n}}}&= {k_r}{q_n}\sum \limits _{i = 1,i \ne n}^N {\frac{{{q_n}{q_i}}}{{r_{ni}^2}}\cos {\theta _{ni}}\cos {\varphi _{ni}}} \nonumber \\ {f_{y_{n}}}&= {k_r}{q_n}\sum \limits _{i = 1,i \ne n}^N {\frac{{{q_n}{q_i}}}{{r_{ni}^2}}\cos {\theta _{ni}}\sin {\varphi _{ni}}} \nonumber \\ {f_{z_{n}}}&= {k_r}{q_n}\sum \limits _{i = 1,i \ne n}^N {\frac{{{q_n}{q_i}}}{{r_{ni}^2}}\sin {\theta _{ni}}} \end{aligned}$$
(9)
where
$$\begin{aligned} \sin {\theta _{ni}}&= \frac{{{z_n} - {z_i}}}{{\left| {{r_{ni}}} \right| }}\\ \cos {\theta _{ni}}&= \frac{{\sqrt{{{\left( {{x_n} - {x_i}} \right) }^2} + {{\left( {{y_n} - {y_i}} \right) }^2}} }}{{\left| {{r_{ni}}} \right| }}\\ \cos {\varphi _{ni}}&= \frac{{{x_n} - {x_i}}}{{\sqrt{{{\left( {{x_n} - {x_i}} \right) }^2} + {{\left( {{y_n} - {y_i}} \right) }^2}} }}\\ \sin {\varphi _{ni}}&= \frac{{{y_n} - {y_i}}}{{\sqrt{{{\left( {{x_n} - {x_i}} \right) }^2} + {{\left( {{y_n} - {y_i}} \right) }^2}} }}\\ {r_{ni}}&= \sqrt{{{\left( {{x_n} - {x_i}} \right) }^2} + {{\left( {{y_n} - {y_i}} \right) }^2} + {{\left( {{z_n} - {z_i}} \right) }^2}} \end{aligned}$$
Consider the nth UAV’s dynamical equation (2), the nth UAV’s dynamical equations based on the virtual structure can be rewritten as:
$$\begin{aligned} {u_{{x_n}}}&= \frac{{{f_{{x_{nc}}}} + {f_{{x_n}}}}}{m}\nonumber \\ {u_{{y_n}}}&= \frac{{{f_{{y_{nc}}}} + {f_{{y_n}}}}}{m}\nonumber \\ {u_{{z_n}}}&= \frac{{{f_{{z_{nc}}}} + {f_{{z_n}}}}}{m} \end{aligned}$$
(10)
Given the above discussion, the repulsive force that is proportional to \(1/{r_{ni}}\) avoids the collision between unmanned vehicles. The control force \((u_{x_n},u_{x_n},u_{x_n})\) in (10) leads the UAVs toward the equilibrium points on the spherical surface.
Control algorithm for the UAV-leader
The motion of the UAV-leader is driven by the attractive force to reach a moving target. The position of leader UAV \(p_l=(x_l,y_l,z_l)\). Consider the target position is \(p_t=(x_t,y_t,z_t)\), the attractive force can be stated as follow:
if \(r < d\)
$$\begin{aligned} {f_{{x_{att}}}}&= - {k_t}\left( {{x_l} - {x_t}} \right) \nonumber \\ {f_{{y_{att}}}}&= - {k_t}\left( {{y_l} - {y_t}} \right) \nonumber \\ {f_{{z_{att}}}}&= - {k_t}\left( {{z_l} - {z_t}} \right) \end{aligned}$$
(11)
else
$$\begin{aligned} {f_{x_{att}}}&= - {k_t}\left( {{x_l} - {x_t}} \right) \frac{{{d_{lt}}}}{{{r_t}}}\nonumber \\ {f_{y_{att}}}&= - {k_t}\left( {{y_l} - {y_t}} \right) \frac{{{d_{lt}}}}{{{r_t}}}\nonumber \\ {f_{z_{att}}}&= - {k_t}\left( {{z_l} - {z_t}} \right) \frac{{{d_{lt}}}}{{{r_t}}} \end{aligned}$$
where \(k_t\) is the positive constants. \(d_{lt}\) is the distance between target and the leader UAV. \({d_{lt}} = \sqrt{{{\left( {{x_t} - {x_l}} \right) }^2} + {{\left( {{y_t} - {y_l}} \right) }^2} + {{\left( {{z_t} - {z_l}} \right) }^2}}. r_t\) is defined as the range of the target.
The relative velocity among the leader and the target is added as a damping force to control the leader-UAV when it approaches the range of the target position. The damping force force is proposed as follows:
$$\begin{aligned} {f_{x_{dam}}}&= - {k_m}\left( {{{\dot{x}}_l} - {{\dot{x}}_t}} \right) \nonumber \\ {f_{y_{dam}}}&= - {k_m}\left( {{{\dot{y}}_l} - {{\dot{y}}_t}} \right) \nonumber \\ {f_{z_{dam}}}&= - {k_m}\left( {{{\dot{z}}_l} - {{\dot{z}}_t}} \right) \end{aligned}$$
(12)
where \(k_m\) is the positive amplification coefficient. The control force is the resultant force of the attractive force and damp force
$$\begin{aligned} {f_{x_{l}}}&= {f_{x_{att}}} + {f_{x_{dam}}}\nonumber \\ {f_{y_{l}}}&= {f_{y_{att}}} + {f_{y_{dam}}}\nonumber \\ {f_{z_{l}}}&= {f_{z_{att}}} + {f_{z_{dam}}} \end{aligned}$$
(13)
Stability analysis
In order to analyze the stability of the formation, we analyze the a UAV at the equilibrium point. It means that \({f_n}\) is zero. Lemma 3.1 provides a trajectory that UAV moves to a spherical surface whose center is \(({x_c}, {y_c}, {z_c})\) and radius is \(r_a\). It is stable when the UAV reaches the spherical surface. In other words velocity of UAV will be zero.
Lemma 1
The desire trajectory of UAV satisfies
$$\begin{aligned} \dot{x}&= -\left( {x - {x_c}} \right) \left( {{{\left( {x - {x_c}} \right) }^2} + {{\left( {y- {y_c}} \right) }^2} + {{\left( {z - {z_c}} \right) }^2} - {{r_a}^2}} \right) \nonumber \\ \dot{y}&= -\left( {y - {y_c}} \right) \left( {{{\left( {x - {x_c}} \right) }^2} + {{\left( {y - {y_c}} \right) }^2} + {{\left( {z - {z_c}} \right) }^2} - {{r_a}^2}} \right) \nonumber \\ \dot{z}&= -\left( {z - {z_c}} \right) \left( {{{\left( {x - {x_c}} \right) }^2} + {{\left( {y - {y_c}} \right) }^2} + {{\left( {z - {z_c}} \right) }^2} - {{r_a}^2}} \right) \end{aligned}$$
(14)
where
\((x, y, z) \ne (x_c, y_c, z_c)\).
Proof
By inserting \({r^2} = {\left( {x - {x_c}} \right) ^2} + {\left( {y - {y_c}} \right) ^2} + {\left( {z - {z_c}} \right) ^2}, \varphi = \arctan \mathrm{}\left( {{{\left( {y - {y_c}} \right) } \big / {\left( {x - {x_c}} \right) }}} \right)\) and \(\theta = \arctan \left( {{{\left( {z - {z_c}} \right) } \Bigg / {\left( {\sqrt{{{\left( {y - {y_c}} \right) }^2} + {{\left( {x - {x_c}} \right) }^2}} } \right) }}}\right)\) into Eq. (6). \(\varphi\) and \(\theta\) are shown in Fig. 2. The equations can be achieved as follows:
$$\begin{aligned} \dot{r} = - r\left( {{r^2} - {{r_a}^2}} \right) \;\;\;\;\dot{\theta }= 0\;\;\dot{\varphi }= 0 \end{aligned}$$
(15)
To prove the stability of UAV arriving at the spherical surface, it is obvious that \(\dot{\theta }\) and \(\dot{\varphi }\) converge to zero when \(r={r_a}\), we define the error \(\varepsilon =r-{r_a}\) and choose the following Lyapunov function candidate:
$$\begin{aligned} V\left( \varepsilon \right) = {\varepsilon ^2} \end{aligned}$$
(16)
The derivative of the Lyapunov function (4) is given by
$$\begin{aligned} \dot{V}\left( \varepsilon \right) = 2\varepsilon \dot{r} \end{aligned}$$
(17)
Substituting (3) into (5), we have
$$\begin{aligned} \dot{V}\left( \varepsilon \right) = - 2{\varepsilon ^2}r\left( {r +{r_a} } \right) \end{aligned}$$
(18)
Because \((x, y, z) \ne (x_c, y_c, z_c)\), it can be deuced that \(r>0\). It is obvious that \(\dot{V}\left( \varepsilon \right) \le 0\). Since \(V\left( \varepsilon \right) = \mathrm{{0}}\) only if \(r={r_a}\), it follows that velocity is non-increasing, \(\dot{\theta }=0\), \(\dot{\varphi }=0\) and \(V\left( \varepsilon \right)\) is bounded. In other words, UAV’s trajectory converges to the spherical surface. Unmanned vehicle will have no rotational motion when arrives at the spherical surface.\(\square\)