CN111955289B - 一种双孢菇采摘机器人的末端执行器 - Google Patents

Abstract

本发明提供了一种双孢菇采摘机器人的末端执行器，包括：连接壳体、力矩电机、夹持机构、电机控制器、扭矩传感器；所述夹持机构包括左右对称设置的两个夹爪机构，夹爪机构包括平行四边形连杆机构和夹爪，夹爪具有菇盖接触面，菇盖接触面的朝向为朝左或朝右，用于与菇盖的侧面配合；所述力矩电机用于向所述主动杆输入扭矩，带动所述夹爪左右移动以夹持/放松菇盖；所述扭矩传感器用于采集电机负载扭矩并输送给电机控制器；所述电机控制器用于调控力矩电机的输入电压；本发明提供的双孢菇采摘机器人的末端执行器，具有恒力夹持、采摘机械损伤小等特点，能够代替人工采摘双孢菇，能适应不同大小的双孢菇采摘。

Description

一种双孢菇采摘机器人的末端执行器

技术领域

本发明涉及作物采摘机械，尤其涉及双孢菇采摘机器人的末端执行器。

背景技术

现有技术中用于食用菌自动采摘的采摘机器人，如公告号CN210143460 U的中国实用新型专利公开的采摘机械手，包括手臂和夹持套，夹持套能够弹性形变，用来夹持双孢菇的菇盖，夹持套在手臂的推拉、旋转动作配合下，能够完成按压、夹持、旋转、摘取四个动作。由于末端执行器作为采摘机器人与菇盖直接接触的机构，夹持力不能过大，否则会造成蘑菇外观伤痕，影响蘑菇品质，末端执行器如何能够在不损伤菇肉的前提下完成双孢菇采摘是采摘机器人设计的一个难点。

发明内容

本发明的目的是提供一种双孢菇采摘机器人的末端执行器，能够可靠夹持并减少机械损伤。

本发明的一种双孢菇采摘机器人的末端执行器，包括：连接壳体、力矩电机、夹持机构、电机控制器、扭矩传感器；所述夹持机构包括左右对称设置的两个夹爪机构，夹爪机构包括平行四边形连杆机构和夹爪，定义左右方向为两个夹爪对合夹持的夹持方向，左右方向延伸的平面为水平面；所述平行四边形连杆机构包括：主动杆、从动杆、输出杆，主动杆、从动杆分别与连接壳体铰接，主动杆、从动杆与连接壳体铰接的铰接轴线为第一、二固定铰接轴线；所述夹爪固定在输出杆上，夹爪具有菇盖接触面，菇盖接触面的朝向为朝左或朝右，用于与菇盖的侧面配合；所述力矩电机用于向所述主动杆输入扭矩，带动所述夹爪左右移动以夹持/放松菇盖；所述扭矩传感器用于采集电机负载扭矩并输送给电机控制器；所述电机控制器用于调控力矩电机的输入电压；所述第一、二固定铰接轴线平行而处于一个共有平面，所述共有平面与所述水平面呈设定角度，以使所述菇盖接触面的朝向在所述夹爪移动时始终保持不变。

进一步的，所述力矩电机为直流电机，所述电机控制器包括处理器、储存器；所述存储器中存储有夹持控制程序，夹持控制程序包括执行以下步骤的指令：

S01：启动：接收到采摘指令，控制直流电机在设定电压下启动，驱动夹持机构的夹爪合拢，当电机的负载扭矩超限，即判断出夹持机构的夹爪接触到菇盖且电机堵转，进入S02；

S02：目标设定：根据预先获得的电机堵转时夹持机构主动杆与夹持方向的夹角、夹持力阈值、电机负载扭矩阈值与输入电压阈值之间的关系，根据当前堵转时的夹持机构主动杆与夹持方向的夹角，获得与夹持力阈值对应的输入电压阈值以及电机负载阈值，进入S03；

S03：增压增扭：以输入电压阈值的上限值作为输入电压目标值，每次以设定量调控当前输入电压增大，进入S04；

