点击上方“3D视觉工坊”，选择“星标”

干货第一时间送达

在大规模场景下的地图构建时候，纯激光雷达传感器无法很好的完成地图构建工作，这时需要考虑使用不同的传感器，获取多种数据来进行传感器的融合，实现位姿的矫正，最终完成复杂大规模场景下的地图构建，这种方法也被称之为多传感器融合。

本篇文章要讲述的是发表在ICRA2020上的一篇多传感器融合实现点云地图构建的文章。

涵盖的知识面有：ESKF，IKF，IMU，Quaternion等。

论文地址：

https://arxiv.org/abs/1907.02233

视频演示

1、论文概览 

R-LINS的本质是一个以机器人为中心的激光惯导状态估计器。它使用以下两种传感器来估计机器人的运动姿态：

6轴 IMU：高频，聚焦自身运动，不采集外界环境数据 3D LiDAR：低频，聚焦车体运动，采集外界环境数据 这两种传感器想必大家都不陌生。但是，这两个传感器在复杂大规模领域里，单独的依靠自己的数据是很难实现地图构建的。所以，为了能够在具有挑战性的环境下也能保持鲁棒性和计算效率，本文使用了迭代的误差状态卡尔曼滤波器(ESKF)来通过重复生成新的对应特征来递归的修正机器人的状态信息。

为了防止滤波发散和降低计算成本，本文采用了一种新的以机器人为中心的公式，该公式重新定义了一个移动的局部帧的状态估计量，而不是像标准的以世界为中心的LIO中的固定全局帧的状态估计量。

这里需要注意的是，LINS和LIO的区别如下：

本篇文章相比于LIO，降低了计算的时间消耗，是第一个使用ESKF的LIO框架。

2、雷达里程计分类 

现在基于激光雷达里程计的划分主要有三大类：

纯雷达模型 松耦合的LiDAR-IMU模型 紧耦合的LiDAR-IMU模型 下面将依次介绍下对应的模型。

2.1、纯雷达模型

一般来说纯雷达的里程计模型大部分都是基于帧间匹配的，主要分为两大类：

基于迭代最近点法（ICP） 基于正态分布变换（NDT） 所以，现阶段基于迭代最近点法的纯雷达模型，比较出名的是LOAM框架，主要为LOAM和LeGO-LOAM

而基于正态分布变换的纯雷达模型，比较出名的是HDL-Localization。

这些算法都是基于单独的激光雷达传感器数据而进行的里程计推演，实现地图构建。

2.2、松耦合的LiDAR-IMU模型 

纯雷达模型使用的传感器是激光雷达，可以很好的探测到外界的环境信息。但是，同样的，也会受到这些信息的干扰，再长时间的运算中会产生一定的累计误差。为了防止这种误差干扰到后续的地图构建中，需要使用另一种传感器来矫正机器人自身的位姿信息。

在实际使用中，为了矫正激光雷达传感器所得到的位姿信息，一般使用的是IMU传感器。

IMU传感器由于是自身运动估计的传感器，所以，采集的都是自身运动的姿态信息。可以很好的矫正激光雷达里程计的位姿信息。所以，通常使用激光雷达和惯导来进行数据融合，实现姿态信息的矫正，这种模型也被称之为LiDAR-Inertial Odometry（LIO） 。

这里主要介绍的是松耦合的LiDAR-IMU模型。

高斯粒子滤波就是比较常见的松耦合模型。

松耦合模型由于是对两个传感器的数据直接进行使用，得到新的姿态解算后的数据，并没有构建新的损失函数，所以，得到的结果相对来说也不是特别的精准。

但是，由于其模型简单，可以快速实现，所以，松耦合的多传感器融合模型，也有着自己的用处。

2.3、紧耦合的LiDAR-IMU模型

紧耦合饿到多传感器融合模型，是将各个传感器的数据都输入到了一个模型之中，构建约束关系，使用优化方法最小化约束，得到最终的结果。其方法可以分为：

基于优化的紧耦合模型 基于EKF的紧耦合模型 基于优化的紧耦合模型目前比较热门的是LIO算法。

基于EKF的紧耦合模型目前比较热门的是LIOM算法。

本文提出的基于迭代ESKF的R-LINS是基于EKF模型的变种的一种紧耦合模型。

由于EKF模型是将非线性模型局部线性化，所以，对于雷达观测约束这种典型的非线性模型而言，容易出现性能低下甚至发散的情况，所以，需要使用一种针对非线性模型的卡尔曼滤波模型来替换EKF。本文使用的是误差状态卡尔曼滤波模型（ESKF）来最小化非线性约束，从而实现姿态的更新，这也是R-LINS的一个创新点。

