*4.4. Collision Detection*

In some cases it may be desirable to quickly check the feasibility of a trajectory rather than finding a minimum distance. The collision detection algorithm can be used in these cases. The two major differences between the Collision Detection Algorithm and the Minimum Distance Algorithm described previously are a modification of the GJK algorithm and a change in the stopping criteria. Rather than having the GJK algorithm return a minimum distance, it simply returns whether a collision has been detected (i.e., convex hulls intersecting). The stopping criteria is set to return the moment no collisions are found rather than continuing iterations to meet a desired tolerance. For example, if the original convex hulls of two Bernstein polynomials do not intersect, the collision detection algorithm will return *no collision* after the first iteration while the minimum distance algorithm will continue to iterate until the desired tolerance is met. Therefore, this algorithm is computationally inexpensive compared to the minimum distance algorithm, with the drawback that it only returns a binary value (no collision or collision possible) rather than a minimum distance.

The collision detection algorithm is shown in Algorithm 3. The inputs are the coefficients of the Bernstein polynomials being compared, P and Q, and the maximum number of iterations *max*\_*iter*. The *while* loop beginning on line 2 runs until it is determined that a collision does not exist or until the maximum number of iterations is met. The find\_collisions() function on line 3 uses the modified GJK algorithm to determine which convex hulls from the set P collide with those from the set Q. The *if* statement on line 4 checks to see whether collisions were found. If both P*col* and Q*col* are empty, then no collisions exist. If collisions do exist then the *for* loops starting on lines 7 and 11 split all the convex hulls that were found to collide and add them to the set to be checked. Note that the parent set that is split is removed from the set of convex hulls to check. If the maximum number of iterations is met, then the algorithm returns that a collision is possible. The execution time when a collision is possible is 1.10 ms on a Lenovo ThinkPad laptop using an Intel Core i7-8550U, 1.80 GHz CPU. However, when a collision does not exist, the execution time is only 7.25 μs.

```
Algorithm 3: Collision Detection
  Input:P, Q, max_iter
1 k = 0
2 while k < max_iter do
3 Pcol, Qcol = find_collisions(P, Q)
4 if Pcol ∪ Qcol = {} then
5 return No Collision
6 end
7 for Pi ∈ Pcol do
8 P A,PB = split(Pi)
9 P = P ∪ {P A,PB}\Pi
10 end
11 for Qi ∈ Qcol do
12 QA, QB = split(Qi)
13 Q = Q ∪ {QA, QB}\Qi
14 end
15 k + +
16 end
17 return Collision Possible
```
#### *4.5. Penetration Algorithm*

If two convex shapes intersect, in order to derive information such as the penetration depth and vector, the EPA [46] can be used. A slight modification of the EPA algorithm is proposed here, which is referred to as the DEPA, whose objective is to find the penetration of one convex shape relative to another along a specific direction −→*<sup>d</sup>* . The top left plot of Figure 20 shows two shapes intersecting each other, and the remaining plots show examples of penetration vectors, i.e., the vector −→*<sup>d</sup>* needed to move the second shape so that it no longer intersects the first shape. The DEPA algorithm finds the shortest possible penetration vector. The pseudocode is reported below (see Algorithm 4).

**Figure 20.** Illustration of penetration.

**Algorithm 4:** Directed Extended Polytope Algorithm

```
Data: −→d
  Data: MinkowskiDi f f erence
  Data: simplex
1 ContainsOrigin(simplex);
2 A, B ← InterectingEdge(simplex, Ray(
                                       −→0 , −→d ));
3 Loop
4 −→n ← TripleProduct(
                          −→AB,
                              −→A ,
                                 −→AB);
5 C ← Support(MinkowskiDi f f erence,
                                          −→n );
6 if Parallel(
               −→C ,
                   −→d ) then
7 −vec
          → ← −→C ;
8 return;
9 else if Equals(C,B) OR Equals(C,A) then
10 break;
11 else if Intersection(Ray(0,
                              −→d ), CA) then
12 B ← C;
13 else if Intersection(Ray(0,
                              −→d ), CB) then
14 A ← C;
15 −vec
      → ←Intersection(Ray(0, −→d ), AB);
16 EndLoop
```
Figure 21 demonstrates the algorithm in 4 steps. The first plot shows the Minkowski Difference of the shapes depicted in Figure 22, which contains the origin and with a triangle simplex that contains the origin. This is the desired direction to move a polytope A (blue polytope) of Figure 22 such that it no longer contains polytope B (beige polytope). Once the norm of the point along the edge of the Minkowski Difference parallel to −→*<sup>d</sup>* is found, A can then move in the same direction with the same length to no longer intersect B.

