雷达系列论文翻译（七）：Lio-mapping

Tightly Coupled 3D Lidar Inertial Odometry and Mapping

摘要

自我运动估计是大多数移动机器人应用的基本要求。通过传感器融合，我们可以弥补独立传感器的不足，并提供更可靠的估计。本文介绍了一种紧耦合的雷达-惯性测量单元融合方法。通过联合最小化激光雷达和惯性测量单元测量的代价函数，激光雷达-惯性测量单元里程计(LIO)可以在长期实验后以可接受的漂移误差表现良好，即使在激光雷达测量可能退化的挑战性情况下也是如此。此外，为了获得更可靠的激光雷达位姿估计，提出了旋转约束约束算法(LIO-mapping)来进一步将激光雷达位姿与全局地图对齐。实验结果表明，该方法即使在快速运动或特征不足的情况下，也能以较高的精度估计出惯性测量单元更新速率下的传感器对位姿。

引言

自我运动估计在许多导航任务中起着重要作用，是自主机器人的关键问题之一。它提供机器人位姿的知识，并能向位姿控制器提供实时反馈。此外，与感知环境的各种传感器一起，它为同时定位和建图(SLAM)提供了关键信息。对机器人位置的准确估计有助于降低风险和成功规划。

激光雷达传感器可以为周围环境提供距离测量，在机器人系统中得到了广泛应用。具体来说，典型的三维激光雷达可以在360度的水平视场(FOV)下以大约10Hz的频率感知周围环境。此外，作为主动传感器，它对光照具有不变性。高可靠性和高精度使激光雷达传感器成为位姿估计的普遍选择。

尽管激光雷达传感器有其优点，但它并不完美，有几个缺点。从激光雷达本身来看，它具有很低的垂直分辨率，并且它获得的稀疏点云提供了有限的特征，因此使得特征跟踪成为一个棘手的问题。此外，安装在移动机器人上的激光雷达存在运动畸变，这直接影响测量精度。在现实场景中，有些激光雷达退化的情况下，激光雷达接收到的信号点很少。例如，三维激光雷达在狭窄的走廊环境中只接收到很少的可用点。接收点主要来自侧壁，从天花板和地板只能观察到一小部分点。在这种情况下，匹配的激光雷达特征很容易导致欠约束的位姿估计。另一个缺点是它的低更新速度。这限制了它在需要快速响应的任务中的应用，例如机器人位姿的控制。

本文提出了一种紧耦合的三维激光雷达位姿估计算法来克服上述问题。激光雷达和惯性测量单元的测量结果用于联合优化。为了实现实时和更一致的估计，应用了固定滞后平滑和旧位姿的边缘化，随后是旋转约束细化。我们工作的主要贡献如下:

• 提出了一种紧耦合的激光雷达-惯性测量单元里程计算法。它提供实时准确的位姿估计和高更新频率。
• 给定激光雷达-惯性测量单元里程计的先验知识，旋转约束精化方法进一步优化最终的位姿和生成的点云地图。即使在某些激光雷达退化的情况下，它也能确保一致和鲁棒的估计。
• 该算法通过大量的室内和室外测试得到了验证。它优于最先进的仅激光雷达或松耦合激光雷达-惯性测量单元算法。
• 源代码可在网上获得。这是第一个面向社区的紧耦合激光雷达和惯性测量单元融合的开源实现。

论文的其余部分组织如下。第二部分回顾了相关的工作。第三部分解释了符号和一些预备知识。第四和第五部分介绍了所提出的里程计和精化方法。实现和测试如六和第七部分所示。结论在第八部分中给出。

相关工作

有多种方法与IMU和激光雷达测量的融合有关。一个重要的类别是松耦合融合。这类方法分别考虑激光雷达的估计和惯性测量组合的估计。在[1]中，带有惯性测量单元辅助的激光雷达里程计依赖于由惯性测量单元计算的旋转，并假设在使用加速度时速度为零。它将激光雷达和惯性测量单元的测量解耦，主要是将惯性测量单元作为整个系统的先验项，因此无法利用惯性测量单元的测量进行进一步优化。在[2]中，在2D的情况下，松耦合的扩展卡尔曼滤波器(EKF)被用来融合惯性测量单元和激光雷达，但它不能处理三维或更复杂的环境。Lynen等人[3]提出了一种模块化方法，用来融合惯性测量单元测量值和其他相对位姿传感器，比如相机、雷达甚至压力传感器，它主要基于三维情况下的扩展卡尔曼滤波器。这种松耦合的方法具有计算效率，但不如紧耦合的方法[4]精确，因为它将里程计部分作为一个黑盒，不会用惯性测量单元的测量值更新它。

