1 Introduction↩︎

Aerial target tracking has been widely applied in fields such as cinematography [1] and surveillance [2]. Although a single micro aerial vehicle (MAV) is usually employed in these applications, the utilization of multiple MAVs can bring benefits. For example, multiple views of actors captured by a team of MAVs provide movie directors with more footage [3]–[5]. Also, multi-agent tracking can be deployed in moving motion-capture systems [6]–[8], and a large number of cameras increases the accuracy of pose estimation.

Despite great attention and research on motion generation for multi-robot systems, multi-agent target tracking remains a challenging task. The main challenge is finding constraints that prevent both inter-agent occlusion and inter-agent collision, while considering the moving target. Additionally, several other requirements should be considered: occlusion and collision against obstacles, actuator limits, and tracking distances. To respond to frequent changes in the motion of the target and moving obstacles, the trajectory planning should be updated quickly to reflect these considerations.

In this letter, we present an online distributed trajectory planning algorithm that can generate a target-visible and safe trajectory. The key ideas of the proposed method are Dynamic Buffered Voronoi Cell (DBVC) and Dynamic Inter-Visibility Cell (DIVC). The DBVC is a time-varying, inter-collision-free region developed from Buffered Voronoi Cell (BVC) [9]. This time-varying region helps maintain consistent distances between the target and the agents, whereas the BVC can cause an agent to become stuck in a static space, making it difficult for the agent to follow the target. We design the DIVC to prevent inter-agent occlusion. Specifically, the DIVC ensures that the Line-of-Sight connecting the target and an agent does not collide with other agents. Based on the DBVC and the DIVC, the proposed planner generates a target-visible and safe trajectory using Bernstein polynomial motion primitives, in an improved way over our previous work [10]. By reducing the conservativeness in [10], our method achieves a higher success rate. The main contributions of this paper are as follows.

• A distributed multi-agent trajectory planning algorithm for target tracking that generates a collision-free and occlusion-free trajectory in complex environments, such as dozens of obstacles.

• A construction method of Dynamic Buffered Voronoi Cell (DBVC) and Dynamic Inter-Visibility Cell (DIVC) that impose inter-agent collision and inter-agent occlusion avoidance constraints, while adapting to the future movements of the target.

• An integration of the DBVC and the DIVC with an improved Bernstein polynomial motion primitive-based trajectory generator, which lessens the conservativeness of our previous method [10].

2 Related Works↩︎

2.1 Target Tracking Using a Single Drone↩︎

Various studies have proposed methods that consider target visibility for single-drone path planning in obstacle environments. [11]–[13] address target occlusion in the presence of static and unstructured obstacles. To be specific, [11] and [12] design polytope-shaped target-visible regions and formulate a spatio-temporal optimization problem. [13] proposes a visibility metric using Euclidean Signed Distance Fields (ESDF) and formulates a graph optimization to select optimal viewpoints.

There are works that address target tracking problems in dynamic obstacle environments [14]–[16]. [14] designs a visibility cost to avoid target occlusion, inspired by the GPU ray casting model. [15] utilizes partial convex structures of the target tracking optimization problem to handle non-convex visibility constraints, while [16] designs a time-varying half-space-shaped region to apply visibility constraints within a quadratic programming problem.

The above approaches can be utilized for multi-agent target tracking missions by treating neighboring drones as dynamic obstacles. However, such approaches may result in collisions between agents or occlusions of the target by neighboring agents, as the newly updated trajectories may differ from the predicted movements of other agents. In contrast, our planner enables cooperative target tracking, in a distributed manner.

2.2 Target Tracking Using Multiple Drones↩︎

Research on target tracking using multiple MAVs has been widely studied in recent years. [5]–[7] focus on the formation of MAVs to follow the moving target. [5] and [6] decompose the multi-MAV trajectory optimization to tackle non-convex constraints, which allows replanning in real-time. Both methods calculate viewpoints considering formations, and then successive optimization generates a safe [5] and smooth [6] trajectory. [7] develops external control inputs to prevent inter-agent collision, enabling the conversion of the optimization problem into a convex one. However, the above approaches address only collision avoidance with respect to the target and neighboring agents, while omitting considerations of visibility issues caused by neighboring agents.

On the other hand, there are studies that consider mutual visibility. [3] optimize MAVs’ viewpoints in discretized state and time spaces in a centralized manner. The finer the discretized states, the more computation time and memory are required. Therefore, the authors set the number of time steps small to enable real-time execution of the algorithm. The methods in [4] and [17] try to avoid the appearance of the other MAVs in the MAV’s camera image. Both methods operate in a distributed manner to meet real-time criteria; however, [4] uses a priority-based approach, while [17] employs a sequential consensus approach. Therefore, there may be situations where all mutual visibility constraints are not simultaneously satisfied. In contrast, our method ensures inter-agent occlusion avoidance and does not require sequential planning updates, resulting in a fully distributed approach.

2.3 Target Tracking in Crowded Environments↩︎

Most trajectory planning research aims to ensure real-time performance. The works [14], [18], and [19] formulate non-convex optimization methods to generate a target-visible trajectory; however, as the number of obstacles increases, computation time quickly increases. The work [16] adopts the QP-based approach for fast calculation, but it cannot handle unstructured obstacles. To address this, this work builds upon our previous work [10], using a Bernstein polynomial motion-primitive to efficiently find a tracking trajectory among both unstructured-but-static and dynamic-but-structured obstacles. Furthermore, this work reduces the conservativeness of the method in [10], further improving the tracking success rate in challenging conditions, such as environments with numerous dynamic obstacles.

3 Problem Formulation↩︎

In this section, we formulate a trajectory planning problem for multiple tracking agents. We suppose that \(N_{c}\) homogenous agents are deployed to follow a target in a 2-dimensional space \(\mathcal{X}\subset \mathbb{R}^{2}\) with static and \(N_{o}\) dynamic obstacles. We aim to generate trajectories so that the agents 1) avoid collisions, 2) maintain the visibility of the target and 3) do not exceed dynamical limits.

Extending to a 3D setup is straightforward, but fixed flying altitude enables the acquisition of consistent images of the target. Therefore, we address the 2D problem in this paper. Since the drone must find feasible movements only on the \(x-y\) plane, the mission becomes more challenging. Throughout this paper, the notations in Table 1 are used.

3.1 Environments↩︎

