Robust Cooperative Positioning Algorithm In The Multi-source Information Fusion Navigation System

Cooperative positioning(CP)technology enables users to share theirs’ s position information by communication channel which can reduce user’s dependence on surrounding facilities and anchor nodes.Therefore,CP is an enhancement scheme of the existing navigation and positioning system as well.With the help of CP technology,users who could not locate originally can determine theirs’ s position with acceptable accuracy.Furthermore,the navigation performance of the original navigation system and the application scope of the navigation service is improved and builds a more accurate,more robust and more adaptable cooperative positioning algorithm based on multi-source information fusion.With the increasing demands for location services,CP concepts and technology are expected to play an important role in the military and civilian fields,such as intelligent transportation systems,swarms of Unmanned-Vehicles,rescue and search,etc.Unfortunately,the practical application scenarios of CP are not always ideal.There are some problems in those scenarios,such as poor observation quality,relative measurements are susceptible to environments,and the arrivals of delayed measurements,which challenge the availability,adaptability,robustness and delay resistance of CP algorithms.In this situation,CP algorithms working in the multi-source fusion system should be the fundamental content of the research in this paper.Key technologies are carried out from the aspects of enhancing adaptability,improving relative measurement methods,improving robustness and processing capacity of delay measurements.The major study and innovative work can be described:(1)To address the issue of poor accuracy and weak adaptability of classical CP algorithm in harsh CP environments,the Global Navigation Satellite System(GNSS)/ranging collaborative positioning algorithm based on adaptive Kalman filter(CPAKF)is presented.By setting the adaptive filter slide window and applying the maximum likelihood criterion to estimate the adaptive parameters from the angle of maximum probability of occurrence of the observed quantity.The adaptive updating equation of the measured noise covariance matrix is derived by estimating the new covariance matrix in the sliding window.In this way,the estimation performance of continuously poor observation noise in harsh CP environments is significantly improved.Simulations and experimental tests show that compared with standalone positioning based on Kalman filter(SKF)and cooperative positioning based on Kalman filter(CPKF)algorithm,the CPAKF algorithm has significant advantages in positioning accuracy,measurement noise covariance matching rate,and adaptability in harsh CP environments.(2)In order to achieve reliable positioning of cooperative users in indoor environments and avoid problems such as poor measurement accuracy caused by reflections in indoor environments using relative measurements methods based on RF signals,an inertial navigation system(INS)/ visual topology measurements(TOP)integrated algorithm for CP is presented.By collecting images of users by setting an external camera indoors,the proposed TOP algorithm uses Faster RCNN to complete the objects detections of pre-trained users,and estimates the user’s distance,azimuth,and elevation relative to the monocular camera based on the mapping relationship between the user’s projection onto the camera’s imaging plane,and achieves three-dimensional location estimation of cooperative users.In order to further improve the positioning performance in the visual occlusion environment,the fusion of INS transient auxiliary information and TOP measurements can effectively improve the dynamic pose estimations of cooperative users.Experimental tests show that the INS /TOP algorithm has better positioning accuracy than Ultra-Wide Band(UWB)solution and can more effectively decrease the positioning errors caused by occlusion than the pure TOP algorithm in indoor areas with dense objects.(3)To address the deficiencies in standard filters and low robustness in the cooperative localization system,a novel INS/TOP/GNSS cooperative positioning algorithm based on robust factor graphs is proposed.In terms of optimizing the filtering model,by constructing the factor functions of INS,TOP and GNSS,a scalable parameter optimization model based on the factor graph can be derived,which can flexibly realize the optimization and updating of online and offline sensors.In terms of suppressing outliers,an improved switch constraint algorithm is proposed by introducing a weight decision approach that considers residuals and detection scores comprehensively.This algorithm can effectively eliminate abnormal topological measurement values with multiple borders of the same user,improve the prediction success rate of outlier points of topological measurement,and suppress the low-valued switching variable to ensure that the cooperative positioning algorithm is not affected by outliers.Simulations and experimental tests show that the improved switch constraint algorithm not only reduces the calculation overhead,but also improves the positioning accuracy and robustness.(4)Aiming at the problem of CP measurements delayed arrivals in the multi-source fusion system,the delayed measurements processing algorithm based on minimum Mahalanobis distance selection mechanism is presented which considers the time difference between the delayed arrivals of measurements and the navigation states.In each sliding window,a navigation state with the minimum Markov distance from the cooperative positioning measurement is always selected as the fusion node to reduce estimation error due to the delayed arrival.In addition,in order to realize the parameter estimation of the latent variable of delay,an augmented delay factor algorithm is proposed.This algorithm constructs a corresponding delay factor node for each navigation state in the sliding window.By augmenting the delayed latent variable and the delay factor node to jointly optimize the navigation state,the global optimization accuracy of the fusion system is significantly improved.Simulation experiment analysis shows that,compared with the minimum Mahalanobis distance selection mechanism,the delayed fusion system using the augmented delay factor algorithm has higher positioning and velocity accuracy but with higher time consumption.As the total time of iterative optimization in a single sliding window required is much shorter than the total length of the corresponding sliding window,it will not affect the output of the next fusion result.