果园监控机器人自主行驶及视觉导航系统研究

金 李， 高何璇， 高晓阳*， 李红岭， 杨 梅， 贾尚云 张 旭， 唐渲运

(1.甘肃农业大学机电工程学院，甘肃 兰州 730070；2.甘肃省葡萄与葡萄酒工程学重点实验室，甘肃 兰州 730070；3.甘肃省干旱生境作物学重点实验室，甘肃 兰州 730070；4.兰州银行网络金融部，甘肃 兰州 730000)

我国是园艺作物大国之一，水果生产具有良好的经济价值。目前大多数果园种植都使用人工巡检，人力资源耗费巨大。目前农村劳动力大量流失，研究开发果园监控机器人就显得十分必要[1，2]。随着国家大力推动农作物生产全程机械化，促进农业装备技术持续升级，迫切需要研究农业机械的自动行驶以及导航技术，以提高农业生产效率。

近年来世界各国在整地耕作、播种施肥、除草施药、采摘收获等作业领域的农业机器人研究越来越多，其实现自动行驶的关键在于导航技术，其中全球定位系统(GPS)、机器视觉导航[3，4]、惯性导航、地磁导航以及激光雷达导航[5]等都已经得到研究和应用。国外农业机器人的研究起步较早，MKPatel[6]等开发了农药喷洒机器人，将实际生产环境通过建图实现导航，完成机器人移动喷药。Ball[7]等研究开发了田间机器人的障碍识别和导航系统。JM Bengochea-Guevara[8]等设计了一款监控巡检机器人，通过相关传感器采集作物生长信息，采用导航技术对作业区域巡检。在国内，刘婷婷[9]等提出将电涡流传感器加入收割机的自动驾驶导航系统，通过传感器获取距离，实现导航功能。邢永友[10]提出将GPS技术加入农田导航，融合罗盘等传感器达到精准导航。药林桃[11]等设计了一款采用了RTK-GPS的自动导航系统，可以在作业区域内提供路径规划。赵荣阳[12]等设计了一种基于机器视觉的导航系统，搭载的高清摄像头采集工作区域数据建立地图，实现全局路径规划以及定位导航。袁红斌[13]等融合激光雷达与机器视觉技术，可准确识别障碍物并控制机器人跟踪导航。薛金林[14]等采用融合基于激光雷达的导航模式，满足果园田间机器人的运输和避障导航要求。

本文研究开发了一款适用于果园道路的机器人自主行驶及视觉导航系统，探索了自主行驶道路信息获取和路经规划，进行了视觉图像的分割识别和道路导航线的提取实验，并进行了软件仿真实验。

1 机器人平台构建

果园监控机器人平台如图1所示，其自主行驶及视觉导航系统包含Jeston nano主控制板、STM32底盘控制板、RPLIDAR S2激光雷达、Astra深度相机、惯性计量单元(IMU)等模块，可实现道路信息采集、路径规划、运动系统控制以及建图与导航避障功能。

图1 监控机器人平台

1.1 硬件结构及软件系统

考虑到果园道路相对平坦坑洼较少，开发的果园监控机器人平台的硬件参数如表1所示，以满足在运行平稳性、实际运行速度以及通过性等方面的实际要求。

表1 监控机器人平台的基本参数

机器人平台的行驶动力来自于后轮搭载的两个MD60直流有刷电机，减速器降低电机转速增加扭矩驱动机器人运动。驱动系统由一块22.2 V 20 000 mAh的电池提供电力。

机器人软件系统采用了基于LINUX的ROS[15]操作系统，PC端搭载Ubuntu系统，选用Python和C++编程语言。ROS系统的分布式进程利于开发使用，众多开发包可以实现田间机器人的自主行驶、路径规划以及导航避障功能。ROS系统支持分布式主从通信，以节点的方式运行，可以将实时定位以及导航等数据回传到PC，利用PC端Ubuntu系统实现数据处理并显示。

