(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211238685.4 (22)申请日 2022.10.11 (71)申请人 深圳市智鼎自动化 技术有限公司 地址 518000 广东省深圳市宝安区新 安街 道兴东社区68区美生创谷二期慧谷 501A室 (72)发明人 吴楷龙　苏飞　陈磊　 (74)专利代理 机构 深圳市能闻知识产权代理事 务所(普通 合伙) 44717 专利代理师 赖银杰 (51)Int.Cl. B25J 9/16(2006.01) B25J 19/02(2006.01) (54)发明名称 一种任务分级的双机械臂协同规划及控制 方法及相关装置 (57)摘要 本发明公开了一种任务分级的双机械臂协 同规划及控制方法及相关装置， 该方法包括： 采 取任务优先分级时序优化方法， 保障任务安全执 行； 充分利用双机械臂的对称性优势， 通过对双 臂机器人的抓取动作进行合理的任务优先规划 和对偶运动规划， 对双机械臂机器人的抓取动作 进行如下任务细分， 并对其执行顺序进行优化划 分； 对任务序列进行类别划分： 类别1： 物理约束 与安全控制目标， 类别2： 机械臂能够抓取目标物 的先决条件， 类别3： 核心抓取任务， 类别4： 控制 优化目标任务。 本发明提供一种任务分级的双机 械臂协同规划及控制方法及相关装置， 该方法能 降低机械手臂对协同抓取作业的影 响， 从而提高 协同抓取作业的控制精度。 权利要求书2页 说明书5页 附图1页 CN 115401697 A 2022.11.29 CN 115401697 A 1.一种任务分级的双机 械臂协同规划及控制方法， 其特 征在于， 其工作步骤如下： 采取任务优先分级时序优化方法， 保障任务 安全执行； 充分利用双机械臂的对称性优势， 通过对双臂机器人的抓取动作进行合理的任务优先 规划和对偶运动规划， 对双机械臂机器人 的抓取动作进行如下任务细分， 并对其执行顺序 进行优化划分， 序号代 表任务执 行顺序： (1)机械臂关节角度限位,保持关节角在角度限制范围内变化θ mi n<θ <θ max； (2)保持抓取目标物处于摄 像机视野范围中心； (3)双机械臂末端执 行器的位置控制； (4)双机械臂末端执 行器的姿态控制； (5)双机械臂构型的优化； (6)双机械臂运动最小化； 对任务序列进行如 下类别划分： 类别1： 任务(1)对应物 理约束与 安全控制目标， 类别2： 任务(2)是机械臂能够抓取目标物的先决条件， 类别3： 任务(3)～(4)对应核心 抓取任务， 类 别4： 任务(5)～(6)对应控制优化目标。 2.根据权利要求1所述任务分级的双机械臂协同规划及控制方法， 其特征在于， 为运动 规划系统设置任务分级时序优化函数TaskPr iority， 针对类别1至类别3任务进行优 先级规 划， TaskPriority的取值范围为{1,2,3}， 当TaskPriority取值为i时,执行第i类任务； TaskPriority取1时优先级最高， 此时执 行类别 （1） ， 依次类 推； 其中类别 （3） ： 当接收到预设的抓取轨 迹信息时， 获取双机 械臂初始 跟随位置 。 3.根据权利要求2所述任务分级的双机械臂协同规划及控制方法， 其特征在于， 为运动 规划系统设置任务分级时序优化函数TaskPr iority， 针对类别1至类别3任务进行优 先级规 划， TaskPriority的取值范围为{1,2,3}， 当TaskPriority取值为i时,执行第i类任务； TaskPriority取1时优先级最高， 此时执 行类别 （1） ， 依次类 推； 其中类别 （3） ： 当接收到预设的抓取轨 迹信息时， 获取双机 械臂初始 跟随位置 。 4.根据权利要求3所述任务分级的双机械臂协同规划及控制方法， 其特征在于， 根据 所 述抓取轨迹信息、 所述初始跟随位置， 确定各个所述机械臂对应的一致性跟踪信息； 基于所 述一致性跟踪信息建立积分型滑模面； 根据所述积分型滑模面的一阶导数， 确定各个所述 机械臂对应的控制信息； 双机械臂需要到达的位置： 其中，  为在t时刻的目标跟 随位置； 为在初始时刻位置， ； 为 为需要调整的信息， 用于表示 目标跟随位置与初始 跟随位置之间的相对位置 差， 。 系统可以实现一致性跟踪： ， 其中 的大小满足 ， 且p和q都是正奇数； 控制信息指的是每个机械臂在x  ， y ， z方向上的目标参考速度， 以向 量 进行表示。 5.根据权利要求4所述任务分级的双机械臂协同规划及控制方法， 其特征在于， 具体的 积分型滑模面 可以如下： 其中,  权　利　要　求　书 1/2 页 2 CN 115401697 A 2, 为机械手臂在0时刻的零时位置； ； 分别为x ， y ， z方向上积 分型滑模面。 6.一种任务分级的双机 械臂协同规划及控制系统， 其特 征在于， 该系统包括： 任务分类模块， 用于采取任务优先分级时序优化方法， 保障任务 安全执行； 任务划分模块， 用于充分利用 双机械臂 的对称性优势， 通过对双臂机器人的抓取动作 进行合理的任务优先规划和对偶运动规划， 对双机械臂机器人的抓取动作进 行如下任务细 分， 并对其执 行顺序进行优化划分， 序号代 表任务执 行顺序： (1)机械臂关节角度限位,保持关节角在角度限制范围内变化θ mi n<θ <θ max； (2)保持抓取目标物处于摄 像机视野范围中心； (3)双机械臂末端执 行器的位置控制； (4)双机械臂末端执 行器的姿态控制； (5)双机械臂构型的优化； (6)双机械臂运动最小化； 对任务序列进行如 下类别划分： 类别1： 任务(1)对应物 理约束与 安全控制目标， 类别2： 任务(2)是机械臂能够抓取目标物的先决条件， 类别3： 任务(3)～(4)对应核心 抓取任务， 类 别4： 任务(5)～(6)对应控制优化目标。 7.一种计算机设备， 其特征在于， 包括处理器以及用于存储处理器可执行程序的存储 器， 所述处理器执行存储器存储的程序时， 所述计算机设备执行如权利要求1至5中任一项 所述的方法。 8.一种存储介质， 存储程序， 其特征在于， 所述程序被处理器执行时， 所述处理器执行 权利要求1至 5中任意一项所述的方法。权　利　要　求　书 2/2 页 3 CN 115401697 A 3