機(jī)械臂運(yùn)動(dòng)學(xué)資料

1、機(jī)械臂運(yùn)動(dòng)學(xué)基礎(chǔ)1機(jī)械臂的運(yùn)動(dòng)學(xué)模型機(jī)械臂運(yùn)動(dòng)學(xué)研究的是機(jī)械臂運(yùn)動(dòng)，而不考慮產(chǎn)生運(yùn)動(dòng)的力。運(yùn)動(dòng)學(xué)研究機(jī)械臂的位置，速度和加速度。機(jī)械手的運(yùn)動(dòng)學(xué)的研究涉及到的幾何和基于時(shí)間的內(nèi)容，特別是各個(gè)關(guān)節(jié)彼此之間的關(guān)系以及隨時(shí)間變化規(guī)律。典型的機(jī)器人由一些串行連接的關(guān)節(jié)和連桿組成。每個(gè)關(guān)節(jié)具有一個(gè)自由度，平移或旋轉(zhuǎn)。對(duì)于具有n個(gè)關(guān)節(jié)的機(jī)械臂，關(guān)節(jié)的編號(hào)從 1到n,有n +1個(gè)連桿，編號(hào)從0到n。連桿0 是機(jī)械臂的基礎(chǔ)，一般是固定的，連桿n上帶有末端執(zhí)行器。關(guān)節(jié)i連接連桿i和連桿i-1。一個(gè)連桿可以被視為一個(gè)剛體，確定與它相鄰的兩個(gè)關(guān)節(jié)的坐標(biāo)軸之間的相對(duì)位置。一個(gè)連桿可以用兩個(gè)參數(shù)描述，連桿長(zhǎng)度和連桿扭轉(zhuǎn)，

2、這兩個(gè)量定義了與它相關(guān)的兩個(gè)坐標(biāo)軸在空 間的相對(duì)位置。而第一連桿和最后一個(gè)連桿的參數(shù)沒(méi)有意義，一般選擇為0。一個(gè)關(guān)節(jié)用兩個(gè)參數(shù)描述，一是連桿的偏移，是指從一個(gè)連桿到下一個(gè)連桿沿的關(guān)節(jié)軸線(xiàn)的距離。二是關(guān)節(jié)角度，指一個(gè)關(guān)節(jié)相對(duì)于下一個(gè)關(guān)節(jié)軸的旋轉(zhuǎn)角度。為了便于描述的每一個(gè)關(guān)節(jié)的位置，我們?cè)诿恳粋€(gè)關(guān)節(jié)設(shè)置一個(gè)坐標(biāo)系，對(duì)于一個(gè)關(guān)節(jié)鏈，Denavit和Hartenberg提出了一種用矩陣表示各個(gè)關(guān)節(jié)之間關(guān)系的系統(tǒng)方法。對(duì)于轉(zhuǎn)動(dòng)關(guān)節(jié) i，規(guī)定它的轉(zhuǎn)動(dòng)平行于坐標(biāo)軸zi-1，坐標(biāo)軸xi-1對(duì)準(zhǔn)從zi-1到zi的法線(xiàn)方向，如果 zi-1與zi相交，則xi-1取zi-1 x的方向。連桿，關(guān)節(jié)參數(shù)概括如下：連桿長(zhǎng)度

3、ai沿著xi軸從zi-1和zi軸之間的距離；連桿扭轉(zhuǎn)ai 從zi-1軸到zi軸相對(duì)xi-1軸夾角;連桿偏移di從坐標(biāo)系i-1的原點(diǎn)沿著zi-1軸到xi軸的距離；關(guān)節(jié)角度Qi xi-1軸和xi軸之間關(guān)于zi-1軸的夾角。對(duì)于一個(gè)轉(zhuǎn)動(dòng)關(guān)節(jié) Q i是關(guān)節(jié)變量，di是常數(shù)。而移動(dòng)關(guān)節(jié) di是可變的，Q i是恒定的。為 了統(tǒng)一，表示為乜轉(zhuǎn)動(dòng)關(guān)節(jié)di移動(dòng)關(guān)節(jié)0T運(yùn)用Denavit-Hartenberg (DH)方法，可以將相鄰的兩個(gè)坐標(biāo)系之間的變換關(guān)系表示為一個(gè)-cosq-sinq coswsin Qsi njai cosqsinqCOS COSi-cosq sinwai sinq0sinwcostidi