监控机器人平台的主控制器采用英伟达的Jeston nano开发板，搭载有NVIDIA Maxwell GPU，满足机器人自动行驶时的数据处理需求。通过搭载的视觉传感器采集道路信息，经过图像处理后提取出导航线实现机器人导航。依据搭载的激光雷达绘制的道路地图对道路上障碍物做出及时躲避。田间监控机器人总体框图如图2所示。

图2 监控机器人总体框图

1.2 底盘转向系统

本文机器人是基于曲柄摇杆的阿克曼转向结构，通过控制器发出偏转角度的PWM信号驱动舵机带动舵盘，舵盘再通过机械传动控制两个前轮进行转向。

转向系统包含转向舵机，舵盘以及相应的机械传动装置。根据机器人实际运行要求，数字舵机的扭矩选为8 N·m。

1.3 机器人的自动导航

机器人的自动导航选择采用视觉导航与激光雷达[16]导航相结合的双模式导航。由于全球卫星导航系统(GNSS)造价昂贵，并且导航精度容易受到干扰，尤其不适用于偏远地区。而机器视觉依靠色彩对比度来得到果园田间的边界点和线等信息，使用图像处理算法提取出机器人的导航线，更适用于路面信息相对简单的果园道路导航作业。加之激光导航的测量精度高，距离远，使用激光雷达可将周围复杂的道路信息预先建图保存，在机器视觉导航受到干扰的转角等路段，利用保存的路网信息修正机器人定位及导航。

激光导航系统采用激光雷达，测量半径达30 m，采样频率32 000次/s，精度误差低于±5 cm。视觉导航采用Astra系列深度相机采集果园道路图像，性能参数为1 280×720@30 FPS。

2 自主行驶的道路信息采集与路径规划

2.1 道路信息获取

果园面积较大，植株间距集中，机器人进入作业环境，需要在运动过程中不断提取周围物体的特征，并且将其在地图中绘制出来，在不断获取新地标的同时修正存储的地图模型，实现环境建图以及同步确定自身定位。因此，监控机器人首先采集道路地图信息，进而对道路行驶做出全局路径规划。本研究选用手动控制机器人平台在果园道路行进，利用配置的激光雷达以及视觉系统采用slam(Simultaneous Localization and Mapping，同步定位与地图构建)算法采集田间道路信息并建图保存。道路采集建图过程中将道路的点云信息提取，获得道路网络结构，并储存在控制系统。本文实验地为甘肃武威市威龙有机庄园种植基地，研究区域的路面信息如图3所示，该果园为典型的大面积果树种植，田间行驶道路经过人为修整，路面宽度2～2.5 m，坡度小于10°。

图3 研究区域路面信息

slam算法通常包含信息获取、前端、后端优化以及建图。后端优化分为基于滤波器和基于图优化。基于粒子滤波的Gmapping算法是将建图与定位分离，适用于面积较小且计算量较小的室内。而基于图优化的Cartographer算法依靠激光雷达获取数据，支持在二维或者三维下建图。Cartographer算法的原理框图如图4，包括数据获取、前端的局部slam和后端的全局slam。体素滤波器的基本原理是调用体素滤波，如果体素滤波后点数大于阈值，则返回，如果小于阈值，则接着使用二分法进行体素滤波。

图4 Cartographer原理框图

如图4所示，数据采集信息主要包括激光雷达信息、里程计信息、惯性测量单元信息等。激光雷达扫描点云数据通过体素滤波器降低采样率，并提取出有用的物体特征。惯性测量单元(Inertial Measurement Unit)数据与里程计数据进入姿态外推器，对行驶路线做运算，推演位置信息，将数据传输到扫描匹配环节匹配出最优位置。体素滤波器的滤波公式1为：

(1)

前端即局部路径规划，数据经过系列处理后进入前端，利用ceres库对接收到的数据做扫描匹配，将众多栅格点的概率进行双三次样条插值处理，建立如下公式2所示的方程，获得具有最高概率的位置地点。

(2)

