基于改进Gmapping算法的果园二维环境地图精准构建

薛金林 王培晓 周 俊 程 峰

(南京农业大学工学院, 南京 210031)

0 引言

果园作业机器人的定位与导航技术研究主要集中于基于SLAM算法的果园环境地图构建研究,果园环境地图能否实现精准构建直接影响后续定位与导航作业的效果。

在众多环境信息获取传感器中,激光雷达以其不易受光照影响、精度和实时性高等优点被广泛应用[1-3]。针对激光SLAM算法,典型的三维环境地图构建算法有基于3D特征匹配的LOAM算法[4-5]和LeGO-LOAM算法[6]。LOAM算法通过提取环境中面特征和线特征进行帧间匹配,特征匹配效果良好,地图构建较为精准。LeGO-LOAM算法凭借基于线的地面分割方法提取地面点,有效提升了算法整体的速度。上述三维地图构建算法虽然实现了果园环境的精准构建,但果树树冠的季节性变化及果树的成长和衰老造成的果树特征变化不利于后续的地图匹配,为解决此问题有部分研究针对果园中树干这一不变特征进行二维环境地图构建。针对较小的简单果园场景,张莹莹等[7]对DBSCAN算法采用自适应密度阈值和聚类半径实现对果园树干地图的精准构建;GRISETTI等[8]提出的Gmapping算法以较小的计算量实现了稳定且较高精度的地图构建。但对于特征复杂的大场景果园,张莹莹提出的改进DBSCAN算法在作业机器人转向角度过大时可能超出阈值范围造成树木的漏检;Gmapping算法基于3D点云匹配方法的ICP前端里程计在后期累计误差较大[9],并且后端优化部分对前端里程计的初始定位信息的优化效果有限,导致最终的定位精度较低,不利于精准二维地图的构建。LIM等[10]采用了鲁棒的基于多平面的R-GPF地面分割方法,有效提高了前端里程计精度,但该方法在面对果园内较为竖直的果树树干和低矮平缓的草坪或灌木时可能分割失败。针对后端优化,ROH等[11]提出的改进PSO粒子群优化算法的前期优化效果稳定,但在迭代后期由于无法权衡全局搜索的程度易陷入局部最优的困境。

本文对Gmapping算法进行改进,前端里程计以LeGO-LOAM测程框架为底层,针对果园实际环境对R-GPF方法的探测区模型和地面点估计方法进行相应改进,提高果园内地面分割效果,进而获得高精高频的激光雷达里程计。后端优化部分采用一种受蝙蝠寻找猎物行为启发的自适应重采样方法实现对初始定位信息的有效和合理程度的优化,以提高最终的定位精度,从而获得精准的果园环境地图。

1 精准果园环境地图构建方法

本文提出的精准果园环境地图的构建算法需要对Gmapping算法的前端里程计和后端优化部分进行改进,其中前端里程计用基于改进R-GPF方法的激光雷达里程计模块进行替换,后端优化Rao-Blackwellised粒子滤波器[12]的重采样方式采用BAT启发式自适应重采样,基于改进Gmapping算法的精准地图构建算法框架如图1所示。

图1 精准地图构建算法框架

1.1 精准地图构建算法整体流程

基于改进Gmapping算法的整体流程就是实现作业平台的位姿预估和利用预估位姿最终实现地图的获取。位姿预估即作业平台依靠激光雷达观测信息z1:t=z1,z2,…,zt与前端里程计信息u1:t-1=u1,u2,…,ut-1进行后端优化得到最终定位信息x1:t=x1,x2,…,xt,即求解自身位姿的后验概率分布p(x1:t|z1:t,u1:t-1),后端优化步骤概括如下:

(1)

再进行权重归一化处理

(2)

(4)重采样。根据归一化后得到的权重进行重采样,权重较大的粒子得以保留,权重较小的粒子予以剔除。重采样结束后,可计算获得最终定位信息,同时所有粒子的权重都重新设置为1/nr,nr为剩余粒子数。

