CN107590835B - 一种核环境下机械臂工具快换视觉定位系统与定位方法 - Google Patents

Abstract

本发明公开了一种核环境下机械臂工具快换视觉定位系统与定位方法，涉及特种装备技术领域，包括依次通信相连的图像采集单元、靶标特征提取单元、靶标位姿计算模块、机械臂姿态计算模块和机械臂控制系统，且靶标特征提取单元还与相机标定模块通信相连，靶标位姿计算模块还与手眼标定模块相连，解决在放射性环境下，工具换装过程中机械臂末端工具夹持头如何快速、准确定位工具搁架上待夹持工具的难题，提高工具换装的效率，实现机械手的多功能的用途，降低“核三废”处理的成本，提高“核三废”处理的工作效率。

Description

一种核环境下机械臂工具快换视觉定位系统与定位方法

技术领域

本发明涉及特种装备技术领域，特别涉及一种核环境下机械臂工具快换视觉定位系统与定位方法。

背景技术

多关节机械臂是在机械与自动化领域耀眼的结晶，在工业制造、医学治疗、娱乐服务、军事、半导体制造以及太空探索等领域都能见到它的身影。通常，在自动化生产线上，如汽车的装配流水线上，每一台机械臂在固定工位间按照固定的工序循环进行作业，一旦其工作位置姿态被示教完成，一般就不再需要进行部件更换、工位改变等操作，因此这类机械臂的研制已经相当成熟，使用也都相对简单。

然而，随着我国核电的不断发展，在原子能、核退役与放射性废物处理领域进行作业的机器人、机械臂亟待上线，与一般自动化线上的机械臂不同的是，特种机械臂有如下显著的特征：

1.机械部件、电气连接件、通信设备均需要具备相应的耐辐照性能，关键部件需要良好的可靠性和较长的寿命；

2.工作范围较大，需要将机械臂底座置于行车移动平台上以获取更大的行程；

3.工作姿态不确定，需要采用图像监控设备配合远程遥操作；

4.作业性质不确定，包括对放射性废物进行夹取、搬运、切割等处理作业，以及对核设施(如高放玻璃固化线)进行零件更换等维护作业，机械臂固定单一的末端工具不能满足多功能的需求。

现有的方案是在机械臂末端安装统一的工具夹持头，根据不同的工况需求，人通过视频遥操作机械臂至工具搁架进行工具换装，工具搁架上预先配备多种工具。如公开号为CN101876532A，公开时间为2010年11月3日，名称为“测量系统中的摄像机现场标定方法”，公开了一种摄像机现场标定方法，在测量系统中有两个摄像机和一个投影仪，标定方法的步骤如下：制作摄像机内、外参数标定靶标；投影内参数靶标，并拍摄图像；通过Matlab中的图像处理算法提取图像特征点；列方程求解摄像机内参数；处理左右摄像机同时拍摄的图像；用左、右经纬仪测得靶标圆心实际距离，求比例因子，进而求得实际的外参数。本发明现场适应性较强，采用投影仪投射靶标，克服了双目测量大型锻件系统中滤光片过滤红外光带来的成像不透清晰的不良影响；适用于大场景，背景复杂的场合使用。目前工具夹持点的对准完全通过人肉眼观察视频上的工具搁架上的标志点(如十字靶标)，缓慢移动机械臂，使工具夹持头趋近待夹持工具。整个过程需要富有经验的工人进行操作，换装效率低且容易出现工具撞伤等安全事故。因此需要有效率更高更安全的工具换装方法，以适应现有放射性环境下机械臂对废物的处理作业。

发明内容

本发明的目的在于解决在放射性环境下，工具换装过程中机械臂末端工具夹持头如何快速、准确定位工具搁架上待夹持工具的难题，提高工具换装的效率，实现机械手的多功能的用途，降低“核三废”处理的成本，提高“核三废”处理的工作效率。

本发明的目的是通过以下技术方案实现的：

一种核环境下机械臂工具快换视觉定位系统，其特征在于：包括依次通信相连的图像采集单元、靶标特征提取单元、靶标位姿计算模块、机械臂姿态计算模块和机械臂控制系统，且靶标特征提取单元还与相机标定模块通信相连，靶标位姿计算模块还与手眼标定模块相连，相机标定模块通过其相机内参矩阵对靶标特征提取单元所提取的特征数据进行处理并将处理结果发送至手眼标定模块，手眼标定模块结合相机内参矩阵的处理结果和靶标位姿计算模块的计算结果进行数据处理并将处理结果传送至机械臂姿态计算模块。

所述图像采集单元包括面阵工业相机和照明光源，安装于机械臂末端用于对靶标成像。

一种核环境下机械臂工具快换视觉定位系统的定位方法，其特征在于，包括以下步骤：

相机标定步骤，调节相机的焦距及光圈至成像清晰，从不同的角度、高度拍摄一套靶标图像，使靶标成像覆盖靶标图像图幅的50％至75％，将一套靶标图像随机分为两组，使用两组靶标图像采用“张正友标定法”计算相机的内参数及畸变系数并交替重投影得到重投影误差，保留误差较小的组计算出的相机参数；

机械臂手眼标定步骤，将经过标定的相机安装在机械臂末端支架上，移动机械臂至靶标上方，并使靶标在图像中成像完整且清晰，检测图像中靶标特征的像素坐标，提取靶标上标志物的亚像素坐标，计算靶标的空间位姿；使用相机标定得到的内参矩阵及畸变向量计算当前位置相机坐标系与靶标坐标系间的变换矩阵H_cam；向机械臂控制系统发送位置反馈请求，保存当前机械臂末端在基坐标系下的位置数据，并变换矩阵H_robot；根据变换矩阵H_cam及H_robot，采用“Tsai-Lenz方法”计算机械臂末端工具夹持头与相机间的相对位置得到手眼关系矩阵H_eye；

