# Feasible Trajectories Generation for Autonomous Driving Vehicles
## 5 Speed Quartic Polynomial Trajectory Generation  
From reference [22] of trajectory generation for a nonholonomic system, for faster and smoother feasible trajectory generation, we propose a quartic function for generating the vehicle feasible trajectory:  
The vehicle dynamics in Equation (1) can be separated into four variables, \(z_{1}\), \(z_{2}\), \(z_{3}\), and \(z_{4}\).  
\[z_{1}=x,\;z_{2}=\frac{tan\phi}{lcos^{3}\theta},\;z_{3}=tan\theta,\;\text{and} \;z_{4}=y \tag{26}\]  
In this,  
\[\dot{z}_{1}=\dot{x} \tag{27}\]  
and  
\[\dot{z}_{2}=\frac{\dot{\phi}_{cos^{2}\phi}lcos^{2}\phi+\dot{d}lcos^{2}\phi sintan\phi}{lcos^{2}\phi} \tag{28}\]  
and  
\[\dot{z}_{3}=\frac{tan\phi}{lcos^{2}\theta}rv_{1} \tag{29}\]  
and  
\[\dot{z}_{4}=\dot{y}=sin\theta rv_{1} \tag{30}\]  
The vehicle travels from the starting point \((x_{0},y_{0},\theta_{0},\phi_{0})\) at time \(t=0\) to the destination point \((x_{T},y_{T},\theta_{T},\phi_{T})\) at time \(t=T\) similarly to the system in (26) from the new starting point at \((z_{1,0},z_{2,0},z_{3,0},z_{4,0})\) to the new destination point at \((z_{1,T},z_{2,T},z_{3,T},z_{4,T})\).  
For \(0\leq t\leq T\), the identification of \([z_{1}(t),z_{2}(t),z_{3}(t),z_{4}(t)]\) is:  
\[z_{1}(t)=z_{1.0}+gt \tag{31}\]  
and  
\[z_{2}(t)=z_{2.0}+h_{1}t+\frac{1}{2}h_{2}t^{2}+\frac{1}{3}h_{3}t^{3} \tag{32}\]  
and  
\[z_{3}(t)=z_{3.0}+gz_{2.0}t+\frac{1}{2}gh_{1}t^{2}+\frac{1}{6}gh_{2}t^{3}+\frac {1}{12}gh_{3}t^{4} \tag{33}\]  
Figure 7: Vehicle velocity vs. the steering angle.  
and  
\[z_{4}(t)=z_{4.0}+gz_{3.0}t+\frac{1}{2}g^{2}z_{2.0}t^{2}+\frac{1}{6}g^{2}h_{1}t^{3}+ \frac{1}{24}g^{2}h_{2}t^{4}+\frac{1}{60}g^{2}h_{3}t^{5} \tag{34}\]  
in which, \(g=\frac{z_{1.T}-z_{1.0}}{T}\) and \(\left[h_{1},h_{2},h_{3}\right]^{\prime}=D^{-1}e\), and  
\[D=\left[\begin{array}{ccc}T&\frac{1}{2}T^{2}&\frac{1}{3}T^{3}\\ \frac{1}{2}gT^{2}&\frac{1}{6}gT^{3}&\frac{1}{12}gT^{4}\\ \frac{1}{6}g^{2}T^{3}&\frac{1}{24}g^{2}T^{4}&\frac{1}{60}g^{2}T^{5}\end{array} \right]\text{ and }e=\left[\begin{array}{c}z_{2.T}-z_{2.0}\\ z_{3.T}-z_{3.0}-gz_{2.0}T\\ z_{4.T}-z_{4.0}-gz_{3.0}T-\frac{1}{2}g^{2}z_{2.0}T^{2}\end{array}\right]\]  
The new trajectory will be achieved from Equations (35)-(42):  
\[\theta=arctan(z_{3}) \tag{35}\]  
and  
\[\phi=arctan(z_{2}lcos^{3}\theta) \tag{36}\]  
and  
\[\dot{x}(t)=\dot{z}_{1}(t)=g \tag{37}\]  
and  
\[\dot{y}(t)=\dot{z}_{4}(t)=gz_{3.0}+g^{2}z_{2.0}t+\frac{1}{2}g^{2}h_{1}t^{2}+ \frac{1}{8}g^{2}h_{2}t^{3}+\frac{1}{12}g^{2}h_{3}t^{4} \tag{38}\]  
and  
\[v_{1}(t)=\frac{\sqrt{\dot{x}^{2}(t)+\dot{y}^{2}(t)}}{r} \tag{39}\]  
and  
\[\dot{\theta}=\dot{z}_{3}cos^{2}\theta=\left(gz_{2.0}+gh_{1}t+\frac{1}{2}gh_{2 }t^{2}+\frac{1}{3}gh_{3}t^{3}\right)cos^{2}\theta \tag{40}\]  
and from Equation (27)  
\[\dot{\phi}=\dot{z}_{2}(t)lcos^{3}\theta cos^{2}\phi-3\dot{\theta}tan\theta sin \phi cos\phi \tag{41}\]  
Figure 8 shows the new trajectory of the vehicle and the vehicle velocity.  
Figure 8: Vehicle trajectory vs. velocity.  
Figure 9 shows the steering angle, \(\phi\), and the vehicle body angle, \(\theta\), corresponding to the angular velocity \(\dot{\phi}\), and \(\dot{\theta}\).  
As shown in Figure 9, this feasible trajectory requires a maximum steering angle of \(\phi_{MAX}=41.5736^{\circ}\) and satisfied the steering angle constraint, \(-45^{\circ}\leq\phi\leq 45^{\circ}\). Thus, this method is better than the previous method in part 3 with \(\phi_{MAX}=62.870^{\circ}\) that violated the vehicle steering angle constraint. However, in this method, the vehicle velocity is increasing and not in a smooth curve. In reality, the speed of vehicle must be controlled under smooth changes. The next part proposes another method, which can generate smoother velocity changes.