3、LiDAR-Inertial建图 

本章节将着重讲解LiDAR和IMU传感器融合实现地图构建的部分。本文的系统架构如图所示：

一共分为三大块：

特征提取：从原始点云中提取稳定的特征 LIO：状态传递和状态更新模块组成，使用迭代ESKF，输出纯里程数据和不失真的点云特征 地图构建：细化纯里程数据，得到全局位姿信息并输出全局地图 其中，特征提取使用的是LeGO-LOAM部分的特征提取模块，LIO模块和之前的LIO论文不一样，改变了ESKF作为约束优化的方法，地图构建使用的是LeGO-LOAM部分的地图构建模块。

由于本文的重点是LIO模块，所以，这里直接略过了特征提取和地图构建，只讲解LIO模块的内容。

这里用到了一个迭代卡尔曼滤波。迭代卡尔曼滤波器主要目的在于克服EKF线性化过程中对高阶误差的舍弃而造成的估计不准确。“迭代“主要是指在量测更新过程中添加了一步迭代，直到状态收敛。

3.1、状态定义

这里和VIO中表示使用误差向量中的局部重力不同。我们使用估计的roll和pitch，将重力从世界帧转换到局部帧，来获取重力信息。

本文使用的方法会更加合理一点，假设在极短的时间内，roll和pitch不会急剧变化，则在地图闭环后，得到的结果有着很小的漂移，就可以得到比较精准的重力信息了。

3.2、状态递推

可以推导得到：

其中，有个关系，大家需要注意：

估计值 = 测量值 - 偏差

通过上面的公式(1)，我们可以得到

3.3、状态更新 

我们知道，卡尔曼滤波（KF）是有着五大方程的，分别是：

预测：状态预测方程，协方差预测方程

更新：状态更新方程，协方差更新方程

卡尔曼增益方程

误差状态卡尔曼滤波（ESKF）也有着上面的五大方程组。唯一的区别在于卡尔曼滤波针对的是物体的运动状态，而误差状态卡尔曼滤波针对的是物体运动状态的误差状态。

在前面3.2部分，我们已经得到了状态预测方程（公式2）和协方差预测方程（公式3）。剩下的就是要得到状态更新方程，协方差更新方程和卡尔曼增益方程了。

本文用的是迭代卡尔曼滤波（IKF）。这里就可以将状态更新问题变成了一个优化问题

这里其实就是LOAM的点线距离和点面距离的公式了：

所以，上面的式子其实可以拆解为：

这样看的话，其实就能理解上面公式是什么意思了。

其中，ESKF的卡尔曼增益方程和状态更新方程：

这就是迭代的卡尔曼滤波和常规的卡尔曼滤波的区别之处。

当迭代终止的时候，假定终止下标为  ，我们就可以算出协方差更新方程为：

3.4 状态合成 

我们前面算出来的是以机器人为中心的参照系下的位姿信息。所以，为了得到全局坐标信息，需要进行状态合成，合成公式为：

3.5、初始化 

我们使用的传感器为IMU和激光雷达，所以有一些参数信息需要提前知晓：

4、地图构建

通过前面的部分，我们已经得到了每个时刻精准的状态估计信息。就可以使用LeGO-LOAM进行地图构建，得到全局地图。

但是也有一些不同。本文使用地图构建模块导出的roll和pitch作为LIO模块的反馈输入，这种策略被证明可以有效地提高系统的鲁棒性。

本文来自知乎

https://zhuanlan.zhihu.com/p/134621840

本文仅做学术分享，如有侵权，请联系删文。

下载1

在「3D视觉工坊」公众号后台回复：3D视觉，即可下载 3D视觉相关资料干货，涉及相机标定、三维重建、立体视觉、SLAM、深度学习、点云后处理、多视图几何等方向。

下载2

在「3D视觉工坊」公众号后台回复：3D视觉github资源汇总，即可下载包括结构光、标定源码、缺陷检测源码、深度估计与深度补全源码、点云处理相关源码、立体匹配源码、单目、双目3D检测、基于点云的3D检测、6D姿态估计源码汇总等。

下载3

在「3D视觉工坊」公众号后台回复：相机标定，即可下载独家相机标定学习课件与视频网址；后台回复：立体匹配，即可下载独家立体匹配学习课件与视频网址。

重磅！3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流等微信群。

一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的知识点汇总、入门进阶学习路线、最新paper分享、疑问解答四个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近2000星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

圈里有高质量教程资料、可答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~