工具搁架平台标定步骤，建立工具坐标系、机械臂基座位置坐标系、靶标坐标系，将靶标固定在工具搁架平台上，保证靶标平面与待夹持工具的快换盘表面平行，工具搁架平台上的固定位置顺次摆放所有待夹持工具，依次编号；移动机械臂至每一个工具快换盘上方，调整机械臂姿对准快换盘导向柱，待导向柱完全插入工具夹持头后，记录下机械臂末端工具夹持头的位置信息；移动机械臂，使其工具中心点对准靶标原点，并调整工具坐标系的Z轴角度使工具坐标系X轴、Y轴与靶标坐标系的行列平行，记录靶标在机械臂基座位置坐标系下的位置数据根据每一个工具快换盘的位置及靶标原点的位置，计算工具快换盘相对于靶标的位置及角度，并将位置信息与工具编号一一对应，建立工具位置数据文件；

目标工具定位步骤，根据相机标定、机械臂手眼标定和工具搁架平台标定的结果定位目标工具位置完成换装。

所述相机标定步骤中，得到重投影误差的具体方法为

步骤一，将一套靶标图像随机分为两组，记为组1和组2，分别对组1和组2的靶标图像采用“张正友标定法”计算相机的内参数及畸变系数，提取出的靶标上所有特征圆的圆心像素坐标，得到内参矩阵

及畸变向量[k1,k2,p1,p2,k3,k4,k5,k6]

其中，焦距f、像素尺度dx、dy、成像中心偏置量Cx、Cy；

步骤二，将组2靶标图像上的特征圆的圆心像素坐标从世界坐标系重新投影到像素空间，与用组1靶标图像提取出的靶标特征像素坐标作差，并求平均，得到平均重投影误差Project_err1；

步骤三，将组1靶标图像上的特征点从世界坐标系重新投影到像素空间，与用组2靶标图像计算出的靶标特征像素坐标作差，并求平均，得到平均重投影误差Project_err2；

步骤四，重复上述步骤一、二和三至少5次，选取重投影误差最小的那组相机参数作为相机标定的最终结果。

所述机械臂手眼标定步骤中，变换矩阵H_robot的构造方法为向机械臂控制系统发送位置反馈请求，保存当前机械臂末端在机械臂基座位置坐标系下的位置数据[X Y Z AB C]，构造4X4变换矩阵H_robot：

所述机械臂手眼标定步骤中，得到手眼关系矩阵H_eye的方法为

步骤一，重复计算得到两组变换矩阵H_cam及H_robot，根据两组变换矩阵H_cam及H_robot，采用“Tsai-Lenz方法”计算机械臂末端工具夹持头与相机间的相对位置，得到为4X4的手眼关系矩阵H_eye和H_eye_new；

步骤二，比较H_eye矩阵与H_eye_new矩阵中平移量X、Y、Z的相对差值，取最大的为参考，与预设的相对差值的最大阈值max_Thres及最小阈值min_Thres进行比较；

若大于预设阈值max_Thres则舍弃新测得的数据，然后重复上述步骤一；

若介于min_Thres＝1％与max_Thres之间则令H_eye等于H_eye_new，然后重复上述步骤一；

若小于预设阈值min_Thres，则以H_eye为最终手眼标定计算结果，结束手眼标定。

所述机械臂手眼标定步骤中，计算靶标的空间位姿的具体方法为

步骤一，移动机械臂至靶标上方，使靶标图像成像完整且清晰，将采集到的靶标图像转为8位灰度图，并使用双边滤波过滤噪声，检测图像灰度分布，统计各个灰度值的频次，获取前景与背景灰度峰值，设定为二值化阈值区间；

步骤二，对二值化阈值区间取步长为5～15，对图像进行二值化得到二值图像集{Img0,Img1……ImgN}，对每张二值图像进行连通域检测，并计算各连通域的圆度，排除背景中非靶标特征圆的干扰斑点，并采用图像灰度矩计算公式计算符合面积、圆度要求的连通域的中心坐标；

步骤三，对所有二值图像均检测并计算出的形心位置进行聚类分析，间距dis小于0.1个像素的形心序列认为是同一个靶标圆，聚类后的形心坐标序列，采用最小外接圆法外包同一个靶标圆计算出的所有形心坐标，并令最小外接圆的圆心坐标作为该靶标圆的最终形心坐标；

步骤四，以靶标图像中心四个大圆为基准，建立行列直线簇，搜索所有在行列直线簇上的特征圆，并以左上角为原点，靶标行为Y轴，靶标列为X轴，进行排列，获取靶标在像素坐标系下的所有特征圆的亚像素坐标。

所述图像灰度矩计算公式：

其中f(i,j)是一幅灰度图像，i、j为矩的阶数，x、y为图像像素坐标

其中零阶矩表示为：

其中D(i,j)为连通域所在灰度图像。

一阶矩表示为：

那么图像的重心即形心(xc,yc)则为：

所述机械臂手眼标定步骤中，计算变换矩阵H_cam的具体方法为

步骤一，设置靶标的物理坐标系，以靶标左上角为原点，行方向为X轴、列方向为Y轴，计算所有特征圆中心的物理坐标，并按照与像素坐标系下相同的排列方式，进行排序；

步骤二，从靶标图像的左上角开始，选择相邻的4个点，以每个点的物理坐标(XwYw Zw)及像素坐标(u v)共5个参数作为姿态计算的输入，根据所有特征圆的物理坐标及对应像素坐标，建立姿态求解方程

将每个点的物理坐标(Xw Yw Zw)及像素坐标(u v)带入方程，计算得到矩阵

中所有坐标转换参数，记为H_obj

采用非线性最小二乘优化算法Levenberg–Marquardt算法优化H_obj，使特征点的重投影误差最小化，得到最终的H_cam。

所述目标工具定位步骤，定位目标工具位置的具体方法为

根据机械臂手眼标定步骤的标定结果，相机相对于工具坐标系的转换矩阵记为H_eye，则靶标坐标系相对工具坐标系的转换矩阵可表示为H_eye×H_obj；

从机械臂控制系统获取当前工具坐标系在基坐标系下的位置，包括平移量X、Y、，欧拉角A、B、C，并构造4X4的坐标变换矩阵H_base：

计算靶标在基坐标系下坐标转换矩阵H_obj_base＝H_base×H_eye×H_obj；

选择需要换装的工具，检索工具位置文件，获取待换装工具在靶标坐标系中的位置POS＝[X Y Z RotX RotY RotZ]，并构造成4X4变换矩阵H_pos

将靶标坐标系下的待换工具位置转换到机械臂基坐标系下，通过H_tool＝H_obj_base×H_pos转换得到待换工具坐标系到基坐标系的变换矩阵H_tool；

