Uncontrolled Two-Step Iterative Calibration Algorithm for Lidar–IMU System

Calibration of sensors is critical for the precise functioning of lidar–IMU systems. However, the accuracy of the system can be compromised if motion distortion is not considered. This study proposes a novel uncontrolled two-step iterative calibration algorithm that eliminates motion distortion and improves the accuracy of lidar–IMU systems. Initially, the algorithm corrects the distortion of rotational motion by matching the original inter-frame point cloud. Then, the point cloud is further matched with IMU after the prediction of attitude. The algorithm performs iterative motion distortion correction and rotation matrix calculation to obtain high-precision calibration results. In comparison with existing algorithms, the proposed algorithm boasts high accuracy, robustness, and efficiency. This high-precision calibration result can benefit a wide range of acquisition platforms, including handheld, unmanned ground vehicle (UGV), and backpack lidar–IMU systems.


Introduction
Accurate state estimation of a platform is a critical requirement for autonomous driving. While satellite navigation systems can provide accurate positioning information in the wilderness, their signal loss is commonplace in metropolitan areas, limiting their usefulness for autonomous driving applications. The Simultaneous Localization and Mapping (SLAM) method, which enables autonomous localization and mapping, has emerged as a research hotspot in the fields of autonomous driving and robot navigation [1][2][3][4]. Depending on the type of data used, SLAM can be classified into two main categories: visual SLAM and lidar SLAM.
Visual SLAM was the dominant focus of early research. Traditional methods were primarily implemented through filtering [5], and classical algorithms were utilized to accomplish localization and mapping through the Extended Kalman Filter (EKF) [4]. Subsequently, keyframe-based visual SLAM techniques matured, and mapping algorithms such as ORB-SLAM became popular [6]. While visual sensors can more accurately extract environmental features when operating at low speeds in texture-rich areas, their accuracy decreases with increasing motion speed. Thus, multi-sensor fusion SLAM techniques, particularly those that combine cameras and IMUs (VI-SLAM), have emerged. VI-SLAM is a low-cost combination of camera and IMU, which has gained a lot of attention. Experiments have demonstrated that VI-SLAM can operate effectively in complex scenes with violent motions [7]. Cameras can capture rich scene details, while IMUs can be used to predict the platform's attitude and position. The two sensors complement each other to yield better positioning accuracy and mapping results. The VI-ORB algorithm has been proposed based on the ORB-SLAM algorithm [8]. The VINS-Mono algorithm employs a tightly coupled, non-linear optimization-based approach for scale recovery through a monocular camera and IMU, demonstrating better robustness and generality [9]. The PL-VIO algorithm proposes a tightly coupled monocular visual inertial odometry system based on the use of point and line features [10], and the VINS-Fusion algorithm extends 2. Materials and Methods 2.1. Related Work 2.1.1. Calibration Principle for Lidar-IMU System As illustrated in Figure 1, two distinct methods exist to transform a point p imu k+1 in the IMU coordinate system at time k + 1 to the lidar coordinate system at time k. The first involves transforming the point to the lidar coordinate system at time k + 1 and then to the lidar coordinate system position at time k based on the relative position attitude of the lidar. The second involves first transforming the point to the IMU coordinate system position at time k and then to the lidar coordinate system position at time k based on the relative position attitude of the IMU. the point cloud again to obtain a higher accuracy calibration matrix. The calibr is updated iteratively until convergence. Our proposed algorithm is effectiv ing the rotation matrix for lidar-IMU systems, and can significantly improve of localization and mapping.

Calibration Principle for Lidar-IMU System
As illustrated in Figure 1, two distinct methods exist to transform a poin IMU coordinate system at time k+1 to the lidar coordinate system at time k. volves transforming the point to the lidar coordinate system at time k+1 and lidar coordinate system position at time k based on the relative position at lidar. The second involves first transforming the point to the IMU coordinat sition at time k and then to the lidar coordinate system position at time k b relative position attitude of the IMU.
The quaternion multiplication can be represented as the multiplication with quaternions [19], as shown in Equations (4)- (7), where Q + is the left m In Equation (1), the transformation of the point p imu k+1 is shown in two ways as mentioned earlier.
This equation involves the rotation matrices R lidar k,k+1 , R lidar,imu , and R imu k,k+1 . Simplified versions of these matrices are shown in Equations (2) and (3), where q b l is the quaternion of rotation from the lidar to the IMU coordinate system, is the result of IMU preintegration measured from moment b k to moment b k+1 , and q l k l k+1 represents the attitude change at moment l k+1 with respect to the lidar coordinate system at moment l k .
The quaternion multiplication can be represented as the multiplication of matrices with quaternions [19], as shown in Equations (4)-(7), where Q + 1 is the left multiplication matrix and Q − 2 is the right multiplication matrix.
is the left multiplication matrix of is the right multiplication matrix of the attitude change of moment b k+1 with respect to the lidar coordinate system of moment b k matrix. After merging, these equations can be transformed into Equation (8). By assuming that there are n sets of measurement data for attitude calibration, the system of chi-square linear equations shown in Equation (9) can be established and solved by the singular value decomposition (SVD) method to obtain the value of q b l [26].

