基于Scan Context与NDT-ICP相融合的果园建图方法研究*

耿丽杰，顾健，别晓婷，冉维旭，兰玉彬, 2, 3

(1. 山东理工大学农业工程与食品科学学院，山东淄博，255000；2. 山东理工大学生态无人农场研究院，山东淄博，255000；3. 山东理工大学国际精准农业航空应用技术研究中心，山东淄博，255000)

0 引言

近年来，随着国家对智慧农业的大力支持，中国首个无人农场落地，进一步推动了智能农业装备技术的快速发展[1-2]。机器人技术随着计算机、激光雷达等技术的进步，获得了快速发展。即时定位与地图构建(Simultaneous Localization and Mapping，SLAM)是机器人导航技术的关键技术之一，也是机器人自动导航的重要基础[3]。传感器技术的不断发展，尤其是近年来三维激光雷达的快速发展，使得即时定位与地图构建技术在建图精度及鲁棒性上有了很大程度的提升。根据传感器类型可以将SLAM技术分为视觉SLAM与激光SLAM两大类[4]。其中视觉SLAM借助视觉传感器获取环境信息，成本低、结构简单，但运算量大，易受光照影响，不适用于农田等光照变化明显的环境[5-6]。激光SLAM技术相对成熟，测距准确，受光照影响较小，且激光雷达相比相机、超声波、红外传感器等具有抗干扰能力强、精度高、测量范围广等优势，更适用于果园环境[7-9]。目前，农业机器人搭载激光雷达实现局部定位导航的方法不能提供全局的定位信息[10]，非实时动态测量技术(Real Time Kinematic，RTK)全局定位的方法主要通过三维建图以及重定位来实现。实现建图的主要算法以雷达里程计和实时建图(Lidar Odometry and Mapping in Real-time，LOAM)以及轻量级地面优化的雷达里程计和实时建图(Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain，LeGO-LOAM)为主。LOAM算法，缺少回环检测与后端图优化，运行在果园复杂环境中精度缺失严重。在此基础上LeGO-LOAM算法增加了回环检测与图优化部分。然而，回环检测[11-13]与图优化部分不可避免地引入了果园环境地图误匹配及累计误差大等问题。

回环检测，又称闭环检测，是机器人识别曾到达某场景，使地图闭环的能力，通常使用3D描述符的匹配，如快速点特征直方图(FPFH)、快速激光兴趣区域变换(FLIRT)和方向直方图签名(SHOT)等算法[14-16]。若回环检测成功，可以显著减小累计误差，帮助机器人更精准、快速地实现避障导航工作。因此，回环检测成为近年来机器人研究领域中的热点之一。随着深度学习的发展，从点云图像中可以很方便地获取语义信息[17]，用于辅助SLAM回环检测。例如基于3D点云分割匹配的方法SegMatch[18]，以及基于surf的方法SUMA++[19]，通过动态去除，在高速公路场景中实现了惊人的准确性。

除回环检测外，点云配准也是机器人避障导航中的重要步骤[20]。常用三维激光雷达点云配准方法有两种，迭代最近点(Iterative Closest Point，ICP)和正态分布变换(Normal Distributions Transform，NDT)[21-24]。其中，ICP算法配准速度较慢，但配准精度高。NDT算法配准速度较快但配准精度较低。

针对LeGO-LOAM算法引入的果园环境地图误匹配及累计误差大等问题，本文使用履带底盘搭建了软硬件系统，在机器人操作系统(Robot Operating System，ROS)下，以基于Scan Context的回环检测方法为基础进行改进，加入更优的NDT-ICP点云配准方法进行建图。最后通过在葡萄园进行实地建图试验，验证所提出建图方法的性能。

1 果园环境地图建图软硬件系统

果园环境地图构建的移动机器人的软硬件系统主要包括履带式差速转向移动底盘、嵌入式人工智能处理器、三维激光雷达、Linux操作系统、ROS机器人操作系统以及建图算法。

1.1 移动机器人组成

图1为移动机器人与机体坐标系{V}。

图1 移动机器人与机体坐标系

移动机器人使用履带式两驱差速转向运动底盘，定义满足右手定则的机体坐标系{V}。以三维激光雷达中心为坐标原点O，x轴指向机器人的正前方，y轴平行于机器人的轮轴且指向左方，z轴垂直指向正上方。

1.2 硬件组成

果园环境地图构建硬件系统主要由三维激光雷达(OS1-64)、装有Ubuntu18.04操作系统的Jetson AGX Xavier。

OS1-64为一款64线数字激光雷达，安装在移动底盘正上方，其水平视场角为360°，转速为10～20 Hz，垂直视场角为±15.8°，垂直角分辨率为0.52°，水平角分辨率为0.09°，点云数量高达131万点/s，最大测距为120 m，测距精度为±3 cm，内置6轴IMU，包括3轴陀螺仪和3轴加速度计，每秒采样数1 000个。

