0 概要

对于大多数的 LiDAR-惯性里程计,Accurate initial condition（包括LiDAR和6轴IMUThe time migration and the transformation between）非常重要,Often seen as advance conditions.但是,The information in their assembly laser radar inertial system may not be able to directly obtain,Often need extra calibration process of space and time ahead,The whole calibration process time-consuming to.

本文提出了LI-Init：A complete and real-time LiDAR-The inertial system initialization process.The process by by LiDAR Measuring estimate the state and IMU Measuring the alignment algorithm based on state,To calibrate the LiDAR 和 IMU 之间的时间偏移、外参、Gravity vector and IMU 偏置.The author will encapsulate the method into the initialization module,To be able to automatically detect the data collected by the movement of the incentive degree.The initialization module can be used in different types of LiDAR-In the inertial combination,And it has been integrated into the most advanced LIO 系统 FAST-LIO2 当中.

本文的具体贡献如下：

• 提出了一种高效、准确、Don't need special time synchronization hardware calibration method to estimate the unknown but constant Lidar-Inertia time migration between,The method based on cross-correlation and unified space-time optimization.
• This paper proposes a new optimization pattern space initialization,And put forward a kind of to evaluate the degree of data movement incentive.Through further by LiDAR Estimate of the state and IMU After noise measurements aligned,The initialization process can automatically extract the initialization required data,And real-time estimation and、重力向量、Gyroscope bias and accelerometer bias.
• In a variety of types of LiDAR-Is carried out on the inertial combination experiment,And verify the efficiency and precision of the initialization process.This method is known the first open source 3 d LiDAR-The inertial system initialization algorithm of time and space,At the same time support the mechanical rotating laser radar and not repeat scanning laser radar.

1 框架概述

整个 LiDAR-The inertial system initialization process framework as shown in the figure below：

The whole framework consists of two parts：LiDAR 里程计和 LiDAR-Inertial initialization.

1.1 LiDAR 里程计

由于IMUOnly can get incentive in sports,Thus the initialization process is a kind of method based on motion,This means that you need enough exercise motivation.

框架中使用的 LiDAR Odometer is by changing the FAST-LIO2 得到的.LiDAR 里程计在 LiDAR Scanning using uniform model（Includes angular velocity also include linear velocity）来预测 LiDAR Movement and the LiDAR Movement distortion compensation.通过将输入的 LiDAR Frame split into several sub frame,提高 LIDAR Odometer the frame rate of,To relieve the uniform model and the actual movement between the sensor does not match.

如果 LiDAR Odometer no failure（Such as the degradation of）,And the estimate of the LiDAR Angular velocity and linear velocity meet the evaluation criteria,Thinks that incentives are sufficient to.这种情况下,就可以将 LiDAR The output of the speedometer and the corresponding IMU The original measurement data to initialize the module.

1.2 LiDAR-Inertial initialization

在初始化过程中,First of all, by moving IMU Measurement data in order to align LiDAR 里程计数据,To calibrate IMU 和 LiDAR Odometer time migration between.The time offset calibration is completed,进行进一步的优化,To further calibration time migration,And calibration parameter、 IMU Bias and gravity vector.

After the initialization of state can be input to tightly coupled LIO 系统中（比如FAST-LIO2）,Through the fusion of subsequent LiDAR 数据和 IMU Data for on-line state estimation.

1.3 符号说明

• $⊞/\boxminus$：On the state of manifold space enclosed“加”和“减”操作
• $t_k$：第 $k$ 次 Lidar Scan the timestamp of the
• ${\rho }_j$：一次 Lidar 扫描中的第 $j$ A point of time stamp
• ${\tau }_i$：一次 Lidar 扫描中的第 $i$ 个 IMU Measuring the timestamp of the
• $L_j,L_k$：分别表示 ${\rho }_j$ 和 $t_k$ 时刻的 Lidar 坐标系
• $\mathbf{x},\hat{\mathbf{x}},\overline{\mathbf{x}}$：Respectively the system state the true value of、Forecast and update the value
• $\tilde{\mathbf{x}}$：表示真值 $\mathbf{x}$ 和估计值 $\overline{\mathbf{x}}$ 之间的误差
• ${\check{\mathbf{x}}}_j$：Said at the time of back propagation,与 ${\mathbf{x}}_k$ The estimate ${\mathbf{x}}_j$
• ${}^I{\mathbf{R}}_L,{}^I{\mathbf{p}}_L$：表示从 LiDAR 到 IMU The outside and the rotation and translation
• ${}^I{t}_L$：表示 LiDAR 和 IMU The total time migration between
• ${\mathbf{b}}_{\mathbf{\omega }},{\mathbf{b}}_{\mathbf{a}}$：Said gyroscope and accelerometer bias
• ${}^G\mathbf{g}$：Said the gravitational acceleration vector
• ${\mathcal{I}}_i,{\mathcal{I}}_k$：表示在 ${\tau }_i$ 和 $t_k$ Moment in the initialization step IMU 数据序列
• ${\overline{\mathcal{I}}}_k$：Said after using synchronization time stamp $t_k$ Compensation after initialization time migrationMU数据序列
• ${\mathcal{L}}_k$：表示在 $t_k$ Moment in the initialization step LiDAR 数据

