紧急求助！
本人刚学Matlab，现自己编了一个程序，但是调试中出错，由于老师着急要结果，请高手帮帮忙！function Mfile_robot
angel=(0:pi/100:2*pi);
x=200*cos(angel);
y=200*sin(angel);
z=sqrt(420^2-(200*cos(angel)).^2);
plot3(x,y,z);
grid;
xlabel({'X'});
ylabel({'Y'});
zlabel({'Z'});
m=size(angel);
for i=1:m(2)
A=sqrt(x(i)^2*z(i)^4+(y(i)*z(i)^2+x(i)^2*y(i))^2+x(i)^4*z(i)^2);
B=sqrt(x(i)^2*y(i)^4+(x(i)^2*z(i)+y(i)^2*z(i))^2+x(i)^4*y(i)^2);
AA1=A*x(i)*y(i)^2-z(i)^2*x(i)*B;
AA2=-y(i)*z(i);
BB1=-z(i)^2*y(i)*B-x(i)^2*y(i)*(A+B);
BB2=x(i)*z(i);
CC1=z(i)*x(i)^2*A+z(i)*y(i)^2*A+z(i)*x(i)^2*B;
CC2=x(i)*y(i);
L=[y(i)*z(i),-x(i)*z(i),-x(i)*y(i)];
M=[det([BB1,CC1;BB2,CC2]),det([CC1,AA1;CC2,AA2]),det([AA1,BB1;AA2,BB2])];
N=[det([M(2),M(3);L(2),L(3)]),det([M(3),M(1);L(3),L(1)]),det([M(1),M(2);L(1),L(2)])];
T=vpa([L(1),N(1),M(1),x(i);L(2),N(2),M(2),y(i);L(3),N(3),M(3),z(i);0,0,0,1],10);
E=vpa([0,0,200,1],10)*T;
rE=sqrt(E(1)^2+E(2)^2);
rD=220*sin(pi/6)+rE;
zD=220*cos(pi/6)+E(3);
l=sqrt((rD-230)^2+(zD-800)^2);
thet1(i)=atan(E(2)/E(1));
thet2(i)=atan((800-zD)/(rD-230))-acos((l^2+325^2-300^2)/2*325*l);
thet3(i)=pi-acos((l^2-325^2-300^2)/2*325*300);
thet4(i)=pi/2-thet2(i)-thet3(i);
T1=[cos(thet1(i)),-sin(thet1(i)),0,0;sin(thet1(i)),cos(thet1(i)),0,0;0,0,1,800;0,0,0,1];
T2=[cos(thet2(i)),-sin(thet2(i)),0,230;0,0,1,0;-sin(thet2(i)),-cos(thet2(i)),0,0;0,0,0,1];
T3=[cos(thet3(i)),-sin(thet3(i)),0,325;sin(thet3(i)),cos(thet3(i)),0,0;0,0,1,0;0,0,0,1];
T4=[cos(thet4(i)),-sin(thet4(i)),0,300;sin(thet4(i)),cos(thet4(i)),0,0;0,0,1,0;0,0,0,1];
TZ1=T1*T2*T3*T4;
TZ2=inv(TZ1)*T;
thet5(i)=atan(TZ2(2,3)/TZ2(1,3));
thet6(i)=acos(TZ2(3,3));
thet7(i)=asin(TZ2(3,1)/TZ2(3,3));
end
plot(thet1)
最后是想求出thet1到thet7的曲线，最好能在一个图上或是两张，谢谢！