紧耦合方法是另一个重要的类别。对于2D平面运动估计，Soloviev等人[5]提出了一种在二维激光雷达扫描中提取和匹配直线的方法，其中倾斜的激光雷达由惯性测量单元的预测方向进行补偿。在激光雷达测量到来时，采用卡尔曼滤波器对惯性测量单元的状态进行校正。Hemann等人[6]提出了一种以误差状态卡尔曼滤波器的形式来紧密紧耦合惯性测量单元传播和累积激光雷达高度图的方法。状态的校正是用雷达高度图和先验数字高程模型(DEM)之间的匹配来更新的。这种方法在已知环境的情况下显示了长期GPS拒绝导航的能力，但是如果没有事先的地图信息，这种方法就无法工作。在[7]和[8]中，直接来自惯性测量单元的原始测量值和来自连续轨迹的预测的惯性测量单元的测量值被用于计算待优化的残差。这些方法中不涉及状态的转换和估计，这使得即使有额外的相机，系统在快速运动下也不可行[9]。

受其他视觉惯性工作[10]，[11]的启发，我们设计了紧耦合激光雷达-惯性测量单元融合的方法。我们“预积分”并使用原始惯性测量单元测量值和激光雷达测量值来优化整个系统的状态，该系统可以在激光退化或运动快速的情况下工作。据我们所知，我们的算法是为数不多的适用于复杂3D环境的3D激光雷达-惯性测量单元融合算法之一。

符号和预备知识

A.符号

我们将三维激光雷达传感器捕获的每一条测量线表示为scan C\mathcal{C}，并将包含所有scan的单次测量表示为sweep S\mathcal{S}。例如，16线三维激光雷达在一次sweep中包含16次scan。

在下面的部分中，我们定义变换矩阵Tba∈SE(3)T_b^a \in SE(3)，它将坐标系Fb\mathcal{F}_b中的点xb inR3x^b \ in \mathbb{R}^{3转换到坐标系Fa\mathcal{F}_a。IMU预测的̄变换表示为Tˉba\bar{T}_b}a。Rba∈SO(3)R_b^a \in SO(3)和pba∈R3p_b^a \in \mathbb{R}^{3分别是变换矩阵TbaT_b}a的旋转矩阵和平移向量。使用Hamilton四元数表示法的四元数qbaq_b^{a对应与RbaR_b}a。⊗\otimes用于表示两个四元数相乘。我们用a^{k\hat{a}_k和ω}k\hat{\omega}_k来表示惯性测量单元在时间点kk的原始测量值。提取的特征在原始捕获坐标系Fa\mathcal{F}_a中被表示为FaF_a，它可以被转换到坐标系Fb\mathcal{F}_b中并表示为FabF_a^b。

B.IMU动力学

1）状态

机体坐标系FBi\mathcal{F}{B_i}和FLi\mathcal{F}{L_i}分别在离散时间ii时获取雷达sweep Si\mathcal{S}_i对应的惯性测量单元的参考和激光雷达中心的参考。

我们将估计的状态是世界坐标系FW\mathcal{F}W中的惯性测量单元状态XBiWX{B_i}^{W和激光雷达与惯性测量单元传感器之间的外参TBLT_B}L。具体来说，我们可以将ii时刻的惯性测量单元状态和外参写为
XBiW=[pBiWTvBiWTqBiWTbaiTbgiWT]TTBL=[pBLTqBLT]T(1) X_{B_i}^W = ^T \ T_B^L = ^T \tag{1}
其中，pBiW,vBiW,qBiW{p_{B_i}^W} , {v_{B_i}^{W}}, {q_{B_i}^W}分别是机体坐标系相对于世界坐标系的位置、速度和方向，bab_a是惯性测量单元加速度零偏，bgb_g是陀螺仪零偏。

