基于深度相机的苹果采摘机器人路径规划研究

王亮 赵德安 刘晓洋

摘 要：为了优化苹果采摘机器人采摘路径，在获得苹果树场景三维位置信息的基础上，提出一种具有多种地形损耗的A*算法。结合ToF（Time-of-Flight）深度相机和Hu不变矩获取苹果和不同障碍物的三维位置信息，建立存在果实和多种障碍物的二维地图。在二维地图上，利用具有不同地形损耗函数的A*算法进行仿真实验。改进后的A*算法将障碍物分为可通过的障碍物（树叶）和不可通过的障碍物（树枝），且障碍物存在位置处的自带移动耗费向周围以线性递减，避免了基本的A*算法中只具有单种障碍物问题，从而增加了不同种类障碍物对路径选择的影响程度，优化了路径质量。对比实验表明：改进后的算法提高了对于复杂地图的处理能力，产生的路径长度更短，转折次数更少。

关键词：苹果采摘;ToF;Hu不变矩;A*算法;路径规划

DOI：10. 11907/rjdk. 182257

中图分类号：TP301文献标识码：A文章编号：1672-7800（2019）003-0001-06

0 引言

中国是世界上最大的苹果生产国。随着我国城镇化步伐的加快，大量青年进城务工，导致农村劳动力短缺，不能满足苹果采摘对劳动力的需求，苹果人工采摘还存在作业量大和效率低下问题[1]，农业生产方式向自动化智能化转变迫在眉睫。

使用机器人采摘果实最先由Schertz & Brown[2]在20世纪60年代提出，之后采摘机器人经历了50多年的发展，从最早期的机器人采摘果实需要数十秒到如今采摘果实在10秒以内。国内采摘机器人发展起步较晚，采摘果实的快速性与实时性不能满足要求。为提高机器人采摘苹果效率，必须解决苹果图像处理和采摘路径规划两大难题。江苏大学2011年研制的苹果采摘机器人，采用单目USB 摄像头，图像处理程序用C++编写，图像处理算法基于苹果在RGB空间的颜色特征，平均采摘一个苹果的时间为15s[3-4]。由于没有使用路径规划算法，导致采摘路径出现重叠，采摘时间过长，不能胜任野外采摘工作。

相关研究主要有：吕继东等[5]首先对苹果果实进行OTSU动态阈值分割，之后利用采集图像之间的信息关联性缩小目标果实处理区域，并采用快速去均值归一化积相关算法跟踪识别目标果实。司永胜等[6]先使用红绿色差法分割出苹果并提取圆心和半径，通过建立基于面积特征与极线几何相结合的匹配策略，实现双目视觉下的果实匹配与定位。以上两个研究仅对普通的RGB图像进行算法处理，复杂度较高。苑严伟、张小超等[7]将苹果采摘的路径规划问题转化为三维旅行商问题进行求解，结合双目摄像头获得的苹果位置信息提出了改进的蚁群算法，解决了局部收敛问题，提高了采摘效率。但该算法没有考虑到实际采摘情况下的障碍物问题，对于现实情况下果树生长的复杂环境适用性不是很高。比利时学者 Tien Thanh Nguyen利用 Kinect 相机提供的颜色信息和三维形状信息，基于点云PCL（PointCloud Library），苹果识别率高，定位精度在10mm以下，同步识别20个苹果的图像处理时间在1s以内[8]。该研究利用深度相机识别和定位苹果，速度很快，但没有从采摘路径规划方面提高苹果采摘效率。

针对以上研究存在的问题，本文选择ToF深度相机作为采摘摄像头，快速获取整个苹果树的多种类型图像，从三维点云图像和深度图像中准确定位多个成熟苹果的三维位置，并且筛选苹果所处范围内的障碍物，将三维视图转化为二维图像，形成存在多种障碍物的二维地图。根据苹果采摘的实际情况，改进A*算法中的地形移动损耗，在具有多种障碍物的二维地图上进行路径规划。编写路径规划的算法代码，对改进后的A*算法在路径规划中的效果进行验证，从而在有限时间内达到路径距离最短、规划速度最快的采摘要求。本文从图像处理和采摘路径规划两个方面提高了苹果采摘效率。

1 三維信息获取

1.1 Balser ToF工作原理

