【论文阅读】Tightly Coupled 3D Lidar Inertial Odometry and Mapping

本文主要是对LIO-Mappingt论文的一些学习总结，具体的参考文献请参考原文，如有不对之处，希望大家提出宝贵的意见，共同进步~

摘要

运动估计是移动机器人的基本要求之一，通过传感器融合我们可以弥补单一传感器的缺陷并提供更可靠的运动估计。在这篇文章中，我们介绍一种Lidar-IMU紧耦合融合方法。提供通过联合最小化雷达和IMU测量的误差，雷达-IMU里程计（LIO）能在长时间的实验中，甚至在激光测量环境出现退化的情况下，也能保持较小的漂移。 除此之外，为了获得更可靠的雷达位姿估计，此外，为了获得更可靠的位姿估计，提出了一种旋转约束的优化算法(LIO-mapping)，进一步将激光雷达位姿与全局地图对齐。实验结果表明，该方法能够在IMU高更新速率下，高精度地估计传感器的位姿，即使是在快速运动的条件下，或在特征不足的情况下。

I. 介绍

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

激光雷达传感器可以对周围环境进行距离测量，在机器人系统中得到了广泛的应用。典型的3D激光雷达可以在360度水平视场(FOV)下，以10Hz左右的频率感知周围环境。此外，作为一种主动传感器，它对光照具有不变性。高可靠性和高精度使激光雷达传感器成为姿态估计的热门选择。

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

在本文中，我们提出了一种紧耦合的3D Lidar-IMU位姿估计算法来克服上述问题。激光雷达和IMU的测量数据用于联合优化。为了实现实时和一致的估计，对历史位姿采用滑动窗口和边缘化优化，然后进行旋转约束的优化。我们工作的主要贡献如下:

• 提出了一种紧耦合激光雷达- imu测程算法。它提供了实时准确的状态估计和较高的更新速度。
• 基于lidar-IMU里程计的先验，旋转约束精化方法进一步优化最终的姿态和生成的点云地图。即使在一些激光雷达退化的情况下，它也能确保一致和稳健的估计。
• 通过大量的室内和室外测试，验证了算法的有效性。它优于最先进的激光纯或松散耦合激光- imu算法。
• 源代码可在online1上获得。这是社区提供的第一个紧密耦合激光雷达和IMU融合的开源实现。

本文的其余部分组织如下：第二节对相关工作进行了回顾。第三节解释了符号和一些初步说明。所提出的里程计和精化方法分别在第四章和第五章。实现和测试见第六章和第七章。结论见第八章。

II. 相关工作

这一部分分别介绍了基于松耦合的Lidar-IMU以及基于紧耦合的Lidar-IMU融合算法的研究现状，并简单介绍了作者提出的融合算法。

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

III. 符号与说明

A. 符号

我们将3D激光雷达传感器捕获的每一条激光测量线表示为scan $C$，而sweep $S$表示包含一次测量中所有scan。例如，16行3D激光雷达在一次扫描中包含16次scan。

在以下部分中，我们将变换矩阵表示为$T_b^a\in SE(3)$ ，表示将${\mathcal{F}}_b$坐标系下的点 $x^b\in \mathbb{R}$转换到${\mathcal{F}}_a$坐标系下。$\overline{T_b^a}$表示IMU预测的转换矩阵，$R_b^a\in SO(3)$和$p_b^a$是$T_b^a$的旋转矩阵和平移向量。同时与$R_b^a$对应的是用四元数$q_b^a$表示旋转，$\otimes$表示四元数乘法。我们使用$\widehat{a_k}$和$\widehat{w_k}$表示IMU在$k$时刻的原始测量数据。${\mathbf{F}}_{\mathbf{a}}$表示在初始坐标系${\mathcal{F}}_a$下提取的特征点，可以通过通过转换矩阵转换到${\mathcal{F}}_b$坐标系下，表示为${\mathbf{F}}_{\mathbf{a}}^{\mathbf{b}}$。

B. IMU运动

1) 状态

${\mathcal{F}}_{{\mathcal{B}}_i}$和${\mathcal{F}}_{{\mathcal{L}}_i}$分别表示IMU和Lidar中心的参考坐标系，与此同时，我们获得每一个sweep $S_i$的时间戳$i$。我们需要估计的IMU状态$X_{B_i}^W$是在世界坐标系${\mathcal{F}}_{\mathcal{W}}$下的，Lidar和IMU之间的外参可以表述为$T_B^L$,IMU在$i$时刻的状态和外参可以表述为：

