# Simultaneous Trajectory and Speed Planning for Autonomous Vehicles Considering Maneuver Variants
## 2 Motion-Planning Concept
,\ \ \mathbf{D}_{d}=diag\big{(}\mathbf{D} \quad\mathbf{D}\big{)} \tag{6}\]  
where \(k=((p+1)/2-1)\). Thus, the functions \(\mathbf{F}\) of Equation (3) may be replaced by the basis functions of the parameter \(\xi\)  
\[\mathbf{F}=\mathbf{D}_{d}\mathbf{F}_{\xi},\ \ y(x)=y(\mathbf{Q}_{tr},D,\xi)=\mathbf{Q}_{tr}^{T}\mathbf{D} _{d}\mathbf{F}_{\xi} \tag{7}\]  
Then, for the \(k\)-th derivative,  
\[\frac{d^{k}y}{dx^{k}}=\frac{d}{Dd\xi}\Bigg{(}\frac{1}{D^{k-1}}\mathbf{Q}_{tr}^{T} \mathbf{D}_{d}\frac{d^{k-1}\mathbf{F}_{\xi}}{d\xi^{k-1}}\Bigg{)}=\frac{d}{Dd\xi} \Bigg{(}\frac{1}{D^{k-1}}\mathbf{Q}_{tr}^{T}\mathbf{D}_{d}\frac{d^{k-1}\mathbf{F}_{\xi}}{ d\xi^{k-1}}\Bigg{)}=\mathbf{Q}_{tr}^{T}\bigg{(}\frac{\mathbf{D}_{d}}{D^{k}}\bigg{)} \frac{d^{k}\mathbf{F}_{\xi}}{d\xi^{k}} \tag{8}\]  
**Speed Model** can be organized in the same way as the trajectory. We assume that the AV longitudinal speed \(V_{\xi}\) is distributed along the trajectory projection on the \(x\)-coordinate. According to Equations (3)-(8), the longitudinal component and its \(k\)-th derivative may be expressed as follows  
\[V_{\xi}(x)=\mathbf{Q}_{sp}^{T}\mathbf{D}_{d}\mathbf{F}_{\xi},\ \ \frac{d^{k}V_{\xi}}{dx^{k}}=\mathbf{Q}_{sp}^{T} \bigg{(}\frac{\mathbf{D}_{d}}{D^{k}}\bigg{)}\frac{d^{k}\mathbf{F}_{\xi}}{d\xi^{k}} \tag{9}\]  
where \(\mathbf{Q}_{sp}=\) set of speed nodal parameters. **Full set of parameters**. Since we consider one segment with initial (0) and final (f) points, each of them containing three DOFs, we need six variable parameters for each \(\mathbf{Q}_{tr}\) and \(\mathbf{Q}_{sp}\) to simultaneously search the trajectory \(y(x)\) and speed distribution \(V_{\xi}(x)\). Then, introducing nodal vectors  
\[\mathbf{q}_{tr0}= \begin{pmatrix}q_{tr(1)}\\ q_{tr(2)}\\ \frac{d^{2}y}{dx^{2}}\end{pmatrix} _{0},\ \ \mathbf{q}_{trf}= \begin{pmatrix}q_{tr(4)}\\ q_{tr(5)}\\ q_{tr(6)}\end{pmatrix} = \begin{pmatrix}y_{f}\\ \frac{d^{2}y}{dx}\end{pmatrix} _{f},\ \ \ \mathbf{Q}_{tr}= \begin{pmatrix}q_{tr 0}\\ \mathbf{q}_{trf}\end{pmatrix}  \tag{10}\]  
The vectors \(\mathbf{Q}_{tr}\) and \(\mathbf{Q}_{sp}\) completely determine the configuration of the trajectory and speed distributions within the segment \(D\). Nevertheless, not all of their members are variable parameters. Note that at the initial node the vectors \(\mathbf{q}_{tr0}\), \(\mathbf{q}_{sp0}\) are usually known from the previous solution. In turn, strict constraints may be imposed on the values of vectors \(\mathbf{q}_{r0}\), \(\mathbf{q}_{sp0}\), which reduces the number of parameters to be optimized. Note that the nodal parameter \(y_{f}\) is not independent in its pure form but is calculated from Equation (1), where the parameters \(s_{r}\) and \(L\) are involved. Their variation determines a combination of \(y_{f}\) and \(D\). Thus, it is possible to form a set of variable parameters that best reflect both the trajectory shape and the speed distribution. \[\mathbf{q}=\left(s_{r}\quad L_{f}\quad\frac{dy}{dx}\Big{|}_{f}\quad\frac{d^{2}y}{ dx^{2}}\Big{|}_{f}\quad V_{\xi f}\quad\frac{d^{\prime}V_{\xi}}{dx}\Big{|}_{f} \quad\frac{d^{\prime}V_{\xi}}{dx^{2}}\Big{|}_{f}\quad\right)^{T} \tag{11}\]  
In the case of the strict finite transverse displacement \(L_{f}\) and the corresponding angle \(\alpha_{f}\), these parameters depend only on the variable \(s_{r}\), and the set of Equation (11) is shortened  
\[\mathbf{q}=\left(s_{r}\quad\frac{d^{2}y}{dx^{2}}\Big{|}_{f}\quad V_{\xi f}\quad \frac{d^{\prime}V_{\xi}}{dx}\Big{|}_{f}\quad\frac{d^{\prime}V_{\xi}}{dx^{2}} \Big{|}_{f}\quad\right)^{T} \tag{12}\]  
**Numerical Integration Technique**. To simultaneously satisfy the rapidity and quality of calculations, we shall use the numerical integration based on the \(N\)-point Gaussian quadrature scheme.