Robust Real

Robust Real"/>

Robust Real

0 概要

对于大多数的 LiDAR-惯性里程计，精确的初始状态（包括LiDAR和6轴IMU之间的时间偏移和外参变换）非常重要，通常被视为先觉条件。但是，这些信息在自己组装的激光雷达惯性系统中可能没办法直接获得，往往需要提前进行额外的时空标定过程，整个标定过程耗时费力。

本文提出了LI-Init：一个完整并且实时的 LiDAR-惯性系统初始化过程。该过程通过将由 LiDAR 测量估计出的状态与 IMU 测量得到的状态进行对齐，从而标定出 LiDAR 和 IMU 之间的时间偏移、外参、重力向量和 IMU 偏置。作者将该方法封装成初始化模块，能够自动检测所收集到的数据的运动激励程度。该初始化模块能够用于不同类型的 LiDAR-惯性组合中，并且已经集成到最先进的 LIO 系统 FAST-LIO2 当中。

本文的具体贡献如下：

• 提出了一种高效、准确、不需要专用时间同步硬件的时间标定方法来估计未知但恒定的 Lidar-惯性之间的时间偏移，该方法基于互相关和统一时空优化。
• 提出了一种新的优化范式来进行空间初始化，并提出了一种评估数据的运动激励程度的方法。通过进一步将由 LiDAR 估计出的状态与 IMU 降噪后的测量值对齐，初始化过程能够自动提取初始化所需的数据，并实时估计外参、重力向量、陀螺仪偏置和加速度计偏置。
• 在多种类型的 LiDAR-惯性组合上进行了实验，并验证了整个初始化过程的效率和精度。该方法是已知的第一个开源的三维 LiDAR-惯性系统时空初始化算法，同时支持机械旋转激光雷达和非重复扫描式激光雷达。

1 框架概述

整个 LiDAR-惯性系统初始化过程的框架如下图所示：

整个框架主要分为两个部分：LiDAR 里程计和 LiDAR-惯性初始化。

1.1 LiDAR 里程计

由于IMU只有在运动中才能够得到激励，因此整个初始化过程是一种基于运动的方法，这意味着需要足够的运动激励。

框架中使用的 LiDAR 里程计是通过修改 FAST-LIO2 得到的。LiDAR 里程计在 LiDAR 扫描中使用匀速模型（既包括角速度也包括线速度）来预测 LiDAR 运动和对 LiDAR 运动畸变进行补偿。通过将输入的 LiDAR 帧拆分为几个子帧，提高 LIDAR 里程计的帧率，来缓解匀速模型与实际传感器运动之间的不匹配。

如果 LiDAR 里程计没有失效（比如退化），并且所估计的 LiDAR 角速度和线速度满足评估标准，则认为激励足够。这种情况下，就可以将 LiDAR 里程计的输出和对应的 IMU 原始测量数据送入到初始化模块。

1.2 LiDAR-惯性初始化

在初始化过程中，首先通过移动 IMU 测量数据以对齐 LiDAR 里程计数据，从而标定 IMU 和 LiDAR 里程计之间的时间偏移。时间偏移标定完成之后，进行进一步的优化，以进一步标定时间偏移，并标定外参、 IMU 偏置和重力矢量。

初始化之后的状态便可以输入到紧耦合的 LIO 系统中（比如FAST-LIO2），通过融合后续的 LiDAR 数据和 IMU 数据进行在线的状态估计。

1.3 符号说明

• ⊞ / ⊟ \boxplus/\boxminus ⊞/⊟：在状态流形空间上封装的“加”和“减”操作
• t k t_{k} tk​：第 k k k 次 Lidar 扫描的时间戳
• ρ j \rho_{j} ρj​：一次 Lidar 扫描中的第 j j j 个点的时间戳
• τ i \tau_{i} τi​：一次 Lidar 扫描中的第 i i i 个 IMU 测量的时间戳
• L j , L k L_{j}, L_{k} Lj​,Lk​：分别表示 ρ j \rho_{j} ρj​ 和 t k t_{k} tk​ 时刻的 Lidar 坐标系
• x , x ^ , x ‾ \mathbf{x}, \widehat{\mathbf{x}}, \overline{\mathbf{x}} x,x ,x：分别表示系统状态的真值、预测值和更新值
• x ~ \widetilde{\mathbf{x}} x ：表示真值 x \mathbf{x} x 和估计值 x ‾ \overline{\mathbf{x}} x 之间的误差
• x ˇ j \check{\mathbf{x}}_{j} xˇj​：表示在反向传播时，与 x k \mathbf{x}_{k} xk​ 相关的估计值 x j \mathbf{x}_{j} xj​
• I R L , I p L { }^{I} \mathbf{R}_{L},{ }^{I} \mathbf{p}_{L} IRL​,IpL​：表示从 LiDAR 到 IMU 的外参旋转和平移
• I t L { }^{I} t_{L} ItL​：表示 LiDAR 和 IMU 之间的总的时间偏移
• b ω , b a \mathbf{b}_{\boldsymbol{\omega}}, \mathbf{b}_{\mathbf{a}} bω​,ba​：表示陀螺仪和加速度计的偏置
• G g { }^{G} \mathbf{g} Gg：表示重力加速度向量
• I i , I k \mathcal{I}_{i}, \mathcal{I}_{k} Ii​,Ik​：表示在 τ i \tau_{i} τi​ 和 t k t_{k} tk​ 时刻用在初始化步骤的 IMU 数据序列
• I ‾ k \overline{\mathcal{I}}_{k} Ik​：表示使用同步后的时间戳 t k t_k tk​ 补偿初始化时间偏移后的MU数据序列
• L k \mathcal{L}_{k} Lk​：表示在 t k t_{k} tk​ 时刻用在初始化步骤的 LiDAR 数据

2 LiDAR 里程计

LiDAR 里程计和建图部分（仅仅使用 LiDAR）建立在匀速模型上，该模型假设角速度和线速度在时间间隔 ( t k , t k + 1 ) \left(t_{k}, t_{k+1}\right) (tk​,tk+1​) 内为常量（ t k t_k tk​ 和 t k + 1 t_{k+1} tk+1​ 分别对应两次 LiDAR 扫描的结束时刻） ，也就是说：

