A Gridmap-Path Reshaping Algorithm for Path Planning

Path planning is a vital function of robotic systems. Different from existing roadmap algorithms which first determine the free space and then determine the collision-free path, researchers recently proposed several convex relaxation based smoothing algorithms which first select an initial path to link the starting configuration and the goal configuration and then reshape this path to meet other requirements (e.g., collision-free conditions) by using convex relaxation. However, convex relaxation based smoothing algorithms often fail to give a satisfactory path, since the initial paths are selected improperly. Moreover, the curvature constraints were not considered in many existing convex relaxation based smoothing algorithms. In this paper, we show that we can first grid the whole configuration space to pick a candidate path and reshape the shortest path to meet our goal. This new algorithm inherits the merit of roadmap algorithms and a convex feasible set algorithm. We further discuss how to meet the curvature constraints by using both the Beamlet algorithm to select a better initial path and an iterative optimization algorithm to adjust the curvature of a path. Theoretical analysis and numerical testing results show that our algorithm can almost surely find a feasible path and require a short computation time.