CN114431147B - 应用于蝴蝶兰种苗切割的双臂切割装置及双臂切割方法 - Google Patents

Abstract

本发明涉及一种应用于蝴蝶兰种苗切割的双臂切割装置及双臂切割方法，属于蝴蝶兰种苗切割领域，从机械臂末端为亚克力透明板，在不损伤种苗的情况下，既能保证固定住蝴蝶兰种苗，也能够保证在手外的相机能够完整的拍摄下种苗，使得主机械臂末端能够准确地切割蝴蝶兰种苗；在主机械臂上安装深度相机，实时获取从机械臂信息，将从机械臂设置为障碍物，从而能够使用Informed‑RRT*实时进行路径规划以实现安全的切割作业；并且本发明能够通过压力传感末端实时得到压力反馈，避免机械臂末端以及种苗在切割过程中受到不必要的损伤。

Description

应用于蝴蝶兰种苗切割的双臂切割装置及双臂切割方法

技术领域

本发明涉及蝴蝶兰种苗切割领域，特别是涉及一种应用于蝴蝶兰种苗切割的双臂切割装置及双臂切割方法。

背景技术

蝴蝶兰因其花形别致，色彩艳丽，深受人们的喜爱。现如今，蝴蝶兰组织培养技术已经逐步成熟，但蝴蝶兰组种苗的移植仍然以手工为主，尤其是种苗根部切除工作。这使得工人的工作强度大且工作内容重复、枯燥，大量的工人进入培养间操作也会破坏蝴蝶兰培养所需要的无菌环境，导致种苗染病几率增加，种苗存活率较低，造成经济及人力的损失。因此，使用机械臂对蝴蝶兰种苗实现自动切割的方案显得尤为重要。

机械臂在切割空间内合理的运动路径是保证切割蝴蝶兰组培苗成功率和切割效率的关键因素。在切割蝴蝶兰种苗时，需要以一定的姿态进行切割，才能保证不伤害到种苗并且达到所要求的切割深度。常见的机械臂路径规划算法有RRT、RRT*和Informed-RRT*等。Steven M提出的快速扩展随机树(Rapidly-exploring Random Trees,RRT)算法是一种在多维空间中有效率的规划方法，但是其有规划时间长、寻找的路径非最优等问题；Karaman等人通过对RRT算法进行改进，提出了渐进最优的RRT*算法，优化了路径但是计算路径的效率下降；Jonathan D等提出了Informed-RRT*算法，对比之前的算法，该算法能够更快的搜索到最优路径，搜索效率更高。

现有的方法可在已知场景内进行路径规划，但是在蝴蝶兰切割场景中，蝴蝶兰种苗每次放置的位置不一样可能导致存在多种切割位姿，并且蝴蝶兰种苗不仅存在一个切割点，每次切割过后可能会导致种苗移动从而计算的坐标点不准确；切割过程中，不清楚什么时候种苗被完整的切除。

发明内容

本发明的目的是提供一种应用于蝴蝶兰种苗切割的双臂切割装置及双臂切割方法，以在双臂协作下准确、完全地切割蝴蝶兰种苗。

为实现上述目的，本发明提供了如下方案：

一种应用于蝴蝶兰种苗切割的双臂切割装置，所述双臂切割装置包括：从机械臂、亚克力板、主机械臂、压力传感末端、深度相机和控制器；

亚克力板设置在从机械臂的末端，作为从机械臂末端；压力传感末端设置在主机械臂的末端，作为主机械臂末端；深度相机固定在主机械臂上；

从机械臂、主机械臂、压力传感末端和深度相机均与控制器连接；

所述从机械臂用于在控制器的控制下将从机械臂末端移动至蝴蝶兰种苗后利用所述亚克力板固定蝴蝶兰种苗；

所述深度相机用于在从机械臂固定蝴蝶兰种苗后，拍摄从机械臂深度图像，并将所述从机械臂深度图像传输至控制器；所述控制器用于根据所述从机械臂深度图像获取从机械臂的位姿，并将从机械臂设定为障碍物，根据从机械臂的位姿规划主机械臂切割蝴蝶兰种苗的最优路径，进而按照最优路径控制主机械臂移动和切割；

所述压力传感末端用于切割蝴蝶兰种苗，并检测主机械臂末端的受力，同时将所述受力传输至控制器；所述控制器还用于根据所述受力判断蝴蝶兰种苗的切割结束时刻，并在切割结束时刻控制主机械臂末端停止对蝴蝶兰种苗的切割操作。

可选的，所述压力传感末端包括：螺纹连接法兰、薄膜式压力传感器、弹簧、刀夹、紧固件和刀片；

螺纹连接法兰固定在主机械臂的末端，薄膜式压力传感器设置在螺纹连接法兰的内侧；弹簧的一端与薄膜式压力传感器接触，弹簧的另一端与刀夹的一端接触；刀夹的另一端开设一个有内螺纹的固定孔，刀片的固定端设置有外螺纹，所述固定孔的内螺纹与刀片固定端的外螺纹相配合，刀片通过螺纹锁紧的方式固定在刀夹的固定孔中；所述刀片用于切割蝴蝶兰种苗；

