（每日一读2019.10.21）紧耦合的3D雷达惯性里程计及建图（LIO）

论文：pdf
代码：github
视频：video

摘要

自身运动估计是大部分移动机器人应用的基本需求。通过传感器融合，我们能够补偿单个传感器的缺陷并且提供更可靠的估计。在本文中，我们引入一种紧耦合的雷达-惯导融合的方法。通过联合最小化雷达和惯导的测量损失，雷达-惯导里程计(LIO)在长期的试验后，在可接受的漂移，甚至是在激光雷达测量会降级的具有挑战性的情况下表现不错。此外，为了获取雷达位姿的更可靠的估计，我们还提出了一种旋转约束精炼算法(LIO-mapping)来进一步对齐雷达位姿和和全局地图。

I 介绍

自身运动估计在许多导航任务中起着重要作用，是自主机器人的关键问题之一。它提供了机器人姿态的认知，并能向姿态控制器提供即时反馈。此外，它与感知环境的各种传感器一起，为同时定位和映射（slam）提供了关键信息。准确估计机器人的姿态有助于降低风险，并有助于成功规划。激光雷达传感器可以为周围环境提供距离测量，在机器人系统中得到了广泛的应用。具体来说，典型的三维激光雷达可以在360度的水平视场（FOV）下以10Hz左右的频率来感知周围环境。作为一种主动传感器，它对光照具有不变性。激光雷达传感器具有高可靠性和高精度，我们很多人都会使用这种传感器。
尽管激光雷达传感器具有许多优点，但它还不够完善，存在一些缺点。

• 从激光雷达本身来看，它的垂直分辨率较低，得到的稀疏点云提供的特征有限，使得特征跟踪成为一个棘手的问题（跟踪算法）。
• 安装在移动机器人上的激光雷达也会受到运动畸变的影响，（需要去除运动畸变算法）这直接影响到传感精度。
• 在实际场景中，存在一些激光雷达退化（获得的数据无效）的情况，其中激光雷达接收的点很少或丢失。例如，在狭窄的走廊环境中，3d lidar只能接收很少的可用点。接收点主要来自侧墙，只有一小部分是从天花板和地板观察到的。在这种情况下，匹配的激光雷达特征很容易导致非约束姿态估计。
• 更新率低。这限制了它在需要快速响应的任务中的应用，例如机器人姿态的控制。

本文针对上述问题，提出了一种紧耦合的三维激光雷达imu姿态估计算法。激光雷达和惯性测量单元的测量结果用于联合优化。为了实现实时和更一致的估计，采用固定滞后平滑和边缘化旧的姿态，然后进行旋转约束优化。我们工作的主要贡献如下：

• 提出了一种紧密耦合的激光雷达里程计算法。它提供了实时准确的状态估计和高更新率。
• 考虑到来自激光雷达IMU里程计的先验信息，旋转约束优化方法进一步优化了最终姿势和生成的点云地图。即使在某些激光雷达退化的情况下，它也能确保一致和稳健的估计。
• 通过广泛的室内和室外测试验证了该算法。它的性能优于目前最先进的纯激光雷达或松耦合雷达+imu算法。
• 在线提供源代码1。这是第一个可供社区使用的用于紧密耦合的lidar和imu融合的开源实现。

II 相关研究

有几种方法与imu和lidar测量的融合有关。一个重要的范畴是松耦合融合。这类方法分别考虑了激光雷达的估计和惯性测量单元的估计。在文献[1]中，利用imu辅助的lidar里程计依赖于imu计算的方向，并且在使用加速度时假设速度为零。它将激光雷达和惯性测量单元的测量数据解耦，主要以惯性测量单元为先验，无法利用惯性测量单元进一步优化。在文献[2]中，采用松耦合扩展卡尔曼滤波器（ekf）融合二维情况下的imu和lidar，但它不能处理三维或更复杂的环境。Lynen等人[3]提出了一种模块化的方法，在三维情况下，通过ekf将imu测量与其他相对姿态测量（例如，从相机、激光雷达甚至压力传感器）融合。这种松散耦合的方法具有计算效率，但是比紧耦合方法[4]更不精确，因为它将里程计部分作为一个黑盒，并且不使用来自IMU的测量更新它。紧耦合方法是另一个重要的范畴。对于二维平面运动估计，Soloviev等人[5]提出了一种在二维激光雷达扫描中提取和匹配直线的方法，其中倾斜的激光雷达由IMU的预测方向进行补偿。在激光雷达测量领域中，采用卡尔曼滤波对imu状态进行校正。Hemann等人[6]提出了一种以误差状态卡尔曼滤波器的形式将IMU传播与积累的激光雷达高度图紧密耦合的方法。利用激光雷达高度图与先验数字高程模型（dem）的匹配更新状态修正。该方法在环境已知的情况下，显示了远距离gps拒绝导航的能力，但没有先验的地图信息，该方法无法正常工作。在[7]和[8]中，直接从惯性测量单元得到的原始测量值和从连续轨迹得到的预测惯性测量值用于计算待优化的残差。这些方法不涉及状态的转换和估计，这使得即使使用额外的摄像机，系统在快速运动下也不可行[9]。受其他视觉惯性工作[10]、[11]的启发，我们设计了一种紧密耦合的激光雷达imu融合方法。我们“预集成”并使用原始IMU测量和激光雷达测量来优化整个系统的状态，这可以在激光退化情况下或在运动迅速时工作。据我们所知，我们是少数几个适合复杂三维环境的三维激光雷达imu融合算法之一。

