CN113664835A - 机器人自动手眼标定方法与系统 - Google Patents

Abstract

本发明提供了一种机器人自动手眼标定方法与系统，包括控制机械臂运动至初始位置点pos0，机械臂末端位置位于标定板正对面；开始标定；机械臂采用自动规划路径方法，自动计算并规划路径，计算得到多组拍照点，将机械臂当前末端位置姿态数据发给相机模块的工控机系统；通过相机拍照判断拍照数据结果是否达到预设置的模型匹配度阈值，如果达到设置的阈值，标定数据OK，如果小于设置的阈值，则标定数据异常；计算相机的外参和内参；得到手眼标定结果和标定时的重投影误差。本发明只需要人工设置标定空间参数，即可完成自动标定，免去了人工示教，提升了标定效率；降低了人工成本，部署会更加简单，大大提升了工作人员效率。

Description

机器人自动手眼标定方法与系统

技术领域

本发明涉及机器人标定的技术领域，具体地，涉及机器人自动手眼标定方法与系统。

背景技术

为了满足制造业转型升级发展的需求，计算机视觉系统与智能机器人协同作业方式被越来越多地使用，相机标定是计算机视觉领域中的一个基本问题，手眼标定过程即是求解相机和机械臂之间的位姿旋转矩阵关系的过程。

由视觉传感器与工业机器人机械手的位置关系标定，都是通过选取一系列采样点，每一个采样点中标定板和相机之间的姿态关系不同，能够计算得到相机相关的內参与外参；再结合拍照时机械臂的位姿，计算得到手眼之间的位姿关系。

目前手眼标定方法，采样点的选取主要是通过人工示教预设点位实现。现有的方法无法自动生成机械臂的标定姿态，因此在标定过程中所有的机械臂运动都需要由标定人员根据经验来控制，手动示教点位。由于手眼标定的结果对获取的数据十分敏感，若拍照时的数据不理想，会导致标定结果具有较大的误差。同时只要环境改变时，就需要人为干预，重新在去示教标定该手眼关系，都是重复性工作，这也将大大引入人工成本。

在公告号为CN110497386B的专利文献中公开了一种协作机器人手眼关系自动标定方法，包括：放置标定板；牵引协作机器人至示教点并记录机器人坐标；牵引协作机器人至初始拍照点，记录机器人坐标，配置匹配模板；控制机器人不断变换拍照点，获取机器人坐标和标识物像素坐标点对；计算标定参数。

在公开号为CN107256567A的专利文献中公开了一种用于工业机器人手眼相机的自动标定装置，包括激光发生器、标定板和相机；所述激光发生器设置在机器人末端工具上，机器人末端工具安装在工业机器人上；所述标定板放置在工作台面上，标定板上设置有黑白相间的棋盘格；所述相机固定在工作台面上方，且位于标定板的上方。

针对上述中的相关技术，发明人认为上述方法采用了人为牵引协作机器人使标定探针触碰标定中心点，需要人为的去拖动示教，相比传统的标定方法，该方法也只是简化了标定过程，不够智能化，而且比较耗时，效率不高。因此，需要提出一种技术方案以改善上述技术问题。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种机器人自动手眼标定方法与系统。

根据本发明提供的一种机器人自动手眼标定方法，所述方法包括如下步骤：

步骤S1：控制机械臂运动至初始位置点pos0，在初始位置点pos0时，机械臂末端位置位于标定板正对面，其中，pos为标定过程中机械臂需要运动至该位置的拍照点；

步骤S2：根据相机至机械臂末端的距离d和机械臂运动时的位置边界点，开始标定；

步骤S3：基于设置的参数，机械臂采用自动规划路径方法，自动计算并规划路径，计算得到多组拍照点，覆盖整个标定板，机械臂每运动至拍照点i，通过mqtt协议，控制相机拍照，将机械臂当前末端位置姿态数据发给相机模块的工控机系统；

步骤S4：通过相机拍照判断拍照数据结果是否达到预设置的模型匹配度阈值，如果达到设置的阈值，标定数据OK，如果小于设置的阈值，则标定数据异常；

步骤S5：基于拍照得到的拍照数据，计算相机的外参和内参；结合每个拍照点时机械臂的末端位姿信息，计算求解AX＝XB矩阵方程，得到手眼标定结果和标定时的重投影误差，其中，A表示机械臂末端至机械臂基座的旋转矩阵关系，即机械臂末端位姿信息；B为相机坐标系相对标定板的旋转矩阵关系，即相机拍照数据计算得到的内参、外参数据，X即为求解的手眼旋转矩阵关系。

