荒野环境中的移动机器人定位方法研究*

钟钎钎 郭剑辉

（南京理工大学计算机科学与工程学院 南京 210094）

1 引言

近年来移动机器人的应用范围越来越广泛，而如何实现移动机器人在所处运行环境中自主准确的定位导航，是移动机器人技术的关键问题之一。在室外环境中，基于激光雷达的定位方法以其可以准确测量障碍点的角度与距离、无需预先布置场景、可融合多传感器、可以在光线较差的环境中工作以及能够生成便于导航的地图等优势，成为目前不可或缺的一种定位方案。

当前基于激光雷达的定位方法主要包含了基于滤波器的激光同时定位与建图（Simultaneous Localization and Mapping，SLAM）方案以及基于图优化的激光SLAM。Smith R 等［1］提出了使用最大似然算法进行数据关联的扩展卡尔曼滤波SLAM 方法（EKF-SLAM）。Montemerlo M 等［2］提出了Fast-SLAM 方法［2］，该方法将SLAM 问题分解成移动机器人的定位问题与已知移动机器人当前位置条件下的地图构建问题。Grisetti G 等［3］提出了一种使用了自适应重采样技术的Gmapping 方法。Lu F等［4］首次提出利用图优化的数学框架优化SLAM问题，通过非线性最小二乘方法来优化建图过程中累积的误差。Konolige K 等［5］提出了首个基于图优化框架的开源SLAM 方法Karto SLAM，谷歌的Cartographer开源方法［6］在其基础上融合了多传感器数据创建局部子图以及采用了闭环检测的扫描匹配策略。在3D 激光SLAM 领域中，Zhang J 等人提出了基于特征点的LOAM 方法［7］，并在其基础上结合视觉 提 出 了V-LOAM 方 法［8～10］；Daniel Lawrence Lu等［11］提出了增强视觉结合激光雷达的VELO方法。

本文所提出的定位方案针对荒野环境中特征稀少的特点，在建图过程中使用RTK-GPS、IMU 以及激光雷达三种传感器获取的信息，利用GPS信息优化正态分布变换算法的配准过程，建立高精度的离线点云地图；定位过程则先加载离线地图，使用激光雷达和IMU两种传感器获取的信息，然后结合正态分布变换配准算法与无迹卡尔曼滤波算法得到位姿估计，并对得到的位姿估计添加地图修正量，得到最终的位姿估计。

2 NDT-OMP算法和UKF算法

2.1 NDT-OMP配准算法

正态分布变换（Normal Distribution Transform，NDT）算法由Biber 和Straßer 提出［12］，Magnusson 等人将其拓展到了三维空间中［13］，算法的核心思想是将点云映射到平滑表面来表示，使用一组局部概率密度函数（PDF）来描述，每个PDF 描述表面的一部分的形状［14～15］。算法首先将扫描所占用的空间细分为单元网格，基于单元网格内点的分布计算每个单元网格的PDF。假设可以通过对单元网格内点的分布的刻画来生成x→的位置，并且参考扫描表面点的位置是由D维正态随机过程产生的，则测量得到的的概率：

对目标函数进行数学形式的简化后得到

2.2 UKF算法

3 基于NDT 算法与UKF 算法的定位方法

传统的基于特征点匹配的的SLAM 方法在环境特征较少的环境中，缺乏足够的特征点进行匹配定位，本文方法的定位过程使用了RTK-GPS、IMU以及激光点云信息，利用NDT-OMP 算法实现点云间的配准，并使用GPS 信息对配准过程进行了优化，最后使用g2o 框架对配准后得到的位姿估计进一步优化，最后建立高精度的离线地图；定位过程使用了离线点云地图、IMU 以及激光点云的信息，结合了NDT-OMP 算法和UKF 算法得到当前的位姿估计，并添加地图误差修正量，得到移动机器人当前位姿的最终估计。

3.1 建立离线地图

建图过程如图1 所示，首先对点云进行预处理，由于激光雷达获得点云每一帧都包含了大量的点云数据，其中也包含了一部分野值点，所以需要对点云下采样以及野值点的去除。完成对点云的预处理后，为了提升配准算法的稳定性，利用RTK-GPS 信息计算得到相邻两帧点云之间的位姿变换的初始估计值，然后使用NDT-OMP 算法进行相邻帧之间的配准，得到位姿估计，再利用g2o 框架添加GPS以及IMU的约束，对点云地图进一步优化后建立离线地图。

