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

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

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

3、比矩陣進展計算研究。關(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 機器人簡介PUMA560 是屬于關(guān)節(jié)式機器人，6 個關(guān)節(jié)都是轉(zhuǎn)動關(guān)節(jié)，如圖 11 所示，前三個關(guān)節(jié)確定手腕參考點的位置，后三個關(guān)節(jié)確定手腕的方位。和大多數(shù)工業(yè)機器人一樣，后三個關(guān)節(jié)軸線交于一點。該點選作為手腕參考點，也選作為4、5、6的原點。關(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在更進一步了解PUMA560 機器人的轉(zhuǎn)動角度問題時，我們先來定義一下 PUMA560 機器人的初始位姿。首先,定義機器人的初始位置.取大臂處于*一朝向時,作為腰關(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é)的零點位置得到確定.PUMA560 機器人的正解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é)的運動軸;*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 編程編程運行 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、驗證、驗證由課本給出的驗證公式進展所編程序的驗證，經(jīng)歷證，編程所得結(jié)果與課本給出驗證公式得到的結(jié)果一致。進一步說明所編程序是正確的。PUMA560 機器人的逆解將 PUMA 560 的運動方程3.64寫為:假設(shè)末端連桿的位姿已經(jīng)給定,求關(guān)節(jié)變量的值成為運動逆解。161 1、求、求1式中,正、負號對應(yīng)于的兩個可能解。1-. z2 2、求、求3由以上兩式的平方加上的平方可以得到：112-sxypc pd223 34 3a cd sk在

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

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

13、程兩端的元素(3,1)和(1,1)分別對應(yīng)相等,則得兩方程：得到最后的表達式：6666atan2(,)s c7 7、解的多重性、解的多重性PUMA560 的運動反解可能存在 8 種解，但是，由于構(gòu)造的限制，例如各關(guān)節(jié)變量不能在全部 360 度圍運動，有些解不能實現(xiàn)。在機器人存在多種解的情況下，應(yīng)選取其中最滿意的一組解，以滿足機器人的工作要求。8 8、MatlabMatlab 編程編程在 Matlab 中運行 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、對于機器人解的分析、對于機器人解的分析通過編程可以知道，我們最終得到八組解。然后對八組解進展分析，對于的變化圍為從，所以程序中我們得到的兩個解都是正確的。然100160160后對進展分析，由于的角度變化圍是從，所以在我們所得到220045225的結(jié)果中，后四組是超出的變化圍的，所以我們可以舍去后四組解。再逐個2對、進展角度分析，最終可獲得適合的解。3456機器人的雅克比矩陣1 1、定義、定義機械手的操作速度與關(guān)節(jié)速度間的線性變換定義為機械手的雅可比矩陣。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運動矢量為(3-117),對應(yīng)的夾持器的微分運動矢量為(3-118)：于是,J(q)的第 i 列如下:對轉(zhuǎn)動關(guān)節(jié) i：,zzTTliaizzzzpnnJp oJopaa對移動關(guān)節(jié) i：0,00zTTlizaiznJoJa , ioniiiiioniizpzJqzpzwv-. z3 3、微分變換法求機器人的雅可比矩陣、微分變換法求機器人的雅可比矩陣PUMA560 的 6 個關(guān)節(jié)都是轉(zhuǎn)動關(guān)節(jié),所以利用(3-121)求取雅克比矩陣的列矢量。對于第 1 個關(guān)節(jié)來說,將中

17、的 n,o,a,p 向量代入式(3-121)，得到雅克比矩陣16T的列矢量。其中、的表達式如下所示：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ù)的表達式如下所示：xTJ2yTJ2zTJ2同理,可求得：所以又以上六個矩陣便最后組成了的雅克比矩陣。16664 4、矢量積法求機器人的雅克比矩陣、矢量積法求機器人的雅克比矩陣PUMA560 的 6 個關(guān)節(jié)都是轉(zhuǎn)動關(guān)節(jié),因而其雅克比矩

18、陣具有以下形式：5 5、MatlabMatlab 編程編程1用微分變換法求解雅克比矩陣在 Matlab 中運行 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 中運行 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é)果中我們可以看出其運行結(jié)果與用微分變換編程所得到的結(jié)果是一致的，進一步證明了所編寫程序的正確性。附錄1 1、程序、程序1、運動學(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、運動學(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;