**Figure 21.** Iteration of the DEPA algorithm.

**Figure 22.** Two intersecting polygons and resulting Minkowski Difference.

#### **5. Numerical Examples**

In this section, numerical examples using the BeBOT toolkit and Python's Scipy Optimization package are examined (flight tests are available at [47]). The implementation of the following examples can be found in [40].

#### *5.1. Dubins Car—Time Optimal*

In this simple example, several trajectories for a vehicle with Dubins car dynamics are generated to illustrate the properties of Bernstein polynomials. We let the desired trajectory assigned to the vehicle be given by the Bernstein polynomial

$$\begin{bmatrix} \mathbf{C}\_{n}^{[x]}(t) \\ \mathbf{C}\_{n}^{[y]}(t) \end{bmatrix} = \mathbf{C}\_{n}(t) = \sum\_{i=0}^{n} \mathbf{P}\_{i,n} B\_{i,n}(t), \quad t \in [t\_{0}, t\_{f}].\tag{11}$$

The square of the speed of the vehicle is a 1D Bernstein polynomial given by

$$v^2(t) = ||\mathbf{C}\_n(t)||^2.$$

The heading angle is

$$\psi(t) = \tan^{-1} \frac{\dot{\mathbb{C}}\_n^{[y]}(t)}{\dot{\mathbb{C}}\_n^{[x]}(t)},\tag{12}$$

and the angular rate is a 1D rational Bernstein polynomial given by

$$
\omega(t) = \frac{\mathsf{C}\_{n}^{\lfloor y \rfloor}(t)\mathsf{C}\_{n}^{\lfloor x \rfloor}(t) - \mathsf{C}\_{n}^{\lfloor y \rfloor}(t)\mathsf{C}\_{n}^{\lfloor x \rfloor}(t)}{||\dot{\mathsf{C}}\_{n}(t)||^{2}}. \tag{13}
$$

The objective at hand is to find a trajectory that arrives at a desired destination in the minimum possible time while adhering to feasibility and safety constraints. In particular, the trajectory generation problem is as follows:

$$\min\_{\mathbf{P}\_n, t\_f} t\_f$$

subject to

$$\begin{split} \mathbf{C}\_{\mathfrak{n}}(t\_{0}) &= \mathbf{C}\_{0} \quad \mathbf{C}\_{\mathfrak{n}}(t\_{f}) = \mathbf{C}\_{f}, \\ \psi(t\_{0}) &= \psi\_{0} \quad \psi(t\_{f}) = \psi\_{f} \\ ||\mathbf{C}\_{\mathfrak{n}}(t\_{0})|| &= \upsilon\_{0} \quad ||\mathbf{C}\_{\mathfrak{n}}(t\_{f})|| = \upsilon\_{f}, \\ ||\mathbf{C}\_{\mathfrak{n}}(t)||^{2} &\leq \upsilon\_{\max}^{2} \quad \forall t \in [t\_{0}, t\_{f}] \\ ||\dot{\psi}(t)|| &\leq \omega\_{\max}, \quad \forall t \in [t\_{0}, t\_{f}] \\ ||\mathbf{C}\_{\mathfrak{n}}(t) - \mathbf{O}\_{i}||^{2} &\geq d\_{s}^{2}, \quad \forall t \in [t\_{0}, t\_{f}], \quad i = 1, 2. \end{split}$$

