工業(yè)機器人課程設(shè)計--基于matlab的工業(yè)機器人運動學(xué)和雅克比較矩陣求解_第1頁
已閱讀1頁,還剩23頁未讀, 繼續(xù)免費閱讀

下載本文檔

版權(quán)說明:本文檔由用戶提供并上傳,收益歸屬內(nèi)容提供方,若內(nèi)容存在侵權(quán),請進行舉報或認領(lǐng)

文檔簡介

1、<p><b>  工業(yè)機器人課程設(shè)計</b></p><p>  基于Matlab的工業(yè)機器人運動學(xué)和雅克比較矩陣求解</p><p><b>  目錄</b></p><p>  PUMA560機器人簡介4</p><p>  PUMA560機器人的正解5</p>&

2、lt;p>  1、確定D-H坐標(biāo)系5</p><p>  2、確定各連桿D-H參數(shù)和關(guān)節(jié)變量5</p><p>  3、求出兩桿間的位姿矩陣5</p><p>  4、求末桿的位姿矩陣6</p><p>  5、Matlab編程7</p><p><b>  6、驗證7</b>&

3、lt;/p><p>  PUMA560機器人的逆解8</p><p><b>  1、求8</b></p><p><b>  2、求8</b></p><p><b>  3、求9</b></p><p><b>  4、求10<

4、/b></p><p><b>  5、求10</b></p><p><b>  6、求11</b></p><p>  7、解的多重性11</p><p>  8、Matlab編程11</p><p>  9、對于機器人解的分析12</p>

5、<p>  機器人的雅克比矩陣12</p><p><b>  1、定義12</b></p><p>  2、雅可比矩陣的求法12</p><p>  3、微分變換法求機器人的雅可比矩陣13</p><p>  4、矢量積法求機器人的雅克比矩陣15</p><p>  5、Ma

6、tlab編程15</p><p><b>  附錄16</b></p><p><b>  1、程序16</b></p><p><b>  2、三維圖24</b></p><p><b>  摘要</b></p><p>

7、  機器人學(xué)作為一門高度交叉的前沿學(xué)科,引起許多具有不同專業(yè)背景人們的廣泛興趣,對其進行深入研究,并使其獲得快速發(fā)展。尤其是近年來各種新興技術(shù)飛速發(fā)展,機械工業(yè)產(chǎn)品的自動化、高精度、重負載等性能指標(biāo)變得越來越突出。因此在機器人學(xué)的計算中就要求更高的精度,計算機技術(shù)的發(fā)展很好的解決了這一問題。本文將以PUMA560為例,利用個人電腦平臺的Matlab對其運動學(xué)的正解、逆解以及雅克比矩陣進行計算研究。</p><p>

8、;<b>  關(guān) 鍵 詞</b></p><p>  PUMA560 Matlab 正解 逆解 雅克比矩陣 微分變換法 矢量積法</p><p><b>  ABSTRACT</b></p><p>  As a highly interspersed subject, the robotics makes m

9、any 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,h

10、igh 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 res

11、earch </p><p><b>  KEY WORDS</b></p><p>  PUMA560 Matlab Kinematics problem Positive-solution Inverse-solution Jacobian array Differential transformation Vector product transfo

