哈工大PUMA机器人大作业

2017 年秋季学期研究生课程考核（读书报告、研究报告）
考核科目:机器人技术
学生所在院（系）:机电学院
学生所在学科:机械电子
学生姓名:王
学号:17S
学生类别:学硕
考察结果:
阅卷人:
PUMA 机器人正逆运动学推导及运动空间解算
图1 PUMA 机器人模型
任务:
1.建立坐标系；
2.给出D-H参数表；
3.推导正、逆运动学；
4.编程得工作空间。

一、 建立坐标系
根据PUMA 机器人运动自由度，在各关节处建立坐标系如图2所示。

图2 PUMA 机器人坐标系建立图
其中0O ∑与1O ∑原点交于一点, 4O ∑与5O ∑原点交于一点。

二、 D-H 参数表
D-H 参数表可根据坐标系设定而得出，见表1。

1) i θ为绕1i Z -轴，从1i X -到i X 的角度； 2) i a 为绕i X 轴，从1i Z -到i Z 的角度； 3) i l 为沿i X 轴，从1i Z -与i X 交点到i O 的距离； 4) i d 为沿1i Z -轴，从1i Z -与i X 到1i O -的距离。

）
90 90) 90 ) )
)
表1 PUMA 机器人杆件参数表
三、 正运动学推导
由坐标系及各杆件参数可得到6个连杆变换矩阵。

11
110
1cos 0sin 0sin 0cos 00
1000001T θθθθ-⎡⎤⎢⎥⎢
⎥=-⎢⎥⎢
⎥⎣⎦ 2
2
2222
22122cos sin 0cos sin cos 0sin 0010
001a a T d θθθθθθ-⎡⎤⎢⎥
⎢
⎥=⎢⎥
⎢⎥
⎣⎦
3
33333332
3cos 0sin cos sin 0
cos sin 01000001a a T θθθθ
θθ-⎡⎤⎢⎥⎢
⎥=-⎢⎥⎢⎥⎣⎦ 4
444
344cos 0sin 0sin 0cos 00
1000
01T d θθθθ-⎡⎤
⎢⎥⎢
⎥=-⎢⎥⎢
⎥⎣⎦
55554
5cos 0sin 0sin 0
cos 001000
001T θθθ
θ⎡⎤⎢⎥-⎢
⎥=-⎢⎥⎢⎥⎣⎦ 6
666
566cos sin 00sin cos 0
00
0100
01T d θθθθ-⎡⎤⎢⎥⎢
⎥=⎢⎥
⎢
⎥⎣⎦
根据各连杆变换矩阵相乘，可以得到PUMA 机械手变换矩阵，其矩阵为关节变量的函数。

()()()()()()0
0123456112233445566T T T T T T T θθθθθθ=
将上述变换矩阵逐个依次相乘可以得到06T 。

