Research on NDT-based Positioning for Autonomous Driving

Autonomous driving technology is one of the currently popular technologies, while positioning is the basic problem of autonomous navigation of autonomous vehicles. GPS is widely used as a relatively mature solution in the outdoor open road environment. However, GPS signals will be greatly affected in a complex environment with obstruction and electromagnetic interference, even signal loss may occur if serious, which has a great impact on the accuracy, stability and reliability of positioning. For the time being, L4 and most L3 autonomous driving modules still provide registration and positioning based on the highprecision map constructed. Based on this, this paper elaborates on the reconstruction of the experimental scene environment, using the SLAM (simultaneous localization and mapping) method to construct a highprecision point cloud map. On the constructed prior map, the 3D laser point cloud NDT matching method is used for real-time positioning, which is tested and verified on the “JAC Electric Vehicle” platform. The experimental results show that this algorithm has high positioning accuracy and its real-time performance meets the requirements, which can replace GPS signals to complete the positioning of autonomous vehicles when there is no GPS signal or the GPS signal is weak, and provide positioning accuracy meeting the


Introduction
The paper mainly discusses the application of highprecision positioning based on NDT algorithm in the autonomous driving system. The autonomous driving system usually has a three-layer structure of perception, planning and control, of which, perception is subdivided into environmental perception and positioning. Environmental perception and positioning are considered one of the most challenging topics in the field of autonomous driving research and application [1] .Currently, the mainstream positioning technology for autonomous driving is based on the global positioning and navigation system combined with differential positioning technology (RTK technology) to complete positioning. However, GPS signals are easily compromised in complex environments such as "urban canyons" with tall buildings, environments surrounded by trees, tunnels, underground garages and viaducts. In severe cases, the signals are easily weakened or even lost, leading to inaccurate positioning. Therefore, the positioning method based on global satellite navigation system is difficult to be popularized in the field of autonomous driving. Given the above problems, the current most popular solutions include: dead reckoning positioning based on odometer and inertial sensor and simultaneous localization and mapping technology (SLAM). These two types of methods mainly use sensors to measure the attitude changes of the autonomous vehicle within a known time interval, such as displacement, speed and acceleration to predict the current attitude from the previous attitude. However, such methods will inevitably involve cumulative errors due to the presence of noise in the process of sensor measurement and the noise of motion model at the prediction stage, that is, as the number of reckoning steps increases, the positioning error will continue to increase [2] . It is not easy to use the above methods alone to complete long-range highprecision positioning in the autonomous driving scenarios.
Thus, it is more accurate and simpler to locate the autonomous vehicle based on high-precision map. NDT algorithm is a kind of technology that leverages existing high-precision maps and real-time measurement data of LiDAR to realize high-precision positioning. NDT was originally proposed by Biber P [3] , et al. for 2D scanning matching algorithm. In this paper, NDT algorithm is used to match point cloud data in real time based on 3D laser data, which can complete the high-precision realtime positioning of autonomous vehicles without GPS signals.

