Tightly-Coupled Fusion of VINS and Motion Constraint for Autonomous Vehicle
Zhelin Yu, Lidong Zhu, Guoyu Lu
Abstract
In this paper, we develop a novel visual-inertial navigation system with motion constraint (VINS-Motion), which extends the visual-inertial navigation system (VINS) to incorporate vehicle motion information for improving the autonomous vehicles localization accuracy. We introduce the vehicle stop detection module in the pre-processing measurement procedure. During the back-end processing, if the vehicle is detected to be in a stopping state, we perform optimization based on the stop constraint to help eliminate the abnormal jitter of the estimated pose, thus ensuring the reasonability of the trajectory. Otherwise, besides the prior information, IMU measurement residual, and visual measurement residual utilized in VINS, vehicle orientation/velocity constraint built by Ackerman steering model is first exploited to constitute residuals. We minimize the sum of priors and Mahalanobis norms of three kinds of residuals to obtain a maximum posterior estimation, thus increasing system consistency and accuracy. The proposed approach is validated on public datasets and compared with state-of-the-art algorithms, which demonstrates that the motion constraint proposed in this paper improves the standard VINS performance and achieves significantly higher positioning accuracy.