金玉成 高 杨 刘继展 胡春华 周 尧 李萍萍

(1.江苏大学现代农业装备与技术教育部重点实验室， 镇江 212013；2.南京林业大学信息科学技术学院， 南京 210037； 3.南京林业大学生物与环境学院， 南京 210037)

0 引言

“手-眼”协调是果蔬采摘机器人的主体和完成自主采摘作业的关键[1-4]，而植株冠层环境的高度复杂性、果实目标的随机性与差异性，对机器人手-眼系统“感知-动作”的智慧性提出了极大挑战[5]。“Eye-to-Hand”结构首先在采摘机器人领域得到了广泛研究并有力推动了采摘机器人技术的进步，但是外置相机远景探测造成图像内冗余信息过多、目标果实搜寻过程复杂，手眼及果叶相互遮挡问题突出，识别可靠性与视觉定位误差较大[6-7]；“先看后动”的静态割裂方式缺乏对农业非结构化复杂环境和动态变化的适应能力[8-13]；同时其依赖精确手-眼坐标系转换、机器人运动学建模和逆像问题求解，造成机械手误差累积而影响定位精度[2-3,14]。与之相比，“Eye-in-Hand”的“看并动”手眼协调模式中末端对目标的定位可实现持续的视觉反馈和修正，达到实时视觉伺服控制[4,10, 14-16]，因而在采摘机器人领域逐渐得到重视。日本、美国、比利时等国面向柑橘[17]、茄子[18-19]、番茄[20]、苹果[21]等开发的采摘机器人系统，都采用了掌心或腕部安装相机的“Eye-in-Hand”配置方案。针对“Eye-in-Hand”的手眼协调控制问题，VAN HENTEN 等[7, 22]提出通过掌上相机的位置变换获得两幅图像，进而3维重构获得果实的空间位置信息；BARTH 等[14]通过掌上相机以20 Hz持续检测果实并计算机械手位姿，驱动末端以速度0.03 m/s逼近目标果实，但试验仅实现了二维图像像素位置逼近，而无法实现深度(距离)定位，一个视觉伺服控制周期达到了45 s。瓦根宁大学的甜椒收获机器采用CCD相机在手模式，实验室检测与采摘成功率达97%和79%，但机械采放果路径过长，在运动规划和动作执行上耗费过多时间，单次作业时间高达106 s[23-24]。MEHTA等[25]研究发现末端的视觉伺服定位精度约为15 mm，仅能满足大中型果实的采摘定位需要，同时路径中枝干的存在会影响伺服控制性能甚至会造成末端执行器损坏，而测量噪声对基于高增益的反馈控制效果影响较大。

为实现复杂实际条件下的采摘机器人高效作业，基于RGB-D传感器进行“Eye-in-Hand”的手眼协调控制已成为客观需要和研究共识[26-29]。CHEN等[30]开发了仿人型双臂式番茄采摘机器人，在头部和腕部分别安装Xtion和PrimeSense 传感器，首先由外置Xtion获取并引导手臂到达果枝的近位，再由手上Carmine完成更细的果实识别定位，但仅通过室内的简化试验证实了其可行性，试验精度不足且未完全实现完全自主的手眼协调控制。澳大利亚昆士兰科技大学LEHNERT 等[16,31]研究的甜椒采摘机器人，需通过手上RealSense与作物行垂直进行水平和竖直移动扫描进而重构建立3维点云模型，然后由RGB-HSV转换的颜色特征识别出红色甜椒，用深度信息给出植株的位姿估计并引导机械臂抓取和分离果实，试验发现手眼协调完成采摘的耗时达到35～40 s，效率偏低[31-33]；SORIA 等[34]构建了基于RealSense SR300传感器的手-眼系统，首先定位在果枝前拍照确定机械手与苹果植株的相对角度，进而控制机械手沿球面移动进行多视角的点云获取，将多视角点云图像合成获得真实的相对姿态，但尚未进行手眼协调的控制研究。

为此，本文在RealSense深度传感器的景深160～1 200 mm范围和160 mm近景的果实可靠识别[35-36]基础上，结合升降式采摘机器人结构设计与新型手眼配置模式和“远近景”组合的手眼协调控制，进行RealSense深度伺服的采摘机器人设计与远近景运动规划，并通过试验进行验证。

1 采摘机器人结构

RealSense深度伺服小型升降式采摘机器人结构示意图如图1所示，由自主移动底盘、剪叉升降机构、抽拉式果箱、3回转机械臂、夹剪式末端执行器和腕部RealSense SR300深度传感器构成。