The environment \(\mathcal{X}\) consists of target space \(\mathcal{T}(t)\), obstacle space \(\mathcal{O}(t)\), and drone space \(\mathcal{C}(t)\). \(\mathcal{O}(t)\) is divided into spaces occupied by static and dynamic obstacles, \(\mathcal{O}_{s}\) and \(\mathcal{O}_{d}(t)\), respectively. \(\mathcal{C}(t)\) is an occupied space by all agents and is partitioned into the \(i\)-th agent space \(\mathcal{C}_{i}(t)\), \(i \in \mathcal{I}_{c}\). We define the following free spaces for target-tracking missions. \[\label{eq:free95spaces} \begin{align} &\mathcal{F}_{s} = \mathcal{X}\setminus \mathcal{O}_{s}, \; \mathcal{F}_{d}(t) = \mathcal{F}_{s}\setminus \mathcal{O}_{d}(t),\; \mathcal{F}_{q}(t) = \mathcal{F}_{d}(t)\setminus \mathcal{T}(t),\\ &\mathcal{F}_{c}^{i}(t) = \mathcal{X}\setminus \mathcal{C}^{i}(t), \;\text{where}\;\mathcal{C}^{i}(t) = \mathcal{C}(t) \setminus \mathcal{C}_{i}(t). \end{align}\tag{1}\]

3.2 Trajectory Representation↩︎

Due to the virtue of differential flatness of the dynamics of various MAV platforms, such as quadrotors, we represent trajectories of all agents as polynomial functions of time \(t\). Employing the Bernstein basis [20], we represent the trajectory of the \(i\)-th agent, \(\boldsymbol{x}^{i}_{c}(t)\in \mathbb{R}^{2}, i\in \mathcal{I}_{c}\) as follows: \[\boldsymbol{x}_{c}^{i}(t) = \boldsymbol{C}^{i\top}\boldsymbol{b}_{n_{c}}(t),\;t \in [0,T] \label{eq:berstein95representation}\tag{2}\] where \(T\) is the planning horizon, \(n_{c}\) is the degree of the polynomials, \(\boldsymbol{C}^{i}\in\mathbb{R}^{(n_c+1)\times2}\) is a coefficient matrix for the agent \(i\), and \(\boldsymbol{b}_{n_{c}}(t)\in \mathbb{R}^{(n_{c}+1)\times1}\) is a vector that consists of \(n_{c}\)-th order Bernstein bases for time interval \([0,T]\). We represent the trajectory of the target, \(\boldsymbol{x}_{q}(t)\), the \(k\)-th dynamic obstacles, \(\boldsymbol{x}_{o}^{k}(t)\), \(k\in \mathcal{I}_{o}\) in the same manner.

3.3 Assumptions↩︎

In this study, we make the following assumptions.

• Agents: All agents share their current positions, and they start trajectory planning at the same time.

• Obstacles: The information about the static obstacle space \(\mathcal{O}_{s}\) is given as a point cloud, and the current positions of dynamic obstacles can be acquired.

• Target: The current position of the target can be observed.

• Shape: The moving objects, such as the target, dynamic obstacles, and tracking agents are modeled as balls with radius \(r_{q}\), \(r_{o}\) and \(r_{c}\), respectively.

In this paper, we use extended Kalman filters to estimate the velocity of each moving object, and the future trajectories of the obstacles and the target are predicted using a constant velocity model and the method described in [10], respectively.

3.4 Mission Description↩︎

In the multi-agent tracking trajectory planning, we focus on the following objectives.

3.4.1 Collision Avoidance↩︎

For safety, all agents should not collide with obstacles and the target. \[\label{eq:collision95avoidance} \mathcal{B}(\boldsymbol{x}_{c}^{i}(t),r_{c})\subset \mathcal{F}_{q}(t),\;\forall i\in \mathcal{I}_{c},\;\forall{t}\in [0,T]\tag{3}\]

3.4.2 Occlusion Avoidance↩︎

To avoid target occlusion by obstacles, the Lines-of-Sight between the agents and the target should not intersect with any obstacles. \[\label{eq:occlusion95avoidance} \mathcal{L}(\boldsymbol{x}_{c}^{i}(t),\boldsymbol{x}_{q}(t))\subset \mathcal{F}_{d}(t),\;\forall i\in \mathcal{I}_{c},\;\forall{t}\in [0,T]\tag{4}\]

3.4.3 Target Distance↩︎

In order to avoid being too close or too far from the target, we formulate the distance constraints. \[\label{eq:distance95target} \|\boldsymbol{x}_{c}^{i}(t)-\boldsymbol{x}_{q}(t)\|\in [d_{\min}, d_{\max}],\;\forall i \in \mathcal{I}_{c},\; \forall t \in [0,T]\tag{5}\]

3.4.4 Dynamical Limits↩︎

Due to the actuator limits of the drone, the trajectory should not exceed the maximum velocity, \(v_{\max}\), and acceleration, \(a_{\max}\). Also, the yaw rate should not exceed \(\dot{\psi}_{\max}\) to prevent motion blurs in the camera. \[\label{eq:dynamical95limits} \begin{align} & \|\dot{\boldsymbol{x}}_{c}^{i}(t)\|\leq v_{\max},\; \forall i\in \mathcal{I}_{c},\;\forall t \in [0,T]\\ & \|\ddot{\boldsymbol{x}}_{c}^{i}(t)\|\leq a_{\max},\;\forall i\in \mathcal{I}_{c},\;\forall t \in [0,T]\\ & |\dot{\psi}_{c}^{i}(t)|\leq \dot{\psi}_{\max},\;\forall i\in \mathcal{I}_{c},\;\forall t \in [0,T]. \end{align}\tag{6}\]

3.4.5 Inter-Agent Collision Avoidance↩︎

For safety, the tracker team should also avoid inter-agent collisions. \[\label{eq:inter95collision} \mathcal{B}(\boldsymbol{x}_{c}^{i}(t),r_{c})\subset \mathcal{F}_{c}^{i}(t),\;\forall i\in \mathcal{I}_{c},\;\forall{t}\in [0,T]\tag{7}\]

3.4.6 Inter-Agent Occlusion Avoidance↩︎

To avoid inter-agent occlusions, we ensure that the Lines-of-Sight between each agent and the target do not intersect with other agents. \[\label{eq:inter95occlusion} \mathcal{L}(\boldsymbol{x}_{c}^{i}(t),\boldsymbol{x}_{q}(t))\subset \mathcal{F}_{c}^{i}(t),\;\forall i\in \mathcal{I}_{c},\;\forall{t}\in [0,T]\tag{8}\]