图1 本文算法的建图过程

3.2 定位

定位过程如图2 所示，第一步，加载建立好的离线地图，对离线地图进行预处理后作为配准时的参考帧，然后接受当前时刻的点云同样进行预处理后作为配准时的当前帧；第二步，将IMU 信息中的线加速度与角速度作为UKF 算法的预测量结合上一时刻移动机器人的位姿获得当前时刻的位姿初始估计值，将其作为配准时的初始值；第三步，将当前点云与离线地图进行配准，得到配准结果，将其作为UKF 算法的观测量；第四步，使用UKF 算法计算得到当前时刻的位姿估计值，最后添加地图修正量，修正建图过程带来的误差，得到当前时刻移动机器人的最终位姿估计。

图2 本文算法的定位过程

其中地图修正量通过当前点云与路标点云间的配准结果计算得到：首先在特征多的区域选取路标并保存，定位时先依据当前点云位置与各个路标之间的距离选取与当前点云相对应的路标，然后使用ICP 算法进行当前点云与路标点云之间的配准，计算得到当前点云与路标点云之间的距离d 以及变换关系T，并设定距离阈值τ，当d＜τ，认为当前点云与路标点云配准成功，每一次点云配准成功时记录距离d，通过比较得到其中的最小值，将对应的转换关系T 作为当前时刻点云与路标之间的变换关系，当点云配准成功的次数大于3，计算并更新当前的地图修正量。

4 实验结果与分析

为了验证本文所提出的定位方案的有效性，在内蒙古阿拉善的荒野环境中采集了所需的数据，并进行了实验验证。本文所使用的激光雷达是禾赛科技的激光雷达Pandar40P，探测距离为200m，垂直视角范围为-25°～+15°，最小垂直分辨率0.33°，系统的运行环境为Ubuntu16.04，所用计算机的处理器为Intel Core i5-8600，显卡为GeForce GTX 1050 Ti，内存大小16G。

4.1 建图结果

图3 是建图结果的对比，其中（a）、（b）分别为开源算法lego_loam 与本文建图算法的建图结果对比，结果表明在特征点较少的区域，lego_loam 算法无法很好地匹配，无法建立完整的地图，而本文的建图算法有效地建立了高精度的离线点云地图；图（c）、（d）分别为同一环境下配准算法优化前后的建图效果对比，可以看出，利用GPS 信息对配准过程所做的优化解决了在特征较少的环境中可能会出现的配准失败的情况。

图3 建图结果对比图

另外，在提升配准算法稳定性的同时也提升了配准算法的效率，本文分别在340m×470m 的矩形区域与8000m 的不重复路段两种不同的环境在进行了验证，优化前后算法的平均配准时间的对比如表1 所示，结果表明优化后的配准算法效率有了明显的提升。

表1 点云配准的平均时间对比

4.2 定位结果

图4 为本文算法的定位结果，在已有离线地图的基础上，实现了移动机器人的精确定位。

图4 本文算法的定位结果

图5 为本文定位算法在地图误差修正前与误差修正后的定位误差对比，修正前最大误差为4.77m，平均误差为2.39m，修正后的最大误差为2.57m，平均误差为0.60m。从修正前后的误差对比可以看出，本文的地图误差修正算法有效地减少了建图过程所带来的误差，提高了定位的精度。

图5 定位误差分析

5 结语

为了解决在特征点较少的荒野荒野环境中的定位问题，本文提出了一种先建立离线地图，再进行定位的方法，建图过程使用GPS、IMU、激光点云信息，利用GPS 信息优化NDT-OMP 配准算法的配准过程，建立高精度的离线点云地图；定位过程先加载离线地图，使用激光点云、IMU 信息，结合正态分布变换配准算法与无迹卡尔曼滤波算法得到位姿估计，并对得到的位姿估计添加地图修正量，得到最终的位姿估计。

实验结果表明，本文方法能有效地利用GPS信息建立高精度的离线地图，并在无GPS条件下利用离线地图实现了移动机器人的精确定位。