Fast numerical solution of optimal control problems for switched systems: An application to path planning

We consider a finite element approximation of the Hamilton-Jacobi-Bellman equation for the optimal control of switched systems. In particular, we show that the obtained finite dimensional problem belongs to a special class of problems that we already studied in a previous work. In that work, we also presented a simple and efficient solution algorithm.As an application, we present the problem of generating parking maneuvers for self-driving vehicles. The vehicle is described by a switched system. Each change between forward and backward motion is associated to a switching cost. This added cost penalizes the maneuvers with a larger number of direction changes and allows obtaining simpler and more human-like trajectories.