KUKA機(jī)器人運(yùn)動(dòng)學(xué)分析及simmulink仿真

1、 KUKA KR40PA機(jī)器人運(yùn)動(dòng)學(xué)分析及simmulink仿真1 Kuka KR40PA碼垛機(jī)器人簡(jiǎn)介Kuka KR40PA機(jī)器人是一種有四個(gè)自由度的碼垛機(jī)器人，有四個(gè)驅(qū)動(dòng)器，很好地運(yùn)用了平行四邊形機(jī)構(gòu)，固定其姿態(tài)從而大大簡(jiǎn)化了控制難度，并且提高了精度及壽命，本文所用kuka碼垛機(jī)器人如下圖所示：2、 機(jī)構(gòu)簡(jiǎn)圖，及其簡(jiǎn)化。1、 機(jī)構(gòu)簡(jiǎn)圖如下：第一步簡(jiǎn)化原因：第一步我們簡(jiǎn)化了兩個(gè)平行四邊形機(jī)構(gòu)，在此我們分析，這兩個(gè)平行四邊形機(jī)構(gòu)的作用是約束末端執(zhí)行器在XZ平面的姿態(tài)，即：使末端執(zhí)行器始終豎直向下。在此我們?nèi)藶榈哪J(rèn)末端執(zhí)行器始終豎直向下，不隨前面?zhèn)鬟f構(gòu)件的影響。此時(shí)便可以將兩組平行四邊形機(jī)構(gòu)去

2、除而不影響末端執(zhí)行器的姿態(tài)和位移。第一步簡(jiǎn)化后機(jī)構(gòu)簡(jiǎn)圖第二步簡(jiǎn)化原因：在此我將主動(dòng)桿1及連桿4去除。桿1 2 3 4組成了一個(gè)平行四邊形機(jī)構(gòu)，因此3=2-1.所以我們將桿1桿4去除，只要使3=2-1便不影響末端執(zhí)行器的位置和姿態(tài)。第二步簡(jiǎn)化后的圖形第三步簡(jiǎn)化原因：為了使參數(shù)更簡(jiǎn)潔，便于計(jì)算。我們將桿2的第一個(gè)關(guān)節(jié)與第一個(gè)旋轉(zhuǎn)軸相交，這樣簡(jiǎn)化的模型更好計(jì)算。不影響總體機(jī)構(gòu)的功能。最終簡(jiǎn)化后的機(jī)構(gòu)簡(jiǎn)圖3、 建立連桿坐標(biāo)系。如下圖：4、 D-H參數(shù)表 i i-1 ai-1 di i 10 0 0 1 2 -90 0 0 2 3 0 l2 0 3 4 0 l3 0 4 5 90 0 0 55、 求正運(yùn)

3、動(dòng)學(xué)公式 10T=c1-s1s1c100000000100121T=c2-s2000010-s2-c200000132T=c3-s3s3c30l2000000100143T=c4-s4s4c40l3000000100154T=c5-s50000-10s5c500000110T=c1-s1s1c1000000001001 20T= 10T*21T=c1c2-c1s2s1c2c1000000001001 30T= 20T*32T= c1c2-c1s2s1c2c1000000001001 40T= 30T*43T= 40Rl3c1c23+c1c2l2l3s1c23+s1c2l200-l3s23-s2

4、l201 由于平行四邊形機(jī)構(gòu)的存在使得 41R= -100000110 40R= 10R * 41R= c1-s1s1c100001 * -1000 00 110 = -c10-s10-s1c1 0 10 所以 40T= 40Rl3c1c23+c1c2l2l3s1c23+s1c2l200-l3s23-s2l201= -c1 0-s1l3c1c23+c1c2l2-s1 0c1l3s1c23+s1c2l20 1 0-l3s23-s2l20 0 01 50T= 40T* 54T= -c(1-5)-s(1-5)0l3c1c23+c1c2l2-s(1-5)c(1-5)0l3s1c23+s1c2l20 0

5、 -1-l3s23-s2l20 0 01 雅克比矩陣： 50J=50JV50Jw=-l3s1c23-s1c2l2-l3c1s23-c1s2l2-l3c1s2300l3c1c23+c1c2l2-l3s1s23-s1s2l2-l3s1s23000-l3c23-l2c2-l3c23000-s1-s1-s100c1c1c101000-1 至此正解完成。 六、運(yùn)動(dòng)學(xué)逆解在此只對(duì)位置逆解進(jìn)行分析，姿態(tài)逆解只與1 5 有關(guān)因此很簡(jiǎn)單就可以計(jì)算出來(lái)。假設(shè)我們給出目標(biāo)位置在0坐標(biāo)系表示坐標(biāo)為( X , Y , Z ) ,由變換矩陣我們可以得出：X= l3c1c23+c1c2l2 Y= l3s1c23+s1c2l