4 Dynamic Cells for Multi-Agent Tracking↩︎

In this section, we design time-varying regions free from inter-agent collisions and inter-agent occlusions, which are suitable for dynamic target tracking.

4.1 Dynamic Buffered Voronoi Cell↩︎

Buffered Voronoi Cell (BVC) [9] has been used in various research as a technique for reciprocal collision avoidance in multi-agent trajectory planning. Since the BVC is static during the planning horizon, agents can be confined in the static cell while the target moves, leading to situations where the agents miss the target. Accordingly, we consider the inter-collision-free region that varies with the target’s future trajectory. We define a Dynamic Buffered Voronoi Cell (DBVC) to avoid collisions between the \(i\)-th and \(j\)-th agents as follows: \[\label{eq:moving95buffered95voronoi95cells} \begin{align} \nonumber \mathcal{H}_{s}^{ij}(t) = \bigg\{ & \boldsymbol{x}(t)\in \mathbb{R}^{2}| (\boldsymbol{x}_{c0}^{j}-\boldsymbol{x}_{c0}^{i})^{\top}\bigg(\boldsymbol{x}(t)-\boldsymbol{x}_{q}(t)+\boldsymbol{x}_{q0}-\\ & \frac{\boldsymbol{x}_{c0}^{i}+\boldsymbol{x}_{c0}^{j}}{2} \bigg) + r_{c}\|\boldsymbol{x}_{c0}^{j}-\boldsymbol{x}_{c0}^{i}\| \leq 0 \bigg\},\\ \nonumber \mathcal{H}_{s}^{ji}(t) = \bigg\{ & \boldsymbol{x}(t)\in \mathbb{R}^{2}| (\boldsymbol{x}_{c0}^{i}-\boldsymbol{x}_{c0}^{j})^{\top}\bigg(\boldsymbol{x}(t)-\boldsymbol{x}_{q}(t)+\boldsymbol{x}_{q0}-\\ & \frac{\boldsymbol{x}_{c0}^{j}+\boldsymbol{x}_{c0}^{i}}{2} \bigg) + r_{c}\|\boldsymbol{x}_{c0}^{i}-\boldsymbol{x}_{c0}^{j}\| \leq 0 \bigg\} \end{align}\tag{9}\]

4.2 Dynamic Inter-Visibility Cell↩︎

Similar to the Dynamic Buffered Voronoi Cell (DBVC), we define a Dynamic Inter-Visibility Cell (DIVC) that moves in accordance with the target’s future trajectory and prevents inter-agent occlusion between the \(i\)-th and \(j\)-th agents.

We design the DIVC by dividing the cases where an angle formed by the two Lines-of-Sight, \(\mathcal{L}(\boldsymbol{x}_{c0}^{i},\boldsymbol{x}_{q0})\) and \(\mathcal{L}(\boldsymbol{x}_{c0}^{j},\boldsymbol{x}_{q0})\), is either obtuse or acute. Also, depending on whether the relative configuration of current positions of the target, the \(i\)-th and \(j\)-th agents, i.e. \(\boldsymbol{x}_{q0}\), \(\boldsymbol{x}_{c0}^{i}\), and \(\boldsymbol{x}_{c0}^{j}\). \[\label{eq:configuration95inner} z^{ij} = \text{sign}\bigg(\det\bigg(\begin{bmatrix} (\boldsymbol{x}_{c0}^{j}-\boldsymbol{x}_{q0})^{\top}\\(\boldsymbol{x}_{c0}^{i}-\boldsymbol{x}_{q0})^{\top} \end{bmatrix}\bigg)\bigg)\tag{11}\] \(z^{ij}\) is a variable determined by the sign function. Fig. 2 illustrates the cases where \(z^{ij}=-1\).

4.2.1 Obtuse case↩︎

Let \(\textit{V}_{i}\), \(\textit{V}_{j}\), and \(\textit{V}_{Q}\) be points at \(\boldsymbol{x}_{c0}^{i}\), \(\boldsymbol{x}_{c0}^{j}\), and \(\boldsymbol{x}_{q0}\), respectively. Then, let points \(V_{oi}\) and \(V_{oj}\) be the points on line segments \(\overline{V_{i}V_{Q}}\) and \(\overline{V_{j}V_{Q}}\) that are each at a distance \(\alpha_{o}^{ij}r_{c}\) (\(\alpha_{o}^{ij}\geq1\)) from \(V_{Q}\). We draw lines (red in Fig. 2 (a)) that are perpendicular to \(\overline{V_{i}V_{Q}}\) and \(\overline{V_{j}V_{Q}}\), crossing \(V_{oi}\) and \(V_{oj}\), and we call half-spaces made by the lines \(\mathcal{H}_{o1}^{ij}\) and \(\mathcal{H}_{o1}^{ji}\), respectively. In addition, we draw the rays (blue in Fig. 2 (a)) starting from \(V_{oi}\) and \(V_{oj}\), which are parallel to the lines that pass through \(V_{Q}\) and are tangential to the balls, \(\mathcal{B}(\textit{V}_{oj},r_{c})\) and \(\mathcal{B}(\textit{V}_{oi},r_{c})\). Half-spaces made by the rays are represented as \(\mathcal{H}_{o2}^{ij}\) and \(\mathcal{H}_{o2}^{ji}\).

