GPS/Visual/INS Multi-sensor Fusion Navigation System

Navigation and positioning algorithm is the fundamental part of realizing vehicles'motion control and route planning.This paper puts forward a GPS/Visual/INS integrated navigation algorithm to optimize the positioning accuracy in signal blocking and electronic warfare situation,experiment results show that this algorithm is more accurate,stable and robust than neither GPS/INS nor Visual/INS integrated system.The main work and contributions of this paper are as follows:(1)The Visual/INS integrated navigation system is studied.In the situation when IMU and GPS are inconsistent with the frequency of image acquisition,a multi-sensor data asynchronous processing algorithm is proposed.This paper establishes IMU kinematics propagation equation and its linearized Jacobian matrix,and calculates the kinematic residuals.Considering the drift phenomenon of visual SLAM algorithm,a weighted least square objective functional is constructed based on reprojection error and kinematics residual equation,and the Levenberg-Marquadt(LM)method is introduced to solve this non-linear optimization problem.(2)A GPS/Visual/INS multi-sensor fusion navigation algorithm is proposed to solve the problem of GPS positioning accuracy reduced.To raise the feature-matching accuracy and adaptability of visual SLAM in inhomogeneous light condition,this paper regards the GPS/IMU filtered output on the basis of EKF algorithm as the initial state of a new image frame,and the cost function constructed on the basis of multi-frame and multi-moment sensor data as well as the integrated graph-optimization method enhances the stability and robustness of navigation algorithm.As for the reduction of real-time performance comes along with massive sensor data,the marginalization algorithm is introduced in the process of graph optimization.By updating some instead of all iterative variables,system's computational burden is limited,optimization speed and real-time performance is improved at the same time.(3)Experiment is designed to compare the algorithm proposed in this paper with the GPS/INS integrated navigation algorithm,Visual/INS fusion navigation algorithm respectively,the results show that the GPS/Visual/INS integrated navigation algorithm can improve the stability and feature matching accuracy of the Visual/INS algorithm,as well as the positioning accuracy,smooth continuity,anti-jamming and adaptability of GPS/INS algorithm in complex environment.