Msmooth(x)为一个栅格函数x由临近的栅格值计算得到平滑函数值，hk代表包含激光扫描点的临近栅格，Tξ为向量转换矩阵，ξ为偏移向量(ξx，ξy，ξθ)T。通过前端优化以及运动滤波器滤波后，建立最优位置的子地图。子地图通过激光雷达逐帧扫描获得，扫描栅格可能占用所建地图中任一像素点。像素点更新公式如下式3和式4所示

(3)

其中p为发生的概率，odds(p)为比值。

Mnew(x)=clamp(odds-1(odds(Mold(x))
·odds(Phit)))

(4)

phit为未被观察到的像素点的分配的击中概率。采集的数据经前端处理再经回环检测及后端处理后建成全局地图。

2.2 全局路径规划

机器人在果园道路自主行驶时，在建立的全局地图范围内，选取一定数量的位置点后就能依靠全局路径规划[17]获取对应的行驶路线。机器人的路径规划使用ROS系统中的movebase功能包，该功能包包含传感器数据采集、局部路径规划以及全局路经规划，总框图如图5所示。

图5 Movebase框架图

利用全局代价地图来实现全局路径规划，本文采用耗时较少的深度优先的Astar算法，推算出目标点与机器人间的路经，满足机器人行进中的路径规划和不间断运行。

3 监控机器人的视觉导航

由于田间道路无车道辅助线、路面边界模糊，在图像分割及导航线提取时，这些不规则的图片将影响图像处理效率，首先需要通过试验挑选出符合实验标准的道路图片，对收集到的图片做统一化处理，像素设置为800*600；并根据道路是否弯曲和是否有阴影干扰分为常规道路、弯曲道路、干扰道路三类数据。本文选用将聚类分割算法与HSV空间阈值分割算法相融合的果园道路识别[18]方法，并提取道路中心点进行拟合获得机器人运动导航线。

3.1 RGB和HSV颜色空间

RGB颜色空间[19]是最为常见的颜色空间模型，以红、绿、蓝三元色作为基础，通过三种颜色叠加，形成丰富的颜色效果。为了能提高系统适应性，强化道路的特征信息，缩减图像处理的数据量，以常规道路为例，采用MATLAB软件将采集的道路图像提取RGB颜色空间下R、G、B分量图如图6所示。HSV颜色空间是一种生活中常见的颜色系统，包含色相(H)、饱和度(S)、色调(V)更加符合人们对于颜色的描述。

图6 RGB空间分量

3.2 聚类算法

K-Means聚类算法[20]，是一种常见的无监督学习方法，它是将相似的对象归到同一个簇中，使得同一个簇内的数据对象的相似性尽可能大，同时不在同一个簇中的数据对象的差异性也尽可能地大。即聚类后同一类的数据尽可能聚集到一起，不同数据尽量分离。K-Means聚类算法的簇数k需要实验确定，本研究针对常规道路、弯曲道路以及阴影道路进行不同k值试验，结果如图7所示，当K=4时对路面的分割效果最好。

图7 K-Means聚类分割

3.3 HSV空间阈值分割

利用聚类算法对道路信息进行分割，当聚类数K=4时，道路与周围环境差距明显，效果最好，但还未将有用的道路完全从图中剥离。因此，将RGB颜色空间转换为HSV颜色空间，通过阈值分割的方法将道路彻底分割。如图8为HSV颜色空间下不同分量图。

图8 HSV颜色空间分量

通过HSV颜色空间不同分量所示，V分量中道路信息的区分度明显高于H分量以及S分量。故本研究在对道路图像分割时，先将RGB颜色空间转成HSV颜色空间并提取其中V分量图像。图像阈值分割结果直观且容易实现，本文采用灰度阈值法获得合适的分割阈值来实现路面的最终分割。

图像阈值T的数学函数运算如下式：

T=T[x，y，p(x，y)，f(x，y)]

(5)

x，y分别代表像素点的横坐标和纵坐标，p(x，y)代表像素点的局部特性，f(x，y)代表像素点的灰度值。

