一种串型水果分布式视觉主动感知方法及其应用与流程

本发明涉及水果智能采摘领域，特别涉及一种串型水果分布式视觉主动感知方法及其应用。

背景技术：

我国是龙眼、葡萄、荔枝等串型水果的主要生产国，种植和生产总量位居世界前列，具有巨大的经济潜力。但目前这些串型水果的收获基本依靠人工采摘，劳动强度大，采摘费用高。人工采摘的过程包括肉眼判断水果成熟度和品质，然后结合工具进行手工采摘。有研究表明，在整个水果种植生产过程中，采摘水果所耗费的劳动力占水果种植劳动力总量的35％～45％。随着果园发展的规范化、规模化，人工采摘成本会变得更高。近年来，随着智能机器人的发展，采摘机器人的研究和应用越来越多，给果园管理和水果采摘带来一定的效益。水果采摘机器人在采摘水果之前，需要先将机器人人工或自动移动到果树前，然后通过视觉传感器将水果从复杂的果园环境中识别出来，再根据水果空间信息进行水果的定位、选择和采摘。目前水果采摘机器人的研究重点主要是单个水果的识别采摘，比如苹果、猕猴桃和草莓等，而对串型水果的识别方法较少，因此迫切需要开展串型水果采摘机器人的相关研究。

目前，水果采摘机器人的视觉系统主要是单一集中式系统，在同一时刻，视觉传感器仅获取采摘对象及其狭小空间范围的视觉信息，这些视觉信息可以辅助机器人近距离摘落水果，但不能帮助机器人获取果树大范围的水果分布信息，难以对水果采摘顺序进行规划，因此整体采摘效率较低。

水果采摘机器人获取水果采摘点信息的传感器主要包括二维的单目视觉、多目视觉、三维kinect、红外设备和激光设备等；水果具体采摘点的识别是单次规划的，即水果采摘机器人移动到目标水果前，根据此时的视觉信息确定采摘点和采摘任务。如果水果和采摘点干扰较少，则采摘成功；如果干扰较大，则可能导致采摘任务失败，即受复杂的野外采摘环境、视觉系统限制、角度、遮挡和光照条件变化等因素影响，采摘机器人的小视场视觉系统容易丢失采摘目标，从而导致采摘任务失败。因此，如何重新确定最佳的采摘角度、位置和采摘姿态，成为水果智能采摘领域亟需解决的重要问题。

技术实现要素：

本发明的目的在于克服现有技术中存在的缺点，提供一种串型水果分布式视觉主动感知方法，可以实现采摘场景下大视场目标感知与小视场采摘决策的联合协作，有效解决采摘作业信息决策不足和机械手作业活动冗余的问题，提高串型水果识别率，有效降低采摘遮挡或重叠水果的不确定性，提高串型水果的采摘效率。

本发明的另一目的在于提供一种串型水果采摘机器人，采用上述串型水果分布式视觉主动感知方法进行串型水果采摘。

本发明的目的通过下述技术方案实现：

一种串型水果分布式视觉主动感知方法，包括下述步骤：

(1)确定多类视觉传感器信息的耦合关系，布置分布式视觉方案；

(2)采用大视场相机获取果树的大视场图像，以进行大视场目标感知：即利用实例分割网络(maskr-cnn)对大视场图像中的串型水果目标进行检测和识别，获得串型水果的空间信息，利用视觉显著性算法评估大视场图像中的串型水果目标的品质状态，然后根据串型水果目标的空间信息和品质状态进行多目标采摘顺序规划，得到各个串型水果目标的采摘优先级，同时确定二次感知的各采摘预备点；

(3)驱动机械手到达一个采摘预备点后，小视场相机随着机械手运动，近距离感知和观察串型水果，获取动态的小视场图像，以进行二次感知：结合智能推理与知识建模方法建立视觉与机械手关联的运动控制模型，实现小视场目标的主动感知与采摘智能决策，即机械手通过自主学习和探索到达采摘串型水果的最优视角，小视场相机在最优视角处再次获取图像进行姿态估计，并计算采摘点的位置，机械手运动至采摘点的位置并执行采摘动作，将采摘完成的串型水果放入收集装置；