优选地，所述步骤S3包括如下步骤：

步骤S3.1：机械臂初始位置姿态为pos0，pos0至标定板的距离d由用户设置，根据d计算得到标定板初始位置pos_board，机械臂标定的矩形工作区间由用户设置，其中，pos_board为标定板的中心点；

步骤S3.2：根据pos0和d以及矩形的大小计算得到pos1、pos3、pos5、pos7的笛卡尔位置值；

步骤S3.3：pos2为pos1与pos3的中间点，即pos2＝0.5*(pos1+pos3)，得到pos2的位置值，pos4为pos3与pos5的中间点，pos6为pos5与pos7的中间点，pos8为pos7与pos1的中间点，用同样方法确定pos4,pos6和pos8的笛卡尔位置。

优选地，所述步骤S3.2中pos1的姿态计算过程如下：姿态保证相机是指向标定板的，pos1-标定板pos_board，归一化之后确定姿态的rz，rz和叉乘确定rx，pos0-标定板pos_board，根据右手准则rz叉乘rx确定ry，确定pos1的末端姿态旋转矩阵；确定pos1的位置姿态值，同样方法计算得到pos3，pos5,pos7的位置姿态值。

优选地，所述步骤S5中如果标定数据OK，则反馈给机械臂成功，机械臂接着运行至下一个拍照点，重复步骤S4和S5，当拍照的图片数据达到9张，则跳转至步骤S6，若拍照的图片数据不足9张，则继续运动采集机械臂位姿和标定板图像；如果标定数据异常，机械臂控制系统基于当前位置重新调整位姿,并运动至调整之后的位置，通过mqtt协议，控制相机拍照，将机械臂当前末端位置姿态数据发给相机模块工控机系统，等待相机反馈，直至相机反馈数据OK。

优选地，所述步骤S5还包括如下步骤：

步骤S5.1：在pos1处，重新计算拍照位姿，计算时以该点为圆心，构建一个球体模型，获取计算得到矩形区域的高度d和pos1-pos7的宽度Width，取d/2与Width/2中较小的值做为球体的最大半径r_max，即r_max＝min(d/2,Width/2)；沿着球心向外方向进行路径搜索，步长设置为r_max/4；

步骤S5.2：往搜索方向第i步，i为1,2,3,4，在该球体上计算中间的拍照点，球体半径为r_max/4*i；在球体上确定选取四个位姿平面，每个平面之间的夹角为45度，按顺序搜索平面；

步骤S5.3：平面与球体的交线确定一个圆，该圆上取四个点，pos1_0,pos1_1,pos1_2和pos1_3；

步骤S5.4：根据pos1和该球体半径r_max/4*i和当前平面计算得到pos1_0,pos1_1,pos1_2和pos1_3的位置值，各位置处的姿态保证指向标定板，每个调整平面选取4个位置点，每个球体确定16个调整之后的拍照点；

步骤S5.5：第一个球体确定的16个拍照点计算完成，机械臂会按顺序运动至各个点，重复步骤S4和步骤S5，调整步骤过程中相机返回数据OK之后，则认为在该位置pos1的位姿调整数据采集完成，则接着运行下一个拍照点pos2，接着重复步骤S4骤S5；如果第一个球体确定的16个调整之后的位置点均不适合，则继续扩大球体半径沿着半径向外搜索，将半径扩大，确定下一个球体；如果所有搜寻位置完成之后，均不符合，则再调整pos1的姿态，调整当前位置姿态与平面的夹角，基于现有值加减5度和10度，位置值计算方式不变；直至标定数据ok。

本发明还提供一种机器人自动手眼标定系统，所述系统包括如下模块：

模块M1：控制机械臂运动至初始位置点pos0，在初始位置点pos0时，机械臂末端位置位于标定板正对面，其中，pos为标定过程中机械臂需要运动至该位置的拍照点；

模块M2：根据相机至机械臂末端的距离d和机械臂运动时的位置边界点，开始标定；

模块M3：基于设置的参数，机械臂采用自动规划路径系统，自动计算并规划路径，计算得到多组拍照点，覆盖整个标定板，机械臂每运动至拍照点i，通过mqtt协议，控制相机拍照，将机械臂当前末端位置姿态数据发给相机模块的工控机系统；

模块M4：通过相机拍照判断拍照数据结果是否达到预设置的模型匹配度阈值，如果达到设置的阈值，标定数据OK，如果小于设置的阈值，则标定数据异常；

