Time-optimal Path Planning for Autonomous Vehicles Based on Bi-Level Programming

In this paper, a path planner is proposed for autonomous vehicles to find the time-optimal path that visits a given set of nodes in a directed graph, which is widely regarded as a NP-hard problem. Toward this end, a bi-level programming model is introduced to formulate and solve the underlying problem. In particular, an improved Dijkstra algorithm named directed-Dijkstra, considering turning time of vehicles, is proposed to obtain the time-optimal paths between all given nodes. Then, Branch and Bound-LKH hybrid algorithm is presented to allocate the given set of nodes in a reasonable sequence. Finally, the global time-optimal path is obtained by concatenating the corresponding sub-paths. Simulation results verified the effectiveness of the proposed scheme.