基于固定臂构型优选的双臂协同采摘路径规划方法及系统

摘要: 本发明涉及农业采摘机器人路径规划,尤其涉及一种基于固定臂构型优选的双臂协同采摘路径规划方法及系统。背景技术:1、苹果采摘作业劳动强度大、作业季节性强,且果园内枝...
基于固定臂构型优选的双臂协同采摘路径规划方法及系统

本发明涉及农业采摘机器人路径规划,尤其涉及一种基于固定臂构型优选的双臂协同采摘路径规划方法及系统。

背景技术:

1、苹果采摘作业劳动强度大、作业季节性强,且果园内枝条、邻果、树干交错分布,作业环境复杂,人工采摘成本逐年升高。因此,苹果采摘机器人已成为果蔬智能装备领域的重点研究方向。

2、现阶段,主流采摘机器人采用单机械臂作业模式,依靠视觉识别果实后规划采摘路径完成采收。然而,果园中错综复杂的障碍物对单臂运动形成大量遮挡,机械臂末端极易剐蹭邻近苹果、拉扯枝条,造成落果与枝条破损,不仅大幅降低采摘成功率,还会损伤果树,影响次年挂果。

3、为克服单臂采摘的固有缺陷,业内提出了双机械臂协同采摘方案:利用其中一条机械臂(固定臂)夹持并绷紧目标果实周边的枝条,另一条机械臂(采摘臂)执行采摘作业,以此稳定作业环境、减少碰撞扰动。然而,该方案在实践中暴露出新的问题——固定臂夹持枝条后,自身即转化为采摘臂运动空间内的静态障碍物;若固定臂的末端姿态选取不当,会直接挤占采摘臂的运动通道,导致路径规划无解。

4、现有双臂路径规划算法大多针对工业场景设计,研究重心集中于双臂自身的碰撞规避及静态障碍物环境下的路径搜索,并未适配果园采摘的专属工况。事实上,针对同一枝条夹持点位,机械臂逆运动学可求解出多组可行的固定臂构型,而不同构型对采摘通道的遮挡程度和碰撞风险差异显著。目前,现有技术缺少对多组候选固定臂构型的量化协同评价机制,无法提前筛选出遮挡最小的夹持姿态。同时,现有方案不具备容错逻辑,一旦选定的固定臂构型堵塞采摘通道,只能终止本次采摘任务,作业鲁棒性较差。

5、综上,如何在固定臂夹持枝条前完成多候选构型的量化评价,优选低遮挡目标固定姿态,并配套路径规划失败后的容错重规划逻辑,是双机械臂苹果协同采摘领域亟待解决的问题。

技术实现思路

1、本发明实施例提供一种基于固定臂构型优选的双臂协同采摘路径规划方法及系统,用以解决现有双臂采摘固定臂姿态遮挡采摘通道、规划失败无容错、碰撞损伤果树的技术问题。

2、在本发明实施例的第一方面,提供了一种基于固定臂构型优选的双臂协同采摘路径规划方法,包括:

3、步骤s1,获取采摘任务信息,所述采摘任务信息包含待采摘果实、枝条固定点、环境障碍物以及双机械臂初始构型信息;

4、步骤s2,基于所述采摘任务信息,构建采摘环境的障碍物安全域;

5、步骤s3,以所述枝条固定点为末端约束,生成固定臂的多组候选固定臂构型,并对各组候选固定臂构型进行可行性筛选,得到可行候选构型集合;

6、步骤s4,针对所述可行候选构型集合中的每组候选固定臂构型,构建对应的固定臂障碍模型,并将所述固定臂障碍模型与障碍物安全域融合,得到各候选固定臂构型对应的总障碍物集合;

7、步骤s5,基于所述总障碍物集合,分别计算各候选固定臂构型的协同代价,并选取协同代价满足预设条件的候选固定臂构型作为目标固定臂构型;

8、步骤s6,以所述双机械臂初始构型为起点、所述目标固定臂构型为终点,结合所述障碍物安全域规划固定臂无碰撞固定路径,并控制固定臂抵达所述目标固定臂构型以固定所述枝条固定点;

9、步骤s7,将固定状态下的所述固定臂障碍模型加入采摘臂路径规划障碍物集合;

10、步骤s8,基于所述采摘臂路径规划障碍物集合,规划所述采摘臂由初始构型至所述待采摘果实采摘位姿的无碰撞采摘路径。

11、在本发明实施例的第二方面,提供了一种基于固定臂构型优选的双臂协同采摘路径规划系统,包括:

12、信息获取模块,用于获取采摘任务信息,所述采摘任务信息包含待采摘果实、枝条固定点、环境障碍物以及双机械臂初始构型信息;

13、障碍物建模模块,用于基于所述采摘任务信息,构建采摘环境的障碍物安全域;

14、候选构型生成模块,用于以所述枝条固定点为末端约束,生成固定臂的多组候选固定臂构型,并对各组候选固定臂构型进行可行性筛选,得到可行候选构型集合;

