CN109773788B - 一种果蔬采摘机器人及其控制方法 - Google Patents

Abstract

本发明公开了一种果蔬采摘机器人及其控制方法。该系统，包括移动平台、采摘机械臂、采摘手、控制系统及拖车；移动平台与拖车连接；采摘机械臂安装在移动平台上，采摘手安装在采摘机械臂的末端；采摘手边沿安装有获取目标物位姿信息的eye‑in‑hand摄像机，采摘手安装有采摘果蔬的真空采摘器，真空采摘器通过真空管将果蔬输送到拖车上；并联调整机构由六个伺服电动缸驱动实现摄像机的六自由度位姿调节。控制系统用于处理摄像机图像信息及位姿调节指令的生成与发送。此系统具有成本低、精度高、适用范围广等优点，便于在多种农业果蔬采摘过程中应用。

Description

一种果蔬采摘机器人及其控制方法

技术领域

本发明涉及机器人机构设计技术领域，具体是一种果蔬采摘机器人及其控制方法。

背景技术

随着现代精细农业的发展，农业机械的自动化、智能化程度要求越来越高。近年来，我国的果蔬产量逐年增长，并且果蔬的采摘期往往较短，这就造成果蔬采摘时需要在短时间投入大量劳动力进行高强度工作。但目前我国的老龄化程度却日益严重，这就造成了劳动力的短缺和成本的增加。为缓解劳动力短缺、减轻劳动强度、降低生产成本，国内外学者研究了不少果蔬采摘机器人。目前的研究主要多采用串联形式，虽工作空间大，但串联机器人固有的特性(存在累积误差、动态特性差等)决定了此类采摘机器人采摘效率较低。

发明内容

针对以上采摘机器人存在的问题，本发明提供了一种果蔬采摘机器人及其控制方法，该果蔬采摘机器人在3自由度串联采摘机械臂末端增加3自由度并联采摘手，利用并联机构动态性能好、运动控制精度高的优点来弥补串联机构动态性能差、运动精度低的缺点。

为达到上述目的，本发明采取如下技术方案：

一种果蔬采摘机器人，包括移动平台、采摘机械臂、采摘手、控制系统及拖车；

所述的移动平台与拖车连接；所述的移动平台上安装有用于获取果蔬园中全局位置信息的北斗或GPS定位模块、用于获取移动平台姿态信息的电子罗盘模块、用于获取果树信息完成移动平台局部定位的识别测量装置以及用于获取当前待采摘果蔬的eye-to-hand摄像机；

所述的采摘机械臂安装在移动平台上，所述的采摘手安装在采摘机械臂的末端；采摘手边沿安装有获取目标物位姿信息的eye-in-hand摄像机，采摘手安装有采摘果蔬的真空采摘器，真空采摘器通过真空管将果蔬输送到拖车上；

所述的控制系统包括：

计算机或图像处理设备，用于处理所有摄像机的图像信息；

运动控制器，一方面用于获取识别测量装置的处理结果、测距传感器测得的移动平台与果蔬的距离信息、北斗或GPS全局位置信息及电子罗盘的方向信息，并依据获得的信息规划移动平台的运动路径，控制移动平台在果园行走；另一方面用于获取eye-to-hand摄像机和eye-in-hand摄像机的图像处理结果，对采摘机械臂和采摘手进行轨迹规划和运动控制，并控制真空采摘器动作，实现果蔬采摘。

作为本发明的进一步改进，所述的移动平台采用两动力轮软差速驱动方式，通过控制系统调节两轮转速实现平台的转弯或直行。

作为本发明的进一步改进，所述的采摘机械臂为三自由度串联结构，包括三个串联的关节，第一关节由垂直于移动平台上安装平面的移动副构成，第二关节、第三关节为转动副，转动副的转动轴线与第一关节移动副的移动方向平行，用于调节采摘手相对于移动平台的位置。

