(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210406473.6 (22)申请日 2022.04.18 (71)申请人 浙江谱麦科技有限公司 地址 315048 浙江省宁波市宁波高新区 聚 贤路587弄 15号2#楼0 33幢7-1-1 (72)发明人 陈章位　张翔　林威威　 (74)专利代理 机构 杭州知见专利代理有限公司 33295 专利代理师 司敏 (51)Int.Cl. B25J 19/00(2006.01) B25J 9/22(2006.01) B25J 9/16(2006.01) (54)发明名称 工业机器人性能测量的姿态对齐方法 (57)摘要 本发明公开了一种工业机器人性能测量的 姿态对齐方法， 包括工业串联机器人、 机器人示 教器、 计算机、 双 目视觉测量相机结构和通过工 装安装于机器人法兰盘上的Q个被测的靶球， Q≥ 3； 工业串联机器人和机器人示教器数据连接， 计 算机分别与机器人示教器和双目测量相机结构 数据连接； 本发 明具有对机器人末端的姿态数据 测量精度高， 操作简单， 测量操作所耗费的时间 短的特点。 权利要求书2页 说明书5页 附图1页 CN 114571506 A 2022.06.03 CN 114571506 A 1.一种工业机器人性能测量的姿态对齐方法， 其特征是， 包括工业串联机器人、 机器人 示教器、 计算机、 双目视觉测量相机结构和通过工装安装于机器人法兰盘上的Q个被测的靶 球， Q≥3； 工业串 联机器人和机器人示 教器数据连接， 计算机 分别与机器人示教器和双目测 量相机结构数据连接； 包括以下步骤： (1‑1)双目视觉测量相机结构包括两台距离＞1.5m的相机， 使两台相机的视野范围覆 盖被测机器人的工作空间， 对所述相机进行 标定， 使相机的3D误差小于 0.05mm； (1‑2)工作人员通过机器人示教器选择工业串联机器人灵活工作空间中的任意不在同 一直线上的n个位置点； 利用n个位置点的坐标计算机器人坐标系到相机中的测量坐标系的 位置转换矩阵R2M； 其中， n≥5； (1‑3)所述相机依次测量各个被测靶球在测量坐标系下的坐标， 计算机得到第i个靶球 Pt(i)的坐标(xi， yi， zi),i＝1,2, …,Q； 计算机计算得到测量坐标系下靶球平 面的姿态矩阵 Meas； (1‑4)计算机从机器人示教器上读取机器人法兰盘在机器人坐标系下的姿态 矩阵Mr； 计算测量 坐标系下的机器人法兰盘的姿态 矩阵mt： mt＝R2M ×Mr； (1‑5)计算机假设机器人测量坐标系下靶球平面到机器人法兰盘平面的转换矩阵为 Mb2f， 设Mb2f为测量 坐标系下机器人法兰盘平面到靶球平面的转换矩阵MTrans的逆矩阵； 由于在测量 坐标系下机器人法兰盘平面到靶球平面的转换关系为： Meas＝mt ×MTrans， 设 mt的逆矩阵为Mt； 则MTrans＝Mt ×Meas， 利用公式Mb2f＝MTrans‑1计算得到测量坐标系下， 靶球平面到机 器人法兰盘平面的转换矩阵Mb2f； 所述相机测量靶球在测量坐标系下的坐标， 计算机使用上述坐标计算出靶球平面在测 量坐标系下的姿态， 再利用Mb2f将靶球在测量坐标系下的姿态 转换到测量坐标系下机器人 法兰盘平面的姿态， 即得到 机器人法兰盘姿态的实际值； 计算机将从机器人示教器读取的机器人法兰盘姿态转换到相机测量坐标系下， 即得到 机器人法兰盘姿态的理论 值； 此时已经将机器人法兰盘姿态和由相机测量得到的测量坐标系下的坐标计算得到的 姿态， 统一到测量 坐标系下的机器人法兰盘平面上， 即完成姿态对齐。 2.根据权利要求1所述的工业机器人性能测量的姿态对齐方法， 其特征是， 步骤(1 ‑2) 包括如下步骤： 计算机选取各个被测靶球中的一个作为TCP点， 所述相机采集TCP点Pt(1)的n个位置点 Pt1(1)， Pt1(2)， ...,Pt1(n)的坐标数据； 其中， Pt1(1)的坐标为(x1(j),y1(j),z1(j)),j＝ 1, 2,...,n； 计算机读取机器人示教器中TCP点的n个位置点M1(1),M1(2),...,M1(n)的坐标数 据； 其中， M1(1)的坐标为(xm1(j),ym1(j),zm1(j))； 设 利用公式R2M＝TPt1(n)×TM1(n)‑1计算得到R2M 。权　利　要　求　书 1/2 页 2 CN 114571506 A 23.根据权利要求2所述的工业机器人性能测量的姿态对齐方法， 其特征是， 步骤(1 ‑3) 包括如下步骤： (1‑3‑1)计算机读取测量坐标系下靶球Pt(1)从零位开始运动的第一个点的坐标(x1, y1,z1)， 靶球Pt(2)从零位开始运动的第一个点的坐标(x2,y2,z2)， 靶球Pt(3)从零位开始运 动的第一个点的坐标(x3,y3,z3)， 过Pt(1)、 Pt(2)和Pt(3)做 平面， 计算得到平面的法向量V； 设Pt(4)的坐标为(x4,y4,z4； 其中， x4＝(x1+x2+)x3/， y4＝(y1+y2+y3)/3， z4＝(z1+z2+z3)/ 3； 计算Pt(2)在过Pt(4)、 法向量为V的平面上的投影点A1的向量Pt2； 计算Pt(3)在过Pt (4)、 法向量 为V的平面上的投影点A 2的向量Pt3； (1‑3‑2)设过A1和Pt(1)的向量PtX＝Pt 2‑Pt(1),PtX的模|PtX|为： 向量PtX的单位向量Ptx为： 其中， PtX.x为PtX在X轴方向的分量， Pt X.y为PtX在Y轴方 向的分量， PtX.z为PtX在Z轴方向的分量； 设 过 A 2 ,P t ( 1 的 向 量 P t Y ＝ P t 3 ‑P t ( 1 ) ,P t Y 的 模 | P t Y | 为 ： 其中， PtY.x为PtY在X轴方向的分量， PtY.y为PtY在Y轴方向的分量， PtY.z为PtY在Z轴 方向的分量； 向量PtY的单位向量Pty为： (1‑3‑3)设向量PtZ等于PtX和PtY的叉乘， 向量PtZ的单位向量Ptz为： 利用Ptx,Pty,Ptz组成测量 坐标系下靶球平面的姿态 矩阵Meas： 其中， PtZ.x为PtZ在X轴方向的分量， PtZ.y为PtZ在Y轴方向的分量， PtZ.z为PtZ在Z轴 方向的分量， |PtZ|为PtZ的模。权　利　要　求　书 2/2 页 3 CN 114571506 A 3