x k + 1 = x k ⊞ ( Δ t f ( x k , w k ) ) (1) \mathbf{x}_{k+1}=\mathbf{x}_{k} \boxplus\left(\Delta t \mathbf{f}\left(\mathbf{x}_{k}, \mathbf{w}_{k}\right)\right)\tag{1} xk+1​=xk​⊞(Δtf(xk​,wk​))(1)

其中， Δ t \Delta t Δt 是两次扫描的时间间隔，状态矢量 x \mathbf{x} x，噪声 w \mathbf{w} w 和离散状态转移函数 f \mathbf{f} f 被定义如下：

x = [ G R L G p L G v L ω L ] , w = [ n v n ω ] , f ( x , w ) = [ ω L G v L n v n ω ] (2) \mathbf{x}=\left[\begin{array}{c}{ }^{G} \mathbf{R}_{L} \\{ }^{G} \mathbf{p}_{L} \\{ }^{G} \mathbf{v}_{L} \\\boldsymbol{\omega}_{L}\end{array}\right], \mathbf{w}=\left[\begin{array}{c}\mathbf{n}_{\mathbf{v}} \\\mathbf{n}_{\boldsymbol{\omega}}\end{array}\right], \mathbf{f}(\mathbf{x}, \mathbf{w})=\left[\begin{array}{c}\boldsymbol{\omega}_{L} \\{ }^{G} \mathbf{v}_{L} \\\mathbf{n}_{\mathbf{v}} \\\mathbf{n}_{\boldsymbol{\omega}}\end{array}\right]\tag{2} x=⎣ ⎡​GRL​GpL​GvL​ωL​​⎦ ⎤​,w=[nv​nω​​],f(x,w)=⎣ ⎡​ωL​GvL​nv​nω​​⎦ ⎤​(2)

其中， G R L ∈ S O ( 3 ) , G p L { }^{G} \mathbf{R}_{L} \in S O(3),{ }^{G} \mathbf{p}_{L} GRL​∈SO(3),GpL​ 表示 LiDAR 在世界坐标系（也就是第一帧 LiDAR 体坐标系）下的位姿， G v L { }^{G} \mathbf{v}_{L} GvL​ 表示 LiDAR 在世界坐标系下的线速度， ω L \boldsymbol{\omega}_{L} ωL​ 表示 LiDAR 在 LiDAR 自身坐标系下的角速度。 G v L { }^{G} \mathbf{v}_{L} GvL​ 和 ω L \boldsymbol{\omega}_{L} ωL​ 分别被建模为高斯噪声 n v \mathbf{n}_{\mathbf{v}} nv​ 和 n ω \mathbf{n}_{\mathbf{\omega}} nω​ 驱动的随机游走过程。

在公式（1）中，使用在状态流形空间上封装的“加”和“减”操作来进行运算。特别的是，对于公式（2）中的状态流形， ⊞ \boxplus ⊞ 和它的逆运算 ⊟ \boxminus ⊟ 分别被定义如下：