III 符号注释

载体运动过程中从第i帧到第j帧的预积分。

IV紧耦合激光雷达里程计

为了确保有效的估计，许多关于lidar建图的工作，如[7]、[1]和[13]，将任务分为两部分，里程计和建图。受这些工作的启发，提出的系统由两个并行部分组成。第一部分是紧密耦合的激光雷达IMU里程计，它优化了一个局部窗口内的所有状态。第二部分是旋转约束优化（导致全局一致的建图过程），它使用来自优化姿势和重力约束的信息将激光雷达扫描与全局地图对齐。

A 雷达-IMU 里程计框图

图1提供了雷达-IMU里程计的简要概述。

• （1）我们可以基于之前的数据的状态估计，将lidar的原始数据$S_j$和IMU的原始输入i和j（从上一个时间戳i到当前时间戳j）来进行新的状态优化。
  在$S_j$到达之前IMU状态通过公式2进行更新。
• （2）同时这些输入被预积分用于联合优化。
• （3）当收到最新的激光雷达扫描$S_j$时先对原始数据进行去除运动畸变处理
• （4）应用特征提取来降低数据的维度（这里不清楚具体如何提取来降低维度）并提取最重要的特征点$F_{L_j}$
• （5）根据先前相应的优化状态，局部窗口内之前的lidar特征点$F_{L_{o,i}}$被边缘化(marged)为局部地图$M_{L_{o,i}}^{L_p}$
• （6）利用预测的$F_j$激光雷达姿态，我们可以找到相对激光雷达测量$m_{L_{p+1,j}}$
• （7）最后一步是联合非线性优化，采用相对激光雷达测量和IMU预积分，获得局部窗口内状态的MAP估计。将第一步中的优化结果应用于更新预测状态，以避免imu漂移。

B 去倾斜和特征提取

三维激光雷达内部有旋转机构，可接收一整周的数据。当3d激光雷达运动时，来自它的原始数据$S_j$会受到运动畸变的影响，这使得扫描点与真实位置不同。为了解决这一问题，我们利用imu传播对lidar运动$T_{L_j}^{L_j}$的预测，并假设扫描过程中是线性运动模型。然后，对每个点$x(t)∈S_j∈R^3$进行$T_{L_j}^{L_j}$的线性插值校正，得到去除倾斜的扫描数据，其中$t∈ t_{j^`},t_j$是扫描点的时间戳，t_{j^`}和t_j分别是扫描开始和结束的时间戳。

C 激光相对测量

通过IMU和另一个能够提供传感器对相对姿态的传感器的融合，估计出的状态，如果我们固定第一个参考系，$X_B^W$和$T_B^L$将是局部可观测的。为了适当地融合来自imu的预积分，我们建议使用扫描之间的相对lidar测量来约束lidar姿态，作为算法1。在找到点对应之前，我们先建立一个局部地图，因为一次扫描中的点密度不足以计算精确的对应。
局部地图包含来自nm离散时间戳{o，···，p，···，i}的lidar特征点，其中o，p和i分别是窗口内第一次lidar扫描、pivot lidar扫描和最后处理的lidar扫描的时间戳，如图2所示。局部地图$M^{L_p}_{L_{o,i}}$是在pivot lidar的框架内从特征扫描而来。$F^{L_p}_{L_γ}，γ∈{o,····,i}$，通过先前优化的激光雷达姿态$T_{L_p}^{L_γ}$进行变换。待估计状态是ns时间戳 {p+1,····,i,j} 处的状态，其中p+1和j是激光雷达扫掠 pivot1旁边的时间戳和窗口中当前激光雷达扫描的时间戳。

D 雷达帧匹配

激光雷达相对测量可以提供pivot雷达姿态和以下激光雷达姿态之间的相对约束。我们的方法优化了优化窗口中的所有姿态，包括第一姿态$T^W_{L_p}$，即$F_{Lp}$不固定。因此，激光雷达代价函数中的每一项都涉及两个激光雷达扫描的姿态，$T^W_{L_p}$和$T^W_{L_α}$，α∈{p+1,···,j}。优化pivot姿态有助于更好地减小预积分误差，并确保传感器对与重力对准。我们估计的状态是imu的状态，因此需要引入外部参数来表示imu状态对lidar的约束。从后一个激光雷达姿态到窗口中的轴姿态的相对转换可以定义为

根据前面的对应关系，每个相对激光雷达测量的残差m=[x，ω，d]∈$m_{L_α}$，α∈{p+1，···，j}可以表示为点到平面的距离.

