果园喷药机器人的单目视觉导航定位算法研究

万现全， 王虎奇， 丛佩超， 肖宜轩， 赵曰炜， 陈熙来， 刘俊杰

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

柑橘作为我国两广地区的重要水果品种，种植面积广、产量大。传统柑橘喷药作业以人工为主，存在喷药效率低、成本高、环境污染大等缺点，因此，引入自主移动式机器人进行喷药作业意义重大[1]。机器人喷药的前提是能够实现自主导航，而其定位精度尤为关键，因此，该问题已成为当前农业机器人领域的热点问题[2]。

目前，果园作业机器人常用定位方式有卫星定位、激光雷达定位、视觉定位等。其中卫星定位技术最为成熟，已得到广泛应用。熊斌等基于北斗卫星导航系统实现施药机在果园的定位，平均跟踪误差不超过0.13 m[3]。刘兆朋等依靠GNSS实现喷雾机的自动定位导航，可满足水田、旱田环境的作业要求[4]。但柑橘果园中树叶遮挡严重，卫星信号易丢失，导致其定位失败。激光雷达抗干扰能力强，扫描速度快。李秋洁等采用二维激光雷达获取果园树干测量数据，拟合树行直线，将树行中心线作为导航路径[5]。刘伟洪等针对二维激光雷达无法应对复杂三维果园场景的问题，提出基于3D LiDAR的果园定位导航方法，具有可靠的稳定性[6]。然而，激光雷达价格昂贵，不能满足农业机器人经济性要求。视觉导航使用的设备简单、成本较低，在GNSS信号弱的地方也可以进行定位，是果园机器人的热点方向，Ji等最早提出基于Hough变换的图像处理方法实现农业机器人的导航定位[7]。孟庆宽等提出一种基于人工蜂群算法的视觉导航路径提取方法，解决了农业机器人导航基准线提取速度慢、检测精度低等问题[8]。张雄楚等提出一种红枣收获机在灰枣枣园进行视觉导航的路径检测算法，通过图像处理拟合导航路径[9]。毕松等利用实例分割神经网络估计果园机器人视觉导航行间位姿和果树位置[10]。上述研究应用场景不包含复杂柑橘果园场景，且均利用Hough变换或者最小二乘法通过作物或道路检测拟合导航路径，其导航线精度依赖于图像分割效果，但往往因为遮挡造成图像分割失败。视觉同时定位与建图(simultaneous localization and mapping，SLAM)技术可较好地处理复杂和动态场景，具有良好的应用前景，但针对果园场景的该方面研究较少，且定位精度不足[2]。

本研究针对果园喷药机器人视觉导航过程中定位精度低的问题，优化ORB-SLAM2系统特征点选取策略，设置描述子阈值，并优化图像金字塔层级，增强其尺度及旋转不变性，以保证农药喷洒机器人定位精度，通过TUM数据集和搭建模拟柑橘园环境验证不同工况下的算法性能，为实现喷药机器人自主定位提供算法基础。

1 视觉SLAM数学表示及相机模型构建

1.1 视觉SLAM数学表示

喷药机器人通过单目相机获取周围果园环境信息的过程为：离散时刻t=t1，t2，…，ti下，机器人在位置x1，x2，…，xi处观测果园环境中路标点q1，q2，…，qi(图1)。视觉SLAM算法处理图像数据最终实现机器人状态估计及周围环境重建[11]。

相机的运动、观测方程如下：

xi=f(xi-1,pi,wi)[12]；

(1)

zi,t=g(qt,xi,vi,t)[12]。

(2)

式中：xi表示机器人i时刻所在位置；pi为相机i时刻的测量数据；wi为测量噪声；zi，t为相机在xi处对路标点qt的观测数据；qt为相机观测到的路标点；vi，t为观测噪声。

1.2 单目相机模型构建

基于经济性考虑，本研究果园喷药机器人采用单目相机进行定位。单目相机可简化为针孔模型，用于描述世界坐标系下三维点映射到像素坐标系的过程(图2)。世界坐标系中点P(xw，yw，zw)经过相机光心投影后，在物理成像平面产生成像点P(x′，y′，z′)，坐标系转换涉及世界坐标系、像素坐标系、归一化坐标系及相机坐标系[13](图3)。

设物理成像平面到相机光心O的距离为焦距f，点P在相机坐标系中坐标为P(xc，yc，zc)，由△P′O′O与△PAO的相似关系得：