[ R a ] ⊞ [ r b ] = [ R E x p ( r ) a + b ] ; [ R 1 a ] ⊟ [ R 2 b ] = [ log ⁡ ( R 2 T R 1 ) a − b ] \left[\begin{array}{c}\mathbf{R} \\\mathbf{a}\end{array}\right] \boxplus\left[\begin{array}{l}\mathbf{r} \\\mathbf{b}\end{array}\right]=\left[\begin{array}{c}\mathbf{R E x p}(\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] [Ra​]⊞[rb​]=[RExp(r)a+b​];[R1​a​]⊟[R2​b​]=[log(R2T​R1​)a−b​]

其中， R , R 1 , R 2 ∈ S O ( 3 ) \mathbf{R}, \mathbf{R}_{1}, \mathbf{R}_{2} \in S O(3) R,R1​,R2​∈SO(3)， r , a , b ∈ R n \mathbf{r}, \mathbf{a}, \mathbf{b} \in \mathbb{R}^{n} r,a,b∈Rn， Exp ⁡ ( ⋅ ) : R 3 ↦ S O ( 3 ) \operatorname{Exp}(\cdot): \mathbb{R}^{3} \mapsto S O(3) Exp(⋅):R3↦SO(3) 表示在 S O ( 3 ) S O(3) SO(3) 上的指数映射， log ⁡ ( ⋅ ) : S O ( 3 ) ↦ R 3 \log (\cdot) :S O(3) \mapsto \mathbb{R}^{3} log(⋅):SO(3)↦R3 表示对数映射。

【注意】这里补充一点， ⊞ \boxplus ⊞ 和它的逆运算 ⊟ \boxminus ⊟ 在原论文中的定义为：

⊞ S : S × R n → S ⊟ S : S × S → R n \boxplus_{\mathcal{S}}: \mathcal{S} \times \mathbb{R}^{n} \rightarrow \mathcal{S}\\\boxminus_{\mathcal{S}}: \mathcal{S} \times \mathcal{S} \rightarrow \mathbb{R}^{n} ⊞S​:S×Rn→S⊟S​:S×S→Rn

它们的实际意义为：

• 运算 y = x ⊞ δ y=x \boxplus \delta y=x⊞δ 表示， 在状态 x x x 上添加一个微小扰动 δ ∈ R n \delta \in \mathbb{R}^{n} δ∈Rn 得到状态 y y y。
• 相反，运算 δ = y ⊟ x \delta=y \boxminus x δ=y⊟x 确定添加到状态 x x x 产生状态 y y y 的扰动 δ ∈ R n \delta \in \mathbb{R}^{n} δ∈Rn。

实际上，传感器运动的速度可能并不是常量。为了减轻模型误差的影响，我们将激光雷达扫描输入分为多个比较小的连续子帧，在每个子帧的时间范围内，传感器运动与匀速模型更加一致。

2.1 基于误差状态的迭代卡尔曼滤波ESIKF

基于公式（1）中表示的流形系统，我们使用基于误差状态的迭代卡尔曼滤波器（ESIKF）来估计系统状态。ESIKF的预测步骤包括状态预测和协方差传播，表示如下：

x ^ k + 1 = x ‾ k ⊞ ( Δ t f ( x ‾ k , 0 ) ) \widehat{\mathbf{x}}_{k+1}=\overline{\mathbf{x}}_{k} \boxplus \left(\Delta t \mathbf{f}\left(\overline{\mathbf{x}}_{k}, \mathbf{0}\right)\right) x k+1​=xk​⊞(Δtf(xk​,0))

P ^ k + 1 = F x ~ P ‾ k F x ~ T + F w Q F w T \widehat{\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} P k+1​=Fx~​Pk​Fx~T​+Fw​QFwT​

其中， P \mathbf{P} P 和 Q \mathbf{Q} Q 分别是状态估计和过程噪声 w \mathbf{w} w 的协方差矩阵。 F x ~ \mathbf{F}_{\tilde{\mathbf{x}}} Fx~​ 和 F w \mathbf{F}_{\mathbf{w}} Fw​ 分别表示如下：

F x ~ = ∂ ( ( x ‾ k ⊞ x ~ k ) ⊞ ( Δ t f ( x ‾ k ⊞ x ~ k , 0 ) ) ⊟ ( x ‾ k ⊞ Δ t f ( x ‾ k , 0 ) ) ) ∂ x ~ k = [ Exp ⁡ ( − ω ^ L k Δ t ) 0 3 × 3 0 3 × 3 I 3 × 3 Δ t 0 3 × 3 I 3 × 3 I 3 × 3 Δ t 0 3 × 3 0 3 × 3 0 3 × 3 I 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 I 3 × 3 ] \begin{aligned}\mathbf{F}_{\tilde{\mathbf{x}}} &=\frac{\partial\left(\left(\overline{\mathbf{x}}_{k} \boxplus \tilde{\mathbf{x}}_{k}\right) \boxplus\left(\Delta t \mathbf{f}\left(\overline{\mathbf{x}}_{k} \boxplus \tilde{\mathbf{x}}_{k}, \mathbf{0}\right)\right) \boxminus\left(\overline{\mathbf{x}}_{k} \boxplus \Delta t \mathbf{f}\left(\overline{\mathbf{x}}_{k}, \mathbf{0}\right)\right)\right)}{\partial \tilde{\mathbf{x}}_{k}} \\&=\left[\begin{array}{cccc}\operatorname{Exp}\left(-\widehat{\boldsymbol{\omega}}_{L_{k}} \Delta t\right) & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{I}_{3 \times 3} \Delta t \\\mathbf{0}_{3 \times 3} & \mathbf{I}_{3 \times 3} & \mathbf{I}_{3 \times 3} \Delta t & \mathbf{0}_{3 \times 3} \\\mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{I}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\\mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{I}_{3 \times 3}\end{array}\right]\end{aligned} Fx~​​=∂x~k​∂((xk​⊞x~k​)⊞(Δtf(xk​⊞x~k​,0))⊟(xk​⊞Δtf(xk​,0)))​=⎣ ⎡​Exp(−ω Lk​​Δt)03×3​03×3​03×3​​03×3​I3×3​03×3​03×3​​03×3​I3×3​ΔtI3×3​03×3​​I3×3​Δt03×3​03×3​I3×3​​⎦ ⎤​​

F w = ∂ ( x ‾ k ⊞ ( Δ t f ( x ‾ k , w k ) ) ⊟ ( x ‾ k ⊞ Δ t f ( x ‾ k , 0 ) ) ) ∂ w k = [ 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 I 3 × 3 Δ t 0 3 × 3 0 3 × 3 I 3 × 3 Δ t ] (5) \begin{aligned}\mathbf{F}_{\mathbf{w}} &=\frac{\partial\left(\overline{\mathbf{x}}_{k} \boxplus\left(\Delta t \mathbf{f}\left(\overline{\mathbf{x}}_{k}, \mathbf{w}_{k}\right)\right) \boxminus\left(\overline{\mathbf{x}}_{k} \boxplus \Delta t \mathbf{f}\left(\overline{\mathbf{x}}_{k}, \mathbf{0}\right)\right)\right)}{\partial \mathbf{w}_{k}} \\&=\left[\begin{array}{cc}\mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\\mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\\mathbf{I}_{3 \times 3} \Delta t & \mathbf{0}_{3 \times 3} \\\mathbf{0}_{3 \times 3} & \mathbf{I}_{3 \times 3} \Delta t\end{array}\right]\end{aligned}\tag{5} Fw​​=∂wk​∂(xk​⊞(Δtf(xk​,wk​))⊟(xk​⊞Δtf(xk​,0)))​=⎣ ⎡​03×3​03×3​I3×3​Δt03×3​​03×3​03×3​03×3​I3×3​Δt​⎦ ⎤​​(5)

其中， x ~ \tilde{\mathbf{x}} x~ 表示误差状态。

2.2 LiDAR 运动补偿

在我们考虑的问题中，IMU 数据和 LIDAR 数据是不同步的，因此采用 IMU 辅助运动补偿方法是不可行的。

在接收到时间戳 t k + 1 t_{k+1} tk+1​ 处的新激光雷达扫描之后，为了补偿运动失真，我们将在时间点 ρ j ∈ ( t k , t k + 1 ) \rho_{j} \in\left(t_{k}, t_{k+1}\right) ρj​∈(tk​,tk+1​) 采样的每个点 L j p j { }^{L_{j}} \mathbf{p}_{j} Lj​pj​ 投影到 LiDAR 扫描帧 L k + 1 L_{k+1} Lk+1​ 的最后时刻 t k + 1 t_{k+1} tk+1​。由于匀速模型，我们有： G v ^ L k + 1 = G v ‾ L k , ω ^ L k + 1 = ω ‾ L k { }^{G} \widehat{\mathbf{v}}_{L_{k+1}}={ }^{G} \overline{\mathbf{v}}_{L_{k}}, \widehat{\boldsymbol{\omega}}_{L_{k+1}}=\overline{\boldsymbol{\omega}}_{L_{k}} Gv Lk+1​​=GvLk​​,ω Lk+1​​=ωLk​​，也就是说从时间点 ρ i \rho_i ρi​ 到时间点 t k + 1 t_{k+1} tk+1​，每个点云的相对变换为： L k + 1 T L j = ( L k + 1 R ˇ L j , L k + 1 p ˇ L j ) L_{k+1} \mathbf{\mathbf { T }}_{L_{j}}=\left({ }^{L_{k+1}} \check{\mathbf{R}}_{L_{j}},{ }^{L_{k+1}} \check{\mathbf{p}}_{L_{j}}\right) Lk+1​TLj​​=(Lk+1​RˇLj​​,Lk+1​pˇ​Lj​​)，其中：

L k + 1 R ˇ L j = Exp ⁡ ( − ω ^ L k + 1 Δ t j ) L k + 1 p ˇ L j = − G R ^ L k + 1 T G v ^ L k + 1 Δ t j Δ t j = t k + 1 − ρ j (6) \begin{aligned}{}^{L_{k+1}} \check{\mathbf{R}}_{L_{j}} &=\operatorname{Exp}\left(-\widehat{\boldsymbol{\omega}}_{L_{k+1}} \Delta t_{j}\right) \\{}^{L_{k+1}} \check{\mathbf{p}}_{L_{j}} &=-{ }^{G} \widehat{\mathbf{R}}_{L_{k+1}} ^{T} {}^G \widehat{\mathbf{v}}_{L_{k+1}} \Delta t_{j} \\\Delta t_{j} &=t_{k+1}-\rho_{j}\end{aligned}\tag{6} Lk+1​RˇLj​​Lk+1​pˇ​Lj​​Δtj​​=Exp(−ω Lk+1​​Δtj​)=−GR Lk+1​T​Gv Lk+1​​Δtj​=tk+1​−ρj​​(6)

然后，可以局部测量的点 L j p j { }^{L_{j}} \mathbf{p}_{j} Lj​pj​ 就可以被投影到 LiDAR 扫描帧 L k + 1 L_{k+1} Lk+1​ 的最后时刻 t k + 1 t_{k+1} tk+1​，也就是：

L k + 1 p j = L k + 1 T ˇ L j L j p j (7) { }^{L_{k+1}} \mathbf{p}_{j}={}^{L_{k+1}} \check{\mathbf{T}}_{L_{j}}{ }^{L_{j}} \mathbf{p}_{j}\tag{7} Lk+1​pj​=Lk+1​TˇLj​​Lj​pj​(7)

然后，失真补偿后的扫描 { L k + 1 p j } \left\{{}^{L_{k+1}} \mathbf{p}_{j}\right\} {Lk+1​pj​} 提供了一个关于未知状态 G T L k + 1 { }^{G} \mathbf{T}_{L_{k+1}} GTLk+1​​ 的隐式测量，表示为点到面的距离残差。在此基础上，在迭代卡尔曼滤波框架中迭代估计整个状态 x k + 1 \mathbf{x}_{k+1} xk+1​，直到收敛。收敛后的状态估计表示为 x ‾ k + 1 \overline{\mathbf{x}}_{k+1} xk+1​，然后将用于传播后续的 IMU 测量。

3 LiDAR-惯性初始化

上面的 LiDAR 里程计在每次扫描结束的时候，即 t k t_k tk​ 时刻，输出 LiDAR 的角速度 ω L k \boldsymbol{\omega}_{L_{k}} ωLk​​ 和线速度 G v L k { }^{G} \mathbf{v}_{L_{k}} GvLk​​。同时，IMU提也供了原始测量值，即带有时间戳 τ i \tau_{i} τi​ 的角速度 ω m i \omega_{m_{i}} ωmi​​ 和线性加速度 a m i \mathbf{a}_{m_{i}} ami​​。这些数据被积累，并且通过激励准则反复被访问。一旦收集到足够激励的数据，就会调用初始化模块，该模块最终输出时间偏移 I t L ∈ R { }^{I} t_{L} \in \mathbb{R} ItL​∈R ，外参 I T L = ( I R L , I p L ) ∈ S E ( 3 ) { }^{I} \mathbf{T}_{L}=\left({ }^{I} \mathbf{R}_{L},{ }^{I} \mathbf{p}_{L}\right) \in SE(3) ITL​=(IRL​,IpL​)∈SE(3) ，IMU 偏置 b ω , b a ∈ R 3 \mathbf{b}_{\omega}, \mathbf{b}_{\mathbf{a}} \in \mathbb{R}^{3} bω​,ba​∈R3 和 G g ∈ R 3 { }^{G} \mathbf{g} \in \mathbb{R}^{3} Gg∈R3。

3.1 数据预处理

IMU原始测量结果受到噪声 n ω i \mathbf{n}_{\omega_{i}} nωi​​ 和 n a i \mathbf{n}_{\mathbf{a}_{i}} nai​​ 的影响。IMU测量模型为：

ω m i = ω i g t + b ω + n ω i , a m i = a i g t + b a + n a i (8) \boldsymbol{\omega}_{m_{i}}=\boldsymbol{\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}}\tag{8} ωmi​​=ωigt​+bω​+nωi​​,ami​​=aigt​+ba​+nai​​(8)

