Research On Multi-Sensor Fusion Algorithms Of Inertial Navigation System Based On Factor Graph Optimization

With the development of unmanned ground vehicle and unmanned aerial vehicle,high-precision navigation and positioning technology as a basic function gradually plays an indispensable role in military and civil fields.A single sensor is not able to adapt various environments,and a multi-sensor fusion architecture which can provide high-precision positioning and achieve plug and play is thus required.Based on the probability model of factor graph,this paper presents a novel algorithm framework that is able to satisfy the demands of a robust,high-precision multi-sensor fusion navigation system.Firstly,the mathematical model of the optimal navigation solution is deduced by Bayesian inference theory,and the concept of factor graph framework is introduced.Then The relationship between the probability model of factor graph and the optimal navigation solution is discussed,which consists of sensor error model and factor node,and the derivation of the optimization algorithm based on factor graph,and it provide theoretical basis for multi-sensor information fusion algorithm.A Plug and Play integrated navigation system can be realized by characteristics of the factor nodes.Then the inertial navigation system and integrated navigation system are discussed,and the error model based on inertial navigation system is constructed.The Kalman filter is introduced by Bayesian inference method,so that the linear state equation and observation equation of multi-sensor navigation system are arranged.the error model function based on factor diagram is constructed.The simulation results show that the effect of the algorithm based on linear graph is the same as that of Kalman filtering algorithmA multi-sensor fusion algorithm based on factor graph is proposed to solve the divergence of state estimation for linear systems.The retract and the derivative of function in manifold is described,and the mapping between orthogonal group and rigid body transformation is analyzed.Aiming at the problem of synchronization between high frequency signal and low frequency signal,the thesis constructed a new observation information by pre-integration method,and analysis of error uncertainty Caused by noise propagation and construct a new IMU factor.The number of variable nodes in the factor graph is limited and the calculation cost is reduced.The simulation results show that the method can reduce the divergence of nonlinear state estimation.Finally,The Two sets of simulation experiments which verify the feasibility of the multi-sensors fusion algorithm based on factor graph were tested,it uses various sensors including IMU,GPS receiver,camera and so on.Experiments show that the fusion algorithm can provide high precision navigation information in real time.In order to improve multi-sensor fusion navigation system,the method based on innovation detection and robust kernel is proposed.The simulation experiments show that the method can reduce the error caused by fault information.