其中$p_{B_i}^W$、$v_{B_i}^W$和$q_{B_i}^W$分别表示位置、速度以及IMU坐标系到世界坐标系的转换。而$b_a$和$b_g$分别表示加速度计和陀螺仪的零偏。

2) 运动模型

通过IMU中加速度计和陀螺仪的输入，我们可以通过公式2所示的离散化方程将IMU的之前的状态$X_{B_i}^W$更新到当前的状态$X_{B_j}^W$,其中$\Delta t$表示两次连续IMU测量的时间间隔。同时我们将激光雷达第$k=i$帧和第$k=j$帧中间的所有IMU测量值进行积分。我们不严谨的使用$k=j-1$表示在第$k=j$帧之前的IMU测量值。

其中$g^W$是世界坐标系下的重力向量，为了使用方便，我们速记$(\cdot {)}_i\doteq (\cdot {)}_{B_i}^W，\Delta t_{ij}={\sum }_{k=i}^{j-1}$，以及将$\prod {j-1}_{k=i}$表示为四元数的连续相乘。

3) 预积分

在$i$时刻和$j$时刻之间的运动，我们可以通过预积分来表示：$z_j^i=\{\Delta p_{ij},\Delta v_{ij},\Delta q_{ij}\}$,其中协方差矩阵$C_{B_j}^{B_i}$表示状态误差模型。

IV. 紧耦合Lidar-IMU里程计

为了进行有效的运动估计，许多雷达建图的工作如[7]、[1]和[13]将任务分为两个部分：里程计和建图。受这些工作的启发，我们提出的系统由两个并行的部分组成。在Sec. IV介绍了第一部分，一个紧耦合的Lidar-IMU里程计，优化滑动窗口内的所有状态。在Sec. V介绍了第二部分：细化旋转约束（建立全局一致地图的过程），使用已经优化的位姿和重力约束中的信息将激光雷达sweep与全局地图对齐。

A. Lidar-IMU里程计概述

图1：激光雷达-IMU里程计框架。讨论每一块的小节在循环号之后给出

 

图2：滑动窗口。局部地图由第$j$帧之前且从$o$开始历史点云组成。优化窗口包含$p$和$j$之间的Lidar点云和距离变化，与[1]中一样；即选择面特征和线特征点

 

图1展示了我们提出的Lidar-IMU里程计的一个简单的概述，利用先前估计的状态，我们可以使用当前雷达的原始输入$S_j$已经历史时刻$i$到当前时刻$j$的IMU原始输入$I_{ij}$以进行一次新的状态优化。里程计估计步骤如下：

1. 在$S_j$到达之前，IMU状态通过公式(2)迭代进行更新
2. 同时，这些输入的$\Delta p_{ij}$、$\Delta v_{ij}$和$\Delta q_{ij}$经过预积分被用于联合优化。
3. 当接收到一帧激光数据$S_j$时,对原始数据进行去畸变处理，以获得去畸变激光雷达帧$\overline{S_j}$（Sec. IV -B）。
4. 接下来，进行特征提取减少数据维度和提取更重要的特征点${\mathbf{F}}_{{\mathbf{L}}_{\mathbf{i}}}$（Sec. IV -B）。
5. 根据之前相应的优化状态$T_{B_{o,i}}$和$T_B^L$（Sec. IV -c)，将滑动窗口下的历史雷达特征点云${\mathbf{F}}_{{\mathbf{L}}_{\mathbf{o},\mathbf{i}}}$被合并成一个局部子图$M_{L_{o,i}}^{L_p}$。
6. 通过预测的Lidar位姿${\mathbf{F}}_{\mathbf{j}}$，可以找到相对测量关系$m_{L_{p+1,j}}$(Sec. IV -C)。
7. 最后一部是联合非线性优化，根据雷达测量的相对关系和IMU预积分可以获得滑动窗口内的状态的最大后验估计。优化结果用于更新步骤1）中的预测状态，避免IMU传播的漂移。

B. 去畸变和特征提取

3D激光雷达内部有旋转机构，可接收整圈数据。当3D激光雷达移动时，其原始数据$S_j$j会发生运动失真，这使得扫描中的点与真实位置不同。为了解决这个问题，我们使用IMU传播预测激光雷达运动${\bar{T}}_{L_j^{\prime }}^{L_j}$，并假设一个完整的sweep中运动符合线性模型。然后，每个点$x(t)\in S_j\subset \mathbb{R}$通过${\bar{T}}_{L_{j^{\prime }}}^{L_j}$进行线性插值校正获得校正后的点云帧$\overline{S_j}$，其中$t\in (t_{j^{\prime }},t_j]$是扫描点的时间戳，$t_{j^{\prime }}$和$t_j$分别是扫描开始和结束的时间戳。

为了提高计算效率，需要进行激光雷达特征提取。这里，我们只对平面或边缘上最相似的点感兴趣[1]，[7]，因为这些点可以从激光雷达扫描sweep的每根线束scan中提取。这些在点云$\overline{S_j}$中特征点${\mathbf{F}}_{{\mathbf{L}}_{\mathbf{j}}}$通过曲率进行选择。

C. 相对Lidar测量

通过IMU和另一个传感器的融合，该传感器能够提供传感器对的相对姿态，估计当前状态。如果固定第一个第一个参考帧，$X_B^W$和$T_B^L$也是局部可观的。为了适当的融合来自IMU的预积分值，我们提出了使用两帧之间的雷达相对测量来约束雷达位姿，如算法1所示。在找到对应点之前，我们构建一个局部地图，因为单次扫描中的点不够密集，无法计算准确的对应。

为了简化，我们将预测变换矩阵${\bar{T}}_{L_j}^{L_p}$表示为$T_{L_j}^{L_p}$，${\mathbf{F}}_{{\mathbf{L}}_{\mathbf{j}}}^{{\mathbf{L}}_{\mathbf{p}}}$通过${\bar{T}}_{L_j}^{L_p}$转换而来

 

局部地图包含来自离散时间戳$N_m$ $\{o,....,p,.....,i\}$的激光雷达特征点。其中$o$，$p$和$i$分别是窗口内第一帧、中心位置和最后一帧雷达点云的时间戳，如图2所示。局部地图$M_{L^{o,i}}^{L_p}$是建立在pivot lidar帧的坐标系下，通过已经优化好的$T_{L_{\gamma }}^{L_p}$将历史帧的点云转换成${\mathbf{F}}_{{\mathbf{L}}_{\mathbf{\gamma }}}^{{\mathbf{L}}_{\mathbf{p}}}$，$\gamma \in \{o,......,i\}$。待估计的状态是时间戳$N_s$中的 $\{p+1,......,i,j\}$处的状态，其中$p+1$和$j$分别是靠近主节点pivot lidar帧时间戳以及窗口中的当前激光雷达点云帧时间戳。

通过建立的局部地图，我们可以找到$M_{L^{o,i}}^{L_p}$和${\mathbf{F}}_{L_{\alpha }}$，$\alpha \in \{p+1,......,j\}$。我们将这种对应定义为相对激光雷达测量，因为它们是相对于pivot 位姿的，并且pivot 位姿会随着滑动窗口的改变而改变。Sec. IV -B 部分中，我们在${\mathcal{F}}_{L_{\alpha }}$坐标系下提取的原始特征是平面点或者边缘点。在实际应用中，我们发现边缘点不能改善Lidar-IMU里程计的结果。因此，在下文中，我们只讨论平面特征。 对变换后的每个特征点${\mathbf{x}}^{L_p}\in {\mathbf{F}}_{L_{\alpha }}^{L_p}$使用KNN寻找$M_{L^{o,i}}^{L_p}$中的$K$个近似点$\pi (x^{L_p})$，然后对平面点进行拟合，将这些邻接点拟合到${\mathcal{F}}_{L_p}$的一个平面上。平面点的系数可以用线性方程${\omega }^T{x}^{\prime }+d=0,x^{\prime }\in \pi (x^{L_p})$来求解，其中$\omega$是平面法线方向，$d$是到${\mathcal{F}}_{L_p}$原点的距离。我们表示每个平面特征点$x\in {\mathcal{F}}_{L_{\alpha }}$的$m=[x，\omega ，d]\in m_{L_{\alpha }}$作为相对激光雷达测量之一。值得一提的是，在每个相对激光雷达测量$m\in m_{L_{\alpha }}$中，$x$在${\mathcal{F}}_{L_{\alpha }}$中定义，并且$\omega$和$d$在${\mathcal{F}}_{L_p}$中定义。

D.激光雷达扫描匹配

Lidar的相对测量关系可以提供pivot雷达帧位姿和接下来的雷达帧位姿的相对约束。我们的方法优化了滑动窗口内所有位姿，包括第一个位姿$T_{L_p}^W$,即${\mathcal{F}}_{L_p}$未固定。因此，Lidar的损失函数中每一项都包括两帧Lidar点云的位姿，$T_{L_p}^W$和$T_{L_{\alpha }}^W$，$\alpha \in \{p+1,.....j\}$。优化pivot位姿将有助于更好地减小预积分误差，并确保传感器对与重力对准。我们估计的状态是IMU的状态，因此需要引入外部参数来表示IMU状态对激光雷达的约束。可以定义窗口中从后面激光雷达姿态到pivot姿态的相对变换：

利用前面的对应关系，每个相对激光雷达测量值$m=[x，\omega ，d]\in m_{L_{\alpha }},\alpha \in \{p+1,......,j\}$的残差可以表示为点到面的距离:

E. 优化

为了获得优化的状态，采用了固定滞后平滑器和边缘化方法。固定滞后平滑器将$N_s$个IMU状态保持在滑动窗口中，从$X_{B_p}^W$到$X_{B_j}^W$，如图2所示。滑动窗口有助于限制计算量。当新的测量约束到来时，更平滑的将包括新状态并将窗口中最旧的状态边缘化。将优化的的所有状态为：

然后，最小化具有马氏范数的下面的代价函数，以获得状态$X$的MAP估计:

其中，${\mathbf{r}}_p(X)$是边缘化后的先验约束，${\mathbf{r}}_{\mathcal{L}}(m,X)$是相对雷达约束的残差,${\mathbf{r}}_{\mathcal{B}}(z_{\beta +1}^{\beta })$是IMU约束的残差，非线性最小二乘形式的代价函数可以用$H\theta X=-b$的高斯-牛顿算法来求解。我们使用Ceres Solver[15]来求解该问题。

激光雷达约束${\mathbf{r}}_{\mathcal{L}}(m,X)$可以从公式(4)每次相对激光雷达测量导出。协方差矩阵$C_{L_{\alpha }}^m$由激光雷达精度决定。与文献[10]类似，IMU约束${\mathbf{r}}_{\mathcal{B}}(z_{\beta +1}^{\beta })$可以由状态和IMU预积分得到，$||{\mathbf{r}}_p(X)|{|}^2=b_p^T{H}_p^{+}{b}_p$可以由舒尔补得到(详见[12])。

V.使用旋转约束进行细化

将特征点注册到全局地图(而不是局部地图)可以将激光雷达位姿约束到一致的世界坐标系${\mathcal{F}}_W$。我们的细化方法使用了Sec. IV中的相对雷达测量$m_L$。因为全局地图是细化方法的附带产品，我们也称之为建图方法。将最新的激光雷达特征点与全局地图对齐的损失函数可定义为：

其中$T=T_L^W$是最新的待估计激光雷达姿态，而Lidar相对测量值$m$中的特征点$x$定义在${\mathcal{F}}_L$中。系数$\omega$，$d$定义在${\mathcal{F}}_W$。然后，我们可以使用类似的高斯-牛顿法来最小化$C_{\mathcal{M}}$。通过残差$C_{\mathcal{M}}$和Jacobian 矩阵$J_p^C$和$J_{\theta }^C$进行优化，其中$\theta$是相应四元数$q$的误差状态，但是由于累积的旋转误差和长期的操作，合并后的全局图不能与重力精确对准。这可能会导致进一步建图错误地与倾斜的地图对齐。根据[16]中使用SE(2)约束优化SE(3)的建议，我们提出了一种约束建图策略。该策略利用激光雷达IMU里程计的旋转约束，确保最终地图始终与重力对齐。图3示出了旋转约束建图的结构。

图3：旋转约束建图。里程计姿势首先用作全局点云注册的先验。然后将里程计中的旋转分量作为虚拟测量值应用于约束优化。

 

鉴于沿z轴的方向具有更高的不确定性，且方向的其他两个自由度更接近真实值，我们可以通过修改旋转的雅可比矩阵来约束损失函数，如[12]中的详细推导所示，
其中$\breve{(\cdot )}$表示上一次迭代的状态，${\breve{\Omega }}_z$姿态相对于坐标系$F_W$的旋转信息矩阵的近似值，${ϵ}_x$和${ϵ}_y$可以通过x、y轴与z轴的信息比得到。

然后，我们使用$J_p^C$和$J_{{\theta }_z}^C$作为雅可比矩阵，这是优化步骤所需要的。激光雷达姿态的增量可表示为$\delta {\theta }_z$和$\delta p$，这可以实现更新激光雷达状态$\tilde{p}$和$\tilde{q}$。

VI. 实验

未完待续~

VII. 测试与分析

未完待续~