机器人正逆运动学分析（ABB-IRB2600)+matlab代码

function [ikine_t]=mymodikine(Tbe) % ti di ai-1 alphai-1 MDH=[0 0.445 0 0; pi/2 0 0.15 -pi/2; 0 0 -0.7 0; 0 0.795 -0.115 pi/2; 0 0 0 -pi/2; 0 0.085 0 pi/2]; nx=Tbe(1,1); ny=Tbe(2,1); nz=Tbe(3,1); ox=Tbe(1,2); oy=Tbe(2,2); oz=Tbe(3,2); ax=Tbe(1,3); ay=Tbe(2,3); az=Tbe(3,3); px=Tbe(1,4); py=Tbe(2,4); pz=Tbe(3,4); d1=0.445; d4=0.795; d6=0.085; a1=0.15; a2=-0.7; a3=-0.115; %t1(3,4) t11=-atan2(d6*ay-py,d6*ax-px); t12=-atan2(d6*ay-py,px-d6*ax); %t2 k11=a1-(py-d6*ay)/sin(t11); k12=a1-(py-d6*ay)/sin(t12); k2=d1-(pz-az*d6); t21=-(asin((k11*k11+k2*k2+a2*a2-a3*a3-d4*d4)/(2*a2*(k11*k11+k2*k2)^0.5))-asin(k2/(k11*k11+k2*k2)^0.5)); t22=-(asin((k12*k12+k2*k2+a2*a2-a3*a3-d4*d4)/(2*a2*(k12*k12+k2*k2)^0.5))-asin(k2/(k12*k12+k2*k2)^0.5)); t23=-(-asin((k11*k11+k2*k2+a2*a2-a3*a3-d4*d4)/(2*a2*(k11*k11+k2*k2)^0.5))-asin(k2/(k11*k11+k2*k2)^0.5)); t24=-(-asin((k12*k12+k2*k2+a2*a2-a3*a3-d4*d4)/(2*a2*(k12*k12+k2*k2)^0.5))-asin(k2/(k12*k12+k2*k2)^0.5)); %t3 A1=k11-a2*sin(t21); A2=k12-a2*sin(t22); A3=k11-a2*sin(t23); A4=k12-a2*sin(t24); B1=k2-a2*cos(t21); B2=k2-a2*cos(t22); B3=k2-a2*cos(t23); B4=k2-a2*cos(t24); q11=asin((d4*B1+a3*A1)/(a3*a3+d4*d4)); q12=asin((d4*B2+a3*A2)/(a3*a3+d4*d4)); q13=asin((d4*B3+a3*A3)/(a3*a3+d4*d4)); q14=asin((d4*B4+a3*A4)/(a3*a3+d4*d4)); t31=q11-t21; t32=q12-t22; t33=q13-t23; t34=q14-t24; %t5 t51=acos(ax*cos(q11)*cos(t11)+ay*cos(q11)*sin(t11)-az*sin(q11)); t52=acos(ax*cos(q12)*cos(t12)+ay*cos(q12)*sin(t12)-az*sin(q12)); t53=acos(ax*cos(q13)*cos(t11)+ay*cos(q13)*sin(t11)-az*sin(q13)); t54=acos(ax*cos(q14)*cos(t12)+ay*cos(q14)*sin(t12)-az*sin(q14)); %t55=atan2(((ay*cos(t12)-ax*sin(t12))^2+(m5_3*cos(t33)+n5_3*sin(t33))^2)^0.5,(m5_3*sin(t33)-n5_3*cos(t33))/(sin(f3)*sin(f4))); %t56=atan2(-((ay*cos(t12)-ax*sin(t12))^2+(m5_3*cos(t33)+n5_3*sin(t33))^2)^0.5,(m5_3*sin(t33)-n5_3*cos(t33))/(sin(f3)*sin(f4))); %t57=atan2(((ay*cos(t12)-ax*sin(t12))^2+(m5_4*cos(t34)+n5_4*sin(t34))^2)^0.5,(m5_4*sin(t34)-n5_4*cos(t34))/(sin(f3)*sin(f4))); %t58=atan2(-((ay*cos(t12)-ax*sin(t12))^2+(m5_4*cos(t34)+n5_4*sin(t34))^2)^0.5,(m5_4*sin(t34)-n5_4*cos(t34))/(sin(f3)*sin(f4))); %t4 if sin(t51)==0 t41=0; else t41=asin((ay*cos(t11)-ax*sin(t11))/sin(t51)); end if sin(t52)==0 t42=0; else t42=asin((ay*cos(t12)-ax*sin(t12))/sin(t52)); end if sin(t53)==0 t43=0; else t43=asin((ay*cos(t11)-ax*sin(t11))/sin(t53)); end if sin(t54)==0 t44=0; else t44=asin((ay*cos(t12)-ax*sin(t12))/sin(t54)); end if sin(t51)==0 t45=0; else t45=-asin((ay*cos(t11)-ax*sin(t11))/sin(t51)); end if sin(t52)==0 t46=0; else t46=-asin((ay*cos(t12)-ax*sin(t12))/sin(t52)); end if sin(t53)==0 t47=0; else t47=-asin((ay*cos(t11)-ax*sin(t11))/sin(t53)); end if sin(t54)==0 t48=0; else t48=-asin((ay*cos(t12)-ax*sin(t12))/sin(t54)); end %t61=-atan2(ox*cos(q11)*cos(t11)+oy*cos(q11)*sin(t11)-oz*sin(q11),(nz*sin(q11)-nx*cos(q11)*cos(t11)-ny*cos(q11)*sin(t11))); t62=atan2(ox*cos(q12)*cos(t12)+oy*cos(q12)*sin(t12)-oz*sin(q12),(nz*sin(q12)-nx*cos(q12)*cos(t12)-ny*cos(q12)*sin(t12))); %t63=-atan2(ox*cos(q13)*cos(t11)+oy*cos(q13)*sin(t11)-oz*sin(q13),(nz*sin(q13)-nx*cos(q13)*cos(t11)-ny*cos(q13)*sin(t11))); %t64=-atan2(ox*cos(q14)*cos(t12)+oy*cos(q14)*sin(t12)-oz*sin(q14),(nz*sin(q14)-nx*cos(q14)*cos(t12)-ny*cos(q14)*sin(t12))); %t65=-atan2(ox*cos(q11)*cos(t11)+oy*cos(q11)*sin(t11)-oz*sin(q11),-(nz*sin(q11)-nx*cos(q11)*cos(t11)-ny*cos(q11)*sin(t11))); %t66=-atan2(ox*cos(q12)*cos(t12)+oy*cos(q12)*sin(t12)-oz*sin(q12),-(nz*sin(q12)-nx*cos(q12)*cos(t12)-ny*cos(q12)*sin(t12))); %t67=-atan2(ox*cos(q13)*cos(t11)+oy*cos(q13)*sin(t11)-oz*sin(q13),-(nz*sin(q13)-nx*cos(q13)*cos(t11)-ny*cos(q13)*sin(t11))); %t68=-atan2(ox*cos(q14)*cos(t12)+oy*cos(q14)*sin(t12)-oz*sin(q14),-(nz*sin(q14)-nx*cos(q14)*cos(t12)-ny*cos(q14)*sin(t12))); ikine_t=[; t12 t22 t32 t42 t52 t62; ]; % theta=[120*pi/180,30*pi/180,10*pi/180,2.2*pi/180,34.55*pi/180,131.96*pi/180];[T06]=mymodfkine(theta) % % theta=[120*pi/180,-50.13*pi/180,87.5*pi/180,2.06*pi/180,37.18*pi/180,132.128*pi/180];[T06]=mymodfkine(theta) % % theta=[60*pi/180,-50.13*pi/180,87.5*pi/180,19.15*pi/180,47.49*pi/180,61.225*pi/180];[T06]=mymodfkine(theta)