从H_tool中解析出待换工具在基坐标系中位置及角度，得到POS_inbase＝[X Y ZA B C]；

发送位置指令数据[X Y Z-100 A B C]给机械臂控制系统，使机械臂快速移动至待换工具正上方，并保证不会与快换盘导向柱相撞；

发送位置指令数据[X Y Z A B C]给机械臂控制系统，使机械臂工具夹头准确插入快换盘导向柱，完成工具快换定位。

本发明的有益效果如下：

一、本发明提供的一种核环境下机械臂工具快换视觉定位系统，通过靶标特征提取单元对图像采集单元所采集的图像进行特征提取并把提取结果作为分析用的数据传递至靶标位姿计算模块、机械臂姿态计算模块进行处理和计算配合手动操作完成手眼标定后即可形成完整的自动视觉快换操作指令用于机械臂控制系统实现对机械臂工具快换视觉定位，机械臂自动并准确的找到目标工具完成换装。

二、本发明提供的一种核环境下机械臂工具快换视觉定位方法，通过相机标定、机械臂手眼标定和工具搁架平台标定的过程，采用变换矩阵结合张氏标定、Tsai-Lenz方法等算法，完成工具快换的指令搭建，结合使用当时的各部件位置状态实时准确的控制机械臂到达对应的目标工具位置完成工具换装；定位方式采用了基于视觉的主动定位，定位过程中只需要拍摄一张靶标的图像，即可计算出待换装的工具相对于机械臂工具夹头的位置及角度，然后快速驱动机械臂对准工具，进行换装，不再需要人为进行试凑调整定位，定位效率高；定位精度可达±1mm，完全满足定位要求，采用圆形阵列靶标及对应的特征提取算法取代通用的棋盘格靶标及其角点提取算法，使得在辐射高噪声的成像环境中仍然可以精确的提取特征位置，相同环境下，圆形阵列靶标及对应特征提取算法的重投影误差较棋盘格小一个数量级。

附图说明

图1是本发明一种核环境下机械臂工具快换视觉定位系统的结构示意图；

图2是本发明靶标特征提取算法流程；

图3是本发明相机标定流程图；

图4是本发明手眼标定流程图；

图5是本发明工具快换定位流程图；

图6是本发明靶标图像的示意图；

具体实施方式

以下通过几个具体实施例来进一步说明实现本发明目的的技术方案，需要说明的是，本发明的技术方案包含但不限于以下实施例。

实施例1

一种核环境下机械臂工具快换视觉定位系统，包括依次通信相连的图像采集单元、靶标特征提取单元、靶标位姿计算模块、机械臂姿态计算模块和机械臂控制系统，且靶标特征提取单元还与相机标定模块通信相连，靶标位姿计算模块还与手眼标定模块相连，相机标定模块通过其相机内参矩阵对靶标特征提取单元所提取的特征数据进行处理并将处理结果发送至手眼标定模块，手眼标定模块结合相机内参矩阵的处理结果和靶标位姿计算模块的计算结果进行数据处理并将处理结果传送至机械臂姿态计算模块。

这是本发明一种核环境下机械臂工具快换视觉定位系统的最基本实施方案；通过靶标特征提取单元对图像采集单元所采集的图像进行特征提取并把提取结果作为分析用的数据传递至靶标位姿计算模块、机械臂姿态计算模块进行处理和计算配合手动操作完成手眼标定后即可形成完整的自动视觉快换操作指令用于机械臂控制系统实现对机械臂工具快换视觉定位，机械臂自动并准确的找到目标工具完成换装。

实施例2

一种核环境下机械臂工具快换视觉定位系统，包括依次通信相连的图像采集单元、靶标特征提取单元、靶标位姿计算模块、机械臂姿态计算模块和机械臂控制系统，且靶标特征提取单元还与相机标定模块通信相连，靶标位姿计算模块还与手眼标定模块相连，相机标定模块通过其相机内参矩阵对靶标特征提取单元所提取的特征数据进行处理并将处理结果发送至手眼标定模块，手眼标定模块结合相机内参矩阵的处理结果和靶标位姿计算模块的计算结果进行数据处理并将处理结果传送至机械臂姿态计算模块；所述图像采集单元包括面阵工业相机和照明光源，安装于机械臂末端用于对靶标成像。

这是本发明一种核环境下机械臂工具快换视觉定位系统的优选的实施方案；通过靶标特征提取单元对图像采集单元所采集的图像进行特征提取并把提取结果作为分析用的数据传递至靶标位姿计算模块、机械臂姿态计算模块进行处理和计算配合手动操作完成手眼标定后即可形成完整的自动视觉快换操作指令用于机械臂控制系统实现对机械臂工具快换视觉定位，机械臂自动并准确的找到目标工具完成换装。

实施例3

如图1至图6一种核环境下机械臂工具快换视觉定位系统的定位方法，包括以下步骤：

相机标定步骤，调节相机的焦距及光圈至成像清晰，从不同的角度、高度拍摄一套靶标图像，使靶标成像覆盖靶标图像图幅的50％至75％，将一套靶标图像随机分为两组，使用两组靶标图像采用“张正友标定法”计算相机的内参数及畸变系数并交替重投影得到重投影误差，保留误差较小的组计算出的相机参数；

机械臂手眼标定步骤，将经过标定的相机安装在机械臂末端支架上，移动机械臂至靶标上方，并使靶标在图像中成像完整且清晰，检测图像中靶标特征的像素坐标，提取靶标上标志物的亚像素坐标，计算靶标的空间位姿；使用相机标定得到的内参矩阵及畸变向量计算当前位置相机坐标系与靶标坐标系间的变换矩阵H_cam；向机械臂控制系统发送位置反馈请求，保存当前机械臂末端在基坐标系下的位置数据，并变换矩阵H_robot；根据变换矩阵H_cam及H_robot，采用“Tsai-Lenz方法”计算机械臂末端工具夹持头与相机间的相对位置得到手眼关系矩阵H_eye；