2 LiDAR 里程计

LiDAR Odometer and built figure part（仅仅使用 LiDAR）Based on uniform model,The model assumes that the angular velocity and linear velocity in the interval $\left(t_k,t_{k+1}\right)$ For const（$t_k$ 和 $t_{k+1}$ 分别对应两次 LiDAR The end of the scanning time） ,也就是说：

$${\mathbf{x}}_{k+1}={\mathbf{x}}_k⊞\left(\Delta t\mathbf{f}\left({\mathbf{x}}_k,{\mathbf{w}}_k\right)\right)\text{\hspace{1em}(1)}$$

其中, $\Delta t$ Are two scanning time interval,状态矢量 $\mathbf{x}$,噪声 $\mathbf{w}$ And discrete state transfer function $\mathbf{f}$ 被定义如下：

$$\mathbf{x}=\left[\begin{array}{c}{}^G{\mathbf{R}}_L\\ {}^G{\mathbf{p}}_L\\ {}^G{\mathbf{v}}_L\\ {\mathbf{\omega }}_L\end{array}\right],\mathbf{w}=\left[\begin{array}{c}{\mathbf{n}}_{\mathbf{v}}\\ {\mathbf{n}}_{\mathbf{\omega }}\end{array}\right],\mathbf{f}(\mathbf{x},\mathbf{w})=\left[\begin{array}{c}{\mathbf{\omega }}_L\\ {}^G{\mathbf{v}}_L\\ {\mathbf{n}}_{\mathbf{v}}\\ {\mathbf{n}}_{\mathbf{\omega }}\end{array}\right]\text{\hspace{1em}(2)}$$

其中,${}^G{\mathbf{R}}_L\in SO(3),{}^G{\mathbf{p}}_L$ 表示 LiDAR 在世界坐标系（也就是第一帧 LiDAR The body coordinate system）下的位姿,${}^G{\mathbf{v}}_L$ 表示 LiDAR In the world coordinate system under linear velocity,${\mathbf{\omega }}_L$ 表示 LiDAR 在 LiDAR Their coordinates the angular velocity of.${}^G{\mathbf{v}}_L$ 和 ${\mathbf{\omega }}_L$ Respectively is modeled as a gaussian noise ${\mathbf{n}}_{\mathbf{v}}$ 和 ${\mathbf{n}}_{\omega }$ Drive the random walk process.

在公式（1）中,Using encapsulation on state manifold space“加”和“减”Operation for operation.特别的是,对于公式（2）The state of manifold,$⊞$ And the inverse operation of it $\boxminus$ Were defined as follows：

$$\left[\begin{array}{c}\mathbf{R}\\ \mathbf{a}\end{array}\right]⊞\left[\begin{array}{l}\mathbf{r}\\ \mathbf{b}\end{array}\right]=\left[\begin{array}{c}\mathbf{RExp}(\mathbf{r})\\ \mathbf{a}+\mathbf{b}\end{array}\right];\left[\begin{array}{c}{\mathbf{R}}_1\\ \mathbf{a}\end{array}\right]\boxminus \left[\begin{array}{c}{\mathbf{R}}_2\\ \mathbf{b}\end{array}\right]=\left[\begin{array}{c}\log \left({\mathbf{R}}_2^T{\mathbf{R}}_1\right)\\ \mathbf{a}-\mathbf{b}\end{array}\right]$$

其中,$\mathbf{R},{\mathbf{R}}_1,{\mathbf{R}}_2\in SO(3)$,$\mathbf{r},\mathbf{a},\mathbf{b}\in {\mathbb{R}}^n$,$\mathrm{Exp}(\cdot ):{\mathbb{R}}^3\mapsto SO(3)$ 表示在 $SO(3)$ 上的指数映射,$\log (\cdot ):SO(3)\mapsto {\mathbb{R}}^3$ Said the logarithmic mapping.

