Hierarchical Graph Search for Multi-Goal Route Planning in Autonomous Driving

Route planning is a fundamental function for autonomous vehicles (AVs) tasked with navigating complex road networks. Traditional formulations of the route planning problem typically assume a single destination, where the solution is defined as the optimal path from the starting point to the designated goal. In this paper, we introduce a two-stage hierarchical route planning algorithm designed to determine a feasible and optimal route that sequentially connects multiple target points within a road network. Our approach employs a graph-based representation of the road network and systematically integrates global and local search strategies to guarantee both the feasibility and minimality of the resulting route. The proposed approach also introduces a semantic representation of the planned route, providing natural language indications and specifying the planned behaviour of the car. This is accomplished by classifying the discrete points that define the planned route. The validity of the proposed approach was tested experimentally on a 1:10 scale autonomous vehicle.