(1)设计了由平置直流电机驱动的多级剪叉升降机构，并设计了弹性辅助启停装置以改善启动性能并实现下降的缓冲，从而有效解决狭小空间通行的底盘小型化与竖直作业的较大升降需要之间的矛盾。

(2)在2关节机械臂基础上，设计了剪叉升降平台上的基座水平伺服回转机构，由大小臂、水平回转与竖直升降组合成为PRRR 4自由度机械臂，并将抽拉式果箱安装于升降平台上，在特定高度区域仅由机械臂的3回转动作规划即可完成采摘和空中就近放果，大大缩短了周期作业的路径长度、运动规划复杂性和耗时；满足规范化种植下的采摘要求。

(3)针对该少自由度、短路径的“采-放”周期作业，通过机械臂腕部安装RealSense深度传感器构成“眼在手上”系统，利用其景深160～1 200 mm和较大视场范围(竖直55°×水平71°)的1 280×720深度探测特点和在手RealSense深度伺服控制，从而保障远近景组合的手-眼协调精准定位。

2 手-眼协调的姿态与坐标

2.1 采放果工作空间与姿态分析

该采摘机器人通过竖直升降与机械臂的3自由度回转实现大范围采摘作业，而仅需机械臂的3自由度回转实现对空中果箱的放果。升降与回转的末端工作空间与姿态应满足矮化密植或吊蔓栽培的果实采摘需要，同时通过机械臂的关节回转满足将果实安全送入空中果箱的需要。

2.1.1工作空间分析

根据蒙特卡罗法对机械臂的工作空间进行求解，调用Matlab机器人工具箱的Rand函数与Plot函数，画出机械臂的工作空间点云图。

回转机械臂在升降机构的基础上，可对竖直范围374～2 613 mm分布的果实进行采摘，底盘距植株400 mm时的水平方向最大采摘深度为753 mm，从而满足矮化密植果园冠幅在1.5 m内的双侧采摘。

2.1.2姿态分析

结合机器人的工作空间与果树的尺寸，对采摘机器人采放果姿态进行分析，如图2所示。

由图2可知，根据果实位置和末端可达性，机器人采果呈现俯摘、平摘、仰摘3种不同姿态，同时根据空中抽拉式果箱与机械臂的相对安装关系，放果时机械臂末端放果能到达的姿态为小臂与水平角度范围70°～110°。

2.2 手-眼协调的坐标变换

对“眼在手上”模式，果实定位需获得果实相对于RealSense的位置，进而根据RealSense与机械臂的相对安装位置及其运动学方程，获得果实相对于机器人坐标系的位置坐标。

假设由RealSense SR300获得的果实位置坐标为S0=(x0,y0,z0)，则目标果实相对于机器人坐标系的位置S1为

S1=S0M

(1)

其中

(2)

式中M——果实相机坐标与机器人坐标变换矩阵

z0——多级剪叉升降机构竖直直线位移，mm

θ1——水平旋转关节角位移，rad

θ2——机械臂大臂转动角位移，rad

θ3——小臂转动角位移，rad

L1～L6——机械臂各杆长及RealSense与机械臂的相对安装尺寸

L1～L6依次为168、8、313、-100、285、125 mm。

3 基于RealSense远近景组合的机械臂运动规划

3.1 基于RealSense伺服的远近景手眼协调策略

与CCD被动视觉相比，主动深度视觉干扰信息少、受光线影响较小，但是消费级RGB-D相机受分辨率和精度影响，只进行一次远景识别定位不能获得果实个体清晰的深度图像且出现较大定位误差。使用RealSense SR300作为视觉传感器，可在景深160～1 200 mm范围内获得远景的冠层深度与坐标信息，同时可实现160 mm超近景和较小视场内有限数量果叶的“大而清”图像获取和可靠识别定位[35-36]。基于RealSense将远景的冠层粗探测、子区域划分定位和近景的果实精确识别定位有效结合，实现对机械臂的由粗及细引导和精确定位，并通过路径关键点的分段路径规划和组合以提高“眼在手上”的图像伺服控制实时性和稳定性。

基于RealSense伺服的远近景手眼协调动作流程如图3所示。OABCD为初始采摘路径；DBCD为周期作业路径。

(1)机械臂腕部由初始位置O运动到远景位A，由RealSense SR300自远景位A获得较大树冠区域的深度图和对应坐标信息；根据RealSense的竖直和水平视角范围(55°×71°)和目标距离，可获得不同距离的视场范围，进而根据RealSense SR300的最佳近景视场范围将远景深度区域划分为若干近景子区域并完成子区域的定位(图4)。

