LIO-SAM论文阅读

Catalogue
  1. 1. LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping
  2. 2. 摘要
  3. 3. 对比介绍
  4. 4. LIO-SAM系统
    1. 4.1. 系统概述
      1. 4.1.1. IMU因子
      2. 4.1.2. Lidar里程计因子
      3. 4.1.3. GPS因子
      4. 4.1.4. 回环因子
  5. 5. 论文展示的效果图

LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping

摘要

提出了基于因子图的紧耦合的Lidar-IMU系统,允许大量的相对或绝对的观测(来自不同传感器的观测如回环检测、GPS等)作为因子添加到系统中。

为了确保实时性能,对位姿优化采用边缘化旧的激光雷达扫描操作,而不是使用激光扫描来匹配全局点云地图。

使用小范围的局部Scan-Matching来代替全局的Scan-Matching明显提高实时性能,其他还有关键帧的选取、滑动窗口方法(将新的关键帧与固定size的先验子关键帧之间进行配准)

对比介绍

LOAM虽然达到了state-of-art的性能,但是也有一些缺点,通过储存数据到全局体素地图中,通常难以进行回环检测以及和其他绝对的观测进行耦合。

Despite its success, LOAM presents some limitations - by saving its data in a global voxel map, it is often difficult to perform loop closure detection and incorporate other absolute measurements, e.g., GPS, for pose correction.

此外,在特征丰富的环境中,体素地图变得稠密,使其在线优化处理过程效率变得less efficient,LOAM对于大环境下,也会产生漂移,毕竟仅仅是scan-matching的方法为核心。

后面主要对LOAM和LIOM进行了简要的对比,可参见原文。。。

LIO-SAM系统

系统概述

  • \(W\):世界坐标系
  • \(B\):机器人坐标系
  • 假设:IMU坐标系与机器人坐标系一致
  • \(x=[R^T,p^T,v^T,b^T]^T\)
  • \(R \in SO(3)\):从机器人坐标系到世界坐标系的旋转变换
  • \(p\):平移量
  • \(T=[R|p]\in SO(3)\):从机器人坐标系到世界坐标系的变换

系统使用因子图来对SLAM问题进行建模,在高斯噪声模型的假设下,我们问题的最大后验概率推理等价于求解一个非线性最小二乘问题,提出的系统也可以整合其他传感器如高度计、电子罗盘等传感器的测量值。

提出的系统使用了4种因子:

  • IMU预积分因子
  • Lidar里程计因子
  • GPS因子
  • 回环因子

IMU因子

IMU测量的角速度、加速度如下:

其中,

  • \(\hat{w}_t,\hat{a}_t\):表示机体坐标系下的IMU测量值
  • \(b_t^w\):陀螺仪bias
  • \(b_t^a\):加速度计bias
  • \(n_t^w,n_t^a\):白噪声
  • \(R_t^{BW}\):从世界坐标系到机体坐标系的旋转矩阵
  • \(g\):世界坐标系\(W\)的重力向量

可推断出\(\Delta t\)时刻后的状态:

其中

  • \(R_t=R_t^{WB}=(R_t^{BW})^T\)
  • 在这里,假设机体坐标系的角速度以及加速度在积分区间是恒定的

IMU预积分

预积分的观测值:\(\Delta v_{ij},\Delta p_{ij},\Delta R_{ij}\),可以理解为:在超级短的时间内,IMU的相邻两个时刻之间的测量值可认为是准确的,可作为观测值。

由于篇幅限制,作者没有给出更加详细的说明,推荐阅读[19]的式7-9

On-Manifold Preintegration for Real-Time Visual-Inertial Odometry,” IEEE Transactions on Robotics, vol. 33(1): 1-21, 2016.

IMU预积分除了可以减少运算时间,同时还可以向因子图中提供一种约束

Lidar里程计因子

当新的一帧激光扫描产生,首先进行特征提取,可以通过计算点的粗糙度来进行边缘线以及平面点的提取,粗糙度值较大的会作为边缘线上的点。

  • \(F_i^e\):在时间i的扫描中的边缘线点
  • \(F_i^p\):在时间i的扫描中的平面点
  • \(\mathbb{F}_i=\{F_i^e,F_i^p\}\)
  • 以上的特征都是在机体坐标系\(B\)下的描述
  • 更加详细的描述,可参见Loam和Lego-Loam

J. Zhang and S. Singh, “Low-drift and Real-time Lidar Odometry andMapping,” Autonomous Robots, vol. 41(2): 401-416, 2017. 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

关键帧阈值设置:

  • 平移1m
  • 旋转10度