12、rmation</p><p>  PUMA560機器人簡介</p><p>  PUMA560是屬于關(guān)節(jié)式機器人,6個關(guān)節(jié)都是轉(zhuǎn)動關(guān)節(jié),如圖1—1所示,前三個關(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的軸線為水平,且平行,距離為。關(guān)節(jié)

13、1和關(guān)節(jié)2的軸線垂直相交,關(guān)節(jié)3和關(guān)節(jié)4的軸線垂直交錯。距離為。各個連桿坐標(biāo)系如圖1—1所示,相應(yīng)的連桿參數(shù)列于表1—2中。其中,,,, 。</p><p>  在更進一步了解PUMA560機器人的轉(zhuǎn)動角度問題時,我們先來定義一下PUMA560機器人的初始位姿。首先,定義機器人的初始位置.取大臂處于某一朝向時,作為腰關(guān)節(jié)的初始位置.大臂處在水平位置時,作為肩關(guān)節(jié)的初始位置.小臂處在下垂位置,關(guān)節(jié)軸線Z4和Z0平行

14、時,作為肘關(guān)節(jié)的初始位置.關(guān)節(jié)軸線Z5和Z3平行時,作為腕扭轉(zhuǎn)關(guān)節(jié)的初始位置.關(guān)節(jié)軸線Z6和Z4平行時,作為腕彎曲關(guān)節(jié)的初始位置.抓手兩個指尖的連線與大臂平行時,作為腕旋轉(zhuǎn)關(guān)節(jié)的初始位置.在上述初始位置的前下,各個關(guān)節(jié)的零點位置得到確定.</p><p>  PUMA560機器人的正解</p><p>  1、確定D-H坐標(biāo)系</p><p>  PUMA 560的

15、關(guān)節(jié)全為轉(zhuǎn)動關(guān)節(jié):</p><p>  Zi坐標(biāo)軸:沿著i+1關(guān)節(jié)的運動軸;</p><p>  Xi坐標(biāo)軸:沿著Zi和Zi-1的公法線,指向離開Zi-1軸的方向;</p><p>  Yi坐標(biāo)軸:按右手直角坐標(biāo)系法則制定;</p><p>  連桿長度ai; Zi和Zi-1兩軸心線的公法線長度;</p><p>  

16、連桿扭角αi: Zi和Zi-1兩軸心線的夾角;</p><p>  兩連桿距離di: Xi和Xi-1兩坐標(biāo)軸的公法線距離;</p><p>  兩桿夾角θi :Xi和Xi-1兩坐標(biāo)軸的夾角;</p><p>  2、確定各連桿D-H參數(shù)和關(guān)節(jié)變量</p><p>  確定各連桿D-H參數(shù)和關(guān)節(jié)變量:</p><p> 

17、 3、求出兩桿間的位姿矩陣</p><p>  第i連桿與第i-1連桿間的變換矩陣</p><p>  Ai =Rot(x, αi-1)trans(ai-1,0,0)Rot(z, θi)trans(0,0,di)</p><p><b>  =</b></p><p>  相鄰兩個連桿間的位姿變換矩陣</p>

18、<p>  4、求末桿的位姿矩陣</p><p>  由上面的矩陣,我們可以得到最終結(jié)果:</p><p>  5、Matlab編程</p><p>  運行zhengjie.m,根據(jù)提示輸入的值,回車后的出T的解如下:</p><p><b>  T=</b></p><p>  

19、0 1.0000 0 -149.0900</p><p>  0 0 1.0000 864.8700</p><p>  1.0000 0 0 20.3200</p><p>  0 0 0 1.0000</p><p>

20、;<b>  6、驗證</b></p><p>  由課本給出的驗證公式進行所編程序的驗證,經(jīng)驗證,編程所得結(jié)果與課本給出驗證公式得到的結(jié)果一致。進一步表明所編程序是正確的。</p><p>  PUMA560機器人的逆解</p><p>  將PUMA 560的運動方程(3.64)寫為:</p><p>  若末端連桿

21、的位姿已經(jīng)給定,求關(guān)節(jié)變量 的值成為運動逆解。</p><p><b>  1、求</b></p><p>  式中,正、負號對應(yīng)于的兩個可能解。</p><p><b>  2、求</b></p><p>  由以上兩式的平方加上的平方可以得到:</p><p><b

22、> ?。?—2)</b></p><p><b>  在上式中, </b></p><p>  式(2—2)中已經(jīng)消去,所以可以由三角代換求解得到的解。</p><p>  所以:在的表達式中正、負號對應(yīng)于的兩種可能解。</p><p><b>  3、求</b></p>

23、;<p><b> ?。?—3)</b></p><p>  令矩陣方程(2—3)兩端的元素(1,4)和(2,4)分別對應(yīng)相等,則得兩方程:</p><p>  由以上兩式可得的表達式:</p><p><b>  由求得的,可求出:</b></p><p>  根據(jù)解的四種可能組合可

24、以得到相應(yīng)的四種可能值,于是可得到 的四種可能解。</p><p><b>  4、求</b></p><p>  令上式的矩陣方程的兩端的元素(1,4)和(2,4)分別對應(yīng)相等,則得兩方程:</p><p>  當(dāng)S5=0時,機械手處于奇異形位.此時,關(guān)節(jié)軸4和6重合,只能解出和的和或差.奇異形位可以由式的表達式中的atan2的兩個變

