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

《delta机械手运动学反解代码.docx》由会员上传分享，免费在线阅读，更多相关内容在工程资料-天天文库。

1、%delta机械手运动学正解代码symsxyzLa=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+y^2+(La*sin(o1)+z)^2-Lb^2;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-Lb^2;eq3

2、=(-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-Lb^2;[x,y,z]=solve(eq1,eq2,eq3);x=vpa(x,5);y=vpa(y,5);z=vpa(z,5);fori=1:2z(i);ifz(i)<0ans=[x(i)y(i)z(i)];elsedisp('')endendans=vpa(ans,5);ifans(1).^2+ans(2).^2+ans(3).^2<10000000ans=[a

3、ns(1)ans(2)ans(3)];elsedisp('noanswer!')endans=vpa(ans,5)%注解：坐标位置的控制是精度控制之一%运动学反解代码symsA1A2A3BC1C2C3R=0.348;r=0.1068;La=0.200;Lb=0.600;x=input('请输入分量坐标x=');y=input('请输入分量运动y=');z=input('请输入分量运动z=');ifz<0ifx.^2+y.^2+z.^2>=100000disp('Theprogramhasnotfoundsolution!')e

4、lseA1=(La-R+r)^2+2*(x*cos(0)+y*sin(0))*(La-R+r)+(x^2+y^2+z^2-Lb^2);A2=(La-R+r)^2+2*(x*cos(2*pi/3)+y*sin(2*pi/3))*(La-R+r)+(x^2+y^2+z^2-Lb^2);A3=(La-R+r)^2+2*(x*cos(4*pi/3)+y*sin(4*pi/3))*(La-R+r)+(x^2+y^2+z^2-Lb^2);B=4*La*z;C1=(La+R-r)^2-2*(La+R-r)^2*(x*cos(0)+y*sin

5、(0))+(x^2+y^2+z^2-Lb^2);C2=(La+R-r)^2-2*(La+R-r)^2*(x*cos(2*pi/3)+y*sin(2*pi/3))+(x^2+y^2+z^2-Lb^2);C3=(La+R-r)^2-2*(La+R-r)^2*(x*cos(4*pi/3)+y*sin(4*pi/3))+(x^2+y^2+z^2-Lb^2);t11=(-B-sqrt(B^2-4*A1*C1));t11=(-B-sqrt(B^2-4*A1*C1))/(2*A1);t12=(-B+sqrt(B^2-4*A1*C1))/(2

6、*A1);t21=(-B-sqrt(B^2-4*A2*C2))/(2*A2);t22=(-B+sqrt(B^2-4*A2*C2))/(2*A2);t31=(-B-sqrt(B^2-4*A3*C3))/(2*A3);t32=(-B+sqrt(B^2-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=[o11o12];b=[o21o22];c

7、=[o31o32];fori=1:2ifabs(a(i))

8、sin⁡(t)

9、和sx=04f(t)dtt=0.05;t=0:dt:4;Ft=exp(-abs(sin(t)));%Ft数组与自变量数组有相同数目的元素，Ft数组的元素值就是t数组相应

10、位置的函数值f（t）Sx=dt*cumtrapz(Ft);%plot(t,Ft,'*r','MarkerSize',4)%Sx数组与自变量有相同数目的元素，Sx数组的元素给出与t数组采样点对应的积分值holdonplot(t,Sx,'.k','MarkerSize',6)ho