CN113812262B - 一种基于机器视觉的油茶果采摘机器人 - Google Patents

Abstract

本发明公开了一种基于机器视觉的油茶果采摘机器人，包括运输平台、六自由度机械臂、末端执行器、输送管、收集筐和视觉装置；所述收集筐、六自由度机械臂分别安装在运输平台的前部和中部；所述末端执行器安装在六自由度机械臂的第六轴上，并通过输送管与收集筐相连通；所述末端执行器包括左机械臂杆、连接杆、右机械臂杆、外部齿梳、果实轨道、电机、行星轮机构、大轴承、小轴承。本发明的机器人能自主在油茶果田间行驶并自主完成油茶果采摘，采摘效率高。

Description

一种基于机器视觉的油茶果采摘机器人

技术领域

本发明属于农业机械领域，具体涉及一种基于机器视觉的油茶果采摘机器人。

背景技术

油茶是世界四大木本油料之一，也是中国特有的油料物种，茶油因具有独特的营养价值，被誉为“油中之王”“东方橄榄油”。市场对油茶果采摘机械的需求很大，专利CN111713259A公开了一种变间距梳刷式油茶果采摘机，包括动力装置和梳刷装置；电机接头、传动轴和采摘杆同轴布置，梳刷装置包括可转动地定位在梳刷指驱动轴末端的采摘头横杆、铰接在采摘头横杆上的若干梳刷指、与各梳刷指滑动配合的变间距横杆以及两端分别铰接在变间距横杆和传动箱外壁上的变间距支撑杆；但是当梳刷指与油茶果的接触点为油茶果的表面时，梳刷指改变间距会对油茶果产生指向油茶果中心的力，从而损伤油茶果，而且该采摘机容易被油茶果的树叶和树枝阻挡，降低采摘的效率。专利CN213662529U公开了一种齿梳回转式油茶果采收末端执行器，驱动旋转架通过主轴设置在机架上且随主轴旋转，驱动旋转架上沿周向均布有多组齿梳，保持旋转架与各组齿梳之间还分别设置有连杆，齿梳的齿梳脊梁一端端部穿过驱动旋转架与相对应连杆的一端过盈配合，连杆的另一端与保持旋转架转动连接；但是该末端执行器所占用的体积较大，而且栅板结构会导致采摘过程中整个末端执行器容易被树枝挡住，栅板结构还会横放着树枝，影响油茶果的收集和采摘效率。因此，急需研制一种采摘效率高且不损伤油茶果的采摘机器人。

发明内容

本发明的目的在于克服现有技术中存在的缺点，提供一种采摘效率高、不损伤油茶果的基于机器视觉的油茶果采摘机器人。

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

一种基于机器视觉的油茶果采摘机器人，包括运输平台1、六自由度机械臂2、末端执行器3、输送管4、收集筐5和视觉装置；所述收集筐5、六自由度机械臂2分别安装在运输平台1的前部和中部；所述视觉装置包括工控机71、双目立体相机72和相机架73，工控机71安装在运输平台上，双目立体相机72通过相机架73安装在六自由度机械臂2上；所述末端执行器3安装在六自由度机械臂2的第六轴上，并通过输送管4与收集筐5相连通。

所述末端执行器3包括左机械臂杆31、连接杆32、右机械臂杆33、外部齿梳34、果实轨道35、电机36、行星轮机构37、大轴承38、小轴承39；所述行星轮机构37包括太阳轮371、行星轮372和齿轮圈373，所述电机36安装在左机械臂杆31内并与行星轮机构37的太阳轮371相连接，太阳轮371与行星轮372相啮合，齿轮圈373套在太阳轮371和行星轮372的外圈，齿轮圈373与外部齿梳34相连接；外部齿梳34包括连接环和仿手指钩子，外部齿梳的连接环与左机械臂杆31、右机械臂杆33之间均设有大轴承38，以防止外部齿梳34转动时与机械臂杆摩擦；所述外部齿梳34内有果实轨道35，果实轨道35与右机械臂杆33之间装有小轴承39，以减小回转时零件间的摩檫力，果实轨道35为圆柱形斜坡状，即圆柱的径向进行切割，从而使得其轴向形成一个斜坡，因此掉落在果实轨道上的油茶果可以沿着斜坡滚出果实轨道，斜坡的低端与输送管4相连，进入果实轨道35的油茶果会通过输送管4掉入收集筐5内。

