(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210387941.X (22)申请日 2022.04.13 (71)申请人 上海电气集团股份有限公司 地址 200336 上海市长 宁区兴义路8号3 0层 (72)发明人 吴炳坚　吕燕　石海　 (74)专利代理 机构 上海弼兴律师事务所 31283 专利代理师 林嵩　罗朗 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 机器人机械臂的运动控制系统、 协作机器人 及存储介质 (57)摘要 本发明公开了一种机器人机械臂的运动控 制系统、 协作机器人及存储介质， 所述运动控制 系统包括： 标准姿态角获取模块获取机器人坐标 系下末端工具以下一个作业拐点为运动目标时， 当前作业点的标准欧拉姿态角； 力检测模块实时 检测在当前作业点作用于末端工具的作用力； 分 力选取模块将作用力分解到标准欧拉姿态角的 三个角度方向上， 并选取朝向下一个作业拐点的 作用分力； 作业控制模块基于作用分力控制末端 工具运动到下一个作业拐点。 本申请通过获取标 准姿态角， 并将实时的作用力分解到标准欧拉姿 态角的三个角度方向上， 并以朝向下一个作业拐 点的作用分力进行后续控制， 解决了传统协作机 器人抖动、 偏移、 工具末端遇硬物反弹等缺点， 使 得操作手感灵活。 权利要求书2页 说明书8页 附图4页 CN 114750153 A 2022.07.15 CN 114750153 A 1.一种机器人机械臂的运动控制系统， 其特征在于， 所述机械臂末端的法兰上设有末 端工具， 所述 运动控制系统包括： 标准姿态角获取模块， 用于获取机器人坐标系下所述末端工具以下一个作业拐点为运 动目标时， 当前作业 点的标准欧拉姿态角； 力检测模块， 用于实时检测在所述当前作业 点作用于所述末端工具的作用力； 分力选取模块， 用于将所述作用力分解到所述标准欧拉姿态角的三个角度方向上， 并 选取朝向下一个作业拐点的作用分力； 作业控制模块， 用于基于所述作用分力控制所述末端工具运动到所述下一个作业拐 点。 2.如权利要求1所述的机器人机械臂的运动控制系统， 其特征在于， 所述运动控制系统 还包括： 法兰姿态角获取模块， 用于实时获取机器人坐标系下 所述法兰的法兰欧拉姿态角； 所述标准姿态角获取模块用于根据所述当前作业点与下一个作业拐点之间的第一矢 量向量以及所述法兰欧拉姿态角， 计算得到所述标准欧拉姿态角。 3.如权利要求2所述的机器人机械臂的运动控制系统， 其特征在于， 所述标准姿态角获 取模块包括： 目标点选取单元， 用于在所述末端工具的作业面上选取一目标点， 所述目标点和所述 当前作业 点之间的连线与所述当前作业 点和下一个作业拐点之间的连线垂直； 矢量获取 单元， 用于获取 所述当前作业 点与所述目标点之间的第二矢量向量； 姿态矩阵获取单元， 用于根据 所述法兰欧拉姿态角确定与所述末端工具的不同作业动 作对应的姿态 矩阵方程； 所述作业动作包括偏航、 俯仰和横滚按照任意 顺序作业； 旋转矩阵计算单元， 用于根据所述第一矢量向量、 所述第二矢量向量和所述姿态矩阵 方程计算得到姿态旋转矩阵； 所述姿态矩阵用于表征从当前作业点的姿态运动到下一个作 业拐点的姿态之间的旋转矩阵； 分解单元， 从所述姿态旋转矩阵中分解得到所述标准欧拉姿态角。 4.如权利要求3所述的机器人机械臂的运动控制系统， 其特征在于， 所述运动控制系统 通过以下公式计算得到所述标准欧拉姿态角， 具体包括： 其中， T为姿态旋转矩阵， 为第一矢量向量对应的单位方向向量， 为第二矢量向量 对应的单位方向向量， R为姿态矩阵方程， R中的每项值根据法兰欧拉姿态角计算得到， 标准 欧拉姿态角根据T中的每项值反推 计算得到 。权　利　要　求　书 1/2 页 2 CN 114750153 A 25.如权利要求3所述的机器人机械臂的运动控制系统， 其特征在于， 所述标准姿态角获 取模块还 包括： 光学位置获取单元， 用于获取光学坐标系下所述下一个作业拐点的第 一光学位置数据 和所述目标点的第二 光学位置数据； 数据转换单元， 用于根据所述机器人坐标系与 所述光学坐标系的坐标系转换矩阵以及 所述第一光学位置数据、 所述第二光学位置数据， 得到所述机器人坐标系 下所述下一个作 业拐点的第一机器位置数据和所述目标点的第二机器位置数据； 所述矢量获取单元用于根据所述第一机器位置数据和所述第二机器位置数据得到所 述第二矢量向量。 6.如权利要求2所述的机器人机械臂的运动控制系统， 其特征在于， 所述标准姿态角获 取模块包括： 工具位置获取单元， 用于实时获取机器人坐标系下所述末端工具在 当前作业点的工具 位置数据； 矢量获取单元， 用于根据所述工具位置数据和所述下一个作业拐点的位置数据计算得 到所述第一矢量向量。 7.如权利要求6所述的机器人机械臂的运动控制系统， 其特征在于， 所述工具位置获取 单元用于获取机器人坐标系下所述法兰的法兰位置数据， 并根据所述末端工具与所述法兰 的位置对应关系以及所述法兰位置数据得到所述工具位置数据。 8.如权利要求6所述的机器人机械臂的运动控制系统， 其特征在于， 所述工具位置获取 单元用于获取光学坐标系下所述末端工具的光学位置数据， 并根据所述机器人坐标系与所 述光学坐标系的坐标系转换矩阵以及所述 光学位置数据得到所述工具位置数据。 9.一种协作机器人， 其特征在于， 包括如权利要求1 ‑8中任意一项所述的机器人机械臂 的运动控制系统。 10.一种计算机可读存储介质， 其上存储有计算机程序， 其特征在于， 所述计算机程序 被处理器执行时实现如下机器人机械臂的运动控制方法， 所述机械臂末端的法兰上设有末 端工具， 所述方法包括： 获取机器人坐标系下所述末端工具以下一个作业拐点为运动目标时当前作业点的标 准欧拉姿态角； 实时检测作用于所述末端工具的作用力； 将所述作用力分解到所述标准欧拉姿态角的三个角度方向上， 并选取朝向下一个作业 拐点的作用分力； 基于所述作用分力控制所述末端工具运动到所述下一个作业拐点。权　利　要　求　书 2/2 页 3 CN 114750153 A 3