Lie-theoretic multi-robot localization

Abstract

This chapter presents a new distributed cooperative localization technique based on a second-order sensor fusion method developed for the special Euclidean group. Uncertainties in the robot pose, sensor measurements, and landmark positions (neighboring robots in this case) are modeled as Gaussian distributions in exponential coordinates. This proves to be a better fit for both the prior and posterior distributions resulting from the motion of nonholonomic kinematic systems with stochastic noise (as compared to standard Gaussians in Cartesian coordinates). We provide a recursive closed-form solution to the multi-sensor fusion problem that can be used to incorporate a large number of sensor measurements into the localization routine and can be implemented in real time. The technique can be used for nonlinear sensor models without the need for further simplifications given that the required relative pose and orientation information can be provided, and it is scalable in that the computational complexity does not increase with the size of the robot team and increases linearly with the number of measurements taken from nearby robots. The proposed approach is validated with simulation in Matlab.