紧固件为带有内螺纹的空心圆柱结构，紧固件的内螺纹与螺纹连接法兰的外螺纹相配合，紧固件通过螺纹锁紧的方式将螺纹连接法兰、薄膜式压力传感器、弹簧和刀夹固定为一体；

薄膜式压力传感器与控制器连接；所述薄膜式压力传感器用于检测主机械臂末端的受力，同时将所述受力传输至控制器。

可选的，所述从机械臂的末端还设置有夹爪，所述夹爪夹住所述亚克力板。

一种应用于蝴蝶兰种苗切割的双臂切割方法，所述双臂切割方法包括：

获取蝴蝶兰种苗初始图像；

根据所述蝴蝶兰种苗初始图像，确定蝴蝶兰种苗上的固定线；

利用从机械臂的亚克力板沿着所述固定线覆盖蝴蝶兰种苗的叶子，固定蝴蝶兰种苗；

获取固定后的蝴蝶兰种苗图像；

根据所述固定后的蝴蝶兰种苗图像，确定蝴蝶兰种苗的所有切割点和每个切割点对应的切割角度；

通过设置在主机械臂上的深度相机拍摄用于固定蝴蝶兰种苗的从机械臂深度图像；

根据所述从机械臂深度图像获得从机械臂在固定蝴蝶兰种苗时的位姿；

将从机械臂设定为障碍物，并根据从机械臂固定蝴蝶兰种苗时的位姿，采用Informed-RRT*算法规划主机械臂切割蝴蝶兰种苗的最优路径；

控制主机械臂末端按照所述最优路径依次对切割点以相应的切割角度进行切割。

可选的，根据所述从机械臂深度图像获得从机械臂在固定蝴蝶兰种苗时的位姿，具体包括：

根据所述从机械臂深度图像，确定从机械臂在固定蝴蝶兰种苗时的位置信息和深度信息；

根据所述位置信息和所述深度信息对从机械臂进行位姿估计，获得从机械臂在固定蝴蝶兰种苗时的位姿。

可选的，所述将从机械臂设定为障碍物，并根据从机械臂固定蝴蝶兰种苗时的位姿，采用Informed-RRT*算法规划主机械臂切割蝴蝶兰种苗的最优路径，具体包括：

将从机械臂设定为障碍物，并将主机械臂所能活动的空间减去从机械臂固定蝴蝶兰种苗时的位姿对应的空间后的空间确定为主机械臂的运动空间；

设置切割点为目标点，并在主机械臂的运动空间中采用RRT算法计算出一条到目标点的初始路径；

根据所述初始路径的长度，采用Informed-RRT*算法规划主机械臂完成所有切割点切割蝴蝶兰种苗的最优路径。

可选的，当切割点的数量为一个时，最优路径为起始点到切割点再到起始点的路径；

当切割点的数量为多个时，最优路径包括依次连接的第一路径、多个第二路径和第三路径；所述第一路径为起始点到第一个切割点的路径；所述第二路径为第i个切割点到中间点再到第i+1个切割点的路径；所述第三路径为最后一个切割点到起始点的路径；其中，i＝1,2,…,n，n表示切割点的总数量。

可选的，主机械臂末端在切割点开始切割的判定方法为：当压力传感末端检测的主机械臂末端的受力骤增时，判定为开始切割种苗；

主机械臂末端在切割点结束切割的判定方法为：当压力传感末端检测的主机械臂末端的受力骤减时，判定为在切割点切割完毕。

可选的，所述获取蝴蝶兰种苗初始图像，之前还包括：

采用眼在手外的手眼标定方式对从机械臂进行标定，获得工业相机坐标系到从机械臂底座坐标系的转换矩阵；所述工业相机用于拍摄蝴蝶兰种苗图像；

采用眼在手外的手眼标定方式对主机械臂进行标定，获得工业相机坐标系到主机械臂底座坐标系的转换矩阵；

采用眼在手上的手眼标定方式对主机械臂进行再次标定，获得深度相机坐标系到主机械臂底座坐标系的转换矩阵。

可选的，根据所述蝴蝶兰种苗初始图像，确定蝴蝶兰种苗上的固定线，具体包括：

将所述蝴蝶兰种苗初始图像输入至ShuffleNetv2-YOLOv5目标识别网络，对蝴蝶兰种苗的根部和茎部进行识别，并在蝴蝶兰种苗图像中生成部位检测框；

当蝴蝶兰种苗图像中同时存在根部检测框和茎部检测框时，提取根部检测框图像；

从根部检测框图像中提取根部轮廓线，并填充轮廓线形成的最大连通区域，获得根部连通域图像；

连接根部连通域图像中任意两个像素点，形成一条直线；

计算根部连通域图像中每个像素点到所述直线的距离；

根据所述距离，利用距离函数

计算每个像素点与所述直线的距离函数值；式中，ρ(d)为距离函数值，d为像素点到直线的距离，C为常数；

确定根部连通域图像中所有像素点与所述直线的距离函数值总和，并将距离函数值总和的最小值对应的直线作为根部连通域图像的连通域拟合直线；

