Simultaneous Localization And Mapping For Mobile Robot Based On Incremental Smoothing

SLAM is the abbreviation of Simultaneous Localization and Mapping, which is one process that a robot begins to move from an unknown environment and positions itself according to the motion model and sensor carried in the movement, while building the surrounding environment map. The solution of SLAM is based on probabilistic method. There are mainly two kinds of classic algorithms:one is based on Extended Kalman Filter, the other is based on Particle Filter. The EKF recursively estimates a Gaussian density over the current pose of the robot and the position of all landmarks. However, it is well known that the computational complexity of the EKF becomes intractable fairly quickly, and hence it is difficult to cope with larger-scale environments. And, filtering itself has been shown to be inconsistent when applied to the inherently nonlinear SLAM problem. So, there has recently been considerable interest in the optimization version of the SLAM problem. Comparing to filtering, the optimization-based approaches have been found to be more efficient, stable, versatile and scalable. Therefore, we mainly research on the optimization-based approaches in this paper.The main idea of the optimization-based SLAM is transforming SLAM into the least squares formulation and solving it by means of optimization theory, then computing global map and the robot’s whole trajectory. Therefore, it’s also called smoothing approaches. However, it is inevitable that linearization errors would be produced in the process of optimization. So, we propose one way to reduce linearization errors and improve performance of the system on the basis of smoothing method. The main work is as follows:Firstly, model the SLAM problem. We use factor graph to represent the SLAM problem and transform it to nonlinear least squares optimization problem of large sparse system through detailed formula deduction.Secondly, introduce the least squares optimization solution. The least squares optimization problem includes linear least squares problem and nonlinear least squares problems, but their solutions are different.This paper focuses on the method of the nonlinear least squares problems and we solve a simple SLAM example by this way.Thirdly, study two popular algorithm based on optimization method, namely the smoothing algorithm, which includes batch Smoothing and Mapping algorithm (SAM) and incremental Smoothing and Mapping algorithm (iSAM). The former all at once computes the full map and trajectory at last, which is offline. The latter performs fast incremental updates of the square root information matrix yet is able to compute the full map and trajectory at any time, which is an effective and practical solution.Finally, on the basis of the incremental smoothing algorithm,we improve the algorithm by using unscented transformation and numerical differentiation theory. We demonstrate the performance of new algorithm by comparing with exiting algorithm in simulations and experiments in precision performance and computational cost etc.