复杂果园场景中基于DBP的激光回环检测算法

2023-05-28苗中华何创新李云辉

农业机械学报 2023年5期

欧芳苗中华李楠何创新李云辉

(上海大学机电工程与自动化学院, 上海 200444)

0 引言

机器人在果园场景中基于雷达点云数据在定位的同时构建地图,即激光同步定位建图 (Simultaneous localization and mapping, SLAM)技术,是果园机器人领域的核心问题。在SLAM过程中加入回环检测能够实时矫正位姿估计误差,提高定位精度[1-2]。目前回环检测主要采用融合基准的全球定位系统 (Global positioning system,GPS)与视觉传感器的方法[3-4]。然而,视觉传感器对长期的光照变化敏感,往往不能成功地跟踪视觉特征。并且,在果园、森林等树冠茂密的场景中,卫星信号容易被阻拦,导致GPS失效[5-6]。相较而言,点云数据不受室外光照变化以及信号遮挡影响,更适用于复杂果园场景[7-9]。

回环检测是机器人SLAM技术中的关键内容,常见的回环检测算法包括基于手工特征与深度学习的方法。文献[10-11]提取关于点云的直方图表示用于点云配准,然而直方图提取需要大量的算力消耗,不适用于在大尺度场景中检测点云帧的回环对。文献[1]提出了基于点云扫描上下文信息的回环检测方法,通过提取区域点云的最大高度值,将单帧雷达扫描投影到鸟瞰图矩阵得到回环检测特征Scan Context。文献[12]利用点云几何分布与强度投影提取ISC描述子,实现两阶段快速回环检索,避免了直接暴力匹配。文献[13]基于高通滤波提取具有旋转不变性的点云描述子。这些手工描述子将三维空间点云表示为二维矩阵,提高了回环检测效率。然而,单一的点云强度、高度最值并不具备点云扫描空间的全局代表性,在具有多个高度相似场景的果园,无法从细节区分场景目标,导致回环检测的高失败率。

深度学习为回环检测带来了新的可能性。文献[14]提出了一种联合在线 LiDAR 扫描和强度图并通过卷积匹配实现回环检测的方法。文献[15]利用高斯混合模型对多个可能的回环检测结果建模,有效排除错误回环索引。文献[16]直接对点云进行处理,对输入点云中的每一个点,学习其对应的空间编码,最后集成所有局部点的特征得到一个全局的点云特征。文献[17]提出了一个多层次点云特征提取结构Pointnet++,有效提取局部特征。然而,深度学习模型训练需要大量的差异性数据,且通常只对特定场景有效,不适用于高度相似的果园场景。

本文面向非结构化、大尺度、高度相似的复杂果园场景,提出一种基于密度二进制模式(Density binary pattern,DBP)描述子的回环检测算法。将当前帧点云分割成Nr×Ns×Nh个体素块,并进一步地将单元块的点云数据编码投影到Nr×Ns矩阵,以此构建具有区分性的密度二进制模式全局DBP。DBP描述子为鸟瞰图矩阵,矩阵元素值由编码函数决定,值区间为0～255,为提高回环检索效率,以两阶段搜索方法实现快速回环检测。基于历史帧的DBP环因子构造KD-Tree(K-dimensional tree,K维度树),从KD-Tree中初步检索候选(可能的)回环帧。对所有候选帧的DBP描述子进行列平移,计算得到与当前帧最相似的候选帧作为回环目标帧。

1 回环检测系统与关键技术

1.1 回环检测算法

如图1所示,在SLAM问题中,机器人位姿估计是一个递推的过程,即由上一帧位姿解算当前帧位姿。随着时间推移,位姿误差会不断累积导致轨迹漂移。成功的回环检测,可以显著地减小建图与定位累积误差,使机器人更精准、快速地进行避障导航等工作。

图1 回环检测示意图

