2015机器人标定实验报告_271407104

1、机器人课程实验报告（二）工业机器人的标定学号班级/专业姓名实验时间小组其他同学名单2013010558机械34班刘攀12.8梅斌、皇甫旗帆、刘猛、孟庆良、孙帆、俞泽明、郝孟轩一.实验目的1 .掌握工业机器人的标定原理和算法；2 .学习测量臂的使用方法；3 .学习和使用Matlab等分析与计算软件；二.实验步骤（按小组进行，分组名单与时间安排和上次实验一样，如时间不便，可与其他同学调换）1 .在各关节转交范围内随机产生10组转角数据，或利用示教器调整机器人位姿并从示教器中读取10组转角数据；102 .编制标定程序，要求迭代计算出的参数误差的最大绝对值小于10；三.实验结果1 .所建立的IRB12

2、0型机器人的坐标系说明（配图说明，要遵守DH规则，并列表说明各关节DH参数的初始值）DH参数初始值:iabi%i0i10290兀/27t227000兀/23700-九/2040302兀/20500兀/27t6072002 .小组利用测量臂测量得到的8组末端位姿数据，以及机器人基础坐标系的测量结果（只写拟合后的结果）末端位姿数据：平面1,376.404023,395.525692,452.428021,-0.922086,-0.274843,-0.272430圆1,377.149896,395.495935,449.933502,-0.922086,-0.274843,-0.272430,25.

3、526532点1,382.736000,380.084105,451.460603,-1.000000,0.000000,0.000000平面2,378.654244,412.341970,438.649950,-0.882541,-0.058837,-0.466541圆2,380.061149,411.044330,436.152204,-0.882541,-0.058837,-0.466541,25.512595点2,380.866162,395.728111,439.052399,-1.000000,0.000000,0.000000平面3,314.593120,495.181332,4

4、01.023014,-0.856495,0.126937,-0.500304圆3,315.246124,493.517700,399.483008,-0.856495,0.126937,-0.500304,25.513644点3,312.922674,478.910874,401.904549,-1.000000,0.000000,0.000000平面4,405.372498,418.653216,591.909495,-0.952325,0.092064,-0.290864圆4,406.013291,417.548586,589.461826,-0.952325,0.092064,-0.29

5、0864,25.491008点4,403.768260,405.159818,597.630716,-1.000000,0.000000,0.000000平面5,570.972407,326.137432,734.238627,-0.936625,-0.349827,0.018815圆5,570.747713,326.576397,731.214907,-0.936625,-0.349827,0.018815,37.490052点5,576.021245,317.140170,743.608146,-1.000000,0.000000,0.000000平面6,667.486931,213.42

6、8828,672.467965,-0.752408,-0.657850,-0.033399圆6,666.178744,215.009914,670.796453,-0.752408,-0.657850,-0.033399,25.494313点6,672.048668,208.813665,682.235459,0.000000,-1.000000,0.000000平面7,646.127752,194.866917,554.888337,-0.865565,-0.085886,-0.493377圆7,646.281059,196.808332,554.281423,-0.865565,-0.08

7、5886,-0.493377,37.470361点7,645.542246,182.762211,560.393353,-1.000000,0.000000,0.000000平面8,490.149203,181.586680,484.493981,-0.281079,-0.953492,-0.108851圆8,489.558947,182.132206,481.239568,-0.281079,-0.953492,-0.108851,25.506748点8,502.793573,179.049005,487.317549,0.000000,-1.000000,0.000000基础坐标系的测量结

8、果：平面1,900.438361,629.078761,9.260903,-0.001065,0.002095,0.999997圆1,808.530494,680.856713,9.054558,-0.001065,0.002095,0.999997,744.044375直线1,535.815441,427.436998,9.295062,0.997962,0.063811,0.000929,592.5288443.列表说明每次迭代计算的结果（参数误差的最大绝对值小于10-10时停止迭代）次数最大误差次数最大误差次数最大误差次数最大误差10.028207.20E-05396.17E-07585

9、.83E-0920.0123215.60E-05404.81E-07594.60E-0930.0054224.36E-05413.75E-07603.63E-0940.0041233.39E-05422.92E-07612.86E-0950.0032242.64E-05432.28E-07622.26E-0960.0024252.05E-05441.77E-07631.78E-0970.0019261.60E-05451.38E-07641.40E-0980.0015271.24E-05461.08E-07651.11E-0990.0011289.67E-06478.40E-08668.73E