作为本发明的进一步改进，所述的采摘手为三自由度并联结构，包括动平台、定平台和三条主动链，动平台和定平台为中空的圆环，eye-in-hand摄像机安装在动平台上边沿；真空采摘器安装在动平台中空部分；三条主动链在动、定平台上均匀分布，主动链由伺服电动缸构成，伺服电动缸一端通过球铰链与定平台连接，伺服电动缸另一端通过转动铰链与动平台连接；定平台固定在采摘机械臂的末端。

作为本发明的进一步改进，所述的识别测量装置包括：导航摄像机、测距传感器、步进电机和霍尔传感器；步进电机和霍尔传感器设置在移动平台上，导航摄像机、测距传感器与步进电机的电机轴连接；霍尔传感器及固定于电机轴上的磁钢用于标记电机零位。

一种果蔬采摘机器人的控制方法，包括以下步骤：

1)控制系统通过处理北斗或GPS信息判断采摘机器人是否达到果树行尽头，若没有达到尽头，则利用识别测量装置获取采摘机器人相对于果树的局部位置，融合电子罗盘的方向信息实现移动平台的行走控制，使采摘机器人运动；

2)当采摘机器人移至果树旁时，利用eye-to-hand摄像机获取果蔬的分布情况及成熟度信息，对采摘顺序进行规划，并控制采摘机械臂移动至当前需要采摘的果蔬附近；

3)利用eye-in-hand摄像机获取当前果蔬的位姿信息，据此调整采摘手动平台的位姿，使采摘手正对待采摘果蔬，打开真空采摘器完成果蔬采摘作业。

作为本发明的进一步改进，步骤1)中利用识别测量装置获取采摘机器人相对于果树的局部位置，其具体方法为：首先通过导航摄像机识别到果树树干；然后控制步进电机转动保证导航摄像机和测距传感器始终正对树干，并获取该测量装置与当前测量树干的距离，根据步进电机控制指令可得到该测量装置当前的偏转角度；最后结合识别测量装置测量相邻果树树干的信息，得到移动平台当前行驶方向与果树所在行偏离的角度及距离。

作为本发明的进一步改进，控制移动平台在果树行间运动或移动至待采摘果树处具体步骤为：

首先通过安装于平台单侧或双侧识别测量装置中的摄像头识别到果树树干；然后通过控制步进电机转动保证摄像头和超声传感器跟随树干转动，并利用超声波获取该测量装置距离当前测量树干的距离l_i(i＝1,2,3,4)，根据步进电机控制指令可得到该测量装置当前的偏转角度θ_i(i＝1,2,3,4)；平台左侧测量装置与所测量树干之间距离在移动平台横向的分量为：

AG＝l₁·cosθ₁

BE＝l₂·cosθ₂

结合果树行间距d₁和移动平台宽度d₂计算得到平台几何中心偏离果树行中线的距离d：

根据EF和d的值即可构建运动控制算法，根据北斗或GPS、电子罗盘的信号可获取机器人当前在果园中的位姿信息，并控制移动平台在果树行间运动或移动至待采摘果树处。

作为本发明的进一步改进，步骤2)中，对采摘机械臂进行控制的转换矩阵为

其中，θ₁、θ₂和L₁、L₂分别代表采摘机械臂两转动关节的关节角和连杆长度，h为移动关节的移动距离。

作为本发明的进一步改进，步骤3)中采摘手动平台的位姿由下式确定

r＝B_b(i)+l_a(i)·E_a(i)-R·M_m(i),i＝1,2,3

其中，B_b(i)＝R_b·(cosθ_i,sinθ_i,0)^T为定平台铰点在定坐标系下的位矢；R_b为定平台的半径；

为点B_b(i),M_m(i)在定、动坐标系下的位置角；l_a(i),E_a(i)分别为主动支链i的杆长和单位矢量；

为动平台相对于定平台的旋转矩阵；M_m(i)＝R_m·(cosθ_i,sinθ_i,0)^T为动平台铰点在动坐标系下的位矢；R_m为动平台半径。