To satisfy the conditions that \(V_{oi}\) and \(V_{oj}\) are on \(\overline{V_{i}V_{Q}}\) and \(\overline{V_{j}V_{Q}}\), and the rays do not intersect, \(\alpha_{o}^{ij}\) should be in the following range. \[\label{subeq:obtuse95alpha95range}\; \begin{align} &1\leq \alpha_{o}^{ij} \leq \min\Bigg(\frac{1}{r_{c}}\min(\|\boldsymbol{x}_{c0}^{i}-\boldsymbol{x}_{q0}\|, \|\boldsymbol{x}_{c0}^{j}-\boldsymbol{x}_{q0} \|),\\ & \quad \quad \quad \quad \quad \quad \sqrt{\frac{2}{1-\frac{(\boldsymbol{x}_{c0}^{i}-\boldsymbol{x}_{q0})^{\top}(\boldsymbol{x}_{c0}^{j}-\boldsymbol{x}_{q0})}{\|\boldsymbol{x}_{c0}^{i}-\boldsymbol{x}_{q0}\|\|\boldsymbol{x}_{c0}^{j}-\boldsymbol{x}_{q0}\|}}}\;\Bigg) \end{align}\tag{12}\] With the \(\alpha_{o}^{ij}\) satisfying (12 ), for \(\forall\boldsymbol{x}^{i} \in \mathcal{H}_{o1}^{ij}\cap\mathcal{H}_{o2}^{ij}\) and \(\forall\boldsymbol{x}^{j} \in \mathcal{H}_{o1}^{ji}\cap\mathcal{H}_{o2}^{ji}\), the following inequalities are satisfied. \[\label{eq:mvc95line95of95sight95ok} \begin{align} &\underset{\epsilon\in[0,1]}{\min} \|\epsilon\boldsymbol{x}^{i}+(1-\epsilon)\boldsymbol{x}_{q0}-\boldsymbol{x}^{j}\|> r_{c},\\ &\underset{\epsilon\in[0,1]}{\min} \|\epsilon\boldsymbol{x}^{j}+(1-\epsilon)\boldsymbol{x}_{q0}-\boldsymbol{x}^{i}\|> r_{c} \end{align}\tag{13}\] The above inequalities mean that the distances between neighboring agents and Line-of-Sight connecting the tracker and the target are always greater than the size of the trackers \(r_{c}\); therefore, the inter-occlusion constraints (8 ) are satisfied. To make the inter-occlusion-free regions move with the target, we maintain the shapes of \(\mathcal{H}_{o1}^{ij}\cap\mathcal{H}_{o2}^{ij}\) and \(\mathcal{H}_{o1}^{ji}\cap\mathcal{H}_{o2}^{ji}\) and translate them by \(\boldsymbol{x}_{q}(t)-\boldsymbol{x}_{q0}\), equivalent to the amount of target’s movement. The moving half-spaces are represented as \(\mathcal{H}_{o1}^{ij}(t)\), \(\mathcal{H}_{o2}^{ij}(t)\), \(\mathcal{H}_{o1}^{ji}(t)\), and \(\mathcal{H}_{o2}^{ji}(t)\). Their mathematical expressions are simplified as follows, and \(\mathcal{H}_{o1}^{ji}(t)\) and \(\mathcal{H}_{o2}^{ji}(t)\) can be constructed by exchanging the indices \(i\) and \(j\) in (14 ). \[\tag{14} \begin{align} \tag{15} \nonumber \mathcal{H}_{o1}^{ij}(t) = & \{\boldsymbol{x}(t)\in\mathbb{R}^{2}|(\boldsymbol{x}_{c0}^{i}-\boldsymbol{x}_{q0})^{\top}(\boldsymbol{x}(t)-\boldsymbol{x}_{q}(t))- \\ & \;\alpha_{o}^{ij} r_{c}\|\boldsymbol{x}_{c0}^{i}-\boldsymbol{x}_{q0}\| \geq 0 \}, \\ \tag{16} \nonumber \mathcal{H}_{o2}^{ij}(t)= & \bigg \{ \boldsymbol{x}(t)\in \mathbb{R}^{2}| \begin{bmatrix} z^{ij}\sin({\theta}_{cq}^{j}+z^{ij}\theta_{o}^{ij})\\ -z^{ij}\cos({\theta}_{cq}^{j}+z^{ij}\theta_{o}^{ij}) \end{bmatrix}^{\top} \bigg(\boldsymbol{x}(t)- \\ & \;\boldsymbol{x}_{q}(t)-\alpha_{o}^{ij} r_{c}\frac{(\boldsymbol{x}_{c0}^{i}-\boldsymbol{x}_{q0})}{\|\boldsymbol{x}_{c0}^{i}-\boldsymbol{x}_{q0}\|}\bigg) \leq 0 \bigg \} \end{align}\] where \(\alpha_{o}^{ij}\), \(\theta_{cq}^{i}\), \(\theta_{cq}^{j}\), and \(\theta_{o}^{ij}\) are defined as follows. \[\tag{17} \begin{align} \tag{18} & \begin{bmatrix} \cos(\theta_{cq}^{i})\\ \sin(\theta_{cq}^{i}) \end{bmatrix} = \frac{(\boldsymbol{x}_{c0}^{i}-\boldsymbol{x}_{q0})}{\|\boldsymbol{x}_{c0}^{i}-\boldsymbol{x}_{q0}\|},\;\begin{bmatrix} \cos(\theta_{cq}^{j})\\ \sin(\theta_{cq}^{j}) \end{bmatrix} = \frac{(\boldsymbol{x}_{c0}^{j}-\boldsymbol{x}_{q0})}{\|\boldsymbol{x}_{c0}^{j}-\boldsymbol{x}_{q0}\|},\\ \tag{19} &\sin(\theta_{o}^{ij}) =\frac{1}{\alpha_{o}^{ij}},\;\cos(\theta_{o}^{ij})=\sqrt{1-\sin^{2}(\theta_{o}^{ij})} \end{align}\]

4.2.2 Acute case↩︎