25、量是否接近零來判別.若都接近零,則為奇異形位,否則,不是奇異形位.在奇異形位時,可任意選取值,再計算相應(yīng)的值。</p><p><b>  5、求</b></p><p>  根據(jù)求出的,可以進一步解出:</p><p>  因為,,,在前面均已解出,逆變換為:</p><p>  令矩陣方程兩端的元素(1,3)和(3,

26、3)分別對應(yīng)相等,則得兩方程:</p><p>  所以可以得到的最終表達式:</p><p><b>  6、求</b></p><p>  令矩陣方程兩端的元素(3,1)和(1,1)分別對應(yīng)相等,則得兩方程:</p><p><b>  得到最后的表達式:</b></p><

27、p><b>  7、解的多重性</b></p><p>  PUMA560的運動反解可能存在8種解,但是,由于結(jié)構(gòu)的限制,例如各關(guān)節(jié)變量不能在全部360度范圍內(nèi)運動,有些解不能實現(xiàn)。在機器人存在多種解的情況下,應(yīng)選取其中最滿意的一組解,以滿足機器人的工作要求。</p><p>  8、Matlab編程</p><p>  在Matlab中

28、運行nijie.m,根據(jù)提示輸入的值并回車,可得的8組解值如下:</p><p>  90.0000 -2.6918 -84.6272 -180.0000 2.6810 180.0000</p><p>  90.0000 -0.0000 -90.0000 0 0.0000 0</p><p>  90.000

29、0 -2.6918 -84.6272 0.0000 -2.6810 -0.0000</p><p>  90.0000 -0.0000 -90.0000 0 -0.0000 0</p><p>  -70.4385 180.0000 -84.6272 104.7629 20.2581 74.3103</p>

30、;<p>  -70.4385 182.6918 -90.0000 97.5292 19.7387 82.0067</p><p>  -70.4385 180.0000 -84.6272 -75.2371 -20.2581 -105.6897</p><p>  -70.4385 182.6918 -90.0000 -82.4708 -

31、19.7387 -97.9933</p><p>  9、對于機器人解的分析</p><p>  通過編程可以知道,我們最終得到八組解。然后對八組解進行分析,對于的變化范圍為從,所以程序中我們得到的兩個解都是正確的。然后對進行分析,由于的角度變化范圍是從,所以在我們所得到的結(jié)果中,后四組是超出的變化范圍的,所以我們可以舍去后四組解。再逐個對、、、進行角度分析,最終可獲得適合的解。<

32、/p><p><b>  機器人的雅克比矩陣</b></p><p><b>  1、定義</b></p><p>  機械手的操作速度與關(guān)節(jié)速度間的線性變換定義為機械手的雅可比矩陣。</p><p>  2、雅可比矩陣的求法</p><p><b>  (1)矢量積法

33、 </b></p><p><b>  對移動關(guān)節(jié)</b></p><p><b>  對轉(zhuǎn)動關(guān)節(jié)</b></p><p><b> ?。?)微分變換法</b></p><p>  對于轉(zhuǎn)動關(guān)節(jié)i,相對連桿i-1,繞坐標(biāo)系{i}的軸所作微分轉(zhuǎn)動,其微分運動矢量為(3

34、-117),對應(yīng)的夾持器的微分運動矢量為(3-118):</p><p>  于是,J(q)的第i列如下:對轉(zhuǎn)動關(guān)節(jié)i:</p><p><b>  對移動關(guān)節(jié)i:</b></p><p>  3、微分變換法求機器人的雅可比矩陣</p><p>  PUMA560的6個關(guān)節(jié)都是轉(zhuǎn)動關(guān)節(jié),所以利用(3-121)求取雅克比矩

35、陣的列矢量。</p><p>  對于第1個關(guān)節(jié)來說,將中的n,o,a,p向量代入式(3-121),得到雅克比矩陣的列矢量。</p><p>  其中、、的表達式如下所示:</p><p>  對于第2個關(guān)節(jié)來說,將中的n,o,a,p向量代入式(3-121),得到雅克比矩陣的列矢量</p><p><b>  可以得到:</b

36、></p><p>  其中、、三個參數(shù)的表達式如下所示:</p><p><b>  同理,可求得:</b></p><p>  所以又以上六個矩陣便最后組成了的雅克比矩陣。</p><p>  4、矢量積法求機器人的雅克比矩陣</p><p>  PUMA560的6個關(guān)節(jié)都是轉(zhuǎn)動關(guān)節(jié),因

