# Simultaneous Trajectory and Speed Planning for Autonomous Vehicles Considering Maneuver Variants
## 2 Motion-Planning Concept
If the lane's midline as the vehicle's current trajectory is considered, then, similarly to AV, we can compose interpolation dependencies \(x_{i}=x_{i}(s_{i})\), \(y_{i}=y_{i}(s_{i})\), \(i\in[1,N_{v}]\), where \(N_{v}\) is the number of fixed vehicles. Therefore, we may estimate the vehicle motion parameters in discrete points over time intervals \(\Delta t\) and the total AV's motion time \(t_{f}\). If vehicles' speeds \(V_{i}\) are accepted to be constants, then, the paths for the intervals \(\Delta t\) are \(\Delta s_{i}=V_{i}\Delta t\) and for the total time \(s_{i}=V_{i}t_{f}\). Knowing the paths, it is possible to determine coordinates \(x_{i}\), \(y_{i}\) by the predefined interpolation. Then, considering the radius of safety zones \(r_{i}\), the distance between the AV (0) and an \(i\)-th obstacle vehicle can be evaluated as  
\[d_{i}=\sqrt{\left(x_{cp0}-x_{cpi}\right)^{2}+\left(y_{cp0}-y_{cpi}\right)^{2} }-r_{0}-r_{i},\ p\in[f,r] \tag{20}\]  
Note that, strictly speaking, to determine the vehicle mass center's position, it is necessary to identify a vehicle and estimate its overall dimensions and wheelbase, which  
Figure 3: Trajectory variants and distances to moving obstacles. somewhat complicates the calculations. In this regard, we assume that moving obstacles behind AV may be conditionally defined by circles centered at the vehicles' fronts, and obstacles ahead of AV may be represented by circles centered at the vehicles' rears. The corresponding centers can be easily found by estimating the overall width of a moving obstacle. Then, the parameters \(L_{i,0}\) can be replaced by \(L_{i,f}\) and \(L_{i,r}\), respectively. Thus, the trajectories of moving obstacles can be considered as the trajectories of the safety contour circles' centers. **Time. ** The time interval \(\Delta t_{i}\) between AV's positions \(i-1\) and \(i\) can be calculated by the integration, considering the trajectory \(s\) and speed \(V\)  
\[\Delta t_{i}=\int_{\tau_{i-1}}^{\tau_{i}}d\tau=\int_{s_{(i-1)}}^{s_{(i)}}\frac{ ds}{V(s)}=\int_{\tau_{i-1}}^{x_{i}}\frac{dx}{V_{x}(x)}=\int_{\tau_{i-1}}^{x_{i}} z_{ti}(x)dx,\ \ \ z_{ti}(x)=\frac{1}{V_{x}(x)} \tag{21}\]  
Each segment \([x_{i-1},\,x_{i}]\), in turn, may also be represented by a finite element corresponding to a range \([\xi_{i-1},\xi_{i}]\) and again considered in the segment \(\vartheta\in[0,1]\). Then  
\[\int_{x_{i-1}}^{x_{i}}z_{ti}(x)dx=D\int_{\xi_{i-1}}^{\xi_{i}}z_{ti}(\xi)d\xi=D \Delta\xi_{i}\int_{0}^{1}z_{ti}(D\xi(\vartheta))d\vartheta \tag{22}\]  
Passing to numerical integration according to Equation (17), we obtain  
\[\Delta t_{i}=D\frac{\Delta\xi_{i}}{2}\sum_{k=1}^{N}z_{ti}(D\xi_{i}(\vartheta_{ k}))w_{k}=D\frac{\Delta\xi_{i}}{2}z_{ti}^{T}w \tag{23}\]  
Thus, it is possible to form a vector of time increments  
\[\Delta t=\big{(}\Delta t_{1}\ \ \ \cdots\ \ \ \Delta t_{n}\big{)}^{T} \tag{24}\]  
where \(n\) = number of intervals. The vector of time points can be obtained by the accumulative addition:  
\[t=\begin{pmatrix}0&\sum_{i=1}^{1}\Delta t_{i}&\cdots&\sum_{i=1}^{n}\Delta t_{ i}\ \ \big{)}^{T},\ \ \ t_{f}=\sum_{i=1}^{n}\Delta t_{i} \tag{25}\]  
Provided that the segment \(\xi\in[0,1]\) is represented by the vector of increments \(\Delta\xi\), the final time can also be obtained as follows  
\[t_{f}=\frac{D}{2}\Delta\xi diag\Big{(}\mathbf{z}_{t}^{T}\Big{)}\mathbf{w} \tag{26}\]  
**Spatial Constraints**. The requirement of non-violating the roadway boundaries is the main limiting factor. It can be used as a hard or soft constraint. In the first case, it is assumed that there must be no safety contour exceeding the roadway boundaries. In the second case, it is possible to allow the crossing of the solid marking line subject to a critical approach to an obstacle. In this case, we shall use strict conditions.