最后进行地图更新。根据历史观测信息z1:t和步骤(4)获得的最终定位信息计算p(m|x1:t,z1:t),实现地图更新。

1.2 基于改进R-GPF地面分割方法的激光雷达里程计

图1里程计模块以LeGO-LOAM框架为底层,并将LeGO-LOAM基于线的地面分割方法用改进R-GPF方法代替,测程过程概括为以下4个步骤:①基于改进R-GPF方法进行地面分割,识别得到地面点。②将全部点云和地面点分别进行点云投影。③基于投影图像中的平滑度提取边缘特征和平面特征。④利用特征的对应关系,获得连续扫描之间的变换矩阵来估计测程。激光雷达里程计框架如图2所示。

图2 激光雷达里程计框架

1.2.1地面分割问题定义

(3)

1.2.2探测区模型

LeGO-LOAM使用激光雷达位于不同水平分辨率上同一线上任意两点之间的角度差来提取地面,它往往对某些噪声很敏感,如崎岖不平的地面,草坪或灌木丛也会阻碍地面分割,因为它们的形状梯度是任意的。鉴于基于线的地面分割方法易受干扰,因此提出了鲁棒的基于多平面的R-GPF地面分割方法,该方法以激光雷达为坐标原点建立二维圆形探测网格,再根据点云的三维坐标进行空间上点云的多平面分布。

R-GPF方法的探测网格M沿径向和周向进行规则划分,最终形成环区和扇区,其中Nr和Nθ分别是环数和扇区的数目,这样M最终将分为径向距离为Rmax/Nr,对应轴向角为2π/Nθ的Nr×Nθ个网格,其中Rmax为最大边界,探测网格M划分结果如图3a所示。

图3 探测区模型示意图

然而在语义KITTI数据集上对所有序列的各帧点云数据进行测量的实验数据[14]表明,有90%以上的地面点位于激光雷达原点附近20 m范围内。因此,R-GPF方法的探测网格M的划分方式面临以下两个问题:①当点云距离坐标原点过远时,对应网格内的点云过于稀疏,因此难以拟合出地平面。②由于靠近坐标原点的网格太小并且原点附近点云过于密集,这种情况有时会导致地平面的法向向量估计失败[15]。

(4)

其中

θk=arctan2(yk,xk)

(5)

ΔRm=Rmax,m-Rmin,m

(6)

Rmax,m=Rmin,m+1

(7)

进行参数的合理设定,取{Nr,1,Nr,3}={4,6},{Nθ,1,Nθ,3}={30,40}。同时,Rmax,3=Rmax,Rmin,1=Rmin,Rmax=80.0 m,且全局最小边界Rmin取1.0 m来去除掉移动平台对点云数据的影响。

同时Z2中区的网格尺寸根据实时点云密度进行自适应调整,其中整数Nr,2和Nθ,2变化范围分别为[6,8]和[36,60]。改进的探测区模型如图3b所示。

与R-GPF方法的探测网格划分方式相比,改进的探测区模型中,Z1内区和Z3外区的网格更大,可以有效地解决外侧的稀疏性问题,同时可以实现对原点周围地平面法向量的稳健估计,加以Z2中区的自适应调整策略,最终有助于提高地面分割的效果;此外,探测区模型网格的数量减少到576～840个,有助于里程计的高频输出。

1.2.3地面点似然估计

R-GPF地面分割方法将点云分配至各个网格对应的空间后,拟合出该空间内符合设定条件的地平面,进而得到分割的地面点。代替常用的平面拟合方法RANSAC[16],R-GPF方法使用了主成分分析PCA[17]方法,虽然RANSAC对异常值的敏感性往往较低,但PCA的处理速度相较RANSAC更快,并能达到可接受的拟合效果[18]。

R-GPF方法获取地面点的步骤如下:①任一网格空间内高度最低的点最有可能属于地面点[19],选择最低高度的点作为初始种子,根据高度阈值得到初始估计地面点。②结合平面距离阈值,对初始估计地面点进行多次PCA平面拟合迭代,获取最终估计地面点,设定迭代次数为3次[20]。

