Optimal Path Planning Through a Sequence of Waypoints

This letter presents a deterministic approach for finding the optimal path through a sequence of spatial waypoints while accounting for vertex or turn costs. A case study is presented where the proposed algorithm is used to determine the optimal path through a sequence of waypoints. This is then compared with the path obtained when considering only two consecutive waypoints at a time. Further, an approximation that uses three waypoints at a time in a staggered manner is described. This approach is shown to be computationally efficient and finds the optimal path even in cases with 2000 waypoints.