st.m

机器人运行轨迹计算

nx=0;ny=1;nz=0;
ox=-1;oy=0;oz=0;
ax=0;ay=0;az=1;
px=20;py=-200;pz=120;
d1=140;d2=255;d3=255;d4=65;
st1=atan(-(d4*ax+px)/(d4*ay+py))+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=atan(-k2/k1)-atan(k3/sqrt(k1^2+k2^2-k3^2));
%st2=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))+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))+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));

%SHUXUE
T01=zeros(4,4);
T12=zeros(4,4);
T23=zeros(4,4);
T34=zeros(4,4);
T45=zeros(4,4);
T56=zeros(4,4);
st6=0.0;
d1=140;
d2=255;
d3=255;
d4=65;
T01=[cos(st1+pi/2), -sin(st1+pi/2),0,0;
     sin(st1+pi/2),cos(st1+pi/2),0,0;
     0,0,1,d1;
     0,0,0,1];
T12=[cos(st2+pi/2),-sin(st2+pi/2),0,0;
     0,0,-1,0;
     sin(st2+pi/2),cos(st2+pi/2),0,0;
     0,0,0,1]; 
T23=[cos(st3+pi/2),-sin(st3+pi/2),0,d2;
    sin(st3+pi/2),cos(st3+pi/2),0,0;
    0,0,1,0;
    0,0,0,1];
T34=[cos(st4),-sin(st4),0,0;
    0,0,1,-d3;
    -sin(st4),-cos(st4),0,0;
    0,0,0,1];
T45=[cos(st5),-sin(st5),0,0;
    0,0,-1,0;
    sin(st5),cos(st5),0,0;
    0,0,0,1];
T56=[cos(st6),-sin(st6),0,0;
    0,0,1,-d4;
    -sin(st6),-cos(st6),0,0;    
    0,0,0,1];

T06=T01*T12*T23*T34*T45*T56