相对于现有技术，本发明具有以下有益效果：

本发明的果蔬采摘机器人具有结构简单、成本低、测量精度高、适应性强的优点。本发明利用移动平台实现了移动作业；利用串联采摘臂实现了机器人较大的采摘工作空间；利用并联采摘手实现了对待采摘果实的快速、精确定位；利用真空采摘器实现了果实的快速、无损采摘；利用拖车实现了果实的装载，相对于其它将果实直接存放在采摘机器人上的方式，减少了果实转移的程序，且统一采用标准拖车转载的形式，便于自动化操作，提高了效率。相对于现有的采摘机器人，本发明首次将串联采摘臂和并联采摘手组合的形式引入果蔬采摘机器人中，结合了串联机构工作空间大和并联机构精度高、动态性能好的优点，使系统具有更好的综合性能。

进一步，利用串联机构工作空间大的优点来弥补并联机构工作空间小的缺点，利用并联机构动态性能好、运动控制精度高的优点来弥补串联机构动态性能差、运动精度低的缺点。将串联采摘机械臂和并联采摘手进行组合，取长补短，设计一种工作空间大、采摘效率高的新型采摘机器人。为便于多个果树的采摘作业，该采摘机器人还配有适合果园行走的移动平台。为装载采摘下来的果实，并减少果实转移次数，该采摘机器人还配有专用拖车。

本发明的控制方法包括：摄像机参数标定；系统初始化；控制系统利用北斗或GPS、电子罗盘、识别测量装置获取机器人当前的位姿信息，并控制移动平台在果树行间运动或移动至待采摘果树跟前；控制系统根据eye-to-hand摄像机获取的果实信息，对采摘顺序及当前采摘机械臂的运动路径进行规划，并控制采摘机械臂移动至当前待采摘果实附近；控制系统根据eye-in-hand摄像机获取的果实信息控制并联采摘手快速对准待采摘果实，并控制真空采摘器动作完成采摘任务。

附图说明

图1是果蔬采摘机器人整体示意图；

图2是采摘机械臂示意图；

图3是采摘手示意图；

图4是识别测量装置树干测量示意图；

图5是果蔬采摘机器人工作流程图。

图中标号的具体含义为：1.采摘机械臂，2.eye-to-hand摄像机，3.移动平台，4.空压机，5.识别测量装置，6.拖车，7.采摘手，8.eye-in-hand摄像机，9.果树，10.第一关节，11.第二关节，12.第三关节，13.小臂，14.大臂，15.导轨，16.丝杠，17.转动铰链，18.动平台，19.伺服电动缸，20.定平台，21.球铰链。

具体实施方式

下面结合附图和具体实施例对本发明做进一步说明，本发明不限于以下实施例。

如图1所示，本发明一种果蔬采摘机器人(以下以采摘苹果为例进行说明)，包括移动平台3、采摘机械臂1、采摘手7、控制系统及拖车6；

移动平台3用于实现采摘机器人的移动行走，并拖动拖车6移动，其行走方式可以采用履带式、轮式或腿式等适合在果园行走的形式；移动平台3采用两动力轮软差速驱动方式，软差速即两轮独立转动，通过控制系统调节两轮转速实现平台的转弯或直行，相较于传统的采用艾克曼转向机构的方式，软差速结构简单、转向灵活。

移动平台上安装有用于获取机器人在果园中全局位置信息的北斗或GPS定位模块、用于获取平台姿态信息的电子罗盘模块、用于获取果树信息完成移动平台局部定位的识别测量装置5，以及用于获取当前待采摘果树上苹果大致位置的eye-to-hand摄像机2，eye-to-hand摄像机2安装于转台上用以通过转台转动获取果树行两侧果树上的苹果信息。