We set the initial and final position, heading, and speed to **C**<sup>0</sup> = [3, 0] m, **C***<sup>f</sup>* = [7, 10] m, *ψ*<sup>0</sup> = *ψ<sup>f</sup>* = *<sup>π</sup>* <sup>2</sup> rad, and *<sup>v</sup>*<sup>0</sup> <sup>=</sup> *vf* <sup>=</sup> <sup>1</sup> <sup>m</sup> <sup>s</sup> . The maximum speed, maximum angular rate, and minimum safe distance constraints are *v*max = 5 <sup>m</sup> <sup>s</sup> , *<sup>ω</sup>*max <sup>=</sup> <sup>1</sup>rad <sup>s</sup> , and *<sup>d</sup>*<sup>2</sup> *<sup>s</sup>* = 1 m, respectively. The positions of the obstacles are **O**<sup>1</sup> = [3, 2] m and **O**<sup>2</sup> = [6, 7] m.

In the problem above, the initial and final constraints for position, heading, and speed are enforced using the End Point Values property (Property A2) together with Equations (A2), (12) and (13). Similarly, the same property is used to enforce the initial and final speeds and headings (see (A2)). Note that the norm squared of the speed and of the distance between the trajectory and the obstacles can be expressed as Bernstein polynomials (the sum, the difference, and the product between Bernstein polynomials are also Bernstein polynomials). A similar argument can be made for the norm square of the angular rate, which can be expressed as a rational Bernstein polynomial (see Property A7). Thus, the maximum speed and angular rate, and collision avoidance constraints can be enforced using the Evaluating Bounds or Evaluating Extrema procedures described in Sections 4.1 and 4.2.

Figure 23 shows the results with *n* = 10. The blue curve is obtained by enforcing the constraints using the Evaluating Bounds procedure. The optimal time of arrival at the final destination is *tf* = 9.14 s. Next, we solve the same problem by enforcing the constraints using the Evaluating Bound procedure together with Degree Elevation. Recall that by degree elevating a Bernstein polynomial, the Bernstein coefficients converge towards the curve. Thus, degree elevation can be used to enforce constraints with tighter bounds. The orange and green lines show the trajectories obtained using degree elevations of 30 and 100, respectively. Degree elevation to degree 30 results in an optimal final time *tf* = 7.64 s. The elevation to degree 100 provides an optimal value *tf* = 7.12 s. Finally, the trajectory with smallest optimal final time, *tf* = 6.45 s, depicted as the red curve in

Figure 23, is obtained by enforcing the constraints using the Evaluating Extrema algorithm (Section 4.2). While higher degree elevations or evaluating the exact extrema can produce more optimal trajectories, that optimality comes at the cost of additional computation time. Using a Lenovo Thinkpad P52s with an Intel Core i7-8550U CPU with a 1.8 GHz clock and 8 GB of memory, the computation time required for no degree elevation, a degree elevation of 30, a degree elevation of 100, and the exact extrema algorithm was 0.105 s, 0.146 s, 0.201 s, and 0.573 s, respectively.

Figure 24 illustrates the squared speed of each example. Figure 25 shows the angular rate of each trial. It can be seen that the vehicle correctly adheres to the speed and angular rate constraints for each trial with the only differences being the final time and proximity to the obstacles.

**Figure 23.** Time optimal trajectory for vehicle with initial and final speeds and headings, maximum speed, maximum angular rate, and maximum safe distance constraints ranging from least to most conservative distance estimates.

**Figure 24.** Plot of the squared speed constraints for each separate trial.

**Figure 25.** Plot of the angular rate constraints for each separate trial.

**Remark 2.** *The Exact Extrema function is a complex non-linear and non-smooth function. When it is used to enforce constraints, gradient-based optimization solvers such as the one used in this work can fail to converge to a feasible solution, especially if the initial guess is not feasible. One option is to use an iterative procedure where (i) a feasible sub-optimal solution is obtained by enforcing the collision avoidance constraint using the Evaluating Bounds function, and (ii) this solution is then used as an initial guess to solve the (more accurate) problem with the Exact Extrema constraint.*

#### *5.2. Air Traffic Control—Time Optimal*

In this example, we consider the problem of routing several commercial flights between major US cities in two dimensions (i.e., constant altitude). Assuming that each flight departs at the same time, the goal is to minimize the combined flight time of all the vehicles. Let the position, speed, heading, and angular rate of each vehicle under consideration be parameterized as in Section 5.1. We shall also make the assumption that the trajectories are on a 2D plane rather than on the surface of the Earth.

