(19)国家知识产权局 (12)发明 专利 (10)授权公告 号 (45)授权公告日 (21)申请 号 202210883755.5 (22)申请日 2022.07.26 (65)同一申请的已公布的文献号 申请公布号 CN 114952868 A (43)申请公布日 2022.08.30 (73)专利权人 之江实验室 地址 310023 浙江省杭州市文一西路1818 号 (72)发明人 黄秋兰　宛敏红　 (74)专利代理 机构 杭州求是专利事务所有限公 司 33200 专利代理师 邱启旺 (51)Int.Cl. B25J 9/16(2006.01) B25J 11/00(2006.01)B25J 13/00(2006.01) 审查员 韩胜南 (54)发明名称 7自由度SRS型机械臂控制方法及装置、 弹琴 机器人 (57)摘要 本发明公开了一种7自由度SRS型机械臂控 制方法及装置、 弹琴机器人， 该方法包括： 根据机 械臂末端的当前位姿和下一个节奏对应琴键的 目标位姿， 分别对机械臂关节空间的起始关节角 序列和目标关节角序列进行逆运动学解算； 根据 当前位姿和目标位姿， 计算机械臂末端当前位置 和目标位置的距离； 若距离小于等于预定阈值， 则进行关节空间的轨迹插补， 从而控制机械臂运 动至所述目标位姿； 反之则进行笛卡尔空间的轨 迹规划， 在预设的轨迹上取至少三个轨迹点， 对 轨迹点对应的机械臂肘部和肩部的四个关节角 进行逆运动学解算， 且轨迹点对应的机械臂腕部 的三个关节角与所述当前位姿的关节角保持一 致， 根据每个轨迹点的关节角序列控制机械臂运 动至所述目标位姿。 权利要求书2页 说明书8页 附图3页 CN 114952868 B 2022.11.11 CN 114952868 B 1.一种7自由度SRS型机 械臂控制方法， 其特 征在于， 应用于弹琴机器人， 包括： 以弹琴机器人的腰部 中心为原点， 建立世界坐标系， 其中， 坐标原点位于机器人腰部 中 心， X轴正方向指向机器人正前， Y轴正方向指向机器人左侧， Z轴正方向指向机器人 上方； 根据机械臂末端的当前位姿和下一个节奏对应琴键的机械臂末端的目标位姿， 分别对 机械臂关节空间的起始关节角序列和目标关节角序列进行逆运动学解 算； 根据所述当前位姿和目标位姿， 计算机械臂末端当前位置和目标位置的距离， 其中机 械臂末端当前位置和目标位置的距离为当前位置与目标位置在Y方向上坐标的差值； 若所述距离小于等于预定阈值， 则进行关节空间的轨迹插补， 并以一定时间周期计算 机械臂的关节角序列、 关节角速度序列和关节角加速度序列， 从而控制 机械臂运动至所述 目标位姿； 若所述距离大于预定阈值， 则进行笛卡尔空间的轨迹规划， 在预设的轨迹上取至少三 个轨迹点， 对所述轨迹点对应的机械臂肘部和肩部的四个关节角进行逆运动学解算， 且所 述轨迹点对应的机械臂腕部的三个关节角与所述当前位姿对应的关节角保持一致， 根据每 个所述轨 迹点的关节角序列控制机 械臂运动至所述目标位姿。 2.根据权利要求1所述的方法， 其特征在于， 机械臂末端当前位置和目标位置的距离为 当前位置与目标位置在Y方向上坐标的差值。 3.根据权利要求1所述的方法， 其特征在于， 在关节空间的轨迹插补的过程中， 当前位 置与目标位置对应的角度、 角速度和 加速度满足如下式所示的关系： 其中， 起始点角度、 角速度和角加速度为 ， ， ， 目标点的角度、 角速度和角加速度 为 ， ， ， 为运动起始时间， 为运动结束时间； 根据上式， 得到多 项式系数 = *  根据所述多项式系数， 以一定时间周期计算机械臂 的关节角序列、 关节角速度序列和 关节角加速度 序列， 从而控制机 械臂运动至所述目标位姿。 4.根据权利要求1所述的方法， 其特征在于， 所述预设的轨迹为直线轨迹或抛物线轨 迹。 5.根据权利要求1所述的方法， 其特 征在于， 在预设的轨 迹上取至少三个轨 迹点， 包括： 根据经验值以及机 械臂末端当前位置和目标位置的距离， 计算抛物线的系数； 根据所述系数以及当前位置的Y坐标、 目标位置的Y坐标， 计算每个轨迹点在Z方向的坐权　利　要　求　书 1/2 页 2 CN 114952868 B 2标。 6.一种7自由度SRS型机 械臂控制装置， 其特 征在于， 应用于弹琴机器人， 包括： 建立模块， 用于以弹琴机器人的腰部中心为原点， 建立世界坐标系， 其中， 坐标原点位 于机器人腰部中心， X轴正方向指向机器人正前， Y轴正方向指向机器人左侧， Z轴正方向指 向机器人 上方； 解算模块， 用于根据机械臂末端的当前位姿和下一个节奏对应琴键的机械臂末端的目 标位姿， 分别对机 械臂关节空间的起始关节角序列和目标关节角序列进行逆运动学解 算； 计算模块， 用于根据所述当前位姿和目标位姿， 计算机械臂末端当前位置和目标位置 的距离， 其中机械臂末端当前位置和目标位置的距离为当前位置与目标位置在Y方向上坐 标的差值； 轨迹插补模块， 用于若所述距离小于等于预定阈值， 则进行关节 空间的轨迹插补， 以一 定时间周期计算机械臂的关节角序列、 关节角速度序列和关节角加速度序列， 从而控制 机 械臂运动至所述目标位姿； 轨迹规划模块， 用于若所述距离大于预定阈值， 则进行笛卡尔空间的轨迹规划， 在预设 的轨迹上取至少三个轨迹点， 对所述轨迹点对应的机械臂肘部和肩部的四个关节角进 行逆 运动学解算， 且所述轨迹点对应的机械臂腕部的三个关节角与所述当前位姿对应的关节角 保持一致， 根据每 个所述轨 迹点的关节角序列控制机 械臂运动至所述目标位姿。 7.一种电子设备， 其特 征在于， 包括： 一个或多个处 理器； 存储器， 用于存 储一个或多个程序； 当所述一个或多个程序被所述一个或多个处理器执行， 使得所述一个或多个处理器实 现如权利要求1 ‑5任一项所述的7自由度SRS型机 械臂控制方法。 8.一种计算机可读存储介质， 其上存储有计算机指令， 其特征在于， 该指令被处理器执 行时实现如权利要求1 ‑5中任一项所述的7自由度SRS型机 械臂控制方法的步骤。 9.一种弹琴机器人， 其特征在于， 包括存储器和处理器， 所述存储器中存储有计算机程 序， 所述处理器调用所述存储器中的计算机程序时实现如权利要求 1‑5任一项所述的7自由 度SRS型机 械臂控制方法的步骤。权　利　要　求　书 2/2 页 3 CN 114952868 B 3