Matching Algorithms for Point Clouds
In this study, various point cloud matching algorithms are investigated and compared for calculating the relative rotation and translation between two adjacent frames. The algorithms considered include the Iterative Closest Point (ICP) [27], Generalized ICP (GICP) [28], Normal Distributions Transform (NDT) [29], and OMP-NDT [30]. The ICP algorithm aims to minimize the distance between the two point clouds by finding the spatial transformation of the overlapping area, which is simple and does not require segmentation or feature extraction of point clouds. However, its computational cost for finding the nearest corresponding point is high, and it lacks the use of point cloud structure information. The GICP algorithm improves on the ICP algorithm by comprehensively considering point-to-point, point-to-surface, and surface-to-surface strategies, resulting in higher accuracy and robustness. The NDT algorithm divides the point cloud space into voxel grids and calculates the normal distribution of the point cloud in each grid, providing fast computation and less influence from moving targets in the scene. The OMP-NDT algorithm is an OpenMP-based parallel NDT algorithm derived from PCL that uses a multi-threaded approach for faster alignment speed, with a small deviation in matching results. The voxel nearest neighbor search in this study was performed using the DIRECT 7 algorithm. Figure 2 illustrates the proposed two-step calibration algorithm. In Step 1, the initial rotation matrix is calculated based on the matching of the original point cloud and the integration of IMU data. The pre-integrated attitude measurement values of the IMU from time b k to b k+1 are obtained using the angular velocity data. A system of homogeneous linear equations is established based on Equation (9), and the SVD method is applied to solve for the value of q b l . In Step 2, the initial calibration result is used to perform motion distortion correction on the original point cloud. Attitude prediction is made based on the IMU integral, and the corrected point cloud is matched and calibrated again to obtain a more accurate calibration result. The process is iterated until convergence is achieved. 2023, 23, x FOR PEER REVIEW 5 time b k to b k+1 are obtained using the angular velocity data. A system of homogen linear equations is established based on Equation (9), and the SVD method is appli solve for the value of q l b . In Step 2, the initial calibration result is used to perform m distortion correction on the original point cloud. Attitude prediction is made based o IMU integral, and the corrected point cloud is matched and calibrated again to obt more accurate calibration result. The process is iterated until convergence is achieve In the motion distortion correction process, each point in a scan period's point c data is transformed into a reference coordinate system c 0 , which has its origin typ located at the start of the scan period. The rotation and translation of each point's co nate system c k with respect to c 0 is determined, allowing the transformation of c k to c 0 . Since the 6-axis IMU only outputs angular velocity and linear acceleration correction of motion distortion caused by attitude is considered. The three attitude a corresponding to each coordinate system c k can be obtained from the integration o gular velocities measured by the IMU. Figure 3 illustrates this principle.  In the motion distortion correction process, each point in a scan period's point cloud data is transformed into a reference coordinate system c 0 , which has its origin typically located at the start of the scan period. The rotation and translation of each point's coordinate system c k with respect to c 0 is determined, allowing the transformation of p k from c k to c 0 . Since the 6-axis IMU only outputs angular velocity and linear acceleration, the correction of motion distortion caused by attitude is considered. The three attitude angles corresponding to each coordinate system c k can be obtained from the integration of angular velocities measured by the IMU. Figure 3 illustrates this principle. time b k to b k+1 are obtained using the angular velocity data. A system of homogeneous linear equations is established based on Equation (9), and the SVD method is applied to solve for the value of q l b . In Step 2, the initial calibration result is used to perform motion distortion correction on the original point cloud. Attitude prediction is made based on the IMU integral, and the corrected point cloud is matched and calibrated again to obtain a more accurate calibration result. The process is iterated until convergence is achieved. In the motion distortion correction process, each point in a scan period's point cloud data is transformed into a reference coordinate system c 0 , which has its origin typically located at the start of the scan period. The rotation and translation of each point's coordinate system c k with respect to c 0 is determined, allowing the transformation of from c k to c 0 . Since the 6-axis IMU only outputs angular velocity and linear acceleration, the correction of motion distortion caused by attitude is considered. The three attitude angles corresponding to each coordinate system c k can be obtained from the integration of angular velocities measured by the IMU. Figure 3 illustrates this principle. To correct motion distortion in the lidar-IMU system, we first evaluate the rotation matrices R ax , R ay , and R az around the x, y, and z axes, respectively, by using the angular velocities ω xi , ω yi , and ω zi measured by the 6-axis IMU at each point i. Equation (10) is used to calculate the rotation matrix for each axis, and then Equation (11) is used to To correct motion distortion in the lidar-IMU system, we first evaluate the rotation matrices R ax , R ay , and R az around the x, y, and z axes, respectively, by using the angular velocities ω xi , ω yi , and ω zi measured by the 6-axis IMU at each point i. Equation (10) is used to calculate the rotation matrix for each axis, and then Equation (11) is used to determine the transformation matrix R 0,k from c k to c 0 . The motion distortion is then solved using Equation (12). The correction process is related to the initial calibrated rotation matrix and can be further optimized through iterations. By utilizing the rotation matrix obtained from these equations, we can predict the transformation in the subsequent frame and improve the speed and accuracy of the matching process.

