Research On Multi-sensor Fusion Algorithms For Autonomous Driving Localization And Mapping Under Complex Working Conditions

Vehicle localization technology is one of the key technologies in the field of autonomous driving,and accurate and robust vehicle localization is a prerequisite for achieving path planning and control of autonomous driving.Since LIDAR measurements are not affected by lighting conditions and weather conditions,and can obtain accurate distance information of the surrounding environment,the accuracy of LIDAR-based localization algorithms is guaranteed from the principle,so LIDAR-based core sensor localization and map building technology becomes the solution for most autonomous driving companies.In this paper,we focus on the multi-sensor fusion localization and map building algorithm with LIDAR as the core sensor for autonomous driving.The robustness,accuracy,and output frequency of this solution cannot meet the localization requirements of autonomous driving in the face of various complex conditions,because the localization and mapping solution using pure Li DAR cannot solve the problems of environmental degradation and low sampling frequency.The high-frequency inertial measurement element as an auxiliary sensor can solve the above problems to a large extent,and the localization and mapping scheme based on the fusion of Li DAR and inertial measurement element can achieve high-precision localization in most scenarios.However,since the cumulative error of inertial measurement elements grows rapidly with time,the localization error of this scheme can dissipate rapidly in scenarios where the LIDAR degrades over time.In this paper,based on the localization and map building scheme based on the fusion of Li DAR and inertial measurement elements,a multi-sensor fusion scheme based on Li DAR,inertial measurement elements and vehicle sensors is proposed by introducing the wheel speed sensor and steering wheel angle sensor on the vehicle chassis.The main research elements of this paper are as follows.（1）In order to effectively fuse the measurement data of LIDAR and inertial measurement elements,accurate extrinsic parameters of LIDAR and inertial measurement elements need to be provided.This paper solves the extrinsic parameters of LIDAR and inertial measurement elements in space based on the principle of "hand-eye calibration" in robotics.Before solving the extrinsic parameters,the motion poses of the lidar and inertial measurement elements need to be estimated separately,and this paper solves the motion poses of the lidar based on the normal distribution transformation algorithm,and then obtains the motion poses of the inertial measurement elements through the combined navigation system.For the errors of the two sensors on the time scale,this paper uses the synchronization signal in the satellite signal to realize the hard synchronization of the lidar data and the inertial measurement element data.（2）To address the problems of low computational efficiency and low accuracy of point cloud alignment of LIDAR odometry.In this paper,we adopt the LIDAR odometry based on point cloud geometric features,firstly,the original point cloud is voxel filtered to reduce the number of laser points,then the point cloud distortion is removed by combining the odometry information of inertial measurement elements,and then the processed point cloud is clustered to remove the noise and extract the point cloud geometric features in turn.Finally,the feature points extracted from the point cloud are aligned with the local map composed of historical key point cloud frames within a certain distance from the current point cloud frame,and the motion position of the LIDAR is solved.（3）In order to improve the localization accuracy and robustness of the algorithm,this paper establishes a multi-sensor fusion scheme based on factor graph to jointly optimize the data from multiple sensors.Firstly,the internal parameters of the inertial measurement elements are calibrated,and the discrete integration model of the inertial measurement elements is combined to derive the inertial measurement element pre-integration model between two LIDAR point cloud keyframes,which will be used to constrain the poses between the point cloud keyframes and provide the initial values of the poses for the point cloud alignment.Meanwhile,this paper derives the longitudinal velocity,lateral velocity and transverse angular velocity of the vehicle based on the two-degree-of-freedom vehicle dynamics model and the input from the vehicle chassis sensors,and pre-integrates the above velocities,which will be used to constrain the poses between two point cloud keyframes and help eliminate the errors of the inertial measurement elements.In addition,in order to eliminate the accumulated error of the algorithm for a long time,this paper adopts an efficient loop closure detection method based on the reduced-dimensional point cloud,and constructs a loop closure constraint factor between two loop closure points,which is used to eliminate the accumulated error between two loop closure points.（4）Finally,in order to verify the performance of the multi-sensor fusion-based autonomous driving localization and map building algorithm proposed in this paper,a variety of complex working conditions are collected by the data acquisition platform built by this paper.The absolute trajectory error and relative trajectory error of the estimated trajectory relative to the ground truth of the algorithm and other algorithms are compared,and the results show that the algorithm proposed in this paper has certain localization accuracy and robustness under various complex scenarios.Compared with the localization and mapping algorithm using LIDAR alone or the localization and mapping algorithm fusing LIDAR and inertial measurement elements,the algorithm proposed in this paper can improve the localization accuracy and robustness to a certain extent and keep the algorithm running normally in the environment of LIDAR degradation for a long time.