Optimal Stochastic Vehicle Path Planning Using Covariance Steering

This work addresses the problem of vehicle path planning in the presence of obstacles and uncertainties, which is a fundamental problem in robotics. While many path planning algorithms have been proposed for decades, many of them have dealt with only deterministic environments or only open-loop uncertainty, i.e., the uncertainty of the system state is not controlled and, typically, increases with time due to exogenous disturbances, which leads to the design of potentially conservative nominal paths. In order to deal with disturbances and reduce uncertainty, generally, a lower-level feedback controller is used. We conjecture that, if a path planner can consider the closed-loop evolution of the system uncertainty, it can compute less conservative but still feasible paths. To this end, in this work we develop a new approach that is based on optimal covariance steering, which explicitly steers the state covariance for stochastic linear systems with additive noise under non-convex state chance constraints. The proposed framework is verified using simple numerical simulations.