(3)

(4)

设像素坐标系在轴u、v分别缩放α、β倍，原点平移[cx，cy]后得归一化坐标系，则点P归一化坐标(x，y)与像素坐标(u，v)关系如公式(5)所示：

(5)

分别代入公式(3)、公式(4)，整理为矩阵形式，并令gx=αf，gy=βf，得

(6)

相机坐标系与世界坐标系间的转换可由相机位姿矩阵R、平移向量T表示，即：

(7)

联立(6)(7)方程，得到针孔模型中像素坐标系与世界坐标系的转换关系：

(8)

2 ORB-SLAM2算法

2.1 ORB-SLAM2算法整体流程

ORB-SLAM2是基于特征点法的实时SLAM系统，支持单目、双目、RGB-D相机3种模式[14]，采用3个并行线程：追踪、局部建图、回环检测实现移动式机器人的定位及建图功能，其算法流程见图4[11]。

其中，跟踪线程从图像中提取、追踪ORB特征后，根据帧间匹配初始化相机位姿，并跟踪、优化重建的局部地图；局部建图线程完成关键帧的插入，并剔除劣质关键帧，生成新地图点，进一步优化位姿；回环检测模块检测到回环时，根据该回环计算、消除漂移误差，并将相同地图点合并，最后进行位姿图优化，实现全局一致性[15]。

2.2 特征提取与追踪

特征提取、追踪是ORB-SLAM2系统的重要组成部分，其结果将直接影响算法定位的精度及建图效果[14]。算法以ORB为图像特征，由Oriented FAST关键点、BRIEF描述子组成(图5)[16]。算法仅比较局部像素的亮度大小，实现关键点提取，保证算法实时性。描述子采用二进制描述向量，利用汉明距离表示两特征点间的差异，提高图像匹配的实时性[17]。

ORB-SLAM2算法计算旋转前后特征点附近的图像灰度质心及特征点方向，实现特征的旋转不变性[18]。另外，算法构建图像金字塔，得到不同分辨率的图像，进行不同层的角点匹配，保证尺度不变性。但描述子数量较少时，特征点提取质量较差，会导致旋转不变性降低；图像金字塔层数有限，只能保证一定范围内的尺度不变性。

为提高特征提取质量，保证果园喷药机器人定位精度， 本研究基于ORB-SLAM2算法， 优化描述子提取阈值，剔除劣质特征点，保证特征匹配精度及关键帧质量，增强其旋转不变性，并且优化图像金字塔构造算法，增加下采样产生的图像层级，对每层图像进行高斯滤波，提高不同距离下特征匹配的精度。本文算法具体实现过程如下。

2.2.1 特征匹配 首先，利用opencv库imread函数检测图像中Oriented FAST角点的位置，实现过程为：(1)在图像中选取亮度为Ia的像素块A；(2)在以像素块A为中心、半径为r的圆上选取16个像素点，设置亮度阈值T0。若选取的圆上存在连续11个点的亮度值不在区间[Ia+T0，Ia-T0]之内，则像素块A为特征点；(3)对图像中所有像素均执行上述步骤，以筛选出优质特征点。

其次，依据角点位置计算BRIEF描述子，其提取数量阈值范围设置为[T1，T2]，剔除不足T1个描述子的关键点，提高特征点质量，且限制描述子数量低于T2，以降低SLAM系统所占算力。

最后，使用汉明距离对2幅图像的描述子进行匹配，并计算点对的距离极值。当描述子间汉明距离大于2倍上述极小值时，可认为出现误匹配，需利用运动估计算法将其剔除。

2.2.2 旋转不变性 利用灰度质心法计算图像灰度质心及其特征点方向。首先，计算特征点附近像素块A的矩：

(9)

其次，基于像素块的矩计算灰度质心：

(10)

最后，计算特征点方向：

(11)

式中：m为像素块的矩；u，v为像素块坐标；I(u，v)为像素坐标的灰度值；φ为特征点方向角。

2.2.3 尺度不变性 优化图像金字塔层级，提高果园环境下特征点的尺度不变性，增强特征匹配精度。算法将图像进行缩放，并利用opencv的copyMakeBorder复制图像并填充边缘。继而，对图像进行高斯滤波，利用opencv的pyrDown函数下采样形成图像金字塔(图6)，高斯金字塔计算过程如下：

Pi=Down(Pi-1)