(8)

同时假设每个网格的对应空间都相互独立。

(9)

σ(u3,n)=

(10)

式中z——单位竖直向量

case(·)——条件判断函数

在式(10)中,case(·)满足条件时返回1,否则返回0。一般情况下激光雷达周围的地平面相较参考X-Y平面变化不大,但较远处的地面可能为陡峭的坡面,故水平度阈值φ2的设定应更加保守,故取φ1=30°,φ2=45°。

(2)高度:经水平度评价函数评估后,一些FPs如较为竖直的果树树干和灌木侧面已经被剔除,但灌木等物体的平缓顶端未被过滤,故针对此类FPs提出高度评价函数为

(11)

因此,最终估计的地面点可以直接表示为

(12)

1.2.4改进R-GPF地面分割方法的有效性验证

验证改进R-GPF地面分割方法的有效性,在语义KITTI数据集00序列第5帧附近的地面估计结果如图4所示。由图4可以看到,原始R-GPF方法错误地将一些较为竖直的树干、灌木侧面和墙体识别为地面点,同时灌木和汽车的平缓顶端也未能正确分割,而改进R-GPF方法成功地过滤掉了错误估计的地面部分,从而显著地减少了FPs,验证了较为竖直的树干、灌木侧面和墙体以及灌木和汽车的平缓顶端可以分别被水平度和高度过滤器过滤。

图4 在语义KITTI数据集00序列第5帧附近的地面分割结果

比较基于原始R-GPF地面分割方法的激光雷达里程计和基于改进R-GPF地面分割方法的激光雷达里程计的测程效果,直观比较如图5所示,具体数据如图6和表1所示。可以明显看出,本文提出的改进激光雷达里程计显示出更好的性能,在整个过程中都保持较小且稳定的绝对误差产出,同时可以得出,精确的地面分割可以有效提高激光雷达里程计的性能。

表1 绝对误差对应数据

图5 在语义KITTI数据集00序列上的测程结果与实际轨迹的绝对误差

图6 绝对误差对应数据图

1.3 BAT启发式自适应重采样方法

Gmapping算法由于其后端较为粗暴的重采样方式和重采样过程中对粒子收敛程度的较少干预分别导致其粒子的多样性表述能力和定位准确性难以适应果园等大场景下的环境地图构建。受蝙蝠寻找猎物行为启发本文提出了一种重采样方法来解决这两个问题从而实现在果园等大场景下环境地图的精准构建。

BAT启发式自适应重采样方法的流程概括为图7,rand为在[0,1]范围内均匀分布的随机数[21]。首先筛选权重最高的粒子作为最佳粒子,再自适应生成重采样粒子集并将每个粒子对应为蝙蝠,最佳粒子对应为目标猎物,每个蝙蝠随机提取固定频率,并将其定位和分配速度,根据其与目标猎物的接近程度自动调整脉冲的发射率和响度来搜索猎物,并不断靠近猎物,同时不断判断是否到达自适应调整的迭代时间,以生成最终定位信息。

图7 BAT启发式自适应重采样流程图

1.3.1重采样粒子集的自适应生成

执行完后端优化的权重归一化处理步骤后,对整个粒子群进行权重差异性评估,判断当前是否需要执行重采样步骤,粒子群权重差异性指标Neff的计算公式为

(13)

如果权重差异性指标计算值大于设定重采样阈值,说明差异性很小,不需要执行重采样步骤;反之则表明差异性较大,易造成定位不准,需执行自适应重采样步骤。

相比于权重较大的粒子,权重较小的粒子并不能很好地反映真实的后验分布,并且由于部分高权重粒子已经接近于真实状态,并不需要过度优化。故为提高整个粒子群的定位准确性并保证算法的实时性,该改进算法的重采样步骤仅作用于权重较小的部分粒子。

