LOAM是Ji Zhang于2014年提出的使用激光雷达完成定位与三维建图的算法,即Lidar Odometry and Mapping。之后许多激光SLAM算法借鉴了LOAM中的一些思想,可以说学习LOAM对学习3D激光SLAM很有帮助。本文对LOAM原论文进行了翻译。

摘要

我们提出了一种实时里程测量和地图绘制方法,该方法使用$ 6$ 自由度移动的$ 2$ 轴激光雷达的距离测量。这个问题很难解决,因为距离测量是在不同的时间接收的,运动估计中的错误可能会导致结果点云的错误配准。迄今为止,我们可以通过离线批处理方法构建连贯的 $3D$ 地图,通常使用循环闭合来校正随时间的漂移。我们的方法实现了低漂移和低计算复杂度,无需高精度测距或惯性测量。获得这一性能水平的关键思想是通过两种算法来划分同时定位和建图的复杂问题,该问题寻求同时优化大量变量。一种算法以高频但低保真度进行里程测量,以估计激光雷达的速度。另一种算法以较低数量级的频率运行,用于点云的精细匹配和配准。这两种算法的结合允许该方法实时建图。该方法已通过大量实验以及 KITTI 里程计基准进行了评估。结果表明,该方法可以达到离线批处理方法的精度水平。

I. 介绍

$3D$ 建图仍然是一种流行的技术 [1]–[3]。使用激光雷达进行建图是常见的,因为激光雷达可以提供高频范围测量,无论测量的距离如何,误差都相对恒定。在激光雷达的唯一运动是旋转激光束的情况下,点云的配准很简单。然而,如果激光雷达本身在许多环境的应用中都在移动,则精确建图需要在连续激光测距期间了解激光雷达的位姿。解决这个问题的一种常见方法是使用独立的位置估计(例如通过GPS/INS)将激光点注册到一个固定的坐标系中。另一组方法使用里程计测量,如从轮编码器或视觉里程计系统[4],[5]记录激光点。由于里程计集成了随时间推移的微小增量运动,它必然会发生漂移,因此人们非常关注如何减少漂移(例如使用环路闭合)。

在这里,我们考虑使用 $6$ 自由度移动的 $2$ 轴激光雷达创建低漂移里程计地图的情况。使用激光雷达的一个关键优势是它对场景中的环境光线和光学纹理不敏感。随着激光雷达的发展,它们的尺寸和重量缩小了很多。激光雷达在人手持的情况下穿越环境 [6],也可以连接到微型飞行器 [7]。由于我们的方法旨在推动里程计估计中的最小化漂移相关的问题,因此目前不涉及回环问题。

利用移动两轴激光雷达进行运动估计和建图

图1所示。该方法的目标是利用移动两轴激光雷达进行运动估计和建图。由于接收激光点的时间不同,激光雷达的运动导致点云出现失真(如图左侧的激光雷达云所示)。我们提出的方法是将问题分解为两个并行运行的算法。里程计算法估计激光雷达的速度和校正点云中的失真,然后建图算法匹配并注册点云以创建地图。两种算法的结合保证了问题的实时可行性。

该方法实现了低漂移和低计算复杂度,无需高精度测距惯性测量。获得这一性能水平的关键思想是用两种算法将通常复杂的同时定位与建图(SLAM)[8]问题分割开来,该问题旨在同时优化大量变量。其中一种算法采用高频但保真度较低的里程计来估计激光雷达的速度。另一种算法以较低数量级的频率运行,用于点云的精细匹配和配准。虽然没有必要,但如果IMU可用,可以提供一个运动先验来帮助描述高频运动。两种算法分别提取尖锐边缘和平面表面上的特征点,并将其分别与边缘线段和平面表面斑块进行匹配。里程计算法在保证快速计算的前提下,找到特征点的对应关系。在建图算法中,通过相关特征值和特征向量检查局部点簇的几何分布来确定对应关系。

通过对原问题的分解,首先解决了一个简单的问题,即在线运动估计。之后,建图作为批量优化(类似于迭代最近点(ICP)方法 [9])进行,以生成高精度运动估计和地图。并行算法结构保证了问题实时求解的可行性。此外,由于运动估计是在更高的频率下进行的,因此给建图足够的时间来提高精度。当以较低的频率运行时,建图算法能够包含大量的特征点,并使用足够多的迭代进行收敛。

II.相关工作

激光雷达已经成为机器人导航中应用广泛的传感器 [10]。对于定位和建图,大多数都使用 $2D$ 激光雷达[11]。当激光雷达扫描速率远高于其外部运动时,扫描中的运动失真通常可以忽略。在这种情况下,标准 $ICP$ 方法[12] 可用于匹配不同扫描之间的激光数据。此外,提出了一种两步法来消除失真[13]:基于 $ICP$ 的速度估计步骤,然后使用计算的速度进行失真补偿步骤。类似的技术也用于补偿单轴 $3D$ 激光雷达引入的失真[14]。然而,如果扫描运动相对缓慢,则运动失真可能严重。这也是为什么使用两轴激光雷达,因为一个轴通常比另一个轴慢得多。通常,其他传感器用于提供速度测量,通过这些测量,可以消除运动失真。例如,激光雷达点云可以通过与 IMU[15] 集成的视觉里程计的状态估计来注册。当多个传感器(如 GPS/INS 和车轮编码器)同时可用时,问题通常通过扩展卡尔曼滤波器[16] 或粒子滤波器 [1] 解决。这些方法可以实现实时建图,以辅助机器人导航中的路径规划和避障。