Fig. 2 (b) illustrates an acute case. First, we draw the lines that are parallel to \(\overline{V_{i}V_{Q}}\) and \(\overline{V_{j}V_{Q}}\) and apart by \(\alpha_{a}^{ij}r_{c}\) (\(\alpha_{a}^{ij}\geq1\)) from them. The half-spaces made by the lines are represented as \(\mathcal{H}_{a1}^{ji}\) and \(\mathcal{H}_{a1}^{ij}\) (red in Fig. 2 (b)), and the intersections between the segments and lines are denoted as \(V_{aj}\) and \(V_{ai}\), respectively. Then, we draw the rays (blue in Fig. 2 (b)) starting from \(V_{ai}\) and \(V_{aj}\), which are parallel to the lines that pass through \(V_{Q}\) and are tangential to the balls \(\mathcal{B}(V_{aj},r_{c})\) and \(\mathcal{B}(V_{ai},r_{c})\). Half-spaces divided by the rays are represented as \(\mathcal{H}_{o2}^{ij}\) and \(\mathcal{H}_{o2}^{ji}\). To satisfy conditions that \(V_{ai}\) and \(V_{aj}\) are on \(\overline{V_{i}V_{Q}}\) and \(\overline{V_{j}V_{Q}}\), and the rays do not intersect, \(\alpha_{a}^{ij}\) should satisfy the following range. \[\label{eq:acute95alpha95range} \begin{align} &1\leq \alpha_{a}^{ij} \leq \min\Bigg(\frac{\bigg\|\det\bigg(\begin{bmatrix} (\boldsymbol{x}_{c0}^{i}-\boldsymbol{x}_{q0})^{\top}\\(\boldsymbol{x}_{c0}^{j}-\boldsymbol{x}_{q0})^{\top} \end{bmatrix} \bigg)\bigg\|}{r_{c}\max(\|\boldsymbol{x}_{c0}^{i}-\boldsymbol{x}_{q0}\|,\|\boldsymbol{x}_{c0}^{j}-\boldsymbol{x}_{q0}\|)},\\ & \quad \quad \quad \quad \quad \quad \sqrt{2\bigg(1+\frac{(\boldsymbol{x}_{c0}^{i}-\boldsymbol{x}_{q0})^{\top}(\boldsymbol{x}_{c0}^{j}-\boldsymbol{x}_{q0})}{\|\boldsymbol{x}_{c0}^{i}-\boldsymbol{x}_{q0}\|\|\boldsymbol{x}_{c0}^{j}-\boldsymbol{x}_{q0}\|}\bigg)} \;\Bigg), \\ \end{align}\tag{20}\] Similarly to the Obtuse case, \(\forall\boldsymbol{x}^{i} \in \mathcal{H}_{a1}^{ij}\cap\mathcal{H}_{a2}^{ij}\) and \(\forall\boldsymbol{x}^{j} \in \mathcal{H}_{a1}^{ji}\cap\mathcal{H}_{a2}^{ji}\) satisfy (13 ). The moving version of these half-spaces are denoted as \(\mathcal{H}_{a1}^{ij}(t)\), \(\mathcal{H}_{a2}^{ij}(t)\), \(\mathcal{H}_{a1}^{ji}(t)\), and \(\mathcal{H}_{a2}^{ji}(t)\) and mathematically formulated as follows. \[\tag{21} \begin{align} \tag{22} \nonumber \mathcal{H}^{ij}_{a1}(t)= &\bigg\{ \boldsymbol{x}(t)\in \mathbb{R}^{2}| z^{ij}\det\bigg(\begin{bmatrix} (\boldsymbol{x}(t)-\boldsymbol{x}_{q}(t))^{\top}\\ (\boldsymbol{x}_{c0}^{j}-\boldsymbol{x}_{q0})^{\top} \end{bmatrix}\bigg)+\\ &\quad \quad \quad \alpha_{a}^{ij}r_{c}\|\boldsymbol{x}_{c0}^{j}-\boldsymbol{x}_{q0} \| \leq 0 \bigg\},\\ \tag{23} \nonumber \mathcal{H}^{ij}_{a2}(t)= &\bigg\{ \boldsymbol{x}(t) \in \mathbb{R}^{2}| \begin{bmatrix} z^{ij}\sin(\theta_{cq}^{j}+z^{ij}\theta_{a}^{ij})\\ -z^{ij}\cos(\theta_{cq}^{j}+z^{ij}\theta_{a}^{ij}) \end{bmatrix}^{\top}\bigg(\boldsymbol{x}(t)-\\ &\;\boldsymbol{x}_{q}(t)-\alpha_{a}^{ij}r_{c}\frac{\|\boldsymbol{x}_{c0}^{j}-\boldsymbol{x}_{q0}\|(\boldsymbol{x}_{c0}^{i}-\boldsymbol{x}_{q0})}{\bigg\|\det\Big(\begin{bmatrix} (\boldsymbol{x}_{c0}^{i}-\boldsymbol{x}_{q0})^\top \\(\boldsymbol{x}_{c0}^{j}-\boldsymbol{x}_{q0})^\top \end{bmatrix} \Big)\bigg\|}\bigg)\leq 0 \bigg\} \end{align}\] where \(\theta_{a}^{ij}\) are defined as follows. \[\label{eq:mvc95acute95parameters} \begin{align} &\sin(\theta_{a}^{ij}) = \frac{\bigg\|\det\Big(\begin{bmatrix} (\boldsymbol{x}_{c0}^{i}-\boldsymbol{x}_{q0})^{\top}\\(\boldsymbol{x}_{c0}^{j}-\boldsymbol{x}_{q0})^{\top} \end{bmatrix}\Big)\bigg\|}{\alpha_{a}^{ij}\|\boldsymbol{x}_{c0}^{i}-\boldsymbol{x}_{q0} \| \|\boldsymbol{x}_{c0}^{j}-\boldsymbol{x}_{q0} \|},\\ &\cos(\theta_{a}^{ij}) = \sqrt{1-\sin^{2}(\theta_{a}^{ij})} \end{align}\tag{24}\]

Lemma 2 indicates that the DBVC and DIVC offer feasible constraints to handle inter-agent collisions and occlusions.

5 Tracking Trajectory Planning↩︎

In this section, we formulate the tracking trajectory generation using Bernstein polynomial motion primitives. First, we sample a bundle of motion primitives and filter out the primitives that do not satisfy constraints. Then, the best trajectory is selected.

5.1 Primitive Sampling↩︎

Based on the \(i\)-th agent’s current position \(\boldsymbol{x}_{c0}^{i}\) and velocity \(\dot{\boldsymbol{x}}_{c0}^{i}\), we formulate the following optimal control problem to sample primitives, which can be solved in the closed form. \[\label{eq:nearly95constant95velocity95model} \begin{align} &\underset{\boldsymbol{u}_{c}^{i}(t)}{\text{min}} && \frac{1}{T}\int_{0}^{T}\|\boldsymbol{u}_{c}^{i}(t)\|^{2}dt \\ &\text{s.t.} && \dot{\boldsymbol{z}}_{c}^{i}(t) = F_{c}\; \boldsymbol{z}_{c}^{i}(t) + G_{c}\boldsymbol{u}_{c}^{i}(t),\\ & \;&& \boldsymbol{z}_{c}^{i}(t)=\begin{bmatrix} \boldsymbol{x}_{c}^{i}(t) \\ \dot{\boldsymbol{x}}_{c}^{i}(t) \end{bmatrix}, F_{c} = \begin{bmatrix} 0_{2} & I_{2}\\ 0_{2} & 0_{2} \end{bmatrix} , \;G_{c} = \begin{bmatrix} 0_{2} \\ I_{2} \end{bmatrix},\\ & \;&& \boldsymbol{x}_{c}^{i}(0)= \boldsymbol{x}_{c0}^{i}, \;\dot{\boldsymbol{x}}_{c}^{i}(0) = \dot{\boldsymbol{x}}_{c0}^{i}, \;\boldsymbol{x}_{c}^{i}(T)=\boldsymbol{x}_{cf}^{i} \end{align}\tag{25}\] \(0_{2}\) is a \(2\times2\) zero matrix, and \(I_{2}\) is a \(2 \times 2\) identity matrix. The \(\boldsymbol{x}_{cf}^{i}\) are terminal points, sampled around the target’s terminal points \(\boldsymbol{x}_{q}(T)\):

