CN116439018B - 一种七自由度水果采摘机器人及其采摘方法 - Google Patents

Abstract

本发明提供一种七自由度水果采摘机器人，包括履带式底盘（10）、七自由度机械臂组件（20）、末端执行器（30）、视觉感知***及避障路径规划***；七自由度机械臂组件（20）由一自由度机械臂（21）与六自由度机械臂（22）组成；视觉感知***包括深度相机（40）与定位模块。该采摘机器人能够自动定位识别乔木类水果的空间位置坐标，同时完成避开树枝或树叶等障碍物的采摘路径规划，实现乔木类水果的自动、高效采摘，有效节省人力物力，避免采摘过程受障碍物影响而出现错采、漏采等问题，进而确保采摘的果实品质好、避免采摘对果实或果树造成损伤。

Description

一种七自由度水果采摘机器人及其采摘方法

技术领域

本发明涉及果实采摘技术领域，具体涉及一种七自由度水果采摘机器人及其采摘方法。

背景技术

乔木类水果，如苹果、梨、番石榴、柑橘等不仅可以补充人体所需的各类维生素，还对预防疾病、减缓衰老、美容养颜具有一定的效果。由于乔木类水果的需求量与日俱增、主要以新鲜水果为主，因此，乔木类水果成熟后需在短时间内完成采摘、运输等，以保证其新鲜程度，从而让满足人们的日常需求。然而，现有的农业劳动力匮乏，且人工采摘的单日采摘效率低，导致采摘成本高，进而增加水果的生产成本；同时，乔木类水果又具有一定的采摘时限要求，超过时限会导致水果卖相、口感等变差，从而导致产品滞销。

现阶段，机械化采摘多为拉拽装置辅助人工进行采摘；在拉拽过程中，采摘头会挤压水果，对果实、果树枝条等造成伤害，进而影响果实的运输、出售，影响果树的二次结果等。此外，乔木类水果在果树的枝条上呈现不规则的分布，果实周围常伴随障碍物遮挡，现阶段我国乔木类果树基本都为三维种植，由于种植面广、种植量大，几乎很少有进行剪枝的情况，从而会导致乔木类水果被树枝、树叶等遮挡情况频发，现有的采摘装置无法有效规避树枝、树叶等障碍物，极易造成错采、漏采、误采等问题，降低水果采摘的成功率，还易对果实、果树枝条造成不可逆的损伤，造成果树减产、水果滞销等问题。

发明内容

针对以上现有技术存在的问题，本发明的目的在于提供一种七自由度水果采摘机器人，该采摘机器人能够自动定位识别乔木类水果，同时完成采摘路径的规划、自动避开树枝或树叶等障碍物，完成乔木类水果的自动化、机械化采摘，操作简便、采摘效率高，从而有效节省采摘的劳动力成本，确保果树的产量与果实的品质。

本发明的另一个目的在于提供一种上述七自由度水果采摘机器人的采摘方法，从而完成乔木类水果的智能化采摘。

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

一种七自由度水果采摘机器人，其特征在于：包括履带式底盘、七自由度机械臂组件、末端执行器、视觉感知***及避障路径规划***；七自由度机械臂组件由一自由度机械臂与六自由度机械臂组成，一自由度机械臂安装在履带式底盘上，六自由度机械臂安装在一自由度机械臂上；末端执行器安装在六自由度机械臂远离一自由度机械臂的一端端部；视觉感知***包括深度相机与定位模块，深度相机安装在末端执行器上、跟随末端执行器一同运动；避障路径规划***与定位模块集成在中控装置中，中控装置安装在履带式底盘上且分别与履带式底盘、七自由度机械臂组件、末端执行器、深度相机电连接。

作进一步优化，所述履带式底盘上设置用于采摘后水果存放的水果篮。

作进一步优化，所述末端执行器包括底座、曲柄滑块机构及四杆机构，曲柄滑块机构设置在底座上，包括转动盘、连接杆与滑块，转动盘转动设置在底座端面上且底座对应端面滑动设置滑块，连接杆一端与转动盘外圈转动连接、另一端与滑块顶面转动连接，从而通过转动盘的转动控制滑块的平移；四杆机构为对称设置的两组，包括第一连杆、“L”形连杆、“7”字形连杆与第二连杆，滑块远离连接杆一端且对应两组四杆机构设置安装块，两组四杆机构的第一连杆分别与安装块转动连接，“L”形连杆的一端分别与对应的第一连杆转动连接、另一端分别与对应的“7”字形连杆一端转动连接，且“L”形连杆的拐角与底座转动连接，两根第二连杆设置在两根“L”形连杆之间且第二连杆一端分别与底座端面转动连接、另一端分别与对应的“7”字形连杆拐角转动连接。

