Double-lane change optimization for a stochastic vehicle model subject to collision probability constraints

This paper presents an approach to autonomous passenger vehicle path planning, which accounts for the probability of collision arising from noise affected motion and imprecise future observations of the vehicle state. The probabilities of future system states are approximated by Gaussian distributions and the mean and covariance trajectories are predicted for the closed-loop system consisting of vehicle and path following controller. Linear approximations of vehicle model and controller at multiple points of the state mean trajectory relate the future covariance to the path under optimization. The situation dependent constraints on safety distance to obstacles, imposed by the dependent closed-loop covariance and a limit to the probability of collision, lead to more accurate results than a static margin of error or a distance maximization objective as currently employed by other state of the art path planners. The reference path optimization under collision probability constraints is reduced to a deterministic and static optimization problem and formulated as a nonlinear program. Numerical results for the optimization of two example double-lane change maneuvers illustrate the benefits of the approach.