Balser ToF（Time-of-Flight）相机是德国Balser公司生产的一种工业3D相机。相机自带的传感器发出经调制的近红外光，遇物体后反射，传感器通过计算光线发射和反射时间差或相位差计算被拍摄景物的距离，产生深度信息。再结合传统的相机拍摄，就能将物体的三维轮廓以不同颜色代表不同距离的地形图方式呈现。原理如图1所示，实物如图2所示。

本文先利用ToF相机对苹果树整体场景进行采集，同时获得整个场景的强度图、置信图、深度图、三维点云图像。强度图代表整个场景接收到的反射光强弱，如图3（a）所示。置信图代表整个场景的每一个点深度的可信度，如图3（b）所示。图中点的亮度越大，则该点的深度距离准确度越高。深度图表示了场景中的深度信息，如图3（c）所示，越靠近相机，颜色显示越深，若显示黑色，表示该点距离不可测。三维点云图像是场景中点云数据的集合，如图3（d）所示。

1.2 苹果形状特征获取

由于Basler的ToF相机获得图像中并没有RGB图像，通过在强度图和三维图中分析苹果和障碍物可以发现，苹果和障碍物在图像中颜色上是相近的，因此利用颜色特征确定苹果位置是不准确的，但是苹果反射的光强和障碍物树枝树叶返回的光强不一样，导致在图像中有着明显区别。因此本文先对场景图像在像素方面进行预处理，RGB值大于150设置为白色，小于150设置为黑色（如图4所示），然后在预处理的图像上对苹果形状特征进行提取。场景的形状特征提取采用平移不变形、旋转不变性、缩放不变性的Hu不变矩方法[8]。

1.2.1 Hu不变矩提取苹果图像特征

矩特性主要表征了图像区域的几何特征，又称为几何矩。因为其存在旋转、平移等多个不变特征，所以又称其为不变矩。在图像处置中，不变矩能够描述图像的整体性质，在边缘提取、图像匹配及目标识别中得到广泛应用[9]。

Hu不变矩概念由Hu[10]提出，阐述了Hu不变矩中连续函数矩的定义和矩的基本性质，并给出了由二阶和三阶归一化中心矩的非线性组合构成的7个量值，并证明了这些矩具有平移、比例、旋转不变性，后被人们称为Hu不变矩。

Hu不变矩对分割后的图像中的目标连通域进行形状特征提取。设图像中的目标连通域上的某一像素点位置为[（x，y）]，则整个连通域的（[p+q]）阶矩为：

在对图片中物体的实际识别过程中发现，只有一阶矩到四阶矩保持较好，其它几个不变矩带来的误差较大，所以只选择前4个矩进行计算。表1是任意选取的3幅苹果目标区域和从图中截取的叶子的不变矩特征值。从表中可以看出不变矩值整体呈现递减趋势，苹果和叶子的不变矩值差异明显，即不同目标区域的Hu不变矩值是不同的，因此采用不变矩值获取目标物的形状特征可行。

因为苹果本身纹理特征并不复杂，并且形状特征和周边障碍物区别较大，所以利用Hu不变矩识别苹果的轮廓速度很快，效果很好。识别结果如图5所示。

1.3 果实与障碍物三维位置获取

1.3.1 果实三维位置获取

在确定了苹果在图像中的二维位置后，再利用Tof相机的深度图获取苹果的深度信息。为了得到苹果的準确位置，在深度图上对应苹果区域以苹果为中心随机选取5个点作为预选的苹果三维数据，然后对这5个点求平均值，得到苹果中心点的最终三维位置。

在Tof显示图像中，图像中心点的二维位置是（0，0），x轴坐标值随着方向向右增大，y轴坐标值随着方向向下增大。表2展示从左到右苹果的三维位置信息（苹果3：从左往右时，图像上偏下部的苹果。苹果4：从左往右时，图像上偏上部的苹果，坐标单位为dm）。

1.3.2 附近障碍物三维位置获取

以上节得到的苹果位置作为中心，选取边长13cm的正方形作为苹果和障碍物共存的区域位置，选取边长8cm的正方形区域作为苹果存在区域（实验室所用塑料苹果的宽度在8～10cm左右），在三重构图和强度图上找寻预测的障碍物区域内亮度和苹果亮度有明显区别的点，利用找寻到的点坐标，在深度图上找到对应的深度位置，若深度位置与苹果的深度位置在±8cm范围内，就认定以该点为苹果附近障碍物的位置。

