Visual/INS/LiDAR Integration For High-precision Pose Estimation In Complex Urban Environment

High-precision pose estimation plays an irreplaceable supporting role in national defense security,space exploration,economic development and social services.With the advent of the intelligent era,the rapid development of Internet and Internet of things,the continuous improvement of autonomous driving solutions such as Google,Baidu,Tesla and Uber,and the implementation of Beidou project “Xihe” plan(full airspace,full time domain,indoor and outdoor seamless navigation and positioning services),the demand for Location Based Services(LBS)is increasing.Current single-sensor pose estimation method can not meet the needs of people for the accuracy and reliability of navigation and positioning services in complex urban environment.Therefore,the integration of multiple sensors for high-precision pose estimation has become the focus of current academic research and engineering practice in complex environment.This paper focuses on the topic of reliable and high-precision pose estimation by multisource integration in complex environment,and made a profound study of theoretical methods and key technologies based on integration of visual,INS(Inertial Navigation System)and Li DAR(Light Detection and Ranging),the main work and contribution are as follows:(1)When ground vehicles are driving under complex conditions such as high-speed straight,sharp turns,up and down slopes,the key frame selection method of VO/VSLAM(Visual Odometry/Visual Simultaneous Localization and Mapping)with equidistant or fixed time interval will cause accuracy loss,and the method of using image overlap rate and parallax is time-consuming.To take into account the accuracy and flexibility of key frame selection and solve the time-consuming problem of VO/VSLAM in multi-source integration for pose estimation,this paper proposed an adaptive key frame selection method for VO/VSLAM based on inter-frame essential matrix decomposition.The experimental results show that the data redundancy is reduced by 40-60% under the premise of ensuring the accuracy,and improved the realtime performance of the system.(2)Accurate and efficient point cloud map generation in complex urban environment is the key to correct the accumulated error of autonomous vehicle dead reckoning navigation.At present,the visual point cloud map generation method based on sequence images still has some problems in outdoor large-scale rapid reconstruction and efficient processing.To this end,this paper combined initial pose provided by the Visual Li DAR Odometry(VLO)and the disparity estimated by the Pyramid Stereo Matching Network(PSMNet),and proposed a method for generating large-scale visual point clouds from coarse to fine,and the effectiveness of the method is verified by extensive experiments.(3)GNSS signal is easily blocked by urban canyon,three-dimensional traffic,boulevard in complex urban environment,and can not work properly.The pose estimation using visual,INS and Li DAR essentially belongs to the dead reckoning navigation.When there is no loop optimization or high-precision GNSS position constraint,the accumulated error will be generated after long-term operation.Therefore,this paper proposed a low-cost pose estimation method based on prior visual point cloud map constraint for mobile vehicles.The prior visual point cloud map is generated from sequence images as constraint information,and the current frame visual point cloud is estimated by Semi-global Block Matching(SGBM)algorithm,and then the Normal Distribution Transform(NDT)matching algorithm is used to achieve pose correction between the current frame visual point cloud and the prior candidate sub-map.The experimental results show that the proposed method can effectively suppress the accumulation of pose error,the Root Mean Square Error(RMSE)of translation and rotation are less than 5.59 m and 0.08° after running 2.25 km and 3.72 km for the field test data and KITTI data,respectively.(4)The calibration of sensor spatial relationship is the basis for pose estimation by multi-source integration.Aiming at the shortcomings of existing Li DAR and camera calibration algorithms,this paper proposed a simple and easy-to-operate calibration method.Firstly,the fitting edge of the original algorithm is obtained by Li DAR point cloud superposition,which improved the accuracy of corner fitting;secondly,the mean value of the fitting corner is used to improve the convergence of the algorithm;then,the plane normal vector is added to calculate the mean value,which improved the accuracy of the calibration result;Finally,the reprojection RMSE of the eight fitting corners is 0.03 m.(5)Aiming at the accuracy and robustness of pose estimation by multi-source integration in complex urban environments,this paper proposed a Visual Inertial Li DAR Odometry(VILO)pose estimation method based on Visual,INS and Li DAR sensor data integration.In VO module,IMU(Inertial Measurement Unit)information is used to assist image spatio-temporal feature matching,and the pose information provided by IMU is used to constrain VO for pose estimation;in Li DAR Odometry(LO)module,IMU is used to remove the point cloud distortion,and the dynamic objects in the scene are removed,then performed LO pose calculation.Finally,a sliding window Factor Graph Optimization(FGO)algorithm is designed to integrate sensor data such as Visual,INS,and Li DAR for high-precision pose estimation,and a priori visual point cloud map is used to constrain the VILO pose estimation to suppress the error accumulation after long-term dead reckoning navigation.Extensive experimental analysis has been carried out on campus boulevards,complex urban streets and KITTI datasets,the results show that the RMSE of position and attitude are less than 3.21 m and 0.10° respectively after driving 5.03 km,which verified the accuracy and robustness of the proposed method.