(4)驱动机械手到达另一个采摘预备点，重复执行步骤(3)，直到所有采摘预备点的串型水果目标采摘完毕。

一种串型水果采摘机器人，包括机械手、移动平台、大视场相机、小视场相机和控制系统；大视场相机竖直固定在移动平台上，且位于机械手的一侧，其相对于机器人基点的位置是固定的；小视场相机固定在机械手末端执行器的下方，其相对于机器人基点的位置是随末端执行器一起变化；是采用所述的串型水果分布式视觉主动感知方法进行串型水果采摘。

所述大视场相机和小视场相机采用kinect相机；其中，大视场相机是kinectv2，小视场相机是kinectdk。每个kinect相机均包含两个视觉传感器，即彩色视觉传感器和深度视觉传感器。

步骤(1)中，所述确定多类视觉传感器信息的耦合关系，包括大视场相机和小视场相机的空间映射关系，以及相机内部视觉传感器之间的变换矩阵。首先对kinect相机中的彩色视觉传感器和深度视觉传感器进行标定，得到深度视觉传感器到彩色视觉传感器的外参数矩阵tdepth2rgb，再结合深度图、深度视觉传感器内参数kdepth和彩色视觉传感器的内参数krgb，得到深度图像素点所对应的彩色图像素点，获得深度图对应的彩色映射图，公式如下：

其中：

式中，zrgb：彩色视觉传感器像素点的对应的深度值

prgb：彩色视觉传感器像素点的像素坐标，要求解的目标值

zdepth：深度视觉传感器像素点的对应的深度值

pdepth：深度视觉传感器像素点的像素坐标

krgb：单目标定时获得的彩色视觉传感器内参数

kdepth：单目标定时获得的深度视觉传感器内参数

rrgb：两个视觉传感器(即彩色视觉传感器和深度视觉传感器)对同一幅棋盘格计算获得的彩色视觉传感器的旋转矩阵

rdepth：两个视觉传感器对同一幅棋盘格计算获得的深度视觉传感器的旋转矩阵

trgb：两个视觉传感器对同一幅棋盘格计算获得的彩色视觉传感器的平移矩阵

tdepth：两个视觉传感器对同一幅棋盘格计算获得的深度视觉传感器的平移矩阵

r：最终的旋转矩阵

t：最终的平移矩阵

rdepth2rgb：深度传感器到彩色传感器的旋转矩阵

rdepth2rgb：深度传感器到彩色传感器的平移矩阵。

步骤(1)中，所述布置分布式视觉方案，是指根据自然环境图像采集与分析实验，设计大视场相机和小视场相机结合的分布式视觉方案，进而确定大视场相机、小视场相机与机械手的空间坐标映射关系，建立关系模型。如图3所示，假设{b}为机器人基坐标系，{c1}为大视场相机坐标系，{c2}为小视场相机坐标系，{e}为末端执行器坐标系，{p}为基于大视场相机的采摘预备点坐标系，{h}为基于小视场相机的采摘点坐标系。

步骤(2)中，所述视觉显著性算法，即改进itti显著图模型，是提取多尺度亮度、方向、拮抗颜色对(r-g,b-y,cr-cb,s-h)等金字塔特征，在中等尺度上进行特征融合加权计算，得到像素级视觉显著图。

步骤(2)中，所述评估水果目标品质状态，是指大视场相机通过maskr-cnn识别的水果区域在显著图中匹配，获得各水果目标的视觉显著信息，然后计算各水果目标的平均显著度来量化完好水果的品质状态。

步骤(2)中，所述多目标采摘顺序规划，是对maskr-cnn识别的大视场图像中所有串型水果区域，采摘顺序规划模型收集各串水果的面积、距离信息和平均显著度数据，归一化之后映射到三维空间，将顺序规划问题转化为旅行商问题，利用遗传算法定义遗传算法的适应度函数，在显著度-面积-深度距离空间下迭代搜索最优采摘顺序路径，为每个水果分配采摘优先级，最终得到串型水果的采摘顺序并在大视场图像中标记出序号，进而引导机械手有序采摘树上串型水果目标。