60
1x x x x y
y y y z z z z n o a p n o a p T n o a p ⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦
等式联立，对比等式左右两侧元素可得：
nx=sin(c6)*(cos(c4)*sin(c1)-cos(c2+c3)*cos(c1)*sin(c4))+
cos(c6)*(cos(c5)*(sin(c1)*sin(c4)+cos(c2+c3)*cos(c1)*cos(c4))+ sin(c2+c3)*cos(c1)*sin(c5))；
ox=cos(c6)*(cos(c4)*sin(c1)-cos(c2+c3)*cos(c1)*sin(c4))-
sin(c6)*(cos(c5)*(sin(c1)*sin(c4)+cos(c2+c3)*cos(c1)*cos(c4))+ sin(c2+c3)*cos(c1)*sin(c5))；
ax=sin(c5)*(sin(c1)*sin(c4)+cos(c2+c3)*cos(c1)*cos(c4))- sin(c2+c3)*cos(c1)*cos(c5)；
px=d6*(sin(c5)*(sin(c1)*sin(c4)+cos(c2+c3)*cos(c1)*cos(c4))- sin(c2+c3)*cos(c1)*cos(c5))-d2*sin(c1)-d4*sin(c2+c3)*cos(c1)+ a2*cos(c1)*cos(c2)+a3*cos(c1)*cos(c2)*cos(c3)- a3*cos(c1)*sin(c2)*sin(c3)；
ny=-sin(c6)*(cos(c1)*cos(c4)+cos(c2+c3)*sin(c1)*sin(c4))- cos(c6)*(cos(c5)*(cos(c1)*sin(c4)-cos(c2+c3)*cos(c4)*sin(c1))- sin(c2+c3)*sin(c1)*sin(c5))；
oy=sin(c6)*(cos(c5)*(cos(c1)*sin(c4)-cos(c2+c3)*cos(c4)*sin(c1))- sin(c2+c3)*sin(c1)*sin(c5))-cos(c6)*(cos(c1)*cos(c4)+ cos(c2+c3)*sin(c1)*sin(c4))；
ay=-sin(c5)*(cos(c1)*sin(c4)-cos(c2+c3)*cos(c4)*sin(c1))-
sin(c2+c3)*cos(c5)*sin(c1)；
py=d2*cos(c1)-d6*(sin(c5)*(cos(c1)*sin(c4)-
cos(c2+c3)*cos(c4)*sin(c1))+sin(c2+c3)*cos(c5)*sin(c1))- d4*sin(c2+c3)*sin(c1)+a2*cos(c2)*sin(c1)+
a3*cos(c2)*cos(c3)*sin(c1)-a3*sin(c1)*sin(c2)*sin(c3)； nz=cos(c6)*(cos(c2+c3)*sin(c5)-sin(c2+c3)*cos(c4)*cos(c5))+ sin(c2 + c3)*sin(c4)*sin(c6)；
oz=sin(c2+c3)*cos(c6)*sin(c4)-sin(c6)*(cos(c2+c3)*sin(c5)-
sin(c2+c3)*cos(c4)*cos(c5))；
az=-cos(c2+c3)*cos(c5)-sin(c2+c3)*cos(c4)*sin(c5)；
pz=-d4*cos(c2 + c3)- a3*sin(c2+c3)-d6*(cos(c2+c3)*cos(c5)+ sin(c2+c3)*cos(c4)*sin(c5))-a2*sin(c2)；
上述12个公式中，c1-c6分别对应16θθ，将关节变量角度的当前值代入以上公式得到：
22460
630-10-001-1000
001d a d d T a ⎡⎤⎢⎥
++⎢
⎥=⎢⎥
⎢⎥
⎣⎦
计算结果与当前位置姿态一致，推导结果无误。

四、 逆运动学推导
逆运动学推导为末端位置姿态n,o,a,p 确定，求解关节变量1θ。

1101
111cos sin 0
00010sin cos 000001T θθθθ-⎡⎤⎢⎥-⎢
⎥=-⎢⎥⎢
⎥⎣⎦ 2
2
222
1122cos sin 0sin cos 000010
001a T d θθθθ--⎡⎤⎢⎥
-⎢
⎥=-⎢⎥
⎢⎥⎣⎦ 33
321
333cos sin 00010sin cos 000001a T θθθθ--⎡⎤⎢⎥-⎢
⎥=-⎢⎥⎢
⎥⎣⎦ 4
443144
4
cos sin 00001sin cos 000001d T θθθθ-⎡⎤⎢⎥
-⎢
⎥=-⎢⎥
⎢
⎥⎣⎦ 5
541
555
cos sin 000010sin cos 000
01T θθθθ-⎡⎤⎢⎥⎢
⎥=-⎢⎥⎢⎥⎣⎦ 6
666
5166cos sin 00sin cos 0
00
010
01T d θθθθ-⎡⎤⎢⎥-⎢
⎥=-⎢⎥
⎢⎥⎣⎦
五、 工作空间的逆解运算
从上述的推导中我们已经得到了关节变量16θθ的推导公式。

根据逆解的求解思想，在这里我们参照正解所求得的工作间，将机器的运动空间划分成了x=-1000:n1:1000, y=-1000:n2:1000, z=-1000:n3:1000,所组成的空间点集合，其中n1,n2,n3代表步距，单位为mm 。

