《机器人技术与应用》D.docx

《机器人技术与应用》D-H参数标定实验程序 clear; close all;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);for j = 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(alpha i),0]);% Ad矩阵Ad = @(R,P)([R', -R'*P;zeros(3,3), R']);%实时显示收敛情况semilogy(0,1);xlabel( 'Iteration N');ylabel('DH error');grid on;hold on;count=0;%进行迭代while (1)N = N + 1;for j = 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);p2 7 =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,dZ RR = 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,dThetaZ enddDH_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( 'Iteration N = ',num2str(N)]);disp( 'Current DH error = ',num2str(error)]);disp 'DH parameters are:');disp' a_i b_i alpha_i thetaL/'disp(DH_nominal + dDH); DH_2=DH_nominal + dDH; DH_2(:,3)= DH_2(:,3)*180/pi;DH_2(:,4)= DH_2(:,4)*180/pi;%判断DH参数误差是否小于阈值if (error < EP) break;elsedDH = dDH + dDH_delta; end% if(N>12)% break;% enddDHenddisp('The iteration is over!');toc(tl);。