jiaodu.m

机器人运行轨迹计算

function [st1,st2,st3,st4,st5]=jiaodu(px,py,pz,s1,s2,s3,s4,s5)
nx=2/sqrt(5);ny=0;nz=1/sqrt(5);
ox=0;oy=-1;oz=0;
ax=-1/sqrt(5);ay=0;az=2/sqrt(5);
d1=140;d2=255;d3=255;d4=65;
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)*(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;
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=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;