Multi-stage Local Path Planning Algorithm Based on Obstacles as Anchor Points

To address the problem of poor smoothness and solvability in dense obstacle scenarios of traditional local path planning algorithms for unmanned vehicles, a multi-stage local path planning algorithm based on obstacles as anchor points is proposed. First, the planning node digraph based on tree structure is constructed while multi-stage sampling points are set up when the solution space is generated, thus providing reference line fits by cubic spiral that can change in real time according to the position of obstacles. Then, to generate smooth and safe local path for unmanned vehicle, quadratic planning algorithm based on multi-stage reference line as guidance to get local path. Simulation results show that the multi-stage local path planning algorithm with obstacles as anchor points can reduce the smoothness cost of planning path by 29%, the solution success rate is not less than 90% which suggests our method has stronger solvability compared with traditional path planning algorithms.