Stochastic optimal control navigation with the avoidance of unsafe configurations

Abstract

Long-time planning horizons are required to safely navigate one vehicle in the presence of another, possibly non-cooperative vehicle. They give rise to computational issues preventing the real-time implementation of safe navigation algorithms. In this paper, we consider two nonholonomic vehicles, of which one (blue) has the goal to enter the “tail” of the other (red). Neither the goal nor the navigation strategy of the red vehicle is known by the blue vehicle. To anticipate this uncertainty, the blue vehicle uses infinite horizon stochastic optimal control. Using the stochastic optimal control and backward Kolmogorov equation, the blue is navigated to avoid unsafe configurations from which the red can enter the “tail” of the blue and gain advantage over it. Our results are illustrated by numerical simulations and the feasibility of the control for the real-time implementation is tested with small-scale robot experiments.