PointLocalization: Real-Time,Environmentally-Robust3D LiDAR Localization

港科大2019论文

Abstract

         定位或位置确定是机器人技术研究中的一个重要问题。在本文中，我们提出了一种利用三维激光雷达在不断变化的环境中进行长期定位的新方法。我们首先使用GPS和激光雷达创建一个真实环境的地图。然后，我们将地图划分为几个小部分作为点云配准的目标，这不仅提高了鲁棒性，而且还减少了配准时间。点定位技术使我们能够融合不同类型的里程计，这可以优化定位结果的准确性和频率。我们使用激光雷达和车轮编码器在无人地面车辆（UGV）上评估了我们的算法，并获得了融合后20 Hz以上的定位结果。该算法还可以在180度视场（FOV）中定位UGV。利用六个月前捕获的过时地图，该算法显示出了很强的鲁棒性，测试结果表明，该算法可以达到10cm的精度。点定位已经在一个拥挤的工厂进行了6个多月的测试，并在2000多公里的距离内成功运行。

I. INTRODUCTION  

         自2009年谷歌首次推出其自动驾驶汽车项目以来，自动驾驶汽车的部署一直在迅速增加。机器人技术研究的一个关键方面是定位，特别是在自动驾驶和无人地面车辆（UGVs）[3]的应用中，车辆在特殊领域运行。车辆的定位很重要，因此路径规划模块可以向其发送导航命令。定位是导航的基础。

        根据Paul [4]的说法，导航模块包含两部分：位置固定，包括比较当前位置的特征，如地标、路点和地图；航迹推算，即测量位置或速度的变化，如视觉里程计法和车轮里程计法。

        ugv沿着某条路线行驶，比如行李投递，并在工厂和采矿地点导航。在这些情况下，我们可以使用地图来指导车辆沿途行驶。许多不同类型的传感器可以用于定位[5]。当定位固定用于导航时，全球定位系统（GPS）是最常见的传感器之一，它可以定位世界上几乎任何位置。然而，它高度依赖卫星，这意味着它在信号可能被屏蔽的城市里工作得不好。为了克服这一不足，GPS可以与一个惯性导航系统（INS）通过使用卡尔曼滤波器。这提高了准确性和鲁棒性。但是，该系统的成本很高，当GPS信号丢失时，它只能运行一分钟左右。激光雷达或照相机的定位或定位固定需要一个固定的地图或地标。同步定位和建图（SLAM）可以用于获得固定的建图，它在许多场景中有广泛的应用，如清扫机器人。定向快速和旋转简报式（ORB-SLAM）[6]是一种基于相机的SLAM框架，在姿态估计方面具有很强的鲁棒性。它还提供了一个定位的函数。然而，ORB的特征在光的变化期间发生变化，这种变化可能会导致失败，例如，建图发生在早上，而位置发生在晚上。当我们使用相机导航一个固定的地图时，很难用其他的位置固定系统，如GPS来取代ORB-SLAM。该方法在视觉里程计方面具有较高的精度，定位功能在工业领域也有很大的应用潜力。

        激光雷达最近已经成为自动驾驶的基本传感器，因为它通过照亮目标来测量到目标的距离。它不像照相机那样，并不依赖于光线。Zhang [7]提出了一种名为激光雷达测程和建图（LOAM）的方法，该方法利用激光雷达得到点云图，并利用角点和平面点的距离来优化LM方法的目标。LOAM显示了实时建图和最先进的精度。但是，这种方法不能消除累积误差，我们无法使用地图进行定位。Thrun [8]使用激光雷达的反射率来构建一个地图，该方法显示了大约10厘米的精度。然而，这种方法仅在地面上有足够数量的地标，且受到雨雪的影响时才有效。

        为了解决上述挑战，我们建立了一个点云图来消除光照变化，并使用GPS来约束我们的地图。这一步减少了在长距离上的漂移。为了使定位更加鲁棒性，我们使用其他类型里程计来预测地图上的对应点，并且里程计法也实现了比激光雷达更快的更新速度。

        本文的其余部分的结构如下。第二节说明了相关的工作，并将其他方法与我们的方法联系起来。第三节介绍了传感器和平台，我们介绍使用，以及一种基于图优化的建图方法。第四节介绍了点定位的方法。第五节讨论了在KITTI数据集上的结果和真实环境中的结果。第六节说明了实验结果，并概述了未来的工作。

