《KUKA机器人运动学分析及simmulink仿真.docx》由会员分享，可在线阅读，更多相关《KUKA机器人运动学分析及simmulink仿真.docx（19页珍藏版）》请在三一办公上搜索。
1、KUKA KR40PA机器人运动学分析及simmulink仿真一.Kuka KR40pA码垛机器人简介KukaKR40PA机器人是一种有四个自由度的码垛机器人，有四个驱动器，很好地运用了平 行四边形机构，固定其姿态从而大大简化了控制难度，并且提高了精度及寿命，本文所用 kuka码垛机器人如下图所示：二、机构简图，及其简化。1、机构简图如下：第一步简化原因：第一步我们简化了两个平行四边形机构，在此我们分析，这两个平行四 边形机构的作用是约束末端执行器在XZ平面的姿态，即：使末端执行器始终竖直向下。在 此我们人为的默认末端执行器始终竖直向下，不随前面传递构件的影响。此时便可以将两组 平行四边形机构
2、去除而不影响末端执行器的姿态和位移。第一步简化后机构简图第二步简化原因：在此我将主动杆1及连杆4去除。杆1 2 3 4组成了一个平行四边形机构， 因此P3=P2-P1.所以我们将杆1杆4去除，只要使P3=P2-P1便不影响末端执行器的位置和姿 态。第二步简化后的图形第三步简化原因：为了使参数更简洁，便于计算。我们将杆2的第一个关节与第一个旋转轴 相交，这样简化的模型更好计算。不影响总体机构的功能。最终简化后的机构简图三、建立连杆坐标系。如下图：四、D-H参数表ia i-1ai-idiei1000Qi2-9000e2301,0e】40L0e459000e5五、求正运动学公式- -o OO 1o
3、O1 OS 1 )-co O一一0- -o OO 1o lo O2C2O-SO 一。2 2C2OSO一一1T2c1c2c1s2 002T= 1T* 1T=s1c20c1 00100L0001c1c2c1s2000T= 2t* 3T=s1c20c1 001000001l200 1 l300 1 0 00 o O1 o o o 1 o OTOO -SC3OO-SC4OO-SOC5O 3 3 0 0 4 4 0 0 5 o s -s - os ( T T 23 34415- -o oo 1o O1 o011cs一一TO1I3c1c23 + c1c2l2-| I3s1c23 + s1c2l2 l s2
4、3 s2l10R 4 0T= 3T* 4T=00由于平行四边形机构的存在使得-1 0 01R= 0 0140 10所以0R400l3c1c23 + c1c2l2-| l3s1c23 + s1c2l2l s23 s2l01-c1-s1000010-s1c100c1c23 + c1c21 -%-2l3s1c23 + s1c2l2 l s23 s2l312c1 -s1 0-1 0 0-c1 0 -s10R= 0R * 1R=s1 c1 0*001=-s1 0 c1414001010010oT= ot* 4T=-c(1-5)-s(l - 5)0000s(1 - 5)c(1 - 5)-10I3c1c23
5、 + c1c2l2 I3s1c23 + s1c2l2 I3S23 s2l雅克比矩阵:0J=至此正解完成。六、运动学逆解-l s1c23 s1c2l-l c1s23 c1s2l-l c1s230032323l c1c23 + c1c2132s1s23 s1s2l2l s1s23000-l c23 - l c232-l c23000-s13-s1-s100c1c1c101000-10JV0Jw在此只对位置逆解进行分析，姿态逆解只与禹05有关因此很简单就可以计算出来。 假设我们给出目标位置在0坐标系表示坐标为（X , Y , Z ），由变换矩阵我们可以得出： X= I3c1c23 + c1c2l2Y
6、= l3s1c23 + s1c2l2Z= -l3s23 一 s2l2用式除以式可以得到tan禹=Y/X利用2幅角反正切公式可以得到91 =Atan2（y , x）。式与式可以写成如下形式：X/c1= l3c23 + c2l2-Z= l3s23 + s2l2两式左右两边分别平方相加得到下式:(X/c1)2 +z2= l22 +l32 +2l2l3 c3又因为tan1=Y/X 可解得 c3=（X2 +Y2 +72_l 2- l 2）/2l liJ /trr J m CQ（A + 1 +乙-2 七 ）/匕、2 3S3=V1-c32 （在此期望值S3大于0，因此取正）再次利用2幅角反正切公式可以得到9
7、3 Atan2（s3 , c3）。 可写成如下形式X/c1K2 c2-K3 s2-z K2 s2+K3 c2式中K2 l2 +l3c3K3 l3 s3r VK22 + K32Y = Atan2 (K3， K2)那么 K2 = rcosyK3 = rsinY式可以写成X/（rc1）=c（Y+2）Z/（-r）=s（Y+2）所以 Y+02 =Atan2 （-zcl, X）02 = Atan2 （-zcl, X） -Atan2 （K3, K2）至此逆解完成七、轨迹规划：加速度表达式是一个三次多项式，速度表达式是一个四次多项式，位移表达式是一个五次多 项式。设S为无量纲的运动位移，为无量纲的运动总时间，
8、V、A为无量纲的运动速度及 加速度，那么3-4-5多项式运动曲线可以简单表示如下：S = aT 5 + bV 4 + eV 3 + dT 2 + e+ fV = 5aT + 4bV 3 + 3eV 2 + 2dT + eA = 20aT 3 + 12bV 2 + 6eV + 2d + e（3-1）（3-2）（3-3）根据如下边界条件:(1)当 T = 0 时 S = 0 V = 0 A = 0(2)当= 1 时，S = 1，V = 0，A = 0将以上边界条件代入上述多项式表达式，可解出多项式的系数a = 6，b = 一15，e = 10，d = 0，e = 0，f = 0。于是，我们得到3-
9、4-5多项式运动曲线的表达式:(3-4)V = 30V 4 -603 + 30V 2(3-5)A = 1203-180 V 2 + 60 V（3-6）S = 6 T 5 -15T 4 + 10 T 3由加速度多项式可解出max = 5774为了得到带有量纲的多项式运动曲线表达式，我们令其加速度的最大值为amax，运行总时 间为T，运行瞬时时刻t =VX T，有a = 4 (120V3 - 180V2 + 60V)（3-7）5.774将上式对*积分得:v = m t(30r 4 - 60r 3 + 30r 2) 5.774同理有：a 一 -一一 一一s = -m t 2(6r 5 - i5r 4
10、 + ior 3) 5.774（3-8）（3-9）由此，当当 = T时:s = smaxa T 2max = 0.1732 a T25.774max(3-10)八、基于 matlab simmechanics工具箱的运动学仿真在matlab命令窗口中输入simulink命令回车 弹出simulink library browser如下图:ComrTHjnly LFe-d Blocks0 Slmullnk CQmm&nly Uaad, Blo-c：k&ii i-antlnuaus:. o-jseantinurtiesi Laglc mml Dtt OprstlnnsLacikup TablesM
11、ath Operationsr Model Ve-rificaticHi! Ports & Subua-atBniB! Sign a I Attributes! Slone I Routin & sinksLb&：up T-b hl e-5Dlcre-tel_gIq and BiLOpwriB tic-nFew Li & muAdditional M-ath & DIOTfiiA;Si-gncii Att-rlbuttsS-QufCC-EdUtllltlwSi-griBl Rouiln-gLTw-口Fi n Functionsi. ussr-De-Tined: Functions-EHU-IL
12、;广申由申申卜卜卜-|.|广厂印中l+l A-cMflianal Math DrciKR.t; a uwLmmM jlfld bc*i3r. &Jr dgfinbd br aacc inertlia.L uni. crdlnntE priern. an4 etcs Us cnt:t sfETEVltT CG)and nd ccd i-n: !: Lcn, unlai-i. Bad- kie./st Ecnn: t Joints 3= : uxtd xapk： ii: kLj. Ttiix di al-sg sim jzriWLEBx cpt iani-L .i*tt: ngx far cu.i
13、tsiEaxB-d tdr jot-r and MM.Rdpir日泰si: j. 4r-d4jirs.d rliid tody. Eodr daflud by 3 In&TLim w Ii and ciArdZyt三 alglni. si fg tnx&r at ETEn-lty ICG) and other TisEr-SDecified tk?Sy c-MTilinate eteIeij. This dlalw setE BodT EltdiiL Dcsitian emd mentation, irnles My Hud-gt ccnneotec! JcdntA sj- ac tuated
14、 sETEjatelr. This di日lx* Ieq jrraT-：d.*j! aEtxnnaij eksMacc Dr-ODGirt l;ME55：5PiKoSiUrvin F口Vac torllflTranx U Jit*Qr e *Lraaxpcin-iai-vn盘：导CS0 o见H m3 Ld吝h LdLi： TCS1s五们AdJLnlm vt回E. 11 -csz0 *0 flT B丁CSkPiiitica QxLfnTBtlcn | vmALLiaticinI区*凶5.网 NSi N-WfUiT：UnJbllMti-MD44 11 f Ccinv.日Li： T CG：=心年Wu
15、lU TEiil4J X- r WBody2的参数Fr bodj. Bcdf o*f ind by niK a, aid c： fr!ril zai 4 ulflns 工1fcs can taxr7-T*E9n . 1.ra sic hcdy. Bg.j dai hk. by aka.! a,a LaI, hh Escrdi nat- ccifa ni nd bci-i u= crn-i-T &fETilTltF (CC d CthGX 4y-pC*2llAd Ifcd COrdia ATMtGE. ThE dlalDE -tfi P&dr nltl-al isitiDn and. aiSen
16、cat Lan. uileacand dt ccfincrtti Jcints eh atunt-rt scEErirteLT. This i_ErarLtr (W-) dnd rthii ujer-5njT InlrtaL rwEjticn Bd OTlenr nricr.0 F-Kb :；lWiDr 3 Eint Fo VEtor rIftnTr JU3B 1 Ht EniponM M nLb. Cifi C 9二-rWul口T-Lf TC5LH 0 Q】2 TAdJvliLi.ni t已由皿nil ,3Ki| tesabn a flQ3： S1WCSfaPo-j Ltxan&T1C nt
17、CMlF. EUXl fk-fllttd VF fifil L inM-i: ti-niioc :! ,= z-ocrdina: erdi ini. and jj far ci-E-tar c-f flafLlr (rd bnl Qtls t；4E-；DGiEniGd P15, K4Ti3nbCG crstaa;. Thd ddaLoi cl: i ttd I.L ps-iii Aimi bh d -st ii-ntM： d-：i etIi-i tkitr 6rrf-,ci：F gni胸nd T-Ents ars aecidiGd. c-Eoaratarr. Th-； i3-alEr al p
18、rovi a-x CiptiaEJil ati: inga. Ecr cuxsecd 3,-d hsd需 .=，七=丁 jnd o&lsr.归” DZ-：oainacMrPttiesHits d dMi-difSn-rf rLeSd. bwlr. Mr deflntl tr w” l xnTli i. tEjcr . nd umpdluart- :-rifnx =d aaa. far Entr of 己rtHlXT i：网)blJ rther ue-Ej(-&KifEea Mr oDf-rdjult srstet Tta-i dalsi： xi.j Body tnxLal工 y= arL-in
19、tLtiscj ielLis BcSj Htd/pr optnetffld Jolirts et ictBited seDEJBrtLr. This dla-LE li: prixa sfilxcnlJ:*x 二 ni.二n:.二 = ba&7 F*3=t-7 &ncPD1GT.UjiecLdE凰X1*J1Joint actuator 的参数喘 Block Parameters: Joint Artustorl驱动函数模块的编程：1、基于末端执行器x,y,z分别按照轨迹规划路径的函数编程function c =sita(t,T,a1,b1,c1,a2,b2,c2) s1=1;s2 = 1;s3
20、=1;sxmax=abs(a2-a1);axmax=sxmax*5.774/T八2;if a2=a1s1 = 1;endx=a1+s1*(axmax/5.774)*(6*(t八5)/T八3-15*(t八4)/T八2+10*(t八3)/T);symax=abs(b2-b1);aymax=symax*5.774/T八2;if b2=b1s2 = 1;endy=b1+s2*(aymax/5.774)*(6*(t八5)/T八3-15*(t八4)/T八2+10*(t八3)/T);sxmax=abs(c2-c1);axmax=sxmax*5.774/T八2;if c2=c1s3=1;endz=c1+s3*
21、(axmax/5.774)*(6*(t八5)/T八3-15*(t八4)/T八2+10*(t八3)/T);m12=atan2(y,x);%关节1终点时旋转角度12=850;13=850;cs32=(x八2+y八2+z八2-12八2-13八2)/(2火12火13);ss32=sqrt(1-cs32八2);m32=atan2(ss32,cs32);%关节3终点旋转角度k22=12+13*cs32;k32=13*ss32;m22=atan2(-z*cos(m12),x)-atan2(k32,k22); % 关节2 终点旋转角度sita1=m12*18 0/pi;%转换成角度制sita2=m22*180
22、/pi;sita3=m32*180/pi;c=sita1;sita2;sita3;2、基于关节旋转角度分别按照轨迹规划路径的函数编程： function c =sita(t,T,a1,b1,c1,a2,b2,c2) m12=atan2(b2,a2);12=850;13=850;cs32=(a2八2+b2八2+c2八2-12八2-13八2)/(2火12火13);ss32=sqrt(1-cs32八2);m32=atan2(ss32,cs32);k22=12+13火cs32;k32=13火ss32;m22=atan2(-c2火cos(m12),a2)-atan2(k32,k22);m11=atan2
23、(b1,a1);cs31=(a1八2+b1八2+c1八2-12八2-13八2)/(2*12*13);ss31=sqrt(1-cs31八2);m31=atan2(ss31,cs31);k21=12+13火cs31;k31=13火ss31;m21=atan2(-c1*cos(m12),a1)-atan2(k31,k21);s1=1;s2 = 1;s3=1;sxmax=abs(m12-m11);axmax=sxmax*5.774/T八2;if m12=m11s1 = 1;endm1=m11 + s1*(axmax/5.774)*(6*(t八5)/T八3-15*(t八4)/T八2 + 10*(t八3)
24、/T);symax=abs(m22-m21);aymax=symax*5.774/T八2;if m22=m21s2=1;endm2=m21+s2*(aymax/5.774)*(6*(t八5)/T八3-15*(t八4)/T八2+10*(t八3)/T);szmax=abs(m32-m31);azmax=szmax*5.774/T八2;if m32=m31s3=1;endm3=m31+s3*(azmax/5.774)*(6*(t八5)/T八3-15*(t八4)/T八2+10*(t八3)/T);sita1=m1*180/pi;sita11=sita1;if sita11180sita1=sita11-
25、360;elseif sita11180sita2=sita22-360;elseif sita22180sita3=sita33-360;elseif sita33(-180)sita3=sita33+360;endc=sita1;sita2;sita3;运动学模型建成的效果如下:我们输入初始位移坐标，以及终点位移坐标，末端会根据规划的轨迹由初始位置，移动 到末端位置。九、Matlab动力学的仿真（1）对kuka机器人用solidworks进行建模 建成模型如下:（2）安装 simmechanicslink 插件安装方法：安装方法步骤如下：1. 选择对应的solidworks及matlab版
26、本的插件下载（mathwork公司免费），不需要解 压然后你打开MATLAB；2. 将下载的两个文件所在目录置为MATLAB当前路径；3. 在 MATLAB 命令窗口输入 install_addon（）命令，引号内 的是你下载的压缩文件的名字；4. 然后MATLAB就会将这个插件装上的；5. 最后在关联solidworks，主要分两步：1）在mat lab命令窗口运行smlink_linksw，提示成功;2）打开solidworks，点击工具，选择插件，再选择SimMechanics Link，将会看到 SimMechanics Link新的菜单（注意需要在打开装配体*.SLDASM时才能看到）。（3）将solidworks建成的模型导入到simmechanics。1. 将*.SLDASM装配体文件保存成*.xml文件，路径及文件名不能包含中文，否则无法 保存.2. 在matlab命令窗口输入命令mech_import命令选择生成的*.xml文件（*.xml文件应 包含在mat lab的搜索路径中）自动生成simulink模型图如下：此模型为自动生成，大多数参数不可修改，否则会报错。为模型图的三个主动关节加上驱动，输入一定的信号，通过对某些关节加上传感器示波器，我们可以直观的看出信号 输出，但是由于为内部计算生成，得不到数学模型。运行效果图如下：