其中，所述的识别测量装置5可以根据需要在移动平台单侧安装2到3套或左右两侧安装4或6套，每套主要包括5部分：导航摄像机、测距传感器、步进电机、霍尔传感器和保护罩；测距传感器可以是激光测距仪，也可以是小锥角的超声波传感器以降低成本；步进电机基座和霍尔传感器固定于移动平台上，导航摄像机、测距传感器及保护罩与步进电机电机轴固定；霍尔传感器及固定于电机轴上的磁钢用于标记电机零位；保护罩用于保护导航摄像机、测距传感器、霍尔传感器及步进电机。

采摘机械臂1安装在移动平台3上，用于将采摘手7送到果树9附近；采摘机械臂1为三自由度串联的形式，第一关节10主要由垂直于移动平台3上安装平面的移动副构成，用于实现二、三关节在竖直方向的运动，移动副由导轨15和丝杠16组成；第二、三关节为转动副，转动轴线第一关节10移动副的移动方向平行，用于调节采摘手7相对于移动平台3的位置。

采摘手7安装在采摘机械臂1的末端，实现对准并采摘果树9上苹果的功能；所述的采摘手7采用三自由度并联形式，包括动平台18、定平台20和三条主动链，动平台18和定平台20为中空的圆环，动平台18上边沿安装有获取苹果位姿信息的eye-in-hand摄像机8，动平台18中空部分安装有采摘苹果的真空采摘器，真空管穿过定平台20中空部分并最终将苹果输送到拖车上；三条主动链在动、定平台上均匀分布，主动链由伺服电动缸19构成，一端通过球铰链21与定平台联接，另一端通过转动铰链17与动平台18联接；定平台20固定在采摘机械臂1的末端。

控制系统一方面感知果园环境控制移动平台3在果树行间运动或运动至待采摘的果树9旁边，另一方面获取苹果成熟度及苹果相对与采摘手7的位姿信息，控制采摘机械臂1和采摘手7动作，实现对成熟苹果的自动采摘。

拖车6用于装载机器人采摘的果实，其通过弹簧等柔性铰链由移动平台3拖动行走，也可以自带动力行走。所述的控制系统包括：

计算机或图像处理设备，用于处理所有摄像机的图像信息；

运动控制器，一方面获取导航摄像机的图像处理结果、测距传感器测得的移动平台3与果树的距离信息、北斗或GPS全局位置信息及电子罗盘的方向信息，并对这些信息进行融合处理规划出移动平台3的运动路径，控制平台在果园行走；另一方面，获取eye-to-hand摄像机2和eye-in-hand摄像机8的图像处理结果，根据苹果的分布情况及成熟度信息，规划采摘顺序，对采摘机械臂1和采摘手7进行轨迹规划和运动控制，并控制真空采摘器动作，实现对成熟果实的采摘。

本发明利用串联机构工作空间大的优点来弥补并联机构工作空间小的缺点，利用并联机构动态性能好、运动控制精度高的优点来弥补串联机构动态性能差、运动精度低的缺点。将串联采摘机械臂和并联采摘手进行组合，取长补短，设计一种工作空间大、采摘效率高的新型采摘机器人。为便于多个果树的采摘作业，该采摘机器人还配有适合果园行走的移动平台。为装载采摘下来的果实，并减少果实转移次数，该采摘机器人还配有专用拖车。

如图5所示，本发明的工作过程如下：

当果园机器人放置于果园中准备作业时，需要先给导航装置上电运行，此时各识别测量装置自动归零位，同时板卡读取GPS定位模块和电子罗盘的当前值，以判断移动平台当前在果园中所处的方位。