工具搁架平台标定步骤，建立工具坐标系、机械臂基座位置坐标系、靶标坐标系，将靶标固定在工具搁架平台上，保证靶标平面与待夹持工具的快换盘表面平行，工具搁架平台上的固定位置顺次摆放所有待夹持工具，依次编号；移动机械臂至每一个工具快换盘上方，调整机械臂姿对准快换盘导向柱，待导向柱完全插入工具夹持头后，记录下机械臂末端工具夹持头的位置信息；移动机械臂，使其工具中心点对准靶标原点，并调整工具坐标系的Z轴角度使工具坐标系X轴、Y轴与靶标坐标系的行列平行，记录靶标在机械臂基座位置坐标系下的位置数据根据每一个工具快换盘的位置及靶标原点的位置，计算工具快换盘相对于靶标的位置及角度，并将位置信息与工具编号一一对应，建立工具位置数据文件；

目标工具定位步骤，根据相机标定、机械臂手眼标定和工具搁架平台标定的结果定位目标工具位置完成换装。

这是本发明一种核环境下机械臂工具快换视觉定位系统的定位方法最基本的实施方案，通过相机标定、机械臂手眼标定和工具搁架平台标定的过程，采用变换矩阵结合张氏标定、Tsai-Lenz方法等算法，完成工具快换的指令搭建，结合使用当时的各部件位置状态实时准确的控制机械臂到达对应的目标工具位置完成工具换装；定位方式采用了基于视觉的主动定位，定位过程中只需要拍摄一张靶标的图像，即可计算出待换装的工具相对于机械臂工具夹头的位置及角度，然后快速驱动机械臂对准工具，进行换装，不再需要人为进行试凑调整定位，定位效率高；定位精度可达±1mm，完全满足定位要求，采用圆形阵列靶标及对应的特征提取算法取代通用的棋盘格靶标及其角点提取算法，使得在辐射高噪声的成像环境中仍然可以精确的提取特征位置，相同环境下，圆形阵列靶标及对应特征提取算法的重投影误差较棋盘格小一个数量级。

实施例4

如图1至图6一种核环境下机械臂工具快换视觉定位系统的定位方法，包括以下步骤：

相机标定步骤，调节相机的焦距及光圈至成像清晰，从不同的角度、高度拍摄一套靶标图像，使靶标成像覆盖靶标图像图幅的50％至75％，将一套靶标图像随机分为两组，使用两组靶标图像采用“张正友标定法”计算相机的内参数及畸变系数并交替重投影得到重投影误差，保留误差较小的组计算出的相机参数；

机械臂手眼标定步骤，将经过标定的相机安装在机械臂末端支架上，移动机械臂至靶标上方，并使靶标在图像中成像完整且清晰，检测图像中靶标特征的像素坐标，提取靶标上标志物的亚像素坐标，计算靶标的空间位姿；使用相机标定得到的内参矩阵及畸变向量计算当前位置相机坐标系与靶标坐标系间的变换矩阵H_cam；向机械臂控制系统发送位置反馈请求，保存当前机械臂末端在基坐标系下的位置数据，并变换矩阵H_robot；根据变换矩阵H_cam及H_robot，采用“Tsai-Lenz方法”计算机械臂末端工具夹持头与相机间的相对位置得到手眼关系矩阵H_eye；

工具搁架平台标定步骤，建立工具坐标系、机械臂基座位置坐标系、靶标坐标系，将靶标固定在工具搁架平台上，保证靶标平面与待夹持工具的快换盘表面平行，工具搁架平台上的固定位置顺次摆放所有待夹持工具，依次编号；移动机械臂至每一个工具快换盘上方，调整机械臂姿对准快换盘导向柱，待导向柱完全插入工具夹持头后，记录下机械臂末端工具夹持头的位置信息；移动机械臂，使其工具中心点对准靶标原点，并调整工具坐标系的Z轴角度使工具坐标系X轴、Y轴与靶标坐标系的行列平行，记录靶标在机械臂基座位置坐标系下的位置数据根据每一个工具快换盘的位置及靶标原点的位置，计算工具快换盘相对于靶标的位置及角度，并将位置信息与工具编号一一对应，建立工具位置数据文件；

目标工具定位步骤，根据相机标定、机械臂手眼标定和工具搁架平台标定的结果定位目标工具位置完成换装。

所述相机标定步骤中，得到重投影误差的具体方法为

步骤一，将一套靶标图像随机分为两组，记为组1和组2，分别对组1和组2的靶标图像采用“张正友标定法”计算相机的内参数及畸变系数，提取出的靶标上所有特征圆的圆心像素坐标，得到内参矩阵

及畸变向量[k1,k2,p1,p2,k3,k4,k5,k6]

其中，焦距f、像素尺度dx、dy、成像中心偏置量Cx、Cy；

步骤二，将组2靶标图像上的特征圆的圆心像素坐标从世界坐标系重新投影到像素空间，与用组1靶标图像提取出的靶标特征像素坐标作差，并求平均，得到平均重投影误差Project_err1；

步骤三，将组1靶标图像上的特征点从世界坐标系重新投影到像素空间，与用组2靶标图像计算出的靶标特征像素坐标作差，并求平均，得到平均重投影误差Project_err2；

步骤四，重复上述步骤一、二和三至少5次，选取重投影误差最小的那组相机参数作为相机标定的最终结果。

所述机械臂手眼标定步骤中，变换矩阵H_robot的构造方法为向机械臂控制系统发送位置反馈请求，保存当前机械臂末端在机械臂基座位置坐标系下的位置数据[X Y Z AB C]，构造4X4变换矩阵H_robot：

所述机械臂手眼标定步骤中，得到手眼关系矩阵H_eye的方法为

步骤一，重复计算得到两组变换矩阵H_cam及H_robot，根据两组变换矩阵H_cam及H_robot，采用“Tsai-Lenz方法”计算机械臂末端工具夹持头与相机间的相对位置，得到为4X4的手眼关系矩阵H_eye和H_eye_new；

步骤二，比较H_eye矩阵与H_eye_new矩阵中平移量X、Y、Z的相对差值，取最大的为参考，与预设的相对差值的最大阈值max_Thres及最小阈值min_Thres进行比较；

若大于预设阈值max_Thres则舍弃新测得的数据，然后重复上述步骤一；

若介于min_Thres＝1％与max_Thres之间则令H_eye等于H_eye_new，然后重复上述步骤一；

若小于预设阈值min_Thres，则以H_eye为最终手眼标定计算结果，结束手眼标定。

所述机械臂手眼标定步骤中，计算靶标的空间位姿的具体方法为