【注意】这里补充一点,$⊞$ And the inverse operation of it $\boxminus$ In the original paper is defined as：

$$\begin{gathered} {⊞}_{\mathcal{S}}:\mathcal{S}\times {\mathbb{R}}^n\to \mathcal{S} \\ {\boxminus }_{\mathcal{S}}:\mathcal{S}\times \mathcal{S}\to {\mathbb{R}}^n \end{gathered}$$

They are of practical significance for：

• 运算 $y=x⊞\delta$ 表示, 在状态 $x$ To add a tiny disturbance $\delta \in {\mathbb{R}}^n$ 得到状态 $y$.
• 相反,运算 $\delta =y\boxminus x$ Sure is added to the state $x$ 产生状态 $y$ 的扰动 $\delta \in {\mathbb{R}}^n$.

实际上,The speed of the sensor movement may not be constant.In order to reduce the effect of model error,We will laser radar scanning input is divided into several smaller continuous child frame,Within the scope of each frame of time,Sensor movement with uniform model more consistent.

2.1 Based on the iterative kalman filtering error stateESIKF

基于公式（1）Said in a manifold system,We use iterative kalman filter based on the error state（ESIKF）To estimate the system state.ESIKFThe prediction of forecast steps include state and covariance propagation,表示如下：

$${\hat{\mathbf{x}}}_{k+1}={\overline{\mathbf{x}}}_k⊞\left(\Delta t\mathbf{f}\left({\overline{\mathbf{x}}}_k,0\right)\right)$$

$${\hat{\mathbf{P}}}_{k+1}={\mathbf{F}}_{\tilde{\mathbf{x}}}{\overline{\mathbf{P}}}_k{\mathbf{F}}_{\tilde{\mathbf{x}}}^T+{\mathbf{F}}_{\mathbf{w}}\mathbf{Q}{\mathbf{F}}_{\mathbf{w}}^T$$

其中,$\mathbf{P}$ 和 $\mathbf{Q}$ State estimation and process noise $\mathbf{w}$ 的协方差矩阵.${\mathbf{F}}_{\tilde{\mathbf{x}}}$ 和 ${\mathbf{F}}_{\mathbf{w}}$ 分别表示如下：

$$\begin{array}{rl}{\mathbf{F}}_{\tilde{\mathbf{x}}}& =\frac{\partial \left(\left({\overline{\mathbf{x}}}_k⊞{\tilde{\mathbf{x}}}_k\right)⊞\left(\Delta t\mathbf{f}\left({\overline{\mathbf{x}}}_k⊞{\tilde{\mathbf{x}}}_k,0\right)\right)\boxminus \left({\overline{\mathbf{x}}}_k⊞\Delta t\mathbf{f}\left({\overline{\mathbf{x}}}_k,0\right)\right)\right)}{\partial {\tilde{\mathbf{x}}}_k}\\ & =\left[\begin{array}{cccc}\mathrm{Exp}\left(-{\hat{\mathbf{\omega }}}_{L_k}\Delta t\right)& 0_{3\times 3}& 0_{3\times 3}& {\mathbf{I}}_{3\times 3}\Delta t\\ 0_{3\times 3}& {\mathbf{I}}_{3\times 3}& {\mathbf{I}}_{3\times 3}\Delta t& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& {\mathbf{I}}_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& {\mathbf{I}}_{3\times 3}\end{array}\right]\end{array}$$

$$\begin{array}{rl}{\mathbf{F}}_{\mathbf{w}}& =\frac{\partial \left({\overline{\mathbf{x}}}_k⊞\left(\Delta t\mathbf{f}\left({\overline{\mathbf{x}}}_k,{\mathbf{w}}_k\right)\right)\boxminus \left({\overline{\mathbf{x}}}_k⊞\Delta t\mathbf{f}\left({\overline{\mathbf{x}}}_k,0\right)\right)\right)}{\partial {\mathbf{w}}_k}\\ & =\left[\begin{array}{cc}{0}_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}\\ {\mathbf{I}}_{3\times 3}\Delta t& 0_{3\times 3}\\ 0_{3\times 3}& {\mathbf{I}}_{3\times 3}\Delta t\end{array}\right]\end{array}\text{\hspace{1em}(5)}$$

其中,$\tilde{\mathbf{x}}$ Said error state.

2.2 LiDAR 运动补偿