确定根部检测框的外接圆；

将连通域拟合直线与外接圆的两个交点中与茎部检测框的距离最近的交点确定为工业相机坐标系下的固定点；

根据连通域拟合直线与外接圆的两个交点，利用公式

计算固定角度；式中，θ为固定角度，(x₁，y₁)为连通域拟合直线与外接圆的第一个交点的坐标，(x₂，y₂)为连通域拟合直线与外接圆的第二个交点的坐标；

利用工业相机坐标系到从机械臂底座坐标系的转换矩阵，将工业相机坐标系下的固定点转换为从机械臂底座坐标系下的固定点；

根据从机械臂底座坐标系下的固定点和所述固定角度，确定蝴蝶兰种苗上的固定线。

根据本发明提供的具体实施例，本发明公开了以下技术效果：

本发明公开一种应用于蝴蝶兰种苗切割的双臂切割装置及双臂切割方法，从机械臂末端为亚克力透明板，在不损伤种苗的情况下，既能保证固定住蝴蝶兰种苗，也能够保证在手外的相机能够完整的拍摄下种苗，使得主机械臂末端能够准确地切割蝴蝶兰种苗；在主机械臂上安装深度相机，实时获取从机械臂信息，将从机械臂设置为障碍物，从而能够使用Informed-RRT*实时进行路径规划以实现安全的切割作业；并且本发明能够通过压力传感末端实时得到压力反馈，避免机械臂末端以及种苗在切割过程中受到不必要的损伤。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明提供的压力传感末端的爆炸图；

图2为本发明提供的压力传感末端的组合图；

图3为本发明提供的夹爪的示意图；

图4为本发明提供的亚克力板固定蝴蝶兰种苗的示意图；

图5为本发明提供的应用于蝴蝶兰种苗切割的双臂切割方法的原理图；

图6为本发明提供的眼在手外标定的示意图；

图7为本发明提供的眼在手上标定的示意图；

图8为本发明提供的Informed-RRT*算法路径规划示意图；

图9为本发明提供的椭圆参数定义的示意图。

符号说明：1-螺纹连接法兰，2-薄膜式压力传感器，3-弹簧，4-刀夹，5-紧固件，6-刀片。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明的目的是提供一种应用于蝴蝶兰种苗切割的双臂切割装置及双臂切割方法，以在双臂协作下准确、完全地切割蝴蝶兰种苗。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

本发明提供了一种应用于蝴蝶兰种苗切割的双臂切割装置，双臂切割装置包括：从机械臂、亚克力板、主机械臂、压力传感末端、深度相机和控制器。亚克力板设置在从机械臂的末端，作为从机械臂末端；压力传感末端设置在主机械臂的末端，作为主机械臂末端；深度相机固定在主机械臂上。

从机械臂、主机械臂、压力传感末端和深度相机均与控制器连接。从机械臂用于在控制器的控制下将从机械臂末端移动至蝴蝶兰种苗后利用亚克力板固定蝴蝶兰种苗。深度相机用于在从机械臂固定蝴蝶兰种苗后，拍摄从机械臂深度图像，并将从机械臂深度图像传输至控制器；控制器用于根据从机械臂深度图像获取从机械臂的位姿，并将从机械臂设定为障碍物，根据从机械臂的位姿规划主机械臂切割蝴蝶兰种苗的最优路径，进而按照最优路径控制主机械臂移动和切割。

压力传感末端用于切割蝴蝶兰种苗，并检测主机械臂末端的受力，同时将受力传输至控制器；控制器还用于根据受力判断蝴蝶兰种苗的切割结束时刻，并在切割结束时刻控制主机械臂末端停止对蝴蝶兰种苗的切割操作。

当种苗存在多个切割点时，切割完前一个点时，可能会造成种苗移动，导致识别出的后一个切割点不准确，所以本发明使用从机械臂，末端为亚克力透明板，在不损伤种苗的情况下，既能保证固定住蝴蝶兰种苗，也能够保证在手外的相机能够完整的拍摄下种苗。

示例性的，参照图1，压力传感末端包括：螺纹连接法兰1、薄膜式压力传感器2、弹簧3、刀夹4、紧固件5和刀片6。螺纹连接法兰1固定在主机械臂的末端，薄膜式压力传感器2设置在螺纹连接法兰1的内侧；弹簧3的一端与薄膜式压力传感器2接触，弹簧3的另一端与刀夹4的一端接触；刀夹4的另一端开设一个有内螺纹的固定孔，刀片6的固定端设置有外螺纹，固定孔的内螺纹与刀片6固定端的外螺纹相配合，刀片6通过螺纹锁紧的方式固定在刀夹4的固定孔中；刀片6用于切割蝴蝶兰种苗。紧固件5为带有内螺纹的空心圆柱结构，紧固件5的内螺纹与螺纹连接法兰1的外螺纹相配合，紧固件5通过螺纹锁紧的方式将螺纹连接法兰1、薄膜式压力传感器2、弹簧3和刀夹4固定为一体。薄膜式压力传感器2与控制器连接；薄膜式压力传感器2用于检测主机械臂末端的受力，同时将受力传输至控制器。压力传感末端的整体结构如图2所示。

