**2. Mathematical Preliminaries**

The motion planning problems addressed in this work can be in general formulated as optimal control problems. Letting the states and control inputs of the vehicles be denoted by *x*(*t*) and *u*(*t*), respectively, the optimal motion planning problem can formally be stated as follows:

$$\min\_{\mathbf{x}(t),\mathbf{u}(t)} I(\mathbf{x}(t),\mathbf{u}(t)) = E(\mathbf{x}(0),\mathbf{x}(t\_f)) + \int\_0^{t\_f} F(\mathbf{x}(t),\mathbf{u}(t))dt\tag{1}$$

subject to

$$\dot{\mathbf{x}}(t) = f(\mathbf{x}(t), \boldsymbol{\mu}(t)) \,, \quad \forall t \in [0, t\_f] \,\tag{2}$$

$$\mathbf{e}(\mathbf{x}(0), \mathbf{x}(t\_f)) = \mathbf{0},\tag{3}$$

$$\left(\mathfrak{h}(\mathfrak{x}(t),\mathfrak{u}(t))\right)\leq\mathfrak{0},\quad\forall t\in[0,t\_f],\tag{4}$$

where *<sup>I</sup>* : <sup>R</sup>*nx* <sup>×</sup> <sup>R</sup>*nu* <sup>→</sup> <sup>R</sup>, *<sup>E</sup>* : <sup>R</sup>*nx* <sup>×</sup> <sup>R</sup>*nx* <sup>→</sup> <sup>R</sup>, *<sup>F</sup>* : <sup>R</sup>*nx* <sup>×</sup> <sup>R</sup>*nu* <sup>→</sup> <sup>R</sup>, *<sup>f</sup>* : <sup>R</sup>*nx* <sup>×</sup> <sup>R</sup>*nu* <sup>→</sup> <sup>R</sup>*nx* , *<sup>e</sup>* : <sup>R</sup>*nx* <sup>×</sup> <sup>R</sup>*nx* <sup>→</sup> <sup>R</sup>*ne*, and *<sup>h</sup>* : <sup>R</sup>*nx* <sup>×</sup> <sup>R</sup>*nu* <sup>→</sup> <sup>R</sup>*nh* .

Here, *I* defined in Equation (1) is a Bolza-type cost functional, with end point cost *E* and running cost *F*. The constraint in Equation (2) enforces the dynamics of the vehicles considered, Equation (3) enforces the boundary conditions, e.g., initial and final position, speed, heading angles of the vehicles, and Equation (4) describes feasibility and mission specific constraints, e.g., minimum and maximum speed, acceleration, collision avoidance constraints, etc.

In previous work [36–38] we presented a discretization method to approximate state and input by *n*th order Bernstein polynomials. This approximation allows us to transcribe the optimal control problem into a non-linear programming problem, which can then be solved by off-the-shelf optimization solvers. In particular, we show that the solution to the non-linear programming problem converges to the solution of the original optimal control problem as *n* increases. The present paper focuses on the geometric properties of Bernstein polynomials and their implementation for computationally efficient and safe trajectory generation. In the following, we report the properties of Bernstein polynomials and rational Bernstein polynomials which are relevant to this paper.

An *n*th order Bernstein polynomial defined over an arbitrary interval [*t*0, *tf* ] is given by

$$\mathbf{C}\_{n}(t) = \sum\_{i=0}^{n} \mathbf{P}\_{i,n} B\_{i,n}(t), \quad t \in \left[t\_{0}, t\_{f}\right], \tag{5}$$

,

where **<sup>P</sup>***i*,*<sup>n</sup>* <sup>∈</sup> <sup>R</sup>*<sup>D</sup>* is the *<sup>i</sup>*th Bernstein coefficient, *<sup>D</sup>* is the number of dimensions, and *Bi*,*n*(*t*) is the Bernstein polynomial basis defined as

$$B\_{i,n}(t) = \binom{n}{i} \frac{(t-t\_0)^i (t\_f - t)^{n-i}}{(t\_f - t\_0)^n}, \qquad \binom{n}{i} = \frac{n!}{i!(n-i)!}$$

for all *i* = 0, ... , *n*. Typically the dimensionality of a Bernstein polynomial, *D*, is either 2 or 3 for 2D or 3D spatial curves, respectively. In this case, Bernstein polynomials are often referred to as Bézier curves and their Bernstein coefficients are known as *control points*. While Bézier's original work did not explicitly use the Bernstein basis [41,42], it was later shown that the original formulation is equivalent to the Bernstein form polynomial [43].

An *n*th order *rational* Bernstein polynomial, *Rn*(*t*), is defined as

$$R\_{\boldsymbol{\pi}}(t) = \frac{\sum\_{i=0}^{n} P\_{i,\boldsymbol{n}} w\_{i,\boldsymbol{n}} B\_{i,\boldsymbol{n}}(t)}{\sum\_{i=0}^{n} w\_{i,\boldsymbol{n}} B\_{i,\boldsymbol{n}}(t)}, \quad t \in [t\_{0\prime} t\_f], \tag{6}$$

where *wi*,*<sup>n</sup>* <sup>∈</sup> <sup>R</sup>, *<sup>i</sup>* <sup>=</sup> 0, ... , *<sup>n</sup>*, are referred to as weights. A list of relevant properties of Bernstein polynomials used throughout this article can be found in Appendix A.