其中， ω i g t \boldsymbol{\omega}_{i}^{\mathrm{gt}} ωigt​ 和 a i g t \mathbf{a}_{i}^{\mathrm{gt}} aigt​ 是IMU角速度和线加速度的真值。同样，来自 LiDAR 里程计的估计 ω L k \boldsymbol{\omega}_{L_{k}} ωLk​​ 和 G v L k { }^{G} \mathbf{v}_{L_{k}} GvLk​​ 也包含噪声。

为了减轻这些高频噪音的影响，通常使用一种**非零相低通滤波器（a non-causal zero phase low-pass filter）**来过滤噪声，而无需引入任何滤波延迟。零相位滤波器是通过向后和向后运行Butterworth低通滤波器来实现，从而产生了噪声衰减的IMU测量 ω I i = ω i g t + b ω , a I i = a i g t + b a \boldsymbol{\omega}_{I_{i}}=\boldsymbol{\omega}_{i}^{\mathrm{gt}}+\mathbf{b}_{\omega}, \mathbf{a}_{I_{i}}=\mathbf{a}_{i}^{\mathrm{gt}}+\mathbf{b}_{\mathbf{a}} ωIi​​=ωigt​+bω​,aIi​​=aigt​+ba​。为了简洁，噪声衰减的 LiDAR 估计仍表示为 ω L k \boldsymbol{\omega}_{L_{k}} ωLk​​ 和 G v L k { }^{G} \mathbf{v}_{L_{k}} GvLk​​ 。