如果在不借助其他传感器的情况下使用双轴激光雷达,运动估计和失真校正将成为一个问题。Barfoot 等人使用的一种方法是从返回激光强度创建视觉图像,并在图像之间匹配视觉上不同的特征 [17],以恢复地面车辆的运动[18]–[21]。在[18]、[19] 中,车辆运动被建模为恒定速度,在 [20]、[21] 中,车辆运动被建模为高斯过程。我们的方法使用了与 [18]、[19] 中类似的里程计算法线性运动模型,但具有不同类型的特征。方法 [18]–[21] 涉及强度图像的视觉特征,需要稠密点云。该方法在笛卡尔空间中提取和匹配几何特征,对点云密度要求较低。最接近我们的方法是Bosse和Zlot的[3]、[6]、[22]。利用两轴激光雷达获取点云,通过匹配局部点簇[22]的几何结构进行配准。此外,他们使用多个 2 轴激光雷达绘制地下矿井地图 [3]。此方法融合了 IMU,并使用回环检测构建大型地图。由同一作者提出的Zebedee是一个由2D激光雷达和IMU组成的建图设备,IMU通过弹簧[6]连接到手杆上。通过手动操作装置进行建图。采用批量优化方法对分段数据集进行处理,并在分段数据集之间添加边界约束。该方法利用IMU的测量值对激光点进行配准,并通过优化来校正IMU的偏差。从本质上说,Bosse和Zlot的方法需要批处理来开发精确的地图,因此不适用于需要实时地图的应用。相比之下,该方法在实时生成的地图在定性上与Bosse和Zlot的方法相似。区别在于,我们的方法可以为自动驾驶汽车的引导提供运动估计。此外,该方法利用了激光雷达扫描模式和点云分布。在里程计和建图算法中,分别实现了保证计算速度和精度的特征匹配。

III.符号和任务描述

本文所要解决的问题是利用三维激光雷达感知的点云进行自动估计,并建立所经过环境的地图。我们假设激光雷达是预先校准的。我们还假设激光雷达的角速度和线速度随时间的变化是平滑和连续的,没有突变。第二个假设将在第VII-B节中通过使用IMU来释放。

在本文中,我们习惯用大写右标来表示坐标系。我们定义扫描(a sweep)为激光雷达完成一次扫描覆盖,我们使用正确的订阅 $k,k\in Z^+$ 表示扫描,$P_k$ 表示扫描为 $k$ 时感知的点云。让我们如下定义两个坐标系。

  • 激光雷达坐标系 $\left\{L\right\}$ 是一个以激光雷达几何中心为原点的三维坐标系。$x$ 轴是向左的,$y$ 轴是向上的,$z$ 轴是向前的。点 $i, i\in P_k$ 在 $\left\{L\right\}$ 中的坐标表示为 $X^L_{(k,i)}$ 。
  • 世界坐标系$\left\{W\right\}$是在初始位置与$\left\{L\right\}$重合的三维坐标系。点$i, i\in P_k$在$\left\{W\right\}$中的坐标为$X^W_{(k,i)}$ 。

有了假设和符号,我们的激光雷达里程测量和建图问题可以定义为:

问题:给定一个激光雷达云序列$i, i\in P_k$,计算每次扫描$k$时激光雷达的帧间运动(ego-motion),用$P_k$构建遍历环境的地图。

IV.系统总览

A.激光雷达硬件

本文的研究是在基于Hokuyo UTM-30LX激光扫描仪的定制3D激光雷达上进行验证的,但并不限于此。通过本文,我们将使用从激光雷达收集的数据来说明该方法。该激光扫描仪的视场为$180^◦$,分辨率为$0.25^◦$以及$40$行/秒扫描速率。所述激光扫描仪与电机相连,电机被控制以$180^◦/s$的角速度旋转,在$−90^◦$和$90^◦$之间,激光扫描仪的水平方向为零。对于这个特定的单位,扫描是从$−90^◦$旋转到$90^◦$或在相反的方向(持续1秒)。这里,请注意,对于一个连续旋转的激光雷达,扫描只是一个简单的半球面旋转。板载编码器以$0.25^◦$的分辨率测量电机旋转角度,利用该分辨率将激光点投影到激光雷达坐标$\left\{L\right\}$中。

三维激光雷达

