Integration of SINS, laser doppler Velocimeter, and monocular visual odometry for autonomous navigation in complex road environments

For the integrated navigation method based on the strapdown inertial navigation system (SINS) and laser doppler velocimeter (LDV), the accuracy of the LDV is easily affected by the complex environment, such as sand and dust on the road, which leads to the degradation of the integrated navigation system's positioning accuracy. In this paper, we propose an integrated navigation method for land vehicles that uses SINS, LDV, and monocular visual odometry (Mono-VO). We considered the boresight errors, lever-arm residual, and scale factor errors of Mono-VO when establishing an error model for the integrated SINS/LDV/Mono-VO navigation system. The new state vector's observability is investigated. A Huber-based Kalman filter is also used to fuse these data. Finally, simulations and semi-physical simulations are carried out to validate the effectiveness of the proposed method, which demonstrates that positioning accuracy under complex road conditions is superior to that of the SINS/LDV navigation system.