模块M5：基于拍照得到的拍照数据，计算相机的外参和内参；结合每个拍照点时机械臂的末端位姿信息，计算求解AX＝XB矩阵方程，得到手眼标定结果和标定时的重投影误差，其中，A表示机械臂末端至机械臂基座的旋转矩阵关系，即机械臂末端位姿信息；B为相机坐标系相对标定板的旋转矩阵关系，即相机拍照数据计算得到的内参、外参数据，X即为求解的手眼旋转矩阵关系。

优选地，所述模块M3包括如下模块：

模块M3.1：机械臂初始位置姿态为pos0，pos0至标定板的距离d由用户设置，根据d计算得到标定板初始位置pos_board，机械臂标定的矩形工作区间由用户设置，其中，pos_board为标定板的中心点；

模块M3.2：根据pos0和d以及矩形的大小计算得到pos1、pos3、pos5、pos7的笛卡尔位置值；

模块M3.3：pos2为pos1与pos3的中间点，即pos2＝0.5*(pos1+pos3)，得到pos2的位置值，pos4为pos3与pos5的中间点，pos6为pos5与pos7的中间点，pos8为pos7与pos1的中间点，用同样系统确定pos4,pos6和pos8的笛卡尔位置。

优选地，所述模块M3.2中pos1的姿态计算过程如下：姿态保证相机是指向标定板的，pos1-标定板pos_board，归一化之后确定姿态的rz，rz和叉乘确定rx，pos0-标定板pos_board，根据右手准则rz叉乘rx确定ry，确定pos1的末端姿态旋转矩阵；确定pos1的位置姿态值，同样系统计算得到pos3，pos5,pos7的位置姿态值。

优选地，所述模块M5中如果标定数据OK，则反馈给机械臂成功，机械臂接着运行至下一个拍照点，重复模块M4和S5，当拍照的图片数据达到9张，则跳转至模块M6，若拍照的图片数据不足9张，则继续运动采集机械臂位姿和标定板图像；如果标定数据异常，机械臂控制系统基于当前位置重新调整位姿,并运动至调整之后的位置，通过mqtt协议，控制相机拍照，将机械臂当前末端位置姿态数据发给相机模块工控机系统，等待相机反馈，直至相机反馈数据OK。

优选地，所述模块M5还包括如下模块：

模块M5.1：在pos1处，重新计算拍照位姿，计算时以该点为圆心，构建一个球体模型，获取计算得到矩形区域的高度d和pos1-pos7的宽度Width，取d/2与Width/2中较小的值做为球体的最大半径r_max，即r_max＝min(d/2,Width/2)；沿着球心向外方向进行路径搜索，步长设置为r_max/4；

模块M5.2：往搜索方向第i步，i为1,2,3,4，在该球体上计算中间的拍照点，球体半径为r_max/4*i；在球体上确定选取四个位姿平面，每个平面之间的夹角为45度，按顺序搜索平面；

模块M5.3：平面与球体的交线确定一个圆，该圆上取四个点，pos1_0,pos1_1,pos1_2和pos1_3；

模块M5.4：根据pos1和该球体半径r_max/4*i和当前平面计算得到pos1_0,pos1_1,pos1_2和pos1_3的位置值，各位置处的姿态保证指向标定板，每个调整平面选取4个位置点，每个球体确定16个调整之后的拍照点；

模块M5.5：第一个球体确定的16个拍照点计算完成，机械臂会按顺序运动至各个点，重复模块M4和模块M5，调整模块过程中相机返回数据OK之后，则认为在该位置pos1的位姿调整数据采集完成，则接着运行下一个拍照点pos2，接着重复模块M4骤S5；如果第一个球体确定的16个调整之后的位置点均不适合，则继续扩大球体半径沿着半径向外搜索，将半径扩大，确定下一个球体；如果所有搜寻位置完成之后，均不符合，则再调整pos1的姿态，调整当前位置姿态与平面的夹角，基于现有值加减5度和10度，位置值计算方式不变；直至标定数据ok。

与现有技术相比，本发明具有如下的有益效果：

1、本发明只需要人工设置标定空间参数，即可完成自动标定，免去了人工示教，提升了标定效率；

2、本发明降低了人工成本，部署会更加简单，大大提升了工作人员效率。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为本发明的模块示意图；

图2为本发明自动路径规划计算位置点示意图；

图3为本发明位姿自调整示意图；

图4为本发明位姿调整搜索平面图；

图5为本发明位姿调整平面上位置图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

参照图1，本发明提供一种机器人自动手眼标定方法与系统，涉及机械臂控制系统、视觉模块和人机交互界面。

人机交互界面模块可以设置标定工作空间参数，包括相机至机械臂末端的距离，以及机械臂运动时的姿态边界点，界面同时会显示当前机械臂的位置姿态，最终手眼标定结果显示，标定误差显示。

