LiDAR-IMU Time Delay Calibration Based on Iterative Closest Point and Iterated Sigma Point Kalman Filter

Abstract

The time delay calibration between Light Detection and Ranging (LiDAR) and Inertial Measurement Units (IMUs) is an essential prerequisite for its applications. However, the correspondences between LiDAR and IMU measurements are usually unknown, and thus cannot be computed directly for the time delay calibration. In order to solve the problem of LiDAR-IMU time delay calibration, this paper presents a fusion method based on iterative closest point (ICP) and iterated sigma point Kalman filter (ISPKF), which combines the advantages of ICP and ISPKF. The ICP algorithm can precisely determine the unknown transformation between LiDAR-IMU; and the ISPKF algorithm can optimally estimate the time delay calibration parameters. First of all, the coordinate transformation from the LiDAR frame to the IMU frame is realized. Second, the measurement model and time delay error model of LiDAR and IMU are established. Third, the methodology of the ICP and ISPKF procedure is presented for LiDAR-IMU time delay calibration. Experimental results are presented that validate the proposed method and demonstrate the time delay error can be accurately calibrated.

1. Introduction

In today’s world, Light Detection and Ranging (LiDAR) devices and Inertial Measurement Units (IMUs) often found on vehicles, airplanes and robots are increasingly being used to perform localization or for navigation tasks. The LiDAR and IMU sensors, together, can supply accurate attitude estimation and are very suitable in many applications. However, in GPS-denied environments the IMUs are usually unreliable with respect to position for long periods of time due to time drift, which may cause large cumulative errors, the a LiDAR is a device which uses laser beams to determine the distance and azimuth from the sensor to an object, which can provide 3D localization information with high accuracy and efficiency to reduce or bound IMUs drift. Therefore, the integrated LiDAR-IMU system can provide highly accurate position and pose information over long periods of time in GPS-denied environments.

Usually combining information from multiple sensors offers several potential advantages, including enhanced accuracy and improved robustness, but these come at a cost: a fundamental requirement in any multiple sensor system is time delay calibration. To ensure optimal performance, the LiDAR and IMU sensors must be properly time delay calibrated, including estimates of the relative timing of each sensor measurement, coordinate transformation between the different sensors, and time delay calibration parameters estimation are required [1,2,3,4].

Several methods exist for LiDAR, IMU and camera time delay calibration. The work of Kelly et al. [5] presented a time delay iterative closest point method for determining the time delay between inertial and visual sensor measurements. Aghili et al. [6] presented a robust 6-Degree of Freedom (DOF) relative navigation sysetm by combining the iterative closest point (ICP) registration algorithm and a noise Adaptive Kalman Filter (AKF) in a closed loop configuration together with measurements from a LiDAR and an IMU. Pothou et al. [7] investigated the determination of the misalignment between the IMU body frame and the LiDAR frame, using a Quality Assurance/Quality Control (QA/QC) technique to evaluate the LiDAR-IMU bore sight misalignment. Deymier et al. [8] proposed the self-calibration of a vehicle’s acquisition system with cameras, IMU and 3D LiDAR. Jian et al. [9] integrated two complementary technologies—Inertial Navigation System (INS) and LiDAR Simultaneous Localization and Mapping (SLAM)—into one navigation frame with a loosely coupled Extended Kalman Filter (EKF) to use the advantages and overcome the drawbacks of each system to establish a stable long-term navigation process. Mirzaei et al. [10] presented an EKF for precisely determining the unknown transformation between a camera and an IMU. Yun et al. [11] developed the IMU/Vision/LiDAR integrated navigation system which can provide accurate relative navigation information in GNSS-denied environments; and construct an overall integrated navigation filter based on the EKF approach. Li et al. proposed [12] a method of integrating the measurements from a LIDAR and a Micro-Electro-Mechanical System (MEMS) IMU, and using the Kalman Filter (KF) to estimate the error of IMU and LIDAR sensors.

As mentioned above, the AKF, EKF, KF and ICP algorithms are used in LiDAR, IMU and camera sensors to calibrate the time delay. During the course of LiDAR-IMU calibration, neither the EKF algorithm nor AKF algorithm can avoid time delay calibration bias and filtering divergences. Initially, the correspondences between LiDAR and IMU measurements are usually unknown; as a result the relative time delay information between the LiDAR-IMU data flows cannot be computed directly [13,14,15,16]. The question is posed differently as follows: first, the measurement rate information of LiDAR and IMU cannot be directly compared. Second, in general, the rigid body transformation between the LiDAR and the IMU is unknown. Third, both signals are discrete and have different temporal resolutions, the LiDAR typically scanning information is available at 5–20 Hz or less, however the IMU produces data is normally available at 100 Hz or more [17,18,19,20].

In order to solve the problem of LiDAR-IMU time delay calibration, we present a fusion method based on ICP and an iterated sigma point Kalman filter (ISPKF), which combines the advantages of ICP and ISPKF [21,22,23]. The total least squares cost function is used by the ICP algorithm for registration, which allows us to merge the IMU orientation measurement uncertainty in a principled way, and to reduce the longer time intervals due to the accumulated noise effects by integrated LiDAR orientation measurement. The ISPKF algorithm measurement update step is iterated until the change in the posterior state estimate drops below some small threshold, and can optimally estimate the time delay calibration parameters. First of all, the coordinate transformation from the LiDAR frame to the IMU frame is realized. Second, the measurement model and time delay error model of LiDAR and IMU sensors are established. Third, the methodology of the ICP and ISPKF procedure is presented for LiDAR-IMU time delay calibration. Experimental results are presented that validate the proposed method and demonstrate the time delay can be accurately calibrated.

This paper is organized as follows: the coordinate transformation between LiDAR and IMU are described in Section 2. In Section 3, we establish the LiDAR and IMU measurement models and the time delay error model in LiDAR-IMU. Section 4 given the implementation procedure of ICP-ISPKF for LiDAR-IMU time delay calibration. Section 5 experimental studies and analysis the results using LiDAR, IMU and optical measuring system based our proposed method are described, followed by the conclusions in Section 6.