图$2$所示。三维激光雷达在本研究中使用的是一个由马达驱动旋转运动的Hokuyo激光扫描仪,以及一个测量旋转角度的编码器。该激光扫描仪的视场为$180^◦$的分辨率为$0.25^◦$。扫描速度为$40$行/秒。电机被控制从$−90^◦$旋转到$90^◦$与激光扫描仪的水平方向为零。

激光雷达测程与建图软件系统框图。

图$3$所示。激光雷达测程与建图软件系统框图。

B.软件系统概述

图$3$显示了软件系统的示意图。设$\hat{P}$为激光扫描接收到的点。在每次扫描期间,$\hat{P}$在$\left\{L\right\}$中注册。扫描$k$时合并的点云形成$P_k$,然后用两种算法处理$P_k$。激光雷达里程计取点云并计算激光雷达在两次连续扫描之间的运动。在$P_k$中使用估计的运动来校正失真,算法运行在$10Hz$左右的频率。激光雷达建图进一步处理输出,以$1Hz$的频率匹配并将未失真的云注册到地图上。最后,将两种算法发布的位姿变换集成在一起,生成一个关于激光雷达相对于地图的位姿的约$10Hz$的变换输出。第V、VI节详细介绍了软件框图中的模块

V.激光雷达里程计

A.特征点提取

我们首先从激光雷达云$P_k$中提取特征点,如图$3$所示的激光雷达在$P_k$中自然地产生了分布不均匀的点。在一个扫描中,从激光扫描仪返回的分辨率为$0.25^◦$,这些点位于扫描平面上。然而,当激光扫描仪以$180^◦/s$的角速度旋转,并产生40Hz的扫描,在垂直方向的扫描平面的分辨率是$180^◦/40 = 4.5^◦$。考虑到这一事实,在$P_k$中提取特征点时,仅使用单个扫描的信息,且具有共面几何关系。

我们选择尖锐边缘上的特征点和平面表面的特征点。设$i$是$P_k$中的一个点,$i\in P_k$,设$S$是激光扫描仪在同一次扫描中返回的$i$的连续点的集合。由于激光扫描器以$CW$或$CCW$顺序生成点返回,$S$包含一半位于$i$和两点之间$0.25^◦$间隔的每一边上的点。定义一个术语来评估局部表面的平滑度,

$$ c=\frac{1}{|S|·\parallel X^L_{(k,i)}\parallel} \sum_{j\in S,j\ne i} \parallel (X^L_{(k,i)}-X^L_{(k,j)}) \parallel \tag{1} $$

根据$c$值对扫描中的点进行排序,选择$c$值最大的点为边缘点,$c$值最小的点为平面点的特征点。为了在环境中均匀分布特征点,我们将扫描分割成四个相同的子区域。每个子区域最多可提供$2$个边缘点和$4$个平面点。只有当点$i$的$c$值大于或小于一个阈值,且所选点的个数不超过最大值时,才能将点$i$选择为边或平面点。

特征点提取

图4所示。$(a)$实线段表示局部表面贴片。点$A$位于与激光束有一定角度的表面上(虚线橙色线段)。点$B$在一个大致平行于激光束的表面上。我们将$B$作为一个不可靠的激光返回点,不选择它作为特征点。$(b)$实线段是激光的可观测对象。点$A$位于被遮挡区域(虚线橙线段)的边界上,可以被检测为边缘点。然而,如果从不同的角度看,被遮挡的区域会发生变化,变得可以观察到。我们不把$A$作为显著边缘点,也不选择它作为特征点。

在选择特征点时,我们希望避免选择周围点被选择的点,或者大致平行于激光束的局部平面上的点(图$4(a)$中的点$B$)。这些观点通常被认为是不可靠的。此外,我们希望避免在被遮挡区域边界上的点[23]。如图$4(b)$所示为一个例子。点$A$是激光雷达云中的边缘点,因为它的连接面(虚线段)被另一个物体遮挡。然而,如果激光雷达移动到另一个视点,被遮挡区域就会发生变化,成为可观测的。为了避免前面提到的待选点,我们再次找到点$S$的集合。只有当$S$它不能在大致平行于激光束的表面上,也不能在被遮挡区域的边界上。不形成一个大致平行于激光束的表面贴片时,点$i$才能被选择。在$S$中,没有一个点在激光束方向上通过间隙与$i$断开,同时比$i$更靠近激光雷达(如图$4(b)$中的B点)。

综上所述,从$c$值的最大值开始选取特征点作为边缘点,从$c$值的最小值开始选取特征点作为平面点,如果选取一个点,

  • 选择的边缘点或平面点的数量不能超过子区域的最大值
  • 它周围的点都没有被选中
  • 它不能在大致平行于激光束的表面上,也不能在被遮挡区域的边界上。

从走廊场景中提取特征点的示例如图$5$所示。边缘点和平面点分别用黄色和红色标记。

图$5$所示。从走廊的激光雷达云中提取边缘点(黄色)和平面点(红色)的例子。同时,激光雷达以$0.5m/s$的速度向图左侧的壁面移动,导致壁面运动失真。

B.寻找特征点对应

