Optimal Stochastic Vehicle Path Planning Using Covariance Steering

This letter addresses the problem of vehicle path planning in the presence of obstacles and uncertainties, a fundamental robotics problem. While several path planning algorithms have been proposed over the years, many of them have dealt with only deterministic environments or with only open-loop uncertainty, i.e., the uncertainty of the system state is not controlled and, typically, increases with time because of exogenous disturbances. This may lead to potentially conservative nominal paths. The typical approach to deal with disturbances and reduce uncertainty is to use a lower level feedback controller. We advocate the premise that, if a path planner can consider the closed-loop evolution of the system uncertainty, it can lead to less conservative, but still feasible, paths. To this end, in this letter, we develop an approach that is based on optimal covariance steering, which explicitly steers the state covariance for stochastic linear systems. We verify the proposed framework using extensive numerical simulations.