delta机械手运动学反解代码.docx

%delta机械手运动学正解代码syms x y zLa=0.2;Lb=0.6;R=0.348;r= 0.1068;o1=input(请输入弧度制角o1=);o2=input(请输入弧度制角o1=);o3=input(请输入弧度制角o3=);eq1=(R-r+La*cos(o1)-x)2+y2+(La*sin(o1)+z)2-Lb2;eq2=(-1/2*(R-r+La*cos(o2)-x)2+(1/2*sqrt(3)*(R-r+La*cos(o2)-y)2+(La*sin(o2)+z)2-Lb2;eq3=(-1/2*(R-r+La*cos(o3)-x)2+(-1/2*sqrt(3)*(R-r+La*cos(o3)-y)2+(La*sin(o3)+z)2-Lb2;x,y,z=solve(eq1,eq2,eq3);x=vpa(x,5);y=vpa(y,5);z=vpa(z,5);for i=1:2 z(i); if z(i)0 ans=x(i) y(i) z(i); else disp() endendans=vpa(ans,5);if ans(1).2+ans(2).2+ans(3).210000000 ans=ans(1) ans(2) ans(3);else disp(no answer!)endans=vpa(ans,5)%注解：坐标位置的控制是精度控制之一 %运动学反解代码syms A1 A2 A3 B C1 C2 C3R=0.348;r=0.1068;La=0.200;Lb=0.600;x=input(请输入分量坐标x=);y=input(请输入分量运动y=);z=input(请输入分量运动z=);if z=100000 disp(The program has not found solution!)elseA1=(La-R+r)2+2*(x*cos(0)+y*sin(0)*(La-R+r)+(x2+y2+z2-Lb2);A2=(La-R+r)2+2*(x*cos(2*pi/3)+y*sin(2*pi/3)*(La-R+r)+(x2+y2+z2-Lb2);A3=(La-R+r)2+2*(x*cos(4*pi/3)+y*sin(4*pi/3)*(La-R+r)+(x2+y2+z2-Lb2);B=4*La*z;C1=(La+R-r)2-2*(La+R-r)2*(x*cos(0)+y*sin(0)+(x2+y2+z2-Lb2);C2=(La+R-r)2-2*(La+R-r)2*(x*cos(2*pi/3)+y*sin(2*pi/3)+(x2+y2+z2-Lb2);C3=(La+R-r)2-2*(La+R-r)2*(x*cos(4*pi/3)+y*sin(4*pi/3)+(x2+y2+z2-Lb2);t11=(-B-sqrt(B2-4*A1*C1);t11=(-B-sqrt(B2-4*A1*C1)/(2*A1);t12=(-B+sqrt(B2-4*A1*C1)/(2*A1);t21=(-B-sqrt(B2-4*A2*C2)/(2*A2);t22=(-B+sqrt(B2-4*A2*C2)/(2*A2);t31=(-B-sqrt(B2-4*A3*C3)/(2*A3);t32=(-B+sqrt(B2-4*A3*C3)/(2*A3);o11=2*atan(t11);o12=2*atan(t12);o21=2*atan(t21);o22=2*atan(t22);o31=2*atan(t31);o32=2*atan(t32);endenda=o11 o12 ;b=o21 o22;c=o31 o32;for i=1:2 if abs(a(i)pi/2 x=o(i) else disp( ) endendfor i=1:2 if abs(b(i)pi/2 y=o(i) else disp( ) endendfor i=1:2 if abs(c(i)pi/2 z=o(i) else disp( ) endend%数值计算法求积分方程的特解ft=e-|sin(t)|和sx=04f(t)dtt=0.05;t=0:dt:4;Ft=exp(-abs(sin(t);%Ft数组与自变量数组有相同数目的元素，Ft数组的元素值就是t数组相应位置的函数值f（t）Sx=dt*cumtrapz(Ft);%plot(t,Ft,*r,MarkerSize,4)%Sx数组与