步骤一，移动机械臂至靶标上方，使靶标图像成像完整且清晰，将采集到的靶标图像转为8位灰度图，并使用双边滤波过滤噪声，检测图像灰度分布，统计各个灰度值的频次，获取前景与背景灰度峰值，设定为二值化阈值区间；

步骤二，对二值化阈值区间取步长为5～15，对图像进行二值化得到二值图像集{Img0,Img1……ImgN}，对每张二值图像进行连通域检测，并计算各连通域的圆度，排除背景中非靶标特征圆的干扰斑点，并采用图像灰度矩计算公式计算符合面积、圆度要求的连通域的中心坐标；

步骤三，对所有二值图像均检测并计算出的形心位置进行聚类分析，间距dis小于0.1个像素的形心序列认为是同一个靶标圆，聚类后的形心坐标序列，采用最小外接圆法外包同一个靶标圆计算出的所有形心坐标，并令最小外接圆的圆心坐标作为该靶标圆的最终形心坐标；

步骤四，以靶标图像中心四个大圆为基准，建立行列直线簇，搜索所有在行列直线簇上的特征圆，并以左上角为原点，靶标行为Y轴，靶标列为X轴，进行排列，获取靶标在像素坐标系下的所有特征圆的亚像素坐标。

所述图像灰度矩计算公式：

其中f(i,j)是一幅灰度图像，i、j为矩的阶数，x、y为图像像素坐标

其中零阶矩表示为：

其中D(i,j)为连通域所在灰度图像。

一阶矩表示为：

那么图像的重心即形心(xc,yc)则为：

所述机械臂手眼标定步骤中，计算变换矩阵H_cam的具体方法为

步骤一，设置靶标的物理坐标系，以靶标左上角为原点，行方向为X轴、列方向为Y轴，计算所有特征圆中心的物理坐标，并按照与像素坐标系下相同的排列方式，进行排序；

步骤二，从靶标图像的左上角开始，选择相邻的4个点，以每个点的物理坐标(XwYw Zw)及像素坐标(u v)共5个参数作为姿态计算的输入，根据所有特征圆的物理坐标及对应像素坐标，建立姿态求解方程

将每个点的物理坐标(Xw Yw Zw)及像素坐标(u v)带入方程，计算得到矩阵

中所有坐标转换参数，记为H_obj

采用非线性最小二乘优化算法Levenberg–Marquardt算法优化H_obj，使特征点的重投影误差最小化，得到最终的H_cam。

所述目标工具定位步骤，定位目标工具位置的具体方法为

根据机械臂手眼标定步骤的标定结果，相机相对于工具坐标系的转换矩阵记为H_eye，则靶标坐标系相对工具坐标系的转换矩阵可表示为H_eye×H_obj；

从机械臂控制系统获取当前工具坐标系在基坐标系下的位置，包括平移量X、Y、，欧拉角A、B、C，并构造4X4的坐标变换矩阵H_base：

计算靶标在基坐标系下坐标转换矩阵H_obj_base＝H_base×H_eye×H_obj；

选择需要换装的工具，检索工具位置文件，获取待换装工具在靶标坐标系中的位置POS＝[X Y Z RotX RotY RotZ]，并构造成4X4变换矩阵H_pos

将靶标坐标系下的待换工具位置转换到机械臂基坐标系下，通过H_tool＝H_obj_base×H_pos转换得到待换工具坐标系到基坐标系的变换矩阵H_tool；

从H_tool中解析出待换工具在基坐标系中位置及角度，得到POS_inbase＝[X Y ZA B C]；

发送位置指令数据[X Y Z-100 A B C]给机械臂控制系统，使机械臂快速移动至待换工具正上方，并保证不会与快换盘导向柱相撞；

发送位置指令数据[X Y Z A B C]给机械臂控制系统，使机械臂工具夹头准确插入快换盘导向柱，完成工具快换定位。

实施例5

一种核环境下机械臂工具快换视觉定位系统，其特征在于：包括依次通信相连的图像采集单元、靶标特征提取单元、靶标位姿计算模块、机械臂姿态计算模块和机械臂控制系统，且靶标特征提取单元还与相机标定模块通信相连，靶标位姿计算模块还与手眼标定模块相连，相机标定模块通过其相机内参矩阵对靶标特征提取单元所提取的特征数据进行处理并将处理结果发送至手眼标定模块，手眼标定模块结合相机内参矩阵的处理结果和靶标位姿计算模块的计算结果进行数据处理并将处理结果传送至机械臂姿态计算模块。

所述图像采集单元包括面阵工业相机和照明光源，安装于机械臂末端用于对靶标成像。照明光源便于辅助相机的成型保证图像采集质量。

所述靶标特征提取单元从由图像采集单元传送来的靶标图像中提取出靶标圆点中心的亚像素坐标，并与靶标世界坐标系下的圆心坐标建立一一对应关系。

所述靶标位姿计算模块用于将从靶标图像中提取的圆心阵列坐标计算成像位置下靶标世界坐标系与相机成像坐标系间的4X4变换矩阵。

所述机械臂姿态计算模块根据靶标位姿解算单元计算结果以及手眼标定得到的手眼变换矩阵，计算靶标所在工具搁架上待夹持工具在机械臂基坐标系下的位置与角度，并将其作为位置指令发送给机械臂控制系统，驱使机械臂末端准确驶向待夹持工具。

所述相机标定模块以张正友标定法为基础，计算相机的内参数包括焦距f、像素尺度dx、dy、成像中心偏置量Cx、Cy，镜头畸变系数，并采用“交叉验证法”，对标定结果进行优选。

所述手眼标定模块以Tsai-Lenz方法为基础，计算机械臂当前激活的工具坐标系与相机成像坐标系间的手眼变换矩阵，并采用“增补校验”标定策略，以减少采集过程中的系统误差，实现手眼标定的高精度和置信度。

一种核环境下机械臂工具快换视觉定位系统的定位方法，其特征在于，包括以下步骤：

相机标定步骤，调节相机的焦距及光圈至成像清晰，从不同的角度、高度拍摄一套靶标图像，使靶标成像覆盖靶标图像图幅的50％～75％，将一套靶标图像随机分为两组，使用两组靶标图像采用“张正友标定法”计算相机的内参数及畸变系数并交替重投影得到重投影误差，保留误差较小的组计算出的相机参数；