Experimental Data
In order to evaluate the proposed method, point cloud data were collected from five distinct locations and datasets, including Rotation, Park, Campus, Walk, and Cnu, using high-precision equipment. The Rotation, Park, Campus, and Walk data are publicly available datasets collected using a Velodyne VLP-16 lidar and a Microstrain 3DM-GX5-25 IMU at the Massachusetts Institute of Technology (MIT) [15]. Meanwhile, the Cnu dataset was obtained at Capital Normal University using a Velodyne VLP-16 lidar and an Inertial Labs INS-D IMU. The specific details of each dataset are outlined in Table 1. To obtain the position and attitude information of each lidar frame, we utilized the Simultaneous Localization and Mapping (SLAM) algorithm. As shown in Figure 4, the resulting stitched point cloud represents the collected data from each location. To compare the performance of the proposed method, we calculated the error by converting the estimated rotation matrix to Euler angles and comparing them to the ground truth values. The ground truth calibration values of Rotation, Park, Campus, and Walk datasets are reported as (0, 0, 0), as established in previous research [15]. On the other hand, for the Cnu dataset, the ground truth calibration values were established offline in the laboratory using control points and measured at (−179.90, −0.35, −88.54).

Comparative Analysis of Calibration Accuracy of Different Processing Methods
In this study, the accuracy of the calibration is evaluated by decomposing the rotation matrix between the calibrated and actual values into three Euler angles (roll angle, pitch angle, heading angle), and calculating the distance error (in degrees) between the three angles. The smaller the distance error, the higher the calibration accuracy. In Equation (13), ∆ r , ∆ p , and ∆ y represent the differences in roll, pitch, and heading angles, respectively. To assess the effectiveness of different calibration methods, we compared four approaches, three of which were processed following the steps outlined in Figure 2 with only slight variations between them. The first method (Method I) utilized the original point cloud without motion distortion correction, IMU prediction, or iterative processing, thus eliminating the frame-to-local map matching. The second (Method II) and third

