1、工業機器人課程設計基于Matlab的工業機器人運動學和雅克比較矩陣求解目錄PUMA560 機器人簡介 PUMA560 機器人的正解 1、確定 D-H坐標系 2、確定各連桿 D-H參數和關節變量 3、求出兩桿間的位姿矩陣 4、求末桿的位姿矩陣 5、MATLAB 編程6、驗證PUMA560 機器人的逆解 61、求，1 十飛2、求 223、求24、求"4十155、求 6、求 67、解的多重性 8、MATLAB 編程9、對于機器人解的分析 機器人的雅克比矩陣 1、定義2、雅可比矩陣的求法 93、微分變換法求機器人的雅可比矩陣 4、矢量積法求機器人的雅克比矩陣 5、MATLAB 編程 附錄1、

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

3、cs 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 automation， high precision and the re-load are becoming more and more outstanding.

4、 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 WORDSPUMA560 Matlab Kinematics problem Positive-solution Inverse-solutionJacobian arra

5、y Differential transformation Vector product transformationPUMA560機器人簡介PUMA560是屬于關節式機器人，6個關節都是轉動關節，如圖1 1所小， 前三個關節確定手腕參考點的位置， 后三個關節確定手腕的方位。和大多數工業 機器人一樣，后三個關節軸線交于一點。該點選作為手腕參考點，也選作為 4、 5、6的原點。關節一的軸線為垂直方向，關節 2和關節3的軸線為水平，且 平行，距離為a2。關節1和關節2的軸線垂直相交，關節3和關節4的軸線垂直交錯。距離為a3。各個連桿坐標系如圖11所示，相應的連桿參數列于d2 -149.09mma2

6、 = 431.8mma3 = 20.32mm表 1 2中。其中，機器人的轉動角度問題時，我們先來定義一下d3= 433.07mm。在更進一步了解 PUMA560 PUMA560 機器人的 初 始位姿。首先，定義機器 人的初始位置.取大臂處 于某一朝向時，作為腰關 節的初始位置.大臂處在 水平位置時，作為肩關節 的初始位置.小臂處在下 垂位置，關節軸線Z4和Z0平行時，作為肘關節的 初始位置.關節軸線Z5和Z3平行時，作為腕扭轉關節的初始位置.關節軸線Z6和Z4平行時，作為腕彎曲 關節的初始位置.抓手兩個指尖的連線與大臂平行時，作為腕旋轉關節的初始位置 在上述初始位置的前下，各個關節的零點位置得

7、到確定.PUMA560機器人的正解1、確定D-H坐標系PUMA 560的關節全為轉動關節：Zi坐標軸:沿著i+1關節的運動軸；Xi坐標軸:沿著Zi和Zi-1的公法線，指向離開Zi-1軸的方向;Yi坐標軸:按右手直角坐標系法則制定；連桿長度ai; Zi和Zi-1兩軸心線的公法線長度；連桿扭角a i: Zi和Zi-1兩軸心線的夾角;兩連桿距離di: Xi和Xi-1兩坐標軸的公法線距離；兩桿夾角9 i :Xi和Xi-1兩坐標軸的夾角;2、確定各連桿D-H參數和關節變量確定各連桿D-H參數和關節變量:連桿i變量Qi0Ci-1aidi變量范圍191000-160 1602色-900d2-225 453打

8、0a20-45 2254由-90a3d4-110 170559000-100 1006-9000-266 2663、求出兩桿間的位姿矩陣第i連桿與第i-1連桿間的變換矩陣Ai =Rot(x, oi-1 )trans(a-1,0,0)Rot(z, 6)trans(0,0,d)一 c-si0a,_sQiCaicQiCai-s»i-s%二dis9iSai cQsu*0a i/ udi_ 0001_相鄰兩個連桿間的位姿變換矩陣4、求末桿的位姿矩陣由上面的矩陣，我們可以得到最終結果:5、Matlab 編程 運行zhengjie.m,根據提示輸入61d的值，回車后的出T的解如下:T=001.00

