# Fast Path Planning of Autonomous Vehicles in 3D Environments
## 3 Fast Path Planner  
A time-efficient 3D path planner utilizing one virtual agent \(R\) is addressed in Algorithm 1. We address Algorithm 1 in detail. Algorithms 2 and 3 are sub-algorithms of Algorithm 1. At the initial phase of Algorithm 1, we generate a virtual agent \(R\) at the position of a real AV. \(R\) deploys a virtual sensor at its position. Let \(n_{init}\) define a virtual sensor deployed at the position of a real AV. Figure 3: \(R\) deploying a new virtual sensor. Frontier points are marked as large dots on the footprint boundary of one sensor. Whenever \(R\) deploys a new virtual sensor, the sensor is connected to the network and we update \(I\). Furthermore, sensing capability of the sensor is enabled. Recall that each virtual sensor has footprint radius \(r_{s}\). Algorithm 2 is utilized to generate candidate frontier points. Furthermore, Algorithm 3 is utilized to make \(R\) jump to a frontierSensor, say \(n^{c}\), which is closest to the goal \(\mathbf{g}\). Here, we say that a sensor is a _frontierSensor_ if the sensor has a frontier point. Here, \(R\) jumps to a frontierSensor, which is closest to \(\mathbf{g}\), since we want to find a shortest path to \(\mathbf{g}\). ```
1:Generate the goal-closest point of \(n\) and set the goal-closest point as the first candidate frontier point;
2:Generate \(q\) candidate frontier points on \(S_{n}\);
3:for i = 1:1:q + 1 do
4:if\(i\)-th candidate frontier point satisfies the conditions for a frontier point then
5: Store the candidate frontier point as a frontier point of \(n\);
6:endif
7:endfor
```  
**Algorithm 2**\(FrontierGenerate(n)\)  
In Algorithm 3, \(n^{c}\) is a frontierSensor, that is closest to the goal \(\mathbf{g}\). In Algorithm 3, \(f_{R}\) is a frontier point of \(n^{c}\), that will be visited by \(R\) after \(R\) jumps to \(n^{c}\). If \(n^{c}\) has the goal-closest point as a frontier point, then \(R\) sets the goal-closest point as \(f_{R}\). Otherwise, \(R\) randomly selects one frontier point, which is not the goal-closest point, as \(f_{R}\). Due to this random selection, our approach is based on random sampling. Thereafter, \(R\) jumps to \(f_{R}\). In this way, \(R\) visits the goal-closest point of \(n^{c}\) before visiting other frontier points of \(n^{c}\). This makes \(R\) head towards the goal if possible. This movement is utilized for minimizing the path length to the goal. In this way, the virtual agent's maneuver is biased towards the goal, in order to find a path to the goal as fast as possible. Our approach is similar to A-star planners [12], since A-star planners utilize heuristics for guiding its search towards the goal. Once \(R\) reaches \(f_{R}\), it deploys a new sensor at \(f_{R}\). Whenever a new sensor is deployed, we enable the sensing capability of the sensor. Thereafter, every frontier point within \(r_{s}\) distance from the sensor is removed. Algorithm 1 iterates until the newly deployed sensor \(n\) satisfies that \(L(\mathbf{n},\mathbf{g})\) is a collision-free path. The sensor \(n\) is set as \(n_{g}\), representing the sensor nearest to the goal. We then generate a shortest path from \(n_{init}\) to \(n_{g}\) utilizing \(I\). Then, the real AV moves along the generated shortest path. After the AV moves along the generated path, it moves along \(L(\mathbf{n}_{g},\mathbf{g})\) until reaching \(\mathbf{g}\). As far as we know, this direct maneuver towards the goal was not utilized in other path planners, such as RRT-star or A-star planners. Algorithm 1 cannot proceed if \(R\) cannot find any frontierSensor. The following theorem proves that if \(R\) cannot find any frontierSensor, then the entire open space is covered by footprints. Theorem 1: _If \(R\) cannot find any frontierSensor, then the entire open space is covered by footprints._Proof. : Under the transposition rule, we prove the following statement: if an open space, which has not been covered by footprints, exists, then \(R\) can find a frontierSensor. Suppose that an open space, which has not been covered by footprints, exists.