在我们考虑的问题中,IMU 数据和 LIDAR The data is out of sync,因此采用 IMU Auxiliary motion compensation method is not feasible.

Upon receiving the timestamp $t_{k+1}$ The new laser radar scanning after,To compensate for movement distortion,我们将在时间点 ${\rho }_j\in \left(t_k,t_{k+1}\right)$ Sampling of each point ${}^{L_j}{\mathbf{p}}_j$ 投影到 LiDAR 扫描帧 $L_{k+1}$ 的最后时刻 $t_{k+1}$.Due to the uniform model,我们有：${}^G{\hat{\mathbf{v}}}_{L_{k+1}}={}^G{\overline{\mathbf{v}}}_{L_k},{\hat{\mathbf{\omega }}}_{L_{k+1}}={\overline{\mathbf{\omega }}}_{L_k}$,That is to say, from the point in time ${\rho }_i$ 到时间点 $t_{k+1}$,Each point cloud of relative transformation as：$L_{k+1}{\mathbf{T}}_{L_j}=\left({}^{L_{k+1}}{\check{\mathbf{R}}}_{L_j},{}^{L_{k+1}}{\check{\mathbf{p}}}_{L_j}\right)$,其中：

$$\begin{array}{rl}{}^{L_{k+1}}{\check{\mathbf{R}}}_{L_j}& =\mathrm{Exp}\left(-{\hat{\mathbf{\omega }}}_{L_{k+1}}\Delta t_j\right)\\ {}^{L_{k+1}}{\check{\mathbf{p}}}_{L_j}& =-{}^G{\hat{\mathbf{R}}}_{L_{k+1}}^T{}^G{\hat{\mathbf{v}}}_{L_{k+1}}\Delta t_j\\ \Delta t_j& =t_{k+1}-{\rho }_j\end{array}\text{\hspace{1em}(6)}$$

然后,Local measurement points can be ${}^{L_j}{\mathbf{p}}_j$ Can be projected to LiDAR 扫描帧 $L_{k+1}$ 的最后时刻 $t_{k+1}$,也就是：

$${}^{L_{k+1}}{\mathbf{p}}_j={}^{L_{k+1}}{\check{\mathbf{T}}}_{L_j}{}^{L_j}{\mathbf{p}}_j\text{\hspace{1em}(7)}$$

然后,Distortion compensation after scanning $\left\{{}^{L_{k+1}}{\mathbf{p}}_j\right\}$ Provides a about an unknown state ${}^G{\mathbf{T}}_{L_{k+1}}$ The implicit measure,Residual said to point to the distance.在此基础上,In the frame of the iterative kalman filtering iterative estimates the state ${\mathbf{x}}_{k+1}$,直到收敛.After convergence of state estimation is expressed as ${\overline{\mathbf{x}}}_{k+1}$,And then will be used to spread the subsequent IMU 测量.

3 LiDAR-Inertial initialization

上面的 LiDAR The end of the odometer in each scan,即 $t_k$ 时刻,输出 LiDAR 的角速度 ${\mathbf{\omega }}_{L_k}$ 和线速度 ${}^G{\mathbf{v}}_{L_k}$.同时,IMUBring also for the original measurements,With a timestamp ${\tau }_i$ 的角速度 ${\omega }_{m_i}$ And linear acceleration ${\mathbf{a}}_{m_i}$.These data are accumulated,And through the incentive criterion to be accessed repeatedly.Once the data collect enough incentive,Is called initialization module,The final output module time migration ${}^I{t}_L\in \mathbb{R}$ ,外参 ${}^I{\mathbf{T}}_L=\left({}^I{\mathbf{R}}_L,{}^I{\mathbf{p}}_L\right)\in SE(3)$ ,IMU 偏置 ${\mathbf{b}}_{\omega },{\mathbf{b}}_{\mathbf{a}}\in {\mathbb{R}}^3$ 和 ${}^G\mathbf{g}\in {\mathbb{R}}^3$.

3.1 数据预处理

IMUOriginal measurements by noise ${\mathbf{n}}_{{\omega }_i}$ 和 ${\mathbf{n}}_{{\mathbf{a}}_i}$ 的影响.IMUMeasurement model for：

$${\mathbf{\omega }}_{m_i}={\mathbf{\omega }}_i^{\mathrm{gt}}+{\mathbf{b}}_{\omega }+{\mathbf{n}}_{{\omega }_i},{\mathbf{a}}_{m_i}={\mathbf{a}}_i^{\mathrm{gt}}+{\mathbf{b}}_{\mathbf{a}}+{\mathbf{n}}_{{\mathbf{a}}_i}\text{\hspace{1em}(8)}$$