9、0001.000000001.0000 00-149.0900 864.8700 20.32001.00006、驗證由課本給出的驗證公式進行所編程序的驗證，經驗證，編程所得結果與課本給出驗證公式得到的結果一致。進一步表明所編程序是正確的。PUMA560機器人的逆解將PUMA 560的運動方程（3.64）寫為:若末端連桿的位姿已經給定，求關節變量91-06的值成為運動逆解。1、求“1式中，正、負號對應于d的兩個可能解。2、求一由以上兩式的平方加上-s1Px +GPy =d2的平方可以得到:a3c3 -d4s3 =k（2 2）222222a2在上式中，k 二 px pyPz-aLa3式（22）中已

10、經消去02,所以可以由三角代換求解得到仇的解。所以：在a的表達式中正、負號對應于 仇的兩種可能解。3、求c1c23s1c23s23-a2c3丸oxaxPxl一 c1s23一 s1s23c23a2 s3nyoyayPy_slC10-d2nzozazPz-0001 10001 _(23)令矩陣方程（23）兩端的元素（1,4）和（2,4）分別對應相等，則得兩方程: 由以上兩式可得823的表達式: 由求得的023 ,可求出為：根據自和色解的四種可能組合可以得到相應的生3四種可能值，于是可得到的02四種可能解。4、求:令上式的矩陣方程的兩端的元素（1,4）和（2,4）分別對應相等，則得兩方程：當S5=0

11、時,機械手處于奇異形位.此時，關節軸4和6重合，只能解出句和”的和或 差.奇異形位可以由式3的表達式中的atan2的兩個變量是否接近零來判別.若都接 近零,則為奇異形位，否則，不是奇異形位.在奇異形位時，可任意選取為值，再計算相 應的均值。5、求可根據求出的名,可以進一步解出斗：因為斗，日2,日3, 2在前面均已解出，逆變換0T4,但,a包,“）為：令矩陣方程兩端的元素（1,3）和（3,3）分別對應相等，則得兩方程：所以可以得到 仇的最終表達式：a=atan2( SsQ) 6、求備令矩陣方程兩端的元素(3,1)和(1,1)分別對應相等，則得兩方程：得到最后備的表達式：為=atan2(0,C6)

12、7、解的多重性PUMA560的運動反解可能存在8種解，但是，由于結構的限制，例如各關 節變量不能在全部360度范圍內運動，有些解不能實現。在機器人存在多種解的 情況下，應選取其中最滿意的一組解，以滿足機器人的工作要求。8、Matlab 編程在Matlab中運行nijie.m,根據提示輸入nx，Ox，ax，ny, Oy, ay,nz，Oz，az的值并回車，可得4的8組解值如下:90.0000-2.6918-84.6272-180.00002.6810180.000090.0000-0.0000-90.000000.0000090.0000-2.6918-84.62720.0000-2.6810-

13、0.000090.0000-0.0000-90.00000-0.00000-70.4385180.0000-84.6272104.762920.258174.3103-70.4385182.6918-90.000097.529219.738782.0067-70.4385180.0000-84.6272-75.2371-20.2581-105.6897-70.4385182.6918-90.0000-82.4708-19.7387-97.99339、對于機器人解的分析通過編程可以知道，我們最終得到八組解。然后對八組解進行分析，對于斗 的變化范圍為從-1600 1600,所以程序中我們得到的兩個

14、解都是正確的。然后對進行分析，由于仇的角度變化范圍是從-2250 450,所以在我們所得到的 結果中，后四組是超出仇的變化范圍的，所以我們可以舍去后四組解。再逐個對 仇、仇、仇、06進行角度分析，最終可獲得適合的解。機器人的雅克比矩陣1、定義機械手的操作速度與關節速度間的線性變換定義為機械手的雅可比矩陣2、雅可比矩陣的求法(1)矢量積法對移動關節對轉動關節(2)微分變換法V；Zi/poL .|ZiXiPn .1=1qi, Ji = 1M .乙.乙.對于轉動關節i,相對連桿i-1,繞坐標系i的z軸所作微分轉動d。,其微分運動矢量為(3-117)對應的夾持器的彳分運動矢量為(3-118):hlOz

