Prediction and Optimal Feedback Steering of Probability Density Functions for Safe Automated Driving

We propose a stochastic prediction-control framework to promote safety in automated driving by directly controlling the joint state probability density functions (PDFs) subject to the vehicle dynamics via trajectory-level state feedback. To illustrate the main ideas, we focus on a multi-lane highway driving scenario although the proposed framework can be adapted to other contexts. The computational pipeline consists of a PDF prediction layer, followed by a PDF control layer. The prediction layer performs moving horizon nonparametric forecasts for the ego and the non-ego vehicles' stochastic states, and thereby derives safe target PDF for the ego. The latter is based on the forecasted collision probabilities, and promotes the probabilistic safety for the ego. The PDF control layer designs a feedback that optimally steers the joint state PDF subject to the controlled ego dynamics while satisfying the endpoint PDF constraints. Our computation for the PDF prediction layer leverages the structure of the controlled Liouville PDE to evolve the joint PDF values, as opposed to empirically approximating the PDFs. Our computation for the PDF control layer leverages the differential flatness structure in vehicle dynamics. We harness recent theoretical and algorithmic advances in optimal mass transport, and the Schrodinger bridge. The numerical simulations illustrate the efficacy of the proposed framework.