工业六轴机器人常见的STD(标准)-DH模型建立方法
文章目录
- 一、D-H模型
- 二、形式1
- 三、形式2
- 四、形式3
- 五、形式4
一、D-H模型
二、形式1
利用MATLAB机器人工具箱Matlab-Robotics-Toolbox建立标准Link函数，其中offset是以z轴为旋转轴的初始零位的补偿角度。
L1=160;L2=449.5;L3=580;L4=200;L5=640;L6=228;
a1=L1;d1=L2;a2=L3;a3=L4;d4=L5;d6=L6;
% theta(z) d(z) a(x) alpha(x)
RL1=Link([ 0 d1 a1 -pi/2 ],'standard');
RL2=Link([ 0 0 a2 0 ],'standard');RL2.offset=-pi/2;
RL3=Link([ 0 0 a3 -pi/2 ],'standard');
RL4=Link([ 0 d4 0 pi/2 ],'standard');
RL5=Link([ 0 0 0 -pi/2 ],'standard');
RL6=Link([ 0 d6 0 0 ],'standard');
Robot_6=SerialLink([RL1,RL2,RL3,RL4,RL5,RL6],'name','Robot');
Robot_6.teach;
三、形式2
L1=160;L2=449.5;L3=580;L4=200;L5=640;L6=228;
RL1=Link('revolute','d',L2,'a',L1,'alpha',-pi/2,'standard');
RL2=Link('revolute','d',0,'a',L3,'alpha',0,'standard');RL2.offset=-pi/2;
RL3=Link('revolute','d',0,'a',L4,'alpha',-pi/2,'standard');
RL4=Link('revolute','d',L5,'a',0,'alpha',pi/2,'standard');
RL5=Link('revolute','d',0,'a',0,'alpha',-pi/2,'standard');
RL6=Link('revolute','d',L6,'a',0,'alpha',0,'standard');
Robot_6=SerialLink([RL1,RL2,RL3,RL4,RL5,RL6],'name','Robot');
Robot_6.teach;
四、形式3
逐步建立各关节轴的矩阵方程，主要依据DH模型。其中w=-pi/2就是上面offset设定的值。
L1=160;L2=449.5;L3=580;L4=200;L5=640;L6=228;
%初始角度设置
w1=0;w2=-pi/2;w3=0;w4=0;w5=0;w6=0;
%Rz*Tz(L2)*Tx(L1)*Rx(-pi/2)
T01=[cos(w1) -sin(w1) 0 0;sin(w1) cos(w1) 0 0;0 0 1 0;0 0 0 1]*...
[1 0 0 0;0 1 0 0;0 0 1 L2;0 0 0 1]*...
[1 0 0 L1;0 1 0 0;0 0 1 0;0 0 0 1]*...
[1 0 0 0;0 cos(-pi/2) -sin(-pi/2) 0;0 sin(-pi/2) cos(-pi/2) 0;0 0 0 1];
%Rz*Tx(L3)
T12=[cos(w2) -sin(w2) 0 0;sin(w2) cos(w2) 0 0;0 0 1 0;0 0 0 1]*...
[1 0 0 L3;0 1 0 0;0 0 1 0;0 0 0 1];
%Rz*Tx(L4)*Rx(-pi/2)
T23=[cos(w3) -sin(w3) 0 0;sin(w3) cos(w3) 0 0;0 0 1 0;0 0 0 1]*...
[1 0 0 L4;0 1 0 0;0 0 1 0;0 0 0 1]*...
[1 0 0 0;0 cos(-pi/2) -sin(-pi/2) 0;0 sin(-pi/2) cos(-pi/2) 0;0 0 0 1];
%Rz*Tz(L5)*Rx(pi/2)
T34=[cos(w4) -sin(w4) 0 0;sin(w4) cos(w4) 0 0;0 0 1 0;0 0 0 1]*...
[1 0 0 0;0 1 0 0;0 0 1 L5;0 0 0 1]*...
[1 0 0 0;0 cos(pi/2) -sin(pi/2) 0;0 sin(pi/2) cos(pi/2) 0;0 0 0 1];
%Rz*Rx(-pi/2)
T45=[cos(w5) -sin(w5) 0 0;sin(w5) cos(w5) 0 0;0 0 1 0;0 0 0 1]*...
[1 0 0 0;0 cos(-pi/2) -sin(-pi/2) 0;0 sin(-pi/2) cos(-pi/2) 0;0 0 0 1];
%Rz*Tz(L6)
T56=[cos(w6) -sin(w6) 0 0;sin(w6) cos(w6) 0 0;0 0 1 0;0 0 0 1]*...
[1 0 0 0;0 1 0 0;0 0 1 L6;0 0 0 1];
化简后得：
T=[cos(w1) 0 -sin(w1) L1*cos(w1);sin(w1) 0 cos(w1) L1*sin(w1);0 -1 0 L2;0 0 0 1]*...
[cos(w2) -sin(w2) 0 L3*cos(w2);sin(w2) cos(w2) 0 L3*sin(w2);0 0 1 0;0 0 0 1]*...
[cos(w3) 0 -sin(w3) L4*cos(w3);sin(w3) 0 cos(w3) L4*sin(w3);0 -1 0 0;0 0 0 1]*...
[cos(w4) 0 sin(w4) 0;sin(w4) 0 -cos(w4) 0;0 1 0 L5;0 0 0 1]*...
[cos(w5) 0 -sin(w5) 0;sin(w5) 0 cos(w5) 0;0 -1 0 0;0 0 0 1]*...
[cos(w6) -sin(w6) 0 0;sin(w6) cos(w6) 0 0;0 0 1 L6;0 0 0 1]
运算结果：
T =
1.0e+03 *
0.0000 0 0.0010 1.0280
0 -0.0010 0 0
0.0010 0 -0.0000 1.2295
0 0 0 0.0010
五、形式4
直接对矩阵进行化解：
L1=160;L2=449.5;L3=580;L4=200;L5=640;L6=228;
%初始角度设置
w1=0;w2=-pi/2;w3=0;w4=0;w5=0;w6=0;
deg=180/pi;
theta1=w1*deg;theta2=w2*deg;theta3=w3*deg;theta4=w4*deg;theta5=w5*deg;theta6=w6*deg;
s1=sind(theta1);s2=sind(theta2);s3=sind(theta3);s4=sind(theta4);s5=sind(theta5);s6=sind(theta6);
c1=cosd(theta1);c2=cosd(theta2);c3=cosd(theta3);c4=cosd(theta4);c5=cosd(theta5);c6=cosd(theta6);
s23=s2*c3+c2*s3;c23=c2*c3-s2*s3;
Nx=c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)+s1*(s4*c5*c6+c4*s6);
Ny=s1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-c1*(s4*c5*c6+c4*s6);
Nz=-s23*(c4*c5*c6-s4*s6)-c23*s5*c6;
Ox=c1*(c23*(-c4*c5*s6-s4*c6)+s23*s5*s6)-s1*(s4*c5*s6-c4*c6);
Oy=s1*(c23*(-c4*c5*s6-s4*c6)+s23*s5*s6)+c1*(s4*c5*s6-c4*c6);
Oz=s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
Ax=c1*(-c23*c4*s5-s23*c5)-s1*s4*s5;
Ay=s1*(-c23*c4*s5-s23*c5)+c1*s4*s5;
Az=s23*c4*s5-c23*c5;
Px=c1*(-c23*c4*s5*L6-s23*(c5*L6+L5)+c23*L4+c2*L3)-s1*s4*s5*L6+c1*L1;
Py=s1*(-c23*c4*s5*L6-s23*(c5*L6+L5)+c23*L4+c2*L3)+c1*s4*s5*L6+s1*L1;
Pz=s23*c4*s5*L6-c23*(c5*L6+L5)-s23*L4-s2*L3+L2;
TT=[Nx Ox Ax Px;
Ny Oy Ay Py;
Nz Oz Az Pz;
0 0 0 1];
运算结果：
TT =
1.0e+03 *
0 0 0.0010 1.0280
0 -0.0010 0 0
0.0010 0 0 1.2295
0 0 0 0.0010