工業(yè)機(jī)器人課程設(shè)計(jì)報告

1、-. z工業(yè)機(jī)器人課程設(shè)計(jì)基于 Matlab 的工業(yè)機(jī)器人運(yùn)動學(xué)和雅克比較矩陣求解目錄PUMA560PUMA560 機(jī)器人簡介機(jī)器人簡介 4 4PUMA560PUMA560 機(jī)器人的正解機(jī)器人的正解 5 51、確定 D-H 坐標(biāo)系 52、確定各連桿 D-H 參數(shù)和關(guān)節(jié)變量 53、求出兩桿間的位姿矩陣 54、求末桿的位姿矩陣 65、MATLAB編程 76、驗(yàn)證 7PUMA560PUMA560 機(jī)器人的逆解機(jī)器人的逆解 8 81、求182、求383、求294、求4105、求5106、求6117、解的多重性 118、MATLAB編程 119、對于機(jī)器人解的分析 12-. z機(jī)器人的雅克比矩陣機(jī)器人的

2、雅克比矩陣 12121、定義 122、雅可比矩陣的求法 123、微分變換法求機(jī)器人的雅可比矩陣 134、矢量積法求機(jī)器人的雅克比矩陣 155、MATLAB編程 15附錄附錄 16161、程序 162、三維圖 24摘要機(jī)器人學(xué)作為一門高度穿插的前沿學(xué)科，引起許多具有不同專業(yè)背景人們的廣泛興趣，對其進(jìn)展深入研究，并使其獲得快速開展。尤其是近年來各種新興技術(shù)飛速開展,機(jī)械工業(yè)產(chǎn)品的自動化、高精度、重負(fù)載等性能指標(biāo)變得越來越突出。因此在機(jī)器人學(xué)的計(jì)算中就要求更高的精度，計(jì)算機(jī)技術(shù)的開展很好的解決了這一問題。本文將以 PUMA560 為例，利用個人電腦平臺的 Matlab 對其運(yùn)動學(xué)的正解、逆解以及雅克

3、比矩陣進(jìn)展計(jì)算研究。關(guān) 鍵 詞PUMA560 Matlab 正解 逆解 雅克比矩陣 微分變換法 矢量積法ABSTRACTAs a highly interspersed subject,the roboticsmakes many people who major in different subjectinterest in it, research and develop it.Especially in recent years, with the rapid development of varieties of emerging technologies,mechanicalprod

4、ucts inde*es of automation，high precision and the re-load are being more and more outstanding. There is a need of greater precision in the calculation of robotics and puter technology makes it possible. In this paper we will use Matlab to research the kinematics problem and Jacobian array of PUMA560

5、.KEY WORDSPUMA560MatlabKinematicsproblemPositive-solutionInverse-. zsolutionJacobianarrayDifferentialtransformationVector product transformationPUMA560 機(jī)器人簡介PUMA560 是屬于關(guān)節(jié)式機(jī)器人，6 個關(guān)節(jié)都是轉(zhuǎn)動關(guān)節(jié)，如圖 11 所示，前三個關(guān)節(jié)確定手腕參考點(diǎn)的位置，后三個關(guān)節(jié)確定手腕的方位。和大多數(shù)工業(yè)機(jī)器人一樣，后三個關(guān)節(jié)軸線交于一點(diǎn)。該點(diǎn)選作為手腕參考點(diǎn)，也選作為4、5、6的原點(diǎn)。關(guān)節(jié)一的軸線為垂直方向，關(guān)節(jié) 2 和關(guān)節(jié) 3 的軸線為