S04：增压监控1：每次增大输入电压后，判断扭矩传感器采集到电机负载扭矩是否高于电机负载扭矩阈值下限值设定余量，若是，停止调控并维持当前输入电压，进入S06；否则进入S05；

S05：增压监控2：判断当前输入电压是否达到输入电压阈值的上限值，若是，停止调控，进入S12；否则返回S03；

S06：夹持监控1：持续监控扭矩传感器采集到的电机负载扭矩，判断是否处于电机负载扭矩阈值的范围内，若是，进入S11；否则进入S07；

S07：夹持监控2：判断扭矩传感器采集到电机负载扭矩是高于电机负载扭矩阈值上限值还是低于电机负载扭矩阈值下限值，若高于电机负载扭矩阈值上限值，需要减压减扭，进入S08，若低于电机负载扭矩阈值下限值，需要增压增扭，进入S03；

S08：减压减扭：以输入电压阈值的下限值为输入电压目标值，每次以设定量减少当前输入电压，进入S09；

S09：减压监控1：每次减少输入电压后，判断扭矩传感器采集到电机负载扭矩是否低于电机负载扭矩阈值上限值设定余量，若是，停止调控并维持当前输入电压，进入S06；否则进入S10；

S10：减压监控2：判断当前输入电压是否达到输入电压阈值的下限值，若是，停止调控，进入S12；否则返回S08；

S11：判断是否接收到放松指令，若是则控制直流电机反向输出动力，驱动夹持机构松开对菇盖的夹持；否则返回S06继续监控；

S12：电机停机，生成故障信号。

进一步的，所述夹爪具有弧面仿形结构，弧面仿形结构与菇盖的边缘形状吻合，所述菇盖接触面设置在弧面仿形结构上。

进一步的，所述弧面仿形结构为内凹柱面，内凹柱面的母线与所述水平面垂直。

进一步的，所述夹爪还具有柔性材料层，柔性材料层贴在所述弧面仿形结构上形成菇盖接触面。

进一步的，所述夹爪在左右移动的同时在垂直于夹持方向的竖直方向上有位移，用于使两个夹爪在左右相向移动夹持菇盖的同时，还朝向远离连接壳体的方向移动而向下按压菇盖，以使双孢菇的菇柄压实培养料土。

本发明提供的双孢菇采摘机器人的末端执行器，采用平行四边形机构保持菇盖接触面的朝向始终与夹持方向一致，菇盖接触面在夹爪合拢过程中不会倾斜，所以不会刮擦菇盖表面而导致菇盖损伤，在夹持过程中两个夹爪的菇盖接触面始终沿夹持方向相对施力，而且力矩电机能够在堵转时输出恒定扭矩维持夹持力恒定，从而能够稳定的夹持菇盖。本发明机械结构简单，制造成本低廉，能够代替人工采摘双孢菇，对实现双孢菇采摘作业的机械化、自动化和智能化以及促进我国双孢菇规模化、工厂化的生产具有重要意义。

进一步的，本发明采用基于预先获得的堵转时夹持机构主动杆与夹持方向的夹角、夹持力阈值、电机负载扭矩阈值与输入电压阈值之间的关系，对电机输入电压进行控制以获得合适的夹持力，堵转时夹持机构主动杆与夹持方向的夹角的预先设定和采集，与夹爪张开角度相关联，能适应不同大小的双孢菇采摘，自动化程度高。

附图说明

图1是本发明末端执行器实施例的外部结构示意图；

图2是图1打开连接壳体后显示出部分内部结构的示意图；

图3是本发明末端执行器实施例的三个控制阶段示意图；

图4是本发明末端执行器实施例的控制原理图；

图5是本发明末端执行器实施例的夹持机构受力分析示意图；

图6是本发明末端执行器实施例的夹持机构运动过程示意图；

图7是本发明末端执行器实施例的夹持控制流程图。

图中：