根据粒子权重将整个粒子群进行降序排序,筛选权重最高的粒子作为最佳粒子,其次设定重采样粒子集自适应生成阈值为粒子群权重差异性指标计算值向下舍入得到的最近整数。将集合中粒子次序大于自适应生成阈值的粒子放入新生成的粒子集中。该方法可以根据粒子群权重差异性程度来自适应调整重采样粒子集的大小来获得更合理的重采样粒子覆盖范围。

1.3.2迭代时间的自适应调整

由重采样粒子集的自适应生成过程可知需要重新采样的粒子已经接近于真实状态,所以重采样不需要过长时间的迭代优化,过长时间的迭代优化会导致粒子的退化[22],故应根据重采样前作业平台预估位姿粒子与最佳粒子之间的实际差异程度指标dt来自适应调整迭代时间

(14)

zt——全局最佳粒子对应的测量值

同时结合重采样粒子集自适应生成阈值,提出了一种能够在重采样步骤中自适应调整粒子优化迭代时间的策略

(15)

式中T——迭代时间Tmin——迭代时间下限

Tmax——迭代时间上限

l——自适应生成阈值

基于以上分析,BAT启发式自适应重采样方法可以在保证粒子多样性的前提下获得更好的粒子群定位效果,即获得更准确的最终定位信息来实现果园等大场景地图的精准构建。

2 果园环境地图构建试验

2.1 试验环境及设备

试验在江苏省农业科学院梨园进行,由于处于堆肥时期故选取可进行试验的3行梨树,梨树行间距为 6 m 左右,梨树和棚架立柱交叉分布,间距为1.5 m左右,梨树单行长25 m左右,试验环境如图8所示。

图8 梨园试验环境

试验采用Velodyne公司的VLP-16型激光雷达进行果园环境信息采集,数据的处理与分析使用Dongtintech公司的DTB-3049-H310型工控机,处理器为i7-9700,16 GB内存,500 GB固态硬盘,无显卡。

2.2 地面分割及里程计测程试验

首先进行地面分割的试验,试验结果如图9所示。可以看到原始R-GPF方法错误地将果树树干、棚架的竖直立柱部分和远处的竖直墙体识别为地面点,而改进R-GPF方法在避免以上问题的同时获得了更好的空旷地面分割效果,故改进R-GPF方法拥有更好的地面分割效果。

图9 梨园环境下地面分割结果

再在梨园中进行前端里程计试验,基于原始R-GPF方法的激光雷达里程计和基于改进R-GPF方法的激光雷达里程计的输出结果与实际轨迹的对比如图10所示,进行调头和弯曲行进的目的是检测里程计在场景变化较大的情况下的鲁棒性。针对实际轨迹的获取,由于果园上方存在金属棚架导致GPS信号接收较差,影响实际定位效果,故通过直接测量获取实际轨迹:沿激光雷达前进方向每隔50 cm左右选取一测量点,该测量点即为激光雷达实际位置,设定初始测量点为全局坐标系原点,通过卷尺测量初始测量点与下一测量点的相对坐标即可获得下一测量点的全局坐标,按照该方法遍历剩余测量点来获得所有测量点的全局坐标便可得到实际轨迹;再分析该实际轨迹的精度,每4个测量点选取一点直接测量该点在全局坐标系下的具体坐标,并计算该坐标与之前遍历所获得坐标的偏差,所有所选测量点在x和y方向上的偏差不超过7 cm,故可通过以上方法获取实际轨迹。相比于原始R-GPF方法的激光雷达里程计的平均输出频率10.40 Hz,改进R-GPF方法的平均输出频率可达到15.58 Hz,满足前端里程计的输出频率要求。

图10 梨园环境下里程计输出结果

再通过计算两种里程计的输出轨迹和实际轨迹的横向偏差来评价其测程性能,如图11所示,设定里程计输出轨迹在实际轨迹左侧时横向偏差为正值,在右侧时为负值。计算横向偏差的相关数据,原始R-GPF方法所对应的最大横向偏差接近50 cm,横向偏差均值为22.5 cm,标准差为24.7 cm;改进R-GPF方法所对应的最大横向偏差小于25 cm,横向偏差均值为12.7 cm,标准差为13.4 cm。可以明显看出,本文提出的改进激光雷达里程计在整个过程中都保持着较小且较为稳定的横向偏差,而原始激光雷达里程计的横向偏差变化幅度大且一直处于较高位。综合分析基于改进R-GPF方法的激光雷达里程计在更高频率的输出情况下仍能获得更好的测程效果。