The goal is to compute cumulatively time optimal trajectories subject to maximum speed and angular velocity bounds, initial and final position, angle, and speeds. The vehicles must also maintain a minimum safe distance between each other. This problem can be formulated as follows:

$$\min\_{\mathbf{P}\_m, \mathbf{t}\_f} \sum\_{k=1}^m t\_f^{[k]}$$

subject to

$$\begin{split} & \mathbf{C}\_{n}^{[k]}(0) = \mathbf{C}\_{0}^{[k]}, \quad \mathbf{C}\_{n}^{[k]}\left(t\_{f}^{[k]}\right) = \mathbf{C}\_{f}^{[k]}, \\ & \psi^{[k]}(0) = \psi\_{0}^{[k]}, \quad \psi^{[k]}\left(t\_{f}^{[k]}\right) = \psi\_{f}^{[k]}, \\ & ||\mathbf{C}\_{n}^{[k]}(0)|| = v\_{0}^{[k]}, \quad ||\mathbf{C}\_{n}^{[k]}\left(t\_{f}^{[k]}\right)|| = v\_{f}^{[k]}, \\ & v\_{\min}^{2} \le ||\mathbf{C}\_{n}^{[k]}(t)||^{2} \le v\_{\max}^{2} \quad \forall t \in [0, t\_{f}^{[k]}], \\ & ||\psi^{[k]}(t)|| \le \omega\_{\max} \quad \forall t \in [0, t\_{f}^{[k]}], \\ & ||\mathbf{C}\_{n}^{i}(t) - \mathbf{C}\_{n}^{j}(t)||^{2} \ge d\_{s}^{2}, \quad \forall i, j \in \{1, \ldots, m\}, i \ne j. \end{split}$$

where the superscript [*k*] corresponds to the *<sup>k</sup>*th vehicle out of *<sup>m</sup>* vehicles, **<sup>C</sup>**[*k*] <sup>0</sup> and **<sup>C</sup>**[*k*] *<sup>f</sup>* are the initial and final positions, *ψ*[*k*] <sup>0</sup> and *<sup>ψ</sup>*[*k*] *<sup>f</sup>* are the initial and final headings, *v* [*k*] <sup>0</sup> and *v* [*k*] *<sup>f</sup>* are the initial and final speeds, *v*min and *v*max are the minimum and maximum speeds, *ω*max is the maximum angular velocity, *ds* is the minimum safe distance, and *t* [*k*] *<sup>f</sup>* is the final time of the *k*th vehicle.

The departure cities, in vehicle order, are: San Diego, New York, Minneapolis, and Seattle. The arrival cities, in vehicle order, are: Minneapolis, Seattle, Miami, and Denver. The initial and final speeds are all *v* [*k*] <sup>0</sup> = *v* [*k*] *<sup>f</sup>* <sup>=</sup> <sup>205</sup> <sup>m</sup> <sup>s</sup> ∀*k* ∈ {1, ... , *m*}, the initial headings are *ψ*<sup>0</sup> = [0, *π*, 0, 0] rad, the final headings are *<sup>ψ</sup><sup>f</sup>* = [0, *<sup>π</sup>*, <sup>−</sup>*<sup>π</sup>* <sup>2</sup> , 0] rad, the minimum speed is *v*min = 200 <sup>m</sup> <sup>s</sup> , the maximum speed is *<sup>v</sup>*max <sup>=</sup> <sup>260</sup> <sup>m</sup> <sup>s</sup> , the maximum angular velocity is *<sup>ω</sup>*max = 3 deg <sup>s</sup> <sup>=</sup> 0.0524rad <sup>s</sup> , the minimum safe distance is *ds* = 5 km, and the degree of the Bernstein polynomials being used is 5.

The initial and final position constraints are enforced using the End Point Values property (Property A2). Similarly, the same property is used to enforce the initial and final speeds and headings (see (A2)). Note that the norm square of the speed and the norm square of the distance between vehicles can be expressed as 1D Bernstein polynomials (the sum, difference, and product between Bernstein polynomials are also Bernstein polynomials). A similar argument can be made for the norm square of the angular rate, which can be expressed as a rational Bernstein polynomial (see Property A7). Thus, the maximum speed and angular rate, and collision avoidance constraints can be enforced using the Evaluating Bounds or Evaluating Extrema procedures described in Sections 4.1 and 4.2.

