Air-ground multi-agent system cooperative navigation based on factor graph optimization SLAM

Abstract

In this study, a cooperative navigation algorithm centered on factor graph optimization—simultaneous localization and mapping (FGO-SLAM) is presented for an air-ground multi-agent system. The algorithm prioritizes the control of error statuses during the position and attitude estimation procedure throughout the entire back-end optimization process. In the conventional extended kalman filtering (EKF) algorithm, periodic cumulative errors may arise, introducing uncertainty to the estimation process. The application of the FGO algorithm not only mitigates deviation but also stabilizes errors, thereby eliminating the accumulation of periodic errors. In comparison to the practical EKF-SLAM, FGO-SLAM serves as a semi-offline optimization system that leverages key frames to minimize computational load. During multi-agent simulations, when two or more agents have overlapping field views, landmark data is merged, enhancing the optimization effectiveness. Through simulation experiments, the proposed algorithm demonstrates a 40% reduction in position error and a 41% reduction in attitude error, affirming the efficacy of FGO-SLAM for cooperative navigation.