0. 简介
对于车辆来说，我们更希望能够得到一个有效的定位系统，能够保证高精度的同时，拥有较高的鲁棒性，而《Robust SRIF-based LiDAR-IMU Localization for Autonomous Vehicles》就是这样一篇文章，在各种场景中实现了厘米级的精度和高鲁棒性。为了实现鲁棒、准确的点云特征匹配，文中提出了一种从激光雷达点云中提取结构化、高识别力特征的新方法。对于高频运动预测和噪声传播，我们采用增量式流形IMU预积分。我们还采用了多帧滑动窗平方根逆滤波器，使系统在功耗有限的前提下保持数值稳定的结果。为了验证文中的方法，作者还在配备LiDAR-IMU系统的多个应用程序和平台上测试了融合算法。实验结果表明，该融合框架具有较高的定位精度、较高的鲁棒性和较好的泛化能力。
1. 文章贡献
该框架的概述如图1所示。本文的贡献如下：
- 本文提出了一种新颖的激光雷达点云特征提取方法。利用这些新颖特征可以在动态或部分改变的环境中实现精确配准和操作。与基于经典迭代最近点（ICP）[5]，[31]或基于正态分布变换（NDT）[3]，[25]的方法相比表现更好。
- 本文的定位框架建立在平方根逆滤波器（SRIF）[27]之上。这使得我们的工作具有数值稳定性和低计算功耗，即使在基于ARM的平台上也可以实现实时操作。
- 通过本文的实验结果证明，我们的框架在高度动态的车辆或特征匮乏的场景中表现良好。我们的系统的精度和计算负载也优于最先进的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类：左拐角点、右拐角点、单一拐角点和普通拐角点。四种拐角类型定义如下：
-
左拐角点和右拐角点（参见图2（a）和图2（b），通常出现在建筑物或大型庇护所中）。对于一个左拐角点，激光束例如扫描车辆右侧的建筑物，只有部分LiDAR扫描将覆盖该建筑物。位于第i个水平层上且仍位于建筑物上的最左点被定义为左拐角点，表示为p_{i,j}。让p_{i, j−m}是p_{i,j}左边的第一个有效点。左拐角点应满足以下条件之一：
τ_2和τ_3是两个预设的阈值。类似地，右角点需要满足以下条件之一：
其中n的定义是这样的：p_{i,j + n}是扫描行i中往右第一个有效的点。在我们的系统中，τ_2=5，τ_3=0.5m。 -
单个角点（见图2(c)）：描述像树干或电线杆这样的类似杆的对象。单角点必须同时满足两个条件：
相邻的点被忽略，不会用于构成特征。 -
普通角点（见图2(d)）：普通角点是最常见的角点。当激光束扫描建筑物时，可能会扫描两个相邻的非平行表面。两个表面相交处的点将被识别为普通角点。它们满足以下约束：
在我们的系统中，τ4 = 134°，τ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^w_i = (c^w_i , n^w_i , s^w_i )为滑动窗口中的第i个特征，其中 c^w_i 是特征l^w_i 的中心点，n^w_i 是相应的法线，s^w_i 是特征类型。因此，当前帧 \mathbb{F}_k 的第j个特征由 l^f_j 表示。将滑动窗口帧 \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^f_i，我们首先在滑动窗口内找到其10个最近的邻居 N(l^f_i )，并根据它们的空间距离进行匹配。与N(l^f_i )中第 j 个邻居特征的匹配得分为：
其中，I()是相同特征类型的指示函数（如果s^w_j == s^f_i，则I(s^w_j ,s^f_i ) = 1，否则I(s^w_j ,s^f_i ) = 0)，g()表示法向量偏差。在我们的系统中，g(n_1,n_2) = |n^T_1 n_2|。我们通过最大化来找到特征匹配。
这里的m_{i,j}∗是指匹配分数，τ_6是匹配分数的阈值，τ6=0.9在我们的系统中。使用类似的策略可以在当前帧和全局地图之间进一步匹配。图3（a）和图3（b）展示了匹配结果的示例。
请参见图3。图中展示了特征跟踪的性能。红色表示当前帧中提取的线和surfel特征，蓝色表示通过预测从上一帧中投影的特征，黄色表示具有全局对应的特征。左侧是线条匹配，右侧是surfel匹配。
5. SRIF求解
位姿优化器是一个滑动窗口滤波器。该滤波器的状态被定义为：
n是窗口的长度，x^i_F是在滑动窗口中第i_{th}帧的状态，由以下变量组成：
q^i_{WI} 和 p^i_{WI} 是由IMU预测的世界坐标系下的绝对位姿；v^i_{WI} 是该帧的速度向量；b^i_g 和 b^i_a 表示陀螺仪和加速度计的偏置。每当添加一个新的关键帧k时，求解器的目标是最小化成本函数\mathscr{C}_k:
其中\mathscr{C}_{k-1}是先前帧的成本项，\mathscr{C}_{u}表示与IMU预积分项有关的损失，\mathscr{C}_{f}表示滑动窗口\mathbb{W}_k中LiDAR特征的配准误差。先前的成本项被公式化为\mathscr{C}_{k-1}(\tilde{x}_{k-1}) = | I_{k-1} \tilde{x}_{k-1} -r_{k-1}|^2，其中I_{k-1}和r_{k-1}是先前的上三角信息滤波器矩阵和相应的残差。\tilde{x}_{k-1}反过来表示上一个状态的误差表示。我们的求解器是SRIF的扩展。有关传播和边缘化步骤的详细信息，请参见[27]。更新通过堆叠来自不同传感器来源的所有Jacobian和残差完成。因此，我们构建了滑动窗口中所有状态的超定约束函数\mathscr{C}_{k}。
残差: 如第II节所述，我们从激光雷达点云中提取了线特征和面元特征，因此用于构建\mathscr{C}_{f}。对于两个相应的线特征，令p^j_{obs}是激光扫描中检测到的线上测量的第j个点。另外，p^0_m和p^1_m是全局地图上相应线上的任意两个点。d_l表示观测和地标之间的距离误差，其由以下公式给出：
并且对于对应的平面残差的推导类似。对于平面上的第j个观测点，令 p_{j}^{obs} 为滑动窗口中对应的平面上的观测点。令 p_{0}^{m}、p_{1}^{m} 和 p_{2}^{m} 分别为地图中平面上的三个点。则两个平面之间的距离 d_p 给出为：
函数h_x(.)代表用Huber范数替换L2范数的健壮（robust）范数。
评论（0）
您还未登录，请登录后发表或查看评论