The optimized flight plans can be seen in Figure 26. The squared speed of each vehicle is shown in Figure 27. Note that each vehicle begins and ends with the same speed. The vehicles never slow down less than their initial speeds which means they never reach the minimum speed constrain, nor do the vehicles go faster than the maximum speed. In Figure 28, the angular velocity of each vehicle is shown. The minimum and maximum angular rate constraints are shown by the dotted lines. The vehicles' angular rates never approach the minimum or maximum angular rate constraints due to the large area being covered. Finally, the squared euclidean distance between vehicles is shown in Figure 29. As expected, the squared Euclidean distance between two vehicles never falls below the minimum safe distance. Note that curves within the constraint plots end at different times. This is expected since each vehicle has a different final time. The furthest time reached in Figure 29 is less than that of the other plots because the other vehicles have already reached their final time before the longest flight reaches its final time.

**Figure 26.** Commercial flight trajectories between major US cities.

**Figure 27.** Verifying speed constraints for the Air Traffic Control example.

**Figure 28.** Verifying angular rate constraints for the Air Traffic Control example.

**Figure 29.** Verifying minimum safe distance constraints for the Air Traffic Control example.

#### *5.3. Cluttered Environment*

In many real world scenarios, robots must safely traverse cluttered environments. In this example, three aerial vehicles traveling at a constant altitude must navigate around several obstacles while also adhering to dynamic and minimum safe distance constraints. Let the position, speed, heading angle, and angular rate of each vehicle be defined as in Section 5.1. The goal of this example is to compute trajectories whose arc length is minimized subject to maximum speed constraints along with initial and final positions, heading angles, and speeds. The vehicles should also adhere to a minimum safe distance between each other and between obstacles. We formulate the problem as follows:

$$\min\_{\mathbf{P}\_n} \sum\_{i=1}^m \sum\_{k=0}^{n-1} ||\mathbf{P}\_{k+1,n}^{[i]} - \mathbf{P}\_{k,n}^{[i]}|| \tag{14}$$

subject to

$$\begin{split} \mathbf{C}\_{n}^{[k]}(0) &= \mathbf{C}\_{0}^{[k]}, \quad \mathbf{C}\_{n}^{[k]}(t\_{f}) = \mathbf{C}\_{f}^{[k]}, \\ \Psi^{[k]}(0) &= \Psi\_{0}^{[k]}, \quad \Psi^{[k]}(t\_{f}) = \Psi\_{f}^{[k]}, \\ ||\dot{\mathbf{C}}\_{n}^{[k]}(0)|| &= v\_{0}^{[k]}, \quad ||\dot{\mathbf{C}}\_{n}^{[k]}(t\_{f})|| = v\_{f}^{[k]}, \\ ||\dot{\mathbf{C}}\_{n}^{[k]}(t)||^{2} &\leq v\_{\text{max}}^{2}, \quad \forall t \in [0, t\_{f}], \\ ||\mathbf{C}\_{n}^{[i]}(t) - \mathbf{C}\_{n}^{[j]}(t)||^{2} &\geq d\_{s}^{2}, \quad \forall i, j \in \{1, \dots, m\}, i \neq j, \\ ||\mathbf{C}\_{n}^{[j]}(t) - \mathbf{O}\_{j}||^{2} &\geq d\_{\text{abs}}^{2}, \quad \forall t \in [0, t\_{f}], \quad i \in \{1, \dots, m\}, \\ j &\in \{1, \dots, b\}. \end{split}$$

where **O***<sup>j</sup>* is the position of the *j*th obstacle out of *b* obstacles.

