实用文档
把代换式 (6) 代入式 (5) 得 ??1：
??2??22
√
sin(??? ??1)=??; cos(??? ??1)=1?(??)
??2??22
??? ??1=atan2[,±√1?()]
??=atan2(??,??)?atan2[??2,±√1?(??2)2]
1???? ???? {
式中，正、负号对应于 ??1的两个可能解。 2) 求 ??3
矩阵方程两端的元素（1，4）和（2，4）分别对应相等
????
?c1px?s1py?a3c23?a2c2?d4s23???pz?d4c23?a3s23?a2s2
平方和为：
d4s3?a3c3?k
其中
k?222222px?py?pz2?d2?d4?a2?a32a2
解得：
2?3?atan2(a3,d4)?atan2(k,?d42?a3?k2)
3) 求 ??2 在矩阵方程
0T6?0T11T22T33T44T55T6
两边左乘逆变换
0T3?1。
0T3?10T6?3T44T55T6
axayaz0px?py???3T6pz??1?
?c1c23??cs?123??s1??0
s1c23?s1s23c10?s23?c2300?a2c3??nx?na2s3???y?d2??nz??1??0oxoyoz0实用文档
方程两边的元素（1，4）和（3，4）分别对应相等，得
?c1c23px?s1c23py?s23pz?a3?a2c3?0??c1s23px?s1s23py?c23pz?a2s3?d4?0联立，得
s23
和
c23
??a2s3?d4??c1px?s1py??pz?a2c3?a3??s23?22?pc?ps?p??x1y1z???a2c3?a3??c1px?s1py??pz?a2s3?d4??c??2322pc?ps?p???x1y1z?
s23和
c23表达式的分母相等，且为正，于是
??23??2??3?atan2???a2s3?d4??c1px?s1py??pz?a2c3?a3?,?a2c3?a3??c1px?s1py??pz?a2s3?d4?? 根据解
?1和?3的四种可能组合，??可以得到相应的四种可能值23，于是可得2
的四种可能解
?2??23??3
式中
?2取与?3相对应的值。
4) 求 ??4
令两边元素（1，3）和（2，3）分别对应相等，则可得
?c1c23ax?s1c23ay?s23az??c4s5??s1ax?c1ay?s4s5?
只要
s5?0，便可求出
?4
?4?atan2??s1ax?c1ay,c1c23ax?s1c23ay?s23az?当
s5?0时，机械手处于奇异形位。
5) 求 ??5
0T4?10T6?4T55T6
oxoyoz0axayaz0px?py???4T6pz??1??c1c4c23?s1s4??scc?sc?412314??c1s23?0?
s1c4c23?c1s4?s4s1c23?c1c4?s1s230?s23c4s23s4?c230?c3c4a2?d2s4?c4a3??nx?nc3s4a2?d2c4?s4a3???y??nzs3a2?d4??1??0实用文档
根据矩阵两边元素（1，3）和（2，3）分别对应相等，可得
??azs23c4?ax?c1c4c23?s1s4??ay?s1c4c23?c1s4??s5??axc1s23?ays23s1?azc23?c5??
?5?atan2?azs23c4?ax?c1c4c23?s1s4??ay?s1c4c23?c1s4?,?axc1s23?ays23s1?azc23?
6) 求 ??6
0T5?10T6?5T6
根据矩阵两边元素（2，1）和（1，1）分别对应相等，可得
?nx?c1s4c23?s1c4??ny?s4s1c23?c1c4??nzs23s4?s6?????nx?c1c4c5c23?s1c5s4?c1s5s23??ny?c4s1c5c23?s1s5s23?s4c1c5??nz?s5c23?c4s5s23??c6 从而求得
?6?atan2?s6,c6?④ 用Matlab编程得出工作空间
工作空间：机器人的手臂或手部安装点所能到达的所有空间区域，不包括手部本身所能到达的空间区域。
可将第5个坐标系的坐标原点看作手部安装点，计算工作空间时，将第5个坐标系的坐标原点当作动点，取
??6=0
将其带入步骤②计算出表达式 ????,????,????，即可求得动点的位置（工作空间）。相应的Matlab程序如下：
clc;clear format long
% 给出PUMA机器人的基本设计参数； a2 = 431.8; a3 = 20.32; d2 = 149.09; d4 = 433.07;
% 计算该机器人动点的位置，即 px，py，pz；
% 设置步长l,计数器初始值为1,并预先为px，py，pz分配内存空间； l = pi/180; k = 1;
px = linspace(0,1,100000); py = linspace(0,1,100000);
实用文档
pz = linspace(0,1,100000); for theta1 = -160*l:10*l:160*l for theta2 = -225*l:10*l:45*l for theta3 = -45*l:10*l:225*l
px(k) = a2*cos(theta1)*cos(theta2) - d2*sin(theta1) -
d4*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2)) + a3*cos(theta1)*cos(theta2)*cos(theta3) - a3*cos(theta1)*sin(theta2)*sin(theta3);
py(k) = d2*cos(theta1) - d4*(cos(theta2)*sin(theta1)*sin(theta3)
+ cos(theta3)*sin(theta1)*sin(theta2)) + a2*cos(theta2)*sin(theta1) +
a3*cos(theta2)*cos(theta3)*sin(theta1) - a3*sin(theta1)*sin(theta2)*sin(theta3);
pz(k) = - d4*cos(theta2 + theta3) - a3*sin(theta2 + theta3) - a2*sin(theta2);
k = k+1; end end end
% 根据px，py，pz的值,绘制工作空间的示意图，并设置标题等图形属性； plot3(px,py,pz,'.');
title('PUMA机器人的工作空间'); xlabel('X/mm'); ylabel('Y/mm'); zlabel('Z/mm'); grid on
最后得到PUMA的机器人的工作空间如下图所示：
实用文档