# Fast Path Planning of Autonomous Vehicles in 3D Environments
## 2 Assumptions and Definitions  
Let _goal_ define a 3D point in an open area. We assume that the location of the goal is known to the AV. The problem solved in this paper is as follows: _generate a safe 3D path to the goal so that the AV can reach the goal as fast as possible while avoiding collision._  
Consider the case where we generate a path for the AV, that can be approximated as a sphere with radius \(r\). We utilize one _virtual agent_\(R\) to plan the path of the real AV. \(R\) is a sphere with radius \(r\) centered at \(\mathbf{R}\). Here, \(\mathbf{R}\in\mathcal{R}^{3}\) is the 3D position of \(R\). We plan the AV's path so that the virtual agent \(R\) with radius \(r\) does not collide with obstacles as it moves along the generated path. As we increase the radius \(r\), we increase the safety margin of the agent. This shows that as we increase \(r\), we plan the path while increasing the AV's safety. Note that the virtual agent \(R\) is utilized to generate a real AV's path under simulations. Since \(R\) moves in simulated (virtual) environments, we can make it move with unbounded speed in simulations. The term \"virtual\" is utilized to indicate that the agent exists in simulated environments. Note that as a real AV moves along the constructed path, it cannot move with infinite speed due to hardware limits. We say that \(R\)_collides_ with an obstacle if the sphere centered at \(\mathbf{R}\) meets an obstacle boundary. We plan the path of \(R\) so that \(R\) with radius \(r\) does not collide with an obstacle while traversing the constructed path. For convenience, let \(L(\mathbf{c}_{1},\mathbf{c}_{2})\) define a straight line segment connecting two 3D coordinates given by \(\mathbf{c}_{1}\in\mathcal{R}^{3}\) and \(\mathbf{c}_{2}\in\mathcal{R}^{3}\) respectively. We say that \(L(\mathbf{c}_{1},\mathbf{c}_{2})\) is a _collision-free_ path once the following condition is met: consider the case where \(R\) with radius \(r\) is located at \(\mathbf{c}_{1}\) initially. As \(R\) moves along \(L(\mathbf{c}_{1},\mathbf{c}_{2})\), \(R\) does not collide with an obstacle. In order to generate a 3D path to the goal as fast as possible, \(R\) deploys virtual sensors iteratively, such that the connected sensor network can be formed. The constructed sensor network serves as a topological map for the AV, and we find a shortest path from the start to the goal utilizing the network. Each virtual sensor has spherical sensing coverage with radius \(r_{s}\). Let _footprint_ of a sensor \(n\) define the sphere whose center corresponds to \(n\) and the radius of which is \(r_{s}\). Let _footprint boundary_\(S_{n}\) define the boundary of the footprint of \(n\). Let \(\mathbf{n}\in\mathcal{R}^{3}\) indicates the 3D coordinates of sensor \(n\). Recall that \(R\) deploys virtual sensors iteratively, such that the connected sensor network can be formed. Let the connectivity graph \(I\) be defined by a set \(I=(V(I),E(I))\), where \(V(I)\) is the vertex set and \(E(I)\) is the edge set. Every vertex in \(V(I)\) represents a deployed virtual sensor. Every edge, say \(\{n_{i},n_{j}\}\in E(I)\), indicates that \(L(\mathbf{n}_{i},\mathbf{n}_{j})\) is a collision-free path and that \(\|\mathbf{n}_{i}-\mathbf{n}_{j}\|<r_{c}\) such that \(r_{c}>r_{s}\). Here, \(\mathbf{n}_{i}\in\mathcal{R}^{3}\) indicates the 3D coordinates of sensor \(n_{i}\). In MATLAB simulations (Section 4), we utilize \(r_{c}=5*r_{s}\). In other words, the connection distance is 5 times longer than the footprint radius \(r_{s}\). For instance, Figure 1 shows an illustration of \(I\). \(I\) has five vertices \(n_{1},n_{2},n_{3},n_{4},n_{5}\). There is a rectangular obstacle in this figure. Also, the length of \(r_{c}\) is depicted at the bottom of this figure. Each edge in \(I\) is plotted with dashed line segments in Figure 1. See that each edge length is shorter than \(r_{c}\) and that each edge is a collision-free path. Figure 1: \(I\) has five vertices \(n_{1},n_{2},n_{3},n_{4},n_{5}\). There is a rectangular obstacle.