经典文献阅读之--SRIF-based LiDAR-IMU Localization(SRIF的LiDAR-IMU自动驾驶鲁棒定位)

0. 简介

对于车辆来说，我们更希望能够得到一个有效的定位系统，能够保证高精度的同时，拥有较高的鲁棒性，而《Robust SRIF-based LiDAR-IMU Localization for Autonomous Vehicles》就是这样一篇文章，在各种场景中实现了厘米级的精度和高鲁棒性。为了实现鲁棒、准确的点云特征匹配，文中提出了一种从激光雷达点云中提取结构化、高识别力特征的新方法。对于高频运动预测和噪声传播，我们采用增量式流形IMU预积分。我们还采用了多帧滑动窗平方根逆滤波器，使系统在功耗有限的前提下保持数值稳定的结果。为了验证文中的方法，作者还在配备LiDAR-IMU系统的多个应用程序和平台上测试了融合算法。实验结果表明，该融合框架具有较高的定位精度、较高的鲁棒性和较好的泛化能力。

1. 文章贡献

该框架的概述如图1所示。本文的贡献如下：

1. 本文提出了一种新颖的激光雷达点云特征提取方法。利用这些新颖特征可以在动态或部分改变的环境中实现精确配准和操作。与基于经典迭代最近点（ICP）[5]，[31]或基于正态分布变换（NDT）[3]，[25]的方法相比表现更好。
2. 本文的定位框架建立在平方根逆滤波器（SRIF）[27]之上。这使得我们的工作具有数值稳定性和低计算功耗，即使在基于ARM的平台上也可以实现实时操作。
3. 通过本文的实验结果证明，我们的框架在高度动态的车辆或特征匮乏的场景中表现良好。我们的系统的精度和计算负载也优于最先进的LOAM [31]框架的定位模块。
   

图1. 所提出的多传感器融合框架概述：灰色块表示原始传感器输入；绿色块代表特征提取、分类和聚合模块；蓝色块是离线生成全局地图先验的相应对应部分；SRIF求解器的核心部分在黄色块中。

2. 特征提取

LiDAR数据中对于点级别特征的判别能力较弱，因为LiDAR点是稀疏的且缺乏纹理信息。我们提出了提取多个线和surfel特征，这些特征在图1中用绿色方块表示。与基于ICP的跟踪相比，我们基于高层特征的跟踪器可以减轻计算负荷，同时提高鲁棒性。

3D点云被格式化为一个矩阵，其中列索引表示水平角度，行索引表示垂直光束层。点$p_{i;j}$（列索引$i$和行索引$j$）的相对时间$D_t$表示自扫描的第一个点被采样以来经过的时间，并且我们通过$l_{i;j}$定义$p_{i;j}$的深度。在所有点都在该矩阵中注册之后，我们可以进行运动补偿和高级特征提取。

2.1 运动补偿(比较通用的部分)

LiDAR通过周期性扫描环境来获取点云。然而，当LiDAR捕获一个扫描时，LiDAR也在移动，这会扭曲捕获到的点云。去扭曲的过程是将点云与相同时间瞬间进行配准。在这项工作中，我们使用IMU作为辅助设备进行相对运动估计和运动补偿。我们使用成本低廉的MTI-3 MEMS IMU，它输出200 Hz的动态测量值，并通过总线通信与LiDAR进行硬件同步。IMU预积分和LiDAR运动补偿遵循在SO（3）流形上提出的标准预积分方法，如[10]和[9]所建议的那样。

2.2 地面平面识别(比较通用的部分)

对于自动驾驶车辆来说，通常约有三分之一的LiDAR扫描是地面点，这些点无法为特征的提取、描述和跟踪提供独特的信息。为了减轻后续特征提取的复杂性，我们首先借助IMU识别出地面点。我们从IMU预积分中获得当前帧的重力方向ng。我们还知道LiDAR的高度d。有了$n_g$和给定的高度$d$，如果$d-{\tau }_1<|[p_{i;j}{l}_{i;j}{]}^T{n}_g|<d+{\tau }_1$，其中${\tau }_1$是预定义的阈值，我们便可以识别出地面上的点。我们在系统中将地面平面作为另一个Surfel约束添加到位姿估计中。

2.3 LiDAR 特征提取和描述

为了使系统在动态场景下更加稳健，提取点云特征并将它们放入正确的类别中是至关重要的。我们首先将点分为两类：角点和普通点，对于这些点，我们依赖于几何属性。我们不使用曲率来定义点的平滑度，而是简单地考虑目标点和相邻两个点之间形成的线段的角度的反余弦。它由以下公式给出：

如图2所示，拐角点进一步分为4类：左拐角点、右拐角点、单一拐角点和普通拐角点。四种拐角类型定义如下：