60
1x
x x x y
y y y z z z z n o a p n o a p T n o a p ⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦
在程序中通过循环，将此空间上的点的坐标一一代入06T 中的x p ，y p ，z p 。

6T 中n,o,a ，所对应的9个参数，代表了末端坐标系（执行器）各坐标轴与零
坐标系（固定坐标系）各坐标轴之间的夹角余弦值。

在这里为了简便运算，减少运算量，我们讨论固定姿态下机器人的工作空间。

令=-1x o ，=1y a ，=-1z n ，其余六个参数等于零，即当前PUMA 机器人末端执行器相对于固定坐标系的姿态。

至此，求解关节变量的参数均已给出。

另外考虑到手腕环节（后三个坐标系）对工作空间的影响不大，为了简化运算，在这里只将逆解求解到13θθ，而不再讨论46θθ。

逆解源代码，请参照附
录。

逆解求解的结果如图36。

六、 工作空间的正解运算
机器人正解的末端执行器（6O ∑）相对于固定坐标系（0O ∑）的转换矩阵06
T 已经求出。

取6O ∑上的原点6(0,0,0,1)T O =,066*T O 就可得到6O ∑上的原点在固定坐标系下的坐标，即工作空间中一个点。

通过将16θθ在对应的变量范围内变换便可以得到整个工作空间。

在这里为了减少运算量，同逆解，我们不讨论4
6θθ的变化，只讨论13
θθ在各自运动范围内的变化。

正解源代码及改进的高效率代码请参照附录。

逆解正解的结果如图710。

