A new EKF SLAM algorithm of lidar-based AGV fused with bearing information

H.X. Ye C. Zhou
Stevens Institue of Techonlogy,
United States

Keywords: automated guided vehicle, simultaneous localization and mapping, extended Kalman filter, electronic compass, laser guidance


Lidar-based Automated Guided Vehicle (AGV) is widely used in various kinds of transportation tasks as a kind of intelligent logistics equipment. To realize the navigation, lidar-based AGV has to know its location and pose when it’s moving and create the environment map at the same time. That is the Simultaneous Localization and Mapping (SLAM). The Extended Kalman Filter (EKF) is the most common method to solve the SLAM problem. When EKF SLAM is used in AGV, which works in a new environment and the trajectory of motion is open loop, the bearing variance will increase and the linearization errors will accumulate, that eventually results in inconsistent mapping estimation, the map will have a larger error. Therefore, EKF SLAM method relies heavily on closed-loop path of AGV. In order to improve the accuracy of EKF SLAM method and reduce the location and pose error of lidar-based AGV, this paper presents an improved EKF SLAM method in which the additional absolute bearing information of compass is used. The new approach broke the EKF algorithm for SLAM application into two layers. The inner layer of EKF compute the position and pose of AGV based on the encoder information and then correct the estimated result carrier with the actual value of the compass. The outer layer of EKF uses the output of the inner EKF structure as the predictions of AGV’s pose and corrects AGV’s pose and the map of the environment by using the data acquired by lidar. Based on above principle, the motion model and observation model of both layers are built respectively, and the improved EKF SLAM algorithm is given. In order to demonstrate the new algorithm, simulations were carried out on Matlab. In the open-loop path and the closed-loop path, the conventional algorithm and the new algorithm were simulated respectively. It shows that the position error and bearing error of the conventional method will diverge under the circumstance of open-loop path. On the contrary, method proposed in this paper can reduce the pose error of AGV effectively and maintain the pose error of AGV in a small interval around zero even its path in open-loop. The new approach was also experimented and verified on a self-developed lidar-based AGV. The experiment was carried out in a semi-closed empty room. The experiment results are as follows. The mean square root errors of pose of AGV estimated by the conventional method and the new method are 26.8mm and 9.1mm respectively, the maximum errors between real trajectory and calculated trajectory with the conventional method and the new method are 80mm and 25mm respectively. It shows that the approach proposed in the paper could improve the accuracy of EKF SLAM method and reduce the location and pose error of lidar-based AGV. Because of high positioning accuracy and no reliance on a closed-loop path, the new method has a broad application prospect. It can be used not only for AGV and industrial robots, but also for mobile service robots in hospitals and families.