Research On Underwater All Source Positioning And Navigation Algorithm Based On Factor Graph

The sea is rich in resources and an important area for human exploration and development.Underwater vehicle is an important tool to assist people to explore the marine environment,and the navigation system of the vehicle is the premise to ensure its smooth underwater operations,so the research on underwater navigation algorithm is very necessary.With the increasing complexity of underwater tasks,the traditional underwater integrated navigation methods can no longer meet the requirements,and the dynamic cooperation among multiple underwater navigation methods is required to ensure the reliability of navigation results.Therefore,the flexible configuration of multiple underwater navigation methods and the fast fusion of navigation information are the urgent problems to be solved.In this context,this thesis studies the underwater all source positioning and navigation algorithm based on factor graph,which is a probability graph model.First of all,the different ways of underwater navigation is introduced in this thesis,the concept and characteristics of "All Source Positioning and Navigation" are described,the basic knowledge of the factor graph,the problem of state estimation in navigation can be migrated to factor graph to solve,and it will be transformed into least squares problems through inference are introduced in this thesis,for the following design of underwater all source positioning and navigation algorithm lays a foundation.Then,an underwater all source positioning and navigation algorithm based on factor graph is designed in this thesis,including modeling the factors of different underwater navigation,and constructs a factor graph framework of the underwater all source positioning and navigation system.Then,aiming at the problem that the number of factors increases too fast due to too high output frequency of IMU,the preintegration method of IMU is adopted to solve the problem.In order to quickly integrate the information of various navigation methods and ensure the real-time performance of optimization,the sparsity of the factor graph is used to carry out incremental update,and the feasibility of the designed algorithm is verified by simulation.Then,this thesis compares the designed underwater all source positioning and navigation algorithm based on factor graph with the traditional information fusion algorithm,including the classical kalman filter algorithm and federal kalman filter algorithm,and highlights the advantages of the designed algorithm in information fusion of multiple navigation methods through simulation experiments.Then,by using the flexibility and extensibility of the underwater navigation factor graph framework,the plug and play of navigation sensors with different frequencies can be realized,which is verified by simulation experiments.Finally,this thesis studies the underwater all source positioning and navigation algorithm dealing with the navigation sensors’ abnormal observations,owing to the inertial navigation results in a short time is reliable and make use of the IMU preintegration result,a robust underwater all source positioning and navigation algorithm is proposed in this thesis,shows how to use the result of IMU preintrgration to identify and deal with abnormal measurements,the effectiveness of the proposed algorithm is verified by simulation experiments.