5.7.1. Single Vehicle Case

We consider a problem where a single vehicle is supposed to visit *M* neighborhoods *Bi* <sup>=</sup> {*<sup>x</sup>* <sup>∈</sup> *<sup>R</sup>*<sup>2</sup> : *<sup>x</sup>* <sup>−</sup> *bi*<sup>≤</sup> *<sup>r</sup>* } in minimum time *tf* . Here *<sup>r</sup>* <sup>&</sup>gt; 0 and the vectors *bi* <sup>∈</sup> *<sup>R</sup>*2, *i* ∈ [1, *M*] represent a sequence of points of interest that has been generated by a Traveling Salesman Problem (TSP) algorithm. Let *ti* denote a time instance when the vehicle's position satisfies **C***n*(*ti*) ∈ *Bi*. Then, the dynamic routing problem for a single vehicle can be formulated as follows, **DR**<sup>1</sup>

> min *ti*,*i*∈[1,*M*], **<sup>P</sup>***<sup>n</sup> tf* (20)

subject to

$$\begin{aligned} \mathbf{C}\_{\mathfrak{n}}(t\_i) &\in B\_i \\ t\_{i-1} < t\_i < t\_{i+1}, \ \forall i = 2, M-1 \\ t\_1 &> 0, \ t\_f > t\_M \\ ||\dot{\mathbf{C}}\_{\mathfrak{n}}||\_{\infty} &\le 1, \\ ||\dot{\mathbf{C}}\_{\mathfrak{n}}||\_{\infty} &\le 1, \\ \mathbf{C}\_{\mathfrak{n}}(0) &= \mathbf{C}\_{\mathfrak{n}}(t\_f) = \mathbf{C}\_0 \end{aligned}$$

5.7.2. Numerical Solution: Single Vehicle Case

Let

$$\mathbf{C}\_{n}^{i}(t) = \begin{cases} \sum\_{k=0}^{n} \mathbf{P}\_{i,k} B\_{k,n}(t), t \in [t\_{i-1}, t\_i], t\_0 = 0, t\_{M+1} = t\_f \\\\ 0, \text{ o.w.} \end{cases}, i \in [1, M+1] \tag{21}$$

Define

$$\mathbf{C}\_{\mathfrak{n}}(t) = \sum\_{i=1}^{M+1} \mathbf{C}\_{\mathfrak{n}}^{i}(t) \tag{22}$$

Then the numerical solution of the dynamic routing problem **DR**<sup>1</sup> was obtained by solving the optimization problem

$$\min\_{t\_i, i \in \{1, M\}, \ \mathbf{P}^i\_n} t\_f \tag{23}$$

subject to

$$\begin{aligned} \|\mathbf{C}\_{\mathbf{n}}^{i}(t\_{i}) - b\_{i}\| &\le r\\ t\_{i-1} < t\_{i} < t\_{i+1} \; \forall i = 2, M - 1\\ t\_{1} > 0, \; t\_{f} > t\_{M} \\ \|\hat{\mathbf{C}}\_{\mathbf{n}}\|\_{\infty} &\le 1,\\ \|\hat{\mathbf{C}}\_{\mathbf{n}}\|\_{\infty} &\le 1,\\ \mathbf{C}\_{\mathbf{n}}(0) = \mathbf{C}\_{\mathbf{n}}(t\_{f}) &= \mathbf{C}\_{0} \\ \mathbf{C}\_{\mathbf{n}}^{i}(t\_{i}) = \mathbf{C}\_{\mathbf{n}}^{i+1}(t\_{i}), \; i \in [1, M - 1] \\ \dot{\mathbf{C}}\_{\mathbf{n}}^{i}(t\_{i}) = \dot{\mathbf{C}}\_{\mathbf{n}}^{i+1}(t\_{i}) \; i \in [1, M - 1] \end{aligned}$$

A simulation was performed illustrating a single agent visiting 30 neighborhoods. The resulting trajectory is shown in Figure 38. The agent is limited to velocities of arbitrary units ranging from −1 to 1 and is similarly limited to accelerations of arbitrary units also from −1 to 1. The velocities and accelerations of the vehicle can be seen in Figure 39 and Figure 40, respectively.

**Figure 38.** Trajectory of a single agent visiting 30 neighborhoods. The circles with an X in their center represent the neighborhoods and the circles along the vehicle's trajectory represent the points at which the vehicle makes its delivery.

**Figure 39.** Velocity components of a single agent visiting 30 neighborhoods.

**Figure 40.** Acceleration components of a single agent visiting 30 neighborhoods.

#### 5.7.3. Multiple Vehicle Case

In this case, *K* drones are assigned a total of *K* neighborhood sets to visit. Each neighborhood set, P*k*, consists of an equal number of neighborhoods *Bij* which are defined by a set of points of interest *bik* <sup>∈</sup> *<sup>R</sup>*2, i.e.,

