FOCS: Planning by Fusion of Optimal Control and Search and its Application to Navigation

Abstract

Both Optimal Control and Search-based Planning are used extensively for path planning and have their own set of advantages and disadvantages. In this paper, we propose an algorithm FOCS (Fusion of Optimal Control and Search) that combines these two classes of approaches together. FOCS finds a path exploiting the advantages of both approaches while providing a bound on the sub-optimality of its solution. The returned path is a concatenation of the path found in the implicit graph constructed by search and the path generated by following the negative gradient of the value function obtained as a solution of the Hamilton-Jacobi-Bellman equation. We analyze the algorithm and illustrate its effectiveness in finding a minimum-time path for a car-like vehicle in different environments.