10、-10108.91E-04297.53E-06486.55E-08676.89E-10116.92E-04305.86E-06495.11E-08685.43E-10125.38E-04314.56E-06503.98E-08694.28E-10134.18E-04323.55E-06513.11E-08703.38E-10143.25E-04332.77E-06522.42E-08712.66E-10152.53E-04342.15E-06531.91E-08722.10E-10161.97E-04351.68E-06541.51E-08731.66E-10171.53E-04361.31E

11、-06551.19E-08741.31E-10181.19E-04371.02E-06569.38E-09751.03E-10199.25E-05387.92E-07577.40E-09768.11E-11最终结果:iabi%i0i11.5747-3.14460.00002900.000120.00031.550627003-1.5695-0.015170.0001041.5707-0.0065030251.57333.1387006-0.0208-0.036-0.000372计算时间：0.271322秒。迭代次数：76次。4.标定程序代码(要求代码条理清晰，并具有适当的解释与说明)clccl

12、oseall;tick=0;%输入DH初值alpha=pi/20-pi/2pi/2pi/20;thita_0=pipi/200pi0;a=027070000;b=29000302072;%各关节角度变量thita(:,:,1)=-147.89,34.0618,-8.93532,-106.918,21.1661,121.949;thita(:,:,2)=-147.89,34.0618,-8.93505,-72.901,30.6594,102.98;thita(:,:,3)=-159.497,40.9238,-13.0571,-72.9016,30.6594,102.981;thita(:,:,4

13、)=-144.907,29.603,-28.7333,-64.1624,48.659,106.47;thita(:,:,5)=-121.103,29.5278,-53.2854,-64.1618,48.6592,106.47;thita(:,:,6)=-105.71,37.7112,-53.285,-64.1618,42.694,106.469;thita(:,:,7)=-105.709,48.1137,-53.285,-56.1537,79.8642,103.87;thita(:,:,8)=-128.72,53.9517,-44.111,-83.4722,-17.0143,105.482;%

14、将0转换为弧度制fori=1:8thita(:,:,i)=thita(:,:,i)*pi/180;end%定义DH参数的6*4矩阵fori=1:6xx(i,:)=alpha(i)thita_0(i)a(i)b(i);end%O1X1Y1Z1各坐标单位向量在测量臂坐标系下的坐标值zc_0=-0.001065-0.0020950.999997;xc_0=0.9979620.0638110.000929;yc_0=cross(zc_0,xc_0);%生成测量臂到O1X1Y1Z1的齐次变换矩阵Tc_0(:,3)=zc_00;Tc_0(:,1)=xc_00;Tc_0(:,2)=yc_00;Tc_0(:,

15、4)=808.530494680.8567139.0545581;%由测量的圆心坐标得出各次测量的工具坐标系原点在测量臂坐标系下的坐标值%c_*下标代表celiang(测量)即测量臂坐标下各连杆坐标系的坐标值O1=377.149896,395.495935,449.933502;O2=380.061149,411.044330,436.152204;O3=315.246124,493.517700,399.483008;O4=406.013291,417.548586,589.461826;O5=570.747713,326.576397,731.214907;O6=666.178744,21

16、5.009914,670.796453;O7=646.281059,196.808332,554.281423;O8=489.558947,182.132206,481.239568;%由测量的法兰盘平面的法向充当工具坐标系的z轴z1=-0.922086,-0.274843,-0.272430;z2=-0.882541,-0.058837,-0.466541;z3=-0.856495,0.126937,-0.500304;z4=-0.952325,0.092064,-0.290864;z5=-0.936625,-0.349827,0.018815;z6=-0.752408,-0.657850,

17、-0.033399;z7=-0.865565,-0.085886,-0.493377;z8=-0.281079,-0.953492,-0.108851;%由光孔的位置得到y轴的方向，由于光孔不在平面上，需投影，见下文y1=-O1+382.736000,380.084105,451.460603;y2=-O2+380.866162,395.728111,439.052399;y3=-O3+312.922674,478.910874,401.904549;y4=-O4+403.768260,405.159818,597.630716;y5=-O5+576.021245,317.140170,743

