《机器人技术与应用》D

1、% 机器人技术与应用 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(isn

2、an(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:DataSize circleEE(j,:) = EEvalue.data(3*j-2,1:6); planeEE(j,:) = EEvalue

3、.data(3*j-1,1:6);pointEE(j,:) = EEvalue.data(3*j,1:6);end% 测量坐标系下，机器人基坐标系 Tm_b pm_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

4、,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), a_i*cos(theta_i);sin(alpha_i)*sin(theta_i),-cos(alph

5、a_i)*sin(theta_i), a_i*cos(theta_i);sin(alpha_i)*sin(theta_i),sin(theta_i), cos(alpha_i)*cos(theta_i),-sin(alpha_i)*cos(thea_i),a_i*sin(theta_i);-sin(alpha_i)*cos(thea_i),a_i*sin(theta_i);0,sin(alpha_i),cos(alpha_i),b_i;0,0,1;);% 关联矩阵通式G_i = (a_i,alpha_i)(0,1,0,0;0,0,a_i*cos(alpha_i),sin(alpha_i);0,

6、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 矩阵0,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) + EE

7、thetaValue(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

8、(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=T

9、27(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);R

10、57 = 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);

11、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_

12、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) /

13、(2 * sin(fai); e(6*j-2:6*j) = dTheta(3,2);dTheta(1,3);dTheta(2,1);% dThetaX,dThetaY,dThetaZend dDH_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