This paper presents a technique to estimate in real time the egomotion of a vehicle based solely on laser range data. This technique calculates the discrepancy between closely spaced two-dimensional laser scans due to the vehicle motion using scan matching techniques. The result of the scan alignment is converted into a nonlinear motion measurement and fed into a nonholonomic extended Kalman filter model. This model better approximates the real motion of the vehicle when compared to more simplistic models, thus improving performance and immunity to outliers. The motion estimate is intended to be used for egomotion compensation in a target-tracking algorithm for situation awareness applications. In this paper, several recent scan matching algorithms were evaluated for their accuracy and computational speed: metric-based iterative closest point (MbICP), point-to-line ICP (PIICP), and polar scan matching. The proposed approach is performed in real time and provides an accurate estimate of the current robot motion. The MbICP algorithm proved to be the most advantageous scan matching algorithm, but it is still comparable to PlICP. The motion estimation algorithm is validated through experimental testing in real world conditions.
Graph Optimization with Unstructured Covariance: Fast, Accurate, Linear Approximation
This manuscript addresses the problem of optimization- based Simultaneous Localization and Mapping (SLAM), which is of concern when a robot, traveling in an unknown environment, has to build a world model, exploiting sensor measurements. Although the optimization problem underlying SLAM is nonlinear and nonconvex, related work showed that it is possible to compute an accurate linear approximation of the optimal solution for the case in which measurement covariance matrices have a block diagonal structure. In this paper we relax this hypothesis on the structure of measurement covariance and we propose a linear approximation that can deal with the general unstructured case. After presenting our theoretical derivation, we report an experimental evaluation of the proposed technique. The outcome confirms that the technique has remarkable advantages over state-of-the-art approaches and it is a promising solution for large-scale mapping.