6、水平，且平行，距離為。關(guān)節(jié) 1 和關(guān)節(jié) 2 的軸線垂直相交，關(guān)節(jié) 3 和關(guān)節(jié) 4 的a2軸線垂直交織。距離為。各個連桿坐標(biāo)系如a3圖 11 所示，相應(yīng)的連桿參數(shù)列于表 12 中。其中，mma8 .4312，mma32.203，mmd09.1492。mmd07.4333在更進(jìn)一步了解PUMA560 機(jī)器人的轉(zhuǎn)動角度問題時，我們先來定義一下 PUMA560 機(jī)器人的初始位姿。首先,定義機(jī)器人的初始位置.取大臂處于*一朝向時,作為腰關(guān)節(jié)的初始位置.大臂處在水平位置時,作為-. z肩關(guān)節(jié)的初始位置.小臂處在下垂位置,關(guān)節(jié)軸線 Z4 和 Z0 平行時,作為肘關(guān)節(jié)的初始位置.關(guān)節(jié)軸線 Z5 和 Z3 平行

7、時,作為腕扭轉(zhuǎn)關(guān)節(jié)的初始位置.關(guān)節(jié)軸線 Z6和 Z4 平行時,作為腕彎曲關(guān)節(jié)的初始位置.抓手兩個指尖的連線與大臂平行時,作為腕旋轉(zhuǎn)關(guān)節(jié)的初始位置.在上述初始位置的前下,各個關(guān)節(jié)的零點(diǎn)位置得到確定.PUMA560 機(jī)器人的正解1 1、確定、確定 D-HD-H 坐標(biāo)系坐標(biāo)系PUMA 560 的關(guān)節(jié)全為轉(zhuǎn)動關(guān)節(jié):Zi 坐標(biāo)軸:沿著 i+1 關(guān)節(jié)的運(yùn)動軸;*i 坐標(biāo)軸:沿著 Zi 和 Zi-1 的公法線,指向離開 Zi-1 軸的方向;Yi 坐標(biāo)軸:按右手直角坐標(biāo)系法則制定;連桿長度 ai; Zi 和 Zi-1 兩軸心線的公法線長度;連桿扭角 i: Zi 和 Zi-1 兩軸心線的夾角;兩連桿距離 di:

8、 *i 和*i-1 兩坐標(biāo)軸的公法線距離;兩桿夾角 i :*i 和*i-1 兩坐標(biāo)軸的夾角;2 2、確定各連桿、確定各連桿 D-HD-H 參數(shù)和關(guān)節(jié)變量參數(shù)和關(guān)節(jié)變量確定各連桿 D-H 參數(shù)和關(guān)節(jié)變量：連桿連桿 i i變量變量 i ii-1i-1a ai-1i-1d di i變量圍變量圍1 11 1000 00 0-160160-1601602 22 2-90-900 0d d2 2-22545-225453 33 300a a2 20 0-45225-452254 44 4-90-90a a3 3d d4 4-110170-1101705 55 590900 00 0-100100-1001

9、006 66 6-90-900 00 0-266266-2662663 3、求出兩桿間的位姿矩陣、求出兩桿間的位姿矩陣第 i 連桿與第 i-1 連桿間的變換矩陣Ai =Rot(*, i-1)trans(ai-1,0,0)Rot(z, i)trans(0,0,di)-. z= =11111111100001iiiiiiiiiiiiiiiiicsas cc cssds sc sccd 相鄰兩個連桿間的位姿變換矩陣4 4、求末桿的位姿矩陣、求末桿的位姿矩陣由上面的矩陣，我們可以得到最終結(jié)果：5 5、MatlabMatlab 編程編程運(yùn)行 zhengjie.m，根據(jù)提示輸入的值，回車后的出 T 的解如

