[论文笔记|LIC-Fusion]LIC-Fusion: LiDAR-Inertial-Camera Odometry

Abstract

本文提出了一种称名为激光雷达-惯性导航仪-相机三合一（LIC-Fusion）的紧耦合多传感器融合算法，该算法可有效地融合IMU测量，稀疏的视觉特征和提取的激光雷达点。尤其是，提出的LIC-Fusion能够在三个异步传感器之间执行在线的空间和时间传感器标定，以补偿可能的标定偏差。

本方法的关键贡献在于，能够基于高效的MSCKF框架，利用从激光雷达检测和跟踪到的系数边缘/surf特征点，以及稀疏的视觉特征与IMU读数，完成理想的（根据线性误差）多模态传感器融合。

我们在室内和室外环境中均进行了广泛的实验，结果表明，所提出的LIC-Fusion在估计精度和对剧烈运动的鲁棒性方面均优于最新的视觉惯性里程计（VIO）和激光雷达里程计方法。

Ⅰ. INTRODUCTION AND RELATED WORK

• 惯性导航系统(INS)辅助单目摄像机对于跟踪自动驾驶车辆和移动感知系统的三维运动效果很好------>互补传感模式、低成本和小尺寸
• 相机受到光照条件的限制，在低光或夜间条件下无法提供高质量的信息
• 3D激光雷达传感器无论在何种光照条件下都能提供更稳健、更精确的距离测量---->存在点云稀疏、成本高、采集率低的问题
• 惯性测量单元(IMU)测量局部角速度和线性加速度，可以在动态轨迹中提供大量的信息，但如果不与其他信息融合，则会由于噪声而产生较大的漂移
• 所以，着重于激光-惯性-视觉里程计(LIC)。

本文在计算效率高的多状态约束卡尔曼滤波(MSCKF)框架下，提出了一种快速、紧耦合、单线程的激光-惯性-视觉(LIC)里程计算法，该算法具有在线时空多传感器标定功能。这项工作的主要贡献如下：
1.开发了一种紧密耦合的LIC里程计技术(称为LIC-Fusion)，通过在线空间和时间校准实现了高效的6自由度姿态估计 。在MSCKF框架内，所提出的LIC-Fusion有效地结合了IMU测量值、稀疏视觉特征和两种不同的稀疏激光雷达特征(见图1)。明确地建立了标定的外部参数和对测量值的姿态估计两者之间的依赖关系模型，并进行了解析推导。
2. 对所提出的方法进行了大量的实验验证，包括室内和室外环境的真实实验，表明所提出的LIC-Fusion比最先进的方法更准确、更鲁棒。

图1:LIC-Fusion方法融合了图像中所跟踪的稀疏视觉特征和点云中提取的激光雷达特征。红色和蓝色的激光雷达点分别是边缘和平面特征。估计的轨迹用绿色标记。

II. THE PROPOSED LIC-FUSION

A. State Vector

1. 状态向量

• k时刻，IMU的状态：$x_I$
• IMU和相机之间的转换外参：$x_{calib\_C}$
• IMU和激光雷达之间的转换外参：$x_{calib\_L}$
• 克隆的滑动窗口，包括过去m个图像时间的局部IMU克隆状态$x_C$和过去n个激光雷达扫描时间的局部IMU克隆状态$x_L$

总状态向量为:

其中${}_G^{I_k}q$表示在$t_k$时刻，IMU从世界坐标系{G}到IMU局部坐标系$I_k$的旋转。${}^G{v}_{I_k}$和${}^G{p}_{I_k}$代表在世界坐标系中，IMU的速度和$t_k$时刻的位置。$b_g$和$b_a$是陀螺仪和加速度计的偏置。${}_I^C\bar{q}$和${}^C{p}_I$表示相机坐标系{C}和IMU坐标系{I}之间的刚体变换。${}_I^L\bar{q}$和${}^L{p}_I$就是激光雷达和IMU坐标系之间的三维刚性转换。

2. 估计外感传感器和IMU之间的时间偏移量：
   IMU通常存在于低成本设备中。以IMU时间为基准时间，将相机和激光雷达都建模为具有偏移$t_{dC}$和$t_{dL}$的基钟，可以修正如下的测量时间:

$t_C$和$t_L$分别是相机和激光雷达报告的时间。

3. 状态误差的定义
   我们定义状态的真实值为$x$，状态估计值为$\hat{x}$,对应的状态误差为$\delta x$，则其关系为：(都是些比较基础的知识了)

其操作是对于向量空间中的状态$v$是简单的欧几里德加法，比如$v=\hat{v}+\delta v$,对于四元数有：

其操作为四元数乘法

B. IMU Propagation

IMU提供角速率和线性加速度测量，使用以下连续时间运动学模型进行建模：(以下基础知识应该比较熟悉)

其中，

${}^{I_k}\omega$ 和${}^{I_k}a$表示局部IMU坐标系下的真实角速度和线性加速度。${}^{G}g$表示世界坐标系下的重力加速度。陀螺仪和加速度计的$b_g$和$b_a$分别受高斯白噪声$n_{wg}$和$n_{wa}$的影响。

然后对这个连续时间系统进行积分和线性化，使状态协方差矩阵在时间上向前传播。

C. State Augmentation

当系统接收到新的图像或激光雷达扫描时，IMU状态将向前传播到该时间瞬间，并将传播的惯性状态复制(克隆)到$x_C$或$x_L$状态向量中。为了校准不同传感器之间的时间偏移，我们将一直传播到IMU时间${\hat{t}}_{I_k}$，这是目前对IMU时钟中测量采集时间的最佳估计。例如，如果以时间戳$t_{L_k}$接收到一个新的激光雷达扫描，我们将一直传播到${\hat{t}}_{Ik}=t_{Lk}+{\hat{t}}_{dL}$，并增加状态向量$x_L$来包括这个新的克隆状态估计。

同时，将协方差矩阵增广为:

其中,J是新克隆的${\hat{x}}_{Lk}$对当前状态的雅可比矩阵:

在上面的表达式中，$J_I$是关于IMU状态$x_I$的雅可比矩阵:

$J_{cali{b}_L}$为IMU与激光雷达关于外参(包括时间偏移)方面的雅可比矩阵:

同理，$J_{cali{b}_C}$、$J_C$、$J_L$分别是IMU与camera、camera时刻克隆、LiDAR时刻克隆在外参方面的雅可比，在本例中为0。

要注意的是,与激光雷达测量相对应的新克隆IMU状态对$t_{dL}$的依赖性是通过IMU运动学建模的，因此我们的测量模型（见第II-D.1节）可以直接作为IMU时钟帧中真实测量时间的克隆函数。这个激光雷达克隆过程类似于一个新的相机测量过程。

D. Measurement Models

1. 激光雷达特征测量:

为了限制所需的计算成本，我们希望从原始激光雷达扫描中选择一组稀疏的高质量特征进行状态估计。

因此，提取激光雷达扫描环的高曲率部分和低曲率部分，分别对应于边缘和平面surf特征(见图1)。我们使用KD-树快速索引，将当前激光雷达扫描中提取的边缘和surf特征投影到之前的扫描中，并找到最近的对应特征。

例如，我们在激光雷达扫描{$L_{l+1}$}到{$L_l$}中投影一个特征点${}^{L_{l+1}}{p}_{fi}$，这个投影点记作${}^{L_l}{p}_f$：

其中${}_{L_l+1}^{L_l}R$和${}^{L_l}{p}_{L_{l+1}}$是两个激光雷达帧间的相对旋转和平移,可由状态向量中的状态计算得到:

跟踪之后，我们会在原先的扫描中找到两个边缘特征${}^{L_l}{p}_{fj}$，${}^{L_l}{p}_{fk}$,对应于投影的边缘特征${}^{L_l}{p}_{fi}$。我们假设它们是从与${}^{L_l}{p}_{fi}$相同的物理边缘采样的。如果最近的边缘特征${}^{L_l}{p}_{fj}$在第$r$个扫描环上，则第二个最近的边缘特征${}^{L_l}{p}_{fk}$应该在其邻近的环$r-1$或$r+1$上。因此，边缘特征${}^{L_{l+1}}{p}_{fi}$的测量残差是其投影特征点${}^{L_l}{p}_{fi}$与${}^{L_l}{p}_{fj}$和${}^{L_l}{p}_{fk}$形成的直线之间的距离。

其中$||.|{|}_2$为欧几里德范数，×为两个向量的外积。

在当前状态估计时，我们将上述边缘特征的距离测量线性化:

其中$H_x$为距离相对于状态向量中的状态的雅可比矩阵，$n_r$为方差是$C_r$的高斯白噪声。$H_x$中的非零元素只与克隆的poses：

以及IMU和激光雷达之间的严格校准：

相关，所以得到：