机械臂控制系统负责进行机械臂路径规划，速度规划以及轨迹插补，负责和伺服模块通讯，下发规划位置。

机械臂控制系统同时还负责和相机模块通讯，控制相机拍照同时采集机械臂末端位姿，获取相机计算结果。

视觉模块负责在每个采样点拍照，计算相机内参，外参，以及最后计算得到手眼标定结果和标定误差。

参照图2和图3，本发明提供一种机器人自动手眼标定方法，其基于设置的标定工作空间参数实现机械臂自动路径规划，自动计算机械臂的拍照点位姿，当拍照所得数据与模型匹配度小于设置的阈值时，机械臂控制系统会基于相机的反馈结果，在当前拍照位置重新调整姿态，再次拍照，直至拍照的匹配度达到设置的阈值大小；机器人自动手眼标定方法包括如下步骤：

步骤S1：机械臂先运动至初始位置点pos0，标定板放在机械臂末端位置pos0正对面。

步骤S2：应用层参数设置，设置相机至机械臂末端的距离d，以及机械臂运动时的位置边界点，并点击触发开始标定。

步骤S3：机械臂基于设置的参数，设置参数可以被认为是一个矩形工作空间模型:机械臂会在这个矩形工作区间模型内，采用自动规划路径方法，自动计算并规划路径，计算得到9组拍照点，覆盖整个标定板图像；步骤S3.1：机械臂初始位置姿态为pos0，pos0至标定板的距离d由用户设置，则根据d可以计算得到标定板初始位置pos_board，机械臂标定的矩形工作区间由用户设置；步骤S3.2：根据pos0和d以及矩形的大小可以计算得到pos1的笛卡尔位置值，姿态保证相机是指向标定板的，pos1-标定板pos_board，归一化之后即确定姿态的rz,然后rz和叉乘即可确定rx，pos0-标定板pos_board，然后根据右手准则rz叉乘rx可以确定ry，确定了rx，ry和rz，则确定了pos1的末端姿态旋转矩阵；根据以上方法即确定了pos1的位置姿态值，同样方法可以计算得到pos3，pos5,pos7的位置姿态值；步骤S3.3：pos2为pos1与pos3的中间点，即pos2＝0.5*(pos1+pos3)；这样即可得到了pos2的位置值，其姿态信息仍按上述描述计算得到rx,ry和rz，从而得到pos2的姿态旋转矩阵，按如上描述确定了pos2的位置姿态值，用同样方法即可确定pos4,pos6和pos8的位置姿态，根据计算得到的笛卡尔位置数据pos1和pos_board，pos1-pos_board该向量即为机械臂末端指向，该向量归一化之后，就是机械臂末端工具指向，即机械臂沿机械臂基座坐标系沿Z轴的旋转角，该旋转角即为rz；再根据pos1-pos_board确定的向量和pos0-pos_board确定的向量，两个向量叉乘即可已确定末端工具沿X方向的旋转值，即rx；再根据右手准则，rz叉乘rx即可确定末端沿Y轴的旋转ry；rx，ry和rz即为机械臂末端的姿态旋转矩阵，即确定了pos1的姿态值；上面两步即确定了pos1的位置姿态值；用同样方法计算其他位置点的位置姿态。

步骤S4：机械臂运动至拍照点i；通过mqtt协议，控制相机拍照，同时将机械臂当前末端位置姿态数据发给相机模块的工控机系统；同时相机会自动曝光，以保证标定板能被识别。