\[\label{eq:terminal95point95sampling} \begin{align} &\boldsymbol{x}_{cf}^{i} = \boldsymbol{x}_{q}(T)+[r_{cs}^{i}\cos{\psi_{cs}^{i}},r_{cs}^{i}\sin{\psi_{cs}^{i}} ]^{\top},\\ &r_{cs}^{i} \sim U[\underbar{r}_{cs}^{i},\bar{r}_{cs}^{i}],\;\psi_{cs}^{i} \sim U [\underbar{\psi}_{cs}^{i},\bar{\psi}_{cs}^{i}]. \end{align}\tag{26}\] \(U\) represents the uniform distribution, and \((\underbar{r}_{cs}^{i},\bar{r}_{cs}^{i})\) and \((\underbar{\psi}_{cs}^{i},\bar{\psi}_{cs}^{i})\) are pairs of the lower and upper bound of distribution of radius and azimuth, respectively.

5.2 Feasibility Check↩︎

5.2.1 Collision Check↩︎

To make the \(i\)-th agent safe, we check whether trajectories satisfy (28 ): being confined in safe and visible corridors, and (29 )-(31 ): avoiding collision with dynamic obstacles, target, and the other agents. \[\tag{27} \begin{align} \tag{28} & \Xi(\boldsymbol{C}_{m}^{i})\oplus \mathcal{B}(\boldsymbol{0},r_{c})\subset \mathcal{S}_{m}^{i},\;\forall m=1,\ldots,M_{i}, \\ \tag{29} & \|\boldsymbol{x}_{c}^{i}(t)-\boldsymbol{x}_{o}^{k}(t)\|^{2}-(r_{c}+r_{o})^{2}\geq 0,\;\forall k\in \mathcal{I}_{o}\\ \tag{30} & \|\boldsymbol{x}_{c}^{i}(t)-\boldsymbol{x}_{q}(t)\|^{2}- (r_{c}+r_{q})^{2}\geq0,\\ \tag{31} & \boldsymbol{x}_{c}^{i}(t) \in \bigcap_{j\in\mathcal{I}_{c}\setminus i} \mathcal{H}_{s}^{ij}(t), \end{align}\] \(\mathcal{S}_{m}^{i}\) is the \(m\)-th corridors generated by [10], \(\boldsymbol{C}_{m}^{i}\) is the \(m\)-th Bernstein coefficients, which is split from \(\boldsymbol{C}^{i}\). \(\Xi\) is a set of points in Bernstein coefficients. Please refer to [10] for the details. Similarly to (28 ), we verify whether the control points of (31 ) belong to the affine spaces by using the convex hull property to determine if trajectories are located within regions where inter-collision does not occur. The primitives that pass the tests in (27 ) satisfy (3 ) and (7 ).

5.2.2 Visibility Check↩︎

To enable the \(i\)-th agent to see the target, we check whether the primitives satisfy (33 ), (34 ), and (35 ) to avoid occlusion by static obstacles, dynamic obstacles, and the other agents, respectively. \[\tag{32} \begin{align} \tag{33} & \Xi(\boldsymbol{C}_{m}^{i}) \subset \mathcal{S}_{m}^{i},\;\forall k=1,\ldots,M_{i},\\ \tag{34} & \|\epsilon\boldsymbol{x}_{c}^{i}(t)+(1-\epsilon)\boldsymbol{x}_{q}(t)-\boldsymbol{x}_{o}^{k}(t)\|^{2}-r_{o}^{2}\geq 0, \\ \nonumber & \forall k\in \mathcal{I}_{o},\;\forall \epsilon \in [0,1],\\ \tag{35} &\boldsymbol{x}_{c}^{i}(t)\in \bigcap_{j\in\mathcal{I}_{c}\setminus i} \big(\mathcal{H}_{\mu1}^{ij}(t) \cap \mathcal{H}_{\mu2}^{ij}(t) \big),\;\mu = o\;\text{or}\;a \end{align}\] The primitives that pass the checks in (32 ) satisfy (4 ) and (8 ). (34 ) represents a condition that Line-of-Sight does not collide with dynamic obstacles, and the inequality in (34 ) can be reformulated as (36 ). \[\begin{align} \tag{36} & \epsilon^{2}\sigma_{1}(t)+2\epsilon(1-\epsilon)\sigma_{2}(t)+(1-\epsilon)^{2}\sigma_{3}(t)\geq 0, \\ \tag{37} & \sigma_{1}(t) = \|\boldsymbol{x}_{c}(t)-\boldsymbol{x}_{o}^{k}(t)\|^{2}- (r_o+r_c)^{2},\\ \tag{38} & \sigma_{2}(t) = (\boldsymbol{x}_{c}(t)-\boldsymbol{x}_{o}^{k}(t))^{\top}(\boldsymbol{x}_{q}(t)-\boldsymbol{x}_{o}^{k}(t))\\ \nonumber &\quad \quad \quad +(r_{o}+\min(r_{q},r_{c}))^{2}-2r_{o}^{2},\\ \tag{39} & \sigma_{3}(t) = \|\boldsymbol{x}_{q}(t)-\boldsymbol{x}_{o}^{k}(t)\|^{2}-(r_o+r_q)^{2} \end{align}\] Since \(\epsilon^{2}\), \(2\epsilon(1-\epsilon)\), and \((1-\epsilon)^{2}\) are nonnegative for \(\forall \epsilon\in[0,1]\), if \(\sigma_{1}(t)\), \(\sigma_{2}(t)\), \(\sigma_{3}(t)\geq0\) for \(\forall t\in[0,T]\), (36 ) holds. Fig. 3 shows that the proposed check method is less conservative compared to our previous work [10]. By employing the less conservative feasibility check, our planner can find more feasible motions.