10、下：16T= 0 1.0000 0 -149.0900 0 0 1.0000 864.8700 1.0000 0 0 20.3200 0 0 0 1.00006 6、驗(yàn)證、驗(yàn)證由課本給出的驗(yàn)證公式進(jìn)展所編程序的驗(yàn)證，經(jīng)歷證，編程所得結(jié)果與課本給出驗(yàn)證公式得到的結(jié)果一致。進(jìn)一步說明所編程序是正確的。PUMA560 機(jī)器人的逆解將 PUMA 560 的運(yùn)動方程3.64寫為:假設(shè)末端連桿的位姿已經(jīng)給定,求關(guān)節(jié)變量的值成為運(yùn)動逆解。161 1、求、求1式中,正、負(fù)號對應(yīng)于的兩個可能解。1-. z2 2、求、求3由以上兩式的平方加上的平方可以得到：112-sxypc pd223 34 3a cd sk在

11、上式中，2222222232422xyzpppaaddka式22中已經(jīng)消去，所以可以由三角代換求解得到的解。23所以：在的表達(dá)式中正、負(fù)號對應(yīng)于的兩種可能解。333 3、求、求222331 231 23232 31 231 23232 336112000010001xxxxyyyyzzzzc cs csa cnopac ss sca snoapTscdnoap令矩陣方程(23)兩端的元素(1,4)和(2,4)分別對應(yīng)相等,則得兩方程:由以上兩式可得的表達(dá)式：23由求得的，可求出：232根據(jù)解的四種可能組合可以得到相應(yīng)的四種可能值，于是可得到 13和23的四種可能解。24 4、求、求4令上式的矩

12、陣方程的兩端的元素(1,4)和(2,4)分別對應(yīng)相等,則得兩方程：當(dāng) S5=0 時,機(jī)械手處于奇異形位.此時,關(guān)節(jié)軸 4 和 6 重合,只能解出和的46和或差.奇異形位可以由式的表達(dá)式中的 atan2 的兩個變量是否接近零來判別.4假設(shè)都接近零,則為奇異形位,否則,不是奇異形位.在奇異形位時,可任意選取值,再計(jì)算相應(yīng)的值。46-. z5 5、求、求5根據(jù)求出的，可以進(jìn)一步解出：45因?yàn)椋谇懊婢呀獬?，逆變換為：12340141234( ,)T 令矩陣方程兩端的元素(1,3)和(3,3)分別對應(yīng)相等,則得兩方程：所以可以得到的最終表達(dá)式：5555atan2( ,)s c6 6、求、求6令矩陣方

13、程兩端的元素(3,1)和(1,1)分別對應(yīng)相等,則得兩方程：得到最后的表達(dá)式：6666atan2(,)s c7 7、解的多重性、解的多重性PUMA560 的運(yùn)動反解可能存在 8 種解，但是，由于構(gòu)造的限制，例如各關(guān)節(jié)變量不能在全部 360 度圍運(yùn)動，有些解不能實(shí)現(xiàn)。在機(jī)器人存在多種解的情況下，應(yīng)選取其中最滿意的一組解，以滿足機(jī)器人的工作要求。8 8、MatlabMatlab 編程編程在 Matlab 中運(yùn)行 nijie.m,根據(jù)提示輸入的值并回車，,xxxyyyzzzn o a n o an o a可得的 8 組解值如下：i90.0000 -2.6918 -84.6272 -180.0000

14、2.6810 180.0000 90.0000 -0.0000 -90.0000 0 0.0000 0 90.0000 -2.6918 -84.6272 0.0000 -2.6810 -0.0000 90.0000 -0.0000 -90.0000 0 -0.0000 0 -70.4385 180.0000 -84.6272 104.7629 20.2581 74.3103 -70.4385 182.6918 -90.0000 97.5292 19.7387 82.0067 -70.4385 180.0000 -84.6272 -75.2371 -20.2581 -105.6897 -70.4