作进一步优化，所述底座的底面固定设置舵机，舵机输出轴与转动盘共轴线且转动盘固定套接在舵机输出轴外壁。

作进一步优化，所述底座上且对应滑块设置滑移导轨，滑块卡在滑移导轨上且滑动连接。

作进一步优化，所述“7”字形连杆相对侧面分别设置夹紧胶条且“7”字形连杆位于夹紧胶条上侧的端面设置刀片。

作进一步优化，所述深度相机采用RealSense D435i相机。

一种七自由度水果采摘机器人的采摘方法，采用上述采摘机器人，其特征在于：包括采摘点与障碍物定位算法、手眼标定算法与路径规划算法；

所述采摘点与障碍物定位算法具体为：

首先，使用深度相机采集RGB图像与深度图像，并应用语义分割网络对RGB图像进行分割；

之后，对图像中的每一个像素均打上标签，即背景为0，连通域分别打上1、2、…、N-1、N的标签，同一个连通域的像素打上同样的标签；若物体存在重叠、会把不同物体的多个连通域计为一个连通域，从而提取果实连通域；再结合深度图像，将各个果实连通域转换为三维点云、同时将障碍物二值化区域也转换为三维点云，应用统计分析法移除三维点云中的噪声；然后，使用最小二乘法对三维点云中的果实连通域进行球体拟合，获得果实中心点位置和半径值；最终，以果实中心点铅垂向上一个半径值的位置作为果实采摘点；

之后，应用体素栅格化的点云简化法对上述获得的三维点云进行体素化处理，实现使用大量立方体逼近障碍物和果实的空间分布；

所述手眼标定算法用于求解相机与末端执行器之间的位姿转换关系、从而获得末端执行器相对于采摘目标之间的坐标转换关系；

所述路径规划算法具体为：首先，在初始点与目标点之间随机选取2～3个点，对选择的点位进行判断、看其是否处于障碍物上，若处于，则更换对应的随机点；确定随机点Q_r后，将随机点与初始点、目标点形成多棵搜索树，同时沿随机点Q_r方向以单位步长进行扩展，寻找到新节点Q_n，再以新节点重复上述步骤，期间同样要考虑与障碍物有无碰撞、步长等，直至多棵搜索树同时存在相互连接(多棵搜索树同时存在相互连接说明找到路径、完成规划)；

上述路径规划算法基于RRT的双树对向交替扩展探索方式来选择代价小的树进行扩展，一定程度上拥有了方向性，避免采样时过于随机，在面对狭窄通道搜索采样时、成功率更高，搜索速度和搜索效率上有明显提升。

作进一步优化，所述使用最小二乘法对三维点云中的果实连通域进行球体拟合，获得果实中心点位置和半径值具体步骤为：

首先建立球体方程：

然后，假设第i个果实的三维点云坐标为(x_i,y_i,z_i)，带入球体方程中，可得：

令：

则：

经上述各点的计算能够获得球心坐标(x₀,y₀,z₀)与球体半径r₀；

之后，预设合理范围值H，计算三维点云中每个点到球心坐标(x₀,y₀,z₀)的距离h₀，若|h₀-r₀|＜H，则该点在拟合球体内，依次记录整个拟合球体内的点的数量；

重复多次进行球体拟合，选取模型内点的数量最多的球体对应的模型参数为最佳拟合参数，输出最佳拟合参数的球心坐标(x,y,z)及半径r作为果实的三维拟合结果。

作进一步优化，所述合理范围值H为0～0.5，通过大量前期实验数据获得。

作进一步优化，所述体素栅格化的点云简化法对上述获得的三维点云进行体素化处理具体步骤为：

首先，根据点云创建一个最小三维体素长方体，其体积为V：

V＝L_x·L_y·L_z；

式中：L_x表示点云X轴方向最大范围；L_y表示点云Y轴方向最大范围；L_z表示点云Z轴方向最大范围；

然后，计算需要划分的小立方栅格的边长L，根据L的大小将最小三维体素长方体分解成个小立方体格栅；栅格划分完毕后，将点云数据放到相应的小格栅中，同时删除那些不包含数据点的小格栅；在每个小栅格中，将离小格栅中心最近的数据点保留下来，代表本小格栅中的所有点，删除其余点。

作进一步优化，所述手眼标定算法具体为：

首先，根据标定板棋盘格的所有角点的像素值、深度值和深度相机的参数，通过像素坐标系与世界坐标系之间的转换公式进行转换，从而获得标定板棋盘格上所有角点的空间坐标，转换为对应的点的坐标数组，同时将其与所拍照的图片中所对应的所有角点、在图像坐标系下的成像点的坐标数组记录为多组控制点；转换公式为：

