Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

Motion Planning for Autonomous Vehicles: When Model Predictive Control Meets Ensemble Kalman Smoothing

Iman Askari, Yebin Wang, Vedeng M. Deshpande, Huazhen Fang

TL;DR

This work tackles the computational bottleneck of model predictive control for autonomous driving when using neural network vehicle models. By reformulating NMPC as a Bayesian estimation problem and solving it with a sequential Ensemble Kalman Smoother, the authors achieve real-time capable motion planning without sacrificing safety or performance. Key contributions include a barrier-function-based constraint handling, a MAP-based equivalence to the NMPC objective, and a one-pass EnKS algorithm (EnKS-NMPC-MP) that dramatically reduces computation while preserving planning quality. Simulations show substantial speedups over gradient-based solvers, especially for longer horizons, with maintained safety during overtaking on curved roads.

Abstract

Safe and efficient motion planning is of fundamental importance for autonomous vehicles. This paper investigates motion planning based on nonlinear model predictive control (NMPC) over a neural network vehicle model. We aim to overcome the high computational costs that arise in NMPC of the neural network model due to the highly nonlinear and nonconvex optimization. In a departure from numerical optimization solutions, we reformulate the problem of NMPC-based motion planning as a Bayesian estimation problem, which seeks to infer optimal planning decisions from planning objectives. Then, we use a sequential ensemble Kalman smoother to accomplish the estimation task, exploiting its high computational efficiency for complex nonlinear systems. The simulation results show an improvement in computational speed by orders of magnitude, indicating the potential of the proposed approach for practical motion planning.

Motion Planning for Autonomous Vehicles: When Model Predictive Control Meets Ensemble Kalman Smoothing

TL;DR

This work tackles the computational bottleneck of model predictive control for autonomous driving when using neural network vehicle models. By reformulating NMPC as a Bayesian estimation problem and solving it with a sequential Ensemble Kalman Smoother, the authors achieve real-time capable motion planning without sacrificing safety or performance. Key contributions include a barrier-function-based constraint handling, a MAP-based equivalence to the NMPC objective, and a one-pass EnKS algorithm (EnKS-NMPC-MP) that dramatically reduces computation while preserving planning quality. Simulations show substantial speedups over gradient-based solvers, especially for longer horizons, with maintained safety during overtaking on curved roads.

Abstract

Safe and efficient motion planning is of fundamental importance for autonomous vehicles. This paper investigates motion planning based on nonlinear model predictive control (NMPC) over a neural network vehicle model. We aim to overcome the high computational costs that arise in NMPC of the neural network model due to the highly nonlinear and nonconvex optimization. In a departure from numerical optimization solutions, we reformulate the problem of NMPC-based motion planning as a Bayesian estimation problem, which seeks to infer optimal planning decisions from planning objectives. Then, we use a sequential ensemble Kalman smoother to accomplish the estimation task, exploiting its high computational efficiency for complex nonlinear systems. The simulation results show an improvement in computational speed by orders of magnitude, indicating the potential of the proposed approach for practical motion planning.
Paper Structure (7 sections, 1 theorem, 31 equations, 3 figures, 1 table, 1 algorithm)

This paper contains 7 sections, 1 theorem, 31 equations, 3 figures, 1 table, 1 algorithm.

Table of Contents

  1. Introduction
  2. NMPC Motion Planning Problem Formulation
  3. Motion Planning via EnKS-Based NMPC
  4. Setup of NMPC as Bayesian Estimation
  5. Sequential Ensemble Kalman Smoothing
  6. Numerical Simulation
  7. Conclusion

Key Result

Theorem 1

Assume that $w_t$ and $v_t$ are independent white noises with $w_t \sim \mathcal{N}(0,Q^{-1})$ and $v_t \sim \mathcal{N}(0, R^{-1})$. Then, the problems in NMPC-Standard and MAP-Formulation have the same optima if neglecting inequality-constraints.

Figures (3)

  • Figure 1: Vehicle trajectories and positions during the simulation. The EV performing EnKS motion planning is denoted in blue. The other green and red vehicles are OVs. The color gradient from light to dark represents the vehicle's position from the past to the future.
  • Figure 2: The control profile and constraint satisfaction by the EV running optimization and Ensemble Kalman Motion Planner. (a) Acceleration control profiles with respective bounds. (b) Steering control profiles with respective bounds. (c) Distance between the EV and OVs when $N=200$ with a safety margin of 1 m.
  • Figure 3: Cost comparison over the simulation time between optimization and EnKS with different particle numbers for two horizon lengths. (a) $H = 40$. (b) $H = 60$.

Theorems & Definitions (2)

  • Theorem 1
  • proof