UWB-VIO Fusion for Accurate and Robust Relative Localization of Round Robotic Teams

The relative pose estimation is one of the most fundamental components for multi-robot systems, while it still remains an open and challenging research topic in infrastructure-free environment. In this letter, we target improving the accuracy and robustness of relative pose estimation for ground robotic teams, and propose to fuse range and odometry measurements to estimate the relative pose using sliding window optimization. In the system, multiple UWB tags for ranging are equipped on each robot, and visual inertial odometry is applied for estimating the ego-motion pose for each robot. Aiming for simple and effective relative pose initialization, the triangulation uncertainty for multi-tag robots is analyzed, and an initialization method is designed. To cope with the complex environments such as continuous NLOS condition, a NLOS detection and range measurements filtering method is presented. We have conducted series of experiments to demonstrate the performance of the proposed approach.