公开(公告)号：US20210373855A1

公开(公告)日：2021-12-02

申请号：US17236220

申请日：2021-04-21

Applicant: Harbin Engineering University

Inventor： Qian SUN ,  Ya ZHANG ,  Zicheng Wang ,  Hongze Gao ,  Guochang Zhang ,  Qingxin Wang ,  Jiachong Chang ,  Shiwei Fan

IPC: G06F7/78 ,  G06F7/48 ,  G06F7/57 ,  G06F9/30

Abstract: The present disclosure designs a rotation matrix-based factor graph cooperative localization algorithm. Firstly, the reasons of sudden increase in an error in an operation process of a conventional factor graph cooperative localization algorithm are analyzed; secondly, a rotation matrix is designed, and the size of a rotation angle is determined; then, a rotation matrix-based cooperative localization algorithm factor graph model is constructed, and a specific algorithm flow is designed; and finally, filtering fusion estimation is performed on position status information of a slave boat. Therefore, coordinate values of master and slave boats can be transformed within a factor graph in real time without changing the measurement accuracy of an inertial device in a system to cause the coordinates of the master and slave boats that participate in the calculation of the factor graph to be inconsistent, thereby solving the problem of sudden increase in a localization error of the conventional factor graph cooperative localization algorithm, and improving the robustness of the cooperative localization system.