步骤(2)中，所述确定采摘预备点，是取串型水果区域中心点作为水果点，结合大视场相机所获得的深度图计算出水果点的三维世界坐标，将水果点坐标的深度减去x厘米(x为根据实验确定的最佳距离，一般为：5～15cm)确定为预备点pbig。

步骤(3)中，所述的智能推理与知识建模方法建立视觉与机械手关联的运动控制模型，是建立用于近距离视图规划和位姿估计的小视场二次感知系统，实现小视场目标的主动感知与采摘智能决策。在对所述小视场目标进行主动感知与采摘智能决策之前，先对小视场相机获取的小视场图像进行自适应球状栅格化，使得机械手能够在串型水果目标的一定距离内运动，避免小视场相机寻找目标过程中离串型水果目标过近而影响串型水果采摘点的识别，因此通过对小视场相机的数据分析建立关系函数g，基于该关系函数g与机械手采摘域信息进行小视场相机所获取图像的自适应球状栅格划分。假设关系函数g为：

g(xc,yc,zc)＝h×a^-1×n

其中xc,yc,zc为小视场相机坐标系{c}中某点的坐标，h设定为小视场相机与串型水果目标的深度距离，矩阵a包含了小视场相机的内参参数(f，dx，dy，u0，v0)，其中f是小视场相机的焦距，dx和dy是小视场相机拍摄图像的x、y方向的单位像素物理大小，u0和v0是图像坐标系的原点位置；n＝[uv1]^t，其中u和v是自适应栅格划分后的栅格中心所在像素点位置。拟定的小视场视觉图像的自适应栅格划分示意图如图3所示。

为了使得自适应划分栅格引导机械手在目标左右和上下移动的同时，也满足机械手在串型水果目标的侧后方观察目标需求，拟定自适应球状栅格在相应像素点上所对应的深度距离表示为：

其中，r为根据视觉实验确定的球状栅格半径，与视场大小相关；px和py为图像中心像素点的位置，pz＝h+r，l(x,y,z)是机械手的有效工作范围，因此结合关系函数g和深度距离z球进行小视场从视觉的自适应球状栅格划分。

步骤(3)中，所述小视场目标的主动感知与采摘智能决策，是指进行所述图像自适应球状栅格化后，结合ppo强化学习算法，对栅格图像建立目标函数：f(x)＝w1*p(x)+w2*m(q(x))，式中，权重值w1+w2＝1，x∈r3表示机械手末端执行器的三维位置向量，p为场景中检测到的串型水果的归一化大小；为经典的机械手可操作性得分指标，其中q(x)为机械手在x状态下时的各关节角，det(j(q))为雅可比行列式。

步骤(3)中，所述机械手通过自主学习和探索到达采摘串型水果的最优视角，是指为了得到目标函数最优解，进而构建最优视角，是采用虚拟实验的方法，在仿真系统内建立ppo强化学习网络，其示意图如图6所示。首先，根据机械手各转轴运动特点建立仿真机械手随机运动模型，当机械手末端执行器移动到小视域栅格图像中的下一观察位置时，目标函数的值发生改变，为了使串型水果目标的信息最大化，即最大化目标函数，建立对应奖赏机制；通过随机运动探寻不同观察位置不同的目标函数值，更新最大目标函数maxf(x)的值，即f(xi)>maxf(x)时，maxf(x)＝f(xi)，终止回合并根据maxf(x)大小给予奖励；同时根据时间的花销给予惩罚，惩罚指标按串型水果分布形态复杂程度和不同遮挡程度进行设置。具体为：

rpublish＝rrestart-k1f1(x)-k2f2(x)

式中，rrestart为惩罚初始值，权重值k1+k2＝1，f1(x)为串型水果分布形态复杂程度归一化大小，f2(x)为不同遮挡程度归一化大小。在下一回合中机械手会随机探寻另一条道路以获取更高的maxf(x)，通过多次训练，得出目标函数最优解，最终得到机械手最优观测视角的姿态信息以及对应的采摘预备点。

