IEVL-Fusion: An Inertial/Encoder/Vision/Laser Fusion Positioning Method with Global Velocity Constraint

Answering the high-precision positioning requirements of mobile robot in satellite-denied environments, an error-state extended Kalman filter (ES-EKF) based positioning method is proposed with loosely coupled fusion of subsystems of laser positioning, visual positioning, and global velocity measurement, and an integrated positioning system with low error drift is designed. Firstly, the error of system state is represented with vector addition and matrix multiplication in a minimum form, and a Kalman filter model in the error form is established, in which the optimal estimation of error state is used to compensate the estimated value of system state. Then, the pose output is transformed into the pose increment according to the time stamp, and the pose increment observation model is established, to deal with the problem of unknown pose uncertainty of the laser and visual positioning subsystems. Secondly, a global velocity measurement subsystem is constructed by using the attitude heading reference system (AHRS) and forward kinematics model, and a global velocity observation model is established, to make up the lack of global velocity constraint in the integrated positioning system. Finally, tests are carried out in street and field scenes, and results show that the relative positioning error of the proposed algorithm is less than 0.4%, which is about 40% lower than that of EKF and ES-EKF positioning algorithms with local velocity constraint. Experimental results demonstrate that the proposed method effectively improves the accuracy of the positioning system.