当果园移动平台在果树行间行驶时，果树树干测量示意图如图4所示。其中，四周两排6个黑色的圆点代表果园的两行果树，中间的虚线代表两行果树的中线，黑色矩形框代表移动平台，四周的4个小圆圈A、B、C、D代表4个识别测量装置(此处以平台两侧各装2个为例进行说明)，箭头方向代表移动平台的前进方向。首先通过安装于平台单侧或双侧识别测量装置中的摄像头识别到果树树干；然后通过控制步进电机转动可保证摄像头和超声传感器跟随树干转动，并利用超声波获取该测量装置距离当前测量树干的距离l_i(i＝1,2,3,4)，根据步进电机控制指令可得到该测量装置当前的偏转角度θ_i(i＝1,2,3,4)。图4中，E,F,G,M,N分别为图中所示果树树干位置到过测量装置横向连线所做垂线与过测量装置横向连线的交点。此时，平台左侧测量装置与所测量树干之间距离在移动平台横向(图中过测量装置的两条平行虚线)的分量为：

AG＝l₁·cosθ₁

BE＝l₂·cosθ₂

右侧分量的计算方法与此相同。则图中EF的长度即反映了移动平台当前的前进方向偏离果树行向的程度。EF越大，代表移动平台偏斜程度越大，反之亦然。而AG-DN与BE-CM的均值d则反映了平台几何中心偏离果树行中线的距离。在知道果树行间距的情况下，也可以采用单侧测量的方法，此时可以利用AG与BE的均值，结合果树行间距d₁和移动平台宽度d₂计算得到d。

根据EF和d的值即可构建运动控制算法，根据北斗或GPS、电子罗盘的信号可获取机器人当前在果园中的位姿信息，并控制移动平台在果树行间运动或移动至待采摘果树跟前。

根据eye-to-hand摄像机获取的果实信息，对采摘顺序及当前采摘机械臂的运动路径进行规划，并控制采摘机械臂移动至当前待采摘果实附近；根据eye-in-hand摄像机获取的果实信息控制并联采摘手快速对准待采摘果实，并控制真空采摘器动作完成采摘任务。

如图5所示，本发明还提供了一种果蔬采摘机器人的控制方法，包括以下步骤：

1)对各摄像机进行参数标定，对同一品种的果园只需标定一次，或一年标定一次；

2)系统初始化，包括各系统参数初始化、各驱动关节和伺服电动缸初始化；

3)控制系统通过处理北斗或GPS信息判断采摘机器人是否达到果树9行尽头，若没有达到尽头，则利用识别测量装置5获取采摘机器人相对于果树的局部位置，融合电子罗盘的方向信息实现移动平台3的行走控制，使采摘机器人沿果树行行走或者移动至待采摘的果树9旁边；

步骤3)中利用识别测量装置5获取采摘机器人相对于果树的局部位置，其具体方法为：首先通过导航摄像机识别到果树树干；然后控制步进电机转动保证导航摄像机和测距传感器始终正对树干，并获取该测量装置与当前测量树干的距离，根据步进电机控制指令可得到该测量装置当前的偏转角度；最后结合其它识别测量装置5测量相邻果树树干的信息，即可得到移动平台当前行驶方向与果树所在行偏离的角度及距离。

4)当采摘机器人移至果树旁时，利用eye-to-hand摄像机2获取苹果的分布情况及成熟度信息，对采摘顺序进行规划，并控制采摘机械臂1移动至当前需要采摘的苹果附近；

步骤4)中所述的采摘顺序规划采用时间最短的规划方法，所述的对采摘机械臂1进行控制的转换矩阵为

其中，θ₁、θ₂和L₁、L₂分别代表采摘机械臂两转动关节的关节角和连杆长度，h为移动关节的移动距离。

5)利用eye-in-hand摄像机8获取当前苹果的位姿信息，据此调整采摘手7动平台的位姿，使采摘手7正对苹果，打开真空采摘器完成苹果采摘作业。

步骤5)中采摘手(7)动平台的位姿由下式确定：

r＝B_b(i)+l_a(i)·E_a(i)-R·M_m(i),i＝1,2,3

其中，B_b(i)＝R_b·(cosθ_i,sinθ_i,0)^T为定平台铰点在定坐标系下的位矢；R_b为定平台的半径；

为点B_b(i),M_m(i)在定、动坐标系下的位置角；l_a(i),E_a(i)分别为主动支链i的杆长和单位矢量；