其中,${\mathbf{\omega }}_i^{\mathrm{gt}}$ 和 ${\mathbf{a}}_i^{\mathrm{gt}}$ 是IMUThe true value of angular velocity and linear acceleration.同样,来自 LiDAR Odometer estimate ${\mathbf{\omega }}_{L_k}$ 和 ${}^G{\mathbf{v}}_{L_k}$ 也包含噪声.

In order to reduce the impact of these high frequency noise,Usually use a**The zero phase low pass filter（a non-causal zero phase low-pass filter）**To filter the noise,Without introducing any filtering delay.Zero phase filter is through backward and run backButterworthLow pass filter to realize,To produce the attenuation of noiseIMU测量 ${\mathbf{\omega }}_{I_i}={\mathbf{\omega }}_i^{\mathrm{gt}}+{\mathbf{b}}_{\omega },{\mathbf{a}}_{I_i}={\mathbf{a}}_i^{\mathrm{gt}}+{\mathbf{b}}_{\mathbf{a}}$.为了简洁,Noise attenuation LiDAR Estimates are still expressed as ${\mathbf{\omega }}_{L_k}$ 和 ${}^G{\mathbf{v}}_{L_k}$ .

从 LiDAR Odometer estimate ${\mathbf{\omega }}_{L_k}$ 和 ${}^G{\mathbf{v}}_{L_k}$ 中,We through the causal central difference（non-causal central difference）的方法可以得到 LiDAR The angular acceleration and linear acceleration ${\mathbf{\Omega }}_{L_k},{}^G{\mathbf{a}}_{L_k}$ .

最终的 LiDAR Odometer data can be expressed in a collection：

$${\mathcal{L}}_k=\left\{{\mathbf{\omega }}_{L_k},{}^G{\mathbf{v}}_{L_k},{\mathbf{\Omega }}_{L_k},{}^G{\mathbf{a}}_{L_k}\right\}\text{\hspace{1em}(9)}$$

类似的,We from the noise attenuation of gyroscope measure ${\mathbf{\omega }}_{I_i}$ 可以得到 IMU 的角加速度 ${\mathbf{\Omega }}_{I_i}$,也可以用集合表示：

$${\mathcal{I}}_i=\left\{{\mathbf{\omega }}_{I_i},{\mathbf{a}}_{I_i},{\mathbf{\Omega }}_{I_i}\right\}\text{\hspace{1em}(10)}$$

由于 IMU The frequency is generally higher than LiDAR 的频率,Therefore two sequences ${\mathcal{I}}_i$ 和 ${\mathcal{L}}_k$ 的大小不相同.为了解决此问题,We extracted at the same time LIDAR 和 IMU 数据,And through in everyLiDAR Odometer moment $t_k$ On linear interpolation for the sampling（如上图）.下采样的 IMU 数据表示为 ${\mathcal{I}}_k$：

$${\mathcal{I}}_k=\left\{{\mathbf{\omega }}_{I_k},{\mathbf{a}}_{I_k},{\mathbf{\Omega }}_{I_k}\right\}\text{\hspace{1em}(11)}$$

其具有与 ${\mathcal{L}}_k$ 相同的时间戳 $t_k$ （But the data is actually delayed the time constant of the known ${}^I{t}_L$）.

3.2 Through each other（Cross-Correlation）Initialization of time

在大多数情况下,由于 LIDAR-惯性 Odometer module before receiving inevitably, transmission and processing delay,这是LIDAR ${\mathcal{L}}_k$ 和IMU ${\mathcal{I}}_k$ Between the unknown but constant offset ${}^I{t}_L$,因此如果将 IMU 测量 ${\mathcal{I}}_k$ 提前 ${}^I{t}_L$,将与 LIDAR 里程计 ${\mathcal{L}}_k$ 对齐.由于 LIDAR 数据和 IMU The data in the discrete time $t_k$ 中,因此将 IMU Data operation is basically in discrete step size in advance $d={}^I{t}_L/\Delta t$ 中进行的,其中 $\Delta t$ 是两次 LiDAR Scanning the time interval between.具体而言,For the angular velocity,我们有：

$${\mathbf{\omega }}_{I_{k+d}}={}^I{\mathbf{R}}_L{\mathbf{\omega }}_{L_k}+{\mathbf{b}}_{\omega }\text{\hspace{1em}(12)}$$