图1中的符号φ表示直径，刀片6上标记的12表示刀片长度为12。

其中，弹簧3为劲度系数为1.2N/mm的弹簧，弹簧3直接作用于压力传感器上，通过弹簧3与压力传感器能够实时监测机械臂末端的受力情况，从而判断是否将种苗切割完成。刀夹4可以保证刀片6固定的同时方便更换刀片6。使用这种带弹簧3的末端进行切割时，既能保证能够完全切除种苗根部，也不会因为机械臂的规划误差而造成对实验平台的耗损，以上除刀片6外的部件均采用3D打印得到，材料为ABS工程塑料，刀片6材料为sk2工具钢。在切割过程中，可以对薄膜式压力传感器2中的数据进行分析，当由薄膜式压力传感器2检测到的压力突然增大即可认为种苗已经开始切割，当压力值波形又一次突变即可认为切割完毕，使用带压力反馈的末端一定程度上减少了末端的损耗，保护了末端刀片6，也在一定程度上能够保护试验台。

示例性的，参照图3，从机械臂的末端还设置有夹爪，夹爪夹住亚克力板。亚克力是一种特殊处理的有机玻璃，其具有透光性好、颜色纯正的特点，因此使用亚克力板作为从机械臂的末端来固定种苗，这样避免了从机械臂末端遮挡住种苗，从而影响切割点识别。亚克力板固定蝴蝶兰种苗的示意图如图4所示。

本发明设计了一种带力反馈的切割机械臂末端，能够实时监测机械臂末端的受力情况，通过传感器将力信号转换成电信号，观察电信号的波形，如果波形发生突变即机械臂末端受力突然改变，可以认为机械臂末端已经触碰到种苗，若波形再一次发生突变即可认为切割完成，从受力的角度来判断种苗是否完整被切除。

本发明还提供了一种应用于蝴蝶兰种苗切割的双臂切割方法，如图5所示，双臂切割方法包括：

在对蝴蝶兰种苗实施切割之前，首先要进行手眼标定预处理。

1)从机械臂手眼标定

从机械臂的标定方式如图6所示，使用眼在手外的标定方式，获得工业相机坐标系到从机械臂底座坐标系的转换矩阵。图6中Camera表示相机，Object表示标定板，End表示末端，Base表示机械臂底座。

示例性的，采用海康MV-CE060-10UC彩色工业相机采集蝴蝶兰种苗图像，以眼在手外的方式进行手眼标定。

分别使用示教器令机械臂移动到空间任意位置1和任意位置2(能够从示教器上得到两个点的位姿信息)。

令

有

可化为：

其中

分别为相机坐标系到机械臂底座坐标系的转换矩阵、机械臂底座坐标系到机械臂末端坐标系的转换矩阵、世界坐标系到相机坐标系的转换矩阵，下标1、2分别代表位置1和位置2所得到的转换矩阵。

化简为：

A·X＝X·B

其中，

求解矩阵X，即为相机到机械臂底座旋转矩阵。在相机中得到目标坐标后，通过手眼标定得到的相机到机械臂底座旋转矩阵，可以将相机坐标系下的目标坐标转化到机械臂底座坐标系下。

2)主机械臂手眼标定

因为主机械臂会使用到“眼在手上”、“眼在手外”两个相机，所以使用两种手眼标定方式对其进行手眼标定。采用眼在手外的手眼标定方式对主机械臂进行标定，获得工业相机坐标系到主机械臂底座坐标系的转换矩阵。采用眼在手上的手眼标定方式(如图7所示)对主机械臂进行再次标定，获得深度相机坐标系到主机械臂底座坐标系的转换矩阵。

示例性的，采用ToF深度相机采集从机械臂图像，以眼在手外方式进行手眼标定。在这种标定方式下，将相机固定在机器人末端，保证二者位姿关系始终不变，通过两次运动，求解相机和机器人末端之间的位姿关系。

标定时，得到物体在图片中的位置P_img(x，y)，可以通过相机标定得到的内外参确定该点在相机坐标系下的坐标

控制机械臂抓取物体还需要求解相机坐标系到机械臂底座坐标系之间的转换。即

(

为机械臂末端坐标系到机械臂底座坐标系的转换矩阵，

为相机坐标系到机械臂末端坐标系的转换矩阵)，其中

已知，移动机械臂分别到空间任意位置1和任意位置2即可求解，求解过程如下：

令

有

上述方程可以化简为：

其中

为世界坐标系到相机坐标系的转换矩阵，下标1、2表示位置1和位置2的点的参数。

化简为：

A·X＝X·B

其中

求解矩阵X，即为相机和机械臂末端转换矩阵。

步骤1，获取蝴蝶兰种苗初始图像。

示例性的，采用海康MV-CE060-10UC彩色工业相机对种苗进行拍摄。

步骤2，根据蝴蝶兰种苗初始图像，确定蝴蝶兰种苗上的固定线。

在一个示例中，步骤2的详细实现过程如下：

2-1，目标识别