从 LiDAR 里程计的估计 ω L k \boldsymbol{\omega}_{L_{k}} ωLk​​ 和 G v L k { }^{G} \mathbf{v}_{L_{k}} GvLk​​ 中，我们通过非因果中心差分（non-causal central difference）的方法可以得到 LiDAR 的角加速度和线加速度 Ω L k , G a L k \boldsymbol{\Omega}_{L_{k}},{ }^{G} \mathbf{a}_{L_{k}} ΩLk​​,GaLk​​ 。

最终的 LiDAR 里程计数据可以用一个集合表示：

L k = { ω L k , G v L k , Ω L k , G a L k } (9) \mathcal{L}_{k}=\left\{\boldsymbol{\omega}_{L_{k}},{ }^{G} \mathbf{v}_{L_{k}}, \boldsymbol{\Omega}_{L_{k}},{ }^{G} \mathbf{a}_{L_{k}}\right\}\tag{9} Lk​={ωLk​​,GvLk​​,ΩLk​​,GaLk​​}(9)

类似的，我们从噪声衰减的陀螺仪测量 ω I i \boldsymbol{\omega}_{I_{i}} ωIi​​ 可以得到 IMU 的角加速度 Ω I i \boldsymbol{\Omega}_{I_{i}} ΩIi​​，也可以用集合表示：

I i = { ω I i , a I i , Ω I i } (10) \mathcal{I}_{i}=\left\{\boldsymbol{\omega}_{I_{i}}, \mathbf{a}_{I_{i}}, \boldsymbol{\Omega}_{I_{i}}\right\}\tag{10} Ii​={ωIi​​,aIi​​,ΩIi​​}(10)

由于 IMU 的频率通常高于 LiDAR 的频率，因此两个序列 I i \mathcal{I}_{i} Ii​ 和 L k \mathcal{L}_{k} Lk​ 的大小不相同。为了解决此问题，我们在同一时间段内提取 LIDAR 和 IMU 数据，并通过在每个LiDAR 里程计时刻 t k t_k tk​ 上进行线性插值来进行下采样（如上图）。下采样的 IMU 数据表示为 I k \mathcal{I}_{k} Ik​：

I k = { ω I k , a I k , Ω I k } (11) \mathcal{I}_{k}=\left\{\boldsymbol{\omega}_{I_{k}}, \mathbf{a}_{I_{k}}, \boldsymbol{\Omega}_{I_{k}}\right\}\tag{11} Ik​={ωIk​​,aIk​​,ΩIk​​}(11)

其具有与 L k \mathcal{L}_{k} Lk​ 相同的时间戳 t k t_k tk​ （但是数据实际上延迟了已知的时间常数 I t L { }^{I} t_{L} ItL​）。

3.2 通过互相关（Cross-Correlation）进行时间初始化

在大多数情况下，由于 LIDAR-惯性 里程计模块在接收之前不可避免地传输和处理延迟，这是LIDAR L k \mathcal{L}_{k} Lk​ 和IMU I k \mathcal{I}_{k} Ik​ 之间存在的未知但恒定的偏移 I t L { }^{I} t_{L} ItL​，因此如果将 IMU 测量 I k \mathcal{I}_{k} Ik​ 提前 I t L { }^{I} t_{L} ItL​，将与 LIDAR 里程计 L k \mathcal{L}_{k} Lk​ 对齐。由于 LIDAR 数据和 IMU 数据在离散时间 t k t_k tk​ 中，因此将 IMU 数据提前的操作基本上是在离散的步长 d = I t L / Δ t d = {}^{I}{t_{L}} / \Delta t d=ItL​/Δt 中进行的，其中 Δ t \Delta t Δt 是两次 LiDAR 扫描之间的时间间隔。具体而言，对于角速度，我们有：

ω I k + d = I R L ω L k + b ω (12) \boldsymbol{\omega}_{I_{k+d}}={ }^{I} \mathbf{R}_{L} \boldsymbol{\omega}_{L_{k}}+\mathbf{b}_{\omega}\tag{12} ωIk+d​​=IRL​ωLk​​+bω​(12)

由于陀螺仪偏置 b ω \mathbf{b}_{\omega} bω​ 通常很小，我们可以忽略不计。如果不管 R L \mathbf{R}_{L} RL​ ，我们发现 ω I k + d \boldsymbol{\omega}_{I_{k+d}} ωIk+d​​ 和 ω L k \boldsymbol{\omega}_{L_{k}} ωLk​​ 的大小应该一样。我们使用互相关来量化其数量之间的相似性。然后，可以通过以下优化问题中求解出 d d d ：

d ∗ = arg ⁡ max ⁡ d ∑ ∥ ω I k + d ∥ ⋅ ∥ ω L k ∥ (13) d^{*}=\underset{d}{\arg \max } \sum\left\|\boldsymbol{\omega}_{I_{k+d}}\right\| \cdot\left\|\boldsymbol{\omega}_{L_{k}}\right\|\tag{13} d∗=dargmax​∑∥ωIk+d​​∥⋅∥ωLk​​∥(13)

即通过在 L k \mathcal{L}_{k} Lk​ 的索引范围内枚举偏移 d d d。

3.3 统一的外参的旋转部分和时间标定