步骤S5：相机拍照判断拍照数据结果是否达到预设置的模型匹配度阈值，如果达到设置的阈值，标定数据OK，如果小于设置的阈值，则标定数据异常。如果标定数据OK，则反馈给机械臂成功；机械臂接着运行至下一个拍照点，重复步骤S4和S5。当拍照的图片数据达到9张，则跳转至步骤S6，若拍照的图片数据不足9张，则继续运动采集机械臂位姿和标定板图像。如果标定数据异常，机械臂控制系统基于当前位置重新调整位姿,并运动至调整之后的位置；然后通过mqtt协议，控制相机拍照，同时将机械臂当前末端位置姿态数据发给相机模块工控机系统；重新等待相机反馈；直至相机反馈数据OK；然后重复步骤S4和S5，当拍照的图片数据达到9张，则跳转至步骤S6，若拍照的图片数据不足9张，则继续运动采集机械臂位姿和标定板图像。相机返回标定数据异常之后，机械臂控制系统会自动调整当前的位置姿态，重新进行拍照，位姿调整按如下步骤重新计算拍照位置，步骤S5.1：在pos1处，重新计算拍照位姿，计算时以该点为圆心，构建一个球体模型，其中球体的半径确定如下：获取计算得到矩形区域的高度d，以及pos1-pos7的宽度Width，取d/2与Width/2中较小的值做为球体的最大半径r_max，即r_max＝min(d/2,Width/2)；沿着球心向外方向进行路径搜索，步长设置为r_max/4；步骤S5.2：往搜索方向第i步，i为1,2,3,4，在该球体上计算中间的拍照点，此时球体半径为r_max/4*i。在球体上再按如图4所示，确定选取位姿平面，共4个平面，平面1，平面2，平面3和平面4，每个平面之间的夹角为45度，搜索顺序按平面1，平面2，平面3和平面4的先后顺序；步骤S5.3：平面与球体的交线会确定一个圆，该圆上取四个点，如图5所示，pos1_0,pos1_1,pos1_2和pos1_3；步骤S5.4：根据pos1和该球体半径r_max/4*i和当前平面可以计算得到pos1_0,pos1_1,pos1_2和pos1_3的位置值，各位置处的姿态保证指向标定板，其姿态计算方法如步骤S3所述，这样每个调整平面选取4个位置点，每个球体即可确定16个调整之后的拍照点。步骤S5.5：第一个球体确定的16个拍照点计算完成，机械臂会按顺序运动至各个点，重复步骤S4和步骤S5，调整步骤过程中相机返回数据OK之后，则认为在该位置pos1的位姿调整数据采集完成，则接着运行下一个拍照点pos2；接着重复步骤S4骤S5；如果第一个球体确定的16个调整之后的位置点均不适合，则继续扩大球体半径沿着半径向外搜索，将半径扩大，确定了下一个球体，接着按上述步骤计算得到新的调整位置点；如果所有搜寻位置完成之后，均不符合；则再调整pos1的姿态，调整当前位置姿态与平面的夹角，基于现有值加减5度和10度，重新按上面描述计算姿态，位置值计算方式不变；直至标定数据ok。

步骤S6：相机基于拍照得到的相机数据，计算相机的外参和内参；同时结合每个拍照点时机械臂的末端位姿信息，计算求解AX＝XB矩阵方程，得到手眼标定结果和标定时的重投影误差。

按如上步骤，即可实现相机的自动手眼标定计算。

本发明还提供一种机器人自动手眼标定系统，所述系统包括如下模块：

模块M1：控制机械臂运动至初始位置点pos0，在初始位置点pos0时，机械臂末端位置位于标定板正对面，其中，pos为标定过程中机械臂需要运动至该位置的拍照点；

模块M2：根据相机至机械臂末端的距离d和机械臂运动时的位置边界点，开始标定；

模块M3：基于设置的参数，机械臂采用自动规划路径系统，自动计算并规划路径，计算得到多组拍照点，覆盖整个标定板，机械臂每运动至拍照点i，通过mqtt协议，控制相机拍照，将机械臂当前末端位置姿态数据发给相机模块的工控机系统；模块M3.1：机械臂初始位置姿态为pos0，pos0至标定板的距离d由用户设置，根据d计算得到标定板初始位置pos_board，机械臂标定的矩形工作区间由用户设置，其中，pos_board为标定板的中心点；模块M3.2：根据pos0和d以及矩形的大小计算得到pos1、pos3、pos5、pos7的笛卡尔位置值；姿态保证相机是指向标定板的，pos1-标定板pos_board，归一化之后确定姿态的rz，rz和叉乘确定rx，pos0-标定板pos_board，根据右手准则rz叉乘rx确定ry，确定pos1的末端姿态旋转矩阵；确定pos1的位置姿态值，同样系统计算得到pos3，pos5,pos7的位置姿态值；模块M3.3：pos2为pos1与pos3的中间点，即pos2＝0.5*(pos1+pos3)，得到pos2的位置值，pos4为pos3与pos5的中间点，pos6为pos5与pos7的中间点，pos8为pos7与pos1的中间点，用同样系统确定pos4,pos6和pos8的笛卡尔位置。

模块M4：通过相机拍照判断拍照数据结果是否达到预设置的模型匹配度阈值，如果达到设置的阈值，标定数据OK，如果小于设置的阈值，则标定数据异常；

