# Feasible Trajectories Generation for Autonomous Driving Vehicles
## 3 Position Quinic Polynomial for Trajectory Generation  
A basic position quintic polynomial for trajectory generation can be achieved from the nonlinear vehicle movement curve along the \(x\) and \(y\) axes on the flat system as referred to in [21]. Then, the control input \(v_{1}\) can be identified as:  
\[rv_{1}=\sqrt{\dot{x}^{2}+\dot{y}^{2}}\Rightarrow v_{1}=\frac{\sqrt{\dot{x}^{2 }+\dot{y}^{2}}}{r} \tag{4}\]  
The vehicle body angle, \(\theta\), along the \(x\) axis can be formulated:  
\[\frac{\dot{y}}{\dot{x}}=\frac{rv_{1}sin\theta}{rv_{1}cos\theta}=tan\theta \Rightarrow\theta=arctan\left(\frac{\dot{y}}{\dot{x}}\right) \tag{5}\]  
Then, the vehicle body angular velocity, \(\dot{\theta}\), will always be identical:  
\[\dot{\theta}=\frac{\ddot{y}\dot{x}\ -\ \ddot{x}\dot{y}}{\dot{x}^{2}}\frac{1}{ \left(\frac{\dot{y}}{\dot{x}}\right)^{2}+1}=\frac{\ddot{y}\dot{x}-\ddot{x}\dot {y}}{\dot{x}^{2}+\dot{y}^{2}}=\frac{tan\phi}{l}rv_{1}\Rightarrow\phi=arctan \left(l\frac{\ddot{y}\dot{x}-\ddot{x}\dot{y}}{r\left(\dot{x}^{2}+\dot{y}^{2} \right)^{\frac{3}{2}}}\right) \tag{6}\]  
The vehicle steering angle \(\phi\) and the body angle \(\theta\) in Equations (5) and (6) can be formulated directly from \(\dot{x}\), \(\dot{x}\), and \(\dot{y}\), \(\ddot{y}\). Similarly, all state variables and inputs can be represented by the output coordinates \(x\) and \(y\). It is assumed that the vehicle moves from the starting point at time \(t=0\) to the destination point at \(t=T\) for \(x(0)\), \(y(0)\) and \(x(T)\), \(y(T)\) from \(t=0\div T\) at the starting point of \(x(t)=x(0)\):  
\[x(0)=x_{0}\Rightarrow x(T)=x_{T} \tag{7}\]  
and at the starting point of \(y(t)=y(0)\)  
\[y(0)=y_{0}=tan\theta_{0}\Rightarrow\frac{\partial^{2}y}{\partial x^{2}}|t=0 \ =\frac{tan\phi_{0}}{lcos^{3}\theta_{0}} \tag{8}\]At the destination point \(y(t)=y(T)\)  
\[y(T)=y_{T}=tan\theta_{T}\Rightarrow\frac{\partial^{2}y}{\partial x^{2}}|t=T= \frac{tan\phi_{T}}{lcos^{3}\theta_{T}} \tag{9}\]  
It is assumed that \(\frac{|x_{T}-x_{0}|}{2T}>0\), then the trajectory of \(x(t)\) can be chosen by the following Equation:  
\[x(t)=\left(\frac{T-t}{T}\right)x_{0}+\frac{t}{T}x_{T}+|x_{T}-x_{0}|\frac{t(t-T )}{2T^{2}} \tag{10}\]  
Then, \(y(t)\) can be chosen as  
\[y(t)=y_{0}+t\alpha_{1}tan\theta_{0}+t^{2}\frac{\alpha_{2}tan\phi_{0}}{2 lcos^{ 3}\theta_{0}}+t^{3}b_{1}+t^{4}b_{2}+t^{5}b_{3} \tag{11}\]  
where \(\alpha_{1}=\frac{2(x_{T}-x_{0})-|x_{T}-x_{0}|}{2T}\), \(\alpha_{2}=\frac{|x_{T}-x_{0}|}{T^{2}}\), and \(\alpha_{3}=\frac{2(x_{T}-x_{0})+|x_{T}-x_{0}|}{2T}\); and \(b=[b_{1},b_{2},b_{3}]^{\prime}=A^{-1}c\), in which:  
\[A=\left[ \begin{array}{ccc}T^{3}&T^{4}&T^{5}\\ 3T^{2}&4T^{3}&5T^{4}\\ 6T&12T^{2}&20T^{3}\end{array} \right]\text{and }c=\left[ \begin{array}{c}y_{T}-y_{0}-T \alpha_{1}tan\theta_{0}-T^{2}\frac{\alpha_{2}tan\phi_{0}}{2 lcos^{3}\theta_{0} }\\ \alpha_{3}tan\theta_{T}-\alpha_{1}tan\theta_{0}-T\frac{\alpha_{2}tan\phi_{0}}{ lcos^{3}\theta_{0}}\\ \frac{\alpha_{2}tan\phi_{T}}{lcos^{3}\theta_{T}}-\frac{\alpha_{2}tan\phi_{0}}{ lcos^{3}\theta_{0}}\end{array} \right]\]  
Then, the vehicle body angle, \(\theta\), is  
\[\theta=arctan\left(\frac{2T^{2}\left(\alpha_{1}tan\theta_{0}+\frac{\alpha_{2 }tan\phi_{0}}{lcos^{3}\theta_{0}}t+3b_{1}t^{2}+4b_{2}t^{3}+5b_{3}t^{4}\right)} {\left(2T(x_{T}-x_{0})-T|x_{T}-x_{0}|+2|x_{T}-x_{0}|t\right)}\right) \tag{12}\]  
and the vehicle steering angle, \(\phi\), is  
\[ \begin{array}{l}\phi=arctan\left(\frac{\partial^{2}(y)lcos^{3}\theta}{ \partial x^{2}}\right)\\ =arctan\left(\frac{\left(\frac{\alpha_{2}tan\theta_{0}}{lcos^{3}\theta_{0}}+6b _{1}t+12b_{2}t^{2}+20b_{3}t^{3}\right)\left(2T^{2}\right)^{2}lcos^{3}\theta} {\left(2T(x_{T}-x_{0})-T|x_{T}-x_{0}|+2|x_{T}-x_{0}|t\right)^{2}}\right)\end{array}