基于改进ORB-SLAM2的果园喷药机器人定位与稠密建图算法

丛佩超 崔利营 万现全 李佳星 刘俊杰 张 欣

(广西科技大学机械与汽车工程学院, 柳州 545616)

0 引言

当前,我国水果品类丰富、产量稳居世界第一,但其种植区域多分布在丘陵地区。受地形条件影响,果园的农药喷施还以人工操作为主,该方式存在成本高、效率低、危害身体健康、环境污染大等问题。为解决上述问题,用于除草、喷药、采摘等工作的果园机器人不断涌现,其相关技术的研究已成为机器人领域的热点问题[1-2]。

果园喷药机器人是将环境感知、目标识别、导航定位和运动控制等技术高度集成的智能化系统,精准定位技术是其中的关键问题之一。目前广泛使用的定位方法有:全球导航卫星系统(Global navigation satellite system, GNSS)、激光雷达定位、视觉定位[3-4]。室外场景下GNSS的应用最为广泛,其无遮挡情况下的定位精度可达厘米级[5-7]。虽然卫星定位在室外空旷环境下的表现良好,但在枝叶茂密的果园环境中,遮挡造成的卫星信号丢失,会严重影响定位精度[3]。激光雷达定位精度高、抗干扰能力强[8-9]。激光雷达虽能获得较高精度的位置信息,但在运动过程中易产生点云畸变和误匹配,且多线激光雷达价格较高[10]。视觉定位通过单目、双目和RGB-D相机不仅能够获得丰富的位置信息,而且其硬件成本低廉,功耗低、体积小、安装方便,适用于大范围的工作场景,被广泛地应用于移动机器人的导航问题[11]。文献[12-14]研究的视觉定位技术通常先由相机获取环境的目标信息,再通过图像分割等方法对目标图像进行处理,并利用Hough变换拟合导航路线,提高导航的定位精度。然而,果园喷药机器人的作业环境复杂多样,茂密的枝叶易导致图像分割失败,继而影响果园喷药机器人导航过程中的定位精度。视觉同时定位与建图(Simultaneous localization and mapping,SLAM)技术可较好地解决复杂场景下的定位与建图问题,已成为当前农业机器人领域的研究热点。

迄今为止,比较经典的视觉SLAM算法有MonoSLAM[15]、PTAM[16]、SVO[17]、ORB-SLAM[18]等。其中,MUR-ARTAL研究团队[18]提出的ORB-SLAM系统,利用单目相机完成了全自动初始化,其在室内、外环境下均可实时运行,且具有较好的鲁棒性,与当时其它单目SLAM算法相比,实现了前所未有的性能。之后,该团队[19]又提出了一种基于特征点法的ORB-SLAM2算法,它主要由跟踪、局部建图、回环检测三大线程组成,在大场景下实现了移动机器人的高精度定位与稀疏点云地图构建。李盛辉等[20]提出了一种基于全景相机的视觉SLAM算法,该方法可应用于真实农业环境下的智能车辆自主导航,其定位平均误差0.108 m。李月华等[21]提出了一种基于人工信标修正的视觉SLAM算法,该算法利用双目相机实现了工厂环境下自动引导车的自主定位,其定位误差不超过0.1 m。黎达等[22]提出了一种双目视觉SLAM算法,利用地面、树木等周围环境的纹理信息,可实时确定相机传感器的空间位置和姿态,其均方根误差不超过0.9 m。董蕊芳等[23]提出了一种改进型的ORB-SLAM2算法,通过融合环境的3D、2D占据信息、路标点空间位置以及视觉特征等信息,实现了轮式机器人在森林公园内的准确定位,其定位误差不超过1.43 m。上述视觉SLAM方法在农、林等复杂环境下的定位虽然取得了一定的效果,但相应的精度还有很大提升空间,特别是针对稠密果园环境的视觉定位和建图方法仍然比较欠缺[24]。因此,研究适用于果园喷药机器人的定位与稠密建图方法,具有重要的现实意义[25]。

基于以上分析,为进一步改善果园喷药应用场景下机器人定位与建图能力,本文提出一种改进的ORB-SLAM2定位与稠密建图算法,以ORB-SLAM2为基础,围绕图像像素亮度的差异性,引入噪声抑制机制与高斯滤波,优化ORB特征点的提取过程;借助局部建图线程的优质关键帧,运用点云滤波处理与位姿处理技术,构建稠密建图线程。