The initial positions for each vehicle, in order, are [0, 0] m, [10, 0] m, and [20, 0] m. The initial speeds are all 1 <sup>m</sup> <sup>s</sup> and the initial heading angles are all *<sup>π</sup>* <sup>2</sup> rad. The final positions for each vehicle are, in order, [20, 30] m, [0, 30] m, and [10, 30] m. The final speeds and final heading angles are the same as the initial speeds and heading angles. The order of the Bernstein polynomials being used is 7, the final time is *tf* = 30 s, the minimum safe distance between vehicles is *ds* = 1 m, the minimum safe distance between vehicles and obstacles is *dobs* = 2 m, and the maximum speed is *v*max = 10 <sup>m</sup> <sup>s</sup> . The vehicles traversing the cluttered environment can be seen in Figure 30. This experiment has been repeated in the Cooperative Autonomous Systems (CAS) lab using three AR Drones 2.0. The flight tests can be viewed at [47].

**Figure 30.** Aerial vehicles navigating a cluttered environment.

#### *5.4. Vehicle Overtake*

Here we consider an autonomous driving example in which one vehicle attempts to overtake another vehicle while driving around a 90◦ corner. The corner is defined by two arcs with a center point located at [140, 0] m. The inner track has a radius of *rinner* = 125 m and the outer track has a radius of *router* = 140 m. To clearly distinguish the vehicle being overtaken, it will be referred to as the opponent.

For simplicity, we consider the objective of minimizing the arc-length of the trajectory, which can be done by minimizing the sum of the squared Euclidean norm of consecutive control points, i.e.,

$$E(\mathbf{P}\_n) = \sum\_{i=1}^n ||\mathbf{P}\_{i,n} - \mathbf{P}\_{i-1,n}||^2. \tag{15}$$

The desired endpoint of the vehicle is at the end of the corner. This is computed by measuring the angle between the vehicle's position and the end of the curve,

$$A(\mathbf{P}\_n) = \left( \arctan2\left(\mathbf{P}\_{n,n}^{[y]} - \mathbf{q}^{[y]}, \mathbf{P}\_{n,n}^{[x]} - \mathbf{q}^{[x]}\right) - \frac{\pi}{2}\right)^2,\tag{16}$$

where the function arctan 2 returns an angle in the correct quadrant [48]. Given Equations (15) and (16), we formulate the problem as

$$\min\_{\mathbf{P}\_n} ((1 - \boldsymbol{\kappa})E(\mathbf{P}\_n) + \boldsymbol{\kappa}A(\mathbf{P}\_n)) \boldsymbol{\beta} \tag{17}$$

subject to

$$\begin{aligned} \mathbf{C}\_{n}(0) &= \mathbf{C}\_{0}, \quad \psi(0) = \psi\_{0}, \quad ||\dot{\mathbf{C}}\_{n}(0)|| = v\_{0}, \\ ||\dot{\mathbf{C}}\_{n}(t)||^{2} &\leq v\_{\text{max}}^{2}, \quad \forall t \in [0, t\_{f}], \\ |\dot{\psi}(t)| &\leq \omega\_{\text{max}}, \quad \forall t \in [0, t\_{f}], \\ r\_{inner}^{2} &\leq ||\mathbf{C}\_{n}(t) - \mathbf{q}||^{2} \leq r\_{outer}^{2}, \\ ||\mathbf{C}\_{n}(t) - \mathbf{O}\_{n}(t)||^{2} &\geq d\_{s}^{2} \end{aligned}$$

where *α* and *β* are tuning parameters. **C**0, *ψ*0, and *v*<sup>0</sup> are the initial position, heading, and speed of the vehicle, respectively. *v*max and *ω*max are the maximum speed and angular rate, respectively. The predicted trajectory of the opponent is represented as the Bernstein polynomial **O***n*(*t*) and the minimum safe distance to the opponent is *ds*. Using a sensor such as a camera or LiDAR, one could measure the state of the opponent and then predict its future position using a method such as the one presented in [49].

At time *t* = *t*0, when planning occurs, the position of the vehicle is [5, 0] m, its speed is 50 <sup>m</sup> <sup>s</sup> , and its initial heading angle is *<sup>π</sup>* <sup>2</sup> rad. The control points of the Bernstein polynomial representing the opponent's trajectory are

$$
\begin{bmatrix} 10 & 25 & 50 & 70 \\ 10 & 75 & 90 & 105 \end{bmatrix} \text{m.}
$$

