An incremental LiDAR/POS online calibration method

Extrinsic Calibration between LiDAR and POS (position and orientation system) is a fundamental prerequisite for varieties of MLS (mobile laser scanner) applications. Due to the sparse structure of LiDAR data, the current calibration methods relying on common point feature matching are unreliable, and the low accuracy POS results make the extrinsic calibration of MLS system more challenging. In this paper, we propose an incremental estimation method of six degree of freedom extrinsic transformation of LiDAR and POS. Firstly, the POS-SLAM is used to accumulate LiDAR scans as online sub maps. Attitudes of the carrier are calculated by using GNSS/INS loose combination method of bidirectional adjustment, and scans are associated with sub map based on the time interpolation. Then, the extrinsic calibration parameters are estimated by optimizing corresponding points difference between SLAM and MLS coordinate frame. Finally, field tests have been conducted to the proposed method. RMS between the map by the calibrated MLS and by the static measurement is 0.57 cm. The results demonstrate that the accuracy and robustness of our calibration approach are sufficient for mapping requirement of MLS.


Introduction
In modern industrial applications, LiDAR, GNSS and IMU are assuming increasingly critical roles in autonomous driving robot control, etc. However, methods by single sensor are limited by their shortcomings and rarely perform well. LiDAR is an optical metrological technology that captures target surfaces in the three-dimensional (3D) space with highly redundant sets of discrete points. However, LiDAR is also defective in high-speed carrier, due to its limited vertical FOV, discreteness of scanned points, and scanning distortion. IMU * Author to whom any correspondence should be addressed.
Original content from this work may be used under the terms of the Creative Commons Attribution 4.0 licence. Any further distribution of this work must maintain attribution to the author(s) and the title of the work, journal citation and DOI. is a technology that captures high frequency self-motion. It provides the carrier's motion state at a higher frequency than LiDAR, thereby helping data association of scans and reducing scanning distortion [1]. The POS (position and orientation system) composed of IMU and GNSS can eliminate the accumulated error of IMU and obtain more stable attitude information. Similarly, the integrated navigation scheme MLS (mobile laser scanner) is more and more widely used in engineering. Cedric proposed a distortion correction method for LiDAR based on IMU pre-integration [2]. This method optimizes LiDAR matching by using IMU results, and limits IMU errors drift by LiDAR matching. Yang et al [3] propose a tightly-coupled aided inertial navigation with LiDAR and camera. Feature points tracked from camera and planes feature from LiDAR are used as the geometric constraints to enhance its state estimator. The combination method of IMU and LiDAR is more robust and become one of the mainstream methods in navigation.
Uncertainty of attitude calculated by LiDAR or POS will hinder their joint applications, so the extrinsic calibration is required to obtain accurate transformation between LiDAR and POS. Due to the discrete of LiDAR data, little work has been focused on their calibration.
In this paper, we propose an online LiDAR/POS calibration method based on incremental scan matching. Sensors achieved time synchronization with PPS signal from GNSS, and their time delay only a few milliseconds. Inspired by the Cartographer [4], we designed a SLAM method combined with GNSS. It obtains the global cloud point map and the trajectory of LiDAR. Next, the trajectory of the carrier is obtained through the loosely coupled GNSS/IMU algorithm. Correlate scan data to global map through carrier trajectory interpolation. Finally, residual optimization process between the corresponding data points will calculate the exact calibration parameters. The method has no restrictions on scanning mode, and does not require assumptions based on feature extraction. The main contributions of our work are as follows: • A novel field calibration model for LiDAR/POS guidance is proposed. More accurate and robust calibration parameters are obtained using long-range trajectory optimization. • A multi-resolution-based scan-to-map matching method is used, enabling the generation of high-precision point cloud map even on low-cost platforms. • The performance of the method has been evaluated by practical tests. In addition, the errors of each step and the effects on the overall accuracy and of the system are investigated.
The remainder of the paper is organized as follows: section 2 briefly reviews related works. In section 3, we first introduce the notation, task, and assumption of the paper and summarize the proposed calibration approach. We detail the processing procedure of our calibration method in section 4. Section 5 provides some implementation discussions and shows experimental results. Finally, the conclusion is drawn in section 6.