II. R ELATED W ORK

         在本节中，我们将概述现有的SLAM方法，并将点定位与其他定位方法进行比较。如前所述，ORB-SLAM [6]提供了一种定位方法，使用ORB特征[9]来计算视觉里程，但它对环境中亮度和特征的变化很敏感。为了解决这个问题，[10]首先使用激光雷达基于激光雷达地图几何信息构建了之前的地图，并提出了一个混合bundle光束调整框架来修正当前的姿态。沃尔科特和尤斯蒂斯[11]也使用激光雷达地图作为环境的先验知识；然而，它使用激光雷达生成的地面地图来匹配相机的结果。

        [12] [13]提出了另一种使用相机定位的方法，提出了一种使用无监督算法的在线地标选择方法，根据这些地标的可能性对分布式长期视觉定位系统进行选择。他们还提出了一种长期终身定位的地图管理系统。对于自动驾驶来说，定位仍然是工业应用中的一个具有挑战性的问题，因为它高度依赖于相机的特性，如动态范围和光敏性等，这可能会影响视觉定位在不同条件下的鲁棒性。   

        Koide等人[14]使用图形SLAM来融合激光雷达和GPS信息。他们使用正态分布变换（NDT）[15]算法来扫描匹配，并使用GPS作为姿态图的约束。他们还将NDT配准算法与激光雷达提供的角速度数据相结合，使用无迹卡尔曼滤波器（UKF）进行定位。

        Pfrunder和Egger [16]，[17]提出了一种新的SLAM方法，称为CSIRO SLAM（C-SLAM），它使用了IMU和LiDAR。它通过激光雷达的空间关系计算曲面，并将面匹配的变换误差和IMU的加速度和旋转数据最小化，从而得到当前姿态。他们还提供了一种定位方法，称为CSIRO定位（C-LOC），它也使用surfel匹配，并融合来自IMU的数据来获得当前的定位结果。

        定位的一个问题是如何获得一个高质量的预建地图。许多研究者使用环闭合方法来减少地图生成过程中的漂移。LEGO-LOAM[18]利用轨迹估计环路闭合，然后计算当前激光雷达帧和关键帧之间的均方根误差，这是过去获得的环路闭合条件。ORB-SLAM还使用闭环来减少累积错误。它检测单词袋（BOW）[19]，它描述了在某一帧中的对象类型作为特征向量，以及是否有轨迹的相机到达同一个地方，将显示相同的矢量值。在使用激光雷达进行定位时出现的另一个问题是实时性能。例如，一个64光束的激光雷达每秒将产生220万个点，这使得很难实时计算结果。

        本文提出了一种基于GPS的无漂移地图构建方法。在建图部分，我们使用LOAM作为传感器轨迹的估计，并建立了一个姿态地图来减少累积误差。在提出的建图部分，我们添加了一个GPS约束和环闭包约束，得到了一个无漂移的地图，该基于GPS的建图方法算法可以实现实时性能。

III. EXPERIMENTAL SET-UP AND G-LOAM

A. Sensor Settings  

 1)工业个人电脑（IPC）：我们的IPC配备了英特尔酷睿i7 7700 CPU（3.6 GHz）和16 GB内存。IPC可以在Ubuntu 16.04上使用机器人操作系统（ROS）的Kinetic版本进行工作。

2)在我们的实验中使用了三维激光雷达： RS-LiDAR-16激光雷达。RS-LiDAR-16是一种机械激光雷达，具有30°垂直角场和360°水平角场。RSLiDAR-16有16个不同的激光通道，每个通道以10 Hz的旋转速率工作。

3)轮式编码器：在我们的测试车辆上，我们使用霍尔效应传感器作为我们的车轮速度传感器，用来测量磁场的大小。我们建立了一个36T的齿轮，可以旋转车轮。当车辆向前移动时，霍尔效应传感器将根据旋转齿轮产生的磁场变化产生一个电信号。我们通过计算来自霍尔效应传感器的脉冲量来计算行程距离。我们还根据左右车轮产生的不同电信号得到了偏航角。轮式里程计

