# 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  
In this subsection, we perform an optimization-based LPP to generate a route from a start point to a goal. The lane-changing is a typical example and activated whenever motion planning orders. The Frenet coordinate [22; 29] applied to represent the vehicle's planar trajectory into a longitudinal and lateral axis as shown in Figure 3a. The coordinates are represented by s and d parameters, which are the ahead and side distance of the endpoint respect to the initial temporal position respectively. This work employs the temporary Frenet coordinate to efficiently solve an optimization problem, as shown in Figure 3b. The best way to represent the smooth of a driving route is the derivative of acceleration called jerk. Different from previous work [22; 29], an optimization problem for a spatial and temporal path with nonlinear constraints is introduced to minimize the sum of square jerks. The algorithm can also operate in SAG mode as presented in Section 2.1, which is a straightforward problem of this solution. Figure 3: Jerk minimization based-trajectory generation on the local Frenet coordinates. (**a**) the lane change scenario; (**b**) jerk minimization algorithm on the temporal Frenet coordinate. The problem is solved by minimizing the amount of squared-jerk over the whole temporary trajectory. The results [22; 29] indicated that a polynomial function could efficiently represent a trajectory as a function over time. Two quintic polynomial functions are applied to represent a path. Once having the polynomial functions, the trajectory can reconstruct by performing each value corresponding to a discrete-time step. The path regard to s and d coordinate are quintic functions as yielded  
\[z(t)=\xi_{0}+\xi_{1}t+\xi_{2}t^{2}+\xi_{3}t^{3}+\xi_{4}t^{4}+\xi_{5}t^{5}, \tag{1}\]  
where \(t\) is time (second) started counting from the initial pose of temporary trajectory, and \(\xi_{0:5}\) are the coefficients of the quintic function. The velocity, acceleration and jerk are respectively given by  
\[v_{z}=\frac{dz(t)}{dt}=\xi_{1}+2\xi_{2}t+3\xi_{3}t^{2}+4\xi_{4}t^{3}+5\xi_{5}t^ {4}. \tag{2}\]  
\[a_{z}=\frac{d^{2}z(t)}{dt^{2}}=\frac{d\upsilon(t)}{dt}=2\xi_{2}+6\xi_{3}t+12 \xi_{4}t^{2}+20\xi_{5}t^{3}. \tag{3}\]  
\[j_{z}=\frac{d^{3}z(t)}{dt^{3}}=\frac{d\upsilon^{2}(t)}{dt^{2}}=\frac{da(t)}{dt }=6\xi_{3}+24\xi_{4}t+60\xi_{5}t^{2}. \tag{4}\]  
We assume that the velocity vector of the initial and the end pose have the same length, and their direction coincides with the car heading angle. Therefore, the values at the initial pose \(\{s_{i},d_{i}\}\) and the endpoint \(\{s_{e},d_{e}\}\) are selected as  
\[\left\{ \begin{array}{l}s_{i}=0,\dot{s}_{i}=\upsilon,\dot{s}_{i}=0\\ d_{i}=0,\dot{d}_{i}=0\\ s_{e}=S,s_{e}=\upsilon,\dot{s}_{e}=0\\ d_{e}=D,d_{e}=0,\dot{d}_{e}=0\end{array} \right.. \tag{5}\]  
The cost function of the optimization problem is the sum of the entire square-jerk in the whole temporary trajectory for both s and d direction. The cost is expressed with s and d parameters in term of the quintic function, which is defined by  
\[ \begin{array}{l}\xi_{s}^{*},\xi_{d}^{*},T=\underset{\xi_{s},\xi_{d},T}{\arg \min}(\int\limits_{t=0}^{T}\dot{f}_{s}^{2}(t)dt+\int\limits_{t=0}^{T}\dot{f}_ {d}^{2}(t)dt)\\ =\underset{\xi_{s},\xi_{d},T}{\arg\min}\left(\int\limits_{t=0}^{T}\left(6\xi_ {3}^{*3}+24\xi_{4}^{*5}t+60\xi_{5}^{*2}t^{2}\right)^{2}dt+\int\limits_{t=0}^{T }\left(6\xi_{3}^{*d}+24\xi_{4}^{*d}t+60\xi_{5}^{*d}t^{2}\right)^{2}dt\right). \end{array}  \tag{6}\]  
Combining Equations (2) and (3) with their values at initial position (\(t=0\)) and Equation (5). The value of \(\xi_{0}\), \(\xi_{1}\) and \(\xi_{2}\) for both s and d coefficients are indicated undoubtedly as  
\[ \begin{array}{l}\xi_{0}=z(t=0)\\ \xi_{1}=\upsilon_{z}(t=0)\\ \xi_{2}=\frac{1}{2}a_{z}(t=0).\end{array}  \tag{7}\]  
Finally, six variables \(\xi_{3}\), \(\xi_{4}\) and \(\xi_{5}\) for both s and d need to be determined with their constrains at the end pose as yielded by  
\[