This paper studies the problem of Cooperative Localization (CL) for multi-robot systems, where a group of mobile robots jointly localize themselves by using measurements from onboard sensors and shared information from other robots. We propose a novel distributed invariant Kalman Filter (DInEKF) based on the Lie group theory, to solve the CL problem in a 3-D environment. Unlike the standard EKF which computes the Jacobians based on the linearization at the state estimate, DInEKF defines the robots' motion model on matrix Lie groups and offers the advantage of state estimate-independent Jacobians. This significantly improves the consistency of the estimator. Moreover, the proposed algorithm is fully distributed, relying solely on each robot's ego-motion measurements and information received from its one-hop communication neighbors. The effectiveness of the proposed algorithm is validated in both Monte-Carlo simulations and real-world experiments. The results show that the proposed DInEKF outperforms the standard distributed EKF in terms of both accuracy and consistency.

翻译：本文研究多机器人系统的协同定位问题，即一组移动机器人通过自身搭载的传感器测量值以及从其他机器人接收的共享信息实现联合定位。基于李群理论，我们提出一种新颖的分布式不变卡尔曼滤波算法，用于解决三维环境下的协同定位问题。与标准扩展卡尔曼滤波（EKF）基于状态估计线性化计算雅可比矩阵不同，DInEKF将机器人运动模型定义在矩阵李群上，具有状态估计无关的雅可比矩阵优势，从而显著提升估计器的一致性。此外，该算法完全分布式，仅依赖各机器人自身的运动测量值及其单跳通信邻居传递的信息。通过蒙特卡洛仿真和实际实验验证了所提算法的有效性。结果表明，本文提出的DInEKF在精度和一致性方面均优于标准分布式EKF。