BDPGO: Balanced Distributed Pose Graph Optimization Framework for Swarm Robotics

Distributed pose graph optimization (DPGO) is one of the fundamental techniques of swarm robotics. Currently, the sub-problems of DPGO are built on the native poses. Our validation proves that this approach may introduce an imbalance in the sizes of the sub-problems in real-world scenarios, which affects the speed of DPGO optimization, and potentially increases communication requirements. In addition, the coherence of the estimated poses is not guaranteed when the robots in the swarm fail, or partial robots are disconnected. In this paper, we propose BDPGO, a balanced distributed pose graph optimization framework using the idea of decoupling the robot poses and DPGO. BDPGO re-distributes the poses in the pose graph to the robot swarm in a balanced way by introducing a two-stage graph partitioning method to build balanced subproblems. Our validation demonstrates that BDPGO significantly improves the optimization speed without changing the specific algorithm of DPGO in realistic datasets. What's more, we also validate that BDPGO is robust to robot failure, changes in the wireless network. BDPGO has capable of keeps the coherence of the estimated poses in these situations. The framework also has the potential to be applied to other collaborative simultaneous localization and mapping (CSLAM) problems involved in distributedly solving the factor graph.