Related work
The system based on optical sensors to realize environmental perception, location and navigation by collecting surface information of space objects, and IMU assist providing relative position. However, the space calibration between sensors is a difficult matter which relies on professional equipment such as manual turntable and consumes time. IMU is widely used in the early vision integrated navigation system, and many researches have investigated on calibration methods of spatial parameters based on cameras and IMU. Mirzaei [5] and Kelly [6] proposed a method, which is not limited by the calibration turntable. Under the framework of Kalman filter, the spatial relationship between inertial sensor and visual sensor is used as an unknown parameter to estimate the optimal solution in the process of data fusion. Jonathan [7] used the chessboard calibration board to achieve calibration, and Celyn [8] used hand-eye calibration method. However, the visual method is limited by the scene, and the perception degradation problem occurs in the underground tunnel, mine and other planar subsurface void scenes [9,10]. Compared to vision, LiDAR is more widely used. Referring to vision based calibration, paper [11][12][13] designed a hand-eye calibration based LiDAR and IMU calibration method. Limited by the scanning principle of LiDAR, its raw scanning has problems such as distortion, irregular data and deviation, which makes the calibration of LiDAR and IMU a challenge. To deal with the motion distortion caused by the rapid movement of the carrier, we propose an online calibration method based on LiDAR/POS, compensating motion distortion of LiDAR scans.
The advancement of sensor technology has expanded the application scenarios of LiDAR and attracted the interest of many researchers. Their calibration methods are mainly divided into three categories according to the difference of the carrier platform. UAV platform-based, vehicle-mounted platform-based, and adaptive methods suitable for multiple scenarios.
Early calibration methods focused on LiDAR scans on UAVs. LiDAR scans the strip area covered by the flight trajectory, and derives the coordinates of the ground points using direct geo-referencing information provided by the GPS/IMU unit and external parameters determined by the calibration procedure [14]. The calibration is completed based on the minimization of the difference value of different strip scanning feature points [15]; To solve the problem of excessive labor cost in the selection of conjugate points, based on photogrammetric block adjustment (BAIM), Habib proposed a method of feature extraction based on conjugate points and lines. This method uses the expansion of variance covariance matrix to enhance the linear feature points of data to achieve semiautomatic calibration method [16]; Jiao realized the calibration of two LiDAR by using line feature matching [17]; Hebel [18] and Lindenthal [19] proposed a calibration method based on planar features by using region growth method and random sample consistent segmentation method to extract planar features; Gautam used the feature method to calibrate the GNSS antenna, IMU, spectrometer and camera of the UAV platform [20]. However, it is difficult to match scans due to the area density data of LiDAR on the ground such as vehicles.
In recent years, with the development of intelligent vehicles, commercial MLS has been applied to a wider range of fields [20][21][22]. The ever-changing applications put forward updated requirements for the reliability of the system, more sensing data flow, high mobility, and wider field of view [23,24]. Qiu proposed heterogeneous sensor calibration for motion correlation analysis [25]. In order to reduce labor consumption and get rid of site constraints, to improve the flexibility of calibration, researchers have proposed a series of feature-based field calibration methods. Automatically extract features from the environment and use them for data association. Liu added artificial targets with special shapes to the scanning scene, constructed geometric constraints by using a called cone-column features and adding surface construction constraints to get the calibration parameters [26]. Guo obtained the calibration parameters of the LiDAR-IMU system by fitting the self-made target ball with LiDAR. Furthermore, a more robust calibration method based on multiple features is proposed [27]. This calibration method combines features of the ball, cylinder and plane in the scanning data, to avoiding the possible nonlinear problem in few features. Lv et al further used the information theory data filtering strategy to achieve a calibration method without infrastructure such as benchmark labels [28]. Srinara directly put the external parameter expression of each epoch into the geographical reference equation and proposed an INS/GNSS/LiDAR navigation system suitable for featureless environments [29]. The above methods will spend a lot of calculation on feature extraction, and the calibration parameters is unstable due to the correctness of feature matching and search.
A natural way for adaptive calibration in multiple scenarios is trajectory alignment. The different localization solving principles based on IMU and LiDAR allow the error models of trajectories to be independent and the residual optimization between trajectories to be able to obtain convergent calibration parameters. Li et al [30] used GP (Gaussian process) regression to model the independent timestamp delay of IMU, but this method relies on the correlation of scanned data and can only be effective in the pre-built map. Lv et al [31] propose a continuous time trajectory formula based on B-spline to solve the problem of asynchronous measurement in highspeed scenes. Kim introduced the manifold based pre integration method to generate the inertial propagation model of the sensor coordinate system in the rigid body [1]; Le gentil et al used IMU interpolation to compensate motion distortion [2]. In a structured environment, the IMU track uses Gaussian process regression to model independent sampling timestamps, then divides the point cloud into a structured plane and uses octree management to associate environmental objects for calibration [12]. On-site calibration refers to solving the spatial relationship of sensors as parameters in the process of state estimation, which requires rapid processing of large amounts of data from multi-beam LiDAR and IMU. Lv et al proposed a method based on batch processing by using the information theory data screening strategy [28]; Kim proposed a calibration method based on optimization under the nonlinear framework [1]; Keyetieu proposed a method for automatically selecting the most relevant LiDAR plane elements, which spreads the uncertainty of each point to data selection and correction, and increases the robustness and computational efficiency of the calibration method [32]. The sliding window method is an incremental data processing method. The sliding window method is an incremental method for processing large amounts of data. Jiao implemented a odometer based on sliding window [33]; Lee uses the pre-processing stage of the sliding window to aggregate multi frame point clouds to achieve the extraction and tracking of plane plates [34]; The lic-fusion2 proposed by Zou is to add a sliding window filter to the original work to achieve calibration [35]. There is also a data-driven approach. Usayiwevu et al used the information path planner based on maximizing information income to find the most accurate acceptable path in the unknown environment to achieve the calibration of spatial parameters [36]. However, the drift from accelerometer and gyroscope of low-cost IMU makes it difficult to match the trajectory, and the asynchronous timestamp of multi-mode sensor also leads to the complexity of solution. Positioning and scanning data contains more noise, and the original methods are no longer applicable. Li et al in [8] proposed a calibration method for low-cost airborne systems, estimated the boresight angles and carrier trajectories at the same time, built dynamic grids with observations, matched planar voxels with references, and optimized and estimated the parameters of the LiDAR-IMU system. Pothou proposed QA/QC (quality assurance/quality control) technology to evaluate the calibration results, and based on the least square principle, determine the validity, accuracy and precision of different statistical tests for outlier monitoring in positioning and attitude data [37]. However, the research is limited to the evaluation of simulation data sets.
The problems of the existing calibration methods are summarized as follows: 1. Maintaining a specific calibration site makes these methods not flexible enough 2. A large number of calculations in the process of feature data association are difficult to implement on low-cost equipment. 3. In feature extraction, the correctness of matching and searching results is low, and the calculation of calibration parameters is unstable.
To overcome these drawbacks, we propose a two-step optimization method. The time synchronization method based on GNSS hardware avoids the asynchronous problem of timestamp. This method does not require specific artificial facilities, and also avoids the computational burden and instability due to feature matching.