优选的，所述外部齿梳34的仿手指钩子安装在连接环的圆周上，整个圆周上有三行仿手指钩子，即每一行相差60°，且一行中每两个仿手指钩子之间的距离根据成熟油茶果的大小进行设计，保证成熟的油茶果可以被顺利采摘下来；所述仿手指钩子的外形为类似人的手指，其安装的一端为垂直于圆周的圆柱，另一端是带有弧度的圆柱，其作用是能够钩住油茶果，保证采摘有效性；所述连接环由两个圆环和三条弧形板组成，两个圆环用于连接两端的轴承，三条弧形板夹在两个圆环之间，弧形板彼此间相差60°，而弧形板之间的距离可以让油茶果顺利掉落到果实轨道上。

所述末端执行器的工作原理是：末端执行器的外部齿梳在旋转过程中，外部齿梳上的仿手指钩子会从下至上的钩住油茶果与树枝相接的地方，因为外部齿梳会连续旋转，由于钩子的作用力，油茶果会与树枝分离；同时仿手指钩子会托住油茶果并将其由下往上运送，当仿手指钩子运动到外部齿梳运动轨迹的顶端时，由于重力的作用，油茶果会顺着仿手指钩子掉落到果实轨道上，由于果实轨道的结构特点，在重力的作用下，油茶果会沿着果实轨道滚入输送管中。而且，由于末端执行器对油茶果的作用点在油茶果与树枝的连接处，末端执行器的外部齿梳由下至上的方向进行转动，因此不会对油茶果造成损伤；同时，本发明以束为单位进行油茶果采摘，在完成油茶果识别与运动轨迹规划后，末端执行器移动到一束油茶果的位置，然后对这束油茶果进行采摘，因此采摘效率很高。

所述末端执行器的工作步骤是：工控机完成运动轨迹的规划后，六自由度机械臂将末端执行器带到油茶果前，电机启动，与电机36相连的太阳轮371转动以带动行星轮372转动，从而带动齿轮圈373转动，与齿轮圈373相固定连接的外部齿梳34便可进行回转运动，进而带动仿手指钩子转动；仿手指钩子在转动过程中就会把油茶果从树上采摘下来，并在仿手指钩子运动到圆周轨迹的最高点时，油茶果掉落到果实轨道。

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

(1)本发明的机器人能自主在油茶果田间行驶并自主完成油茶果采摘，采摘效率高。

(2)本发明的末端执行器为新型的梳式结构，通过与机械臂的配合能精确采摘油茶果，采用回转梳刮的方式将油茶果从树枝上采摘下来，再通过末端执行器内的果实轨道使采摘后的油茶果能直接进入收集筐完成收集。

(3)本发明采用机器视觉，使机器人实现完全自主的采摘，摘效率和精确性大幅提升。

(4)本发明末端执行器的采摘方式以一束油茶果为单位进行采摘，有较高的采摘效率；而且本发明末端执行器的仿手指钩子在采摘过程中作用于油茶果与树枝的连接处，对油茶果的损伤较少。

附图说明

图1为油茶果采摘机器人的整体结构示意图。

图2为末端执行器的结构示意图。

图3为末端执行器的部分结构示意图。

图4为末端执行器的部分结构的左视图。

图5为末端执行器的部分结构的右视图。

图6为果实轨道的结构示意图。

图7为油茶果采摘机器人的工作流程图。

图8为六自由度机械臂的控制流程图。

图9为油茶果采摘机器人的视觉流程图。

图中：1、运输平台；2、六自由度机械臂；3、末端执行器；31、左机械臂杆；32、连接杆；33、右机械臂杆；34、外部齿梳；35、果实轨道；36、电机；37、行星轮机构；371、太阳轮；372、行星轮；373、齿轮圈；38、大轴承；39、小轴承；4、输送管；5、收集筐；6、履带装置；71、工控机；72、双目立体相机；73、相机架。

具体实施方式

为了便于理解本发明，下面将结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但是，不以任何形式限制本发明。应该指出的是，对本领域的技术人员来说，在不脱离本发明构思的前提下，本发明还可以做出若干变形和改进，这些都属于本发明的保护范围。

实施例1