步骤(3)中，所述进行二次感知，是首先通过手眼标定获得小视场相机到末端执行器的转换矩阵tsmall2end，该矩阵是固定的，获得一次即可，使用目标检测算法检测出小视场图像中的串型水果目标位置，选取水果预测框正上方的150*150像素的区域为结果母枝感兴趣区域，利用语义分割网络分割出果梗的像素，取其像素区域中心点作为采摘点，结合小视场相机获得的深度图计算出采摘点的三维世界坐标，结合所述手眼标定获得的小视场相机到末端执行器的转换矩阵tsmall2end，和当前末端执行器到基座标的转换矩阵tend2base，将采摘点转换到机器人基坐标下，控制机器人进行采摘，小视场坐标psmall到机器人基座标pbase的转换公式如下：pbase＝tend2base·tsmall2end·psmall。

步骤(3)中，所述结合智能推理与知识建模方法建立视觉与机械手关联的运动控制模型，首先是大视场相机求出各个串形水果区域在大视场相机坐标系下的预备点pbig并进行排序，结合所述手眼标定获得小视场相机到机器人基座标的转换矩阵tbig2base，将预备点由大视场相机坐标系转换到机器人基坐标系pbase，再结合小视场相机到末端执行器的转换矩阵tsmall2end，算出预备点坐标p预备点，将该坐标发送给机械手，即可使小视场相机到达预备点，进行小视场二次感知，过程公式如下：为了使末端执行器移动到采摘点执行采摘任务，假设机械手末端执行器相对于机器人基坐标系有如下变换关系：

t采摘点＝tend2base·tsmall2end·t采摘点2small

其中t采摘点2small为采摘点坐标系到小视场相机的变换矩阵。将t采摘点发送给机械手，即可使机械手末端执行器到达采摘点，完成水果采摘作业。

本发明与现有技术相比具有如下优点和效果：

(1)本发明通过分布式视觉方案，大视场相机确定大范围水果和品质判断，辅助采摘顺序规划；小视场相机确定具体采摘水果的姿态和位置信息，辅助具体采摘过程，减少采摘作业信息决策不足和机械手作业活动冗余的技术问题，有效提高串型水果的采摘效率。

(2)本发明通过分布式视觉主动感知方法来智能观察串型水果目标，机械手通过不断探索找到最适宜的采摘点，降低采摘遮挡或重叠水果的不确定性，切实解决单次采摘失败导致机器人终止任务的问题，提高采摘机器人的智能化与自动化水平，提高采摘机器人的工作效率和水果收获率。

附图说明

图1为本发明的串型水果采摘机器人的结构示意图。

图2为机械手的结构示意图。其中，1、基座；2、j1-j2模组；3、下臂；4、j3-j4模组；5、上臂；6、j5-j6模组；7、末端执行器。

图3为本发明的大视场相机、小视场相机与机械手的空间坐标映射关系图。

图4为本发明的maskr-cnn网络结构图。

图5为本发明的自适应栅格划分示意图。

图6为本发明的确定最优视角示意图。

图7为本发明的采摘流程图。

具体实施方式

下面将结合附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。需要说明，若本发明实施例中有涉及方向性指示(诸如上、下、左、右、前、后……)，则该方向性指示仅用于解释在某一特定姿态下各部件之间的相对位置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。另外，若本发明实施例中有涉及“第一”、“第二”等的描述，则该“第一”、“第二”等的描述仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。

实施例1

一种串型水果采摘机器人，如图1所示，包括机械手、移动平台、大视场相机、小视场相机和控制系统；大视场相机即kinectv2相机，竖直固定在移动平台上，且位于机械手的一侧，其相对于机器人基点的位置是固定的；小视场相机即kinectdk相机，固定在机械手末端执行器的下方，其相对于机器人基点的位置是随末端执行器一起变化；控制系统为内置有串型水果分布式视觉主动感知方法的嵌入式设备。

如图2所示，机械手的本体结构包括基座、j1-j2模组2、下臂3、j3-j4模组4、上臂5、j5-j6模组6、末端执行器7，这些部件依次相连接。

