1、工業機器人課程設計基于 Matlab 的工業機器人運動學和雅克比較矩陣求解目錄PUMA560 機器人簡介機器人簡介 .4PUMA560 機器人的正解機器人的正解 .51、確定 D-H 坐標系.52、確定各連桿 D-H 參數和關節變量.53、求出兩桿間的位姿矩陣.54、求末桿的位姿矩陣.65、MATLAB編程.76、驗證.7PUMA560 機器人的逆解機器人的逆解 .81、求1.82、求3.83、求2.94、求4.105、求5.106、求6.117、解的多重性.118、MATLAB編程.119、對于機器人解的分析.12機器人的雅克比矩陣機器人的雅克比矩陣 .121、定義.122、雅可比矩陣的求法

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

3、b 正解 逆解 雅克比矩陣 微分變換法 矢量積法ABSTRACTAs a highly interspersed subject, the robotics makes many people who major in different subject interest in it, research and develop it. Especially in recent years, with the rapid development of varieties of emerging technologies, mechanical products indexes of automa

4、tion，high precision and the re-load are becoming more and more outstanding. There is a need of greater precision in the calculation of robotics and computer technology makes it possible. In this paper we will use Matlab to research the kinematics problem and Jacobian array of PUMA560.KEY WORDSPUMA56

5、0 Matlab Kinematics problem Positive-solution Inverse-solution Jacobian array Differential transformation Vector product transformationPUMA560 機器人簡介PUMA560 是屬于關節式機器人，6 個關節都是轉動關節，如圖 11 所示，前三個關節確定手腕參考點的位置，后三個關節確定手腕的方位。和大多數工業機器人一樣，后三個關節軸線交于一點。該點選作為手腕參考點，也選作為4、5、6的原點。關節一的軸線為垂直方向，關節 2 和關節 3 的軸線為水平，且平行，距離

6、為。關節 1 和關節 2 的軸線垂直相交，關節 3 和關節 4 的a2軸線垂直交錯。距離為。各個連桿坐標系如a3圖 11 所示，相應的連桿參數列于表 12 中。其中，mma8 .4312，mma32.203， mmd09.1492。mmd07.4333在更進一步了解PUMA560 機器人的轉動角度問題時，我們先來定義一下 PUMA560機器人的初始位姿。首先,定義機器人的初始位置.取大臂處于某一朝向時,作為腰關節的初始位置.大臂處在水平位置時,作為肩關節的初始位置.小臂處在下垂位置,關節軸線 Z4 和 Z0 平行時,作為肘關節的初始位置.關節軸線Z5 和 Z3 平行時,作為腕扭轉關節的初始位置

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

8、線距離;兩桿夾角 i :Xi 和 Xi-1 兩坐標軸的夾角;2、確定各連桿、確定各連桿 D-H 參數和關節變量參數和關節變量確定各連桿 D-H 參數和關節變量：連桿連桿 i變量變量 ii-1ai-1di變量范圍變量范圍11000-16016022-900d2-22545330a20-4522544-90a3d4-110170559000-10010066-9000-2662663、求出兩桿間的位姿矩陣、求出兩桿間的位姿矩陣第 i 連桿與第 i-1 連桿間的變換矩陣 Ai =Rot(x, i-1)trans(ai-1,0,0)Rot(z, i)trans(0,0,di) =11111111100

9、001iiiiiiiiiiiiiiiiicsas cc cssds sc sccd 相鄰兩個連桿間的位姿變換矩陣 111101000000100001csscT221222000010000001csTsc 333323000000100001csscT443444000010000001csTsc 554555000010000001csTsc665666000010000001csTsc4、求末桿的位姿矩陣、求末桿的位姿矩陣00123456112233445566( )()()()()()TTTTTTT0016160001xxxxyyyyzzzznoapnoapTT Tnoap由上面的矩陣

