LIO-SAM: 通过SAM紧耦合雷达惯性里程计

摘要

我们提出一个框架用于通过SAM紧耦合雷达惯性里程计LIO-SAM，可实现高精度、实时的移动机器人轨迹估计和地图构建．LIO-SAM是基于因子图构建雷达惯性里程计，可以将大量的相对测量值、绝对测量值、回环等多种不同数据作为因子融入雷达惯性里程计系统中．通过imu预积分获得的运动估计可以用于点云的偏斜矫正和用于雷达里程计优化的初始值．获得的雷达里程计结果又可以反过来估计imu的偏置．为了确保实时高性能，我们在进行位姿优化时边缘化掉了一些老的雷达数据，而不是将雷达点云与整个地图进行匹配．Scan-matching 在局部范围内进行而不是在全局范围内进行，可以有效的提高系统的实时性，原因是因为选择性引入关键帧和有效的滑动窗口策略(只将新的关键帧与固定尺寸的多个先前关键帧匹配)．提出的方法已经在拥有多种场景多种范围的三个测试平台采集的数据集上进行了广泛的评估测试．

1. 介绍

对于一个可以反馈控制、避障、轨迹规划和其它应用的智能移动机器人，状态估计、定位和建图是基本的要求．使用基于视觉和基于雷达的传感技术，巨大的努力已经被投入到支持移动机器人进行6*度估计的SLAM技术中．基于视觉的方法一般是使用一个单目相机或立体相机，通过连续采集的照片三角化特征点，从而估计相机的位姿．虽然基于视觉的方法非常适合于位置识别，但对初始化、光照、范围等条件特别敏感，以致于单独使用它们去支持一个自动导航系统是不可靠的．另一方面，基于雷达的方法对于光照的改变是没有影响的．特别是最近长距离高分辨率的3D激光雷达(像Velodyne VLS-128和Ouster OS1-128)已经出现了，使得雷达变得更加适合于直接在3D空间获取精细的环境信息．因此，这篇论文集中在基于雷达的状态估计和建图方法．
许多的基于激光雷达的状态估计和建图方法在最近20年被提出来．其中LOAM(Lidar Odometry And Mapping)方法[1]对于小漂移实时状态估计和建图是最被广泛使用的．LOAM使用一个雷达和一个imu实现了最好的性能，自从它发布在KITTI odometry benchmark site上，就一直排在最上面．虽然LOAM算法很成功，但依然存在一些限制．首先是它保存地图在一个全局体素地图中，其次它没法进行回环检测，也没法融入其它的像GPS这样的绝对测量数据进行位姿矫正．而且LOAM也存在在大场景测试中偏移问题，原因是因为它核心还是一个基于scan-matching的方法．
在这篇论文中，我们提出了一个通过SAM紧耦合雷达惯性里程计框架LIO-SAM，用于解决上面提到的问题．我们假设了一个非线性运动模型用于点云的偏斜矫正，该模型是在一次激光雷达扫描过程中使用imu测量数据进行了雷达的运动估计．该运动估计除了用于点云的偏斜矫正而且还可以作为雷达里程计优化过程中的初始位姿估计．获得的雷达里程计结果反过来被用于估计因子图中imu的偏置．通过引入因子图用于机器人的轨迹估计，我们可以进行有效的基于雷达和imu作为测量值的传感器融合，比方说引入位置识别，引入GPS位置信息和指南针的朝向信息等绝对测量值．这些来自不同数据源的多种因子集合将会在因子图*同优化．此外，我们在进行位姿优化时，边缘化掉老的雷达帧，而不是像LOAM那样将雷达帧同整个全局点云进行匹配．Scan-matching是在局部范围而不是全局范围内执行，明显的提升了系统的实时性，因为采取了关键帧的选择引入和有效的滑动窗口方法(只将新的关键帧与固定尺寸的多个先前关键帧匹配)．我们工作的主要贡献可以被总结如下:

• 一个基于因子图的紧耦合的雷达惯性里程计框架，适合多传感器融合和全局优化．
• 一个有效的基于局部滑窗的scan-matching方法，其通过匹配选择的关键帧和估计数量的先前关键帧策略，确保了系统的实时性
• 提出的框架已在多范围、多设备和多种环境下进行了广泛的测试验证．

2. 相关工作