37、而其雅克比矩陣具有下列形式:</p><p>  5、Matlab編程</p><p> ?。?)用微分變換法求解雅克比矩陣</p><p>  在Matlab中運行wfbh.m,根據(jù)提示輸入的值并回車,得雅克比矩陣如下:</p><p>  -864.8700 0 0 0 0

38、 0</p><p>  -149.0900 20.3200 20.3200 0 0 0</p><p>  0 -864.8700 -433.0700 0 0 0</p><p>  0 -1.0000 -1.0000

39、 0 -1.0000 0</p><p>  0 0 0 1.0000 0 1.0000</p><p>  1.0000 0 0 0 0 0</p><p>  (2)用矢量積法求解雅克比矩陣&l

40、t;/p><p>  在Matlab中運行slj.m,根據(jù)提示輸入的值并回車,得雅克比矩陣如下:</p><p>  -864.8700 0 0 0 0 0</p><p>  -149.0900 20.3200 20.3200 0 0

41、 0</p><p>  0 -864.8700 -433.0700 0 0 0</p><p>  0 -1.0000 -1.0000 0 -1.0000 0</p><p>  0 0 0 1.0000

42、0 1.0000</p><p>  1.0000 0 0 0 0 0</p><p>  從以上結(jié)果中我們可以看出其運行結(jié)果與用微分變換編程所得到的結(jié)果是一致的,進一步證明了所編寫程序的正確性。</p><p><b>  附錄</b></p>

43、;<p><b>  1、程序</b></p><p><b>  1、運動學(xué)正解</b></p><p>  function zhengjie(c1,c2,c3,c4,c5,c6)</p><p><b>  clc;</b></p><p><b>

44、;  a2=431.8;</b></p><p><b>  a3=20.32;</b></p><p>  d2=149.09;</p><p>  d4=433.07;</p><p><b>  d6=56.25;</b></p><p>  c1=inpu

45、t('c1=');</p><p>  c2=input('c2=');</p><p>  c3=input('c3=');</p><p>  c4=input('c4=');</p><p>  c5=input('c5=');</p><

46、;p>  c6=input('c6=');</p><p>  T1=[cosd(c1) -sind(c1) 0 0;</p><p>  sind(c1),cosd(c1),0,0;</p><p><b>  0,0,1,0;</b></p><p>  0 0 0 1];</p>

47、<p>  T2=[cosd(c2),-sind(c2),0,0;</p><p><b>  0,0,1 d2;</b></p><p>  -sind(c2) -cosd(c2) 0 0;</p><p>  0 0 0 1];</p><p>  T3=[cosd(c3) -sind(c3) 0 a

48、2;</p><p>  sind(c3) cosd(c3) 0 0;</p><p><b>  0 0 1 0;</b></p><p><b>  0 0 0 1];</b></p><p>  T4=[cosd(c4) -sind(c4) 0 a3 ;</p><p>

49、;<b>  0 0 1 d4;</b></p><p>  -sind(c4) -cosd(c4) 0 0;</p><p><b>  0 0 0 1];</b></p><p>  T5=[cosd(c5) -sind(c5) 0 0;</p><p><b>  0 0 -1 0;

50、</b></p><p>  sind(c5) cosd(c5) 0 0;</p><p><b>  0 0 0 1];</b></p><p>  T6=[cosd(c6) -sind(c6) 0 0;</p><p><b>  0 0 1 0;</b></p>&l

51、t;p>  -sind(c6) -cosd(c6) 0 0;</p><p><b>  0 0 0 1];</b></p><p>  T=T1*T2*T3*T4*T5*T6;</p><p>  disp('T=');</p><p><b>  disp(T)</b>&l

52、t;/p><p><b>  2、運動學(xué)逆解</b></p><p>  function nijie(T)</p><p><b>  clc;</b></p><p>  nx=input('nx=');ox=input('ox=');ax=input('ax

53、=');</p><p>  ny=input('ny=');oy=input('oy=');ay=input('ay=');</p><p>  nz=input('nz=');oz=input('oz=');az=input('az=');</p><p> 

54、 px=input('px=');py=input('py=');pz=input('pz=');</p><p><b>  a2=431.8;</b></p><p><b>  a3=20.32;</b></p><p>  d2=149.09;</p>

55、<p>  d4=433.07;</p><p>  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))];%求解c1</p><p>  c1=c1/pi*180;</p><p>  k=(px*px+py*

56、py+pz*pz-a2*a2-a3*a3-d2*d2-d4*d4)/(2*a2);</p><p>  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))];%求解c3</p><p>  c3=c3/pi*180;</p><p>

