机器人手眼标定

 writen by wqj1212@yahoo.com.cn

位置0到位置1 各个参数关系
位置0  末端到机器人基坐标位置
----PulseNO.S 82501,L 69953,U -114458,R -2,B -1,T 1
----CartPos.S 190.184,L 355.823,U -141.89,R 180,B -4.86,T 61.88

a=180/180 *pi;
b=-4.86/180 *pi;
r=61.88/180*pi;
Rx=[1 0 0;0 cos(a) -sin(a);0 sin(a) cos(a)];
Ry=[cos(b) 0 sin(b);0 1 0;-sin(b) 0 cos(b)];
Rz=[cos(r) -sin(r) 0;sin(r) cos(r) 0;0 0 1];
R=Rz*Ry*Rx

R =

    0.4696    0.8820    0.0399
    0.8788   -0.4713    0.0747
    0.0847    0.0000   -0.9964


G =  [0.4696    0.8820    0.0399    190.184;
    0.8788   -0.4713    0.0747    355.823;
    0.0847    0.0000   -0.9964    -141.89;
    0           0         0          1   ];

0 位置图像外参数

Extrinsic parameters:

Translation vector: Tc_ext = [ -15.564192      -31.592337      447.897520 ]
Rotation vector:   omc_ext = [ -2.359346      -2.071994      0.076281 ]
Rotation matrix:    Rc_ext = [ 0.128482      0.991024      -0.036917
                               0.991056      -0.129661      -0.031550
                               -0.036054      -0.032533      -0.998820 ]
Pixel error:           err = [ 0.15477      0.26929 ]


G = [ 0.128482      0.991024      -0.036917   -15.564192 ;
      0.991056      -0.129661      -0.031550    -31.592337 ;
     -0.036054      -0.032533      -0.998820     447.897520 ;
       0           0              0             1       ];
位置1  末端到机器人基坐标位置

----PulseNO.S 75359,L 48202,U -107236,R -3,B -1,T 1
----CartPos.S 214.629,L 324.508,U -101.991,R 180,B -10.24,T 56.52

a=180/180 *pi;
b=-10.24/180 *pi;
r=56.52/180*pi;
Rx=[1 0 0;0 cos(a) -sin(a);0 sin(a) cos(a)];
Ry=[cos(b) 0 sin(b);0 1 0;-sin(b) 0 cos(b)];
Rz=[cos(r) -sin(r) 0;sin(r) cos(r) 0;0 0 1];
R=Rz*Ry*Rx

R =

    0.5429    0.8341    0.0981
    0.8208   -0.5516    0.1483
    0.1778    0.0000   -0.9841



G =[0.5429    0.8341    0.0981    214.629;
    0.8208   -0.5516    0.1483    324.508;
    0.1778    0.0000   -0.9841    -101.991;
    0           0          0         1    ];
1位置图像外参数

Extrinsic parameters:

Translation vector: Tc_ext = [ -63.060175      -21.839437      500.823439 ]
Rotation vector:   omc_ext = [ 2.391549      1.911963      -0.154389 ]
Rotation matrix:    Rc_ext = [ 0.218178      0.975410      -0.031214
                               0.967780      -0.220369      -0.121813
                               -0.125696      -0.003632      -0.992062 ]
Pixel error:           err = [ 0.11963      0.22145 ]


G = [ 0.218178      0.975410      -0.031214    -63.060175;
     0.967780      -0.220369      -0.121813     -21.839437 ;
    -0.125696      -0.003632      -0.992062      500.823439 ;
      0            0                0              1    ];
由位置0 到位置1
摄像机外参数
Bc1  1右图像外参数   Ac1  0右图像外参数
Bc1  = [ 0.218178      0.975410      -0.031214    -63.060175;
     0.967780      -0.220369      -0.121813     -21.839437 ;
    -0.125696      -0.003632      -0.992062      500.823439 ;
      0            0                0              1    ];

Ac1= [ 0.128482      0.991024      -0.036917   -15.564192 ;
      0.991056      -0.129661      -0.031550    -31.592337 ;
     -0.036054      -0.032533      -0.998820     447.897520 ;
       0           0              0             1       ];
C1=Ac1*inv(Bc1)

工具在基坐标系下

  Ad1  0图像时，工具的姿态；Bd1  1图像时工具的姿态
Ad1=[0.4696    0.8820    0.0399    190.184;
    0.8788   -0.4713    0.0747    355.823;
    0.0847    0.0000   -0.9964    -141.89;
    0           0         0          1   ];

 Bd1=[0.5429    0.8341    0.0981    214.629;
    0.8208   -0.5516    0.1483    324.508;
    0.1778    0.0000   -0.9841    -101.991;
    0           0          0         1    ];

       
   d1=inv(Ad1)*Bd1



结果：


右摄像机在A（0个右图像 ） B（1个右图像）两位置的关系
C1 = [ 0.9958   -0.0896    0.0169   36.8261;
    0.0907    0.9915   -0.0928   42.2616;
   -0.0084    0.0939    0.9955  -49.1724;
         0         0         0    1.0000];
     工具在两位置的关系    
         
d1 =[0.9913   -0.0931    0.0931  -12.6618;
    0.0920    0.9956    0.0166   36.3170;
   -0.0942   -0.0079    0.9956  -41.1195;
         0         0         0    1.0000];


位置2 末端到机器人基坐标位置

----PulseNO.S 86502,L 25258,U -100704,R -3,B -1,T 1
----CartPos.S 153.909,L 328.212,U -70.151,R 180,B -15.1,T 64.87
---Status: 1-cycle