对蝴蝶兰种苗初始图像用以ShuffleNet v2为主干网络的ShuffleNet v2-YOLOv5目标识别网络(以下称为目标检测网络)进行蝴蝶兰种苗识别，检测成功后在原图像中生成检测边框。

2-2，判断是否满足切割条件

判断步骤2-1中识别的目标是否满足切割的条件，若是同时检测到根和茎，则判断能够切割，否则不能。

2-3，固定点定位

若同时检测出根部区域和单独的黑色块茎区域，则开始计算固定点。

对每个根部区域进行预处理，首先对其进行高斯滤波降噪，然后将图片二值化，在对图像进行膨胀化处理，对提取到的图像提取轮廓线，找到最大连通区域并填充，得到连通域图像E_S。

读取图像连通域的所有像素点，任取两个像素点确定一条直线L_G，求连通域内所有像素点到直线L_G的距离d，再代入距离函数ρ(d)，当∑ρ_(d)最小时，直线L_G记为连通域拟合直线L_f。根据M-estimator算法，为防止对干扰点鲁棒性不够，ρ(d)计算公式为：

其中d为连通域内点到直线L_G距离，而C为定值1.345。直线L_f表达式为：

其中(x₁，y₁)和(x₂，y₂)为当∑ρ_(d)最小时用于确定直线的两点坐标。然后求取根部检测框外接圆Qi，公式为：

(x-x_s)²+(y-y_s)²＝r′² (5)

其中x_s，y_s为根部检测框中心点坐标，r′为检测框外接圆半径，r′求取公式为：

其中w_s和h_s分别为检测框宽和高。联立公式(4)和(5)计算外接圆与直线L_f交点坐标I₁，I₂得：

令

C＝ky₂-(k)²x₁-ky_s-x_s该方程为一元二次方程求解得到两个点I₁，I₂的x坐标为：

代入(4)求得两点的y坐标，比较两交点与黑色块茎检测框距离，判定两交点中距黑色块茎检测框近点为固定点I(机械臂工作台高度固定，所以仅求x、y即可)，通过手眼标定得到的转换矩阵，可以将该坐标转换为机械臂底座坐标系下的坐标。切割线与图像横轴夹角θ计算公式为：

当并计算固定线与图像横轴夹角θ。

利用工业相机坐标系到从机械臂底座坐标系的转换矩阵，将工业相机坐标系下的固定点转换为从机械臂底座坐标系下的固定点。

根据从机械臂底座坐标系下的固定点和固定角度，确定蝴蝶兰种苗上的固定线。

步骤3，利用从机械臂的亚克力板沿着固定线覆盖蝴蝶兰种苗的叶子，固定蝴蝶兰种苗。

由计算出的蝴蝶兰种苗上的固定线传给机械臂，以固定姿态固定种苗。

步骤4，获取固定后的蝴蝶兰种苗图像。

示例性的，将种苗固定后，通过海康MV-CE060-10UC相机拍摄固定后的蝴蝶兰种苗图像。

步骤5，根据固定后的蝴蝶兰种苗图像，确定蝴蝶兰种苗的所有切割点和每个切割点对应的切割角度。

蝴蝶兰种苗的所有切割点和每个切割点对应的切割角度的确定方法与步骤2-3固定点定位的方法相同，具体确定过程如下：

将固定后的蝴蝶兰种苗图像送入目标检测网络中，判断检测结果中是否同时检测出根部区域以及单独的黑色块茎区域，满足则开始计算切割点，并计算切割线与图像横轴夹角。

读取图像连通域的所有像素点，任取两个像素点确定一条直线L_G，求连通域内所有像素点到直线L_G的距离d，再代入距离函数ρ(d)，当∑ρ_(d)最小时，直线L_G记为连通域拟合直线L_f。根据M-estimator算法，为防止对干扰点鲁棒性不够，ρ(d)计算公式为：

其中d为连通域内点到直线L_G距离，而C为定值1.345。直线L_f表达式为：

其中(x₁，y₁)和(x₂，y₂)为当∑ρ_(d)最小时用于确定直线的两点坐标。然后求取根部检测框外接圆Q_i，公式为：

(x-x_s)²+(y-y_s)²＝r′² (12)

其中x_s，y_s为根部检测框中心点坐标，r′为检测框外接圆半径，r′求取公式为：

其中w_s和h_s分别为检测框宽和高。联立公式(10)和(11)计算外接圆与直线L_f交点坐标P₁，P₂得：

令

C＝ky₂-(k)²x₁-ky_s-x_s则

该方程求解得到两个点P₁，P₂的x坐标，代入(11)求得两点的y坐标，比较两交点与黑色块茎检测框距离，判定两交点中距黑色块茎检测框近点为切割点P(机械臂工作台高度固定，所以仅求x，y即可)。切割线与图像横轴夹角θ计算公式为：

当并计算切割线与图像横轴夹角θ。

利用工业相机坐标系到主机械臂底座坐标系的转换矩阵，将工业相机坐标系下的切割点转换为主机械臂底座坐标系下的切割点。

步骤6，通过设置在主机械臂上的深度相机拍摄用于固定蝴蝶兰种苗的从机械臂深度图像。