1.夹持机构末端；2.夹持机构；3.第一爪；4.连接壳体；5.第二爪；6.柔性材料；7.第二连接杆；8.第一连接杆；9.第三连接杆；10.齿轮组；11.直流电机；12.传动机构；13.第六连接杆；14.第四连接杆；15.第五连接杆。

具体实施方式

本发明的双孢菇采摘机器人的末端执行器(以下简称末端执行器)的实施例，如图1、图2所示，包括：连接壳体4、直流电机10、齿轮组11、夹持机构2、电机控制器(图中未显示)、扭矩传感器12。

连接壳体4用于将末端执行器其他部分安装在一起，并留有与采摘机器人采摘机械臂连接的连接口。直流电机10通过螺丝固定在连接壳体4上，齿轮组11通过连接销与连接壳体4连接，电机控制器同样通过螺丝固定在连接壳体上，电机控制器包括处理器和存储器，存储器中储存有在处理器上运行的程序，用于控制直流电机10的输入电压。

直流电机10是力矩电机，力矩电机能够在堵转条件下长时间输出恒定扭矩，直流电机10是末端执行器夹持动作及夹持力的动力来源，由于直流电机10可以长时间处于堵转状态，工作性能可靠。

齿轮组11为力矩电机输出端的减速器，用于对电机的输出减速增扭，将动力传递给夹持机构2。优选的，所述齿轮组11为保证传动过程柔顺性，所用材料为磁性材料。

扭矩传感器12安装于直流电机10与齿轮组11之间，能够实时向电机控制器反馈直流电机10的负载扭矩。

夹持机构2用于夹持双孢菇的菇盖，包括左右对称设置的第一夹爪机构3和第二夹爪机构5，两个夹爪机构的结构相同，均包括平行四边形连杆机构和夹爪1，定义左右方向为夹持方向，即夹持机构2的开合方向，前后方向为平行四边形连杆机构的铰接轴的轴线方向，夹爪1的爪末端延伸方向为向上。第一夹爪机构3的平行四边形连杆机构包括：主动杆8、从动杆9、输出杆7，其中主动杆8、从动杆9是一端与连接壳体4铰接的连架杆，主动杆8、从动杆9与连接壳体4铰接的铰接轴线为第一、二铰接轴线，输出杆7为连杆。第二夹爪机构5的平行四边形连杆机构包括：主动杆14、从动杆13、输出杆15。

由于两个夹爪机构的结构相同，以第一夹爪机构3为例进行详细介绍，主动杆8具有动力输入端，动力输入端与连接壳体4铰接，且与齿轮组11末端齿轮固定连接，用于输入动力。

输出杆15上固定有夹爪1，所述夹爪1为与双孢菇菇盖接触部件，夹爪1具有弧面仿形结构，与菇盖的边缘形状吻合，以增大接触面积减小接触应变力。具体的，本实施例中的弧面仿形结构为内凹圆柱面，内凹柱面的母线与水平面垂直。夹爪1还具有柔性材料层6，柔性材料层6贴在弧面仿形结构上形成菇盖接触面，用以减少机械损伤。夹爪机构的平行四边形连杆机构是为了保证菇盖接触面的朝向在夹爪1夹持菇盖的过程中始终不变，即菇盖接触面始终朝向夹持方向而不会倾斜。由于输出杆15的两个活动铰接点的连线与两个固定铰接点的连线是永远平行的，两个活动铰接点连线方向将保持不变，将夹爪1固定在输出杆15上，就使得夹爪1的夹持方向为左右方向，菇盖接触面的朝向也是左右方向且保持不变。平行四边形连杆机构保证了夹爪1在空间上相对连接壳体的姿态一致性，夹持机构2的夹持方向为左右水平方向，则夹爪1始终保持上下竖直状态，菇盖接触面朝向夹持方向且保持不变。两夹爪运动过程一直朝向相对状态且保持不变，使双孢菇菇盖两侧的夹持受力均衡。

