(19)国家知识产权局 (12)发明 专利 (10)授权公告 号 (45)授权公告日 (21)申请 号 202210746684.4 (22)申请日 2022.06.29 (65)同一申请的已公布的文献号 申请公布号 CN 114800534 A (43)申请公布日 2022.07.29 (73)专利权人 杭州三坛医疗科技有限公司 地址 310030 浙江省杭州市西湖区三墩镇 西园九路8号3幢E座7楼701室 (72)发明人 张巍　沈丽萍　雷俊勇　李明　 牛乾　 (74)专利代理 机构 北京柏杉松知识产权代理事 务所(普通 合伙) 11413 专利代理师 孟维娜　马敬 (51)Int.Cl. B25J 9/16(2006.01)审查员 张浩 (54)发明名称 一种机械臂控制方法及装置 (57)摘要 本发明实施例提供了一种机械臂控制方法 及装置， 涉及数据处理技术邻域， 上述方法包括： 在机械臂的基座对应的基座坐标系下建立机械 臂的正运动学模 型； 根据正运动学模 型确定机械 臂末端在基座坐标系下的位姿数据与机械臂中 各个关节的转动角度之间的第一 关系； 获得机械 臂末端在基座坐标系下的期望位姿数据； 根据正 运动学模型、 第一关系以及期望位姿数据， 计算 对机械臂中各个 关节进行转动的目标角度； 控制 各个关节按照对应的目标角度转动， 以使得机械 臂末端在基座坐标系下的位姿数据为期望位姿 数据。 应用本发明实施例提供的方案控制机械 臂， 能够提高控制机 械臂的效率。 权利要求书5页 说明书23页 附图7页 CN 114800534 B 2022.10.14 CN 114800534 B 1.一种机 械臂控制方法， 其特 征在于， 所述方法包括： 在机械臂 的基座对应的基座坐标系下建立机械臂 的正运动学模型， 其中， 在所述正运 动学模型中所述机械臂各个关节相对于预设基准位置的转动角度为零， 所述机械臂配置有 六个关节， 第一关节、 第二关节、 第三关节、 第四关节、 第五关节和 第六关节 通过连杆依次连 接， 所述第一关节的一端与所述机械臂基座相连， 所述第六关节的一端与所述机械臂末端 相连， 所述第二关节、 第三关节以及第四关节的轴线平行； 根据所述正运动学模型确定所述机械臂末端在基座坐标系下的位姿数据与所述机械 臂中各个关节的转动角度之间的第一关系； 获得所述机械臂末端在所述基座 坐标系下的期望位姿数据； 根据所述正运动学模型、 第一关系以及期望位姿数据， 计算对所述机械臂中各个关节 进行转动的目标角度； 控制各个关节按照对应的目标角度转动， 以使得所述机械臂末端在基座坐标系下的位 姿数据为所述期望位姿数据； 所述第一关系按照以下表达式表示： 其中， 所述 为所述机械臂末端在所述基座坐标系下的位姿数据， 所述N为所述机械臂 中关节总数量， 所述 为自然常数， 所述 为第n个关节在所述正运动学模型中的运动旋 量， 所述 为第n个关节相对于所述预设基准位置的转动角度， 所述 为所述机械臂末 端在所述 正运动学模型中的初始位姿数据； 所述根据所述正运动学模型、 第一关系以及期望位姿数据， 计算对所述机械臂中各个 关节进行转动的目标角度， 包括： 在所述正运动学模型中获得所述第五关节和第六关节的轴线交点的交点坐标以及平 行关节的关节轴线向量， 其中， 所述平行关节为： 所述第二关节、 第三关节或第四关节； 根据所述第一关系、 期望位姿数据、 交点坐标以及 关节轴线向量， 获得对所述第 一关节 进行转动的第一角度； 根据所述期望位姿数据和所述第 一角度， 计算所述机械臂末端在转动第 一角度后的第 一关节对应的目标关节坐标系下的期望坐标； 基于预先获得的第四关系， 根据计算得到的期望坐标， 获得对所述第五关节进行转动 的第五角度， 其中， 所述第四关系为： 所述机械臂末端在所述目标关节坐标系下的坐标与所 述第五关节的转动角度之间的关系； 根据所述第一角度、 第五角度以及期望位姿数据， 获得对所述第六关节进行转动的第 六角度； 根据所述第一角度、 第五角度、 第六角度、 第 一关系以及所述第四关节在所述正运动学 模型中的关节坐标， 预测所述第四关节经所述第二关节和 第三关节转动后在所述基座坐标 系下的关节坐标；权　利　要　求　书 1/5 页 2 CN 114800534 B 2根据所述第二关节、 第三关节、 第 四关节在所述正运动学模型中的关节坐标以及预测 得到的关节坐标， 获得对所述第二关节进 行转动的第二角度和对所述第三关节进 行转动的 第三角度； 基于预先获得的所述第二关节、 第三关节以及第 四关节的转动角度之间的第五关系， 根据计算得到的第二角度和第三角度， 获得对所述第四关节进行转动的第四角度。 2.根据权利要求1所述的方法， 其特征在于， 所述根据所述第 一关系、 期望位姿数据、 交 点坐标以及关节轴线向量， 获得对所述第一关节进行转动的第一角度， 包括： 根据所述第一关系、 期望位姿数据和交点坐标， 预测所述轴线交点经所述第 一关节、 第 二关节、 第三关节以及第四关节转动 后在基座 坐标系统下对应的第一 坐标点的第一 坐标； 根据所述交点坐标、 关节轴 线向量、 第一坐标以及预先获得的点轴位置关系， 预测所述 轴线交点经所述第二关节、 第三关节以及第四关节转动后在所述基座坐标系下对应的第二 坐标点的第二坐标， 其中， 所述点轴位置 关系为： 所述轴线交点、 第一坐标点、 第二坐标点以 及所述第一关节的轴线之间的位置关系； 基于预先获得的第三关系， 根据所述第一坐标、 第 二坐标， 获得对所述第 一关节进行转 动的第一角度， 其中， 所述第三关系为： 所述第一坐标点的坐标、 第二坐标点的坐标以及所 述第一关节的转动角度之间的对应关系。 3.根据权利要求2所述的方法， 其特 征在于， 所述 点轴位置关系按照以下表达式表示： 其中， 所述 为所述第一坐标点在所述基座坐标系下的坐标， 所述 为所述第一关节 的轴线上任一点在所述基座坐标系下的坐标， 所述 为所述第二坐标点在所述基座坐标系 下的坐标， 所述 为所述交点 坐标， 所述 为所述关节轴线向量。 4.根据权利要求2所述的方法， 其特 征在于， 所述第三关系按照以下表达式表示： 其中， 所述 为所述第一关节的轴线在所述正运动学模型中的运动旋量， 所述 为所述 第一角度， 所述 为自然常数， 所述 为所述第一坐标点在所述基座坐标系下的坐标， 所述 为所述第二 坐标点在所述基座 坐标系下的坐标。 5.根据权利要求1所述的方法， 其特征在于， 所述目标关节坐标系的第 一坐标轴方向与 所述第二关节的轴线方向之间的夹角为所述第一角度； 所述第四关系按照以下表达式表示： 其中， 所述 为所述机械臂末端在所述第一坐标轴上的坐标， 所述 为第一端点与第 二端点之间的距离， 所述第一端点为所述第 五关节中与所述第六关节相连的端点， 所述第权　利　要　求　书 2/5 页 3 CN 114800534 B 3