LIO-SAM论文翻译

2023-05-16

摘要

我们提出了一个通过smoothing and mapping实现的紧耦合激光惯性里程计框架，LIO-SAM，能够取得高精度、实时的移动机器人的轨迹估计和地图构建。LIO-SAM基于因子图构建，把多个相对测量数据或者绝对测量数据，包括回环检测，以因子的形式加入到系统中进行联合优化。通过IMU预积分获得的运动估计可以用于点云运动畸变的修正和作为激光里程计优化的初值。反之，基于获得的激光里程计可以估计IMU的零偏。为了保证实时性能，当位姿估计时，我们边缘化老的激光帧；而不是将雷达点云与全局地图进行匹配。Scan-matching在局部而不是全局进行极大的提高了系统的实时性能，这是由于选择性的引入了关键帧和采用了一种非常有效的滑窗方法（将关键帧与之前固定数量的先验关键帧进行匹配）。LVI-SAM在三个平台收集的数据集上进行了评估。

1 介绍

前面省略了价值不大的部分。

在本文中，我们提出了一个通过smoothing and mapping实现的紧耦合激光惯性里程计框架，来解决上面提到的问题。首先我们假设一个非线性运动模型用于点云运动畸变的矫正，主要是使用IMU测量值估计雷达的运动。除了用于矫正点云之外，基于IMU估计的运动还可作为激光里程计优化的初值。然后再基于激光里程计估计因子图中IMU的零偏。在机器人的轨迹估计时，基于因子图，我们可以有效的使用雷达和imu的测量值进行多传感器融合，比如引入位置识别，引入GPS位置信息和指南针的朝向信息等绝对测量值。这些来自不同源的多种因子集合将会在因子图中共同优化。除此之外，我们在位姿估计时边缘化旧的激光雷达帧，而不是像LOAM一样令激光帧与全局地图匹配。Scan-matching在局部而不是全局进行极大的提高了系统的实时性能，这是由于选择性的引入了关键帧和采用了一种非常有效的滑窗方法（将关键帧与之前固定数量的先验关键帧进行匹配）。我们工作的主要贡献总结如下：

基于因子图的紧耦合激光惯性里程计框架，非常适合多传感器和全局优化。
一种有效的基于局部滑窗的scan-matching方法，其通过将筛选的新关键帧与之前固定数量的先验关键帧匹配，保证了系统的实时性。
提出的框架在不同尺度、多个平台和各种环境下进行了测试和验证。

2 相关工作

3 LIDAR INERTIAL ODOMETRY VIA SMOOTHING AND MAPPING

A 系统概述

首先我们定义贯穿全文所使用到的坐标系和符号。我们定义全局坐标系为W，并且机器人的机体坐标系为B。另外为了方便，我们假设IMU坐标系与机器人机体坐标系是一致的。机器人的系统状态x可以写为下式：

其中R是旋转矩阵，P是位置向量，v是速度，b是IMU的零偏。变换矩阵T是从机体坐标系变换到全局坐标系，也就是 $T=R|P$ 。

关于本文所提出的系统概览如图1所示。系统接收传感器数据如3D激光雷达、一个IMU和可选的GPS。基于这些传感器的观测数据我们估计机器人的状态及其轨迹。状态估计问题最终被构建为最大后验估计问题（MAP）。We use a factor graph to model this problem，as it is better suited to perform inference when compared with Bayes nets。基于高斯噪声模型的假设，the MAP inference问题等价于求解最小二乘问题【18】。注意，我们的系统可以不失一般性地引入其它传感器的测量数据，例如高度计的海拔数据、指南针的朝向数据。

在构建因子图的时候，我们为机器人的状态变量引入了四种类型的因子。这四种因子如下：（a）IMU预积分约束，（b）激光里程计约束，（C）GPS因子约束，（d）回环检测因子。当机器人的位姿改变超过了预定的阈值，则一个新的机器人状态就被添加到因子图中。upon the inserton of a new node，因子图就会使用incremental smoothing and mapping with the Bayes tree (iSAM2)【19】进行优化。下面部分重点介绍了这些因子的产生过程。

B IMU预积分

IMU的角速度和加速度的测量值在等式（2）和等式（3）中定义：

其中 $\hat{w}_{t}$ 和 $\hat{a}_{t}$ 是机体坐标系在t时刻IMU的原始测量值。 $\hat{w}_{t}$ 和 $\hat{a}_{t}$ 会收到缓慢变换的零漂 $b_{t}$ 和白噪声的影响 $n_{t}$ 。 $R_{t}^{BW}$ 是从世界坐标系到机体坐标系的旋转矩阵。g是世界坐标系下的不变量重力加速度。

