一种多臂协作采摘机器人的作业方法及系统

本发明涉及农业采摘机器人，具体涉及一种多臂协作采摘机器人的作业方法及系统。

背景技术：

1、芒果等果实在非结构化果树冠层中通常呈现空间聚集、枝叶遮挡和果柄方向不一致的分布特征。为降低人工采摘的劳动强度，果实采摘机器人通常利用视觉识别系统获取冠幅图像，确定果实或果柄采摘点，并控制机械臂及末端执行器完成剪切、夹持、转移和释放。

2、现有多臂采摘作业方案通常根据采摘点的三维位置进行空间聚类或固定纵向分区，再按照各机械臂的作业区域、可达范围或当前负载分配采摘任务；对于相邻机械臂共同到达的重叠区域，可通过强化学习、路径规划或碰撞检测生成采摘顺序，并在臂间距过小时对机械臂运动轨迹进行避让调整。

3、然而，仅依据采摘点位置进行聚类时，空间相邻但果柄方向差异较大的采摘点容易被归入同一任务，导致后续机械臂姿态调整与任务执行关系不稳定；采用固定分区或面向单个采摘点分配任务时，重叠区域内还容易出现机械臂可达性、基座距离与队列负载不匹配的问题。此外，局部避让打断原采摘作业队列后，若未同步保存当前规划位置和队列状态，未完成任务难以按照原承接关系继续执行。

4、公开于该背景技术部分的信息仅仅旨在增加对本发明的总体背景的理解，而不应当被视为承认或以任何形式暗示该信息构成已为本领域一般技术人员所公知的现有技术。

技术实现思路

1、本发明提供一种多臂协作采摘机器人的作业方法及系统，用以解决现有多臂采摘作业中仅依据空间位置聚类而使果柄方向差异较大的采摘点被错误归并、重叠作业区域内任务单元与机械臂可达性及队列负载不匹配，以及在线避让后当前规划位置和采摘作业队列状态缺少承接而导致未完成采摘任务难以继续执行的问题。

2、本发明的技术方案为：一方面，本发明提供了一种多臂协作采摘机器人的作业方法，包括：获取果实冠幅区域的图像数据，识别果柄采摘点，提取各果柄采摘点的三维空间坐标及果柄主方向；基于任意两个果柄采摘点之间的空间距离和果柄主方向差构建复合相似性度量并进行密度聚类；当初始聚类单元内局部密度高于其平均密度且果柄方向一致性不满足聚类判据时，将对应区域分离为子聚类单元，当相邻聚类单元边界处的复合相似性度量满足合并条件时进行合并，得到簇状采摘任务单元；根据采摘机械臂的安装位置、可达空间和所述任务单元的空间范围划分边界重叠的作业区域；对重叠区域内的任务单元进行可达性筛选，排除不可达的采摘机械臂，按所述任务单元至各可达采摘机械臂基座的距离及其采摘作业队列长度确定目标采摘机械臂，并将所述任务单元加入目标采摘机械臂的采摘作业队列；控制各采摘机械臂按对应采摘作业队列协同运行；预测任一采摘机械臂进入重叠区域或臂间距小于安全阈值时，在线调制相关运动轨迹并保存当前规划位置和采摘作业队列状态，按调制所得关节控制量执行避让，间距恢复至安全阈值以上后读取所保存的状态，接续未完成的采摘任务；控制末端执行器完成果柄剪切、果实夹持、向收集系统转移及释放。

3、可选地，所述提取各果柄采摘点的三维空间坐标及果柄主方向，包括：从所述图像数据中提取果实候选区域及果柄采摘点；根据所述图像数据的深度信息，将各果柄采摘点转换至同一空间坐标系；在各果柄采摘点对应的局部区域内提取果柄特征，并根据所述果柄特征拟合果柄主方向向量。

4、可选地，所述构建复合相似性度量并进行密度聚类，包括：计算任意两个果柄采摘点之间的三维欧氏距离以及对应果柄主方向向量之间的夹角；对所述三维欧氏距离和所述夹角进行加权融合，得到复合相似性度量值；基于所述复合相似性度量值对果柄采摘点进行密度聚类，形成多个初始聚类单元。

5、可选地，所述聚类判据和合并条件的确定，包括：统计各初始聚类单元内果柄采摘点的局部密度，得到对应平均密度；根据果柄主方向向量之间的夹角分布确定果柄方向一致性；根据所述平均密度和果柄方向一致性调整密度聚类的邻域参数，形成所述聚类判据；计算相邻聚类单元边界处果柄采摘点之间的复合相似性度量，将其与合并阈值比较，形成所述合并条件。

