Research On LiDAR/INS/ODO/GNSS Vehicle Integrated Navigation Algorithm Based On Graph Optimization

With the rapid development of digital earth and smart city construction,people's demand for location services is becoming more and more urgent.The State Council of China emphasized in the “New Generation Artificial Intelligence Development Plan”:“Focus on breaking through common technologies such as the computing framework of autonomous unmanned systems,perception and understanding of complex dynamic scenes,real-time precise positioning,and adaptive intelligent navigation for complex environments to support unmanned systems applications and industry development”.However,GNSS positioning accuracy is poor or even invalid in complex GNSS signal environments such as urban canyons;SLAM has poor positioning availability when environmental features are insufficient,such as tunnels;the positioning error of low-cost INS quickly diverges over time without external information assistance.The continuous,accurate and reliable positioning navigation in complex environments is a common key technical issue that urgently needs to be broken.In order to improve the positioning and navigation service capabilities,China will build a more ubiquitous,more integrated and smarter national integrated PNT(Positioning,Navigation and Timing)framework with the Beidou system as the core in 2035.Multi-sensor information fusion is one of the main contents of the comprehensive PNT construction in the future.Supported by the national key R&D program “Collaborative Precise Positioning Technology”,“High-availability and High-precision Indoor Intelligent Positioning and Indoor GIS Technology”,this thesis carried out research on key technologies of LiDAR/INS/ODO/GNSS vehicle integrated navigation based on graph optimization,facing the requirement of continuous,accurate and reliable positioning and attitude determination information for vehicle in complex outdoor environments.And this thesis focuses on optimizing the calibration of the sensor external parameters,enhancing the accuracy and environmental adaptability of low-line LiDAR odometry,and improving the accuracy and usability of multi-source fusion positioning in complex environments.The main works and contributions of this thesis include:(1)Aiming at the problem that the error of sensor external parameters restricts the accuracy of multi-sensor fusion,and manual measurement cannot meet the requirement,methods for accurately calibrating IMU/ODO and LiDAR/IMU external parameters using ODO pre-integration and IMU pre-integration are proposed:(a)A method of using ODO pre-integration to calibrate IMU/ODO external parameters through the graph optimization is proposed,which is not sensitive to the initial values of external parameters,avoids the estimation of the vehicle's position and attitude,and has the lever arm estimation capability.The simulation test and actual test show that the lever arm calibration error is less than 5(88),the mounting angles calibration error is less than 0.1°.And the ODO measurement value after the calibrated external parameters conversion can provide centimeter-level mileage increment in the IMU coordinate frame.So the calibration method meets the external parameter calibration requirements of centimeter-level positioning.(b)A precise way of using IMU pre-integration to eliminate the motion distortion of LiDAR point cloud is proposed,which no longer depends on GNSS information,avoids the repeated alignment process in common calibration methods,and the integral form of IMU also mitigates the impact of IMU noise.The simulation test and actual test show that the lever arm calibration accuracy is about 1(88),and the mounting angles calibration accuracy is about 0.1°.And the LiDAR measurement value after the calibrated external parameters conversion can reduce the distance residual on the calibration plane to centimeter level.(2)Aiming at the problems of degraded accuracy of LiDAR odometer and poor environmental adaptability caused by the insufficient vertical resolution of the low-line LiDAR,a LiDAR odometer based on probability map matching by the feature points is proposed,which is combined with the characteristics of feature point matching and probability map matching.This method separates the ground feature points and the non-ground feature points,and matches them in the probability map of the corresponding resolution,effectively reducing the impact of the insufficient vertical resolution of the low-line LiDAR.The use of environmental feature points for probabilistic map matching reduces the dependence on the line and surface features of the environment,and increases the stability of the matching while properly and effectively downsampling at the same time.The comparison test of this method with the well known open source LiDAR odometer solutions,Cartographer and Le GO-LOAM,in three scenes with different feature richness shows that: In the areas with rich line and surface features,the positioning accuracy of this proposed method and Le GO-LOAM is better than Cartographer,especially the elevation position and horizontal attitude errors.However,in areas lack of line and surface features and the ramped ground,the positioning error of Le GO-LOAM is worse than the proposed method and Cartographer,and it may even locate abnormally in some challenging scenarios where there are no line and surface features in the environment.(3)In order to improve the accuracy and usability of vehicle positioning in various complex environments,taking the complementary advantages of LiDAR,INS,ODO and GNSS,a multi-source fusion algorithm based on graph optimization is designed and implemented.On the basis of the LiDAR odometer based on feature point probability map matching,this algorithm uses IMU/ODO pre-integration constraints to enhance the positioning stability in an environment with insufficient LiDAR point cloud features,and the absolute pose assistance provided by pre-built probability map matching to ensure the positioning accuracy when the GNSS fails.And the marginalization of the sliding window is used to remove the historical optimization parameters,so as to keep the calculation volume relatively stable.In order to verify the effect of the proposed whole algorithm in this thesis,a fully functional LiDAR/INS/ODO/GNSS vehicle integrated navigation data processing software — LIOGNS has been developed.Through multiple sets of measured data,three positioning and navigation modes were tested and analyzed,including the positioning with pre-built probability map,GNSS/INS/LiDAR-SLAM fusion positioning and GNSS/INS/ODO/LiDAR-SLAM fusion positioning.(a)The international competition data set test was used to evaluate the accuracy level of LIOGNS in the positioning mode with pre-built probability map matching correction.In the “Autonomous Driving Map Optimization and Sensor Fusion”track of the JD Digital Technology 2018 Global Explorer Competition,LIOGNS's multi-source fusion algorithm was used to optimize the point cloud map,and the positioning mode with pre-built probability maps which was used to perform the navigation positioning.It achieved a position accuracy of 5(88)and a attitude accuracy of 0.1°,and won the championship of the global finals.(b)The GNSS/INS/LiDAR-SLAM integrated positioning performance of LIOGNS,Cartographer and LIO-SAM was tested based on the open-sky environment data with simulated GNSS interruption and real frequent GNSS occlusion campus environment.Since the motion model of Cartographer is designed suitable for low dynamic vehicles,its roll,pitch and elevation errors are relatively large.Limited by the level of IMU error modeling,the horizontal attitude and elevation error of LIO-SAM is are equivalent to Cartographer,and the horizontal position accuracy is better than Cartographer.The position and attitude accuracy of the proposed LIOGNS is better than LIO-SAM and Cartographer,especially in the environment where the GNSS signal is weak or blocked.(c)The accuracy and usability of the ODO assistance was tested based on the opensky environment with simulated GNSS interruptions and the real tunnel scene lack of environmental features.In an environment where LiDAR-SLAM works well,the assistance of ODO has no obvious effect on improving the fusion accuracy.Insufficient LiDAR features in the tunnel lead to lack of constraints in the forward direction,resulting in significant longitudinal position drift,while the ODO limits the position drift along the longitudinal direction of the tunnel effectively,and therefore significantly enhancing the positioning availability of the whole solution when both GNSS and LiDAR fail.In summary,this thesis has conducted a comprehensive research on the LiDAR/INS/ODO/GNSS vehicle integrated navigation algorithm based on graph optimization,and completed the core algorithm design and software implementation.Through the datasets of open-sky area with simulated GNSS interruption,GNSS frequent failure environment and tunnel scene,the proposed algorithm in this thesis has been comprehensively and thoroughly tested and analyzed.The research results of this thesis can meet the requirements of continuous,accurate and reliable positioning and navigation of land vehicles in complex environments,and provide a complete and feasible integrated navigation and positioning reference scheme for autonomous driving and mobile robot applications.