6、2 Z= -l3s23-s2l2 用 式除以 式可以得到 tan1 =Y/X 利用2幅角反正切公式可以得到 1 =Atan2（y , x）。 式與 式可以寫(xiě)成如下形式：X/c1= l3c23+c2l2 -Z= l3s23+s2l2 兩式左右兩邊分別平方相加得到下式：(X/c1)2+z2= l22+l32+2l2l3c3 又因?yàn)?tan1=Y/X 可解得c3=(X2+Y2+Z2-l22- l32)/2l2l3 S3=1-c32 (在此期望值S3大于0，因此取正)再次利用2幅角反正切公式可以得到 3 =Atan2（s3 , c3）。 可寫(xiě)成如下形式X/c1=K2c2-K3s2 -z= K2s2+K

7、3c2 式中 K2= l2+l3c3 K3= l3s3r= K22+K32 =Atan2（K3，K2） 那么 K2=rcos K3=rsin 式可以寫(xiě)成X/(rc1)=c(+2)Z/(-r)=s(+2)所以 +2 =Atan2（-zc1，X）2= Atan2（-zc1，X）-Atan2（K3，K2） 至此逆解完成 七、軌跡規(guī)劃：加速度表達(dá)式是一個(gè)三次多項(xiàng)式，速度表達(dá)式是一個(gè)四次多項(xiàng)式，位移表達(dá)式是一個(gè)五次多項(xiàng)式。設(shè)S為無(wú)量綱的運(yùn)動(dòng)位移，為無(wú)量綱的運(yùn)動(dòng)總時(shí)間，V、A為無(wú)量綱的運(yùn)動(dòng)速度及加速度，那么3-4-5多項(xiàng)式運(yùn)動(dòng)曲線可以簡(jiǎn)單表示如下： （3-1） （3-2） （3-3）根據(jù)如下邊界條件：(1

8、) 當(dāng)時(shí)，；(2) 當(dāng)時(shí)，。將以上邊界條件代入上述多項(xiàng)式表達(dá)式，可解出多項(xiàng)式的系數(shù)，。于是，我們得到3-4-5多項(xiàng)式運(yùn)動(dòng)曲線的表達(dá)式：（3-4）（3-5）（3-6）由加速度多項(xiàng)式可解出。為了得到帶有量綱的多項(xiàng)式運(yùn)動(dòng)曲線表達(dá)式，我們令其加速度的最大值為，運(yùn)行總時(shí)間為，運(yùn)行瞬時(shí)時(shí)刻，有 （3-7）將上式對(duì)積分得： （3-8）同理有： （3-9）由此，當(dāng)時(shí)： （3-10）八、基于matlab simmechanics工具箱的運(yùn)動(dòng)學(xué)仿真在matlab命令窗口中輸入 simulink命令回車(chē) 彈出simulink library browser如下圖： 新建一個(gè)model ，在左側(cè) libraries

9、中選擇simscape 接著選擇simmechanics 將我們見(jiàn)立模型的構(gòu)建分別拖到model并組成下圖形式 ，其中 各模塊參數(shù)輸入如下： 環(huán)境模塊 關(guān)節(jié)模塊 Body0 的參數(shù) Body1的參數(shù) Body2的參數(shù) Joint actuator的參數(shù)驅(qū)動(dòng)函數(shù)模塊的編程：1、基于末端執(zhí)行器x,y,z分別按照軌跡規(guī)劃路徑的函數(shù)編程function c =sita(t,T,a1,b1,c1,a2,b2,c2)s1=1;s2=1;s3=1;sxmax=abs(a2-a1);axmax=sxmax*5.774/T2;if a2<a1 s1=-1;elseif a2>=a1 s1=1;end