互相关方法对噪声和小尺度的陀螺仪偏置是鲁班的。但是（13）的一个明显缺陷是时间偏移的校准分辨率要大于 LiDAR 里程计的一个采样时间间隔 Δ t \Delta t Δt ，因此无法识别任何出小于 Δ t \Delta t Δt 的偏移残差 δ t \delta t δt。设 I t L { }^{I} t_{L} ItL​ 为 LiDAR 里程计 ω L \boldsymbol{\omega}_{L} ωL​ 和 IMU 数据 ω I \boldsymbol{\omega}_{I} ωI​ 之间的总偏移量，则有： I t L = d ∗ Δ t + δ t { }^{I} t_{L}=d^{*} \Delta t+\delta t ItL​=d∗Δt+δt。那么将 IMU 测量 ω I \boldsymbol{\omega}_{I} ωI​ 前移 I t L { }^{I} t_{L} ItL​ 与 LiDAR 里程计 ω L \boldsymbol{\omega}_{L} ωL​ 对齐，则公式（12）可以写成：

ω I ( t + I t L ) = I R L ω L ( t ) + b ω (14) \boldsymbol{\omega}_{I}\left(t+{ }^{I} t_{L}\right)={ }^{I} \mathbf{R}_{L} \boldsymbol{\omega}_{L}(t)+\mathbf{b}_{\omega}\tag{14} ωI​(t+ItL​)=IRL​ωL​(t)+bω​(14)

由于在公式（9）中， LiDAR 里程计 ω L \boldsymbol{\omega}_{L} ωL​ 的数据在 t k t_k tk​ 时刻得到，将 t = t k t=t_{k} t=tk​ 和 I t L = d ∗ Δ t + δ t { }^{I} t_{L}=d^{*} \Delta t+\delta t ItL​=d∗Δt+δt 代入到公式（14）中有：

ω I ( t k + d ∗ Δ t + δ t ) = I R L ω L k + b ω (15) \boldsymbol{\omega}_{I}\left(t_{k}+d^{*} \Delta t+\delta t\right)={ }^{I} \mathbf{R}_{L} \boldsymbol{\omega}_{L_{k}}+\mathbf{b}_{\omega}\tag{15} ωI​(tk​+d∗Δt+δt)=IRL​ωLk​​+bω​(15)

注意到 ω I ( t k + d ∗ Δ t + δ t ) \omega_{I}\left(t_{k}+d^{*} \Delta t+\delta t\right) ωI​(tk​+d∗Δt+δt) 是 IMU 在时间 t k + d ∗ Δ t t_{k}+d^{*} \Delta t tk​+d∗Δt 之后的角速度，其中在 t k + d ∗ Δ t t_{k}+d^{*} \Delta t tk​+d∗Δt 时刻的IMU 角速度和角加速度分别为： ω I ( t k + d ∗ Δ t ) = ω I k ′ \boldsymbol{\omega}_{I}\left(t_{k}+d^{*} \Delta t\right)=\boldsymbol{\omega}_{I_{k^{\prime}}} ωI​(tk​+d∗Δt)=ωIk′​​ 和 Ω I ( t k + d ∗ Δ t ) = Ω I k ′ \boldsymbol{\Omega}_{I}\left(t_{k}+d^{*} \Delta t\right)=\boldsymbol{\Omega}_{I_{k^{\prime}}} ΩI​(tk​+d∗Δt)=ΩIk′​​ （ k ′ = k + d ∗ k^{\prime}=k+d^{*} k′=k+d∗ 。

如上图所示，我们可以通过假设在 δ t \delta t δt 这一段非常小的时间段内角加速度是常量来插值得到 ω I ( t k + d ∗ Δ t + δ t ) \omega_{I}\left(t_{k}+d^{*} \Delta t+\delta t\right) ωI​(tk​+d∗Δt+δt)，即：

ω I ( t k + d ∗ Δ t + δ t ) ≈ ω I k ′ + δ t Ω I k ′ (16) \boldsymbol{\omega}_{I}\left(t_{k}+d^{*} \Delta t+\delta t\right) \approx \boldsymbol{\omega}_{I_{k^{\prime}}}+\delta t \boldsymbol{\Omega}_{I_{k^{\prime}}}\tag{16} ωI​(tk​+d∗Δt+δt)≈ωIk′​​+δtΩIk′​​(16)

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

ω I k ′ + δ t Ω I k ′ = I R L ω L k + b ω (17) \boldsymbol{\omega}_{I_{k^{\prime}}}+\delta t \boldsymbol{\Omega}_{I_{k^{\prime}}}={ }^{I} \mathbf{R}_{L} \boldsymbol{\omega}_{L_{k}}+\mathbf{b}_{\omega}\tag{17} ωIk′​​+δtΩIk′​​=IRL​ωLk​​+bω​(17)

最后，基于（17）中的约束，统一时空的优化问题可以表述为：

arg ⁡ min ⁡ I R L , b ω , δ t ∑ ∥ I R L ω L k + b ω − ω I k ′ − δ t ⋅ Ω I k ′ ∥ 2 (18) \underset{{ }^{I} \mathbf{R}_{L}, \mathbf{b}_{\omega}, \delta t}{\arg \min } \sum\left\|^{I} \mathbf{R}_{L} \boldsymbol{\omega}_{L_{k}}+\mathbf{b}_{\boldsymbol{\omega}}-\boldsymbol{\omega}_{I_{k^{\prime}}}-\delta t \cdot \boldsymbol{\Omega}_{I_{k^{\prime}}}\right\|^{2}\tag{18} IRL​,bω​,δtargmin​∑∥ ∥​IRL​ωLk​​+bω​−ωIk′​​−δt⋅ΩIk′​​∥ ∥​2(18)

由于存在约束 I R L ∈ S O ( 3 ) { }^{I} \mathbf{R}_{L} \in S O(3) IRL​∈SO(3)，该问题可以通过 Ceres Solver 进行迭代求解，初值设置为 ( I R L , b ω , δ t ) = ( I 3 × 3 , 0 3 × 1 , 0 ) \left({ }^{I} \mathbf{R}_{L}, \mathbf{b}_{\omega}, \delta t\right)=\left(\mathbf{I}_{3 \times 3}, \mathbf{0}_{3 \times 1}, 0\right) (IRL​,bω​,δt)=(I3×3​,03×1​,0)。

