An Iterative Graph Optimization Approach for 2D SLAM

Abstract

The-state-of-the-art graph optimization method can robustly converge into a solution with least square errors for the graph structure. Nevertheless, when a biased edge (erroneous transformation with over-confident information matrix) exists, the optimal solution can produce the large deviation because of error propagation produced by the biased edge. In order to solve this problem in graph-based 2D SLAM system, this paper proposed an iterative graph optimization approach. To reduce the errors propagated from the biased edges, we iteratively reconstruct the graph structure by referring to the result of the graph optimization process. Meanwhile, to maintain the information of the other well estimated edges, we strictly update the graph structure by considering the scan-correlation score and the marginal covariance. In addition, we apply a novel key-node mechanism to robustly detect the loop-closure by a linear interpolation algorithm. The experiments show that the proposed method is more robust and accurate than the previous methods when the biased edges exist.