1 算法框架及视觉SLAM理论基础

1.1 算法框架

ORB-SLAM2算法是基于特征点法的实时SLAM开源算法,但其构建的三维稀疏点云地图仅保留图像的部分ORB特征点,且建立的地图表征不够充分,缺少占据信息,难以描绘地图中的障碍物,因此,ORB-SLAM2算法仅适用于机器人的空间定位[23]。此外,果园环境复杂多样、纹理缺失,也易造成果园农药喷洒机器人的视觉定位精度不足,无法满足路径规划等任务的需求[26]。针对上述问题,本文提出基于改进ORB-SLAM2算法的果园喷药机器人定位与稠密建图算法,如图1所示。该算法基于ORB-SLAM2算法框架,保留原有的局部建图与回环检测线程[19],在跟踪线程中优化FAST角点、描述子阈值,利用高斯滤波对图像进行处理,并采用图像下采样方法形成高斯图像金字塔,实现ORB特征提取过程的优化,以降低劣质特征点数量,提高算法的关键帧质量及特征匹配准确率,从而改善算法的定位精度。此外,本文算法增加稠密建图线程,将相机周围场景以三维稠密点云地图的形式呈现,实现稠密地图的构建功能,以备果园喷药机器人后续自主导航。

图1 基于改进ORB-SLAM2算法的视觉定位与稠密建图算法框架

1.2 视觉SLAM理论基础

为实现果园喷药机器人的高精度定位与三维稠密地图构建,需有丰富的图像信息作为保证。基于经济性考虑,本文采用Astra RGB-D相机。果园喷药机器人通过RGB-D相机获取环境信息,如图2所示,机器人在pose1、pose2、…、poseN处观测果园环境中路标点y1、y2、…、yn。通过视觉SLAM算法处理图像数据,根据相机的运动方程、观测方程估计相机位姿,再借助坐标系转换关系确定机器人的位置与姿态,以实现其自身的精准定位[27]。

图2 果园场景下的相机运动状态

RGB-D相机的运动方程为

xt=f(xt-1,pt,qt)

(1)

其观测方程为

zt,i=h(yi,xt,wt,i)

(2)

式中xt——相机t时刻位置

pt——测量t时刻数据

qt——测量t时刻噪声

wt,i——观测t时刻路标点噪声

yi——相机观测到的路标点

zt,i——相机在xt处对路标点yi的观测数据

Astra RGB-D相机由RGB彩色相机、红外传感器和接近传感器3部分组成,结构如图3所示。该相机通过搭载的光学相机和红外相机获取RGB图像,利用红外结构光原理获取目标图像对应的深度信息,为三维稠密地图重建提供丰富的三维场景点云数据,再由相机点云坐标系转换为全局点云坐标系,最终实现环境信息重建[28]。

图3 Astra RGB-D结构

点云坐标系变换关系式为

Wo,j=To,ciWci,j

(3)

式中To,ci——第i个关键帧位姿

Wci,j——第i个关键帧的坐标系点云

Wo,j——全局坐标系点云

2 同时定位与三维稠密地图构建

2.1 ORB特征点提取优化

ORB特征由Oriented FAST角点、BRIEF描述子组成,其不仅计算速率和精度较高,还具备旋转、光照和尺度的不变性,以及匹配速度快、实时性高等特点。因此,ORB特征点提取在算法中起着至关重要的作用,其结果将直接影响定位的精度及建图效果[29]。但ORB-SLAM2提取的特征点分布不均匀,且输出特征点重叠较多、质量较差。为解决该问题,本文提出一种如图4所示的特征点提取算法,其主要由5部分组成:FAST角点提取、BRIEF描述子计算、旋转角度计算、图像金字塔层数优化、高斯处理。

图4 特征点提取流程图