经过阈值化的图像定义为：

(6)

通过对图像中所有像素点进行扫描匹配，根据像素点与设定阈值T的大小比较，将其标记为目标或背景实现图像的分割。道路图像的V分量转换成直方图如图9所示，利用双峰法[21]，当阈值K=30时，可实现完整分割，图10为V分量阈值分割图像。

图9 灰度直方图

图10 V分量图像

3.4 去噪

分割后的图片仍存在噪点，需要对图像进行形态学滤波[22]以及连通域处理，更好的把道路信息提取出来。形态学滤波中，使用7×7的结构元素一次对采集的道路信息进行开运算和闭运算。去除图像信息中的小孔，修补断裂的轮廓线。接着选择4邻接对图像检测标记，拟合描绘出大轮廓曲线，进而得到完整的道路图像。如图11为去噪后实际效果。

图11 去噪效果图

3.5 导航点提取

图像分割处理获得果园道路与环境背景分开的图像，但机器人实际运行时需要可量化的行驶导航线来指导行驶。由于Hough检测面对曲折道路时实时性差，本系统提出对机器人视觉摄像头采集区域进行分块处理，并求取中心点标记在图像中作为导航点，如图12所示。

图12 导航点提取图

xm=(∑mi，xi)/(∑mi)

(7)

ym=(∑mi，yi)/(∑mi)

(8)

其中：xi，yi是坐标点，m是坐标点数量。

4 建图与导航仿真实验

4.1 建图避障仿真

为了提高实际研究开发效率，采用Gazebo仿真工具对所构建的机器人平台进行试验仿真。为使测试系统接近实际环境，通过Gazebo软件建立仿真实验，设置果园监控机器人的URDF(统一机器人描述文件)，并且设置对应的惯性参数及各种属性，调节机器人平台的传动控制模型，调用软件中控制器模块。接着利用仿真软件中Build editor插件手动生成机器人运动模型，调用基于图优化的Cartographer算法文件，通过信息采集得到用于验证的地图模型。对系统中move_base功能包节点关系进行配置，指定机器人的初始位姿以及目标点位姿，最后启动系统。通过模拟传感器采集信息，构建环境与障碍物地图文件，实现路径自动规划与避障功能。仿真过程如图13所示。

图13 仿真过程

4.2 视觉导航实验

为了验证道路导航中线提取的有效性以及分析与实际道路信息的偏差，将常规道路、弯曲道路以及阴影道路三类道路图片传入MATLAB软件，在图像中的路面上绘制同等间距的分割线，如图14所示。通过中心点公式求取相应分割块中心点的虚拟坐标，并且利用相机标定把中心点虚拟坐标转变成实际屏幕坐标。将实际的道路中心点连成的线与软件拟合成的虚拟导航线对比分析偏差，如表2所示。

图14 路面分割图

表2 道路中心点拟合坐标与实际坐标的误差分析

续表2

由表2可知，拟合道路和实际道路偏差小于4%，精准度较好。

5 结果与讨论

(1)本文使用英伟达公司的Jeston 开发板为机器人主控制器，通过RPLIDAR S2激光雷达、视觉摄像头以及惯性测量单元采集信息，利用ROS分布式系统和基于图优化的Cartographer算法搭建了果园监控机器人的自主行驶系统和基于机器视觉的行驶导航系统。对机器人进行建图避障仿真及视觉道路导航线提取测试，结果表明该系统性能优良，可实现果园道路行驶和导航功能。

(2)提出了聚类算法和HSV颜色空间阈值分割算法相结合的果园田间道路分割方法，并采用形态学滤波及连通域处理分割后的图像噪点，获得完整的道路图像。

(3)通过机器视觉提取果园田间道路中心导航线。对摄像头采集的道路图像阈值分割后进行分块操作并标记中心点，利用多项式操作对中心点拟合提取导航线，实验仿真结果表明与实际道路信息偏差小于4%。