**1. Introduction**

Trajectory generation is a key element for the execution of complex autonomous vehicle missions. It can be defined as the computational problem of finding a valid trajectory that guides a vehicle from an initial state to a given final state in an environment with static and/or moving obstacles. In most applications, the main concern, rather than just finding a feasible trajectory between the initial and final states, is to obtain the optimal trajectory with respect to a certain objective function. In such a setting, trajectory generation is formulated as an optimization problem with a cost function that quantifies the accomplishment of mission goals and objectives, and different types of constraints to ensure safety and feasibility of resulting trajectories.

With rapid advances in communication and computational technology, autonomous vehicles continue to take part in more complex missions, and even engage in teams of collaborating vehicles to take on increasingly demanding tasks. This necessitates incorporating intervehicle collision avoidance constraints in the optimization problem to guarantee that generated trajectories for a group of vehicles sharing a common workspace are collisionfree. Therefore, for a large group of vehicles, the optimization problem would involve a large number of constraints and decision variables, and the computational cost of solving it centrally can be prohibitively high. To reduce the computational complexity, a multitude of distributed schemes, reviewed below, have been proposed for decomposing the optimization problem into smaller sub-problems that can be solved locally by each vehicle. The major challenge is to ensure that local decisions do also satisfy the coupling collision avoidance constraints. This is mainly addressed by exchanging information among the

**Citation:** Sabetghadam, B.; Cunha, R.; Pascoal, A. A Distributed Algorithm for Real-Time Multi-Drone Collision-Free Trajectory Replanning. *Sensors* **2022**, *22*, 1855. https:// doi.org/10.3390/s22051855

Academic Editor: Sašo Blažiˇc

Received: 26 January 2022 Accepted: 24 February 2022 Published: 26 February 2022

**Publisher's Note:** MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations.

**Copyright:** © 2022 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https:// creativecommons.org/licenses/by/ 4.0/).

vehicles on their current states, future input sequences, etc. Depending on the communication strategy, the sub-problems might be solved sequentially or concurrently, with possibly several iterations of optimization and communication to achieve the required performance.

In [1], the collision avoidance constraint, usually expressed in terms of the two-norm of a relative position vector, is approximated by a set of linear constraints. The sub-problem for each vehicle is then formulated as a mixed-integer linear programming (MILP) that includes the vehicle's individual variables as well as variables of a subset of neighbors. This enables cooperation among vehicles by allowing a vehicle to make feasible perturbations to neighboring vehicles' decisions. The sub-problems are solved sequentially by each vehicle, and the algorithm iterates over the group of vehicles until convergence, during each cycle of a model predictive control (MPC) scheme.

Sequential convex programming (SCP)-based methods have also been used for solving distributed multiple vehicle trajectory generation problems [2,3]. The work in [4] addresses the infeasiblity of intermediate problems in decoupled-SCP methods, arising from convex approximation of collision avoidance constraints, i.e., linearizing them, and proposes incremental SCP (iSCP), which tightens collision constraints incrementally. Compared to sequential approaches in [5–7], that cast the trajectory of anterior vehicles as dynamic obstacles for a posterior vehicle, the methods proposed in [1,4] result in less constrained intermediate problem and faster convergence rate, yet, similar to most MPC-SCP-based methods, they would require the vehicles to exchange a full representation of their decisions to neighboring vehicles over a communication network.

The synchronous approach in [8] extends the distributed MPC (dMPC) scheme in [9] for formation control, based on alternating direction method of multipliers (ADMM), to problems with intervehicle collision avoidance constraints. These constraints are decoupled using separating hyperplanes, which enforces each vehicle to stay within one half-space of a time-varying plane over a certain time horizon. The resulting sub-problems are solved simultaneously by vehicles, while the normal vector and offset shared between a vehicle and a neighbor, for characterizing their separating hyperplane, are updated at each cycle of the dMPC, using the interchanged information about generated trajectories at the previous cycle.

In the decentralized trajectory planner proposed in [10], vehicles replan their trajectories asynchronously, independent of the planning status of other vehicles. At each iteration, a vehicle considers trajectories assigned to neighboring vehicles as constraints, and solves an optimization problem including as decision variables the normal and offset of planes that separate the outer polyhedral representation of its trajectory and those of its neighbors. A check–recheck scheme is then performed to ensure that the generated trajectory does not collide with trajectories other vehicles have committed to during the optimization time. Therefore, to guarantee deconfliction between vehicles, the planner requires a vehicle to broadcast its computed trajectory to its neighboring vehicles at the end of each replanning iteration.

The on-demand approach to local collision avoidance, proposed in [11], imposes constraints only at specific time instances when collisions between a vehicle and its neighbors are predicted. Predicting collisions along a time horizon, however, relies on an accurate knowledge of the neighbors' future actions which must be communicated at every sampling time. The dMPC scheme in [12] for distributed trajectory generation is based on this predict–avoid paradigm and an event-triggered replanning strategy, and has been shown to result in less conservative trajectories, but at the cost of voiding the collision avoidance guarantees for all time instances over the horizon. To capture the downwash effect of quadrotor's propellers, the collision avoidance constraint in [12] is modified with a diagonal scaling matrix, which approximates the quadrotor body with a translating ellipsoid elongated along the vertical axis, yet it ignores the quadrotor's rotational motion.

Reciprocal velocity obstacle (RVO) and its variants have been widely used in distributed collision avoidance [13–17]. At each time step, RVO [13] builds the set of all relative velocities, leading to a collision between a vehicle and its neighbors, and chooses a