10、x=a1+s1*(axmax/5.774)*(6*(t5)/T3-15*(t4)/T2+10*(t3)/T);symax=abs(b2-b1);aymax=symax*5.774/T2;if b2<b1 s2=-1;elseif b2>=b1 s2=1;endy=b1+s2*(aymax/5.774)*(6*(t5)/T3-15*(t4)/T2+10*(t3)/T);sxmax=abs(c2-c1);axmax=sxmax*5.774/T2;if c2<c1 s3=-1;elseif c2>=c1 s3=1;endz=c1+s3*(axmax/5.774)*(6*(t5

11、)/T3-15*(t4)/T2+10*(t3)/T);m12=atan2(y,x); %關(guān)節(jié)1終點(diǎn)時(shí)旋轉(zhuǎn)角度l2=850;l3=850;cs32=(x2+y2+z2-l22-l32)/(2*l2*l3);ss32=sqrt(1-cs322);m32=atan2(ss32,cs32); %關(guān)節(jié)3終點(diǎn)旋轉(zhuǎn)角度k22=l2+l3*cs32;k32=l3*ss32;m22=atan2(-z*cos(m12),x)-atan2(k32,k22); %關(guān)節(jié)2終點(diǎn)旋轉(zhuǎn)角度sita1=m12*180/pi; %轉(zhuǎn)換成角度制sita2=m22*180/pi;sita3=m32*180/pi; c=sita1;s

12、ita2;sita3;2、基于關(guān)節(jié)旋轉(zhuǎn)角度分別按照軌跡規(guī)劃路徑的函數(shù)編程：function c =sita(t,T,a1,b1,c1,a2,b2,c2)m12=atan2(b2,a2);l2=850;l3=850;cs32=(a22+b22+c22-l22-l32)/(2*l2*l3);ss32=sqrt(1-cs322);m32=atan2(ss32,cs32);k22=l2+l3*cs32;k32=l3*ss32;m22=atan2(-c2*cos(m12),a2)-atan2(k32,k22);m11=atan2(b1,a1);cs31=(a12+b12+c12-l22-l32)/(2*

13、l2*l3);ss31=sqrt(1-cs312);m31=atan2(ss31,cs31);k21=l2+l3*cs31;k31=l3*ss31;m21=atan2(-c1*cos(m12),a1)-atan2(k31,k21); s1=1;s2=1;s3=1;sxmax=abs(m12-m11);axmax=sxmax*5.774/T2;if m12<m11 s1=-1;elseif m12>=m11 s1=1;endm1=m11+s1*(axmax/5.774)*(6*(t5)/T3-15*(t4)/T2+10*(t3)/T);symax=abs(m22-m21);aymax

14、=symax*5.774/T2;if m22<m21 s2=-1;elseif m22>=m21 s2=1;endm2=m21+s2*(aymax/5.774)*(6*(t5)/T3-15*(t4)/T2+10*(t3)/T);szmax=abs(m32-m31);azmax=szmax*5.774/T2;if m32<m31 s3=-1;elseif m32>=m31 s3=1;endm3=m31+s3*(azmax/5.774)*(6*(t5)/T3-15*(t4)/T2+10*(t3)/T);sita1=m1*180/pi;sita11=sita1;if sita

15、11>180 sita1=sita11-360;elseif sita11<(-180) sita1=sita11+360;end sita2=m2*180/pi;sita22=sita2;if sita22>180 sita2=sita22-360;elseif sita22<(-180) sita2=sita22+360;endsita3=m3*180/pi;sita33=sita3;if sita33>180 sita3=sita33-360;elseif sita33<(-180) sita3=sita33+360;endc=sita1;sita2;

16、sita3;運(yùn)動(dòng)學(xué)模型建成的效果如下：我們輸入初始位移坐標(biāo)，以及終點(diǎn)位移坐標(biāo)，末端會(huì)根據(jù)規(guī)劃的軌跡由初始位置，移動(dòng)到末端位置。九、Matlab動(dòng)力學(xué)的仿真（1）對(duì)kuka機(jī)器人用solidworks進(jìn)行建模建成模型如下：（2）安裝simmechanicslink插件安裝方法：安裝方法步驟如下：1.選擇對(duì)應(yīng)的solidworks及matlab版本的插件下載（mathwork公司免費(fèi)），不需要解壓然后你打開(kāi)MATLAB；2. 將下載的兩個(gè)文件所在目錄置為MATLAB當(dāng)前路徑；3. 在MATLAB命令窗口輸入install_addon('<add-on ZIP file name>')命令,引號(hào)內(nèi)的是你下載的壓縮文件的名字；4. 然后MATLAB就會(huì)將這個(gè)插件裝上的;5.最后在關(guān)聯(lián)solidworks，主要分兩步：1）在matlab命令窗口運(yùn)行smlink_linksw，提示成功；2）打開(kāi)solidworks，點(diǎn)擊工具，選擇插件，再選擇 SimMechanics Link，將會(huì)看到SimMechanics Link新的菜單（注意需要在打開(kāi)裝配體*.SLDASM時(shí)才能看到）。（3）將solidw