Multi-UAVs Tracking Non-Cooperative Target Using Constrained Iterative Linear Quadratic Gaussian

Abstract

This study considers the problem of controlling multi-unmanned aerial vehicles (UAVs) to consistently track a non-cooperative ground target with uncertain motion in a hostile environment with obstacles. An active information acquisition (AIA) problem is formulated to minimize the uncertainty of the target tracking task. The uncertain motion of the target is represented as a Wiener process. First, we optimize the configuration of the UAV swarm considering the collision avoidance, horizontal field of view (HFOV), and communication radius to calculate the reference trajectories of the UAVs. Next, a novel algorithm called Constrained Iterative Linear Quadratic Gaussian (CILQG) is introduced to track the reference trajectory. The target’s state with uncertainty and the UAV state are described as beliefs. The CILQG algorithm utilizes the Unscented Transform to propagate the belief regarding the UAVs’ motions, while also accounting for the impact of navigation errors on the target tracking process. The estimation error of the target position of the proposed method is under 4 m, and the error of tracking the reference trajectories is under 3 m. The estimation error remains unchanged even in the presence of obstacles. Therefore, this approach effectively deals with the uncertainties involved and ensures accurate tracking of the target.

1. Introduction

Unmanned aerial vehicle (UAV) swarms have been extensively applied for surveillance in a hostile complex environment. A crucial surveillance scenario involves persistent tracking of suspicious ground targets [1] using a swarm of UAVs. Typically, the tracking process consists of two steps. (1) UAVs in the swarm collaboratively estimate the state of the target, i.e., position and velocity, based on the measurements from onboard devices, and (2) UAVs adjust their trajectories to ensure the motion of the target can be persistently and accurately estimated. These two steps have been intensively and separately investigated [2,3]. Specifically, the studies on the first step always assume precise knowledge of UAV trajectories [4], and the studies on the second step typically assume prior knowledge of the target’s state [5].

Regarding the first step, when the trajectories of UAVs are precisely known, the study on the first step can be recast as an optimal estimation problem. Various types of Kalman filters, such as the original Kalman Filter for linear systems, Extended Kalman Filter (EKF) [6], Unscented Kalman Filter (UKF) [7], and the particle filter for nonlinear systems, have been widely employed. For uncertain target motion, the interacting multiple model (IMM) filtering algorithm offers a solution by employing multi-Kalman-type filters to synchronously estimate the state of the target, and fusing the solutions from the filters. When the motion of the target is nonlinear, the original IMM filter is modified to be IMM-UKF [8] or IMM-CKF [3]. However, IMM filters rely on accurate model assumptions, and the performance can degrade if the true target motion deviates significantly. In practical tracking tasks, the trajectories of the UAVs are not perfectly accurate due to errors in the positions provided by navigation units and deviations from the planned trajectory. These factors should not be overlooked.

Under the assumption that the motion of the target of interest has already been obtained, the second step focuses on planning an optimal trajectory to accurately track the target. Various approaches consider constraints like obstacle avoidance, dynamics, communication, and energy consumption to plan optimal trajectories [5,9]. Control strategies like Iterative Linear Quadratic Regular (ILQR) [10,11] then guide the UAVs along these planned trajectories. However, the assumption that the states of non-cooperative targets are already known is impractical since their motion is unpredictable.

Belief space planning is a methodology that integrates estimation with control, commonly applied to robot navigation [12] and autonomous driving [13]. In this case, the robots estimate their own motion to form the belief state. The maximum a posteriori (MAP) estimate is used in [12] to minimize the uncertainty of the robots. However, in dynamic target tracking scenarios, the robots are supposed to estimate the target motion and their own motion. The challenge of bilateral uncertainties triggers the need for a refined control strategy. To tackle this issue, we propose a two-stage optimization process for tracking. The first stage focuses on minimizing the target uncertainty under the frame of an active information acquisition (AIA) problem [14]. Subsequently, the second stage entails tracking reference trajectories using Constrained Iterative Linear Quadratic Gaussian (CILQG), which addresses the UAV’s uncertainty reduction.

AIA is specifically used to describe the target tracking process via a swarm of robots, which attempts to jointly estimate the motion of the target and optimize the actions of robots in the swarm, recognizing their interdependence. In other words, the two aforementioned steps are combined. The trajectories of robots are dynamically planned and optimized to achieve accurate target tracking. A relevant approach to solving the AIA problem can be categorized as greedy and non-myopic. Greedy approaches [15] focus on myopic decision-making over non-Gaussian processes. Although myopic approaches offer computational efficiency, they are susceptible to local optima. Non-myopic robot-motion algorithms [14,16] provide optimal solutions under linear-Gaussian assumptions but suffer from high computational demands, especially in complex scenarios. Distributed non-myopic algorithms [17,18] alleviate this burden but necessitate all-to-all communication. Despite progress, a critical gap remains in developing non-myopic AIA algorithms that are both computationally efficient and communication-thrifty. Moreover, practical AIA tasks involve complex environments with potential obstacles, sudden failures, and adversarial attacks. While Resilient Active Information gathering [19,20] addresses some of these challenges, obstacle avoidance often remains overlooked.

The objective functions commonly employed in AIA problems typically revolve around maximizing information gain about the target’s state. This can be achieved by either maximizing the mutual information between the target states and measurements [16] or minimizing the conditional entropy of the target states given the measurements [14]. Based on the Gaussian assumption, the AIA problem simplifies to minimizing the covariance of the target estimation [14]. This approach aligns with the goal of reducing uncertainty in the target’s state over a finite time horizon [21]. Alternatively, maximizing the Fisher Information Matrix (FIM) [22,23,24,25,26] offers another avenue for uncertainty minimization. By optimizing control actions to maximize the FIM, we enhance the system’s sensitivity to changes in the target’s state, leading to improved observability and reduced uncertainty.

The searching and sampling processes to solve the AIA problem using non-myopic optimization are time-consuming. A promising alternative lies in determining optimal reference trajectories that adhere to the system constraints and employing efficient tracking controllers. Iteration Linear Quadratic Regulator (ILQR) [11] emerges as a strong candidate due to its computational advantages over methods such as deep deterministic policy gradient (DDPG) [27]. ILQR leverages iterative linear-quadratic approximations of the cost function and system dynamics, efficiently solving an LQR subproblem at each step. This method is free of extensive offline computation and pre-processing. Constrained Iteration Linear Quadratic Regulator (CILQR) [28,29] extends the ILQR method to handle constraints on control inputs and states. Recognizing the inherent uncertainties in UAV navigation and target state estimation, CILQR can be further enhanced by incorporating belief states, leading to CILQG [13,30].

This work studies the problem of persistently tracking a ground target using UAVs in the frame of AIA and optimizes the motions of UAVs over a horizon to minimize the uncertainty of the target states. The contributions of this work are as follows:

• Bilateral uncertainty from the motion of the target and UAVs poses significant challenges to control, particularly in dynamic target tracking scenarios. To address this challenge, we propose a two-stage optimization process for tracking. The first stage focuses on minimizing the target uncertainty using the Fisher Information Matrix (FIM) under the frame of an AIA problem. The second stage is tracking the reference trajectories using CILQG, which addresses the UAV’s uncertainty reduction.
• The interdependence between target motion estimation and UAV trajectory tracking also forms a crucial aspect of our approach. Due to the intertwined nature of these two processes, the impact of the bilateral uncertainty on the accuracy of the target estimation and the precision of UAVs’ trajectories will be intensified. Furthermore, the uncertainty inherent in target motion necessitates a control method capable of rapidly adapting to its movements. This challenge is amplified by limited computational resources and the critical need for real-time performance. The reasons above necessitate a control method that balances computational speed and accuracy. While rarely applied to target tracking, the Constrained Iterative Linear Quadratic Gaussian (CILQG) method demonstrates superior computational efficiency. Moreover, CILQG exhibits enhanced robustness to noise, effectively handling system uncertainties. This stems from its use of belief states and trajectory profile tracking to minimize uncertainty propagation. Conversely, conventional solvers typically treat estimated target states as deterministic values within the optimization problem, neglecting both target and robot uncertainties. This can lead to divergence and suboptimal control performance.

This work is organized as follows. In Section 2, we describe the target tracking task executed by a swarm of UAVs. In Section 3, we introduce the observation model, the belief dynamics of the UAVs, and the target deduced by the Unscented Transform. In Section 4, the AIA problem is formulated to solve the optimal configuration according to the visibility constraints and collision avoidance. In addition, the trajectory tracking problem is proposed. In Section 5, we use CILQG to track the reference trajectory. Simulations are conducted in Section 6. The conclusions are provided in Section 7.

2. Description of the Investigated Task

This work focuses on the task of tracking a non-cooperative ground target using a swarm of UAVs, comprising I individual UAVs, as illustrated in Figure 1. The tracking scenario unfolds in a complex environment cluttered with obstacles, demanding robust and efficient strategies. The objective of the UAV swarm is to estimate the target’s uncertain state based on the measurements and select the optimal control inputs to consistently track the target. To achieve this, we reframe the target tracking task as an AIA problem, in which the UAVs optimize their actions to maximize the information gathered from the target over a finite time horizon.

Recognizing the inherent uncertainties in both UAV positioning (estimated by onboard navigation units) and target motion, we represent the states of both the target and UAVs using belief states. The control block diagram of the system is depicted in Figure 2. The belief dynamics are based on the measurements, propagating the beliefs with uncertainty. To maximize the observability while adhering to system constraints arising from HFOV of UAVs, communication radius, and obstacle avoidance, we formulate the AIA problem to determine an optimal configuration for the UAV swarm. The reference trajectory for each UAV is calculated based on the optimal configuration and the belief of the target. Finally, the CILQG is introduced to track the reference trajectories of the UAVs, ensuring an accurate tracking.

3. Model for Target Tracking Problem

3.1. Measurement Acquisition Model

The UAVs are supposed to gather information on the target, and every UAV is equipped with a radar which is used for monitoring the ground target inside its sensing range. Specifically, the ith UAV receives a set of noisy measurements from the target, and the measurement model is as follows:

$${\mathit{z}}_{i,k}=h({\mathit{s}}_{i,k}^v,{\mathit{s}}_k^t)+{\mathit{n}}_k$$

(1)

$$h({\mathit{s}}_{i,k}^v,{\mathit{s}}_k^t)=\left[\begin{array}{c}\sqrt{{(x_{i,k}^v-x_k^t)}^2+{(y_{i,k}^v-y_k^t)}^2+{{\mathit{z}}_{i,k}^v}^2}\\ \arctan \frac{y_{i,k}^v-y_k^t}{x_{i,k}^v-x_k^t}\\ \arctan \frac{{\mathit{z}}_{i,k}^v}{\sqrt{{(x_{i,k}^v-x_k^t)}^2+{(y_{i,k}^v-y_k^t)}^2}}\end{array}\right]$$

(2)

where ${\mathit{s}}_{i,k}^v={[x_{i,k}^v,y_{i,k}^v,{\mathit{z}}_{i,k}^v,{\phi }_{i,k}^v,{\beta }_{i,k}^v]}^T$ is the state of the ith UAV at ${\mathit{t}}_k$, including the position $x_{i,k}^v$, $y_{i,k}^v$, ${\mathit{z}}_{i,k}^v$ in the East–North–Up (ENU) coordinate system, the pitch angle ${\phi }_{i,k}^v$, and the yaw angle ${\beta }_{i,k}^v$. ${\mathit{s}}_k^t={[x_k^t,y_k^t,{\theta }_k^t]}^T$ denotes the target’s state at ${\mathit{t}}_k$, including the position $x_k^t$, $y_k^t$ in the ENU coordinate system, and the yaw angle ${\theta }_k^t$. ${\mathit{n}}_k$ denotes the measurement noise, which is modeled as a zero-mean Gaussian white noise with a variance of ${\mathit{\Sigma }}_n$. Therefore, the distribution of ${\mathit{z}}_{i,k+1}$ is $\mathcal{N}(h({\mathit{s}}_{i,k}^v,{\mathit{s}}_k^t),{\mathit{\Sigma }}_n)$.

3.2. Belief Dynamics of UAV

The dynamics of an autonomous UAV are described by a discrete-time nonlinear Wiener process:

$${\mathit{s}}_{i,k+1}^v={\mathit{s}}_{i,k}^v+g({\mathit{s}}_{i,k}^v)({\mathit{u}}_{i,k}^v+{\mathit{m}}_k)\Delta T$$

(3)

