Heterogeneous Sensor Information Fusion Localization Algorithm Based On Factor Graph

In recent years,unmanned ground vehicles(UGVs)have become the trend of ground vehicles development in the future.With the in-depth research of driverless technology,the working performance requirements of unmanned vehicle positioning system are also gradually improved.Existing unmanned ground vehicles often carry multiple sensors to obtain redundant measurement information.The working conditions and output frequencies of different kinds of sensors are different.As a result,the positioning information available to driverless vehicles will constantly change with the dynamic change of environment or time.Multi-source information fusion technology can dynamically combine the available measurement information and calculate the positioning results,which has achieved good results in the positioning system of multiple carriers.In this paper,the multi-source information fusion positioning system of unmanned vehicle is established based on the factor graph model.The problems existing in the system are studied and the factor graph model is improved.The dynamic adaptability,reliability and accuracy of multi-source fusion positioning system of unmanned ground vehicle are further improved.In this paper,the theory of factor graph algorithm is discussed,and its estimation method based on probability and incremental optimization method for reducing computation are introduced.In the real-time positioning problem of unmanned vehicle,the accuracy of the positioning result at the current time is required to be higher,and the global optimization process of factor graph has a large amount of calculation and a small gain for the positioning result at the current time.To solve this problem,the structure of factor graph is simplified to improve the real-time performance.The sensor factor nodes are designed for each sensor,which are combined into a complete factor map multi-source information fusion positioning system,and simulation experiments are carried out to prove its feasibility.Aiming at the problem of sensor measurement delay in the system,a method was designed based on factor graph model to compensate the delayed positioning information of satellite positioning system(GPS)and visual odometer(VO)using the measurement information of inertial navigation system(IMU).After compensating the delayed positioning information,the compensated positioning information is integrated into the factor graph at the next variable node corresponding to the receiving time.The method proposed in this paper not only gives consideration to real-time performance,but also reduces the errors in system positioning results caused by time delay.Simulation experiments are carried out to prove the feasibility of measuring delay compensation method proposed in this paper.Aiming at the problem that the factor graph algorithm is difficult to identify and exclude abnormal observations,an adaptive factor graph method is designed by combining the robust estimation algorithm with the factor graph.The reliability factor is added in the construction of the factor node,which can be adjusted adaptively according to the current measured value to reduce the weight of abnormal observations and ensure the normal operation of the system.Aiming at the problem that the system error cannot be recovered quickly when GPS is reconnected in the environment of GPS rejection for a short time,the reliability factor that changes with time is designed.Aiming at the problem that it is difficult to judge the reliability of reconnected GPS positioning information,a sensor reconnection screening method is designed.Simulation experiments are carried out to prove that the adaptive factor graph algorithm proposed in this paper can effectively reduce the errors of system positioning results when dealing with the influence of abnormal measurement values.