平行四边形连杆机构的具体结构和运动过程结合图5、6所示，为了更好的反映平行四边形连杆机构的各连杆的几何关系，以及夹爪1的菇盖接触面，在图5、6中将平行四边形连杆机构的各铰接点分别用A-D表示：第一固定铰接点A，第二固定铰接点D，第一活动铰接点C，第二活动铰接点D；各连杆及夹爪1用线段表示：主动杆8为DC、从动杆9为AB、输出杆7为BC，夹爪1为CP，设定夹持机构的夹持方向为水平方向，即OX方向，图中CD与水平方向的夹角为θ₁，BC与水平方向夹角为θ₂，AB与水平方向夹角为θ₃，DA与水平方向方向夹角θ₄，BC与CP的夹角∠BCP；CD、BC、AB、DA的长分别为l₁-l₄。其中l₁＝l₃，l₂＝l₄，θ₁＝θ₃，θ₂＝θ₄；由于A、D两点固定，BC与AD平行，所以θ₂＝θ₄，且都是定值。由于设定∠BCP＝90°+θ₂，从而保证CP(即夹爪1)始终与水平方向垂直，即夹爪1沿夹持方向运动时夹爪1保持与夹持方向垂直。

本实施例中在直流电机上还安装有用于采集直流电机10在夹持机构夹持过程转过总角度θ_m的角度传感器，具体为旋转编码器。采集该角度的主要目的，是通过齿轮组传动比换算夹持机构2的主动杆8(CD)与夹持方向(水平面)的夹角θ₁，当然在其它实施例中，也可以直接在铰接点D处安装角度传感器来测量主动杆在夹持过程中与夹持方向的夹角θ₁。

为了实现对上述平行四边形连杆机构运动过程的控制和夹持力的控制，本发明采用如图3、图4的控制策略，该控制策略是以电机控制器调控直流电机电压的方式，使夹持机构达到适宜的夹持力(即抓取力)，该策略在实际运行前，需要先对末端执行器的直流电机负载扭矩、电机转动角度、直流电机电压、夹持力的关系进行标定，其原理分析和过程如下：

1、推导直流电机的负载扭矩、电机转动角度与输入电压之间的关系。

根据：

直流电机10电磁扭矩：

M_e＝K_ti (1)

基尔霍夫定律：

反电势：

功率放大器：

u_m＝K_su_c (4)

上式中：L为直流电机10电感，H；R为电枢电阻，Ω；

E为直流电机10反电势，V；K_b为直流电机10反电势常数；θ_m为直流电机10转动角度；

u_m为直流电机10电枢电压，V；K_s为功率放大系数；u_c为控制电压，V。

考虑到通常多数场合L小到可以忽略不计，因此，如果L＝0，则由上式整理可得到：

它表示直流电机10的输入电压和电磁扭矩、电机转动角度之间的关系。

再结合直流电机10的动力学特性方程：

J为直流电机10转动惯量，kg·m²；D为直流电机10黏滞摩擦阻力系数；M_Coul为未知的库仑摩擦力矩，N·m；M_n为折算到直流电机10端的负载扭矩，N·m。

当电机堵转时，θ_m变化量为0，

为0，由式(5)、(6)就可得到直流电机10的负载扭矩M_n与输入电压u_c之间的关系，并表达为负载扭矩M_n关于输入电压u_c的函数一：

M_n＝f1(u_c)，具体为：

2、推导夹持力F_px与直流电机的负载扭矩M_n与输入电压u_c之间的关系。

电机的负载扭矩M_n输出经过齿轮组11的传动环节进行转换：

M_l＝nM_n (8)

式中：n为齿轮组传动比；M_l为经齿轮组11传动后负载扭矩，N·m；

由式(7)、(8)，就可得到直流电机10的传动后负载扭矩与输入电压之间的关系，并表达为传动后负载扭矩M_l关于输入电压u_c的函数二：。

M_l＝f2(u_c)，具体为：

如图5、图6所示，夹持机构2在夹持阶段的受力分析及运行过程，由于ABCD是平行四边形连杆机构的四个铰接轴的轴线投影形成的铰接点，图中CD与水平方向(夹持方向)的夹角为θ₁，BC与水平方向夹角为θ₂，AB与水平方向夹角为θ₃，DA与水平方向方向夹角θ₄，BC与CP的夹角∠BCP；CD、BC、AB、DA的长分别为l₁-l₄。其中l₁＝l₃，l₂＝l₄，θ₁＝θ₃，θ₂＝θ₄；由于A、D两点固定，BC与AD平行，所以θ₂＝θ₄，且都是定值。设定∠BCP＝90°+θ₂，用于保证CP始终与水平方向垂直，CP始终为水平移动。

夹持机构2受力分析如图5，齿轮组对主动杆CD施加的扭矩为M_l，被夹持的双孢菇的菇盖对夹爪施加的反作用力为F_P，F_P的方向垂直于夹爪的延伸方向，与加持机构2施加的夹持力方向相反，F_P的大小与夹持力的大小相同；以及双孢菇与夹爪间的摩擦力F_f，F_f方向与夹爪延伸方向一致。本实施例中，夹爪在夹持时电机堵转而静止，末端执行器在夹持阶段近似为平衡力系，以夹持机构整体为研究对象，并将四杆机构视为刚体轻质杆，忽略杆的质量，忽略双孢菇的质量，摩擦力F_f相比夹持力过小也在计算中被忽略，列出以D点为中心的扭矩平衡方程：

M_l-F_P(l₁sinθ₁+l)＝0 (10)

由方程(10)得出：

式中l为双孢菇受力点到夹爪端部距离，实际夹持过程中l值变化不大，可简化为定值,如取夹爪长度的一半。θ_l为夹持机构2的主动杆与夹持方向的夹角，rad；通过角度传感器可得到θ₁，反馈给控制系统，结合上述式(9)获得的M_l实现自适应恒力控制。

具体的，结合式(9)负载扭矩M_l与输入电压u_c之间的关系，就能获得夹持力F_p与θ₁、输入电压u_c之间的关系，并表达为F_p关于输入电压u_c、角度θ₁的函数三：

F_p＝f3(u_c，θ₁)，具体为：

同时，如图6所示，夹爪移动时从P点到P’点，它在垂直于夹持方向的竖直方向有位移，但位移量很小，两个夹爪在左右相向移动的同时朝向远离连接壳体的方向移动，其作用是在采摘时先向下按压双孢菇的菇盖，让双孢菇的菇柄压实与菇柄接触的培养料土，减少双孢菇采摘菇柄带出培养料土量，避免影响采后双孢菇洁净度。具体的，本发明的末端执行器在夹持阶段，双孢菇受力简化为P点受力，双孢菇受力为夹爪受力的反作用力，双孢菇除了受到夹持力(与F_p大小相等，方向相反)，还受到竖直向下的摩擦力(与F_f大小相等，方向相反)，从而符合双孢菇采摘要求。所以本发明的末端执行器在夹持阶段能够对双孢菇有向下按压力保证其采摘效果。

3、根据预先选定的不同大小的采集对象进行直流电机10在堵转状态下的夹持试验，以能够夹持住采集对象不脱落、不损伤为目标，标定适宜的夹持力阈值F_p，并采集对应的工况数据，根据前面获得的函数一(式7)和函数三(式12)，就能够标定出在直流电机10堵转状态下，有确定的夹持机构2的主动杆与夹持方向的夹角θ₁时夹持力F_p阈值、输入电压u_c阈值、负载扭矩M_n阈值的对应关系，例如下表的形式：

表1：电机堵转状态下的夹持机构的主动杆与夹持方向的夹角、夹持力阈值、输入电压阈值、负载扭矩阈值的对应关系

	θ<sub>1</sub>	F<sub>p</sub>	u<sub>c</sub>	M<sub>n</sub>
					1	θ<sub>1</sub>1	F<sub>p</sub>1(阈值)	u<sub>c</sub>1(阈值)	M<sub>n</sub>1(阈值)
2	θ<sub>1</sub>2	F<sub>p</sub>2(阈值)	u<sub>c</sub>2(阈值)	M<sub>n</sub>2(阈值)
					……	……	……	……	……

上述双孢菇采摘机器人的末端执行器的实施例在进行夹持操作时，如图3所示，分为三个阶段：

1、合拢阶段：电机控制器接收到采摘指令后，电机控制器控制直流电机10在合拢电压下启动，合拢电压为直流电机10的低负载电压，直流电机10以设定的低转速运动输出扭力，经齿轮组11传动将动力传送到夹持机构2，夹持机构2以设定的低速合拢，这样与双孢菇接触瞬间冲击载荷小，避免了双孢菇损伤；直至接触到双孢菇，夹爪1被阻挡不能继续合拢，直流电机10堵转，合拢阶段结束。在合拢阶段，电机控制器保持直流电机10在极低的合拢电压下运动，即使直流电机10堵转而增大扭矩也达不到所需的夹持力。本实施例中，为实现夹持机构2低速合拢，是根据已知的直流电机10低负载电压与转速的关系(属于现有技术)，设定合拢电压的电压值。

2、夹持阶段：夹持机构2的两个夹爪1接触到双孢菇后，直流电机10堵转，受到反作用力的直流电机10的负载增大，扭矩传感器将采集到的电机负载扭矩输给电机控制器，当控制器获得的负载扭矩超过第一限值时，夹持阶段开始。

夹持阶段开始后，如图4所示，电机控制器调控直流电机的输入电压，根据预先获得的堵转时夹持机构的主动杆与夹持方向的夹角、夹持力阈值、电机负载扭矩阈值与输入电压阈值之间的关系(该关系可见表1)，根据当前堵转时的夹持机构的主动杆与夹持方向的夹角，获得与夹持力阈值对应的输入电压阈值以及电机负载阈值，以输入电压阈值的上限值作为输入电压目标值，调控当前输入电压增大，直至夹持力增大与菇盖的反作用力平衡，扭矩传感器采集到的电机负载扭矩高于电机负载扭矩阈值下限值设定余量，停止调控输入电压，所述设定余量取上下限值差值的一半，即使没有达到输入电压目标值也停止继续增大。电机负载扭矩达标是停止调控的条件，由于主动杆与夹持方向的夹角、输入电压和夹持力是由预先的试验数据确定的，夹持力范围在试验时被设定为对双孢菇不造成损伤且能有效完成采摘目标，这样就使特定输入电压目标值下的夹持机构2对双孢菇不造成损伤且能有效完成采摘目标夹持力，直流电机10在堵转状态下保持负载扭矩恒定不变，直到采摘过程结束。

在夹持阶段，由于菇盖的流变特定而会发生变形，比如菇盖凹陷反作用力会变小，凹陷复原反作用力会变大，另外直流电机10堵转电压、时间与输出扭矩关系非成正比，夹持力也会发生波动，因此在夹持阶段中夹持力与菇盖的反作用力的平衡将可能被打破，夹爪1随之移动，夹持量随之变化，电机也会随之转动而导致夹持机构主动杆与夹持方向的夹角变化，解除堵转状态，即电机从堵转状态下开始转动，此时重新调控输入电压。

下面分两种情况来介绍：

a，扭矩传感器采集到电机负载扭矩高于电机负载扭矩阈值上限值，以输入电压阈值的下限值为输入电压目标值，电机控制器减少当前输入电压，负载扭矩随之减小，直至扭矩传感器采集到的电机负载扭矩下降低于电机负载扭矩阈值上限值设定余量，停止调控并维持当前输入电压。

b，扭矩传感器采集到电机负载扭矩低于电机负载扭矩阈值下限值，以输入电压阈值的上限值为输入电压目标值，电机控制器增大当前输入电压，负载扭矩随之增大，直至扭矩传感器采集到的电机负载扭矩上升高于电机负载扭矩阈值下限值设定余量，停止调控并维持当前输入电压。

上述电压调控过程中，所述设定余量均取上下限值差值的一半，也可以根据实际情况进行调整。如果当前输入电压达到输入电压目标值(增大达上限，减小达到下限)，扭矩传感器采集到的电机负载扭矩仍未达到目标，说明发生了故障，比如传感器故障，或是夹错了采摘目标，则停止工作。

3、放松阶段，电机控制器接收到放松指令后，控制系统控制直流电机10动力反向输出，夹持机构2松开对双孢菇的夹持。放松阶段的触发条件是：达到设定的采摘过程所需时间，该时间由采摘机械臂带动末端执行器从双孢菇生长位置到达双孢菇收集位置的移动距离决定。或者是用图像识别来看采摘机器人采摘机械臂将采摘对象移动到位，移到到位则向电机控制器发出发送指令。

上述过程中，由于夹爪与菇盖接触且扭矩传感器采集到的电机负载扭矩超过第一扭矩之后，菇盖的形变量很小，将电机角度的变化忽略不计，以初次获得的电机角度为准。

上述的夹持控制过程，如图7所示，是由电机控制器的处理器执行存储器中的夹持控制程序，夹持控制程序包括执行以下步骤的指令：

S01：启动：接收到采摘指令，控制直流电机在设定电压下启动，驱动夹持机构的夹爪合拢，当电机的负载扭矩超限，即判断出夹持机构的夹爪接触到菇盖且电机堵转，进入S02；

S02：目标设定：根据预先获得的堵转时主动杆与夹持方向的夹角、夹持力阈值、电机负载扭矩阈值与输入电压阈值之间的关系，根据当前堵转时的主动杆与夹持方向的夹角，获得与夹持力阈值对应的输入电压阈值以及电机负载阈值，进入S03；

S03：增压增扭：以输入电压阈值的上限值作为输入电压目标值，每次以设定量调控当前输入电压增大，进入S04；

S04：增压监控1：每次增大输入电压后，判断扭矩传感器采集到电机负载扭矩是否高于电机负载扭矩阈值下限值设定余量，若是，停止调控并维持当前输入电压，进入S06；否则进入S05；

S05：增压监控2：判断当前输入电压是否达到输入电压阈值的上限值，若是，停止调控，进入S12；否则返回S03；

S06：夹持监控1：持续监控扭矩传感器采集到的电机负载扭矩，判断是否处于电机负载扭矩阈值的范围内，若是，进入S11；否则进入S07；

S07：夹持监控2：判断扭矩传感器采集到电机负载扭矩是高于电机负载扭矩阈值上限值还是低于电机负载扭矩阈值下限值，若高于电机负载扭矩阈值上限值，需要减压减扭，进入S08，若低于电机负载扭矩阈值下限值，需要增压增扭，进入S03；

S08：减压减扭：以输入电压阈值的下限值为输入电压目标值，每次以设定量减少当前输入电压，进入S09；

S09：减压监控1：每次减少输入电压后，判断扭矩传感器采集到电机负载扭矩是否低于电机负载扭矩阈值上限值设定余量，若是，停止调控并维持当前输入电压，进入S06；否则进入S10；

S10：减压监控2：判断当前输入电压是否达到输入电压阈值的下限值，若是，停止调控，进入S12；否则返回S08；

S11：判断是否接收到放松指令，若是则控制直流电机反向输出动力，驱动夹持机构松开对菇盖的夹持；否则返回S06继续监控；

S12：电机停机，生成故障信号。

Claims (4)