式中：X、Y、Z表示世界坐标系下的坐标；f表示焦距；R表示3x3的正交旋转矩阵；t表示三维平移向量；u₀、v₀表示图像坐标系原点在像素坐标***的坐标；dx、dy表示每个像素在图像平面x方向、y方向的物理尺寸；u、v表示像素坐标；Z_C表示相机坐标中Z轴的向量；

然后，利用多个控制点在三维场景中的坐标及其在图像中的透视投影坐标，获得摄像机坐标系与表示三维场景结构的世界坐标系之间的绝对位姿关系，包括绝对平移向量t以及旋转矩阵R，从而获得多组标定板棋盘格上所有角点的空间坐标到相机坐标系的变换矩阵T中的旋转和平移量；

之后确定机械臂DH参数表；

再然后，通过网口通信读出平移位置，计算出x、y、z三轴上的单位矢量a：

由于相邻两根连杆T_i与T_i-1的变换关系式为：

式中：a_i表示公共法线间的距离与垂直于所在平面内两轴的夹角；d_i表示两根连杆的相对位置；θ_i表示两根连杆的相对位置d_i和两根连杆公垂线的夹角；c表示三角函数中的cos()函数；s表示三角函数中的sin()函数；

多次移动七自由度机械臂组件，获得多组末端执行器相对于基坐标的旋转和平移量；将末端执行器相对于基坐标的多组旋转和平移参数记作多组姿态矩阵B，将上述多组标定板棋盘格上所有角点的空间坐标到相机坐标系的变换矩阵T视为外参矩阵A，之后，得到组A、B，利用AX＝XB计算得到相机坐标系到七自由度机械臂组件末端坐标系的旋转和平移变换，从而获得相机坐标系到七自由度机械臂组件末端坐标系的变换矩阵T_e；

最后，移动七自由度机械臂组件、并使用深度相机对采摘目标进行拍照，像素坐标系与世界坐标系之间的转换公式获得采摘目标的空间坐标；再利用变换矩阵T_e以及标定板棋盘格上所有角点的空间坐标到相机坐标系的变换矩阵T，获得采摘目标相对于末端执行器坐标系下的位姿。

作进一步优化，为了优化路径较少不必要的转折，所述路径规划算法中，在搜索树集合T_ree中的节点中任取两点Q_i与Q_j，其中i,j∈[1,2,3,…,n]；对Q_i与Q_j间的路径进行碰撞检测，若无碰撞在，则删除Q_i与Q_j之间的所有节点。

作进一步优化，所述碰撞检测为：采用向空间坐标系的三个坐标轴上进行投影的方式、对立方体和圆柱体之间进行碰撞检测，若投影后立方体和圆柱体连接中心点的线段的投影长度大于各自中心点投影后到各自最长边界的距离和，且三个轴同时满足下，则视为立方体和圆柱体不会发生碰撞；

具体为：

首先，找到立方体的四个顶点、并通过对其坐标求平均的方法获得立方体为投影时的中心点P_lm；再找到圆柱体的上下底面圆心坐标、并通过对其求平均的方法获得圆柱体未投影时的中心点P_ym；然后，计算获得中心点P_lm与中心点P_ym之间的连线l_d、其在x轴上的投影为l_D，同时，分别获得中心点P_lm与中心点P_ym在x轴上的投影P_lM与P_yM；之后，计算P_lM与P_yM到各自物体(即立方体、圆柱体)投影后的边界的距离r_a与r_b：

若|l_D|＞r_a+r_b，且在y轴、z轴上也满足获得的|l_D|＞r_a+r_b，则立方体与圆柱体无碰撞；否则，立方体与圆柱体存在碰撞。

本发明具有如下技术效果：

本申请履带式底盘、七自由度机械臂组件与末端执行器的配合，实现了对于乔木类水果的自动化采摘，有效降低人为采摘过程中的劳动力成本，提高采摘效率。

同时，本申请通过RGB图像与深度图像的配合、利用语义分割网络进行分割，之后通过标签与连通域的配合，获得果实和障碍物感兴趣区域，鲁棒性与实时性高；通过结合深度图像的数据，将各个果实连通域转换为三维点云，应用统计分析法移除三维点云中的噪声，使用最小二乘法对三维点云进行球体拟合、获得果实中心点位置和半径值，从而获得果实的采摘点，定位准确且精度高、定位误差小，能够有效实现乔木类水果的精确采摘；通过路径规划算法，完成采摘路径的规划，进而有效完成采摘过程中的障碍躲避，避免采摘过程中对于果树造成的损伤、确保采摘的有效性，避免采摘过程中由于障碍物的影响而出现错采、漏采等问题，造成果实或果树的损伤、影响果实的品质或果树的再次结果。此外，本申请通过底座、曲柄滑块机构及四杆机构的协同配合，实现对于乔木类水果的剪切，采摘连贯性好，同时能够保证采摘后对果实的收集、采摘的时效性强，能够有效保证采摘效率、节省人力物力与采摘成本。

