puma560的运动学及matlab实现(正解+逆解) | 您所在的位置:网站首页 › 什么是puma机器人 › puma560的运动学及matlab实现(正解+逆解) |
表1 PUMA560机器人的连杆参数 关节i
变化范围/(o) 1 90 0 0 0 -160~160 2 0 -90 0 149.09 -225~45 3 -90 0 431.8 0 -45~225 4 0 -90 20.32 443.07 -110~170 5 0 90 0 0 -100~100 6 0 -90 0 0 -266~266
正解源码 DEG = pi/180; cta1=-70.4385 cta2=182.6918 cta3=-90.0000 cta4=-82.4708 cta5=-19.7387 cta6=-97.9933 T01=[cosd(cta1),-sind(cta1),0,0; sind(cta1), cosd(cta1),0,0; 0,0,1,0; 0,0,0,1];
T02=T01*[cosd(cta2),-sind(cta2),0,0; 0,0,1, 149.09; -sind(cta2),-cosd(cta2),0,0; 0,0,0,1] ;
T03=T02*[cosd(cta3),-sind(cta3),0,431.8; sind(cta3), cosd(cta3),0,0; 0,0,1,0; 0,0,0,1]; T04=T03*[cosd(cta4),-sind(cta4),0,20.32; 0,0,1,433.07; -sind(cta4),-cosd(cta4),0,0; 0,0,0,1]; T05=T04*[cosd(cta5),-sind(cta5),0,0; 0,0,-1,0; sind(cta5), cosd(cta5), 0,0; 0,0,0,1]; T06=T05*[cosd(cta6),-sind(cta6),0,0; 0,0,1,0; -sind(cta6),-cosd(cta6),0,0; 0,0,0,1]; O=T06*[0;0;0;1]; ===================================================== 逆解源码 fid = fopen('inverseout.txt','w');%逆解的保存文件 %赋初值 T06 =[0.0000 1.0000 0.0000 -149.0900; 0.0000 -0.0000 1.0000 864.8700; 1.0000 0 -0.0000 20.3200; 0 0 0 1.0000] ; a0=0; a1=0; a2=431.8; a3=20.32; a4=0; a5=0; d1=0; d2=149.09; d3=0; d4=433.07; d5=0; d6=0; n_x=T06(1); n_y=T06(2); n_z=T06(3); o_x=T06(5); o_y=T06(6); o_z=T06(7); a_x=T06(9); a_y=T06(10); a_z=T06(11); p_x=T06(13); p_y=T06(14); p_z=T06(15); disp(['八组解分别是:']); for i=1:2 for j=1:2 for k=1:2 %求解theta1(为弧度) sqr1=[sqrt(p_x^2+p_y^2-d2^2),-sqrt(p_x^2+p_y^2-d2^2)]; ta1=atan2(p_y,p_x)-atan2(d2,sqr1(i)); %求解theta3(弧度表示) k1=(p_x^2+p_y^2+p_z^2-a2^2-a3^2-d2^2-d4^2)/(2*a2); sqr3=[sqrt(a3^2+d4^2-k1^2),-sqrt(a3^2+d4^2-k1^2) ]; ta3=atan2(a3,d4)-atan2(k1,sqr3(j)); fs23=-((a3+a2*cos(ta3))*p_z)+(cos(ta1)*p_x+sin(ta1)*p_y)*(a2*sin(ta3)-d4); sc23=(-d4+a2*sin(ta3))*p_z+(cos(ta1)*p_x+sin(ta1)*p_y)*(a2*cos(ta3)+a3); ta23=atan2( fs23,sc23); %求解theta2 (弧度表示) ta2=ta23-ta3; %求解theta4 (弧度表示) fs4=[ -a_x*sin(ta1)+a_y*cos(ta1),a_x*sin(ta1)-a_y*cos(ta1)]; sc4=[ -a_x*cos(ta1)*cos(ta23)-a_y*sin(ta1)*cos(ta23)+a_z*sin(ta23), a_x*cos(ta1)*cos(ta23)+a_y*sin(ta1)*cos(ta23)-a_z*sin(ta23)]; fprintf(fid,'%d,',sc4(1,1)); fprintf(fid,'\t'); fprintf(fid,'%d,',sc4(2,1)); fprintf(fid,'\t'); fprintf(fid,'%d,',fs4(1,1)); fprintf(fid,'\t'); fprintf(fid,'%d,',fs4(1,2)); fprintf(fid,'\t'); fprintf(fid,'\n'); ta4=atan2(fs4(k),sc4(k)); %求解theta5 (弧度表示) fs5=-a_x*(cos(ta1)*cos(ta23)*cos(ta4)+sin(ta1)*sin(ta4))... -a_y*(sin(ta1)*cos(ta23)*cos(ta4)-cos(ta1)*sin(ta4))... +a_z*(sin(ta23)*cos(ta4)); sc5=a_x*(-cos(ta1)*sin(ta23))+a_y*(-sin(ta1)*sin(ta23))+a_z*(-cos(ta23)); ta5=atan2(fs5,sc5); %求解theta6 (弧度表示) fs6=-n_x*(cos(ta1)*cos(ta23)*sin(ta4)-sin(ta1)*cos(ta4))... -n_y*(sin(ta1)*cos(ta23)*sin(ta4)+cos(ta1)*cos(ta4))... +n_z*(sin(ta23)*sin(ta4)); sc6= n_x*(cos(ta1)*cos(ta23)*cos(ta4)+sin(ta1)*sin(ta4))*cos(ta5)... -n_x*cos(ta1)*sin(ta23)*sin(ta5)... +n_y*(sin(ta1)*cos(ta23)*cos(ta4)+cos(ta1)*sin(ta4))*cos(ta5)... -n_y*sin(ta1)*sin(ta23)*sin(ta5)... -n_z*(sin(ta23)*cos(ta4)*cos(ta5)+cos(ta23)*sin(ta5)); ta6=atan2(fs6,sc6); %save %将其化为角度 Theta=[ta1 ta2 ta3 ta4 ta5 ta6]./pi*180 end end end
关于C++版本的运动学正解和逆解的代码,可以在以下链接下载 https://download.csdn.net/download/kobesdu/12099712
|
CopyRight 2018-2019 实验室设备网 版权所有 |