如图1所示，一种基于机器视觉的油茶果采摘机器人，包括运输平台1、六自由度机械臂2、末端执行器3、输送管4、收集筐5、履带装置6和视觉装置；所述收集筐5、六自由度机械臂2和视觉装置中的工控机71分别安装在运输平台1上方的前端、中部、后端；所述末端执行器3安装在六自由度机械臂2的第六轴上，并通过输送管4使末端执行器3与收集筐5相连通；所述履带装置6安装在运输平台1的下方，给机器人提供移动动力。

所述六自由度机械臂2包括一轴底座、二轴机械臂、三轴机械臂、四轴机械臂、五轴机械臂关节、六轴末端执行器，且依次相连接；一轴底座固定在运输平台1上方的中部位置，在一轴底座下方的运输平台中装有机械臂控制电路板，在机械臂连接的关节处有舵机、减速器和电位计；进行油茶果采摘的末端执行器3安装在第六轴上。所述运输平台1的四周装有距离传感器、红外线传感器，内部装有速度传感器、加速度传感器、位置传感器和GPS定位器；距离传感器和红外线传感器用于收集机器人周围的环境信息并反馈传输给机器人的控制系统；速度传感器、加速度传感器和位置传感器用于收集机器人当前的移动速度和位置信息并反馈传输给机器人的控制系统；GPS定位器用于收发机器人的位置和方向信息；控制系统根据接收到的传感器数据控制机器人移动。所述双目立体相机72通过相机架73安装在二轴机械臂上，双目立体相机用于拍摄油茶果树图像，拍摄下来的图像传输给视觉装置的图像处理器，图像处理器对双目立体相机72拍摄的油茶果树图像中的油茶果进行目标识别，将识别后的油茶果位置信息传输给机器人的控制系统，控制系统通过油茶果的位置信息对六自由度机械臂进行控制，使机械臂将末端执行器伸到油茶果旁进行采摘。

如图2、3、4、5所示，所述末端执行器3包括左机械臂杆31、连接杆32、右机械臂杆33、外部齿梳34、果实轨道35、电机36、行星轮机构37、大轴承38、小轴承39，所述电机安装在左机械臂杆31内并与行星轮机构37相连接，行星轮机构37的齿轮圈373与外部齿梳34相固定连接，外部齿梳34与左机械臂杆31之间设有大轴承38，该轴承的作用是防止外部齿梳34转动时与左机械臂杆31摩擦，同样在外部齿梳34与右机械臂杆33之间设有大轴承38，当电机36开始运作后，与电机36相连的太阳轮371转动以带动行星轮372转动，从而带动齿轮圈373转动，与齿轮圈373相固定连接的外部齿梳34便可进行回转运动，所述外部齿梳34内有果实轨道35，果实轨道35与右机械臂杆33之间装有小轴承39，该轴承的作用同样也是减小回转时零件间的摩檫力，外部齿梳34进行回转运动时可将油茶果从树枝上摘下，摘下后的油茶果会因为重力落入果实轨道35中，果实轨道35为圆柱形斜坡状(如图6所示)，斜坡低端与输送管4相连，进入果实轨道35的油茶果会通过输送管4掉入收集筐5内。

如图7所示，基于机器视觉的油茶果采摘机器人的应用，包括下述步骤：

(1)前期准备：根据农场的实际种植情况对农场进行建模，并根据建模后的地图进行路径规划，将规划好的路径信息导入机器人的控制系统内，使机器人能自主的按照规定好的路径行进和停留作业；

(2)在田地中工作时，油茶果采摘机器人从指定位置开始，工控机中的控制系统根据规划好的路径信息发出控制信号给运输平台，从而控制机器人的整体移动，在移动过程中红外线传感器和距离传感器负责感应前进方向上的障碍物信息，速度、加速度和位置传感器负责实时检测机器人当时的速度、加速度和位置信息，GPS定位器将实时收发机器人当前的定位和姿态，所有传感器信息在机器人工作过程中都实时反馈到机器人的控制系统中，机器人控制系统将实时监控机器人的状态，若传感器检测到障碍物时，机器人能自主偏离预定轨迹，并根据红外线传感器和距离传感器所获得的环境信息进行自主的避障，若检测到没有足够空间进行避障时，机器人会对远程监控的工作人员进行提醒，前方障碍物移除后便可继续按照规定路线进行；