10、，我們可以得到最終結果：1234 5 64 623 5 614 5 64 61234 5 64 623 5 614 5 64 6234 5 64 623 5 61234 5 64 623 5 614 64 5 61234 5 64 623 5 614xyzxynccc c cs ss s css c cc snscc c cs ss s ccs c cc snsc c cs sc s coccc c ss cs s ss c cs c soscc c ss cs s sc c c 64 5 6234 5 64 623 5 6123 4 523 51 4 5123 4 523 51 4 523

11、4 523 51223 234 232 11223 234 232 13 232 2423zxyzxyzs c cosc c ss cc s sac c c ss cs s sas c c ss cc s sas c sc cpc a ca cd sd sps a ca cd sd cpa sa sd c 5、Matlab 編程編程運行 zhengjie.m，根據提示輸入的值，回車后的出 T 的解如下：16T= 0 1.0000 0 -149.0900 0 0 1.0000 864.8700 1.0000 0 0 20.3200 0 0 0 1.00006、驗證、驗證1234562234690

12、 ,0,90 ,0,00431.8,149.09,20.32,433.07,56.25ooammdmmammdmmdmm 2240630100011000001dadTa由課本給出的驗證公式進行所編程序的驗證，經驗證，編程所得結果與課本給出驗證公式得到的結果一致。進一步表明所編程序是正確的。PUMA560 機器人的逆解將 PUMA 560 的運動方程（3.64）寫為:00123456112233445566( )()()()()()0001xxxxyyyyzzzznoapnoapTTTTTTTnoap若末端連桿的位姿已經給定,求關節變量 的值成為運動逆解。161、求、求1 0101234511