为了进行EKF更新，我们需要知道距离测量的显式协方差$C_r$。由于这种测量不是直接从激光雷达传感器中获得的，我们在激光雷达扫描$C_r$中传播原始测量(点)的协方差。假设${}^{L_{l+1}}{p}_{fi}$,${}^{L_l}{p}_{fj}$,${}^{L_l}{p}_{fk}$点对应的协方差分别为$C_i,C_j,C_k$;则$C_r$可如下计算:

我们执行简单的基于Mahalanobis距离的概率离群拒绝:

其中，$P_x$表示相关状态的协方差矩阵，$r_m$应该服从于${\chi }^2$卡方分布，因此，如果它通过了这个测试，$r{(}^{L_{l+1}}{p}_{fi})$将在EKF更新中使用。

同样地，为了投影surf特征${}^{L_l}{p}_{fi}$，我们将发现三个相应的surf特征${}^{L_l}{p}_{fj}$、${}^{L_l}{p}_{fk}$、${}^{L_l}{p}_{fl}$,假设这些特征都是在与${}^{L_l}{p}_{fi}$相同的物理平面上进行采样的。surf特征${}^{L_{l+1}}{p}_{fi}$的测量残差为其投影特征点${}^{L_l}{p}_{fi}$与${}^{L_l}{p}_{fj}$、${}^{L_l}{p}_{fk}$、${}^{L_l}{p}_{fl}$构成的平面之间的距离。距离测量的协方差传播特性，线性化和Mahalanobis距离测试与边缘特征相似。

2. 视觉特征测量:

给定一个新图像，我们同样地传播和增益状态。使用KLT光流从图像中提取FAST特征并跟踪到未来帧中。一旦某个视觉特征丢失或在整个滑动窗口中被跟踪，我们使用相机克隆的当前估计值在3D空间中对该特征进行三角化测量。更新中使用了标准的视觉特征重投影误差。给定某个三维视觉特征点${}^G{p}_{fi}$ 的一组特征方位测量值$z_i$，一般的线性化残差为：

其中$H_f$是视觉特征测量相对于3D特征${}^G{p}_{fi}$的雅可比矩阵,并且两个雅可比矩阵都是按当前的最佳估计值计算的。因为我们的测量值是关于${}^G{p}_{fi}$的函数(公式29),我们利用MSCKF零空间投影去除这种依赖性。在零空间投影之后，我们有:

需要注意的是，IMU和camera{${}_I^C\bar{q}{,}^C{p}_I$}之间的刚性变换的雅可比矩阵是非0的，也就是说IMU和camera之间的变换可以在线校准。

E. Measurement Compression

在当前状态估计下对激光雷达特征和视觉特征测量进行线性化之后，我们可以执行EKF更新，但是由于大量的视觉和激光雷达特征测量，这会带来很大的计算成本。考虑所有测量残差和雅可比（来自激光雷达或视觉特征）的叠加:

其中$r$和$n$是带有残差和噪声块元素的向量(公式25/30)，通常假设所有的测量数据在统计上是独立的，那么噪声向量$n$就不相关。为了降低计算复杂度，我们使用Givens旋转来执行thin QR来压缩测量值，即

其中$Q_{H1}$和$Q_{H2}$是酉矩阵。经过测量压缩得到:

其中压缩后的雅可比矩阵$T_H$应该是状态向量x的维数的平方，压缩后的噪声由$n_c=Q_{H_1}^{T}n$给出。然后利用这个压缩的线性测量残差来有效地更新状态估计和与标准EKF的一致性。

III. EXPERIMENTAL RESULTS

1. 实验设备
   
   自组装的激光-惯性-相机，由激光雷达，Xsens IMU，和单目相机组装而成

2. 部分实验结果

IV. CONCLUSIONS AND FUTURE WORK

在本文中，我们开发了一种紧密耦合的高效多模态传感器融合算法，用于激光-惯性-相机里程计。在MSCKF框架内。在三个传感器之间进行在线时空校准，以补偿校准灵敏度，并简化传感器部署。该方法通过激光雷达扫描检测和跟踪稀疏边缘和平面特征点，并融合这些测量值和从单目图像中提取的视觉特征。因此，通过利用不同的传感模式，所提出的LIC-Fusion能够在不同的环境和主动运动下提供精确和鲁棒的6自由度运动跟踪。在未来，我们将研究如何有效地集成从激光雷达和摄像机获得的环路闭合约束，以约束导航误差。