因此,在大尺度SLAM任务上执行回环检测实现轨迹优化是非常必要的。回环检测的实质是为了求两帧点云的相似性配准,在机器人的运动过程中实时查找与当前帧相似的历史点云帧,构建回环对之间的位姿约束。回环检测过程中,当前帧xi(i=1,2,…,N)在历史帧xj(j=1,2,…,N-C)数据库中查询相似的场景并配准。为保证回环检测的可靠性,查询的历史帧需与当前帧点云保持一定距离C。考虑到在大尺度场景中,点云逐帧配准查询将导致巨大的计算量,因此,为减少回环检测时间,保证SLAM任务的实时性,通常将回环检测分为两阶段检索方法。首先,将每帧点云抽象表示为词袋模型[18]、鸟瞰图[1,12]等特征描述子;其次,在候选回环帧中查找最终回环。

1.2 关键技术

近年来,随着传感器技术的快速发展,果园机器人的使用已显著增加。果园机器人任务主要包括果园物境信息获取、剪枝套袋、对靶喷药、疏花疏果、果实采摘等,它们对机器人移动过程中建图与定位具有高精度要求[19-21]。为了缓解SLAM过程中由累积误差导致的轨迹 “大漂移”,引入回环检测构造位姿估计约束。然而,在复杂的果园环境中回环检测需要克服非结构化、大尺度、场景高度相似的挑战。如图2所示,在复杂果园场景中,果树的长期生长变化、枝叶运动以及果树的无规则布局,决定了果园的非结构化特性。

图2 复杂果园场景

不同于成行成列的半结构化/结构化果园,机器人可以以等间距的行距与列距作为回环检测的先验信息。复杂果园的非结构化特性给回环检测带来巨大挑战。机器人建图与定位是增量式累积的过程,回环检测过程中当前雷达帧在所有历史帧中遍历查询最相似场景。随着作业场景尺度的增加,历史帧数据库内容剧烈增加,回环检测将带来巨大的计算消耗。不同于常见的户外场景可以人、车、楼宇等特征明显的目标辅助回环检测。果园场景中目标单一且主要为果木。同一个果园可能包含多处相似的具有弱可区分性的场景,从而给回环检测中成对相似场景的识别带来巨大挑战。

针对以上提及的复杂果园的非结构化、大尺度、高度相似特性,本文提出如图3所示的激光回环检测系统。系统具备的关键技术包括:

图3 基于密度二进制模式的回环检测算法框图

(1)针对非结构化特性,将点云扫描空间分割为多个3D体素块,并编码体素块为鸟瞰图描述子DBP。描述子只考虑目标点云的整体轮廓,不受树叶运动等导致的细微动态环境变化影响。

(2)针对果园弱可区分特性,全局描述子能体现点云的高度与密度信息,具备对果树、杂草以及低矮灌木的细节区分能力。

(3)针对果园的大尺度特性,避免直接的关键帧匹配,以有效的两步法提高回环检测效率。

2 基于DBP的回环检测算法

本文面向果园场景的回环检算法主要包括:提取点云DBP描述子与两阶段回环检索。

2.1 DBP点云描述子

文献[13,22]表明,将无序点云有序化是最常见的提高计算速度的方法。本文定义了一种在果园场景中回环检测的密度二进制模式描述子DBP,一种关于单帧雷达扫描的全局描述子,提取流程如图4所示。

图4 提取果园中单帧雷达扫描的密度二进制模式

DBP描述子将雷达360°扫描的球体空间投影为鸟瞰图矩阵。以单帧雷达扫描中的果树为例,将果树点云分割到垂直方向8个3D体素块,若体素块内存在点云占位,则其二进制值为1,否则为0。进一步将得到的8位二进制值基于DBP编码函数投影为鸟瞰图矩阵元素。鸟瞰图以雷达为原点,沿扫描半径与方位角方向将雷达的3D扫描空间展开。DBP描述子提取的具体流程描述为:

首先,将单个点云pk=(xk,yk,zk)在极坐标下表示为Γ=(rk,θk,hk),其中

(1)

(2)

hk=zk+ph

(3)

式中rk、θk、hk——点云半径、方位角、实际高度

ph——雷达安装高度

将点云沿垂直高度方向分割成Nr×Ns×Nh个体素块P,设置雷达最大有效扫描半径Rmax=80 m、最大有效高度Hmax=8 m,以及雷达扫描一周的方位角为2π。

(4)

