来源 | 知乎@yc zhang、当SLAM遇见小王同

知圈 | 进“汽车HMI社群”请加微信15221054164，备注HMI

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