1.3 软件组成

系统的软件模块框图如图2所示。

图2 软件模块框图

首先，将3D Lidar点云图编码到Scan Context中。然后，计算每一环最大高度的均值，组成一个向量，记为Ring key。用Ring key构造KD Tree，进行快速地上层搜索。找到最相似的候选帧，而后才将少数候选帧与当前帧进行相似度评分，检测是否回环。确认回环后将两帧点云进行NDT-ICP匹配，将得到的矩阵作为约束进行图优化。

2 果园环境地图建图方法

果园环境地图构建方法包括点云扫描与处理、SLAM回环检测、点云配准以及图优化。

2.1 点云扫描

果园环境地图构建中，首先进行环境扫描。OS1-64激光雷达安装在履带式移动底盘上，通过手动遥控，以0.5～1 m/s的速度在葡萄园中行进，三维激光雷达以10 Hz的频率，录制点云数据。

2.2 回环检测方法

在获得点云数据后，首先进行雷达点云帧数据扫描，进行环境比对。本文选用Scan Context进行回环检测。

首先，对单帧果园点云数据进行区域分割。将单帧果园点云数据进行径向与环向划分，如图3所示。假设，激光雷达射线扫描的最远距离为Lmax，则径向区域分辨率为2π/Ns、环向区域分辨率为Lmax/Nr。图3中Pij表示第i环，第j扇区内的点云集，当前帧点云区域划分表达式为

图3 沿环向和径向的点云块划分

(1)

式中：P——当前帧点云；

i——环序号；

j——扇序号；

Nr——环向区域数量；

Ns——径向区域数量。

果园点云数据确定间隔划分，远离距离传感器划分区域比靠近距离传感器划分区域具有更大的果园物理面积。然而，两者同等编码到Scan Context的单个像素中。因此，Scan Context补偿了由远点的稀疏性引起的信息量不足，并将附近果园动态对象视为稀疏噪声。

果园当前点云帧分区后，受果园内复杂环境影响，使用点云的最大高度为区域赋值，数学表达式为

(2)

式中：p——第i环，第j扇内的区域点云集坐标；

z(·)——点集p的z轴坐标值函数；

Φ(Pij)——Pij点云集中最大的z值。

最终果园点云数据的Scan Context可以表达为Nr×Ns的矩阵

I=(bij)∈Nr×Ns

(3)

其中，bij=Φ(Pij)。

(4)

式中：Iq——待匹配点云帧；

Ic——候选点云帧；

因果园中雷达视点发生变化，同一地点的点云帧将随之发生变化，候选Scan Context的扇向量也可能会发生改变，为避免该现象引入更多未知状况，本文遍历计算点云扇偏移的Scan Context。则最佳匹配扇位移数n*与最短距离d(Iq,Ic)的数学表达式为

(5)

(6)

由于Scan Context距离计算量过大，无法对所有矩阵进行计算，本文使用Ring key，进行两阶段分层搜索。首先利用Ring key进行果园点云候选帧挑选，然后，对Scan Context矩阵进行计算。

通过使用环的L0范式‖ri‖0，函数ψ被赋予单个实值，数学表达式为

(7)

其中，ψ:ri→。

式中：ri——每一环的最大高度均值。

向量k第一个实值来自离LiDAR最近的圆环，随后，实值按序来自下一个环。因此，Nr维向量Ring key数学表达式为

k=(ψ(r1),…,ψ(rNr))

(8)

将向量k使用KD Tree存储。将待匹配点云帧的Ring key在KD Tree中查找n个相似值及其相应扫描索引。然后将选定的n个候选帧的Scan Context与待匹配点云的Scan Context计算距离值。满足阈值的最短距离候选帧留下，表达式为

(9)

式中：c*——确定为回环位置的索引；

C——KD Tree中候选索引集合；

cn——第n个候选帧索引值；

Icn——选定的第n个候选帧；

τ——给定的阈值为0.5。

2.3 点云配准算法

为了得到匹配点云帧与回环点云帧的相对位姿矩阵，进行后端图优化，确认回环位置后，本文选用NDT-ICP将果园匹配点云帧与候选点云帧进行点云配准。

将果园匹配点云帧空间分为若干相同立方体，并满足立方体内至少有5个点，每个匹配点云帧立方体内点的均值μ和协方差矩阵C数学表达式为

(10)

(11)

式中：m——匹配点云帧空间立方体的点云数；

k——点云序号，k=1，2，…，m；

果园匹配点云帧立方体中每个点位的概率密度

(12)

将匹配点云帧映射到回环检测帧坐标系中，每个点映射变换后的正态分布

(13)

将每个点的概率密度相加，评估坐标变换参数数学表达式为

(14)