式中Nr、Ns、Nh——点云在半径、方位角、高度的单元块分割数量,分别取20、60、7

i、j、ξ——点云分割到单元块中对应扫描半径、方位角、高度方向的序号

Pijξ——点云分割到单元块的表示

可能有多个点落在同一个相同i、j、ξ序号的单元块P内。

(5)

在全局描述子DBP编码过程中,沿高度方向将点云分割到8位体素块,同时将体素块二进制值编码投影到DBP矩阵,以此构建具有区分性的空间二进制模式全局表示

(6)

(7)

式中δ(P)——体素块赋值函数,由块内点云占位决定

δ(P)函数对体素块赋二进制值,块内存在点云则为有效扫描,表示为占位;反之为无效扫描,非占位。在描述子编码函数中,对点云占位体素块赋值为1,否则为0。由于体素块分割考虑到点云的几何信息,同时点云占位与果树点云密度关联。因此,全局描述子DBP元素内容同时解释了点云的几何与密度信息。并且,由于二进制编码函数的指数特性,占据高位体素块的果树编码值明显大于低矮杂草、灌木的特征编码值。因此,描述子能对不同高度与密度分布的植被进行有效区分。

2.2 两阶段回环检测

回环检测需要在所有历史帧中检索与当前场景最相似的雷达扫描帧作为目标回环帧。然而,随着机器人经过的路程越长,复杂果园场景尺度不断增加,增量式建图与定位的同时伴随着巨大计算量。激光回环检测作为SLAM系统中的优化环节,应尽可能减少时间占比,保证机器人任务的实时性。本文基于两阶段搜索算法实现在大尺度场景中高效地检测回环。如图5所示,基于历史帧DBP描述子的环因子构造KD-Tree,从历史帧中检索K近邻候选帧;计算候选回环帧与当前查询帧的DBP描述子相似度,确定最终目标回环。

图5 两阶段回环检测算法原理框图

(1)提取候选(可能的)回环帧。提取关于DBP的低维向量表示,引入环因子向量概念,统计DBP每行非零元素的占有率

(8)

式中 ‖ri‖0——DBP矩阵行向量的零范数

因此,环因子V是关于DBP的Nr维列向量表示

V=(φ(r1),φ(r2),…,φ(rNr)),whereφ:ri→R

(9)

如图6所示,将以雷达为中心的360°扫描空间投影到DBP鸟瞰图矩阵,矩阵的行对应扫描的水平方位角方向。

图6 DBP描述子对比

同一场景中,不同雷达扫描帧只存在水平方位角偏差,导致投影到鸟瞰图时产生列平移。列平移不影响行向量中非零元素的统计,因此环因子不受视角变化影响。为保证高效候选帧检索,基于历史帧描述子的环因子构造KD-Tree。当前扫描(查询)帧在KD-Tree中查询K近邻帧作为候选回环帧(K=25,K为候选回环帧数量)。虽然相比于全局描述子DBP,环因子的信息含量更低,但是低维环因子可实现快速回环帧初步检索。

(2)当前帧与候选回环帧描述子相似度计算。由于同一场景下的不同扫描帧存在水平偏差。因此,对所有候选帧平移n列计算与当前帧的相似度

(10)

式中DBPq——查询点云的描述子

DBPc——候选点云的描述子

确定候选帧与当前帧之间的最大相似度为d的最小值

(11)

(12)

式中c*——与查询帧距离最小的候选帧索引

ck——所有候选帧索引

3 复杂果园环境回环检测实验

3.1 实验设置

果园实验平台为图7a所示的果园采摘机器人。机器人搭载内存为500 GB的Intel i7 NUC微型工控机与16线速腾激光雷达。实验将3种回环检测算法都集成到机器人操作系统(Robot operating system, ROS),评估算法在3个不同特点果园的性能。

图7 实地果园场景实验