15、385 182.6918 -90.0000 -82.4708 -19.7387 -97.9933-. z9 9、對于機(jī)器人解的分析、對于機(jī)器人解的分析通過編程可以知道，我們最終得到八組解。然后對八組解進(jìn)展分析，對于的變化圍為從，所以程序中我們得到的兩個解都是正確的。然100160160后對進(jìn)展分析，由于的角度變化圍是從，所以在我們所得到220045225的結(jié)果中，后四組是超出的變化圍的，所以我們可以舍去后四組解。再逐個2對、進(jìn)展角度分析，最終可獲得適合的解。3456機(jī)器人的雅克比矩陣1 1、定義、定義機(jī)械手的操作速度與關(guān)節(jié)速度間的線性變換定義為機(jī)械手的雅可比矩陣。2 2、雅可比矩陣的求法、雅可

16、比矩陣的求法1矢量積法 對移動關(guān)節(jié)對轉(zhuǎn)動關(guān)節(jié)2微分變換法對于轉(zhuǎn)動關(guān)節(jié) i,相對連桿 i-1,繞坐標(biāo)系i的軸所作微分轉(zhuǎn)動,其微分izid運(yùn)動矢量為(3-117),對應(yīng)的夾持器的微分運(yùn)動矢量為(3-118)：于是,J(q)的第 i 列如下:對轉(zhuǎn)動關(guān)節(jié) i：,zzTTliaizzzzpnnJp oJopaa對移動關(guān)節(jié) i：0,00zTTlizaiznJoJa , ioniiiiioniizpzJqzpzwv-. z3 3、微分變換法求機(jī)器人的雅可比矩陣、微分變換法求機(jī)器人的雅可比矩陣PUMA560 的 6 個關(guān)節(jié)都是轉(zhuǎn)動關(guān)節(jié),所以利用(3-121)求取雅克比矩陣的列矢量。對于第 1 個關(guān)節(jié)來說,將中

17、的 n,o,a,p 向量代入式(3-121)，得到雅克比矩陣16T的列矢量。其中、的表達(dá)式如下所示：xTJ1xTT2xTT3對于第 2 個關(guān)節(jié)來說,將中的 n,o,a,p 向量代入式(3-121),得到雅克比矩陣26T的列矢量 2TJq可以得到： 22224 54 664 5 64 64 5TxTyTTzJJJJqs cc scs c sc cs s 其中、三個參數(shù)的表達(dá)式如下所示：xTJ2yTJ2zTJ2同理,可求得：所以又以上六個矩陣便最后組成了的雅克比矩陣。16664 4、矢量積法求機(jī)器人的雅克比矩陣、矢量積法求機(jī)器人的雅克比矩陣PUMA560 的 6 個關(guān)節(jié)都是轉(zhuǎn)動關(guān)節(jié),因而其雅克比矩

18、陣具有以下形式：5 5、MatlabMatlab 編程編程1用微分變換法求解雅克比矩陣在 Matlab 中運(yùn)行 wfbh.m，根據(jù)提示輸入的值并回車，得雅克比矩陣如下：16-864.8700 0 0 0 0 0 -149.0900 20.3200 20.3200 0 0 0 0 -864.8700 -433.0700 0 0 0 0 -1.0000 -1.0000 0 -1.0000 0 0 0 0 1.0000 0 1.0000 1.0000 0 0 0 0 0-. z2用矢量積法求解雅克比矩陣在 Matlab 中運(yùn)行 slj.m，根據(jù)提示輸入的值并回車，得雅克比矩陣如下：16-864.87

19、00 0 0 0 0 0 -149.0900 20.3200 20.3200 0 0 0 0 -864.8700 -433.0700 0 0 0 0 -1.0000 -1.0000 0 -1.0000 0 0 0 0 1.0000 0 1.0000 1.0000 0 0 0 0 0從以上結(jié)果中我們可以看出其運(yùn)行結(jié)果與用微分變換編程所得到的結(jié)果是一致的，進(jìn)一步證明了所編寫程序的正確性。附錄1 1、程序、程序1、運(yùn)動學(xué)正解function zhengjie(c1,c2,c3,c4,c5,c6)clc;a2=431.8;a3=20.32;d2=149.09;d4=433.07;d6=56.25;c1