5.2.3 Distance Check↩︎

To keep suitable distance from the targets, the following conditions are established. \[\tag{40} \begin{align} \tag{41} & \|\boldsymbol{x}_{c}^{i}(t)-\boldsymbol{x}_{q}(t)\|^{2}-d_{\min}^{2}\geq 0, \\ \tag{42} & d_{\max}^{2}-\|\boldsymbol{x}_{c}^{i}(t)-\boldsymbol{x}_{q}(t)\|^{2}\geq 0 \end{align}\] We set \(d_{\min}\geq r_{q}+r_{c}\) to avoid collision with the target, and the primitives that pass the tests in (40 ) satisfy (5 ).

5.2.4 Dynamical Limit Check↩︎

To ensure the drone does not exceed dynamic limits (6 ), we verify whether the primitives satisfy the following inequalities. (46 ) is calculated under the assumption that the agents directly head toward the target. \[\tag{43} \begin{align} \tag{44} &v_{\max}^{2}-\|\dot{\boldsymbol{x}}_{c}^{i}(t)\|^{2} \geq 0,\\ \tag{45} &a_{\max}^{2}-\|\ddot{\boldsymbol{x}}_{c}^{i}(t)\|^{2} \geq 0,\\ \tag{46} &\dot{\psi}_{c}(t) = \frac{ \det \Big(\begin{bmatrix} (\boldsymbol{x}_{q}(t)-\boldsymbol{x}_{c}^{i}(t))^\top \\ (\dot{\boldsymbol{x}}_{q}(t)-\dot{\boldsymbol{x}}_{c}^{i}(t))^\top \end{bmatrix} \Big)}{\|\boldsymbol{x}_{q}(t)-\boldsymbol{x}_{c}^{i}(t)\|^{2}},\\ \tag{47} & -\dot{\psi}_{\max} \leq \dot{\psi}_{c}(t) \leq \dot{\psi}_{\max} \end{align}\]

5.3 Best Trajectory Selection↩︎

Among the primitives that pass (27 )-(43 ), we select the best tracking trajectory. We evaluate the following cost, which consists of a penalty term for jerkiness of trajectory and a cost term to maintain appropriate distance from the target. \[\label{eq:chasing95cost} \begin{align} \min \;&J_{1}+J_{2}, \\ &J_{1} = w_{j}\int_{0}^{T}\|\dddot{\boldsymbol{x}}_{c}^{i}(t)\|_{2}^{2}dt,\\ &J_{2} = \sum_{i=1}^{N_{q}} \int_{0}^{T}(\|\boldsymbol{x}_c^{i}(t)-\boldsymbol{x}_{q}(t)\|_{2}^{2}-d_{des,i}^{2})^{2}dt \end{align}\tag{48}\] \(w_{j}\) is a weight factor, and \(d_{des,i}\) is the desired distance between the target and the \(i\)-th agents, which is set to \(\frac{1}{2}(\underbar{r}_{cs}^{i}+\bar{r}_{cs}^{i})\) in the validation.

6 Validations↩︎

In this section, the proposed method is validated through various target-tracking settings. We measure the distance between the drone and environments, defined as (50 ), the distance among agents (51 ), and the distance between the target and drone, defined as (52 ), to evaluate drone safety. Also, we measure the distance between the Lines-of-Sight and environments (53 ) and the distance between the Lines-of-Sight and the other agents (54 ) to assess the target visibility. \[\tag{49} \begin{align} \tag{50} &\chi_{1}(t) = \min_{i\in\mathcal{I}_{c}}\min_{\substack{\boldsymbol{x}(t)\in\mathcal{C}_{i}(t)\\\boldsymbol{y}(t)\in\mathcal{O}(t)}}\|\boldsymbol{x}(t)-\boldsymbol{y}(t)\|, \\ \tag{51} & \chi_{2}(t) = \min_{i\in\mathcal{I}_{c}}\min_{\substack{\boldsymbol{x}(t)\in\mathcal{C}_{i}(t)\\\boldsymbol{y}(t)\in\mathcal{C}^{i}(t)}}\|\boldsymbol{x}(t)-\boldsymbol{y}(t)\|,\\ \tag{52} & \chi_{3}(t) = \min_{i\in\mathcal{I}_{c}}\min_{\substack{\boldsymbol{x}(t)\in\mathcal{C}_{i}(t)\\\boldsymbol{y}(t)\in\mathcal{T}(t)}}\|\boldsymbol{x}(t)-\boldsymbol{y}(t)\|,\\ \tag{53} &\phi_{1}(t)= \min_{i\in \mathcal{I}_{c}}\min_{\substack{\boldsymbol{x}(t)\in \mathcal{L}(\boldsymbol{x}_{c}^{i}(t),\boldsymbol{x}_{q}(t))\\ \boldsymbol{y}(t)\in \mathcal{O}(t)}}\|\boldsymbol{x}(t)-\boldsymbol{y}(t)\|,\\ \tag{54} &\phi_{2}(t)= \min_{i\in \mathcal{I}_{c}}\min_{\substack{\boldsymbol{x}(t)\in \mathcal{L}(\boldsymbol{x}_{c}^{i}(t),\boldsymbol{x}_{q}(t))\\ \boldsymbol{y}(t)\in \mathcal{C}^{i}(t)}}\|\boldsymbol{x}(t)-\boldsymbol{y}(t)\| \end{align}\]

Using the above performance metrics, we validate the operability of the proposed planner under two environmental conditions: 1) unstructured static obstacles, and 2) dynamic obstacles. Through simulations and hardware experiments, we show successful target tracking. Table 2 shows the reported performance, and the details of the tests are explained in the following subsections. In addition, we validate the effectiveness of the proposed method for multi-agent tracking missions through a comparative analysis.

For the target trajectory prediction, we employ a method in [10], applicable to both test conditions: environments with unstructured-but-static obstacles and dynamic-but-structured obstacles. The radii of the targets, trackers, and dynamic obstacles are set to 7.5 cm, matching the size of Crazyflie [22] quadrotors. Also, since the test environments are either narrow or crowded, we set the sampling range of the tracking distance \((\underbar{r}_{cs},\bar{r}_{cs})\) to (0.3, 0.6) [m].

In the tests, we use computers with an Intel i7 12th-gen CPU and 16GB RAM. The number of sampled primitives is set to 1000, and four threads are used for parallel computation. The reported computation time in all tracking scenarios is less than 10 milliseconds.