a=180/180 *pi;
b=-15.1/180 *pi;
r=64.87/180*pi;
Rx=[1 0 0;0 cos(a) -sin(a);0 sin(a) cos(a)];
Ry=[cos(b) 0 sin(b);0 1 0;-sin(b) 0 cos(b)];
Rz=[cos(r) -sin(r) 0;sin(r) cos(r) 0;0 0 1];
R=Rz*Ry*Rx

R =

    0.4100    0.9053    0.1106
    0.8741   -0.4247    0.2358
    0.2605    0.0000   -0.9655


G =  [0.4100    0.9053    0.1106   153.909;
    0.8741   -0.4247    0.2358   328.212;
    0.2605    0.0000   -0.9655   -70.151;
    0         0             0       1    ];

位置2图像外参数

Extrinsic parameters:

Translation vector: Tc_ext = [ 14.913514      -27.935731      544.266948 ]
Rotation vector:   omc_ext = [ 2.201095      2.042448      -0.254298 ]
Rotation matrix:    Rc_ext = [ 0.070836      0.996832      -0.036178
                               0.975269      -0.076825      -0.207238
                               -0.209361      -0.020603      -0.977621 ]
Pixel error:           err = [ 0.08458      0.19457 ]

G = [ 0.070836      0.996832      -0.036178    14.913514 ;
     0.975269      -0.076825      -0.207238     -27.935731 ;
     -0.209361      -0.020603      -0.977621     544.266948 ;
      0            0               0               1  ];


由位置1-位置2
摄像机外参数
Bc2  2右图像外参数   Ac2 1右图像外参数
Bc2 =[ 0.070836      0.996832      -0.036178    14.913514 ;
     0.975269      -0.076825      -0.207238     -27.935731 ;
     -0.209361      -0.020603      -0.977621     544.266948 ;
      0            0               0               1  ];


Ac2=[ 0.218178      0.975410      -0.031214    -63.060175;
     0.967780      -0.220369      -0.121813     -21.839437 ;
    -0.125696      -0.003632      -0.992062      500.823439 ;
      0            0                0              1    ];
C2=Ac2*inv(Bc2)

工具在基坐标系下

  Ad2  1图像时，工具的姿态；Bd2  2图像时工具的姿态
Ad2=[0.5429    0.8341    0.0981    214.629;
    0.8208   -0.5516    0.1483    324.508;
    0.1778    0.0000   -0.9841    -101.991;
    0           0          0         1    ];
 Bd2=  [0.4100    0.9053    0.1106   153.909;
    0.8741   -0.4247    0.2358   328.212;
    0.2605    0.0000   -0.9655   -70.151;
    0         0             0       1    ];
       
   d2=inv(Ad2)*Bd2



结果：


右摄像机在A（0个右图像 ） B（1个右图像）两位置的关系
C2 = [0.9889    0.1443   -0.0353  -54.5862;
   -0.1467    0.9860   -0.0790   50.8843;
    0.0234    0.0833    0.9963  -39.4254;
         0         0         0    1.0000];
         
     工具在两位置的关系    
         

d2 =  [0.9863    0.1428    0.0819  -24.2573;
   -0.1403    0.9894   -0.0378  -52.6877;
   -0.0865    0.0258    0.9959  -36.7371;
         0         0         0    1.0000];



标定结果


Rc1=  [ 0.9958   -0.0896    0.0169;   
    0.0907    0.9915   -0.0928 ; 
   -0.0084    0.0939    0.9955  ];
        
       
   
Rd1= [0.9913   -0.0931    0.0931 
    0.0920    0.9956    0.0166   
   -0.0942   -0.0079    0.9956];  

     
Rc2=[0.9889    0.1443   -0.0353  
   -0.1467    0.9860   -0.0790   
    0.0234    0.0833    0.9963 ]

        
Rd2=  [0.9863    0.1428    0.0819 
   -0.1403    0.9894   -0.0378  
   -0.0865    0.0258    0.9959  ];
                
     

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 =[0.0936;
    0.0127;
    0.0904];



 kc2=[ 0.0815;
   -0.0295;
   -0.1462];


 kd1= [-0.0123;
    0.0939;
    0.0928];

 kd2=[0.0320;
    0.0846;
   -0.1422];


kc3=cross(kc1,kc2);
kd3=cross(kd1,kd2);
a=[kc1 kc2 kc3]
b=[kd1 kd2 kd3]
R=a*inv(b)

R=[kc1 kc2 cross(kc1,kc2)]*inv([kd1 kd2 cross(kd1,kd2)])
     最终
     
R =  [  0.0155    0.9835    0.0155;
   -0.9906    0.0131   -0.0077;
   -0.0147   -0.0328    1.0053];


Rc1=  [ 0.9958   -0.0896    0.0169;   
    0.0907    0.9915   -0.0928 ; 
   -0.0084    0.0939    0.9955  ];
       
   
Rd1= [0.9913   -0.0931    0.0931 
    0.0920    0.9956    0.0166   
   -0.0942   -0.0079    0.9956];  

     
Rc2=[0.9889    0.1443   -0.0353  
   -0.1467    0.9860   -0.0790   
    0.0234    0.0833    0.9963 ]

        
Rd2=  [0.9863    0.1428    0.0819 
   -0.1403    0.9894   -0.0378  
   -0.0865    0.0258    0.9959  ];
                
        
tc1=  [36.8261;42.2616;-49.1724];
   

       
td1=   [-12.6618;36.3170;-41.1195];       
 
tc2= [-54.5862;50.8843;-39.4254];  

td2= [-24.2573;-52.6877;-36.7371];

c=R*td1-tc1;
d=R*td2-tc2;
a=Rc1-[1 0 0;0 1 0;0 0 1];
b=Rc2-[1 0 0;0 1 0;0 0 1];
h=[a;b];
y=[c;d];
t=inv(h'*h)*h'*y
  

   t =

   
t =

    7.7295
   84.1694
  312.1403