机械臂手眼标定步骤，将经过标定的相机安装在机械臂末端支架上，移动机械臂至靶标上方，并使靶标在图像中成像完整且清晰，检测图像中靶标特征的像素坐标，提取靶标上标志物的亚像素坐标，计算靶标的空间位姿；使用相机标定得到的内参矩阵及畸变向量计算当前位置相机坐标系与靶标坐标系间的变换矩阵H_cam；向机械臂控制系统发送位置反馈请求，保存当前机械臂末端在基坐标系下的位置数据，并变换矩阵H_robot；根据变换矩阵H_cam及H_robot，采用“Tsai-Lenz方法”计算机械臂末端工具夹持头与相机间的相对位置得到手眼关系矩阵H_eye；

工具搁架平台标定步骤，建立工具坐标系、机械臂基座位置坐标系、靶标坐标系，将靶标固定在工具搁架平台上，保证靶标平面与待夹持工具的快换盘表面平行，工具搁架平台上的固定位置顺次摆放所有待夹持工具，依次编号；移动机械臂至每一个工具快换盘上方，调整机械臂姿对准快换盘导向柱，待导向柱完全插入工具夹持头后，记录下机械臂末端工具夹持头的位置信息；移动机械臂，使其工具中心点对准靶标原点，并调整工具坐标系的Z轴角度使工具坐标系X轴、Y轴与靶标坐标系的行列平行，记录靶标在机械臂基座位置坐标系下的位置数据根据每一个工具快换盘的位置及靶标原点的位置，计算工具快换盘相对于靶标的位置及角度，并将位置信息与工具编号一一对应，建立工具位置数据文件。

工具中心点:即“TCP点”，全称为Tool Central Point，是工业机械臂上的一个术语。

工具坐标系通过人为对准，实现工具坐标系与靶标坐标系重合，机械臂基座位置坐标系则是指机械臂基座所在坐标系，用于描述工具坐标系在机械臂基座的空间位置。

目标工具定位步骤，根据相机标定、机械臂手眼标定和工具搁架平台标定的结果定位目标工具位置完成换装。

所述相机标定步骤中，得到重投影误差的具体方法为

步骤一，将一套靶标图像随机分为两组，记为组1和组2，分别对组1和组2的靶标图像采用“张正友标定法”计算相机的内参数及畸变系数，提取出的靶标上所有特征圆的圆心像素坐标，得到内参矩阵

及畸变向量[k1,k2,p1,p2,k3,k4,k5,k6]

其中，焦距f、像素尺度dx、dy、成像中心偏置量Cx、Cy；

步骤二，将组2靶标图像上的特征圆的圆心像素坐标从世界坐标系重新投影到像素空间，与用组1靶标图像提取出的靶标特征像素坐标作差，并求平均，得到平均重投影误差Project_err1；

张氏标定法计算内参矩阵前，必须提取靶标特征的像素坐标，建立求解方程。在计算得到内参矩阵后，再根据内参矩阵反求靶标特征的像素坐标(计算值)，与之前提取出的像素坐标进行作差，这个过程称为重投影。

步骤三，将组1靶标图像上的特征点从世界坐标系重新投影到像素空间，与用组2靶标图像计算出的靶标特征像素坐标作差，并求平均，得到平均重投影误差Project_err2；

步骤四，重复上述步骤一、二和三至少5次，选取重投影误差最小的那组相机参数作为相机标定的最终结果。

所述机械臂手眼标定步骤中，变换矩阵H_robot的构造方法为向机械臂控制系统发送位置反馈请求，保存当前机械臂末端在机械臂基座位置坐标系下的位置数据[X Y Z AB C]，构造4X4变换矩阵H_robot：

所述机械臂手眼标定步骤中，得到手眼关系矩阵H_eye的方法为

步骤一，重复计算得到两组变换矩阵H_cam及H_robot，根据两组变换矩阵H_cam及H_robot，采用“Tsai-Lenz方法”计算机械臂末端工具夹持头与相机间的相对位置，得到为4X4的手眼关系矩阵H_eye和H_eye_new；

步骤二，比较H_eye矩阵与H_eye_new矩阵中平移量X、Y、Z的相对差值，取最大的为参考，与预设的相对差值的最大阈值max_Thres及最小阈值min_Thres进行比较；

若大于预设阈值max_Thres则舍弃新测得的数据，然后重复上述步骤一；

若介于min_Thres＝1％与max_Thres之间则令H_eye等于H_eye_new，然后重复上述步骤一；

若小于预设阈值min_Thres，则以H_eye为最终手眼标定计算结果，结束手眼标定。

相对差值计算公式：

其中x₀y₀z₀为H_eye矩阵中的平移量，x₁y₁z₁为H_eye_new矩阵中的平移量。

如，max_Thres即指相对差值的最大阈值，设定为5％；min_Thres为最小阈值，设定为1％，比较计算出的差值与预设阈值的大小关系决定是否停止计算。

所述机械臂手眼标定步骤中，计算靶标的空间位姿的具体方法为

步骤一，移动机械臂至靶标上方，使靶标图像成像完整且清晰，将采集到的靶标图像转为8位灰度图，并使用双边滤波过滤噪声，检测图像灰度分布，统计各个灰度值的频次，获取前景与背景灰度峰值，设定为二值化阈值区间；

步骤二，对二值化阈值区间取步长为5～15，对图像进行二值化得到二值图像集{Img0,Img1……ImgN}，对每张二值图像进行连通域检测，并计算各连通域的圆度，排除背景中非靶标特征圆的干扰斑点，并采用图像灰度矩计算公式计算符合面积、圆度要求的连通域的中心坐标；

步骤三，对所有二值图像均检测并计算出的形心位置进行聚类分析，间距dis小于0.1个像素的形心序列认为是同一个靶标圆，聚类后的形心坐标序列，采用最小外接圆法外包同一个靶标圆计算出的所有形心坐标，并令最小外接圆的圆心坐标作为该靶标圆的最终形心坐标；

步骤四，以靶标图像中心四个大圆为基准，建立行列直线簇，搜索所有在行列直线簇上的特征圆，并以左上角为原点，靶标行为Y轴，靶标列为X轴，进行排列，获取靶标在像素坐标系下的所有特征圆的亚像素坐标。

