并联机器人大作业—徐齐平(4)

运动轨迹逆解分析程序：

syms st l1 l2 l3 l4 l5 real; syms st1 st2 st3 st4 st5 real; l1=80;l2=100;l3=100;l4=80;l5=120; close all

syms xA yA xO yO ra dtpsai psai real; pap=40; xO=100;xO yO=100;yO ra=20;ra dtpsai=2*pi/pap;

xyA=zeros(2,41); psaiA=zeros(4,41); st1b=zeros(2,41); st4b=zeros(2,41); for pa=1:(pap+1) intev=mod(pa,5); psai=dtpsai*(pa-1); psaiA(1,pa)=psai; xA=xO+ra*cos(psai); yA=yO+ra*sin(psai); xyA(1,pa)=xA; xyA(2,pa)=yA; M=xA^2+yA^2+l1^2-l2^2; N=-2*xA*l1; K=-2*yA*l1;

DT2=4*K^2-4*(M^2-N^2); if DT2<0 error; end %if st1a=zeros(1,2);

st1a(1,1)=2*atan((-K+sqrt(K^2-(M^2-N^2)))/(M-N)); st1a(1,2)=2*atan((-K-sqrt(K^2-(M^2-N^2)))/(M-N)); st1a st1a*180/pi

st1b(1,pa)=st1a(1,1); st1b(2,pa)=st1a(1,2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% P=xA^2+yA^2+l5^2+l4^2-l3^2-2*xA*l5; Q=2*l5*l4-2*xA*l4; S=-2*yA*l4;

DT3=4*S^2-4*(P^2-Q^2); if DT3<0 error;end %if st4a=zeros(1,2);

st4a(1,1)=2*atan((-S+sqrt(S^2-(P^2-Q^2)))/(P-Q)); st4a(1,2)=2*atan((-S-sqrt(S^2-(P^2-Q^2)))/(P-Q)); st4a st4a*180/pi

st4b(1,pa)=st4a(1,1); st4b(2,pa)=st4a(1,2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% for m=1:2 m; figure(m);

15

for n=1:2 st4=st4a(1,n); xa4=l5;ya4=0;

xb4=l5+l4*cos(st4);yb4=l4*sin(st4); xp4=xyA(1,pa);yp4=xyA(2,pa); xleft=[xa4,xb4,xp4];yleft=[ya4,yb4,yp4]; if intev==0

plot(xleft,yleft,'bo-');hold on end %if intev end %for n st1=st1a(1,m); xa1=0;ya1=0;

xb1=l1*cos(st1);yb1=l1*sin(st1); xp1=xyA(1,pa);yp1=xyA(2,pa); xleft=[xa1,xb1,xp1];yleft=[ya1,yb1,yp1]; if intev==0

plot(xleft,yleft,'ro-');hold on end %if intev end %for m

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% end %for pa

psaiA=psaiA*180/pi; st1b=st1b*180/pi; st4b=st4b*180/pi; k=3; for i=1:2 for j=1:2 figure(k);

xlabel('\\phi-{A}/\\circ','Fontsize',18); plot(psaiA,st4b(j,:),'bo-');hold on k=k+1;

plot(psaiA,st1b(i,:),'r+-'); end %for j end %for i

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(1); plot(xyA(1,:),xyA(2,:),'ko-'); axis equal

figure(2); plot(xyA(1,:),xyA(2,:),'ko-'); axis equal

16

由此可得雅克比矩阵为：

J??J1J2?

式中：

?1/2(?a1a3cos(?1??3??2)?a1a3cos(?1??3??2)???arcos(?????)?arcos(?????))/a/sin(????)?13211132332??11?1/2(?a1a3sin(?1??3??2)?a1a3sin(?1??3??2)?J1????arsin(?????)?arsin(?????))/a/sin(????)13211132332??11??a1sin(??2??1)/a3/sin(??3??2)???????1/2(a3a4cos(?4??3??2)?a3a4cos(??4??3??2)???arcos(??????)?arcos(??????))/a/sin(????)?43241432332??41?1/2(a3a4sin(?4??3??2)?a3a4sin(??4??3??2)?J2????arsin(??????)?arsin(??????))/a/sin(????)43241432332??41??a4sin(?2??4)/a3/sin(??3??2)??????由速度的定义可知：

VA?Jq

??式中：VA??xA???

yAT?------动平台上参考点A的速度矢量； ?3????T????--------广义速度矢量；

q???1?4???又位置关系可知：

?3?2arctan(x1)?2?arcsin?其中：

?a4sin?4?a1sin?1?a3sin?3?

?a?2?D?D2?(A?B?C)(A?B?C)x1?A?B?CA?a12?a32?a42?a52?a22B?2a4a5cos?4?2a1a5cos?1?2a1a4cos(?4??1) C?2a3a4cos?4?2a3a5?2a1a3cos?1D?2a3a4sin?4?2a1a3sin?1根据机构的尺寸可知：a1?80，a2?100，a3?100，a4?80和a5?120，可得相应的图像如图所示：

17

18

19