Materials and methods
Due to above problems in the calibration of LiDAR and POS on vehicle, a GNSS aided mapping method of LiDAR and IMU is adopt. Based on the integrated navigation method we design an online calibration method to estimate lever arm and rotation. First, GNSS-RTK and IMU data are loosely coupled based on Kalman filter to obtain the position and attitude of the carrier in the global coordinate system. Continuous frames point cloud data from LiDAR are matched by using occupancy based method. The GNSS positioning result is used as the initial position of inter frame matching. In order to reduce the cumulative error of matching, a graph-based optimization framework is adopted. A closed-loop constraints method based on bundle adjust are used in the back-end optimization to calculate continuous trajectory and corresponding reference maps in the global coordinate framework. The global coordinates and attitudes of vehicle are obtained through the synchronized GNSS/IMU. The result of multiplying the carrier attitudes, calibration parameters and LiDAR scans can correspond to the reference map point by point, forming a constraint condition. The sensor calibration is transformed into a nonlinear optimization problem. Combined multiple data frames, the optimal calibration parameters are iteratively solved. As subsequent experiments show, this method has high calculation accuracy to meet the mapping requirements of MLS system. The flow of this method is shown in figure 1.
This paper contains three coordinate systems: (1) LiDAR coordinate {l} takes the laser transmitter as the origin. We segment LiDAR scans based on sliding windows and take the coordinate of first frame in each segment as sub map coordinate {m} = {l 0 }. (2) IMU coordinate {b} takes the center of the sensor as the origin and each axial direction parallel to the accelerometer axial coordinate. In the calibration process of data, multi-frame data is unified under the world coordinate {w} based on the matching results. The purpose of this paper is to find out the rotation and translation parameters R b l between {l} and {b} respectively.
The pipeline above three main parts: GNSS position constrained LiDAR-SLAM, loosely coupled GNSS/IMU Integrated trajectory estimation and calibration parameter optimization of map matching. The detailed calibration procedure is implemented as follows.