里程计算法估计激光雷达在扫描范围内的运动。设$t_k$为扫描$k$的起始时间。在每次扫描结束时,将扫描过程中感知到的点云$P_k$重新投影到时间戳$t_{k+1}$,如图$6$所示。我们将重新投影的点云表示为$\overline{P}_k$。在下一次扫描$k+1$时,$\overline{P}_k$和新接收到的点云$P_{k+1}$一起使用,来估计激光雷达的运动。

图$6$所示。将点云重新投影到扫描的末端。蓝色线段表示扫描$k$时感知到的点云 $P_k$,在扫描$k$结束时,将$P_k$重新投影到时间戳$t_{k+1}$,得到绿色线段$\overline{P}_k$。然后,在扫描$k+1$时,$\overline{P}_k$和新感知的点云$P_{k+1}$(橙色线段)一起估计激光雷达运动。

让我们假设$\overline{P}_k$和$P_{k+1}$现在都是可用的,然后从找到两个激光雷达云之间的对应关系开始。对于$P_{k+1}$,我们使用上一节讨论的方法从激光雷达云中找到边缘点和平面点。设$\xi_{k+1}$和$H_{k+1}$分别为边点和平面点的集合。我们将从$\overline{P}_k$中找到与$\xi_{k+1}$中的点对应的边线,从平面图中找到与$H_{k+1}$中的点对应的平面贴片(patch)。

请注意,在扫描$k+1$开始时,$P_{k+1}$是一个空集,它在扫描过程中随着接收到更多点数而增长。激光雷达里程计递归地估计了扫描期间的$6-DOF$运动,并随着$P_{k+1}$的增加逐渐包含更多的点。在每次迭代中,使用当前估计的变换将$\xi_{k+1}$和$H_{k+1}$重新投影到扫描的开始,记$\tilde{\xi}_{k+1}$和$\tilde{H}_{k+1}$为重投影的点集。对于每个点在$\xi_{k+1}$和$H_{k+1}$,我们将在$\overline{P}_k$中找到最近的邻居点。在这里,$\overline{P}_k$被存储在$3D$ $ KD-tree$[24]中,用于快速索引。

图$7(a)$给出了寻找边缘点对应的边线的过程。设 $i$ 是属于 $ \tilde{\xi}_{k+1} $ 的点,即$ i \in \tilde{\xi}_{k+1}$。边线由两个点表示。设 $j$ 是 $i$ 在 $\overline{P}_k$ 中的最近邻,$j\in \overline{P}_k$,设 $l$ 是 $i$ 在连续两次扫描$j$时的最近邻,$(j, l)$ 构成 $i$ 的对应关系。然后,为了验证$j$和$l$都是边缘点,我们根据公式$(1)$来检查局部表面的平滑度。这里,我们特别要求$j$和$l$来自不同的扫描,考虑到一次扫描不能包含来自同一边线的多个点。只有一个例外,即边缘线在扫描平面上。然而,如果是这样,边缘线就会退化且在扫描平面上显示为一条直线,并且特征点在边缘线上不应该首先被提取。

图7所示。在 $\tilde{\xi}_{k+1}$ 中找到一条边缘线作为边缘点的对应关系,如图$(a)$,以及作为 $\tilde{H}_{k+1}$ 中平面点的对应关系的平面贴片(patch),如图 $(b)$ 。在 $(a)$ 和 $(b)$ 中,$j$ 是距离特征点最近的点,在 $\overline{P}_k$ 中找到。橙色的线表示对j的相同扫描,蓝色的线表示连续的两次扫描。为了找出 $(a)$ 中的边线对应关系,我们在蓝色的直线上找到另一个点$l$,对应关系用 $(j, l)$ 表示。为了找到 $(b)$ 中的平面贴片(patch)对应关系,我们发现另外两个点,$l$ 和 $m$,分别位于橙色和蓝色线上。对应关系为 $(j, l, m)$。

图$7(b)$显示了寻找平面贴片(patch)作为平面点对应的过程。设 $i$ 为 $\tilde{H}_{k+1}$ 中的一个点,即 $i\in\tilde{H}_{k+1}$ 。平面贴片(patch)由三个点表示。与上一段类似,我们在 $\overline{P}_k$ 中找到 $i$ 的最近邻,记为 $j$ 。 然后,我们找到另外两个点 $l$ 和 $m$,作为 $i$ 的最近邻,一个在 $j$ 的同一扫描中,另一个在 $j$ 的两次连续扫描中。 这保证了三个点是非共线的。 为了验证 $j$、$l$ 和 $m$ 都是平面点,我们根据 $(1)$ 再次检查局部表面的平滑度。

根据特征点的对应关系,推导出特征点到对应关系的距离表达式。在下一节中,我们将通过最小化特征点的整体距离来恢复激光雷达运动。我们从边点开始。对于点 $i\in\tilde{\xi}_{k+1}$,如果 $(j, l)$是对应的边线,$j,l\in\overline{P}_k$,则点到线的距离可以计算为

