LiDAR-IMU Time Delay Calibration Based on Iterative Closest Point and Iterated Sigma Point Kalman Filter. Sensors 2017, 17, 539

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.

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$.

The authors would like to apologize for any inconvenience caused to the readers by these changes.