new constant velocity outside this set, and closest to the desired value, to avoid collisions. Therefore, RVO requires the position and velocity information to be communicated, or sensed, between nearby neighbors. Other variants, such as acceleration velocity obstacle (AVO), which addresses the instantaneous change of velocity in RVO by taking into account acceleration constraints, need further information such as acceleration to be interchanged. Reciprocally-rotating velocity obstacle (RRVO) [18] uses rotation information to mitigate deadlocks caused by symmetries of representing vehicles with translating discs in RVO. It relies on the assumption that neighbors may rotate equally (or equally opposite), bounded by a maximum value, to compute an approximation of swept areas for rotating polygonshaped vehicles, and uses them for constructing velocity obstacles. A new velocity and rotation is then selected at each time step to avoid collisions.

Another approach to distributed collision avoidance is to construct the Voronoi diagram of the group of vehicles and generate the trajectory for each vehicle so that it is entirely within the vehicle's Voronoi cell [19–22]. Since Voronoi cells do not overlap, it can be guaranteed that the generated trajectories are collision-free. To consider the physical size of a vehicle, the modified Voronoi cell used in [23,24] retracts boundary hyperplanes of the general Voronoi cell by a safety radius for disc-shaped vehicles. At each sampling time, upon receiving the relative position information, trajectories are replanned to conform to the updated Voronoi diagram. The resulting sub-problems can be solved simultaneously, in a receding horizon manner, until the vehicles reach their final positions. The Voronoi-based approaches only require the vehicles to know relative positions to neighboring vehicles, and therefore are well suited to applications where vehicles only have relative position sensing and no communication network [25].

In this paper we develop a distributed trajectory generation framework, with low computation and communication demands, for multiple quadrotors flying in (relatively) close proximity to each other. We specifically address the shortcomings of approximating the drone body with a disc (or sphere) for generating feasible collision-free trajectories for large groups of drones. A sphere model, used in most existing distributed collision avoidance schemes, may be overly conservative in confined spaces since it invalidates trajectories whose feasibility depends on the consideration of the flight attitude. Instead, we model the drone body with an ellipsoid, and employ the Voronoi partitioning of space to derive local collision avoidance constraints that take into account the drone's real size and orientation. The same approach can be integrated into other distributed schemes that utilize separating hyperplanes for decoupling collision avoidance constraints. Yet the main reason for adopting Voronoi diagram is that using time-invariant boundary hyperplanes determined prior to solving a sub-problem, despite being more conservative, can significantly reduce communication and computational load, allowing for higher replanning rates. Incorporating the resulting set of constraints into sub-problems, solved by each vehicle, allows finding collision-free trajectories for guiding a group of drones through confined spaces by proper adjustment of attitude angles. In addition, the obtained set of constraints can be expressed as Bézier curves, and hence can be efficiently evaluated to guarantee that intervehicle collision avoidance requirements are met at any instant of time even over a long planning horizon.

In the proposed synchronous distributed scheme, each vehicle uses the position information of its neighbors, updated at each sampling time, and solves a sub-problem to generate its trajectory inside (a subset of) its Voronoi cell towards the closest point (in the cell) to its goal position. We present an efficient method to compute this point, which is needed to appropriately define the terminal constraint and cost in the sub-problem. A sequence of sub-problems are then solved in a receding-horizon manner until the vehicles reach their goal positions. The simulation results show that the proposed method has a higher success rate at finding collision-free trajectories for larger groups of quadrotors compared to other Voronoi diagram-based methods. In addition, it can effectively reduce the total flight time required to perform point-to-point maneuvers. Furthermore, the computation time of generating those trajectories satisfies timing constraints imposed by real-time applications.

The rest of this paper is organized as follows: In Section 2 we formulate the optimization sub-problem solved by each vehicle. In Section 2.1 we study the differentially flat system describing the drone equations of motion, and parameterize trajectories with Bézier curves. We derive the set of local collision avoidance constraints in Section 2.2, and present an efficient algorithm for finding the closest point in a Voronoi polytope to a goal position in Section 2.3. In Section 3 we obtain the continuity conditions between two adjacent Bézier curve segments, and present a method for evaluating inequalities in Bézier form without discretization. Finally, we provide simulation results in Section 4.

### **2. Problem Formulation**

The multiple vehicle trajectory generation problem addressed in this paper can be defined as finding optimal trajectories that act as references to guide a group of vehicles from their initial positions to some desired final positions. The generated trajectories should jointly minimize a cost function, corresponding to the accomplishment of mission goals and objectives, and satisfy a set of local and coupling constraints, so that they are dynamically-feasible and collision-free. For *Nv* vehicles, this problem can be formulated as the following optimal control problem.

$$\begin{aligned} \min\_{\mathbf{u}\_{i}(\cdot)} & \sum\_{i \in [N\_{v}]} J(\mathbf{x}\_{i}(\cdot), \mathbf{u}\_{i}(\cdot)) \\ \text{s.t.} & \quad \dot{\mathbf{x}}\_{i}(t) = f(\mathbf{x}\_{i}(t), \mathbf{u}\_{i}(t)) \\ & \mathbf{x}\_{i}(0) = \mathbf{x}\_{i,0} & \text{(Initial state)} \\ & \mathbf{x}\_{i}(t\_{f}) = \mathbf{x}\_{i,f} & \text{(Final state)} \\ & c(\mathbf{x}\_{i}(t), \mathbf{x}\_{i}(t)) \le 0 \quad j \in [N\_{v}] \cup \{i\} & \text{(collision covariance)} \\ & \mathbf{x}\_{i}(t) \in \mathcal{X}\_{i} & \text{(state constraints)} \\ & \mathbf{u}\_{i}(t) \in \mathcal{U}\_{i} & \text{(Input constraints)} \end{aligned}$$

