A Trajectory Planning Method Based on Quad-tree Convex Feasible Set in Unstructured Environments

Abstract

Trajectory planning in unstructured environments is a basic task for mobile robots. When trajectory planning is considered an optimization problem, the nonlinear kinematic constraints and the non-convex collision avoidance constraints often make it challenging. A trajectory planning algorithm that consists of two steps is proposed to address these problems in this paper. The first step is to combine the quad-tree map with the improved convex feasible set(CFS) method to construct a local convex feasible space. The second step is to solve the optimization problem iteratively, and an optimal margin judgment condition is proposed to reuse the local feasible spaces. The main contribution of this paper is that we combine the quad-tree map with the improved CFS to construct local convex feasible spaces efficiently. Numerical simulation results show that this algorithm can obtain collision-free and feasible vehicle trajectory in a short time. And the completeness of the trajectory can be guaranteed under a certain number of iterations.