2）动力学模型

利用惯性测量单元的加速度计和陀螺仪的输入，我们可以通过离散积分来更新先前的惯性测量单元的状态XBiWX_{B_i}^{W到当前的惯性测量单元状态XBjWX_{B_j}}W，如公式(2)所示，其中Δt\Delta t是两个连续的惯性测量单元测量之间的时间间隔，激光雷达sweep在k=ik=i和k=jk = j之间的所有惯性测量单元测量的时间被积分。稍微滥用一下符号，我们使用k=j−1k = j -1作为k=jk = j前面的IMU时间戳。
pj=pi+∑k=ij−1[vkΔt+12gWΔt2+12Rk(a^{k−bak)Δt2]vj=vi+gWΔtij+∑k=1j−1Rk(a}k−bak)Δkqj=qi⊗∏k=1j−1δqk=qi⊗∏k=1j−112Δk(ω^k−bgk)1 p_j = p i + \sum{k=i}^{j-1}{[v_k \Delta t + \frac{1}{2}g^W \Delta t^2 + \frac{1}{2} R_k(\hat{a}k - b{a_k})\Delta t ^2]} \ v_j = v_i + g^W \Delta t_{ij} + \sum_{k=1}^{j-1}{R_k(\hat{a}k - b{a_k})\Delta k} \ q_j = q_i \otimes \prod_{k=1}^{j-1}{\delta q_k} = q_i \otimes \prod_{k=1}^{j-1}{} \tag{2}
其中gWg^W是世界坐标系中的重力矢量。为了清楚起见，我们使用了(⋅)i=(⋅)BiW(\cdot)i = (\cdot){B_i}^{W，∑k=1j−1Δt\sum_{k=1}}{j-1}\Delta t和∏k=1j−1\prod_{k=1}^{j-1}来表示四元数乘法序列。

3）预积分

时刻ii和时刻jj之间的机体运动可以通过预积分测量值zji=Δpij,Δvij,Δqijz_j^i = {\Delta p_ij, \Delta v_{ij},\Delta q_{ij}}来表示，它在误差状态模型下具有协方差CBjBiC_{B_j}^{B_i}(参见补充资料[12]中的详细信息)。

紧耦合的雷达-惯性测量单元里程计

为了确保有效的估计，许多激光雷达建图工作，如[7]，[1]和[13]，将任务分为两部分，里程计和建图。受这些工作的启发，本文所提出的系统包括两个平行的部分。第一部分是紧耦合的雷达-惯性测量单元里程计，它在一个局部窗口内优化所有位姿。第二部分是旋转约束细化(导致全局一致的建图过程)，它使用来自优化后的位姿和重力约束的信息将激光雷达sweep与全局地图对齐。

A.雷达-惯性测量单元里程计概述

图1简要概述了我们提出的雷达-惯性测量单元里程计。

有了之前估计的状态，我们可以使用从上一个时间点ii到当前时间点jj的激光雷达原始输入Sj\mathcal{S}j和惯性测量单元原始输入IijI{ij}，以获得状态优化的新步骤。里程计估计的执行步骤如下:1)在Sj\mathcal{S}j到达之前，通过等式(2)迭代更新惯性测量单元状态。2)同时，这些输入被预积分为用于联合优化的Δpij\Delta p{ij}，Δvij\Delta v_{ij}和Δqij\Delta q_{ij}。3)当接收到最新的激光雷达sweep Sj\mathcal{S}j时，对原始数据应用去畸变以获得去畸变的激光雷达sweep Sj\mathcal{S}j。4)接下来，应用特征提取步骤来降低数据的维数并提取最重要的特征点FLjF{L_j}。四-B)。5)根据先前相应的优化状态TBo,iWT{B_{o,i}}^{W}和TBLT_BL，局部窗口中的先前激光雷达特征点FLo,iF_{L_{o,i}}被合并为局部地图MLo,iLpM_{L_{o,i}}^{L_p}。6)利用预测的激光雷达位姿FjF_j，我们可以找到相对的激光雷达测量mLp+1,jm_{L{p+1,j}}。7)最后一步是联合非线性优化相对雷达测量和惯性测量单元的预积分，以获得局部窗口内状态的MAP估计。优化的结果被应用于更新步骤1中的预测状态，避免来自惯性测量单元传播导致的漂移。

