jiaodu3.m

机器人运行轨迹计算

function [st1,st2,st3,st4,st5]=jiaodu3(px,py,pz,s1,s2,s3,s4,s5)
nx=0;ny=1;nz=0;
ox=-1;oy=0;oz=0;
ax=0;ay=0;az=1;
d1=140;d2=255;d3=255;d4=65;
st1=atan2((d4*ax+px),-(d4*ay+py))-s1*pi;
%st1=atan(-(d4*ax+px)/(d4*ay+py))+s1*pi; 
k1=d4*sin(st1)*ax-d4*cos(st1)*ay+sin(st1)*px-cos(st1)*py;
k2=d4*az+pz-d1;
k3=(k1^2+k2^2+d2^2-d3^2)/(2*d2);
st2=(1-s2)*(atan2(k2,-k1)-atan2(k3,sqrt(k1^2+k2^2-k3^2)))+...
     s2*(atan2(k2,-k1)-atan2(k3,-sqrt(k1^2+k2^2-k3^2)));
%st2=(1-s2)*(atan(-k2/k1)-atan(k3/sqrt(k1^2+k2^2-k3^2)))+...
     %s2*(atan(-k2/k1)-atan(-k3/sqrt(k1^2+k2^2-k3^2)));
%st3=atan((k1*cos(st2)-k2*sin(st2))/(k1*sin(st2)+k2*cos(st2)-d2))+s3*pi; 
st3=atan2((k1*cos(st2)-k2*sin(st2)),(k1*sin(st2)+k2*cos(st2)-d2));
st4=atan2((-cos(st2+st3)*sin(st1)*ox+cos(st2+st3)*cos(st1)*oy+sin(st2+st3)*oz),...
    (-cos(st1)*ox-sin(st1)*oy));
    %st4=atan((-cos(st2+st3)*sin(st1)*ox+cos(st2+st3)*cos(st1)*oy+sin(st2+st3)*oz)/...
    %(-cos(st1)*ox-sin(st1)*oy))+s4*pi;
st5=atan2((-sin(st2+st3)*sin(st1)*nx+sin(st2+st3)*cos(st1)*ny-cos(st2+st3)*nz),...
    (-sin(st2+st3)*sin(st1)*ax+sin(st2+st3)*cos(st1)*ay-cos(st2+st3)*az));
%st5=atan((-sin(st2+st3)*sin(st1)*nx+sin(st2+st3)*cos(st1)*ny-cos(st2+st3)*nz)/...
    %(-sin(st2+st3)*sin(st1)*ax+sin(st2+st3)*cos(st1)*ay-cos(st2+st3)*az))+s5*pi;