Comparative Analysis of Calibration Accuracy of Different Processing Methods
In this study, the accuracy of the calibration is evaluated by decomposing the rotation matrix between the calibrated and actual values into three Euler angles (roll angle, pitch angle, heading angle), and calculating the distance error (in degrees) between the three angles. The smaller the distance error, the higher the calibration accuracy. In Equation (13), ∆ r , ∆ p , and ∆ y represent the differences in roll, pitch, and heading angles, respectively. To assess the effectiveness of different calibration methods, we compared four approaches, three of which were processed following the steps outlined in Figure 2 with only slight variations between them. The first method (Method I) utilized the original point cloud without motion distortion correction, IMU prediction, or iterative processing, thus eliminating the frame-to-local map matching. The second (Method II) and third (Method III) methods were novel techniques introduced in this study, with Method III differing from Method II only in the inclusion of IMU prediction. Finally, we compared our approach to LI-Calib proposed by ZJU [24] and LI-Init proposed by HKU [25]. By comparing the results of these four calibration methods, we aim to determine the most effective approach for lidar-IMU calibration.
The first three calibration methods utilize the OMP-NDT algorithm for point cloud frame matching, as illustrated in Figure 5. Method II proposed in this paper showed an overall improvement in accuracy after incorporating motion distortion correction and iterative processing. However, the introduction of IMU prediction did not consistently improve the accuracy and, in some cases, led to a decrease in accuracy. This variability in results may be attributed to the accuracy of the IMU measurements. The current study did not account for the drift phenomenon of IMU hardware over time during IMU integration, which may impact the accuracy of the predictions and overall matching results.  [24] and LI-Init proposed by HKU [25]. By comparing the results of these four calibration methods, we aim to determine the most effective approach for lidar-IMU calibration.
Rotation_error= ∆ r 2 +∆ p 2 +∆ y 2 2 (13) The first three calibration methods utilize the OMP-NDT algorithm for point cloud frame matching, as illustrated in Figure 5. Method II proposed in this paper showed an overall improvement in accuracy after incorporating motion distortion correction and iterative processing. However, the introduction of IMU prediction did not consistently improve the accuracy and, in some cases, led to a decrease in accuracy. This variability in results may be attributed to the accuracy of the IMU measurements. The current study did not account for the drift phenomenon of IMU hardware over time during IMU integration, which may impact the accuracy of the predictions and overall matching results. Although LI-Calib provides open-source software with a GUI, the format of the point cloud used by LI-Calib is different from our experimental data, and only the Park dataset can be processed at present. To compare the accuracy of LI-Calib with our proposed method, we conducted three tests in the same environment and evaluated the stability of the algorithms. The results presented in Table 2 show that our proposed Method II has an average error of 11.01°, which is stable and consistent across all three tests. On the other hand, the average error of LI-Calib is 36.63°, and it exhibits larger fluctuations. The intermediate results of LI-Calib open-source software are shown in Figure 6.  Although LI-Calib provides open-source software with a GUI, the format of the point cloud used by LI-Calib is different from our experimental data, and only the Park dataset can be processed at present. To compare the accuracy of LI-Calib with our proposed method, we conducted three tests in the same environment and evaluated the stability of the algorithms. The results presented in Table 2 show that our proposed Method II has an average error of 11.01 • , which is stable and consistent across all three tests.  In order to evaluate the performance of the proposed Method II, we conducted experiments on Rotation data and compared it with the LI-Init method. Our results demonstrate that Method II outperforms the LI-Init method in terms of calibration accuracy, with slightly better results and more efficient running time. These findings are consistent with our expectations, and support the potential of Method II to address calibration issues in a more Sensors 2023, 23, 3119 9 of 13 accurate and time-effective manner. A detailed presentation of the experimental results can be found in Table 3. Although LI-Calib provides open-source software with a GUI, the format of the point cloud used by LI-Calib is different from our experimental data, and only the Park dataset can be processed at present. To compare the accuracy of LI-Calib with our proposed method, we conducted three tests in the same environment and evaluated the stability of the algorithms. The results presented in Table 2 show that our proposed Method II has an average error of 11.01°, which is stable and consistent across all three tests. On the other hand, the average error of LI-Calib is 36.63°, and it exhibits larger fluctuations. The intermediate results of LI-Calib open-source software are shown in Figure 6.

Calibration Accuracy of Different Point Cloud Matching Algorithms
In this study, the impact of various point cloud matching algorithms on the calibration accuracy is investigated, as point cloud matching quality is a crucial factor in calibration. Four representative matching algorithms, including ICP, NDT, GICP, and OMP-NDT, are selected to evaluate their impact on calibration accuracy. As presented in Table 4, calibration results vary among the different matching algorithms. The effects of the matching algorithms on the calibration accuracy can be clearly seen in Figure 7, which demonstrates that the accuracy of calibration results varies for different data. Overall, the calibration accuracy is substantially improved by using GICP or NDT, while ICP exhibits inferior performance in comparison.

Analysis of the Influence of the Scenario on the Calibration Accuracy
In order to assess the performance of the calibration methods and matching algorithms, we analyzed the angular change between adjacent frames of the first 100 frames of point clouds for each scene. This was carried out by calculating the angular distance based on the matching results, and the results are visualized in Figure 8a. Furthermore, we evaluated the calibration errors of the five datasets, which were acquired using different calibration methods and matching algorithms. The results are shown in Table 5.

