# Simultaneous Trajectory and Speed Planning for Autonomous Vehicles Considering Maneuver Variants
## 3 Motion-Modeling Technique
### Basics of Vehicle Kinematics Model  
The vehicle's mass center trajectory is geometrically related to the longitudinal \(V_{\zeta}\) and transverse \(V_{\mu}\) speeds (Figure 2) based on the assumption that there is no tire's sideslip. The mass center's absolute speed may be decomposed as follows  
\[\overset{\rightarrow}{V}=V\overset{\rightarrow}{\tau}+0\overset{ \rightarrow}{\
u}=V_{\zeta}\overset{\rightarrow}{u}_{\zeta}+V_{\mu}\overset{ \rightarrow}{u}_{\mu}=V_{x}\overset{\rightarrow}{u}_{x}+V_{y}\overset{ \rightarrow}{u}_{y} \tag{47}\]  
where \(\overset{\rightarrow}{\tau}\), \(\overset{\rightarrow}{\
u}\) = natural coordinate system's unit vectors, \(\overset{\rightarrow}{u}_{\zeta}\), \(\overset{\rightarrow}{u}_{\mu}\) = unit vectors of the vehicle local coordinate system \(\zeta\mu\), and \(\overset{\rightarrow}{u}_{x}\), \(\overset{\rightarrow}{u}_{y}\), \(V_{x}\), \(V_{y}\) = unit vectors and speed components of the fixed (global) coordinate system \(xy\). The longitudinal speed \(V_{\zeta}\) and its \(k\)-th derivatives, according to Equation (9), is expressed in the forms  
\[V_{\zeta}(x)=\mathbf{Q}_{sp}^{T}\mathbf{D}_{d}\mathbf{F}_{\zeta},\ \ \ \frac{d^{k}V_{\zeta}}{dx^{k}}=\mathbf{Q}_{sp}^{T}\left(\frac{\mathbf{D}_{d}}{D^{k }}\right)\frac{d^{k}\mathbf{F}_{\zeta}}{d\xi^{k}} \tag{48}\]  
To obtain the kinematic parameters related to the AV dynamics, the following derivatives concerning the time \(t\) are needed  
\[\frac{dV_{\zeta}}{dt}=\frac{dV_{\zeta}}{dx}\frac{dx}{dt}=\frac{dV_{\zeta}}{dx} V_{x},\ \ \ \frac{d^{2}V_{\zeta}}{dt^{2}}=\frac{d^{2}V_{\zeta}}{dx^{2}}V_{x}^{2}+\frac{dV_{ \zeta}}{dx}\frac{dV_{x}}{dx}V_{x} \tag{49}\]The projection \(V_{x}\) and longitudinal speed \(V_{\zeta}\) are tied through the tangent angle \(\alpha\) and central slip angle \(\beta\). Then,  
\[V=\frac{ds}{dt}=\frac{ds}{dx}\frac{dx}{dt}=\frac{V_{x}}{cos(\alpha)}=\frac{V_{ \zeta}}{cos(\beta)},\ \ \ V_{x}=V_{\zeta}\frac{cos(\alpha)}{cos(\beta)} \tag{50}\]  
The first derivative concerning the coordinate \(x\) is  
\[\frac{dV_{x}}{dx}=\left(\frac{dV_{\zeta}}{dx}+V_{\zeta}\left(\frac{d\beta}{dx} tan(\beta)-\frac{d\alpha}{dx}tan(\alpha)\right)\right)\frac{cos(\alpha)}{ cos(\beta)} \tag{51}\]  
The lateral speed \(V_{\mu}\) is defined by the longitudinal component \(V_{\zeta}\) and the central slip angle \(\beta\)  
\[V_{\mu}=V_{\zeta}tan(\beta) \tag{52}\]  
The derivatives concerning time and \(x\)-coordinate obtain  
\[ \begin{array}{c}\frac{dV_{y}}{dt}=\frac{dV_{y}}{dx}\frac{dx}{dt}=\frac{dV_{y }}{dx}V_{x},\ \ \ \frac{dV_{y}}{dx}=\frac{dV_{\zeta}}{dx}tan(\beta)+V_{\zeta}\frac{d\beta}{dx} sec^{2}(\beta),\\ \frac{d^{2}V_{y}}{dt^{2}}=\left(\frac{d^{2}V_{y}}{dx^{2}}V_{x}+\frac{dV_{y}}{ dx}\frac{dV_{x}}{dx}\right)V_{x},\\ \frac{d^{2}V_{y}}{dx^{2}}=\frac{d^{2}V_{\zeta}}{dx^{2}}tan(\beta)+\left(V_{ \zeta}\frac{d^{2}\beta}{dx}+2\frac{d\beta}{dx}\right)\end{array}  \tag{53}\]  
The yaw rate is the derivation of yaw angle \(\phi=\alpha-\beta\). Thus, its view and derivative component concerning the \(x\)-coordinate are  
\[\omega=\frac{d\phi}{dt}=\frac{d\phi}{dx}\frac{dx}{dt}=\frac{d\phi}{dx}V_{x},\ \ \ \frac{d\omega}{dx}=\frac{d}{dx}\left(\frac{d\phi}{dx}V_{x}\right)=\frac{d^{2} \phi}{dx^{2}}V_{x}+\frac{dV_{x}}{dx}\frac{d\phi}{dx}, \tag{54}\]  
The angular acceleration \(\varepsilon\) is the derivative of the yaw rate \(\omega\) concerning the time \(t\)  
\[\varepsilon=\frac{d\omega}{dt}=\frac{d\omega}{dx}\frac{dx}{dt}=\frac{d\omega} {dx}V_{x}=\frac{d^{2}\phi}{dx^{2}}V_{x}^{2}+\frac{dV_{x}}{dx}\omega \tag{55}\]  
The longitudinal and lateral accelerations in the vehicle's local coordinates \(\zeta\mu\)  
\[\overset{\rightarrow}{a}=\frac{d\overset{\rightarrow}{V}}{dt}= \begin{pmatrix} a_{\zeta}\\ a_{\mu}\end{pmatrix} ^{T} \begin{pmatrix}\overset{\rightarrow}{u}_{\zeta}\\ \overset{\rightarrow}{u}_{\mu}\end{pmatrix} ,\ \ \ \begin{pmatrix}a_{\zeta}\\ a_{\mu}\end{pmatrix} = \begin{pmatrix}\frac{dV_{\zeta}}{dt}-\omega V_{\mu}\\ \frac{dV_{\zeta}}{dt}+\omega V_{\zeta}\end{pmatrix}