运动学正解
说明：由几何关系算得连杆转角，带入验证x y z 的坐标关系。a1 a2 a3 表示连杆1、2、3的转角。最后解得，矩阵最后一列表示小球在原点坐标系中的位置。
syms a1 a2 a3 a4 d1 d2 d3 d4 ;
%连杆间齐次变换矩阵
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
%代入西门子PLC编程培训初值：
a1=0*pi/180;
a2=0*pi/180;
a3=-90*pi/180;
a4=0*pi/180;
d1=0;
d2=87;
d3=0;
d4=0;
%解得
T40 =
 
    0.0000    1.0000         0  201.0000
   -0.0000    0.0000   -1.0000   -87.0000
   -1.0000    0.0000    0.0000    0.0000
         0         0         0    1.0000
即 
2.2 运动学反解
  运动学反解是已知末端连杆的位姿，求解关节变量的过程。
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(a台达PLC编程培训1)*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;