where [*Nv*] = {1, ... , *Nv*}. The cost to be minimized is the sum of the vehicles' individual costs, *J*, given by the functional,

$$J[\mathbf{u}\_i(.)] = \int\_0^{t\_f} L(\mathbf{x}\_{i\prime}\mathbf{u}\_i)dt\tag{2}$$

where **<sup>x</sup>***i*(*t*) <sup>∈</sup> <sup>R</sup>*nx* and **<sup>u</sup>***i*(*t*) <sup>∈</sup> <sup>R</sup>*nu* are the state and the input vectors of the vehicle's model described by an ODE, and **x***i*,0 and **x***i*, *<sup>f</sup>* are the initial and final values of the state of the *i*-th vehicle, respectively. X*<sup>i</sup>* and U*<sup>i</sup>* denote the set of admissible states and inputs for the *i*-th vehicle derived from limits imposed by vehicle dynamics and the surrounding environment.

In order to reduce the computational complexity of solving (1) for larger *Nv* with increased numbers of constraints and variables, one can divide the problem into a set of small-scale sub-problems. Here, the sub-problems are formulated such that each involves only a vehicle's individual costs and constraints, and hence can be solved independently by the vehicle. The sub-problems must include constraints to ensure that the trajectory generated locally by a vehicle does satisfy the coupling collision avoidance constraints.

The key idea to ensure intervehicle collision avoidance is to decompose the space into non-overlapping regions, provided by a Voronoi diagram, and generate the trajectory for each vehicle such that it is entirely within its partition. The Voronoi diagram is updated at each sampling time according to the relative positions of vehicles, and a sequence of sub-problems is solved in a receding horizon manner until the vehicles reach their final positions. For the *i*-th vehicle, the problem that has to be solved at the time instant *tk* can be formulated as

$$\begin{array}{ll}\min\_{\mathbf{x}\_{i,k}(.),\mathbf{u}\_{i,k}(.)} J(\mathbf{x}\_{i,k}(.),\mathbf{u}\_{i,k}(.)) & \text{(3)}\\ \text{s.t.} & \dot{\mathbf{x}}\_{i,k}(t) = f(\mathbf{x}\_{i,k}(t),\mathbf{u}\_{i,k}(t)) & \text{(Dynamic)}\\ & \mathbf{x}\_{i,k}(t\_k) = \dot{\mathbf{x}}\_{i,k} & \text{(Initial state)}\\ & \mathbf{x}\_{i,k}(t) \in \mathcal{C}\_{i,k}(\dot{\mathbf{x}}\_{k}) & \text{(collision covariance)}\\ & \mathbf{x}\_{i,k}(t) \in \mathcal{U}\_{i,k} & \text{(State constraints)}\\ & \mathbf{u}\_{i,k}(t) \in \mathcal{U}\_{i,k} & \text{(Input constraints)}\\\end{array}$$

where **x***i*,*k*(*t*) and **u***i*,*k*(*t*) are the state and the input profiles of the vehicle over the time interval [*tk*, *tk* + *th*], with *th* being the planning horizon, and **xˆ***i*,*<sup>k</sup>* denotes its state at the time instant *tk*. The cost function in the above sub-problem is modified as

$$J[\mathbf{u}\_{i,k}(.)] = \int\_{t\_k}^{t\_k + t\_h} L(\mathbf{x}\_{i,k}, \mathbf{u}\_{i,k}) dt + \phi(\mathbf{x}\_{i,k}(t\_k + t\_h)) \tag{4}$$

where the second term is added to penalize the distance, at *tk* + *th*, to the point in the Voronoi partition that is closest to the goal position.

In the optimization problem (3), C*i*,*<sup>k</sup>* may denote the Voronoi partition assigned to the *i*-th vehicle. The Voronoi diagram is updated for each sub-problem according to the vehicles' configuration at each time instant *tk*, i.e., **<sup>x</sup>**¯ *<sup>k</sup>* = {**xˆ***j*,*k*}*j*∈[*Nv*]. Since Voronoi partitions are disjoint and the assigned trajectory to each vehicle for the time horizon *th* is contained within its partition, it can be guaranteed that there is no collision between the trajectories over the time interval [*tk*, *tk* + *th*].

The distributed trajectory generation framework is summarized in Algorithm 1. In Section 2.2, we study the Voronoi diagram for a group of vehicles and modify C*i*,*<sup>k</sup>* to explicitly take into account the orientation while generating collision-free trajectories for multiple drones.

### **Algorithm 1** Distributed Trajectory Generation Framework

1: *k* = 0

2: **x**ˆ*i*,0 ← Initial position of the *i*-th vehicle


9: Set the constraints -Sections 2.2 and 3.1

10: Solve the optimization sub-problem

*2.1. Quadrotor Model*

In this paper, the simplified quadrotor equations of motion are described by

$$m\ddot{\mathbf{p}} = m\mathbf{g}\mathbf{e}\_3 + f\_{\prime} \tag{5}$$

where **<sup>p</sup>** <sup>∈</sup> <sup>R</sup><sup>3</sup> is the position and *<sup>m</sup>* is the mass of the quadrotor. In addition, *<sup>g</sup>* <sup>=</sup> 9.8 <sup>m</sup> s2 is the gravitational acceleration, and **e**<sup>3</sup> = [001] *<sup>T</sup>*. The first term on the right-hand side of (5) is gravity in the **<sup>z</sup>***<sup>I</sup>* direction, and the second term, *<sup>f</sup>* <sup>∈</sup> <sup>R</sup>3, is the thrust force aligned with the body's **z**-axis.

$$f = -T^I \mathbf{z}\_B \tag{6}$$

