# Fast Path Planning of Autonomous Vehicles in 3D Environments
## 3 Fast Path Planner
Let \(O\) indicate this uncovered open space. Utilizing the definition of a frontier, at least one sensor has a frontier on the boundary of \(O\). Therefore, \(R\) can find this frontierSensor. Theorem 1 shows that if \(R\) cannot find any frontierSensor, then the goal must be covered by a footprint of a sensor. Therefore, the path to the goal is constructed before \(R\) cannot find any frontierSensor. In this way, Algorithm 1 proceeds until a path to the goal is found. Theorem 2 proves that the real AV does not collide with an obstacle while moving along the path constructed utilizing Algorithm 1. This shows that Algorithm 1 generates a safe path for the real AV. Theorem 2.: _As the real AV moves along the path constructed utilizing Algorithm 1, it does not collide with an obstacle._  
Proof. : The path to \(n_{g}\) is constructed utilizing \(I\). Under the definition of \(I\), every edge, say \(\{n_{1},n_{2}\}\in E(I)\), indicates that \(L(\mathbf{n}_{1},\mathbf{n}_{2})\) is a collision-free path. Since the real AV moves along edges of \(I\) until meeting \(n_{g}\), the real AV does not collide with an obstacle during the maneuver. Moreover, utilizing the definition of \(n_{g}\), \(L(\mathbf{n}_{g},\mathbf{g})\) is a collision-free path. Thus, as the real AV moves along \(L(\mathbf{n}_{g},\mathbf{g})\), it does not collide with an obstacle. We proved this theorem. Theorem 1 shows that if \(R\) cannot find any frontierSensor, then the goal must be covered by a footprint of a sensor. Hence, Algorithm 1 continues until a path to the goal is found. _Handle the Case Where a Narrow Passage Exists_  
In practice, frontier points are generated to discretize a frontier with discrete points. At each sensor \(n\), we generate \(q\) points on the footprint boundary of \(n\), so that they are evenly spaced on the footprint boundary. As \(q\) increases, we obtain densely spaced points on a footprint boundary. If \(q\) is not sufficiently large, then we may have a case where a frontier point is not generated on the boundary of \(O\) (uncovered open space). This case may happen when \(R\) is at the entrance of a narrow passage (tunnel) in the workspace. For instance, let \(S\) define the entrance cross area of a narrow tunnel. The area of a footprint boundary is \(4\pi r_{s}^{2}\). Since \(q\) points are evenly generated on a footprint boundary, each point has its associated area as \(\frac{4\pi r_{s}^{2}}{q}\). In order to generate a frontier point on \(S\), it is required that \(\frac{4\pi r_{s}^{2}}{q}<S\). Thus, we require that  
\[q>\frac{4\pi r_{s}^{2}}{S} \tag{3}\]  
Equation (3) implies that as the footprint radius \(r_{s}\) decreases, we have more probability to make the AV move through a narrow tunnel. (3) further shows that as \(q\) increases, we have more probability to make the AV move through a narrow tunnel. If \(q\) is sufficiently large, then we can assure of finding a path to the goal utilizing Theorem 1. If we can access the minimum entrance cross area \(S\) in the environment, then we can select \(q\) that satisfies (3). In this way, our path planner can handle narrow tunnels.