(2)机械臂依次根据某一近景子区域的坐标，由远景位A向该子区域近景位B快速逼近，并在近景位完成对该子区域内果实的近景精确识别定位。

(3)根据近景位的果实识别定位结果，由机械臂将末端执行器向果实C送进并完成对果实的摘取。

(4)末端由果实位置C运动到果箱内的放果位D，完成放果作业后返回该子区域的近景位B，对这一子区域的剩余果实进行依次采摘，在采摘点C完成采摘后送至果箱放果位D。

(5)将该子区域内的果实采摘结束后，依上述流程对下一子区域继续进行采摘放果作业，直至果实采摘完成后，采摘机器人移动至下一远景位进行下一轮循环采摘作业。

在该策略中，RealSense分别在远景点和近景点两次反馈“粗、细”不同深度信息，引导机械臂由远及近迫近目标并完成采摘。该方法基于有限关键点的深度视觉伺服实现高精度的机械臂引导和连续作业，运算量少，可靠性和稳定性得到有效保证，从而为实现快速高成功率的机器人采摘作业提供保证。

3.2 手眼协调的关键路径点

如图5所示，RealSense视觉传感器安装在机械臂的腕部，在远近景协调策略下分别在远景点与近景点反馈目标深度信息，引导机械臂运动至目标果实采摘。根据机械臂的作业范围，进行RealSense远近景最佳探测范围试验，可知RealSense SR300的最佳远景探测范围为500～700 mm，最佳近景识别距离为200 mm。关键位的设定对机械臂的精确定位非常关键，在每个关键位的手-眼任务如下：

(1)远景位：机械臂距离冠层500～700 mm处以水平姿态扫描冠层，顺利获得一定高度远景的完整冠层深度数据，以进行子区域划分。700 mm处探测区域为1 008 mm×729 mm，500 mm处探测区域为720 mm×520 mm。

(2)近景位：机械臂在确定子区域后，运动至目标子区域的中心点，即为距离子区域200 mm处的最佳近景位，仍然以水平姿态作为理想姿态，视觉传感器探测区域为288 mm×208 mm。

(3)采摘位：机械臂根据目标果实的坐标，经过姿态规划到达采摘位，以水平或俯或仰的姿态进行采摘。

(4)放果位：在双侧果箱的中心点，采摘完成后，机械臂移送果实至放果位。

3.3 采放果机械臂作业轨迹分段规划

3.3.1轨迹规划方法

为实现对机械臂的控制，需要在规划之前给定初始与目标点的机械臂末端的位姿。将机器人在笛卡尔空间的位姿换算为关节角，用函数对关节角拟合进行轨迹描述。关节空间下常用的轨迹插值方法有三次多项式和五次多项式，三次多项式函数只对轨迹上的位置与速度进行约束，为保证机械臂运动的平滑性，还需对关节起停处的加速度进行约束，即使用五次多项式进行轨迹插值。五次多项式插值函数式为

