sensors Correction
LiDAR-IMU Time Delay Calibration Based on Iterative Closest Point and Iterated Sigma Point Kalman Filter. Sensors 2017, 17, 539 Wanli Liu
ID
School of Mechanical and Electrical Engineering, China University of Mining and Technology, Xuzhou 221116, China;
[email protected]; Tel.: +86-516-8359-0706 Received: 16 November 2017; Accepted: 4 December 2017; Published: 5 December 2017
The authors wish to make the following corrections to this paper [1]: 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: .
P I (t) = F (t) PI (t) + PI (t) F T (t) + G (t) Q g (t) G T (t)
(18)
.
∂ρ(t) 1 T = (ρ (t) · ω (t)) I3 − [ω (t)]× + ρ(t)ω (t)T − ω (t) · ρ T (t) ∂v 2 . ∂ρ(t) 1 G (t) = = − (1 − kρ(t)k2 ) I3 + 2[ρ(t)]× + 2ρ(t)ρ T (t) ∂n g 4 . 1 (1 − kρ(t)k2 ) I3 + 2[ρ(t)]× + 2ρ(t) ω I (t) ρ(t) = 4
F (t) =
(19) (20) (21)
where PI (t) is the IMU orientation covariance at a time t, Qg (t) is an additive noise vector, ρ(t) is the modified Rodrigues parameters vector, F(t) and G(t) are the system matrix for ρ(t), [ρ(t)]× is the skew-symmetric cross-product matrix for ρ(t), I3 is the 3 × 3 identity matrix, ω I (t) = ωm (t) − bg − ng (t) is the true angular velocity of IMU, and bg 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 I
0 dij = (W Li ρ, I ρ ) = 4arctan j
Sensors 2017, 17, 2821; doi:10.3390/s17122821
q
σijT σij
(30)
www.mdpi.com/journal/sensors
Sensors 2017, 17, 2821
2 of 2
I
I
L 0 σij = −(W0 ρˆ ·W Li ρ · I ρˆ ) · I ρ
(31)
j
I
0 where dij is the incremental rotation arc length taking from W Li ρ to Ij ρ in radians orientation, and σij is the spatial transform model on the unit sphere. Step II: Nonlinear Iterative Registration Rules. We can getncorrespondence 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
I0 L W ρ, I ρ
=
n
n
k =1
k =1
∑ sk PL−k1 skT + ∑ tk PI−f (1k) tkT
(32)
W sk = W Lk ρ − Lk ρˆ
tk =
I0 I f (k) ρ
(33)
− II0 ρˆ
(34)
f (k)
where PLk and PI f (k) are the associated covariance matrices, s and t are the stacked residuals vectors, I
and the W0 ρ and LI ρ are transform parameters. By using Lagrange multipliers and incorporating the constraints
I0 I f (k) ρˆ
=
W ρˆ I0
·
W ρˆ Lk
ˆ · LI ρ,
differentiating and rearranging, Equation (32) can be minimized as U
I0 L W ρ, I ρ
=
n
∑ Jk ( PI f (k) + Hk PLk HkT )
k =1
−1 T Jk
−1
n
I
W L T ∑ ( I f (k0) ρˆ − W I0 ρˆ · Lk ρˆ · I ρˆ ) Jk ( PI f (k) + Hk PLk Hk )
k =1
I I ∂(W0 ρ · W ρ · LI ρ ) ∂(W0 ρ · W ρ · LI ρ ) L L k k = , I ∂( LI ρ ) ∂(W0 ρ )
−1 T Jk
(35)
I L H (W0 ρ, W Lk ρ, I ρ )
(36)
where Hk is the block-diagonal Jacobian matrix of the constraints with respect to W L ρ, and Jk is the I
k
stacked Jacobian matrix of the constraints with respect to the transform parameters W0 ρ and LI ρ. The authors would like to apologize for any inconvenience caused to the readers by these changes. Reference 1.
Liu, W.L. LiDAR-IMU Time Delay Calibration Based on Iterative Closest Point and Iterated Sigma Point Kalman Filter. Sensors 2017, 17, 539. [CrossRef] [PubMed] © 2017 by the author. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).