20、=input(c1=);c2=input(c2=);c3=input(c3=);c4=input(c4=);c5=input(c5=);c6=input(c6=);T1=cosd(c1) -sind(c1) 0 0; sind(c1),cosd(c1),0,0;-. z 0,0,1,0; 0 0 0 1;T2=cosd(c2),-sind(c2),0,0; 0,0,1 d2; -sind(c2) -cosd(c2) 0 0; 0 0 0 1;T3=cosd(c3) -sind(c3) 0 a2; sind(c3) cosd(c3) 0 0; 0 0 1 0; 0 0 0 1;T4=cosd(c

21、4) -sind(c4) 0 a3 ; 0 0 1 d4; -sind(c4) -cosd(c4) 0 0; 0 0 0 1;T5=cosd(c5) -sind(c5) 0 0; 0 0 -1 0; sind(c5) cosd(c5) 0 0; 0 0 0 1;T6=cosd(c6) -sind(c6) 0 0; 0 0 1 0; -sind(c6) -cosd(c6) 0 0; 0 0 0 1;T=T1*T2*T3*T4*T5*T6;disp(T=);disp(T)2、運(yùn)動學(xué)逆解function nijie(T)clc;n*=input(n*=);o*=input(o*=);a*=input

22、(a*=);-. zny=input(ny=);oy=input(oy=);ay=input(ay=);nz=input(nz=);oz=input(oz=);az=input(az=);p*=input(p*=);py=input(py=);pz=input(pz=);a2=431.8;a3=20.32;d2=149.09;d4=433.07;c1=atan2(py,p*)-atan2(d2,sqrt(p*p*+py*py-d2*d2),atan2(py,p*)-atan2(d2,-sqrt(p*p*+py*py-d2*d2);%求解 c1c1=c1/pi*180;k=(p*p*+py*py

23、+pz*pz-a2*a2-a3*a3-d2*d2-d4*d4)/(2*a2);c3=atan2(a3,d4)-atan2(k,sqrt(a3*a3+d4*d4-k*k),atan2(a3,d4)-atan2(k,-sqrt(a3*a3+d4*d4-k*k);%求解 c3c3=c3/pi*180;for i=1:2 for j=1:2 m1=cosd(c1(i);m2=sind(c1(i); n1=cosd(c3(j);n2=sind(c3(j);c23(i,j)=atan2(-(a3+a2*n1)*pz+(m1*p*+m2*py)*(a2*n2-d4),(-d4+a2*n2)*pz+(m1*p

24、*+m2*py)*(a2*n1+a3); c23(i,j)=c23(i,j)/pi*180; c2(i,j)=c23(i,j)-c3(1,j); endend%求解 c2for i=1:2 for j=1:2 m1=cosd(c1(i);n1=sind(c1(i); m2=cosd(c23(i,j);n2=sind(c23(i,j); c41(i,j)=atan2(-a*n1+ay*m1,-a*m1*m2-ay*n1*m2+az*n2);-. z c411(i,j)=atan2(a*n1-ay*m1,a*m1*m2+ay*n1*m2-az*n2); c41(i,j)=c41(i,j)/pi*1

25、80; c411(i,j)=c411(i,j)/pi*180; endend%求解 c4c4=c41,c411;disp(c4)c23=c23(1,:),c23(1,:);c23(2,:),c23(2,:);for i=1:2 for j=1:4 m1=cosd(c1(i);n1=sind(c1(i); m2=cosd(c23(i,j);n2=sind(c23(i,j); m3=cosd(c4(i,j);n3=sind(c4(i,j);sinc5(i,j)=-a*(m1*m2*m3*n1*n3)-ay*(n1*m2*m3-m1*n3)+az*(n2*m3); cosc5(i,j)=a*(-m1

26、*n2)+ay*(-n1*n2)+az*(-m2); c5(i,j)=atan2(sinc5(i,j),cosc5(i,j); c5(i,j)=c5(i,j)/pi*180; endend%求解 c5for i=1:2 for j=1:4 m1=cosd(c1(i);n1=sind(c1(i); m2=cosd(c23(i,j);n2=sind(c23(i,j); if sind(c5(i,j)-0.01 c4(i,j)=0; end endend%奇異形位判斷-. zfor i=1:2 for j=1:4 m1=cosd(c1(i);n1=sind(c1(i); m2=cosd(c23(i,

27、j);n2=sind(c23(i,j); m3=cosd(c4(i,j);n3=sind(c4(i,j); m4=cosd(c5(i,j);n4=sind(c5(i,j);sinc6(i,j)=-n*(m1*m2*n3-n1*m3)-ny*(n1*m2*n4+m1*m4)+nz*(n2*n3);cosc6(i,j)=n*(m1*m2*m3+n1*n3)*m4-m1*n2*n4)+ny*(n1*m2*m3-m1*n3)*m4-m1*m2*m4)-nz*(n2*m3*m4+m2*n4); c6(i,j)=atan2(sinc6(i,j),cosc6(i,j); c6(i,j)=c6(i,j)/pi

28、*180; endend%求解 c6C1=c1(1),c1(1),c1(1),c1(1),c1(2),c1(2),c1(2),c1(2);C2=c2(1,:),c2(1,:),c2(2,:),c2(2,:);C3=c3(1),c3(2),c3(1),c3(2),c3(1),c3(2),c3(1),c3(2);C23=c23(1,:),c23(2,:);C4=c4(1,:),c4(2,:);C5=c5(1,:),c5(2,:);C6=c6(1,:),c6(2,:);%排序C=C1;C2;C3;C4;C5;C6;%輸出C=C;disp(C);3、微分變換法求雅克比矩陣function wfbh(c

29、1,c2,c3,c4,c5,c6)c1=input(c1=);c2=input(c2=);-. zc3=input(c3=);c4=input(c4=);c5=input(c5=);c6=input(c6=);a2=431.8;a3=20.32;d2=149.09;d4=433.07;d6=56.25;T10=cosd(c1) -sind(c1) 0 0; sind(c1),cosd(c1),0,0; 0,0,1,0; 0 0 0 1;T21=cosd(c2),-sind(c2),0,0; 0,0,1 d2; -sind(c2) -cosd(c2) 0 0; 0 0 0 1;T32=cosd(

30、c3) -sind(c3) 0 a2; sind(c3) cosd(c3) 0 0; 0 0 1 0; 0 0 0 1;T43=cosd(c4) -sind(c4) 0 a3 ; 0 0 1 d4; -sind(c4) -cosd(c4) 0 0; 0 0 0 1;T54=cosd(c5) -sind(c5) 0 0; 0 0 -1 0; sind(c5) cosd(c5) 0 0; 0 0 0 1;-. zT65=cosd(c6) -sind(c6) 0 0; 0 0 1 0; -sind(c6) -cosd(c6) 0 0; 0 0 0 1;T64=T54*T65;T63=T43*T64;

31、T62=T32*T63;T61=T21*T62;T60=T10*T61;T(:,:,1)=T61;T(:,:,2)=T62;T(:,:,3)=T63;T(:,:,4)=T64;T(:,:,5)=T65;N=T(1:3,1,:);O=T(1:3,2,:);A=T(1:3,3,:);P=T(1:3,4,:);PN=cross(P,N);PN=PN(3,1,:);PO=cross(P,O);PO=PO(3,1,:);PA=cross(P,A);PA=PA(3,1,:);J=PN;PO;PA;N(3,1,:);O(3,1,:);A(3,1,:);J6=0;0;0;0;0;1;J=J(:,:,1),J(:,:,2),J(:,:,3),J(:,:,4),J(:,:,5),J6;R60=T60(1:3,1:3);J=R60,zeros(3);zeros(3),R60*J;