2. Coordinate Transformation between LiDAR and IMU

2.1. Coordinate Frame

In general, there are basically four coordinate frames in the LiDAR and IMU, The relationship between these frames is shown in Figure 1:

(1)

LiDAR frame, {L}, is represented in this frame of reference, in which the axes are defined as right, forward and up.

(2)

IMU frame, {I}, is defined by the IMU, in which angular rotation rates and linear accelerations are measured, with its origin at a point on the IMU body.

(3)

Object frame, {O}, is the coordinate of moving object, the axes in the object frame are forward, right and down.

(4)

World frame, {W}, is considered to be the fundamental coordinate frame and serves as an absolute reference for both the {I} and the {L}.

2.2. Transformation from LiDAR Frame to IMU Frame

2.2.1. Transformation from LiDAR Frame to the World Frame

Suppose a point ^WP in the {W} frame is located at ^LP in the {L}, the transformation from the {W} coordinate to the {L} coordinate system can be expressed as [24]:

$${}^{L}P=R_W^L{}^{W}P+{}^L{T}_{LW}$$

(1)

where $R_W^L$ is a 3 × 3 orthonormal matrix representing the rotation from the {W} frame to the {L} frame, and ${}^L{T}_{LW}$ is the translation vector. The subscript is the origin of the {L} coordinate, and is the origin of the tangent frame. Our goal is to calculate $R_W^L$ and ${}^L{T}_{LW}$.

The transformation is implemented through the observation of LiDAR scanning to a calibration plane. A geometric constraint can be obtained between LiDAR scanning points and the calibration plane. Since LiDAR scanning points lie on the calibration plane, and ^Wr is the normal vector to the plane, we have:

$${}^{W}r\cdot {}^{W}P=d$$

(2)

where ^WP is the coordinate of LiDAR scanning point in {W} frame; d is a scalar representing the distance from the origin of the {W} frame to the calibration plane, which is calculated from the position and orientation of the calibration plane. From Equation (1) we know that:

$${}^{W}P={\left(R_W^L\right)}^{-1}\left({}^{L}P-{}^L{T}_{LW}\right)$$

(3)

By substituting Equation (3) into Equation (2), we have:

$${}^{W}r\cdot {\left(R_W^L\right)}^{-1}\left({}^{L}P-{}^L{T}_{LW}\right)=d$$

(4)

For any given LiDAR scanning point and calibration plane position, Equation (4) gives a constraint on $R_W^L$ and ${}^L{T}_{LW}$. It will be solved in two consecutive steps: a linear solution, followed by a non-linear optimization [25,26]:

(1) Linear solution. The LiDAR scanning plane in the {L} is ${}^{L}Z=0$. Each LiDAR scanning point can be represented as ${}^{L}P={[{}^L{P}_x{}^L{P}_y{}^L{P}_z]}^T$. Then Equation (4) is rewritten as:

$${}^{W}r\cdot {\left(R_W^L\right)}^{-1}\left(\left[\begin{array}{c}{}^L{P}_x\\ {}^L{P}_y\\ {}^L{P}_z\end{array}\right]-{}^L{T}_{LW}\right)={}^{W}r\cdot {\left(R_W^L\right)}^{-1}\left(\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]-{}^L{T}_{LW}\right)\left[\begin{array}{c}{}^L{P}_x\\ {}^L{P}_y\\ {}^L{P}_z\end{array}\right]=d$$

(5)

Let’s define $Z={\left(R_W^L\right)}^{-1}\left(\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]-{}^L{T}_{LW}\right)$, Equation (5) is rewritten as:

$${}^{W}r\cdot Z\cdot {}^{L}P=d$$

(6)

where Z is the parameter to be solved, which is the integration of two unknown parameters $R_W^L$ and ${}^L{T}_{LW}$.

Equation (6) can be solved using the standard linear least squares algorithm. In order to obtain a better result, multiple calibration planes should be used in the transformation. Suppose in the transformation we use a total of N calibration planes, with M_i (i = 1,…,N) LiDAR scanning points on the i-th plane. Let $Z=\left[\begin{array}{ccc}{z}_{11}& z_{12}& z_{13}\\ z_{21}& z_{22}& z_{23}\\ z_{31}& z_{32}& z_{33}\end{array}\right]$, the normal vector for the i-th plane ${}^W{r}_i=\left[r_{i,1}{r}_{i,2}{r}_{i,3}\right]$, the distance from origin of the {W} frame and the i-th calibration plane is d_i and the j-th LiDAR scanning point on the i-th calibration plane is ${}^L{P}_{ij}={[{}^L{P}_{ij,x}{}^L{P}_{ij,y}{}^L{P}_{ij,z}]}^T$, Equation (6) is rewritten as:

$$(r_{i,1}{z}_{11}+r_{i,2}{z}_{21}+r_{i,3}{z}_{31})\cdot {}^L{P}_{ij,x}+(r_{i,1}{z}_{12}+r_{i,2}{z}_{22}+r_{i,3}{z}_{32})\cdot {}^L{P}_{ij,y}+(r_{i,1}{z}_{13}+r_{i,2}{z}_{23}+r_{i,3}{z}_{33})\cdot {}^L{P}_{ij,z}=d_i$$

(7)

where i = 1,2,…,N, j = 1,2,…,M.

Therefore for each LiDAR scanning point we have a linear equation which is a row in Equation (7) can be calculated using the standard linear least square algorithm. Then $R_W^L$ and ${}^L{T}_{LW}$ will be obtained from Z.

(2) Nonlinear solution. The linear solution is obtained by the least squares method, which aims to minimize an algebraic distance. A nonlinear minimization method is used to minimize the differences between the measured Euclidean distances as well as the calculated distance from the LiDAR scanning points to the calibration plane, which is physically meaningful.

