Optimal Path Planning for Unmanned Surface Vehicle Based on Fast Marching Method

Path planning is one of the key technologies for unmanned surface vehicle (USV) to realize intelligent navigation. However, most path planning algorithms only consider the shortest path length and ignore other constraints during the navigation, which may generate a path that is not practically optimal in the view of safety and angular constraints. To solve this problem, an optimal path planning algorithm for USV based on fast marching method (FMM) is proposed in this paper. Firstly, a arrival time potential field with the only minimum point is generated by executing FMM on the initial binary map, which is used as a basic model for path planning. Secondly, to modeling the safety and heading angle constraints of the vehicle, the saturated safety map based on SFMS and the virtual obstacles formed by USV’s turning ability are introduced and applied respectively. Thirdly, the gradient descent method is used on the improved potential field to search out an optimal path that can well meet the actual constrains of USV. The effectiveness and rationality of the algorithm are then verified by simulations.