## 4 Initial Trajectory Generation
### B-Spline-Based Trajectory Generation  
\(d\)th-order B-spline can be defined for a given knot sequence \(p_{k}=\{t_{0},t_{1},\ldots,t_{n_{k}}\}\) and control points \(p_{c}=\{\mathbf{p}_{0},\mathbf{p}_{1},\ldots,\mathbf{p}_{n_{p}}\}\), where \(t_{*}\in\mathbb{R}\), \(\mathbf{p}_{*}\in\mathbb{R}^{d}\) and \(n_{k}=n_{p}+d+1\). If \(d\) is set to 3, each \(\mathbf{p}_{i}\) represents position in \(\mathbb{R}^{3}\), where \(i=0,\ldots,n_{p}\). For a given time index \(t\), the corresponding position \(\mathbf{p}(t)\) can be fully determined by using the de Boor-Cox formula [76]. \[\mathbf{p}(t)=DeBoorCox(t,p_{c}). \tag{28}\]  
Estimation is not limited to the position; velocity, acceleration, or any high-order derivative of \(p_{c}\) can be estimated using \(DeBoorCox(t,p_{c}^{(*)})\), as given in Algorithm 1, where \((*)\) depicts the order of the derivative of \(p_{c}\) such that \((*)<d\). ```
1:procedureDeBoorCox(\(t,p\))
2:\(t=\left\{ \begin{array}{ll}p_{k}[d],&if\ t<p_{k}[d]\\ p_{k}[n_{k}],&if\ t>p_{k}[n_{k}]\\ t,&otherwise\end{array} \right.\)
3:\(k=d\)
4:while\(true\)do
5:if\(p_{k}[k+1]\geq t\)then
6: break
7:\(k\)++
8:\(\mathbf{p}_{c}[d]\)
9:for\(i\gets 0\) to \(d\)do
10:\(\mathbf{p}_{c}[i]\gets p[k-d+i]\)
11:for\(r\gets 1\) to \(d\)do
12:for\(i\gets d\) to \(r\)do
13:\(\beta\leftarrow\frac{t-p_{k}[i+k-d]}{p_{k}[i+1+k-r]-p_{k}[i+k-d]}\)
14:\(\mathbf{p}_{c}[i]\leftarrow(1-\beta)\times\mathbf{p}_{c}[i-1]+\beta\times \mathbf{p}_{c}[i]\)
15:return\(\mathbf{p}_{c}[d]\)
```  
**Algorithm 1** The B-spline trajectory (\(p\)) and its derivative estimation for a given time index \(t\), where \(p\) equals \(p_{c}^{(*)}\). Later, the B-spline matrix representation was proposed by [PERSON] [77]. B-spline can be formulated as uniform or nonuniform. [PERSON] et al. [78] detailed the uniform B-spline matrix representation. In uniform B-spline, knot span is the same for any considered consecutive time interval, i.e., \(\Delta t=t_{i+1}-t_{i},\ i\in[0,n_{k})\). Any position of the trajectory can be parameterized by considering only \(d+1\) consecutive control points: \([\mathbf{p}_{i},\mathbf{p}_{i+1},\ldots,\mathbf{p}_{i+d}]\). Hence, corresponding normalized time \(q(t)\) can be calculated as follows:  
\[q(t)=\frac{t-t_{i}}{t_{i+1}-t_{i}}=\frac{t-t_{i}}{\Delta t},\quad t\in[t_{i},t _{i+1}]. \tag{29}\]  
In the matrix representation, \(c(q(t))\), which is given in (28), can be determined by:  
\[ \begin{split}& c(q(t))=\mathbf{q}(t)M_{d}p_{i},\quad\mathbf{q}(t) =[1,q(t),q^{2}(t),\ldots,q^{d}(t)]^{T},\ p_{i}=[\mathbf{p}_{i},\mathbf{p}_{i +1},\ldots,\mathbf{p}_{i+d}]^{T},\\ & M_{d}\in\mathbb{R}^{d+1\times d+1},\quad M_{r,c}=\frac{1}{d!} \binom{d}{d-r}\Sigma_{s=c}^{d}(-1)^{s-c}\times\binom{d}{s-c}(d-s)^{d+1-r-s}. \end{split}  \tag{30}\]Since each control point \(\mathbf{p}_{i}\) belongs to \(d+1\) of successive spans, B-spline can be controlled locally. Due to such controllability, B-spline is suitable for local trajectory planning [30]. Moreover, the derivatives of a given B-spline are also B-spline [8]. Hence, B-spline's derivatives (e.g., velocity, acceleration, jerk) can be calculated considering corresponding span \([t_{i},t_{i}+1)\) for a given \(d+1\) consecutive control points \(p_{i}=[\mathbf{p}_{i},\mathbf{p}_{i+1},\ldots,\mathbf{p}_{i+d}]^{T}\in\mathbb{ R}^{d\times 3}\) and corresponding knot vector.