$$ d_{\xi}=\frac{|(\tilde{X}^L_{(k+1,i)}-\overline{X}^L_{(k,j)})×(\tilde{X}^L_{(k+1,i)}-\overline{X}^L_{(k,l)})|} {|\overline{X}^L_{(k,j)}-\overline{X}^L_{(k,l)}|},\tag{2} $$

其中$\tilde{X}^L_{(k+1,i)},\overline{X}^L_{(k,j)}$, 和$\overline{X}^L_{(k,l)}$分别是点$i, j, l$在$\left\{L\right\}$中的坐标。那么,对于点$i \in \tilde{H}_{k+1}$,如果$(j, l, m)$是对应的平面贴片(patch),$j,l,m \in \overline{P}_k$,则点到平面的距离为

$$ d_H=\frac {\begin{vmatrix} (\tilde{X}^L_{(k+1,i)}-\overline{X}^L_{(k,j)})\\ ((\overline{X}^L_{(k,j)}-\overline{X}^L_{(k,l)})×(\overline{X}^L_{(k,j)}-\overline{X}^L_{(k,m)})) \end{vmatrix} } {\begin{vmatrix} (\overline{X}^L_{(k,j)}-\overline{X}^L_{(k,l)})×(\overline{X}^L_{(k,j)}-\overline{X}^L_{(k,m)}) \end{vmatrix} },\tag{3} $$

其中$\overline{X}^L_{(k,m)}$是点$m$在$\left\{L\right\}$中的坐标。

C.运动估计

激光雷达运动在扫描期间以恒定的角速度和线速度建模。这允许我们在一个扫描中对在不同时间接收到的点进行线性插值。假设$t$是当前的时间戳,$t_{k+1}$是扫描$k+1$的开始时间。设$T^L_{k+1}$为$[t_{k+1}, t]$之间的激光雷达位姿变换。$T^L_{k+1}$包含$6$自由度激光雷达的刚性运动,$T^L_{k+1} = [t_x, t_y, t_z, θ_x, θ_y, θ_z]^T$,其中$t_x$、$t_y$和$t_z$分别是$\left\{L\right\}$沿$x-$、$y-$和$z-$轴的平移,$θ_x$、$θ_y$和$θ_z$是旋转角度,遵循右手规则。给定点$i,i \in P_k$,设$t_i$为其时间戳,设为$[t_{k+1}, t_i]$之间的$T^L_{(k+1,i)}$位姿变换。$T^L_{(k+1,i)}$可以通过$T^L_{k+1}$的线性插值得到,

$$ T^L_{(k+1,i)}=\frac{t_i-t_{k+1} }{t-t_{k+1} }T^L_{k+1}.\tag{4} $$

回想一下,$\xi_{k+1}$ 和 $H_{k+1}$ 是从 $P_{k+1}$ 中提取的边缘点和平面点的集合,$\tilde \xi_{k+1}$ 和$\tilde H_{k+1}$ 是重新投影到扫描开始$ t_{k+1}$的点集合 。为了解决激光雷达运动,我们需要建立 $\xi_{k+1}$ 和 $\tilde \xi_{k+1}$ 或 $H_{k+1}$ 和 $\tilde H_{k+1}$ 之间的几何关系。利用$(4)$中的变换,我们可以推导,

$$ X^L_{(k+1,i)}=R\tilde X^L_{(k+1,i)}+T^L_{(k+1,i)}(1:3),\tag{5} $$

其中$X^L_{(k+1,i)}$是$\xi_{k+1}$或$H_{k+1}$中的点$i$的坐标,$\tilde X^L_{(k+1,i)}$是$\tilde \xi_{k+1}$或$\tilde H_{k+1}$中的点$i$的坐标,$(a:b)$是$T^L_{(k+1,i)}$的第$a$到$b$个项,$R$是由$Rodrigues$公式[25]定义的旋转矩阵。

$$ R=e^{\hat{w} \theta}=I + \hat{w}sin\theta+\hat{w}^2(1-cos\theta).\tag{6} $$

在上式中,$θ$为旋转的大小,

$$ \theta = \parallel T^L_{(k+1,i)}(4:6) \parallel,\tag{7} $$

$w$是表示旋转方向的单位向量,

$$ w=\frac{T^L_{(k+1,i)}(4:6)}{\parallel T^L_{(k+1,i)}(4:6) \parallel},\tag{8} $$

$\hat{w}$为$w$的斜对称矩阵[25]。

回忆$(2)$和$(3)$计算属于$\tilde \xi_{k+1}$ 和$\tilde H_{k+1}$ 的点之间的距离及其对应关系。结合$(2)$和$(4)-(8)$,我们可以推导出$\xi_{k+1}$中的一个边点与相应的边线之间的几何关系,

$$ f_{\xi}(X^L_{(k+1,i)},T^L_{k+1})=d_\xi,i\in \xi_{k+1}.\tag{9} $$

同样,结合$(3)(4)-(8)$,我们可以在$H_{k+1}$中的一个平面点与对应的平面贴片(patch)建立另一个几何关系,