13、62233445566TTTTTTT1111160000001000010001xxxxyyyyzzzzcsnopascnoapTnoap212122221222122sin()/;cos()1 (/)atan2,1atan2(,)atan2(,yxxyddddppdppd 式中,正、負號對應于的兩個可能解。12、求、求3113 234 23223 234232 2xyzc ps pa cd sa cpa sd ca s由以上兩式的平方加上的平方可以得到：112-sxypc pd（22）3 34 3a cd sk在上式中， 2222222232422xyzpppaaddka式（22）中已經消去

14、，所以可以由三角代換求解得到的解。23所以：在的表達式中正、負號對應于的兩種可能解。3322233434atan2(,)atan2( ,)a dkadk3、求、求2 01034531236445566,TTTTT （23）1 231 23232 31 231 23232 336112000010001xxxxyyyyzzzzc cs csa cnopac ss sca snoapTscdnoap4 5 64 64 5 64 64 535 65 6543346464 5 64 64 5 64 64 500001c c cs sc c ss sc sas ss scdTT Ts s sc ss c

15、 sc cs s令矩陣方程(23)兩端的元素(1,4)和(2,4)分別對應相等,則得兩方程:1 231 23232 331 231 23232 34xyzxyzc s ps c ps pa cac s ps s pc pa sd由以上兩式可得的表達式：23232332 3112 3442 3112 33tan2 ()()(),()()()zxyxyaaa cpc ps pa sdda spc ps pa ca由求得的，可求出：2322233根據解的四種可能組合可以得到相應的四種可能值，于是可得到 13和23的四種可能解。24、求、求41 231 23232 31 231 23232 33611

16、2000010001xxxxyyyyzzzzc cs csa cnopac ss sca snoapTscdnoap令上式的矩陣方程的兩端的元素(1,4)和(2,4)分別對應相等,則得兩方程： 1 231 23234 5114 5xyzxya c ca s ca sc sa sa cs s 541111 231 232342111 231 23230atan2(,)atan2(,)xyxyzxyxyzsa sa ca c ca s ca sa sa c a c ca s ca s當當 S5=0 時,機械手處于奇異形位.此時,關節軸 4 和 6 重合,只能解出和的和46或差.奇異形位可以由式的表

17、達式中的 atan2 的兩個變量是否接近零來判別.若4都接近零,則為奇異形位,否則,不是奇異形位.在奇異形位時,可任意選取值,再計4算相應的值。65、求、求5根據求出的，可以進一步解出：45 0104544123465566656,TTTTT 因為，在前面均已解出，逆變換為：12340141234( ,)T 1 23 41 41 23 41 423 42 3 42 43 41 23 41 41 23 41 423 42 3 4243 41 231 23232 340001c c cs ss c cc ss ca c cd sa cc c ss ss c sc cs sa c sd ca sc

18、ss sca sd令矩陣方程兩端的元素(1,3)和(3,3)分別對應相等,則得兩方程：1 23 41 41 23 41 423 451 231 23235xyzxyzacc cs sas c cc sas csac sas sacc 所以可以得到的最終表達式：5555atan2( ,)s c6、求、求6 0105512345666,TTT 令矩陣方程兩端的元素(3,1)和(1,1)分別對應相等,則得兩方程：61 23 41 41 23 41 423 461 23 41 451 23 51 23 41 451 23 523 4 523 5()()()()()xyzxyzsn c c ss cns

19、 c sc cn s scnc c cs scc s sns c cc s cs s sn s c cc s 得到最后的表達式：6666atan2(,)s c7、解的多重性、解的多重性PUMA560 的運動反解可能存在 8 種解，但是，由于結構的限制，例如各關節變量不能在全部 360 度范圍內運動，有些解不能實現。在機器人存在多種解的情況下，應選取其中最滿意的一組解，以滿足機器人的工作要求。8、Matlab 編程編程在 Matlab 中運行 nijie.m,根據提示輸入的值并回車，可,xxxyyyzzzn o a n o an o a得的 8 組解值如下：i90.0000 -2.6918 -8

20、4.6272 -180.0000 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.258

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

22、( )xx qxJ q q( )( ),1,2,6,1,2,iijjx qJqijnq2、雅可比矩陣的求法、雅可比矩陣的求法（1）矢量積法 對移動關節,00iiiivzzqJw 對轉動關節iooininpR p（2）微分變換法對于轉動關節 i,相對連桿 i-1,繞坐標系i的軸所作微分轉動,其微分運動izid, ioniiiiioniizpzJqzpzwv矢量為(3-117),對應的夾持器的微分運動矢量為(3-118)：000 ,0(3 117)(3 11801TxzTyzTzziiTzxTzyTzzpndp odpaddddnoa ）于是,J(q)的第 i 列如下:對轉動關節 i：,zzTTl

23、iaizzzzpnnJp oJopaa對移動關節 i：0,00zTTlizaiznJoJa 3、微分變換法求機器人的雅可比矩陣、微分變換法求機器人的雅可比矩陣PUMA560 的 6 個關節都是轉動關節,所以利用(3-121)求取雅克比矩陣的列矢量。對于第 1 個關節來說,將中的 n,o,a,p 向量代入式(3-121)，得到雅克比矩陣的16T列矢量。 1111234 5 64 62356234 5 64 6235623 4 523 5TxTyTTzJJJJqsc c cs sc s csc c ss cc s ss c sc c 其中、的表達式如下所示：xTJ1xTT2xTT314 5 64

24、53 232342234 5 64 623 5 614 5 64 63 232342234 5 64 623 5 614 53 23234223 4 523 5()()()()()()()TxTyTzJs s sc sa cs dd cc c cs ss s sJs c sc ca cs dd cc c cs ss s sJs s a cs dd c c ss c 對于第 2 個關節來說,將中的 n,o,a,p 向量代入式(3-121),得到雅克比矩陣的列26T矢量 2TJq可以得到： 22224 54 664 5 64 64 5TxTyTTzJJJJqs cc scs c sc cs s 其

25、中、三個參數的表達式如下所示：xTJ2yTJ2zTJ2同理,可求得：234 5 64 63 5 63 334234 5 64 63 5 63 334234 5 64 63 5 63 334234 5 64 63 5 63 33423 53 4 53 33423 4 53 53()()()()()()()(TxTyTzJsc c cs sc s sa cs dacc c cs ss s sa sc dJsc c ss sc s sa cs dacc c ss ss s s a sc dJc cs c sa cs dac c ss ca334)sc d 3456,TTTTJqJqJqJq 44 5

26、 64 635 644 5 64 635 644 53 634 5 64 64 5 64 64 5Tdc c cs sas cdc c ss cas sd c sa cJqs c cc ss c sc cs s 4565 665 665000000000,0001TTTJqJqJqs css scc 所以又以上六個矩陣便最后組成了的雅克比矩陣。16664、矢量積法求機器人的雅克比矩陣、矢量積法求機器人的雅克比矩陣PUMA560 的 6 個關節都是轉動關節,因而其雅克比矩陣具有下列形式： 102060162666126zpzpzpJ qzzz5、Matlab 編程編程（1）用微分變換法求解雅克比

27、矩陣在 Matlab 中運行 wfbh.m，根據提示輸入的值并回車，得雅克比矩陣如下：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（2）用矢量積法求解雅克比矩陣在 Matlab 中運行 slj.m，根據提示輸入的值并回車，得雅克比矩陣如下：16-864.8700 0 0 0 0 0 -149.0900 20.3200 20.3200 0 0

28、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從以上結果中我們可以看出其運行結果與用微分變換編程所得到的結果是一致的，進一步證明了所編寫程序的正確性。附錄1、程序、程序1、運動學正解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=input(c1=);c2=input(c2=);c3=input(c3=);c4=in

29、put(c4=);c5=input(c5=);c6=input(c6=);T1=cosd(c1) -sind(c1) 0 0; sind(c1),cosd(c1),0,0; 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(c4) -sind(c4) 0 a3 ; 0 0 1 d4; -sind(c4) -cosd(c4)

30、 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、運動學逆解function nijie(T)clc;nx=input(nx=);ox=input(ox=);ax=input(ax=);ny=input(ny=);oy=input(oy=);ay=input(ay=);n

31、z=input(nz=);oz=input(oz=);az=input(az=);px=input(px=);py=input(py=);pz=input(pz=);a2=431.8;a3=20.32;d2=149.09;d4=433.07;c1=atan2(py,px)-atan2(d2,sqrt(px*px+py*py-d2*d2),atan2(py,px)-atan2(d2,-sqrt(px*px+py*py-d2*d2);%求解 c1c1=c1/pi*180;k=(px*px+py*py+pz*pz-a2*a2-a3*a3-d2*d2-d4*d4)/(2*a2);c3=atan2(a3

32、,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*px+m2*py)*(a2*n2-d4),(-d4+a2*n2)*pz+(m1*px+m2*py)*(a2*n1+a3); c23(i,j)=c23(i,j)/pi*180; c2(

33、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(-ax*n1+ay*m1,-ax*m1*m2-ay*n1*m2+az*n2); c411(i,j)=atan2(ax*n1-ay*m1,ax*m1*m2+ay*n1*m2-az*n2); c41(i,j)=c41(i,j)/pi*180; c411(i,j)=c411(i,j)/pi*180; endend%求解 c4c4=c41

34、,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)=-ax*(m1*m2*m3*n1*n3)-ay*(n1*m2*m3-m1*n3)+az*(n2*m3); cosc5(i,j)=ax*(-m1*n2)+ay*(-n1*n2)+az*(-m2); c5(i,j)=atan2(sinc5(i

35、,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%奇異形位判斷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)

36、; m4=cosd(c5(i,j);n4=sind(c5(i,j);sinc6(i,j)=-nx*(m1*m2*n3-n1*m3)-ny*(n1*m2*n4+m1*m4)+nz*(n2*n3);cosc6(i,j)=nx*(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*180; endend%求解 c6C1=c1(1),c1(1),c1(1),c1(1),c1(2)

37、,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(c1,c2,c3,c4,c5,c6)c1=input(c1=);c2=input(c2=);c3=in

38、put(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(c3) -sind(c3) 0 a2; sind(c3) cosd(c3) 0 0; 0 0 1 0; 0

39、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;T65=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;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: