转载自：柑橘全程机械化平台
编辑：一点人工一点智能
原文：SLAM经典：FSAT-LIO
今天给大家分享IEEE Robotics and Automation Letters上发表经典文章：FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter。文章中主要介绍了香港大学火星实验室研发的一种SLAM算法—FAST-LIO。
论文地址：https://arxiv.org/abs/2010.08196v3
01 背景介绍
SLAM是随着机器人的发展而衍生出的一个概念，为的是解决机器人的自主定位和导航问题，现在已经被广泛应用在汽车自动驾驶、无人机、测绘以及AR和VR方面。SLAM又有激光SLAM和视觉SLAM之分，其主要区别是所用传感器和获取数据不同。FAST-LIO算法就是属于激光SLAM。
02 问题与方法
早在FAST-LIO之前，就已经出现了许多经典的激光SLAM算法，例如LOAM、 LeGO-LOAM等等，但文章的作者他们慢慢发现这些算法都或多或少得存在以下局限性：
1）激光雷达测量中的特征点通常是环境中的几何结构(例如，边缘和平面)。当无人机在杂乱的环境中工作时，由于没有强有力的特征，基于激光雷达的解决方案很容易发生退化。在激光雷达只具有小视场的时候，这个问题更加明显。
2）由于沿着扫描方向的高分辨率，激光雷达扫描通常包含许多特征点。虽然这些特征点并不能保证在发生退化的情况下可靠地确定姿态，但是将这些大量的特征点融合到IMU测量中依旧需要巨大的计算资源，这是UAV机载计算机难以负担的。
3）由于激光雷达通过多个激光器/接收器进行采样点，扫描中的激光点无法保证在同一时间采样，这会导致出现运动失真，也将显著降低扫描配准。另外，无人机螺旋桨和电机的持续旋转也给IMU测量带来了明显的噪声。
为了解决以上问题，文章中采取了以下方法：
1）为了应对发生退化的快速运动、噪声或杂乱环境，文章中采用紧耦合迭代卡尔曼滤波器来融合激光雷达特征点和IMU测量。
2）为了降低大量激光雷达特征点带来的巨大计算量，文章中提出了一种新的卡尔曼增益计算公式，并证明了它与传统卡尔曼增益公式的等价性。新公式的计算复杂度取决于状态维数而不是测量维数。
3）文章提出了一个反向传播过程来补偿激光雷达采样时带来的运动失真。
03 算法
FAST-LIO算法的整个过程如下图所示。在建图过程中，首先将激光雷达输入数据输入到特征提取模块，以此来获得平面特征和边缘特征。然后将提取的特征和IMU测量值输入到状态估计模块，在10Hz−50Hz下进行状态估计。之后，估计的姿态将特征点注册到全局框架中，并将它们与到目前为止构建的特征点图合并。更新后的地图会在下一步中加入更多新的点。与过去的算法明显不同的是，在IMU的状态估计模块中增加了反向传播的过程，并将反向传播所得的结果与正向传播所得结果一起输入到剩余计算中去。这其中各个部分涉及的大量数学推导，感兴趣的朋友可以去自行阅读原文。
04 实验
文章的作者们为了验证FAST-LIO的卓越性，分别进行了三个实验。
在第一个实验中，他们在其他条件不变得情况下分别使用新、旧得卡尔曼增益计算公式，并比较了最终的的计算时间和特征点的数量。结果如表所示，事实证明，新的卡尔曼增益计算公式确实可以很好地减少计算量和计算时间。
在第二个实验中，作者们将搭载了FAST-LIO算法的无人机置于室内进行飞行测试。并与LOAM算法进行比较。测试结果表明，FAST-LIO的运算速度更快、更稳定。其实验过程中无人机飞行的角速度和加速度以及最终比较结果如下图所示。
在第三个实验中，作者们将搭载了FAST-LIO算法的无人机置于室外进行飞行测试。最终该实验中的漂移率小于0.05% (在140米轨道上漂移0.07米)。该实验将扫描速率设置为 10 Hz，一次扫描的平均处理时间为25 ms，平均有1497个有效特征点。该实验对香港大学主楼的测绘结果如下图所示。
05 总结
这篇文章提出了一种基于紧耦合迭代卡尔曼滤波器的激光雷达惯性测程框架。该框架使用正向和反向传播来预测状态和补偿激光雷达扫描中的运动。此外，文章中证明并实现了一个复杂度更低，但与卡尔曼增益计算等效的公式。最后，所有的测试都证明，该算法可以提供精确、实时和可靠的导航结果。