The maximum speed is 65 <sup>m</sup> <sup>s</sup> , the maximum angular rate is *<sup>π</sup>* <sup>5</sup> rad <sup>s</sup> , and the minimum safe distance is 3 m. The tuning parameters are *<sup>α</sup>* <sup>=</sup> <sup>1</sup> <sup>−</sup> <sup>10</sup>−<sup>6</sup> and *<sup>β</sup>* <sup>=</sup> 100.

Figure 31 illustrates the optimized vehicle trajectory overtaking the opponent's trajectory. Figure 32 shows the squared speed of the vehicle along with the maximum speed constraint. As expected, the vehicle's speed approaches the maximum speed in order to successfully overtake the opponent. Figure 33 shows the vehicle's angular rate and its upper and lower constraints. It is clear that the vehicle remains within the desired bounds. Figure 34 shows the squared distance between the vehicle and the opponent. While the vehicle does come close to the opponent, it is never closer than the minimum safe distance.

**Figure 32.** Squared speed profile of the vehicle.

**Figure 33.** Angular rate profile of the vehicle.

**Figure 34.** Squared distance between vehicle and opponent.

#### *5.5. Swarming*

This section examines two methods for generating trajectories for large groups of autonomous aerial vehicles. The centralized method optimizes every trajectory at once. On the other hand, the decentralized method generates trajectories one at a time and compares them to previously generated trajectories.

The position of each vehicle in a swarm of *m* vehicles for the following examples is parameterized as a 3D Bernstein polynomial, i.e.,

$$\sum\_{i=0}^{n} \mathbf{P}\_{i,n}^{[j]} B\_{i,n}(t) = \mathbf{C}\_n^{[j]}(t), \quad \forall j \in \{1, \dots, m\}, \quad \mathbf{P}\_n^{[j]} \in \mathbb{R}^{3 \times n}.$$

#### 5.5.1. 101 Vehicle—Centralized

The centralized method optimizes the trajectories for each vehicle simultaneously. The goal is to minimize the arc length of each trajectory. There are *m* vehicles with 3rd order Bernstein polynomials representing their trajectories which are constrained to a minimum safe distance between each other and initial and final positions. This is formulated as follows:

$$\min\_{\mathbf{P}\_n} \sum\_{i=1}^m \sum\_{k=0}^{n-1} ||\mathbf{P}\_{k+1}^{[i]} - \mathbf{P}\_k^{[i]}||\_2$$

subject to