模块M5：基于拍照得到的拍照数据，计算相机的外参和内参；结合每个拍照点时机械臂的末端位姿信息，计算求解AX＝XB矩阵方程，得到手眼标定结果和标定时的重投影误差，其中，A表示机械臂末端至机械臂基座的旋转矩阵关系，即机械臂末端位姿信息；B为相机坐标系相对标定板的旋转矩阵关系，即相机拍照数据计算得到的内参、外参数据，X即为求解的手眼旋转矩阵关系。如果标定数据OK，则反馈给机械臂成功，机械臂接着运行至下一个拍照点，重复模块M4和S5，当拍照的图片数据达到9张，则跳转至模块M6，若拍照的图片数据不足9张，则继续运动采集机械臂位姿和标定板图像；如果标定数据异常，机械臂控制系统基于当前位置重新调整位姿,并运动至调整之后的位置，通过mqtt协议，控制相机拍照，将机械臂当前末端位置姿态数据发给相机模块工控机系统，等待相机反馈，直至相机反馈数据OK。模块M5.1：在pos1处，重新计算拍照位姿，计算时以该点为圆心，构建一个球体模型，获取计算得到矩形区域的高度d和pos1-pos7的宽度Width，取d/2与Width/2中较小的值做为球体的最大半径r_max，即r_max＝min(d/2,Width/2)；沿着球心向外方向进行路径搜索，步长设置为r_max/4；模块M5.2：往搜索方向第i步，i为1,2,3,4，在该球体上计算中间的拍照点，球体半径为r_max/4*i；在球体上确定选取四个位姿平面，每个平面之间的夹角为45度，按顺序搜索平面；模块M5.3：平面与球体的交线确定一个圆，该圆上取四个点，pos1_0,pos1_1,pos1_2和pos1_3；模块M5.4：根据pos1和该球体半径r_max/4*i和当前平面计算得到pos1_0,pos1_1,pos1_2和pos1_3的位置值，各位置处的姿态保证指向标定板，每个调整平面选取4个位置点，每个球体确定16个调整之后的拍照点；模块M5.5：第一个球体确定的16个拍照点计算完成，机械臂会按顺序运动至各个点，重复模块M4和模块M5，调整模块过程中相机返回数据OK之后，则认为在该位置pos1的位姿调整数据采集完成，则接着运行下一个拍照点pos2，接着重复模块M4骤S5；如果第一个球体确定的16个调整之后的位置点均不适合，则继续扩大球体半径沿着半径向外搜索，将半径扩大，确定下一个球体；如果所有搜寻位置完成之后，均不符合，则再调整pos1的姿态，调整当前位置姿态与平面的夹角，基于现有值加减5度和10度，位置值计算方式不变；直至标定数据ok。

本发明通过采用MQTT协议，将应用层、机械臂控制系统和视觉系统这三个独立的模块融合在一起，有效控制机械臂完成标定时自动路径规划，以及控制相机拍照；机器人标定系统具有智能化，只需要设置标定工作空间，就可以自动完成标定路径生成，相机拍照，机械臂数据采集。路径运动完成之后，自动计算手眼标定参数。

本发明只需要人工设置标定空间参数，即可完成自动标定，免去了人工示教，提升了标定效率；降低了人工成本，部署会更加简单，大大提升了工作人员效率。

本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统及其各个装置、模块、单元以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统及其各个装置、模块、单元以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同功能。所以，本发明提供的系统及其各项装置、模块、单元可以被认为是一种硬件部件，而对其内包括的用于实现各种功能的装置、模块、单元也可以视为硬件部件内的结构；也可以将用于实现各种功能的装置、模块、单元视为既可以是实现方法的软件模块又可以是硬件部件内的结构。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (10)

1.一种机器人自动手眼标定方法，其特征在于，所述方法包括如下步骤：

步骤S1：控制机械臂运动至初始位置点pos0，在初始位置点pos0时，机械臂末端位置位于标定板正对面，其中，pos为标定过程中机械臂需要运动至该位置的拍照点；

步骤S2：根据相机至机械臂末端的距离d和机械臂运动时的位置边界点，开始标定；

步骤S3：基于设置的参数，机械臂采用自动规划路径方法，自动计算并规划路径，计算得到多组拍照点，覆盖整个标定板，机械臂每运动至拍照点i，通过mqtt协议，控制相机拍照，将机械臂当前末端位置姿态数据发给相机模块的工控机系统；

步骤S4：通过相机拍照判断拍照数据结果是否达到预设置的模型匹配度阈值，如果达到设置的阈值，标定数据OK，如果小于设置的阈值，则标定数据异常；

步骤S5：基于拍照得到的拍照数据，计算相机的外参和内参；结合每个拍照点时机械臂的末端位姿信息，计算求解AX＝XB矩阵方程，得到手眼标定结果和标定时的重投影误差，其中，A表示机械臂末端至机械臂基座的旋转矩阵关系，即机械臂末端位姿信息；B为相机坐标系相对标定板的旋转矩阵关系，即相机拍照数据计算得到的内参、外参数据，X即为求解的手眼旋转矩阵关系。