V 旋转约束的优化

将特征点加入到全局地图而不是局部地图可以将激光雷达姿态约束到一致的世界帧$F_W$。我们的改进方法使用相对激光雷达测量值$m_L$。由于全局地图是精细化的副产品，我们也把它称为建图方法。最近的激光雷达特征点与全局地图对齐后形成的代价函数如下：

式中$T=T^W_L$是最新估计的激光雷达姿态，$m$是特征点$x$在$F_L$和系数$ω$，$d$在$F_W$中定义的相对激光雷达测量。然后我们可以使用类似的高斯牛顿法来最小化$C_M$。利用残差$C_M$和雅可比$J^C_p$和$J^C_θ$进行优化，其中$θ$是相应四元数q的误差状态，但由于累积的旋转误差，经过长期的运算，合并后的全局地图不能与重力精确对齐。这可能导致进一步的建图错误地与倾斜的地图对齐。受文献[16]在SE2约束下优化SE3的启发，我们提出了一种约束建图策略。该策略利用了来自激光雷达imu里程计的旋转约束，确保最终地图始终与重力保持一致。图3示出了旋转约束建图的结构。
给定沿z轴的方向具有更高的不确定性，并且方向的其它两个DOF更接近于真值，我们可以通过修改方向的雅可比（12）中的详细推导来约束代价函数，

其中表示最后一次迭代中状态的估计，z是w.r.t fw方向的信息矩阵的近似，x和y可以通过fw中x轴和y轴方向与z轴方向的信息比得到。
之后，我们使用$J^C_p$和$J^C_{θ_z}$作为雅可比，这是优化步骤所需要的。增量的激光雷达姿态可以用$\delta θ_z$和$\delta θ_p$表示，从而得到最新的激光雷达状态p和q

VI 运行

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

A 不同传感器的配置

传感器对的配置可以不同。对于手持式传感器对，例如图4a中的传感器对，lidar和imu彼此接近。因此，管道与之前介绍的一样。但对于安装在汽车上的传感器对，这两个传感器通常彼此相距较远。例如，图4b示出了安装在汽车的基座连杆上方的imu，而激光雷达则安装在汽车的前部。在公式（6）中增加了一个用于汽车测试的外部平移参数的先验项，而不是自动校准所有平移参数。

B 初始化

首先，没有对激光雷达特征点进行姿态估计。因此，需要大致精确的匹配算法。在初始化阶段采用了[1]的激光雷达里程计。在提供的激光雷达姿态下，需要传感器对的充分运动，以使IMU状态可见[14]。然后利用激光雷达的姿态和imu的测量值来初始化imu的状态，这些状态可以用文[10]和文[17]中介绍的方法来求解。在几秒钟内进行测试。vii，我们遵循[10]中的初始化方法，该方法也对外部参数进行线性初始化。接下来，利用初始状态和新的测量值，在局部窗口内迭代进行非线性优化以估计状态。

VII 测试和分析

A 定量分析

为了定量分析我们的方法，使用图4a所示的传感器对。带有16条线路的Velodyne VLP-16激光雷达安装在XSENS MTI-100 IMU上方。反射标记可以通过运动捕捉系统提供地面真实姿态。激光雷达配置为10Hz更新速率，IMU更新为400Hz。使用[18]将不同方法估计的轨迹与地面真实情况对齐。
不同运动条件下的试验：表1显示了不同运动速度和不同方法下的均方根误差（rmse）结果，其中loam[1]被视为基线。lio是我们的局部窗口优化的里程计方法。lio-raw和lio-no ex分别与lio期望的运动补偿或在线外部参数估计被截断相同。lio映射是由具有旋转约束的建图结果得到的。最好的两个结果用粗体显示。结果表明，在任何情况下，lio映射都能提供平移（位置）和旋转（方向）状态的精确估计。lio在运动速度较快时有较好的性能，产生更多的imu激励。但是，如果运动缓慢，它会受到漂移的影响，因为此时的局部地图相对稀疏。该表还显示，通过运动补偿和在线外部参数估计，lio可以提供更好的性能，特别是在运动速度很快的情况下。

B 定性分析

为了显示在具有挑战性的场景中的改进，进行了几种不同传感器配置和环境的测试，包括室内手持和室外校园高尔夫球车测试（配置如图4所示），以及在Kaist Urban数据集上的测试[19]。由于空间有限，这些姿态估计和映射结果显示在补充视频中。

VIII 总结

本文提出了一种新的紧密耦合激光雷达里程计方法。它包括里程计的状态优化和带旋转约束的优化。结果表明，我们的方法优于目前最先进的激光雷达方法和松耦合方法。尽管所提出的方法需要初始化，但即使在具有挑战性的测试场景（例如快速运动情况、激光雷达退化情况和有限重叠的激光雷达扫描）下，通过充分的IMU激励，我们的方法确实显示了具有快速更新率的鲁棒姿态估计结果。