(3)当机器人到达设定好的油茶果树附近时，机器人将转动面向油茶果树，双目立体相机对油茶果树进行拍摄，然后将图像传入机器人的图像处理器中对图像中的油茶果进行识别和定位，机器人根据识别出的油茶果位置信息对六自由度机械臂进行控制，将末端执行器送至目标油茶果附近；

(4)末端执行器开始运作，末端执行器的外部齿梳开始回转，机械臂继续将末端执行器向目标油茶果不停靠近，该过程中双目立体相机继续拍摄并把图像传给机器人的图像处理器，当目标油茶果掉落后，机器人再根据图像处理器所得到的图像信息进一步控制机械臂运作至下一目标油茶果，并对其进行采摘，采摘后的油茶果会掉入末端执行器的果实轨道中并通过输送管进入收集筐内；

(5)当双目立体相机拍摄到当前油茶果树中无可采摘的油茶果时，机器人继续按着预定轨道前进，到下一棵油茶果树前并重复上述采摘动作。

如图8所示，六自由度机械臂的控制过程包括以下步骤：机器人的控制系统将控制信号传给机械臂的控制电路板，控制电路板控制机械臂内的电机转动并同时带动一系列齿轮组减速器，减速后传动至输出舵盘；舵机的输出轴和位置反馈电位计是相互连接的，舵盘转动的同时，带动位置反馈电位计，然后电位计输出一个电压信号给控制电路板进行反馈，在控制过程中采用了PID控制，PID控制能有效提高控制的反应速度，精确率，有效提高控制系统的鲁棒性并能减少控制的响应时间和控制误差，控制电路板在接受到反馈后进行调节，再进一步调整电机转动的方向和速度。

如图9所示，所述双目立体相机的视觉识别，包括下述步骤：

(1)前期训练：拍摄不同背景、不同光照强度下的油茶果在油茶果树上的照片，然后将照片输入到计算机中进行图像预处理，包括图片去噪、滤波，再对图像的颜色差进行加强；将处理后的图片用labelimg软件进行标注，标注后的结果是使用矩形框将一束油茶果框起来，这也是末端执行器所要作用的对象；将标注好的照片作为训练集、测试集与验证集投入到YOLOv5算法中进行学习，根据实现标注好的矩形框对生成ROI(Region ofInterest)区域不断修正，使得YOLOv5算法最终得到的是以油茶果树为背景的油茶果的Feature Maps(特征图)；

(2)图像识别过程：

(2-1)先使用张正友标定法对双目立体相机进行标定，采集标定板不同位姿图片，提取图片中角点像素坐标，通过单应矩阵计算出相机的内外参数初始值，利用非线性最小二乘法估计畸变系数，再使用极大似然估计法优化参数，从而完成相机的标定；

(2-2)将油茶果图像实时传输到工控机中，工控机对相机传输过来的图像进行降噪、滤波、灰度化的图像预处理；再利用已经训练好的YOLOv5的学习模型对相机传输过来的照片进行识别，在照片中框出油茶果的位置；

(2-3)建立三维点云：双目立体相机获得的图像可分为左右两幅图像，在相机标定完成之后，需要将左右两张图像合并成一张图像，使用SGBM算法进行立体匹配，得到油茶果图片的视差图；双目立体相机拍摄的图像包含油茶果在图像中的位置信息(u，v)，且在经过对视差图进行分割之后，可获得油茶果的深度值d，根据这些信息，建立油茶果的三位点云图，再对点云图进行直通滤波、统计滤波和半径滤波的降噪处理，能得到一片噪声少且平整的油茶果三维点云；

(2-4)规划运动轨迹：根据图像识别后确定的一束油茶果的位置，进而确定该油茶果在三维点云中的位置；读取该束油茶果的最大与最小的深度值d，两者作差之后与上述识别的Feature Maps中的矩形框形成空间中的矩形块，得出整个矩形块的八个点的空间位置，求出矩形块的质心与法向量，计算出矩形块在空间中的位姿(x,y,z,a,b,c)，其中x、y、z为该矩形块质心的空间位置，a、b、c为矩形块的转动角；取矩形块的空间中的左下角与右上角两个点为运动轨迹的开始点与结束点，利用机器人的机械臂参数进行运动学反算得出六自由度机械臂各个关节的转动角度，使得末端执行器能采摘到一束油茶果的所有果实。

