# Simultaneous Trajectory and Speed Planning for Autonomous Vehicles Considering Maneuver Variants
## 2 Motion-Planning Concept  
Let us assume that AV forms a motion trajectory relative to the midline of a lane on which it is currently located [18; 19] (Figure 2). Such a line may be obtained, for example, by the cubic spline interpolation providing sufficient smoothness and representing the curvature of a road section. If functional dependencies \(x=x(s_{r})\), \(y=y(s_{r})\) are built for a section, then, varying the parameter \(s_{r}\) relative to the initial path length \(s_{r0}\), it is possible to determine the values \(x_{sr}\), \(y_{sr}\) corresponding to the zero point of the lateral displacement  
Figure 1: Scheme of the study’s structure. along the road width. Thus, the coordinates \(x_{f}\), \(y_{f}\) of the AV mass center's final position can be calculated as  
\[x_{f}=x_{sr}-L_{f}sin\Big{(}\alpha_{f}\Big{)},\ \ \ y_{f}=y_{sr}+L_{f}cos\Big{(} \alpha_{f}\Big{)} \tag{1}\]  
where \(\alpha_{f}\) = the midline curve's tangent slope at the point \(x_{sr}\), and \(L_{f}\) = AV mass center's lateral displacement at the final point. Since the transverse coordinate \(L\) is always perpendicular to the curve \(s_{r}\), the AV's final angular position on any lane has ideally the same value \(\alpha_{f}\). However, the angle \(\alpha\) actual value may for various reasons deviate from the ideal value, which is associated with the desired angle \(\alpha_{d}\). In the transverse direction, AV may be located in any lane by introducing such a parameter as the desired lateral displacement (position) \(L_{d}\). Thus, the trajectory can be represented as a piecewise polynomial function with the basis \(D=x_{f}-x_{0}\). **Trajectory**. Let us apply the Hermitian interpretation of Lagrange polynomials [20] using basis functions and nodal degrees of freedom (DOFs). To model the trajectory as a function \(y(x)\), the fifth-degree polynomial (\(p\) = 5) may be considered, which requires three DOFs in a node. Then, \(y(x)\) may be expressed in the vector form  
\[\mathbf{A}=\big{(}c_{0}\ \ \ \cdots\ \ c_{p}\big{)}^{T},\ \ \mathbf{X}=\big{(}x^{0}\ \ \ \cdots\ \ \ x^{p}\big{)}^{T},\ \ y(x)=\mathbf{A}^{T}\mathbf{X}, \tag{2}\]  
where \(c_{j}\) = polynomial coefficient, \(j\in[0,p]\). If define \(k=((p+1)/2-1)\) first derivatives of the vector \(\mathbf{X}\) (where \(p\) is odd) and substitute the \(x\)-coordinates of the initial (0) and final (\(D\)) nodes, the matrix \(B\) is composed as  
\[\mathbf{B}=\left( \begin{array}{c}\mathbf{X}^{T}(0)\\ \vdots\\ d^{k}\mathbf{X}^{T}/dx^{k}(0)\\ \mathbf{X}^{T}(D)\\ \vdots\\ d^{k}\mathbf{X}^{T}/dx^{k}(D)\end{array} \right),\mathbf{B}\mathbf{A}=\mathbf{Q}_{tr},\mathbf{A}= \mathbf{B}^{-1}\mathbf{Q}_{tr},\mathbf{F}=\Big{(}\mathbf{B}^{T}\Big{)}^{-1}\mathbf{X}, \tag{3}\]  
where \(\mathbf{Q}_{tr}\) = set of trajectory nodal parameters. Figure 2: Scheme of forming AV trajectory based on Frenet coordinates. As a result, expressions of Equations (2) and (3) are linked in the form  
\[y(x)=\Big{(}\mathbf{B}^{-1}\mathbf{Q}_{tr}\Big{)}^{T}\mathbf{X}=\mathbf{Q}_{tr}^{T}\Big{(}\mathbf{B}^ {T}\Big{)}^{-1}\mathbf{X}=\mathbf{Q}_{tr}^{T}\mathbf{F} \tag{4}\]  
Considering one final element (FE) of the length \(D\), the function \(y(x)\) may be represented by the sets of shape functions and DOF values. Then  
\[\mathbf{Q}_{tr}= \begin{pmatrix}q_{tr(1)}\\ \vdots\\ q_{tr(p+1)}\end{pmatrix} ,\ \ \mathbf{F}= \begin{pmatrix}f_{1}\\ \vdots\\ f_{p+1}\end{pmatrix} ,\ \ y(x)=\mathbf{Q}_{tr}^{T}\mathbf{F} \tag{5}\]  
where \(f_{j}=\) shape function and \(q_{j}=\) weight coefficient or DOF, \(j\in[1,p+1]\). If we assume \(x=\xi D\), where parameter \(\xi\in[0,1]\), then the shape functions may be derived through basis functions for an element of unit length. Thus, the basis functions \(\mathbf{F}_{\xi}\) of the argument \(\xi\in[0,1]\) are universal for the variable length \(D\) of a roadway segment. Defining the vector \(\mathbf{D}\) and matrix \(\mathbf{D}_{d}\)  
\[\mathbf{D}= \begin{pmatrix}D^{0}&\cdots&D^{k}\end{pmatrix}