本文算法根据像素亮度不同进行FAST角点提取与描述子计算,引入噪声抑制机制,以优化ORB特征点的处理流程,如图5所示。首先,在图像中选取像素块P,假设其像素亮度为LP,并设置亮度阈值为A0。以P为中心,在半径为r的圆上选取16个像素点,若存在连续12个像素点的亮度不在区间[LP-A0,LP+A0]之内,则像素块P可列为优质特征点。对图中所有像素均重复以上步骤,筛选出图像中所有的优质特征点。其次,利用OpenCV库中的imread函数检测出Oriented FAST角点位置,依据角点位置计算BRIEF描述子,设置其提取数量阈值范围为[A1,A2],并在描述子阈值范围内选取关键点,且剔除范围以外的关键点,以提高SLAM系统特征点的质量。最后,采用OpenCV库中的BruteForceMatcher函数结合汉明距离对2幅图像的描述子进行匹配,计算相邻点之间的极小值距离,若描述子间汉明距离大于2倍极小值,判断其为误匹配。此时,运用随机采样一致性算法减少误匹配,抑制噪声,剔除外点,以提高系统定位精度。

图5 ORB特征点提取

ORB-SLAM2算法构建图像金字塔是对原图像进行多尺度结构表达。首先,按照图像比例因子进行逐级向下采样;其次,采样后的图像按不同分辨率由高到低向上排列,进行不同层的角点匹配,从而解决尺度不变性问题。但当描述子数量较少时,易出现特征点提取与匹配精度下降的问题[30]。为解决上述问题,通过优化图像金字塔层级,对图像进行高斯滤波,利用OpenCV的pyrDown函数形成高斯图像金字塔,如图6所示。高斯图像金字塔从第K层生成第K+1层,利用高斯滤波对第K层进行卷积,删除所有的偶数行与列,具体计算过程为

图6 高斯图像金字塔

图7 稠密建图流程图

FK=pyrDown(FK+1)

(4)

式中FK、FK+1——第K、K+1次下采样图像

pyrDown——下采样函数

所形成的高斯图像金字塔可用于构建图像的尺度,避免引入其它噪声,从而提高本文算法的特征点提取与匹配精度。

2.2 三维稠密地图构建

针对ORB-SLAM2所建立地图的表征不够充分、缺少占据信息问题,本文在ORB-SLAM2架构基础上添加稠密地图构建线程,如图1、7所示,该线程以局部建图线程的优质关键帧为基础,构建相机周围果园环境的三维稠密点云地图。首先,基于RGB-D相机的成像原理,如图8所示,运用点云恢复算法、统计滤波方法,将输入关键帧的2D像素坐标转换为3D点云坐标,从而形成像素点云队列[31]。其计算过程为

图8 针孔相机成像原理图

(5)

式中px、py——水平与垂直方向的相机焦距

qx、qy——像素坐标与成像平面的平移量

(x,y,z)——图像坐标系坐标

(u,v)——图像坐标

d——深度相机测量出的像素点距离

n——实际距离和测得距离d的比例系数

同时,运用位姿处理技术,将相机位姿生成位姿队列。其次,点云队列与位姿队列采取点云融合拼接算法,进行点云地图拼接,完成新旧点云地图叠加,点云拼接公式为

(6)

式中Rwc——3×3旋转矩阵

twc——3×1平移向量

由于高采样频率、运动速度的约束,相邻关键帧之间存在大量的冗余信息,导致系统运算速度较慢,本文通过体素滤波处理掉该冗余信息,以获得新的局部点云地图。最后,进行点云地图更新,输出三维稠密点云地图,并将稠密建图部分编写成独立的pointcloud_mapping功能包。生成的稠密点云地图可通过Octomap_Server转换成八叉树地图,为果园喷药机器人在复杂场景下的自主导航提供环境地图[31-33]。

稠密建图线程若单独运行,系统需重新进行特征点提取与匹配等工作,会产生大量重复计算。为减少稠密建图计算量,本文基于ORB-SLAM2系统的ROS节点进行改进。ORB-SLAM2系统主要分为3个节点:相机驱动节点、位姿估计节点、建图节点,如图9所示。相机驱动节点采集视觉传感器数据,位姿估计节点提供相机位姿(TCW),建图节点接收图像数据和位姿数据,进行点云拼接[34]。本文在ORB-SLAM2的建图节点中增加KeyFrame输出接口与位姿发布话题,通过TrackRGBD接口函数调用KeyFrameDataBase库,逐层添加关键帧状态标志位,利用Tracking::Track函数计算位姿,NeedNewKeyFrame函数选取ORB-SLAM2生成的优质关键帧,作为稠密建图线程的输入,从而减少处理帧数,节约运算资源,保证系统的实时性,关键帧选取过程如图10所示。

图9 基于ROS的ORB-SLAM2系统节点框架

图10 关键帧选取示意图

3 仿真与实验