where *<sup>T</sup>* <sup>∈</sup> <sup>R</sup> is the net thrust, *<sup>I</sup>* **z***<sup>B</sup>* = *RB***z***<sup>B</sup>* = *R***e**<sup>3</sup> is the body frame **z**-axis expressed in {*I*}, and *<sup>R</sup>* <sup>≡</sup> *<sup>I</sup> <sup>B</sup>R* ∈ *SO*(3) is the rotation matrix from the body frame {*B*}, centered at the

<sup>3:</sup> **repeat**

<sup>11:</sup> **until x**ˆ*i*,*<sup>k</sup>* = **x***i*, *<sup>f</sup>* .

quadrotor's center of gravity, to the fixed inertial frame {*I*}. For simplicity, we drop the superscript *<sup>I</sup>* and consider **z***<sup>B</sup>* = *<sup>I</sup>* **z***B*. Figure 1 is a graphical representation of the quadrotor and the associated reference frames.

**Figure 1.** The quadrotor reference frames.

#### Trajectory Parametrization

The quadrotor dynamics (5) with the four inputs is differentially flat [26], and therefore the state and the input of the system can be expressed as functions of the flat outputs and a finite number of its derivatives. The position vector together with the yaw angle can be selected as flat outputs of the system. Here, **<sup>p</sup>** <sup>∈</sup> <sup>R</sup><sup>3</sup> is parameterized as a Bézier curve, given by

$$\mathbf{p}(\boldsymbol{\pi}) = \sum\_{l=0}^{n} p\_l B\_{l,n}(\boldsymbol{\pi}),\tag{7}$$

where *<sup>p</sup>*¯*<sup>l</sup>* <sup>∈</sup> <sup>R</sup><sup>3</sup> are the control points, *Bl*,*<sup>n</sup>* are Bernstein basis polynomials of degree *<sup>n</sup>*, and *τ* ∈ [0, 1] is defined as

$$
\pi = \frac{t}{t\_f} \tag{8}
$$

The linear velocity, **v** = **p**˙ , and linear acceleration, **a** = **p**¨, can be expressed as parametric Bézier curves through the first and second derivative of **p** with respect to time, yielding

$$\mathbf{v}(\tau) = \sum\_{l=0}^{n-1} \vartheta\_l B\_{l,n-1}(\tau)$$

$$\mathbf{a}(\tau) = \sum\_{l=0}^{n-2} \bar{a}\_l B\_{l,n-2}(\tau) \tag{9}$$

where the control points *v*¯*<sup>l</sup>* and *a*¯*<sup>l</sup>* are obtained as

$$\begin{aligned} \bar{v}\_l &= \frac{n}{t\_f} (\bar{p}\_{l+1} - \bar{p}\_l) & l &= 0, \dots, n-1 \\ \bar{a}\_l &= \frac{n(n-1)}{t\_f^2} (\bar{p}\_{l+2} - 2\bar{p}\_{l+1} + \bar{p}\_l) & l &= 0, \dots, n-2 \end{aligned} \tag{10}$$

The thrust *T* and rotation matrix *R* can also be expressed as functions of the flat output and its derivatives. The net thrust *T* can be written as

$$T = m \|\|\mathbf{\tilde{p}} - \mathbf{g}\mathbf{e\_3}\|.\tag{11}$$

Assuming that the rotation matrix *R* = [**x***B*, **y***B*, **z***B*] is parameterized by the *Z*-*Y*-*X* Euler angles *λ* = [*φ*, *θ*, *ψ*] *<sup>T</sup>* as

$$R = R\_z(\psi)R\_y(\theta)R\_x(\phi),\tag{12}$$

then the columns of the rotation matrix are extracted from

$$\mathbf{z}\_B = \frac{\mathbf{g}\mathbf{e}\mathbf{s} - \ddot{\mathbf{p}}}{||\mathbf{g}\mathbf{e}\_3 - \ddot{\mathbf{p}}||} \quad \mathbf{x}\_B = \frac{\mathbf{r} \times \mathbf{z}\_B}{||\mathbf{r} \times \mathbf{z}\_B||} \quad \mathbf{y}\_B = \mathbf{z}\_B \times \mathbf{x}\_B \tag{13}$$

where the unit vector **r** is defined as

$$\mathbf{r} = [-\sin\psi, \cos\psi, 0]^T \tag{14}$$

The above equations declare that the vehicle's orientation can be fully determined from the second derivative of the trajectory and the yaw angle. As mentioned before, the yaw angle *ψ* is a component of the flat output, and therefore it can be controlled independently without affecting the trajectory generation. Using the differential flatness property of the system, trajectories consistent with dynamics can be planned in the space of flat outputs, where (5) is trivially satisfied and the original input and state constraints are transformed into constraints on the flat output and its derivatives.

### *2.2. Collision Avoidance*

In this section, we present a Voronoi diagram-based approach to decoupling intervehicle collision avoidance constraints. Although the presence of obstacles, interpreted as non-decision-making agents, is not explicitly considered here, incorporating vehicle– obstacle collision avoidance constraints into the problem simply amounts to taking into account the obstacles' position when updating the Voronoi partition (step 6 of Algorithm 1).

The widely used approach in the literature to avoiding collisions with obstacles in the environment is to model the drone body as a sphere with radius *r***D**, and then simply building the collision-free space, C*f ree*, by inflating the obstacles with a factor *r***D**. As a result, collision-free trajectories can be obtained by enforcing the vehicle, which is now treated as a point in the space, to be inside C*f ree* [27]. Considering now the collision avoidance between the *i*-th and *j*-th drones, the corresponding constraint can be derived similarly by

$$\|\mathbf{p}\_i - \mathbf{p}\_j\| \ge 2\mathbf{r}\mathbf{p} \tag{15}$$