使用Hessian矩阵法优化s(p)。然后，重新映射到回环检测帧坐标系中，直到满足收敛条件为止。优化后的回环点集为Ic*′，匹配点集Iq。用E(R,t)来表示果园原点集Ic*′在变换矩阵(R，t)下与果园目标点集Iq之间的误差。则求解最优变换矩阵的问题转化为满足minE(R，t)的最优解(R，t)。其中E(R，t)称为目标函数，它表示两个点集之间的差异程度。该目标函数表达式为

(15)

R——旋转参数；

t——平移参数。

为了最小化目标函数，求解最优变换矩阵，即R和t。首先，计算果园目标点集Ic*′中每一个点在Iq点集中的对应最近点。利用SVD分解求得旋转参数R和平移参数t，使得变换矩阵E(R，t)最小。对点集Ic*′使用上一步求得的旋转参数R和平移参数t，得到新变换点集Ic*″。如果变换后点集Ic*″与Iq点集满足目标函数要求，即两点集的平均距离小于给定阈值，则停止迭代计算；否则重新计算新点集Ic*″作为新点集Ic*′继续迭代，直到满足收敛条件为止，得到最优解(R，t)与相对位姿矩阵。

3 试验结果与分析

为验证本文提出的基于Scan Context与NDT-ICP的果园环境建图方法，使用标准化种植葡萄园环境数据集与KITTI公开数据集进行试验。其中，果园环境数据集全长1 707.645 m，持续时间为1 167 s。KITTI数据集00序列，全长3 724.187 m，持续470 s。

3.1 回环检测试验

在果园回环检测试验中，对比了半径搜索回环方式与ICP匹配算法相融合的算法以及Scan Context搜索回环方式与ICP匹配算法。试验结果如图4所示。其中图4(a)与图4(b)分别表示半径搜索回环方式与Scan Context搜索回环方式的局部效果图。

(a) 原算法局部放大图

从图4(a)可以看出，使用半径搜索回环点云模糊，且植株行中离群点较多。从图4(b)可以看出，Scan Context搜索回环对拐角处影响较大，图4(b)拐角处明显比图4(a)轮廓清晰。

表1为两种不同的回环搜索方式所检测到的回环准确率结果，可以看出，在果园数据集环境下，使用Scan Context搜索回环检测方式进行检测的回环次数更多，因其环路检测不受激光雷达视点变化的影响，所以在反向重访和拐角等地方检测环路。半径搜索回环检测的回环个数为196，Scan Context回环检测的回环个数为261，回环数提高了33.2%。在KITTI数据集环境下，半径搜索回环检测的回环个数为42，Scan Context回环检测的回环个数为51，回环数提高了21.4%。

表1 果园检测回环准确率

3.2 点云匹配试验

3.2.1 轨迹对比试验

为了评估本文提出的基于Scan Context与NDT-ICP的建图方法在KITTI公开数据集中效果，将其与基于Scan Context与ICP最终的建图轨迹进行对比。

使用GNSS持续获取经纬度信息作为标准轨迹信息。由图5可知基于Scan Context与NDT-ICP最终的建图轨迹更加贴近标准轨迹。

图5 轨迹对比图

3.2.2 试验指标

均方根误差(Root-mean-square error，RMSE)用来衡量观测值同真值之间偏差，可以表征建图轨迹精准度，其值越小表明建图轨迹距GNSS轨迹越近，数学表达式为

(16)

式中：X′——真实值；

Xa——测量值；

d——位姿数；

a——位姿序号，a=1,2,…,d。

标准差(Standard Deviation)用来衡量观测值同真值之间偏差，能反映建图轨迹误差离散程度，其值越小表明建图轨迹距GNSS轨迹越近，数学表达式为

(17)

表2为不同匹配方式下回环检测方法的误差评价，结果表明，NDT-ICP点云匹配方法优于ICP点云匹配方法，点云建图误差率缩小至原来的80%。即本文提出的点云匹配优化方法能大大减少果园环境点云建图产生的累积漂移误差。

表2 LeGO-LOAM系统误差表

4 结论

本文针对在果园环境同时定位与建图中误匹配、累计误差大等问题，提出了一种基于Scan Context与NDT-ICP相融合的果园建图方法。并分别在KITTI数据集与果园环境中进行相关试验。

1) 通过利用Ring key进行快速地上层搜索，得到少量候选帧，随后对候选帧与当前帧进行相似度评分，通过两阶段搜索算法来有效地检测回环以减少误匹配；同时使用一种基于NDT粗配准与ICP精确配准融合的点云配准方法来降低建图的累计误差。

2) 在果园数据集环境下，半径搜索回环个数为196，Scan Context回环个数为261，回环数提高了33.2%。

3) 在KITTI数据集环境下，半径搜索回环个数为42，Scan Context回环个数为51，回环数提高了21.4%。本文提出的NDT-ICP点云匹配方法的各项误差评价参数都小于ICP点云匹配方法的对应误差评价参数。表明本文改进的系统具有足够的回环数和低误差率，可以基本满足果园环境下点云建图。