为验证本文算法的有效性与实用性,分别进行数据集仿真与真实场景实验。仿真分析选用自带标准轨迹与比较工具的TUM数据集,将本文算法与ORB-SLAM2进行对比,验证其定位精度与稠密建图效果。果园真实场景选取广西壮族自治区柳州市某处柑橘种植场地,利用果园喷药机器人实体样机的移动底盘进行实物验证。上述仿真、真实场景实验的计算机采用Intel core i7-6700HQ处理器、32 GB运行内存的ThinkPad T460P型便携式计算机,其操作系统为Ubuntu 18.04。

3.1 数据集仿真分析

TUM数据集为Kinect视觉传感器采集的标准数据集,该数据集中包含彩色图、深度图及相机运动过程中的真实轨迹(groundtruth)[23]。仿真分析利用TUM数据集的freiburg1_room序列进行验证,该序列具有场景大、运动回环等特点,适于评估SLAM系统的位姿估计与回环检测能力。基于上述序列分别测试ORB-SLAM2及本文所提算法,其运行效果如图11所示。由图11可知,本文算法相较ORB-SLAM2可实时地构建三维稠密点云地图。

图11 运行效果对比

借助测评工具EVO对图11得到的运行结果进行处理,以评估上述两种SLAM算法的定位精度。首先,将两种SLAM算法运行后的估计位姿KeyFrame Trajectory与数据集真实位姿(groundtruth)进行对比,结果如图12所示;其次,分别获取两种SLAM算法对应的绝对轨迹误差(APE,图13)、相对轨迹误差(RPE,图14);最后,运行两种算法各15次,求取相应的绝对轨迹误差与相对轨迹误差平均值,结果如表1、2所示。其中,采用最大误差(max)、平均误差(mean)、误差中值(median)、最小误差(min)、均方根误差(RMSE)、方差(SSE)与标准差(STD)作为算法的评价指标。

表1 绝对轨迹误差平均值

表2 相对轨迹误差平均值

图12 真实位姿与估计位姿对比

图13 绝对轨迹误差对比

图14 相对轨迹误差对比

图12表明:本文算法相比ORB-SLAM2算法的估计位姿更接近真实位姿。观察图13、14可知,本文算法相较ORB-SLAM2的单帧误差波动、绝对和相对轨迹误差范围更小;此外,分析表1、2可知,本文算法的绝对和相对轨迹误差平均值均有所下降。其中,绝对轨迹平均误差0.043 612 m,降低44.01%;均方根误差0.048 419 m,降低43.26%;标准差0.021 032 m,降低46.68%。相对轨迹平均误差0.018 938 m,降低7.93%;均方根误差0.026 485 m,降低9.89%;标准差0.018 515 m,降低11.81%。数据集仿真结果表明:本文改进算法的估计位姿更接近真实位姿,定位精度也较ORB-SLAM2算法有明显提升。

3.2 果园喷药机器人定位与建图实验

为进一步验证本文算法在真实场景下的定位与稠密建图效果,以柑橘果园为应用场景,选取36 m×16 m的测试范围,如图15所示。将ORB-SLAM2及本文算法分别在果园喷药机器人实体样机的移动底盘上加以测试。移动底盘搭载Astra RGB-D相机,主控为ThinkPad T460P型便携式计算机,如图16所示。

图15 真实果园场景

图16 实验平台

3.2.1算法性能测试

在果园内的不同工况下进行多次实验,根据特征点匹配数量、算法初始化时间、关键帧匹配数量测试算法性能。

两种算法在运行过程中,通过调整机器人位置改变相机在环境中的位姿,每种算法进行20次特征点匹配实验,实验过程及结果如图17、18所示。实验结果表明,在相同场景下本文算法匹配到的特征点数量普遍高于ORB-SLAM2算法,特征点数量平均提升19.03%。

图17 特征点匹配实验(部分)

图18 特征点匹配数量结果

在晴天与阴雨天两种工况下对同一场景进行30次初始化时间与关键帧匹配数量测试,分别计算其平均值,结果如表3、4所示。实验表明,本文算法30次初始化时间的均值为15.19 s。在晴朗天气条件下初始化时间较短,两类算法同一工况下的平均初始化时间相差不到4.3 s。本文算法通过优化ORB特征点的提取过程,滤除劣质特征点,减少了关键帧数量,其平均值为105帧,保证了本文算法在两种工况下关键帧的平均值均低于ORB-SLAM2算法。

