Combined Localization Research Based On GNSS/INS/LiDAR

With the development of navigation technology,environmental perception,and intelligent control technology,the research of autonomous vehicle has become a current hot spot.Precise vehicle positioning is an important prerequisite for the realization of autonomous driving functions.Sensors such as Global Satellite Navigation System(GNSS),Inertial Navigation System(INS)and Light Detection and Ranging(Li DAR)have their own advantages and disadvantages,and high-precision positioning of the vehicle cannot be achieved in scenarios such as GNSS signal loss.Aiming at the problem of multi-sensor positioning information fusion,this paper study the combined positioning algorithm based on GNSS/INS/Li DAR,design a combined positioning terminal system.The main work of this article is as follows:(1)Research on sensor-related positioning algorithms.Based on the analysis of the definition and conversion relationship between the coordinate systems and the attitude description method,the principles and measurement models of the INS,GNSS and Li DAR are studied.Aiming at the problem of inaccurate positioning caused by GNSS signal loss,an error model of INS in geographic coordinate system is derived,and a GNSS/INS combined positioning Kalman filter model is constructed and verified by simulation experiments.(2)Time and space synchronization of sensors.The time synchronization and space synchronization of Li DAR and GNSS/INS receiver are the prerequisites for sensor data processing.Coarse time alignment is achieved through hardware design;based on the coarse alignment,a parallel algorithm of linear interpolation and quaternion spherical interpolation is proposed,which linearly interpolates displacement offsets and performs spherical interpolation compensation for attitude quaternions.Aiming at the problem of sensor spatial synchronization,a density clustering algorithm based on Kd-Tree nearest-point optimization algorithm is proposed.Iteratively iterates the six-dimensional installation parameters,and each iteration converts the position of the point cloud set of adjacent frames to a unique coordinate system,Evaluate the coincidence degree of the point cloud of the two frames and retain the optimal placement parameters until the six-dimensional parameters are solved.(3)Real-time SLAM mapping algorithm based on Li DAR/INS.Aiming at the problem of pose consistency of the carrier,a matching algorithm based on the motion estimation of the carrier between the point clouds of adjacent frames and the local octree map is proposed,associate the motion of the point cloud of adjacent frames with the local map to improve the accuracy of point cloud mapping.The splice of the Li DAR point cloud is realized at the front end.In the back end,Levenberg-Marquart optimized factor graph algorithm is introduced to optimize the motion state of the carrier and saved the pose and point cloud of key frames.Aiming at the problem of error accumulation for point cloud map stitching,a loop-back factor detection algorithm based on the coincidence degree of Li DAR point cloud is proposed.By comparing the point cloud coincidence between the current frame and the previous key frames,detect whether the carrier returns to its previous position in real-time,and add the detected loopback factors to the factor graph framework to obtain globally consistent trajectories and maps.(4)Li DAR/INS/vehicle speed combination algorithm based on a priori point cloud map.Save the positioning data when the GNSS fails,and use the service mechanism of the ROS system to obtain the sub-maps around the carrier from the prior point cloud map.The sub-map is matched with the current Li DAR data to obtain the real-time pose of the carrier.Aiming at the problem that it takes a lot of time to retrieve the sub-map,based on the current pose of the carrier,a multi-mode algorithm is used to predict the pose of the carrier after the movement,calculate the position of the carrier in the map during a short time,realize positioning and reduce the program operating load.Through repeated map matching and moving pose prediction algorithms,continuous high-precision positioning of the carrier in the case of GNSS failure is achieved.Experimental results verify the effectiveness of the proposed method.