附图说明

图1为本申请实施例中水果采摘机器人的结构示意图。

图2为本申请实施例中水果采摘机器人的末端执行器的结构示意图。

图3为本申请实施例中果实和障碍物的语义分割效果图。

图4为本申请实施例中障碍物的三维重构效果图。

其中，10、履带式底盘；11、水果篮；20、七自由度机械臂组件；21、一自由度机械臂；22、六自由度机械臂；30、末端执行器；31、底座；310、舵机；321、转动盘；322、连接杆；323、滑块；3230、安装块；331、第一连杆；332、“L”形连杆；333、“7”字形连杆；3331、刀片；3332、夹紧胶条；334、第二连杆；40、深度相机；50、中控装置。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。

实施例1：

如图1～2所示：一种七自由度水果采摘机器人，其特征在于：包括履带式底盘10、七自由度机械臂组件20、末端执行器30、视觉感知***及避障路径规划***；履带式底盘10采用松灵机器人(深圳)有限公司的履带底盘，该底盘顶部具有一套支撑装置，由铝合金平板、8080铝型材等组成。七自由度机械臂组件20由一自由度机械臂21与六自由度机械臂22组成，一自由度机械臂21安装在履带式底盘10上，六自由度机械臂22安装在一自由度机械臂21上；一自由度机械臂21采用24V步进电机作为原动机，应用滚珠丝杆机构实现直线运动；六自由度机械臂22采用睿尔曼RML63系列机器人，该机械臂自重10.2kg、工作半径为900mm的球体。

末端执行器30安装在六自由度机械臂22远离一自由度机械臂21的一端端部；包括底座31、曲柄滑块机构及四杆机构，曲柄滑块机构设置在底座31上，包括转动盘321、连接杆322与滑块323，转动盘321转动设置在底座31端面上且底座31对应端面滑动设置滑块323(如图2所示，即底座31上端面)，连接杆322一端与转动盘321外圈转动连接、另一端与滑块323顶面转动连接，从而通过转动盘321的转动控制滑块323的平移；底座31的底面固定设置舵机310，舵机310输出轴与转动盘321共轴线且转动盘321固定套接在舵机310输出轴外壁；底座31上且对应滑块323设置滑移导轨，滑块323卡在滑移导轨上且滑动连接，从而对滑块323的滑动方向形成硬限位。四杆机构为对称设置的两组，包括第一连杆331、“L”形连杆332、“7”字形连杆333与第二连杆334，滑块323远离连接杆322一端且对应两组四杆机构设置安装块3230，两组四杆机构的第一连杆331分别与安装块3230转动连接，“L”形连杆332的一端分别与对应的第一连杆331转动连接、另一端分别与对应的“7”字形连杆333一端转动连接，且“L”形连杆332的拐角通过设置在底座31上的第一转轴与底座31实现转动连接(即如图2所示：第一转轴转动设置在底座31端面，“L”形连杆332的拐角固定套接在对应的第一转轴外壁)，两根第二连杆334设置在两根“L”形连杆332之间且第二连杆334一端分别通过设置在底座31上的第二转轴与底座31端面转动连接(即如图2所示：第二转轴转动设置在底座31端面，第二连杆334一端固定套接在第二转轴外壁)、另一端分别与对应的“7”字形连杆333拐角转动连接。“7”字形连杆333相对侧面分别设置夹紧胶条3332且“7”字形连杆333位于夹紧胶条3332上侧的端面设置刀片3331(如图2所示)。

视觉感知***包括深度相机40与定位模块，深度相机40采用RealSense D435i相机，其安装在末端执行器30上(具体可为底座31端面)、跟随末端执行器30一同运动；避障路径规划***与定位模块集成在中控装置50中，中控装置50安装在履带式底盘10上且分别与履带式底盘10、七自由度机械臂组件20、末端执行器30、深度相机40电连接。

履带式底盘10上设置用于采摘后水果存放的水果篮11。

实施例2：

一种七自由度水果采摘机器人的采摘方法，采用如实施例1所示的水果采摘机器人，其特征在于：包括采摘点与障碍物定位算法、手眼标定算法与路径规划算法；采摘点与障碍物定位算法与手眼标定算法由定位模块进行运算，路径规划算法由避障路径规划***进行运算。

采摘点与障碍物定位算法具体为：

首先，使用深度相机采集分辨率为640*480像素的RGB图像与深度图像，并应用语义分割网络DeepLabV3+对RGB图像进行分割；

之后，对图像中的每一个像素均打上标签，即背景为0，连通域分别打上1、2、…、N-1、N的标签，同一个连通域的像素打上同样的标签；若物体存在重叠、会把不同物体的多个连通域计为一个连通域，从而提取果实连通域；

