This paper presents an approach for time-optimal trajectory planning of a car in an environment with multiple goals and obstacles. We formulate the problem as an Optimal Control (OC) framework, incorporating both trajectory optimization and goal ordering within a unified structure. To achieve this, we leverage the Traveling Salesman Problem (TSP) solution as an initial guess for the global solution within the OC framework. This approach enables joint optimization of individual trajectories and the goal visiting sequence. Our primary goal is to develop a method for the car to efficiently navigate through designated goals while respecting a set of equality and inequality constraints arising from the car’s dynamic model, the environment, and the task itself. The evaluation is performed in simulation using the CasADI library [1]. Please refer to our paper for more information!
[1] J. A. E. Andersson, J. Gillis, G. Horn, J. B. Rawlings, and M. Diehl, “CasADi– A software framework for nonlinear optimization and optimal control,” Mathematical Programming Computation, vol. 11,no. 1, pp. 1–36, 2019