57、<b>  for i=1:2</b></p><p><b>  for j=1:2</b></p><p>  m1=cosd(c1(i));m2=sind(c1(i));</p><p>  n1=cosd(c3(j));n2=sind(c3(j));</p><p>  c23(i,j)=a

58、tan2(-(a3+a2*n1)*pz+(m1*px+m2*py)*(a2*n2-d4),(-d4+a2*n2)*pz+(m1*px+m2*py)*(a2*n1+a3));</p><p>  c23(i,j)=c23(i,j)/pi*180;</p><p>  c2(i,j)=c23(i,j)-c3(1,j);</p><p><b>  end<

59、;/b></p><p><b>  end%求解c2</b></p><p><b>  for i=1:2</b></p><p><b>  for j=1:2</b></p><p>  m1=cosd(c1(i));n1=sind(c1(i));</p&g

60、t;<p>  m2=cosd(c23(i,j));n2=sind(c23(i,j));</p><p>  c41(i,j)=atan2(-ax*n1+ay*m1,-ax*m1*m2-ay*n1*m2+az*n2);</p><p>  c411(i,j)=atan2(ax*n1-ay*m1,ax*m1*m2+ay*n1*m2-az*n2);</p><

61、p>  c41(i,j)=c41(i,j)/pi*180;</p><p>  c411(i,j)=c411(i,j)/pi*180;</p><p><b>  end</b></p><p><b>  end%求解c4</b></p><p>  c4=[c41,c411];</p

62、><p><b>  disp(c4)</b></p><p>  c23=[c23(1,:),c23(1,:);c23(2,:),c23(2,:)];</p><p><b>  for i=1:2</b></p><p><b>  for j=1:4</b></p>

63、;<p>  m1=cosd(c1(i));n1=sind(c1(i));</p><p>  m2=cosd(c23(i,j));n2=sind(c23(i,j));</p><p>  m3=cosd(c4(i,j));n3=sind(c4(i,j));</p><p>  sinc5(i,j)=-ax*(m1*m2*m3*n1*n3)-ay*(n

64、1*m2*m3-m1*n3)+az*(n2*m3);</p><p>  cosc5(i,j)=ax*(-m1*n2)+ay*(-n1*n2)+az*(-m2);</p><p>  c5(i,j)=atan2(sinc5(i,j),cosc5(i,j));</p><p>  c5(i,j)=c5(i,j)/pi*180;</p><p>

65、<b>  end</b></p><p><b>  end%求解c5</b></p><p><b>  for i=1:2</b></p><p><b>  for j=1:4</b></p><p>  m1=cosd(c1(i));n1=sin

66、d(c1(i));</p><p>  m2=cosd(c23(i,j));n2=sind(c23(i,j));</p><p>  if sind(c5(i,j))<0.01&&sind(c5(i,j))>-0.01</p><p>  c4(i,j)=0;</p><p><b>  end</

67、b></p><p><b>  end</b></p><p>  end%奇異形位判斷</p><p><b>  for i=1:2</b></p><p><b>  for j=1:4</b></p><p>  m1=cosd(c1(i

68、));n1=sind(c1(i));</p><p>  m2=cosd(c23(i,j));n2=sind(c23(i,j));</p><p>  m3=cosd(c4(i,j));n3=sind(c4(i,j));</p><p>  m4=cosd(c5(i,j));n4=sind(c5(i,j));</p><p>  sinc6(

69、i,j)=-nx*(m1*m2*n3-n1*m3)-ny*(n1*m2*n4+m1*m4)+nz*(n2*n3);</p><p>  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);</p><p>  c6(i,j)=atan2(sinc

70、6(i,j),cosc6(i,j));</p><p>  c6(i,j)=c6(i,j)/pi*180;</p><p><b>  end</b></p><p><b>  end%求解c6</b></p><p>  C1=[c1(1),c1(1),c1(1),c1(1),c1(2),c1(

71、2),c1(2),c1(2)];</p><p>  C2=[c2(1,:),c2(1,:),c2(2,:),c2(2,:)];</p><p>  C3=[c3(1),c3(2),c3(1),c3(2),c3(1),c3(2),c3(1),c3(2)];</p><p>  C23=[c23(1,:),c23(2,:)];</p><p>