$$g({\mathit{s}}_{i,k}^v)=\left[\begin{array}{ccc}\cos \left({\phi }_{i,k}^v\right)\cos \left({\beta }_{i,k}^v\right)& 0& 0\\ \cos \left({\phi }_{i,k}^v\right)\sin \left({\beta }_{i,k}^v\right)& 0& 0\\ \sin \left({\phi }_{i,k}^v\right)& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$$

(4)

where ${\mathit{u}}_{i,k}^v={[\begin{array}{ccc}{V}_{i,k}^v& {\dot{\phi }}_{i,k}^v& {\dot{\beta }}_{i,k}^v\end{array}]}^T$ is the control input of the ith UAV at ${\mathit{t}}_k$. ${\mathit{m}}_k$ is the process noise, which is modeled as a zero-mean Gaussian noise with a variance of ${\mathit{\Sigma }}_m$.

The belief of the ith UAV at ${\mathit{t}}_k$, ${\mathit{b}}_{i,k}^v$, is updated according to Equations (5) and (6):

$${\mathit{b}}_{i,k+1}^v={\displaystyle \int G({\mathit{s}}_{i,k+1}^v|{\mathit{s}}_{i,k}^v,{\mathit{u}}_{i,k}^v)}{\widehat{\mathit{b}}}_{i,k}^{v}d{\mathit{s}}_{i,k}^v,i=1,2,3$$

(5)

$${\widehat{\mathit{b}}}_{i,k+1}^v=\frac{\Phi ({\mathit{z}}_{i,k+1}^v|{\mathit{s}}_{i,k+1}^v){\mathit{b}}_{i,k+1}^v}{{\displaystyle \int \Phi ({\mathit{z}}_{i,k+1}^v|{\mathit{s}}_{i,k+1}^v){\mathit{b}}_{i,k+1}^{v}d{\mathit{s}}_{i,k+1}^v}}$$

(6)

where $G({\mathit{s}}_{i,k+1}^v|{\mathit{s}}_{i,k}^v,{\mathit{u}}_{i,k}^v)$ is the UAV state transition density at ${\mathit{t}}_k$, which can be described by

$$\begin{array}{l}G({\mathit{s}}_{i,k+1}^v|{\mathit{s}}_{i,k}^v,{\mathit{u}}_{i,k}^v)=\frac{1}{\sqrt{2\pi |g({\mathit{s}}_{i,k}^v){\mathit{\Sigma }}_{m}g{({\mathit{s}}_{i,k}^v)}^T|}\Delta T}\\ \exp [-({\mathit{s}}_{i,k+1}^v-{\mathit{s}}_{i,k}^v-g({\mathit{s}}_{i,k}^v){\mathit{u}}_{i,k}^v\Delta T){(2\Delta T^{2}g({\mathit{s}}_{i,k}^v){\mathit{\Sigma }}_{m}g{({\mathit{s}}_{i,k}^v)}^T)}^{-1}\\ {({\mathit{s}}_{i,k+1}^v-{\mathit{s}}_{i,k}^v-g({\mathit{s}}_{i,k}^v){\mathit{u}}_{i,k}^v\Delta T)}^T]\end{array}$$

(7)

Since the dynamics of the UAV shown in Equation (3) are nonlinear, the integral in Equation (5) is calculated via the Unscented Transform. Ref. [31] elaborates on the details of the belief propagation via the Unscented Transform.

In Equation (6), ${\widehat{\mathit{b}}}_{i,k}^v$ is the posterior belief of the UAV’s state when the measurement from the onboard device is available, and $\Phi ({\mathit{z}}_{i,k+1}^v|{\mathit{s}}_{i,k+1}^v)$ is the measurement likelihood function according to Section 3.1, which determines the probability density that the navigation unit of the UAV, such as the Inertial Measurement Unit, will receive the measurement ${\mathit{z}}_{i,k+1}^v$ from the ith UAV with state ${\mathit{s}}_{i,k+1}^v$ at $t_{k+1}$.

3.3. Belief Dynamics of the Target

In this work, the target is a ground vehicle whose motion is assumed to be a discrete-time Wiener process:

$${\mathit{s}}_{k+1}^t={\mathit{s}}_k^t+f({\mathit{s}}_k^t)({\mathit{u}}_k^t+{\mathit{m}}_k)\Delta T$$

(8)

$$f({\mathit{s}}_k^t)=\left[\begin{array}{cc}\cos \left({\theta }_k^t\right)& 0\\ \sin \left({\theta }_k^t\right)& 0\\ 0& 1\end{array}\right]$$

(9)

where ${\mathit{u}}_k^t={[\begin{array}{cc}{V}_k^t& {\dot{\theta }}_k^t\end{array}]}^T$ is the control input of the target, including the velocity $V_k^t$ and the angle velocity ${\dot{\theta }}_k^t$, allowing the target to change its orientation and speed.

The belief of the target at ${\mathit{t}}_k$, ${\mathit{b}}_k^t$, is updated according to Equations (10) and (11):

$${\mathit{b}}_{k+1}^t={\displaystyle \int F({\mathit{s}}_{k+1}^t|{\mathit{s}}_k^t,{\mathit{u}}_k^t)}{\widehat{\mathit{b}}}_k^{t}d{\mathit{s}}_k^t$$

(10)

$${\widehat{\mathit{b}}}_{k+1}^t=\frac{\Psi ({\mathit{z}}_{\upsilon ,k+1}^t|{\mathit{s}}_{\upsilon ,k+1}^v,{\mathit{s}}_{k+1}^t){\mathit{b}}_{k+1}^t}{{\displaystyle \int \Psi ({\mathit{z}}_{\upsilon ,k+1}^t|{\mathit{s}}_{\upsilon ,k+1}^v,{\mathit{s}}_{k+1}^t){\mathit{b}}_{k+1}^{t}d{\mathit{s}}_{k+1}^t}}$$

(11)

where $F({\mathit{s}}_{k+1}^t|{\mathit{s}}_k^t,{\mathit{u}}_k^t)$ is the target state transition density at ${\mathit{t}}_k$, which can be described by

$$\begin{array}{l}F({\mathit{s}}_{k+1}^t|{\mathit{s}}_k^t,{\mathit{u}}_k^t)=\frac{1}{\sqrt{2\pi |f({\mathit{s}}_k^t){\mathit{\Sigma }}_{m}f{({\mathit{s}}_k^t)}^T|}\Delta T}\\ \exp [-({\mathit{s}}_{k+1}^t-{\mathit{s}}_k^t-f({\mathit{s}}_k^t){\mathit{u}}_k^t\Delta T){(2\Delta T^{2}f({\mathit{s}}_k^t){\mathit{\Sigma }}_{m}f{({\mathit{s}}_k^t)}^T)}^{-1}\\ {({\mathit{s}}_{k+1}^t-{\mathit{s}}_k^t-f({\mathit{s}}_k^t){\mathit{u}}_k^t\Delta T)}^T]\end{array}$$

(12)

Since the dynamics of the target shown in (8) are nonlinear, the integral in Equation (10) is also calculated via the Unscented Transform.

In Equation (11), ${\widehat{\mathit{b}}}_k^t$ is the posterior belief on the target’s state when the measurement from the onboard device is available. $\upsilon =\{1,2,\dots ,I\}$ represents the set of UAVs, ${\mathit{s}}_{\upsilon ,k+1}^v=\{{\mathit{s}}_{1,k+1}^v,{\mathit{s}}_{2,k+1}^v,\dots ,{\mathit{s}}_{I,k+1}^v\}$ includes the states of all the UAVs of the swarm, and ${\mathit{z}}_{\upsilon ,k+1}^t=\{{\mathit{z}}_{1,k+1}^t,{\mathit{z}}_{2,k+1}^t,\dots ,{\mathit{z}}_{I,k+1}^t\}$ contains the measurements received by the UAVs from the target. The measurement likelihood function $\Psi ({\mathit{z}}_{\upsilon ,k+1}^t|{\mathit{s}}_{\upsilon ,k+1}^v,{\mathit{s}}_{k+1}^t)$ gives the probability that the UAVs with states ${\mathit{s}}_{\upsilon ,k+1}^v$ will receive a set of measurements ${\mathit{z}}_{\upsilon ,k+1}^t$ from the target with state ${\mathit{s}}_{k+1}^t$ at ${\mathit{t}}_{k+1}$.

3.4. Obstacle Model

The pth obstacle is modeled as a cylinder O_p[($x_p^{obs}$, $y_p^{obs}$), $h_p^{obs}$, $r_p^{obs}$], where ($x_p^{obs}$, $y_p^{obs}$) is the position of the center of the obstacle, $h_p^{obs}$ is the height, and $r_p^{obs}$ is the radius. Assuming that $h_p^{obs}$ is greater than the height of the UAVs, the UAVs cannot fly over the obstacles.

4. Optimization Problem behind the Active Information Acquisition

In this paper, the AIA problem is composed of two cascaded optimization problems. The first optimization problem is to determine an optimal configuration of the swarm relative to the target, and the configuration is transferred to the reference trajectories of the UAVs. The second problem entails tracking these reference trajectories, which are derived from the solution to the first problem.

4.1. Optimization Problem about the Configuration of the Swarm

4.1.1. Pre-Designed Configuration

To ensure continuous target tracking within the UAVs’ HFOV, it is crucial to determine the angle between each UAV and the target. Assuming that the radar is fixed on the UAVs with an orientation identical to the UAV itself, and denoting the radar’s HFOV as α⁺, we need to maintain the target within this angular range. Additionally, effective communication necessitates that the relative distances between any two UAVs must be within the maximum communication radius, denoted as R⁺, which depends on the power consumption of the communication devices. Assuming that the UAVs maintain fixed altitudes, we propose a symmetrical configuration of the UAV swarm relative to the target, as illustrated in Figure 3. The blue sector represents the sensor detection range. We aim to optimize both the angle ${\alpha }_{i,k}$ between the y-axis and the UAV-target line of sight, and the distance $R_{i,k}$ between the UAV and the target. This optimization considers the aforementioned constraints to achieve an optimal configuration for continuous tracking and communication.

4.1.2. Optimization Problem for ${\alpha }_{i,k}$ and $R_{i,k}$

The optimal trajectories are not directly obtained. Instead, only the configuration of the swarm at the current time step can be determined. According to Section 4.1.1, ${\alpha }_{i,k}$ and $R_{i,k}$ are the variables that need to be optimized when determining the optimal configuration. When ${\mathit{b}}_k^t$ are available, the optimal configuration of the swarm at ${\mathit{t}}_k$ can be determined by solving the following receding horizon optimization problem over an N-length planning horizon:

$$\underset{R_{i,k},{\alpha }_{i,k}}{\max }J(R_{1:I,k:k+N},{\alpha }_{1:I,k:k+N},{\mathit{b}}_{k:k+N}^t)\triangleq {\displaystyle \sum _{i=1}^I{\displaystyle \sum _{j=k}^{k+N}E[({\nabla }_{{\mathit{s}}_j^t}\ln p({\mathit{z}}_{\upsilon ,1:j}^t|{\mathit{s}}_j^t)){({\nabla }_{{\mathit{s}}_j^t}\ln p({\mathit{z}}_{\upsilon ,1:j}^t|{\mathit{s}}_j^t))}^T]}}$$

(13)

$$\mathrm{s}.\mathrm{t}. {\mathit{b}}_{j+1}^t={\lambda }^{-1}\Psi ({\mathit{z}}_{\upsilon ,j+1}^t|{\mathit{s}}_{\upsilon ,j+1}^v,{\mathit{s}}_{j+1}^t){\displaystyle \int F({\mathit{s}}_{j+1}^t|{\mathit{s}}_j^t,{\mathit{u}}_j^t)}{\mathit{b}}_j^{t}d{\mathit{s}}_j^t$$

(14)

$$\begin{array}{l}p({\mathit{z}}_{\upsilon ,1:j}^t|{\mathit{s}}_j^t)=p({\mathit{z}}_{\upsilon ,1}^t,{\mathit{z}}_{\upsilon ,2}^t,\dots ,{\mathit{z}}_{\upsilon ,j}^t|{\mathit{s}}_j^t)={\displaystyle \prod _{j=1}^k\Psi ({\mathit{z}}_{\upsilon ,j}^t|{\mathit{s}}_{\upsilon ,j}^v,{\mathit{s}}_j^t)}\\ ={\displaystyle \prod _{j=1}^{k}1/\sqrt{2\pi |{\mathit{\Sigma }}_n|}}\exp [-\frac{1}{2}{({\mathit{z}}_j-h({\overline{\mathit{s}}}_{i,j}^v,{\mathit{b}}_j^t))}^T{\mathit{\Sigma }}_n^{-1}({\mathit{z}}_j-h({\overline{\mathit{s}}}_{i,j}^v,{\mathit{b}}_j^t))]\end{array}$$

(15)

$$||[{\overline{x}}_{i,j}^v,{\overline{y}}_{i,j}^v]-[x_p^{obs},y_p^{obs}]|{|}_2>r,p=1,2,\dots ,P$$

(16)

$${\alpha }_{i,k}\in [-{\alpha }^{+}/2,{\alpha }^{+}/2]$$

(17)

$$R_{i,k}\in (0,R^{+})$$

(18)

where ${\nabla }_{{\mathit{s}}_k^t}$ is the gradient operator, and $p({\mathit{z}}_{1:k}|{\mathit{s}}_k^t)$ is the batch measurement likelihood [32,33,34,35]. $I$ is the number of the UAVs, and $P$ is the number of the obstacles. ${\overline{\mathit{s}}}_{i,j}^v$ is the reference state of the ith UAV at ${\mathit{t}}_k$, which is related to the target belief, ${\alpha }_{i,k}$ and $R_{i,k}$. ${\overline{\mathit{s}}}_{i,j}^v$ includes the positions ${\overline{x}}_{i,j}^v$, ${\overline{y}}_{i,j}^v$, ${\overline{z}}_{i,j}^v$, the yaw ${\overline{\phi }}_{i,j}^v$, and the pitch ${\overline{\beta }}_{i,j}^v$.

The objective function shown in Equation (13) is essentially the FIM for the horizon from ${\mathit{t}}_k$ to ${\mathit{t}}_{k+N}$. As depicted in Figure 4, receding horizon control optimizes the actions to maximize the FIM over a finite prediction horizon, and applies the first action of the optimized sequence at the next time step. Equation (14) is the belief dynamics of the target, and Equation (16) is the obstacle avoidance constraint, which keeps the distance between the UAV and the obstacle center greater than the radius of the obstacle. Equations (17) and (18) restrict the maximum distance and angle of the UAV relative to the target.

4.2. Optimization Problem for Trajectory Tracking

The reference trajectory is not directly obtained; it is determined by the optimal configuration and the target belief ${\mathit{b}}_k^t$. When the optimal $R_{i,k}$ and ${\alpha }_{i,k}$ are found, the configuration can be converted to the position of the UAV relative to the target, ${\mathit{D}}_{i,k}$. When there are only three UAVs, the reference state of the UAV, ${\overline{\mathit{s}}}_{i,k}^v$ can be expressed as follows:

$${\overline{\mathit{s}}}_{i,k}^v={[{\overline{x}}_{i,k}^v,{\overline{y}}_{i,k}^v,{\overline{\mathit{z}}}_{i,k}^v,{\overline{\phi }}_{i,k}^v,{\overline{\beta }}_{i,k}^v]}^T={[{\tilde{x}}_k^t,{\tilde{y}}_k^t,H,0,{\tilde{\theta }}_k^t]}^T+{[{\mathit{D}}_{i,k}^T,0,0,0]}^T$$

(19)

$${\mathit{D}}_{i,k}=\left\{\begin{array}{c}{\mathit{C}}_k\left[\begin{array}{c}-R_{i,k}\sin {\alpha }_{i,k}\\ -R_{i,k}\cos {\alpha }_{i,k}\end{array}\right],i=1\\ {\mathit{C}}_k\left[\begin{array}{c}-R_{i,k}\\ 0\end{array}\right],i=2\\ {\mathit{C}}_k\left[\begin{array}{c}-R_{i,k}\sin {\alpha }_{i,k}\\ R_{i,k}\cos {\alpha }_{i,k}\end{array}\right],i=3\end{array}\right.$$

(20)

$${\mathit{C}}_k=\left[\begin{array}{cc}\cos {\tilde{\theta }}_k^t& -\sin {\tilde{\theta }}_k^t\\ \sin {\tilde{\theta }}_k^t& \cos {\tilde{\theta }}_k^t\end{array}\right]$$

(21)

where ${\mathit{C}}_k$ is the target heading rotation matrix. ${\tilde{x}}_k^t$, ${\tilde{y}}_k^t$, and ${\tilde{\theta }}_k^t$ are beliefs of positions and the yaw angle of the target. To avoid the collision between the UAVs, each UAV has an identical altitude.

The reference state ${\overline{\mathit{s}}}_{i,j}^v$ composes a reference trajectory $\left\{{\overline{\mathit{s}}}_{i,k}^v,{\overline{\mathit{s}}}_{i,k+1}^v,\dots ,{\overline{\mathit{s}}}_{i,k+N}^v\right\}$ during an N-length planning horizon. The reference trajectories are derived in this way. The optimization problem to track the reference trajectories of the UAVs is described by the following:

$$\underset{{\mathit{U}}_{1:I,k}}{\min }J({\mathit{b}}_{1:I,k:k+N}^v,{\mathit{U}}_{1:I,k})\triangleq {\displaystyle \sum _{i=1}^I{\displaystyle \sum _{j=k}^{k+N}[{({\mathit{b}}_{i,j}^v-{\overline{\mathit{s}}}_{i,j}^v)}^T({\mathit{b}}_{i,j}^v-{\overline{\mathit{s}}}_{i,j}^v)}}+{{\mathit{u}}_{i,j}^v}^T{\mathit{W}}_u{\mathit{u}}_{i,j}^v]$$

(22)

$$\mathrm{s}.\mathrm{t}. {\mathit{b}}_{i,j+1}^v={\eta }^{-1}\Phi ({\mathit{z}}_{i,j+1}^v|{\mathit{s}}_{i,j+1}^v){\displaystyle \int G({\mathit{s}}_{i,j+1}^v|{\mathit{s}}_{i,j}^v,{\mathit{u}}_{i,j}^v)}{\mathit{b}}_{i,j}^{v}d{\mathit{s}}_j^v$$

(23)

$$||[{\tilde{x}}_{i,j}^v,{\tilde{y}}_{i,j}^v]-[x_p^{obs},y_p^{obs}]|{|}_2>r,p=1,2,\dots ,P$$

(24)

$${\mathit{u}}_{i,k}^v\in [{\mathit{u}}_{\min },{\mathit{u}}_{\max }]$$

(25)

where ${\mathit{U}}_{i,k}=\left\{{\mathit{u}}_{i,k}^v,{\mathit{u}}_{i,k+1}^v,\dots ,{\mathit{u}}_{i,k+N-1}^v\right\}$ is the control input sequence of the ith UAV from ${\mathit{t}}_k$ to ${\mathit{t}}_{k+N-1}$, ${\mathit{W}}_{\mathit{u}}$ is the weight matrix of the control input, and ${\tilde{x}}_k^t$, ${\tilde{y}}_k^t$ are beliefs of the position of the UAV. ${\mathit{b}}_{i,k}^v$ is the belief state vector of the UAV at ${\mathit{t}}_k$. Additionally,

$${\mathit{b}}_{i,k}^v={[x_k^v,y_k^v,{\mathit{z}}_k^v,{\phi }_k^v,{\beta }_k^v,{\mathit{\Sigma }}_k^{xx},{\mathit{\Sigma }}_k^{xy},\dots ,{\mathit{\Sigma }}_k^{\beta \beta }]}^T$$

(26)

where $x_k^v$, $y_k^v$, ${\mathit{z}}_k^v$, ${\phi }_k^v$, and ${\beta }_k^v$ are the mean of the UAV state, and ${\mathit{\Sigma }}_k^{11}$ is the element of the variance matrix. ${\overline{\mathit{s}}}_{i,k}^v$ includes the reference mean and reference variance. Therefore, the profile of the trajectory instead of a specific trajectory is tracked.

While reference trajectories for the UAVs are generated based on the estimated target belief and optimized configuration, the target belief itself is updated using measurements obtained by the UAVs along their trajectories. This highlights the intertwined nature of these two processes. The accuracy of tracking is affected by the uncertainty in the motion of the target and UAVs, which is represented by the process noise. Additionally, the measurement process is influenced by the measurement noise. Therefore, an effective tracking controller is needed.

5. CILQG for Active Information Acquisition Problem

CILQG provides an elegant solution by incorporating belief states directly into the control framework. This makes it particularly well suited for scenarios like ours, where both target motion and UAV states are subject to uncertainties. By constructing a state vector comprising the mean and variance of the UAV belief states, CILQG effectively tracks the entire trajectory profile, encompassing both the mean (blue line in Figure 5) and variance (blue circle in Figure 5). CILQG mainly contains the backward and the forward pass. In the backward pass, the objective function of tracking the reference trajectory is approximated as a quadratic form to calculate the optimal control input. In the forward pass, the belief dynamics of the UAVs are used to update the trajectory and the cost of the next iteration. The constraints in Equations (24) and (25) are converted to the cost via barrier function.

5.1. Backward Pass

The Bellman equation typically defines the cost-to-go from the current time to the terminal time. However, in the case of the target tracking problem, a specific terminal time does not exist since the tracking process is continuous and lacks a defined endpoint; therefore, we recursively optimize the control input sequence in an N-length planning horizon instead of a fixed terminal time. Therefore, Equation (22) is defined as the cost incurred from the current ${\mathit{t}}_k$ until the end of the planning horizon, ${\mathit{t}}_{k+N}$. This approach allows us to continuously adapt the UAV trajectories based on the latest target information.

The optimal cost-to-go V at ${\mathit{t}}_k$ is as follows:

$$V({\mathit{b}}_{i,k}^v,k)=\underset{{\mathit{U}}_{i,k}}{\min }{J}_{i,k}({\mathit{b}}_{i,k}^v,{\mathit{U}}_{i,k})$$

(27)

The optimal cost-to-go is separated into two parts, i.e., the cost at ${\mathit{t}}_{k+N}$ in Equation (28) and the cost-to-go at ${\mathit{t}}_k$ in Equation (29):

$$V({\mathit{b}}_{i,k}^v,k+N)=\underset{{\mathit{U}}_{i,k}}{\min }{({\mathit{b}}_{i,k+N}^v-{\overline{\mathit{s}}}_{i,k+N}^v)}^T{\mathit{W}}_N({\mathit{b}}_{i,k+N}^v-{\overline{\mathit{s}}}_{i,k+N}^v)$$

(28)

$$V({\mathit{b}}_{i,k}^v,k)=\underset{{\mathit{U}}_{i,k}}{\min }[{({\mathit{b}}_{i,k}^v-{\overline{\mathit{s}}}_{i,k}^v)}^T{\mathit{W}}_k({\mathit{b}}_{i,k}^v-{\overline{\mathit{s}}}_{i,k}^v)+{{\mathit{u}}_{i,k}^v}^T{R}_k{\mathit{u}}_{i,k}^v+V_{k+1}({\mathit{b}}_{i,k+1}^v,k+1)]$$

(29)

The variation of $V({\mathit{b}}_{i,k}^v,k)$ around the belief ${\mathit{b}}_{i,k}^v$ and control input ${\mathit{u}}_{i,k}^v$ can be expressed as a quadratic function of small perturbations, denoted as $Q(\delta \mathit{b},\delta \mathit{u})$. It is expanded to second order:

$$\begin{array}{cc}\hfill Q(\delta \mathit{b},\delta \mathit{u})=& {({\mathit{b}}_{i,k}^v+\delta \mathit{b}-{\overline{\mathit{s}}}_{i,k}^v)}^T{\mathit{W}}_k({\mathit{b}}_{i,k}^v+\delta \mathit{b}-{\overline{\mathit{s}}}_{i,k}^v)-{({\mathit{b}}_{i,k}^v-{\overline{\mathit{s}}}_{i,k}^v)}^T{\mathit{W}}_k({\mathit{b}}_{i,k}^v-{\overline{\mathit{s}}}_{i,k}^v)\hfill \\ & +{({\mathit{u}}_{i,k}^v+\delta u)}^T{\mathit{R}}_k({\mathit{u}}_{i,k}^v+\delta u)-{\mathit{u}}_{i,k}^v{\mathit{R}}_k{\mathit{u}}_{i,k}^v\hfill \\ & +V_{k+1}^v({\mathit{b}}_{i,k+1}^v+\delta \mathit{b},k+1)-V_{k+1}({\mathit{b}}_{i,k+1}^v,k+1)\hfill \\ & \approx \frac{1}{2}\left[\begin{array}{c}1\\ \delta {\mathit{b}}_k\\ \delta {\mathit{u}}_k\end{array}\right]\left[\begin{array}{lll}0& {\mathit{Q}}_x^T& {\mathit{Q}}_u^T\\ {\mathit{Q}}_x& {\mathit{Q}}_{xx}& {\mathit{Q}}_{?x}\\ {\mathit{Q}}_u& {\mathit{Q}}_{ux}& {\mathit{Q}}_{uu}\end{array}\right]\left[\begin{array}{c}1\\ \delta {\mathit{b}}_k\\ \delta {\mathit{u}}_k\end{array}\right]\hfill \end{array}$$

(30)

in which

$${\mathit{Q}}_x={\mathit{c}}_x+{\mathit{f}}_x^T{\mathit{V}}_x$$

(31)

$${\mathit{Q}}_u={\mathit{c}}_u+{\mathit{f}}_u^T{\mathit{V}}_x$$

(32)

$${\mathit{Q}}_{xx}={\mathit{c}}_{xx}+{\mathit{f}}_x^T{\mathit{V}}_{xx}{\mathit{f}}_x+{\mathit{V}}_x{\mathit{f}}_{xx}$$

(33)

$${\mathit{Q}}_{uu}={\mathit{c}}_{uu}+{\mathit{f}}_u^T{\mathit{V}}_{xx}{\mathit{f}}_u+{\mathit{V}}_x{\mathit{f}}_{uu}$$

(34)

$${\mathit{Q}}_{ux}={\mathit{c}}_{ux}+{\mathit{f}}_u^T{\mathit{V}}_{xx}{\mathit{f}}_x+{\mathit{V}}_x{\mathit{f}}_{ux}$$

(35)

where $c_x$, $c_u$, $c_{xx}$, $c_{uu}$, and $c_{ux}$ are the gradients and Hessians of the cost function described in Equation (22), $f_x$, $f_u$, $f_{xx}$, $f_{uu}$, and $f_{ux}$ are the gradients and Hessians of the nonlinear dynamics shown in Equation (3), and $V_x$, $V_{xx}$ are the gradients and Hessians of the optimal cost-to-go depicted in Equation (27). The local optimum of $\delta \mathit{u}$ can be found by setting the derivative of $Q(\delta \mathit{b},\delta \mathit{u})$ with respect to $\delta \mathit{u}$ to zero. Additionally,

$$\delta \mathit{u}=\arg \min Q(\delta \mathit{b},\delta \mathit{u})=-{\mathit{Q}}_{uu}^{-1}({\mathit{Q}}_u+{\mathit{Q}}_{ux}\delta \mathit{b})={\mathit{K}}_C+{\mathit{K}}_A\delta \mathit{b}$$

(36)

where ${\mathit{K}}_A=-{\mathit{Q}}_{uu}^{-1}{\mathit{Q}}_{ux}$ and ${\mathit{K}}_C=-{\mathit{Q}}_{uu}^{-1}{\mathit{Q}}_u$. By substituting Equation (36) back into Equation (30), we get the following:

$$\Delta V({\mathit{b}}_{i,k}^v,k)=-\frac{1}{2}{{\mathit{K}}_{\mathit{C}}}^T{\mathit{Q}}_{uu}{\mathit{K}}_{\mathit{C}}$$

(37)

$${\mathit{V}}_x({\mathit{b}}_{i,k}^v,k)={\mathit{Q}}_x-{{\mathit{K}}_A}^T{\mathit{Q}}_{uu}{\mathit{K}}_{\mathit{C}}$$

(38)

$${\mathit{V}}_{xx}({\mathit{b}}_{i,k}^v,k)={\mathit{Q}}_{xx}-{{\mathit{K}}_A}^T{\mathit{Q}}_{uu}{\mathit{K}}_{\mathit{C}}$$

(39)

By checking $\Delta V$, we can assess whether the current local optimal controller is the global optimal controller. Equations (38) and (39) form the quadratic approximation of $V({\mathit{b}}_{i,k}^v,k)$. To approach the local minimum of $V({\mathit{b}}_{i,k}^v,k)$, the quadratic approximation of $V({\mathit{b}}_{i,k}^v,k)$ and the control feedback coefficients $\{K_A,K_C\}$ are recursively computed backward from ${\mathit{t}}_{k+N}$.

5.2. Forward Pass

The optimal control input ${\mathit{u}}_{i,k}$ is the sum of the current control input ${\overline{\mathit{u}}}_{i,k}$ and the local optimum of $\delta \mathit{u}$ described in Equation (36). ${\mathit{u}}_{i,k}$ is input to the belief dynamics of UAV to form a new trajectory. The nonlinear nature of our system introduces a challenge: when a newly computed trajectory deviates significantly from the model’s region of validity, the expected cost reduction may not materialize. To mitigate this issue, we introduce a backtracking line-search parameter, $0<\lambda \le 1$, to search for a valid trajectory. This parameter scales the control input update, effectively controlling the step size taken in the direction determined by the optimization process. Different values of $\lambda$ are tested until the cost decreases; therefore, an appropriate step size is found to improve the control policy in the given search direction.

$$\delta {\mathit{b}}_{i,k}^v={\mathit{b}}_{i,k}^v-{\overline{\mathit{s}}}_{i,k}^v$$

(40)

$${\mathit{u}}_{i,k}={\overline{\mathit{u}}}_{i,k}+\delta {\mathit{u}}_{i,k}={\overline{\mathit{u}}}_{i,k}+\lambda {\mathit{K}}_C+{\mathit{K}}_A\delta {\mathit{b}}_{i,k}^v$$

(41)

5.3. Constraints Handling by CILQG

At the backward pass, CILQG transforms the constraints into quadratic cost terms using barrier function. The input constraints of the ith UAV at ${\mathit{t}}_k$ are $P_k({\mathit{u}}_{i,k}^v)$ and $L_k({\mathit{u}}_{i,k}^v)$.

$$P_k({\mathit{u}}_{i,k}^v)={\mathit{u}}_{i,k}^v-{\mathit{u}}_{\max }\le 0, L_k({\mathit{u}}_{i,k}^v)={\mathit{u}}_{\min }-{\mathit{u}}_{i,k}^v\le 0$$

(42)

The collision avoidance constraint in Equation (24) is nonlinear, and we first linearize it as follows:

$${F^{\prime }}_k({\mathit{b}}_{i,k}^v+\delta {\mathit{b}}_{i,k}^v)\approx {(\delta {\mathit{b}}_{i,k}^v)}^T\nabla F_k({\mathit{b}}_{i,k}^v)+F_k({\mathit{b}}_{i,k}^v)$$

(43)

where $\nabla F_k({\mathit{b}}_{i,k}^v)$ is the first order derivative of $F_k$ on ${\mathit{b}}_{i,k}^v$. Taking ${F^{\prime }}_k$ for example, the barrier function of ${F^{\prime }}_k$ is as follows:

$$B_k({\mathit{b}}_{i,k}^v)=q_1\exp (q_2{F^{\prime }}_k({\mathit{b}}_{i,k}^v))$$

(44)

in which $q_1$ and $q_2$ are coefficients to tune the shape of the barrier function. The exponential barrier function in Equation (44) is second-order differentiable, which can be transferred to quadratic form as follows:

$$B_k({\mathit{b}}_k+\delta {\mathit{b}}_k)\approx \delta {\mathit{b}}_k^T{B}_{ss}\delta {\mathit{b}}_k+\delta {\mathit{b}}_k^T{B}_s+B_k({\mathit{b}}_k)$$

(45)

where $B_s$, $B_{ss}$ are the gradient and Hessian matrix of $B_k(s_k)$.

$$B_s=q_1{q}_2\exp (q_2{F^{\prime }}_k({\mathit{b}}_k))\nabla {F^{\prime }}_k({\mathit{b}}_k)$$

(46)

$$B_{ss}=q_1{q}_2\exp (q_2{F^{\prime }}_k({\mathit{b}}_k))\nabla {F^{\prime }}_k({\mathit{b}}_k){(\nabla {F^{\prime }}_k({\mathit{b}}_k))}^T$$

(47)

The quadratic cost of collision avoidance in Equation (45) and the cost of input are added to the total cost. At the same time, the gradient and Hessian matrix is used in the backward pass. The pseudo-code of the method is shown in Algorithm 1.

Algorithm 1 CILQG for trajectory tracking

Input: ${\mathit{b}}_{i,0}^v$, ${\mathit{b}}_0^t$, ${\mathit{U}}_{\nu ,0}$, $\lambda$

Output: ${\mathit{b}}_{\nu ,k+1}^v$

1: ${\mathit{b}}_{i,k}^v\leftarrow {\mathit{b}}_{i,0}^v$, ${\mathit{b}}_k^t\leftarrow {\mathit{b}}_0^t$

2: for k = 0 to Termination

3:         ${\mathit{z}}_{\upsilon ,k+1}^t\leftarrow h({\mathit{s}}_{\upsilon ,k+1}^v,{\mathit{s}}_k^t)$

4:         ${\mathit{b}}_{k+1}^t\leftarrow Beliefdynamic({\mathit{z}}_{\upsilon ,k+1}^t,{\mathit{b}}_k^t)$ via Equations (5) and (6)

5:         for i = 1 to 3 (i is the number of the UAVs)

6:                   ${\mathit{s}}_{k+1}^t={\mathit{s}}_k^t+f({\mathit{s}}_k^t)({\mathit{u}}_k^t+m_k)\Delta T$

7:                   Calculate the reference trajectory according to Equation (19) to Equation (21)

8:                   $\{{\mathit{b}}_{i,k}^v,{\mathit{U}}_k\}\leftarrow forwardpass(0)$ according to Equations (40) and (41)

9:                    $cost\leftarrow computecost({\overline{\mathit{s}}}_{i,k}^v,b_{i,k}^v,{\mathit{U}}_k)$ using Equation (22)

10:                  for ii = 1 to iteration (ii is the number of the iterations of CILQG)

11:                           $\{{\mathit{K}}_A,{\mathit{K}}_{\mathit{C}},cost\_redu\}\leftarrow backwardpass$ using Equation (30) to Equation (39)

12:                           if cost_redu < expected_cost_redu (The gradient and Hessian of the cost)

13:                           break

14:                           while (flag = 0)

15:                           $\{{\mathit{b}}_{i,k+1}^v,{\mathit{U}}_k\}\leftarrow forwardpass(\lambda )$

16:                           cost $\leftarrow computecost({\overline{\mathit{b}}}_{i,k}^v,{\mathit{b}}_{i,k+1}^v,{\mathit{U}}_k)$

17:                           flag = (cost-currentcost)/cost_redu > threshold

18:                           if flag = 1

19:                                     cost $\leftarrow$ currentcost, input $\leftarrow$ Uk, state $\leftarrow {\mathit{b}}_{i,k+1}^v$

20:                           else

21:                                     λ $\leftarrow$ λ*learnspeed

22:                 currentcost $\leftarrow$ cost, Ui,k $\leftarrow$ input

23: Update ${\mathit{b}}_{i,k+1}^v$

6. Simulation

6.1. Simulation Setup and ROS Scenario

In the simulation, the speed and heading angle of the ground maneuvering target are provided in Figure 6. The maximum communication radius is 100 m and the maximum detection range is π/3. The parameters of the UAV swarm are shown in

Table 1. Simulation parameters.

Parameter	Value
Number of the UAVs I	3
Number of obstacles P	3
Obstacle 1	O1 [(3600 m, 800 m), 320 m, 400 m]
Obstacle 2	O2 [(5800 m, 2810 m), 321 m, 400 m]
Obstacle 3	O3 [(7850 m, 4900 m), 322 m, 400 m]
UAV initial position	[−50 m, −86.6 m, 300 m], [50 m, −86.6 m, 301 m], [0 m, −100 m, 302 m]
Target initial position	[0 m, 0 m, 0 m]
Relative angle αi,0	[−π/6 rad, 0 rad, π/6 rad]
Relative distance Ri,0	[100 m, 100 m, 100 m]
Initial linear velocity and angular velocity	[20 m/s, 0 rad/s, 0 rad/s]
Initial heading and pitch angle	[0 rad, 0 rad]
UAV angle range	[−π/20 rad, π/20 rad]
Distance measurement noise	$n_k^d\sim N(0,0.01)$ m
Angle measurement noise	$n_k^a\sim N(0,0.0003)$ m
Length of horizon	4

. During the 500 s flight process, the three UAVs maintain their altitudes at 300 m, 301 m, and 302 m, respectively. The simulation step is 0.1 s.

The comparison is made in two aspects. First, we analyze the impact of the optimized configuration by examining the estimation errors across different values of α and R. Second, we compare CILQG against the Interior Point Optimizer (IPOPT) in terms of the error of tracking the reference trajectory, the calculation time, and the position estimation error of the target. IPOPT transfers the trajectory tracking problem to a nonlinear programming problem (NLP) and solves NLP based on the Interior Point method [36]. IPOPT requires fewer iterations to solve the problems than the Sequential Nonlinear Programming Technique (SNOPT), the Modular In-core Nonlinear Optimization System (MINOS), and Artelys Knitro (KNITRO) [37]. Moreover, IPOPT can reach a higher accuracy and better convergence rate than Finite Minimization with Constraints (FMINCON), nonlinear optimization using augmented Lagrange method (SOLNP), and SNOPT [38]. The simulation environment is established in both Python and the Robot Operating System (ROS), and the CPU is AMD Ryzen 9 7845 HX, 3.00 GHz. The target tracking task is executed in the urban environment simulated in the ROS, featuring a radio tower as an obstacle, as shown in Figure 7. The top view confirms the UAV swarm’s ability to maintain the desired configuration throughout the tracking process.

6.2. Comparison of Different Configurations

The reference trajectories to be tracked are calculated based on the configurations and the target belief. To assess the impact of configuration optimization, we vary α_1,k, α_2,k (angles of the first and second UAVs relative to the target, assumed equal due to symmetry), and R_i,k, resulting in four distinct cases outlined in

Table 2. Parameters for different configurations.

	α1,k (rad)	α2,k (rad)	α3,k (rad)	R1,k R2,k R3,k (m)
Case 1	−π/6	π/6	0	100
Case 2	−π/36	π/36	0	100
Case 3	−π/2	π/2	0	100
Case 4	−π/6	π/6	0	50

and visualized in Figure 8. This analysis highlights the benefits of the optimal configuration for minimizing the estimation errors.

The angle of the UAVs relative to the target in Case 1 is optimal under the detection angle constraint and communication radius constraint. In Case 2, the UAVs are close to each other. In Case 3, two UAVs are collinear with the target, causing the target to be outside the HFOV of UAV1 and UAV2. Moreover, the excessive distance between UAV1 and UAV2 (200 m) in Case 3 violates the maximum communication radius, as illustrated in Figure 8. Analyzing the target position estimation error in Figure 9, we observe a significant peak for Case 3 (yellow line) at 270 s. This spike coincides with the target’s acceleration and turning maneuver, highlighting the configuration’s inability to cope with such target dynamics. Conversely, the other three cases exhibit significantly lower estimation errors. Case 4, characterized by a more evenly distributed configuration and closer proximity to the target, consistently achieves the lowest error, while Case 2 shows the highest error among the successful configurations. These results underscore the importance of a well-distributed configuration that satisfies both communication and visibility constraints while maintaining proximity to the target for accurate state estimation.

In the following simulations, α_1,k, α_2,k, and α_3,k are set as −π/6 rad, π/6 rad, and 0 rad, and R_i,k = 100 m.

6.3. Comparison of IPOPT and CILQG

This section compares the performance of IPOPT and CILQG in tracking the reference trajectory while simultaneously estimating the state of a maneuvering target.

The time complexity of IPOPT and CILQG in each iteration is O(n³) and O(n²), respectively, where n represents the dimension of the state vector. Qualitatively, CILQG is asymptotically faster than IPOPT. Quantificationally, Figure 10 presents the average calculation time of three UAVs. On average, CILQG takes 0.0097 s, whereas IPOPT consumes 0.71 s. It demonstrates that the computing speed of CILQG is faster than IPOPT.

Figure 6 illustrates a challenging tracking scenario where the target undergoes turns and changes in speed. Notably, the target executes a turn coupled with acceleration between 250 s and 350 s, followed by another turn with deceleration from 350 s to 450 s. Figure 11 provides the side and top views of the resulting trajectories for both CILQG and IPOPT. The actual trajectory of the target is shown with a black line, and the estimated trajectories of CILQG and IPOPT are shown with a magenta dotted line and a yellow dotted line, respectively. While CILQG demonstrates robust tracking and maintains the desired formation even during the target’s maneuvers, IPOPT struggles to maintain the reference configuration. As evident in the enlarged view of Figure 11b, the UAVs controlled by IPOPT lose track of the target within their HFOV once the target initiates its maneuver, leading to the disappearance of the estimated trajectory (yellow dotted line).

This difference in performance is further emphasized in Figure 12, which presents the tracking error for both controllers. CILQG consistently maintains a tracking error below 3 m, with spikes only occurring during the target’s turns and speed changes. In stark contrast, IPOPT exhibits oscillatory behavior from the outset, and the tracking error rapidly diverges when the target performs its first turn at 150 s.

The time during which the target is within the HFOV of the UAVs is shown in Figure 13, where the value of 1 indicates the target is in the HFOV. IPOPT’s inability to maintain the desired configuration results in the target being lost from the HFOV, aligning with the increasing tracking error observed in Figure 12. While CILQG generally keeps the target within the HFOV during periods of constant target velocity and orientation, temporary losses occur during the target’s turns and shifts. During these instances, the UAVs rely on trajectory extrapolation due to the lack of measurements, leading to an increased estimation error. As depicted in Figure 14, the estimation error of CILQG is stable under 4 m. IPOPT’s estimation error exhibits significant oscillations after 150 s, coinciding with the loss of the target from the HFOV, highlighting the detrimental impact of poor tracking performance on estimation accuracy.

This disparity in performance stems from the fundamental difference in how each controller handles uncertainty. IPOPT treats target states as deterministic values at each time step, neglecting their inherent uncertainty, whereas CILQG regards the state as belief, characterized by its mean and variance. This allows CILQG to track the entire trajectory profile, effectively minimizing the variance of the belief and achieving robust performance even under uncertainty. Consequently, while IPOPT suffers from diverging tracking errors, potentially violating visibility constraints and losing track of the target, CILQG enables accurate trajectory tracking and timely target motion estimation, even in the presence of random target maneuvers.

6.4. Tracking with Obstacle

In this section, we compare the collision avoidance ability of CILQG and IPOPT. The obstacles are strategically positioned near the trajectory where the target makes turns. This setup requires the UAV to maneuver by altering its speed and orientation to avoid the obstacles while maintaining persistent tracking of the target.

Figure 15 showcases the effectiveness of CILQG in navigating a complex environment with obstacles (represented as blue cylinders in the side view and black hollow circles in the top-down view). CILQG adeptly guides the UAVs to adjust their trajectories and circumvent the three obstacles, deviating from the reference path only when necessary. In contrast, IPOPT falters in this challenging scenario, with UAV trajectories deviating significantly from the target’s path and failing to avoid obstacles. In Figure 15a, the z-axis positions of the UAVs even drop below zero. This indicates that the obstacles can cause the trajectories to deviate further apart, and highlights IPOPT’s vulnerability to environmental complexities and its limitations in handling constraints effectively. This conclusion is further supported by Figure 16, which displays the trajectory tracking error for the UAVs. The tracking errors of CILQG exhibit three peaks at 210 s, 340 s, and 430 s only during obstacle avoidance maneuvers, and the errors are under 3 m when the UAVs are away from the obstacles. IPOPT’s tracking errors exhibit an overall increasing trend and eventual divergence. Furthermore, Figure 17 illustrates that the target estimation error of CILQG remains below 4 m, consistent with its performance in the obstacle-free scenario, whereas the estimation error of IPOPT rises rapidly when one of the UAVs hits the obstacle and the target is lost in the HFOV. Therefore, the obstacles do not significantly affect the estimation performance of the proposed method. However, IPOPT is unable to guide the UAVs to track the target accurately and safely due to the randomness of the target states and UAV states.

7. Discussion

In addressing the challenge of target tracking with obstacles and uncertainty in both the target state and UAV navigation unit, this study sequentially tackled two joint optimization problems. Firstly, an optimal configuration for the UAVs was obtained by solving the AIA problem. This involved an online estimation of the target state, which subsequently informed the generation of reference trajectories for each UAV. Subsequently, a fast and efficient CILQG algorithm was developed to tackle the trajectory tracking problem. Recognizing the inherent uncertainties, we represented both the target and UAV states as belief states, employing the Unscented Transform for accurate belief propagation.

Our simulation results validated the effectiveness of this approach. The optimized configuration significantly reduced target estimation errors. Furthermore, CILQG demonstrated superior performance compared to IPOPT in terms of tracking accuracy, speed, and robustness. While IPOPT struggled to maintain visibility of the target, leading to constraint violations, CILQG ensured accurate estimation of target states even when the target maneuvered randomly. Importantly, CILQG ensured reliable obstacle avoidance throughout the tracking process, highlighting its suitability for complex, dynamic environments.