Abstract
Ⅰ. INTRODUCTION AND RELATED WORK
II. THE PROPOSED LIC-FUSION
- A. State Vector
- B. IMU Propagation
- C. State Augmentation
- D. Measurement Models
- - 1. 激光雷达特征测量:
  - 2. 视觉特征测量:
- E. Measurement Compression
III. EXPERIMENTAL RESULTS
IV. CONCLUSIONS AND FUTURE WORK

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

状态向量

k时刻，IMU的状态：xIx_IxI
IMU和相机之间的转换外参：xcalib_Cx_{calib\_C}xcalib_C
IMU和激光雷达之间的转换外参：xcalib_Lx_{calib\_L}xcalib_L
克隆的滑动窗口，包括过去m个图像时间的局部IMU克隆状态xCx_CxC和过去n个激光雷达扫描时间的局部IMU克隆状态xLx_LxL

总状态向量为:

其中GIkq^{I_k}_G qGIkq表示在tkt_ktk时刻，IMU从世界坐标系{G}到IMU局部坐标系Ik{I_k}Ik的旋转。GvIk^Gv_{I_k}GvIk和GpIk^Gp_{I_k}GpIk代表在世界坐标系中，IMU的速度和tkt_ktk时刻的位置。bgb_gbg和bab_aba是陀螺仪和加速度计的偏置。ICqˉ^C_I \bar{q}ICqˉ和CpI^Cp_ICpI表示相机坐标系{C}和IMU坐标系{I}之间的刚体变换。ILqˉ^L_I \bar{q}ILqˉ和LpI^Lp_ILpI就是激光雷达和IMU坐标系之间的三维刚性转换。

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

tCt_CtC和tLt_LtL分别是相机和激光雷达报告的时间。

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

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

其操作为四元数乘法

B. IMU Propagation

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

其中，

Ikω^{I_k} ωIkω 和Ika^{I_k} aIka表示局部IMU坐标系下的真实角速度和线性加速度。Gg^GgGg表示世界坐标系下的重力加速度。陀螺仪和加速度计的bgb_gbg和bab_aba分别受高斯白噪声nwgn_{wg}nwg和nwan_{wa}nwa的影响。

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

C. State Augmentation

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

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

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

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

JcalibLJ_{calib_L}JcalibL为IMU与激光雷达关于外参(包括时间偏移)方面的雅可比矩阵:

同理，JcalibCJ_{calib_C}JcalibC、JCJ_CJC、JLJ_LJL分别是IMU与camera、camera时刻克隆、LiDAR时刻克隆在外参方面的雅可比，在本例中为0。

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

D. Measurement Models

1. 激光雷达特征测量:

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

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

例如，我们在激光雷达扫描{Ll+1L_{l+1}Ll+1}到{LlL_lLl}中投影一个特征点Ll+1pfi^{L_{l+1}}p_{f i}Ll+1pfi，这个投影点记作Llpf^{L_l}p_fLlpf：

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

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

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

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

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

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

相关，所以得到：

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

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

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

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

2. 视觉特征测量:

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

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

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

E. Measurement Compression

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

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

其中QH1Q_{H1}QH1和QH2Q_{H2}QH2是酉矩阵。经过测量压缩得到:

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

III. EXPERIMENTAL RESULTS

实验设备

自组装的激光-惯性-相机，由激光雷达，Xsens IMU，和单目相机组装而成
部分实验结果

IV. CONCLUSIONS AND FUTURE WORK

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

[论文笔记|LIC-Fusion]LIC-Fusion: LiDAR-Inertial-Camera Odometry相关推荐

论文笔记—LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain
论文笔记-LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain 论文链接 ...
论文笔记：FFA-Net Feature Fusion Attention Network for Single Image Dehazing
摘要作者提出了一种端到端的特征融合注意力网络(FFA-Net)来直接恢复无雾图像.FFA-Net架构由以下三个主要部分组成: (1)提出了一种新的特征注意(Feature Attention,FA) ...
论文笔记——C2FNet:Context-aware Cross-level Fusion Network for Camouﬂaged Object Detection
Context-aware Cross-level Fusion Network for Camouﬂaged Object Detection 论文地址:https://arxiv.org/pdf/ ...
论文笔记-F3Net：Fusion, Feedback and Focus for Salient Object Detection
论文笔记之2020-AAAI-F3Net-F3Net:Fusion, Feedback and Focus for Salient Object Detection 论文地址:https://arxi ...
论文笔记—A Review of Visual-LiDAR Fusion based Simultaneous Localization and Mapping
论文笔记-A Review of Visual-LiDAR Fusion based Simultaneous Localization and Mapping 论文链接文章摘要 ~~~~ ~ ...
Deep RGB-D Saliency Detection with Depth-Sensitive Attention and Automatic Multi-Modal Fusion论文笔记
CVPR2021论文笔记题目:Deep RGB-D Saliency Detection with Depth-Sensitive Attention and Automatic Multi-Mod ...
论文笔记，物体六自由度位姿估计，DenseFusion: 6D Object Pose Estimation by Iterative Dense Fusion
论文笔记,物体六自由度位姿估计,DenseFusion: 6D Object Pose Estimation by Iterative Dense Fusion 链接摘要 1,引言 2,模型 2.1 ...
论文笔记：DeepFuse: A Deep Unsupervised Approach for Exposure Fusion with Extreme Exposure Image Pairs
论文笔记:DeepFuse: A Deep Unsupervised Approach for Exposure Fusion with Extreme Exposure Image Pairs co ...
论文阅读：PMF基于视觉感知的多传感器融合点云语义分割Perception-Aware Multi-Sensor Fusion for 3D LiDAR Semantic Segmentation
题目:Perception-Aware Multi-Sensor Fusion for 3D LiDAR Semantic Segmentation 中文:用于 3D LiDAR 语义分割的多传感器感 ...
论文阅读：LIF-Seg: LiDAR and Camera Image Fusion for 3DLiDAR Semantic Segmentation
LIF-Seg:用于 3D LiDAR 语义分割的 LiDAR 和相机图像融合来源:华科 + 商汤未发表2021 链接:https://arxiv.org/abs/2108.07511 个人觉得有 ...

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

目录