θ(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5

(3)

对式(3)进行一阶与二阶求导，可得到机械臂关节角速度时间函数以及加速度时间函数。通过起终点直接选取的各路径点的约束条件可确定线性方程组，从而求解方程各系数。

在设定各关键点并确定直线轨迹后，需在各段轨迹上添加一定的插值点，并对这些插值点进行五次多项式的插值计算，得出插值点的位姿坐标随时间的变化率，即通过插值点描述起点到终点过程末端位姿随时间的变化关系。

3.3.2分段轨迹规划

机械臂作业轨迹可分为采果与放果两段轨迹，考虑到作业效率与控制难度，从初始位到远景位，以及采果完成后移送至果箱的这两段路径都采用直线轨迹作为本文最短路径规划方案。

从远景位到近景范围、再到果实采摘位的过程，可以理解为机械臂从远景位逼近果实的作业路径。在完成果实采摘后，机械臂再从采摘位将果实移送至果箱，进而回复至远景位。由采摘位再结合远景位的位姿进行逆解，在关节空间内进行轨迹规划后生成关节角、角速度和加速度，使机械臂到达给定位姿(图6)。

3.3.3轨迹衔接

从初始位置到远景位，机械臂末端速度在路径两端都为0，从远景点到近景点过程为快速逼近过程，速度不为0，而采果位到放果位这段轨迹的两端速度都为0。根据采摘作业流程与各关键点可知，需对初始位到远景位、远景位到采果位以及采果位到放果位进行三段直线轨迹规划。首先需要将笛卡尔空间内的各关键点坐标转换成关节空间下各转动关节的角度和直动关节(升降)的位置，如表1所示。

表1 关键点坐标与关节参数示例

为保证各拟合点直线轨迹的准确性，每15 mm选取一个插值点，通过逆解得到各插补点对应的机械臂各关节参数。对各段直线轨迹进行Matlab编程仿真，得到P0到P1、P1到P2以及P2到P3的各段空间直线插补轨迹。

4 样机与试验验证

4.1 样机设计

小型升降式采摘机器人样机如图7所示，该样机主要技术参数与性能指标如表2所示。

表2 样机参数

4.2 验证试验

4.2.1试验材料

如图8所示，为方便采摘机器人精度试验的检测，将定制的两块1 m×1 m、间隔尺寸为2 mm的透明刻度板通过铝型材搭建成直角坐标系的坐标检测装置，并在机械臂腕部安装激光笔。

调整并确定机械臂的初始位置，RealSense相机深度方向作为Z轴，测量机械臂初始位置在高度方向即X方向为800 mm，腰部旋转关节中心线为Y方向，在网格刻度板上记录下相对于XOY平面为(800 mm，0，0)的参考点(图9b)。之后每次测量末端坐标的X、Y坐标都通过坐标点相对于该参考点的读数，而Z方向可通过游标卡尺测量，从而得出采摘机器人末端的实际坐标。

4.2.2试验方法

(1)采摘机器人底盘固定不动，调整机械臂至初始位置，在Z方向的透明刻度板上固定若干塑料柑橘果实，将果实中心点作为目标点，并根据刻度记录果实的实际坐标。

(2)应用基于RealSense伺服的远近景手眼协调策略和采放果机械臂作业轨迹分段规划，执行不同关键位的和各关键位之间的手-眼任务，完成单周期的果实采-放。

(3)调整铝型材框架与机器人基坐标系在Z方向的距离，并在不同的Z1、Z2、…、Zn平面改变果实的位置，对不同位置果实自主执行手眼协调的果实采-放动作。

(4)根据激光笔在刻度板上的Mark点读取每次末端在XOY面的投影坐标(图9c)，由游标卡尺测量末端与刻度板的距离(图9d)，结合刻度板在机器人基坐标系中Z方向的距离，得到末端在Z轴的坐标值。同时运行过程中用DV摄像机摄像，用视频编辑软件截取机械臂运动的视频片段，得到其运行时间。

4.2.3试验结果分析

采摘机器人由远及近手眼协调动作的过程如图10所示。末端实际到达的坐标点测量值与相应耗时如表3所示。

由表3可知，在RealSense信息反馈的远近景协调策略下，机器人在X、Y、Z方向的平均误差分别为3.51、2.79、3.35 mm。采摘作业耗时随果实距离的增加而增加，平均耗时为19.24 s，其中机械臂从初始位到采果动作的平均耗时为12.04 s，中间识别与运算的平均耗时为3.82 s，而放果动作平均耗时为7.2 s，机械臂动作耗时占整个环节的80.2%。该手眼协调动作精度基本满足采摘要求，达到了较好的采摘精度及效率的综合效果。但是，其精度、效率仍然有较大的提升可能。本自主开发的机器人各关节存在一定误差，特别是3回转机械臂各关节之间存在一定间隙，经测试静止状态下机械臂的最大间隙为(2.8，2.2，2.5) mm，大约占实际定位误差的80%，通过优化可以显著提高机器人手眼协调动作精度。试验采取间隔15 mm插值计算机械臂路径，机械臂实际到达精度会随着插值点间隔的减小而提高，同时机器人计算能力的提升将使识别定位、插值计算时间进一步减小。

表3 末端位置坐标测量结果

5 结论

(1)设计了在手RealSense深度伺服的小型升降式采摘机器人，建立了手-眼协调的姿态与坐标关系。

(2)采用眼在手上的手眼协调方式，对采摘机器人提出了基于在手RealSense深度伺服的由远及近手眼协调策略，并完成了基于深度视觉和远近景协调策略的作业流程及运动规划。

(3)进行了手眼协调采摘试验，结果表明末端在X、Y、Z方向平均定位精度为3.51、2.79、3.35 mm，平均耗时为19.24 s，其中机械臂从初始位开始采果的平均耗时为12.04 s，中间识别与运算的平均耗时为3.82 s，放果动作平均耗时为7.2 s，机械臂动作耗时占整个环节的80.2%。

(4)该机器人结构和在手RealSense深度伺服的手眼协调策略可满足采摘作业需求，同时关节精度的提高和路径插值计算的优化可进一步提升手眼协调的精度和效率。