面积要求是指图像中检测出的所有连通域中，占据的像素面积在预设最小面积值和最大面积值之间的认为是备选靶标圆，以此排除大量干扰连通域。这两个预设值与实际的相机分辨率，镜头焦距，拍摄距离有关，需要事先根据所有特征圆的像素面积分布进行设定；

圆度要求是指通过上述面积筛选后的连通域中仍然可能存在不是靶标圆的干扰项，通过矩方法计算每个连通域的圆度，保留圆度大于0.8(这个参数可设为0.6～0.9，值越大，要求连通域外形越圆)的连通域，认为经过圆度筛选后的连通域为正确的靶标圆。

所述图像灰度矩计算公式：

其中f(i,j)是一幅灰度图像，i、j为矩的阶数，x、y为图像像素坐标

其中零阶矩表示为：

其中D(i,j)为连通域所在灰度图像。

一阶矩表示为：

那么图像的重心即形心(xc,yc)则为：

所述机械臂手眼标定步骤中，计算变换矩阵H_cam的具体方法为

步骤一，设置靶标的物理坐标系，以靶标左上角为原点，行方向为X轴、列方向为Y轴，计算所有特征圆中心的物理坐标，并按照与像素坐标系下相同的排列方式，进行排序；

步骤二，从靶标图像的左上角开始，选择相邻的4个点，以每个点的物理坐标(XwYw Zw)及像素坐标(u v)共5个参数作为姿态计算的输入，根据所有特征圆的物理坐标及对应像素坐标，建立姿态求解方程

将每个点的物理坐标(Xw Yw Zw)及像素坐标(u v)带入方程，计算得到矩阵

中所有坐标转换参数，记为H_obj

采用非线性最小二乘优化算法Levenberg–Marquardt算法优化H_obj，使特征点的重投影误差最小化，得到最终的H_cam。

所述目标工具定位步骤，定位目标工具位置的具体方法为

根据机械臂手眼标定步骤的标定结果，相机相对于工具坐标系的转换矩阵记为H_eye，则靶标坐标系相对工具坐标系的转换矩阵可表示为H_eye×H_obj；

从机械臂控制系统获取当前工具坐标系在基坐标系下的位置，包括平移量X、Y、，欧拉角A、B、C，并构造4X4的坐标变换矩阵H_base：

计算靶标在基坐标系下坐标转换矩阵H_obj_base＝H_base×H_eye×H_obj；

选择需要换装的工具，检索工具位置文件，获取待换装工具在靶标坐标系中的位置POS＝[X Y Z RotX RotY RotZ]，并构造成4X4变换矩阵H_pos

将靶标坐标系下的待换工具位置转换到机械臂基坐标系下，通过H_tool＝H_obj_base×H_pos转换得到待换工具坐标系到基坐标系的变换矩阵H_tool；

从H_tool中解析出待换工具在基坐标系中位置及角度，得到POS_inbase＝[X Y ZA B C]；

发送位置指令数据[X Y Z-100 A B C]给机械臂控制系统，使机械臂快速移动至待换工具正上方，并保证不会与快换盘导向柱相撞；

发送位置指令数据[X Y Z A B C]给机械臂控制系统，使机械臂工具夹头准确插入快换盘导向柱，完成工具快换定位。

Claims (8)

1.一种核环境下机械臂工具快换视觉定位系统的定位方法，其特征在于，包括以下步骤：

相机标定步骤，调节相机的焦距及光圈至成像清晰，从不同的角度、高度拍摄一套靶标图像，使靶标成像覆盖靶标图像图幅的50％至75％，将一套靶标图像随机分为两组，使用两组靶标图像采用“张正友标定法”计算相机的内参数及畸变系数并交替重投影得到重投影误差，保留误差较小的组计算出的相机参数；

机械臂手眼标定步骤，将经过标定的相机安装在机械臂末端支架上，移动机械臂至靶标上方，并使靶标在图像中成像完整且清晰，检测图像中靶标特征的像素坐标，提取靶标上标志物的亚像素坐标，计算靶标的空间位姿；使用相机标定得到的内参矩阵及畸变向量计算当前位置相机坐标系与靶标坐标系间的变换矩阵H_cam；向机械臂控制系统发送位置反馈请求，保存当前机械臂末端在基坐标系下的位置数据并构造4X4的变换矩阵H_robot；根据变换矩阵H_cam及H_robot，采用“Tsai-Lenz方法”计算机械臂末端工具夹持头与相机间的相对位置得到手眼关系矩阵H_eye；

工具搁架平台标定步骤，建立工具坐标系、机械臂基座位置坐标系、靶标坐标系，将靶标固定在工具搁架平台上，保证靶标平面与待夹持工具的快换盘表面平行，工具搁架平台上的固定位置顺次摆放所有待夹持工具，依次编号；移动机械臂至每一个工具快换盘上方，调整机械臂姿态对准快换盘导向柱，待导向柱完全插入工具夹持头后，记录下机械臂末端工具夹持头的位置信息；移动机械臂，使其工具中心点对准靶标原点，并调整工具坐标系的Z轴角度使工具坐标系X轴、Y轴与靶标坐标系的行列平行，记录靶标在机械臂基座位置坐标系下的位置数据根据每一个工具快换盘的位置及靶标原点的位置，计算工具快换盘相对于靶标的位置及角度，并将位置信息与工具编号一一对应，建立工具位置数据文件；

目标工具定位步骤，根据相机标定、机械臂手眼标定和工具搁架平台标定的结果定位目标工具位置完成换装。

2.如权利要求1所述的一种核环境下机械臂工具快换视觉定位系统的定位方法，其特征在于：所述相机标定步骤中，得到重投影误差的具体方法为

步骤一，将一套靶标图像随机分为两组，记为组1和组2，分别对组1和组2的靶标图像采用“张正友标定法”计算相机的内参数及畸变系数，提取出的靶标上所有特征圆的圆心像素坐标，得到内参矩阵

及畸变向量[k1，k2，p1，p2，k3，k4，k5，k6]

其中，焦距f_x＝1/d_x，焦距f_y＝1/d_y，d_x和d_y为像素尺度；