表3 初始化时间平均值

表4 关键帧匹配平均数量

3.2.2果园喷药机器人定位实验

通过实验平台直线运动与回环运动输出的结果,验证本文算法的定位精度。首先,在果园内选取一条长10 m、宽1.8 m的直线通道,道路两边种满果树;然后,实验平台以速度0.6 m/s沿红色直线喷药通道行驶,如图19所示;本文算法的运行效果如图20所示。

图19 实验平台直线行驶

图20 直线运动过程

上述测试过程中获取的实验数据使用EVO轨迹功能进行处理,直线运动中部分轨迹坐标信息如表5所示,其中,Tx、Ty、Tz为当前关键帧位置坐标,Qx、Qy、Qz、Qw为位姿四元数,整理后的实验结果如图21所示。

表5 直线运动轨迹坐标

图21 直线运动轨迹

通过观察图21直线运动轨迹与分析表5实验数据可知,本文通过优化跟踪线程中ORB特征点的提取过程,将劣质ORB特征滤除,不但提高了算法生成的关键帧质量,且增强了关键帧选取的可靠性与特征匹配的准确性,从而使本文算法的直线运动估计位姿较接近喷药机器人实际的直线运动轨迹。

利用回环检测消除果园喷药机器人运行过程中的累计误差,以提高其定位精度。在真实果园场景内选取长10 m、宽3.6 m的场地进行回环实验,实验过程与结果如图22、23所示。观察图23可知,通过喷药机器人在果园内的闭环运动,本文算法的估计轨迹坐标基本重合,具有良好的回环检测性能。虽然,室外地形复杂,光线不均会导致部分轨迹位姿产生漂移,但本文算法仍可有效绘制果园喷药机器人的位姿及轨迹,表明其在回环运动时的定位效果较为理想。通过上述实验分析可知,本文算法具有较好的定位精度和鲁棒性。

图22 回环运动过程

图23 回环运动轨迹

3.2.3果园喷药机器人稠密建图实验

在真实果园场景内选取两条直线喷药道路,场地长30 m、宽5.4 m。实验平台以速度0.6 m/s沿着喷药路径运动,利用本文算法进行实时三维稠密地图构建,得到的点云地图如图24a所示,将其与真实场景(图24b)进行对比,以验证本文算法的稠密建图效果。

图24 稠密地图构建

观察图24可知,本文算法构建出的三维稠密点云地图清晰地展现了真实果园场景内草地与果树的三维空间位置,且未出现重叠、变形等情况,与现实场景基本吻合。因此,本文算法具备较强的三维稠密点云地图构建能力。

综上可知,相较ORB-SLAM2算法,本文所提算法的绝对轨迹、相对轨迹误差均有所下降,定位精度也获得提升。在真实果园场景下,本文算法特征匹配数量增多,初始化时间接近ORB-SLAM2算法,且在直线运动与回环运动中均实现了有效定位,室外场景也能构建出有效的三维稠密点云地图,可为果园喷药机器人的导航定位打下坚实基础。

4 结论

(1)通过改进FAST角点、描述子阈值,利用图像金字塔法与高斯滤波算法优化图像特征点的提取过程,剔除劣质ORB特征点,提高了关键帧匹配的可靠性,并通过实验证明了本文算法在freiburg1_room序列中比ORB-SLAM2的运行轨迹效果更优,定位精度更高,其绝对轨迹的平均误差、均方根误差与相对轨迹的平均误差、均方根误差分别降低44.01%、43.26%、7.93%和9.89%。

(2)基于ORB-SLAM2算法框架引入稠密建图线程,构建RGB-D相机运动模型,利用点云恢复、点云拼接与滤波处理技术,实现了三维稠密点云地图构建;并在ORB-SLAM2算法的建图节点中引用NeedNewKeyFrame函数,选取局部建图线程所生成的优质关键帧图像,从而减少了系统计算量。在真实果园场景实验验证了本文算法的实际性能,实验结果证明本文算法的ORB特征点匹配数量平均提升19.03%,且关键帧数量的平均值为105帧,系统初始化时间的均值为15.19 s;此外,果园喷药机器人可通过本文算法在直线、回环运动中实现准确定位,并能获取机器人所处环境的三维场景信息,构建出的稠密点云地图可为果园喷药机器人的自主导航提供基础。