Equation (4) gives two types of distances: d is the distance from the calibration plane to the origin of the {W} frame obtained by a field survey, and ${}^{W}r·{\left(R_W^L\right)}^{-1}({}^{L}P-{}^L{T}_{LW})$ is the calculated distance. The difference between these two distances is defined as the distance error for one calibration plane pose. The nonlinear solution aims to minimize the sum function of the distance errors for all the plane positions. The sum function is defined as:

$$sum={\displaystyle {\sum }_{i=1}^N{{\displaystyle {\sum }_{j=1}^{M_i}\left({}^W{r}_i\cdot {\left(R_W^L\right)}^{-1}\left({}^L{P}_{ij}-{}^L{T}_{LW}\right)-d_i\right)}}^2}$$

(8)

where ri defines the i-th calibration plane, d_i is the distance from the i-th plane to the center of the {W} frame, and P_ij is the j-th LiDAR scanning point with the i-th calibration plane; the pair of $R_W^L$ and ${}^L{T}_{LW}$ that minimize Equation (8) are considered to be the rotation and translation matrix to be calculated. Equation (8) can be minimized as a nonlinear optimization problem by getting the translation and rotation matrix between the {W} frame and LiDAR scanning points in the {W} frame can be converted to a point in the {L} frame.

2.2.2. Transformation from LiDAR Frame to IMU Frame

The transformation of the LiDAR frame to the IMU one is to obtain the geometric relationship between the {L} frame and {O} frame. The IMU consists of three gyros and three accelerometers. The gyros provide change of Euler angles, while the accelerometers give the specific force. By integrating the output of the gyros and the accelerometers, we can obtain the translation and rotation matrix between the {W} frame and the {O} frame. Let the rotation matrix be $R_O^W$, and translation vector be ${}^O{T}_{WO}$, then a point in the {O} frame can be converted to a point in the {W} frame by:

$${}^{W}P=R_O^W{}^{O}P+{}^O{T}_{WO}$$

(9)

where ${}^{W}P$ is the point in the {W} frame, and ${}^{O}P$ is the point in the {O} frame.

Finally, by substituting Equation (1) into Equation (9) we have:

$${}^{L}P=R_W^L\left(R_O^W{}^{O}P+{}^O{T}_{WO}\right)+{}^L{T}_{LW}$$

(10)

Equation (10) is the transformation from the {O} frame to the {L} frame.

3. LiDAR and IMU Measurement Model

3.1. LiDAR Measurement Model

The LiDAR scans the laser beam through 360°. Therefore, a post which will be modeled as a vertical line in the tangent frame will appear as a point in the LiDAR frame. Similarly, a wall which will be modeled as a vertical plane in the tangent frame will appear as a line in the LiDAR frame of references. The LiDAR position and point feature detection are shown in Figure 2.

Consider a mapped vertical post at P₀. The post will appear as a point in the LiDAR return. The vector from T to P₀ in world frame is ${}^W{T}_{L{P}_O}={\left[N_0{E}_{0}0\right]}^T$, which is known from the survey. The line is denoted as:

$${}^W{T}_{WP}(m)={}^W{T}_{W{P}_O}+m{e}_3$$

(11)

where ${}^W{T}_{WP}(m)$ is the vector in world frame from T to a point P at height m on the post, m is a scalar with $m\in \left(0,+\infty \right)$. The vector e₃ is parallel to the post. In our application, all the posts and planes are modeled as vertical. For a vertical post $e_3={\left[001\right]}^T$ in the world frame.

For any feature point P, the vector from the LiDAR to the point is:

$$T_{LP}=T_{WP}-(T_{WO}+T_{OL})$$

(12)

As is shown in Figure 3. Equation (12) is valid for all the reference frames.

In Equation (12) and Figure 3, T_WP is a constant in the world frame known from the feature survey. T_WO will be calculated in the world frame. T_OL in the object frame is known, and can be determined by the pre-calibration.

$R_O^L$ is the rotation matrix from the object frame to the LiDAR frame, so the vector from the LiDAR to a feature point in LiDAR coordinates is denoted as:

$${}^L{T}_{LP}=R_O^L\left(R_W^O({}^W{T}_{WP}-{}^W{T}_{WO})-{}^O{T}_{OL}\right)$$

(13)

By substituting Equation (11) into Equation (13) we have:

$${}^L{T}_{LP}(m)=R_O^L\left(R_W^O({}^W{T}_{WP}+m{e}_3-{}^W{T}_{WO})-{}^O{T}_{OL}\right)$$

(14)

which is the vector in the LiDAR coordinates from the origin of the LiDAR sensor to a point with height m on the post. By expanding Equation (14), the coordinate of ${}^L{T}_{LP}\left(m\right)$ in the $-{}^{L}Z$ direction is:

$$Z(m)=e_3^W\cdot {}^L{T}_{LP}(m)=m\cdot e_3^W\cdot R_O^L\cdot R_W^O\cdot e_3+e_3^W\cdot R_O^L\left(R_W^O({}^W{T}_{WP}-{}^W{T}_{WO})-{}^O{T}_{OL}\right)$$

(15)

Equation (15) is the theoretical equation. Note that for a single-planar LiDAR, the scan plane in LiDAR coordinate is ${}^{L}Z=0$. Therefore, for all the LiDAR points we have ${}^{L}Z=0$, and we can use this fact to calculate m as:

$$m=-\frac{e_3^W\cdot R_O^L\left(R_W^O({}^W{T}_{WP}-{}^W{T}_{WO})-{}^O{T}_{OL}\right)}{e_3^W\cdot R_O^L\cdot R_W^O\cdot e_3}$$

(16)

By substituting Equation (16) into Equation (14), the detected point is:

$${}^L{T}_{LP}(m)=R_O^L\left(R_W^O({}^W{T}_{WP}-\frac{e_3^W\cdot R_O^L\left(R_W^O({}^W{T}_{WP}-{}^W{T}_{WO})-{}^O{T}_{OL}\right)}{e_3^W\cdot R_O^L\cdot R_W^O\cdot e_3}{e}_3-{}^W{T}_{WO})-{}^O{T}_{OL}\right)$$

(17)

Equation (17) is the LiDAR measurement model.