图3 PUMA 机器人逆解空间（轴测图）
图5 PUMA 机器人逆解空间（XOZ）
图7 PUMA 机器人正解空间（轴测图）
图8 PUMA 机器人正解空间（XOY）
图9 PUMA 机器人正解空间（XOZ）
图10 PUMA 机器人正解空间（YOZ）
附录
a)逆解程序
clc;
clear;
a2=431.8;
a3=-20.32;
d2=149.09;
d4=433.07;
d6=56.25;
i=1;
j=1;
t1=[0,0];
mt1=[0,0];
x=zeros(450000,1);
y=zeros(450000,1);
z=zeros(450000,1);
for px=-1000:10:1000
for py=-1000:10:1000
for pz=-1000:10:1000
j=j+1;
m=1;
t=0;
k=px^2+(py-d6)^2-d2^2;
if k<0
m=0;
else
s1=atan2(py-d6,px)-atan2(d2,sqrt(k));
s12=atan2(py-d6,px)-atan2(d2,-sqrt(k));
end
if m==1
if s1<160*pi/180 && s1>-160*pi/180
t1(1)=s1;
mt1(1)=1;
end
if s12<160*pi/180 && s12>-160*pi/180
t1(2)=s12;
mt1(2)=1;
end
if mt1(1)==0&&mt1(2)==0
m=0;
end
end
if m==1
for n=1:2
if mt1(n)==1
s1=sin(t1(1));
c1=cos(t1(1));
k=4*a2^2*(a3^2+d4^2)-(pz^2+(px*c1+py*s1-d6*s1)^2-a3^2-d4^2-a2^2)^2; k0=pz^2+(px*c1+py*s1-d6*s1)^2-a2^2-a3^2-d4^2;
if k<0
m=0;
else
s31=-atan2(d4,a3)-atan2(sqrt(k),k0);
s32=-atan2(d4,a3)+atan2(sqrt(k),k0);
if (s31<225*pi/180 && s31>-45*pi/180)…
||(s32<225*pi/180 && s32>-45*pi/180)
else
m=0;
end
if m==1
k=a3^2+d4^2-a2^2-pz^2-(px*c1+py*s1-d6*s1)^2;
k0=4*a2^2*(a3^2+d4^2)-(a3^2+d4^2-a2^2-pz^2-…
(px*c1+py*s1-d6*s1)^2)^2;
k1=d6*s1-px*c1-py*s1;
if k0>=0
t21=-atan2(k,sqrt(k0))-atan2(k1,pz);
t22=atan2(k,sqrt(k0))-atan2(k1,pz);
if (t21<45*pi/180 && t21>-225*pi/180)…
||(t22<45*pi/180 && t22>-225*pi/180) t=1;
end
end
end
end
end
end
end
if t==1
x(i)=px;
y(i)=py;
z(i)=pz;
i=i+1;
end
end
end
end
scatter3(x(1:(i-1)),y(1:(i-1)),z(1:(i-1)),2,'.');
xlabel('X/mm')
ylabel('Y/mm')
zlabel('Z/mm')
title('逆解运算，步距10');
b)正解程序
clear;
clc;
syms c1 c2 c3 c4 c5 c6;
a2=431.8;
d2=149.09;
a3=20.32;
d4=433.07;
d6=56.25;
A1=[cos(c1),0,-sin(c1),0;...
sin(c1),0,cos(c1),0;...
0,-1,0,0;0,0,0,1];
A2=[cos(c2),-sin(c2),0,a2*cos(c2);... sin(c2),cos(c2),0,a2*sin(c2);... 0,0,1,d2;0,0,0,1];
A3=[cos(c3),0,-sin(c3),a3*cos(c3);... sin(c3),0,cos(c3),a3*sin(c3);... 0,-1,0,0;0,0,0,1];
A4=[cos(c4),0,-sin(c4),0;...
sin(c4),0,cos(c4),0;...
0,-1,0,d4;...
0,0,0,1];
A5=[cos(c5),0,sin(c5),0;...
sin(c5),0,-cos(c5),0;...
0,1,0,0;0,0,0,1];
A6=[cos(c6),-sin(c6),0,0;...
sin(c6),cos(c6),0,0;...
0,0,1,d6;...
0,0,0,1];
c4=pi;
c5=0;
c6=0;
A4=eval(A4);
A5=eval(A5);
A6=eval(A6);
T36=A4*A5*A6;
x=zeros(26000,1);
y=zeros(26000,1);
z=zeros(26000,1);
i=0;
for c1=(-160:10:160)*pi/180
T01=eval(A1);
for c2=(-225:10:45)*pi/180
T12=eval(A2);
T02=T01*T12;
for c3=(-225:10:45)*pi/180
T23=eval(A3);
T03=T02*T23;
T06=T03*T36;
Object=T06*[0;0;0;1];
i=i+1;
x(i)=Object(1);
y(i)=Object(2);
z(i)=Object(3);
end
end
end
plot3(x(1:i),y(1:i),z(1:i),'.');
fprintf(1,'\n循环次数为%d\n',i);
grid on;
xlabel('X');
ylabel('Y');
zlabel('Z');
title('正解运算，步距10');
c)改进的正解程序
clear;
clc;
syms c1 c2 c3;
x=zeros(3000000,1);
y=zeros(3000000,1);
z=zeros(3000000,1);
i=0;
for c1=(-160:2:160)*pi/180
for c2=(-225:2:45)*pi/180
for c3=(-225:2:45)*pi/180
i=i+1;
x(i)=(2159*cos(c1)*cos(c2))/5 - (12233*sin(c2 + c3)*cos(c1))/25 ... - (14909*sin(c1))/100 + (508*cos(c1)*cos(c2)*cos(c3))/25 ...
- (508*cos(c1)*sin(c2)*sin(c3))/25;
y(i)=(14909*cos(c1))/100 - (12233*sin(c2 + c3)*sin(c1))/25 ...
+ (2159*cos(c2)*sin(c1))/5 + (508*cos(c2)*cos(c3)*sin(c1))/25 ...
- (508*sin(c1)*sin(c2)*sin(c3))/25;
z(i)=- (12233*cos(c2 + c3))/25 - (508*sin(c2 + c3))/25 - (2159*sin(c2))/5; end
end
end
scatter3(x(1:i),y(1:i),z(1:i),5,'.','r');
fprintf(1,'\n循环次数为%d\n',i);
grid on;
xlabel('X');
ylabel('Y');
zlabel('Z');
title('逆解运算，步距2');
-----精心整理，希望对您有所帮助!。