。

(12)

式中：Pi、Pi-1分别为第i、i-1次下采样图像，Down表示下采样函数。

3 结果与分析

3.1 TUM数据集仿真分析

本试验利用TUM数据集分别运行原ORB-SLAM2系统及本研究优化算法，并使用测评工具EVO，对比数据集中真实位姿文件groundtruth.txt与2个SLAM算法的估计位姿文件KeyFrame.txt(图7)，得到绝对轨迹误差，以此评测上述2个SLAM算法的定位精度。

试验平台采用Ubuntu18.04系统、Inteli5-9600KF 3.70GHZ处理器、16 GB内存。在TUM数据集freiburg1_xyz(图8)上分别运行改进前后的算法各10次，得出相应的绝对轨迹误差，求取平均值，具体结果如表1所示。

从表1可知，本研究算法得到的绝对轨迹误差平均值各项指标均有所提高。其中：最大误差 0.092 610 m，较原ORB-SLAM2算法降低28.62%；平均误差0.067 311 m，降低15.06%；均方根误差0.068 630 m，降低16.44%。仿真结果表明，本研究优化算法定位精度较原ORB-SLAM2算法有较大优势。

表1 绝对轨迹误差平均值

3.2 果园定位模拟试验

本研究搭建虚拟柑橘园环境(图9)，在光照明、暗等多种工况条件下，将原ORB-SLAM2算法及本研究优化的算法分别应用于Spark移动机器人平台进行实物验证。Spark移动机器人搭载HIKVISION DS-E11 单目摄像头，IBM ThinkPad T460P笔记本(Intel core i7-6700HQ CPU)(图10)。

3.2.1 算法性能试验 本研究选取特征匹配数、初始化时间作为算法性能测试指标，在同一场景下对不同工况进行试验。在不同距离、角度条件下共进行20次特征点匹配试验，试验过程及结果如图11、图12所示。

由试验结果可知，在相同场景下本研究算法匹配到的特征点数量普遍高于原ORB-SLAM2算法，数量平均提升18.18%。

在光照明、暗条件下对同一场景进行40次初始化时间测试，统计其平均值。试验结果(图13)表明，2种算法在光照充足条件下表现更为优异，且2类算法在同一种工况下的平均初始化时间相差不到0.5 s。

3.2.2 喷药机器人果园定位试验 选取长度3.6 m的直线运行轨迹，在光照明、暗2种工况下分别对优化前后ORB-SLAM2算法进行20次试验。直线运动中部分关键帧坐标信息见表2。其中，tx、ty、tz为关键帧当前位置坐标，qx、qy、qz、qw为位姿四元数。由图14可知，本研究算法产生的关键帧略少，这是由于描述子优化阈值后，局部建图线程滤除特征不足的图像帧，图像关键帧选取的可靠性大大增强。分析20组试验数据，本研究算法在模拟果园环境下定位精度良好(图15)。由于单目视觉定位具有尺度不确定性，无法确定真实尺度[19]，在初始化时系统会固定尺度将单位设置为1，因此图中轨迹坐标值均为相对值。

表2 直线运动轨迹坐标

回环检测可消除果园喷药机器人运行中的累计误差，提高其定位精度[20]，因此本研究设计回环轨迹进行试验，试验结果(图16)表明， 2种工况下，多次回环轨迹的坐标基本重合，表明回环时定位效果良好。光照不足时，由于环境亮度较低且初始化时间过长，导致部分初始位姿产生漂移，但仍可有效绘制喷药机器人的位姿及轨迹。

综上可知，相较原ORB-SLAM2系统，本研究所提算法的绝对误差下降、定位精度提升。在模拟柑橘果园环境下，本研究算法特征匹配数量增多，初始化时间及关键帧数量接近原算法，并且在直线、回环运动中均可实现有效定位。

4 结论

本研究提出一种基于ORB-SLAM2的单目视觉定位算法，构建喷药机器人单目相机模型，优化图像特征点的提取、匹配策略，增强其尺度及旋转不变性，并通过TUM数据集和搭建模拟果园环境验证了算法在果园喷药机器人定位方面的有效性。

下一步工作将尝试解决SLAM系统初始化时间过长的问题，克服在光照不足工况下的位姿漂移问题。为解决单目相机的尺度不确定性，将尝试引入其他感知元件，借助多传感器融合思想，进一步提升果园喷药机器人的定位精度。