$$ f_H(X^L_{(k+1,i)},T^L_{k+1})=d_H,i\in H_{k+1}.\tag{10} $$

最后,我们用Levenberg-Marquardt方法[26]求解激光雷达运动。将$\xi_{k+1}$和$H_{k+1}$中每个特征点$(9)$和$(10)$叠加,得到一个非线性函数:

$$ f(T^L_{k+1})=d,\tag{11} $$

其中每一行$f$对应一个特征点,$d$包含相应的距离。我们计算$f$关于$T^L_{k+1}$的雅可比矩阵,记作$J$,其中$J =\partial f /\partial T^L_{k+1}$。然后,$(11)$通过非线性迭代,使$d$趋近于$0$,

$$ T^L_{k+1}\gets T^L_{k+1}-(J^TJ+\lambda diag(J^TJ))^{-1}J^Td.\tag{12} $$

$\lambda$是由Levenberg-Marquardt方法确定的因子。

D.激光雷达里程计算法

激光雷达里程计算法

激光雷达里程计算法如算法1所示。该算法以上次扫描的点云 $\overline{P}_k$、当前扫描的增长点云 $P_{k+1}$ 和上次递归的位姿变换 $T^L_{k+1}$ 作为输入。如果启动了新的扫描,$T^L_{k+1}$ 将被设置为零(第 $4-6$ 行)。然后,算法从 $P_{k+1}$ 中提取特征点构造第 $7$ 行$\xi_{k+1}$ 和 $H_{k+1}$。对于每个特征点,我们可以在 $\overline{P}_k$ (第 $9-19$ 行)中找到对应。运动估计适应于鲁棒拟合[27]。在第 $15$ 行,该算法为每个特征点分配一个平方权值。 与其对应的距离较大的特征点被分配较小的权重,距离大于阈值的特征点被视为异常值并分配零权重。然后,在第 $16$ 行,为一次迭代更新位姿变换。 当发现收敛或满足最大迭代次数时,非线性优化终止。 如果算法到达扫描结束,则使用扫描期间估计的运动将$P_{k+1}$重新投影到时间戳 $t_{k+2}$。 否则,仅返回变换 $T^L_{k+1}$ 用于下一轮递归。

VI.激光雷达建图

图 8. 建图过程示意图。 蓝色曲线表示地图上的激光雷达位姿 $T^W_k$ ,由建图算法在扫描 $k$ 处生成。 橙色曲线表示扫描 $k+1$ 期间的激光雷达运动 $T^L_{k+1}$,由里程计算法计算得出。 使用 $T^W_k$ 和 $T^L_{k+1}$,将里程计算法发布的未失真点云投影到地图上,表示为(绿色线段)$\overline{Q}_{k+1}$,并与地图上现有的云 $Q_k$(黑色线段)进行匹配。

建图算法运行的频率比里程计算法低,并且每次扫描只调用一次。在扫描 $k+1$ 结束时,即介于 $[t_{k+1}, t_{k+2 }]$,激光雷达里程计生成一个未失真的点云 $\overline{P}_{k+1}$ ,同时生成一个激光雷达的位姿变换 $T^L_{k+1}$ 。建图算法在世界坐标 $\left\{W\right\}$ 中匹配并注册 $\overline{P}_{k+1}$,如图 $8$ 所示。为了解释这个过程,让我们将 $Q_k$ 定义为累积到扫描 $k$ 的地图上的点云,设 $T^W_k$ 是扫描 $k$ 在 $t_{k+1}$ 结束时,激光雷达在地图上的位姿。利用激光雷达里程计的输出,建图算法将 $T^W_k$ 扩展到从 $t_{k+1}$ 到 $t_{k+2}$ 的一次扫描,以获得 $T^W_{k+1}$,并将 $\overline{P}_{k+1}$ 投影到世界坐标 $\left\{W\right\}$ 中,表示为 $\overline{Q}_{k+1}$ 。 接下来,该算法通过优化激光雷达位姿 $T^W_{k+1}$ ,将 $\overline{Q}_{k+1}$ 与 $Q_k$ 匹配。

特征点的提取方法与第 $V-A$ 节相同,但使用了 $10$ 倍的特征点。 为了找到特征点的对应关系,我们将点云存储在地图上 $Q_k$,在 $10$ 立方米区域中。立方体中与 $\overline{Q}_{k+1}$ 相交的点被提取并存储在 $3D$ $KD-tree$ [24] 中。 我们发现 $Q_k$中的点在特征点周围的某一区域内。令 $S^\prime$ 为一组周围点。 对于边缘点,我们只在 $S^\prime$ 中保留边缘线上的点,对于平面点,我们只保留平面上的点。然后,计算 $S^\prime$ 协方差矩阵,记为 $M$, $M$ 的特征值和特征向量,分别记为 $V$ 和 $E$。如果 $S^\prime$ 分布在一条边线上,$V$ 包含的一个特征值明显大于其他两个特征值,$E$ 中与最大特征值相关联的特征向量表示边缘线的方向。另一方面,如果 $S^\prime$ 分布在一个平面贴片(patch)上,$V$ 包含两个较大的特征值,第三个特征值明显较小,$E$ 中最小的特征值对应的特征向量表示该平面贴片(patch)的方位。通过 $S^\prime$ 的几何中心确定边缘线或平面贴片(patch)的位置。

