1、PUMA 机器人正逆运动学推导及运动空间解算 求解： 建立坐标系； 给出 D-H 参数表； 推导正、逆运动学； 编程得工作空间 1. 建立坐标系 根据 PUMA 机器人运动自由度，在各关节处建立坐标系如图 2 所示。 图 1 PUMA560 机器人坐标系图 2. D-H 参数表 A4 A6 A5 D-H 参数 表可根据坐标系设定而得出，见表 1。 (1) i 为绕 1iZ 轴从 1iX 到 iX 的角度 ； (2) 1i 为绕 iX 轴从 1iZ 到 iZ 的角度 ； (3) 1ia 为沿 iX 轴从 1iZ 与 iX 交点到 iO 的距离 ； (4) id 为沿 1iZ 轴从 1iZ 与 i
2、X 交点 到 1iO 的距离 。 表 1 PUMA 机器人的杆件参数表 连杆 i 变量 i 1i 1ia id 变量范围 1 1 90 90 0 0 160 160 2 2 0 0 2a 2d 225 45 3 3 90 90 3a 0 45 225 4 4 0 90 0 4d 110 170 5 5 0 90 0 0 100 100 6 6 0 0 0 6d 266 266 3. 正运动学推导 由坐标系图及各杆件参数可得个连杆变换矩阵。 11110100000 1 1 00 0 0 1csscT 2 2 2 22 2 2 2122000 0 10 0 0 1c s c as c s aTd
3、3 3 3 33 3 3 323000 1 0 00 0 0 1c s c as c s aT 4444344000 1 00 0 0 1csscTd 55554500000 1 0 00 0 0 1csscT666656600000 0 10 0 0 1csscTd根据各连杆变换矩阵相乘，可以得到 PUMA560 的机械手变换矩阵，其矩阵为关节变量的函数。 0 0 1 2 3 4 56 1 1 2 2 3 3 4 4 5 5 6 6T T T T T T T 将上述变换矩阵逐个依次相乘可以得到 06T 。 060 0 0 1x x x xy y y yz z z zn o a pn o a
4、pTn o a p 6 5 1 4 1 4 23 1 5 23 6 4 1 1 23 46 5 1 4 4 23 1 1 5 23 6 1 4 23 1 44 6 23 6 23 5 4 5 236 5 1 4 1 4 23 1 5 23 6 4 1 1 23 46 5 1 4 4 23 1 1 5 23 6xyzxyn c c s s c c c c s s s c s c c sn c c c s c c s s s s s c c c s sn s s s c c s c c so s c s s c c c c s s c c s c c so s c c s c c s s s s
5、c 1 4 23 1 46 23 5 4 5 23 6 4 231 5 23 5 1 4 1 4 235 1 23 5 1 4 4 23 15 23 4 5 231 2 2 3 23 2 1 6 5 1 4 1 4 23 1 5 23 1 4 231 2 2 3 23 1 2 6 5 1 4 4 23 1zxyzxyc c c s so s c s c c s c s sa c c s s s s c c ca c s s s c s c c sa a c c s sp c a c a c d s d s s s c c c c c s c d sp s a c a c c d d s c s
6、 c c s c 5 1 23 4 1 2323 4 2 2 3 23 6 5 23 4 5 23zs s d s sp c d a s a s d c c c s s 上式中 2 3 2 3 2 3 2 3c o s , s i ncs 。 将 1 3 2 4 5 69 0 , 0 带入上式检验，得结果 22 4 60630 1 00 0 11 0 00 0 0 1da d dTa 计算结果与实际结果一致，故上述推导无误。 4. 逆运动学推导 逆运动学推导为由末端位姿 , , ,noap 确定，求解关节变量 i 。 110111000 0 1 0000 0 0 1csT sc 2 2 222
7、1220000 0 10 0 0 1c s ascTd 3 3 3233300 0 1 0000 0 0 1c s bT sc 4443444000 0 1000 0 0 1csdTsc 554555000 0 1 0000 0 0 1csT sc 666656600000 0 10 0 0 1csscTd 1. 求 1 采用逆变换 0111T 左乘 06T ，可得： 0 1 0 1 2 3 4 51 1 6 2 2 3 3 4 4 5 5 6 6T T T T T T T 111611000 0 1 0000 0 0 1 0 0 0 1x x x xy y y yz z z zc s n o
8、 a pn o a p Ts c n o a p 1 1 1 1 1 1 1 1161 1 1 1 1 1 1 10 0 0 1x y x y x y x yz z z zx y x y x y x yc n s n c o s o c a s a c p s pn o a pTs n c n s o c o s a c a s p c p 1 2 3 4 52 2 3 3 4 4 5 5 6 64 6 23 6 5 23 4 5 23 6 5 23 4 5 23 6 4 235 23 4 5 23 4 23 2 2 6 5 23 4 5 23 3 234 6 23 6 5 23 4 5 23
9、 6 5 23 4 5 23 6 4 235 23 4 5 23 4 23 2 2,;,T T T T Ts s c c s s c c c s s s c c c s cc s c s c d s a c d c s c s c a cs s s c s c c c s s s c c c s c s sc c c s s d c a s 6 5 23 4 5 23 3 234 6 5 6 4 4 6 5 4 6 4 5 2 6 4 5;, , , ; 0 , 0 , 0 , 1d c c c s s a sc s c c s c c c s s s s d d s s 可得 3,3 , 3
10、,4 对应相等， 故有 1 1 4 5 1 1 2 6 4 5,x y x ys a c a s s s p c p d d s s ，联立求解 ，设 ： 226 1 6 1 1 6 6c o s , s in , , a r c ta n yx x y y x x y y xpp a d p a d p a d p a d p 可以有如下推导： 21 2 1 2 1121 22126 21 226 12sin , c os 1a r c ta na r c ta n a r c ta nyyxxddddp a d dp a d d 2. 求 23, 同 理有： 0 1 0 2 3 4 51
11、1 2 6 3 3 4 4 5 5 6 6,T T T T T T 0 1 01 1 2 61 2 2 2 1 1 2 2 2 11 2 2 2 1 1 2 2 2 2 11 2 2 1 2 1 2 2 1 21 2 2 1 2 1 2 2 1 21 1 1 1 1,;,;,x z y x z yx z y x z yx z y x z yx z y x z yy x y x yTTc c n n s c n s c c o o s c o sa c c a s a c s c c p p s a c p sc n s c n n s s c o s c o o s sa c s a c a
12、s s c p s c p p s sc n n s c o o s a c 1 1 2 1,;0 , 0 , 0 , 1x y xa s c p d p s 2 3 4 53 3 4 4 5 5 6 66 3 5 3 4 5 3 4 6 6 3 5 3 4 5 3 6 4 5 3 3 4 54 3 3 3 6 5 3 3 4 56 3 5 4 5 3 3 4 6 6 3 5 4 5 3 6 3 4 3 3 4 3 53 4 3 3 6 3 5 4 3 54 6 5 6 4 4 6, , ,;, , ,;,T T T Tc s s c c c c s s s s s c c c c c s c
13、 s c c sd s a c d c s c c cc c s c c s s s s s c s c c s c s s c c c s sc d a s d c c c s cc s c c s c c c 5 4 6 4 5 6 4 5, , ; 0 , 0 , 0 , 1s s s s d s s结合前一步骤公式可得： 1 2 2 2 2 1 4 3 3 3 6 5 3 3 4 5x z yc c p p s a c p s d s a c d c s c c c 1 2 2 1 2 3 4 3 3 6 3 5 4 3 5x z yc p s c p p s s c d a s d
14、c c c s c 设 22 42 3 4 3, a r c ta n dad a ， 2 2 6 1 6 1 1 13 6 1 6 1 1 1 6 6, a r c ta n x y x yx y x y z z zzd a c a d s c p s pd a c a d s c p s p p d a p d a 可得计算公式： 2 3 3 2 2 , 2 3 3 2c o s s i n s i n c o s 0a 整理 得： 22 2 2 2 22 2 3 2 2 43 2 2 23 2 2 32 2 26 1 6 1 1 12 2 32 22 2 2 2 2 62 2 2 2 3
15、4a r c ta n a r c ta na r c ta n a r c ta n4x y x yzzaa daad a c a d s c p s pap d aaa 3. 求 4 为求解 2 ，做如下变换： 0 1 0 4 54 1 2 3 4 6 5 5 6 6, , ,T T T T 0 1 03 1 2 3 4 61 4 4 23 1 1 4 1 4 23 4 23 1 4 4 23 1 1 4 1 4 23 4 231 4 4 23 1 1 4 1 4 23 4 23224 3 23 2 2 23 3 23 2 2 23 2 4 1 4 1 4 23 1 4 4 23, , ,
16、y x z y x zy x zxyTTn c s c c s n s s c c c c n s o c s c c s o s s c c c c o sa c s c c s a s s c c c a c cc a c a c c a s a s s d s p s s c c c p c s c c 1 4 2323 1 23 1 23 23 1 23 1 23 23 1 23 1 234 23 2 2 23 2 23 2 1 23 1 231 4 23 1 4 4 1 1 23 4 4 231 4 23 1 4 4 1 1 23 4 4 23;, , ,;,zz x y z x y
17、 z x yz x yy x zy x zys c p sc n c n s n s s c o c o s o s s a c a c s a s sd c p a c s a c s c p s p s sn c c c s s n c s c c s n s so c c c s s o c s c c s o s sac 1 4 23 1 4 4 1 1 23 4 4 23221 4 23 1 4 4 3 23 2 2 23 3 23 2 2 23 4 1 1 23 4 4 2 4 23,;0 , 0 , 0 , 1xzy x zc c s s a c s c c s a s sp c
18、 c c s s s a c a c c a s a s s p c s c c s c d p s s 5 6 5 6 5 6 56 5 5 6 5 5 6455 5 6 666 000 0 0 1c c c s s d sc s s s c c dTTsc 联立求解得： 1 4 2 3 1 4 4 1 1 2 3 4 4 2 3 0y x za c c c s s a c s c c s a s s 1142 3 1 1 2 3 2 3a r c ta n yxy x za c a sa c s a c c a s 4. 求 56, 0 1 0 55 1 2 3 4 5 6 6 6, ,
19、, ,T T T 可得： 1 5 2 3 4 2 3 5 1 4 5 1 5 2 3 4 2 3 5 1 4 5 5 2 3 4 5 2 31 4 2 3 1 4 4 1 1 2 3 4 4 2 3 61 4 2 3 1 4 4 1 1 2 3 4 4 2 3 60x y zy x zy x zo c c c c c s s s s o s c s c c s c s s o c c c s sn c c c s s n c s c c s n s s so c c c s s o c s c c s o s s c 得： 1 4 1 4 2 3 1 4 2 3 1 4 4 2 351 2 3
20、 1 2 3 2 3a r c ta n x x y y zx y zo s s o c c s o s c c o c s o c so c s o s s o c 1 4 2 3 1 4 4 1 1 2 3 4 4 2 36 1 4 2 3 1 4 4 1 1 2 3 4 4 2 3a r c ta n y x zy x zn c c c s s n c s c c s n s so c c c s s o c s c c s o s s 从计算过程 ，我们可以看出其运动反解存在多组解，此时需要根据机构自身运动范围及运动的优化进行选择。 5. 工作空间 使用 MATLAB 绘制其工作空间如
21、图 2-图 5。 此处采用正解方式 进行工作空间求解，理论上经过 6 层循环可以，加之运动范围限制可以得出机器人工作空间，实际求解过程中， 为保证运算速度 能够适应，将工作空间进行简化， 考虑 4、 5、6 部分主要影响末端小范围，对总体工作空间影响较小，故 将 4、 5、 6 轴运动固定，仅对 1 2 3, 进行 组循环计算。 正解及反解计算过程中使用的部分 MATLAB 代码： syms c1 c2 c3 c4 c5 c6 t1 t2 t3 t4 t5 t6 d2 a2 d4 a3 d6; t1=cos(c1) 0 -sin(c1) 0; sin(c1) 0 cos(c1) 0; 0 -1
22、 0 0 ; 0 0 0 1; t2=cos(c2) -sin(c2) 0 cos(c2)*a2; sin(c2) cos(c2) 0 sin(c2)*a2; 0 0 1 d2 ; 0 0 0 1; t3=cos(c3) 0 sin(c3) -cos(c3)*a3; sin(c3) 0 -cos(c3) -sin(c3)*a3; 0 1 0 0 ; 0 0 0 1; t4=cos(c4) 0 -sin(c4) 0; sin(c4) 0 cos(c4) 0; 0 -1 0 d4; 0 0 0 1; t5=cos(c5) 0 sin(c5) 0; sin(c5) 0 -cos(c5) 0; 0 1
23、 0 0; 0 0 0 1; t6=cos(c6) -sin(c6) 0 0; sin(c6) cos(c6) 0 0; 0 0 1 d6; 0 0 0 1; t11=inv(t1); t21=inv(t2); t31=inv(t3); t41=inv(t4); t51=inv(t5); t61=inv(t6); t=t1*t2*t3*t4*t5*t6 图 2 工作空间 轴测图 图 3 工作空间 XZ 平面 投影 图 4 工作空间 YZ 平面 投影 图 5 工作空间 XY 平面 投影 工作空间 正解求解 MATLAB 代码如下 。 clc; clear; %定义各项杆件参数 alpha1=0;
24、 alpha2=-pi/2; alpha3=0; alpha4=pi/2; alpha5=-pi/2; alpha6=pi/2; a1=0; a2=431.8; a3=-20.32; a4=0; a5=0; a6=0; d1=0; d2=149.09; d3=0; d4=433.07; d5=0; d6=56.25; %定义角度范围，计算变换矩阵，开始循环计算工作空间 for c1=(-160:10:160)*pi/180 T01=cos(c1) 0 -sin(c1) 0; sin(c1) 0 cos(c1) 0; 0 -1 0 0 ; 0 0 0 1; for c2=(-225:10:45)
25、*pi/180 T12=cos(c2) -sin(c2) 0 cos(c2)*a2; sin(c2) cos(c2) 0 sin(c2)*a2; 0 0 1 d2 ; 0 0 0 1; T02=T01*T12; for c3=(-45:10:225)*pi/180 T23=cos(c3) 0 sin(c3) -cos(c3)*a3; sin(c3) 0 -cos(c3) -sin(c3)*a3; 0 1 0 0 ; 0 0 0 1; T03=T02*T23; c4=0*pi/180;%此处为减少计算量，将角度赋常值，省去循环语句 %for c4=(-110:20:170)*pi/180 T34
26、=cos(c4) 0 -sin(c4) 0; sin(c4) 0 cos(c4) 0; 0 -1 0 d4; 0 0 0 1; T04=T03*T34; c5=0*pi/180;%for c5=(-100:20:100)*pi/180 T45=cos(c5) 0 sin(c5) 0; sin(c5) 0 -cos(c5) 0; 0 1 0 0; 0 0 0 1; T05=T04*T45; c6=0;%for c6=(-266:20:266)*pi/180 T56=cos(c6) -sin(c6) 0 0; sin(c6) cos(c6) 0 0; 0 0 1 d6; 0 0 0 1; T06=T05*T56; Object=T06*0;0;0;1; plot3(Object(1),Object(2),Object(3);%循环绘图 hold on;%保持绘图，将每次循环结果叠加 %end %end %end end end end view(0,0);%转变角度观察所绘三维图，此时为 XZ 平面 xlabel(X); grid on; ylabel(Y); zlabel(Z); hold off;
