*3.1. Continuity Conditions*

As explained before, at each sampling time, a trajectory, expressed as a parametric Bézier curve, is generated for the time horizon [*tk*, *tk* + *th*], and the trajectory for the entire flight time [0, *T*] is formed by joining segments of these Bézier curves end-to-end. The smoothness of the resulting composite trajectory must be guaranteed by enforcing continuity at the joining points of two consecutive segments up to a certain derivative. In the following, in order to derive conditions that address parameter continuity between consecutive curves, we assume that the time horizon is equal to Δ*tk* = *tk*<sup>+</sup><sup>1</sup> − *tk*, which is

not necessarily the same for all sub-problems. In practice, however, the time horizon is greater than Δ*tk*, in which case a Bézier curve describing the segment over the time interval [*tk*, *tk*<sup>+</sup>1] can be obtained by subdividing **p***k*(.) at *tk*<sup>+</sup><sup>1</sup> with the de Casteljau's algorithm. For simplicity we drop the subscript *i* ∈ [*Nv*].

The Bézier curve describing the trajectory over the time interval [*tk*, *tk*<sup>+</sup>1] is defined as

$$\mathbf{p}\_k(\tau\_k) = \sum\_{l=0}^{n\_k} p\_{l,k} B\_{l,n\_k}(\tau\_k) \tag{34}$$

Assuming that the global parameter *t* runs over the interval [*tk*, *tk*<sup>+</sup>1], the local parameter *τ<sup>k</sup>* is related to *t* by

$$0 \le \tau\_k = \frac{t - t\_k}{t\_{k+1} - t\_k} \le 1 \tag{35}$$

The parametric continuity condition, *C*<sup>r</sup> , requires the r-th derivative and all lower derivatives of two consecutive segments to be equal at the joining point. In other words,

$$\frac{d^r \mathbf{p}\_k(1)}{dt^r} = \frac{d^r \mathbf{p}\_{k+1}(0)}{dt^r} \quad r \in \{0, \dots, r\} \tag{36}$$

Zero-order parametric continuity, *C*0, requires the endpoints of two consecutive curves, **p***k*(.) and **p***k*+1(.), to intersect at one endpoint, that is,

$$\mathbf{p}\_k(1) = \mathbf{p}\_{k+1}(0) \tag{37}$$

Since a Bézier curve is coincident with its control points at the two ends, i.e.,