where . denotes the Euclidean distance. Ignoring the real shape and orientation of the drone, and approximating its body with a sphere, invalidates trajectories that are feasible upon considering the flight attitude. For this reason, the above approach might be too conservative for trajectory generation in confined spaces and can even fail to find feasible collision-free trajectories when multiple drones are involved.

Approximating the drone body with an ellipsoid, whose principal axes are aligned with the body frame axes, allows considering the drone orientation while inspecting for collisions against other vehicles. For the *i*-th drone, the ellipsoid, *Ei*, centered at the drone position **p***i*, is given by

$$E\_i \equiv \{ \mathbf{p} \in \mathbb{R}^3 | \mathbf{p} = \mathbf{p}\_i + R\_i \Lambda \mathbf{w}\_i \, \| \mathbf{w} \| \le 1 \} \tag{16}$$

where Λ is a 3 × 3 diagonal matrix of the form

$$
\Lambda = \begin{bmatrix} r\_\mathbf{D} & 0 & 0 \\ 0 & r\_\mathbf{D} & 0 \\ 0 & 0 & h\_\mathbf{D} \end{bmatrix} \tag{17}
$$

with *r***<sup>D</sup>** and *h***<sup>D</sup>** being the lengths of the principle semi-major and semi-minor axes, respectively. (See Figure 2).

**Figure 2.** The quadrotor body can be represented as a sphere with radius *r***<sup>D</sup>** (**right**), or an ellipsoid aligned with the axes of the body frame (**left**). Approximating the drone body with an ellipsoid allows considering the quadrotor's rotational motion.

As proposed in [28], collision avoidance constraints for two ellipsoid-shaped drones can be derived using separating hyperplanes. Denoting by **<sup>a</sup>** <sup>∈</sup> <sup>R</sup><sup>3</sup> and *<sup>b</sup>* <sup>∈</sup> <sup>R</sup> the normal vector and offset of a hyperplane, respectively, the separating hyperplane for *Ei* and *Ej*, defined as *<sup>H</sup>* ≡ {**p**|**a***T***<sup>p</sup>** <sup>−</sup> *<sup>b</sup>* <sup>=</sup> <sup>0</sup>}, must satisfy

$$\begin{aligned} \mathbf{a}^T \mathbf{p} - b \le 0 \quad \forall \mathbf{p} \in E\_i\\ \mathbf{a}^T \mathbf{p} - b > 0 \quad \forall \mathbf{p} \in E\_j \end{aligned} \tag{18}$$

Since

$$-\left\|\boldsymbol{\Lambda}\boldsymbol{\Lambda}^{\boldsymbol{T}}\mathbf{a}\right\|\leq\mathbf{a}^{\boldsymbol{T}}\boldsymbol{R}\boldsymbol{\Lambda}\mathbf{w}\leq\left\|\boldsymbol{\Lambda}\boldsymbol{R}^{\boldsymbol{T}}\mathbf{a}\right\|\tag{19}$$

The set of inequalities (18) holds if, and only if,

$$\begin{array}{ll} \mathbf{a}^T \mathbf{p}\_i - b \ge & \|\Lambda R\_i^T \mathbf{a}\| \\ \mathbf{a}^T \mathbf{p}\_j - b \le & -\|\Lambda R\_j^T \mathbf{a}\| \end{array} \tag{20}$$

Satisfying the set of constraints (20) will guarantee that there is no collision between the two ellipsoids *Ei* and *Ej* associated with the *i*-th and *j*-th drones, respectively.

For multiple vehicle trajectory generation, collision avoidance constraints, either in the form of the inequality constraint (15), for spheres, or the set of constraints (20), for ellipsoids, must be incorporated in the optimization problem for each pair of vehicles. As the number of vehicles involved in a mission grows, the resulting increase in the number of constraints would inevitably exacerbate the computational issues of finding collision-free trajectories in a centralized manner.

#### Distributed Collision Avoidance

Here, we propose a distributed approach to collision avoidance which takes into account the shape and orientation of a drone. The presented approach uses Voronoi partitioning of space and generates the trajectory of each vehicle such that it is entirely within (a subset of) the vehicle's Voronoi cell for a time interval *th*. Since Voronoi cells are pairwise disjoint, and each vehicle only moves inside its Voronoi cell, intervehicle collision avoidance is guaranteed for all future time before *th*.

Each Voronoi cell in an n-dimensional space is a convex polytope bounded by a number of (*n* − 1)-dimensional convex polytopes. For a group of vehicles in three-dimensional space, the general Voronoi cell of the *i*-th vehicle is defined as

$$\mathcal{V}\_{i} = \{ \mathbf{p} \in \mathbb{R}^{3} | \mathbf{p}\_{i\bar{j}}^{T} (\mathbf{p} - \frac{1}{2} (\mathbf{p}\_{i} + \mathbf{p}\_{\bar{j}})) \le 0, \forall j \in [N\_{v}] \backslash \{i\} \}\tag{21}$$

where **p***ij* = **p***<sup>j</sup>* − **p***i*, and **p***<sup>i</sup>* and **p***<sup>j</sup>* are the position of the *i*-th and *j*-th vehicles at the current time instant. Note that V*<sup>i</sup>* is the intersection of half-spaces corresponding to hyperplanes with *a* = **p***ij* and *b* = <sup>1</sup> 2**p***<sup>T</sup> ij*(**p***<sup>i</sup>* + **p***j*). An arbitrary point in V*<sup>i</sup>* is closer to the *i*-th vehicle than any other vehicle [22], i.e.,