3.4 外参的平移部分和重力初始化

在上一节中，我们获得了外参的旋转部分 R L \mathbf{R}_{L} RL​，陀螺仪偏置 b ω \mathbf{b}_{\omega} bω​ 和时间偏移 I t L { }^{I} t_{L} ItL​ 。在本节中，我们固定这些值，然后进行外参的平移部分、重力向量和加速偏置的标定。

首先，我们使用之前标定好的时间偏移 d ∗ d^{*} d∗ 和偏移残差 δ t \delta t δt 将 IMU 数据 I k \mathcal{I}_{k} Ik​ 与 LIDAR 的数据 L k \mathcal{L}_{k} Lk​ 对齐。对齐的IMU数据表示为 I ‾ k \overline{\mathcal{I}}_{k} Ik​，现在假定它与 L k \mathcal{L}_{k} Lk​ 完全对齐而没有时间偏移。于是，对应于 t k t_k tk​ 时刻 LiDAR 角速度 ω L k \boldsymbol{\omega}_{L_k} ωLk​​ 的 IMU 角速度 ω ˉ I k \bar{\boldsymbol{\omega}}_{I_{k}} ωˉIk​​ 可以记为：

ω ‾ I k = ω I ( t k + d ∗ Δ t + δ t ) ≈ ω I k + d + δ t Ω I k + d \overline{\boldsymbol{\omega}}_{I_{k}}=\boldsymbol{\omega}_{I}\left(t_{k}+d^{*} \Delta t+\delta t\right) \approx \boldsymbol{\omega}_{I_{k+d}}+\delta t \boldsymbol{\Omega}_{I_{k+d}} ωIk​​=ωI​(tk​+d∗Δt+δt)≈ωIk+d​​+δtΩIk+d​​

类似的，对应于 t k t_k tk​ 时刻 LiDAR 线加速度 G a L k { }^{G} \mathbf{a}_{L_{k}} GaLk​​ 的 IMU 线加速度 a ‾ I k \overline{\boldsymbol{a}}_{I_{k}} aIk​​ 可以记为：

