RF-LOAM:城市动态环境中稳健快速的激光雷达里程计与建图
李炯,张旭东(IEEE 会员),张宇,常云飞,赵凯
摘要:在城市动态环境中,现有的大多数激光雷达同步定位与建图(SLAM)研究都基于静态场景假设,易受动态障碍物的显著影响。为解决这一问题,本文基于快速激光雷达里程计与建图(F-LOAM),采用FA-RANSAC算法、改进的ScanContext算法以及全局优化,提出了一种稳健且快速的激光雷达里程计与建图方法(RF-LOAM)。首先,运用区域生长算法对扇形网格进行聚类。接着,提出基于特征信息和自适应阈值的FA-RANSAC算法,以去除动态物体,并提取静态边缘和平面特征点进行初次畸变补偿。随后,利用静态特征点计算估计位姿,并用于进行二次畸变补偿。然后,通过高度差和自适应距离阈值提高ScanContext算法的精度,同时删除回环历史匹配帧和简化特征匹配过程,提升其效率。最后,对关键帧进行全局优化。在KITTI数据集、Urbanloco数据集以及我们采集的数据集上进行实验测试,结果表明,与当前先进的SLAM方法相比,我们的方法不仅能准确完成动态物体去除和回环检测,还能在城市动态场景中实现更稳健、快速的定位和建图。
关键词:自动驾驶;全局优化;激光雷达里程计;回环检测;同步定位与建图(SLAM)
一、引言
同步定位与建图(SLAM)一直是自动驾驶领域的关键技术,特别是在GPS信号不佳或处于未知动态场景时,该技术对自动驾驶起着至关重要的作用。根据传感器类型的不同,现有的SLAM方法主要分为基于激光雷达、基于摄像头和基于毫米波雷达三大类。目前,基于激光雷达(二维和三维)和摄像头(单目、双目和RGB-D)的方法最为常见。由于摄像头受光照影响较大,无法全天候使用,因此本文采用测量更精确、更稳健的激光雷达,开展城市动态环境下的SLAM研究。
尽管现有的激光雷达SLAM研究在公开数据集评估中表现良好,但在实际应用中仍存在一些局限性。一是从静态环境转换到动态环境时,方法的稳健性较差。例如,Hess等人的研究在室内或静态户外环境中精度较高,但在城市动态场景中,定位精度会大幅下降。二是难以平衡计算成本和精度。例如,低漂移实时激光雷达里程计与建图(LOAM)精度较高,但畸变补偿和建图的计算成本高昂。此外,虽然快速LOAM(F-LOAM)作为轻量级激光雷达SLAM方法减少了时间消耗,但它缺乏回环检测功能,导致累积误差较大,在城市动态场景中的精度较低。
因此,本文基于F-LOAM提出了一种适用于动态城市场景的稳健快速激光雷达SLAM方法。该方法的主要框架如图1所示。首先,将点云划分为扇形网格,并通过区域生长算法实现物体分割。其次,提出FA-RANSAC算法,基于特征信息和自适应阈值快速去除动态障碍物,获取更精确的初始位姿。然后,提取静态特征点进行位姿估计。之后,利用高度差和自适应距离改进ScanContext算法的回环检测精度,通过简化特征匹配和删除回环历史匹配帧来提升其回环检测效率。最后,通过全局优化快速准确地实现定位和建图。
本文结构如下:第二部分介绍与激光雷达相关的SLAM方法的研究现状;第三部分详细阐述所提方法,包括点云处理、激光雷达里程计、回环检测和全局优化;第四部分给出实验结果并进行详细分析;第五部分为结论。
二、相关工作
激光雷达SLAM的重要环节是连续帧之间的匹配。现有研究中,主要可分为原始点云匹配和特征点对匹配。
对于原始点云匹配,迭代最近点(ICP)算法及其衍生方法是最典型的代表。它们的核心思想是寻找最近点,并通过迭代最小化距离残差来获得最终的估计位姿。尽管这些方法在初始精度较高的静态场景中能够实现良好的定位,但耗时较长。此外,动态点容易干扰静态点对的配准,导致在动态环境中结果陷入局部最优。另一种常见的原始点云匹配方法是正态分布变换(NDT)及其衍生方法。其核心思想是计算每个网格中的局部正态分布,并迭代最小化概率残差以获得最终的估计位姿。虽然这些方法在点云配准中得到了广泛应用,但其精度和时间消耗受网格大小的影响。
对于特征点对匹配,LOAM及其衍生方法是最典型的代表。它们的核心思想是获取特征点,并迭代最小化特征距离以实现状态估计。虽然使用特征点进行迭代计算提高了效率,避免了网格带来的特征损失,但在动态场景中,动态特征点会影响误差函数的构建。动态物体越多,这些方法的性能越差,因此在动态场景中难以实现准确的状态估计。
近年来,也出现了一些基于深度学习的匹配研究。Velas等人将激光雷达点投影到二维平面,采用三个通道对信息进行编码,然后利用卷积层和全连接层进行位姿回归。该方法在仅估计平移时表现较好,但在估计六自由度位姿时性能较差。Li和Wang提出了一种网络DMLO,用于生成高置信度的特征点对,然后利用奇异值分解(SVD)获得估计位姿。虽然该方法可以得到六自由度位姿,但在城市场景中,动态物体容易干扰特征点对,影响定位精度。Wang等人提出了一种网络PWCLO-Net,基于金字塔、变形和成本体积结构来估计和优化位姿。尽管该方法在KITTI数据集上表现出色,但在一些动态场景中稳健性较差,泛化能力较弱。
上述大多数激光雷达SLAM技术都基于静态场景。然而,在现实生活中,我们面临更多的是动态场景。因此,一些学者也对如何去除动态物体、提高动态场景中的定位稳健性展开了研究。他们大多采用检测和跟踪方法来去除动态物体。虽然这些方法能够在动态场景中实现定位,但其实时性难以满足要求。随后,一些学者尝试研究不依赖上述技术的动态物体去除方法。Kim和Kim提出了Removert方法,用于从误删点中保留静态点,但在发生遮挡时,该方法无法正确过滤动态点。Lim等人提出了一种基于区域地面平面拟合的静态地图构建方法ERASOR,它依赖于地面拟合来去除地面上方的动态点,因此在一些斜坡场景中表现不佳。由于Removert和ERASOR是离线实现的,无法在线运行,Yang和Wang提出了一种基于RANSAC的快速激光雷达SLAM算法,虽然该算法可以实现在线配准,但在动态物体较多时表现较差。Wang等人提出了一种多层RANSAC(ML-RANSAC)算法,基于点云分割,利用迭代关联对象匹配实现配准,同时去除动态物体。该方法可用于更复杂的场景和较大的初始误差,但单匹配点的稳健性较差,迭代次数较多。
上述方法在城市动态环境中,在计算成本或定位精度方面都存在一些问题。因此,我们提出了RF-LOAM方法,通过结合FA-RANSAC算法和改进的ScanContext算法,使其高效且适用于城市动态场景。
三、方法
本文所提方法的整个框架由四个模块组成:点云处理、激光雷达里程计、回环检测和全局优化。在本节中,将详细阐述这四个模块。
(一)点云处理
- 点云栅格化:本文采用HDL-64激光雷达进行SLAM研究。由于激光雷达的旋转特性,点云密度会随距离增加而降低。为了更好地实现物体分割,并便于后续回环特征提取,我们采用更符合点云分布的扇形网格对物体进行聚类。在激光雷达周围[2, 40]米的范围内,根据公式(1)和(2)计算索引,构建扇形网格模型,如图2所示。
- 物体分割:在完成扇形栅格化后,为了快速准确地对物体进行聚类,我们采用区域生长算法实现物体分割。具体过程如下:首先,计算每个扇形网格的高度差Δh,通过高度差阈值TΔh(TΔh = 0.3米)和点云数量阈值Tnp(Tnp = 5)选择种子网格;然后,判断当前种子网格周围八个邻域中未遍历的种子网格,将高度最接近当前种子网格最大高度的邻域种子网格添加到同一物体列表中。接着,对列表中未遍历的网格重复进行8邻域搜索,直到物体列表中没有未遍历的网格,完成单个物体的分割;最后,依次遍历所有未遍历的网格,即可完成所有物体的分割。
- FA-RANSAC去除动态物体:现有的大多数方法通过检测和跟踪来判断动态物体,这通常需要较长时间。然而,在SLAM中,我们不需要获取物体的精确速度,只需要知道它是否在移动。因此,本文跳过检测和跟踪过程,提出一种基于特征信息和自适应阈值的FA-RANSAC算法,快速去除动态物体。
- 动态选择特征点:在完成物体分割后,我们根据文献[37]中的方法,通过凸多边形获取物体的矩形框。考虑到在现实场景中物体到激光雷达的距离不断变化,物体的矩形框也会随之改变,导致重心波动。此时,采用单点匹配的ML-RANSAC算法精度较差。因此,本文利用激光雷达的扫描特性,将激光雷达周围区域划分为四个扇区,提出一种更稳健的特征点选择方法,如图3(a)所示。当物体位于激光雷达的前扇区或后扇区时,它会保留相对完整的宽度特征,如图3(a)中的绿色点所示。当物体位于激光雷达的左扇区或右扇区时,它会保留相对完整的长度特征。因此,通过判断物体重心所在的区域,选择稳定的红色边界点作为特征点,与历史帧中的特征点进行匹配,从而提高去除动态物体的稳健性和准确性。即使物体D1从第k - 5帧到第k帧发生偏航角变化,其相对稳定的长度特征也发生变化,如图3(b)所示,在第三节A3c部分的内点计算中,仍可以从特征点中选择相对稳定的特征点对P1_k - 5和P1_k来判断内点。因此,偏航角的变化不会影响动态物体的去除。
- 基于多特征查询对应点:两帧之间的变换通常涉及六个参数。因此,至少需要两组对应点才能获得相应的变换矩阵。为了通过少量迭代获得去除动态物体的变换矩阵,我们将当前帧m与第m - 5帧进行比较,提出一种基于多特征的快速准确的对应点搜索方法。
- 自适应过滤内点并设置终止条件:由于激光雷达的扫描频率较快,相邻帧之间的点云相似度较高。因此,可以利用前一帧的平均距离和最优内点率来指导当前帧的匹配。在判断内点和外点时,需要分两种情况计算当前距离。如果前一时刻FA-RANSAC算法判断前一帧中的物体在移动,当前距离指的是每次迭代变换后两对特征点之间的最大距离;否则,当前距离指的是两对特征点之间的最小距离。内点率是指距离小于距离阈值的物体对数量与所有物体对数量的比值。具体过程如下。
- FA-RANSAC去除动态物体:基于上述内容,提出FA-RANSAC算法。算法框架如算法1所示,具体过程如下:首先,使用第三节A3a部分中的动态选择特征点方法,获取当前帧m和第m - 5帧中物体的多个稳定特征点;其次,从当前帧中随机选择两个距离合适的物体,然后根据第三节A3b部分中的对应点查询方法,获取当前帧中选定的两个物体与第m - 5帧中物体之间的所有关系对集合;接着,通过每个对应关系下选定物体的特征点匹配计算变换矩阵,然后基于变换后物体中的两对特征点计算当前距离,并使用第三节A3c部分中获得的自适应距离阈值过滤内点,得到每个变换矩阵对应的内点率。考虑到一帧中静态物体的数量通常多于动态物体,最终选择内点率最高的变换矩阵作为本次采样的RANSAC配准结果;最后,重复采样和配准步骤,直到内点率高于第三节A3c部分中获得的自适应内点率阈值,或者采样次数达到固定值。此时,保留内点率最高的变换矩阵,相应的外点将被直接作为动态物体去除。
- 动态选择特征点:在完成物体分割后,我们根据文献[37]中的方法,通过凸多边形获取物体的矩形框。考虑到在现实场景中物体到激光雷达的距离不断变化,物体的矩形框也会随之改变,导致重心波动。此时,采用单点匹配的ML-RANSAC算法精度较差。因此,本文利用激光雷达的扫描特性,将激光雷达周围区域划分为四个扇区,提出一种更稳健的特征点选择方法,如图3(a)所示。当物体位于激光雷达的前扇区或后扇区时,它会保留相对完整的宽度特征,如图3(a)中的绿色点所示。当物体位于激光雷达的左扇区或右扇区时,它会保留相对完整的长度特征。因此,通过判断物体重心所在的区域,选择稳定的红色边界点作为特征点,与历史帧中的特征点进行匹配,从而提高去除动态物体的稳健性和准确性。即使物体D1从第k - 5帧到第k帧发生偏航角变化,其相对稳定的长度特征也发生变化,如图3(b)所示,在第三节A3c部分的内点计算中,仍可以从特征点中选择相对稳定的特征点对P1_k - 5和P1_k来判断内点。因此,偏航角的变化不会影响动态物体的去除。
- 特征提取:在去除动态物体后,每帧仍存在大量点云。如果直接处理这些点云,会花费较长时间。因此,为了提高匹配效率,本文根据文献[16]中的方法,基于局部点的平滑度提取平面特征集SL和边缘特征集EL。然后,将FA-RANSAC算法获得的最优变换矩阵作为初始变换矩阵,根据文献[16]中的方法,将SL和EL从当前激光雷达坐标系变换到全局坐标系,进行初步的畸变补偿。
(二)激光雷达里程计
- 位姿估计:位姿估计的核心思想是将当前未畸变的平面特征集SW和边缘特征集EW与第三节B2部分中的全局关键帧特征子图进行匹配。对于SW中的每个平面特征点pS和EW中的边缘特征点pE,首先通过KD树从全局平面特征子图中选择最接近pS的平面特征点。同样,也选择最接近pE的边缘特征点。然后,从边缘和平面特征子图中收集附近的点来估计直线和平面。接着,计算点到边缘和点到平面的加权距离。之后,使用高斯 - 牛顿法最小化当前未畸变特征的加权距离和,通过迭代优化获得当前估计位姿,直到收敛。上述步骤的详细信息,请参考文献[16]。
- 关键帧子图更新:首先,计算位姿估计得到的当前帧位姿与前一关键帧位姿之间的平移和旋转。如果平移和旋转小于各自的阈值,则当前帧不被视为关键帧,直接跳过;否则,当前帧被视为关键帧。根据文献[16]中的方法,对当前平面特征和边缘特征进行畸变补偿。然后,重新计算的未畸变边缘特征和平面特征将分别更新到全局关键帧边缘子图和平面子图中,并同时传入全局优化模块构建位姿图。
(三)回环检测
ScanContext算法可以实现回环检测,但存在许多问题,比如难以区分颠簸路面、对远处相似场景容易产生误回环检测,以及候选帧搜索和匹配耗时较长。基于上述问题,我们对ScanContext算法进行了改进,以便在去除动态物体后实现更快、更精确的回环检测。具体来说,通过使用高度差来提高场景识别的鲁棒性,采用自适应距离阈值来过滤掉无效的相似候选帧。最后,通过优化距离计算和删除回环历史匹配帧来提高候选帧匹配和搜索的效率。
- 创建描述符:考虑到城市道路上的减速带和斜坡,以及激光雷达安装时的小倾角,仅依靠最大高度很难判断是否为相同的颠簸路面。因此,在去除动态物体后,我们选择每个网格中的高度差Δh作为ScanContext算法特征矩阵中的值。为了尽可能避免对扇形网格的重复划分,并快速获取当前关键帧的描述符,我们将在第三节A2中用于分割的480×95个网格与在第三节A3中标记为动态物体的网格相结合,快速构建一个新的\(N_s×N_r\)特征提取网格模型。在本文中,\(N_s\)为60,\(N_r\)为19,只需在射线方向上包围5个原始红色扇形网格,在圆周方向上包围8个原始红色扇形网格,形成一个新的黑色扇形网格,如图5所示。然后,依次比较新黑色扇形网格中原始非动态物体网格的高度差,如公式(10)所示。接下来,将新扇形网格的最大高度差作为其特征值。最后,通过对所有新扇形网格进行迭代,得到当前关键帧的特征矩阵作为描述符。
- 获取候选帧:在得到当前帧的特征矩阵后,传统的ScanContext算法采用固定距离阈值来确定候选帧,这很容易导致对远处相似场景的误回环检测。因此,我们引入自适应距离阈值来精确选择候选帧。具体来说,我们首先根据文献[39]中的方法,计算特征矩阵每一行向量的范数,以获得旋转不变的描述向量。然后,使用文献[39]中定义的向量偏差,找出与当前帧相隔超过十帧的历史关键帧中,偏差最小的\(N_m\)(\(N_m\) = 10)个初始候选帧。接下来,由于每次回环优化都会大幅降低累积误差,我们设计了一个与累积误差同步变化的距离阈值\(d_T\),以进一步过滤掉误回环检测,如公式(11)和(12)所示。最后,精确地获得当前帧的候选帧。
- 候选帧的快速回环检测:在获得候选帧后,为了最终确认车辆是否进入回环,传统的ScanContext算法将特征矩阵视为列向量的组合,如图6(a)所示,并通过平移特征矩阵来判断相似度。这种方法每次平移都需要计算60次列向量的余弦值,非常耗时。因此,为了降低特征矩阵相似度计算的复杂度,我们改进了文献[39]中平移列向量的方法,使用列向量范数来简化特征矩阵,以便进行平移相似度判断。具体步骤如下:首先,计算当前帧和候选帧的列向量范数,并将它们的特征矩阵简化为单个特征向量,如图6(b)所示。然后,根据文献[39]中的方法,不断平移所选候选帧的特征向量k,并计算它与当前帧特征向量的距离。其中,每次平移只计算一次向量余弦值,就可以快速准确地得到特征矩阵的距离。接下来,从所有平移中选择最小距离值作为k与当前帧特征向量的距离。最后,按照这种平移方法,依次计算所有候选帧。从这些距离中选择最小距离值与距离阈值进行比较,完成回环检测。
- 优化候选帧搜索:为了避免历史数据的积累导致搜索候选帧的时间增加,本文进一步改进了文献[39]中的方法。在当前帧进入回环后,不断清除历史回环帧。我们的方法可以确保随着点云数量的不断增加,历史帧的数量保持稳定,提高候选帧的搜索速度。
(四)全局优化
在当前关键帧完成回环检测后,我们使用SLAM中常用的全局优化算法进一步消除累积误差,具体包括位姿图构建和位姿图优化。
- 位姿图构建:该模块首先从激光雷达里程计接收当前关键帧在全局坐标系中的位姿。然后,将该位姿作为节点添加到位姿图中,完成位姿图的构建,当前关键帧的特征集可以视为该节点的测量值。
- 位姿图优化:如果在构建位姿图后,当前关键帧没有进入回环,则直接跳过以下步骤。否则,根据位姿图中相邻帧的特征集构建回环帧的特征子图。然后,根据第三节B1中的方法计算当前帧与回环帧之间的变换。接下来,将该变换作为当前位姿节点和回环位姿节点对应的边添加到位姿图中。此时,由于回环得到的当前位姿与里程计得到的当前位姿存在偏差,使用文献[24]中的L-M算法对位姿图的全局位姿进行优化。最后,优化后的位姿更新里程计中的子图位姿和回环历史帧的位姿。
四、实验结果与分析
(一)实验设置
- 数据集:为了验证所提方法的有效性,我们在KITTI数据集、Urbanloco数据集以及我们自己收集的城市动态数据集上对RF-LOAM进行评估。KITTI数据集由HDL-64激光雷达、一套RTK-GPS/IMU设备和一个双目摄像头记录。其轨迹足够长,可以测试户外定位方法。Urbanloco数据集由HDL-32E激光雷达、一套GPS/IMU设备和六个360°全景摄像头记录。它包含大量动态物体。为了测试我们的方法在城市场景中的定位和回环检测效果,我们从KITTI数据集中选择了回环重复率最高的三个序列城市数据:00、05和08(08仅包含反向回环,00和05仅包含同向回环事件),并从Urbanloco数据集中选择了两个动态物体较多的序列城市数据:CA_20190828155828(CA-1)和CA_20190828190411(CA-2)进行实验。提取的城市数据集由无人地面车辆(UGV)记录。HDL-64激光雷达安装在UGV顶部,如图7(a)所示。这个在动态城市环境中获得的数据集包含大量行人和车辆,长度约为5.2公里,如图7(b)所示。从图7(b)的局部典型图片可以看出,该数据集包括交通拥堵的道路、十字路口、回环道路、颠簸严重的道路和相似的道路,以便更好地验证算法。
- 实现细节:所有测试均基于机器人操作系统(ROS)用C++实现,并在一台配备i7-8700 CPU、16GB内存和Ubuntu 18.04的计算机上运行。然后,RF-LOAM根据文献[42]和实际经验,使用表I中所示的详细常数参数。
(二)性能评估
- KITTI数据集:我们的方法在指定的KITTI数据集上构建的点云地图如图8(a - 1)-(c - 1)所示。可以看出,我们的方法能够准确构建点云地图。然后,从KITTI数据集中选择典型的局部动态场景进行分析,如图8(a - 3)-(c - 3)所示。从图8(a - 2)到(c - 2)可以看出,我们的方法能够成功去除这些动态物体,实现准确的动态场景构建。为了进一步说明我们方法的优势,将RF-LOAM与当前最先进的SLAM方法进行比较,如优化后的SC-F-LOAM、LeGO-LOAM、F-LOAM、A-LOAM、PWCLO-Net和ML-RANSAC。此外,为了展示扫描匹配前去除动态物体的好处,我们将我们方法中的点云处理和里程计部分提取出来作为Our-Front,在第四节C中进行进一步比较。使用平均平移误差(ATE)和平均旋转误差(ARE)作为评估指标,这些指标由KITTI数据集定义。结果如表II所示,典型轨迹的比较如图8(d)-(f)所示。可以看出,我们的方法优于其他比较方法,在这些KITTI数据集下,我们方法的轨迹最接近真实轨迹,误差最小。由于A-LOAM和F-LOAM缺乏回环检测,并且受到动态物体的干扰,平均误差相对较大,如表II所示。在本文中,我们采用FA-RANSAC算法快速去除动态物体,因此Our-Front的定位精度高于A-LOAM和F-LOAM。此外,由于Our-Front使用多特征点匹配,也比采用单点匹配的ML-RANSAC表现更好。然而,PWCLO-Net不能依赖几何对应关系,而是通过大规模训练实现了比Our-Front更好的激光雷达里程计性能。LeGO-LOAM和优化后的SC-F-LOAM具有回环检测功能,因此它们的累积误差也比Our-Front小。但是,我们使用改进的扫描上下文在动态场景中进行快速准确的回环检测。因此,经过全局优化后,我们的方法比其他比较方法更准确。在三个序列上,它可以实现0.69%的ATE和0.27°/100m的ARE。
- Urbanloco数据集:为了更好地评估我们的方法在城市动态环境中的SLAM性能,我们在Urbanloco数据集上实现了RF-LOAM和当前最先进的SLAM方法。从图9(a)和(b)的右侧可以看出,Urbanloco数据集中的动态物体明显多于KITTI数据集。然而,如图9(a)和(b)的左侧和中间所示,我们的方法仍然能够去除这些动态物体,并构建准确的动态点云地图。然后,使用平均平移误差(MTE)和平均旋转误差(MRE)作为评估指标,这些指标由Urbanloco数据集定义。结果和典型轨迹的比较分别如表III和图9(c)和(d)所示。可以看出,由于Urbanloco数据集的动态场景比KITTI数据集更复杂,误差显著增加。然而,我们的方法仍然可以实现最佳的SLAM性能。结果与在KITTI数据集上的结果一致,但我们的方法具有更明显的优势。与LeGO-LOAM(使用ICP)和优化后的SC-F-LOAM相比,我们的方法分别将轨迹精度提高了31.2%和24.8%。在去除动态物体后,与A-LOAM、F-LOAM和ML-RANSAC相比,Our-Front在复杂动态场景中的优势也更加明显,如表III所示。此外,HDL-32E激光雷达收集的点云密度相对稀疏,Urbanloco数据集中的场景更复杂。因此,使用这些点云训练PWCLO-Net后,轨迹误差相对较大,其精度不如Our-Front。
- 提取的城市数据集:为了进一步证明我们的方法在实际动态环境中的优越性,我们选择提取的城市数据集进行测试,该数据集是在有许多动态物体的城市化场景中收集的。城市数据集的一些典型图片如图7(b)所示,我们方法的映射结果如图10所示。可以看出,我们的方法能够准确构建点云地图。然后,使用ATE和ARE作为评估指标。比较方法的结果如表IV所示,典型轨迹的比较如图11所示。由于城市环境比KITTI更复杂,我们的方法具有更明显的优势,结果与在Urbanloco数据集上的结果相似。我们的方法采用FA-RANSAC算法去除动态物体,因此Our-Front比A-LOAM和F-LOAM具有更高的精度,如表IV所示。考虑到在KITTI数据集上训练的PWCLO-Net在我们提取的城市数据集中泛化能力不足,并且ML-RANSAC在复杂场景中使用单点匹配的鲁棒性有限,Our-Front的性能也优于PWCLO-Net和ML-RANSAC。LeGO-LOAM和优化后的SC-F-LOAM具有回环检测功能,有助于减少累积误差,因此它们的轨迹误差比Our-Front小。然而,它们受到动态物体和颠簸道路的干扰,导致回环检测不稳定,轨迹误差波动较大。我们改进了ScanContext算法的特征提取和自适应距离阈值,以准确检测颠簸道路和相似道路上的回环。虽然我们的轨迹与真实轨迹之间仍然存在误差,但我们的方法获得的轨迹最接近真实轨迹,与其他方法相比,我们的方法具有最佳的SLAM性能。与优化后的SC-F-LOAM相比,我们的方法分别将ATE和ARE降低了48%和33%,如表IV所示。然后,为了进一步阐明动态物体对前端里程计的影响,我们将Our-Front与其他里程计方法(ML-RANSAC、A-LOAM和F-LOAM)进行比较,并从表II至表IV中获取相应的轨迹误差进行分析。可以看出,在去除动态物体后,Our-Front比其他里程计方法具有更好的定位精度。例如,从我们提取的数据集中选择交通拥堵场景1和十字路口场景2进行详细说明。如图12(a)和13(a)所示,这些场景中有大量的动态车辆和行人。由于A-LOAM和F-LOAM受到动态物体的干扰,在局部地图中出现了大量的鬼轨迹,如图12(d)和(e)以及13(d)和(e)所示,并且定位也存在较大偏差,如表IV所示。具体来说,从图12(d)和(e)中的区域A和B可以看出,边界重叠效果较差。虽然ML-RANSAC通过去除动态物体减少了鬼轨迹,但在复杂动态场景中,单点匹配容易导致动态物体遗漏。Our-Front使用更强大的FA-RANSAC算法去除动态物体,以获得准确的静态地图,如图12(b)和13(b)所示。因此,Our-Front的轨迹误差相对较小,并且从图12(b)中的区域A和B可以看出,其边界重叠效果更好。总之,Our-Front可以通过去除动态物体提高动态环境中的SLAM性能。
(三)消融研究
为了分析每个模块的有效性,我们通过去除或更改我们方法的组件进行消融研究。
- 动态物体去除的影响:在这项研究中,首先使用文献[43]中提出的去除率来评估动态物体去除的有效性。我们将Our-Front与当前最先进的方法,即Removert、ERASOR和ML-RANSAC进行比较,并采用包含许多动态物体的Urbanloco数据集和提取的城市数据集进行测试。结果如表V所示。由于可见性问题,Removert在这些动态场景中的去除率急剧下降。ML-RANSAC采用单点匹配,容易导致动态物体遗漏。Our-Front使用多点匹配快速去除动态物体,并且可以实现与不受可见性无效范围内点限制的ERASOR相当的性能。请注意,Removert和ERASOR是离线方法,需要预先构建地图。然而,Our-Front可以在线去除动态点。
- 改进的ScanContext的影响:在这项研究中,首先评估特征矩阵相似度的有效性。为此,将改进的ScanContext方法称为IMSC,我们从IMSC中去除列范数简化步骤,得到RM-IMSC。然后,我们统计RM-IMSC和IMSC获得的回环帧数,并计算相对回环率。结果如表VI所示。可以看出,在简化特征矩阵后,我们可以实现与RM-IMSC相当的回环检测性能。因此,在计算特征矩阵的相似度得分时,使用列范数代替列向量,这种简化方法不会导致列向量信息的丢失。为了进一步阐明我们改进的ScanContext方法的整体性能,我们将Our-Front与优化后的SC结合,得到OF-OPSC,并在Our-Front中添加ICP,得到OF-ICP(Our-Front仅提供位姿,优化后的SC和ICP使用原始点云进行回环检测和映射)。接下来,在我们提取的数据集上对我们的方法与这些方法进行测试,轨迹误差如表VII所示。可以看出,我们改进的ScanContext在动态场景中优于优化后的SC和ICP。然后,从我们提取的数据集中选择典型场景进行详细分析,如回环道路场景3、颠簸道路场景4和相似道路场景5。首先,对于回环道路场景3,车辆离开和返回场景3时的点云如图14所示。静态点用红色表示,我们的方法检测到的动态点用紫色表示。由于OF-OPSC和OF-ICP使用原始点云进行回环检测,在局部地图中存在许多鬼轨迹,如图15(b)和(c)所示。接下来,由于受动态物体影响,离开和返回帧的回环特征差异显著,OF-OPSC难以检测到回环。因此,如表VII所示,轨迹误差相对较大,并且如图15(b)中的区域A和B所示,局部地图重叠效果较差。然后,由于OF-ICP基于半径进行回环检测,当里程计误差较大时,难以准确检测回环。因此,如表VII所示,轨迹误差非常大,并且离开和返回帧无法很好地匹配,如图15(c)所示。我们的方法使用FA-RANSAC去除动态物体,因此如图15(a)所示,局部地图中只有静态点,离开和返回帧的回环特征相似,可以准确实现回环检测。因此,经过全局优化后,我们的方法得到的轨迹误差在表VII中最小,并且如图15(a)中的区域A和B所示,局部地图重叠效果更好。之后,对于颠簸道路场景4,道路上有一个相对陡峭的斜坡和减速带。当车辆快速转弯并经过场景4时,激光雷达坐标系的位姿变化会导致同一场景中点云的高度相应变化。此时,OF-OPSC使用最大高度作为特征,难以准确检测回环。此外,由于行人的干扰,轨迹误差相对较大,如表VII所示,并且如图16(b)中的区域A - C所示,局部地图重叠效果较差。然而,我们的方法通过使用高度差作为特征改进了ScanContext算法,解决了颠簸道路引起的高度变化问题,能够准确检测回环。因此,我们的方法的轨迹误差可以通过全局优化得到很好的修正,如表VII所示,并且局部地图重叠效果更好,如图16(a)中的A - C区域所示。
最后,对于场景5中的相似道路,存在两帧距离较远但场景结构相似的点云,如图17中的红色和白色点所示。因此,这两帧点云的特征矩阵相似。此时,OF - OPSC无法使用随关键帧数增加而增大的自适应距离阈值过滤掉远处的误回环帧。考虑到这两帧场景相似且只有一小部分点云重合,很难为全局优化获得准确的回环约束。因此,如表VII所示,轨迹误差较大,并且如图18(b)中的A和B区域所示,局部地图重叠效果较差。我们的方法在每次全局优化后从最小值自适应增大距离阈值,使距离阈值与轨迹误差同步变化。因此,我们的方法可以准确消除远处相似场景的误回环检测,避免全局优化错误。因此,如表VII所示,轨迹误差较小,并且如图18(a)中的A和B区域所示,局部地图重叠效果更好。
(四)运行时间
在实际应用中,实时性能是评估SLAM系统的关键指标。我们在所有数据集上测试了不同方法的运行时间,结果如表VIII所示。可以看出,Our - Front在这些没有回环检测的里程计方法中是最快的,我们的方法在这些有回环检测的SLAM方法中也是最快的。Our - Front可以通过FA - RANSAC提供的初始位姿快速获得里程计位姿。因此,Our - Front的运行时间比一次性匹配的F - LOAM、端到端的PWCLO - Net和轻量级的ML - RANSAC都要少。A - LOAM花费的时间较长,因为其建图的迭代次数较多。LeGO - LOAM和优化后的SC - F - LOAM比A - LOAM花费的时间更多,因为它们都有回环检测。在我们的方法中,当使用扇形网格的高度差进行物体分割时,这些网格也用于回环特征提取,这可以减少运行时间。然后,通过删除历史匹配帧提高了搜索效率,通过改进特征匹配简化了匹配计算。因此,尽管我们的方法增加了点云处理的分割和配准步骤,增加了运行时间,但与优化后的SC - F - LOAM相比,我们的方法仍将运行时间减少了12%,如表VIII所示。
五、结论
本文在F - LOAM的基础上,通过添加轻量级分割、配准和改进的ScanContext算法,提出了RF - LOAM激光雷达SLAM算法。我们方法的创新点主要体现在以下三个方面:
- 跳过传统的物体检测和跟踪过程,基于物体分割提出了一种新的FA - RANSAC算法,能够快速准确地将动态物体作为异常值去除。
- 基于ScanContext算法,利用高度差和自适应距离阈值提高回环检测精度,通过删除回环历史匹配帧和简化特征匹配提高回环检测效率。
- 以最小的成本将FA - RANSAC和改进的ScanContext算法统一到F - LOAM框架中,在城市动态环境中快速准确地实现激光雷达SLAM。
在KITTI数据集、Urbanloco数据集和提取的城市数据集上对我们的方法进行了验证。实验结果表明,我们的方法可以有效克服动态物体的干扰,准确实现回环检测。在所有数据集中,我们方法的平均轨迹误差都小于其他对比算法。考虑到Urbanloco数据集和提取的城市数据集中有更多的动态物体,我们的方法在这些数据集中具有更明显的优势和更强的鲁棒性。在运行时间方面,我们的方法改进了ScanContext算法的候选帧搜索和特征匹配,使其在这些有回环检测的SLAM方法中运行时间最短,满足实时性要求。因此,我们的方法可以为无人地面车辆提供稳健且快速的SLAM,在当前最先进的算法中具有竞争力。
然而,该方法仍然存在一些缺点。例如,当无人地面车辆行驶到特征单一的场景时,里程计和回环检测的精度较差。接下来,我们将尝试引入点云强度或融合图像的RGB信息来构建地图,以获得更好的SLAM性能。
【推荐】编程新体验,更懂你的AI,立即体验豆包MarsCode编程助手
【推荐】凌霞软件回馈社区,博客园 & 1Panel & Halo 联合会员上线
【推荐】抖音旗下AI助手豆包,你的智能百科全书,全免费不限次数
【推荐】博客园社区专享云产品让利特惠,阿里云新客6.5折上折
【推荐】轻量又高性能的 SSH 工具 IShell:AI 加持,快人一步
· 微软正式发布.NET 10 Preview 1:开启下一代开发框架新篇章
· 没有源码,如何修改代码逻辑?
· PowerShell开发游戏 · 打蜜蜂
· 在鹅厂做java开发是什么体验
· WPF到Web的无缝过渡:英雄联盟客户端的OpenSilver迁移实战