B. G-LOAM

         在本节中，我们将介绍带有GPS约束的基于图的SLAM方法，这称为G-LOAM。首先，在使用激光雷达进行建图时，必须使用不同的点云配准方法作为前端。通过这一步，我们就可以得到激光雷达的测程法。研究人员通常使用迭代最近点（ICP）[20]或NDT作为前端配准方法。由于NDT不需要准确的初始位置，且具有稳定的时间消耗，因此被广泛使用。提出了一种基于LOAM的配准部分的建图方法。

        LOAM更准确，也更不耗时，比其他配准算法，因为它只匹配特征，而不是整个激光雷达扫描。此外，通常采用环闭包法对地图进行优化，并采用ICP作为环闭包约束。我们在环路闭合约束中添加了一个GPS约束，以消除LOAM的累积误差。图优化的一般框架（G2o）[21]是解决我们的问题的基本框架。

        基于图的方法[22]是解决SLAM问题最成功的方法之一。在图中，我们使用该边来表示不同的约束条件，如GPS对激光雷达姿态的测量。该图的目标是最小化所有测量值的误差。设pk为激光雷达节点，其中Zk Hk分别表示pk约束的平均值和pk当前约束的信息矩阵。我们也可以定义pk和观测值之间的误差函数ek（pk，Zk），用边来表示它。我们可以使用一个等式2来描述这个优化问题：

         图1显示了我们的图的结构，我们将Gk设置为由GPS数据生成的固定节点，Pk表示SE (3)中的激光雷达姿态。该图有3个约束条件：激光雷达测程法产生的里程计约束、激光雷达与GPS天线之间的静态变换给出的GPS约束、ICP算法给出的闭环检测约束。图2显示了使用该算法得到的地图。

 

IV. POINTLOCALZATION

 A. PointLocalization Overview

         点定位算法需要两个传感器输入。第一个是激光雷达，它可以给我们当前周围的环境。有了这些信息，我们就可以减少累积误差。另一种传感器可以提供任何类型的里程计。我们使用霍尔效应传感器来提供里程计，即轮式里程计；然而，它可以使用任何其他有更新的里程计速率高于10 Hz。传感器应保持稳定，不受环境变化的影响。

B. PointLocalization Algorithm

         在引入点定位之前，我们应该完成由G-LOAM生成的点云的稀疏化。我们使用一个体素网格过滤器来减少我们的地图的大小和重叠区域。经过稀疏化(降采样)步骤，我们可以得到一个访问速度更快、更小存储大小的地图；对于0.5公里×0.5公里的地图，存储大小只有8 MB。在我们收到来自激光雷达和里程计的信息后，我们计算最后一个激光雷达定位结果TkL−1与当前接收到的里程计Tk O之间的转换。我们将其命名为TkI。如果我们接收到一个激光雷达输入，TkI将是我们当前姿态Tk L的一个预测。我们也使用另一种方法来预测我们的姿态，这看作是我们的激光雷达动量（Tk L−1）T×Tk L−2。在这一步中，我们可以得到一个关于TkL的粗略预测，即Tk I和（Tk L−1）T×Tk L−2。然后，我们使用（Tk L）的粗略预测来得到我们之前获得的地图上的感兴趣区域（ROI）。我们在地图上的最小ROI半径设置为1米，这一步可以帮助我们提高激光雷达扫描点与地图匹配的速度。

        在最后一步中，我们得到目标点云（ROI映射）和源点云（LiDAR扫描）。我们可以用ICP来计算激光雷达的姿态，并表示这个问题如下：

我们定义了等式的误差3；ei是激光雷达点（pi）和地图中的ROI点（p‘i）之间的误差，R是当前的旋转，t是当前的变换。在这一步中，我们设置了一个阈值δ，以防止由动态物体造成的影响，如卡车或行人。平衡4是ICP算法中得到LiDAR当前姿态的目标函数，其中需要调整R和t，确保当前姿态估计具有最小的J。

        最后一步是将里程计结果和激光雷达结果融合，得到高频的定位结果。通常，EKF方法只能从考虑当前状态，这是延迟测量的问题。在这一部分中，我们使用EKF（SSKF）[23]的稳态近似。假设协方差为常数。在该方法中，我们可以得到EKF精度和SSKF运行时之间的延迟测量的权衡；该方法将解决ICP算法导致的计算时间长的问题。