A graph-based algorithm for optimal control of switched systems: An application to car parking

We consider a finite element approximation of the Bellman equation for the optimal control of switched systems. We show that the problem belongs to a special class that we studied in a previous work, for which we developed an efficient solution algorithm. As an application, we present the problem of generating parking maneuvers for self-driving vehicles on two typical urban parking scenarios. The vehicle is described by four different switched systems in which every switching is associated to a penalization term. In this way, we obtain parking paths that have a small number of direction changes and have a simple structure.