机器人手眼标定

 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



  

转载于:https://www.cnblogs.com/wqj1212/archive/2007/12/19/1006387.html