基于视觉伺服的工业机器人系统研究(摄像机标定、手眼标定、目标单目定位)

一、标定技术

常见的机器人视觉伺服中要实现像素坐标与实际坐标的转换,首先就要进行标定,对于实现视觉伺服控制,这里的标定不仅包括摄像机标定,也包括机器人系统的手眼标定。以常见的焊接机器人系统为例,有两种构型,如下:

基于视觉伺服的工业机器人系统研究(摄像机标定、手眼标定、目标单目定位)_第1张图片

即:摄像机固定于机器手和摄像机固定于外部场景;

本文针对前一种构型:摄像机固定于机器手。

1、摄像机标定技术

(1)理论部分:

以张正友的棋盘标定法为摄像机标定方式,由于摄像机标定结果要用到后面的手眼标定中,所以此处进行不同方位的棋盘图片拍摄时需要遵守:标定板固定位置不动,手眼组合体变换姿态拍摄图片。

摄像机标定的目的:得到两组坐标系的两两转化矩阵:T1和T2;

1)得到图片像素坐标系P与摄像机坐标系C之间的转换矩阵T1,准确说应该是摄像机坐标系转化为图片像素坐标系的转换矩阵。可表示为:

P=T1*C;

解释:T1在摄像机标定结果中就是内参矩阵3x3;


2)得到摄像相机坐标系C与棋盘上建立的世界坐标系G之间的转换矩阵T2,准确说应该是坐标系G转化为摄像机坐标系的转换矩阵。可表示为:

C=T2*G;

解释:T2在摄像机标定结果中就是外参矩阵4x4,由旋转矩阵r和平移向量t构成[ t  r; 0 0 0 1];


(2)方法:

摄像机标定方法有两种可选:openCV或者Matlab标定工具箱;

建议选择MATLAB应用程序——图像处理与计算机视觉——Camera Calibrator,直接导入拍摄好的图片即可。但是要注意,使用matlab标定工具箱所得到的内参矩阵、外参旋转矩阵、外参平移向量都要经过转置才是正确的结果

如下图,MATLAB标定得到的红框中依次是外参平移向量、内参矩阵、外参旋转矩阵,它们都需要做转置后才能应用于本文的公式计算:

基于视觉伺服的工业机器人系统研究(摄像机标定、手眼标定、目标单目定位)_第2张图片


2、手眼标定技术

(1)理论部分:

手眼标定目的:得到摄像机坐标系C与机器手(或工具)坐标系H之间的转换矩阵T3,准确说应该是机器手坐标系转化为摄像机坐标系的转化矩阵。可表示为:

C=T3*H;

解释:T3需要根据公式CX=XD得到;实际中,分别知道C、D求出来的X有无穷多个解。所以为了实现唯一解,我们至少需要两组C和D,即至少需要3个位置的摄像机标定结果。

其中C的求法如下:

C是两个摄像机坐标系之间的变换矩阵。可以根据上述任一两张标定图片所得的两个摄像机标定外参A、B按公式C=A*inv(B)计算得到的。假设上述摄像机标定中有3张标定图片的外参标定结果分别是T21、T22、T23,那么可以得到两个C矩阵:

C1=T21*inv(T22);

C2=T22*inv(T23);

D的求法如下:

D是两个机器手坐标系之间的变换矩阵。假设上述摄像机标定中的3张标定图片所一一对应的机器手坐标系在基坐标系(也可以是工件坐标系或者其他固定的参考坐标系)中的描述矩阵结果分别是H1、H2、H3(H需要从机器人控制器或示教器中读取),那么可以得到两个D矩阵:

D1=inv(H1)*H2;

D2=inv(H2)*H3;

由以上两组C和D,代入CX=XD就可以得到唯一解X,从而T3=X;


注:上述H1、H2、H3是每张标定图片对应的机器手坐标系描述矩阵,正好说明了摄像机标定中所谓的“标定板固定,手眼运动”的正确性。如果手眼不动,改变标定板姿态进行拍摄,那么H的值都是一样的。


(2)方法:

1)根据摄像机标定已知摄像机外参矩阵T21、T22、T23,还要从机器人控制器中读取T21、T22、T23分别对应的机器手(或工具)坐标系H1、H2、H3。控制器中的坐标系描述矩阵不是直接读取的,它是以平移向量和欧拉角(或四元数)模式存在的,如下:

平移向量+欧拉角模式:

基于视觉伺服的工业机器人系统研究(摄像机标定、手眼标定、目标单目定位)_第3张图片


平移向量+四元数模式:

基于视觉伺服的工业机器人系统研究(摄像机标定、手眼标定、目标单目定位)_第4张图片


选取其中任一模式即可,然后将其转化为描述矩阵。具体转化方法可参考代码:http://download.csdn.net/detail/xiongchao99/9813141


上述工作完成后,就已经获取了3个外参矩阵(再次提醒,摄像机标定使用MATLAB标定工具箱的话,所得到的外参旋转矩阵和平移向量先要转置,即R=r',T=t',然后外参矩阵EX=[R T;0 0 0 1])和 3个机械手坐标系矩阵,因此可以分别将3个二维矩阵合为一个三维矩阵,matlab命令如下:

C_ext=cat(3, C_ext1, C_ext2, C_ext3);

H=cat(3, H1, H2 ,H3)

最后将C_ext和H作为参数代入到如下MATLAB函数中:

function Tch = GetCamera2HandMatrix(C_ext,H)
%   以下变量:
%   C_ext是3个位置的摄像机外参矩阵:3x4x4
%   H1、H2、H3分别是3个位置的机械手坐标系的姿态矩阵:3x4x4
%   Tcg--机器手坐标系(或工具坐标系)在摄像机坐标系中的姿态和位置变换矩阵

%   C1、D1、C2、D2、R、w、q、kc1、kc2、kc3、kd1、kd2、kd3、a、b、c、d、h、y均为临时变量

    C1=C_ext(:,:,1)*inv(C_ext(:,:,2))
    C2=C_ext(:,:,2)*inv(C_ext(:,:,3))
    D1=inv(H(:,:,1))*H(:,:,2)
    D2=inv(H(:,:,2))*H(:,:,3)
    
    R=C1(1:3,1:3);  
    q=acos((trace(R)-1)/2);
    w(1,1)=q/(2*sin(q))*(R(3,2)-R(2,3));
    w(2,1)=q/(2*sin(q))*(R(1,3)-R(3,1));
    w(3,1)=q/(2*sin(q))*(R(2,1)-R(1,2));
    kc1=w;
    
    R=C2(1:3,1:3);  
    q=acos((trace(R)-1)/2);
    w(1,1)=q/(2*sin(q))*(R(3,2)-R(2,3));
    w(2,1)=q/(2*sin(q))*(R(1,3)-R(3,1));
    w(3,1)=q/(2*sin(q))*(R(2,1)-R(1,2));
    kc2=w;
    
    R=D1(1:3,1:3);  
    q=acos((trace(R)-1)/2);
    w(1,1)=q/(2*sin(q))*(R(3,2)-R(2,3));
    w(2,1)=q/(2*sin(q))*(R(1,3)-R(3,1));
    w(3,1)=q/(2*sin(q))*(R(2,1)-R(1,2));
    kd1=w;
    
    R=D2(1:3,1:3);  
    q=acos((trace(R)-1)/2);
    w(1,1)=q/(2*sin(q))*(R(3,2)-R(2,3));
    w(2,1)=q/(2*sin(q))*(R(1,3)-R(3,1));
    w(3,1)=q/(2*sin(q))*(R(2,1)-R(1,2));
    kd2=w;
    
    kc3=cross(kc1,kc2);
    kd3=cross(kd1,kd2);
    a=[kc1 kc2 kc3];
    b=[kd1 kd2 kd3];
    R=a*inv(b);  %得到旋转关系矩阵
    
    tc1=C1(1:3,4);
    tc2=C2(1:3,4);
    td1=D1(1:3,4);
    td2=D2(1:3,4);
    c=R*td1-tc1;
    d=R*td2-tc2;
    a=C1(1:3,1:3)-[1 0 0;0 1 0;0 0 1];
    b=C2(1:3,1:3)-[1 0 0;0 1 0;0 0 1];
    h=[a;b];
    y=[c;d];
    t=inv(h'*h)*h'*y;   %得到平移关系矩阵
    
    Tch=[R t;0 0 0 1];   %得到最终结果
end

3、根据标定结果对固定高度目标实现单目定位

(1)理论部分:

由上述1、2两个标定已经得到:

摄像机坐标系C->像素坐标系P的转换矩阵Tpc(即内参矩阵,MATLAB标定得到的要转置);

机械手(或工具)坐标系H->摄像机坐标系C的转化矩阵Tch;

从控制器读取的机械手(或工具)坐标系H->基坐标系B(这个根据情况自己在控制器设定是基坐标还是工件坐标系,本文用基坐标系)的转化矩阵Tbh;

已知目标高度固定,为z;

那么基坐标系转化为像素坐标系的变换矩阵就是:Gpb=Tpc*Tch*inv(Tbh);

根据Gpb和z可以得到如下图所示的变换过程,分解后可根据像素坐标(u,v)求得实际坐标(x,y,z):

基于视觉伺服的工业机器人系统研究(摄像机标定、手眼标定、目标单目定位)_第5张图片

其中,Tpc需要注意,应在内参矩阵最后添加一个全零列,变为3x4矩阵,如下:

基于视觉伺服的工业机器人系统研究(摄像机标定、手眼标定、目标单目定位)_第6张图片


(2)代码实现:

function  P= GetObjectLocation( u,v,Gtb)
%   参数(u,v)为目标在图片中的像素坐标
%   参数Gtb是工具在机器人基坐标中的描述矩阵(也就是工具坐标系->基坐标系的变换矩阵)

    %内参矩阵
    Kl=[ 1851    9.7    550.5  0;
         0    1844.4    299.7  0;
         0         0    1.0    0];
    %摄像机与工具关系矩阵
    Gctl= [-0.9620   -0.2974    0.0156   -2.6405;
    0.3266   -0.9552    0.0056   59.7141;
    0.0130    0.0003    1.0161  145.3381;
         0         0         0    1.0000];

    G=inv(Gtb);
    z=10;  %指定物体的高度
    M=Kl*Gctl*G;
    Ml=[u*M(3,1)-M(1,1)  u*M(3,2)-M(1,2)  ;   v*M(3,1)-M(2,1)  v*M(3,2)-M(2,2)];
    Mr=[M(1,4)-u*M(3,4)-(u*M(3,3)-M(1,3))*z;  M(2,4)-v*M(3,4)-(v*M(3,3)-M(2,3))*z];
    P=inv(Ml)*Mr;  %得到物体的位置
end



你可能感兴趣的:(机器人与视觉伺服)