机器人手眼标定

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 =

7.7295

84.1694

312.1403