Point cloud map construction based on LIO-SAM algorithm
LIO-SAM algorithm [4] is a factor graph optimization algorithm based on LiDAR, IMU and GPS sensors proposed by Tixiao Shan, et. al. in 2020, which takes advantage of the tightly coupled laser-inertial odometer method to optimize the laser SLAM through the factor graph method. It introduces four factors: IMU preintegration factor, LiDAR mileage factor, GPS factor, and closed-loop factor, and employs frame-local map instead of frame-global map to improve the efficiency of frame-map matching. New key frames are registered into a fixed-size prior "sub-key frame" set by selectively introducing key frames and an effective sliding window method, which can significantly improve the real-time performance of the system by scanning matching locally.
This algorithm assumes that the point cloud distortion is nonlinear motion model and raw IMU measurement is used to estimate sensor motion during the LiDAR scanning. In addition to eliminating the point cloud distortion, the estimated motion can also be used as an initial guess for LiDAR odometer optimization. Then the obtained LiDAR odometer solution is applied to estimate IMU deviation in the factor graph. It is possible by introducing a global factor graph for robot trajectory estimation to use LiDAR and IMU measurement to effectively perform sensor fusion, incorporate position recognition in the robot attitude, and introduce absolute measurement when available, such as GPS positioning and compass heading. Such sets of factors from various sources are used for joint optimization of graphs. Furthermore, the old LiDAR scanning is marginalized for attitude optimization instead of matching such scanning to a global map like LOAM. The radar attitude is aligned with the global map by using a LiDAR-IMU tightly-coupled fusion method, adding IMU on the basis of LOAM [5] , jointly optimizing the measured values of LiDAR and IMU, and cooperating with the rotation constraint optimization algorithm (LIO-mapping) to effectively reduce the longrange odometer drift and get a good mapping effect. On the basis of LeGO-LOAM and LIOM, LIO-SAM adopts the factor graph optimization algorithm, combines with the sliding window method, and integrates IMU preintegration factor, laser odometer factor, GPS factor and closed-loop detection factor, to complete the multisensor fusion and global optimization in order to provide good attitude estimation and mapping effect.
The flow chart of its algorithm is shown in Figure 1: After the construction of the point cloud map, the NDT registration algorithm is utilized to realize the positioning function of the system. ICP iterative method is the registration algorithm commonly used in 3D registration. But the final iterative result may fall into local optimum due to the defects of the algorithm itself, resulting in registration failure, and frequent failure to achieve the desired effect [6] . Some scholars also perform relative positioning based on 3D laser according to ICP and its improved algorithm. The ICP algorithm repeatedly aligns the two sets of point clouds by identifying the corresponding relationship point by point between the two point clouds until the stop condition is satisfied. Hence, the point cloud registration problem with high ICP processing density presents high computational complexity, strong dependence on the estimation of point pairs, and low robustness.
The NDT registration algorithm referred to in this paper utilizes the standard optimization techniques to determine the optimal match between two point clouds, which can estimate the rigid transformation between the two point clouds [7] . Since the NDT algorithm does not leverage the feature calculation and matching of the corresponding points in the registration process, the calculation time is shorter than other methods, thereby saving time and improving robustness.

Traditional NDT algorithm
Normal distribution transformation (NDT) algorithm is a registration algorithm that is applied to the statistical model of multi-dimensional points, whose main idea is to map the point cloud to a smooth 3D space and employ a set of local probability density functions (PDF) with each PDF representing the shape of one space [8] . NDT first converts the reference point cloud (namely highprecision map) into the normal distribution of multidimensional variables. If the transformation parameters can make the two sets of laser data match well, the transformation point will have high probability density in the reference frame. As a result, consideration may be given to using an optimized method to work out the transformation parameters that maximize the sum of the probability density. In this case, the two sets of laser point cloud data will match the best. The NDT algorithm steps are as follows: (1) The space occupied by the reference point cloud is divided into grids or voxels of specified size, and the multi-dimensional normal distribution parameters of each grid are figured out: 1 Wherein: q represents the mean value; ∑ represents the covariance matrix; x i represents all scanning points within a grid.
(2) Set the initial transformation parameter, which is the attitude to be estimated: (3) For the point cloud that needs to be registered, transform it to the grid of the reference point cloud through conversion T: x´i=T(x i , p): ' cos sin : ' sin cos (4) Calculate the probability density of each transformation point according to the normal distribution parameters: (5) The NDT registration score is obtained by adding the probability density calculated for each grid (score): (6) Newton optimization algorithm is employed to optimize the objective function-score to find the transformation parameters p so that the value of score is the largest.
The flow chart of this algorithm is shown in Figure 2:

NDT positioning process
NDT algorithm first performs normal distribution transformation of the target point cloud to get all voxel grids of the point cloud, then inputs the initial attitude of the current input point cloud, and uses this attitude as the initial search position. The initial attitude is an estimated attitude of the initial point cloud in the target point cloud, which can cause the Newton optimization algorithm in NDT to converge quickly. After initial estimate, searching for a set of optimal NDT matching parameters is started so that the input point cloud gets the highest degree of fit with the target point cloud after being transformed by this set of parameters. During the autonomous driving of a vehicle, the goal is to find the relative attitude of the current input point cloud and the target point cloud when using the NDT algorithm for scanning registration so that such two point clouds can be well matched. The specific algorithm steps are as follows: (1) Down-sampling is carried out to the input point cloud; the denser the input point cloud, the greater the computational complexity required for NDT registration, the worse the initial attitude estimation, and the greater the corresponding computational complexity. Therefore, it is necessary to reduce the input point cloud density through VoxelGrid down-sampling to increase the speed of NDT registration.
(2) Acquisition of initial attitude. All methods that use pre-constructed maps for registration and positioning require an initial attitude, and obtain the X, Y and yaw angle YAW of the initial attitude in the map coordinate system through GNSS.
(3) Get a prior map. The target point cloud in the registration process is the point cloud map constructed using the LIO-SAM algorithm.
(4) Set NDT algorithm parameters. Search for the minimum variable, search step size, NDT voxel grid of the target point cloud, and the number of iterations optimized by use of Newton algorithm.
(5) NDT registration. The initial attitude estimation is used in the initial registration. The previous registration result is used for registration in the subsequent registration process.
(6) Convert the coordinate system and release the positioning information. The constructed point cloud map is a UTM coordinate system. The attitude of the NDT positioning is converted to the geodetic coordinate system and released for vehicle positioning to complete this positioning.
The NDT positioning process is shown in Figure 3:

Experimental platform
This algorithm uses LiDAR sensors to provide highprecision positioning, which is more inclined to provide positioning information with sufficient accuracy when the GPS signal in the environment is weak, so as to make up for the defects of GPS positioning. In the experiment, differential GPS positioning is used to compare with this algorithm, hence, it is necessary to give full consideration to the effect of the strength of the satellite signal on the experimental control in the experimental design. This experiment uses data collected by OUSTER 32line LiDAR for verification as well as the experimental platform "JAC Electric Vehicle", as shown in Figure 4. The experimental site is the Nanhu Campus of Wuhan University of Technology, where the environment distribution is relatively complex and there are two-way lanes in the area with trees and buildings on both sides, and it is a closed park scene. As shown from the quantity of satellite signals received, satellite signal loss occurs to some road sections, but most road sections have good signals.
First, a large-scale park map is constructed. The time periods with fewer people and vehicles are selected to collect data in order to avoid interference of dynamic objects to the algorithm. The speed of the vehicle is kept at about 20km/h, and multiple sets of data are collected to construct a large-scale point cloud map. The vehicle positioning test is performed on the constructed highprecision map. In order to verify the positioning accuracy of NDT algorithm, the value of the differential GPS is used as the control value in the same scene, and the positioning result is compared with the differential GPS trajectory and analyzed.

Large-scale mapping experiment
To construct the target point cloud map, data is collected by driving a vehicle on Nanhu Campus of Wuhan University of Technology. During the map construction process, the registration between two frames is carried out with the starting point as the origin of coordinates, and the new point cloud is converted to the local coordinate system to achieve the purpose of incremental smooth mapping. This point cloud map is constructed in the UTM coordinate system, with the mapping results shown in Figure 5 and the satellite map of this location shown in Figure 6:

Vehicle positioning test
(1) Perform NDT real-time positioning on a highprecision point cloud map constructed in advance, and compare the NDT positioning results with the differential GPS trajectory, as shown in Figure 7. (2) Calculate the mean square error in the data set, namely the positioning accuracy. As shown in Table 1 below: (3) Further visualize the data, with the specific analysis as follows: The positioning accuracy is further verified by calculating the cumulative error. During the driving of the vehicle, the difference S between GPS positioning and NDT positioning is worked out at the interval of t, and the cumulative error VS time curve is plotted with t as the horizontal axis and S as the longitudinal axis to intuitively show the changes in cumulative error of navigation and positioning over time.

Summary
To solve the problem that GPS signals are easy to lose in the process of autonomous driving, this paper employs a method of positioning by use of NDT registration on the basis of high-precision point cloud map. This paper uses the LIO-SAM algorithm to construct high-precision point cloud map and gives out the theoretical derivation and implementation process of the NDT registration algorithm, which can provide precise positioning of the autonomous driving system in the absence of GPS signals. During the experiment, we use "JAC Electric Vehicle" as the experimental platform, which integrates sensors such as LiDAR and IMU, and realizes the interaction with sensor data, the information transmission and positioning information transfer between the sub-nodes on the ROS robot operating system. The experimental test is performed in the campus environment of Wuhan University of Technology. Finally, the NDT positioning results are compared with the GPS signals, and the comparison results verify the timeliness and accuracy of the algorithm, which can provide the expected effect of obtaining higher positioning accuracy during the autonomous driving process. This algorithm can be widely applied in the scenes such as autonomous operating vehicles in industrial parks and unmanned sightseeing vehicles in scenic areas.