[MATLAB图像处理] 求高手相助机械手可操作度

[x1992520]

本人写了段程序如下
R1=4;R2=2;L=-8.5;
for tha=-10:10
for thc=-10:10
beta=tha*pi/180;betb=0*pi/180;betc=thc*pi/180;
l11=-1.732*0.5*R1+0.5*R2*1.732*cos(betc)*cos(betb)+0.5*R2*cos(betc)*sin(betb)*cos(beta)+0.5*R2*sin(betc)*sin(beta);
l12=0.5*R2*(1.732*sin(betc)*cos(betb)-cos(betc)*sin(beta)+sin(betc)*sin(betb)*cos(beta))+L;
l13=0.5*R2*(cos(betb)*cos(beta)-1.732*sin(betb))-0.5*R1;
l1=sqrt(l11^2+l12^2+l13^2);
l21=R2*cos(betc)*sin(betb)*cos(beta)+R2*sin(betc)*sin(beta);
l22=L-R2*sin(betc)*sin(betb)*cos(beta)-R2*cos(betc)*sin(beta);
l23=R1-R2*cos(betb)*cos(beta);
l2=sqrt(l21^2+l22^2+l23^2);
l31=1.732*0.5*R1-0.5*R2*(1.732*cos(betc)*sin(betb)-cos(betc)*sin(betb)*cos(beta)-sin(betc)*sin(beta));
l32=L-0.5*R2*(1.732*sin(betc)*cos(betb)-sin(betc)*sin(betb)*cos(beta)+cos(betc)*sin(beta));
l33=0.5*R2*(cos(betb)*cos(beta)+1.732*sin(betb))-0.5*R1;
l3=sqrt(l31^2+l32^2+l33^2);
A=[-1.732*0.5*R1+0.5*R2*1.732*cos(betc)*cos(betb)+0.5*R2*cos(betc)*sin(betb)*cos(beta)+0.5*R2*sin(betc)*sin(beta)]/l1;
B=[0.5*R2*(1.732*sin(betc)*cos(betb)-cos(betc)*sin(beta)+sin(betc)*sin(betb)*cos(beta))+L]/l1;
C=[0.5*R2*(cos(betb)*cos(beta)-1.732*sin(betb))-0.5*R1]/l1;
D=[R2*cos(betc)*sin(betb)*cos(beta)+R2*sin(betc)*sin(beta)]/l2;
E=[L-R2*sin(betc)*sin(betb)*cos(beta)+R2*cos(betc)*sin(beta)]/l2;
F=[R1-R2*cos(betb)*cos(beta)]/l2;
G=[1.732*0.5*R1-0.5*R2*(1.732*cos(betc)*sin(betb)-cos(betc)*sin(betb)*cos(beta)-sin(betc)*sin(beta))]/l3;
H=[0.5*R2*(1.732*sin(betc)*cos(betb)-sin(betc)*sin(betb)*cos(beta)+cos(betc)*sin(beta))+L]/l3;
Q=[0.5*R2*(cos(betb)*cos(beta)+1.732*sin(betb))-0.5*R1]/l3;
P=0.5*R2*(-1.732*cos(betb)+sin(betb)*cos(beta));
J11=A*(-0.5*R2*cos(beta)*sin(betc)+0.5*R2*cos(betc)*sin(beta)*sin(betb))+B*(0.5*R2*cos(betc)*cos(beta)-0.5*R2*sin(betc)*cos(beta))+C*(0.5*R2*cos(betb)*sin(beta));
J12=A*(-0.5*R2*cos(betb)*cos(betc)*cos(beta)+0.5*R2*1.732*cos(betc)*sin(betb))+B*(-0.5*R2*sin(betc)*cos(betb)*cos(beta)+R2*0.5*1.732*cos(betc)*sin(betb))+C*(0.5*R2*sin(betb)*cos(beta)+0.5*1.732*R2*cos(betb));
J13=A*[0.5*R2*(1.732*sin(betc)*cos(betb)+sin(betb)*sin(betc)*cos(beta)-cos(betc)*sin(beta))]+B*[-0.5*R2*(1.732*cos(betc)*cos(betb)+sin(betc)*sin(beta)+cos(betc)*sin(betb)*cos(beta))];
J21=D*(R2*sin(betc)*cos(beta)+R2*cos(betc)*sin(betb)*sin(beta))+E*(-R2*sin(betc)*sin(betb)*sin(beta)-R2*cos(betc)*cos(beta))+F*(-R2*cos(betb)*sin(beta));
J22=D*(R2*cos(betc)*cos(betb)*cos(beta))+E*(R2*sin(betc)*cos(betb)*cos(beta))+F*(-R2*sin(betb)*cos(beta));
J23=D*(R2*cos(betc)*sin(beta)-R2*cos(beta)*sin(betb)*sin(betc))+E*(R2*cos(betc)*cos(beta)*sin(betb)+R2*sin(betc)*sin(beta));
J31=G*(0.5*R2*cos(betc)*sin(betb)*sin(beta)-0.5*R2*sin(betc)*cos(beta))+H*(0.5*R2*sin(betc)*sin(betb)*sin(beta)+0.5*R2*cos(betc)*cos(beta))+Q*[0.5*R2*sin(beta)*cos(betb)];
J32=G*[0.5*R2*(1.732*cos(betc)*cos(betb)-cos(betc)*cos(betb)*cos(beta))]+H*[-0.5*R2*(1.732*sin(betc)*sin(betb)+sin(betc)*cos(betb)*cos(beta))]+Q*P;
J33=G*[-0.5*R2*(1.732*sin(betc)*sin(betb)-sin(betc)*sin(betb)*cos(beta)+cos(betc)*sin(beta))]+H*[0.5*R2*(1.732*cos(betc)*cos(betb)-cos(betc)*sin(betb)*cos(beta)-cos(betc)*sin(beta))];
J=[J11 J12 J13;J21 J22 J23;J31 J32 J33];
T=inv(J);
R=T.*T;
k=det(R);
W=sqrt(k);Amax=max(max(W));
end
end
plot3(tha,thc,W)
axis([-30,30,-30,30,0,2])
原来在W的语句下面还写了句W(l)=W;l=l+1;但这么写完输出结果就基本都变成零了，无论写不写这句plot3就只出来一个点，请高手指点下应该怎么改