Implementation of efficient algorithms for globally optimal trajectories

We consider a continuous-space shortest path problem in a two-dimensional plane. This is the problem of finding a trajectory that starts at a given point, ends at the boundary of a compact set of ℜ², and minimizes a cost function of the form ∫₀^T r(x(t)) dt + q(x(T)). For a discretized version of this problem, a Dijkstra-like method that requires one iteration per discretization point has been developed by Tsitsiklis [10]. Here we develop some new label correcting-like methods based on the Small Label First methods of Bertsekas [2] and Bertsekas et al. [6]. We prove the finite termination of these methods, and we present computational results showing that they are competitive and often superior to the Dijkstra-like method and are also much faster than the traditional Jacobi and Gauss-Seidel methods.