A Novel Plug-and-Play Factor Graph Method for Asynchronous Absolute/Relative Measurements Fusion in Multisensor Positioning

The presence of asynchronous absolute and relative measurements has posed a great challenge to the current multisensor positioning method in robotic systems. Although traditional factor graph methods possess a capability of plug-and-play, only a compromised performance can be obtained when dealing with an increasing number of asynchronous observations. In this article, a novel plug-and-play factor graph method for asynchronous absolute/relative measurements fusion in multisensor positioning is proposed. Different from traditional methods, a fixed-rate graph model is formed. The variable nodes in the graph are built at a fixed update rate to do the optimization, which is not affected by the arrival of measurements. Asynchronous absolute and relative measurements between two successive variable nodes are associated with corresponding variable nodes in the graph via the propagation of closed-form inertial measurement unit (IMU) preintegration. The simulations, datasets, and field tests are carried out to validate the proposed method. The results indicate that the closed-form IMU preintegration method has better dynamic adaptability in fusion system to formulate the association with asynchronous measurements. On this basis, the proposed method can integrate asynchronous measurements in a plug-and-play manner and achieve better performance compared to current methods. Meanwhile, the computational load can also be reduced.