Research On Indoor And Outdoor Seamless Positioning Method Based On Factor Graph Optimization In Non-cooperative Environment

As an important research direction in the field of navigation,indoor and outdoor seamless positioning technology is related to national security,national economy and people’s livelihood,which has received great attention in academia and industry at home and abroad.At present,many experts and scholars have made great progress in the multi-sensor fusion scheme,but there are still serious challenges in the continuity and reliability of positioning in non-cooperative environment.Low texture,dynamic scene or non-line-of-sight in complex environment seriously affect the accuracy and robustness of positioning.Aiming at the problems existing in indoor and outdoor seamless positioning in non-cooperative environment,combined with the actual application requirements of the project,this paper carried out a research on indoor and outdoor positioning models of multisensor factor graph fusion such as GNSS,UWB,IMU and Vision.This paper takes advantage of GNSS/Vision/IMU/UWB and other multi-source sensors’ characteristics of "observation heterogeneity and performance complementarity" and the advantages of factor graph optimization technology in multi-source heterogeneous data processing,constructs a scene classification and feature system that drives fusion positioning,forms a scene feature driven plug and play positioning model,and realizes high-precision continuous positioning in complex environments.The main work and contributions of this paper are as follows:(1)Aiming at the problem of high-precision positioning of outdoor GNSS in the case of insufficient available satellites,this paper uses the stability of inter frequency code deviation of GPS/GLONASS/BDS in multi system and multi frequency,proposes a multi system and multi frequency compact combination model considering different inter frequency code deviation of different systems,which solves the problem that the equipment has few available satellites in valleys or cities.Aiming at the low fixed rate of multi frequency ambiguity in multi system,a method based on system classification and sequential update is proposed to gradually realize the fixed ambiguity of multi frequency and multi system.The experimental results show that compared with the traditional RTK Positioning Model,the proposed tight combination model scheme has more advantages in the fixed rate of ambiguity,initial and re convergence speed.The RMSE of 3D positioning can reach 1.6cm,1.3cm and 2.2cm,which is increased by40.7%,35.0% and 42.1% respectively.(2)Aiming at the localization degradation of Vision sensors in low texture scenes,this paper proposes a Vision inertial state estimator with point-line features fusion and structural constraints(PLS-VINS)that adds architectural feature constraints to the back-end optimization(such as using the parallel,vertical and collinear characteristics between the centerline features of man-made buildings).This paper focuses on the improvement of the robustness and performance of the Vision system after adding architectural structural constraints in low texture scenes.At the same time,an improved line element error reprojection model is constructed,which improves the optimization efficiency and accuracy of line features.The results of the public dataset Eu Ro C show that compared with VINSMono,the positioning RMSE improves the rotation and translation by 29.46% and 23.79% respectively;The measured results show that compared with VINS-Mono,PLS-VINS shows greater advantages,and the RMSE in translation is increased by 55.64% and 64.88% respectively.Especially,when there are fewer texture features in the scene and more architectural structural constraints,the algorithm advantage of PLS-VINS is more obvious.(3)Aiming at the problem of reducing the positioning accuracy when there is a dynamic scene in the process of camera tracking,the research on camera dynamic detection and elimination technology based on IMU prior constraints is carried out.Combined with the improved clustering algorithm of robust k-means++,a portable camera dynamic feature detection and elimination technology is studied,and the feasibility of VIO location in the presence of more dynamic pedestrians indoors is explored,so as to improve the positioning and mapping performance of the system in dynamic scenes.By testing and analyzing 1530 images with dynamic features in the scene,1311 images have a recognition accuracy of 100%,and other images have at most two feature point recognition errors,and the recognition accuracy of dynamic features is more than 80%.The experimental results show that the proposed dynamic detection and elimination can well identify and eliminate the dynamic features in the scene.(4)Aiming at the serious problems of traditional factor graph in the weight distribution of observation information,a robust factor graph Vision/IMU/UWB tightly coupled positioning model(RFG-TVIU)with dual functions of weight adjustment and gross error elimination is proposed.By changing the size of the observation noise covariance matrix of each sensor to suppress the influence of observation anomalies,the positioning accuracy and robustness of integrated navigation are improved.At the same time,based on the principle of weighted sequential adjustment,a method of real-time initialization of UWB base station location using VIO is proposed.Aiming at the problem of UWB NLOS error in the fusion process,the identification and compensation of UWB NLOS error based on robust extended Kalman filter are carried out,and the influence of UWB NLOS error on the system in the process of Vision/IMU/UWB tight combination is solved.The experimental results show that the plane RMSE of the proposed RFG-TVIU can reach 8.4cm and 10.5cm in Vision scene and non-line-of-sight scene,and the positioning accuracy is greatly improved compared with other Vision/IMU/UWB multi-source sensor fusion algorithms.Compared with the algorithm based on standard factor graph,the greater the NLOS error of UWB,the more obvious the effect of improving the positioning performance of RFG-TVIU.(5)Aiming at the problem of seamless indoor and outdoor location in urban complex environment,a GNSS/Vision/IMU semi-tightly coupled position algorithm based on factor graph optimization is proposed,which realizes high-precision continuous position for urban complex environment and indoor and outdoor transition scenes,and a multi-source sensor synchronous controller is designed.For the asynchronous fusion of non-homologous sensors,the synchronous controller completes the establishment of high-precision time benchmark and realizes the data synchronization of each sensor.To accelerate the initial calibration between VIO and GNSS and solve the problem of seamless coupling between sensors,a nonlinear optimal initial calibration algorithm based on the trajectory of GNSS and VIO is proposed.The experimental results show that the semi-tightly coupled system based on GNSS/Vision/IMU can provide high-precision continuous positioning whether in outdoor occlusion or indoor and outdoor transition areas.The proposed nonlinear optimization initial calibration algorithm based on GNSS and VIO motion trajectory improves the RMSE after calibration by nearly 50% compared with the traditional initial calibration algorithm.The reliability and positioning accuracy of the algorithm model proposed in this paper have been verified by a large number of experiments,and can be used for high-precision indoor and outdoor seamless positioning of robots,vehicles and UAVs.