再结合深度图像，将各个果实连通域转换为三维点云、同时将障碍物二值化区域转换为三维点云，具体为：

式中，(x_i,y_i,z_i)表示像素点i的三维点云坐标；(u_i,v_i)表示乔木类水果果实区域中像素点i的图像坐标；(u₀,v₀)表示深度相机的光心位置，通过前期图像标定获得；(f_x,f_y)表示深度相机的焦距，通过前期图像标定获得；I_d表示获得的深度图像。

应用统计分析法移除三维点云中的噪声，具体为：获得三维点云后，计算三维点云中每个点到其所有临近点的平均距离接着计算每个点到其所有临近点的距离、将其和平均距离作差/>然后求平方，再求平均数得到方差，用方差开根号得到标准偏差，然后利用统计分析法中的拉依达检验法：即如果可疑数据x_p与实验数据的算术平均值/>之间的偏差的绝对值大于三倍的标准偏差，则认为是离群点、进行删除；若不是(即与上述情况相反)，则保留。

然后，使用最小二乘法对三维点云中的果实连通域进行球体拟合，获得果实中心点位置和半径值；具体步骤为：

首先建立球体方程：

然后，假设第i个果实的三维点云坐标为(x_i,y_i,z_i)，带入球体方程中，可得：

令：

则：

(由于(M^T·M)^-1＝M^-1·(M^T)^-1,(M^T)^-1·M^T＝1)；

经上述各点的计算能够获得球心坐标(x₀,y₀,z₀)与球体半径r₀；

之后，预设合理范围值H，其中H为0～0.5、通过大量前期实验数据获得，计算三维点云中每个点到球心坐标(x₀,y₀,z₀)的距离h₀，若|h₀-r₀|＜H，则该点在拟合球体内，依次记录整个拟合球体内的点的数量；

重复多次进行球体拟合，选取模型内点的数量最多的球体对应的模型参数为最佳拟合参数，输出最佳拟合参数的球心坐标(x,y,z)及半径r作为果实的三维拟合结果。

最终，以果实中心点铅垂向上一个半径值的位置作为果实采摘点；

之后，应用体素栅格化的点云简化法对上述获得的三维点云进行体素化处理，实现使用大量立方体逼近障碍物和果实的空间分布；具体为：

首先，根据点云创建一个最小三维体素长方体，其体积为V：

V＝L_x·L_y·L_z；

式中：L_x表示点云X轴方向最大范围；L_y表示点云Y轴方向最大范围；L_z表示点云Z轴方向最大范围；

然后，计算需要划分的小立方栅格的边长L，根据L的大小将最小三维体素长方体分解成个小立方体格栅；栅格划分完毕后，将点云数据放到相应的小格栅中，同时删除那些不包含数据点的小格栅；在每个小栅格中，将离小格栅中心最近的数据点保留下来，代表本小格栅中的所有点，删除其余点。

手眼标定算法用于求解相机与末端执行器之间的位姿转换关系、从而获得末端执行器相对于采摘目标之间的坐标转换关系；其具体步骤为：

首先，根据标定板棋盘格的所有角点的像素值、深度值和深度相机的参数，通过像素坐标系与世界坐标系之间的转换公式进行转换，从而获得标定板棋盘格上所有角点的空间坐标，转换为对应的点的坐标数组，同时将其与所拍照的图片中所对应的所有角点、在图像坐标系下的成像点的坐标数组记录为多组控制点；转换公式为：

式中：X、Y、Z表示世界坐标系下的坐标；f表示焦距；R表示3x3的正交旋转矩阵；t表示三维平移向量；u₀、v₀表示图像坐标系原点在像素坐标***的坐标；dx、dy表示每个像素在图像平面x方向、y方向的物理尺寸；u、v表示像素坐标；Z_C表示相机坐标中Z轴的向量；

然后，利用多个控制点在三维场景中的坐标及其在图像中的透视投影坐标，获得摄像机坐标系与表示三维场景结构的世界坐标系之间的绝对位姿关系，包括绝对平移向量t以及旋转矩阵R，从而获得多组标定板棋盘格上所有角点的空间坐标到相机坐标系的变换矩阵T中的旋转和平移量；

之后确定机械臂DH参数表，如下表1所示：

表中：a_i(mm)表示公共法线间的距离；a_i(°)表示公共法线间的距离a_i(mm)和垂直于a_i(mm)所在平面内两轴的夹角；d_i(mm)表示两根连杆之间的相对位置；θ_i(°)表示两根连杆的相对位置di d_i(mm)和两根连杆公垂线之间的夹角。

再然后，通过网口通信读出平移位置，计算出x、y、z三轴上的单位矢量a：

同时也可以读出每个机械臂此时的DH参数，由于相邻两根连杆T_i与T_i-1的变换关系式为：

