Continuous Curvature Path Planning using Voronoi diagrams and Fermat's spirals

This paper presents a two-dimensional curvature-continuous path planning algorithm based on Voronoi diagrams and Fermat's spiral segments. The map and the obstacles position are assumed to be known a-priori and static. Despite the disposition of the obstacles, the Voronoi diagram always presents at least one collision-free path, maximally distant from all the obstacles. If more than one flyable path is available, the shortest one is selected. The result is further refined to obtain a more practical path that is piecewise linear with discontinuous curvature and velocity. Fermat's spirals are used to smooth the path and provide curvature-continuity. A maximum threshold for the curvature is set so that the result of the algorithm respects kinematics and dynamics constraints of the vehicle. Moreover a minimum clearance from the obstacles can be chosen to respect additional safety constraints. The final result of the algorithm is a simple and intuitive path composed only by straight lines and spiral segments.