为了计算特征点与其对应点的距离,我们在边缘线上选择两个点,在平面贴片(patch)上选择三个点。这允许使用$(2)$和$(3)$相同的公式来计算距离。然后,为每个特征点推导出一个方程为$(9)$ 或 $(10)$,但不同之处在于$\overline{Q}_{k+1}$ 中的所有点共享相同的时间戳$t_{k+2}$。 非线性优化通过 Levenberg-Marquardt方法 [26] 通过鲁棒拟合 [27] 再次求解,并在$\overline{Q}_{k+1}$被注册在地图上。为了均匀分布这些点,地图云通过体素网格过滤器 [28] 缩小尺寸,体素大小为 5 厘米立方体。

图 9. 位姿变换的集成。 蓝色区域说明了建图算法 $T^W_k$ 的激光雷达位姿,每次扫描生成一次。 橙色区域是当前扫描 $T^L_{k+1}$内的激光雷达运动,由里程计算法计算得出。 激光雷达的运动估计是两种变换的组合,频率与 $T^L_{k+1}$相同。

位姿变换的集成如图 $9 $所示。蓝色区域代表激光雷达映射 $T^W_k$ 的位姿输出,每次扫描生成一次。 橙色区域表示来自激光雷达里程计的变换输出$T^L_{k+1}$,频率约为$ 10Hz$。 相对于地图的激光雷达姿态是两种变换的组合,频率与激光雷达里程计相同。

VII.实验

在实验期间,在$2.5GHz$四核$6Gib$内存的笔记本电脑上,在Linux下的机器人操作系统$(ROS)$[29]上对激光雷达数据进行了处理。 该方法总共消耗两个核心,里程计和建图程序在两个独立的核心上运行。 我们的软件代码和数据集是公开的$^{1,2}$。

1wiki.ros.org/loam_back_and_forth
2wiki.ros.org/loam_continuous

A.室内和室外测试

该方法已在室内和室外环境中进行了测试。在室内测试中,激光雷达与电池和笔记本电脑一起放置在一个推车上。一个人推着手推车走。图$10(a)$和图$10(c)$展示了两种典型的室内环境,一个狭长走廊和一个大厅的地图。图$10(b)$和图$10(d)$为同一场景拍摄的两张照片。

图10所示。$(a)-(b)$狭长的走廊,$(c)-(d)$大厅,$(e)-(f)$植被覆盖的道路和$(g)-(h)$两行树木之间的果园生成的地图。激光雷达在室内测试中放置在手推车上,在室外测试中安装在地面车辆上。所有测试使用$0.5m/s$的速度。

为了评估地图的局部准确性,我们从相同的环境中收集了第二组激光雷达云。在数据选择过程中,激光雷达保持静止,并放置在每个环境中的几个不同的位置。使用点对面$ICP$方法[9]对两个点云进行匹配和比较。 匹配完成后,将一个点云与第二个点云中对应的平面块之间的距离视为匹配误差。 图 $11 $显示了误差分布的密度。室内匹配误差小于室外匹配误差。结果是合理的,因为在自然环境中特征匹配不如在人造环境中精确。

图11所示。廊道(红色)、大堂(绿色)、绿化路(蓝色)、果园(黑色)的匹配误差,对应图$10$中的四个场景。

image-20220410153545569

此外,我们还对运动估计的累积漂移进行了测试。我们选择包含闭环的走廊进行室内实验。这使得我们可以在同一个地方开始和结束。运动估计在起始位置和结束位置之间产生一个间隙,该间隙表明了漂移量。对于户外实验,我们选择果园环境。 携带激光雷达的地面车辆配备了高精度 $GPS/INS$,用于获取地面实况。 将测得的漂移与行进距离作为相对精度进行比较,并列于表 $I$。具体来说,测试 $1$ 使用与图 $10(a)$ 和图 $10(g)$ 相同的数据集。 一般来说,室内测试的相对准确度在 $1\%$ 左右,室外测试的相对准确度在 $2.5\%$ 左右。

B.融合IMU

我们将 Xsens MTi-10 IMU 连接到激光雷达以应对快速的速度变化。 点云在发送到所提出的方法之前以两种方式进行预处理,$1)$ 使用来自 $IMU$ 的方向,在一次扫描中接收到的点云被旋转,使得与该扫描中激光雷达的初始方向对齐,$2)$ 使用加速度测量 ,运动失真被部分消除,就好像激光雷达在扫描过程中以恒定速度移动一样。 然后由激光雷达里程计和建图程序处理点云。