18、.608146;y6=-O6+672.048668,208.813665,682.235459;y7=-O7+645.542246,182.762211,560.393353;y8=-O8+502.793573,179.049005,487.317549;%由上述得到的z*和y*得到x*,x*位于测量平面上，再归一化得到单位向量x1=cross(y1,z1)/norm(cross(y1,z1);x2=cross(y2,z2)/norm(cross(y2,z2);x3=cross(y3,z3)/norm(cross(y3,z3);x4=cross(y4,z4)/norm(cross(y4,z4)

19、;x5=cross(y5,z5)/norm(cross(y5,z5);x6=cross(y6,z6)/norm(cross(y6,z6);x7=cross(y7,z7)/norm(cross(y7,z7);x8=cross(y8,z8)/norm(cross(y8,z8);%再由x*和z*得到单位y*,此时y*也落到了测量平面上y1=cross(z1,x1)/norm(cross(z1,x1);y2=cross(z2,x2)/norm(cross(z2,x2);y3=cross(z3,x3)/norm(cross(z3,x3);y4=cross(z4,x4)/norm(cross(z4,x4)

20、;y5=cross(z5,x5)/norm(cross(z5,x5);y6=cross(z6,x6)/norm(cross(z6,x6);y7=cross(z7,x7)/norm(cross(z7,x7);y8=cross(z8,x8)/norm(cross(z8,x8);%生成测量臂坐标系下各次测量得到的工具坐标系的齐次变换矩阵的转置Tc_6(:,:,1)=x10;y10;z10;O11;Tc_6(:,:,2)=x20;y20;z20;O21;Tc_6(:,:,3)=x30;y30;z30;O31;Tc_6(:,:,4)=x40;y40;z40;O41;Tc_6(:,:,5)=x50;y50

21、;z50;O51;Tc_6(:,:,6)=x60;y60;z60;O61;Tc_6(:,:,7)=x70;y70;z70;O71;Tc_6(:,:,8)=x80;y80;z80;O81;%转置后得到齐次变换矩阵fori=1:8Tc_6(:,:,i)=transpose(Tc_6(:,:,i);end%得到基础坐标系下各连杆坐标系的齐次变换矩阵A06,即实际值fori=1:8T0_6(:,:,i)=Tc_0Tc_6(:,:,i);A06(:,:,i)=T0_6(:,:,i);endwhile(1)%由xx矩阵得到B06矩阵，即理论值fori=1:8B01(:,:,i)=cos(xx(1,2)+t

22、hita(:,1,i)-cos(xx(1,1)*sin(xx(1,2)+thita(:,1,i)sin(xx(1,1)*sin(xx(1,2)+thita(:,1,i)xx(1,3)*cos(xx(1,2)+thita(:,1,i);sin(xx(1,2)+thita(:,1,i)cos(xx(1,1)*cos(xx(1,2)+thita(:,1,i)- sin(xx(1,1)*cos(xx(1,2)+thita(:,1,i)xx(1,3)*sin(xx(1,2)+thita(:,1,i);0sin(xx(1,1)cos(xx(1,1)xx(1,4);0001;B12(:,:,i)=cos(x

23、x(2,2)+thita(:,2,i)-cos(xx(2,1)*sin(xx(2,2)+thita(:,2,i)sin(xx(2,1)*sin(xx(2,2)+thita(:,2,i)xx(2,3)*cos(xx(2,2)+thita(:,2,i);sin(xx(2,2)+thita(:,2,i)cos(xx(2,1)*cos(xx(2,2)+thita(:,2,i)- sin(xx(2,1)*cos(xx(2,2)+thita(:,2,i)xx(2,3)*sin(xx(2,2)+thita(:,2,i);0sin(xx(2,1)cos(xx(2,1)xx(2,4);0001;B23(:,:,

24、i)=cos(xx(3,2)+thita(:,3,i)-cos(xx(3,1)*sin(xx(3,2)+thita(:,3,i)sin(xx(3,1)*sin(xx(3,2)+thita(:,3,i)xx(3,3)*cos(xx(3,2)+thita(:,3,i);sin(xx(3,2)+thita(:,3,i)cos(xx(3,1)*cos(xx(3,2)+thita(:,3,i)- sin(xx(3,1)*cos(xx(3,2)+thita(:,3,i)xx(3,3)*sin(xx(3,2)+thita(:,3,i);0sin(xx(3,1)cos(xx(3,1)xx(3,4);0001;

25、B34(:,:,i)=cos(xx(4,2)+thita(:,4,i)-cos(xx(4,1)*sin(xx(4,2)+thita(:,4,i)sin(xx(4,1)*sin(xx(4,2)+thita(:,4,i)xx(4,3)*cos(xx(4,2)+thita(:,4,i);sin(xx(4,2)+thita(:,4,i)cos(xx(4,1)*cos(xx(4,2)+thita(:,4,i)- sin(xx(4,1)*cos(xx(4,2)+thita(:,4,i)xx(4,3)*sin(xx(4,2)+thita(:,4,i);0sin(xx(4,1)cos(xx(4,1)xx(4,

26、4);0001;B45(:,:,i)=cos(xx(5,2)+thita(:,5,i)-cos(xx(5,1)*sin(xx(5,2)+thita(:,5,i)sin(xx(5,1)*sin(xx(5,2)+thita(:,5,i)xx(5,3)*cos(xx(5,2)+thita(:,5,i);sin(xx(5,2)+thita(:,5,i)cos(xx(5,1)*cos(xx(5,2)+thita(:,5,i)- sin(xx(5,1)*cos(xx(5,2)+thita(:,5,i)xx(5,3)*sin(xx(5,2)+thita(:,5,i);0sin(xx(5,1)cos(xx(5

27、,1)xx(5,4);0001;B56(:,:,i)=cos(xx(6,2)+thita(:,6,i)-cos(xx(6,1)*sin(xx(6,2)+thita(:,6,i)sin(xx(6,1)*sin(xx(6,2)+thita(:,6,i)xx(6,3)*cos(xx(6,2)+thita(:,6,i);sin(xx(6,2)+thita(:,6,i)cos(xx(6,1)*cos(xx(6,2)+thita(:,6,i)- sin(xx(6,1)*cos(xx(6,2)+thita(:,6,i)xx(6,3)*sin(xx(6,2)+thita(:,6,i);0sin(xx(6,1)

28、cos(xx(6,1)xx(6,4);0001;B06(:,:,i)=B01(:,:,i)*B12(:,:,i)*B23(:,:,i)*B34(:,:,i)*B45(:,:,i)*B56(:,:,i);end%计算得到伴随矩阵和Gi,从而写出由伴随矩阵和Gi矩阵组成的系数矩阵fori=1:8B46(:,:,i)=B45(:,:,i)*B56(:,:,i);B36(:,:,i)=B34(:,:,i)*B46(:,:,i);B26(:,:,i)=B23(:,:,i)*B36(:,:,i);B16(:,:,i)=B12(:,:,i)*B26(:,:,i);p16(:,:,i)=B16(1,4,i);

29、B16(2,4,i);B16(3,4,i);p26(:,:,i)=B26(1,4,i);B26(2,4,i);B26(3,4,i);p36(:,:,i)=B36(1,4,i);B36(2,4,i);B36(3,4,i);p46(:,:,i)=B46(1,4,i);B46(2,4,i);B46(3,4,i);p56(:,:,i)=B56(1,4,i);B56(2,4,i);B56(3,4,i);pA16(:,:,i)=0-p16(3,1,i)p16(2,1,i);p16(3,1,i)0-p16(1,1,i);-p16(2,1,i)p16(1,1,i)0;pA26(:,:,i)=0-p26(3,1

30、,i)p26(2,1,i);p26(3,1,i)0-p26(1,1,i);-p26(2,1,i)p26(1,1,i)0;pA36(:,:,i)=0-p36(3,1,i)p36(2,1,i);p36(3,1,i)0-p36(1,1,i);-p36(2,1,i)p36(1,1,i)0;pA46(:,:,i)=0-p46(3,1,i)p46(2,1,i);p46(3,1,i)0-p46(1,1,i);-p46(2,1,i)p46(1,1,i)0;pA56(:,:,i)=0-p56(3,1,i)p56(2,1,i);p56(3,1,i)0-p56(1,1,i);-p56(2,1,i)p56(1,1,i

31、)0;R16(:,:,i)=B16(1,1,i)B16(1,2,i)B16(1,3,i);B16(2,1,i)B16(2,2,i)B16(2,3,i);B16(3,1,i)B16(3,2,i)B16(3,3,i);R26(:,:,i)=B26(1,1,i)B26(1,2,i)B26(1,3,i);B26(2,1,i)B26(2,2,i)B26(2,3,i);B26(3,1,i)B26(3,2,i)B26(3,3,i);R36(:,:,i)=B36(1,1,i)B36(1,2,i)B36(1,3,i);B36(2,1,i)B36(2,2,i)B36(2,3,i);B36(3,1,i)B36(3,

32、2,i)B36(3,3,i);R46(:,:,i)=B46(1,1,i)B46(1,2,i)B46(1,3,i);B46(2,1,i)B46(2,2,i)B46(2,3,i);B46(3,1,i)B46(3,2,i)B46(3,3,i);R56(:,:,i)=B56(1,1,i)B56(1,2,i)B56(1,3,i);B56(2,1,i)B56(2,2,i)B56(2,3,i);B56(3,1,i)B56(3,2,i)B56(3,3,i);R16_T(:,:,i)=transpose(R16(:,:,i);R26_T(:,:,i)=transpose(R26(:,:,i);R36_T(:,:

33、,i)=transpose(R36(:,:,i);R46_T(:,:,i)=transpose(R46(:,:,i);R56_T(:,:,i)=transpose(R56(:,:,i);Ad_B16_2(:,:,i)=-R16_T(:,:,i)*pA16(:,:,i);Ad_B26_2(:,:,i)=-R26_T(:,:,i)*pA26(:,:,i);Ad_B36_2(:,:,i)=-R36_T(:,:,i)*pA36(:,:,i);Ad_B46_2(:,:,i)=-R46_T(:,:,i)*pA46(:,:,i);Ad_B56_2(:,:,i)=-R56_T(:,:,i)*pA56(:,:,

34、i);Ad_B16(:,:,i)=R16_T(:,:,i)Ad_B16_2(:,:,i);zeros(3,3)R16_T(:,:,i);Ad_B26(:,:,i)=R26_T(:,:,i)Ad_B26_2(:,:,i);zeros(3,3)R26_T(:,:,i);Ad_B36(:,:,i)=R36_T(:,:,i)Ad_B36_2(:,:,i);zeros(3,3)R36_T(:,:,i);Ad_B46(:,:,i)=R46_T(:,:,i)Ad_B46_2(:,:,i);zeros(3,3)R46_T(:,:,i);Ad_B56(:,:,i)=R56_T(:,:,i)Ad_B56_2(:,

35、:,i);zeros(3,3)R56_T(:,:,i);Ad_B66(:,:,i)=diag(6);G1(:,:,i)=0010;0xx(1,3)*cos(xx(1,1)0sin(xx(1,1);0-xx(1,3)*sin(xx(1,1)0cos(xx(1,1);1000;0sin(xx(1,1)00;0cos(xx(1,1)00;G2(:,:,i)=0010;0xx(2,3)*cos(xx(2,1)0sin(xx(2,1);0-xx(2,3)*sin(xx(2,1)0cos(xx(2,1);1000;0sin(xx(2,1)00;0cos(xx(2,1)00;G3(:,:,i)=0010;0

36、xx(3,3)*cos(xx(3,1)0sin(xx(3,1);0-xx(3,3)*sin(xx(3,1)0cos(xx(3,1);1000;0sin(xx(3,1)00;0cos(xx(3,1)00;G4(:,:,i)=0010;0xx(4,3)*cos(xx(4,1)0sin(xx(4,1);0-xx(4,3)*sin(xx(4,1)0cos(xx(4,1);1000;0sin(xx(4,1)00;0cos(xx(4,1)00;G5(:,:,i)=0010;0xx(5,3)*cos(xx(5,1)0sin(xx(5,1);0-xx(5,3)*sin(xx(5,1)0cos(xx(5,1);

37、1000;0sin(xx(5,1)00;0cos(xx(5,1)00;G6(:,:,i)=0010;0xx(6,3)*cos(xx(6,1)0sin(xx(6,1);0-xx(6,3)*sin(xx(6,1)0cos(xx(6,1);1000;0sin(xx(6,1)00;0cos(xx(6,1)00;Ad_G1(:,:,i)=Ad_B16(:,:,i)*G1(:,:,i);Ad_G2(:,:,i)=Ad_B26(:,:,i)*G2(:,:,i);Ad_G3(:,:,i)=Ad_B36(:,:,i)*G3(:,:,i);Ad_G4(:,:,i)=Ad_B46(:,:,i)*G4(:,:,i);Ad_G5(:,:,i)=Ad_B56(:,:,i)*G5(:,:,i);Ad_G6(:,:,i)=Ad_B66(:,:,i)*G6(:,:,i);%计算末端姿态误差EE(:,:,i)=B06(:,:,i)(A06(:,:,i)-B06(:,:,i);RR(:,:,i)=EE(1:3,1:3,i);fai(:,:,i)=acos(trace(RR(:,:,i)