2.根据权利要求1所述的机器人自动手眼标定方法，其特征在于，所述步骤S3包括如下步骤：

步骤S3.1：机械臂初始位置姿态为pos0，pos0至标定板的距离d由用户设置，根据d计算得到标定板初始位置pos_board，机械臂标定的矩形工作区间由用户设置，其中，pos_board为标定板的中心点；

步骤S3.2：根据pos0和d以及矩形的大小计算得到pos1、pos3、pos5、pos7的笛卡尔位置值；

步骤S3.3：pos2为pos1与pos3的中间点，即pos2＝0.5*(pos1+pos3)，得到pos2的位置值，pos4为pos3与pos5的中间点，pos6为pos5与pos7的中间点，pos8为pos7与pos1的中间点，用同样方法确定pos4,pos6和pos8的笛卡尔位置。

3.根据权利要求2所述的机器人自动手眼标定方法，其特征在于，所述步骤S3.2中pos1的姿态计算过程如下：姿态保证相机是指向标定板的，pos1-标定板pos_board，归一化之后确定姿态的rz，rz和叉乘确定rx，pos0-标定板pos_board，根据右手准则rz叉乘rx确定ry，确定pos1的末端姿态旋转矩阵；确定pos1的位置姿态值，同样方法计算得到pos3，pos5,pos7的位置姿态值。

4.根据权利要求1所述的机器人自动手眼标定方法，其特征在于，所述步骤S5中如果标定数据OK，则反馈给机械臂成功，机械臂接着运行至下一个拍照点，重复步骤S4和S5，当拍照的图片数据达到9张，则跳转至步骤S6，若拍照的图片数据不足9张，则继续运动采集机械臂位姿和标定板图像；如果标定数据异常，机械臂控制系统基于当前位置重新调整位姿,并运动至调整之后的位置，通过mqtt协议，控制相机拍照，将机械臂当前末端位置姿态数据发给相机模块工控机系统，等待相机反馈，直至相机反馈数据OK。

5.根据权利要求4所述的机器人自动手眼标定方法，其特征在于，所述步骤S5还包括如下步骤：

步骤S5.1：在pos1处，重新计算拍照位姿，计算时以该点为圆心，构建一个球体模型，获取计算得到矩形区域的高度d和pos1-pos7的宽度Width，取d/2与Width/2中较小的值做为球体的最大半径r_max，即r_max＝min(d/2,Width/2)；沿着球心向外方向进行路径搜索，步长设置为r_max/4；

步骤S5.2：往搜索方向第i步，i为1,2,3,4，在该球体上计算中间的拍照点，球体半径为r_max/4*i；在球体上确定选取四个位姿平面，每个平面之间的夹角为45度，按顺序搜索平面；

步骤S5.3：平面与球体的交线确定一个圆，该圆上取四个点，pos1_0,pos1_1,pos1_2和pos1_3；

步骤S5.4：根据pos1和该球体半径r_max/4*i和当前平面计算得到pos1_0,pos1_1,pos1_2和pos1_3的位置值，各位置处的姿态保证指向标定板，每个调整平面选取4个位置点，每个球体确定16个调整之后的拍照点；

步骤S5.5：第一个球体确定的16个拍照点计算完成，机械臂会按顺序运动至各个点，重复步骤S4和步骤S5，调整步骤过程中相机返回数据OK之后，则认为在该位置pos1的位姿调整数据采集完成，则接着运行下一个拍照点pos2，接着重复步骤S4骤S5；如果第一个球体确定的16个调整之后的位置点均不适合，则继续扩大球体半径沿着半径向外搜索，将半径扩大，确定下一个球体；如果所有搜寻位置完成之后，均不符合，则再调整pos1的姿态，调整当前位置姿态与平面的夹角，基于现有值加减5度和10度，位置值计算方式不变；直至标定数据ok。

6.一种机器人自动手眼标定系统，其特征在于，所述系统包括如下模块：

模块M1：控制机械臂运动至初始位置点pos0，在初始位置点pos0时，机械臂末端位置位于标定板正对面，其中，pos为标定过程中机械臂需要运动至该位置的拍照点；

模块M2：根据相机至机械臂末端的距离d和机械臂运动时的位置边界点，开始标定；

模块M3：基于设置的参数，机械臂采用自动规划路径系统，自动计算并规划路径，计算得到多组拍照点，覆盖整个标定板，机械臂每运动至拍照点i，通过mqtt协议，控制相机拍照，将机械臂当前末端位置姿态数据发给相机模块的工控机系统；