我们可以用IMU的测量值去推断机器人的运动。机器人在 $t+\Delta t$ 时刻的速度、位置和旋转计算公式如下：

其中， $R_{t}=R_{t}^{WB}=R_{t}^{BW^{T}}$ ，这里我们假设在积分过程中，IMU的测量值角速度和加速度是不变的。

然后我们基于IMU的预积分【20】，获取两个时间戳之间的相对位姿。在时刻i和时刻j之间，预积分 $\Delta v_{ij}$ $\Delta p_{ij}$ 和 $\Delta R_{ij}$ 的测量值使用如下公式计算：

由于篇幅限制，推荐读者进一步阅读【20】了解方程7~9的详细推导过程。Besides its efficiency，IMU预积分也给我们提供了一种IMU预积分因子（因子图约束）。IMU零偏与雷达里程计因子一起在因子图中共同优化。

C 激光惯性里程计

当我们接收到一帧新的激光雷达帧的时候，我们首先执行特征提取。通过计算局部区域中点的曲率提取边和面特征。拥有较大曲率的点被划分为边特征。同样的，拥有较小曲率的点被归为面特征。我们以 $F_{i}^{e}$ 和 $F_{i}^{p}$ 表示t时刻相应的面特征和边特征。在时刻i提取的所有特征构成了激光雷达帧 $F_{i}$ 。需要注意的是激光雷达帧F位于机体坐标系B下。关于特征提取更详细的细节可以参考【1】或者【7】。

把每帧的激光雷达帧都加入到因子图中进行计算是非常棘手的，所以我们这里采用了关键帧的概念（在VSLAM中应用比较广泛）。基于一个简单但非常有效的启发式方法，当机器人的位姿变化超过了预定的阈值，我们选择 $F_{i+1}$ 作为关键帧。新保存的关键帧 $F_{i+1}$ ，与因子图中机器人新的状态节点关联。两个关键帧之间的激光雷达帧则被忽略了。采用这个办法添加关键帧，不仅实现了地图稠密程度和内存消耗的均衡，而且也有助于维护一个相对稀疏的因子图（这个在实时非线性优化中是非常有用的）。在我们的工作中，选择关键帧的位移和旋转的阈值时1m和10°。

现在我们假设我们把一个新的状态节点 $x_{i+1}$ 添加到了因子图中，与该状态关联的关键帧是 $F_{i+1}$ 。产生激光里程计因子的详细步骤如下。

1 Sub-keyframes for voxel map

基于滑窗法，我们构建了一个包含固定数量的最近激光帧的点云地图。不是通过优化两个连续激光帧之间的相对变换，而是最多提取n个最近的关键帧（我们称之为子关键帧）进行估计。使用与子关键帧{ $F_{i-n}$ ， $F_{i}$ }相关的变换矩阵{ $T_{i-n}$ ， $T_{i}$ }，我们把子关键帧转换到世界坐标系W中。转换之后的子关键帧就构成了一个voxel map $M_{i}$ 。由于我们之前对关键帧提取过特征，因此voxel map $M_{i}$ 是由两个sub voxel map构成的，也就是the edge feature voxel map $M_{i}^{e}$ ，和the planar feature voxel map $M_{i}^{p}$ 。激光帧与voxels map的关系如下所示：

其中 $^{'}F_{i}^{e}$ 和 $^{'}F_{i}^{p}$ 是世界坐标系变换之后的边和面特征。另外， $M_{i}^{p}$ 和 $M_{i}^{e}$ 接下来需要进行下采样以消除落入同一个voxel cell中的重复特征。在本文中我们令n=25， $M_{i}^{e}$ 和 $M_{i}^{p}$ 相应的下采样分辨率分别为0.2米和0.4米。

2 scan-matching

我们对新接收到的激光雷达帧 $F_{i+1}$ （包括了 $F_{i}^{e}$ 和 $F_{i}^{p}$ ），通过scan-matching与地图 $M_{i}$ 匹配。有很多的方法可以实现这个目的，比如【3】和【4】。这里我们选择使用方法【1】出于其在复杂极具挑战的环境下鲁棒性很好以及计算效率比较高的缘故。

我们首先把激光帧中的特征从机体坐标系转换到世界坐标系下，并且获得了 $^{'}F_{i+1}^{e}$ 和 $^{'}F_{i+1}^{p}$ 。使用的变换矩阵来自于IMU， $\tilde{T}_{i+1}$ 。对于 $^{'}F_{i+1}^{e}$ 和 $^{'}F_{i+1}^{p}$ ，我们在地图中找到与之相关的特征， $M_{i}^{e}$ 和 $M_{i}^{p}$ 。出于简洁的目的，关于如何确定相关性的细节这里就省略了，不过在【1】中有详细的描述。