Because of the gyroscope bias ${\mathbf{b}}_{\omega }$ 通常很小,我们可以忽略不计.如果不管 ${\mathbf{R}}_L$ ,我们发现 ${\mathbf{\omega }}_{I_{k+d}}$ 和 ${\mathbf{\omega }}_{L_k}$ The size should be the same.We use the similarity between the cross-correlation to quantify the amount of.然后,Can by solving the following optimization problem $d$ ：

$$d^{\ast }=\underset{d}{\mathrm{argmax}}\sum \Vert {\mathbf{\omega }}_{I_{k+d}}\Vert \cdot \Vert {\mathbf{\omega }}_{L_k}\Vert \text{\hspace{1em}(13)}$$

即通过在 ${\mathcal{L}}_k$ Enumeration migration within the limits of index $d$.

3.3 Unified and the rotation of the parts and time calibration

Cross-correlation method of noise and small scale of gyroscope bias is ruban.但是（13）An obvious defect is time offset calibration resolution than LiDAR A sampling interval odometer $\Delta t$ ,Therefore cannot identify any less than $\Delta t$ The deviation of residual $\delta t$.设 ${}^I{t}_L$ 为 LiDAR 里程计 ${\mathbf{\omega }}_L$ 和 IMU 数据 ${\mathbf{\omega }}_I$ The total offset between,则有：${}^I{t}_L=d^{\ast }\Delta t+\delta t$.那么将 IMU 测量 ${\mathbf{\omega }}_I$ 前移 ${}^I{t}_L$ 与 LiDAR 里程计 ${\mathbf{\omega }}_L$ 对齐,则公式（12）可以写成：

$${\mathbf{\omega }}_I\left(t+{}^I{t}_L\right)={}^I{\mathbf{R}}_L{\mathbf{\omega }}_L(t)+{\mathbf{b}}_{\omega }\text{\hspace{1em}(14)}$$

由于在公式（9）中, LiDAR 里程计 ${\mathbf{\omega }}_L$ 的数据在 $t_k$ 时刻得到,将 $t=t_k$ 和 ${}^I{t}_L=d^{\ast }\Delta t+\delta t$ 代入到公式（14）中有：

$${\mathbf{\omega }}_I\left(t_k+d^{\ast }\Delta t+\delta t\right)={}^I{\mathbf{R}}_L{\mathbf{\omega }}_{L_k}+{\mathbf{b}}_{\omega }\text{\hspace{1em}(15)}$$

注意到 ${\omega }_I\left(t_k+d^{\ast }\Delta t+\delta t\right)$ 是 IMU 在时间 $t_k+d^{\ast }\Delta t$ After the angular velocity of,其中在 $t_k+d^{\ast }\Delta t$ 时刻的IMU The angular velocity and angular acceleration respectively：${\mathbf{\omega }}_I\left(t_k+d^{\ast }\Delta t\right)={\mathbf{\omega }}_{I_{k^{\prime }}}$ 和 ${\mathbf{\Omega }}_I\left(t_k+d^{\ast }\Delta t\right)={\mathbf{\Omega }}_{I_{k^{\prime }}}$ （$k^{\prime }=k+d^{\ast }$ .

如上图所示,We can, we assume that the $\delta t$ This a very small period of time inside the acceleration is constant interpolation to get ${\omega }_I\left(t_k+d^{\ast }\Delta t+\delta t\right)$,即：

$${\mathbf{\omega }}_I\left(t_k+d^{\ast }\Delta t+\delta t\right)\approx {\mathbf{\omega }}_{I_{k^{\prime }}}+\delta t{\mathbf{\Omega }}_{I_{k^{\prime }}}\text{\hspace{1em}(16)}$$

将公式（16）代入到（15）中可以得到：

$${\mathbf{\omega }}_{I_{k^{\prime }}}+\delta t{\mathbf{\Omega }}_{I_{k^{\prime }}}={}^I{\mathbf{R}}_L{\mathbf{\omega }}_{L_k}+{\mathbf{b}}_{\omega }\text{\hspace{1em}(17)}$$

最后,基于（17）中的约束,Unified the optimization problem of space and time can be expressed as：

$$\underset{{}^I{\mathbf{R}}_L,{\mathbf{b}}_{\omega },\delta t}{\mathrm{argmin}}\sum {\Vert {}^I{\mathbf{R}}_L{\mathbf{\omega }}_{L_k}+{\mathbf{b}}_{\mathbf{\omega }}-{\mathbf{\omega }}_{I_{k^{\prime }}}-\delta t\cdot {\mathbf{\Omega }}_{I_{k^{\prime }}}\Vert }^2\text{\hspace{1em}(18)}$$