$$\|\mathbf{p} - \mathbf{p}\_i\| \le \|\mathbf{p} - \mathbf{p}\_j\|, \quad \forall \mathbf{p} \in \mathcal{V}\_i \quad \& \quad j \ne i \tag{22}$$

The boundary of the Voronoi cell, *∂*V*i*, is the union of multiple faces, each of which include points in the space that are equidistant to the *i*-th vehicle and a neighboring vehicle.

In order to account for the size of a vehicle, the buffered Voronoi cell (BVC) proposed in [24] retracts the boundary of the general Voronoi cell by a safety radius, so that if the vehicle's center is inside the BVC, its body, approximated by a sphere of radius *r***D**, will be entirely within its Voronoi cell. The BVC of the *<sup>i</sup>*-th vehicle, denoted by <sup>V</sup>¯ *<sup>i</sup>*, is defined as

$$\dot{\mathcal{W}}\_{i} = \{ \mathbf{p} \in \mathbb{R}^{3} | \quad \mathbf{p}\_{ij}^{T} (\mathbf{p} - \frac{1}{2} (\mathbf{p}\_{j} + \mathbf{p}\_{i})) + r\_{\mathbf{D}} \| \mathbf{p}\_{ij} \| \le 0, \forall j \in [N\_{v}] \backslash \{i\} \}\tag{23}$$

It can be easily shown that BVC is a subset of the general Voronoi cell, i.e., <sup>V</sup>¯ *<sup>i</sup>* ⊂ V*i*. In addition, for any two points **<sup>p</sup>** <sup>∈</sup> <sup>V</sup>¯ *<sup>i</sup>* and **<sup>q</sup>** <sup>∈</sup> <sup>V</sup>¯*j*, **<sup>p</sup>** <sup>−</sup> **<sup>q</sup>**  ≥ 2*r***<sup>D</sup>** holds. Therefore, the vehicles are guaranteed to avoid collisions due to the buffer region of *r***<sup>D</sup>** along *∂*V*i*. Figure 3 shows the Voronoi diagram for 10 drones in a collision-free configuration and the BVC for two adjacent drones.

**Figure 3.** (**a**) The Voronoi diagram for six drones in 2D space. The Voronoi boundary edges are shown with solid black lines, and the buffered Voronoi cells are shaded in dark blue. (**b**) The Voronoi diagram for 10 drones in a collision-free configuration in 3D space. The Voronoi boundary *∂*V is shaded in light blue, and the buffered Voronoi cells <sup>V</sup>¯ for two neighboring drones in the center are shown in dark blue.

The BVC is defined based on a symmetrical approximation of the vehicle's body with a translating disc. In order to reduce the conservatism and avoid infeasibility issues due to ignoring the real shape and orientation of the vehicle, we approximate the drone with an ellipsoid (16), and bearing in mind that