3.2. IMU Measurement Model

The IMU consists of three gyros and three accelerometers. The gyro provides change of Euler angles, while the accelerometers give the specific force. This model is based on the inertial measurement system error modeling method presented by Jonathan Kelly [5,27] and integrates the modified Rodrigues parameters kinematic equation. We obtained the IMU measuring equations as follows:

$${\dot{P}}_I(t)=F(t)P_I(t)+P_I(t)F^T(t)+G(t)Q_g(t)G^T(t)$$

(18)

$$F(t)=\frac{\partial \dot{\rho }(t)}{\partial v}=\frac{1}{2}\left(({\rho }^T(t)\cdot \omega (t))I_3-{\left[\omega (t)\right]}_{\times }+\rho (t)\omega {(t)}^T-\omega (t)\cdot {\rho }^T(t)\right)$$

(19)

$$G(t)=\frac{\partial \dot{\rho }(t)}{\partial n_g}=-\frac{1}{4}\left((1-{\Vert \rho (t)\Vert }^2)I_3+2{\left[\rho (t)\right]}_{\times }+2\rho (t){\rho }^T(t)\right)$$

(20)

$$\dot{\rho }(t)=\frac{1}{4}\left((1-{\Vert \rho (t)\Vert }^2)I_3+2{\left[\rho (t)\right]}_{\times }+2\rho (t)\right){\omega }^I(t)$$

(21)

where $P_I(t)$ is the IMU orientation covariance at a time $t$, $Q_g(t)$ is an additive noise vector, $\rho (t)$ is the modified Rodrigues parameters vector, $F(t)$ and $G(t)$ are the system matrix for $\rho (t)$, ${\left[\rho (t)\right]}_{\times }$ is the skew-symmetric cross-product matrix for $\rho (t)$, $I_3$ is the 3 × 3 identity matrix, ${\omega }^I(t)={\omega }_m(t)-b_g-n_g(t)$ is the true angular velocity of IMU, and $b_g$ and $n_g(t)$ are the gyroscope bias vector and the noise vector, respectively.

3.3. Time Delay Error Model

We will identify a specific instantaneous local LiDAR frame as {L_k} with timestamp $t_{L_k}$ according the receiver clock, for k = 1,…,n LiDAR poses. Similarly, for j = 1,…,m IMU poses, the {I_j} will be identified as a specific instantaneous local IMU frame with timestamp $t_{I_j}$. In general, the IMU data are available at a substantially higher rate than the LiDAR data, and m > n.

To compare LiDAR and IMU measurement data, a common representation is required; we use the orientation measurements to match IMU and LiDAR. The IMU orientation is measured with respect to the initial sensor pose. The LiDAR orientation is measured relative to a series of calibration planes. Our algorithm employs the modified Rodrigues parameters as a minimal and convenient orientation representation. The use of the modified Rodrigues parameters vector representation allows us to propagate the IMU orientation uncertainty forward continuously in time [28].

We have the following relationship at any time $t_{I_j}$:

$${}_I^W\rho (t)={}_L^W\rho (t+\tau )\cdot {}_I^L\rho$$

(22)

where ${}_I^L\rho$ is the modified Rodrigues parameter vector with the orientation from the {I} frame to the {L} frame. ${}_L^W\rho \left(t+\tau \right)$ is the modified Rodrigues parameter vector with the orientation from the {L} frame to the {W}, $\tau$ is the time delay. ${}_L^W\rho \left(t\right)$ is the modified Rodrigues parameter vector with the orientation from the {I} frame to the {W} frame.

Although the orientation of the rate measurement information supplied by the IMU cannot be measured directly, by integrating the IMU gyroscope data, we can obtain the IMU measurement orientation change over a period of time. Similarly, we can conveniently compute the LiDAR measured orientation relative to the {W} frame. The measurement relationship between the {I}, {L} and {W} frame can be computed as:

$${}_I^{I_0}\rho (t)={}_W^{I_0}\rho \cdot {}_L^W\rho (t+\tau )\cdot {}_I^L\rho$$

(23)

where ${}_W^{I_0}\rho$ is the modified Rodrigues parameter vector with the orientation from the {I₀} frame to the {W} frame.

(1) Position error analysis: the error in position can be calculated as follows:

$$\delta \left({}^W\dot{T}{}_{WO}\right)=R_O^W\cdot {}^{O}V-{\widehat{R}}_O^W\cdot {}^O\widehat{V}={\widehat{R}}_O^W\cdot \delta \left({}^{O}V\right)-{}^W\widehat{V}\cdot \rho$$

(24)

where $R_O^W={\widehat{R}}_O^W\left(I+\rho \right),I={\left(R_W^L\right)}^T·R_W^L,\rho$ is the calibration plane tilt error, and ${}^{O}V={}^O\widehat{V}+\delta \left({}^{O}V\right)$.

(2) Velocity error analysis: the error in velocity is modeled as follows:

$$\begin{array}{ll}\delta \left({}^O\dot{V}\right)\hfill & ={}^O\dot{V}-{}^O\dot{\widehat{V}}={}^O{a}_O-\left({\Omega }_{IL}^O+{\Omega }_{IO}^O\right)\cdot {}^{O}V-{}^O{\widehat{a}}_O+\left({\widehat{\Omega }}_{IL}^O+{\widehat{\Omega }}_{IO}^O\right)\cdot {}^O\widehat{V}\hfill \\ & ={\widehat{R}}_O^W\left(\left(\frac{\partial \left(g^W\right)}{\partial \left({}^W{T}_{WO}\right)}+{}^W\widehat{V}\cdot \frac{\partial \left({}^W{\omega }_{IL}\right)}{\partial \left({}^W{T}_{WO}\right)}\right)\delta \left({}^W{T}_{WO}\right)+\left(g^W+{}^W{\omega }_{IL}{\left({}^W\widehat{V}\right)}^T-{\left({}^W{\omega }_{IL}\right)}^T\cdot {}^W\widehat{V}\cdot I\right)\rho \right)\hfill \\ & -\left({\widehat{\Omega }}_{IL}^O+{\widehat{\Omega }}_{IO}^O\right)\delta \left({}^{O}V\right)-\delta b_a-{}^O\widehat{V}\cdot \delta b_g-{\gamma }_a-{}^O\widehat{V}\cdot {\gamma }_g\hfill \end{array}$$

