Multi-UAV Trajectory Planning during Cooperative Tracking Based on a Fusion Algorithm Integrating MPC and Standoff

Abstract

In this paper, an intelligent algorithm integrating model predictive control and Standoff algorithm is proposed to solve trajectory planning that UAVs may face while tracking a moving target cooperatively in a complex three-dimensional environment. A fusion model using model predictive control and Standoff algorithm is thus constructed to ensure trajectory planning and formation maintenance, maximizing UAV sensors’ detection range while minimizing target loss probability. Meanwhile, with this model, a fully connected communication topology is used to complete the UAV communication, multi-UAV formation can be reconfigured and planned at the minimum cost, keeping off deficiency in avoiding real-time obstacles facing the Standoff algorithm. Simulation validation suggests that the fusion algorithm proves to be more capable of maintaining UAVs in stable formation and detecting the target, compared with the model predictive control algorithm alone, in the process of tracking the moving target in a complex 3D environment.

1. Introduction

The increasingly complex mission environment in recent years has given UAVs their favored market, seeing them widely used for reconnaissance and monitoring missions due to their low cost, high autonomy and reusability [1,2]. Tracking a moving target, whether for single or cooperative tacking, is a significant sub-problem for UAVs performing monitoring tasks. Yet, a single UAV can hardly meet its actual task requirements as it works on its own [3,4], because its sensor’s range of view may be easily blocked and therefore its ability to accomplish tasks limited. Cooperation of several UAVs, however, helps make target tracking and monitoring easier. Cooperative efforts made by UAVs can reduce the risk of target loss [5,6], and ensure the accomplishment of a task with multi-sensor data fusion, which means multi-UAV collaboration used in trajectory planning for moving target tracking purposes.

At present, multi-UAV collaborative planning mainly involves artificial potential field method [7,8], bionic algorithm and control algorithm. When the artificial potential field method is applied to the collaborative planning process, it is easy to fall into local optimality and difficult to establish a complete mathematical model. Bionic algorithms, which mainly include ant colony algorithms [9], and particle swarm algorithms [10], also prove to be challenging to meet the real-time demand due to their limited processing efficiency. Control algorithms mainly cover PID control [11], optimal control [12], H-infinity robust control [13], sliding mode control [14], and model predictive control [15,16], etc. Most of these algorithms, such as PID control, optimal control, H-infinity robust control and sliding mode control, are not suitable for complex variable control problems such as cooperative planning of multiple UAVs given their limited control variables and application scenarios that appear quite poor, while the model predictive control algorithm, as the only control method that can explicitly handle constraints at present, has leveled itself up to the acknowledged standard for handling complex constrained variable control problems. It adopts a form of rolling optimization and feedback correction, i.e., the predicted trajectory will be corrected online at each sampling cycle. With strong anti-interference ability and strong robustness, it has attracted widespread attention from scholars at home and abroad. Animesh Sahu [17] and others conducted a study on multi-UAV tracking of multiple moving targets in two dimensions based on the model predictive control algorithm and developed a data-driven Gaussian process (GP) based model that relates the hyperparameters used in model predictive control to mission efficiency. Marc Ille [18] and others carried out research on multi-UAV formation collision avoidance in two-dimensional environments based on the model predictive control algorithm, optimized model predictive control cost functions using penalty term methods, and controlled UAVs’ track planning as they tracked a moving target based on formation avoidance constraints. However, relevant research on [17,18] UAV formation control is rare. Tagir Z. Muslimov [19] and others proposed a method based on the Lyapunov vector field for multi-UAV cooperative tracking of the moving target in a two-dimensional environment. The method is grounded around dispersed guided Lyapunov vector fields for path planning. Based on the two-dimensional environment, Q. Guo [20] and others proposed a performance guaranteed $5\frac{1}{3}$-approximation algorithm for the UAV scheduling problem when ignoring the limited flying time of each UAV, such that the maximum spent time of UAVs in their flyingtours is minimized. A fusion algorithm for adaptive multi-model traceless Kalman particle filter was adopted by Niu Yifeng [21] and others to carry out a study on coordinated tracking of ground multi-target trajectory for UAV swarms in complex two-dimensional environments. A pioneering exploration is Zhang Yi [22] and others who solved the problems regarding non-convergence of initial heading and long phase coordination time among UAVs in the process of cooperatively tracking a moving target based on Standoff method, following which Zhu Qian [23] and his team also studied two aircrafts’ cooperative tracking of a moving target by means of angle measurement.

A comprehensive analysis of the above research found that most of the current research on multi-UAV trajectory planning through cooperative formation stays in two-dimensional space, still challenged by problems such as large model calculation and insufficient real-time. At the same time, the current research faces great difficulty in establishing a complete non-linear UAV 3D motion model, and thus fails to meet actual mission requirements [24]. As for traditional multi-UAV sensors, their limited detection coverage as well as weak formation and retention capabilities [25] prevent them from being the hot spot in this field, leaving UAV trajectory planning that integrates collision avoidance and obstacle avoidance not fully explored.

Against such a background, this paper proposes a fusion algorithm that combines the model predictive control algorithm [26] and the Standoff algorithm. The model predictive control algorithm solves the problem of large-scale real-time optimal control in limited time [27] and uses the preview capability to achieve optimal maneuver control in a constrained, non-linear, model-uncertain and unpredictable environment to generate smooth flyable paths suitable for the actual flight of the formation [28]. The Standoff algorithm [29], one of the main algorithms for formation control, maximizes sensor detection range and reduces the probability of target loss with safe distances as grounds [30]. Compared with the traditional multi-UAV cooperative trajectory planning method, the fusion algorithm simplifies the mathematical modelling of UAVs’ three-dimensional motion [31], reduces the computational complexity which is caused by strong non-linearity as defined in the dynamics [32], and enhances real-time performance that an algorithm can show compared with the two papers [33,34]. It integrates the maximization of the sensor’s observation coverage to establish UAV sensors’ monitoring model, and more importantly, reduces the probability that UAVs lose their moving target compared with the sensor detection model proposed by the thesis [35]; Inspired by the minimum long-term operational cost suggested by the paper [36], the present study designs the reconfiguration planning of UAV formation at the minimum cost. As the distributed learning principle reported in the research [37] indicates, it constructs a multi-UAV track planning model using a distributed model predictive control algorithm to transform the challenge of centralized UAV formation mentioned in the paper [38] into that of a distributed flight control optimization, verifying the effectiveness of the fusion algorithm by means of unexpected artificially implanted obstacles.

