Computation and Time constraints in Localization and Mapping Problems

Abstract

Research on simultaneous localization and mapping problems has been extensively carried out by robotics community in the last decade and several subproblems –like data association, map representation, dynamic environments or semantic mapping– have been more or less deeply investigated. One of the most important questions is the online execution of localization and mapping methods. Since observations are periodically captured by robot sensors, localization and mapping algorithms are constrained to complete the execution of an update before a new observation is available. In literature, several partial contributions have been presented, most of them focused on the reduction of computational complexity, but no comprehensive discussion of real-time feasibility had been previously proposed. The reasons that make real-time feasibility difficult are different in the case of localization and of mapping problems, but a general criterion can be found. In this thesis we claim that a locality principle is a general design criterion for real-time or incremental execution of localization and mapping algorithms. The probabilistic robotics paradigm provides a unified formulation for the different problems and a conceptual framework for the application of the proposed criterion. Locality may be applied to perform temporal or spatial decomposition of the global estimation. This thesis provides a general perspective of real-time feasibility and the identification of locality principle as a general design criterion for algorithms to meet time constraints. The particular contributions of this thesis correspond to the application of the locality principle to specific problems. The Real-Time Particle Filter is an advanced version of Particle Filter algorithm conceived to achieve a tradeoff between time constraints and filter accuracy depending on the number of samples. This goal is achieved by partitioning the overall samples required to obtain the required accuracy into sets, each of them corresponding to an observation, and by reconstructing the new set at the end of an estimation window. We proposed two main contributions: first, an analysis of the efficiency of the resampling solution of the Real-Time Particle Filter through the concept of effective sample size; second, a method to compute the mixture weights that balances the the effective sample size of partition sets and is less prone to numerical instability. The second specific contribution is an incremental version of a maximum likelihood map estimator. The adopted technique combines stochastic gradient descent and incremental tree parameterization and exploits an efficient optimization technique and organizes the graph into a spanning tree structure suitable for a decomposition. In this thesis, the incremental version of the original algorithm has been adapted using again the locality principle. Local decomposition is achieved selecting the portion of the network perturbed by the addition of a new constraint. Furthermore, the perturbation of gradient descent iteration is limited for the region already converged by adapting the learning rate. Finally, optimization is scheduled with an heuristic rule that controls the error increase in the constraint network. The constraint solver has been integrated with a map builder that extracts the constraint network from laser scans and represents the environment with a metric-topological hybrid map. While real-time feasibility is not granted, the proposed incremental tree network optimizer is suitable for online execution and the algorithm converges faster than the previous version of the same algorithm and in several condition performs better than other state-of-the-art methods. The final contribution is a parallel maximum likelihood algorithm for robot mapping. The proposed algorithm estimates the map iterating a linearization step and the solution of the linear system with Gauss-Seidel relaxation. The network is divided in connected clusters of local nodes and the reorder induced by this decomposition transforms the linearized information matrix in block-border diagonal form. Each diagonal block of the matrix can then be solved independently. The proposed parallel maximum likelihood algorithm can exploit the computation resources provided by commodity multi-core processor. Moreover, this solution can be applied to multi-robot mapping. The contributions presented in this dissertation outline a novel perspective on real-time feasibility of robot localization and mapping methods, thus bringing these algorithmic techniques closer to applications.