4D Trajectory Planning Based on Fast Marching Square for UAV Teams

Swarms composed of multiple unmanned aerial vehicles (UAVs) are emerging as a key tool in various application fields including aerial surveillance, urban search and rescue, and package delivery. In this context, trajectory planning remains one of the principal concerns regarding complex robotic applications. This paper proposes a 4D path planning algorithm based on the Fast Marching Square (FM2) method for multi-UAV teams in high-dimensional 3D environments with multiple obstacles. The 4D-FM2 algorithm integrates a time-dependent speed function within the Fast Marching (FM) framework. The proposed algorithm was tested in a simulated urban scenario and results demonstrate that the algorithm effectively plans safe, optimal solutions to the shortest path problem for UAVs while avoiding obstacles and other drones, even in complex situations. Additionally, the algorithm excels in providing smooth speed profiles for the vehicles. Furthermore, the study also successfully evaluated the effect of various implementation parameters on the algorithm's outcome, such as the number of concurrent missions and the velocity of the vehicles. Moreover, computational time was kept within acceptable limits, and the method demonstrated overall good performance in terms of air traffic flow. The main contribution of this work is the introduction of a novel 4D trajectory planning algorithm that implicitly incorporates UAV movement and speed and hence the spatio-temporal realization of the path during the planning phase, leveraging the light propagation phenomenon. This approach efficiently addresses the presence of other UAVs and environmental obstacles, thereby preventing collisions and avoiding unnecessary airspace blockages.