15、固定臂障碍转换模块,用于针对所述可行候选构型集合中的每组候选固定臂构型,构建对应的固定臂障碍模型,并将所述固定臂障碍模型与障碍物安全域融合,得到各候选固定臂构型对应的总障碍物集合;

16、协同代价评价模块,用于基于所述总障碍物集合,分别计算各候选固定臂构型的协同代价,并选取协同代价满足预设条件的候选固定臂构型作为目标固定臂构型;

17、固定臂路径规划模块,用于以所述双机械臂初始构型为起点、所述目标固定臂构型为终点,结合所述障碍物安全域规划固定臂无碰撞固定路径,并控制固定臂抵达所述目标固定臂构型以固定所述枝条固定点;

18、障碍物集合更新模块,用于将固定状态下的所述固定臂障碍模型加入采摘臂路径规划障碍物集合;

19、采摘臂路径规划模块,用于基于所述采摘臂路径规划障碍物集合,规划所述采摘臂由初始构型至所述待采摘果实采摘位姿的无碰撞采摘路径。

20、在本发明实施例的第三方面,提供了一种电子设备,包括存储器、处理器以及存储在所述存储器中并可在所述处理器运行的计算机程序,所述处理器执行所述计算机程序时实现如本发明实施例第一方面所述方法的步骤。

21、本发明提供的基于固定臂构型优选的双臂协同采摘路径规划方法及系统,针对同一枝条固定点生成多组可行候选构型,通过协同代价函数量化评估各构型对采摘通道的遮挡程度、碰撞风险及采摘臂可达性,自动筛选对采摘臂运动空间挤占最小的目标固定臂构型,从根源上减少固定臂造成的路径规划无解问题。当选定的最优构型下采摘臂无法规划无碰撞路径时,自动切换协同代价次优的候选构型重新规划,避免单次构型失效导致采摘任务中断,显著提升复杂果园环境下的作业成功率。将邻果、枝条、树干及机械臂连杆均转化为带安全裕度的球体、圆柱体或胶囊体几何包络模型,路径规划全程统一碰撞检测标准,有效规避机械臂剐蹭邻果、拉扯枝条,减少落果与果树损伤。整套方案功能模块解耦,信息采集、障碍物建模、构型评价与路径规划相互独立,可灵活搭载于各类六轴协作双机械臂采摘平台,适配仿真离线规划与实体果园在线作业场景。

1.一种基于固定臂构型优选的双臂协同采摘路径规划方法,其特征在于,包括:

2.根据权利要求1所述的基于固定臂构型优选的双臂协同采摘路径规划方法,其特征在于,步骤s2中,基于所述采摘任务信息,构建采摘环境的障碍物安全域,具体包括:

3.根据权利要求1所述的基于固定臂构型优选的双臂协同采摘路径规划方法,其特征在于,步骤s3中,以所述枝条固定点为末端约束,生成固定臂的多组候选固定臂构型,并对各组候选固定臂构型进行可行性筛选,得到可行候选构型集合,具体包括:

4.根据权利要求1所述的基于固定臂构型优选的双臂协同采摘路径规划方法,其特征在于,步骤s4中,针对所述可行候选构型集合中的每组候选固定臂构型,构建对应的固定臂障碍模型,并将所述固定臂障碍模型与障碍物安全域融合,得到各候选固定臂构型对应的总障碍物集合,具体包括:

5.根据权利要求1所述的基于固定臂构型优选的双臂协同采摘路径规划方法,其特征在于,步骤s5中,基于所述总障碍物集合,分别计算各候选固定臂构型的协同代价,并选取协同代价满足预设条件的候选固定臂构型作为目标固定臂构型,具体包括:

6.根据权利要求5所述的基于固定臂构型优选的双臂协同采摘路径规划方法,其特征在于,所述采摘通道阻塞代价根据采摘臂预采摘通道采样点与固定臂障碍模型之间的最小距离确定,采样点与固定臂障碍模型距离越小,采摘通道阻塞代价数值越高;

7.根据权利要求1所述的基于固定臂构型优选的双臂协同采摘路径规划方法,其特征在于,步骤s8中,基于所述采摘臂路径规划障碍物集合,规划所述采摘臂由初始构型至所述待采摘果实采摘位姿的无碰撞采摘路径,具体包括:

8.根据权利要求1所述的基于固定臂构型优选的双臂协同采摘路径规划方法,其特征在于,还包括:

9.一种基于固定臂构型优选的双臂协同采摘路径规划系统,该系统用于执行权利要求1-8任一项所述的基于固定臂构型优选的双臂协同采摘路径规划方法,其特征在于,该系统包括:

10.一种电子设备,包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序,其特征在于,所述处理器执行所述计算机程序时实现如权利要求1至8任一项所述的基于固定臂构型优选的双臂协同采摘路径规划方法的步骤。