雷达里程计典型实现是通过使用scan-matching方法找到两个相邻帧之间的相对转换关系，这样的方法有ICP、GICP等．除了匹配完整点云，基于特征匹配的方法由于其计算的有效性已经变得流行起来．例如，[5]是一个基于平面特征匹配的雷达里程计，假设运行场景在结构化的环境中，其从点云中提取平面特征，并使用最小二乘法对特征进行匹配。[6]是一个基于线特征的雷达里程计，其是从原始点云里面随机分割线特征，然后再进行匹配。然而由于现代3D激光雷达使用旋转机械件和设备的移动，导致点云经常会有偏斜。因此，仅仅使用激光雷达进行位姿估计是不理想的，原因是因为使用偏斜的点云或特征进行匹配必将会造成大的漂移。
因此，雷达通常是与其它传感器(例如IMU、GPS)融合使用来进行状态估计和建图。这样一种利用传感器融合思路的方法可以分为两类:松耦合融合和紧耦合融合。例如LOAM[1]算法，其使用IMU进行点云的偏斜矫正和为扫描匹配提供运动先验，然而，IMU并没有引入到算法的优化处理过程中，因此LOAM可以被归类为松耦合方法。另一个轻量的、基于地面优化的雷达里程计和建图算法LeGO-LOAM[7]被用于地面车辆的建图任务，该算法融合IMU测量的方法同LOAM是一致的。一种更加流行的松耦合方法是使用EKF，例如[9]-[13]在机器人状态估计的优化阶段使用EKF算法融合雷达、IMU或GPS测量值。
紧耦合系统通常能够提供更高的精度，并且是当前的主要研究方向。在[15]方法中，预积分的IMU测量值被用于对点云进行偏斜矫正。[16]提出了一种以机器人为中心的雷达惯性里程计R-LINS，其用一种紧耦合的方式使用ESKF算法对机器人的状态进行矫正，由于缺乏其它可用于状态估计的传感器，该算法在经过长时间导航后会有漂移问题。[17]提出了一个紧耦合的雷达惯性里程计框架LIOM(雷达惯性里程计建图)，其共同优化雷达和IMU的测试值，实现了比LOAM更好的精度。因为LIOM的设计是处理所有的传感器测量，因此，它不能实时运行。

3. 基于SAM的雷达惯性里程计

A. 系统概述

我们首先定义我们在整个论文中使用的坐标系和符号。我们定义/mathbf{W}为世界坐标系，/mathbf{B}为机器人坐标系。为了方便，我们也假设IMU坐标系和机器人本体坐标系是一致的。机器人的状态\mathbf{x}可按下面定义:

式中 R ∈ S O ( 3 ) \mathbf{R}\in SO(3) R∈SO(3)是旋转矩阵， p ∈ R 3 \mathbf{p}\in\mathbf{R}^{3} p∈R3是位置向量， v \mathbf{v} v是速度， b \mathbf{b} b是IMU偏置。相对转换 / m a t h b f T ∈ S E ( 3 ) /mathbf{T}\in SE(3) /mathbfT∈SE(3)从 B \mathbf{B} B到 W \mathbf{W} W是被表示为 T = [ R ∣ p ] \mathbf{T = [R | p]} T=[R∣p].
我们提出的系统概览如图1所示。系统接收来自3D雷达、IMU(可选择的GPS)等传感器的数据。我们争取使用这些传感器数据估计出机器人的状态和轨迹。这个状态估计问题可以被构造为一个最大化后验问题(MAP)。我们使用一个因子图去刻画这个问题，因为它是最适合进行贝叶斯网络的推断。由于高斯模型假设，我们的MAP推断问题与求解非线性最小二乘问题是等价的。注意，我们的系统可以不失一般性地引入其它传感器的测量数据，例如高度计的海拔数据、指南针的朝向数据。
对于因子图的构造，我们对一个变量引入四种因子。该表示在指定时刻机器人状态的变量对应因子图中的节点。四种类型的因子分别是：(a) IMU预积分因子，(b) 雷达里程计因子，© GPS因子和(d)回环因子。当机器人的位姿改变超过人为设定的阈值，则添加新的机器人状态节点 x \mathbf{x} x到因子图中。一旦插入了新的节点，因子图就会使用iSAM2进行优化。下面部分将介绍产生这些因子的过程。

B. IMU预积分因子

来自于IMU的角速度和加速度测量值定义如等式(2)和(3):