步骤二，将组2靶标图像上的特征圆的圆心像素坐标从世界坐标系重新投影到像素空间，与用组1靶标图像提取出的靶标特征像素坐标作差，并求平均，得到平均重投影误差Project_err1；

步骤三，将组1靶标图像上的特征点从世界坐标系重新投影到像素空间，与用组2靶标图像计算出的靶标特征像素坐标作差，并求平均，得到平均重投影误差Project_err2；

步骤四，重复上述步骤一、二和三至少5次，选取重投影误差最小的那组相机参数作为相机标定的最终结果。

3.如权利要求1所述的一种核环境下机械臂工具快换视觉定位系统的定位方法，其特征在于：所述机械臂手眼标定步骤中，变换矩阵H_robot的构造方法为向机械臂控制系统发送位置反馈请求，保存当前机械臂末端在机械臂基座位置坐标系下的位置数据[X Y Z A BC]，构造4X4变换矩阵H_robot：

4.如权利要求1所述的一种核环境下机械臂工具快换视觉定位系统的定位方法，其特征在于：所述机械臂手眼标定步骤中，得到手眼关系矩阵H_eye的方法为步骤一，重复计算得到两组变换矩阵H_cam及H_robot，根据两组变换矩阵H_cam及H_robot，采用“Tsai-Lenz方法”计算机械臂末端工具夹持头与相机间的相对位置，得到为4X4的手眼关系矩阵H_eye和H_eye_new；

步骤二，比较H_eye矩阵与H_eye_new矩阵中平移量X、Y、Z的相对差值，取最大的为参考，与预设的相对差值的最大阈值max_Thres及最小阈值min_Thres进行比较；

若大于预设阈值max_Thres则舍弃新测得的数据，然后重复上述步骤一、二；

若介于min_Thres＝1％与max_Thres之间则令H_eye等于H_eye_new，然后重复上述步骤一、二；

若小于预设阈值min_Thres，则以H_eye为最终手眼标定计算结果，结束手眼标定。

5.如权利要求1所述的一种核环境下机械臂工具快换视觉定位系统的定位方法，其特征在于：所述机械臂手眼标定步骤中，计算靶标的空间位姿的具体方法为步骤一，移动机械臂至靶标上方，使靶标图像成像完整且清晰，将采集到的靶标图像转为8位灰度图，并使用双边滤波过滤噪声，检测图像灰度分布，统计各个灰度值的频次，获取前景与背景灰度峰值，设定为二值化阈值区间；

步骤二，对二值化阈值区间取步长为5～15，对图像进行二值化得到二值图像集{Img0，Img1……ImgN}，对每张二值图像进行连通域检测，并计算各连通域的圆度，排除背景中非靶标特征圆的干扰斑点，并采用图像灰度矩计算公式计算符合面积、圆度要求的连通域的中心坐标；

步骤三，对所有二值图像均检测并计算出的形心位置进行聚类分析，间距dis小于0.1个像素的形心序列认为是同一个靶标圆，聚类后的形心坐标序列，采用最小外接圆法外包同一个靶标圆计算出的所有形心坐标，并令最小外接圆的圆心坐标作为该靶标圆的最终形心坐标；

步骤四，以靶标图像中心四个大圆为基准，建立行列直线簇，搜索所有在行列直线簇上的特征圆，并以左上角为原点，靶标行为Y轴，靶标列为X轴，进行排列，获取靶标在像素坐标系下的所有特征圆的亚像素坐标。

6.如权利要求5所述的一种核环境下机械臂工具快换视觉定位系统的定位方法，其特征在于，所述图像灰度矩计算公式：

其中f(i，j)是一幅灰度图像，i、j为矩的阶数，x、y为图像像素坐标其中零阶矩表示为：

其中D(i,j)为连通域所在灰度图像；

一阶矩表示为：

那么图像的重心即形心(x_c,y_c)则为：

7.如权利要求1所述的一种核环境下机械臂工具快换视觉定位系统的定位方法，其特征在于：所述机械臂手眼标定步骤中，计算变换矩阵H_cam的具体方法为步骤一，设置靶标的物理坐标系，以靶标左上角为原点，行方向为X轴、列方向为Y轴，计算所有特征圆中心的物理坐标，并按照与像素坐标系下相同的排列方式，进行排序；

步骤二，从靶标图像的左上角开始，选择相邻的4个点，以每个点的物理坐标(X_w，Y_w，Z_w)及像素坐标(u，v)共5个参数作为姿态计算的输入，根据所有特征圆的物理坐标及对应像素坐标，建立姿态求解方程

将每个点的物理坐标(X_w，Y_w，Z_w)及像素坐标(u，v)带入方程，计算得到矩阵

中所有坐标转换参数，记为H_obj

采用非线性最小二乘优化算法Levenberg–Marquardt算法优化H_obj，使特征点的重投影误差最小化，得到最终的H_cam。

8.如权利要求1所述的一种核环境下机械臂工具快换视觉定位系统的定位方法，其特征在于：所述目标工具定位步骤，定位目标工具位置的具体方法为根据机械臂手眼标定步骤的标定结果，相机相对于工具坐标系的转换矩阵记为H_eye，则靶标坐标系相对工具坐标系的转换矩阵可表示为H_eye×H_obj；

从机械臂控制系统获取当前工具坐标系在基坐标系下的位置，包括平移量X、Y，欧拉角A、B、C，并构造4X4的坐标变换矩阵H_base：

计算靶标在基坐标系下坐标转换矩阵H_obj_base＝H_base×H_eye×H_obj；

选择需要换装的工具，检索工具位置文件，获取待换装工具在靶标坐标系中的位置POS＝[X Y Z RotX RotY RotZ]，并构造成4X4变换矩阵H_pos

将靶标坐标系下的待换工具位置转换到机械臂基坐标系下，通过H_tool＝H_obj_base×H_pos转换得到待换工具坐标系到基坐标系的变换矩阵H_tool；

从H_tool中解析出待换工具在基坐标系中位置及角度，得到POS_inbase＝[X Y Z A BC]；

发送位置指令数据[X Y Z-100 A B C]给机械臂控制系统，使机械臂快速移动至待换工具正上方，并保证不会与快换盘导向柱相撞；

发送位置指令数据[X Y Z A B C]给机械臂控制系统，使机械臂工具夹头准确插入快换盘导向柱，完成工具快换定位。

Images