运动学反解
  运动学反解是已知末端连杆的位姿，求解关节变量的过程。
syms a1 a2 a3 a4
T10=[cos(a1) -sin(a1) 0 0;sin(a1)*cos(0) cos(a1)*cos(0) -sin(0) -d1*sin(0);
    sin(a1)*sin(0) cos(a1)*sin(0) cos(0) d1*cos(0);0 0 0 1];
T21=[cos(a2) -sin(a2) 0 0;sin(a2)*cos(pi/2) cos(a2)*cos(pi/2) -sin(pi/2) -d2*sin(pi/2);
    sin(a2)*sin(pi/2) cos(a2)*sin(pi/2) cos(pi/2) d2*cos(pi/2);0 0 0 1];
T32=[cos(a3) -sin(a3) 0 201;sin(a3)*cos(0) cos(a3)*cos(0) -sin(0) -d3*sin(0);
    sin(a3)*sin(0) cos(a3)*sin(0) cos(0) d3*cos(0);0 0 0 1];
T43=[cos(a4) -sin(a4) 0 0;sin(a4)*cos(-pi/2) cos(a4)*cos(-pi/2) -sin(-pi/2) -d4*sin(-pi/2);
    sin(a4)*sin(-pi/2) cos(a4)*sin(-pi/2) cos(-pi/2) d4*cos(-pi/2);0 0 0 1];
T40=T10*T21*T32*T43;
 
 
T1=inv(T10);%求T10的逆矩阵。
T41=T1*T40;
%T41=T21*T32*T43
对应各元素相等即山西太原PLC编程培训可求出各关节变量。
另见附录一
第3章 速度分析
先求出雅克比矩阵，我们可以得到末端执行器速度与各关节的速度关系。
a1=0*pi/180;
a2=0*pi/180;
a3=-90*pi/180;
a4=0*pi/180;
d1=0;
d2=87;
d3=0;
d4=0;
T10=[cos(a1) -sin(a1) 0 0;sin(a1)*cos(0) cos(a1)*cos(0) -sin(0) -d1*sin(0);
    sin(a1)*sin(0) cos(a1)*sin(0) cos(0) d1*cos(0);0 0 0 1];
T21=[cos(a2) -sin(a2) 0 0;sin(a2)*cos(pi/2) cos(a2)*cos(pi/2) -sin(pi/2) -d2*sin(pi/2);
    sin(a2)*sin(pi/2) cos(a2)*sin(pi/2) cos(pi/2) d2*cos(pi/2);0 0 0 1];
T32=[cos(a3) -sin(a3) 0 201;sin(a3)*cos(0) cos(a3)*cos(0) -sin(0) -d3*sin(0);
    sin(a3)*sin(0) cos(a3)*sin(0) cos(0) d3*cos(0);0 0 0 1];
T43=[cos(a4) -sin(a4) 0 0;sin(a4)*cos(0) cos(a4)*cos(0) -sin(0) -d4*sin(0);
    sin(a4)*sin(0) cos(a4)*sin(0) cos(0) d4*cos(0);0 0 0 1];
 
T40=T10*T21*T32*T43;
T30=T10*T21*T32;
T20=T10*T21;
T10=T10;
 
R10=T10(1:3,1:3);%取T10第一行到第三行即第一列到第三列
R20=T20(1:3,1:3);
R30=T30(1:3,1:3);
R40=T40(1:3,1:3);
 
T41=T21*T32*T43;
T42=T32*T43;
T43=T43;
T44=T43;
 
P41=T71(1:3,4);%西门子PLC200/300编程培训取最后一列的前三行
P42=T72(1:3,4);
P43=T73(1:3,4);
P44=T74(1:3,4);
 
Z1=T10(1:3,3);%z轴单位向量
Z2=T20(1:3,3);
Z3=T30(1:3,3);
Z4=T40(1:3,3);
 
R1=cross(Z1,(R10*P41));%速度V
R2=cross(Z2,(R20*P42));
R3=cross(Z3,(R30*P43));
R4=cross(Z4,(R40*P44));
 
J=[R1 R2 R3 R4;Z1 Z2 Z3 Z4]
 
得J =
 
   87.0000         0        0         0
  201.0000     0.0000        0         0
         0   201.0000        0        0
         0         0         0        0
         0    -1.0000   -1.0000   -1.0000
    1.0000     0.0000    0.0000    0.0000
 
 
第4章 轨迹规划
 我们通过三维模拟，得到各个关节角位移和角速度的曲线如下：