LiDAR SLAM with GNSS position constraint
A LiDAR SLAM method of occupancy-based spatial raster map matching is used in this paper. It is fast and robust to noise because of structuring point cloud. As shown in the figure 2 below. The point cloud is described as a grid map and saved as a pyramid of quadtree according to the resolution. In order to store 3D spatial information, LiDAR scans are divided into grids during map generation, each element in the grid is set as a vector to save the occupancy of the corresponding elevation. When a new laser scan comes available, the occupancies of hit elements update according to the probability model. While the occupancy for nearby unhit grids is obtained according to the normal distribution curve of the distance to the nearest hit grids. An occupancy raster of one frame is generated and match witch a sub map for correlation scanning. Another new cloud data becomes available, which occupancy raster is generated and matched to the associated sub map by CSM (correlation scan matching). In CSM, the raster p m matching with sub maps p l for obtaining LiDAR pose Pose l,t is solved by maximizing the posterior probability distribution p(p l |p m , Pose l,t ). The probability distribution of each point location in p l is assumed independent, the above observation model is reduced to equation (1).
The final score is the sum of all raster probability values in the neighborhood. Information from IMU is taken as rough pose, and final pose is search corresponding max score in {w}.
The searching process is accelerated by the pyramid map structure, first matching on low resolution map. Then using the result as the initial value for the next level of resolution search matching. And searching for the best matching result until the best resolution of the leaf node is successfully matched, obtaining the coordinates of the newly added points within the sub-map under the local coordinate system as shown in equation (2).
The pose of LiDAR under the local coordinate system generated in the above process are Pose lidar,t . Continuous pose sequence constitutes the trajectory Trj lidar = [Pose lidar,0 , . . . , Pose lidar,t ].
A robust Euclidean space loop closure detection method and a BA (Bundle Adjustment) back-end optimization method are incorporated to reduce the cumulative error of LiDAR inter-frame matching. In loop closure detection method, LiDAR scans are compress as key frames. And current scan will be additionally compared with key frames to detect loop closure. Construct a cost function for the positional constraints between adjacent sub map and between loop closure frames. And the cost function is solved by nonlinear optimization of Gaussian Newton's method to correct accumulated errors.

Loosely coupled trajectory estimation of POS
In order to obtain the position of carrier in global coordinates, we take a combined navigation method based on GNSS-RTK/IMU. This is a popular solution for high precision trajectories in open scenes. The structure is shown in the following figure 3.
It combines GNSS-RTK and IMU data based on twofilter smoothing method to obtain motion state x including the carrier pose p w in the global coordinate system. As a popular sensor data fusion method, information of GNSS-RTK and IMU are sent to the extended Kalman filter (EKF) and backward Kalman filter (BKF) respectively, and their process noises are obtained independent. Weighted average of two error is used to correct the motion statex Sk .
In forward recursive Kalman filter, the process noises δx k and measurement noise δz k are linearized to correct the motion state.
The prediction and update of the forward recursive Kalman filter obtain optimal estimation of process noise through iterative processing.
To reduce inversion, the following definitions of variables are added as M B = P −1 B , δy B = P −1 B δx B . BKF represents the inverse dynamic process of motion noise from the current time t k to the previous time t k−1 .
According to the estimation of error state by EKF and BKF, the optimal error estimation is obtained by weighting and smoothing them to correct the motion state, and the error covariance is not greater than the covariance of individual EKF or BKF, as equations (3)- (5): The above plus and minus signs in subscript represent the prediction and update of noise.

