# A Hierarchical Control System for Autonomous Driving towards Urban Challenges
## 2 Hierarchical Control System
### Local Path Planning
#### 2.2.1 Optimization-Based LPP on the Local Frenet Coordinate
\begin{array}{l}z^{T}=\xi_{0}+\xi_{1}T+\xi_{2}T^{2}+\xi_{3}T^{3}+\xi_{4}T^{4 }+\xi_{5}T^{5}\\ v_{z}^{T}=\xi_{1}+2\xi_{2}T+3\xi_{3}T^{2}+4\xi_{4}T^{3}+5\xi_{5}T^{4}\\ a_{z}^{T}=2\xi_{2}+6\xi_{3}T+12\xi_{4}^{*}T^{2}+20\xi_{5}T^{3}.\end{array}  \tag{8}\]Thanks to Equations (6) and (8), the optimization problem is established to solve for \(T\), \(\xi_{3}\), \(\xi_{4}\) and \(\xi_{5}\) given as  
\[\xi_{s}^{*},\xi_{d^{\prime}}^{*},T^{*}=\underset{\xi_{s},\xi_{d},T}{ \arg\min}\left(J_{z}(\xi,T)\right)=\underset{\xi_{s},\xi_{d},T}{\arg\min}\left(J _{s}(s_{0:5}^{s},T)+J_{d}(\xi_{0:5}^{d},T)\right), \tag{9}\] \[s.t.\] \[\left\{ \begin{array}{l}\xi_{0}^{s}+\xi_{1}^{s}T+\xi_{2}^{s}T^{2 }+\xi_{3}^{s}T^{3}+\xi_{4}^{s}T^{4}+\xi_{5}^{s}T^{5}-z_{s}^{T}=0\\ \xi_{1}^{s}+2\xi_{2}^{s}T+3\xi_{3}^{s}T^{2}+4\xi_{4}^{s}T^{3}+5\xi_{5}^{s}T^{4} -v_{s}^{T}=0\\ 2\xi_{2}^{s}+6\xi_{3}^{s}T+12\xi_{3}^{s}T^{2}+20\xi_{5}^{s}T^{3}-a_{s}^{T}=0\\ \xi_{0}^{d}+\xi_{1}^{d}T+\xi_{2}^{d}T^{2}+\xi_{3}^{d}T^{3}+\xi_{4}^{d}T^{4}+\xi _{5}^{d}T^{5}-z_{d}^{T}=0\\ \xi_{1}^{d}+2\xi_{2}^{d}T+3\xi_{3}^{d}T^{2}+4\xi_{4}^{d}T^{3}+5\xi_{5}^{d}T^{4} -v_{d}^{T}=0\\ 2\xi_{2}^{d}+6\xi_{3}^{d}T+12\xi_{4}^{d}T^{2}+20\xi_{5}^{d}T^{3}-a_{d}^{T}=0\\ T>0\end{array} \right.\]  
The results of optimization problem in Equation (9) are obtained in Figure 4. It should be noticed that using constrain \(T>0\) in the experiment approximately increases twice the computational cost. Therefore, we drop constrain \(T>0\) to relax the optimization problem, later only need to check the condition. For different velocities, the optimal trajectory is solved and presented as the solid blue line in Figure 4. As observed from Figure 4, when \(v\) large, it significantly affect the results. In this case, if the value of \(T\) is selected slightly different from the optimal value, the result can not produce an optimal trajectory. Moreover, when the travel time \(T\) from the initial point to end point is known, we can easily find \(\xi_{3}\), \(\xi_{4}\) and \(\xi_{5}\) by using Equations (7) and (8) as follows  
\[\left[ \begin{array}{ccc}T^{3}&T^{4}&T^{5}\\ 3T^{2}&4T^{3}&5T^{4}\\ 6T&12T^{2}&20T^{3}\end{array} \right]\left[ \begin{array}{c}\xi_{3}\\ \xi_{4}\\ \xi_{5}\end{array} \right]=\left[ \begin{array}{c}z^{t=T}-\left(\xi_{0}+\xi_ {1}T+\xi_{2}T^{2}\right)\\ v_{z}^{t=T}-\left(\xi_{1}+2\xi_{2}T\right)\\ a_{z}^{t=T}-2\xi_{2}\end{array} \right]. \tag{10}\]  
Figure 4: Optimization solving based-jerk minimization problem, (a) s = 15 m; \(v\) = 5 m/s; d = 3.5 m; (b) s = 20 m; \(v\) = 5m/s; d = 3.5 m; (c) s = 20 m; \(v\) = 10 m/s; d = 3.5 m; (d) s = 30 m; \(v\) = 5 m/s; d = 3.5 m.  
The last step is developing the quintic function of the s and d axis to reconstruct the trajectory at each discrete time as shown in Figure 3b. The obtained results are presented in Figure 5. To reduce the computational complexity, a practical solution uses a lookup table determined by the offline optimization solving process. Whenever the DMM informs lane changing, the lookup table is utilized online to find the optimal time of \(T\), then solve the linear in Equation (10) to get the optimal path.