串型水果采摘机器人采用串型水果分布式视觉主动感知方法进行采摘工作。本实施例以荔枝为例进行说明，但本发明同样适用于其他串型水果。当大视场相机kinectv2获取得到场景图像后，由嵌入式设备运行改进itti模型计算视觉显著图，同步地，运行实例分割算法识别并检测多串荔枝，结合显著图得到各串荔枝区域显著值，采用tsp-ga算法进行采摘顺序规划，以每串荔枝的正前方5～15厘米作为采摘预备点，驱动机械手到达各预备点后，启用小视场相机kinectdk进行二次感知，近距离观察荔枝的遮挡情况，自主学习探索荔枝目标的最优采摘视角，进行姿态估计以及计算采摘点的位置，最后将采摘点发送到机械手，执行采摘动作，将采摘完成的荔枝放入收集装置中，再按照规划好的采摘顺序依次执行大小视场图像获取与采摘动作。

如图4和图5所示，具体实施步骤如下，如图7所示：

步骤1：从kinectv2采集得到的大视场图像，使用实例分割网络和改进itti模型识别荔枝串及计算显著信息，再根据识别结果，建模为旅行商问题求解模式，定义适应度函数，使用遗传算法计算得到荔枝串的采摘顺序{p1,p2,…,pn}。

具体地，本实施例中的实例分割网络为：maskr-cnn，其中损失函数为loss＝lbox+lcls+lmask,lbox为目标边框回归误差，lcls为目标类别预测误差，lmask为目标分割像素误差，网络结构图如图4所示。

具体地，本实施例中的改进itti模型为，对输入图像进行线性滤波，其三通道图像为r,g,b。提取亮度特征图i、超红算子r特征，超绿算子g特征、超黄算子y特征和超蓝算子b特征、ycbcr颜色空间下的cr特征和cb特征、hsv颜色空间下的s特征和h特征，gabor滤波在θ∈{0°,45°,90°,135°}四个角度下的方向特征[g1,g2,g3,g4]，形成13幅特征图。其中i＝(r+g+b)/3,r＝g-(r+b)/2,g＝g-(r+g)/2。

对以上13幅特征图生成九个尺度的高斯图像金字塔，得到117幅金字塔特征图，其中高斯核大小为5*5,且第一尺度是图像原尺寸。令尺度为c∈{2,3,4}的特征图为中心特征图，尺度为s＝c+δ(δ∈{3,4})的特征图为周围特征图。所有中心特征图与周围特征图均缩放至第五尺度，进行以下公式所述运算得到54副新特征图，表示图像减法运算。

对以上54副特征图进行归一化n(·)操作，根据以下公式融合特征图并生成视觉显著图saliency。

具体地，本实施例中使用的顺序规划模型(tsp-ga)，为遗传算法求解旅行商问题最短路径的方法。对每个水果的面积、平均显著度和空间信息(包含水果目标与末端执行器、水果目标之间的距离)归一化之后映射为三维空间，将采摘顺序规划问题转化为旅行商问题，采用遗传算法(本实施例以遗传算法为例进行说明，但其他优化算法同样适用于本发明)求解旅行商经过的最短路径，解得城市点序号即为各水果目标的采摘优先级。

其中，面积、显著度和距离信息归一化操作如下：

遗传算法的适应度函数为d(i,j)表示水果目标i与水果目标j在三维空间下的距离。

步骤2：根据采摘顺序，以每个目标区域前的10cm作为二次感知预备点，先到达第一个二次感知预备点p1，执行二次感知策略。

具体地，本实施例的二次感知策略为，首先从图5所示中的小视场相机获取小视域图像，对小视域图像进行图像自适应球状栅格化。

对于所述的图像自适应球状栅格化，具体实施方式为，建立深度距离与小视场相机视场的关系函数，基于该函数与机械手采摘域进行有效深度图像的自适应球状栅格划分。假设关系函数g为：

式中，h为设定的小视场相机与相机拍摄图像的深度距离，f是小视场相机的焦距，dx和dy是小视场相机拍摄图像的x,y方向每单位像素的物理大小，u0和v0是图像坐标系的原点位置，u和v是自适应球状栅格划分后的栅格中心所在像素点位置。但是该方法自适应划分后的栅格只能使机械手在目标左右和上下移动，不能往水果侧后方观察目标，因此设计了自适应球状栅格划分方法，在上述公式的基础上有：

结合式1.1，1.2，有