步骤7，根据从机械臂深度图像获得从机械臂在固定蝴蝶兰种苗时的位姿。

根据从机械臂深度图像，确定从机械臂在固定蝴蝶兰种苗时的位置信息和深度信息；根据位置信息和深度信息对从机械臂进行位姿估计，获得从机械臂在固定蝴蝶兰种苗时的位姿。利用深度相机坐标系到主机械臂底座坐标系的转换矩阵，可以获得主机械臂底座坐标系下的位姿。

步骤8，将从机械臂设定为障碍物，并根据从机械臂固定蝴蝶兰种苗时的位姿，采用Informed-RRT*算法规划主机械臂切割蝴蝶兰种苗的最优路径。

在一个示例中，步骤8的具体实现过程为：

8-1，将从机械臂设定为障碍物，并将主机械臂所能活动的空间减去从机械臂固定蝴蝶兰种苗时的位姿对应的空间后的空间确定为主机械臂的运动空间。

8-2，设置切割点为目标点，并在主机械臂的运动空间中采用RRT算法计算出一条到目标点的初始路径。

RRT算法的步骤如下：

(1)在机械臂的运动空间(运动空间是其所能活动的所有空间减去障碍物所在的空间)随机采样得到一个点x_rand；

(2)在树上(已知路径点，初始仅有起始点在树上)找到离x_rand最近的点x_near；

(3)x_near往x_rand延伸一个步长l距离，得到点x_new；

(4)判断x_near到x_new之间是否与障碍物碰撞，若没有则将x_new加入树；

(5)重复步骤(1)-(5)直到随机搜索点在目标点x_gool的某一设置邻域内。

8-3，根据初始路径的长度，采用Informed-RRT*算法规划主机械臂完成所有切割点切割蝴蝶兰种苗的最优路径。

Informed-RRT^*是一种随机采样的路径搜索算法，Informed-RRT^*是在RRT算法下改变其随机搜索算法，使其能够更快的找到最优路径，如图8和9所示(以2D空间为例)，其采样算法步骤如下所示：

(1)由RRT算法得到的路径设为c_best，目标点与起始点的距离为c_min，椭圆中心点坐标为x_centre＝(x_init+x_goal)²/2；

(2)将椭圆经过旋转平移至世界坐标系原点，得到旋转矩阵为C；

(3)r₁＝c_best/2，

(4)将r_i构造为一个对角矩阵L＝diag{r₁，r₂，...，r_n}；

(5)在所得到的椭圆内进行随机采样得到点x_ball；

(6)实际采样点x_rand＝CLx_ball+x_centre。

图9中X_start表示起始点，X_goal对应目标点x_goal。

步骤9，控制主机械臂末端按照最优路径依次对切割点以相应的切割角度进行切割。

当切割点的数量为一个时，最优路径为起始点到切割点再到起始点的路径；

当切割点的数量为多个时，最优路径包括依次连接的第一路径、多个第二路径和第三路径；第一路径为起始点到第一个切割点的路径；第二路径为第i个切割点到中间点再到第i+1个切割点的路径；第三路径为最后一个切割点到起始点的路径；其中，i＝1,2,…,n，n表示切割点的总数量。即，P_init(起始点)→P_goal1(切割点1)→P_temp(中间点)→P_goal2(切割点2)→P_temp(中间点)…→P_init(起始点)。

主机械臂末端在切割点开始切割的判定方法为：当压力传感末端检测的主机械臂末端的受力骤增时，判定为开始切割种苗；

主机械臂末端在切割点结束切割的判定方法为：当压力传感末端检测的主机械臂末端的受力骤减时，判定为在切割点切割完毕。

在使用RRT及其改进算法时，需要对机械臂的运动空间的信息已知，也就是说需要知道障碍物的信息，通常为固定障碍物。但是在蝴蝶兰种苗切割过程中，每一棵种苗都是不一样的，具有不一样的形状，切割的位置会不一样，所以从机械臂按压的位置也会不一样，这就会导致从机械臂的姿态不一样，为了避免碰撞，在主机械臂上加入在臂上的深度相机，实时获取从机械臂信息，从而能够实时进行路径规划以实现安全的切割作业。并且本发明能够通过薄膜压力传感器实时得到压力反馈，避免机械臂末端以及种苗在切割过程中受到不必要的损伤。

本发明提出一种双臂协作的切割方法，其中从机械臂对种苗进行固定，主机械臂进行切割，以达到第一次通过相机就能获取到所有待切割的点且每次切割不会改变种苗相对于主机械臂的位姿，并且在从机械臂固定后，能够从主机械臂上的深度相机中获得从机械臂的信息从而进行位姿估计，将从机械臂作为障碍，使用Informed-RRT*进行路径规划实现切割，避免两机械臂发生碰撞。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (9)

1.一种应用于蝴蝶兰种苗切割的双臂切割装置，其特征在于，所述双臂切割装置包括：从机械臂、亚克力板、主机械臂、压力传感末端、深度相机和控制器；