3 Relative transformation

特征与其对应的边缘或平面特征之间的距离可以用以下公式计算:

其中k、u、v和w是他们对应集合中的特征索引。 For an edge feature $P_{i+1, k}^{e}$ in $^{'}F_{i+1}^{e}$ , $P_{i,u}^{e}$ 和 $P_{i,v}^{e}$ are the points that form the corresponding edge line in $M_{i}^{e}$ . For a planar feature $P_{i+1, k}^{p}$ in $^{'}F_{i+1}^{p}$ ， $P_{i,u}^{e}$ 、 $P_{i,v}^{e}$ 和 $P_{i,w}^{e}$ form the corresponding planar patch in $M_{i}^{p}$ 。我们采用高斯牛顿法通过最小化下面的方程来求解最优变换。

最后，我们获得了状态 $x_{i}$ 和 $x_{i+1}$ 的相对变换 $\Delta T_{i, i+1}$ ，这是连接这两个位姿的激光雷达里程计因子:

我们注意到了还有其他方法如通过把子关键帧转换到 $x_{i}$ 坐标系下获得变换矩阵 $\Delta T_{i, i+1}$ 。也就是说，我们令 $F_{i+1}$ 与在坐标系下 $x_{i}$ 的体素地图进行匹配。这样我们可以直接获得真实的变换 $\Delta T_{i, i+1}$ 。不过由于变换后的特征 $^{'}F_{i+1}^{e}$ 和 $^{'}F_{i+1}^{p}$ 可以被重复使用很多次，因此出于计算效率，我们选择使用方Sec. III-C.1中描述的方法。

D GPS因子

尽管基于IMU的预积分和激光里程计因子，我们可以获得比较准确的状态估计和建图，但该系统会在长时间导航任务中会发生缓慢漂移。为了解决这个问题，我们引入提供绝对测量值的传感器消除漂移。这样的传感器包括了GPS、罗盘和高度计。出于说明的目的，我们这里仅仅讨论GPS，由于它在真实导航系统中应用比较广泛。

当我们接收到GPS数据时，我们首先把他们基于【21】的方法转换到Cartesian坐标系。当因子图中新增一个节点时，我们把GPS因子与该节点相关联。如果GPS信号没有同激光雷达帧硬同步，我们则基于激光雷达帧的时间戳对GPS数据进行线性插值。

我们注意到一有GPS数据就在因子图中添加GPS因子是没有必要的，这是因为雷达惯性里程计的漂移变换很缓慢。在实际中，当估计的位置协方差比GPS位置的协方差大很多的时候，我们才在因子图中添加一个GPS因子。

E 回环检测因子

由于因子图的使用，回环检测模块可以无缝的嵌入到我们所提出的系统中，而不是像LOAM和LIOM一样。For the purposes of illustraion，我们描述和实现了一种简单但非常有效的基于欧氏距离的回环检测方法。另外我们也注意到了我们所提出的方法与其他回环检测方法是兼容的，比如【22】和【23】，其产生了一个点云的描述子并用于位置识别。

当一个新的状态 $x_{i+1}$ 被添加到因子图中的时候，我们首先搜索因子图并在欧式空间中找到与 $x_{i+1}$ 非常接近的先验状态。如图1所示，当返回的其中之一的候选状态是 $x_{3}$ 时，我们令 $F_{i+1}$ 与关键帧{ $F_{3-m}$ $F_{m}$ $F_{3+m}$ }进行匹配。Note that $F_{i+1}$ and the past sub-keyframes are first transformed into W before scan-matching。我们得到了一个相对变换，并把它作为一个回环因子添加到因子图中。在整个论文中，我们选择m=12，搜索回环检测的距离设置为15米。

在实际应用中，当GPS是唯一的绝对传感器时，我们发现基于回环检测去修正机器人的海拔高度是非常有用的。这是因为GPS给出的海拔高度是非常不准确的，在我们的测试中误差最大高达100米（没有回环检测因子参与）。

4 实验

5 结论

参考文献

[1] J. Zhang and S. Singh, “Low-drift and Real-time Lidar Odometry and Mapping,” Autonomous Robots, vol. 41(2): 401-416, 2017.
[2] A. Geiger, P. Lenz, and R. Urtasun, “Are We Ready for Autonomous Driving? The KITTI Vision Benchmark Suite”, IEEE International Conference on Computer Vision and Pattern Recognition, pp. 3354-3361, 2012.