模块M4：通过相机拍照判断拍照数据结果是否达到预设置的模型匹配度阈值，如果达到设置的阈值，标定数据OK，如果小于设置的阈值，则标定数据异常；

模块M5：基于拍照得到的拍照数据，计算相机的外参和内参；结合每个拍照点时机械臂的末端位姿信息，计算求解AX＝XB矩阵方程，得到手眼标定结果和标定时的重投影误差，其中，A表示机械臂末端至机械臂基座的旋转矩阵关系，即机械臂末端位姿信息；B为相机坐标系相对标定板的旋转矩阵关系，即相机拍照数据计算得到的内参、外参数据，X即为求解的手眼旋转矩阵关系。

7.根据权利要求6所述的机器人自动手眼标定系统，其特征在于，所述模块M3包括如下模块：

模块M3.1：机械臂初始位置姿态为pos0，pos0至标定板的距离d由用户设置，根据d计算得到标定板初始位置pos_board，机械臂标定的矩形工作区间由用户设置，其中，pos_board为标定板的中心点；

模块M3.2：根据pos0和d以及矩形的大小计算得到pos1、pos3、pos5、pos7的笛卡尔位置值；

模块M3.3：pos2为pos1与pos3的中间点，即pos2＝0.5*(pos1+pos3)，得到pos2的位置值，pos4为pos3与pos5的中间点，pos6为pos5与pos7的中间点，pos8为pos7与pos1的中间点，用同样系统确定pos4,pos6和pos8的笛卡尔位置。

8.根据权利要求7所述的机器人自动手眼标定系统，其特征在于，所述模块M3.2中pos1的姿态计算过程如下：姿态保证相机是指向标定板的，pos1-标定板pos_board，归一化之后确定姿态的rz，rz和叉乘确定rx，pos0-标定板pos_board，根据右手准则rz叉乘rx确定ry，确定pos1的末端姿态旋转矩阵；确定pos1的位置姿态值，同样系统计算得到pos3，pos5,pos7的位置姿态值。

9.根据权利要求6所述的机器人自动手眼标定系统，其特征在于，所述模块M5中如果标定数据OK，则反馈给机械臂成功，机械臂接着运行至下一个拍照点，重复模块M4和S5，当拍照的图片数据达到9张，则跳转至模块M6，若拍照的图片数据不足9张，则继续运动采集机械臂位姿和标定板图像；如果标定数据异常，机械臂控制系统基于当前位置重新调整位姿,并运动至调整之后的位置，通过mqtt协议，控制相机拍照，将机械臂当前末端位置姿态数据发给相机模块工控机系统，等待相机反馈，直至相机反馈数据OK。

10.根据权利要求9所述的机器人自动手眼标定系统，其特征在于，所述模块M5还包括如下模块：

模块M5.1：在pos1处，重新计算拍照位姿，计算时以该点为圆心，构建一个球体模型，获取计算得到矩形区域的高度d和pos1-pos7的宽度Width，取d/2与Width/2中较小的值做为球体的最大半径r_max，即r_max＝min(d/2,Width/2)；沿着球心向外方向进行路径搜索，步长设置为r_max/4；

模块M5.2：往搜索方向第i步，i为1,2,3,4，在该球体上计算中间的拍照点，球体半径为r_max/4*i；在球体上确定选取四个位姿平面，每个平面之间的夹角为45度，按顺序搜索平面；

模块M5.3：平面与球体的交线确定一个圆，该圆上取四个点，pos1_0,pos1_1,pos1_2和pos1_3；

模块M5.4：根据pos1和该球体半径r_max/4*i和当前平面计算得到pos1_0,pos1_1,pos1_2和pos1_3的位置值，各位置处的姿态保证指向标定板，每个调整平面选取4个位置点，每个球体确定16个调整之后的拍照点；

模块M5.5：第一个球体确定的16个拍照点计算完成，机械臂会按顺序运动至各个点，重复模块M4和模块M5，调整模块过程中相机返回数据OK之后，则认为在该位置pos1的位姿调整数据采集完成，则接着运行下一个拍照点pos2，接着重复模块M4骤S5；如果第一个球体确定的16个调整之后的位置点均不适合，则继续扩大球体半径沿着半径向外搜索，将半径扩大，确定下一个球体；如果所有搜寻位置完成之后，均不符合，则再调整pos1的姿态，调整当前位置姿态与平面的夹角，基于现有值加减5度和10度，位置值计算方式不变；直至标定数据ok。

Images