The remainder of this paper is as follows: Section 2 introduces the trajectory planning model that UAVs take while they cooperatively track the moving target in a complex three-dimensional environment, followed by how it is configured and designed based on the fusion algorithm in Section 3, in addition to the cooperative formation reconfiguration and planning when an unexpected situation occurs to the vehicles. Simulation validation is carried out in Section 4 to demonstrate the effectiveness of the fusion algorithm applied to multi-UAV collaborative tracking of moving target trajectory planning. The following section conducts a study on the effectiveness and monitoring capability of multiple UAVs in coordinated formation to track moving targets, and illustrates that the fusion algorithm has better tracking effectiveness and monitoring capability in the test, while the last section offers a conclusion.

2. UAV Model and Environment Model

2.1. UAV Motion Model

Different from most of the previous literature that used the two-dimensional plane to establish the motion model of the UAV, this paper regards the UAV as a mass point and builds a three-dimensional motion model based on the inertial reference system without considering the influence of external disturbances, noise and air resistance on the UAV dynamics, and carries out discretization processing on it. Assuming that the sampling time is $∆t$, the UAV motion model is expressed as Equation (1).

$$\left\{\begin{array}{l}x(k+1)=x(k)+v(k)\cos \theta (k)\sin \phi (k)\Delta t\\ y(k+1)=y(k)+v(k)\cos \theta (k)\cos \phi (k)\Delta t\\ z(k+1)=z(k)+v(k)\sin \theta (k)\Delta t\\ v(k+1)=v(k)+a(k+1)\Delta t\\ \phi (k+1)=\phi (k)+\dot{\phi }(k+1)\Delta t\\ \theta (k+1)=\theta (k)+\dot{\theta }(k+1)\Delta t\\ {s(k)=[x(k),y(k),z(k)]}^{\prime }\in S\\ {u(k)=[v(k),\phi (k),\theta (k)]}^{\prime }\in U\end{array}\right.$$

(1)

where $s(k)$ denotes the UAV state sampling at time $k$; $S$ denotes the feasible state set; $u(k)$ denotes the control input of the UAV at time $k$; $U$ denotes the feasible input set; $(x(k),y(k),z(k))$ is the real time position of the UAV; $v(k)$, $\phi (k)$ and $\theta (k)$ denote the real time speed, heading angle and pitch angle of the UAV respectively, and $a$ dotes the acceleration of the UAV.

2.2. UAV Collision Avoidance Model

Since UAVs need to fly as ultra-low as possible in order to avoid radar detection, the complex ground environment and its obstacles become the primary threat to UAV trajectory planning. This paper creates a map model based on undulating terrain topography to fulfill the actual task requirements, as shown in Figure 1. To improve the robustness of the method, a safety buffer zone is established around the UAV, and the obstacles are divided into static obstacle modelling and emergent obstacles. The static obstacle model is approximated by a cylinder whose co-ordinate center is set to $P_o$, whose co-ordinates are ${[P}_{ox},P_{oy}]$, and whose radius and height are denoted by $P_{or}$ and $P_{oz}$. A collision zone (denoted by $L_{od}$ and ${∆H}_{od}$) and a threat zone (denoted by $L_{OD}$ and ${∆H}_{OD}$) are established around it. $L_{od}$ is the minimum proximity safety distance while ${∆H}_{od}$ is the minimum height proximity distance, and if the distance between the UAV and the static obstacle is less than $L_{od}$ and ${∆H}_{od}$, the UAV will collide. $L_{OD}$ and ${∆H}_{OD}$ are the maximum threat distance of the static obstacle, and if the distance between the UAV and the static obstacle is less than $L_{OD}$ and ${∆H}_{OD}$, the UAV may have the risk of collision. The sudden obstacle model is approximated by a sphere, the centre of which is set to $P_t$, with specific coordinates ${[P}_{tx},P_{ty},P_{tz}]$ and a radius of $R_t$. The collision zone (represented by a sphere with a radius of $R_p$) and the threat zone (represented by a sphere with a radius of $R_w$) are also set up, and the specific UAV collision avoidance and collision avoidance model is shown in Equation (2).

$$\left\{\begin{array}{l}{z}_i(k)-z_{all}(k)>\Delta H_d\\ \left|\begin{array}{l}{x}_i(k),y_i(k),z_i(k)\\ x_j(k),y_j(k),z_j(k)\end{array}\right|\ge 2R_a\\ \sqrt{{(x_i(k)-P_{tx})}^2+{(y_i(k)-P_{ty})}^2+{(z_i(k)-P_{tz})}^2}\ge R_w\\ \begin{array}{c}\sqrt{{(x_i(k)-P_{ox})}^2+{(y_i(k)-P_{oy})}^2}\ge L_{OD} or z_i(k)-P_{oz}\ge \Delta H_{OD}\end{array}\end{array}\right.$$

(2)

where ${(x}_i(k),y_i(k),z_i(k))$ denotes the current UAV position coordinates; $R_a$ denotes the UAV minimum collision avoidance safety distance; ${(x}_j(k),y_j(k),z_j(k))$ denotes the adjacent UAV position coordinates; $z_{all}(k)$ denotes the height of the ground coordinates ${(x}_i(k),y_i(k))$; and ${∆H}_d$ denotes the UAV near-ground minimum safety distance.

2.3. Moving Target Model

The establishment of a rationalized moving target motion model is the prerequisite for the successful track planning of UAVs when they cooperatively track the moving target. This paper defines the target motion model as Equation (3). To simplify the operation, the moving target’s trajectory is compressed from the three-dimensional space to the two-dimensional $\mathrm{y}\mathrm{o}\mathrm{z}$ plane, i.e., the $x$ coordinate of the moving target is set to a constant value.

$$\left[\begin{array}{l}{\dot{x}}_u(k)\\ {\dot{y}}_u(k)\\ {\dot{z}}_u(k)\\ {\dot{v}}_u(k)\\ {\dot{\theta }}_u(k)\\ {\dot{\phi }}_u(k)\end{array}\right]=\left[\begin{array}{l}{v}_u(k)\cos {\theta }_u(k)\sin {\phi }_u(k)\\ v_u(k)\cos {\theta }_u(k)\cos {\phi }_u(k)\\ v_u(k)\sin {\theta }_u(k)\\ -g\sin {\theta }_u(k)\\ \frac{-g\cos {\theta }_u(k)}{v_u(k)}\\ 0\end{array}\right]+\left[\begin{array}{lll}0\hfill & 0& 0\\ 0\hfill & 0& 0\\ 0\hfill & 0& 0\\ 1\hfill & 0& 0\\ 0\hfill & \frac{1}{v_u(k)}& 0\\ 0\hfill & 0& \frac{1}{v_u(k)\cos {\theta }_u(k)}\end{array}\right]\left[\begin{array}{l}{a}_1\\ a_2\\ a_3\end{array}\right]$$

(3)

where $v_u(k)$ denotes the velocity of the moving target at $k$, ${\theta }_u(k)$ denotes the pitch angle of the moving target, ${\phi }_u(k)$ denotes the heading angle of the moving target (${\phi }_u(k)=0$), $g$ is the acceleration of gravity, $a_1$ denotes the horizontal acceleration of the moving target, $a_2$ denotes the vertical acceleration of the moving target and $a_3$ denotes the angular acceleration of the moving target, and the motion constraint of the target can be completed by adjusting according to parameter ${a=[a}_1,a_2,a_3]$.

2.4. Target Observation Coverage Modelling

The modeling of target observation coverage is based on the UAV sensors. In this paper, the mathematical modeling of target observation coverage is based on four factors: $P_f$, $P_w$, $L_{max}$ and $L_{min}$. $P_f$ indicates the probability that the sensor detects the target effectively, $P_w$ indicates the probability that the sensor detects the target incorrectly, and $P_f$,$P_w\in (0.1]$. $L_{max}$ indicates the maximum detection distance of the sensor, and $L_{min}$ indicates the effective distance that the sensor detects completely. When the distance between the sensor and the target is less than $L_{min}$, $P_f=1$. Given the influence of multiple obstacles encountered during UAV trajectory planning, it does not meet the actual needs to only use the maximum detection distance of the sensor as the measurement standard. Based on this, this paper defines that the UAV is only likely to detect a target when the target enters an area where it can be seen by the vehicle, and the sensor is only capable of detecting the target when the target is within its coverage. The intersection of the area where the target is visible and the sensor’s coverage area is defined as the target observation coverage, which circumvents the obstruction of the UAV’s line of sight by environmental obstacles and ensures effective monitoring of the moving target by multiple UAVs in formation, as shown in Figure 2, whose discretization modelling is expressed as Equation (4). Define the effective detection range of UAV sensors and the radius of the target coverage area to be the same, both of which are $L_{min}=40 \mathrm{m}$. The UAV can monitor the moving target when it is within the sensor’s detection range.

$$p(L_t)=\left\{\begin{array}{l}\begin{array}{cc}1\hfill & L_t<L_{\min }\end{array}\\ \begin{array}{cc}{p}_f-\frac{(p_f-p_w)(L_t-L_{\min })}{L_{\max }-L_{\min }}\hfill & L_{\min }<L_t<L_{\max }\end{array}\\ \begin{array}{cc}{p}_w\hfill & L_t>L_{\max }\end{array}\end{array}\right.$$

(4)

where ${P(L}_t)$ denotes the probability of the sensor effectively monitoring the target and $L_t$ denotes the real-time distance between the UAV sensor and the target.

3. Designing a Multi-UAV Cooperative Tracking System Based on the Fusion Algorithm

3.1. System Design

After each UAV receives the tracking task, it initializes the system model according to the prior obstacle information, target movement information and its own motion information. In view of constraints such as obstacle avoidance and collision avoidance, the model predictive control algorithm is used to predict the trajectory of multiple UAVs at the minimum planning cost. In terms of formation and maintenance of multi-UAV formation, the Standoff algorithm is used to complete the multi-UAV formation control, so that the UAV swarm is evenly distributed around the target, and then multi-UAV sensors can maximize the monitoring of the moving target. The specific system framework is shown in Figure 3. The cooperative collision avoidance control module is mainly responsible for obstacle collision avoidance and inter-UAV collision avoidance, taking into account the UAV motion state, obstacle information, map boundaries and other factors to plan a safe and collision-free flight path. The model prediction control module is responsible for predicting the UAV trajectory at the minimum flight cost, and the distributed cooperative controller plans and coordinates the global trajectory. Standoff control module is mainly responsible for UAV formation maintenance, real-time acquisition of multi-UAV phase distribution, and maximizing UAV sensors’ coverage. The formation reconfiguration module means that during the flight of multiple UAVs in accordance with the established formation, the formation needs to carry out reconstruction planning due to unexpected situations.

3.2. Multi-UAV Cooperative Trajectory Planning Based on the Fusion Algorithm

In this paper, a fusion of model predictive control algorithm and Standoff algorithm is used to promote UAVs’ trajectory planning as they reach cooperative formation when tracking the moving target, as illustrated by Figure 4.

3.2.1. Multi-UAV Formation Control Based on the Standoff Algorithm

Multi-UAV formation control research using the Standoff algorithm is carried out in the following steps: introduce UAV-target relative desired distance and UAV sensors’ observation coverage information; use Lyapunov vector field guidance algorithm to guide the UAVs’ trajectory planning during moving target tracking to ensure that the moving target is within UAV sensors’ detection range to the maximum extent possible; control the UAV trajectory rotation characteristics to make it more flexible when optimizing the trajectory, and then better approach the desired position to reduce the probability of target loss. Figure 5 shows a schematic diagram of the UAV swarm model for tracking the moving target based on the Standoff algorithm.

Set the target motion state is known, the multi-UAV cooperative formation moves around the target circular motion through the Lyapunov function, and the multi-UAV speed adjustment is assisted by the feedback-correction mechanism, so as to maintain the ideal tracking of the multi-UAV formation and the moving target. In this paper, the radius of circular distribution is set as $D_r$, and the corresponding Lyapunov energy function is the distance function, as shown in Equation (5).

$$\left\{\begin{array}{l}{L}_d(x,y,z)={(r^2-D_r^2)}^2\\ \left|r-D_r\right|\le \xi \end{array}\right.$$

(5)

where $r$ is the radial distance between UAV position ${(x}_r,y_r,z_r)$ and moving target position ${(x}_d,y_d,z_d)$, $r=\sqrt{{(x_r-x_d)}^2+{(y_r-y_d)}^2+{(z_r-z_d)}^2}$, and $\xi$ denotes the formation coordination error.

Assuming that three UAVs are performing a moving target tracking task at the same time, the positioning process requires any two UAVs to be positioned in comparison to each other to maintain the relative balance of the three UAVs’ positions. In order to simplify the operation, this paper sets three UAVs distributed in the same plane, so only the influence of phase angle positioning needs to be considered. Assuming that the phase angles of any two UAVs are ${\varphi }_i$ and ${\varphi }_j$ respectively, and the expected relative phase angle is ${\varphi }_z$, the phase distribution function of multi-UAV cooperative formation is calculated based on the Lyapunov stability theory as shown in Equation (6).

$$\left\{\begin{array}{l}{\mathsf{\Phi }}_p={({\varphi }_i-{\varphi }_j-{\varphi }_z)}^2\\ {\varphi }_z=\frac{2\pi }{N},N\ge 2\end{array}\right.$$

(6)

where $N$ denotes the number of drones and $N=3$.

The speed calculation of any two UAVs is shown in Equation (7).

$$\begin{array}{l}{v}_i=v\\ v_j=k\cdot ({\varphi }_i-{\varphi }_j-{\varphi }_z)\cdot D_r+v\end{array}$$

(7)

where $v$ represents the real-time velocity of the moving target.

The phase angular velocity of any two UAVs is calculated as Equation (8).

$$\begin{array}{l}{\dot{\varphi }}_i=v_i/D_r\\ {\dot{\varphi }}_j=k\cdot ({\varphi }_i-{\varphi }_j-{\varphi }_z)+v_j/D_r\end{array}$$

(8)

where $k$ is the function coefficient.

Assuming that the moving target position and velocity are known, the optimal desired velocity of the UAV formation can be calculated by combining the multi-UAV predicted velocity with the moving target velocity correction term, which is calculated as Equation (9).

$$\left[\begin{array}{l}{\dot{x}}_t\\ {\dot{y}}_t\\ {\dot{z}}_t\end{array}\right]=\left[\begin{array}{l}{\dot{x}}_i-\dot{x}\\ {\dot{y}}_i-\dot{y}\\ {\dot{z}}_i-\dot{z}\end{array}\right]$$

(9)

where ${(\dot{x}}_i,{\dot{y}}_i,{\dot{z}}_i)$ is the predicted velocity value of the UAV and $(\dot{x},\dot{y},\dot{z})$ is the target velocity correction value.

The predicted speed $v_t$, heading angle ${\phi }_t$ and pitch angle ${\theta }_t$ of the multi-UAV formation can be calculated according to Equation (10).

$$\begin{array}{l}{v}_t=\sqrt{{\dot{x}}_t^2+{\dot{y}}_t^2+{\dot{z}}_t^2}\\ {\phi }_t=\arctan ({\dot{y}}_t^{}/{\dot{x}}_t^{})\\ {\theta }_t=\arctan ({\dot{z}}_t^{}/\sqrt{{\dot{x}}_t^2+{\dot{y}}_t^2})\end{array}$$

(10)

3.2.2. Track Planning UAVs Take during Cooperative Tracking of the Moving Target Based on the Fusion Algorithm

Inspired by the fact that the model predictive control algorithm can predict UAV trajectories in real time, and the applicability of the Standoff algorithm to UAV formation control, this paper reports on the trajectory planning UAVs take during cooperative tracking of the moving target based on the fusion of the two algorithms. Taking the i-th UAV as an example, given constraints such as multi-UAV collision avoidance and collision avoidance, the predicted motion state of the UAV in the finite time domain is constructed based on the model predictive control framework, the UAV cooperative trajectory planning model is constructed based on minimizing the UAV trajectory planning cost, while the fusion Standoff algorithm is used to carry out formation control, based on a “feedback-correction” mechanism using a moving target speed correction term to correct the optimal desired speed of the UAV in real time. With the scaling factor of UAV speed and angular speed added, the predicted velocity $v_t$ and predicted angular velocity ${\omega }_t$ of the multi-UAV formation are calculated in real time, as shown in Equation (11). Each UAV is solved at each sampling moment using the quadratic programming method to obtain its own optimal control sequence and local predicted trajectory, and the information at the current sampling moment is calculated on the basis of control sequence. The specific algorithm flow is displayed in Algorithm 1.

$$\left\{\begin{array}{l}\min (-f_1^i,f_2^i,f_3^i)\\ s.t.\\ \left[\begin{array}{l}{x}_i(k+p+1\left|k\right.)\\ y_i(k+p+1\left|k\right.)\\ z_i(k+p+1\left|k\right.)\end{array}\right]=\left[\begin{array}{l}{x}_i(k+p\left|k\right.)\\ y_i(k+p\left|k\right.)\\ z_i(k+p\left|k\right.)\end{array}\right]+\left[\begin{array}{l}{v}_i(k+p\left|k\right.)\cos {\theta }_i(k+p\left|k\right.)\sin {\phi }_i(k+p\left|k\right.)\\ v_i(k+p\left|k\right.)\cos {\theta }_i(k+p\left|k\right.)\cos {\phi }_i(k+p\left|k\right.)\\ v_i(k+p\left|k\right.)\sin {\theta }_i(k+p\left|k\right.)\end{array}\right]\Delta t\\ \left[\begin{array}{l}{\dot{x}}_t(k+p+1\left|k\right.)\\ {\dot{y}}_t(k+p+1\left|k\right.)\\ {\dot{z}}_t(k+p+1\left|k\right.)\end{array}\right]=\left[\begin{array}{l}{\dot{x}}_i(k+p+1\left|k\right.)-\dot{x}(k+p+1\left|k\right.)\\ {\dot{y}}_i(k+p+1\left|k\right.)-\dot{y}(k+p+1\left|k\right.)\\ {\dot{z}}_i(k+p+1\left|k\right.)-\dot{z}(k+p+1\left|k\right.)\end{array}\right]\\ v_t(k+p+1\left|k\right.)=v_t(k+p\left|k\right.)+(u_i^v(k+p\left|k\right.)-v_t(k+p\left|k\right.))/{\tau }_v\\ {\omega }_t(k+p+1\left|k\right.)={\omega }_t(k+p\left|k\right.)+(u_i^{\omega }(k+p\left|k\right.)-{\omega }_t(k+p\left|k\right.))/{\tau }_{\omega }\end{array}\right.$$

(11)

where $u_i^v(k+p\left|k\right.)$ and $u_i^{\omega }(k+p\left|k\right.)$ are the velocity and angular velocity control inputs of the i-th UAV in the predicted time domain; $v_i(k\left|k\right.)$ is the UAV velocity; ${\omega }_i(k\left|k\right.)$ is the UAV angular velocity; ${(x}_i(k+p+1\left|k\right.),y_i(k+p+1\left|k\right.),z_i(k+p+1\left|k\right.))$ is the three-dimensional position coordinates of this UAV in the predicted time domain; ${\tau }_v$ and ${\tau }_{\omega }$ are the UAV velocity and angular velocity scaling factors respectively; $f_1^i$ is the UAV monitoring target coverage, $f_2^i$ is the control input cost, and $f_3^i$ is the formation planning cost, consisting of two parts: regular planning and reconfiguration planning. Set the formation planning cost in the interval $[0,j)$ for predicted trajectory flight, in the interval $[j,J)$, reconfiguration planning is required based on the unexpected situation multi-UAV formation, in the interval $[J,k)$, the UAV completes the formation planning and continues to fly in accordance with the established formation, as shown in Equation (12).

$$\begin{array}{ll}{f}_3^i& ={\displaystyle \sum _{i=0}^j(w_1{f}_L^i+w_2{f}_H^i+w_3{f}_T^i)}+\hfill \\ & \begin{array}{cc}& \end{array}{\displaystyle \sum _{i=j}^{J-1}({‖x_i(k+j\left|k\right.)-x_g‖}_{A_i}^2+{‖u_i(k+j\left|k\right.)‖}_{B_i}^2)}+\hfill \\ & \begin{array}{cc}& \end{array}{\displaystyle \sum _{i=J}^{J+k}(w_1{f}_L^i+w_2{f}_H^i+w_3{f}_T^i)}\hfill \end{array}$$

(12)

where $x_i(k+j\left|k\right.)$ denotes the UAV $J-1$ step state; $x_g$ denotes the terminal target state; $u_i(k+j\left|k\right.)$ denotes the UAV $J-1$ step control input; $A$ and $B$ are symmetric positive definite weight matrices; $w={(w_1,w_2,w_3)}^T$ is the weight vector; $f_T^i$ denotes the environmental threat cost, calculated by Equation (13); $f_L^i$ denotes the energy consumption cost, calculated by Equation (14); and $f_H^i$ denotes the UAV altitude cost, which is calculated by Equation (15).

$$f_T^i(x_i,y_i,z_i)=\left\{\begin{array}{ll}\infty \hfill & \mathrm{No} \mathrm{fly} \mathrm{zones}\hfill \\ 1\hfill & \mathrm{Safety} \mathrm{zones}\hfill \end{array}\right.$$

(13)

where ${(x}_i,y_i,z_i)$ denotes the coordinates of the current UAV track point.

$$f_L^i=\sqrt{{(x_i-x_l)}^2+{(y_i-y_l)}^2+{(z_i-z_l)}^2}$$

(14)

where ${(x}_l,y_l,z_l)$ denotes the coordinates of the current moving target.

$$f_H^i=\left\{\begin{array}{lll}{z}_1\hfill & z_i<\Delta H_d\hfill & \\ z_i-\Delta H_d\hfill & \Delta H_d\le z_i\le \Delta H_{\max }\hfill \\ z_2\hfill & z_i>\Delta H_{\max }\hfill & \end{array}\right.$$

(15)

where $z_i$ denotes the current track point altitude; $∆H_{max}$ denotes the maximum flight altitude; and $z_1$ and $z_2$ denote the altitude penalty values.

Assuming a fully connected communication topology between UAVs, where each real UAV can obtain information sent by others in real time and without delay within a sampling period, the inter-aircraft communication distance constraint needs to be considered, and the specific fusion algorithm constraint is shown in Equation (16).

$$\left\{\begin{array}{l}{u}_i^{v_{\min }}\le u_i^v(k+P\left|k\right.)\le u_i^{v_{\max }}\\ \left|u_i^{\omega }(k+P\left|k\right.)\right|\le u_i^{{\omega }_{\max }}\\ z_i(k+P+1\left|k\right.)-z_{all}>\Delta H_d\\ \left|\begin{array}{l}{x}_i(k+P+1\left|k\right.),y_i(k+P+1\left|k\right.),z_i(k+P+1\left|k\right.)\\ x_j(k+P+1\left|k\right.),y_j(k+P+1\left|k\right.),z_j(k+P+1\left|k\right.)\end{array}\right|\ge 2R_a\\ \left|\begin{array}{l}{x}_i(k+P+1\left|k\right.),y_i(k+P+1\left|k\right.),z_i(k+P+1\left|k\right.)\\ x_j(k+P+1\left|k\right.),y_j(k+P+1\left|k\right.),z_j(k+P+1\left|k\right.)\end{array}\right|<R_T\\ x_{ix}=x_i(k+P+1\left|k\right.)-P_{tx}\\ y_{iy}=y_i(k+P+1\left|k\right.)-P_{ty}\\ z_{iz}=z_i(k+P+1\left|k\right.)-P_{tz}\\ \sqrt{{(x_{ix})}^2+{(y_{iy})}^2+{(z_{iz})}^2}\ge R_w\\ \sqrt{{(x_i(k+P+1\left|k\right.)-P_{ox})}^2+{(y_i(k+P+1\left|k\right.)-P_{oy})}^2}\ge L_{OD}\\ i\in \{1,\dots ,N_v\}\\ j\in \{1,\dots ,N_v\}\\ x_i(k\left|k\right.)=x_i(k),y_i(k\left|k\right.)=y_i(k),z_i(k\left|k\right.)=z_i(k)\\ v_i(k\left|k\right.)=v_i(k),{\theta }_i(k\left|k\right.)={\theta }_i(k),{\phi }_i(k\left|k\right.)={\phi }_i(k)\end{array}\right.$$

(16)

where ${(x}_j(k+p+1\left|k\right.),y_j(k+p+1\left|k\right.),z_j(k+p+1\left|k\right.))$ is the three-dimensional position coordinates of formation j-th UAVs in the predicted time domain; ${(x}_i(k+p+1\left|k\right.),y_i(k+p+1\left|k\right.),z_i(k+p+1\left|k\right.))$ is the three-dimensional position coordinates of formation i-th UAVs in the predicted time domain; $R_T$ is the maximum communication radius of the formation UAVs; $u_i^{v_{max}}$ and $u_i^{v_{min}}$ are the maximum and minimum velocity constraints of the UAVs and $u_i^{w_{max}}$ is the maximum angular velocity constraint of the UAVs. At moment $k$, the optimization problem above is solved and the first term $u_i(k\left|k\right.)$ of the control sequence is applied to the UAV system, and the process above is repeated at moment $k+1$.

Algorithm 1: Fusion Algorithm Based on MPC and Standoff.

1. Initialize map environment information

2. Initialize fusion algorithm information

3. Initialize multi-UAV movement information

4. For step = 1, 2, …, N:

5.  Obtain the initial state of UAVs in environments $(x_r,y_r,z_r)$,$v$ and ${\varphi }_z$

6.    For k = 1, …, J:

7.      if multi-UAV formations encounter no surprises:

8.       Comprehensive consideration of UAV trajectory planning constraints: $u_{}^{v_{\max }}$,$u_{}^{v_{\min }}$,$u_{}^{w_{\max }}$

9.       Input prediction of velocity and angular velocity control in the time domain $u_{}^v(k+p\left|k\right.)$, $u_{}^{\omega }(k+p\left|k\right.)$

10.        “red” UAV in the environment executing the previous control input of the drone $u_1(k+j\left|k\right.)$ and correcting speed variables $({\dot{x}}_1(k+p+1\left|k\right.),{\dot{y}}_1(k+p+1\left|k\right.),{\dot{z}}_1(k+p+1\left|k\right.))$ based on the Standoff algorithm, and obtains the next state $u_1(k+j+1\left|k\right.+j)$

11.        “yellow” UAV in the environment executing the previous control input of the drone $u_2(k+j\left|k\right.)$ and correcting speed variables $({\dot{x}}_2(k+p+1\left|k\right.),{\dot{y}}_2(k+p+1\left|k\right.),{\dot{z}}_2(k+p+1\left|k\right.))$ based on the Standoff algorithm, and obtains the next state $u_2(k+j+1\left|k\right.+j)$

12.        “green” UAV in the environment executing the previous control input of the drone $u_3(k+j\left|k\right.)$ and correcting speed variables $({\dot{x}}_3(k+p+1\left|k\right.),{\dot{y}}_3(k+p+1\left|k\right.),{\dot{z}}_3(k+p+1\left|k\right.))$ based on the Standoff algorithm, and obtains the next state $u_3(k+j+1\left|k\right.+j)$

13.        Store the above track planning information in the model predictive control module

14.      if multi-UAV formations encounters an unexpected obstacle:

15.        UAV reconfiguration planning based on Computational (12)

16.        Update drone location information $(x_i,y_i,z_i)$ based on minimum generation value

17.      end if

18.      else: break

19.      end if

20.    end for

21.   step = step + 1

22. end for

3.3. Application Steps of Multi-UAV Cooperative Tracking of the Moving Target Based on the Fusion Algorithm

The following steps are taken to plan the coordinated tracking of the moving target by multiple UAVs.

Step 1: Consider the UAV’s own constraints, collision avoidance constraints and other conditions, and determine the number of participating tracking UAVs and UAV formation according to the type of the moving target and tracking needs.

Step 2: The Standoff algorithm and the model predictive control algorithm are fused to complement each other and form a fusion algorithm with more optimized performance. The specific fusion algorithm is as follows: given the basic information of prediction time domain, sampling period, UAV control input $u_i(k-1\left|k\right.)$ and UAV state quantity ${[x}_i(k\left|k\right.),y_i(k\left|k\right.),z_i(k\left|k\right.)]$ at the current $k$ moments, build the planning model that UAVs follow when tracking the moving target, carry out UAV finite time domain prediction trajectory based on collision avoidance constraint, at the same time use the Standoff algorithm to calculate UAV formation phase distribution value, and then build the multi-UAV formation model to reach cooperative tracking of the moving target.

Step 3: In the process of multi-UAV formation movement, determine in real time whether the UAV formation encounters an unexpected situation. If yes, go to step 4; if no, continue to track the moving target.

Step 4: When the UAV formation encounters an unexpected situation during the tracking process, UAVs need to use the fusion algorithm to carry out real-time trajectory planning, and the ‘feedback-correction’ mechanism to correct the trajectory until they resume the formation after the unexpected situation is resolved to continue tracking the moving target.

4. Simulation Verification

With parameters of UAVs and the moving target initialized according to the known information, simulation results have verified that UAVs are able to make trajectory planning through coordinated formation to track the moving target, under the premise that each UAV’s own constraints as well as constraints related to collision avoidance and obstacle avoidance are all considered. Under this verification, an ideal distance and angle between the UAV formation and the moving target is maintained, which makes the UAVs’ monitoring possible and effective. Simulation verification on reconfiguration of multi-UAV formation and trajectory replanning is also carried out, in which different contingencies are handled at the minimized formation planning cost so that UAV trajectory planning can be less dependent on priori information. Initialization information is shown in

Table 1. Initialization of system parameters.

Serial Number	Parameters Name	Parameter Value
1	UAV1 starting position	(200 m, 5 m, 115 m)
2	UAV2 starting position	(160 m, 5 m, 75 m)
3	UAV3 starting position	(240 m, 5 m, 75 m)
4	Target starting position	(200 m, 5 m, 95 m)
5	UAV initial speed	25 m/s
6	UAV speed range	[20 m/s, 40 m/s]
7	Maximum yaw angle of UAV	π/4 rad
8	Maximum pitch angle of UAV	π/4 rad
9	Minimum turning radius for UAV	10 m
10	Number of UAVs $N$	3
11	Maximum speed constraint for UAVs $u_i^{v_{max}}$	40 m/s
12	Minimum speed constraint for UAVs $u_i^{v_{min}}$	10 m/s
13	Maximum angular velocity constraint for UAVs $u_i^{w_{max}}$	0.25 rad/s

.

In this paper, the simulation environment is based on MATLAB R2020b software. The map modelling is based on the undulating terrain of the mountainous landscape, the terrain obstacle composition is mainly derived from the original terrain and the threat of mountain peaks, and the mathematical model of the terrain is artificially formulated. To further approximate the real flight scenario, a safety buffer zone is set up around the UAV and the obstacles are divided into static obstacle modelling and emergent obstacles, with the static obstacle model being approximated by a cylinder and the emergent obstacle model by a sphere. In addition, to further enhance the accuracy of the simulation, the rasterised map environment, i.e., taking into account terrain obstacles, no-fly zones, threat zones, etc., rasterises the map, with each grid called a cell, converts the 3D mathematical model of the map into vector structure data and then into a raster structure, giving each raster cell unique attributes to represent entities. In this paper, the rasterized map unit length is determined to be 5 m with an accuracy of 0.1 m, and its 3D height information is formulated by human.

In this paper, the moving target is set as a low altitude slow speed target, the UAV collision avoidance safety distance is defined as 15 m, the maximum communication radius between UAVs is 90 m, and the UAV detection coverage range is 40 m. For specific sudden obstacle model information, see

Table 2. Sudden obstacle information.

Serial Number	Coordinate Position	Radius Size of Obstacle
1	(100 m, 270 m, 250 m)	50 m
2	(200 m, 300 m, 250 m)	50 m

. The simulation system randomly selects the established sudden obstacle model for testing the fusion algorithm applied to UAV trajectory planning and its formation reconfiguration capability. When the simulation system selects the sudden obstacle 1, UAVs in formation follow the way as planned by conventional trajectory in their flight, taking into account constraints such as collision avoidance and obstacle avoidance, and maximizing multi-UAV sensors’ monitoring coverage. For simulation details, see Figure 6. When the simulation system selects the sudden obstacle 2, it needs to use the fusion algorithm to quickly develop a reconfiguration plan for UAV cooperative formation. Simulation results are shown in Figure 7. To verify the effectiveness of the fusion algorithm, this paper uses the model predictive control algorithm to carry out comparative simulations of same-state trajectory planning, as shown in Figure 8 and Figure 9.

The black trajectory in Figure 6, Figure 7, Figure 8 and Figure 9 is the trajectory of the moving target, and the red, yellow, and green trajectories respectively represent the trajectory planning results of UAV1, UAV2, and UAV3 tracking the moving target. According to the figures, it can be seen that the three UAVs can satisfy several conditions such as their own flight constraints, constraints related to collision avoidance and obstacle avoidance, and carry out real-time stable formation tracking of the moving target. As illustrated by Figure 6 and Figure 7, the simulation of the fusion algorithm makes it possible for UAVs to stably track the target that moves along the established trajectory. Four static obstacles, together with some sudden obstacles, are avoided, which justifies advantages and effectiveness of the fusion algorithm. Unlike the Standoff algorithm that proves to be poor in real-time obstacle avoidance, the fusion algorithm works well in this regard: the three UAVs are distributed around the moving target to maximize the detection coverage of UAV sensors. Thus, the formation reconfiguration task is effectively completed and the unexpected obstacle is successfully bypassed. Figure 8 and Figure 9 only use a single model predictive control algorithm to carry out track planning. Although the vehicles can continue tracking the moving target, their formation is unstable, and the detection coverage for the moving target is insufficient, as shown in Figure 10 and Figure 11.

A comparison of the simulated data in Figure 10 and Figure 11 verifies that the fusion algorithm is effective in avoiding unexpected obstacles when applied to the trajectory planning process UAVs take through cooperative formation when tracking the moving target. Compared with the model predictive control algorithm alone, the fusion algorithm shows its advantage in formation control with the help of the Standoff algorithm, allowing multiple UAVs to move in a circular motion around the target, maximizing UAV sensors’ monitoring range and enabling cooperative formation to track the target. As can be seen in Figure 10, the fusion-based algorithm results in a smaller distance between the UAV and the moving target in real time, and a tighter formation which can be maintained after emergency obstacle avoidance. In Figure 11, the fusion-based UAV spacing remains more stable and less volatile regarding the distance each UAV keeps from the other.

In order to further verify the effectiveness of the fusion algorithm applied to UAVs’ tracking of a moving target, and to verify the real-time obstacle avoidance capability of the fusion algorithm, the number of static obstacles is increased to six in this paper, and the specific system simulation results are shown in Figure 12 and Figure 13. At the same time, the same state comparison simulation experiments are carried out using the model predictive control algorithm, as shown in Figure 14 and Figure 15.

As can be seen from the figure above, by increasing the number of static obstacles to six in the scenario, the three UAVs can still satisfy multiple conditions such as their own flight constraints and obstacle avoidance constraints, and be distributed around the moving target in a class circle to maximize the UAV sensor’s detection coverage, and effectively complete the task of formation reconstruction and real-time stable formation tracking of the moving target on the basis of collaborative formation trajectory planning in complex environments. A comparison between Figure 13 and Figure 15 shows that the single model predictive control algorithm for track planning, although also capable of continuously tracking moving targets, has an unstable formation and thus insufficient detection coverage for moving targets. Specific tracking accuracy parameters are shown in Figure 16 and Figure 17.

In order to test the effectiveness of the fusion optimization algorithm applied to UAV cooperative formation tracking moving target trajectory planning for different trajectory targets, this paper changes the established motion trajectory of the moving target, increases the degrees of freedom of the moving target, expands the 2-dimensional motion of the moving target to 3-dimensional motion, and at the same time adjusts the complex 3-dimensional environment model and changes the dynamic obstacle position, the specific simulation results are shown in Figure 18 and Figure 19. Using the model predictive control algorithm to carry out the same state comparison simulation experiments, as shown in Figure 20 and Figure 21.

According to the figure above, in the context of changing the complex map environment and changing the trajectory of the moving target, the UAVs can still satisfy multiple conditions such as their own flight constraints, collision avoidance and obstacle avoidance constraints, etc., and distribute around the moving target in a class circle to maximize the detection coverage of the UAV sensors, and effectively complete the task of formation reconstruction based on the realization of trajectory planning of multi-UAVs in cooperative formation in a complex environment, and carry out real-time stable formation tracking of the moving target. This demonstrates the effectiveness of the fusion algorithm for tracking moving targets in a complex and variable environment. Specific tracking accuracy parameters are shown in Figure 22 and Figure 23.

As can be seen in Figure 22, the fusion-based algorithm has a smaller distance between the UAV and the moving target in real time and maintains a tighter formation, which can be maintained even after emergency obstacle avoidance. In Figure 23, the fusion-based UAV spacing remains stable and less volatile when comparing distances between UAVs.

5. Discussion

In order to evaluate the proposed fusion algorithm, this paper makes a judgment about the sensors’ detection coverage during multi-UAV tracking of a moving target in coordinated formation, while maximizing their detection range and minimizing the probability of target loss in UAV formation, and compares it with the use of a single model predictive control algorithm to verify that the fusion algorithm helps to improve UAV target monitoring capabilities.

For the target tracking effect and monitoring capability, this paper compares the fusion algorithm and the single model predictive control algorithm in the same environment, guiding multiple UAVs to cooperate in formation as they track the moving target, counting the frequency of UAV sensors to effectively monitor the moving target. Experimental results are shown in

Table 3. Count of effective detection UAV sensors.

UAV Category	Usage	Effective Number of Detected Steps (Scene 1-Total: 100)	Effective Number of Detected Steps (Scene 2-Total: 100)	Effective Number of Detected Steps (Scene 3-Total: 100)
UAV1	Fusion algorithm	100	100	100
	Model predictive control algorithm	100	100	100
UAV2	Fusion algorithm	89	88	92
	Model predictive control algorithm	86	84	88
UAV3	Fusion algorithm	97	95	95
	Model predictive control algorithm	82	80	81

, according to which, the three UAVs effectively monitored target coverage using the fusion algorithm a total of 286 times in Scene 1, compared with 268 effective monitoring times using the single model predictive control algorithm, resulting in a 6.72% increase in combined monitoring coverage; in Scene 2 the three UAVs effectively monitored target coverage a total of 283 times with the help of the fusion algorithm, compared with 264 effective monitoring times using the single model predictive control algorithm, resulting in a 7.20% increase in combined monitoring coverage; and in Scene 3 three UAVs effectively monitored target coverage a total of 287 times with the fusion algorithm, while the effective number of monitoring using a single model predictive control algorithm was 269, with a 6.69% increase in comprehensive monitoring coverage, which in turn can be derived from the advantages of the fusion algorithm in terms of tracking and monitoring effectiveness. The improvement of monitoring ability comes from the effective integration of the model predictive control algorithm and the Standoff algorithm in Section 3.2.1 and Section 3.2.2. The former uses a ‘feedback-correction’ mechanism to correct UAV trajectories, ensuring real-time tracking of moving target trajectory planning, while enabling reconfiguration and planning of multiple UAVs in formation reaching the least-cost goal. The latter ensures cooperative formation control of multiple UAVs, builds UAV sensor monitoring models, maximizes sensors’ monitoring range and reduces the probability of UAVs losing the moving target. Clearly, the fusion algorithm displays a better tracking effect and monitoring capability in the test.

The fusion algorithm promotes the construction of a multi-UAV track planning model, which obtains a more adaptive tracking strategy and effectively solves the problem of multi-UAV formation reconfiguration and obstacle avoidance in emergency situations. From the experimental results, it can be seen that the algorithm has great advantages in terms of tracking effectiveness and monitoring capability, and can support UAV target tracking in uncertain environments. Although some work has been done in this paper on UAV tracking effectiveness and monitoring capability, there are still some challenges in deploying the algorithm to real UAVs. In practice, external interference, noise and air resistance have a dynamic effect on UAV trajectory planning, making it difficult to keep the UAV maneuvering at all times, and time delays in communication between multiple UAVs may occur. No matter how good the UAV’s trajectory planning is in the simulation environment, it is still far from real application. However, we can keep increasing the realism of the scenarios and models in the simulation environment, and thus get closer to the real environment. For future research, we will consider implementing more detailed UAV control, including controlling the UAV with motor speed, acquiring target information through the UAV’s vision sensors and acquiring range information through LIDAR as status information, thus achieving target tracking in a more realistic 3D scene.

6. Conclusions

In this paper, a fusion and optimization method is proposed for trajectory planning UAVs make through cooperative formation when tracking the moving target, a framework for the multi-UAV tracking system is designed, and research on stable tracking is carried out to maximize UAV sensors’ coverage as they monitor the moving target, which in turn reduces the probability of target loss in the tracking process. Against a complex three-dimensional environment in which priori information is insufficient, the fusion algorithm promotes the reconfiguration and planning of multi-UAV formation at the minimum cost, and thus ensures the existence and maintenance of the multi-UAV formation. The simulation verifies the effectiveness of the fusion algorithm applied to multi-UAV cooperative formation, keeping off deficiency in avoiding real-time obstacles facing the Standoff algorithm.

Some future work includes implementing more detailed UAV control for 3D spatial and target tracking in more complex environments, setting up more realistic scenarios (different flight scenarios with different numbers of tracked targets) for extensive simulation validation, and adding on-board sensors to obtain more data as status information, allowing multiple UAVs to carry out collaborative tracking of a moving target closer to realistic scenarios, so that fusion optimization algorithms can find their market in actual UAV trajectory planning in the future.