式中：a_i表示公共法线间的距离与垂直于所在平面内两轴的夹角；d_i表示两根连杆的相对位置；θ_i表示两根连杆的相对位置d_i和两根连杆公垂线的夹角；c表示三角函数中的cos()函数；s表示三角函数中的sin()函数；

多次移动七自由度机械臂组件，获得多组末端执行器相对于基坐标的旋转和平移量；将末端执行器相对于基坐标的多组旋转和平移参数记作多组姿态矩阵B，将上述多组标定板棋盘格上所有角点的空间坐标到相机坐标系的变换矩阵T视为外参矩阵A，之后，得到组A、B，利用AX＝XB计算得到相机坐标系到七自由度机械臂组件末端坐标系的旋转和平移变换，从而获得相机坐标系到七自由度机械臂组件末端坐标系的变换矩阵T_e；

最后，移动七自由度机械臂组件、并使用深度相机对采摘目标进行拍照，像素坐标系与世界坐标系之间的转换公式获得采摘目标的空间坐标；再利用变换矩阵T_e以及标定板棋盘格上所有角点的空间坐标到相机坐标系的变换矩阵T，获得采摘目标相对于末端执行器坐标系下的位姿。最后还可将采摘目标相对于末端执行器坐标系下的位姿转换为相对于机械臂基坐标系坐标下的位姿，从而让机械臂能更好的向采摘目标靠近。

路径规划算法具体为：首先，在初始点与目标点之间随机选取2～3个点，对选择的点位进行判断、看其是否处于障碍物上，若处于，则更换对应的随机点；确定随机点Q_r后，将随机点与初始点、目标点形成多棵搜索树，同时沿随机点Q_r方向以单位步长进行扩展，寻找到新节点Qn，再以新节点重复上述步骤，期间同样要考虑与障碍物有无碰撞、步长等，直至多棵搜索树同时存在相互连接(多棵搜索树同时存在相互连接说明找到路径、完成规划)；

为了优化路径较少不必要的转折，路径规划算法中，在搜索树集合T_ree中的节点中任取两点Q_i与Q_j，其中i,j∈[1,2,3,…,n]；对Q_i与Q_j间的路径进行碰撞检测，若无碰撞在，则删除Q_i与Q_j之间的所有节点。

其中，碰撞检测为：采用向空间坐标系的三个坐标轴上进行投影的方式、对立方体和圆柱体之间进行碰撞检测，若投影后立方体和圆柱体连接中心点的线段的投影长度大于各自中心点投影后到各自最长边界的距离和，且三个轴同时满足下，则视为立方体和圆柱体不会发生碰撞；

具体为：

首先，找到立方体的四个顶点、并通过对其坐标求平均的方法获得立方体为投影时的中心点P_lm；再找到圆柱体的上下底面圆心坐标、并通过对其求平均的方法获得圆柱体未投影时的中心点P_ym；然后，计算获得中心点P_lm与中心点P_ym之间的连线l_d、其在x轴上的投影为l_D，同时，分别获得中心点P_lm与中心点P_ym在x轴上的投影P_lM与P_yM；之后，计算P_lM与P_yM到各自物体(即立方体、圆柱体)投影后的边界的距离r_a与r_b：

若|l_D|＞r_a+r_b，且在y轴、z轴上也满足获得的|l_D|＞r_a+r_b，则立方体与圆柱体无碰撞；否则，立方体与圆柱体存在碰撞。

采摘机器人进行果实采摘的具体方法为：

首先，整个乔木类水果采摘机器人由履带式底盘10驱动、向前移动到某一采摘位置，同时中控装置50对末端执行器30进行位置初始化，即末端执行器30三维空间坐标的初始化；且七自由度机械臂组件20进行归零操作，根据单片机算法，获得乔木类水果果实在水平、竖直、前后方向上相对于末端执行器30(初始化位置)的数据；之后，中控装置50通过视觉感知***获取果实的采摘点、并利用避障路径规划***获得末端执行器30的采摘路径

末端执行器30经过一自由度机械臂21与六自由度机械臂22的水平与竖直的调整、到达一定采摘位置，即使得果实茎部位于末端执行器30的两个刀片3331之下剪切空间内；此时，中控装置50在控制舵机310的运转，舵机310通过转动盘321与连接杆322拉动滑块323向靠近舵机310的一侧运动(即从远点向近点运动)，滑块323通过安装块3230拉动第一连杆331运动、进而使得“L”形连杆332在此机构中为旋转副进行旋转运动，“L”形连杆332推动“7”字形连杆333运动、且“7”字形连杆333收到第二连杆334的位移限制，进而使得两根“7”字形连杆333向相互靠近的方向移动，实现对于果实茎部的剪切。切断完成中，刀片3331下侧的夹紧胶条3332处于夹紧状态，对果实的果梗进行夹持，避免果实切断后掉落。之后，启动七自由度机械臂组件20使得末端执行器30位于水果篮11上方(此处由于最终的位置是确定的，因此控制可通过限位传感器实现、也可通过距离控制实现，为本领域的常规技术，本申请具体实施方式不做过多论述)，继续启动舵机310运行，滑块323向远离舵机310的一侧运动(即从近点向远点运动)，实现“7”字形连杆333的打开，果实掉落在水果篮11中、实现收集；如此循环完成水果的采集工作。