Optimization method based on map point matching
The coordinate of LiDAR scan is transferred from {l} to {b}, and a map in global coordinate is derived from carrier pose in continuous time referenced in the second step, with the following equation (6): The time delay from LiDAR to POS is synchronized by GNSS-PPS. And the optimal calibration parameters R b l is estimated by nonlinear optimization method. The residual error construct as equation (7) with the coordinate difference of associated points between global map and LiDAR scan.
Gauss-Newton iteration method is used to minimize the deviation of the point coordinates between LiDAR and POS to estimate the optimal calibration parameters, as equation (8): These parameters in the above correspond to equations (9) and (10): According to the principle e T Pe = min of least squares, the following equations (11) and (12) can be listed:

Equipment and environment
As shown in figure 4, we verify the effectiveness of the above method by the self-developed MLS. It composed of the following modules: Bynav A1-H3 is a GNSS/IMU integrated navigation module, which adopts GNSS dual antenna mode and collects external real-time differential positioning service. VLP-16 supports synchronization of GNSS-PPS and provides LiDAR scan with 16 line resolution. Other parameters of the equipment are shown in the table 1. We achieve a non-linear optimization framework rely on Ceres Solver and the visualization of trajectory and point cloud map rely on the open source software Cloud-Compare.
We applied our proposed method for calibrating a MLS on a roof terrace, as shown in figure 5. The terrace is a rectangular platform; the width is 20 m and length is 40 m, which can be considered a complicated case, including many detailed structures, like the open ground, ceiling, flat wall and protruding pillars.

Experimental
The multi-source sensor based on software level time synchronization method may get the wrong local optimal value due to the lack of time benchmark. To guarantee the reliability of the test, the satellite timestamp is added to LiDAR and IMU by GNSS-PPS at the hardware level. It is a direct matching solution suppose by VLP-16. VLP-16 contains a time counter based on the internal crystal oscillator. When VLP-16 receives a valid data from Bynav A1-H3, the counter is adjusted on each PPS to keep the internal time consistent with UTC time, and the time stamp will be carried when the data packet is   broadcast. The time delay of the system dropped to 1 ms. The acquisition frequency of LiDAR and inertial measurement unit is different. We interpolate the IMU trajectory with high acquisition frequency to achieve point by point timestamp alignment. As shown in figure 6, the carrier made a highly maneuverable detour on the ground to fully converge the error of IMU. GNSS used RTK positioning mode to obtain centimeter-level precision global positioning results with frequency of 1 HZ in the whole process. LiDAR scanned at a frequency of 10 Hz. In order to ensure the accuracy of subsequent matching, point clouds with scanning distance of 0.5 m-50 m are reserved as feature points. We selected four segment scans from the continuous measurement data calibration. Each segment contains 100, 300, 500, and 600 frame scans, respectively. Calibration is described in chapter 3. First, calculate the reference map according to GNSS-LiDAR SLAM to obtain key points. For single frame LiDAR data, key points match the reference map to calculate calibration parameters; Calibration parameters are iteratively optimized frame by frame using selected LiDAR data. Based on the experimental data, the calibration method is analyzed from the visual mapping effect and accuracy.

