(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210659413.5 (22)申请日 2022.06.10 (71)申请人 合肥工业大 学 地址 230009 安徽省合肥市包河区屯溪路 193号 (72)发明人 朱敏　陈洋　胡若海　储昭碧　 (74)专利代理 机构 合肥和瑞知识产权代理事务 所(普通合伙) 34118 专利代理师 王挺 (51)Int.Cl. B25J 9/16(2006.01) B25J 19/02(2006.01) (54)发明名称 一种机器人机械臂末端 姿态的调整方法 (57)摘要 本发明提供了一种机器人机械臂末端姿态 的调整方法， 属于机器人机械臂控制技术领域。 主要解决移动机器人在操作电气柜按钮时， 因移 动机器人定位误差造成机器人机械臂末端姿态 偏差， 导致机械臂不能精准操作按钮的问题。 调 整方法包括： 机器人移动到目标电气柜操作面板 前、 驱动机械臂带动相机并拍取校正板图片、 识 别得到定位圆位置、 根据三个顶 点和定位圆的位 置关系获取三个顶点的坐标信息， 计算出三个方 向的倾斜角， 再依据坐标转换求取机械臂末端在 基坐标系下的目标姿态， 并驱动机械臂使末端到 达目标姿态。 本发明基于深度相机视觉信息求取 机械臂末端的目标姿态， 实现对机器人机械臂末 端姿态的调整。 权利要求书5页 说明书11页 附图8页 CN 115042175 A 2022.09.13 CN 115042175 A 1.一种机器人机械臂末端姿态的调整方法， 其特征在于， 本调整方法涉及的系统包括 机器人、 电气柜和校正板， 其中， 所述电气柜操作面上装有按钮， 将按钮的圆心记为点A； 所 述校正板为矩形， 粘贴在电气柜操作面的上方， 该校正板上设有一个黑色的直角三角形和 一个黑色的校正板定位圆， 将校正板定位圆的圆心记为点C4， 直角三角形的三个顶点分别 记为顶点C1、 顶点C2和顶点C3， 其中顶点C1为直角三角形直角处的顶点； 校正板粘贴完成后， 顶点C1位于校正板的左上角， 顶点C1和顶点C2的连线为水平线， 顶点C1和顶点C3的连线与顶 点C1和顶点C2的连线垂直， 点C4和顶点C1的连线垂直于顶点C2和顶点C3的连线， 且校正板定 位圆位于直角三角形的右下方； 所述校正板定位圆用于相机识别定位校正板和确定三个顶 点的位置； 所述机器人包括AGV小车、 6自由度机械臂、 末端执行器和深度相机， 所述6自由度机械 臂包括6个旋转关节和机械臂基座， 所述6个旋转关节由机械臂基座开始依次为旋转关节 一、 旋转关节 二、 旋转关节三、 旋转关节四、 旋转关节五、 旋转关节六， 6个旋转关节依次分别 绕第一关节轴、 第二关节轴、 第三关节轴、 第四关节轴、 第五关节轴、 第六关节轴旋转， 其中， 第四关节轴和第 五关节轴相互垂直， 第 五关节轴和第六关节轴相互垂直； 所述末端执行器 和深度相机均安装在6自由度机械臂的末端 上， 且深度相机的光轴与第六关节轴平行， 深度 相机跟随6自由度机 械臂运动； 所述调整方法包括以下步骤： 步骤1， 定义像素坐标系、 图像坐标系、 相机坐标系、 电气柜操作面坐标系、 机械臂末端 坐标系和机 械臂基座 坐标系， 并使用深度相机进行相机标定； 所述像素坐标系以图像左上角为原点建立的以像素为单位的直角坐标系， 包括互相垂 直的像素U轴和像素V轴， 像素U轴上的横坐标为像素在其图像中的列数， 方向为平行于图像 面向右， 像素V轴上的纵坐标为像素在其图像中的行 数， 方向为垂直于像素U轴向下； 所述图像坐标系以相机光轴与图像平面的交点为原点建立的坐标系， 包括互相垂直的 图像X轴和图像Y轴， 图像X轴与像素U轴平行且方向一致， 图像Y轴与像素V轴平行且方向一 致； 所述相机坐标系是以相机光心为原点建立的坐标系， 包括相机Xc轴、 相机Yc轴和相机 Zc轴， 其中， 相机Zc轴 为相机光轴且取摄影方向为正方向， 相机Xc轴与像素U轴平行且方向 一致， 相机Yc轴与像素V轴平行且方向一 致； 所述电气柜操作面坐标系是以点C4为原点建立的坐标系， 包括操作面Xt轴、 操作面Yt 轴、 操作面Zt轴， 其中， 操作面Zt轴 与操作面法线平行， 方向由操作面外指向操作面内， 操作 面Yt轴与顶点C1和顶点C3所在直角边平行， 方向为顶点C1指向顶点C3， 操作面Xt轴与顶点C1 和顶点C2所在直角边平行， 方向为顶点C1指向顶点C2； 所述机械臂末端坐标系以旋转关节六中心为原点建立的坐标系， 包括机械臂末端Xe 轴、 机械臂末端Ye轴和机械臂末端Ze轴， 其中， 机械臂末端Ze轴 为第六关节轴， 即械臂末端 Ze轴与相机光轴平行且方向一致， 机械臂末端Xe轴与相机Xc轴平行且方向一致， 机械臂末 端Ye轴与相机 Yc轴平行且方向一 致； 机械臂基座坐标系以机械臂基座中心点为原点 建立的坐标系， 包括机械臂基座Xb轴、 机 械臂基座Yb轴和机械臂基座Zb轴， 其中， 机械臂基座Zb轴和第一关节轴平行， 机械臂基座Xb 轴以AGV小车正前 方为正方向， 机 械臂基座Yb轴由坐标系右手法则确定；权　利　要　求　书 1/5 页 2 CN 115042175 A 2使用相机标定算法对深度相机进行相机标定， 标定出的结果为深度相机的内参矩阵M， 其表达式为： 式中， fx为图像坐标系中图像X轴上的归一化焦距， fx＝f/dx， 其中， f为深度相机的焦距， 单位 为mm， dx为像素坐标系中的每 个像素在图像X轴上的物理尺寸， 单位 为mm/pixel； fy为图像坐标系中图像Y轴上的归 一化焦距， fy＝f/dy， dy为像素坐标系中的每个像素在 图像Y轴上的物理尺寸， 单位 为mm/pixel； u0为图像坐标系的原点在像素坐标系中的横坐标， v0为图像坐标系的原点在像素坐标 系中的纵坐标， 即图像坐标系的原点在像素坐标系中的坐标为(u0， v0)； 步骤2， 机器人移动到目标电气柜操作面板板前， 驱动6自由度机械臂使深度相机到达 校正板拍摄位置进 行拍照， 然后对拍摄得到的图像做图像处理， 得到顶 点C1、 顶点C2和顶 点 C3的像素坐标和深度值， 分别为： 顶点C 1的像素坐标(u1， v1)和深度值d1， 顶点C2的像素坐标 (u2， v2)和深度值d2， 顶点C3的像素坐标(u3， v3)和深度值d3； 步骤3， 根据步骤1标定 出的内参矩阵M和顶点坐标变换 公式一分别计算出顶点C1在相机 坐标系下的相机坐 标(xc1， yc1， zc1)、 顶点C2在相机坐 标系下的相机 坐标(xc2， yc2， zc2)， 顶点C3 在相机坐标系下的相机坐标(xc3， yc3， zc3)， 所述顶点 坐标变换公式一的表达式为： 其中， Z是顶点在相机坐标系下测出来的深度值， 即顶点到深度相机平面的距离， u为顶 点在像素坐标系下的横坐标， v为顶点在像素坐标系下的纵坐标， (xc， yc， zc)为顶点在相机 坐标系下的相机坐标， 所述深度相机平面 为相机Xc轴 、 相机Yc轴构成的平面； 步骤4， 根据顶点C1、 顶点C2和顶点C3 的坐标关系， 求深度相机 的相机Xc轴与操作面Xt 轴的倾斜角、 相机Yc轴 与操作面Yt轴的倾斜角、 相机Zc轴 与操作面Zt轴的倾斜角， 所述求解 过程如下： 步骤4.1， 过顶点C1做一条平行于相机Xc轴的直线， 过顶点C2做该直线的垂线， 并将其 交点记为交点Q1， 连接顶点C1、 顶点C2、 交点Q1构成一个直角三角形， ∠Q1C1C2即为深度相机 的相机Xc轴与操作面Xt轴的倾 斜角， 其表达式为： 步骤4.2， 沿着深度相机光轴的方向， 将顶点C3投影到过顶点C1且平行于相机Xc ‑相机 Yc轴的平面 上， 记为点Q2， 连接顶点C3、 顶点C1、 点Q2构成一个 直角三角形， ∠Q2C1C3即为深度 相机的相机 Yc轴与操作面Yt轴的倾 斜角， 其表达式为： 步骤4.3， 沿着深度相机光轴的方向， 将顶点C1投影到过顶点C2且平行于相机Xc ‑相机权　利　要　求　书 2/5 页 3 CN 115042175 A 3