$$R\Lambda^2 R^T = r\_\mathbf{D}^2 I + (h\_\mathbf{D}^2 - r\_\mathbf{D}^2) \mathbf{z}\_B \mathbf{z}\_{B'}^T \tag{24}$$

we propose C*<sup>i</sup>* in problem (3) to be defined as

$$\mathcal{L}\_{i} = \left\{ (\mathbf{p}\_{\prime}\ddot{\mathbf{p}}) \in \mathbb{R}^{6} | \mathbf{p}\_{ij}^{T} (\mathbf{p} - \frac{1}{2} (\mathbf{p}\_{j} + \mathbf{p}\_{i})) + \|\Lambda \boldsymbol{\Lambda}^{T} \mathbf{p}\_{ij}\| \le 0, \forall j \in [N\_{\mathbb{D}}] \, | \, \{i\} \right\} \tag{25}$$

If the trajectory of the *i*-th drone **p***i*(*t*) satisfies the above set of local collision avoidance constraints for all *t* ∈ [*tk*, *tk* + *th*], then the ellipsoid representing the drone body is within the Voronoi cell for the entire time horizon; that is, the ellipsoid centered at **p***<sup>i</sup>* and aligned with the columns of *Ri* does not intersect the Voronoi boundary, stated mathematically

$$\|\|\partial E\_i - \partial \mathcal{V}\_i\|\| \ge 0 \tag{26}$$

Noting that

$$\|h\_{\mathbf{D}}\| \|\mathbf{p}\_{ij}\| \le \|\Lambda \boldsymbol{R}^T \mathbf{p}\_{ij}\| \le r\_{\mathbf{D}} \|\mathbf{p}\_{ij}\|\_{\mathsf{M}} \tag{27}$$

it can be induced that

$$\bar{\mathcal{V}}\_{i}(r\_{\mathbf{D}}) \subset \text{proj}\_{XYZ} \mathcal{C}\_{i} \subset \bar{\mathcal{V}}\_{i}(h\_{\mathbf{D}}) \subset \mathcal{V}\_{i} \tag{28}$$

where proj*XYZ*C*<sup>i</sup>* is the projection of C*<sup>i</sup>* onto the three-dimensional subspace spanned by **e**1, **e**2, and **e**3.

Therefore, incorporating (25) into the optimization problem (3) will ensure that the generated trajectories are collision-free while alleviating infeasibility problems by taking orientations into account. Also, since **z***<sup>B</sup>* is fully obtained from **p**¨ (13), the above set of local collision avoidance constraints can be expressed as constraints imposed on Bézier curves. Later, we present an efficient method for evaluating inequalities in Bézier form.

#### *2.3. Finding the Closest Point to the Goal Position*

As explained above, at each time instant the Voronoi cell V*<sup>i</sup>* is updated according to the relative position of the *i*-th vehicle to other vehicles. The optimization problem (3) is then solved to generate a trajectory, for a time horizon *th*, that guides the vehicle towards the point in the cell closest to the goal position. This process is repeated until the vehicle reaches its final position. At each sampling time, the closest point must be found prior to solving the trajectory generation problem. Therefore, having an efficient scheme for finding the closest point is critically important to avoid long computational delays between updating the Voronoi cell and replanning the trajectory.

The point in a convex polytope that is closest to a query point **q** is either **q** itself or a point on the boundary of the polytope. A naive way to find the closest point in a convex polytope in a three-dimensional space, represented by *P* = (F,E, V), where F is the set of faces, E is the set of edges, and V is the set of vertices, is to check the distance between **<sup>q</sup>** <sup>∈</sup> <sup>R</sup><sup>3</sup> to all faces, edges, and vertices for finding the minimum. However, for complex polytopes, the computation time is not negligible.

The geometric approach proposed in [24] for a polygon in a two-dimensional space calculates the barycentric coordinates and an angle from the query point to the two vertices of each edge to find the closest point. Since this approach iterates over all edges, its computational complexity can significantly increase as the number of Voronoi neighbors of a vehicle increases. Here, we make use of the Gilbert–Johnson–Keerthi (GJK) distance algorithm and devise an approach that can efficiently determine whether the query point is inside the polytope, i.e., **q** ∈ *P*, in which case the closest point is **q** itself. Otherwise, the presented algorithm returns the closest feature of *P* to **q**, and the closest point can be obtained by projecting **q** onto it. The proposed approach is not limited to distance queries between a point and a polytope, and can also be used when the final constraint in (3) is relaxed to a small box or sphere around the goal position, and used in conjunction with a terminal cost term.

The GJK distance algorithm or simply GJK algorithm is an iterative algorithm that relies on a support mapping function to incrementally build simplices that are closer to the query point [29]. The algorithm has been extensively used for collision detection between generic convex shapes [30,31]. The original algorithm, however, can be used to compute the minimum distance, and also the respective pair of (closest) points, between two convex shapes [32].

In order to obtain the minimum distance between two general convex sets *A* and *B*, GJK approximates the closest point in the Minkowski difference of the two sets, *C* = *A* − *B* to the origin, denoted by *ν*(*C*), with an iterative search. At each iteration, a simplex in *C* is constructed such that it is closer to the origin O than the simplex in the previous iteration. In R3, a simplex can be a point, a line, a triangle, or a tetrahedron with 1, 2, 3, and 4 affinely independent vertices.

GJK relies on the so-called support mappings to construct a new simplex. A support mapping function *sC*(**d**) of the convex set *C* maps the vector **d** to a point in the set, called the support point, according to

$$\mathbf{d}^T s\_\mathbb{C}(\mathbf{d}) = \max \{ \mathbf{d}^T \mathbf{p}; \mathbf{p} \in \mathbb{C} \}\tag{29}$$

At each iteration, a support point **w***<sup>k</sup>* = *sC*(−*νk*) is added as a vertex to the current simplex, indicated with *Vk*, where *ν<sup>k</sup>* is the closest point of *Vk* to the origin, i.e., *ν<sup>k</sup>* = *ν*(conv(*Vk*)). *Vk*<sup>+</sup><sup>1</sup> is then updated such that it only contains the smallest set of vertices that supports *νk*+<sup>1</sup> = *ν*(conv(*Vk* ∪ **w***k*)), and earlier vertices that do not back *νk*+<sup>1</sup> are discarded [30].

It is proved in [30] that in each iteration the new *ν* is closer to the origin than the previous one, and thus the sequence of {*νk*} converges to the closest point of *C* to the origin. In addition, it is shown that

$$\|\boldsymbol{\nu}\_{k} - \boldsymbol{\nu}(\mathbf{C})\|^{2} \leq \|\boldsymbol{\nu}\_{k}\|^{2} - \boldsymbol{\nu}\_{k}^{T}\mathbf{w}\_{k} \tag{30}$$

which is used to construct the terminating condition of the GJK algorithm for general convex shapes.

The GJK algorithm, as explained above, depends heavily on the computation of *ν<sup>k</sup>* to test the termination condition and to determine the search direction for finding the support point. In each iteration of the algorithm, *ν<sup>k</sup>* must be computed with the *Johnson Distance Subalgorithm* [29] or more robust alternatives such as the signed volume method in [33]. Here, we exploit unique features of polytopes and propose a faster way to evolve simplices in the GJK algorithm without computing *ν<sup>k</sup>* in each iteration.

For polytopes, GJK arrives at the actual *ν*(*C*) in a finite number of iterations [30]. The pseudocode in Algorithm 2 describes the GJK distance algorithm for a polygon *P*. In order to find the support point **w***k*, we employ a search direction **d***<sup>k</sup>* ↑↓ *νk*, which is updated in each iteration of the algorithm with S1D, S2D, or S3D subroutines. To update **d** and *V*, these subroutines, summarized in Algorithms 3–5, inspect the Voronoi regions of the simplex for the one that contains the origin. Figure 4 shows the Voronoi regions of an m-simplex (*m* = 1, 2, 3) where the origin possibly lies. Once the Voronoi region containing the origin is found, **d** is determined as a vector from the associated vertex, edge, or face (of the simplex) pointing towards the origin.

The stop criterion for the conditional loop in Algorithm 1 is also constructed using the search direction, offered as

$$\mathbf{d}\_k^T(\mathbf{w}\_k - \mathbf{v}\_1) \le 0 \tag{31}$$

where **<sup>v</sup>**<sup>1</sup> <sup>∈</sup> *Vk*. Considering that **<sup>d</sup>***<sup>T</sup> <sup>k</sup>* (*ν<sup>k</sup>* − **v**1) = 0, we can conclude that the above criterion, for deciding whether *Vk* represents the closest feature of *P* to the origin, is equivalent to the stop criterion in [30] for determining whether *ν<sup>k</sup>* s the closest point, that is

$$\|\boldsymbol{\nu}\_{k}\|^{2} - \boldsymbol{\nu}\_{k}^{T}\mathbf{w}\_{k} \le 0 \longleftrightarrow \mathbf{d}\_{k}^{T}(\mathbf{w}\_{k} - \mathbf{v}\_{1}) \le 0 \tag{32}$$

If the inequality (31) holds, then the closest feature of *P* to the origin can be determined from *Vk*. Figure 5 shows all possible outputs of Algorithm 2, which can be a vertex, an edge, or a face of *P* or a tetrahedron inside it, and *ν*(*P*) for each of the cases.

**Algorithm 2** Compute the closest point of *P* to the origin

1: **v** = "Arbitrary point in vert(*P*)" 2: **d** = −**v** 3: *V* = {**v**} 4: **repeat** 5: **w** = *sP*(**d**); 6: **if d***T*(**<sup>w</sup>** <sup>−</sup> **<sup>v</sup>**1) <sup>≤</sup> <sup>0</sup> **then** 7: *V* represents the closest feature of *P* to O 8: **return** *ν*(*V*) 9: **end if** 10: *V* ← *V* ∪ **w**; 11: [*V*, **d**] ← CallSmD(*V*); 1 12: **until** |*V*| = 4; 13: *P* contains O 14: **return** *ν* = O <sup>1</sup> One of the three subroutines S1D, S2D or S3D is called in accordance with <sup>|</sup>*V*|.

A support point of a convex polytope can also be computed efficiently. For a polytope *P*, the support point is a vertex of *P*, i.e., *sP*(**d**) ∈ vert(*P*), and we can take **w** = *sP*(**d**) = *s*vert(*P*)(**d**), that is,

$$\mathbf{d}^T s\_P(\mathbf{d}) = \max \{ \mathbf{d}^T \mathbf{v}; \mathbf{v} \in \text{vert}P\} \tag{33}$$

Therefore, for polytopes, the support point can be uniquely determined by simply scanning through the list of vertices for the vertex that is the most extreme in the search direction **d**. Therefore, the computation time is linear in the number of vertices of *P*. For complex polytopes, the vertices adjacency information and the coherence between consecutive calls to support mapping functions can be exploited to find the support point with almost constant time complexity [30].

**Algorithm 3** Sub-routine for |*V*| = 2

1: **function** S1D({**v**2, **<sup>v</sup>**1}) <sup>1</sup> 2: **if v***<sup>T</sup>* <sup>1</sup> **v**<sup>12</sup> ≥ 0 **then** 3: *V* ← {**v**1} 4: **d** ← −**v**<sup>1</sup> 5: **else** 6: *V* ← {**v**2, **v**1} 7: **d** ← −**v**<sup>12</sup> × **v**<sup>1</sup> × **v**<sup>12</sup> 8: **end if** 9: **end function**

<sup>1</sup> The input is the ordered list of vertices, with **v**<sup>1</sup> being the last added element to *Vk*.

**Figure 4.** An m-simplex is linked to 2*m*+<sup>1</sup> <sup>−</sup> 1 Voronoi regions associated with its vertices, edges, faces, and volume. The list of 2*<sup>m</sup>* Voronoi regions that can possibly contain the origin is given in this table. It should be noted that **v**<sup>1</sup> is the latest vertex added to *V*.

**Algorithm 4** Sub-routine for |*V*| = 3

1: **function** S2D ({**v**3, **v**2, **v**1}) 2: **nv**3**v**2**v**<sup>1</sup> = **v**<sup>12</sup> × **v**<sup>13</sup> 3: **if v***<sup>T</sup>* <sup>1</sup> (**nv**3**v**2**v**<sup>1</sup> × **v**12) ≥ 0 **then** 4: [*V*, **d**] ← S1D({**v**2, **v**1}) 5: **else** 6: **if v1** *<sup>T</sup>*(**v**<sup>13</sup> <sup>×</sup> **nv**3**v**2**v**<sup>1</sup> ) <sup>≥</sup> <sup>0</sup> **then** 7: [*V*, **d**] ← S1D({**v**3, **v**1}) 8: **else** 9: **if v***<sup>T</sup>* <sup>1</sup> **nv**3**v**2**v**<sup>1</sup> ≥ 0 **then** 10: *V* ← {**v**3, **v**2, **v**1} 11: **d** ← −**nv**3**v**2**v**<sup>1</sup> 12: **else** 13: *V* ← {**v**3, **v**2, **v**1} 14: **d** ← **nv**3**v**2**v**<sup>1</sup> 15: **end if** 16: **end if** 17: **end if** 18: **end function**

```
Algorithm 5 Sub-routine for |V| = 4
```

```
1: function S3D({v4, v3, v2, v1})
2: nv3v2v1 = v12 × v13
3: if (vT
         1 nv3v2v1 )(vT
                  14nv3v2v1 ) ≥ 0 then
4: [V, d] ← S2D({v3, v2, v1})
5: else
6: nv4v3v1 = v13 × v14
7: if (vT
            1 nv4v3v1 )(vT
                      12nv4v3v1) ≥ 0 then
8: [V, d] ← S2D({v4, v3, v1})
9: else
10: nv4v2v1 = v12 × v14
11: if (vT
               1 nv4v2v1 )(vT
                        13nv4v2v1 ) ≥ 0 then
12: [V, d] ← S2D({v4, v2, v1})
13: else
14: V ← {v4, v3, v2, v1}
15: end if
16: end if
17: end if
18: end function
```
#### **3. Bézier Curves**