$$B\_{ik} = \{ \mathbf{x} \in R^2 : ||\mathbf{x} - b\_{ik}|| \le r, \ i \in [1, M], \ k \in [1, K] \}\_{\forall i}$$

and

$$\mathcal{P}\_k = \{B\_{1k'}, \dots, B\_{Mk'} \: \: k \in [1, K] \}.$$

Let *tf k*, *k* ∈ [1, *K*] denote the total time it takes for the *k*th vehicle to visit every neighborhood in the set P*<sup>k</sup>* once and let *tik* denote a time instance when the *k*th vehicle's position satisfies **C***<sup>k</sup> <sup>n</sup>*(*tik*) ∈ *Bik*. Using this notation, we propose the following definition of the multi-vehicle dynamic routing problem for given positive number *wk* and *d* **DR**<sup>2</sup>

min *tik*,*i*∈[1,*M*],*k*∈[1,*K*], **<sup>P</sup>***<sup>k</sup> n K* ∑ *k*=1 *wktf k* (24) subject to

$$\begin{aligned} \mathbf{C}\_{n}^{k}(t\_{ik}) \in B\_{ik} \in \mathcal{P}\_{k'} \,\forall k \in [1, K] \\ t\_{i-1,k} < t\_{ik} < t\_{i+1,k\prime} \,\forall i \in [2, M-1], k \in [1, K] \\ t\_{1k} > 0, \, t\_{fk} > t\_{Mk} \\ |\mathbf{C}\_{n}^{k}(t)| \le u\_{\max}, \,\forall t \in [0, t\_{fk}] \\ \mathbf{C}\_{n}^{k}(0) = \mathbf{C}\_{n}^{k}(t\_{f}) = \mathbf{C}\_{k0\prime} \,\forall k \in [1, K] \\ |\mathbf{C}\_{n}^{k}(t) - \mathbf{C}\_{n}^{l}(t)| \ge d, \, k \ne l, \forall t \in [0, \max\_{j \in [1, K]} t\_{fj}], k \in [1, K], l \in [1, K]. \end{aligned}$$

Simulations were performed for a two agent and ten agent case each visiting 10 neighborhoods per agent. The resulting trajectories for the two agent case are shown in Figure 41 and for the ten agent case in Figure 42.

**Figure 41.** Two agents visiting ten neighborhoods each.

**Figure 42.** Ten agents visiting ten neighborhoods each.

#### **6. Conclusions**

We presented a method to generate optimal trajectories by using Bernstein polynomials to transcribe the problem into a nonlinear programming problem. By exploiting the useful properties of Bernstein polynomials, our method provides computationally efficient algorithms that can also guarantee safety in continuous time which are useful in optimization routines. These algorithms include evaluating bounds, evaluating extrema, minimum spatial distance between two Bernstein polynomials, minimum spatial distance between a Bernstein polynomial and a convex polygon, collision detection, and the penetration algorithm. We also developed an open source toolbox which makes these transcription methods readily available in the Python programming language.

Numerical examples were provided to demonstrate the efficacy of the method. Simple cost functions and constraints were implemented to generate trajectories for several realistic mission scenarios including air traffic control, navigating a cluttered environment, overtaking a vehicle, trajectory generation for a large swarm of vehicles, trajectory generation for a marine vehicle, and navigation for vehicles operating in a Traveling Salesman mission. Our formulation offers a powerful tool for users to generate optimal trajectories in real time scenarios for single or multiple robot teams. Future work includes developing new cost functions, exploring different optimization frameworks, and replanning trajectories to react to a changing environment.

**Author Contributions:** Conceptualization, C.K.-J., V.C., I.K., A.P., C.W.; methodology, C.K.-J., V.C., I.K., A.P., C.W.; software, C.K.-J., T.B., I.K., C.W.; validation, C.K.-J., T.B., I.K., C.W.; investigation, C.K.- J., V.C., I.K., A.P., C.W.; resources, V.C., I.K., A.P.; writing—original draft preparation, C.K.-J., V.C., I.K.; writing—review and editing, C.K.-J.; supervision, V.C., I.K., A.P., C.W.; project administration, V.C.; funding acquisition, V.C., I.K., A.P. All authors have read and agreed to the published version of the manuscript.

**Funding:** This research was supported by the Office of Naval Research, grants N000141912106, N000142112091 and N0001419WX00155. Antonio Pascoal was supported by H2020-EU.1.2.2—FET Proactive RAMONES, under Grant GA 101017808 and LARSyS-FCT under Grant UIDB/50009/2020. Isaac Kaminer was supported by the Office of Naval Research grant N0001421WX01974.

**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.