4、_ 0001 一上式表示出了坐標(biāo)系i相對(duì)于坐標(biāo)系i-1的關(guān)系。即4x4的齊次變換矩陣*其中Tj表示坐標(biāo)系i相對(duì)于世界坐標(biāo)系0的位置與姿態(tài)，簡(jiǎn)稱(chēng)位姿。2、正向和反向運(yùn)動(dòng)學(xué)對(duì)于一個(gè)n-軸剛性連接的機(jī)械臂，正向運(yùn)動(dòng)學(xué)的解給出的是最后一個(gè)連桿坐標(biāo)系的位置和 姿態(tài)。重復(fù)利用上式，得到工=人人川2人=K（q）機(jī)械臂末端位姿在笛卡爾坐標(biāo)系中有6個(gè)自由度，3個(gè)平移，3個(gè)旋轉(zhuǎn)。所以，一般來(lái)說(shuō)具有6個(gè)自由度的機(jī)械臂可以使末端實(shí)現(xiàn)任意的位姿?？偟臋C(jī)械臂變換Tn 一般簡(jiǎn)寫(xiě)為T(mén)n，對(duì)6個(gè)自由度的機(jī)械臂簡(jiǎn)寫(xiě)為 T6。對(duì)于任意的機(jī)械臂，無(wú)論其它有多少個(gè)關(guān)節(jié)，具有什么結(jié)構(gòu)，正向運(yùn)動(dòng)學(xué)解都是可以得到的。在機(jī)械臂的路徑規(guī)劃中，用

5、到的是反向運(yùn)動(dòng)學(xué)的解q = K J（Tn），它給出了特定的末端位姿對(duì)應(yīng)的機(jī)械臂的關(guān)節(jié)角度。一般來(lái)說(shuō)，反向運(yùn)動(dòng)學(xué)的解不是唯一的，對(duì)具有某種結(jié)構(gòu)的機(jī)械臂，封閉解可能不存在。對(duì)于6自由度的機(jī)器人而言，運(yùn)動(dòng)學(xué)逆解非常復(fù)雜，一般沒(méi)有封閉解。只有在某些特殊情況 下才可能得到封閉解。不過(guò)，大多數(shù)工業(yè)機(jī)器人都滿(mǎn)足封閉解的兩個(gè)充分條件之一（Pieper準(zhǔn)則）（1）三個(gè)相鄰關(guān)節(jié)軸交于一點(diǎn)（2）三個(gè)相鄰關(guān)節(jié)軸相互平行如果機(jī)械臂多于6個(gè)關(guān)節(jié)，稱(chēng)關(guān)節(jié)為冗余的，這時(shí)解是欠定的。如果對(duì)于機(jī)械臂某個(gè)特別的 位姿，解不存在，稱(chēng)這個(gè)位姿為奇異位姿。機(jī)械臂的奇異性可能是由于機(jī)械臂中某些坐標(biāo)軸的平行，或位置不能達(dá)到引起的。機(jī)械臂的奇

