# Simultaneous Trajectory and Speed Planning for Autonomous Vehicles Considering Maneuver Variants
## 4 Optimization and Constraints
### Constraints
To determine the initial parameters \(\mathbf{q}_{tr0}\) and \(\mathbf{q}_{sp0}\), the expressions of Equations (32)-(40) and (49)-(57) are to be used. In Equation (10), the initial parameters for the first cycle are not set. To form geometric nodal parameters, first, the nearest point [\(x_{r0}\), \(y_{r0}\)] corresponding to the zero-path length \(s_{r0}\) is determined on the reference curve. This allows for the estimation of the initial distance \(L_{0}\) to the reference provided that the center of the Cartesian coordinate system is tied to the vehicle's mass center. Then, \(x_{0}=0\), \(y_{0}=0\). Assuming that at the initial time moment, AV follows parallel to the reference,  
\[\left.\frac{dy}{dx}\right|_{0}=\left.\frac{dy_{\tau}}{dx_{\tau}}\right|_{s_{r}=0} \text{, subject to }\left.\frac{dL}{dx}\right|_{0}=0 \tag{91}\]  
We determine the next initial parameter from the curvatures of the trajectory and reference. By analogy with Equations (34) and (35)  
\[K_{r}=\frac{y_{rx}^{\prime\prime}}{s_{rx}^{\prime 3}},\ \ K_{t}=\frac{y_{tx}^{ \prime\prime}}{s_{tx}^{\prime 3}},\ \ K_{t}=\frac{1}{R_{r}-L}=\frac{1}{\frac{1-LK_{r}}{K_{r}}}= \frac{K_{r}}{1-LK_{r}} \tag{92}\]  
where \(r\), \(t=\) reference and trajectory, correspondingly. Then,  
\[\left.\frac{d^{2}y}{dx^{2}}\right|_{0}=y_{tx}^{\prime\prime}=K_{t}s_{tx}^{ \prime 3},\ \ y_{tx}^{\prime\prime}=\frac{s_{tx}^{\prime 3}}{1-Ly_{rx}^{ \prime}/s_{rx}^{\prime 3}}\frac{y_{rx}^{\prime\prime}}{s_{rx}^{\prime 3}}=\frac{y_{rx}^{ \prime\prime}s_{rx}^{\prime 3}}{s_{rx}^{\prime 3}+Ly_{rx}^{\prime\prime}} \tag{93}\]  
Therefore, due to Equations (29) and (91), for the points \([x_{r0},y_{r0}]\) u \([x_{0},y_{0}]\)  
\[s_{tx}^{\prime}=s_{rx}^{\prime},\ \ s_{tx}^{\prime\prime}=s_{rx}^{\prime\prime} \tag{94}\]  
To work with speed derivatives, we need the trajectory's third derivative, which can be obtained from Equation (93)  
\[y_{tx}^{\prime\prime\prime}=\frac{d}{dx}\bigg{(}\frac{y_{x}^{\prime\prime}s_{ tx}^{\prime 3}}{s_{x}^{\prime 3}+Ly_{x}^{\prime\prime}}\bigg{)}=\frac{ \big{(}s_{tx}^{\prime 3}y_{x}^{\prime\prime}+3y_{x}^{\prime\prime}s_{tx}^{ \prime 2}s_{tx}^{\prime\prime}\big{)}\big{(}s_{x}^{\prime 3}+Ly_{x}^{ \prime\prime}\big{)}-\big{(}y_{x}^{\prime\prime}s_{tx}^{\prime 3}\big{)}\big{(}3s_{x}^{ \prime 2}s_{tx}^{\prime\prime}+y_{x}^{\prime\prime}\frac{dL}{dx}+Ly_{x}^{ \prime\prime}\big{)}}{\big{(}s_{x}^{\prime 3}+Ly_{x}^{\prime\prime}\big{)}^{2}} \tag{95}\]  
Or after considering Equation (91)  
\[y_{tx}^{\prime\prime\prime}=\frac{s_{rx}^{\prime 2}\big{(}\big{(}s_{rx}^{ \prime}y_{rx}^{\prime\prime\prime}+3y_{rx}^{\prime\prime}s_{rx}^{\prime\prime }\big{)}\big{(}s_{rx}^{\prime 3}+Ly_{rx}^{\prime\prime}\big{)}-y_{rx}^{\prime\prime}s_{rx}^{ \prime}\big{(}3s_{rx}^{\prime 2}s_{rx}^{\prime\prime}+Ly_{rx}^{\prime\prime\prime} \big{)}\big{)}}{\big{(}s_{rx}^{\prime 3}+Ly_{rx}^{\prime\prime}\big{)}^{2}} \tag{96}\]  
If the reference is mathematically predefined, the first three derivatives are easy to obtain. To define the initial speed parameters, the values \(V_{\zeta 0}\), \(a_{\zeta 0}\), \(j_{\zeta 0}\), and the expressions Equations (47)-(57) are used.