由于存在约束 ${}^I{\mathbf{R}}_L\in SO(3)$,该问题可以通过 Ceres Solver 进行迭代求解,初值设置为 $\left({}^I{\mathbf{R}}_L,{\mathbf{b}}_{\omega },\delta t\right)=\left({\mathbf{I}}_{3\times 3},0_{3\times 1},0\right)$.

3.4 Outside and the translation part of the initialization and gravity

在上一节中,We get out and the rotation of the part ${\mathbf{R}}_L$,陀螺仪偏置 ${\mathbf{b}}_{\omega }$ 和时间偏移 ${}^I{t}_L$ .在本节中,We fixed the values,Then carries on the outside and the translation part of the、The gravity vector and acceleration bias calibration.

首先,We use the good time migration before calibration $d^{\ast }$ And offset residual $\delta t$ 将 IMU 数据 ${\mathcal{I}}_k$ 与 LIDAR 的数据 ${\mathcal{L}}_k$ 对齐.对齐的IMU数据表示为 ${\overline{\mathcal{I}}}_k$,Now assume it and ${\mathcal{L}}_k$ Completely aligned and have no time to offset.于是,对应于 $t_k$ 时刻 LiDAR 角速度 ${\mathbf{\omega }}_{L_k}$ 的 IMU 角速度 ${\bar{\mathbf{\omega }}}_{I_k}$ 可以记为：

$${\overline{\mathbf{\omega }}}_{I_k}={\mathbf{\omega }}_I\left(t_k+d^{\ast }\Delta t+\delta t\right)\approx {\mathbf{\omega }}_{I_{k+d}}+\delta t{\mathbf{\Omega }}_{I_{k+d}}$$

类似的,对应于 $t_k$ 时刻 LiDAR 线加速度 ${}^G{\mathbf{a}}_{L_k}$ 的 IMU 线加速度 ${\overline{\mathbf{a}}}_{I_k}$ 可以记为：

$$\begin{array}{rl}{\overline{\mathbf{a}}}_{I_k}& ={\mathbf{a}}_I\left(t_k+d^{\ast }\Delta t+\delta t\right)\\ & \approx {\mathbf{a}}_{I_{k+d}}+\frac{\delta t}{\Delta t}\left({\mathbf{a}}_{I_{k+d+1}}-{\mathbf{a}}_{I_{k+d}}\right)\end{array}$$

然后,类似于（14）,我们可以找到 IMU 和 LiDAR The acceleration constraints between.在论文中提到,Two outside the fixed reference coordinate system A 和 B Has the following relationship between the acceleration：

$${}^A{\mathbf{R}}_B{\mathbf{a}}_B={\mathbf{a}}_A+{\lfloor {\mathbf{\omega }}_A\rfloor }_{\wedge }^2{}^A{\mathbf{p}}_B+\lfloor {\mathbf{\Omega }}_A\rfloor {}_{\wedge }{}^A{\mathbf{p}}_B$$

其中,${}^A{\mathbf{R}}_B,{}^A{\mathbf{p}}_B$ Says from the coordinate system B 到坐标系 A The outside and transformation of the.${\mathbf{a}}_A$ 和 ${\mathbf{a}}_B$ Are their own coordinates acceleration.

对于 LiDAR-惯性系统,我们有两种选择：用坐标系 A 表示 IMU 坐标系,用坐标系 B 表示 LIDAR 坐标系;或者相反.Notice in the former case,${\mathbf{\omega }}_A={\overline{\mathbf{\omega }}}_{I_k}-{\mathbf{b}}_{\omega }$ The influence of the accuracy of the gyroscope bias estimation,And angular velocity measurement noise can enlarge ${\mathbf{\Omega }}_A$ 的误差.In order to avoid this problem and improve upon the translation part of the robustness of calibration,我们将 LIDAR Coordinate system is set to the A,将 IMU 坐标系设置为 坐标系 B.由于 LIDAR 的加速度 ${}^G{\mathbf{a}}_{L_k}$ Is in the world coordinate system for said,我们需要计算 LiDAR Instant acceleration in LiDAR 坐标系中的表示,记为 ${\mathbf{a}}_{L_k}$：

$${\mathbf{a}}_{L_k}={}^G{\mathbf{R}}_L^T\left({}^G{\mathbf{a}}_{L_k}-{}^G\mathbf{g}\right)$$