[3] P.J. Besl and N.D. McKay, “A Method for Registration of 3D Shapes,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol.14(2): 239-256, 1992.
[4] A. Segal, D. Haehnel, and S. Thrun, “Generalized-ICP,” Proceedings of Robotics: Science and Systems, 2009.
[5] W.S. Grant, R.C. Voorhies, and L. Itti, “Finding Planes in LiDAR Point Clouds for Real-time Registration,” IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 4347-4354, 2013.
[6] M. Velas, M. Spanel, and A. Herout, “Collar Line Segments for Fast Odometry Estimation from Velodyne Point Clouds,” IEEE International Conference on Robotics and Automation, pp. 4486-4495, 2016.
[7] T. Shan and B. Englot, “LeGO-LOAM: Lightweight and Groundoptimized Lidar Odometry and Mapping on Variable Terrain,” IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 4758-4765, 2018.
[8] T. Shan, J. Wang, K. Doherty, and B. Englot, “Bayesian Generalized Kernel Inference for Terrain Traversability Mapping,” In Conference on Robot Learning, pp. 829-838, 2018.
[9] S. Lynen, M.W. Achtelik, S. Weiss, M. Chli, and R. Siegwart, “A Robust and Modular Multi-sensor Fusion Approach Applied to MAV Navigation,” IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 3923-3929, 2013.
[10] S. Yang, X. Zhu, X. Nian, L. Feng, X. Qu, and T. Mal, “A Robust Pose Graph Approach for City Scale LiDAR Mapping,” IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 1175-1182, 2018.
[11] M. Demir and K. Fujimura, “Robust Localization with Low-Mounted Multiple LiDARs in Urban Environments,” IEEE Intelligent Transportation Systems Conference, pp. 3288-3293, 2019.
[12] Y. Gao, S. Liu, M. Atia, and A. Noureldin, “INS/GPS/LiDAR Integrated Navigation System for Urban and Indoor Environments using Hybrid Scan Matching Algorithm,” Sensors, vol. 15(9): 23286-23302, 2015.
[13] S. Hening, C.A. Ippolito, K.S. Krishnakumar, V. Stepanyan, and M. Teodorescu, “3D LiDAR SLAM integration with GPS/INS for UAVs in urban GPS-degraded environments,” AIAA Infotech@Aerospace Conference, pp. 448-457, 2017.
[14] C. Chen, H. Zhu, M. Li, and S. You, “A Review of Visual-Inertial Simultaneous Localization and Mapping from Filtering-Based and Optimization-Based Perspectives,” Robotics, vol. 7(3):45, 2018.
[15] C. Le Gentil,, T. Vidal-Calleja, and S. Huang, “IN2LAMA: Inertial Lidar Localisation and Mapping,” IEEE International Conference on Robotics and Automation, pp. 6388-6394, 2019.
[16] C. Qin, H. Ye, C.E. Pranata, J. Han, S. Zhang, and Ming Liu, “LINS: A Lidar-Inertial State Estimator for Robust and Efficient Navigation,” arXiv:1907.02233, 2019.
[17] H. Ye, Y. Chen, and M. Liu, “Tightly Coupled 3D Lidar Inertial Odometry and Mapping,” IEEE International Conference on Robotics and Automation, pp. 3144-3150, 2019.
[18] F. Dellaert and M. Kaess, “Factor Graphs for Robot Perception,” Foundations and Trends in Robotics, vol. 6(1-2): 1-139, 2017.
[19] M. Kaess, H. Johannsson, R. Roberts, V. Ila, J.J. Leonard, and F. Dellaert, “iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree,” The International Journal of Robotics Research, vol. 31(2): 216-235, 2012.
[20] C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “On-Manifold Preintegration for Real-Time Visual-Inertial Odometry,” IEEE Transactions on Robotics, vol. 33(1): 1-21, 2016.
[21] T. Moore and D. Stouch, “A Generalized Extended Kalman Filter Implementation for The Robot Operating System,” Intelligent Autonomous Systems, vol. 13: 335-348, 2016.
[22] G. Kim and A. Kim, “Scan Context: Egocentric Spatial Descriptor for Place Recognition within 3D Point Cloud Map,” IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 4802-4809, 2018.
[23] J. Guo, P. VK Borges, C. Park, and A. Gawel, “Local Descriptor for Robust Place Recognition using Lidar Intensity,” IEEE Robotics and Automation Letters, vol. 4(2): 1470-1477, 2019.
[24] M. Quigley, K. Conley, B. Gerkey, J. Faust, T. Foote, J. Leibs, R. Wheeler, and A.Y. Ng, “ROS: An Open-source Robot Operating System,” IEEE ICRA Workshop on Open Source Software, 2009.
[25] T. Qin, P. Li, and S. Shen, “Vins-mono: A Robust and Versatile Monocular Visual-Inertial State Estimator,” IEEE Transactions on Robotics, vol. 34(4): 1004-1020, 2018.

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)