15、:az_i(Pxn /于是,J(q)的第i列如下:對轉動關節i: T Jli = (pM。),TJai _(PXa )z_對移動關節i ： T Jli =ozaz JTI, J ai3、微分變換法求機器人的雅可比矩陣PUMA560的6個關節都是轉動關節，所以利用(3-121)求取雅克比矩陣的列矢量。 對于第1個關節來說，將1T6中的n,o,a,p向量代入式(3-121)，得到雅克比矩陣的列 矢量。其中TJ1x、TT2X、TT3x的表達式如下所示：對于第2個關節來說，將2T6中的n,o,a,p向量代入式(3-121)，得到雅克比矩陣的列矢量TJ2 q一TJ2x1TJ2y可以得到:J2 q =TJ

16、2zT4 c5c6 c4 s6S4c5% -C4C6S4s5其中TJ2x、TJ2y、TJ2zm個參數的表達式如下所示:同理，可求得： 所以又以上六個6父1矩陣便最后組成了 6父6的雅克比矩陣 4、矢量積法求機器人的雅克比矩陣PUMA560的6個關節都是轉動關節，因而其雅克比矩陣具有下列形式：5、Matlab 編程(1)用微分變換法求解雅克比矩陣在Matlab中運行wfbh.m ,根據提示輸入4-線的值并回車，得雅克比矩陣如下:-864.870000000-149.090020.320020.32000000-864.8700-433.07000000-1.0000-1.00000-1.0000

17、00001.000001.00001.000000000(2)用矢量積法求解雅克比矩陣在Matlab中運行slj.m,根據提示輸入4-Q的值并回車，得雅克比矩陣如下:-864.870000000-149.090020.320020.32000000-864.8700-433.07000000-1.0000-1.00000-1.000000001.000001.00001.000000000從以上結果中我們可以看出其運行結果與用微分變換編程所得到的結果是致的，進一步證明了所編寫程序的正確性。附錄1 、程序1、運動學正解function zhengjie(c1,c2,c3,c4,c5,c6)clc

18、;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=input('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)

19、 -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) 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*T

20、4*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=');nz=input('nz=');oz=input('oz=');az=input('az=');px=input('px='

21、);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,d4)-atan2(k,sqrt(a3*a3+d4*d4-k*k

22、),atan2(a3,d4)-atan2(k,-sqrt(a3*a3+d4*d4-k*k); %求解c3c3=c3/pi*180;for i=1:2for j=1:2m1=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(i,j)=c23(i,j)-c3(1,j);endend%求解c2for i

23、=1:2for j=1:2m1=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,c411;disp(c4)c23=c23(1,:),c23(1,:);c23(2,:),c23(

24、2,:);for i=1:2for j=1:4m1=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,j),cosc5(i,j);c5(i,j)=c5(i,j)/pi*180;endend%求解c5for i

25、=1:2for j=1:4m1=cosd(c1(i);n1=sind(c1(i);m2=cosd(c23(i,j);n2=sind(c23(i,j);if sind(c5(i,j)<0.01&&sind(c5(i,j)>-0.01c4(i,j)=0;endendend%奇異形位判斷for i=1:2for j=1:4m1=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);m4=cosd(c5(i,j);n4=sind(c5(i,j);s

26、inc6(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*m 3-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%t解 c6C1=c1(1),c1(1),c1(1),c1(1),c1(2),c1(2),c1(2),c1(2);C2=c2(1,:),c2(1,:

27、),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=input('c

28、3=');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) co

29、sd(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;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)=T

30、61;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;disp(J);4、矢量積法求雅克比矩陣function wfbh(c1,c2,c3,c4,c5,c6)c1=i