式中 ω ^ t \hat{\boldsymbol{\omega}}_{t} ω^t​和 α ^ t \hat{\boldsymbol{\alpha}}_{t} α^t​是IMU在时刻 t t t和 B \mathbf{B} B坐标系下的测量值。 ω ^ t \hat{\boldsymbol{\omega}}_{t} ω^t​和 α ^ t \hat{\boldsymbol{\alpha}}_{t} α^t​是受缓慢变化的偏置 b t \mathbf{b}_{t} bt​和白噪声 n t \mathbf{n}_{t} nt​影响的． R t B W \mathbf{R}_{t}^{\mathbf{BW}} RtBW​是从坐标系 W \mathbf{W} W到 B \mathbf{B} B的旋转矩阵． g \mathbf{g} g是在 W \mathbf{W} W坐标系下固定的重力向量．
我们现在可以使用IMU测量值进行机器人运动的推断．机器人在时刻 t + Δ t t+\Delta t t+Δt的位置、姿态和速度计算方法如下:

式中 R = R t W B = R t B W T \mathbf{R}=\mathbf{R}_{t}^{\mathbf{WB}}={\mathbf{R}_{t}^{\mathbf{BW}}}^{T} R=RtWB​=RtBW​T，并且我们假设基座 B \mathbf{B} B的角速度和加速度在上述积分过程中是不变的．
我们然后应用在[20]中提出的IMU预积分方法去获得机体在相邻时间戳内的相对运动．其中在时刻 i i i和 j j j之间的预积分测量值 Δ v i j , Δ p i j \Delta \mathbf{v}_{ij},\Delta\mathbf{p}_{ij} Δvij​,Δpij​和 Δ R i j \Delta \mathbf{R}_{ij} ΔRij​可以按下面公式计算:

由于篇幅的限制，读者们可以阅读[20]获得更多的关于公式(7)~(9)的推导．除了有效性外，应用IMU预积分也自然的给我们一种因子图约束-IMU预积分因子．IMU的偏置是同雷达里程计因子一起在因子图*同优化．

C. 雷达里程计因子

当接收到一个新的雷达扫描数据，我们首先通过评估一个局部区域中点的凹凸程度来提取边和面特征点，当点的凹凸程度较大，则将这些点分类为边特征点．类似的，当凹凸程度较小时，这些点将被分类为面特征点．我们将从在时刻 i i i获取的雷达点云中提取的边特征点和面特征点集合分别表示为 F i e , F i p \mathbf{F}_{i}^{e},\mathbf{F}_{i}^{p} Fie​,Fip​．在时刻 i i i提取的所有特征点构成了雷达帧 F i = [ F i e , F i p ] \mathbf{F}_{i}={[\mathbf{F}_{i}^{e},\mathbf{F}_{i}^{p}]} Fi​=[Fie​,Fip​]．注意雷达帧 F \mathbf{F} F是表达在 B \mathbf{B} B坐标系下．关于特征点提取的详细描述可查看[1][7].
如果将每一个雷达帧进行计算并添加因子到因子图中进行优化是很难计算的，因此我们使用关键帧策略，该策略已在视觉SLMA领域广泛使用．当机器人的位姿相对于先前状态 x i \mathbf{x}_{i} xi​的改变超过用户设定的阈值，则选择对应的雷达帧 F i + 1 \mathbf{F}_{i+1} Fi+1​作为新的关键帧，新的关键帧与因子图中的一个新的机器人状态节点 x i + 1 \mathbf{x}_{i+1} xi+1​相关联的，相邻两个关键帧之间的雷达帧被丢弃．这样添加关键帧的方法不仅实现了地图密度和内存消耗的平衡，而且帮助维护了一个相对稀疏的适用于实时非线性优化的因子图．在我们的工作中，用于添加关键帧的位置和旋转改变阈值参数分别设置为1米和10度．
假设我们在因子图中添加一个新的状态节点 x i + 1 \mathbf{x}_{i+1} xi+1​，其相关的雷达关键帧为 F i + 1 \mathbf{F}_{i+1} Fi+1​.则产生一个雷达里程计因子的步骤如下:
(1)体素地图的子关键帧: 我们使用滑窗的方法产生一个点云地图，该地图包含固定数量的最近的雷达帧．而不是直接通过两个相邻的雷达帧来获得相对转换关系，我们提取了n个最近的关键帧(我们称之为子关键帧)用于估计相对转换关系．子关键帧的集合{ F i − n , . . . , F i \mathbf{F}_{i-n},...,\mathbf{F}_{i} Fi−n​,...,Fi​}然后使用和它们相关的转换关系{ T i − n , . . . , T i \mathbf{T}_{i-n},...,\mathbf{T}_{i} Ti−n​,...,Ti​}将它们转换到坐标系 W \mathbf{W} W．转换过的子关键帧融合到一起构成了一个体素地图 M i \mathbf{M}_{i} Mi​．因为我们在先前特征提取步骤提取了两种类型的特征，因此 M i \mathbf{M}_{i} Mi​是由两个子体素地图组成，分别是边特征体素地图 M i e \mathbf{M}_{i}^{e} Mie​和面特征体素地图 M i p \mathbf{M}_{i}^{p} Mip​，雷达帧和体素地图是按下面彼此相关的:

′ F i e '\mathbf{F}_{i}^{e} ′Fie​和 ′ F i p '\mathbf{F}_{i}^{p} ′Fip​是转换过的在坐标系 W \mathbf{W} W下的边特征和平面特征点集合． M i e \mathbf{M}_{i}^{e} Mie​和 M i p \mathbf{M}_{i}^{p} Mip​然后被降采样去除落在相同体素栅格内的重复特征点．在这篇文章中，我们设置 n n n为25．对于 M i e \mathbf{M}_{i}^{e} Mie​和 M i p \mathbf{M}_{i}^{p} Mip​的降采样分辨率分别设置为0.2米和0.4米．
(2) 扫描匹配: 我们使用扫描匹配的方法匹配新获得的雷达帧 F i + 1 = \mathbf{F}_{i+1}= Fi+1​={ F i + 1 e , F i + 1 p \mathbf{F}_{i+1}^{e},\mathbf{F}_{i+1}^{p} Fi+1e​,Fi+1p​}和 M i \mathbf{M}_{i} Mi​．有多种扫描匹配方法(例如[3][4])可以用来实现匹配．这里，我们使用[1]中提出的方法进行扫描匹配，原因是因为它的高效性和在具有挑战的环境中的鲁棒性．
我们首先将{ F i + 1 e , F i + 1 p \mathbf{F}_{i+1}^{e},\mathbf{F}_{i+1}^{p} Fi+1e​,Fi+1p​}从坐标系 B \mathbf{B} B转换到坐标系 W \mathbf{W} W，获得{ ′ F i + 1 e , ′ F i p '\mathbf{F}_{i+1}^{e},'\mathbf{F}_{i}^{p} ′Fi+1e​,′Fip​}．这个初始的转换关系是通过IMU估计的机器人运动 T ~ i + 1 \widetilde{\mathbf{T}}_{i+1} T i+1​．对于在 ′ F i + 1 e '\mathbf{F}_{i+1}^{e} ′Fi+1e​或 ′ F i + 1 p '\mathbf{F}_{i+1}^{p} ′Fi+1p​中的每一个特征点，我们然后在 M i e \mathbf{M}_{i}^{e} Mie​或者 M i p \mathbf{M}_{i}^{p} Mip​中找到对应的边或者平面．为了简单起见，详细的寻找对应关系的过程可以在论文[1]中找到．
(3) 相对转换: 一个特征和它相关的边或者平面之间的距离可按下式计算:

式中 k , u , v , w k,u,v,w k,u,v,w是特征在他们对应的集合中的索引．对于 ′ F i + 1 e '\mathbf{F}_{i+1}^{e} ′Fi+1e​中一个边特征 p i + 1 , k e \mathbf{p}_{i+1,k}^{e} pi+1,ke​， p i , u e \mathbf{p}_{i,u}^{e} pi,ue​和 p i , v e \mathbf{p}_{i,v}^{e} pi,ve​是在 M i e \mathbf{M}_{i}^{e} Mie​中构成对应边特征的两个点．对于在 ′ F i + 1 p '\mathbf{F}_{i+1}^{p} ′Fi+1p​中的一个平面特征 p i + 1 , k p \mathbf{p}_{i+1,k}^{p} pi+1,kp​， p i , u p , p i , v p \mathbf{p}_{i,u}^{p},\mathbf{p}_{i,v}^{p} pi,up​,pi,vp​和 p i , w p \mathbf{p}_{i,w}^{p} pi,wp​是在 M i p \mathbf{M}_{i}^{p} Mip​中构成对应平面特征的三个点．然后高斯牛顿法被用于求解最优转换通过最小化下面残差模型:

最后，我们可以获得状态 x i \mathbf{x}_{i} xi​和 x i + 1 \mathbf{x}_{i+1} xi+1​之间的相对转换 Δ T i , i + 1 \Delta \mathbf{T}_{i,i+1} ΔTi,i+1​，该转换是连接两个位姿的雷达里程计因子:

注意，有一个可选择的方法去获得 Δ T i , i + 1 \Delta \mathbf{T}_{i,i+1} ΔTi,i+1​，该方法是转换子关键帧到 x i \mathbf{x}_{i} xi​坐标系下．换句话说，我们匹配 F i + 1 \mathbf{F}_{i+1} Fi+1​到体素地图是表示在 x i \mathbf{x}_{i} xi​坐标系下的．这样，真正的转换 Δ T i , i + 1 \Delta \mathbf{T}_{i,i+1} ΔTi,i+1​可以直接获得．由于转换过的特征 ′ F i e '\mathbf{F}_{i}^{e} ′Fie​和 ′ F i p '\mathbf{F}_{i}^{p} ′Fip​会被多次重复使用，因此我们使用在C1部分描述的高效的方法．

D. GPS因子

虽然我们可以只利用IMU预积分因子和雷达里程计因子获得可靠的状态估计和建图，但系统还是会在长时间导航任务中遇到漂移问题．为了解决这个问题，我们可以引入能够提供绝对测量的传感器来消除漂移．这样的传感器包括高度计、指南针和GPS．这儿为了说明目的，我们讨论GPS，因为它在真实世界的导航系统中被广泛的使用．
当我们接收到GPS测量值时，我们首先使用[21]中提出的方法将这些值转换到局部笛卡尔坐标系．当添加一个新的节点到因子图中时，我们然后将一个新的GPS因子关联到这个节点．如果GPS信号不是同雷达帧时间同步的，我们基于雷达帧的时间戳线性插值出GPS测量值．
我们注意到当GPS数据可得到时，我们就添加一个GPS因子是没必要的，原因是因为雷达惯性里程计的漂移增长缓慢．在实际应用中，我们只当估计的位置协方差是比接收到的GPS位置协方差大的时候，我们才添加GPS因子．

E. 回环因子

由于因子图的利用，回环因子可以无缝引入我们提出的系统中，不像LOAM和LIOM．为了说明的目的，我们介绍了和实现了一个简单有效的基于欧拉距离的回环检测方法．我们也注意到我们提出的框架是兼容其它回环检测方法的，例如[22]和[23],其产生点云的描述子并使用这些描述子进行位置识别．
当一个新的状态 x i + 1 \mathbf{x}_{i+1} xi+1​被添加到因子图中，我们首先搜索图中在欧几里得空间上靠近 x i + 1 \mathbf{x}_{i+1} xi+1​的先前状态．拿图1举例来说， x 3 \mathbf{x}_{3} x3​是一个返回的候选者．然后，我们使用扫描匹配的方法尝试匹配 F i + 1 \mathbf{F}_{i+1} Fi+1​和子关键帧{ F 3 − m , . . . , F 3 , . . . F m + 3 \mathbf{F}_{3-m},...,\mathbf{F}_{3},...\mathbf{F}_{m+3} F3−m​,...,F3​,...Fm+3​}，注意 F i + 1 \mathbf{F}_{i+1} Fi+1​和过去的子关键帧在扫描匹配前是首先被转换到 W \mathbf{W} W坐标系．我们获得了相对转换 Δ T 3 , i + 1 \Delta \mathbf{T}_{3,i+1} ΔT3,i+1​，然后将这个转换作为回环因子添加到因子图中．整个论文中，我们设置 m = 12 m=12 m=12，用于回环的搜索距离设置为15米．
在实际应用中，当GPS是可得到的唯一的绝对传感器，我们发现添加回环因子在矫正机器人的海拔漂移问题非常有效．这是因为GPS的高程测量非常不准确，在没有回环的情况下，我们的测试中出现的高度误差接近100m。

4. 实验(放几张图就足以证明lio_sam的牛掰)

5. 结论

我们提出了一个基于SAM的紧耦合雷达惯性里程计框架LIO-SAM，为了实现在复杂环境中实时的状态估计和建图．通过在因子图上构造雷达惯性里程计，LIO-SAM是特别适合于多传感器的融合．除了传感器的测量可以轻松的作为新的因子添加到框架中．提供绝对测量值的传感器(高度计、指南针、GPS)可以被使用去消除长期运行引入的漂移，特别是在特征缺少的环境中．位置识别也可以轻松的融入系统．为了提高系统的实时性，我们提出了一种滑窗方法，我们在扫描匹配过程中，边缘化掉老的雷达帧．关键帧是被选择性的添加到因子图中，并且当雷达里程计和回环因子产生时，新的关键帧只会同固定数量的子关键帧进行匹配．这种扫描匹配在局部范围而不是全局范围的策略，使得LIO-SAM框架能够实时运行．我们提出的方法是在三个平台的数据集上全面的进行评估过，结果显示当同LOAM和LIOM比较时，LIO-SAM可以实现更高的精度．