为动平台相对于定平台的旋转矩阵；M_m(i)＝R_m·(cosθ_i,sinθ_i,0)^T为动平台铰点在动坐标系下的位矢；R_m为动平台半径。

因苹果往往成簇生长，采用此种方式的另一个好处是串联采摘机械臂移动到一簇果实旁边的时候可以保持不动，利用并联采摘手对该簇果实进行快速采摘。此外，如果将图像处理系统中的识别对象特征换成其它果蔬，该采摘机器人同样可以适用于其它对应果蔬的采摘，极大拓宽了该采摘机器人的适用范围，具有非常重要的理论研究价值及现实意义。

该系统主要包括六自由度并联调整机构、摄像机、陀螺仪及控制系统。并联调整机构由六个伺服电动缸驱动实现摄像机的六自由度位姿调节。摄像机和陀螺仪分别安装在并联调整机构动平台的上、下平面中心，用于获取目标的图像信息和摄像机姿态信息。控制系统用于处理摄像机图像信息及位姿调节指令的生成与发送。此系统具有成本低、精度高、适用范围广等优点，便于在多种农业果蔬采摘过程中应用。

最后应当说明的是:以上实施例仅用以说明本发明的技术方案而非对其限制；尽管参照较佳实施例对本发明进行了详细的说明，所属领域的普通技术人员应当理解：依然可以对本发明的具体实施方式进行修改或者对部分技术特征进行等同替换；而不脱离本发明技术方案的精神，其均应涵盖在本发明请求保护的技术方案范围当中。

Claims (4)

1.一种果蔬采摘机器人的控制方法，其特征在于，

所述果蔬采摘机器人，包括移动平台(3)、采摘机械臂(1)、采摘手(7)、控制系统及拖车(6)；

所述的移动平台(3)与拖车(6)连接；所述的移动平台(3)上安装有用于获取果蔬园中全局位置信息的北斗或GPS定位模块、用于获取移动平台(3)姿态信息的电子罗盘模块、用于获取果树信息完成移动平台(3)局部定位的识别测量装置(5)以及用于获取当前待采摘果蔬的eye-to-hand摄像机(2)；

所述的采摘机械臂(1)安装在移动平台(3)上，所述的采摘手(7)安装在采摘机械臂(1)的末端；采摘手(7)边沿安装有获取目标物位姿信息的eye-in-hand摄像机(8)，采摘手(7)安装有采摘果蔬的真空采摘器，真空采摘器通过真空管将果蔬输送到拖车(6)上；

所述的采摘机械臂(1)为三自由度串联结构，包括三个串联的关节，第一关节(10)由垂直于移动平台(3)上安装平面的移动副构成，第二关节(11)、第三关节(12)为转动副，转动副的转动轴线与第一关节(10)移动副的移动方向平行，用于调节采摘手(7)相对于移动平台(3)的位置；

所述的采摘手(7)为三自由度并联结构，包括动平台(18)、定平台(20)和三条主动支链，动平台(18)和定平台(20)为中空的圆环，eye-in-hand摄像机(8)安装在动平台(18)上边沿；真空采摘器安装在动平台(18)中空部分；三条主动支链在动、定平台上均匀分布，主动支链由伺服电动缸(19)构成，伺服电动缸(19)一端通过球铰链(21)与定平台(20)连接，伺服电动缸(19)另一端通过转动铰链(17)与动平台(18)连接；定平台(20)固定在采摘机械臂(1)的末端；

所述的识别测量装置(5)包括：导航摄像机、测距传感器、步进电机和霍尔传感器；步进电机和霍尔传感器设置在移动平台上，导航摄像机、测距传感器与步进电机的电机轴连接；霍尔传感器及固定于电机轴上的磁钢用于标记电机零位；

所述的控制系统包括：

计算机或图像处理设备，用于处理所有摄像机的图像信息；