亚克力板设置在从机械臂的末端，作为从机械臂末端；压力传感末端设置在主机械臂的末端，作为主机械臂末端；深度相机固定在主机械臂上；

从机械臂、主机械臂、压力传感末端和深度相机均与控制器连接；

所述从机械臂用于在控制器的控制下将从机械臂末端移动至蝴蝶兰种苗后利用所述亚克力板固定蝴蝶兰种苗；具体过程为：获取蝴蝶兰种苗初始图像；根据所述蝴蝶兰种苗初始图像，确定蝴蝶兰种苗上的固定线；利用从机械臂的亚克力板沿着所述固定线覆盖蝴蝶兰种苗的叶子，固定蝴蝶兰种苗；其中，根据所述蝴蝶兰种苗初始图像，确定蝴蝶兰种苗上的固定线，具体包括：将所述蝴蝶兰种苗初始图像输入至ShuffleNet v2-YOLOv5目标识别网络，对蝴蝶兰种苗的根部和茎部进行识别，并在蝴蝶兰种苗图像中生成部位检测框；当蝴蝶兰种苗图像中同时存在根部检测框和茎部检测框时，提取根部检测框图像；从根部检测框图像中提取根部轮廓线，并填充轮廓线形成的最大连通区域，获得根部连通域图像；连接根部连通域图像中任意两个像素点，形成一条直线；计算根部连通域图像中每个像素点到所述直线的距离；根据所述距离，利用距离函数

计算每个像素点与所述直线的距离函数值；式中，ρ(d)为距离函数值，d为像素点到直线的距离，C为常数；确定根部连通域图像中所有像素点与所述直线的距离函数值总和，并将距离函数值总和的最小值对应的直线作为根部连通域图像的连通域拟合直线；确定根部检测框的外接圆；将连通域拟合直线与外接圆的两个交点中与茎部检测框的距离最近的交点确定为工业相机坐标系下的固定点；根据连通域拟合直线与外接圆的两个交点，利用公式

计算固定角度；式中，θ为固定角度，(x₁，y₁)为连通域拟合直线与外接圆的第一个交点的坐标，(x₂，y₂)为连通域拟合直线与外接圆的第二个交点的坐标；利用工业相机坐标系到从机械臂底座坐标系的转换矩阵，将工业相机坐标系下的固定点转换为从机械臂底座坐标系下的固定点；根据从机械臂底座坐标系下的固定点和所述固定角度，确定蝴蝶兰种苗上的固定线；

所述深度相机用于在从机械臂固定蝴蝶兰种苗后，拍摄从机械臂深度图像，并将所述从机械臂深度图像传输至控制器；所述控制器用于根据所述从机械臂深度图像获取从机械臂的位姿，并将从机械臂设定为障碍物，根据从机械臂的位姿规划主机械臂切割蝴蝶兰种苗的最优路径，进而按照最优路径控制主机械臂移动和切割；

所述压力传感末端用于切割蝴蝶兰种苗，并检测主机械臂末端的受力，同时将所述受力传输至控制器；所述控制器还用于根据所述受力判断蝴蝶兰种苗的切割结束时刻，并在切割结束时刻控制主机械臂末端停止对蝴蝶兰种苗的切割操作。

2.根据权利要求1所述的应用于蝴蝶兰种苗切割的双臂切割装置，其特征在于，所述压力传感末端包括：螺纹连接法兰、薄膜式压力传感器、弹簧、刀夹、紧固件和刀片；

螺纹连接法兰固定在主机械臂的末端，薄膜式压力传感器设置在螺纹连接法兰的内侧；弹簧的一端与薄膜式压力传感器接触，弹簧的另一端与刀夹的一端接触；刀夹的另一端开设一个有内螺纹的固定孔，刀片的固定端设置有外螺纹，所述固定孔的内螺纹与刀片固定端的外螺纹相配合，刀片通过螺纹锁紧的方式固定在刀夹的固定孔中；所述刀片用于切割蝴蝶兰种苗；

紧固件为带有内螺纹的空心圆柱结构，紧固件的内螺纹与螺纹连接法兰的外螺纹相配合，紧固件通过螺纹锁紧的方式将螺纹连接法兰、薄膜式压力传感器、弹簧和刀夹固定为一体；

薄膜式压力传感器与控制器连接；所述薄膜式压力传感器用于检测主机械臂末端的受力，同时将所述受力传输至控制器。

3.根据权利要求1所述的应用于蝴蝶兰种苗切割的双臂切割装置，其特征在于，所述从机械臂的末端还设置有夹爪，所述夹爪夹住所述亚克力板。

4.一种应用于蝴蝶兰种苗切割的双臂切割方法，其特征在于，所述双臂切割方法包括：

获取蝴蝶兰种苗初始图像；

根据所述蝴蝶兰种苗初始图像，确定蝴蝶兰种苗上的固定线；具体包括：将所述蝴蝶兰种苗初始图像输入至ShuffleNet v2-YOLOv5目标识别网络，对蝴蝶兰种苗的根部和茎部进行识别，并在蝴蝶兰种苗图像中生成部位检测框；当蝴蝶兰种苗图像中同时存在根部检测框和茎部检测框时，提取根部检测框图像；从根部检测框图像中提取根部轮廓线，并填充轮廓线形成的最大连通区域，获得根部连通域图像；连接根部连通域图像中任意两个像素点，形成一条直线；计算根部连通域图像中每个像素点到所述直线的距离；根据所述距离，利用距离函数

