Enhanced Cooperative Relative Localization Using UWB-VIO Fusion Measurements

In GPS denied environment, relative localization without external anchors is crucial for multi-robot systems performing tasks such as formation, cooperative searching and exploration. In this paper, we propose an enhanced optimization-based cooperative relative localization scheme using only onboard Ultra-WideBand (UWB) and visual-inertial odometry (VIO) measurements. First, the nonlinear spacial-temporal constraints, i.e., the local geometric relationship, is introduced in the designed relative position estimator. We also consider a loop-estimation like the idea of Pose Graph Optimization (PGO). Then, we give an enhanced relative localization scheme combined with adaptive estimation and optimization. In addition, in order to improve the relative localization accuracy due to measurement errors, multi UWB tags are equipped and UWB noise model is considered. Finally we conduct comparison simulations to verify the effectiveness of our proposed relative localization algorithm. It shows that our algorithm can improve the relative localization accuracy by about 33.5% with the existing works.