《机器人技术与应用》D

%《机器人技术与应用》D-H参数标定实验程序clear;closeall;t1=tic;%读取数据EEvalue=importdata('./MeasureData/EndValue.txt');EEbasematrix=importdata('./MeasureData/BaseValue.txt');EEthetaValue=importdata('./MeasureData/AngleValue.txt');%去除含有NaN的行EEvalue.data(any(isnan(EEvalue.data),2),:)=[];EEbasematrix.data(any(isnan(EEbasematrix.data),2),:)=[];%theta化为孤度EEthetaValue=EEthetaValue火(pi/180);%进行平面，圆，点的分裂处理DataSize=size(EEthetaValue,1);planeEE=zeros(DataSize,6);circleEE=zeros(DataSize,6);pointEE=zeros(DataSize,6);forj=1:DataSizecircleEE(j,:)=EEvalue.data(3火j-2,1:6);planeEE(j,:)=EEvalue.data(3火j-1,1:6);pointEE(j,:)=EEvalue.data(3火j,1:6);end%测量坐标系下，机器人基坐标系Tm_bpm_b=EEbasematrix.data(2,1:3)';zm_b=EEbasematrix.data(1,4:6)';ym_b=EEbasematrix.data(3,4:6)';%因为不知道Y轴是怎么测的。所以需要尝试。xm_b=cross(ym_b,zm_b);Tm_b=[xm_b,ym_b,zm_b,pm_b;0, 0,0, 1];DH_nominal=[0,338, -pi/2, 0;0,0, pi/2, 0;0,420, -pi/2, 0;0,0, pi/2, 0;0,380, -pi/2,0;0,0, pi/2, 0;0,160, 0, 0];dDHzeros(7,4);e=zeros(DataSize*6,1);J=zeros(DataSize*6,28);%系数矩阵EP=1e-10;%迭代误差阈值N=0;%迭代次数%D-H参数矩阵通式T_i=@(a_i,b_i,alpha_i,theta_i)([cos(theta_i),-cos(alpha_i)*sin(theta_i),sin(alpha_i)*sin(theta_i),a_i*cos(theta_i);sin(theta_i),cos(alpha_i)*cos(theta_i),-sin(alpha_i)*cos(theta_i),a_i*sin(theta_i);0,sin(alpha_i),cos(alpha_i), b_i;0,0, 0,1;]);%关联矩阵通式G_i=@(a_i,alpha_i)([0,1,0,0;0,0,a_i*cos(alpha__i),sin(alpha_i);0,0,--a_i*sin(alpha__i),cos(alpha_i);1,0,0,0;0,0,sin(alpha_i),0;0,0,cos(alphai),0]);%Ad矩阵Ad=@(R,P)([R', -R'*P;zeros(3,3),R']);%实时显示收敛情况semilogy(0,1);xlabel('IterationN');ylabel('DHerror');gridon;holdon;count=0;%进行迭代while(1)N=N+1;forj=1:DataSizeDH=DH_nominal+dDH;DH(:,4)=DH(:,4)+EEthetaValue(j,:)';

%先求系数矩阵JT01=T_i(DH(1,1),DH(1,2),DH(1,3),DH(1,4));T12=T_i(DH(2,1),DH(2,2),DH(2,3),DH(2,4));T23=T_i(DH(3,1),DH(3,2),DH(3,3),DH(3,4));T34=T_i(DH(4,1),DH(4,2),DH(4,3),DH(4,4));T45=T_i(DH(5,1),DH(5,2),DH(5,3),DH(5,4));T56=T_i(DH(6,1),DH(6,2),DH(6,3),DH(6,4));T67=T_i(DH(7,1),DH(7,2),DH(7,3),DH(7,4));T07=T01火T12火T23火T34火T45火T56火T67;T17=T12火T23火T34火T45火T56火T67;T27=T23火T34火T45火T56火T67;T37=T34火T45火T56火T67;T47=T45火T56火T67;T57=T56火T67;R17=T17(1:3,1:3);p17=T17(1:3,4);P17=[0, -p17(3),p17(2);p17(3),0, -p17(1);-p17(2),p17(1),0];Ad1=Ad(R17,P17);R27=T27(1:3,1:3);p27=T27(1:3,4);P27=[0, -p27(3),p27(2);p27(3),0, -p27(1);-p27(2),p27(1),0];Ad2=Ad(R27,P27);R37=T37(1:3,1:3);p37=T37(1:3,4);P37=[0, -p37(3),p37(2);p37(3),0, -p37(1);-p37(2),p37(1),0];Ad3=Ad(R37,P37);R47=T47(1:3,1:3);p47=T47(1:3,4);P47=[0, -p47(3),p47(2);p47(3),0, -p47(1);-p47(2),p47(1),0]；Ad4=Ad(R47,P47);R57=T57(1:3,1:3);p57=T57(1:3,4);P57=[0, -p57(3),p57(2);p57(3),0, -p57(1);-p57(2),p57(1),0];Ad5=Ad(R57,P57);R67=T67(1:3,1:3);p67=T67(1:3,4);P67=[0, -p67(3),p67(2);p67(3),0, -p67(1);-p67(2),p67(1),0];Ad6=Ad(R67,P67);Ad7=eye(6);G1=G_i(DH(1,1),DH(1,3));G2=G_i(DH(2,1),DH(2,3));G3=G_i(DH(3,1),DH(3,3));G4=G_i(DH(4,1),DH(4,3));G5=G_i(DH(5,1),DH(5,3));G6=G_i(DH(6,1),DH(6,3));G7=G_i(DH(7,1),DH(7,3));J(6火j-5:6火j,:)=[Ad1*G1,Ad2火G2,Ad3火G3,Ad4火G4,Ad5火G5,Ad6火G6,Ad7火G7];%然后求整体误差e%测量坐标系下，机器人末端坐标系Tm_ezm_e=planeEE(j,4:6)';pm_e=circleEE(j,1:3)';xm_e=pointEE(j,1:3)'-pm_e;xm_e=xm_e/norm(xm_e);ym_e=cross(zm_e,xm_e);Tm_e=[xm_e,ym_e,zm_e,pm_e;0, 0, 0, 1];%机器人基础坐标系下，末端坐标系count=count+1;Tb_e=Tm_b\Tm_e;AAA=T07-Tb_e;dT_N=T07\Tb_e-eye(4);e(6火j-5:6火j-3)=dT_N(1:3,4);%dX,dY,dZRR=dT_N(1:3,1:3)+eye(3);fai=acos((trace(RR)-1)/2);dTheta=(fai火(RR-RR'))/(2火sin(fai));e(6火j-2:6火j)=[dTheta(3,2);dTheta(1,3);dTheta(2,1)];%dThetaX,dThetaY,dThetaZenddDH_delta=(J'*J)\J'*e;%dDH_delta=pinv(J)*e;error=max(abs(dDH_delta));dDH_delta=reshape(dDH_delta,[4,7])';dDH_delta=[dDH_delta(:,2),dDH_delta(:,4),dDH_delta(:,1),dDH_delta(:,3)];%实时显示收敛情况plot(N,error'ro');drawnow;disp('IterationN=',num2str(N)]);disp('CurrentDHerror=',num2str(error)]);disp'DHparametersare:');disp'a_i b_ialpha_i thetaL/'disp(DH_nominal+dDH);