a1~a6代表绕z轴的旋转角，b1~b6分别代表沿z轴的平移量，c1~c6分别代表沿x轴的平移量，d1~d6分别代表沿x轴的旋转
以上即为D——H参数
syms a1 b1 c1 d1 Trz Tz Tx Trx A1;
Trz = sym('[cos(a1),-sin(a1),0,0;sin(a1),cos(a1),0,0;0,0,1,0;0,0,0,1]');Tz=sym('[1,0,0,0;0,1,0,0;0,0,1,b1;0,0,0,1]');
Tx=sym('[1,0,0,c1;0,1,0,0;0,0,1,0;0,0,0,1]');
Trx=sym('[1,0,0,0;0,cos(d1),-sin(d1),0;0,sin(d1),cos(d1),0;0,0,0,1]');
A1=Trz*Tz*Tx*Trx;
syms a2 b2 c2 d2 A2;
Trz = sym('[cos(a2),-sin(a2),0,0;sin(a2),cos(a2),0,0;0,0,1,0;0,0,0,1]');
Tz=sym('[1,0,0,0;0,1,0,0;0,0,1,b2;0,0,0,1]');
Tx=sym('[1,0,0,c2;0,1,0,0;0,0,1,0;0,0,0,1]');
Trx=sym('[1,0,0,0;0,cos(d2),-sin(d2),0;0,sin(d2),cos(d2),0;0,0,0,1]');
A2=Trz*Tz*Tx*Trx;
syms a3 b3 c3 d3 A3;
Trz = sym('[cos(a3),-sin(a3),0,0;sin(a3),cos(a3),0,0;0,0,1,0;0,0,0,1]');
Tz=sym('[1,0,0,0;0,1,0,0;0,0,1,b3;0,0,0,1]');
Tx=sym('[1,0,0,c3;0,1,0,0;0,0,1,0;0,0,0,1]');
Trx=sym('[1,0,0,0;0,cos(d3),-sin(d3),0;0,sin(d3),cos(d3),0;0,0,0,1]');
A3=Trz*Tz*Tx*Trx;
syms a4 b4 c4 d4 A4;
Trz = sym('[cos(a4),-sin(a4),0,0;sin(a4),cos(a4),0,0;0,0,1,0;0,0,0,1]');
Tz=sym('[1,0,0,0;0,1,0,0;0,0,1,b4;0,0,0,1]');
Tx=sym('[1,0,0,c4;0,1,0,0;0,0,1,0;0,0,0,1]');
Trx=sym('[1,0,0,0;0,cos(d4),-sin(d4),0;0,sin(d4),cos(d4),0;0,0,0,1]');
A4=Trz*Tz*Tx*Trx;
syms a5 b5 c5 d5 A5;
Trz = sym('[cos(a5),-sin(a5),0,0;sin(a5),cos(a5),0,0;0,0,1,0;0,0,0,1]');
Tz=sym('[1,0,0,0;0,1,0,0;0,0,1,b5;0,0,0,1]');
Tx=sym('[1,0,0,c5;0,1,0,0;0,0,1,0;0,0,0,1]');
Trx=sym('[1,0,0,0;0,cos(d5),-sin(d5),0;0,sin(d5),cos(d5),0;0,0,0,1]');
A5=Trz*Tz*Tx*Trx;
syms a6 b6 c6 d6 A6;
Trz = sym('[cos(a6),-sin(a6),0,0;sin(a6),cos(a6),0,0;0,0,1,0;0,0,0,1]');
Tz=sym('[1,0,0,0;0,1,0,0;0,0,1,b6;0,0,0,1]');
Tx=sym('[1,0,0,c6;0,1,0,0;0,0,1,0;0,0,0,1]');
Trx=sym('[1,0,0,0;0,cos(d6),-sin(d6),0;0,sin(d6),cos(d6),0;0,0,0,1]');
A6=Trz*Tz*Tx*Trx;
syms T;
T=A1*A2*A3*A4*A5*A6;
subs(T,{a1,b1,c1,d1,a2,b2,c2,d2,a3,b3,c3,d3,a4,b4,c4,d4,a5,b5,c5,d5,a6,b6,c6,d6},{0,0,0,0,-pi/2,0,0.15,-pi/2,0,0,0.57,0,0,0.65,0.15,-pi/2,0,0,0,pi/2,0,0,0,-pi/2});