Trajectory planning in 3D dynamic environment with non-cooperative agents via fast marching and Bézier curve

Abstract

ABSTRACTIn this paper, we propose a framework for trajectory planning in a 3D dynamic environment where other non-cooperative agents may obstruct the active quadrotor. A trajectory predictor is designed for the non-cooperative agent via least squares. In addition, based on Euclidean signed distance field (ESDF), a velocity map configuration is developed for both dynamic and static obstacles, and then utilized in fast marching method to find a feasible discrete path. The smooth and dynamic feasible trajectory generation problem is formulated as a quadratic programming (QP) problem, where a safe flight corridor generator is developed for safe constraints in a dynamic environment. Piecewise Bézier curves are used to represent the generated trajectory. With the help of flight corridor, along with the convex hull property of Bézier curve, the quadratic programming problem turns into a convex optimization problem. Simulation results show that the proposed approach is validated for collision avoidance in a dynamic environment.