技术领域
[0001] 本
发明涉及一种基于彩色球形光源靶标的机械臂末端位姿测量系统及方法。
背景技术
[0002] 实现机械臂末端的精确位姿测量对机械臂标定和机械臂抓取具有重要意义。当前基于
机器视觉的机械臂末端位姿测量方式存在以下问题：
[0003] 1、机械臂末端由于运动范围大，存在遮挡问题，当机械臂末端被遮挡将难以计算位姿；
[0004] 2、机械臂末端靶标识别容易受到环境光照的干扰；
[0005] 3、在基于单目视觉的位姿测量中，目标特征点和已知的特征点模型的匹配难度大；基于
双目视觉的位姿测量中，两幅图像同一特征点的匹配难度较大。
发明内容
[0006] 本发明为了解决上述问题，提出了一种基于彩色球形光源靶标的机械臂末端位姿测量系统及方法。
[0007] 一种基于彩色球形光源靶标的机械臂末端位姿测量系统，包括彩色球形光源靶标、光源控
制模块、固定安装相机、计算机、机械臂和靶标标定系统，其中：
[0008] 所述彩色球形光源靶标包括四个相互垂直相交的支杆，每个支杆的端部均设置有一组光源，每组光源至少包括一个
定位中心，以及沿所述定位中心分布的三个光源，这12个光源
颜色不同，是球形；
[0009] 所述支杆设置于机械臂上，随机械臂的移动而移动；
[0010] 所述光源
控制模块用于控制光源靶标打开和关闭，与计算机连接；
[0011] 所述固定安装相机采集机械臂末端运动图像，与计算机连接；
[0012] 所述计算机用于处理相机采集的图像，进行
图像处理，计算机械臂末端位姿，所述计算机通过光源控制模块控制光源打开和关闭；
[0013] 所述靶标标定系统用于计算靶标在机械臂末端
坐标系的
位置，靶标标定系统出厂或第一次使用时进行一次计算即可。
[0014] 进一步的，所述视觉系统上设置有颜色校正板，对采集的图像进行颜色校正。
[0015] 进一步的，所述每组光源中的三个光源与定位中心的连线两两垂直。
[0016] 进一步的，所述固定安装相机为一个相机组成单目视觉或/和两个相机组成双目视觉进行位姿测量。
[0017] 进一步的，所述靶标标定系统采用手眼标定的方式计算光源靶标在机械臂末端坐标系位置，所述靶标标定系统包括光源控制模块、颜色校正板、手眼相机、计算机，所述光源控制模块、手眼相机连接计算机，所述颜色校正板通过
连杆固定在手眼相机上。
[0018] 上述测量系统的工作方法，包括以下步骤：
[0019] 测量系统出厂或第一次使用时，在自然光环境下确定固定安装相机图像中颜色校正板区域位置；
[0020] 测量系统出厂或第一次使用时标定靶标在机械臂末端坐标系位置；
[0021] 固定安装相机标定；
[0022] 分别在关闭靶标光源与打开靶标光源状态时，采集至少一幅图像；
[0023] 靶标光源打开前后的图像相减得到光源靶标图像；
[0024] 对采集的图像进行颜色校正，结合颜色和靶标圆形形状识别出光源靶标，计算靶标图像的
重心作为光源球心；
[0025] 判断当前图像中靶标数量，若靶标数量大于等于3，则计算靶标特征点在相机坐标系的位置，根据靶标特征点在机械臂末端坐标系和相机坐标系的位置，计算机械臂末端坐
标系在相机坐标系的旋转平移矩阵以及位置和
姿态角；否则重新采集图像。
[0026] 进一步的靶标标定系统计算光源靶标在机械臂末端坐标系的位置步骤如下：
[0027] 首先确定手眼相机图像中颜色校正板区域位置；
[0028] 单目视觉方式：在机械臂末端上放置相机，标定相机内参数，通过手眼标定计算相机坐标系和机械臂末端坐标系的旋转平移矩阵，通过单目视觉方式计算出每个光源靶标在手眼相机坐标系位置，计算靶标在机械臂末端坐标系的位置；
[0029] 双目视觉方式：在机械臂末端上放置两个相机构成双目视觉，标定两个相机内参数和外参数，通过手眼标定计算其中一个相机的相机坐标系和机械臂末端坐标系的旋转平
移矩阵，通过双目视觉方式计算出每个光源靶标在上述相机坐标系位置，计算靶标在机械
臂末端坐标系的位置。
[0030] 进一步的，关闭靶标光源时采集的图像A，打开靶标光源时采集的图像B，图像B减去图像A得到图像C，根据图像C确定靶标区域，令图像C中靶标区域值是1，非靶标区域值是
0，与图像B对应坐标
像素值相乘，即得到光源靶标图像。
[0031] 进一步的，对采集的图像进行颜色校正的具体过程包括：
[0032] (1)在工作环境中，相机采集图像，计算校正板区域图像的各个颜色通道平均值Ravg、Gavg和Bavg；
[0033] (2)对采集的图像R、G和B通道进行如下运算：r＝Ri/Ravg；g＝Gi/Gavg；b＝Bi/Bavg，Ri、Gi和Bi为采集的图像的各通道的数据，若结果大于1，取值为1；
[0034] (3)将r、g和b转换成8位整型数据；
[0035] (4)组合RGB三个通道矩阵。
[0036] 进一步的，对于空间颜色识别的过程，包括：
[0037] 根据如下公式计算靶标颜色T(TR，TG，TB)和对比颜色C(CR，CG，CB)的相似度
[0038]
[0039] 设定
阈值，当d小于一定阈值则认为靶标颜色是该对比颜色。
[0040] 进一步的，对于空间颜色识别的过程中，或采用将图像从RGB空间转换到HSV空间，进行颜色识别，结合颜色和靶标圆形形状识别出光源靶标，计算靶标图像的重心作为光源球心。
[0041] 进一步的，计算靶标特征点在相机坐标系的位置的具体过程包括：
[0042] 单目视觉方式：通过颜色将识别的靶标和已知的靶标模型进行匹配，通过PnP
算法进行位姿解算，得到各个靶标在相机坐标系中位置坐标。
[0043] 双目视觉方式：通过颜色匹配两幅图像中特征点
[0044] 设两个相机同一时刻拍摄的两幅图像中同一特征点的像素坐标是 和 zC表示物距， 表示特征点在世界坐标系坐标，M表示相机内参数和外参数矩阵的乘积
[0045]
[0046]
[0047] 根据(1)(2)可以求解出
[0048] 进一步的，根据靶标特征点在机械臂末端坐标系和相机坐标系的位置，计算机械臂末端坐标系在相机坐标系的旋转平移矩阵以及位置和姿态角的过程包括：
[0049] 根据向量法可以求出机械臂末端坐标系到相机坐标系的旋转矩阵R
[0050]
[0051] 将R带入公式(1)求出平移矩阵T。
[0052] 机械臂末端坐标系在相机坐标系中的姿态角计算公式如下：
[0053]
[0054] β＝arcsin(-r31)
[0055]
[0056] 与
现有技术相比，本发明的有益效果为：
[0057] 1、本发明的靶标采用彩色球形光源，在黑暗环境或光照条件复杂的环境根据颜色识别光源靶标。
[0058] 2、本发明的光源可以控制打开或关闭，极大的提高了靶标识别的灵活度。光源关闭，采集图像A，光源打开，采集图像B，两幅图像相减，则可以确定靶标区域，极大的减少周围杂点的干扰。
[0059] 3、本发明的靶标是球形，在各个角度拍摄得到的图像均是圆形，靶标图像不会变形，这一特点可以很好提高靶标球心识别
精度；结合靶标圆形形状和颜色，可以很好识别靶标。
[0060] 4、本发明的靶标即使部分靶标被遮挡，只要采集的图像上有三个或三个以上的点即可进行位姿解算，一定程度减少遮挡，提高位姿测量成功率。
[0061] 5、本发明通过靶标颜色进行特征点匹配，对于单目视觉，减少图像中特征点和已知特征点模型的匹配难度；对于双目视觉，减少两幅图像同一特征点匹配难度。
附图说明
[0062] 构成本
申请的一部分的
说明书附图用来提供对本申请的进一步理解，本申请的示意性
实施例及其说明用于解释本申请，并不构成对本申请的不当限定。
[0063] 图1为单目视觉位姿测量系统结构示意图；
[0064] 图2为双目视觉位姿测量系统结构示意图；
[0065] 图3为靶标标定系统结构示意图(单目视觉方式)；
[0066] 图4为靶标标定系统结构示意图(双目视觉方式)；具体实施方式：
[0067] 下面结合附图与实施例对本发明作进一步说明。
[0068] 应该指出，以下详细说明都是例示性的，旨在对本申请提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。
[0069] 需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。
[0070] 在本发明中，术语如“上”、“下”、“左”、“右”、“前”、“后”、“竖直”、“
水平”、“侧”、“底”等指示的方位或位置关系为基于附图所示的方位或位置关系，只是为了便于叙述本发明各部件或元件结构关系而确定的关系词，并非特指本发明中任一部件或元件，不能理解为对本发明的限制。
[0071] 本发明中，术语如“固接”、“相连”、“连接”等应做广义理解，表示可以是固定连接，也可以是一体地连接或可拆卸连接；可以是直接相连，也可以通过中间媒介间接相连。对于本领域的相关科研或技术人员，可以根据具体情况确定上述术语在本发明中的具体含义，不能理解为对本发明的限制。
[0072] 如图1所示，一种基于彩色球形光源靶标的机械臂末端位姿测量系统，主要结构包括：
[0073] 1、彩色球形光源靶标
[0074] 光源颜色分别是红、绿、蓝、黄、橙、紫、白、棕、青、淡蓝、淡绿、粉红，光源是球形，光源支杆尽可能小减少对光源识别造成干扰，光源的分布如图所示。每三个球形光源组成一组，这三个光源与O点的连线两两垂直，4组光源安装在机械臂四周，支杆两两垂直。光源颜色不限于文中描述方式，光源颜色的选择尽可能使得这些颜色在RGB或HSV空间差别较大，
便于识别。光源布置方式不限于文中描述方式，主要为了减少机械臂对光源遮挡，当机械臂运动到新的位置姿态时，即使部分光源被遮挡，只要采集的图像中存在三个及以上的光源
图像，仍然能进行机械臂末端位姿测量。
[0075] 2、光源控制模块
[0076] 用于控制靶标光源打开和关闭；
[0077] 3、颜色校正板，用于对采集的图像进行颜色校正，减少环境光对球形光源识别造成干扰。颜色校正板为白色，安装在相机前方，通过连杆与相机
支架固定，颜色校正板图像在相机图像下方，占整个图像面积较小，不影响对靶标图像的采集。
[0078] 4、计算机，连接固定安装相机、靶标标定系统中手眼相机，用于图像处理；连接光源控制模块，通过光源控制模块控制光源
开关。
[0079] 5、固定安装相机，用于采集光源靶标图像。相机安装在机械臂
工作空间中，可以是一个相机通过单目视觉方式进行位姿解算，如图1中A所示；也可以是两个相机构成双目视觉进行位姿解算，如图2中B所示。
[0080] 6、机械臂。
[0081] 7、靶标标定系统用于计算靶标在机械臂末端坐标系的位置，靶标标定系统在测量系统出厂或第一次使用时进行一次计算即可。优选的，标定系统可以通过手眼标定的方式
计算靶标位置。
[0082] 如图3、4所示，靶标标定系统包括：
[0083] (1)、手眼相机，安装在机械臂末端，用于采集光源靶标图像，可以是单
[0084] 目视觉或双目视觉方式；
[0085] (2)、颜色校正板，安装方式同相机A、B；
[0086] (3)、计算机，用于图像处理；
[0087] 在靶标标定和机械臂末端位姿测量前，首先确定颜色校正板区域在相机图像的位置，由于颜色校正板固定在相机支架上，颜色校正板在相机图像中区域位置不变，因此该步骤只需要进行一次即可。已知颜色校正板区域，在颜色校正过程中可以直接提取该区域像
素值进行图像颜色校正，减少相机图像中颜色校正板区域识别步骤，提高测量速度。具体
为：
[0088] 单目视觉进行位姿测量时，在自然光照环境下进行相机拍照，确定颜色校正板在图像中的区域；
[0089] 双目视觉进行位姿测量时，在自然光照环境下进行相机拍照，确定该颜色校正板在两个相机采集的图像中的区域；
[0090] 靶标标定系统计算靶标在机械臂末端坐标系位置，在测量系统出厂或第一次使用时进行一次即可，具体步骤为：
[0091] 第一种方案，靶标标定系统采用手眼相机单目视觉方式：
[0092] 具体的，如图3所示：
[0093] (1)在机械臂上放置相机，标定相机内参数；
[0094] (2)通过手眼标定计算相机坐标系和机械臂末端坐标系的旋转平移矩阵；
[0095]
[0096] (3)通过单目视觉方式计算出每个球形光源在手眼相机坐标系位置 (具体步骤为机械臂末端位姿测量方式一的2-7)；
[0097] (4)计算靶标在机械臂末端坐标系的位置；
[0098]
[0099] 第二种方案，如图4所示，用双目视觉方式进行标定。
[0100] (1)在机械臂上放置相机，标定两个相机内参数和外参数；
[0101] (2)通过手眼标定计算相机1的相机坐标系和机械臂末端坐标系的旋转平移矩阵；
[0102]
[0103] (3)通过双目视觉方式计算出每个球形光源在相机1坐标系位置 (具体步骤为机械臂末端位姿测量方式二的2-7)；
[0104] (4)计算靶标在机械臂末端坐标系的位置；
[0105]
[0106] 进而，进行机械臂末端位姿测量。
[0107] 方式一：单目视觉方式进行位姿测量，具体包括：
[0108] 1、相机标定
[0109] 采用张正友标定法计算相机内参数矩阵M
[0110]
[0111] 其中，f是焦距，dx和dy表示像素尺寸，u0和v0表示相机光轴在像素坐标系中的坐标。
[0113] 关闭靶标光源，采集第一幅图像A；打开靶标光源，采集第二幅图像B
[0114] 3、图像相减得到光源靶标图像
[0115] 图像B减去图像A得到图像C，能极大减少图像中的干扰，根据图像C可以确定靶标区域，令图像C中靶标区域值是1，非靶标区域值是0，与图像B对应坐标像素值相乘，即得到光源靶标图像
[0116] 4、图像颜色校正
[0117] 采用如下方式进行颜色校正：
[0118] (1)在工作环境中，相机采集图像，计算图像中校正板区域的各个颜色通道平均值Ravg、Gavg和Bavg。
[0119] (2)对采集的图像R、G、B通道进行如下运算：r＝Ri/Ravg；g＝Gi/Gavg；b＝Bi/Bavg，若结果大于1，取值为1
[0120] (3)将r，g，b转换成8位整型数据
[0121] (4)组合RGB三个通道矩阵
[0122] 5、基于颜色和形状的靶标识别
[0123] RGB空间颜色识别
[0124] 根据如下公式计算靶标颜色T(TR，TG，TB)和对比颜色C(CR，CG，CB)的相似度
[0125]
[0126] 设定阈值，当d小于一定阈值则认为靶标颜色是该对比颜色
[0127] 也可以将图像从RGB空间转换到HSV空间，进行颜色识别
[0128] 结合颜色和靶标圆形形状识别出光源靶标，计算靶标图像的重心作为光源球心
[0129] 6、判断当前图像中靶标数量，若靶标数量大于等于3，则进入步骤7；否则进入步骤2；
[0130] 7、计算靶标特征点在相机坐标系的位置
[0131] 通过颜色将识别的靶标和已知的靶标模型进行匹配，通过PnP算法进行位姿解算，得到各个靶标在相机坐标系中位置坐标
[0132] 8、根据靶标特征点在机械臂末端坐标系和相机坐标系的位置，计算机械臂末端坐标系在相机坐标系的旋转平移矩阵以及位置和姿态角。
[0133] 同一个特征点在相机坐标系中坐标向量c和机械臂末端坐标系中坐标向量m存在以下关系：
[0134] c＝Rm+T (1)
[0135] 其中，R表示旋转矩阵，T表示平移矩阵。
[0136] 根据向量法可以求出机械臂末端坐标系到相机坐标系的旋转矩阵R
[0137]
[0138] 将R带入公式(1)求出平移矩阵T。
[0139] 机械臂末端坐标系在相机坐标系中的姿态角计算公式如下：
[0140]
[0141] β＝arcsin(-r31)
[0142]
[0143] 方式二、双目视觉方式进行机械臂末端位姿测量
[0144] 1、通过张正友标定法计算相机内参数矩阵M1和外参数矩阵M2
[0145]
[0146] 其中R表示旋转矩阵，T表示平移矩阵。
[0147] 2、图像采集(同方式一)；
[0148] 3、图像相减得到光源靶标图像(同方式一)；
[0149] 4、图像颜色校正(同方式一)；
[0150] 5、基于靶标颜色和形状进行识别，计算出靶标球心位置(同方式一)；
[0151] 6、判断当前图像中靶标数量，若靶标数量大于等于3，则进入步骤7；否则进入步骤2；
[0152] 7、计算靶标特征点在相机坐标系的位置；
[0153] 根据颜色对两幅图像中特征点进行匹配
[0154] 设两个相机同一时刻拍摄的两幅图像中同一特征点的像素坐标是 和 zC表示物距， 表示特征点在世界坐标系坐标，M表示相机内参数和外参数矩阵的乘积
[0155]
[0156]
[0157] 根据(1)(2)可以求解出
[0158] 根据相机A的外参数，计算特征点到相机A坐标系的位置 H是世界坐标系到相机坐标系的旋转平移矩阵。
[0159] 7、根据靶标特征点在机械臂末端坐标系和相机A坐标系的位置，计算机械臂末端坐标系到相机坐标系的旋转平移矩阵以及位置和姿态角(同方式一)。
[0160] 以上所述仅为本申请的优选实施例而已，并不用于限制本申请，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何
修改、等同替换、改进等，均应包含在本申请的保护范围之内。
[0161] 上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的
基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或
变形仍在本发明的保护范围以内。
