Pathfinding in a 3D Grid for UAV Navigation

In this paper, we propose an approach for computing the 3D trajectory of UAVs between two locations when obstacles are present. The result is an obstruction free path, close to optimal. First, the precomputed 3D environment map is converted into a discrete voxel space. Next, the $\mathrm{A}^{*}$ algorithm is applied on the discretized space. The $\mathrm{A}^{*}$ method provides the optimal path with respect to the graph equivalent representation of the voxel space, however in the continuous 3D space the resulting path will cause unnecessary steering maneuvers for the UAV. The solution is to smooth the resulting path using an iterative linear approximation approach. A new representation of the 3D path is obtained, consisting of line segments and control points. Therefore, we manage to transform the $\mathrm{A}^{*}$ path from the graph equivalent representation of the voxel space to the continuous representation of the 3D environment. The resulting control points can be used as intermediate destinations during autonomous UAV navigation. Experiments are performed on multiple scenarios, demonstrating that the proposed method shortens the standard $\mathrm{A}^{*}$ path for UAV navigation in the 3D environment.