Analysis of the Influence of the Scenario on the Calibration Accuracy
In order to assess the performance of the calibration methods and matching algorithms, we analyzed the angular change between adjacent frames of the first 100 frames of point clouds for each scene. This was carried out by calculating the angular distance based on the matching results, and the results are visualized in Figure 8a. Furthermore, we evaluated the calibration errors of the five datasets, which were acquired using different calibration methods and matching algorithms. The results are shown in Table 5.
between neighboring point cloud frames can result in higher calibration accuracy. To achieve this, we recommend collecting calibration data while the equipment is uniformly rotating in the same location to minimize the impact of equipment movement on matching. Additionally, including wall structures in the collected scene can improve the effect of extracting stable point cloud features, which can lead to a regular point cloud distribution and improve point cloud matching.

Discussion
The findings reveal that the calibration accuracy is significantly affected by the data acquisition method and point cloud matching algorithm employed. The calibration results of the GICP and NDT algorithms are superior to those of the conventional ICP algorithm, indicating that advanced matching algorithms can enhance the calibration performance of the lidar-IMU system. Furthermore, the calibration outcomes of different scenarios show that the calibration results of Rotation data are the most stable and have higher accuracy, while the errors of the Park and Walk scenarios are comparatively larger. These results emphasize the importance of carefully selecting the data acquisition method and point cloud matching algorithm for achieving reliable calibration results.
In addition, this study recommends using handheld lidar, keeping the data acquisition in a single fixed location, and keeping the rotation speed constant and reasonable to  In this study, we evaluated the calibration errors of different scenes using a handheld lidar-IMU combination system for fixed position acquisition, as well as a combined system using a backpack. Our results indicate that the average calibration error for Rotation data was only 3.58 • , which is a relatively small overall error. However, the errors for Park and Walk data were higher, while Campus data had smaller errors, suggesting that calibration errors are influenced by the scene type. The average error for the combined lidar-IMU system using the backpack (Cnu data) was 9.09 • , which was higher than that of the handheld system. Figure 8b shows a scatter plot of the mean angular variation of the different data versus the mean calibration error. Our findings suggest that maintaining a steady point cloud matching throughout calibration and having a large rotational change between neighboring point cloud frames can result in higher calibration accuracy. To achieve this, we recommend collecting calibration data while the equipment is uniformly rotating in the same location to minimize the impact of equipment movement on matching. Additionally, including wall structures in the collected scene can improve the effect of extracting stable point cloud features, which can lead to a regular point cloud distribution and improve point cloud matching.

Discussion
The findings reveal that the calibration accuracy is significantly affected by the data acquisition method and point cloud matching algorithm employed. The calibration results of the GICP and NDT algorithms are superior to those of the conventional ICP algorithm, indicating that advanced matching algorithms can enhance the calibration performance of the lidar-IMU system. Furthermore, the calibration outcomes of different scenarios show that the calibration results of Rotation data are the most stable and have higher accuracy, while the errors of the Park and Walk scenarios are comparatively larger. These results emphasize the importance of carefully selecting the data acquisition method and point cloud matching algorithm for achieving reliable calibration results.
In addition, this study recommends using handheld lidar, keeping the data acquisition in a single fixed location, and keeping the rotation speed constant and reasonable to guarantee stable calibration results. Accurate calibration and successful matching can both be achieved by gathering regular wall information in the scenario.

Conclusions and Future Work
This study introduces an uncontrolled two-step iterative calibration algorithm to address the current calibration issues with the lidar-IMU system. The proposed algorithm directly utilizes the data acquired by the lidar-IMU system to obtain the initial rotation matrix through initial matching and then iteratively increases the matching accuracy by exploiting motion distortion and IMU pre-integration. The experimental comparison demonstrates that the method in this study can produce significantly better calibration results for vehicle-mounted, handheld, backpack, and other lidar acquisition modalities when compared to the conventional method. In comparison with existing algorithms, the proposed algorithm boasts high accuracy, robustness, and efficiency.
In our future work, we aim to further improve the calibration algorithm by increasing the number of experimental scenes, examining the impact of frame-to-submap matching on calibration accuracy, and accounting for IMU data inaccuracy. We also aim to enhance the open-source code and employ GPU to accelerate point cloud matching, thereby enabling the calibration program to operate more efficiently and conveniently. We also plan to investigate the challenge of achieving non-hardware time synchronization between the lidar and IMU sensors in our system. As each sensor has its own timestamp, this can be a complex task. We aim to leverage the results obtained through pose synchronization to develop an algorithm for time synchronization. By integrating the pose and time synchronization algorithms, we expect to improve the overall accuracy and robustness of our system.