运动控制器，一方面用于获取识别测量装置(5)的处理结果、测距传感器测得的移动平台(3)与果树的距离信息、北斗或GPS定位模块获取的全局位置信息及电子罗盘模块的姿态信息，并依据获得的信息规划移动平台(3)的运动路径，控制移动平台(3)在果园行走；另一方面用于获取eye-to-hand摄像机(2)和eye-in-hand摄像机(8)的图像处理结果，对采摘机械臂(1)和采摘手(7)进行轨迹规划和运动控制，并控制真空采摘器动作，实现果蔬采摘；

控制方法包括以下步骤：

1)控制系统通过处理北斗或GPS定位模块获取的信息判断采摘机器人是否达到果树行尽头，若没有达到尽头，则利用识别测量装置(5)获取采摘机器人相对于果树的局部位置，融合电子罗盘模块的姿态信息实现移动平台(3)的行走控制，使采摘机器人运动；

2)当采摘机器人移至果树旁时，利用eye-to-hand摄像机(2)获取果蔬的分布情况及成熟度信息，对采摘顺序进行规划，并控制采摘机械臂(1)移动至当前需要采摘的果蔬附近；

3)利用eye-in-hand摄像机(8)获取当前果蔬的位姿信息，据此调整采摘手(7)的动平台的位姿，使采摘手(7)正对待采摘果蔬，打开真空采摘器完成果蔬采摘作业；

步骤3)中采摘手(7)的动平台的位姿由下式确定

r＝B_b(i)+l_a(i)·E_a(i)-R·M_m(i),i＝1,2,3

其中，B_b(i)＝R_b·(cosθ_i,sinθ_i,0)^T为定平台铰点在定坐标系下的位矢；R_b为定平台的半径；

为点B_b(i),M_m(i)在定、动坐标系下的位置角；l_a(i),E_a(i)分别为主动支链i的杆长和单位矢量；

为动平台相对于定平台的旋转矩阵；M_m(i)＝R_m·(cosθ_i,sinθ_i,0)^T为动平台铰点在动坐标系下的位矢；R_m为动平台半径。

2.根据权利要求1所述的果蔬采摘机器人的控制方法，其特征在于，步骤1)中利用识别测量装置(5)获取采摘机器人相对于果树的局部位置，其具体方法为：首先通过导航摄像机识别到果树树干；然后控制步进电机转动保证导航摄像机和测距传感器始终正对树干，并获取识别测量装置与当前测量树干的距离，根据步进电机控制指令可得到识别测量装置当前的偏转角度；最后结合识别测量装置(5)测量相邻果树树干的信息，得到移动平台当前行驶方向与果树所在行偏离的角度及距离。

3.根据权利要求1所述的果蔬采摘机器人的控制方法，其特征在于，控制移动平台在果树行间运动或移动至待采摘果树处具体步骤为：

首先通过安装于平台单侧或双侧的识别测量装置中的摄像头识别到果树树干；然后通过控制步进电机转动保证摄像头和超声传感器跟随树干转动，并利用超声传感器获取识别测量装置距离当前测量树干的距离l_j，j＝1,2,3,4，根据步进电机控制指令可得到识别测量装置当前的偏转角度α_j；平台左侧识别测量装置与所测量树干之间距离在移动平台横向的分量为：

AG＝l₁·cosα₁

BE＝l₂·cosα₂

结合果树行间距d₁和移动平台宽度d₂计算得到平台几何中心偏离果树行中线的距离d：

根据EF和d的值即可构建运动控制算法，根据北斗或GPS定位模块、电子罗盘模块的信号可获取机器人当前在果园中的位姿信息，并控制移动平台在果树行间运动或移动至待采摘果树处；EF为BE与AG之间的差值。

4.根据权利要求1所述的果蔬采摘机器人的控制方法，其特征在于，步骤2)中，对采摘机械臂(1)进行控制的转换矩阵为

其中，β₁、β₂和L₁、L₂分别代表采摘机械臂第二关节、第三关节的关节角和连杆长度，h为第一关节的移动距离。

Images