假设准备添加新的状态节点\(x_{i+1}\),Lidar关键帧与状态节点\(x_{i+1}\)之间的关联使用\(\mathbb{F}_{i+1}\)表示,Lidar里程计因子如下描述:

  1. 滑窗关键帧:实现了滑动窗口方法来保持固定数量的最近几帧扫描来创建点云地图,与只优化相邻的两帧的方法来相比,系统采用最近的n帧来优化,称为子关键帧(sub-KeyFrames),sub-KeyFrames=\(\{\mathbb{F}_{i-n},\dots,\mathbb{F}_{i}\}\)使用对应的\(\{T_{i-n},\dots,T_{i}\}\)来变换到世界坐标系\(W\)下,然后合并成体素地图\(M_i\)

由于我们提取了两种特征,因此体素地图\(M_i\)由:

  • \(M_i^e\):边缘特征体素地图
  • \(M_i^p\):平面特征体素地图

来组成,其关系如下:

  • \('F_i^e,'F_i^p\):变换到世界坐标系\(W\)的边缘特征和平面特征

然后,对\(M_i^e,M_i^p\)进行降采样,以消除落在同一个体素网格中的重复特征点,在本文中,采用的滑动窗口大小\(n=25\),对\(M_i^e,M_i^p\)降采样的分辨率分别是\(0.2m\)\(0.4m\)

  1. 扫描匹配:对新的LIdar帧\(\mathbb{F}_{i+1}\),也就是\(\{F_i^e,F_i^p\}\)匹配到局部体素地图\(M_i\),系统采用的是LOAM的方法。

首先,将第i+1帧雷达扫描的特征数据从机体坐标系\(B\)转换到世界坐标系\(W\),得到\('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\)寻找对应的边缘或平面特征,具体的,参考LOAM论文描述。

  1. 相对变换

边缘线和平面块的特征距离可表示成如下:

其中,\(k,u,v,w\)是对应的特征关联集合

对于\('F_{i+1}^e\)中的特征点\(p_{i+1,k}^e\)\(p_{i,u}^e\)\(p_{i,v}^e\)是在\(M_i^e\)中与之对应的特征关联边缘线上的点

第i+1帧的特征\(\mathbb{F}_{i+1}\)及其对应的特征之间的几何相对关系可描述为:

最后,通过LM迭代求解得到\(T_{i+1}\),进一步的,可以得到关键帧节点\(x_i\)\(x_{i+1}\)之间的相对变换:

另外,作者还发现一种可替换的方法来获取\(\Delta T_{i,i+1}\),就是变换sub-KeyFrames到\(x_i\)节点。换句话说,直接匹配第i+1帧的特征\(\mathbb{F}_{i+1}\)到节点\(x_i\)所表示的体素地图。 使用这种方式,可以直接获得\(\Delta T_{i,i+1}\),因为变换后的特征\('F_i^e,'F_i^p\)可以被重复使用,我们使用这种方法来替换(1)中描述的方法,以提高计算效率。

GPS因子

当接受到GPS测量时,首先将GPS数据转换到局部的笛卡尔坐标系,使用文献[20]中的方法。当新的节点加入到因子图,然后得到对应的GPS测量值,也就是GPS因子。如果GPS信号没有与Lidar进行硬件同步,那么进行插值同步。

A Generalized Extended Kalman Filter Implementation for The Robot Operating System. In Intelligent Autonomous Systems, vol. 13: 335-348, 2016.

作者注意到,当GPS信号可用时,一直加入GPS因子并不是必要的,在实际使用中,只有在估计的位姿协方差大于GPS位置协方差的时候,才加入GPS因子。

回环因子

得益与因子图的使用,回环检测可以无缝地整合到所提出的系统,为了说明的目的,我们描述并实现了一种朴素但有效的基于距离的环闭检测方法。

当新的状态节点\(x_{i+1}\)添加到因子图中:

  1. 我们首先搜索图,然后寻找在欧几里德空间上与节点\(x_{i+1}\)距离相近的先验状态,如图2所示,例如\(x_3\)
  2. 然后尝试匹配\(x_{i+1}\)时刻的激光扫描特征\(\mathbb{F}_{i+1}\)到附近的关键帧\(\{\mathbb{F}_{3-m},\dots,\mathbb{F}_3,\dots,\mathbb{F}_{3+m}\}\),(注意:\(\mathbb{F}_{i+1}\)以及附近关键帧sub-KeyFrames需要先转换到世界坐标系\(W\)中)。在这一步,可以得到第i+1节点与第3节点之间的变换\(\Delta T_{3,i+1}\)
  3. \(\Delta T_{3,i+1}\)作为回环因子添加到因子图中
  4. 在本系统,选择\(m=12\),回环搜索距离设置为\(15m\)

在实际应用中,作者发现添加回环因子对于矫正高度十分有用,当GPS是唯一的绝对观测传感器时。这是因为GPS的高度测量非常不准确——在作者的测试中,在没有环闭的情况下,会出现接近100米的高度误差。

笔者:实际测试,也没有这么大的偏差吧??

论文展示的效果图