72、  C4=[c4(1,:),c4(2,:)];</p><p>  C5=[c5(1,:),c5(2,:)];</p><p>  C6=[c6(1,:),c6(2,:)];%排序</p><p>  C=[C1;C2;C3;C4;C5;C6];%輸出</p><p><b>  C=C';</b></p&

73、gt;<p><b>  disp(C);</b></p><p>  3、微分變換法求雅克比矩陣</p><p>  function wfbh(c1,c2,c3,c4,c5,c6)</p><p>  c1=input('c1=');</p><p>  c2=input('c2

74、=');</p><p>  c3=input('c3=');</p><p>  c4=input('c4=');</p><p>  c5=input('c5=');</p><p>  c6=input('c6=');</p><p><

75、;b>  a2=431.8;</b></p><p><b>  a3=20.32;</b></p><p>  d2=149.09;</p><p>  d4=433.07;</p><p><b>  d6=56.25;</b></p><p>  T1

76、0=[cosd(c1) -sind(c1) 0 0;</p><p>  sind(c1),cosd(c1),0,0;</p><p><b>  0,0,1,0;</b></p><p>  0 0 0 1];</p><p>  T21=[cosd(c2),-sind(c2),0,0;</p><

77、;p><b>  0,0,1 d2;</b></p><p>  -sind(c2) -cosd(c2) 0 0;</p><p>  0 0 0 1];</p><p>  T32=[cosd(c3) -sind(c3) 0 a2;</p><p>  sind(c3) cosd(c3) 0 0;</p&

78、gt;<p><b>  0 0 1 0;</b></p><p><b>  0 0 0 1];</b></p><p>  T43=[cosd(c4) -sind(c4) 0 a3 ;</p><p><b>  0 0 1 d4;</b></p><p> 

79、 -sind(c4) -cosd(c4) 0 0;</p><p><b>  0 0 0 1];</b></p><p>  T54=[cosd(c5) -sind(c5) 0 0;</p><p><b>  0 0 -1 0;</b></p><p>  sind(c5) cosd(c5) 0

80、 0;</p><p><b>  0 0 0 1];</b></p><p>  T65=[cosd(c6) -sind(c6) 0 0;</p><p><b>  0 0 1 0;</b></p><p>  -sind(c6) -cosd(c6) 0 0;</p><p&

81、gt;<b>  0 0 0 1];</b></p><p>  T64=T54*T65;</p><p>  T63=T43*T64;</p><p>  T62=T32*T63;</p><p>  T61=T21*T62;</p><p>  T60=T10*T61;</p>

82、<p>  T(:,:,1)=T61;</p><p>  T(:,:,2)=T62;</p><p>  T(:,:,3)=T63;</p><p>  T(:,:,4)=T64;</p><p>  T(:,:,5)=T65;</p><p>  N=T(1:3,1,:);</p><

83、p>  O=T(1:3,2,:);</p><p>  A=T(1:3,3,:);</p><p>  P=T(1:3,4,:);</p><p>  PN=cross(P,N);</p><p>  PN=PN(3,1,:);</p><p>  PO=cross(P,O);</p><p&

84、gt;  PO=PO(3,1,:);</p><p>  PA=cross(P,A);</p><p>  PA=PA(3,1,:);</p><p>  J=[PN;PO;PA;N(3,1,:);O(3,1,:);A(3,1,:)];</p><p>  J6=[0;0;0;0;0;1];</p><p>  J=[

85、J(:,:,1),J(:,:,2),J(:,:,3),J(:,:,4),J(:,:,5),J6];</p><p>  R60=T60(1:3,1:3);</p><p>  J=[R60,zeros(3);zeros(3),R60]*J;</p><p><b>  disp(J);</b></p><p>  4、矢

86、量積法求雅克比矩陣</p><p>  function wfbh(c1,c2,c3,c4,c5,c6)</p><p>  c1=input('c1=');</p><p>  c2=input('c2=');</p><p>  c3=input('c3=');</p><

87、;p>  c4=input('c4=');</p><p>  c5=input('c5=');</p><p>  c6=input('c6=');</p><p><b>  a2=431.8;</b></p><p><b>  a3=20.32;&l

88、t;/b></p><p>  d2=149.09;</p><p>  d4=433.07;</p><p><b>  d6=56.25;</b></p><p>  T10=[cosd(c1) -sind(c1) 0 0;</p><p>  sind(c1),cosd(c1),0,0

89、;</p><p><b>  0,0,1,0;</b></p><p>  0 0 0 1];</p><p>  T21=[cosd(c2),-sind(c2),0,0;</p><p><b>  0,0,1 d2;</b></p><p>  -sind(c2) -

90、cosd(c2) 0 0;</p><p>  0 0 0 1];</p><p>  T32=[cosd(c3) -sind(c3) 0 a2;</p><p>  sind(c3) cosd(c3) 0 0;</p><p><b>  0 0 1 0;</b></p><p><b&

91、gt;  0 0 0 1];</b></p><p>  T43=[cosd(c4) -sind(c4) 0 a3 ;</p><p><b>  0 0 1 d4;</b></p><p>  -sind(c4) -cosd(c4) 0 0;</p><p><b>  0 0 0 1];<

92、/b></p><p>  T54=[cosd(c5) -sind(c5) 0 0;</p><p><b>  0 0 -1 0;</b></p><p>  sind(c5) cosd(c5) 0 0;</p><p><b>  0 0 0 1];</b></p><

93、p>  T65=[cosd(c6) -sind(c6) 0 0;</p><p><b>  0 0 1 0;</b></p><p>  -sind(c6) -cosd(c6) 0 0;</p><p><b>  0 0 0 1];</b></p><p>  T64=T54*T65;&l

94、t;/p><p>  T63=T43*T64;</p><p>  T62=T32*T63;</p><p>  T61=T21*T62;</p><p>  T60=T10*T61;</p><p>  T(:,:,1)=T61;</p><p>  T(:,:,2)=T62;</p>

95、<p>  T(:,:,3)=T63;</p><p>  T(:,:,4)=T64;</p><p>  T(:,:,5)=T65;</p><p>  N=T(1:3,1,:);</p><p>  O=T(1:3,2,:);</p><p>  A=T(1:3,3,:);</p>&l

96、t;p>  P=T(1:3,4,:);</p><p>  PN=cross(P,N);</p><p>  PN=PN(3,1,:);</p><p>  PO=cross(P,O);</p><p>  PO=PO(3,1,:);</p><p>  PA=cross(P,A);</p><

97、;p>  PA=PA(3,1,:);</p><p>  J=[PN;PO;PA;N(3,1,:);O(3,1,:);A(3,1,:)];</p><p>  J6=[0;0;0;0;0;1];</p><p>  J=[J(:,:,1),J(:,:,2),J(:,:,3),J(:,:,4),J(:,:,5),J6];</p><p>

溫馨提示

  • 1. 本站所有資源如無特殊說明,都需要本地電腦安裝OFFICE2007和PDF閱讀器。圖紙軟件為CAD,CAXA,PROE,UG,SolidWorks等.壓縮文件請下載最新的WinRAR軟件解壓。
  • 2. 本站的文檔不包含任何第三方提供的附件圖紙等,如果需要附件,請聯(lián)系上傳者。文件的所有權(quán)益歸上傳用戶所有。
  • 3. 本站RAR壓縮包中若帶圖紙,網(wǎng)頁內(nèi)容里面會有圖紙預(yù)覽,若沒有圖紙預(yù)覽就沒有圖紙。
  • 4. 未經(jīng)權(quán)益所有人同意不得將文件中的內(nèi)容挪作商業(yè)或盈利用途。
  • 5. 眾賞文庫僅提供信息存儲空間,僅對用戶上傳內(nèi)容的表現(xiàn)方式做保護處理,對用戶上傳分享的文檔內(nèi)容本身不做任何修改或編輯,并不能對任何下載內(nèi)容負責(zé)。
  • 6. 下載文件中如有侵權(quán)或不適當(dāng)內(nèi)容,請與我們聯(lián)系,我們立即糾正。
  • 7. 本站不保證下載資源的準(zhǔn)確性、安全性和完整性, 同時也不承擔(dān)用戶因使用這些下載資源對自己和他人造成任何形式的傷害或損失。

評論

0/150

提交評論