6.1 Simulations↩︎

Scenario 1 (Unstructured Environment): We compare the proposed planner with the state-of-the-art planner [21] in an environment with various shapes of static obstacles, as shown in Fig. 4. As Table 2 shows, the trackers controlled by the baseline [21] experience inter-agent occlusions and collisions with the target several times, despite finely tuned parameters to adapt to the narrow tracking environment. In contrast, our planner successfully tracks the target without collision and occlusion by thoroughly checking safety and visibility constraints, including the tracking distance \((d_{\min},d_{\max})\) and DIVC.

Scenario 2 (Dynamic Environment): We test our approach in a dense moving-obstacle environment. The target moves in a 6 \(\times\) 6 \(\text{m}^{2}\) space with 40 obstacles. The target and obstacles move around with the maximum speed \(1.0\;\text{m/s}\). The right side of Fig. 4 shows a flight history.

6.2 Hardware Experiments↩︎

For hardware demonstration, we use Crazyflie 2.1 quadrotors. One serves as the target, three serve as the trackers, and the remainder act as dynamic obstacles. The target and dynamic obstacles are both controlled by a single Intel NUC, and they follow a pre-calculated path generated by [23]. On the other hand, to implement distributed calculation, each tracker is connected to a separate mini PC.

Scenario 3: (Unstructured Environment) The target moves in a \(10\times7\) \(\text{m}^{2}\) space with twelve cube-shaped obstacles. The top image in Fig. 1 is a snapshot of the flight, and the left side of Fig. 5 shows the reported results.

Scenario 4: (Dynamic Environment) The target moves in a \(7\times7\) \(\text{m}^{2}\) space with five moving obstacles. The bottom image in Fig. 1 is a snapshot of the mission, and the right side of Fig. 5 summarizes the flight test.

6.3 Comparison Analysis↩︎

The performance of the proposed approach is investigated through two tests. First, we evaluate the effectiveness of the DBVC and the DIVC. Second, we compare the performance of our approach with the planner presented in [10]. In each test, we define success as the absence of occlusion or collision until all objects come to a stop. We conduct 1000 tests for each tracking setup and measure the success rate.

6.3.1 Ablation Study↩︎

We validate the effectiveness of the DBVC and the DIVC by comparing the success rates with cases where these two cells are not applied and where static versions of the cells are used. Specifically, when the cells are not used, each tracker treats the neighboring trackers as dynamic obstacles. In the case of using static cells, we use \(\mathcal{H}_{s}^{ij}\), \(\mathcal{H}_{\mu1}^{ij}\), and \(\mathcal{H}_{\mu2}^{ij}, \mu=o\) or \(a\), instead of \(\mathcal{H}_{s}^{ij}(t)\), \(\mathcal{H}_{\mu1}^{ij}(t)\), and \(\mathcal{H}_{\mu2}^{ij}(t)\).

We test these setups in an obstacle-free space, where the target moves around at the maximum speed 1.0 m/s for an average of 30 seconds. We varied the number of trackers and distance to the targets, and Table 3 summarizes the results. As the number of targets increases and the tracking distance decreases, the difficulty of tracking increases. Without using cells, the level of interference due to the movement of neighboring agents increases as the tracking distance shortens, leading to a higher failure rate. In contrast, the spatially separated characteristics of the DBVC and the DIVC implicitly make a balanced formation, resulting in a higher success rate. Also, static cells can conflict with other constraints, particularly the distance between the target and the trackers. However, the dynamic properties of the DBVC and the DIVC maintain consistent tracking distance, which results in a higher success rate. Moreover, a higher number of trackers narrows the cells, bringing the agents closer to the cell boundaries. Such conditions make it difficult to satisfy all the constraints. However, even in the short tracking distance, the planner using the DBVC and the DIVC achieves the highest success rate because the dynamic properties result in fewer constraint violations under such conditions.

The ranges in the first column mean the sampling distance \([\underbar{r}_{cs}\), \(\bar{r}_{cs}\)].
The dynamic cells denote the DBVC and the DIVC.

noncooperative: feasibility checks in [10], without the DBVC and the DIVC. conservative: feasibility checks in [10], with the DBVC and the DIVC.

6.3.2 Benchmark Test↩︎

We conduct a benchmark test to validate the superiority of our planner in a dynamic obstacle environment. We compare our method with a noncooperative approach and a conservative approach. For the noncooperative approach, we use the BPMP-Tracker [10]. Since the method in [10] is designed for single-agent tracking, we adapt the problem setting so that each tracker treats neighboring agents as obstacles. The conservative approach utilizes the DBVC and the DIVC but applies the conservative feasibility check methods from [10].

The target and obstacles move around for an average of 40 seconds in a 6 \(\times\) 6 \(\text{m}^{2}\) space at the maximum speed of 0.5 m/s. We measure the success rate while varying the number of obstacles. Our approach generates tracking trajectories in a cooperative manner, whereas the noncooperative setup causes consistent interference among trackers. This results in a higher success rate for the proposed planner for all tracking conditions, as shown in Table 4. Moreover, our approach discovers more feasible motions than the conservative approach in tight conditions, leading to a higher success rate.

As the number of trackers increases, the cells become smaller, making it difficult to find a feasible motion within those reduced areas in the presence of adjacent dynamic obstacles. In future work, we plan to design inter-occlusion- and inter-collision-free cells that not only translate but also change shape over time, to enhance robustness against the interference of dynamic obstacles.

7 Conclusion↩︎

We presented a distributed multi-agent trajectory generation method for aerial tracking, which prevents not only occlusion and collision caused by obstacles but also inter-occlusion and inter-collision among agents. Dynamic Buffered Voronoi Cells (DBVC) and Dynamic Inter-Visibility Cells (DIVC) were constructed by dividing the space into multiple cells to avoid inter-agent collision and inter-agent occlusion, respectively. Since both cells are designed based on the shared current positions of agents and the predicted target’s trajectory, it enables distributed planning. We achieved fast computation by combining the DBVC and the DIVC with the Bernstein-polynomial-motion-primitive-based trajectory planner and validated the operability of our planner through tests under various tracking conditions. Lastly, we confirmed that the proposed method achieved a higher success rate in tracking missions in environments with dynamic obstacles, outperforming the state-of-the-art method in noncooperative approaches and methods employing conservative feasibility checks.

References↩︎