# Simultaneous Trajectory and Speed Planning for Autonomous Vehicles Considering Maneuver Variants
## 5 Simulation  
We use the data of the Audi A4 3.2 FSI [23] to represent the AV. All the calculations are accomplished by using MATLAB tools [24]. The basic function to realize the optimization procedure is **fmincon**. The five-point Gauss quadrature scheme is accepted for the numerical integration, and the relative path step is \(\Delta\xi=0.1\). Let us define constraints and initial values. Speed  
\[V_{\zeta 0}=60\ \bigg{[}\frac{km}{h}\bigg{]},\ \ \ V_{\zeta U}=80\ \bigg{[}\frac{km}{h}\bigg{]},\ \ \ V_{\zeta L}=50\ \bigg{[}\frac{km}{h}\bigg{]} \tag{97}\]  
Acceleration  
\[a_{\zeta U}=\varphi_{max}g\ \bigg{[}\frac{m}{s^{2}}\bigg{]},\ \ a_{\zeta L}=-0.5\ \bigg{[}\frac{m}{s^{2}}\bigg{]},\ \ a_{\zeta 0}=a_{\zeta f}=0\ \bigg{[}\frac{m}{s^{2}}\bigg{]},\ \ a_{\zeta lim}=a_{\zeta lim} \big{(}V_{\zeta}\big{)} \tag{98}\]  
where \(a_{\zeta lim}=\) function of the vehicle throttle response characteristic. Jerk  
\[j_{\zeta U}=13\ \bigg{[}\frac{m}{s^{3}}\bigg{]},\ \ j_{\zeta L}=-6.5\ \bigg{[}\frac{m}{s^{3}}\bigg{]},\ \ \ j_{\zeta f}=0\ \bigg{[}\frac{m}{s^{3}}\bigg{]}, \tag{99}\]\[\omega_{U}=0.5\left[\frac{rad}{s}\right],\ \ \omega_{L}=-0.5\left[\frac{rad}{s}\right] \tag{100}\]  
Angular acceleration  
\[\varepsilon_{U}=3\left[\frac{rad}{s^{2}}\right],\ \ \varepsilon_{L}=-3\left[\frac{rad}{s^{2}}\right] \tag{101}\]  
First, let us set the values of the weight coefficients \(W\) in Equation (72). \[\mathbf{W}= \begin{pmatrix}3&1&1&10&10^{3}&10^{3}&5&10\\ \end{pmatrix} ^{T} \tag{102}\]  
Considering the order of variable parameters, the coefficient values were selected based on the condition of the influence proportionality but with an emphasis on safety and maneuver precision. Let us define constraints and initial values. Speed  
\[V_{\zeta 0}=60\ \left[\frac{km}{h}\right],\ \ \ V_{\zeta U}=80\ \left[\frac{ km}{h}\right],\ \ \ V_{\zeta L}=50\ \left[\frac{km}{h}\right], \tag{103}\]  
Acceleration  
\[a_{\zeta U}=\varphi_{max}g\left[\frac{m}{s^{2}}\right],\ \ \ a_{\zeta L}=-0.5\ \left[\frac{m}{s^{2}}\right],\ \ \ a_{\zeta 0}=a_{\zeta f}=0\ \left[\frac{m}{s^{2}}\right],\ \ \ a_{\zeta lim}=a_{\zeta lim}\big{(}V_{\zeta}\big{)} \tag{104}\]  
where \(a_{\zeta lim}=\) function of the vehicle throttle response characteristic. Jerk  
\[j_{\zeta U}=13\ \big{[}\frac{m}{s^{3}}\big{]},\ \ j_{\zeta L}=-6.5\ \big{[}\frac{m}{s^{3}}\big{]},\ \ j_{\zeta f}=0\ \big{[}\frac{m}{s^{3}}\big{]}, \tag{105}\]  
Yaw rate  
\[\omega_{U}=0.5\ \left[\frac{rad}{s}\right],\ \ \omega_{L}=-0.5\ \bigg{[}\frac{rad}{s}\bigg{]} \tag{106}\]  
Angular acceleration  
\[\varepsilon_{U}=3\ \bigg{[}\frac{rad}{s^{2}}\bigg{]},\ \ \varepsilon_{L}=-3\ \bigg{[}\frac{rad}{s^{2}}\bigg{]} \tag{107}\]  
**Experiment 1**. Let us consider variants for the AV motion planning along a curved road section with three lanes (Figure 4) under the condition of perfect adhesion with \(\varphi_{max}=0.85\). Note that many variants are possible regarding the lateral vehicle displacement \(L\). However, for the sake of compactness, we built three prediction variants related to the lane change along the lanes' midlines. The AV is surrounded by four moving obstacles denoted as \(v_{i}\), \(i\in[1,2,\ldots,4]\). Further, Index 0 denotes the initial position, and \(f\)--is the final position. The initial vehicles' speeds were, respectively, \(V_{i0}=\) (45, 50, 50, 60) km/h. Figure 4: Prediction of lane-change trajectories under the condition \(\varphi_{max}=0.85\). As can be seen, moving to Lane 1 requires the greatest change in curvature and, accordingly, a larger basis than for passing to Lane 3. At the same time, the smallest change in curvature on Lane 2 and the distant position of the impeding vehicle allows for the maximum acceleration and length of the prediction basis. Note that while driving along Lane 2, the trajectory forecast does not tend to coincide with the centerline but is formed in such a way as to ensure the least curvature. Figure 5 depicts the results of forecasts for speeds and accelerations along with restrictions.