a ‾ I k = a I ( t k + d ∗ Δ t + δ t ) ≈ a I k + d + δ t Δ t ( a I k + d + 1 − a I k + d ) \begin{aligned}\overline{\mathbf{a}}_{I_{k}} &=\mathbf{a}_{I}\left(t_{k}+d^{*} \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{aligned} aIk​​​=aI​(tk​+d∗Δt+δt)≈aIk+d​​+Δtδt​(aIk+d+1​​−aIk+d​​)​

然后，类似于（14），我们可以找到 IMU 和 LiDAR 之间的加速度约束。在论文中提到，两个具有固定外参的坐标系 A 和 B 的加速度之间具有以下关系：

A R B a B = a A + ⌊ ω A ⌋ ∧ 2 A p B + ⌊ Ω A ⌋ ∧ A p B { }^{A} \mathbf{R}_{B} \mathbf{a}_{B}=\mathbf{a}_{A}+\left\lfloor\boldsymbol{\omega}_{A}\right\rfloor_{\wedge}^{2} {}^A \mathbf{p}_{B}+\left\lfloor\boldsymbol{\Omega}_{A}\right\rfloor{ }_{\wedge}{ }^{A} \mathbf{p}_{B} ARB​aB​=aA​+⌊ωA​⌋∧2​ApB​+⌊ΩA​⌋∧​ApB​

其中， A R B , A p B { }^{A} \mathbf{R}_{B},{ }^{A} \mathbf{p}_{B} ARB​,ApB​ 表示从坐标系 B 到坐标系 A 的外参变换。 a A \mathbf{a}_{A} aA​ 和 a B \mathbf{a}_{B} aB​ 都是他们各自坐标系下的加速度。

对于 LiDAR-惯性系统，我们有两种选择：用坐标系 A 表示 IMU 坐标系，用坐标系 B 表示 LIDAR 坐标系；或者相反。注意到在前一种情况下， ω A = ω ‾ I k − b ω \boldsymbol{\omega}_{A}=\overline{\boldsymbol{\omega}}_{I_{k}}-\mathbf{b}_{\omega} ωA​=ωIk​​−bω​ 的准确性受陀螺仪偏置估计的影响，并且角速度测量的噪声会放大 Ω A \boldsymbol{\Omega}_{A} ΩA​ 的误差。为了避免此问题和提高外参的平移部分标定的鲁棒性，我们将 LIDAR 坐标系设置为坐标系 A，将 IMU 坐标系设置为 坐标系 B。由于 LIDAR 的加速度 G a L k { }^{G} \mathbf{a}_{L_{k}} GaLk​​ 是在世界坐标系中进行表示的，我们需要计算 LiDAR 的即时加速度在 LiDAR 坐标系中的表示，记为 a L k \mathbf{a}_{L_{k}} aLk​​：

a L k = G R L T ( G a L k − G g ) \mathbf{a}_{L_{k}}={ }^{G} \mathbf{R}_{L}^{T}\left({ }^{G} \mathbf{a}_{L_{k}}-{ }^{G} \mathbf{g}\right) aLk​​=GRLT​(GaLk​​−Gg)

其中， G R L T { }^{G} \mathbf{R}_{L}^{T} GRLT​ 是 LiDAR 在世界坐标系中的姿态，已经在 LiDAR 里程计部分得到。

最后，可以通过以下优化问题共同估计外参的平移部分、加速度计偏置和重力向量：

arg ⁡ min ⁡ I p L , b a , G g ∑ ∥ I R L T ( a ‾ I k − b a ) − a L k − ( ⌊ ω L k ⌋ ∧ 2 + ⌊ Ω L k ⌋ ∧ ) L p I ∥ 2 \underset{{ }^{I} \mathbf{p}_{L}, \mathbf{b}_{\mathbf{a}},{ }^{G} \mathbf{g}}{\arg \min } \sum\left\|^{I} \mathbf{R}_{L}^{T}\left(\overline{\mathbf{a}}_{I_{k}}-\mathbf{b}_{\mathbf{a}}\right)-\mathbf{a}_{L_{k}}-\left(\left\lfloor\boldsymbol{\omega}_{L_{k}}\right\rfloor_{\wedge}^{2}+\left\lfloor\boldsymbol{\Omega}_{L_{k}}\right\rfloor_{\wedge}\right)^{L} \mathbf{p}_{I}\right\|^{2} IpL​,ba​,Ggargmin​∑∥ ∥​IRLT​(aIk​​−ba​)−aLk​​−(⌊ωLk​​⌋∧2​+⌊ΩLk​​⌋∧​)LpI​∥ ∥​2

由于存在约束 G g ∈ S 2 {}^ G \mathbf{g} \in \mathbb{S}_{2} Gg∈S2​，该问题可以通过 Ceres Solver 进行迭代求解，初值设置为 ( I p L , b a , G g ) = ( 0 3 × 1 , 0 3 × 1 , 9.81 e 3 ) \left({ }^{I} \mathbf{p}_{L}, \mathbf{b}_{\mathbf{a}},{ }^{G} \mathbf{g}\right)=\left(\mathbf{0}_{3 \times 1}, \mathbf{0}_{3 \times 1}, 9.81 \mathbf{e}_{3}\right) (IpL​,ba​,Gg)=(03×1​,03×1​,9.81e3​)。

L p I { }^{L} \mathbf{p}_{I} LpI​ 被估计出之后，可以计算得到从 LiDAR 到 IMU 的平移为 I p L = − I R L L p I { }^{I} \mathbf{p}_{L}=-{ }^{I} \mathbf{R}_{L}{ }^{L} \mathbf{p}_{I} IpL​=−IRL​LpI​。

3.5 数据积累评估

文中所提出的初始化方法依赖于 LiDAR-惯性装置的足够激励（即装置需要进行足够的运动）。因此，该系统应能够评估是否有足够的运动激励来执行初始化。

理想情况下，激励可以通过公式（18）对于 ( I R L , b ω , δ t ) \left({ }^{I} \mathbf{R}_{L}, \mathbf{b}_{\omega}, \delta t\right) (IRL​,bω​,δt) 和公式（19）对于 ( I p L , b a , G g ) \left({ }^{I} \mathbf{p}_{L}, \mathbf{b}_{\mathbf{a}},{ }^{G} \mathbf{g}\right) (IpL​,ba​,Gg) 的完整雅可比矩阵的秩来评估。

在实践中，我们发现只需要评估外参旋转部分和外参平移部分对应的雅可比就足够了，因为外参的激励往往需要复杂的运动，这些运动同样也足够激励其他状态。

使用 J r \mathbf{J}_{r} Jr​ 来表示外参旋转部分 I R L ^{I}\mathbf{R}_{L} IRL​ 对应的雅可比，使用 J t \mathbf{J}_{t} Jt​ 来表示外参平移部分 I p L ^{I}\mathbf{p}_{L} IpL​ 对应的雅可比：

J r = [ ⋮ − I R L ⌊ ω L k ⌋ ∧ ⋮ ] , J t = [ ⋮ ⌊ ω L k ⌋ ∧ 2 + ⌊ Ω L k ⌋ ∧ ⋮ ] \mathbf{J}_{r}=\left[\begin{array}{c}\vdots \\-{ }^{I} \mathbf{R}_{L}\left\lfloor\boldsymbol{\omega}_{L_{k}}\right\rfloor \wedge \\\vdots\end{array}\right], \mathbf{J}_{t}=\left[\begin{array}{c}\vdots \\\left\lfloor\boldsymbol{\omega}_{L_{k}}\right\rfloor{ }_{\wedge}^{2}+\left\lfloor\boldsymbol{\Omega}_{L_{k}}\right\rfloor \wedge \\\vdots\end{array}\right] Jr​=⎣ ⎡​⋮−IRL​⌊ωLk​​⌋∧⋮​⎦ ⎤​,Jt​=⎣ ⎡​⋮⌊ωLk​​⌋∧2​+⌊ΩLk​​⌋∧⋮​⎦ ⎤​

然后可以通过以下两个矩阵的秩来评估激励程度：

J r T J r = ∑ ⌊ ω L k ⌋ ∧ T ⌊ ω L k ⌋ ∧ \mathbf{J}_{r}^{T} \mathbf{J}_{r}=\sum\left\lfloor\boldsymbol{\omega}_{L_{k}}\right\rfloor_{\wedge}^{T}\left\lfloor\boldsymbol{\omega}_{L_{k}}\right\rfloor_{\wedge} JrT​Jr​=∑⌊ωLk​​⌋∧T​⌊ωLk​​⌋∧​

J t T J t = ∑ ( ⌊ ω L k ⌋ ∧ 2 + ⌊ Ω L k ⌋ ∧ ) T ( ⌊ ω L k ⌋ ∧ 2 + ⌊ Ω L k ⌋ ∧ ) \mathbf{J}_{t}^{T} \mathbf{J}_{t}=\sum\left(\left\lfloor\boldsymbol{\omega}_{L_{k}}\right\rfloor_{\wedge}^{2}+\left\lfloor\boldsymbol{\Omega}_{L_{k}}\right\rfloor _\wedge\right)^{T}\left(\left\lfloor\boldsymbol{\omega}_{L_{k}}\right\rfloor_\wedge ^{2}+\left\lfloor\boldsymbol{\Omega}_{L_{k}}\right\rfloor _\wedge\right) JtT​Jt​=∑(⌊ωLk​​⌋∧2​+⌊ΩLk​​⌋∧​)T(⌊ωLk​​⌋∧2​+⌊ΩLk​​⌋∧​)

更定量地说，激励程度由 J r T J r \mathbf{J}_{r}^{T} \mathbf{J}_{r} JrT​Jr​ 和 J t T J t \mathbf{J}_{t}^{T} \mathbf{J}_{t} JtT​Jt​ 的奇异值表示。

基于此原则，我们开发了一个评估程序，该程序可以指导用户如何移动其设备以获得足够的激励。我们通过雅可比矩阵的奇异值来量化激励，并设置一个阈值来评估激励是否足够。