$$\begin{aligned} \mathbf{C}\_{\mathfrak{n}}^{[i]}(0) &= \mathbf{C}\_{0'}^{i} & \mathbf{C}\_{\mathfrak{n}}^{[i]}(t\_f) &= \mathbf{C}\_{f'}^{[i]} & \forall i \in \{1, \dots, m\}, \\ ||\mathbf{C}\_{\mathfrak{n}}^{[i]}(t) - \mathbf{C}\_{\mathfrak{n}}^{[j]}(t)||^2 &\geq d\_{\mathfrak{s}'}^2 & \forall i, j \in \{1, \dots, m\}, i \neq j. \end{aligned}$$

The initial positions for each vehicle were chosen randomly from a 25 m × 25 m grid at an altitude of *z* = 0 m. The final positions were chosen to spell out "CAS", as seen in Figure 35, at an altitude of *z* = 100 m. In the next section we significantly reduce the number of dimensions in the optimization vector by using the decentralized approach.

**Figure 35.** 101 vehicles spelling out CAS using the centralized method.

#### 5.5.2. 101 Vehicle—Decentralized

The decentralized method iteratively computes trajectories for the *i*th vehicle. Each new iteration is compared to the previously computed trajectories so that the minimum safety distance constraint is met. The problem that is solved at each iteration is written as

$$\min\_{\mathbf{P}\_n^{[i]}} \sum\_{i=1}^m \sum\_{k=0}^{n-1} ||\mathbf{P}\_{k+1}^{[i]} - \mathbf{P}\_k^{[i]}||\_1$$

subject to

$$\begin{aligned} \mathbf{C}\_{n}^{[i]}(0) &= \mathbf{C}\_{0}^{[i]}, \quad \mathbf{C}\_{n}^{[i]}(t\_{f}) = \mathbf{C}\_{f}^{[i]},\\ ||\mathbf{C}\_{n}^{[i]}(t) - \mathbf{C}\_{n}^{[j]}(t)||^{2} &\geq D\_{s}^{2}, \quad \forall j \in \{1, \dots, i-1\}, \quad i > 1. \end{aligned}$$

Note that the first vehicle does not need to satisfy the minimum safe distance constraint since no trajectories have been computed before it.

The parameters used in this example were identical to that of the previous subsection. The resulting figure has been omitted due to its similarity to Figure 35.

#### 5.5.3. 1000 Vehicle—Decentralized

The decentralized method can be used to compute 1000 trajectories. In this example, it is employed to generate the paths seen in Figure 36 to display the University of Iowa Hawkeye logo. The initial points are equally dispersed at an altitude of *z* = 0 m on a 100 m × 100 m grid. The final points are the pattern shown at an altitude of *z* = 100. The cost function aims to maximize the temporal distance between the current *i*th trajectory and the previously generated *j*th trajectories by taking the reciprocal of the sum of the Bernstein coefficients of the norm squared difference, i.e.

$$\min\_{\mathbf{P}^{[i]}\_n} \frac{1}{\sum\_{j=1}^{i-1} \mathbf{P}^{[norm,j]}}, \quad i > 1, \dots$$

subject to

$$\mathbf{C}\_{n}^{[i]}(0) = \mathbf{C}\_{0}^{[i]}, \quad \mathbf{C}\_{n}^{[i]}(t\_{f}) = \mathbf{C}\_{f}^{[i]}.$$

where **P**[*norm*,*j*] are the Bernstein coefficients of the Bernstein polynomial representing the squared temporal distance between the *i*th and *j*th trajectories, i.e.,

$$\left| \left| \mathbf{C}^{[i]}(t) - \mathbf{C}^{[j]}(t) \right| \right|^2 = \sum\_{i=0}^{n} P^{[norm, j]} B\_{i, n}(t) \dots$$

It should be noted that this formulation of cost function and constraints is used as a proof of concept. For other possible cost function and constraint formulations, the reader is referred to [50,51].

**Figure 36.** Trajectories for 1000 aerial vehicles with initial and final position and minimum safety distance constraints.

### *5.6. Marine Vehicle Model*

In this example, we consider a marine vehicle model known as the medusa. The equations of motion of the medusa are as follows

$$\begin{aligned} \dot{x} &= u \cos \psi - v \sin \psi, \\ \dot{y} &= u \sin \psi + v \cos \psi, \\ \dot{\psi} &= r. \end{aligned} \tag{18}$$

$$\begin{aligned} m\_{\mu}\dot{u} - m\_{\upsilon}\upsilon r + d\_{\mu}u &= \tau\_{\mu}, \\ m\_{\upsilon}\dot{\upsilon} + m\_{\mu}u\tau + d\_{\upsilon}\upsilon &= 0, \\ m\_{r}\dot{r} - m\_{\mu\upsilon}u\upsilon + d\_{r}r &= \tau\_{r\prime} \end{aligned} \tag{19}$$

where *x* and *y* represent the vehicle's position, *ψ* is the orientation, *u* (surge) and *v* (sway) are the linear velocities, *r* is the turning rate, and *τ* = [*τu*, *τr*] *<sup>T</sup>* is the vector of forces and torques due to thrusters/surfaces (control input).

In this example, we let the state, [*x*, *y*, *ψ*, *u*, *v*,*r*] -, and input, [*τu*, *τr*] -, be approximated by Bernstein polynomials, and impose the vehicle's dynamics directly through Bernstein polynomial differentiation. Using Property A3, the dynamics constraints given by Equations (18) and (19) reduce to a set of algebraic constraints. Additional constraints imposed on this problem include collision avoidance and input saturation constraints. Figure 37 shows an example of motion planning for a medusa vehicle, which is required to reach a final destination in the minimum time. Ten markers are plotted along the trajectory (shown in blue) to represent the heading of the vehicle at that point in time. It can easily be seen that the vehicle's trajectory avoids the (inflated) unsafe region illustrated by the orange circle.

**Figure 37.** Medusa Example.

*5.7. Dynamic Routing Problem*