Results and discussion
Limited by the scanning principle, LiDAR motion measurement is inevitably affected by motion distortion. In order to minimize the influence of such irrelevant factors on the  mapping results, we selected a trolley with a moving speed of less than 1 m s −1 as the carrier, and selected the following 400 frame scanning data with a trajectory close to a straight line from the scanning results to calculate the point cloud map. As shown in figure 7, the mapping effect before and after calibration has been significantly improved, which proves the effectiveness of this calibration method.
The state-of-the-art method LiDAR-align is not suitable for vehicle-mounted sensors because it requires highly nonplanar motions [38]. In order to evaluate the performance of the algorithm, we compared proposed method with LI-Calib based on absolute [31]. TLS (land laser scanning) with an accuracy of 2 mm is used to collect calibration field data, and use GPS-RTK static positioning to obtain the global coordinates of reference points in the scene. The projection parameters from the reference point cloud to the global coordinates are obtained through the reference point projection. By comparing the absolute errors of the corresponding feature points of TLS and MLS, the numerical accuracy of the proposed calibration method and LI-Calib are evaluated.
As shown in figure 8, the static ground data (a) were scanned by TLS and ground feature points, such as ground bumps, wall corners, edge lighting, are extracted. Through GNSS-RTK static observation, measure the global coordinates of the target ball center and uniform ground feature points as much as possible, so as to realize the splicing between maps. Select the uniformly distributed target ball center and feature points in the map, corresponding to their local coordinates and global coordinates. The transformation parameters of the coordinate system are calculated through seven homonymous points, and the global coordinates (b) of the static observation point cloud map are obtained. The average residual error of the point transformation is 3.44 * 10 ∧ (−4) m ∧ 2, which is higher than the accuracy of the scanner used in the mobile measurement system and serves as an absolute reference for the accuracy evaluation experiment.
In order to compare the calculation results of different solution data, we selected four representative data sets in the complete path as shown in figure 9. The calibration parameters in table 2 below are calculated respectively. Due to the first group data contains 100 frames scans and the track is short, the LI-Calib method cannot get a convergence calibration parameter and is excluded.   In order to intuitively compare the scanning accuracy, we superimpose the mapping results after spatial correction with the TLS data, and the results are shown in figure 10.
Because VLP-16 collects 300 000 points per second, it is far greater than the number of GNSS-RTK static positioning points. So we manually selected the corresponding key points  in the mobile scanning map and the reference map respectively, made a difference in the coordinates of the corresponding key points, and obtained the matching accuracy index RMS using the least square method, as shown in the following table 3. Based on above experiments, the accuracy of the method proposed in this paper is verified. Analysis of this method as follows. It can get rough calibration results in (a) of short trajectory, and (b) adds curve motion on this basis, the system state error is constrained, the error decreases, and the mapping accuracy is greatly improved. With the continuous increase of (c) data, the calibration accuracy is further improved until (d) data contains rapid rotation, and the accuracy starts to decline.
Based on the principle of the calibration method proposed in this paper, only two frames of data that can be correlated are required at least, but the matching results of two frames are unreliable due to the existence of MLS system internal error. On the one hand, the single scan point error of LiDAR has randomness. The LiDAR error conforms to the normal distribution, and the accumulated scan frame can effectively reduce the random error of scanning. On the other hand, the matching degree of each frame in the SLAM method is random. With the increase of matching data, data with low matching progress will inevitably be introduced, which will reduce the accuracy of the sub map used for matching and cause the decline in accuracy. In the future, the method of gross error detection will be introduced, and the method with low matching degree will be removed before the calibration parameters are calculated to further improve the algorithm.

Conclusion
For low-cost MLS system, this paper proposes an incremental online multi-sensor spatial parameter calibration method. This method improves the effect of POS system error on calibration results and overcomes the limitation of LiDAR feature missing. The basic principle of this method is: Constructing sub map by LiDAR SLAM to realize frames data combination.
Then calculating the precise motion trajectory of the sensor by GNSS/IMU integrated navigation method. While the calibration parameters is get by matching LiDAR frames data with the sub map, and is fine-tuned by optimizing as the data grows. The RMS of the map in experimental results is 0.57 cm, which is smaller than inherent noise of VLP-16. It shows that this method can significantly improve the performance of MLS and make it meet the application requirements of mobile mapping.

Data availability statement
The data cannot be made publicly available upon publication because they contain sensitive personal information. The data that support the findings of this study are available upon reasonable request from the authors.