1. 左拐角点和右拐角点（参见图2（a）和图2（b），通常出现在建筑物或大型庇护所中）。对于一个左拐角点，激光束例如扫描车辆右侧的建筑物，只有部分LiDAR扫描将覆盖该建筑物。位于第$i$个水平层上且仍位于建筑物上的最左点被定义为左拐角点，表示为$p_{i,j}$。让$p_{i,j-m}$是$p_{i,j}$左边的第一个有效点。左拐角点应满足以下条件之一：
   
   ${\tau }_2$和${\tau }_3$是两个预设的阈值。类似地，右角点需要满足以下条件之一：
   
   其中n的定义是这样的：$p_{i,j+n}$是扫描行$i$中往右第一个有效的点。在我们的系统中，${\tau }_2=5$，${\tau }_3=0.5m$。

2. 单个角点（见图2©）：描述像树干或电线杆这样的类似杆的对象。单角点必须同时满足两个条件：
   
   相邻的点被忽略，不会用于构成特征。

3. 普通角点（见图2(d)）：普通角点是最常见的角点。当激光束扫描建筑物时，可能会扫描两个相邻的非平行表面。两个表面相交处的点将被识别为普通角点。它们满足以下约束：
   
   在我们的系统中，$\tau 4=134°$，$\tau 5=143°$。

没有被分类为角点或地面点的剩余点被标记为普通surfel点。我们称这样的点为surfel，并通过聚类策略对它们进行聚合。

图2. LiDAR的四种角线类型：左右角代表部分可见的物体，单角表示类似杆子的物体，正常角表示常见的边缘。

2.4 线和Surfel的聚合

我们再次使用格点网格将3D点进一步划分为单元格，以便快速和精确地将点聚合成更高级别的特征。这是顺序地针对线和Surfel完成的。点云中的每一层被分成$x$个角度区间（这里称为单元格），根据它们是否包含特定类型的点，将它们标记为空或占用。如果在相邻层中满足某些兼容性条件，则在相同的角度索引处聚合占用单元格（参见算法2和1）。对于线、正常以及地面Surfel点，我们在每个扫描行中使用了180、72和36个单元格。定义$G_{i,j}$为第$i_{th}$层中的第$j_{th}$个单元格。


我们根据原始点的角点类型将标记点聚类成四种类型的线（左线、右线、单线和正常线）和两种类型的Surfel（正常Surfel和地面Surfel）。

3. 地图生成器

由于我们进行基于地图的定位，全局地图的质量对于定位精度至关重要。因此，我们使用一辆专门装备有高级XW-GI5651惯性导航系统（INS）、RTK系统、32层和两个16层Robosense激光雷达以及一组编码器的车辆。这样一辆设备齐全的车辆可以提供高精度的点云地图。我们使用与第二部分相同的方法来获取全局地图中的线和surfel特征。

4. 特征提取

将新帧 ${\mathbb{F}}_k$ 融合到滑动窗口 ${\mathbb{W}}_{k-1}$ 中，需要根据空间距离和类型关联同类型特征。设$l_i^w=(c_i^w,n_i^w,s_i^w)$为滑动窗口中的第$i$个特征，其中 $c_i^w$ 是特征$l_i^w$ 的中心点，$n_i^w$ 是相应的法线，$s_i^w$ 是特征类型。因此，当前帧 ${\mathbb{F}}_k$ 的第$j$个特征由 $l_j^f$ 表示。将滑动窗口帧 ${\mathbb{W}}_{k-1}$ 与帧${\mathbb{F}}_{k-1}$ 重合。对于新帧 ${\mathbb{F}}_k$，IMU提供了从${\mathbb{F}}_k$到 ${\mathbb{F}}_{k-1}$（因此是 ${\mathbb{W}}_{k-1}$ ）的相对姿态。设 $R_{k,k-1}$ 和 $t_{k,k-1}$ 分别表示从${\mathbb{W}}_{k-1}$ 到 ${\mathbb{F}}_k$ 的旋转和平移。对于当前帧中的每个特征 $l_i^f$，我们首先在滑动窗口内找到其10个最近的邻居 $N(l_i^f)$，并根据它们的空间距离进行匹配。与$N(l_i^f)$中第 $j$ 个邻居特征的匹配得分为：

其中，$I()$是相同特征类型的指示函数（如果$s_j^w==s_i^f$，则$I(s_j^w,s_i^f)=1$，否则$I(s_j^w,s_i^f)=0)$，$g()$表示法向量偏差。在我们的系统中，$g(n_1,n_2)=|n_1^T{n}_2|$。我们通过最大化来找到特征匹配。

…详情请参照古月居