shuxuebiaodashi.m

机器人运行轨迹计算

x=[320,120,190,255];
y=[-104,106,-125,88];
%hold on
for k=1:4
z(k)=-sqrt(25/4*(x(k)-210)^2+25/4*y(k)^2)+398.4593;
f1=-25/2*(x(k)-210);
f2=-25/2*y(k);
f3=2*(z(k)-398.4593);
fd=sqrt(f1^2+f2^2+f3^2);
fc1(k)=f1/fd;
fc2(k)=f2/fd;
fc3(k)=f3/fd;
R=-y(k)/(210-x(k));
a=(1+R^2);
xx(k,1:2)=subs(solve('a*(x-210)^2-159.383^2')); 
yy(k,1)=R*(xx(k,1)-210);
yy(k,2)=R*(xx(k,2)-210);
fr1(k,1)=xx(k,1)-x(k);
fr2(k,1)=yy(k,1)-y(k);
fr3(k,1)=0-z(k);
fr1(k,2)=xx(k,2)-x(k);
fr2(k,2)=yy(k,2)-y(k);
fr3(k,2)=0-z(k);
fdd(k,1)=sqrt(fr1(k,1)^2+fr2(k,1)^2+fr3(k,1)^2);
fdd(k,2)=sqrt(fr1(k,2)^2+fr2(k,2)^2+fr3(k,2)^2);
fr1(k,1)=fr1(k,1)/fdd(k,1);
fr2(k,1)=fr2(k,1)/fdd(k,1);
fr3(k,1)=fr3(k,1)/fdd(k,1);
fr1(k,2)=fr1(k,2)/fdd(k,2);
fr2(k,2)=fr2(k,2)/fdd(k,2);
fr3(k,2)=fr3(k,2)/fdd(k,2);
end
plot3(x,y,z,x(1),y(1),z(1),'*',x(2),y(2),z(2),'r*',x(3),y(3),z(3),'k*',x(4),y(4),z(4),'o');
%plot3()
for k=1:4
a1(k)=fc1(1,k)*fr1(k,1)+fc2(1,k)*fr2(k,1)+fc3(1,k)*fr3(k,1);
a2(k)=fc1(1,k)*fr1(k,2)+fc2(1,k)*fr2(k,2)+fc3(1,k)*fr3(k,2);
end
ax=fc1(1);%k
ay=fc2(1);
az=fc3(1);
nx=fr1(1,1);%k=1,4 ---1 ,k=2,3----2
ny=fr2(1,1);
nz=fr3(1,1);
syms aa bb cc
A=[aa,bb,cc;ax,ay,az;nx,ny,nz];
det(A)