6、異位姿分為兩類(lèi)：（1）邊界奇異位姿，當(dāng)機(jī)械臂的關(guān)節(jié)全部展開(kāi)或折起時(shí)，使得末端處于操作空間的邊界或邊界附近，雅克比矩陣奇異，機(jī)械臂的運(yùn)動(dòng)受到物理結(jié)構(gòu)的約束，這時(shí)機(jī)械臂的奇異位置稱(chēng)為邊界奇異位姿。（2） 內(nèi)部奇異位姿，兩個(gè)或兩個(gè)以上的關(guān)節(jié)軸線(xiàn)重合時(shí)，機(jī)械臂各個(gè)關(guān)節(jié)的運(yùn)動(dòng)相互抵消， 不產(chǎn)生操作運(yùn)動(dòng)，這時(shí)機(jī)械臂的奇異位姿稱(chēng)為內(nèi)部奇異位姿。械臂臂運(yùn)動(dòng)學(xué)逆解的方法可以分為兩類(lèi)：封閉解和數(shù)值解、在進(jìn)行逆解時(shí)總是力求得到封閉解。因?yàn)榉忾]解的計(jì)算速度快，效率高，便于實(shí)時(shí)控制。而數(shù)值法不具有些特點(diǎn)。械臂運(yùn)動(dòng) 學(xué)的封閉逆解可通過(guò)兩種途徑得到：代數(shù)法和幾何法。一般而言，非零連桿參數(shù)越多，到達(dá)某一目標(biāo)的方式也越多，即運(yùn)

7、動(dòng)學(xué)逆解的數(shù)目也越多。在從多重解中選擇解時(shí)，應(yīng)根據(jù)具體情況，在避免碰撞的前提下通常按 最短行程”準(zhǔn)則來(lái)選 擇。同時(shí)還應(yīng)當(dāng)兼顧 多移動(dòng)小關(guān)節(jié)，少移動(dòng)大關(guān)節(jié) ”的原則。n個(gè)自由度的機(jī)械臂的末端位姿由n個(gè)關(guān)節(jié)變量所決定， 這n個(gè)關(guān)節(jié)變量統(tǒng)稱(chēng)為n維關(guān)節(jié)矢量，記為q所有的關(guān)節(jié)矢量構(gòu)成的空間稱(chēng)為 關(guān)節(jié)空間。機(jī)械臂末端的位姿 x是在直角坐標(biāo)空 間中描述的，因此，稱(chēng)該空間為 操作空間或作業(yè)定向空間。機(jī)器人各關(guān)節(jié)驅(qū)動(dòng)器的位置統(tǒng)稱(chēng) 為驅(qū)動(dòng)矢量s，由這些矢量組成的空間稱(chēng)為 驅(qū)動(dòng)空間。正向運(yùn)動(dòng)學(xué)3、Jacobian 矩陣機(jī)械臂的Jacobian矩陣表示機(jī)械臂的操作空間與關(guān)節(jié)空間之間速度的線(xiàn)性映射關(guān)系，對(duì)于個(gè)n-軸的機(jī)

8、械臂，笛卡爾坐標(biāo)系中的基座速度是0Xn 二 0jq末端速度是其中x是6個(gè)元素的向量。對(duì)于6個(gè)關(guān)節(jié)機(jī)械臂Jacobian矩陣是方陣，如果它是可逆的，則可以由機(jī)械臂的末端速度求出各個(gè)關(guān)節(jié)的速度。Jacobian矩陣在運(yùn)動(dòng)的奇異點(diǎn)的位置是不可逆的。在實(shí)際應(yīng)用中，當(dāng)機(jī)械臂的末端位置接近奇異位置時(shí)，Jacobian矩陣是病態(tài)的，可能導(dǎo)致關(guān)節(jié)速度不能準(zhǔn)確地得到。雅可比矩陣可以看成是從關(guān)節(jié)空間到操作空間運(yùn)動(dòng)速度的傳動(dòng)比，同時(shí)也可用來(lái)表示兩空間之間力的傳遞關(guān)系。首先來(lái)看一個(gè)兩自由度的平面機(jī)械手，容易求得X =h G +I2 ci2 G =COS（d）,G2 =COS + 日2） y T1S1+I2S12 s