Claims (6)

1.一种七自由度水果采摘机器人的采摘方法，其特征在于：采用七自由度水果采摘机器人采摘，七自由度水果采摘机器人包括履带式底盘、七自由度机械臂组件、末端执行器、视觉感知***及避障路径规划***；七自由度机械臂组件由一自由度机械臂与六自由度机械臂组成，一自由度机械臂安装在履带式底盘上，六自由度机械臂安装在一自由度机械臂上；末端执行器安装在六自由度机械臂远离一自由度机械臂的一端端部；视觉感知***包括深度相机与定位模块，深度相机安装在末端执行器上；避障路径规划***与定位模块集成在中控装置中，中控装置安装在履带式底盘上且分别与履带式底盘、七自由度机械臂组件、末端执行器、深度相机电连接；

采摘方法包括采摘点与障碍物定位算法、手眼标定算法与路径规划算法；

所述采摘点与障碍物定位算法具体为：

首先，使用深度相机采集RGB图像与深度图像，并应用语义分割网络对RGB图像进行分割；

之后，对图像中的每一个像素均打上标签，即背景为0，连通域分别打上1、2、…、N-1、N的标签，同一个连通域的像素打上同样的标签；若物体存在重叠、会把不同物体的多个连通域计为一个连通域，从而提取果实连通域；再结合深度图像，将各个果实连通域转换为三维点云、同时将障碍物二值化区域也转换为三维点云，应用统计分析法移除三维点云中的噪声；然后，使用最小二乘法对三维点云中的果实连通域进行球体拟合，获得果实中心点位置和半径值；最终，以果实中心点铅垂向上一个半径值的位置作为果实采摘点；

其中，使用最小二乘法对三维点云中的果实连通域进行球体拟合，获得果实中心点位置和半径值具体步骤为：

首先建立球体方程：

然后，假设第i个果实的三维点云坐标为(x_i,y_i,z_i)，带入球体方程中，可得：

令：

则：

经上述各点的计算能够获得球心坐标(x₀,y₀,z₀)与球体半径r₀；

之后，预设合理范围值H，计算三维点云中每个点到球心坐标(x₀,y₀,z₀)的距离h₀，若|h₀-r₀|＜H，则该点在拟合球体内，依次记录整个拟合球体内的点的数量；

重复多次进行球体拟合，选取模型内点的数量最多的球体对应的模型参数为最佳拟合参数，输出最佳拟合参数的球心坐标(x,y,z)及半径r作为果实的三维拟合结果；

之后，应用体素栅格化的点云简化法对上述获得的三维点云进行体素化处理，实现使用大量立方体逼近障碍物和果实的空间分布；

所述手眼标定算法用于求解相机与末端执行器之间的位姿转换关系、从而获得末端执行器相对于采摘目标之间的坐标转换关系；

所述路径规划算法具体为：首先，在初始点与目标点之间随机选取2～3个点，对选择的点位进行判断、看其是否处于障碍物上，若处于，则更换对应的随机点；确定随机点Q_r后，将随机点与初始点、目标点形成多棵搜索树，同时沿随机点Q_r方向以单位步长进行扩展，寻找到新节点Q_n，再以新节点重复上述步骤，期间同样要考虑与障碍物有无碰撞、步长，直至多棵搜索树同时存在相互连接；

为了优化路径较少不必要的转折，路径规划算法中，在搜索树集合T_ree中的节点中任取两点Q_i与Q_j，其中i,j∈[1,2,3,…,n]；对Q_i与Q_j间的路径进行碰撞检测，若无碰撞在，则删除Q_i与Q_j之间的所有节点；

碰撞检测为：采用向空间坐标系的三个坐标轴上进行投影的方式、对立方体和圆柱体之间进行碰撞检测，若投影后立方体和圆柱体连接中心点的线段的投影长度大于各自中心点投影后到各自最长边界的距离和，且三个轴同时满足下，则视为立方体和圆柱体不会发生碰撞；

具体为：

首先，找到立方体的四个顶点、并通过对其坐标求平均的方法获得立方体未投影时的中心点P_lm；再找到圆柱体的上下底面圆心坐标、并通过对其求平均的方法获得圆柱体未投影时的中心点P_ym；然后，计算获得中心点P_lm与中心点P_ym之间的连线l_d、其在x轴上的投影为l_D，同时，分别获得中心点P_lm与中心点P_ym在x轴上的投影P_lM与P_yM；之后，计算P_lM与P_yM到各自物体投影后的边界的距离r_a与r_b：

