A study of single and multi-robot localization: A manifolds approach

We consider the problem of localizing autonomous robots when GPS is not available. Our work consists of two parts. First we examine how the estimation error grows with time when a mobile robot estimates its location from inter-time relative pose measurements without global position or orientation sensors. We show that in both 2-D or 3-D space, both the bias and variance of the position estimation error grows at most linearly with time (or distance traversed) asymptotically. The bias is crucially dependent on the trajectory of the robot. Exact formulas for the bias and the variance of the position estimation error are provided for two specific 2-D trajectories- straight line and periodic. Experiments with a P3-DX wheeled robot and Monte-Carlo simulations are provided to verify the theoretical predictions. A method to reduce the bias is proposed based on the lessons learned. We next consider a group of cooperating robots attempting to localize without the use of GPS. We propose a algorithm for estimating the 3-D pose (position and orientation) of each robot with respect to a common frame of reference. This algorithm does not rely on the use of any maps, or the ability to recognize landmarks in the environment. Instead we assume that noisy relative measurements between pairs of robots are intermittently available, which can be any one, or combination, of the following: relative pose, relative orientation, relative position, relative bearing, and relative distance. The additional information about each robots pose provided by these measurements are used to improve over self-localization estimates. The proposed method is based on solving an optimization problem in an underlying Riemannian manifold (SO(3) x R 3)n(k) by a gradient descent law. The proposed algorithm is easily applicable to 3-D pose estimation, can fuse heterogeneous measurement types, and can handle arbitrary time variation in the neighbor relationships among robots. This algorithm is further refined by choosing a distribution for the various measurement types and developing a maximum likelihood estimator for collaborative localization. Simulations show that the errors in the pose estimates obtained using this algorithm are significantly lower than what is achieved when robots estimate their pose without cooperation. Results from experiments with a pair of ground robots with vision-based sensors reinforce these findings. Additionally, the question of trade-offs between cost (of obtaining a certain type of relative measurement) vs. benefit (improvement in localization accuracy) for the various types of relative measurements is considered. Finally, a set of simulations is present in which our proposed algorithm is compared against two state of the art collaborative localization algorithms. This comparison shows that the proposed method performs better when the error in orientation measurements is large, or when the time interval between inter-robot measurements is large. Finally, we propose an outlier rejection algorithm that functions as a preprocessing step for a pose graph collaborative localization algorithm, such as the one proposed earlier in this work, when all measurements are of the relative pose. Outliers are identified using only the information contained in the remaining relative measurements. In particular, no a priori distribution on the relative measurements is assumed, nor is any information about the absolute pose of the robots utilized. This outlier rejection algorithm exploits properties of pose measurements concatenated over simple cycles in the measurement graph to define an edge consistency cost such that large values are indicative of the presence of an outlier. A hypothesis test approach is then utilized to identify the set of likely outlying measurements. Simulations utilizing the proposed outlier rejection algorithm are presented. The outlier rejection algorithm is shown to successfully identify up to 95% of the outliers in the scenario considered, and successfully mitigate the effect of outliers on collaborative localization.