低漂移实时激光雷达里程计与建图
Ji Zhang,Sanjiv Singh
摘要:本文提出了一种利用三维激光扫描仪在六自由度运动下的距离测量数据进行低漂移里程计和建图的实时方法。该问题具有挑战性,因为距离测量是在不同时间接收的,并且运动估计中的误差(特别是在没有GPS等外部参考的情况下)会导致生成的点云配准错误。到目前为止,连贯的三维地图通常是通过离线批量方法构建的,常常利用回环来校正随时间产生的漂移。我们的方法在运动估计中实现了低漂移,同时具有较低的计算复杂度。实现这种性能水平的关键在于将同时定位与建图这一复杂问题(该问题试图同时优化大量变量)分解为两个算法。一种算法以高频率但低保真度执行里程计,用于估计激光扫描仪的速度。虽然不是必需的,但如果有惯性测量单元(IMU),它可以提供运动先验信息,并减轻剧烈的高频运动影响。另一种算法以低一个数量级的频率运行,用于点云的精细匹配和配准。两种算法的结合使得实时创建地图成为可能。我们的方法通过室内外实验以及KITTI里程计基准测试进行了评估。结果表明,该方法的精度可与当前最先进的离线批量方法相媲美。
关键词:自运动估计;建图;连续时间;激光雷达
一、引言
三维建图仍然是一项热门技术。在激光移动的情况下,激光测距的主要问题与生成的点云配准有关。如果激光的唯一运动是从固定基座以已知的激光雷达内部运动学进行激光束指向,那么这种配准很容易实现。然而,在许多实际应用中,传感器基座是移动的,此时激光点的配准既与内部运动学有关,也与外部运动有关。后者必须包含每次距离测量时传感器的位置和方向信息。由于激光每秒可以进行多达几十万次距离测量,因此高速率的位姿估计成为一个重要问题。解决这个问题的常用方法是使用独立的位姿估计方法(如精确的GPS/INS系统),将距离数据配准到一个固定坐标系下的连贯点云中。当无法获得相对于固定坐标系的独立测量时,常用的技术是使用某种里程计估计来配准点,例如结合车轮运动、陀螺仪数据,以及通过跟踪距离图像或视觉图像中的特征。
在这里,我们考虑使用在六自由度下运动的机械扫描激光测距设备(也可选择使用低精度惯性测量进行辅助),通过低漂移里程计来创建地图的情况。仅使用激光测距的一个关键优势是它对场景中的环境光照和光学纹理不敏感。激光扫描仪的新发展已将此类设备的尺寸和重量减小到可以安装在移动机器人(包括飞行、行走或滚动机器人)上,甚至可以安装在在待映射环境中移动的人员身上。由于我们致力于在实时情况下将里程计漂移降至最低,因此不考虑与回环相关的问题。实际上,虽然回环有助于进一步消除漂移,但我们发现,在许多实际情况中,如对建筑物的某一层进行映射时,回环并非必要。
我们的方法,即LOAM(激光雷达里程计与建图),在六自由度运动估计中实现了低漂移,同时保持较低的计算复杂度。实现这种性能的关键思想是将通常复杂的同时定位与建图问题(如图1所示)分解为两个算法,该问题通常需要同时优化大量变量。一种算法以高频率但低保真度执行里程计,用于估计在环境中移动的激光扫描仪的速度。如果有IMU,它可以提供运动先验信息,并有助于处理剧烈的高频运动,尽管这不是必需的。另一种算法以低一个数量级的频率运行,用于点云的精细匹配和配准。具体来说,两种算法都提取位于边缘和平面表面上的特征点,并分别将这些特征点与边缘线段和平面面片进行匹配。在里程计算法中,通过确保快速计算来找到特征点的对应关系;而在映射算法中,则通过确保准确性来寻找对应关系。
在该方法中,首先解决一个相对简单的问题,即在线速度估计,然后将映射作为批量优化进行,以产生高精度的运动估计和地图。并行算法结构确保了该问题能够在实时情况下得到解决。此外,由于运动估计以较高频率进行,为映射提供了充足的时间来保证准确性。当映射算法的运行频率比里程计算法低一个数量级时,它可以纳入大量特征点,并使用足够多的迭代次数来实现收敛。本文的主要贡献如下:
- 我们提出了一种使用双层优化的软件系统,用于在线估计自运动并构建地图;
- 我们精心实现了几何特征检测和匹配,以满足系统的要求:里程计算法中的特征匹配是粗略且快速的,以确保高频率;而在映射算法中则是精确且缓慢的,以确保低漂移;
- 我们使用大量涵盖各种环境的数据集对该方法进行了全面测试;
- 我们尽力详细地介绍我们的工作,以便读者能够重新实现该方法。
本文的其余部分组织如下。在第2节中,我们讨论相关工作,并说明我们的工作与现有技术相比的独特之处。在第3节中,我们正式提出研究问题。第4节描述激光雷达硬件和软件系统。第5节详细介绍里程计算法,第6节介绍映射算法。第7节展示实验结果。最后,在第7节和第8节中分别进行讨论和总结。
二、相关工作
激光雷达已成为机器人导航中一种有用的测距传感器。对于定位和建图,一种方法是进行停止扫描,以避免点云中的运动失真(Nuchter等人,2007年)。此外,当激光雷达的扫描速率相对于其外部运动较高时,运动失真可以忽略不计。在这种情况下,可以使用ICP方法(Pomerleau等人,2013年)来匹配不同扫描之间的激光回波。另外,有人提出了一种两步法来消除失真(Hong等人,2010年):首先基于ICP进行速度估计,然后使用计算出的速度进行失真补偿。类似的技术也用于补偿单轴三维激光雷达引入的失真(Moosmann和Stiller,2011年)。然而,如果扫描运动相对较慢,运动失真可能会很严重。使用两轴激光雷达时尤其如此,因为其中一个轴的速度通常比另一个轴慢得多。通常,会使用其他传感器来提供速度测量值,从而可以消除失真。例如,可以通过集成了IMU的视觉里程计进行状态估计,来配准激光雷达点云(Scherer等人,2012年)。当同时具备多个传感器,如GPS/INS和车轮编码器时,该问题通常通过卡尔曼滤波器或粒子滤波器来解决,从而实时构建地图。
如果使用两轴激光雷达且没有其他传感器辅助,运动估计和失真校正就成为一个问题。在Barfoot等人的方法中,传感器运动被建模为匀速运动(Dong和Barfoot,2012年;Anderson和Barfoot,2013年a),并使用高斯过程(Tong和Barfoot,2013年;Anderson和Barfoot,2013年b;Tong等人,2013年)。Rosen等人使用高斯过程对连续的传感器运动进行建模,并将该问题转化为因子图优化问题(Rosen等人,2014年)。此外,Furgale等人提出使用B样条函数对传感器运动进行建模(Furgale等人,2012年)。我们的方法在里程计算法中使用了与Dong和Barfoot(2012年)、Anderson和Barfoot(2013年a)类似的线性运动模型。然而,在映射算法中,我们使用刚体变换。另一种方法是Bosse和Zlot的方法(Bosse和Zlot,2009年;Bosse等人,2012年;Zlot和Bosse,2012年)。他们发明了一种名为Zebedee的三维映射设备,由一个二维激光雷达和一个通过弹簧连接到手持杆上的IMU组成(Bosse等人,2012年)。通过手持设备点头进行映射,轨迹通过批量优化方法恢复,该方法处理分段数据集,并在段与段之间添加边界约束。在这种方法中,IMU的测量值用于配准激光点,优化用于校正IMU的漂移和偏差。此外,他们使用多个两轴激光雷达对地下矿井进行映射(Zlot和Bosse,2012年)。这种方法结合了IMU并使用回环来创建大型地图,运行速度比实时更快。然而,由于它依赖批量处理来构建精确地图,目前该方法很难用于在线应用,无法提供实时状态估计和地图。
基于视觉的状态估计也存在同样的运动分布问题。对于滚动快门相机,图像像素是随时间连续感知的,这导致每个像素的读出时间不同。处理滚动快门效应的现有视觉里程计方法得益于IMU(Guo等人,2014年;Li和Mourikis,2014年)。这些方法使用IMU机械装置根据像素的读出时间来补偿运动。在本文中,我们也可以选择使用IMU来消除非线性运动,而我们提出的方法用于解决线性运动问题。
从特征的角度来看,Barfoot等人的方法(Dong和Barfoot,2012年;Anderson和Barfoot,2013年a,b;Tong和Barfoot,2013年)从激光强度回波创建视觉图像,并匹配图像之间视觉上明显的特征(Bay等人,2008年)以恢复运动。这需要具有强度值的密集点云。另一方面,Bosse和Zlot的方法(Bosse和Zlot,2009年;Bosse等人,2012年;Zlot和Bosse,2012年)匹配由局部点簇形成的时空面片。与Dong和Barfoot(2012年)、Anderson和Barfoot(2013年a,b)以及Tong和Barfoot(2013年)相比,我们的方法对密集点云的要求较低,并且不需要强度值,因为它在笛卡尔空间中提取和匹配几何特征。我们的方法使用两种类型的点特征,即边缘上的点和局部平面表面上的点,并分别将它们与边缘线段和局部平面面片进行匹配。
我们提出的方法实时生成的地图在质量上与Bosse和Zlot的方法生成的地图相似。不同之处在于我们的方法可以提供运动估计,为自动驾驶车辆提供导航。本文是我们会议论文(Zhang和Singh,2014年)的扩展版本,我们用更多的实验对该方法进行了评估,并更详细地进行了介绍。
三、符号和任务描述
本文解决的问题是利用三维激光雷达感知的点云进行自运动估计,并为遍历的环境构建地图。我们假设激光雷达已经进行了精确的内部校准,其内部运动学是已知的(内部校准使得激光点的三维投影成为可能)。我们还假设激光雷达的角速度和线速度在时间上是平滑且连续的,没有突变。在第7.2节和7.3节中,将通过使用IMU来放宽第二个假设。
在本文中,我们使用右上标来表示坐标系。我们将一次扫描覆盖定义为一次扫描。使用右下标k,\(k \in Z^{+}\)表示扫描次数,\(P_{k}\)表示在第k次扫描期间感知到的点云。定义两个坐标系如下:
- 激光雷达坐标系 \({L}\):这是一个三维坐标系,原点位于激光雷达的几何中心(见图2)。这里,我们采用相机的惯例。x轴指向左侧,y轴指向上方,z轴指向前方。将在第k次扫描期间接收到的点i表示为\(X_{(k, i)}^{L}\) 。此外,用\(T_{k}^{L}(t)\)表示将在时间t接收到的点投影到第k次扫描开始时的变换。
- 世界坐标系 \({W}\):这是一个三维坐标系,在初始位姿时与 \({L}\) 重合。将世界坐标系 \({W}\) 中的点i表示为\(X_{(k, i)}^{W}\) ,并将\(T_{k}^{W}(t)\)表示为将在时间t接收到的点投影到世界坐标系 \({W}\) 的变换。
基于上述假设和符号,激光雷达里程计和建图问题可以定义为:
问题:给定一系列激光雷达点云\(P_{k}\),\(k \in Z^{+}\),计算激光雷达在世界坐标系中的自运动\(T_{k}^{W}(t)\),并使用\(P_{k}\)为遍历的环境构建地图。
四、系统概述
(一)激光雷达硬件
本文的研究在四个传感器系统上进行了验证:一种往复旋转激光雷达、一种连续旋转激光雷达、一个Velodyne HDL - 32激光雷达,以及KITTI基准测试(Geiger等人,2012年,2013年)中使用的传感器系统。我们以第一个激光雷达硬件为例来说明该方法,因此在文章开头介绍该激光雷达硬件,以帮助读者理解。其余传感器将在实验部分介绍。如图2所示,该激光雷达基于Hokuyo UTM - 30LX激光扫描仪,其视野为180°,分辨率为0.25°,扫描速率为40线/秒。激光扫描仪连接到一个电机上,电机控制其以180°/秒的角速度在 - 90°到90°之间旋转,激光扫描仪的水平方向为零。对于这个特定的设备,一次扫描是从 - 90°到90°或相反方向的旋转(持续1秒)。需要注意的是,对于连续旋转激光雷达,一次扫描简单来说就是半球形或全球形旋转。一个车载编码器以0.25°的分辨率测量电机旋转角度,通过它可以将激光点反向投影到激光雷达坐标系 \({L}\) 中。
(二)软件系统概述
图3展示了软件系统的示意图。设\(\hat{P}\)为在一次激光扫描中接收到的点。在每次扫描期间,\(\hat{P}\)在 \({L}\) 坐标系中进行配准。第k次扫描期间组合的点云形成\(P_{k}\) 。然后,\(P_{k}\)由两个算法进行处理。激光雷达里程计获取点云,并计算激光雷达在两次连续扫描之间的运动。估计的运动用于校正\(P_{k}\)中的失真。该算法以大约10Hz的频率运行。其输出进一步由激光雷达映射处理,激光雷达映射以1Hz的频率将未失真的点云匹配并配准到地图上。最后,将两个算法发布的位姿变换进行整合,以大约10Hz的频率生成一个变换输出,表示激光雷达相对于地图的位姿。第5节和第6节将详细介绍软件示意图中的各个模块。
五、激光雷达里程计
(一)特征点提取
我们从激光雷达点云\(P_{k}\)中提取特征点。注意到许多三维激光雷达自然生成的点云\(P_{k}\)中点的分布不均匀。以图2中的激光雷达为例,激光扫描仪在一次扫描中的返回点分辨率为0.25°,这些点位于一个扫描平面上。然而,由于激光扫描仪以180°/秒的角速度旋转,并以40Hz的频率生成扫描,在垂直于扫描平面的方向上,分辨率为\(180^{\circ} / 40 = 4.5^{\circ}\) 。考虑到这一事实,我们仅使用单个扫描中的信息,基于共面几何关系从\(P_{k}\)中提取特征点。
我们选择位于尖锐边缘和平面面片上的特征点。设i是\(P_{k}\)中的一个点,\(i \in P_{k}\) ,s是激光扫描仪在同一扫描中返回的与i连续的点集。由于激光扫描仪按顺时针或逆时针顺序生成点返回,s在i的每一侧包含其一半的点,且两点之间间隔0.25°(仍以图2中的激光雷达为例)。定义一个术语来评估局部表面的平滑度:
\(c=\frac{1}{|\mathcal{S}| \cdot\left\| X_{(k, i)}^{L}\right\| }\left\| \sum_{j \in \mathcal{S}, j \neq i}\left(X_{(k, i)}^{L}-X_{(k, j)}^{L}\right)\right\|\)
该术语相对于到激光雷达中心的距离进行了归一化,这是为了消除尺度效应,并且该术语对于近点和远点都适用。
点云\(P_{k}\)中的点根据c值进行排序,然后选择c值最大的点作为边缘点,选择c值最小的点作为平面点。为了使特征点在环境中均匀分布,我们将一次扫描划分为四个相同的子区域。每个子区域最多可提供2个边缘点和4个平面点。一个点i只有在其c值大于或小于一个阈值(\(5 × 10^{-3}\)),并且所选点的数量不超过子区域的最大点数时,才能被选为边缘点或平面点。
在选择特征点时,我们要避免选择周围点已被选中的点,或者位于与激光束大致平行的局部平面上的点(如图4a中的点B)。这些点通常被认为不可靠。同时,我们也要避免选择位于遮挡区域边界的点(Li和Olson,2011) 。图4b展示了一个例子,点c在激光雷达点云中是一个边缘点,因为其相连的表面(虚线部分)被另一个物体遮挡。然而,如果激光雷达移动到另一个视角,被遮挡的区域可能会改变并变得可见。为了避免上述点被选中,我们再次找到点集S。只有当S不构成一个法向量与激光束夹角在10°以内的平面面片,并且S中不存在在激光束方向上与点i有间隙且比点i更靠近激光雷达的点(例如图4b中的点B)时,点i才能被选中。
总之,特征点的选择方式为:从c值最大的点开始选择边缘点,从c值最小的点开始选择平面点,并且若一个点被选中,需满足以下条件:所选边缘点或平面点的数量不能超过子区域的最大值;其周围点均未被选中;该点不在法向量与激光束夹角在10°以内的平面面片上,也不在遮挡区域的边界上。
图5展示了从走廊场景中提取特征点的示例,边缘点和平面点分别用黄色和红色标记。
(二)寻找特征点对应关系
里程计算法用于估计激光雷达在一次扫描内的运动。设\(t_{k}\)为第k次扫描的起始时间。在第\(k - 1\)次扫描结束时,将该次扫描期间感知到的点云\(P_{k - 1}\)投影到时间戳\(t_{k}\),如图6所示(我们将在5.3节讨论点的投影变换) 。将投影后的点云记为\(\overline{P}_{k - 1}\) 。在接下来的第k次扫描中,\(\overline{P}_{k - 1}\)与新接收的点云\(P_{k}\)一起用于估计激光雷达的运动。
假设目前\(\overline{P}_{k - 1}\)和\(P_{k}\)均已获取,接下来开始寻找这两个激光雷达点云之间的对应关系。利用\(P_{k}\) ,通过上一节讨论的方法从激光雷达点云中找到边缘点和平面点。设\(\varepsilon_{k}\)和\(H_{k}\)分别为边缘点集和平面点集。我们将从\(\overline{P}_{k - 1}\)中找到边缘线作为\(\varepsilon_{k}\)中点的对应关系,找到平面面片作为\(H_{k}\)中点的对应关系。
需要注意的是,在第k次扫描开始时,\(P_{k}\)为空集,随着扫描的进行,\(P_{k}\)逐渐包含更多的点。激光雷达里程计递归地估计扫描期间的六自由度运动,随着\(P_{k}\)中点的增加,逐渐纳入更多的点。将\(\varepsilon_{k}\)和\(H_{k}\)投影到扫描开始时刻(同样,稍后将讨论点的投影变换) 。设\(\tilde{E}_{k}\)和\(\tilde{H}_{k}\)为投影后的点集。对于\(\tilde{E}_{k}\)和\(\tilde{H}_{k}\)中的每个点,我们要在\(\overline{P}_{k - 1}\)中找到其最近邻点。这里,\(\overline{P}_{k - 1}\)存储在\(L_{k}\)坐标系下的一个三维KD树(Berg等人,2008)中,以便快速索引。
图7a展示了寻找边缘线作为边缘点对应关系的过程。设i是\(\tilde{\varepsilon}_{k}\)中的一个点,\(i \in \tilde{E}_{k}\) 。边缘线由两个点表示。设j是\(\overline{P}_{k - 1}\)中i的最近邻点,\(j \in \overline{P}_{k - 1}\) ,设l是j所在扫描的前一次和后一次扫描中i的最近邻点。\((j, l)\)构成了i的对应关系。然后,为了验证i和l都是边缘点,我们根据公式(1)检查局部表面的平滑度,并要求这两个点的\(c>5 × 10^{-3}\) 。这里,我们特别要求j和l来自不同的扫描,因为单个扫描不能包含同一条边缘线上的多个点。只有一种例外情况,即边缘线位于扫描平面上。但在这种情况下,边缘线会退化,在扫描平面上呈现为一条直线,并且一开始就不应该从这条边缘线上提取特征点。
图7b展示了寻找平面面片作为平面点对应关系的过程。设i是\(\tilde{H}_{k}\)中的一个点,\(i \in \tilde{H}_{k}\) 。平面面片由三个点表示。与上一段类似,我们在\(\overline{P}_{k - 1}\)中找到i的最近邻点,记为j。然后,我们再找到另外两个点,l和m,分别是i在j所在扫描(但不是j点)以及j所在扫描的前一次和后一次扫描中的最近邻点。这确保了这三个点不共线。为了验证j、l和m都是平面点,我们再次检查局部表面的平滑度,并要求\(c<5 × 10^{-3}\) 。
找到特征点的对应关系后,我们推导出计算特征点到其对应关系距离的表达式。在下一节中,我们将通过最小化特征点的总距离来恢复激光雷达的运动。首先从边缘点开始。对于点\(i \in \tilde{E}_{k}\) ,如果\((j, l)\)是其对应的边缘线,\(j, l \in \overline{P}_{k - 1}\) ,点到线的距离可以计算为:
\(d_{\mathcal{E}}=\frac{\left|\left(\tilde{X}_{(k, i)}^{L}-\overline{X}_{(k-1, j)}^{L}\right) \times\left(\tilde{X}_{(k, i)}^{L}-\overline{X}_{(k-1, l)}^{L}\right)\right|}{\left|\overline{X}_{(k-1, j)}^{L}-\overline{X}_{(k-1, l)}^{L}\right|}\)
其中,\(\tilde{X}_{(k, i)}^{L}\) 、\(\overline{X}_{(k-1, j)}^{L}\)和\(\overline{X}_{(k-1, l)}^{L}\)分别是点i、j和l在\(L_{k}\)坐标系中的坐标。然后,对于点\(i \in \tilde{H}_{k}\) ,如果\((j, l, m)\)是其对应的平面面片,\(j, l, m \in \overline{P}_{k - 1}\) ,点到平面的距离为:
\(d_{\mathcal{H}}=\frac{\left|\begin{array}{c} \left(\tilde{X}_{(k, i)}^{L}-\overline{X}_{(k-1, j)}^{L}\right) \\ \left(\left(\overline{X}_{(k-1, j)}^{L}-\overline{X}_{(k-1, l)}^{L}\right) \times\left(\overline{X}_{(k-1, j)}^{L}-\overline{X}_{(k-1, m)}^{L}\right)\right) \end{array}\right|}{\left|\left(\overline{X}_{(k-1, j)}^{L}-\overline{X}_{(k-1, l)}^{L}\right) \times\left(\overline{X}_{(k-1, j)}^{L}-\overline{X}_{(k-1, m)}^{L}\right)\right|}\)
(三)运动估计
激光雷达的运动被建模为在一次扫描内具有恒定的角速度和线速度。这使得我们可以对在不同时间接收的点进行线性插值,以得到位姿变换。设t为当前时间戳,\(t_{k}\)是当前第k次扫描的起始时间。设\(T_{k}^{L}(t)\)是在\([t_{k}, t]\)时间段内激光雷达的位姿变换。\(T_{k}^{L}(t)\)包含激光雷达的六自由度运动,\(T_{k}^{L}(t)=[\tau_{k}^{L}(t), \theta_{k}^{L}(t)]^{T}\) ,其中\(\tau_{k}^{L}(t)=[t_{x}, t_{y}, t_{z}]^{T}\)是平移量,\(\theta_{k}^{L}(t)=[\theta_{x}, \theta_{y}, \theta_{z}]^{T}\)是在\(L_{k}\)坐标系中的旋转量。给定\(\theta_{k}^{L}(t)\) ,相应的旋转矩阵可以由罗德里格斯公式(Murray和Sastry,1994)定义:
\(\begin{aligned} R_{k}^{L}(t)= & e^{\hat{\theta}_{k}^{L}(t)}=I+\frac{\hat{\theta}_{k}^{L}(t)}{\left\| \theta_{k}^{L}(t)\right\| } \sin \left\| \theta_{k}^{L}(t)\right\| \\ & +\left(\frac{\hat{\theta}_{k}^{L}(t)}{\left\| \theta_{k}^{L}(t)\right\| }\right)^{2}\left(1-\cos \left\| \theta_{k}^{L}(t)\right\| \right) \end{aligned}\)
其中,\(\hat{\theta}_{k}^{L}(t)\)是\(\theta_{k}^{L}(t)\)的反对称矩阵。
对于点\(i \in P_{k}\) ,设\(t_{(k, i)}\)是其时间戳,设\(T_{(k, i)}^{L}\)是在\([t_{k}, t_{(k, i)}]\)时间段内的位姿变换。\(T_{(k, i)}^{L}\)可以通过对\(T_{k}^{L}(t)\)进行线性插值得到:
\(T_{(k, i)}^{L}=\frac{t_{(k, i)}-t_{k}}{t-t_{k}} T_{k}^{L}(t)\)
需要注意的是,\(T_{k}^{L}(t)\)是一个随时间变化的变量,插值使用当前时间t的变换。回想一下,\(\varepsilon_{k}\)和\(H_{k}\)是从\(P_{k}\)中提取的边缘点集和平面点集。以下公式有助于将\(\varepsilon_{k}\)和\(H_{k}\)投影到扫描开始时刻,即得到\(\tilde{E}_{k}\)和\(\tilde{H}_{k}\) :
\(\tilde{X}_{(k, i)}^{L}=R_{(k, i)}^{L} X_{(k, i)}^{L}+\tau_{(k, i)}^{L}\)
其中,\(X_{(k, i)}^{L}\)是\(\varepsilon_{k}\)或\(H_{k}\)中的一个点,\(\tilde{X}_{(k, i)}^{L}\)是\(\tilde{\varepsilon}_{k}\)或\(\tilde{H}_{k}\)中对应的点,\(R_{(k, i)}^{L}\)和\(\tau_{(k, i)}^{L}\)是与\(T_{(k, i)}^{L}\)对应的旋转矩阵和平移向量。
回想一下,公式(2)和(3)计算了\(\tilde{E}_{k}\)和\(\tilde{H}_{k}\)中的点与其对应关系之间的距离。结合公式(2)和(6),我们可以推导出\(\varepsilon_{k}\)中的边缘点与其对应边缘线之间的几何关系:
\(f_{\mathcal{E}}\left(X_{(k, i)}^{L}, T_{k}^{L}(t)\right)=d_{\mathcal{E}}, i \in \mathcal{E}_{k}\)
类似地,结合公式(3)和(6),我们可以建立\(H_{k}\)中的平面点与其对应平面面片之间的另一种几何关系:
\(f_{\mathcal{H}}\left(X_{(k, i)}^{L}, T_{k}^{L}(t)\right)=d_{\mathcal{H}}, i \in \mathcal{H}_{k}\)
最后,我们使用列文伯格 - 马夸尔特方法(Hartley和Zisserman,2004)来求解激光雷达的运动。将\(\varepsilon_{k}\)和\(H_{k}\)中每个特征点的公式(7)和(8)堆叠起来,得到一个非线性函数:
\(f\left(T_{k}^{L}(t)\right)=d\)
其中,f的每一行对应一个特征点,d包含相应的距离。我们计算f关于\(T_{k}^{L}(t)\)的雅可比矩阵,记为J,其中\(J=\partial f / \partial T_{k}^{L}(t)\) 。然后,通过将d最小化至零,通过非线性迭代求解公式(9):
\(T_{k}^{L}(t) \leftarrow T_{k}^{L}(t)-\left(J^{T} J+\lambda \text{diag}\left(J^{T} J\right)\right)^{-1} J^{T} d\)
其中,\(\lambda\)是由列文伯格 - 马夸尔特方法确定的一个因子。
(四)激光雷达里程计算法
激光雷达里程计算法如算法1所示。该算法的输入为上一次扫描的点云\(\overline{P}_{k - 1}\) 、当前扫描正在增长的点云\(P_{k}\) ,以及上一次递归得到的位姿变换\(T_{k}^{L}(t)\)作为初始猜测值。如果开始新的一次扫描,则将\(T_{k}^{L}(t)\)设置为零进行重新初始化(第4 - 6行) 。然后,算法在第7行从\(P_{k}\)中提取特征点,构建\(\varepsilon_{k}\)和\(H_{k}\) 。对于每个特征点,在\(\overline{P}_{k - 1}\)中找到其对应关系(第9 - 19行) 。运动估计采用稳健拟合框架(Andersen,2008) 。在第15行,算法为每个特征点分配一个双平方权重,公式如下。与对应关系距离较大的特征点被分配较小的权重,距离大于阈值的特征点被视为异常值,权重设为零。
\(w= \begin{cases}\left(1-\alpha^{2}\right)^{2} & -1<\alpha<1 \\ 0 & \text{otherwise}\end{cases}\)
其中,\(\alpha=\frac{r}{6.9459 \sigma \sqrt{1-h}}\) ,r是最小二乘问题中的相应残差,\(\sigma\)是残差相对于中位数的绝对偏差,h是杠杆值,即矩阵\(J(J^{T} J)^{-1} J^{T}\)对角线上的相应元素,J是公式(10)中使用的雅可比矩阵。然后,在第16行,位姿变换进行一次迭代更新。如果非线性优化收敛,或者达到最大迭代次数,则非线性优化终止。如果算法到达一次扫描的结束时刻,则使用扫描期间估计的运动将\(P_{k}\)投影到时间戳\(t_{k + 1}\) ,形成\(\overline{P}_{k}\) 。这为下一次扫描与\(\overline{P}_{k}\)的匹配做好准备。否则,算法仅返回变换\(T_{k}^{L}(t)\) ,用于下一轮递归。
六、激光雷达建图
建图算法的运行频率低于里程计算法,每次扫描仅调用一次。在第 \(k\) 次扫描结束时,激光雷达里程计生成一个无畸变的点云 \(\overline{P}_{k}\) ,同时生成一个位姿变换 \(T_{k}^{L}(t_{k + 1})\) ,该变换包含了激光雷达在本次扫描(即 \([t_{k}, t_{k + 1}]\) 时间段内)的运动信息。建图算法将 \(\overline{P}_{k}\) 在世界坐标系 \({W}\) 中进行匹配和配准,如图8所示。为解释该过程,定义 \(Q_{k - 1}\) 为截至第 \(k - 1\) 次扫描在地图上累积的点云,\(T_{k - 1}^{W}(t_{k})\) 为第 \(k - 1\) 次扫描结束时激光雷达在地图上的位姿。利用激光雷达里程计的输出,建图算法将 \(T_{k - 1}^{W}(t_{k})\) 从 \(t_{k}\) 扩展一个扫描周期到 \(t_{k + 1}\) ,得到 \(T_{k}^{W}(t_{k + 1})\) ,并将 \(\overline{P}_{k}\) 变换到世界坐标系 \({W}\) 中,记为 \(\bar{Q}_{k}\) 。接下来,算法通过优化激光雷达位姿 \(T_{k}^{W}(t_{k + 1})\) ,将 \(\bar{Q}_{k}\) 与 \(Q_{k - 1}\) 进行匹配。
特征点的提取方式与5.1节相同,但使用的特征点数量是其10倍。为寻找特征点的对应关系,我们将地图上的点云 \(Q_{k - 1}\) 按10立方米的立方体区域存储。提取与 \(\bar{Q}_{k}\) 相交的立方体中的点,并将其存储在世界坐标系 \({W}\) 下的三维KD树(Berg等人,2008)中。在特征点周围特定区域(10cm×10cm×10cm)内寻找 \(Q_{k - 1}\) 中的点,设 \(S'\) 为这些周围点的集合。对于边缘点,仅保留 \(S'\) 中位于边缘线上的点;对于平面点,仅保留位于平面面片上的点。根据点的 \(c\) 值区分边缘点和平面点,这里使用与5.1节相同的阈值(\(5 × 10^{-3}\)) 。然后,计算 \(S'\) 的协方差矩阵,记为 \(M\) ,以及 \(M\) 的特征值和特征向量,分别记为 \(V\) 和 \(E\) 。这些值决定了点簇的位姿,进而决定了点到线和点到平面的距离。具体而言,如果 \(S'\) 分布在一条边缘线上,\(V\) 会包含一个明显大于其他两个的特征值,\(E\) 中与最大特征值相关的特征向量代表边缘线的方向。另一方面,如果 \(S'\) 分布在一个平面面片上,\(V\) 会包含两个较大的特征值,且第三个特征值明显较小,\(E\) 中与最小特征值相关的特征向量表示平面面片的方向。边缘线或平面面片的位置计算方式为使其通过 \(S'\) 的质心。
为计算特征点到其对应关系的距离,我们在边缘线上选择两个点,在平面面片上选择三个点。这样就可以使用与公式(2)和(3)相同的公式来计算距离。然后,为每个特征点推导一个类似公式(7)或(8)的方程,但不同之处在于 \(\overline{Q}_{k}\) 中的所有点共享相同的时间戳 \(t_{k + 1}\) 。再次使用适用于稳健拟合(Andersen,2008)的列文伯格 - 马夸尔特方法(Hartley和Zisserman,2004)求解非线性优化问题,进而将 \(\bar{Q}_{k}\) 配准到地图上。
为使点分布均匀,每次将新的扫描与地图合并时,使用体素网格滤波器(Rusu和Cousins,2011)对地图点云进行下采样。体素网格滤波器对每个体素中的所有点求平均,在体素中留下一个平均点。边缘点和平面点使用不同的体素大小,边缘点的体素大小为5cm×5cm×5cm,平面点的体素大小为10cm×10cm×10cm。地图在传感器周围500m×500m×500m的区域内进行截断,以限制内存使用。
位姿变换的整合如图9所示。蓝色区域代表激光雷达建图输出的位姿 \(T_{k - 1}^{W}(t_{k})\) ,每次扫描生成一次。橙色区域代表激光雷达里程计输出的变换 \(T_{k}^{L}(t)\) ,频率约为10Hz。激光雷达相对于地图的位姿是这两个变换的组合,频率与激光雷达里程计相同。
七、实验
在实验过程中,处理激光雷达数据的算法运行在一台装有2.5GHz四核处理器和6GB内存的笔记本电脑上,操作系统为Linux,基于机器人操作系统(ROS)(Quigley等人,2009) 。该方法共使用两个线程,里程计和建图程序分别在两个独立线程中运行。
(一)精度测试
使用图2中的激光雷达在室内和室外环境中对该方法进行了测试。在室内测试中,激光雷达与电池和笔记本电脑一起放置在小车上,由一人推动小车行走。图10a、c展示了在两个具有代表性的室内环境(狭窄长廊和大型大厅)中构建的地图,图10b、d展示了从相同场景拍摄的照片。在室外测试中,激光雷达安装在地面车辆的前部。图10e、g展示了在植被覆盖的道路和两排树之间的果园中生成的地图,图10f、h分别展示了相应的照片。在所有测试中,激光雷达的移动速度均为0.5m/s。
为评估地图的局部精度,我们从相同环境中收集了第二组激光雷达点云。在数据采集过程中,将激光雷达固定在每个环境中的几个不同位置。使用点到平面ICP方法(Rusinkiewicz和Levoy,2001)对两组点云进行匹配和比较。匹配完成后,将一组点云与另一组点云中相应平面面片之间的距离视为匹配误差。图11展示了误差分布的密度。结果表明,室内环境中的匹配误差小于室外环境,这是合理的,因为在自然环境中特征匹配不如在人造环境中精确。
此外,我们想了解激光雷达里程计和激光雷达建图如何发挥作用以及对最终精度的贡献。为此,我们选取图1中的数据集,展示每个算法的输出。轨迹长度为32m。图12a使用激光雷达里程计的输出直接配准激光点,而图12b是经过激光雷达建图优化后的最终输出。如前文所述,激光雷达里程计的作用是估计速度并去除点云中的运动畸变。激光雷达里程计的低精度无法保证精确的建图,而激光雷达建图则进一步进行仔细的扫描匹配,以确保地图的准确性。表1展示了精度测试中两个程序的计算时间分解。可以看到,激光雷达建图消除漂移所需的计算量是激光雷达里程计的6.4倍。需要注意的是,激光雷达里程计被调用10次,而激光雷达建图仅被调用1次,这使得两个线程的计算负载处于同一水平。
此外,我们进行测试以测量运动估计的累积漂移。室内实验选择包含闭环的走廊环境,这样可以使起始和结束位置相同。运动估计会在起始和结束位置之间产生间隙,该间隙大小表示漂移量。室外实验选择果园环境,搭载激光雷达的地面车辆配备了高精度的GPS/INS用于获取真实位姿。将测量得到的漂移量与行驶距离进行比较,得到相对精度,结果列于表2中。具体而言,测试1使用与图10a、g相同的数据集。一般来说,室内测试的相对精度约为1%,室外测试约为2.5%。
(二)IMU辅助测试
我们在激光雷达上连接了一个Xsens MTi - 10 IMU,以处理快速的速度变化。在将点云输入到所提方法之前,先对其进行两种预处理:(1)利用IMU的方向信息,将一次扫描中接收到的点云旋转,使其与该次扫描中激光雷达的初始方向对齐;(2)利用加速度测量值,部分消除运动畸变,就好像激光雷达在扫描期间做匀速运动一样。这里,IMU的方向是通过在卡尔曼滤波器(Thrun等人,2005)中对陀螺仪的角速率和加速度计的读数进行积分得到的。经过IMU预处理后,剩余需要求解的运动是IMU的方向漂移(假设在一次扫描内为线性漂移)和线速度,这满足了激光雷达在一次扫描内做线性运动的假设。然后,点云由激光雷达里程计和建图程序进行处理。
图13a展示了一个示例结果。一个人手持激光雷达在楼梯上行走,计算红色曲线时,使用IMU提供的方向信息,所提方法仅估计平移量。在5分钟的数据采集过程中,方向漂移超过25°。绿色曲线仅依赖所提方法中的优化,假设没有IMU辅助。蓝色曲线使用IMU数据进行预处理,然后再使用所提方法。可以观察到绿色曲线和蓝色曲线之间的差异较小。图13b展示了与蓝色曲线对应的地图。在图13c中,比较了图13b中黄色矩形区域内地图的两个特写视图,上图和下图分别对应蓝色曲线和绿色曲线。仔细比较发现,上图中的边缘比下图更清晰。
表3比较了使用和不使用IMU时运动估计的相对误差。激光雷达由一个人手持,以0.5m/s的速度行走,并上下移动激光雷达,移动幅度约为0.5m。真实位姿通过卷尺手动测量。在所有四个测试中,使用所提方法并结合IMU辅助获得的精度最高,而仅使用IMU方向信息的精度最低。结果表明,IMU在消除非线性运动方面是有效的,结合所提方法可以处理线性运动。
(三)微型直升机数据集测试
我们进一步使用从八旋翼微型无人机收集的数据评估该方法。如图14所示,无人机上安装了一个两轴激光雷达,其设计与图2中的激光雷达相同,不同之处在于激光扫描仪连续旋转。对于这样的激光雷达单元,一次扫描定义为在慢轴上的半球形旋转,持续1秒。无人机上还安装了一个Microstrain 3DM - GX3 - 45 IMU。里程计和建图程序同时处理激光雷达和IMU数据。
在图15和图16中展示了两个数据集的结果。在两个测试中,无人机均由手动操作,飞行速度为1m/s。在图15中,无人机从桥的一侧起飞,飞到桥下,然后折返再次飞到桥下。在图16中,无人机从地面起飞,最后降落到地面。在图15a和图16a中展示了飞行轨迹,在图15b和图16b中展示了地图的特写视图,对应图15c和图16c中黄色矩形区域内的地图。我们无法获取无人机位姿或地图的真实值。对于这两个测试中的相对较小的环境,该方法在测试开始时构建的地图上不断重新定位,因此计算闭环误差变得没有意义。相反,只能通过放大视图直观检查地图的精度。
(四)Velodyne激光雷达测试
这些实验使用安装在两辆车辆上的Velodyne HDL - 32E激光雷达,如图17所示。图17a是一辆在人行道和越野地形上行驶的多功能车辆,图17b是一辆在街道上行驶的乘用车。对于这两辆车,激光雷达都安装在顶部较高位置,以避免可能被车身遮挡。
Velodyne HDL - 32E是一款单轴激光扫描仪,它同时向三维环境发射32束激光。我们将每束激光形成的平面视为一个扫描平面,一次扫描定义为激光扫描仪的整圈旋转。激光雷达默认以10Hz的频率采集扫描数据。我们将激光雷达里程计配置为以10Hz的频率处理单个扫描,激光雷达建图则将扫描数据堆叠1秒进行批量优化。两个程序的计算时间分解如表4所示。
图18展示了对大学校园进行映射的结果。使用图17a中的车辆记录了1.0km行程的数据,测试期间的行驶速度保持在2 - 3m/s。图18a展示了最终构建的地图,图18b展示了估计的轨迹(红色曲线)以及叠加在卫星图像上的配准激光点。通过将轨迹与人行道(车辆未在街道上行驶)进行匹配,以及将激光点与建筑物墙壁进行匹配,确定水平位置漂移≤1m。通过比较两侧映射的建筑物,确定垂直漂移≤1.5m,这使得总体位置误差≤行驶距离的0.2%。
图19展示了另一个测试的结果。使用图17b中的车辆在街道上行驶3.6km,除了等待红绿灯外,车辆速度大多在11 - 18m/s之间。图19的布局与图18相同。通过与卫星图像比较,确定水平位置误差≤2m,但垂直精度无法评估。
(五)KITTI数据集测试
最后,我们使用KITTI里程计基准测试(Geiger等人,2012,2013)对该方法进行测试。这些数据集是在道路行驶场景中,通过安装在乘用车顶部的传感器记录的。如图20所示,车辆配备了彩色立体相机、单色立体相机、一个Velodyne HDL - 64E激光扫描仪,以及一个高精度GPS/INS用于获取真实位姿。激光数据以10Hz的频率记录,并用于该方法的运动估计。为达到可能的最高精度,扫描数据的处理方式与7.4节略有不同。激光雷达建图不再堆叠激光雷达里程计的10次扫描,而是与激光雷达里程计以相同频率运行,处理每个单独的扫描。这导致系统以实时速度的10%运行,处理一次扫描需要1秒。
该数据集包含11个测试,提供了GPS/INS真实位姿。数据集中的最大行驶速度达到85km/h
该数据集包含11个测试,提供了GPS/INS真实位姿。数据集中的最大行驶速度达到85km/h(23.6m/s)。数据主要涵盖三种环境类型:周围有建筑物的“城市”环境、小路上有植被的“乡村”环境,以及道路宽阔且车速较快的“高速公路”环境。图21展示了来自这三种环境的示例结果。第一行展示了与GPS/INS真实轨迹相比,车辆的估计轨迹。中间和底部行分别展示了每个数据集的地图和相应图像,地图按海拔高度进行了颜色编码。11个数据集的完整测试结果列于表5。图21中从左到右的三个测试分别是表5中的数据集0、3和1。这里,精度是通过基于3D坐标,对长度为100m、200m、...、800m的分段轨迹的相对位置误差求平均来计算的。
在KITTI里程计基准测试中,无论传感方式如何,我们的方法最终精度排名第二,平均位置误差相对于行驶距离为0.88%。这一结果优于当前最先进的基于视觉的方法(立体视觉里程计方法)(Persson等人,2015;Badino和Kanade,2011,2013;Lu等人,2013;Bellavia等人,2013),位置误差降低超过14%,方向误差降低24%。实际上,排名第一的方法也部分使用了我们的方法,该方法先通过视觉里程计进行运动估计,再用我们的方法进行运动优化(Zhang和Singh,2015)。我们认为基于激光雷达的状态估计优于基于视觉的方法,这是因为激光雷达能够测量远距离的点。激光雷达的测距误差相对于测量距离来说相对稳定,并且远离车辆的点在扫描匹配过程中有助于保证方向精度。
数据编号 | 配置 | 平均相对位置误差(%) |
---|---|---|
距离(m) | 环境 | |
0 | 3714 | 城市 |
1 | 4268 | 高速公路 |
2 | 5075 | 城市 + 乡村 |
3 | 563 | 乡村 |
4 | 397 | 乡村 |
5 | 2223 | 城市 |
6 | 1239 | 城市 |
7 | 695 | 城市 |
8 | 3225 | 城市 + 乡村 |
9 | 1717 | 城市 + 乡村 |
10 | 919 | 城市 + 乡村 |
误差是基于3D坐标,对长度为100m、200m、...、800m的分段轨迹进行测量,并以分段长度的平均百分比表示。
八、讨论
在构建复杂系统时,一个关键问题是系统中的哪些组件是确保性能的关键。首先,双层数据处理结构至关重要。它将状态估计问题分解为两个更易于解决的子问题。激光雷达里程计专注于传感器速度估计和运动畸变消除,其速度估计虽不精确(见图12a),但足以校正点云的畸变。之后,激光雷达建图仅需考虑刚体变换,以实现精确的扫描匹配。
我们对几何特征检测和匹配的实现,是实现在线实时处理的关键手段。在激光雷达里程计中,匹配不必过于精确,但高频性对于点云校正更为重要,因此该实现将处理速度放在首位。不过,如果有足够的计算资源,如GPU加速,这种实现方式的必要性会降低。
我们可以选择激光雷达里程计和激光雷达建图的频率比。将频率比设为10是我们的选择(激光雷达建图的运行频率比激光雷达里程计低一个数量级),这意味着激光雷达建图每次批量优化会堆叠激光雷达里程计的10次扫描输出。若频率比设置过高,通常会导致更多漂移,且每次激光雷达建图处理完成后,运动估计会出现跳跃,当前频率比能保证运动估计的平滑性。另一方面,频率比设置过低会增加计算量,通常没有必要。该频率比还平衡了两个CPU线程的计算负载,改变频率比会使一个线程的计算负载高于另一个线程。
九、结论与未来工作
利用旋转激光扫描仪获取的点云进行运动估计和建图颇具挑战,因为该问题涉及运动恢复和激光雷达点云的运动畸变校正。本文提出的方法通过并行运行的两个算法来分解并解决该问题,两个算法的协同工作使精确的运动估计和建图得以实时实现。该方法已在大量涵盖各种环境的实验中进行了测试,使用了作者收集的数据以及KITTI里程计基准测试数据集。由于当前方法未考虑回环检测,我们未来的工作将致力于开发一种通过回环来校正运动估计漂移的方法。
致谢
本文基于美国国家科学基金会(National Science Foundation)资助项目IIS - 1328930的研究成果。特别感谢D. Huber、S. Scherer、M. Bergerman、M. Kaess、L. Yoder和S. Maeta,感谢他们富有洞察力的建议和宝贵帮助。
【推荐】编程新体验,更懂你的AI,立即体验豆包MarsCode编程助手
【推荐】凌霞软件回馈社区,博客园 & 1Panel & Halo 联合会员上线
【推荐】抖音旗下AI助手豆包,你的智能百科全书,全免费不限次数
【推荐】博客园社区专享云产品让利特惠,阿里云新客6.5折上折
【推荐】轻量又高性能的 SSH 工具 IShell:AI 加持,快人一步
· 微软正式发布.NET 10 Preview 1:开启下一代开发框架新篇章
· 没有源码,如何修改代码逻辑?
· PowerShell开发游戏 · 打蜜蜂
· 在鹅厂做java开发是什么体验
· WPF到Web的无缝过渡:英雄联盟客户端的OpenSilver迁移实战