以上所述仅为本发明的实施例，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均为等效的置换方式，都包含在本发明的保护范围之内。

Claims (8)

1.一种基于机器视觉的油茶果采摘机器人，其特征在于：包括运输平台、六自由度机械臂、末端执行器、输送管、收集筐和视觉装置；所述收集筐、六自由度机械臂分别安装在运输平台的前部和中部；所述末端执行器安装在六自由度机械臂的第六轴上，并通过输送管与收集筐相连通；

所述末端执行器包括左机械臂杆、连接杆、右机械臂杆、外部齿梳、果实轨道、电机、行星轮机构、大轴承、小轴承；所述行星轮机构包括太阳轮、行星轮和齿轮圈，所述电机安装在左机械臂杆内并与行星轮机构的太阳轮相连接，太阳轮与行星轮相啮合，齿轮圈套在太阳轮和行星轮的外圈，齿轮圈与外部齿梳相连接；外部齿梳包括连接环和仿手指钩子，外部齿梳的连接环与左机械臂杆、右机械臂杆之间均设有大轴承；所述外部齿梳内有果实轨道，果实轨道与右机械臂杆之间装有小轴承，果实轨道为圆柱形斜坡状，即圆柱的径向进行切割，从而使得其轴向形成一个斜坡，斜坡的低端与输送管相连。

2.根据权利要求1所述的油茶果采摘机器人，其特征在于：所述外部齿梳的仿手指钩子安装在连接环的圆周上，整个圆周上有三行仿手指钩子，每两个仿手指钩子之间的距离根据成熟油茶果的大小进行设计；所述仿手指钩子的外形为类似人的手指，其安装的一端为垂直于圆周的圆柱，另一端是带有弧度的圆柱；所述外部齿梳的连接环由两个圆环和三条弧形板组成，两个圆环用于连接两端的轴承，三条弧形板夹在两个圆环之间，弧形板彼此间相差60°。

3.根据权利要求1所述的油茶果采摘机器人，其特征在于：所述视觉装置包括工控机、双目立体相机和相机架，工控机安装在运输平台上，双目立体相机通过相机架安装在六自由度机械臂上。

4.根据权利要求1所述的油茶果采摘机器人，其特征在于：所述六自由度机械臂包括一轴底座、二轴机械臂、三轴机械臂、四轴机械臂、五轴机械臂关节、六轴末端执行器，且依次相连接；一轴底座固定在运输平台上方的中部位置，在一轴底座下方的运输平台中装有机械臂控制电路板，在机械臂连接的关节处有舵机、减速器和电位计；进行油茶果采摘的末端执行器安装在第六轴上。

5.根据权利要求1所述的油茶果采摘机器人，其特征在于：所述运输平台的四周装有距离传感器、红外线传感器，内部装有速度传感器、加速度传感器、位置传感器和GPS定位器；距离传感器和红外线传感器用于收集机器人周围的环境信息并反馈传输给机器人的控制系统；速度传感器、加速度传感器和位置传感器用于收集机器人当前的移动速度和位置信息并反馈传输给机器人的控制系统；GPS定位器用于收发机器人的位置和方向信息；控制系统根据接收到的传感器数据控制机器人移动。

6.一种权利要求1～5中任一项所述的油茶果采摘机器人的应用，其特征在于包括下述步骤：

(1)前期准备：根据农场的实际种植情况对农场进行建模，并根据建模后的地图进行路径规划，将规划好的路径信息导入机器人的控制系统内，使机器人能自主的按照规定好的路径行进和停留作业；

(2)在田地中工作时，油茶果采摘机器人从指定位置开始，工控机中的控制系统根据规划好的路径信息发出控制信号给运输平台，从而控制机器人的整体移动，在移动过程中红外线传感器和距离传感器负责感应前进方向上的障碍物信息，速度、加速度和位置传感器负责实时检测机器人当时的速度、加速度和位置信息，GPS定位器将实时收发机器人当前的定位和姿态，所有传感器信息在机器人工作过程中都实时反馈到机器人的控制系统中，机器人控制系统将实时监控机器人的状态，若传感器检测到障碍物时，机器人能自主偏离预定轨迹，并根据红外线传感器和距离传感器所获得的环境信息进行自主的避障，若检测到没有足够空间进行避障时，机器人会对远程监控的工作人员进行提醒，前方障碍物移除后便可继续按照规定路线进行；

