Semi-analytical minimum time solutions with velocity constraints for trajectory following of vehicles

We consider the problem of finding an optimal manoeuvre that moves a car-like vehicle between two configurations in minimum time. We propose a two phase algorithm in which a path that joins the two points is first found by solving a geometric optimisation problem, and then the optimal manoeuvre is identified considering the system dynamics and its constraints. We make the assumption that the path is composed of a sequence of clothoids. This choice is justified by theoretical arguments, practical examples and by the existence of very efficient geometric algorithms for the computation of a path of this kind. The focus of the paper is on the computation of the optimal manoeuvre, for which we show a semi-analytical solution that can be produced in a few milliseconds on an embedded platform for a path made of one hundred segments. Our method is considerably faster than approaches based on pure numerical solutions, it is capable to detect when the optimal solution exists and, in this case, compute the optimal control. Finally, the method explicitly considers nonlinear dynamics, aerodynamic drag effect and bounds on the longitudinal and on the lateral acceleration.