puma560机器人D-H参数
puma560采用的是改进D-H参数，其DH参数表如下：
| i | αi | ai | di | θi |
| 1 | 0 | 0 | 0 | t1 |
| 2 | -90 | 0 | 0 | t2 |
| 3 | 0 | r2 | d3 | t3 |
| 4 | -90 | r3 | d4 | t4 |
| 5 | 90 | 0 | 0 | t5 |
| 6 | -90 | 0 | 0 | t6 |
对于参数为(α，a，d, θ)的机械臂，其变换矩阵为：
T = [ cosθ, -sinθ, 0, a;
sinθcosα, cosθcosα, -sinα, -dsinα;
sinθsinα, cosθsinα, cosα, dcosα;
0, 0, 0, 1]
所以可以根据上表写出6个关节变换矩阵：
T01 = [cos(t1),-sin(t1),0,0;
sin(t1),cos(t1),0,0;
0,0,1,0;
0,0,0,1];
T12 = [cos(t2),-sin(t2),0,0;
0,0,1,0;
-sin(t2),-cos(t2),0,0;
0,0,0,1];
T23 = [cos(t3),-sin(t3),0,r2;
sin(t3),cos(t3),0,0;
0,0,1,d3;
0,0,0,1];
T34 = [cos(t4),-sin(t4),0,r3;
0,0,1,d4;
-sin(t4),-cos(t4),0,0;
0,0,0,1];
T45 = [cos(t5),-sin(t5),0,0;
0,0,-1,0;
sin(t5),cos(t5),0,0;
0,0,0,1];
T56 = [cos(t6),-sin(t6),0,0;
0,0,1,0;
-sin(t6),-cos(t6),0,0;
0,0,0,1];
运动学正解
puma560机器人运动学正解计算为：
T06 = T01*T12*T23*T34*T45*T56，具体就不放结果了
运动学逆解
已知T06=[r11,r12,r13,Px;
r21,r22,r23,Py;
r31,r32,r33,Pz;
0, 0, 0, 1]
求出6个关节角t1,t2,t3,t4,t5,t6,就称之为运动学逆解
关节角t1计算
可以先计算T16 = T12*T23*T34*T45*T56，计算结果为：
T16 =
[-c23*(s4s6-c4c5c6)-s23s5c6, s23s5s6-c23(c6s4+c4c5s6),-s23c5-c23c4s5, r3c23-d4s23+r2c2;
-c4s6-s4c5c6, s4c5s6-c4c6, s4s5, d3;
s23(s4s6-c4c5c6)-c23s5c6, s23*(s4c6+c4c5s6)+c23s5s6, s23c4s5-c23c5, -d4c23-r3s23-r2s2;
0, 0, 0, 1]
这里的c23=cos(t2+t3),s23=sin(t2+t3),s2=sint2,s3=sin(t3)，是一种缩写记法。
又有
由T16[1,3]元素相等 ，可得：
-s1Px+c1Py = d3
令
则有：
可以看到t1有2个解。
关节角t3计算
现在t1已知，根据T16的元素（0,3）、元素（1,3）、元素（2,3）列出等式：
三式取平方和有：
三式相加化简有：
有看到了熟悉的式子，可以计算t3等于：
可以看到t3有2个解。
关节角t2的计算
先计算T03 ，建议用matlab符号运算来计算，matlab符号运算是推算公式的利器。
取T36的（0,3）元素及(1,3)元素列出等式：
c23c1Px+c23s1Py-s23Pz-r2c3 = r3
-s23c1Px-s23s1Py-c23Pz+r2s3 = d4
联立上面等式可以解出c23和s23,进而可以求出t2+t3，进而求出t2。
关节角t4的计算
T36矩阵中取(0,2)元素及（2,2）元素，成立等式：
r13c23c1+r23c23s1-r33s23 = -c4s5
-s1r13+c1r23 = s4s5
只要t5不为0,则有t4=atan2(-s1r13+c1r23,-r13c23c1-r23c23s1+r33s23);
当t5为0时，关节4和关节6出于一条直线，所有可能的接都是t4和t6的和或差。
关节角t5和t6的计算
先计算T04及T04的逆矩阵
取T46的（0,2）元素和（2,2）元素构成等式：
-s5=r13(s1s4+c1c4c23)+r23(s1c23c4-c1s4)-r33s23c4
c5=-r13c1s23-r23s1s23+c1c4)-r33c23
故有t5=atan2(-(r13(s1s4+c1c4c23)+r23(s1c23c4-c1s4)-r33s23c4),-r13c1s23-r23s1s23+c1c4)-r33c23)
取T46的（1,0）元素和(1,1)元素构成等式：
s6 = r21(-c1c23s4+s1c4)+r21(-s1c23s4-c1c4)+r3123s4
c6=r12(-c1c23s4+s1c4)+r22(-s1c23s4-c1c4)+r32s23s4
求出t6=atan2(r21(-c1c23s4+s1c4)+r21(-s1c23s4-c1c4)+r3123s4,r12(-c1c23s4+s1c4)+r22(-s1c23s4-c1c4)+r32s23s4)
前面计算t1和t3各有2个解，所以共有4组解，再加上腕关节翻转，可以得出8组解。对于计算出的4组解，由腕关节翻转，可以得到：