(3)当机器人到达设定好的油茶果树附近时，机器人将转动面向油茶果树，双目立体相机对油茶果树进行拍摄，然后将图像传入机器人的图像处理器中对图像中的油茶果进行识别和定位，机器人根据识别出的油茶果位置信息对六自由度机械臂进行控制，将末端执行器送至目标油茶果附近；

(4)末端执行器开始运作，末端执行器的外部齿梳开始回转，机械臂继续将末端执行器向目标油茶果不停靠近，该过程中双目立体相机继续拍摄并把图像传给机器人的图像处理器，当目标油茶果掉落后，机器人再根据图像处理器所得到的图像信息进一步控制机械臂运作至下一目标油茶果，并对其进行采摘，采摘后的油茶果会掉入末端执行器的果实轨道中并通过输送管进入收集筐内；

(5)当双目立体相机拍摄到当前油茶果树中无可采摘的油茶果时，机器人继续按着预定轨道前进，到下一棵油茶果树前并重复上述采摘动作。

7.根据权利要求6所述的油茶果采摘机器人的应用，其特征在于：所述末端执行器的工作步骤是：工控机完成运动轨迹的规划后，六自由度机械臂将末端执行器带到油茶果前，电机启动，与电机相连的太阳轮转动以带动行星轮转动，从而带动齿轮圈转动，与齿轮圈相固定连接的外部齿梳便可进行回转运动，进而带动仿手指钩子转动；仿手指钩子在转动过程中就会把油茶果从树上采摘下来，并在仿手指钩子运动到圆周轨迹的最高点时，油茶果掉落到果实轨道。

8.根据权利要求6所述的油茶果采摘机器人的应用，其特征在于：所述双目立体相机的视觉识别，包括下述步骤：

(1)前期训练：拍摄不同背景、不同光照强度下的油茶果在油茶果树上的照片，然后将照片输入到计算机中进行图像预处理，包括图片去噪、滤波，再对图像的颜色差进行加强；将处理后的图片用labelimg软件进行标注，标注后的结果是使用矩形框将一束油茶果框起来，这也是末端执行器所要作用的对象；将标注好的照片作为训练集、测试集与验证集投入到YOLOv5算法中进行学习，根据实现标注好的矩形框对生成ROI(Region of Interest)区域不断修正，使得YOLOv5算法最终得到的是以油茶果树为背景的油茶果的Feature Maps；

(2)图像识别过程：

(2-1)先使用张正友标定法对双目立体相机进行标定，采集标定板不同位姿图片，提取图片中角点像素坐标，通过单应矩阵计算出相机的内外参数初始值，利用非线性最小二乘法估计畸变系数，再使用极大似然估计法优化参数，从而完成相机的标定；

(2-2)将油茶果图像实时传输到工控机中，工控机对相机传输过来的图像进行降噪、滤波、灰度化的图像预处理；再利用已经训练好的YOLOv5的学习模型对相机传输过来的照片进行识别，在照片中框出油茶果的位置；

(2-3)建立三维点云：双目立体相机获得的图像可分为左右两幅图像，在相机标定完成之后，需要将左右两张图像合并成一张图像，使用SGBM算法进行立体匹配，得到油茶果图片的视差图；双目立体相机拍摄的图像包含油茶果在图像中的位置信息(u，v)，且在经过对视差图进行分割之后，可获得油茶果的深度值d，根据这些信息，建立油茶果的三位点云图，再对点云图进行直通滤波、统计滤波和半径滤波的降噪处理，能得到一片噪声少且平整的油茶果三维点云；

(2-4)规划运动轨迹：根据图像识别后确定的一束油茶果的位置，进而确定该油茶果在三维点云中的位置；读取该束油茶果的最大与最小的深度值d，两者作差之后与上述识别的Feature Maps中的矩形框形成空间中的矩形块，得出整个矩形块的八个点的空间位置，求出矩形块的质心与法向量，计算出矩形块在空间中的位姿(x,y,z,a,b,c)，其中x、y、z为该矩形块质心的空间位置，a、b、c为矩形块的转动角；取矩形块的空间中的左下角与右上角两个点为运动轨迹的开始点与结束点，利用机器人的机械臂参数进行运动学反算得出六自由度机械臂各个关节的转动角度，使得末端执行器能采摘到一束油茶果的所有果实。

Images