Joint Decision Making and Motion Planning for Road Vehicles Using Particle Filtering

Abstract This paper describes a probabilistic framework for real-time, joint decision making and trajectory generation of road vehicles. The selection of desired lane and state profile is posed as a stochastic nonlinear estimation problem. The nonlinear estimator is integrated with a sampling-based motion planner that computes candidate paths and corresponding state trajectories, while accounting for obstacle motion. The motion planner is implemented in receding horizon and repeatedly replans, based on stored candidate trajectories. A simulated autonomous highway-driving example illustrates how vehicle dynamics is naturally handled in the framework. The results show that the framework is capable of making effective online decisions and computing safe trajectories.