其中,${}^G{\mathbf{R}}_L^T$ 是 LiDAR In the world coordinate system attitude,已经在 LiDAR Odometer part get.

最后,Can through the following optimization problem jointly estimated out and the translation of part、The accelerometer bias and gravity vector：

$$\underset{{}^I{\mathbf{p}}_L,{\mathbf{b}}_{\mathbf{a}},{}^G\mathbf{g}}{\mathrm{argmin}}\sum {\Vert {}^I{\mathbf{R}}_L^T\left({\overline{\mathbf{a}}}_{I_k}-{\mathbf{b}}_{\mathbf{a}}\right)-{\mathbf{a}}_{L_k}-{\left({\lfloor {\mathbf{\omega }}_{L_k}\rfloor }_{\wedge }^2+{\lfloor {\mathbf{\Omega }}_{L_k}\rfloor }_{\wedge }\right)}^L{\mathbf{p}}_I\Vert }^2$$

由于存在约束 ${}^G\mathbf{g}\in {\mathbb{S}}_2$,该问题可以通过 Ceres Solver 进行迭代求解,初值设置为 $\left({}^I{\mathbf{p}}_L,{\mathbf{b}}_{\mathbf{a}},{}^G\mathbf{g}\right)=\left(0_{3\times 1},0_{3\times 1},9.81{\mathbf{e}}_3\right)$.

${}^L{\mathbf{p}}_I$ After he was estimate,Can be calculated from the LiDAR 到 IMU The translation as ${}^I{\mathbf{p}}_L=-{}^I{\mathbf{R}}_L{}^L{\mathbf{p}}_I$.

3.5 Data accumulation assessment

In this paper, the proposed initialization method depends on the LiDAR-Inertial device enough incentive（The device requires enough movement）.因此,The system should be able to assess whether there is enough exercise motivation to perform initialization.

理想情况下,Incentive can be achieved by formula（18）对于 $\left({}^I{\mathbf{R}}_L,{\mathbf{b}}_{\omega },\delta t\right)$ 和公式（19）对于 $\left({}^I{\mathbf{p}}_L,{\mathbf{b}}_{\mathbf{a}},{}^G\mathbf{g}\right)$ The complete jacobian matrix rank to evaluate.

在实践中,We found that only need to evaluate and rotating part and external part and translational corresponding jacobian is enough,Because of the participation motivation often require complex sport,These movements are also enough to inspire other state.

使用 ${\mathbf{J}}_r$ To represent and rotating parts ${}^I{\mathbf{R}}_L$ The corresponding jacobian,使用 ${\mathbf{J}}_t$ To represent and translation parts ${}^I{\mathbf{p}}_L$ The corresponding jacobian：

$${\mathbf{J}}_r=\left[\begin{array}{c}⋮\\ -{}^I{\mathbf{R}}_L\lfloor {\mathbf{\omega }}_{L_k}\rfloor \wedge \\ ⋮\end{array}\right],{\mathbf{J}}_t=\left[\begin{array}{c}⋮\\ \lfloor {\mathbf{\omega }}_{L_k}\rfloor {}_{\wedge }^2+\lfloor {\mathbf{\Omega }}_{L_k}\rfloor \wedge \\ ⋮\end{array}\right]$$

Then you can through the following two matrix rank of assessing the extent of the incentive：

$${\mathbf{J}}_r^T{\mathbf{J}}_r=\sum {\lfloor {\mathbf{\omega }}_{L_k}\rfloor }_{\wedge }^T{\lfloor {\mathbf{\omega }}_{L_k}\rfloor }_{\wedge }$$

$${\mathbf{J}}_t^T{\mathbf{J}}_t=\sum {\left({\lfloor {\mathbf{\omega }}_{L_k}\rfloor }_{\wedge }^2+{\lfloor {\mathbf{\Omega }}_{L_k}\rfloor }_{\wedge }\right)}^T\left({\lfloor {\mathbf{\omega }}_{L_k}\rfloor }_{\wedge }^2+{\lfloor {\mathbf{\Omega }}_{L_k}\rfloor }_{\wedge }\right)$$

More quantitative to say,How motivated by ${\mathbf{J}}_r^T{\mathbf{J}}_r$ 和 ${\mathbf{J}}_t^T{\mathbf{J}}_t$ The singular values of said.

基于此原则,We have developed an evaluation process,The program can guide the user how to move the equipment to obtain sufficient incentive.We quantified by jacobian matrix singular value to motivate,And set a threshold value to assess whether incentive enough.