经典的LeGO-LOAM[23]是一种可搭载于嵌入式系统的轻量化激光SLAM,适用于大尺度户外环境。系统由点云分割、特征提取、雷达里程计、雷达建图、位姿估计5个环节组成。在雷达建图环节中基于点云帧之间的距离与时间阈值构建简单回环约束。本文以ISC[12]与本文算法改进LeGO-LOAM的回环检测内容,构造ISC-LeGO-LOAM与DBP-LeGO-LOAM激光SLAM系统。实验以运行时间与位姿估计误差评估LeGO-LOAM、ISC-LeGO-LOAM与DBP-LeGO-LOAM 3个算法在不同尺度果园场景的性能。实验结果证明了DBP-LeGO-LOAM算法在果园场景回环检测的可行性,特别是在大尺度多回环场景具有显著优越性。

3.2 实验结果分析

在果园3的回环检测实验中,机器人基于LeGO-LOAM算法生成的实际果园环境点云地图如图7b所示。机器人重新回到起点时回环识别失败,雷达里程计终点未与起点构成闭环,位姿估计的累积误差导致机器人运动轨迹“大漂移”。基于本文DBP-LeGO-LOAM算法生成的实际果园环境的点云地图如图7c所示。机器人识别曾经到达的场景,成功执行了位姿回环矫正,使得轨迹闭环。如图8所示, 实地果园实验中基于DBP-LeGO-LOAM算法生成的轨迹与真值(Ground truth)轨迹基本保持一致。为直观量化算法性能,本文以算法轨迹与真值轨迹的绝对误差(Absolute trajectory error, ATE)评估3种激光SLAM系统。表1呈现了绝对轨迹误差的均方根误差(Root mean square error, RMSE)、标准差(Standard deviation, STD)与算法用时,反映了算法的鲁棒性、稳定性与实时性。

表1 3种算法的轨迹误差与时间消耗对比

图8 实地果园实验轨迹与真值轨迹

在果园1中,场景尺度为1 604个位姿。本文算法的轨迹误差最小,均方根误差与标准差分别为0.12 m与0.08 m。DBP-LeGO-LOAM算法相比于LeGO-LOAM与ISC-LeGO-LOAM算法定位轨迹的均方根误差分别减少33%和40%。LeGO-LOAM的全局回环检测算法取得最小运行时间44.94 s,本文的DBP-LeGO-LOAM算法运行时间为52 s,提取回环检测描述子使算法运行时间增加7.06 s。

果园2场景尺度为10 458个机器人位姿,场景中地势平坦,机器人运动平滑无大抖动,本文DBP-LeGO-LOAM算法的均方根误差为0.23 m,相比LeGO-LOAM算法轨迹减少32%。随着场景尺度的增加,全局回环检测算法的时间消耗为125.44 s,LeGO-LOAM算法已不再适用。两阶段回环检测DBP-LeGO-LOAM算法的时间消耗为118.38 s,比LeGO-LOAM算法减少7.06 s。

果园3中场景尺度为2 294个机器人位姿,实验模拟机器人实际采摘任务的频繁转向、抖动。DBP-LeGO-LOAM与ISC-LeGO-LOAM算法均使用了改进的回环检测算法,相对于基准的LeGO-LOAM算法机器人位姿估计优化效果显著。DBP-LeGO-LOAM算法取得了最好的实验效果,位姿估计轨迹的均方根误差与标准差分别为0.24 m与0.09 m,减少81%与91%。LeGO-LOAM、ISC-LeGO-LOAM与DBP-LeGO-LOAM算法的时间消耗分别为75.43、63.19、73.60 s。

在小尺度的具有单一回环的果园中(果园1),机器人位姿估计累积误差小,可优化空间有限。DBP-LeGO-LOAM算法以时间代价取得定位精度的提升。然而,在复杂的非结构化、大尺度、多回环果园场景中(果园3)DBP-LeGO-LOAM算法既能保证算法的实时性,同时多次成功的回环检测也保证了机器人定位精度。算法在复杂场景的优越性可解释为:DBP描述子对高度相似的场景有效编码,保证了回环检测的正确性。在大尺度果园场景中,多次成功回环检测与矫正使机器人“大漂移”误差显著减少。同时,相比LeGO-LOAM中直接基于帧间距离与时间阈值的回环检测算法,以两步法执行回环检测,可有效提高算法效率,保证了算法可靠性与实时性,使算法不受场景尺度限制。