Real-Time Longitudinal and Lateral State Estimation of Preceding Vehicle Based on Moving Horizon Estimation

Abstract

In the advanced driver assistance system (ADAS) and autonomous driving systems, accurate information on preceding vehicle motion states is vital for the path planning and control. To develop those intelligent driving systems, a modular integrated estimation algorithm for the preceding vehicle longitudinal and lateral states is proposed in this paper, the coupled nonlinear characteristics of vehicle dynamics are applied to improve state estimation accuracy. First, considering driver aggressiveness, a linear moving horizon estimator for the vehicle longitudinal speed is designed based on the car following model. Then, the estimated vehicle longitudinal speed is delivered to the lateral estimator module in real-time. For the arbitrary driving routes, the Serret-Frenet equations and nonlinear lateral dynamics are combined to describe the lateral motion of preceding vehicle. Furthermore, a nonlinear moving horizon estimator is constructed to estimate vehicle lateral states accurately. To reduce the computational burden, the multiple shooting (MS) method is introduced, then the objective function is divided into several segments to improve the strong nonlinearity produced by multiple iterations. The proposed estimation algorithm can effectively handle the initial biases, the additive biases and model error, which are common in the acquired signals, and provide an accurate estimate of preceding vehicle states.