(25)

where g^W is the local gravity vector at the moving object location represented in the {W} frame, ${}^W{\omega }_{IL}$ is the angular rate of the {W} frame origin to the {I} frame represented in the {L} frame. ${\gamma }_g$ is the white Gaussian measurement noise, b_g is the gyro bias, which is modeled as a random constant plus random noise.

(3) Attitude error analysis: the attitude error is given by:

$$\delta \left(\dot{\Theta }\right)=\dot{\Theta }-\dot{\widehat{\Theta }}={\Omega }_E^{-1}\cdot {}^W{\widehat{\omega }}_{IW}-\delta \left({}^W{\omega }_{IW}\right)-{\widehat{R}}_I^W\left(\delta b_g+{\gamma }_g\right)$$

(26)

where $\delta ({}^W{\omega }_{IW})$ can be calculated as:

$$\delta \left({}^W{\omega }_{IW}\right)=-{\omega }_{IL}\cdot \left[\begin{array}{c}\sin \alpha \\ 0\\ \cos \alpha \end{array}\right]\cdot \frac{\partial \Theta }{\partial \left({}^W{R}_{IW}\right)}\cdot \delta \left({}^W{R}_{IW}\right)$$

(27)

(4) Time delay calibration parameters error model: the measurement noise vector, the time delay parameter vector and process noise vector of LiDAR-IMU can be expressed as:

$$\{\begin{array}{l}x={\left[\begin{array}{ccccc}\delta \left({}^W{T}_{WO}\right)& \delta \left({}^{O}V\right)& \delta \Theta & \delta {\omega }_{ba}& \delta {\omega }_{bg}\end{array}\right]}^T\\ b={\left[\begin{array}{cc}{b}_a& b_g\end{array}\right]}^T\\ \gamma ={\left[\begin{array}{cc}{\gamma }_a& {\gamma }_g\end{array}\right]}^T\end{array}$$

(28)

where the x, b and γ are the time delay of the error state vector, process noise vector and measurement noise vector, respectively.

According to the Equations (25)–(28), the time delay calibration parameters error model of LiDAR-IMU can be computed as:

$$\left[\begin{array}{c}\delta \left({}^W{\dot{T}}_{WO}\right)\\ \delta \left({}^O\dot{V}\right)\\ \delta \dot{\Theta }\\ \delta {\dot{\omega }}_{ba}\\ \delta {\omega }_{bg}\end{array}\right]=\left[\begin{array}{ccccc}{\widehat{R}}_O^W\frac{\partial \left(g^W\right)}{\partial \left({}^W{T}_{WO}\right)}& {\widehat{R}}_O^W& -{}^W\widehat{V}& 0& 0\\ 0& -\left({\widehat{\Omega }}_{IL}^O+{\widehat{\Omega }}_{IO}^O\right)& -{\widehat{R}}_O^W{g}^W& -I& -{}^O\widehat{V}\\ 0& 0& {\widehat{\Omega }}_E^{-1}& 0& -{\widehat{R}}_O^W\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\end{array}\right]\cdot \left[\begin{array}{c}\delta \left({}^W{T}_{WO}\right)\\ \delta \left({}^{O}V\right)\\ \delta \Theta \\ \delta {\omega }_{ba}\\ \delta {\omega }_{bg}\end{array}\right]+\left[\begin{array}{cccc}-I& 0& 0& 0\\ 0& -{}^O\widehat{V}& 0& 0\\ 0& -{\widehat{R}}_O^W& 0& 0\\ 0& 0& I& 0\\ 0& 0& 0& I\end{array}\right]\cdot \left[\begin{array}{c}{\gamma }_a\\ {\gamma }_g\\ b_a\\ b_g\end{array}\right]$$

(29)

4. Time Delay Calibration Using the ICP-ISPKF Integration Method

The ICP algorithm can be used to register the spatial data from IMU and LiDAR measurements, which operates by aligning two curves in a 3-D orientation space generated from integrated IMU gyroscope data and from LiDAR scanner data. Each point on the respective curve has a corresponding timestamp, identifying the time at which the measurement arrived at the receiver. By registering the orientation curves, we are able to use the timestamp values to estimate the relative delay between IMU and LiDAR data streams. ICP incorporates in a principled way the uncertainty in the LiDAR orientation measurements and accounts for the fact that the integrated IMU orientation becomes less accurate over longer intervals due to the incorporation of noise. We specifically avoid this using the ISPKF because of The ISPKF algorithm measurement update step is iterated until the change in the posterior state estimate drops below some small threshold, and can optimally estimate the time delay calibration parameters. The ISPKF is shown to achieve superior performance in terms of efficiency and accuracy compared with the KF, EKF and UKF, also the Gabe Sibley et al. [29] have compared the ISPKF with KF, EKF, UKF and Gauss-Newton filter, and demonstrate the ISPKF’s capabilities in avoiding IMU measuring noise. Therefore, in order to solve the problem of LiDAR-IMU time delay calibration, we use the fusion method based on ICP and ISPKF algorithm.

As shown in Figure 4, the LiDAR-IMU time delay calibration is integrated with ICP and ISPKF method. Firstly, once the ICP succeeds matching the LiDAR measurement points, the LiDAR-IMU state estimations are updated with the precise registration value. Secondly, at the time delay filter estimate step, the obtained predicted pose is required to find the corresponding points between LiDAR and IMU measurements. Thirdly, at each time step, the recursive algorithm is used to update the related covariance estimates and precise registration value for LiDAR-IMU. The ICP convergence information and the fault-detection logic are used to adaptively adjust ISPKF reliable estimate of motion state and a set of related parameters. In closed-loop ICP-ISPKF architecture, through ICP initial guess and fault detection, the LiDAR-IMU robust pose tracking and automatic fault recovery are established. Finally, the pose prediction information used to align the time delay error for ICP [6,30].

4.1. The ICP Algorithm for Estimation the Time Delay and Relative Orientation of LiDAR-IMU

The ICP algorithm is utilized to estimation the LiDAR-IMU time delay and relative orientation. At the beginning, the transforms between the LiDAR-IMU orientation curves are computed through iteratively selecting $n$ correspondences point using the ICP algorithm. We employed the TD-ICP algorithm registration rules proposed by Jonathan Kelly [5,31] and by adjusting the search corresponding time scale and the orientation curves converge, this ICP algorithm can be described in two steps:

Step I: Registration Rules.

The ICP algorithm operates by iteratively selecting the closest point between the IMU orientation curve and the LiDAR measurement point, and the concept of ICP proximity requires a suitable distance measurement. The minimum value of distance function can be computed as

$$d_{ij}={(}_{L_{\mathrm{i}}}^W\rho ,{}_{I_j}^{I_0}\rho )=4\arctan \sqrt{{\sigma }_{ij}^T{\sigma }_{ij}}$$

(30)

$${\sigma }_{ij}=-{(}_W^{I_0}\widehat{\rho }{\cdot }_{L_i}^W\rho {\cdot }_I^L\widehat{\rho }){\cdot }_{I_j}^{I_0}\rho$$

(31)

where $d_{ij}$ is the incremental rotation arc length taking from ${}_{L_i}^W\rho$ to ${}_{I_j}^{I_0}\rho$ in radians orientation, and ${\sigma }_{ij}$ is the spatial transform model on the unit sphere.

Step II: Nonlinear Iterative Registration Rules.

We can get$n$correspondence relationship between IMU and LiDAR orientation measurement curves by selecting the closest IMU point for LiDAR point. The cost function used to align the IMU and LiDAR orientation curves can be calculated as

$$U\left({}_W{}^{I_0}\rho ,{}_I{}^L\rho \right)={\displaystyle \sum _{k=1}^n{s}_k{P}_{L_k}^{-1}{s}_k^T}+{\displaystyle \sum _{k=1}^n{t}_k{P}_{I_{f(k)}}^{-1}{t}_k^T}$$

(32)

$$s_k={}_{L_k}{}^W\rho -{}_{L_k}{}^W\widehat{\rho }$$

(33)

$$t_k={}_{I_{f(k)}}{}^{I_0}\rho -{}_{I_{f(k)}}{}^{I_0}\widehat{\rho }$$

(34)

where $P_{L_k}$ and $P_{I_{f(k)}}$ are the associated covariance matrices, $s$ and $t$ are the stacked residuals vectors, and the ${}_W{}^{I_0}\rho$ and ${}_I{}^L\rho$ are transform parameters.

By using Lagrange multipliers and incorporating the constraints ${}_{I_{f(k)}}{}^{I_0}\widehat{\rho }={}_{I_0}{}^W\widehat{\rho }\cdot {}_{L_k}{}^W\widehat{\rho }\cdot {}_I{}^L\widehat{\rho }$, differentiating and rearranging, Equation (32) can be minimized as

$$U\left({}_W{}^{I_0}\rho ,{}_I{}^L\rho \right)={\left[{\displaystyle \sum _{k=1}^n{J}_k{(P_{I_{f(k)}}+H_k{P}_{L_k}{H}_k^T)}^{-1}{J}_k^T}\right]}^{-1}\left[{\displaystyle \sum _{k=1}^n({}_{I_{f(k)}}{}^{I_0}\widehat{\rho }-{}_{I_0}{}^W\widehat{\rho }\cdot {}_{L_k}{}^W\widehat{\rho }\cdot {}_I{}^L\widehat{\rho })J_k{(P_{I_{f(k)}}+H_k{P}_{L_k}{H}_k^T)}^{-1}{J}_k^T}\right]$$

(35)

$$H({}_W{}^{I_0}\rho ,{}_{L_k}{}^W\rho ,{}_I{}^L\rho )=\left[\frac{\partial ({}_W{}^{I_0}\rho \cdot {}_{L_k}{}^W\rho \cdot {}_I{}^L\rho )}{\partial ({}_W{}^{I_0}\rho )},\frac{\partial ({}_W{}^{I_0}\rho \cdot {}_{L_k}{}^W\rho \cdot {}_I{}^L\rho )}{\partial ({}_I{}^L\rho )}\right]$$

(36)

where $H_k$ is the block-diagonal Jacobian matrix of the constraints with respect to ${}_{L_k}{}^W\rho$, and $J_k$ is the stacked Jacobian matrix of the constraints with respect to the transform parameters ${}_W{}^{I_0}\rho$ and ${}_I{}^L\rho$.

4.2. Iterated Sigma Point Kalman Filter (ISPKF) Algorithm for Compensation Calibration Parameters

After the relative orientation and optimal time delay estimation for the LiDAR-IMU, the ISPKF algorithm is used for compensation of the time delay calibration parameters between the LiDAR and IMU measurements.

The ISPKF algorithm uses the process noise component to implement the incremental state vector and the state covariance matrix, as expressed in:

$${\widehat{X}}_a(t_k)=\left[\begin{array}{c}\widehat{X}(t_k)\\ n(t_k)\end{array}\right]$$

(37)

where n(t_k) is the noise vector in measurement process, ${\widehat{X}}_a\left(t_k\right)$ is the augmented state vector. At time t_k−1, shortly after the LiDAR and IMU measurement update, the enhanced state covariance matrix $P_a^{+}\left(t_{k-1}\right)$ and the enhanced state mean ${\widehat{X}}_a^{+}\left(t_{k-1}\right)$ can be formed as:

$$P_a^{+}(t_{k-1})=\left[\begin{array}{cc}{P}^{+}(t_{k-1})& 0_{m\times 12}\\ 0_{12\times m}& Q_c(t_{k-1})\end{array}\right]$$

(38)

$${\widehat{X}}_a^{+}(t_{k-1})=\left[\begin{array}{c}{\widehat{X}}^{+}(t_{k-1})\\ 0_{12\times 1}\end{array}\right]$$

(39)

where the state vector ${\widehat{X}}_a^{+}\left(t_{k-1}\right)$ has size m = 26 + 3n and $Q_c\left(t_{k-1}\right)$ is the continuous-time covariance matrix for the noise vector n(t_k−1), when n calibration points are included, for the Cartesian and inverse calibration points parameterization, respectively. The scaled form is employed for unscented transform, which requires a scaling term:

$$\lambda ={\alpha }^2(N+\beta )-N$$

(40)

The α parameter is used to control the sigma points spread with respect to the mean of the state; we usually set a small positive value for parameter α. In the a posteriori state distribution Taylor series expansion, the β parameter is used to correct the higher order term. We set β = 2 and minimize the joint Gaussian distribution fourth-order error.

We use the augmented state vector ${\widehat{X}}_a^{+}\left(t_{k-1}\right)$ to generate a set of sigma points according to the following equation:

$$\{\begin{array}{l}{\chi }_a^{(0)}(t_{k-1})={\widehat{X}}_a^{+}(t_{k-1})\\ {\chi }_a^{(l)}(t_{k-1})={\widehat{X}}_a^{+}(t_{k-1})+{\left(S(t_{k-1})\right)}_j,j=l=1,\cdots ,N\\ {\chi }_a^{(l)}(t_{k-1})={\widehat{X}}_a^{+}(t_{k-1})-{\left(S(t_{k-1})\right)}_j,j=1,\cdots ,N,l=N+1,\cdots ,2N\\ S(t_{k-1})=\sqrt{(\lambda +N)P_a^{+}(t_{k-1})}\end{array}$$

(41)

where $P_a^{+}\left(t_{k-1}\right)$ is the augmented state covariance matrix, ${\left(S(·)\right)}_j$ expresses the j-th column of the matrix S. The weight values of the associated sigma point can be described as:

$$\{\begin{array}{l}{W}_m^{(0)}=\lambda /(\lambda +N)\\ W_c^{(0)}=\lambda /(\lambda +N)+(1-{\alpha }^2+\beta )\\ W_m^{(j)}=W_m^{(j)}=\frac{1}{2(\lambda +N)},j=1,\cdots ,2N\end{array}$$

(42)

A single σ-point can be propagated by the enhanced nonlinear process model function f_a. Over the time interval $t\in \left[t_{k-1},t_k\right)$, the a priori state estimate and covariance t_k can be computed as:

$$\{\begin{array}{l}{\chi }_k^{(p)}=f_a\left({\chi }_a^{(p)}(t_{k-1})\right),p=0,\cdots ,2N\\ {\widehat{X}}_k^{-}={\displaystyle \sum _{p=0}^{2N}{W}_m^{(p)}{\chi }_k^{(p)}}\\ P_k^{-}={\displaystyle \sum _{p=0}^{2N}{W}_c^{(p)}\left({\chi }_k^{(p)}-{\widehat{X}}_k^{-}\right){\left({\chi }_k^{(p)}-{\widehat{X}}_k^{-}\right)}^T}\end{array}$$

(43)

Through propagating each σ point through the nonlinear measurement model function h, we can determine the predicted measurement vector as follows:

$$\{\begin{array}{l}{\mathbb{Z}}_k^{(p)}=h\left({\chi }_k^{(p)}\right),p=0,\cdots ,2N\\ {\widehat{Z}}_k={\displaystyle \sum _{p=0}^{2N}{W}_m^{(p)}{{\rm Z}}_k^{(p)}}\end{array}$$

(44)

Through computing the a posteriori state vector, the state covariance matrix, and the Kalman gain matrix, we can perform the state update as follows:

$$\{\begin{array}{l}{P}_{{\widehat{Z}}_k{\widehat{Z}}_k}={\displaystyle \sum _{p=0}^{2N}{W}_c^{(p)}\left({\mathbb{Z}}_k^{(p)}-{\widehat{Z}}_k\right){\left({\mathbb{Z}}_k^{(p)}-{\widehat{Z}}_k\right)}^T}\\ K_k=P_{{\widehat{X}}_k{\widehat{Z}}_k}{\left(P_{{\widehat{Z}}_k{\widehat{Z}}_k}+R_k\right)}^{-1}\\ {\widehat{X}}_k^{+}={\widehat{X}}_k^{-}+K_k(Z_k-{\widehat{Z}}_k)\\ P_k^{+}=P_k^{-}-K_k{P}_{{\widehat{Z}}_k{\widehat{Z}}_k}{K}_k^T\end{array}$$

(45)

where R_k is the measurement covariance matrix for Z_k, while $P_{{\widehat{Z}}_k{\widehat{Z}}_k}$ and $P_{{\widehat{X}}_k{\widehat{Z}}_k}$ are the predicted measurement covariance matrix and the state-measurement cross-covariance matrix, respectively.

5. Experiments and Discussion

To verify the effectiveness of the proposed algorithm, a field experiment was performed on the fourth floor of the School of Mechanical and Electrical Engineering building on the campus of China University of Mining and Technology. The experimental layout is shown in Figure 5. The experiments were conducted with a VLP-16 LiDAR (Velodyne, Morgan Hill, CA, USA) and Spatial FOG IMU (Advanced Navigation Company, Sydney, Australia). The specifications of the IMU and LiDAR are listed in

Table 1. The specifications of the IMU and LiDAR.

IMU				LiDAR
Navigation	Sensors Accelerometers Gyroscopes
Horizontal Position Accuracy: 0.5 m	Range	10 g	490°/s	Channels	16
Vertical Position Accuracy: 0.8 m	Bias Instability	15 μg	0.05°/h	Range	100 m
Velocity Accuracy: 0.007 m/s	Initial Bias	<1 mg	<1°/h	Accuracy	±3 cm
Roll & Pitch Accuracy: 0.01°	Scaling Error	<0.03%	<0.01%	Vertical FOV	30°
Heading Accuracy: 0.05°	Scale Stability	<0.04%	<0.02%	Horizontal FOV	360°
Output Data Rate: Up to 1000 Hz	Non-linearity	<0.03%	<0.005%	Output Data Rate	300,000 pts/s

.

The LiDAR consists of 16 laser scanners that collectively span a 27° of vertical field of view. Firstly, in each experiment, we initialize the IMU biases and keeping the IMU stationary for 3 s. We begin recording data from the LiDAR and the IMU, the moving objector carrying the LiDAR-IMU platform automatically moves following different configurations in front of the calibration plane. The IMU update data is recorded at 100 Hz, while the LiDAR scanning data rate is 100,000 points/s. We chose a set of three different trajectories for the LiDAR-IMU, and for each trajectory, we experimented with a different time shift and a different initial orientation estimate. In all trajectories, the initial true relative orientation of the IMU with respect to the LiDAR was the same. In the experiments, the moving objector ran three different trials, with the LiDAR-IMU platform translating and rotating in front of the calibration plane for approximately 5 s, and the LiDAR-IMU platform then ran in three different orientation and time delay configurations. For configuration 1, we set the IMU-camera relative orientation to a nominal value (a roll of 90°, pitch of 0°, and yaw of 90°), and fixed the estimated time delay value at zero. For configuration 2, we used the mean LiDAR-IMU relative orientation computed by averaging the roll, pitch, and yaw values, while again fixing the time delay at zero. Finally, for configuration 3, we used the averaged LiDAR-IMU relative orientation and the mean time delay value from configuration 2, and we determined the performance of each configuration by computing the RMS error. Once the LiDAR and IMU measurements for each configuration of the calibration plane were available, we used the method described in Section 4 to accurately estimate the LIDAR-IMU time delay calibration parameters and the LIDAR-IMU transformation parameters.

The experimental results for LiDAR-IMU time delay calibration as shown in Figure 6. Figure 6a,d,g, shows the initial alignment between IMU and LiDAR in East, North and Up orientation respectively. Figure 6b,e,h, shows one time alignment using ICP-ISPKF between IMU and LiDAR in East, North and Up orientation, respectively. Figure 6c,f,i, shows ten times alignment using ICP-ISPKF between IMU and LiDAR in East, North and Up orientation respectively. We examined the error in the LiDAR-IMU time delay calibration using ICP-ISPKF for ten times, as shown in

Table 2. Time delay calibration times using ICP-ISPKF for LiDAR-IMU.

Time Delay Calibration Times Using ICP-ISPKF	Time Delay Error (ms)	Alignment Error in East (m)	Alignment Error in North (m)	Alignment Error in Up (m)
0	9.58	0.093	0.168	0.089
1	4.67	0.067	0.097	0.063
2	2.45	0.043	0.068	0.041
3	1.66	0.037	0.047	0.035
4	1.17	0.031	0.039	0.029
5	0.87	0.026	0.032	0.025
6	0.63	0.023	0.027	0.021
7	0.57	0.020	0.024	0.019
8	0.53	0.019	0.021	0.018
9	0.51	0.018	0.020	0.018
10	0.50	0.018	0.019	0.017

.

According to the Table 2 and Figure 6, the initial time delay alignment error is 9.58 ms, the LiDAR-IMU alignment errors in the East, North and Up orientation are 0.093 m, 0.168 m, 0.089 m respectively. After one time calibration using ICP-ISPKF converged, the time delay alignment errors are reduced to 4.67 ms, and the alignment errors in the East, North and Up orientation are reduced to 0.067 m, 0.097 m, 0.063 m, respectively. For clarity, after ten times calibration using ICP-ISPKF converged, the final time delay alignment error was reduced to 0.50 ms, and the final alignment errors in the East, North and Up orientation were reduced to 0.018 m, 0.019 m, 0.017 m, respectively.

In order to prove the efficiency and accuracy of the ISPKF method, we used the ISPKF, KF and EKF methods to estimate the LIDAR-IMU time delay calibration errors. Since obtaining the initial truth time delay value is very difficult, we supposed the initial time delay alignment errors between LiDAR and IMU are zero, when the LiDAR and IMU measurements for each configuration of the calibration plane were available, the ICP-KF, ICP-EKF and ICP-ISPKF method are used to perform the time delay calibration for LiDAR-IMU.

The experimental results are shown in Figure 7, where the red line is the time delay alignment error result using the ICP-KF method, the blue line is the time delay alignment error result using the ICP-EKF method, the green line is the time delay alignment error result using the ICP-ISPKF method, and the one time calibration mean alignment errors using the ICP-KF, ICP-EKF and ICP-ISPKF methods are 0.233 m, 0.151 m, 0.067 m, respectively.

6. Conclusions

In order to solve the problem of LiDAR-IMU time delay calibration, we have presented a fusion method based on an iterative closest point algorithm and an iterated sigma point Kalman filter, which combines the advantages of ICP and ISPKF. The ICP algorithm can precisely determine the unknown transformation between LiDAR-IMU; and the ISPKF algorithm can optimally estimate the time delay calibration parameters. We realized the coordinate transformation from the LiDAR frame to the IMU frame, and established the measurement model and time delay error model of LiDAR and IMU. We also presented the methodology of the ICP and ISPKF procedure for the LiDAR-IMU time delay calibration. Intensive experimental studies were conducted to check the validity of the theoretical results. The results show that after ten times calibration using ICP-ISPKF converged, the time delay alignment error was reduced from 9.58 ms to 0.50 ms, and the alignment errors in the East, North and Up orientation were reduced from 0.093 m, 0.168 m, 0.089 m to 0.018 m, 0.019 m, 0.017 m, respectively. In the future work, the ICP-ISPKF algorithm will be used to calibrate the time delay error in other multi-sensor systems. Within the batch execution framework, different methods will be examined for incorporating varying time delays. We are convince that our proposed methods will be useful for other types of time delay calibration for multiple sensors.