$$\mathbf{p}\_k(0) = \vec{p}\_{0,k} \quad \mathbf{p}\_k(1) = \vec{p}\_{n\_k k'} \tag{38}$$

the position continuity condition (37) translates into

$$
\phi\_{n\_k k} = \phi\_{0, k+1} \tag{39}
$$

The first-order parametric continuity condition, *C*1, for the *k*-th and *k* + 1-th Bézier curves, can be obtained as

$$\begin{aligned} \Delta t\_{k+1} n\_k (\vec{p}\_{n\_k,k} - \vec{p}\_{n\_k - 1,k}) &= \\ \Delta t\_k n\_{k+1} (\vec{p}\_{1,k+1} - \vec{p}\_{0,k+1}) \end{aligned} \tag{40}$$

Finally, the *k*-th and *k* + 1-th Bézier curves are *C*2-continuous if

$$\begin{aligned} \frac{\Delta t\_{k+1}}{\Delta t\_k} (\vec{p}\_{n\_k - 1, k} - \vec{p}\_{n\_k - 2, k}) \\ &+ n\_k (n\_{k+1} - 1) \vec{p}\_{n\_k - 1, k} + n\_k \vec{p}\_{n\_k, k} = \\ \frac{\Delta t\_k}{\Delta t\_{k+1}} (\vec{p}\_{1, k+1} - \vec{p}\_{2, k+1}) \\ &+ n\_{k+1} (n\_k - 1) \vec{p}\_{1, k+1} + n\_{k+1} \vec{p}\_{0, k+1} \\ \end{aligned} \tag{41}$$

Higher-order parametric continuity conditions can be obtained likewise.

#### *3.2. Evaluating Inequalities in Bézier Form*

Parameterizing the trajectory with a Bézier curve converts the original infinite dimensional problem (3) into a semi-infinite optimization problem with a finite number of decision variables and an infinite number of constraints. The commonly used approach to obtaining a standard optimization problem is time gridding, which inspects satisfaction of constraints on a finite number of points only. Although this method is straightforward, it cannot guarantee that constraints are satisfied over the entire time interval. Using fine discretization can remedy this issue, but, it will increase the number of constraints as well as the computation time. Since all constraints involved in the trajectory generation problem addressed above can be expressed as Bézier curves, we can employ the method proposed in [33] to recast the semi-infinite optimization problem into one that is computationally tractable. As explained below this method exploits unique features of Bézier curves to efficiently evaluate constraints while avoiding problems associated with time gridding.

If *h*(*τ*) can be expressed as a Bézier curve, then any inequality constraint of the form *h*(*τ*) ≤ 0, *τ* ∈ [0, 1] can be replaced by a finite set of constraints on its control points. More specifically, if *h*(*τ*) is defined as

$$h(\boldsymbol{\pi}) = \sum\_{l=0}^{n\_h} \bar{h}\_l B\_{l, n\_h}(\boldsymbol{\pi}),\tag{42}$$

then from the *convex hull* property of Bézier curves we know that

$$h(\pi) \in \mathcal{CH}(\bar{H}) \quad \pi \in [0, 1] \tag{43}$$

where *CH*(*H*¯ ) = {*α*<sup>0</sup> ¯ *h*<sup>0</sup> + ··· + *αnh* ¯ *hnh* |*α*<sup>0</sup> + ... , *αnh* = 1, *α<sup>l</sup>* ≥ 0} is the convex hull defined by the set of control points [34]. Thus, the inequality constraint *h*(*τ*) ≤ 0 holds if

$$
\bar{h}\_l \le 0 \quad \text{for} \quad l = 0, \dots, n\_{\text{fl}}.\tag{44}
$$

This finite set of inequality constraints can ensure that the original inequality constraint is satisfied over the entire interval [0, 1]. However, Ineqs. (44) might be conservative due to the existing gap between the control points ¯ *hl* and the actual curve *h*(*τ*). This problem can be alleviated by refining the control polygon and finding closer control points to the curve using recursive subdivision of *h*(*τ*) with the de Casteljau's algorithm. The sequence of control polygons generated with repeated subdivision converges to the underlying Bézier curve [35]. Figure 6 shows a threefold subdivision of a cubic Bézier curve. Furthermore, the de Casteljau's algortithm allows refining the control polygon locally. Using recursive subdivsion of *h*(*τ*) to reduce the conservatism in the finite set of constraints results in an increase in the number of constraints; hence, a trade-off has to be made between the computational effort and the conservatism. Nevertheless, the optimization variables remain the same [36].

**Figure 6.** A cubic Bézier curve (**top left**) is subdivided into two Bézier curves of the same degree (**top right**) using the de Casteljau's algorithm. The control polygon generated by recursive subdivision converges to the original Bézier curve ref. [28]. Copyright 2021 IEEE. Successive refinement of the original control polygon after 2 (**bottom left**) and 3 (**bottom right**) subdivisions.

#### **4. Simulation Results**

In this section, the efficacy of the proposed method for generating feasible and collisionfree trajectories in (vehicle-) dense environments are assessed through different simulation examples. We compare the resulting trajectories to those generated with the well-studied BVC approach. We specifically test the capability of the two methods to generate trajectories that ensure all drones involved in a simulation example reach their final positions, and compare the flight time, obtained with each of them, to complete point-to-point transition missions. We also present the recorded computation time for executing the proposed algorithm in this paper to emphasize its suitability for real-time applications.

In the simulations presented below, we assume all drones have the same size, and their BVC (23) is defined with the safety radius *r***<sup>D</sup>** = 0.30 m. To specify the set (25), we approximate the drone body with an oblate spheroid with Λ = diag([0.30 m, 0.30 m, 0.11 m]). In both methods, trajectories are parameterized with Bézier curves. Upper and lower bounds on the speed and acceleration are assumed to be <sup>±</sup>2.3 <sup>m</sup> <sup>s</sup> and <sup>±</sup>7.1 <sup>m</sup> s2 respectively. At each replanning step, the planner finds the closest point in the updated Voronoi cell to the goal position using the algorithm in Section 2.3. The computed point is then used to define the terminal cost term. The first term of the cost function in all subproblems is defined as 1 <sup>0</sup> **<sup>p</sup>**(4) *<sup>i</sup>*,*<sup>k</sup>* (*τ*)<sup>2</sup>*dτ*. The time horizon and the replanning step are also considered to be the same for both methods. The obtained solution at the previous replanning step is used to set the initial guess for the current sub-problem. We use FORCES Pro [37] to generate solvers for the resulting sub-problems. The sub-problems, involving the set of control points *p*¯*i*,*<sup>k</sup>* as decision variables, can be reformulated to match the supported classes of problem in FORCES Pro. Here, all computations are executed on a single desktop computer, with 2.60 GHz i7-4510U CPU and 6.00 GB RAM; however, in practice, the resulting independent sub-problems can be solved in parallel.

As mentioned before in the paper, in Voronoi-based methods, a vehicle only requires the position information from its neighboring vehicles to generate its trajectory. Therefore, they are more suitable for implementation when vehicles have limited communication capability, and have to rely solely on onboard sensing. In reality, the position sensor noise can impact the planner performance, yet this is more pronounced when estimating other information, such as velocity, from noise-corrupted measurements is needed. Therefore, Voronoi-based planners are more robust when there is no communication network. Nevertheless, in the following simulations, we assume that accurate position information is available with no delay at the replanning time.

In the first example, we consider five drones flying from their initial positions to given final positions. This example is similar to one in [24] where a random offset is added to break the symmetry in the drones' initial and final configurations. Figure 7 (right) shows collision-free trajectories generated with the distributed scheme described above, with a replanning rate of 20 Hz. For this particular example, the resulting trajectories match those generated with BVC with a flight time of 11.6782 s. Figure 7 (left) shows collision-free trajectories obtained from the centralized solution, which delivers a total flight time of 9.4347 s, yet, while the central solution is obtained in 601 ms, the average computation time for solving the sub-problems in the decentralized scheme is only 49 ms.

In the next example, we consider 18 drones switching positions in a 3 m × 5 m <sup>×</sup> 2 m space, with a maximum speed and acceleration of <sup>±</sup>4.7 <sup>m</sup> <sup>s</sup> and <sup>±</sup>9.8 <sup>m</sup> s2 , respectively. Figure 8a shows the initial and final configurations, and Figure 8b displays collision-free trajectories generated with the proposed distributed scheme in the paper implemented at 10 Hz. While both methods could find collision-free trajectories for guiding the team of drones from their initial positions to their goal positions, the flight time achieved with the proposed method is markedly shorter than the time obtained with BVC. We also performed a trial simulation with 34 drones in a similar configuration. Table 1 compares the success rate and the flight time to complete the transition using BVC and the proposed method.

In the third example, we consider 100 drones flying in an 8 m × 8 m × 3.5 m space. The initial and final positions for the drones are displayed with dot and square markers

in Figure 9. We test both methods in 30 different trials. In each trial, final positions are randomly assigned to drones. A trial is considered successful if all drones could reach their final positions within the stipulated time. The proposed method with 23 successful trials and an average total flight of 1*s* outperforms the BVC with only 16 completed trials. It should be noted that using well-devised deadlock prevention strategies or loosening time constraints can improve the success rate of both methods. Figure 9 shows collision-free trajectories generated with the proposed method for one of the trials at different time steps. The average computation time for solving sub-problems in this example was around 115 milliseconds. In addition, compared to the geometric algorithm in [24], the closest point in a Voronoi cell to the goal position was obtained at least 10 times faster with the proposed algorithm in Section 2.3. The computation time for finding the closest point, and solving the optimization problem, depends on the number of neighboring drones (See Table 2 for recorded computation times in simulation examples with 18, 34, and 100 drones). In most applications, with typical Voronoi diagrams, the number of boundary planes, i.e., the number of Voronoi neighbors, is small. Thus, the proposed distributed algorithm is scalable to arbitrary numbers of vehicles.

**Figure 7.** Comparing collision-free trajectories generated with the centralized solution (**left**) and the proposed decentralized approach (**right**) for five drones flying from their initial positions to given final positions. While the central solution yields a shorter flight time, its computation time is significantly longer than average time required to solve the sub-problems in the distributed method.

**Figure 8.** (**a**) Initial (left) and final (right) position configurations for 18 drones. Each drone is assigned a unique color and a number next to it. (**b**) Collision−free trajectories for 18 drones switching their positions in a 3 m × 5 m × 2 m space. The total flight time for the drones to reach their final positions is 5.1 s using the proposed method, which is shorter than the 6.3 s flight time obtained with the BVC.

**Figure 9.** Collision−free trajectories for 100 drones flying from their initial positions (dots) to randomly specified final positions (squares) at different replanning steps.

**Table 1.** Comparing the number of successful trials and the average flight time achieved with the BVC and the proposed method in the paper.


**Table 2.** Recorded computation times for finding the closest point in a Voronoi cell to the goal position and solving the optimization problem in simulation examples with 18, 34, and 100 drones.


#### **5. Conclusions**

In this paper, we introduce an efficient distributed algorithm for generating collisionfree trajectories for multiple drones, taking into account their orientation. In order to avoid substantial communication between drones, we adopt Voronoi-based space partitioning and derive local constraints that guarantee collision avoidance with neighboring vehicles for an entire time horizon. We leverage Bézier curve properties to ensure that the set of collision avoidance constraints are satisfied at any time instant of the planning horizon. The same approach can be employed to obtain local collision avoidance constraints for the cases where the normal vector and offset of separating planes are time-varying parameters or decision variables of sub-problems. Yet, adopting Voronoi diagram with fixed planes for an entire planning horizon, though being conservative, results in simple, small sub-problems allowing for the trajectories to be replanned at a higher rate. We present different simulation results to highlight the scalability of the algorithm to large numbers of drones, and also its capability to generate less conservative trajectories with notably shorter flight times, compared to other Voronoi-based methods.

Our future work includes implementation and experimental validation of the algorithm for teams of drones. As we explain in the paper, at each time sample, upon receiving (or sensing) the new position information, a vehicle must find the closest point in its Voronoi cell to the goal position, and solve an optimization problem that uses the current state of the vehicle as the initial condition, to generate its trajectory for a certain time horizon. Although the time to compute the closest point is mainly negligible, the computation time to find the optimal solution can lead to a (significant) delay between updating the position information and executing the trajectory. Therefore, in practice, the computational delay must be explicitly considered to avoid performance degradation (or even failure) of the planner.

**Author Contributions:** Conceptualization and writing—original draft preparation, B.S.; Supervision and writing—review and editing, R.C. and A.P. All authors have read and agreed to the published version of the manuscript.

**Funding:** This work was partially funded by Fundação para a CiênciaeaTecnologia and FEDER funds through the projects UIDB/50009/2020 and LISBOA-01-0145-FEDER-031411. B. Sabetghadam acknowledges the support of Instituto Superior Técnico through scholarship BL216/2018/IST-ID.

**Institutional Review Board Statement:** Not applicable.

**Informed Consent Statement:** Not applicable.

**Data Availability Statement:** Not applicable.

**Conflicts of Interest:** The authors declare no conflict of interest.