则此时自适应球状栅格在相应像素点上所对应的深度距离为：

其中，r为根据实验确定的球状栅格半径，px,py为图像中心像素点的位置，pz＝h+r,l(x,y,z)是机械手的有效工作范围。

因此建立了g函数和式1.1-1.4进行大视域的自适应球状栅格划分。

具体地，进行图像自适应球状栅格化后，结合ppo强化学习，对栅格图像建立目标函数：f(x)＝w1*p(x)+w2*m(q(x))，式中，权重值w1+w2＝1，x∈r3表示机器人末端执行器的三维位置向量，p为场景中检测到的串型水果的归一化大小；为经典的机械手可操作性得分指标，其中q(x)为机械手在x状态下时的各关节角，det(j(q))为雅可比行列式。

为了得到目标函数最优解，进而构建最优视角，采用虚拟实验的方法，在仿真系统内建立ppo强化学习网络，其示意图如图6所示。首先，根据机械手各转轴运动特点建立仿真机械手随机运动模型，当机械手末端执行器移动到小视域栅格图像中的下一观察位置时，目标函数的值发生改变，为了使串型水果的目标信息最大化，即最大化目标函数，建立对应奖赏机制。通过随机运动探寻不同观察位置不同的目标函数值，更新最大目标函数maxf(x)的值，即f(xi)>maxf(x)时，maxf(x)＝f(xi)，终止回合并根据maxf(x)大小给予奖励，示意图如图3。同时根据时间的花销给予惩罚，惩罚指标按串型水果分布形态复杂程度和不同程度遮挡进行设置。具体为：

rpublish＝rrestart-k1f1(x)-k2f2(x)

式中，rrestart为惩罚初始值，权重值k1+k2＝1，f1(x)为串型水果分布形态复杂程度归一化大小，f2(x)为不同遮挡程度归一化大小。在下一回合中机械手会随机探寻另一条道路以获取更高的maxf(x)，通过多次训练，得出目标函数最优解，最终得到机械手最优观测视角的姿态信息以及对应的采摘预备点。

步骤3：根据改进rrt路径规划方法，进行机械手运动至采摘预备点的路径规划。

具体地，改进的rrt路径规划方法，在传统方法的基础上，通过调节新节点扩展方向，以自适应的方式来改进rrt实现快速高效路径规划。假设自适应函数f为：

其中，k1为随机点方向的步长，单位是mm或(°)；k2为目标点方向的步长，单位是mm或(°)。prand是随机位姿；pnear是搜索树上离随机位姿最近的位姿；pgoal是目标位姿。针对不同环境，调整k1、k2实现自适应变化。则rrt方法中搜索步长为λ，新节点构造函数pnew实现自适应路径规划。

其中，引入遗传算法对rrt方法产生地初始路径进行优化，而适应度函数是影响遗传算法收敛速度及能否找到最优解的直接因素，因此将路径长度和机械手与障碍物最小距离引入适应度函数为：

c＝m/s+nd

式中，c为路径的整体评价值，s为路径长度，d为与障碍物的距离。m和n分别为路径长度和与障碍物距离的加权因子。通过改变加权因子m和n的值，可以得到不同重要程度的路径，然后进行反复迭代，完成路径优化。

步骤4：利用yolov3算法和unet算法进行采摘点识别，获取采摘点的位置坐标。

具体地，yolov3算法检测出水果的区域，取区域上方150x150的像素区域作为果梗的候选区域，用unet语义分割算法分割出果梗的像素，结合深度图去掉深度为0的像素，取剩余像素中心点，结合相机内参算出该点三维坐标作为采摘点。通过ros(roboticoperatingsystem)结合步骤3中的改进rrt路径规划方法对到达采摘点的路径进行规划。

步骤5：机械手根据运动规划结果执行采摘动作，完成采摘任务，继续执行接下来的{p2,…,pn}的采摘。

步骤6：n次顺序采摘执行完成后，重新捕获大视场图像，继续以上步骤。

以上所述仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是在本发明的构思下，利用本发明说明书及附图内容所作的等效结构变换，或直接/间接运用在其他相关的技术领域均包括在本发明的专利保护范围内。