1.一种双孢菇采摘机器人的末端执行器，其特征在于，包括：连接壳体、力矩电机、夹持机构、电机控制器、扭矩传感器；所述夹持机构包括左右对称设置的两个夹爪机构，夹爪机构包括平行四边形连杆机构和夹爪，定义左右方向为两个夹爪对合夹持的夹持方向，左右方向延伸的平面为水平面；所述平行四边形连杆机构包括：主动杆、从动杆、输出杆，主动杆、从动杆分别与连接壳体铰接，主动杆、从动杆与连接壳体铰接的铰接轴线为第一、二固定铰接轴线，主动杆和从动杆远离连接壳体的一端分别与输出杆铰接，连接壳体上的两处铰接点之间的距离等于输出杆上的两处铰接点之间的距离；所述夹爪固定在输出杆上，夹爪具有菇盖接触面，菇盖接触面的朝向为朝左或朝右，用于与菇盖的侧面配合；所述力矩电机用于向所述主动杆输入扭矩，带动所述夹爪左右移动以夹持/放松菇盖；所述扭矩传感器用于采集电机负载扭矩并输送给电机控制器；所述电机控制器用于调控力矩电机的输入电压；所述第一、二固定铰接轴线平行而处于一个共有平面，所述共有平面与所述水平面呈设定角度，以使所述菇盖接触面的朝向在所述夹爪移动时始终保持不变，所述夹爪在左右移动的同时在垂直于夹持方向的竖直方向上有位移，用于使两个夹爪在左右相向移动夹持菇盖的同时，还朝向远离连接壳体的方向移动而向下按压菇盖，以使双孢菇的菇柄压实培养料土；

所述力矩电机为直流电机，所述电机控制器包括处理器、存储器；所述存储器中存储有夹持控制程序，夹持控制程序包括执行以下步骤的指令：

S01：启动：接收到采摘指令，控制直流电机在设定电压下启动，驱动夹持机构的夹爪合拢，当电机的负载扭矩超限，即判断出夹持机构的夹爪接触到菇盖且电机堵转，进入S02；

S02：目标设定：根据预先获得的电机堵转时夹持机构主动杆与夹持方向的夹角、夹持力阈值、电机负载扭矩阈值与输入电压阈值之间的关系，根据采集到的当前堵转时的夹持机构主动杆与夹持方向的夹角，获得与夹持力阈值对应的输入电压阈值以及电机负载阈值，进入S03；

S03：增压增扭：以输入电压阈值的上限值作为输入电压目标值，每次以设定量调控当前输入电压增大，进入S04；

S04：增压监控1：每次增大输入电压后，判断扭矩传感器采集到电机负载扭矩是否高于电机负载扭矩阈值下限值设定余量，若是，停止调控并维持当前输入电压，进入S06；否则进入S05；

S05：增压监控2：判断当前输入电压是否达到输入电压阈值的上限值，若是，停止调控，进入S12；否则返回S03；

S06：夹持监控1：持续监控扭矩传感器采集到的电机负载扭矩，判断是否处于电机负载扭矩阈值的范围内，若是，进入S11；否则进入S07；

S07：夹持监控2：判断扭矩传感器采集到电机负载扭矩是高于电机负载扭矩阈值上限值还是低于电机负载扭矩阈值下限值，若高于电机负载扭矩阈值上限值，需要减压减扭，进入S08，若低于电机负载扭矩阈值下限值，需要增压增扭，进入S03；

S08：减压减扭：以输入电压阈值的下限值为输入电压目标值，每次以设定量减少当前输入电压，进入S09；

S09：减压监控1：每次减少输入电压后，判断扭矩传感器采集到电机负载扭矩是否低于电机负载扭矩阈值上限值设定余量，若是，停止调控并维持当前输入电压，进入S06；否则进入S10；

S10：减压监控2：判断当前输入电压是否达到输入电压阈值的下限值，若是，停止调控，进入S12；否则返回S08；

S11：判断是否接收到放松指令，若是则控制直流电机反向输出动力，驱动夹持机构松开对菇盖的夹持；否则返回S06继续监控；

S12：电机停机，生成故障信号。

2.根据权利要求1所述的双孢菇采摘机器人的末端执行器，其特征在于，所述夹爪具有弧面仿形结构，弧面仿形结构与菇盖的边缘形状吻合，所述菇盖接触面设置在弧面仿形结构上。

3.根据权利要求2所述的双孢菇采摘机器人的末端执行器，其特征在于，所述弧面仿形结构为内凹柱面，内凹柱面的母线与所述水平面垂直。

4.根据权利要求2所述的双孢菇采摘机器人的末端执行器，其特征在于，所述夹爪还具有柔性材料层，柔性材料层贴在所述弧面仿形结构上形成菇盖接触面。

Images