## 4 Dijkstra-Based Path Optimizer  
### Guideline Expansion  
In this section, the simplified guideline \(\mathbf{P}_{s}\) will be expanded into a small planning area. In fact, the smaller passable area (or the smaller planning area) is generated at this step, \(\mathbf{P}_{s}\) obtained in Section 3 is just a guideline instead of an area. \(\mathbf{P}_{E}\) represents a smaller planning area constructed from discrete nodes. It can be obtained after \(\mathbf{P}_{s}\) is processed via a two-step expansion process, in both the s direction and the d direction (under the Frenet coordinate system). The expansion process is made up of the following two aspects.  
(1) Expansion along s-axle direction.  
An isometric sampling strategy is introduced to expand \(\mathbf{P}_{s}\). The nodes expect the first node \(\mathbf{p}_{1}\in\mathbf{P}_{S}\) (start point) and the last node \(\mathbf{p}_{n_{p}}\in\mathbf{P}_{S}\) (goal point) should be expanded. For a given simplified path \(\mathbf{P}_{S}=\left[\mathbf{p}_{p1}\), \(\mathbf{p}_{p2}\), \(\mathbf{p}_{p3}\), \(\dots\), \(\mathbf{p}_{p(n_{p}-1)}\), \(\mathbf{p}_{pn_{p}}\right]\) (\(\mathbf{p}_{k(k=1,2,3,\dots,n_{p})}=\left[s_{k},\ d_{k}\right]\)), the expanded node sequence \(\mathbf{P}_{ESk}\) at the node \(\mathbf{p}_{k}\in\mathbf{P}_{S}\) can be calculated.  
\[\mathbf{P}_{ESk}=\left[\mathbf{p}_{ES1},\ \mathbf{p}_{ES2},\ \mathbf{p}_{ES3}\), \(\dots\), \(\mathbf{p}_{ES(k_{s}-1)}\), \(\mathbf{p}_{ESn_{ES}}\right] \tag{6}\]  
\[\mathbf{p}_{ESk_{s}}=\left\{\begin{array}{ll}\mathbf{L}\mathbf{o}_{fre}&(k=1)\\ \mathbf{p}_{pn_{p}}&(k=n_{p})\\ \left[s_{n}+k_{s}\times th_{s},d_{n}\right](k=2,3,\dots,n_{p}-1)\end{array}\right. \tag{7}\]  
where \(\mathbf{P}_{ESn}=\left[s_{ESn},d_{n}\right]\) represents the node in \(\mathbf{P}_{ES}\), \(thr_{s}\) is a distance threshold between adjacent nodes, \(k\) represents the serial number of node \(\mathbf{p}_{k}\in\mathbf{P}_{S}\), \(k_{s}=(1,2,3,4,\dots,n_{ES})\), \(n_{ES}\) is the number of nodes expanded at \(\mathbf{p}_{k}\). An example of \(\mathbf{P}_{ES}\) is shown by the red dots in Figure 7. Thus, the expanded node sequence \(\mathbf{P}_{ES}\) along s-axle can be obtained, that is:  
\[\mathbf{P}_{ES}=\left[\mathbf{L}\mathbf{o}_{fre},\mathbf{P}_{ES2},\mathbf{P}_{ES3}, \dots,\mathbf{P}_{ES(n-1)},\mathbf{p}_{pn_{p}}\right] \tag{8}\]  
(2) Expansion Along d-axle Direction.  
For a given path \(\mathbf{P}_{ES}\), the nodes expect the first node \(\mathbf{P}_{ES1}\) (start point) and the last node \(\mathbf{P}_{ESn}\) (goal point) should be expanded. If \(\mathbf{P}_{ES}\) contains \(n_{s}\) nodes, the node sequence \(\mathbf{P}_{Edk_{d}}\) at node \(\mathbf{p}_{ESk_{s}}\) (\(k_{s}=1,2,3,\dots,n_{s}\)) \(\in\mathbf{P}_{ES}\) can be calculated  
\[\mathbf{P}_{Edk_{d}}=\left[\mathbf{p}_{Ed1},\ \mathbf{p}_{Ed2},\ \mathbf{p}_{Ed3}\dots, \mathbf{p}_{Ed(k_{d}-1)},\mathbf{p}_{ESk_{d}}\right] \tag{9}\]  
\[\mathbf{p}_{ESk_{d}}=\left\{\begin{array}{ll}\mathbf{L}\mathbf{o}_{fre}&(k_{s}=1))\\ \mathbf{p}_{pn_{p}}&(k=n_{p}\\ \left[s_{ks},d_{k}\right](k=2,3,4,\dots,n-1)\end{array}\right. \tag{10}\]  
\[d_{k_{d}}=\left\{\begin{array}{ll}\frac{k_{d}\left|d_{d}-d_{er}\right|}{thr _{d}}+d_{er}&(|d_{el}-d_{er}|\leq thr_{er})\\ \frac{k_{d}\times thr_{er}}{thr_{d}}+d_{er}&(|d_{el}-d_{er}|>thr_{er})\end{array}\right. \tag{11}\]  
\[\mathbf{P}_{E}=\left[\mathbf{L}\mathbf{o}_{fre},\mathbf{P}_{Ed2},\mathbf{P}_{Ed3},\dots,\mathbf{p}_{pn_ {p}}\right] \tag{12}\]  
Figure 7: An example of P_ES.  
where the road edge at node \(\mathbf{P}_{S}\) is \(d_{el}\) and \(d_{er}\), the threshold distance for sampling is \(thr_{d}\), \(k_{d}=\left(\frac{1.2,3,\ldots,\max(|d_{d}-d_{er}|,thr_{er})}{thr_{er}}\right)\) represents the number of expansions nodes along d-axle, \(thr_{er}=d_{el}-d_{er}\) is the sampling range threshold at node \(\mathbf{p}_{ESk_{s}}\). An example of \(\mathbf{P}_{E}\) is shown by the red dots in Figure 8.