图11 梨园环境下里程计横向偏差

2.3 地图构建试验及BAT启发式自适应重采样方法有效性分析

由于原始Gmapping算法前端里程计采用的ICP方法累计误差大且果园环境复杂、场景大,所以原始Gmapping算法的前端里程计表现效果很差,故最终建图效果对比在基于原始R-GPF的Gmapping算法和基于改进Gmapping算法两者中进行,最终两种算法的建图效果如图12所示。由于梨园两侧地头分别为墙体和较宽的空旷道路,中间部分为3行交叉分布的梨树和棚架立柱,故图12最左侧为构建的墙体,最右侧无构建内容,中间部分为构建的梨树和棚架立柱。

图12 梨园环境下建图效果

先进行地图构建偏差的计算和对比,从每行的最右侧果树或棚架立柱开始分别向左计算列间距离偏差和向下计算行间距离偏差,最终共遍历51棵果树和棚架立柱,建图偏差对比如图13所示。可以看到原始地图构建算法所得到的梨树列间距离偏差在40 cm范围内,行间距离偏差均值为18.2 cm,标准差为9.3 cm。相比于原始地图构建算法波动较大的为列间距离偏差,改进地图构建算法的列间距离偏差始终保持在20 cm范围内,表现较好;同时在行间距离偏差方面改进地图构建算法表现更好,行间距离偏差均值为10.3 cm,标准差为6.3 cm;可以看到相比于原始地图构建算法,改进地图构建算法在列间距离偏差、行间距离偏差和行间距离偏差的标准差3个方面分别减小了50%、43.41%和32.26%;一般情况下地图更新步骤会进一步增加地图构建的误差,在此前提下改进地图构建算法的行间距离偏差的各项数据相比于里程计横向偏差的各项数据仍有进一步减小,这也可以从侧面反映出后端BAT启发式自适应重采样方法有效实现了对初始定位信息的进一步优化。

图13 梨园环境下建图偏差对比

再结合图12分析整体建图效果,当作业平台从第2、3行间沿逆时针方向调头来到第1、2行间时,场景变化较大,而原始地图构建算法地面分割鲁棒性较差导致前端里程计输出误差较大,且其后端优化部分对定位情况的再优化效果不明显导致左侧墙壁出现重叠和偏移,最终导致第1行出现较大偏移,第2行右侧出现重叠,建图效果较差;改进地图构建算法凭借精确和稳健的前端激光雷达里程计和后端优化重采样算法对定位的再优化获得较好的建图效果,可以满足后续重定位、导航等作业的精度要求。

3 结论

(1)为了获得高精度果园二维环境地图,首先通过改进R-GPF方法实现准确和快速的地面分割,进而获得高精度高频率的前端激光雷达里程计信息,再在后端优化部分提出了一种BAT启发式自适应重采样算法对激光雷达里程计信息进行合理程度的再优化获得更优的定位信息,最终获得精准的果园二维环境地图。

(2)在梨园中进行前端里程计试验,基于改进R-GPF方法的激光雷达里程计的平均输出频率可达到15.58 Hz,其对应的最大横向偏差小于25 cm,横向偏差均值为12.7 cm,标准差为13.4 cm,比基于原始R-GPF方法的激光雷达里程计表现更好,并且满足前端里程计的输出频率和精度要求。进行梨园环境地图构建,改进地图构建算法表现更好:列间距离偏差方面表现良好,始终保持在20 cm范围内,在行间距离偏差方面表现更优秀,行间距离偏差均值为10.3 cm,标准差为6.3 cm,建图精度可以满足后续重定位、导航等作业的精度要求,同时行间距离偏差侧面反映出后端BAT启发式自适应重采样方法的有效性。