Windowed multiscan optimization using weighted least squares for improving localization accuracy of mobile robots

The localization and trajectory estimation of mobile robots is one of the fundamental problems in contemporary robotics. To solve it, robots often rely on the laser scanner data, which is being processed by scan-matcher algorithms followed by a simple integration of acquired transformations. Here we propose algorithm to improve the accuracy of trajectory estimation using additional correspondences between scans and the idea that all transformations between pairs of “not too far away" scans should be consistent between themselves. Additionally, weighting based on the scan-matcher error estimation allows us to reduce the importance of scan-matcher results, which can not be reliably matched. Our approach can be used to improve the performance of existing simultaneous localization and mapping setups in the form of an easily pluggable middleware, which depends only on the laser scanner and odometry data. Experimental evaluation on MIT Stata Center dataset shows that our method outperforms standard keyframe approach by more than 20% by root mean square error metric. In an experiment performed at the Skoltech using different setup our method showed almost 35% improvement.