$IMU$ 定位是通过在卡尔曼滤波器 [1] 中集成来自陀螺仪的角速率和来自加速度计的读数来获得的。 图 $12(a)$ 显示了一个示例结果。 一个人拿着激光雷达走在楼梯上。 在计算红色曲线时,我们使用 IMU 提供的方向,我们的方法只估计平移。 在 $5$ 分钟的数据收集过程中,方向漂移超过 $25°$。 假设没有可用的 $IMU$,绿色曲线仅依赖于我们方法中的优化。 蓝色曲线使用 $IMU$ 数据进行预处理,然后采用本文方法。 我们观察到绿色和蓝色曲线之间的微小差异。 图 $12(b) $给出了对应于蓝色曲线的图。 在图 $12(c)$ 中,我们比较了图 $12(b)$ 中黄色矩形中地图的两个闭合视图。 上图和下图分别对应蓝色和绿色曲线。 仔细对比发现,上图的边缘更加清晰。

image-20220410153615690

图 $12$所示。有/无$IMU$ 辅助的结果比较。 一个人拿着激光雷达走在楼梯上。 黑点是起点。 在$(a)$中,红色曲线使用来自$ IMU $的方向和我们的方法估计的平移来计算,绿色曲线仅依赖于我们方法中的优化,蓝色曲线使用$ IMU$数据进行预处理,然后再进行该方法。 $(b)$是对应于蓝色曲线的地图。 在$(c)$中,上图和下图分别对应于蓝色和绿色曲线,使用$(b)$中黄色矩形标记的区域。 上图中的边缘更清晰,表明地图上的准确性更高。

表 $II$ 比较了使用和不使用 $IMU$ 的运动估计的相对误差。 激光雷达由一个人以 $0.5m/s$ 的速度行走并以 $0.5m $左右的幅度上下移动激光雷达。 地面实况(ground truth)是由卷尺手动测量的。 在所有四个测试中,在 $IMU$ 的帮助下使用所提出的方法可以获得最高的准确度,而使用 $IMU$ 定位只会导致最低的准确度。 结果表明, $IMU$ 有效地消除了非线性运动,所提出的方法利用它来处理线性运动。

C.使用 KITTI 数据集进行测试

图 13. $(a) $KITTI 基准测试使用的传感器配置和车辆。该车辆安装有 Velodyne 激光雷达、立体摄像头和用于获取地面实况的高精度 GPS/INS。我们的方法仅使用来自 Velodyne 激光雷达的数据。$(b)$ 来自城市场景的示例激光雷达云(上图)和相应的视觉图像(下图)。

我们还使用来自 KITTI 里程计基准 [30]、[31] 的数据集评估了我们的方法。数据集仔细地与安装在结构化道路上行驶的乘用车顶部的传感器相注册(图 $13(a)$ )。车辆配备 $360°$ Velodyne 激光雷达、彩色/单色立体摄像头、以及用于地面实况(ground truth)的高精度 GPS/INS,激光雷达数据以 $10Hz$ 记录,并由我们的方法用于里程计估计。由于篇幅问题,我们无法将结果包括在内。但是,我们鼓励读者在评估(benchmark)网站上查看我们的结果 $^3$。

3www.cvlibs.net/datasets/kitti/eval_odometry.php

数据集主要涵盖三类环境:周围有建筑物的“城市”、场景中有植被的小路上的“乡村”以及道路较宽且周围环境相对干净的“高速公路”。图 13$(b)$ 显示了一个示例激光雷达云和来自城市环境的相应视觉图像。数据集中包含的总行驶距离为$ 39.2 $公里。上传车辆轨迹后,评估(benchmark)服务器会自动计算准确性和排名。我们的方法在评估(benchmark)的所有方法中排名第一,无论传感方式如何,包括最先进的立体视觉里程计 [32]、[33]。平均位置误差是行进距离的 $0.88\%$,使用 $3D $坐标中 $100m、200m、...、800m $长度的轨迹段生成。

VIII. 结论和未来的工作

使用来自旋转激光扫描仪的点云进行运动估计和建图可能很困难,因为该问题涉及激光雷达云中运动的恢复和运动失真的校正。所提出的方法通过并行运行的两种算法来划分和解决问题:激光雷达里程计进行粗略处理以估计较高频率的速度,而激光雷达建图执行精细处理以在较低频率创建地图。两种算法的合作允许实时准确的运动估计和建图。此外,该方法可以利用激光雷达扫描模式和点云分布。进行特征匹配以确保里程计算法中的快速计算,并增建图算法的准确性。该方法已在室内和室外以及 KITTI 里程计评估(benchmark)上进行了测试。

由于当前的方法不能识别闭环,我们未来的工作包括开发一种通过闭环来修复运动估计漂移的方法。此外,我们将我们方法的输出与卡尔曼滤波器中的 IMU 集成,以进一步减少运动估计漂移。

🧐 本文作者:
😉 本文链接:https://lukeyalvin.site/archives/42.html
😊 版权说明:本博客所有文章除特别声明外,均默认采用 CC BY-NC-SA 4.0 许可协议。
最后修改:2022 年 06 月 10 日
赏杯咖啡