(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211023343.0 (22)申请日 2022.08.25 (71)申请人 浙江工业大 学 地址 310014 浙江省杭州市拱 墅区潮王路 18号 (72)发明人 徐建明　徐彬彬　何德峰　刘安东　 张文安　 (74)专利代理 机构 杭州天正专利事务所有限公 司 33201 专利代理师 楼明阳 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种利用可浮动标准球体的机器人TCP标定 方法 (57)摘要 一种利用可浮动标准球体的机器人TCP标定 方法， 包括如下步骤： 通过双目立体视觉测量可 浮动标准球在无外力状态下球心在机器人基坐 标系中的坐标； 控制机器人以不同姿态使TCP与 标准球体接触三次以上， 接触时记录机器人关节 转角信息， 并测量接触时标准球体下降的高度； 根据测量所得的机器人关节转角信息、 标准球无 外力状态下坐标及接触时标准球体下降高度， 结 合机器人的结构参数， 建立目标函数， 通过非线 性优化求解， 计算出TCP在机器人法兰盘坐标系 中的坐标， 并评估标定精度。 该标定方法不需要 机器人TCP点多次与同一固定点重合， 将点 ‑点重 合要求弱化为点 ‑面重合的同时， 避免了TCP与外 界的撞击。 当机器人TCP出现较小偏移时， 还可 实 现自主标定 。 权利要求书2页 说明书6页 附图2页 CN 115533893 A 2022.12.30 CN 115533893 A 1.一种利用浮动标准球的机器人TCP标定方法， 包括如下步骤： (1)首先通过工业相机获取标准球体无外力状态下的球心在机器人基坐标系中的坐 标。 将三个相机呈正三角形放置， 可浮动标准球置于三角形外心， 整个装置水平放置于机 器人工作空间内， 相机的光轴相交于球体， 通过双目校正建立双目立体视觉系统， 识别定位 无外力作用下的球心在相机坐标系中的位置， 通过手眼标定建立相机坐标系与机器人基坐 标系的坐标变换关系， 进 而得出球心在机器人基坐标系中的坐标bPs。 (2)控制机器人以不同的姿态使得机器人工具中心点与标准球上半球面接触， 记录接 触时标准球下降的高度、 机器人 各关节的转角； 其中接触时尽可能保持末端工具的延长线穿过球心。 接触时机器人各关节转角结合机器人自身结构、 机器人基坐标系原点可以表示出法兰 盘坐标系相对于机器人基坐标系的位姿， 再结合TCP信息， 表示出接触点在机器人基坐标系 中的坐标即工具中心点在机器人基坐标系中的坐标； 所述机器 人工具中心点在机器 人基坐标系{B}中的坐标bPtcp与机器人法兰盘坐标系{E} 中的坐标ePtcp之间的关系如下： 其中，bTe是机器人法兰盘坐标系{E}的原点在 机器人基坐标系{B}中的坐标， θ为机器人 各关节的转角， θ＝{ θ1, θ2, θ3, θ4, θ5, θ6}T， 是机器人法兰盘坐标系 {E}相对于机器人基 坐标系{B}的旋转矩阵， 由机器人 各关节转角及机器人 结构参数确定 。 其中， 所述接触点个数为 k(k≥3)， 规定 接触点在两个相机 视野所在的公共区域。 (3)通过双目立体视 觉获取接触点在相机坐标系中的坐标， 并进行记录 。 (4)利用接触点与 标准球的球心之间的距离等于半径为条件， 建立目标函数， 通过非线 性优化方法， 求解获得机器人工具中心点在机器人法兰盘坐标系中的坐标ePtcp＝{extcp, eytcp,eztcp}T， 完成标定； 接触时机器人工具中心点与球心在机器人基坐标系中的坐标之间的距离等于标准球 体的半径， 第i次接触时(i＝1,2, …,k)， 在机器人基坐标系中接触点坐标bPtcpi与球心坐标 bPsi组成的向量 为: 利用标准球半径不变的约束关系， 建立以下目标函数： 其中，bPtcpi为触点在机器人基坐标系{B}中的坐标；bPsi为接触时球心在机器人基坐标 系{B}中的坐标； 为触点bPtcpi对应的机器人法兰 盘坐标系相对于机器人基 坐标系{B} 的旋转矩阵；bTei为触点bPtcpi对应的机器人法兰盘坐标系{E}的原点在机器人基坐标系{B} 中的坐标；bPs为无外力作用的标准球体球心在机器人基 坐标系{B}中的坐 标； Di＝[0 0 di]T权　利　要　求　书 1/2 页 2 CN 115533893 A 2为接触时， 标准球体球心的坐标变换， 因为此处为可浮动标准球， 此时只有z方向有向下偏 移， 此处di为标准球 体球心下降的高度； R为标准球 体的半径值。 通过Levenberg –Marquardt非线性优化方法来优化求解， 当函数 收敛于解ePtcp＝{extcp ,eytcp,eztcp}T， 且在解处目标函数值小于阈值 ε 时， 表明当前标定结果符合精度要求， 标定成 功。 (5)在机器人工作一段时间后， 工具可 能出现磨损或者弯曲， 导致TCP位置不准确， 此 时 可从在线状态下对TCP进行修正， PC端向机器人发送 步骤(2)中在接触时的机器人各关节 转 角信息θ＝{ θ1, θ2, θ3, θ4, θ5, θ6}T， 机器人运行到相应的转角， 判断工具中心点是否与标准球 体接触， 如果未接触， 通过双目立体视觉系统定位出工具中心 点在相机坐标系中的坐标， 须 通过计算得到两次工具中心点在相 机坐标系 下的位置偏差， 发送给机器人， 引导工具中心 点与标准球 体相接触， 记录 接触时的机器人 各关节转角信息和标准球下降高度信息 。 通过步骤(4)中建立的目标函数， 将接触时的各关节转角信息和标准球下降高度信息 代入， 通过非线性优化， 求解出新的机器人工具中心点在法兰盘坐标系中的坐标ePtcp＝ {extcp,eytcp,eztcp}T， 并验证标定结果是否满足精度要求。 所述可浮动标准球体， 由一个标准球和一个可以测量并导出下降信息的平台组成， 如 果无外力作用， 标准球始终处于同一个位置， 如果外力作用， 平台和标准球下降， 下降高度 作为判断接触的依据， 即d>0时为接触， d＝0时为不接触， 保证了接触的充分性， 使 得机器人 工具中心点与标准球 体接触时， 自动检测接触信号， 实现标定过程的自动控制。 2.根据权利要求1任一项所述的利用浮动标准球的机器人TCP标定方法， 其特征在于， 所述标准球为可浮动标准球 体， 在无外力作用下始终处于同一高度， 只能垂直上 下运动。 3.根据权利要求1任一项所述的利用浮动标准球的机器人TCP标定方法， 其特征在于， 通过可浮动标准球的下降高度判断工具中心点是否与标准球 体接触。权　利　要　求　书 2/2 页 3 CN 115533893 A 3