B.点云去即便和特征提取

三维激光雷达内部有旋转机构，可以接收一整圈的数据。当三维激光雷达移动时，来自它的原始数据Sj\mathcal{S}j遭受运动畸变，这使得sweep中的点不同于真实位置。为了处理这个问题，我们使用来自IMU传播预测的激光雷达运动TˉLj′Lj\bar{T}{L_j^{'}}{L_j}，并假设扫描期间的线性运动模型。然后，通过̄对TˉLj′Lj\bar{T}{L_j^{'}}{L_j}的线性插值校正每个x(t)∈Sj⊂R3x(t) \in S_j \subset \mathbb{R}^3获得在结束位姿坐标系下表示的去畸变后的点云，其中t∈(tj′,tj]t \in (t{j^{{'}},t_j]是sweep中的点的时间戳，并且tj′t_{j}{'}}和tjt_j分别是sweep开始和结束时的时间戳。

为了提高计算效率，需要提取激光雷达特征。这里，我们只对最可能在平面或边缘上的点感兴趣，因为这些点可以从激光雷达sweep的每次scan中提取出来。这种特征点在̄Sˉj\bar{\mathcal{S}}j中的点FLjF{L_j}由曲率和距离改变来选择，如[1]中所述；即选择最像平面或者边缘特征上的点。

C.相对雷达测量

通过惯性测量单元和另一个能够提供传感器对相对位姿的传感器的融合，如果我们固定第一个参考坐标系[14]，待估计的状态XBWX_B^W和TBLT_BL将是局部可观测的。为了适当地包含来自惯性测量单元的预积分，我们建议在sweep之间使用相对激光雷达测量来约束激光雷达位姿，如算法1所示。

在寻找点的对应关系之前，我们构建一个局部地图，因为单次sweep中的点密度不足以计算精确的点的对应关系。

局部地图包含来自NmN_m个离散时间戳o,...,p,...,jo,...,p,...,j的激光雷达特征点，其中oo，pp和ii分别代表了窗口内第一次激光雷达sweep、哨兵雷达sweep和最后处理的激光雷达sweep的时间戳，如图2所示。

局部地图MLo,iLpM_{L_{o,i}}^{{L_p}是根据特征FLγLpF_{L_{\gamma}}}{L_p}在哨兵雷达sweep的坐标系中构建的，它通过先前优化的激光雷达位姿TLγLpT_{L_{\gamma}}^{L_p}进行转换。待估计的状态是NsN_s个在时间点{p+1,...,i,j}{p+1,...,i,j}的状态，其中p+1p+1和jj在窗口中的哨兵的下一个和当前激光雷达sweep对应的时间戳。

通过建立的局部地图，可以找到MLo,iM_{L_{o,i}}和原始的FLα,α∈{p+1,...,j}F_{L_{\alpha}},\alpha \in {p+1,...,j}之间的对应关系。我们定义这种对应关系为相对激光雷达测量，因为它们是相对于哨兵位姿的，并且哨兵位姿将随着滑动窗口而改变。原始特征是FLαF_{L_{\alpha}}中最平坦的点或边缘点。在实践中，我们发现边缘点不能改善激光雷达-惯性测量单元里程计的结果。因此，在下文中，我们只讨论平面特征。KNN被用于对每个变换后的特征点xLpx^{{L_p}在MLo,iLpM_{L_{o,i}}}{L_p}中找到KK个最邻的点π(xLp)\pi(x^{{L_p})。然后对于平面点，我们将这些相邻点拟合到一个平面Fp\mathcal{F}_p中。平面点的系数可以用ωTx′+d=0,x′∈π(xLp)\omega}Tx^{'}+d = 0,x^{'} \in \pi(x^{L_p})定义的线性方程求解，其中ω\omega是平面法线方向，dd是到平面FLp\mathcal{F}{L_p}原点的距离。我们将每个平面特征点x∈FLαx \in F{L_\alpha}对应的m=[x,ω,d]∈mLαm =[x,\omega,d] \in m_{L_{\alpha}}作为相对雷达测量之一。需要提及的是，在每个相对雷达测量m∈mLαm \in m_{L_{\alpha}}中，xx定义在Fα\mathcal{F}{\alpha}中，ω\omega和dd定义在FLp\mathcal{F}{L_p}中。

D.雷达扫掠配准

相对激光雷达测量可以提供哨兵位姿和之后激光雷达位姿之间的相对约束。我们的方法优化了优化窗口中的所有位姿，包括第一个位姿TLpWT_{L_p}^{W，即FLp\mathcal{F}_{L_p}不固定。因此，激光雷达代价函数中的每一项都包含两个激光雷达sweep的位姿，即TLpwT_{L_p}}w和TLαW,α∈{p+1,...,j}T_{L_\alpha}^W,\alpha \in {p+1,...,j}。优化哨兵位姿将有助于更好地最小化预积分误差，并确保传感器对与重力对齐。我们估计的状态是惯性测量单元的状态；因此，我们需要引入外参来表示在惯性测量单元坐标系下的雷达约束。从之后的激光雷达位姿到窗口中的哨兵位姿的相对变换可以定义为
TLαLp=TBLTBpW−1TBαWTBL−1=RLαLppLαLp01 T_{L_\alpha}^{L_p} = T_B^L{T_{B_p}W}^{{-1}T_{B_\alpha}}W {T_B^L}{-1} = \tag{3}

根据前面的对应关系，每个相对激光雷达测量m=[x，ω，d]∈mLα，α∈{p+1，，j}m= [x，ω，d]∈m_{L_α}，α∈ {p+1，，j }的残差可以表示为点到平面的距离
rL(m,TLpW,TLαW,TBL)=ωT(RLαLpx+pLαLp)+d(4) r_{\mathcal{L}}(m,T_{L_p}^{W,T_{L_\alpha}}W,T_B^L) = \omega^{T(R_{L_\alpha}}{L_p}x + p_{L_\alpha}^{L_p}) + d \tag{4}

E.优化

为了获得最优状态，应用了固定滞后平滑器和边缘化。固定滞后平滑器在滑动窗口中保持NsN_s个IMU状态，如图2所示。滑动窗口有助于限制计算量。当新的测量约束出现时，平滑器将包含新的状态，并在窗口中对最老的状态进行边缘化处理。整个窗口中的状态将被估计
X=XBpW,...,XBjW,TBL X = [X_{B_p}^{W,...,X_{B_j}}W,T_B^L] \tag{5}
然后用马氏范数最小化下面的代价函数以获得状态XX的MAP估计，
min⁡x12{∣∣rP(X)∣∣2+∑m∈mLαα∈{p+1,...,j}∣∣rL(m,X)∣∣CLαm2+∑β∈{p,...,j−1}∣∣rB(zβ+1β,X)∣∣CBβ+1Bβ2}(6) \min_x \frac{1}{2}{ { ||r_{\mathcal{P}}(X)||^2 + \sum_{m \in m_{L_{\alpha}} \alpha \in {p+1,...,j}} { || r_{\mathcal{L}}(m,X) ||^{2_{C_{L_\alpha}}m} } + \sum_{\beta \in { p,...,j-1 }}{|| r_{\mathcal{B}}(z_{\beta +1}^{\beta},X) ||^2_{ C_{B_{\beta + 1}}^{B_\beta} } } } } \tag{6}
其中rP(X)r_{\mathcal{P}}(X)是边缘化后的先验约束，rL(m,X)r_{\mathcal{L}}(m,X)是相对激光雷达约束的残差，rB(zβ+1β)r_{\mathcal{B}}(z_{\beta + 1}^{\beta})是惯性测量单元约束的残差。非线性最小二乘形式的代价函数可以通过高斯-牛顿算法求解，该算法采用公式Hδx=−bH \delta x = -b。

我们使用Ceres Solver [15]来求解该问题。每一个相对激光雷达的约束rL(m,X)r_{\mathcal{L}}(m,X)可以从等式(4)中导出。协方差矩阵CLαmC_{L_\alpha}^m由激光雷达精度决定。与[10]中的一个类似，IMU约束rB(zβ+1β)r_{\mathcal{B}}(z_{\beta +1}^{\beta})可以从状态和IMU预积分中获得，∣∣rP(X)∣∣2=bPTHP+bP|| r_{\mathcal{P}}(X) ||^2 = b_{\mathcal{P}}^T H_{\mathcal{P}}^{+} b_{\mathcal{P}}可以通过Schur补获得(详见[12])。

用旋转约束细化位姿

将特征点配准到全局地图而不是局部地图，可以将激光雷达位姿约束到一致的世界坐标系FW\mathcal{F}^W。我们的细化方法使用了相对激光雷达测量。由于全局地图是细化的副产品，我们也称它为建图方法。将最新激光雷达特征点与全局地图对齐的代价函数可以由以下方程给出
CM=∑m∈mL∣∣rM(m,TLW)∣∣2rM(m,T)=ωT(Rx+p)+d(7) C_{\mathcal{M}} = \sum_{m \in m_L}{|| r_{\mathcal{M}}(m,T_L^W) ||^2} \ r_{\mathcal{M}}(m,T) = \omega^T (Rx + p) + d \tag{7}
其中T=TLWT = T_L^{W是最新的待估计激光雷达位姿，并且mm是具有FL\mathcal{F}_L中特征点xx和在FW\mathcal{F}_W中系数ω,d\omega,d的相对激光雷达测量值。然后我们可以使用类似的高斯-牛顿法来最小化CMC_{\mathcal{M}}。优化由残差CMC_{\mathcal{M}}雅各比矩阵JpCJ_p}C和JθCJ^C_{\theta}执行，其中θ\theta是相应四元数qq的误差状态。然而，随着旋转误差的累积，经过长期的操作，合并后的全局地图无法与重力精确对齐。这会导致进一步建图错误地与倾斜的地图对齐。受到[16]的启发，它利用SE(2)SE(2)约束优化SE(3)SE(3)，提出了一种约束建图策略。该策略利用激光雷达-惯性测量单元里程计的旋转约束，确保最终地图始终与重力对齐。图3示出了旋转约束建图的结构。

给定沿zz轴的方向具有更高的不确定性的，并且另外两个自由度更接近真实值的属性，我们可以通过修改旋转的雅可比来训练代价函数，如(在[12]中的详细推导)，
JθzC=JθC(⋅R˘)T⋅Ω˘zΩ˘z=ϵx000ϵy0001 J_{\theta_z}^C = J_\theta^C (\cdot \breve{R})^T \cdot \breve{\Omega}_z \ \breve{\Omega}_z = \tag{8}
其中(⋅˘)(\breve{\cdot})表示在最后一次迭代中的状态估计，而Ω˘z\breve{\Omega}_z表示相对于FW\mathcal{F}_W旋转的信息矩阵的近似。ϵx\epsilon_x和ϵy\epsilon_y可以通过xx轴和yy轴旋转相对于zz轴旋转的信比例来获得。

之后，我们使用JpCJ_p^{C和JθzCJ_{\theta_z}}C来代替雅可比，这些是优化步骤所需要的。增量激光雷达位姿可以获得为p_{\tilde{p}和q}\tilde{q}，这导致更新的激光雷达状态p_{\tilde{p}和q}\tilde{q}
p_=p˘+δpq=[12δθz1]⊗q˘(9) \tilde{p} = \breve{p} + \delta p \ \tilde{q} = \otimes \breve{q} \tag{9}

实现

本节介绍不同的传感器配置、系统初始化以及室内和室外测试的不同参数。

A.不同的传感器配置

传感器对可以有不同的配置。对于手持传感器对，如图4a所示，激光雷达和惯性测量单元彼此靠近。

因此，工作流程与之前引入的工作流程相同。但是对于安装在汽车上的传感器对，这两个传感器通常离得比较远。例如，图4b显示了安装在汽车基础连杆上方的惯性测量单元，而激光雷达安装在汽车前部。不是自动校准所有平移参数，而是将平移外参的先验添加到等式中(6)用于进行汽车的测试。

B.初始化

首先，没有激光雷达特征点的估计位姿。因此，需要大致精确的匹配算法。在初始化步骤中，我们采用了[1]的激光雷达里程计。在提供激光雷达位姿的情况下，需要传感器对有足够的运动来使惯性测量单元状态可观[14]。然后，激光雷达和惯性测量单元测量的位姿被用于初始化惯性测量单元状态，这可以通过[10]和[17]中介绍的方法来解决。为了测试，我们遵循[10]中的初始化方法，该方法也线性初始化外参。接下来，用初始化后的初始状态和新到来的测量值，局部窗口内的非线性优化将被迭代执行以估计状态。

测试和分析

进行室内和室外试验来评估我们的方法。定量和定性结果在以下章节中提供。

A.定量分析

为了定量分析我们的方法，使用了图4a所示的传感器对。Velodyne VLP-16线激光雷达有16条线，安装在Xsens MTi-100惯性测量单元上方。反射标记可以被运动捕捉系统用于提供真实的位姿。激光雷达配置为10Hz更新速率，惯性测量单元以400Hz更新。使用[18]将来自不同方法的估计轨迹与地面真值对齐。

1）不同运动条件下的测试:

表1给出了不同运动速度和不同方法下的均方根误差(RMSE)，其中LOAM[1]被视为评价基准。LIO是我们的局部窗口优化里程计方法。LIO-raw和LIO-no-ex与LIO相同，只是分别切断了运动补偿或在线外参估计。LIO-mapping来自带有旋转约束的建图结果。两个最佳结果以粗体显示。

从结果中，我们看到LIO-mapping总是能够在所有的情况下提供平移(位置)和旋转(方向)状态的精确估计。当运动更快时，LIO有更好的表现，这产生了更多的IMU激励。但是如果运动很弱，它会受到漂移的影响，因为此时局部地图相对稀疏。该表还显示，通过运动补偿和在线外参估计，LIO可以提供更好的性能，尤其是当运动很快时。

2）漂移随时间变化的测试

为了评估误差如何随时间变化，我们在更长的测试中测试算法。前50个估计位姿与地面真值对齐。不同方法的最终轨迹如图5所示，平移和旋转误差如图6所示。

结果表明，LIO可以提供相对准确的姿态，并能约束俯仰和滚转角接近地面真值，但它受到漂移的影响。当运动变得迅速时(在测试的后半部分)，没有惯性测量单元融合的方法(LOAM)和松耦合融合的方法(LOAM+IMU)都不能提供鲁棒的估计。LIO-mapping得益于LIO提供的旋转约束，并进一步将当前sweep配准到全局地图中。因此，它导致轨迹的较小漂移和状态估计结果更好的一致性。

B.定性结果

为了展示在具有挑战性场景中的性能提升，进行了几项具有不同传感器配置和环境的测试，包括室内手持和室外校园高尔夫球车测试，配置如图4所示，以及在KAIST城市数据集上的测试[19]。由于空间有限，这些位姿估计和建图结果显示在补充视频中。

C.时间分析

我们使用4.20GHz、16GB内存的英特尔i7-7700K处理器了该测试。激光雷达间隔在室内(0.2s)和室外(0.3s)有所不同，这取决于扫sweep中的特征点数量(室外的特征点通常为3000个左右，而室内为1000个)。这些间隔有助于建立更大的地图和跳过一些激光雷达sweep，以实现实时计算。使用16线三维激光雷达的数据，我们的方法的平均时间可以在表二中找到。

时间代表不同模块对于每个新输入的处理时间，即原始惯性测量单元测量值、激光雷达测量值和里程计输出。请注意，里程计和建图模块在不同的线程中。建图线程处理里程计线程的输出。惯性测量单元的预测是在优化状态的基础上进行的。因此，它可以和惯性测量单元的输出速率一样快。

结论

本文提出了一种新的激光雷达-惯性测量单元紧耦合融合方法。它包括里程计的状态优化和带旋转约束的细化。结果表明，我们的方法优于现有的仅激光雷达方法和松耦合融合方法。

尽管所提出的方法需要初始化，但我们的方法确实以快速的更新速率显示了鲁棒的位姿估计结果，即使在具有挑战性的测试场景下，例如快速运动的情况、激光雷达退化的情况和激光雷达sweep具有有限的重叠，并由足够的惯性测量单元激励所驱动的情况。