9、=sin但 1）,S2 =sin +玄）兩邊微分后寫(xiě)成矩陣形式r exCX 1嚴(yán)1-rdc02嚴(yán)J!dyj胡 1溯2 一Tl S|2 S12dx|l_l1 cil2 ci2_l2 SI2ZI2C12 g，簡(jiǎn)寫(xiě)成dx=Jd B,式中J就稱(chēng)為機(jī)械手的雅可比(Jacobian)矩陣，它由函數(shù) x, y的偏微分組成，反映了關(guān)節(jié)微小位移d B與機(jī)械臂末端微小運(yùn)動(dòng)dx之間的關(guān)系。dx/dt=Jd B/dt,因此機(jī)械手的雅可比矩陣定義為它的操作空間速度 與關(guān)節(jié)空間速度的線(xiàn)性變換。dx/dt稱(chēng)為手爪在操作空間中的廣義速度，簡(jiǎn)稱(chēng)操作速度， dB/dt為關(guān)節(jié)速度。可以看出，雅可比矩陣的每一列表示其它關(guān)節(jié)不動(dòng)而某一

10、關(guān)節(jié)以單位速度運(yùn)動(dòng)產(chǎn)生的端點(diǎn)速度。h S1 I2 S12IU1 G I2 G2I 2 S12 I即B和B2的改2 12可以看出，J陣的值隨末端位置的不同而不同, I2 c|2變會(huì)導(dǎo)致J的變化。對(duì)于關(guān)節(jié)空間的某些形位，機(jī)械手的雅可比矩陣的秩減少，這些形位稱(chēng) 為操作臂（機(jī)械手）的奇異形位。上例機(jī)械手雅可比矩陣的行列式為：det（J） icosd）,當(dāng)0=0?；?2=180。時(shí)，機(jī)械手的雅可比行列式為0,矩陣的秩為1,因此處于奇異狀態(tài)。比J是滿(mǎn)秩的方陣,在奇異形位時(shí)，機(jī)械手在操作空間的自由度將減少。如果機(jī)械手的雅可相應(yīng)的關(guān)節(jié)速度即可求出，即扌=JJx，上例平面2R機(jī)械手的逆雅可，顯然，當(dāng)02趨于0

11、（或180 ）時(shí)，機(jī)械l2Cl2l2si2Ihl2s2 hG 1$ 丨2$2手接近奇異形位，相應(yīng)的關(guān)節(jié)速度將趨于無(wú)窮大。為了補(bǔ)償機(jī)器人末端執(zhí)行器位姿與目標(biāo)物體之間的誤差，以及解決兩個(gè)不同坐標(biāo)系之間的微位移關(guān)系問(wèn)題，需要討論機(jī)器人桿件在作微小運(yùn)動(dòng)時(shí)的位姿變化。假設(shè)一變換的元素是某個(gè)變量的函數(shù)，對(duì)該變換的微分就是該變換矩陣各元素對(duì)該變量的偏T為:t11t21t3141t12t22t32t42t13 t23 t33 t43t14t24t34t44導(dǎo)數(shù)所組成的變換矩陣乘以該變量的微分。例如給定變換若它的元素是變量x的函數(shù)，則變換 T的微分為：dT 二dxexdxexFt 212252324dxexdx

12、ex“3132醴33醴34dxexdx&總41我4243醴44一 exexexex戲14爲(wèi)2氏1dx下面討論機(jī)械臂的微分運(yùn)動(dòng)， 設(shè)機(jī)器人某一桿件相對(duì)于基坐標(biāo)系的位姿為 該桿件相對(duì)基坐標(biāo)系的位姿變?yōu)?T+dT ,若這個(gè)微運(yùn)動(dòng)是相對(duì)于基坐標(biāo)系 乘），總可以用微小的平移和旋轉(zhuǎn)來(lái)表示，即T,經(jīng)過(guò)微運(yùn)動(dòng)后 （靜系）進(jìn)行的（左T dT =Tran s(dx,dy, dz)Rot(k,dRT所以有dT = Trans(dx,dy,dz)Rot(k,昶)一 1做ti （動(dòng)系）進(jìn)行的（右乘），則T+dT根據(jù)齊次變換的對(duì)稱(chēng)性，若微運(yùn)動(dòng)是相對(duì)某個(gè)桿件坐標(biāo)系 可以表示為T(mén) dT=T Trans(dx,dy,dz)Ro

13、t(k,)所以有dT =T Trans(dx,dy,dz)Rot(k, d日)一144令:-Trans(dx,dy,dz)Rot(k,dr)-打4為微分算子，則相對(duì)基系有 dT= 0T,相對(duì)i系有dT=T i。這里的下標(biāo)不同是由于微運(yùn)動(dòng)相對(duì)不同坐標(biāo)系進(jìn)行的。在機(jī)械臂運(yùn)動(dòng)學(xué)中 微分變換分為微分平移和微分旋轉(zhuǎn)兩類(lèi)。微分平移變換與一般平移變換一樣，其變換矩陣為：10 0 dx0 10dyTrans(dx,dy,dz)=0 01dz衛(wèi) 0 0 1 一由于微分旋轉(zhuǎn) 0 0，所以sin 0 d, cos將它們代入旋轉(zhuǎn)變換通式中得微分旋轉(zhuǎn)表達(dá)式：-1-kzd 日kyd601Rot(k,dT)=kzd日1_k

14、xd 日0-kydOkxdQ100001于是得微分算子也二Trans(dx,dy,dz)Rot(k,dB) 1収4，即0-kzdvkyd vdxkzd 日 0kxd 日 dyA =kyd 日kxd 日0dz.0 000 一微分旋轉(zhuǎn)的無(wú)序性，當(dāng)0時(shí)，有sind,0 cos 01若令 S x=d 0, S y=d 0,y S z=d 0,則繞三個(gè)坐標(biāo)軸的微分旋轉(zhuǎn)矩陣分別為-10001-106y01I1-bz001Rot(xx)=01-5x0Rot(y,6y)=0100Rot(z,5z)=5z1000dx10-5y0100010I00010001L0001略去2次項(xiàng)，得到-10&y0-10紬01y

15、1-5x001-6 x0二 Rot(x,6x)Rot(y, 6y)=J10-6 y6x100 001_-0001 一-1y6y01-106y00 1001一帝x0Rot(y, Oy)Rot(xx)=-y莎 x100 y6x10L0 001 一-0001 一兩者結(jié)果相同，可見(jiàn)這里左乘與右乘等效。結(jié)論：微分旋轉(zhuǎn)其結(jié)果與轉(zhuǎn)動(dòng)次序無(wú)關(guān)，這是與有限轉(zhuǎn)動(dòng)(一般旋轉(zhuǎn))的一個(gè)重要區(qū)別。同理可得-1-6z6y 0Rot（xJ5x） Rot（ y,芬 y） Rot（z, 6 z）=6z1一右x06 ydx100001若Rot(3x谷y SZ和Rot (3 x, S y, S z)表示兩個(gè)不同的微分旋轉(zhuǎn)，則兩次連續(xù)

16、轉(zhuǎn)動(dòng)的結(jié)果為：1_(6z + 6z)6z+6z1Rot(6x,6y,6z)Rotx,6y,6z)=丄_(6y+5y)6x+6x- 0 0上式表明：任意兩個(gè)微分旋轉(zhuǎn)的結(jié)果為繞每個(gè)軸轉(zhuǎn)動(dòng)的元素的代數(shù)和，:.y 、y 0 _x + dxl) 0 1001_即微分旋轉(zhuǎn)是可加的。合稱(chēng)為微分運(yùn)動(dòng)矢量由等效轉(zhuǎn)軸和等效轉(zhuǎn)角與Rot(x,、；x)Rot(yy)Rot(zz)等效，有-1-kzdkyd日01-1d z0kzd91-kxd06z1o x0-kyd 日kxd106 y100001一.0001一kyd 0 =S , kzd 0=S,將它們代入得1_ 0z0dx=0-氏dyy0dz000 0Rot（k,d

17、R 二 Rot（x,、x）Rot（y,、y）Rot（z,、z）d微分平移矢量所以有kxd 0 = S,可見(jiàn)微分變換由兩個(gè)部分組成S微分轉(zhuǎn)動(dòng)矢量可表示為 D =(dx，dy ,dz，、X，、y，、z)T例：已知一個(gè)坐標(biāo)系 A二105，相對(duì)固定系的微分平移矢量d=10 0.5，微-0-冠zydx 1-000.11 1dz0-冠xdy0000-dyQ0dz-0.1000.50000 一10000 一求微分變換dA。分旋轉(zhuǎn)矢量3=00.10 ,-000.110 0 1 10100.101 1000010050000-0.1000.5010000-0.1-0.53000 _0001 _10000 一dA

18、 = . A =第二個(gè)坐標(biāo)系為i系，j系不失一般OxaxPx0T =nyOyayPyi 1nzOzazPz0001 _- 0o z6ydx-0_zyidx 1因?yàn)锳0 =6z0o xdyA0心xdy0-5y6x0dz-冠yi6 Xi0dz0000 _0000 一j系就是固定的0系。下面討論兩坐標(biāo)系之間的微分關(guān)系，設(shè)第一個(gè)坐標(biāo)系為 性，假定所以也oOT=0TAi，舟二0T血0T，整理得到dix = n(C P) d) diy =oK( P) d)diz =a(eP) d)ix = n_；iy =0_iz =adxidyi d乙 hi貝0ny0yaynz0zaz000(P n)x (p n)y(P

19、 o)x (p 0)y (P a)x (P a)ynxny0x0yaxay(pxndx(P8)zdy。(P3)zdz0nzgOzaz工z0 一對(duì)于任何三維矢量P=Px, Py, Pz，其反對(duì)稱(chēng)矩陣S(p)定義為:0-PzPyS(P)= I Pz 0-Px-PyPx0 一h 0x ax0 ci R ny Oy 3yJiz 0z az _上式簡(jiǎn)寫(xiě)成；0rt-0RTs(0pi0)l00rt一A和B之間廣義速度的坐標(biāo)變換為:類(lèi)似地，任意兩坐標(biāo)系bvLBr -：rs(aPb)avvLQr -BArsCPa。)BJ0:RAJ %0 bR例：已知一個(gè)坐標(biāo)系 A105，相對(duì)固定系的微分平移矢量d=100.5，

20、微分旋轉(zhuǎn)矢量3=0解：將d=10衛(wèi)0.1 0 ,0.5和同00.10 代入dix =nkcP)d)diyP)d)diz 二aL(GP)d)求A系中等價(jià)的微分平移矢量dA和微分旋轉(zhuǎn)矢量&ixdiydiz=n_：=o_二 a_、得到 dA - 100 5.1 T - 10.10 0 T o3、 robotics 工具箱中的運(yùn)動(dòng)學(xué)求解函數(shù)使用 函數(shù) fkine 的調(diào)用格式tr =fkine (ROBOT, Q)ROBOT 表示機(jī)械臂對(duì)象， Q 機(jī)械臂關(guān)節(jié)坐標(biāo)值。函數(shù) ikine 的調(diào)用格式 q = ikine(ROBOT, T) q = ikine(ROBOT, T, Q) q = ikine(RO

21、BOT, T, Q, M) 輸入變量ROBOT 表示機(jī)械臂對(duì)象， T 機(jī)械臂末端變換矩陣。 輸出變量 q 機(jī)械臂關(guān)節(jié)的角度 (單位是弧度 )，一般來(lái)說(shuō)逆運(yùn)動(dòng)學(xué)的解不是唯一的，取決于初 始值Q,缺省時(shí)是0向量。如果機(jī)械臂的自由度 (DOF)小于6，此方法無(wú)效。由于解空間的 維數(shù)大于機(jī)械臂的自由度， 這時(shí)需要第 4 個(gè)輸入量 M 來(lái)確定笛卡爾坐標(biāo) (手腕對(duì)應(yīng)的坐標(biāo)系 ) 中的哪些量在求解中被忽略。 M 中有 6 個(gè)元素，分別表示沿著 x,y,z 方向的平移和相對(duì)于 x 軸，y軸，z軸的旋轉(zhuǎn)，值是0(忽略)或1。非零元素的個(gè)數(shù)應(yīng)該等于機(jī)械臂的自由度。例如， 對(duì)典型的有 5個(gè)自由度的機(jī)械臂，一般是忽略

22、相對(duì)手腕坐標(biāo)的轉(zhuǎn)動(dòng)，這時(shí) M = 1 1 1 1 1 0 。 另外一種用法是 qt = ikine(ROBOT, TG) qt = ikine (ROBOT, TG , Q) qt = ikine (ROBOT, TG , Q, M)輸入變量 ROBOT 表示機(jī)械臂對(duì)象， TG 是 4x4xN 機(jī)械臂末端變換矩陣。輸出變量qt是一組(N個(gè))TG對(duì)應(yīng)的關(guān)節(jié)坐標(biāo)。一行對(duì)應(yīng)一個(gè)輸入變換，每一步的初始值取上一步的 值。求解使用機(jī)械臂 Jacobian矩陣的偽逆，這是數(shù)值求解方法，對(duì)于特定機(jī)械臂逆運(yùn)動(dòng)學(xué)解 (如果可能 )應(yīng)該盡量使用解析解。但是這種方法可以得到奇異點(diǎn)上的解，零空間中的關(guān)節(jié)角 度可以任取。

23、函數(shù) transl 的調(diào)用格式tr= transl (X, Y , Z)tr= transl( X Y Z ) 返回機(jī)械臂末端坐標(biāo) X, Y, Z 對(duì)應(yīng)的齊次表?yè)Q表示X Y Z = transl(T) 返回齊次表?yè)Q表示中的平移值，作為一個(gè) 3 元素的列向量X Y Z = TRANSL(TG)從笛卡爾坐標(biāo)系的軌跡 TG 中得到 X, Y 和 Z 的值。函數(shù) ctraj 的調(diào)用格式tc= ctraj(T0, T1, N)tc = ctraj(T0, T1, R)返回從 T0 到 T1 笛卡爾坐標(biāo)系的軌跡 TC N 表示軌跡中的點(diǎn)數(shù)。 在第 1 中情況下， 軌跡中 的點(diǎn)在 T0 到 T1 中等距離分

24、配。在第 2 中情況下，向量 R 給出軌跡中每個(gè)點(diǎn)的距離， R 中 的元素取值為 0 1 。一個(gè)軌跡是 4x4xN 矩陣，最后一個(gè)下標(biāo)表示點(diǎn)索引。函數(shù) trinterp 的調(diào)用格式tr = trinterp (T0, T1, R)返回從 T0 到 T1 齊次變換的插值矩陣。 R 中的元素取值為 0 1 。旋轉(zhuǎn)變換使用 4 元素球坐 標(biāo)線(xiàn)性插值。puma560qr=0 pi/2 -pi/2 0 0 0x=fkine(p560,qr)xyz=transl(x) q=ikine(p560,x)q=-0.0000 1.5238 -1.4768 -0.0000 -0.0470 0.0000 %注意逆解不唯一 transl(fkine(p560,q) %驗(yàn)證末端位置ans= 0.0203 -0.1500 0.8636 x0=0.3 0.2 0.5x1=0.3 0.2 -0.5 tc=ctraj(transl(x0),transl(x1),5) qc=ikine(p560,tc) qc=mod(qc,2*pi) %驗(yàn)證末端位置 transl(fkine(p560,qc(1,:) transl(fkin