Quadrotor Trajectory Planning with Tetrahedron Partitions and B-Splines in Unknown and Dynamic Environments

Trajectory planning is a key task in unmanned aerial vehicle navigation systems. Although trajectory planning in the presence of obstacles is a well-understood problem, unknown and dynamic environments still present significant challenges. In this paper, we present a trajectory planning method for unknown and dynamic environments that explicitly incorporates the uncertainty about the environment. Assuming that the position of obstacles and their instantaneous movement are available, our method represents the environment uncertainty as a dynamic map that indicates the probability that a region might be occupied by an obstacle in the future. The proposed method first divides the free space into non-overlapping tetrahedral partitions using Delaunay triangulation. Then, a topo-graph that describes the topology of the free space and incorporates the uncertainty of the environment is created. Using this topo-graph, an initial path and a safe flight corridor are obtained. The initial safe flight corridor provides a sequence of control points that we use to optimize clamped B-spline trajectories by formulating a quadratic programming problem with safety and smoothness constraints. Using computer simulations, we show that our algorithm can successfully find a collision-free and uncertainty-aware trajectory in an unknown and dynamic environment. Furthermore, our method can reduce the computational burden caused by moving obstacles during trajectory replanning.