## 4 Dijkstra-Based Path Optimizer
### Path Optimization  
The smaller area will be transformed into a node map \(\mathbf{P}_{E}\), and the costs between each node in \(\mathbf{P}_{E}\) will be assessed. Therefore, the path optimization problem is modeled and transformed into a neighborhood cost assessment and discrete minimum cost sequence optimization problem. The minimum cost node sequence will be obtained via a Dijkstra-based optimizer. After the minimum cost node sequence is smoothed, the optimal path can be obtained. To that end, there are two path optimizers: Plan A and Plan B.  
Figure 8: An example of \(\mathbf{P}_{E}\). Figure 10: An example of \(\mathbf{P}_{c}\) generated by strategy Plan B. Every \(\mathbf{P}_{c}\) needs to be assessed. That is, every cost value between each two nodes in \(\mathbf{P}_{E}\) should be assessed. The cost node map \(\mathbf{M}_{m}\) can be obtained. So as the optimized path \(\mathbf{P}_{bt}\) can be obtained through a Dijkstra process [23]. \(\mathbf{P}_{bt}\) should be smoothed before it can be tracked by the vehicle. A Bezier spline curve is introduced to fit \(\mathbf{P}_{bt}\), making it smooth to meet the steering characteristics of the vehicle. An example smoothed path \(\mathbf{P}_{b}\) is shown in Figure 11.  
Here is an example of cost value assessment of connections between two nodes. The cost between each two nodes \(\mathbf{P}_{c}\) can be calculated. \[E=E_{road}+E_{dynamic}+E_{pre} \tag{13}\]  
1. \(E_{road}\) is the cost of road preference. In our scenario, the autonomous vehicle is a level-four vehicle driving in specific area, there is a map marked reference path (or a reference path generated by the mission planner) with constant 50 nodes \(\mathbf{P}_{ref}=[\mathbf{p}_{r1},\,\mathbf{p}_{r2},\mathbf{p}_{r3},\dots,\mathbf{p}_{rd},\mathbf{p}_{ r50}]\) to guide the vehicle. The path planner will perform local path planning based on \(\mathbf{P}_{ref}\), thus, the vehicle should not deviate too far from \(\mathbf{P}_{ref}\). For a given connection line \(\mathbf{P}_{c}\) between two of nodes in \(\mathbf{P}_{E},\mathbf{P}_{c}\) should be discrete into a path \(\mathbf{P}_{cd}\) according to the distance threshold \(thr_{E_{road}}\). \[\mathbf{P}_{cd}=\left[\mathbf{p}_{cd1},\mathbf{p}_{cd2},\mathbf{p}_{r3},\dots,\mathbf{p}_{cd_{cd}}\right]\] (14) where \(\mathbf{p}_{cdk_{cd}}\) represents the \(k\)th node, \(k_{cd}\) is the number of nodes included. If the length of \(\mathbf{P}_{c}\) in the s-axle direction is \(l_{\mathbf{P}_{c}}\). \[k_{cd}= \begin{cases}1&(\left[\frac{l_{\mathbf{P}_{c}}}{thr_{E_{road}}}\right]<1) \\ \left[\frac{l_{\mathbf{P}_{c}}}{thr_{E_{road}}}\right]&(\left[\frac{l_{\mathbf{P}_{c}} }{thr_{E_{road}}}\right]\geq 1)\end{cases} \] (15) If the offset between the node \(p_{cdk}\in\mathbf{P}_{cd}\) can be represented by \(l_{k}\). \(E_{road}\) of \(\mathbf{P}_{c}\) can be obtained \[E_{road}=\sum \begin{cases}\begin{array}{c}\zeta_{p1}l_{k}(l_{k}<R_{p1})\\ \zeta_{p2}l_{k}(R_{p1}\leq l_{k}\leq R_{p2})\\ \zeta_{p3}l_{k}(l_{k}>R_{p2})\end{array}\end{cases} \] (16) where \(\zeta_{p1}\), \(\zeta_{p2}\), and \(\zeta_{p3}\) are gain coefficients, \(R_{p1}\) and \(R_{p2}\) are offset thresholds, \(k=[1,2,3,\dots,k_{cd}]\). An example \(E_{road}\) is shown in Figure 4a. (2) \(E_{dynamic}\) is the cost of vehicle dynamics. Vehicle dynamics constraints cannot be ignored. Generally, the vehicle's lateral acceleration can be used to calculate the cost: the smaller the cost, the smoother the path. However, the lateral acceleration cannot be directly estimated in our node map. Road curvature and relative lateral offset (under the Frenet coordinate) are chosen to calculate the cost of lateral acceleration. \[E_{dynamic}=\zeta_{la1}E_{curv}+\zeta_{la2}E_{lat}\] (17)  
Figure 11: An example of smoothed \(\mathbf{P}_{b}\). where \(\zeta_{l41}\) and \(\zeta_{l42}\) are two gain factors, \(E_{curv}\) is the cost caused by the road curvature, \(E_{lat}\) if the cost of road offset value.