PUMA 560运动分析综合
PUMA 560运动分析综合 PUMA 560机器人的运动分析综合共包括三方面的内容: 1. PUMA 560机器人的正解和逆解 2. PUMA 560机器人的雅克比矩阵的求解 3.PUMA 560机器人的正向动力学分析 F面分别介绍这三个方面的分析过程和程序设计。 PUMA 560机器人的正解和逆解 对机器人进行建模,既可以根据机器人的具体构型对其进行建 模,也可以采用D-H方法。D-H模型表示了对机器人连杆和关节进 行建模的一种非常简便的方法,它可以用于任何机器人构型,而不用 去考虑机器人的具体构型。D-H方法已经成为表示机器人和对机器 人运动进行建模的标准方法。 在对机器人进行设计时,通常需要考虑单个连杆的许多特性。但 是当我们建立机器人的运动学方程时,只需要考虑连杆的有限个参数 即可。 在描述连杆时,需要用到连杆的长度ai和连杆的扭角刃。在确 定杆件相对位置时,需要用到两杆件间的距离di和两杆件的夹角优两 个参数。 下面是PUMA 560正解部分的程序: ::七 function [T6] = robot_zhengjie(al/a2/a3/a4/a5/a6Jength2/length3/d2/d4/d6) %用于机器人正向动力学分析的函数 %函数头上的T6是为了最终显示出预算结果 %使用a来表示两相邻杆的夹角,由于机器人具有6个自由度,因此使用 al#a2/…#a6 %使用d来表示坐标轴的距离 %使用length来表示杆长 %使用T来标记相邻连杆的齐次变换矩阵, %例如{2}坐标系相对于{1}坐标系的齐次变换矩阵记为T21,相对于基坐标系极 为 %TnOz %在这里给程序赋值以方便下面进行验证 %al=pi/2;a2 = 0;a3=-pi/2;a4=0;a5=0;a6=0; %length2=431.8;d2=149.09;length3=20.32;d4=433.07;d6=56.25; T10=[cos(al)/-sin(al)/0/0; sin(al),cos(al)Q0;0Ql,0;0,0Ql]; T21 = [cos(a2)/-sin(a2)/0/0;0/0/l/d2; -sin(a2)/-cos(a2)/0/0;0/0z0/l]; T32=[cos(a3)厂 sin (a3)/0/length2;sin(a3)/cos(a3)/0/0;0,0/l/0;0/0/0,l]; T43=[cos(a4),-sin(a4)/0/length3;0/0/l/d4;-sin(a4)/-cos(a4)/0/0;0/0/0/l]; T54=[cos(a5)/-sin(a5)/0,0;0/0/-l/0;sin(a5)/cos(a5)/0/0;0/0,0,1]; T65 = [cos(a6)/-sin(a6)/0l0;0/0/l/0;-sin(a6)/-cos(a6)/0/0;0/0/0/l]; T60=T10*T21*T32*T43*T54*T65; 在正解中需要使用到的参数有两相邻杆之间的夹角al/a2/./a6/ff的 长度Iength2jength3相邻两杆间的距离d2/d4,d6o这些变量保存 在mat文件T60mat中,每次打开Matlab后,首先加载T60.mat 文件,然后在命令行窗口中输入命令: robot_zhengjie(pi/2/0/-pi/2/0/0/0/length2Jength3/d2,d4/d6)即 可求得齐次变换矩阵T60如下: 0.0000 1.0000 0.0000 -149.0900 0.0000 -0.0000 1.0000 864.8700 1.0000 0 -0.0000 203200 0 0 0 1.0000 与课本上求解得到的齐次变化矩阵相同,经过使用多组不同的数据进 行验证,证明程序是正确的。 下面是逆解部分的程序: function [Result] = robot_nijie(T60Jength2/length3/d2,d4zd6) n x=T60(l/l);ox=T60(l/2);ax=T60(l,3);px=T60(l/4); ny=T60(2/l);oy=T60(2/2);ay=T60(2/3);py=T60(2/4); nz=T60(3/l);oz=T60(3/2);az=T60(3/3);pz=T60(3/4); %下面是具体的求解过程 for i=0:l if(i = =0) al=atan2(py,px)-atan2(d2/sqrt(pxA2+pyA2-d2A2));% 解 1 else al=atan2(py/px)-atan2(d2/-sqrt(pxA2 + pyA2-d2A2));% 解 2 end k=(pxA2 + pyA2+pzA2-length2A2-length3A2-d2A2-d4A2)/(2* Iength2); for j=O:l if(j==O) a3=atan2(length3,d4)-atan2(k/sqrt(length3A2+d4A2-kA2));% 解] else a3=atan2(length3/d4)-atan2(k/-s qrt(length3A2+d4A2-kA2)); %解2 end s_factor_23=( -Iength3-length2*cos(a3))*pz+(cos(al)*px+sin(a I)*py)*(length2*sin(a3)-d4); c_factor_23=(-d4+length2*sin(a3))*pz+(cos(al)*px+sin(al)*py )*(length2*cos(a3)+length3); a23=atan2(s_factor_23,c_factor_23); a2=a23-a3; for t=0:l if(t==O) a4=ata n2(-ax*sin(al)+ay*cos(al),-ax*cos(al)*cos(a23)・ay^sin( al)*cos(a23)+az*sin(a23)); else a4=atan2(ax*sin(al)-ay*cos(al),ax*cos(al)*cos(a23)+ay*si n(al )*cos(a23)-az*sin(a23)); end s_factor_5 = -ax*(cos(al)*cos(a23)*cos(a4)+sin(al)*sin(a4))-ay*( sin(al)*cos(a23)*cos(a4)-cos(al)*sin(a4))+az* (sin(a23) *co s(a4)) ;%解1 c_factor_5=ax*(-cos(al)*sin(a23))+ay*(-sin(al)*sin(a23))+az*(- cos(a23)); a5=atan2(s_factor_5/C_factor_5); s__factor_6=-nx*(cos(al)*cos(a23)*sin(a4)-sin(a