对于找到的障碍物，针对树枝和树叶的亮度不一样设置阈值，当亮度阈值大于85小于100时视为树叶，其余的障碍物视为树枝。图6为障碍物树叶的位置。

由于树枝的亮度和环境背景的亮度相似，所以并不能识别出树枝，只要认定苹果附近存在障碍物，通过亮度判断是否为可通过的树叶，若不为树叶，则直接判定为不可通过的障碍物树枝。表3表示不同障碍物的位置信息。不同障碍物的位置如图7所示，单位为dm。

2 采摘路径最优化方法

2.1 障碍栅格法模型建立

本文在二维空间进行路径规划，用栅格法表示存在障碍物的地图，该方法起源于美国CMU大学。栅格法的基本思想是：将规划空间划分成等面积的小区，每个小区称为一个栅格，栅格面积大小一般由所描述的问题及所要求的搜索精度决定[12]。本文选取8cm长度作为栅格的边长，因为本实验所选用的塑料苹果横向平均长度在8cm左右。图8显示一个规划空间的栅格划分情况。每个栅格上的运动信息规定了机器人在这个栅格上的运动扩展方向，它们分别是前、后、左、右、右前、右后、左后、左前，如图9所示[13]。

通过栅格法得到的地图可把搜索区域简化成一个二维数组，数组的每一个元素是网络的一个方块，方块被标记成完全可通过（无任何障碍）、可通过障碍（树叶）、不可通过障碍（树枝）和苹果。利用苹果和不同障碍物的相对位置，初始化图中的方格，A代表苹果在图中所处位置，B代表不可通过的障碍物树枝的位置，L代表可通过障碍物树叶的位置，如图10所示。

2.2 基于A*算法的路径启发式搜索

对规划空间建立必要的空间障碍物，以及采用合理的数据结构对规划空间中的自由区域和障碍物区域进行描述，无碰撞路径规划就是选取某种有效的搜索方法，搜索出一条经过所有苹果且移动耗费总和最小的无碰撞路径[14]。

路径规划常见的启发式优先搜索包括局部择优搜索、全局择优搜索、A*算法以及近几年发展起来的人工神经网络搜索、遗传算法搜索等[15]，这些算法都使用了启发函数，但在具体选取最佳搜索节点时策略不同。局部择优搜索法是在搜索过程中选取“最佳节点”后，舍弃其它的兄弟、父亲节点，然后一直搜索下去。这种搜索结果由于舍弃了其它节点，可能也把最好的节点舍弃了，求解的最佳节点只是在该阶段的最佳并不一定是全局最佳。最好优先搜索没有舍弃节点（除非该节点是死节点），在每一步的估价中都把当前的节点和以前的节点估价值比较，得到一个“最佳的节点”，这样可有效防止“最佳节点”丢失。在最好优先搜索算法基础加上一些约束条件，就是本文采用的A*算法[16-20]。

A*算法的估价函数是F（n）=G（n）+H（n）。这里，F（n）是估价函数，G（n）是起点到当前指定方格的移动耗费，H（n）是当前点到指定目标方格的预估耗费。

在A*算法中，令水平或垂直方向移动一格的耗费为方格的边长，沿对角线移动耗费是沿水平或垂直移动的1.414倍。之所以选取近似小数，是因为比例基本正确，避免了求根运算，从而加快代码的运算速度[20]。对于完全可通过方格，方格自带的移动耗费是0，对于存在可通过的障碍物（树叶）方格，方格的自带移动耗费为方格边长的一半。然后以该节点为中心，向水平和垂直方向依次递减1/2。而对于不可通过的障碍物（树枝）方格，并不能够到达该节点，所以不会计算到该节点的移动耗费，并且邻近节点进行移动时不能通过其顶点。图11显示了各个方格自带的移动耗费。

事先是没有办法知道路径的长度和路径上是否具有障碍物的，所以使用曼哈顿方法直接计算H值[21]。曼哈顿方法是一种计算城市中一个地方到另一个地方的街区数的方法。该方法只计算沿水平方向和竖直方向的方格数，不能沿对角线穿过方格。并且在计算H值时忽略一切障碍物，仅仅对剩余的距离进行一个估算而非实际值。