若|l_D|＞r_a+r_b，且在y轴、z轴上也满足获得的|l_D|＞r_a+r_b，则立方体与圆柱体无碰撞；否则，立方体与圆柱体存在碰撞。

2.根据权利要求1所述的一种七自由度水果采摘机器人的采摘方法，其特征在于：所述履带式底盘上设置用于采摘后水果存放的水果篮。

3.根据权利要求1或2所述的一种七自由度水果采摘机器人的采摘方法，其特征在于：所述末端执行器包括底座、曲柄滑块机构及四杆机构，曲柄滑块机构设置在底座上，包括转动盘、连接杆与滑块，转动盘转动设置在底座端面上且底座对应端面滑动设置滑块，连接杆一端与转动盘外圈转动连接、另一端与滑块顶面转动连接；四杆机构为对称设置的两组，包括第一连杆、“L”形连杆、“7”字形连杆与第二连杆，滑块远离连接杆一端且对应两组四杆机构设置安装块，两组四杆机构的第一连杆分别与安装块转动连接，“L”形连杆的一端分别与对应的第一连杆转动连接、另一端分别与对应的“7”字形连杆一端转动连接，且“L”形连杆的拐角与底座转动连接，两根第二连杆设置在两根“L”形连杆之间且第二连杆一端分别与底座端面转动连接、另一端分别与对应的“7”字形连杆拐角转动连接。

4.根据权利要求3所述的一种七自由度水果采摘机器人的采摘方法，其特征在于：所述底座的底面固定设置舵机，舵机输出轴与转动盘共轴线且转动盘固定套接在舵机输出轴外壁。

5.根据权利要求4所述的一种七自由度水果采摘机器人的采摘方法，其特征在于：所述“7”字形连杆相对侧面分别设置夹紧胶条且“7”字形连杆位于夹紧胶条上侧的端面设置刀片。

6.根据权利要求5所述的一种七自由度水果采摘机器人的采摘方法，其特征在于：所述手眼标定算法具体为：

首先，根据标定板棋盘格的所有角点的像素值、深度值和深度相机的参数，通过像素坐标系与世界坐标系之间的转换公式进行转换，从而获得标定板棋盘格上所有角点的空间坐标，转换为对应的点的坐标数组，同时将其与所拍照的图片中所对应的所有角点、在图像坐标系下的成像点的坐标数组记录为多组控制点；转换公式为：

式中：X、Y、Z表示世界坐标系下的坐标；f表示焦距；R表示3x3的正交旋转矩阵；t表示三维平移向量；u₀、v₀表示图像坐标系原点在像素坐标***的坐标；dx、dy表示每个像素在图像平面x方向、y方向的物理尺寸；u、v表示像素坐标；Z_C表示相机坐标中Z轴的向量；

然后，利用多个控制点在三维场景中的坐标及其在图像中的透视投影坐标，获得摄像机坐标系与表示三维场景结构的世界坐标系之间的绝对位姿关系，包括绝对平移向量t以及旋转矩阵R，从而获得多组标定板棋盘格上所有角点的空间坐标到相机坐标系的变换矩阵T中的旋转和平移量；

之后确定机械臂DH参数表；

再然后，通过网口通信读出平移位置，计算出x、y、z三轴上的单位矢量a：

由于相邻两根连杆T_i与T_i-1的变换关系式为：

式中a_i表示公共法线间的距离与垂直于所在平面内两轴的夹角；d_i表示两根连杆的相对位置；θ_i表示两根连杆的相对位置d_i和两根连杆公垂线的夹角；c表示三角函数中的cos()函数；s表示三角函数中的sin()函数；

多次移动七自由度机械臂组件，获得多组末端执行器相对于基坐标的旋转和平移量；将末端执行器相对于基坐标的多组旋转和平移参数记作多组姿态矩阵B，将上述多组标定板棋盘格上所有角点的空间坐标到相机坐标系的变换矩阵T视为外参矩阵A，之后，得到组A、B，利用AX＝XB计算得到相机坐标系到七自由度机械臂组件末端坐标系的旋转和平移变换，从而获得相机坐标系到七自由度机械臂组件末端坐标系的变换矩阵T_e；

最后，移动七自由度机械臂组件、并使用深度相机对采摘目标进行拍照，像素坐标系与世界坐标系之间的转换公式获得采摘目标的空间坐标；再利用变换矩阵T_e以及标定板棋盘格上所有角点的空间坐标到相机坐标系的变换矩阵T，获得采摘目标相对于末端执行器坐标系下的位姿。