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

1、工業(yè)機器人課程設(shè)計 基于 Matlab 的工業(yè)機器人運動學和雅克比較矩陣求解 目錄 PUMA560 機器人簡介機器人簡介 .4 PUMA560 機器人的正解機器人的正解 .5 1、確定 D-H 坐標系.5 2、確定各連桿 D-H 參數(shù)和關(guān)節(jié)變量.5 3、求出兩桿間的位姿矩陣.5 4、求末桿的位姿矩陣.6 5、MATLAB編程.7 6、驗證.7 PUMA560 機器人的逆解機器人的逆解 .8 1、求 1 .8 2、求 3 .8 3、求 2 .9 4、求 4 .10 5、求 5 .10 6、求 6 .11 7、解的多重性.11 8、MATLAB編程.11 9、對于機器人解的分析.12 機器人的雅克

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

3、lab 對 其運動學的正解、逆解以及雅克比矩陣進行計算研究。 關(guān) 鍵 詞 PUMA560 Matlab 正解 逆解 雅克比矩陣 微分變換法 矢量積法 ABSTRACT As 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

4、 technologies, mechanical products indexes of automation，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 p

5、roblem and Jacobian array of PUMA560. KEY WORDS PUMA560 Matlab Kinematics problem Positive-solution Inverse-solution Jacobian array Differential transformation Vector product transformation PUMA560 機器人簡介 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é)軸線交于一點。該點

6、選作為手腕參考點，也選作為 4、5、6的原點。關(guān)節(jié)一的軸線為垂直方向，關(guān)節(jié) 2 和關(guān)節(jié) 3 的軸線為水 平，且平行，距離為。關(guān)節(jié) 1 和關(guān)節(jié) 2 的軸線垂直相交，關(guān)節(jié) 3 和關(guān)節(jié) 4 的 a2 軸線垂直交錯。距離為 。各個連桿坐標系如 a3 圖 11 所示，相應(yīng)的連 桿參數(shù)列于表 12 中。 其中，mm a 8 . 431 2 ，mm a 32.20 3 ， mm d 09.149 2 。mm d 07.433 3 在更進一步了解 PUMA560 機器人的轉(zhuǎn) 動角度問題時，我們先 來定義一下 PUMA560 機器人的初始位姿。首 先,定義機器人的初始位 置.取大臂處于某一朝向 時,作為腰關(guān)節(jié)的

7、初始位 置.大臂處在水平位置時, 作為肩關(guān)節(jié)的初始位置. 小臂處在下垂位置,關(guān)節(jié)軸線 Z4 和 Z0 平行時,作為肘關(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é)的零點位置得到確定. PUMA560 機器人的正解 1、確定、確定 D-H 坐標系坐標系 PUMA 560 的關(guān)節(jié)全為轉(zhuǎn)動關(guān)節(jié): Zi 坐標軸:沿著 i+1 關(guān)節(jié)的運動軸; Xi 坐標軸:沿著 Zi 和 Zi-1 的公法線,指向離開 Zi-1 軸的方向;

8、Yi 坐標軸:按右手直角坐標系法則制定; 連桿長度 ai; Zi 和 Zi-1 兩軸心線的公法線長度; 連桿扭角 i: Zi 和 Zi-1 兩軸心線的夾角; 兩連桿距離 di: Xi 和 Xi-1 兩坐標軸的公法線距離; 兩桿夾角 i :Xi 和 Xi-1 兩坐標軸的夾角; 2、確定各連桿、確定各連桿 D-H 參數(shù)和關(guān)節(jié)變量參數(shù)和關(guān)節(jié)變量 確定各連桿 D-H 參數(shù)和關(guān)節(jié)變量： 連桿連桿 i變量變量 ii-1ai-1di變量范圍變量范圍 11000-160160 22-900d2-22545 330a20-45225 44-90a3d4-110170 559000-100100 66-9000-

9、266266 3、求出兩桿間的位姿矩陣、求出兩桿間的位姿矩陣 第 i 連桿與第 i-1 連桿間的變換矩陣 Ai =Rot(x, i-1)trans(ai-1,0,0)Rot(z, i)trans(0,0,di) = 1 1111 1111 0 0001 iii iiiiiii iiiiiii csa s cc cssd s sc sccd 相鄰兩個連桿間的位姿變換矩陣 11 110 1 00 00 0010 0001 cs sc T 22 1 2 22 00 0010 00 0001 cs T sc 33 332 3 00 00 0010 0001 cs sc T 44 3 4 44 00 0

10、010 00 0001 cs T sc 55 4 5 55 00 0010 00 0001 cs T sc 66 5 6 66 00 0010 00 0001 cs T sc 4、求末桿的位姿矩陣、求末桿的位姿矩陣 0012345 6112233445566 ( )()()()()()TTTTTTT 001 616 0001 xxxx yyyy zzzz noap noap TT T noap 由上面的矩陣，我們可以得到最終結(jié)果： 1234 5 64 623 5 614 5 64 6 1234 5 64 623 5 614 5 64 6 234 5 64 623 5 6 1234 5 64 6

11、23 5 614 64 5 6 1234 5 64 623 5 614 x y z x y nccc c cs ss s css c cc s nscc c cs ss s ccs c cc s nsc c cs sc s c occc c ss cs s ss c cs c s oscc c ss cs s sc c c 64 5 6 234 5 64 623 5 6 123 4 523 51 4 5 123 4 523 51 4 5 23 4 523 5 1223 234 232 1 1223 234 232 1 3 232 2423 z x y z x y z s c c osc c s

12、s cc s s ac c c ss cs s s as c c ss cc s s as c sc c pc a ca cd sd s ps a ca cd sd c pa sa sd c 5、Matlab 編程編程 運行 zhengjie.m，根據(jù)提示輸入的值，回車后的出 T 的解如下： 16 T= 0 1.0000 0 -149.0900 0 0 1.0000 864.8700 1.0000 0 0 20.3200 0 0 0 1.0000 6、驗證、驗證 123456 22346 90 ,0,90 ,0,00 431.8,149.09,20.32,433.07,56.25 oo amm

13、dmmammdmmdmm 2 240 6 3 010 001 100 0001 d ad T a 由課本給出的驗證公式進行所編程序的驗證，經(jīng)驗證，編程所得結(jié)果與課本給出驗證 公式得到的結(jié)果一致。進一步表明所編程序是正確的。 PUMA560 機器人的逆解 將 PUMA 560 的運動方程（3.64）寫為: 0012345 6112233445566 ( )()()()()() 0001 xxxx yyyy zzzz noap noap TTTTTTT noap 若末端連桿的位姿已經(jīng)給定,求關(guān)節(jié)變量 的值成為運動逆解。 16 1、求、求 1 01012345 1162233445566 TTTTT

14、TT 11 111 6 00 00 0010 00010001 xxxx yyyy zzzz csnopa scnoap T noap 2 1212 2 22 1 222 122 sin()/;cos()1 (/) atan2,1 atan2(,)atan2(, yxxy dd dd ppdppd 式中,正、負號對應(yīng)于的兩個可能解。 1 2、求、求 3 113 234 2322 3 234232 2 xy z c ps pa cd sa c pa sd ca s 由以上兩式的平方加上的平方可以得到： 112 -s xy pc pd （22） 3 34 3 a cd sk 在上式中， 22222

15、22 2324 2 2 xyz pppaadd k a 式（22）中已經(jīng)消去，所以可以由三角代換求解得到的解。 2 3 所以：在的表達式中正、負號對應(yīng)于的兩種可能解。 3 3 222 33434 atan2(,)atan2( ,)a dkadk 3、求、求 2 010345 31236445566 ,TTTTT （23） 1 231 23232 3 1 231 23232 33 6 112 0 00010001 xxxx yyyy zzzz c cs csa cnopa c ss sca snoap T scdnoap 4 5 64 64 5 64 64 53 5 65 654334 646

16、4 5 64 64 5 64 64 5 0 0001 c c cs sc c ss sc sa s ss scd TT T s s sc ss c sc cs s 令矩陣方程(23)兩端的元素(1,4)和(2,4)分別對應(yīng)相等,則得兩方程: 1 231 23232 33 1 231 23232 34 xyz xyz c s ps c ps pa ca c s ps s pc pa sd 由以上兩式可得的表達式： 23 232332 3112 34 42 3112 33 tan2 ()()(), ()()() zxy xy aaa cpc ps pa sd da spc ps pa ca 由求得

17、的，可求出： 23 2 2233 根據(jù)解的四種可能組合可以得到相應(yīng)的四種可能值，于是可得到 13 和 23 的四種可能解。 2 4、求、求 4 1 231 23232 3 1 231 23232 33 6 112 0 00010001 xxxx yyyy zzzz c cs csa cnopa c ss sca snoap T scdnoap 令上式的矩陣方程的兩端的元素(1,4)和(2,4)分別對應(yīng)相等,則得兩方程： 1 231 23234 5 114 5 xyz xy a c ca s ca sc s a sa cs s 5 41111 231 2323 42111 231 2323 0

18、atan2(,) atan2(,) xyxyz xyxyz s a sa ca c ca s ca s a sa c a c ca s ca s 當 當 S5=0 時,機械手處于奇異形位.此時,關(guān)節(jié)軸 4 和 6 重合,只能解出和的和 4 6 或差.奇異形位可以由式的表達式中的 atan2 的兩個變量是否接近零來判別.若 4 都接近零,則為奇異形位,否則,不是奇異形位.在奇異形位時,可任意選取值,再計 4 算相應(yīng)的值。 6 5、求、求 5 根據(jù)求出的，可以進一步解出： 4 5 010454 4123465566656 ,TTTTT 因為，在前面均已解出，逆變換為： 1 2 3 4 01 412

19、34 ( ,)T 1 23 41 41 23 41 423 42 3 42 43 4 1 23 41 41 23 41 423 42 3 4243 4 1 231 23232 34 0001 c c cs ss c cc ss ca c cd sa c c c ss ss c sc cs sa c sd ca s c ss sca sd 令矩陣方程兩端的元素(1,3)和(3,3)分別對應(yīng)相等,則得兩方程： 1 23 41 41 23 41 423 45 1 231 23235 xyz xyz acc cs sas c cc sas cs ac sas sacc 所以可以得到的最終表達式： 5

20、555 atan2( ,)s c 6、求、求 6 0105 512345666 ,TTT 令矩陣方程兩端的元素(3,1)和(1,1)分別對應(yīng)相等,則得兩方程： 61 23 41 41 23 41 423 4 61 23 41 451 23 51 23 41 451 23 5 23 4 523 5 ()()() () () xyz xy z sn c c ss cns c sc cn s s cnc c cs scc s sns c cc s cs s s n s c cc s 得到最后的表達式： 6 666 atan2(,)s c 7、解的多重性、解的多重性 PUMA560 的運動反解可能存在

21、 8 種解，但是，由于結(jié)構(gòu)的限制，例如各關(guān) 節(jié)變量不能在全部 360 度范圍內(nèi)運動，有些解不能實現(xiàn)。在機器人存在多種解 的情況下，應(yīng)選取其中最滿意的一組解，以滿足機器人的工作要求。 8、Matlab 編程編程 在 Matlab 中運行 nijie.m,根據(jù)提示輸入的值并回車，可, xxxyyyzzz n o a n o an o a 得的 8 組解值如下： i 90.0000 -2.6918 -84.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

22、 -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.4385 182.6918 -90.0000 -82.4708 -19.7387 -97.9933 9、對于機器人解的分析、對于機器人解的分析 通過編程可以知道，我們最

23、終得到八組解。然后對八組解進行分析，對于 的變化范圍為從，所以程序中我們得到的兩個解都是正確的。 1 00 160160 然后對進行分析，由于的角度變化范圍是從，所以在我們所 2 2 00 45225 得到的結(jié)果中，后四組是超出的變化范圍的，所以我們可以舍去后四組解。 2 再逐個對、進行角度分析，最終可獲得適合的解。 3 4 5 6 機器人的雅克比矩陣 1、定義、定義 機械手的操作速度與關(guān)節(jié)速度間的線性變換定義為機械手的雅可比矩陣。 ( )( )xx qxJ q q ( ) ( ),1,2,6,1,2, i ij j x q Jqijn q 2、雅可比矩陣的求法、雅可比矩陣的求法 （1）矢量積

24、法 對移動關(guān)節(jié) , 00 ii ii vzz qJ w 對轉(zhuǎn)動關(guān)節(jié) iooi nin pR p （2）微分變換法 對于轉(zhuǎn)動關(guān)節(jié) i,相對連桿 i-1,繞坐標系i的軸所作微分轉(zhuǎn)動,其微分運動 i z i d , i o n i i ii i o n i i z pz Jq z pz w v 矢量為(3-117),對應(yīng)的夾持器的微分運動矢量為(3-118)： 00 0 ,0(3 117)(3 118 01 T x z T y z T z z ii T zx T zy T zz pnd p od pad ddd n o a ） 于是,J(q)的第 i 列如下:對轉(zhuǎn)動關(guān)節(jié) i： , z z TT li

25、aiz z z z pnn Jp oJo paa 對移動關(guān)節(jié) i： 0 ,0 0 z TT lizai z n JoJ a 3、微分變換法求機器人的雅可比矩陣、微分變換法求機器人的雅可比矩陣 PUMA560 的 6 個關(guān)節(jié)都是轉(zhuǎn)動關(guān)節(jié),所以利用(3-121)求取雅克比矩陣的列矢量。 對于第 1 個關(guān)節(jié)來說,將中的 n,o,a,p 向量代入式(3-121)，得到雅克比矩陣的 1 6 T 列矢量。 1 1 1 1 234 5 64 62356 234 5 64 62356 23 4 523 5 T x T y T Tz J J J Jq sc c cs sc s c sc c ss cc s s

26、s c sc c 其中、的表達式如下所示： x TJ 1x TT 2x TT 3 14 5 64 53 232342234 5 64 623 5 6 14 5 64 63 232342234 5 64 623 5 6 14 53 23234223 4 523 5 ()() ()()() ()() T x T y T z Js s sc sa cs dd cc c cs ss s s Js c sc ca cs dd cc c cs ss s s Js s a cs dd c c ss c 對于第 2 個關(guān)節(jié)來說,將中的 n,o,a,p 向量代入式(3-121),得到雅克比矩陣的列 2 6 T

27、矢量 2 TJ q 可以得到： 2 2 2 2 4 54 66 4 5 64 6 4 5 T x T y T Tz J J J Jq s cc sc s c sc c s s 其中、三個參數(shù)的表達式如下所示： x TJ 2y TJ 2z TJ 2 同理,可求得： 234 5 64 63 5 63 3342 34 5 64 63 5 63 334 234 5 64 63 5 63 3342 34 5 64 63 5 63 334 23 53 4 53 33423 4 53 53 () () () () ()()()( T x T y T z Jsc c cs sc s sa cs da cc c

28、 cs ss s sa sc d Jsc c ss sc s sa cs da cc c ss ss s s a sc d Jc cs c sa cs dac c ss ca 334) sc d 3456 , TTTT JqJqJqJq 44 5 64 635 6 44 5 64 635 6 44 53 6 3 4 5 64 6 4 5 64 6 4 5 T dc c cs sas c dc c ss cas s d c sa c Jq s c cc s s c sc c s s 456 5 66 5 66 5 000 000 000 , 0 0 01 TTT JqJqJq s cs s sc

29、 c 所以又以上六個矩陣便最后組成了的雅克比矩陣。1666 4、矢量積法求機器人的雅克比矩陣、矢量積法求機器人的雅克比矩陣 PUMA560 的 6 個關(guān)節(jié)都是轉(zhuǎn)動關(guān)節(jié),因而其雅克比矩陣具有下列形式： 102060 162666 126 zpzpzp J q zzz 5、Matlab 編程編程 （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 -

30、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，根據(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 從以上結(jié)果中我們可以看出其運行結(jié)果與用微分變換編程所得到的結(jié)果是

31、一致的，進一步證明了所編寫程序的正確性。 附錄 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=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(

32、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) 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;

33、 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=); nz=input(nz=);oz=input(oz=);az=input(az=); px=input(px=);py=input(py=);pz=input(pz=); a2=431.8; a3=2

34、0.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);%求解 c1 c1=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),atan2(a3,d4)-atan2(k,- sqrt(a3*a3+d4*d4-k*k);%求解 c3 c3

35、=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(i,j)=c23(i,j)-c3(1,j); end end%求解 c2 for i=1:2 for j=1:2 m1=cosd(c1(i);n1=sind(c1(i)

36、; 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; end end%求解 c4 c4=c41,c411; disp(c4) c23=c23(1,:),c23(1,:);c23(2,:),c23(2,:); for i=1:2 for j=1:4 m1=

37、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; end end%求解 c5 for i=1:2 for j=1:4 m

38、1=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 end end%奇異形位判斷 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); m4=cosd(c5(i,j);n4=sind(c5(i,j); sinc6(i,j)=-nx*(m1*m2*n3-n1*m3)- ny*

39、(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; end end%求解 c6 C1=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)

40、,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(c3=); c4=input(c4=); c5=input(c5=); c6=input(c

41、6=); 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 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