6、可选地，所述划分相邻边界相互重叠的作业区域，包括：根据各采摘机械臂在机架上的安装位置和对应可达空间确定各采摘机械臂的核心作业区域；根据所述簇状采摘任务单元的空间范围扩展相邻核心作业区域的边界，形成供相邻采摘机械臂共同到达的重叠作业区域；将位于非重叠作业区域内的簇状采摘任务单元分配至对应的采摘机械臂。

7、可选地，所述确定目标采摘机械臂，包括：获取每一簇状采摘任务单元内果柄采摘点与各可达采摘机械臂基座之间的距离数据；获取各可达采摘机械臂当前采摘作业队列的长度数据；基于所述距离数据和长度数据形成各可达采摘机械臂的综合代价；将综合代价最小者确定为目标采摘机械臂，并将所述簇状采摘任务单元整体加入其采摘作业队列。

8、可选地，所述确定目标采摘机械臂，包括：将各采摘机械臂的关节状态、簇状采摘任务单元的三维空间坐标、相邻采摘机械臂的末端位置和重叠作业区域占用状态构成联合状态，将任务单元选择及采摘作业队列更新构成联合动作，并根据末端距离、任务完成状态、多臂并行状态和作业时间构建奖励信息；利用所述联合状态、联合动作和奖励信息训练多智能体策略模型，通过自注意力机制确定任务关联程度，并结合空间可达性、基座距离和采摘作业队列长度确定所述目标采摘机械臂。

9、可选地，所述在线调制，包括：按照控制周期获取相邻采摘机械臂的运动状态，计算末端间距及进入所述重叠作业区域的运动趋势；当所述末端间距小于所述安全阈值或相邻采摘机械臂将同时进入所述重叠作业区域时触发在线调制；在逆运动学求解中将末端位姿偏差和臂间碰撞约束构成加权成本函数，以关节角度为待求变量进行非线性优化，得到所述关节控制量。

10、可选地，所述接续执行未完成的采摘任务，包括：在触发在线调制时保存相关采摘机械臂的当前规划位置及其采摘作业队列状态；根据所述关节控制量控制至少一个相关采摘机械臂执行避让，并持续获取相邻采摘机械臂的末端间距；在所述末端间距恢复至所述安全阈值以上时退出在线调制，读取所述当前规划位置和采摘作业队列状态，将未完成的采摘任务接入对应采摘作业队列。

11、另一方面，本发明还提供了一种多臂协作采摘机器人的作业系统，包括：感知聚类模块，用于获取果实冠幅图像，识别果柄采摘点并提取其三维坐标和果柄主方向，按空间距离和果柄主方向差进行密度聚类；在局部密度高于单元平均密度且果柄方向一致性不满足聚类判据时分离对应区域，相邻单元边界复合相似性度量满足合并条件时合并，输出簇状采摘任务单元；任务分配模块，用于按采摘机械臂安装位置、可达空间和任务单元划分边界重叠作业区域，筛选可达机械臂，按任务单元至可达机械臂基座的距离及采摘作业队列长度确定目标采摘机械臂，并将任务单元整体加入其采摘作业队列；协同控制模块，用于控制各采摘机械臂按采摘作业队列运行，在进入重叠区域或臂间距小于安全阈值时在线调制运动轨迹并保存当前规划位置和采摘作业队列状态，臂间距恢复后读取保存状态并接续未完成任务；采摘执行模块，用于按关节控制量驱动采摘机械臂，并控制末端执行器完成果柄剪切、果实夹持、转移及释放。

12、本发明的有益效果在于：

13、本发明通过提取果柄采摘点的三维空间坐标及果柄主方向，将空间距离和果柄主方向差共同用于密度聚类，并依据局部密度分布和果柄方向一致性对聚类单元进行分离或合并，使形成的簇状采摘任务单元同时反映采摘点的空间聚集关系和果柄取向关系；通过对重叠作业区域内的任务单元进行可达性筛选，并结合任务单元至机械臂基座的距离和采摘作业队列长度确定目标采摘机械臂，使聚类结果直接承接至任务分配；通过在轨迹在线调制时保存当前规划位置和采摘作业队列状态，并在臂间距恢复后读取所保存状态，使避让前后的未完成任务保持队列衔接，从而形成由冠幅感知、任务单元生成、重叠区域分配、协同避让至果柄剪切、果实夹持、转移和释放的连续作业过程。