计算每个像素点与所述直线的距离函数值；式中，ρ(d)为距离函数值，d为像素点到直线的距离，C为常数；确定根部连通域图像中所有像素点与所述直线的距离函数值总和，并将距离函数值总和的最小值对应的直线作为根部连通域图像的连通域拟合直线；确定根部检测框的外接圆；将连通域拟合直线与外接圆的两个交点中与茎部检测框的距离最近的交点确定为工业相机坐标系下的固定点；根据连通域拟合直线与外接圆的两个交点，利用公式

计算固定角度；式中，θ为固定角度，(x₁，y₁)为连通域拟合直线与外接圆的第一个交点的坐标，(x₂，y₂)为连通域拟合直线与外接圆的第二个交点的坐标；利用工业相机坐标系到从机械臂底座坐标系的转换矩阵，将工业相机坐标系下的固定点转换为从机械臂底座坐标系下的固定点；根据从机械臂底座坐标系下的固定点和所述固定角度，确定蝴蝶兰种苗上的固定线；

利用从机械臂的亚克力板沿着所述固定线覆盖蝴蝶兰种苗的叶子，固定蝴蝶兰种苗；

获取固定后的蝴蝶兰种苗图像；

根据所述固定后的蝴蝶兰种苗图像，确定蝴蝶兰种苗的所有切割点和每个切割点对应的切割角度；

通过设置在主机械臂上的深度相机拍摄用于固定蝴蝶兰种苗的从机械臂深度图像；

根据所述从机械臂深度图像获得从机械臂在固定蝴蝶兰种苗时的位姿；

将从机械臂设定为障碍物，并根据从机械臂固定蝴蝶兰种苗时的位姿，采用Informed-RRT*算法规划主机械臂切割蝴蝶兰种苗的最优路径；

控制主机械臂末端按照所述最优路径依次对切割点以相应的切割角度进行切割。

5.根据权利要求4所述的应用于蝴蝶兰种苗切割的双臂切割方法，其特征在于，根据所述从机械臂深度图像获得从机械臂在固定蝴蝶兰种苗时的位姿，具体包括：

根据所述从机械臂深度图像，确定从机械臂在固定蝴蝶兰种苗时的位置信息和深度信息；

根据所述位置信息和所述深度信息对从机械臂进行位姿估计，获得从机械臂在固定蝴蝶兰种苗时的位姿。

6.根据权利要求4所述的应用于蝴蝶兰种苗切割的双臂切割方法，其特征在于，所述将从机械臂设定为障碍物，并根据从机械臂固定蝴蝶兰种苗时的位姿，采用Informed-RRT*算法规划主机械臂切割蝴蝶兰种苗的最优路径，具体包括：

将从机械臂设定为障碍物，并将主机械臂所能活动的空间减去从机械臂固定蝴蝶兰种苗时的位姿对应的空间后的空间确定为主机械臂的运动空间；

设置切割点为目标点，并在主机械臂的运动空间中采用RRT算法计算出一条到目标点的初始路径；

根据所述初始路径的长度，采用Informed-RRT*算法规划主机械臂完成所有切割点切割蝴蝶兰种苗的最优路径。

7.根据权利要求4所述的应用于蝴蝶兰种苗切割的双臂切割方法，其特征在于，

当切割点的数量为一个时，最优路径为起始点到切割点再到起始点的路径；

当切割点的数量为多个时，最优路径包括依次连接的第一路径、多个第二路径和第三路径；所述第一路径为起始点到第一个切割点的路径；所述第二路径为第i个切割点到中间点再到第i+1个切割点的路径；所述第三路径为最后一个切割点到起始点的路径；其中，i＝1,2,…,n，n表示切割点的总数量。

8.根据权利要求4所述的应用于蝴蝶兰种苗切割的双臂切割方法，其特征在于，

主机械臂末端在切割点开始切割的判定方法为：当压力传感末端检测的主机械臂末端的受力骤增时，判定为开始切割种苗；

主机械臂末端在切割点结束切割的判定方法为：当压力传感末端检测的主机械臂末端的受力骤减时，判定为在切割点切割完毕。

9.根据权利要求4所述的应用于蝴蝶兰种苗切割的双臂切割方法，其特征在于，所述获取蝴蝶兰种苗初始图像，之前还包括：

采用眼在手外的手眼标定方式对从机械臂进行标定，获得工业相机坐标系到从机械臂底座坐标系的转换矩阵；所述工业相机用于拍摄蝴蝶兰种苗图像；

采用眼在手外的手眼标定方式对主机械臂进行标定，获得工业相机坐标系到主机械臂底座坐标系的转换矩阵；

采用眼在手上的手眼标定方式对主机械臂进行再次标定，获得深度相机坐标系到主机械臂底座坐标系的转换矩阵。

Images