機械臂運動學(xué)

機械臂運動學(xué)基礎(chǔ)1、機械臂的運動學(xué)模型機械臂運動學(xué)研究的是機械臂運動，而不考慮產(chǎn)生運動的力。運動學(xué)研究機械臂的位置，速度和加速度。機械臂的運動學(xué)的研究涉及到的幾何和基于時間的內(nèi)容，特別是各個關(guān)節(jié)彼此之間的關(guān)系以及隨時間變化規(guī)律。典型的機械臂由一些串行連接的關(guān)節(jié)和連桿組成。每個關(guān)節(jié)具有一個自由度，平移或旋轉(zhuǎn)。對于具有n個關(guān)節(jié)的機械臂，關(guān)節(jié)的編號從1到n，有n+1個連桿，編號從0到n。連桿0是機械臂的基礎(chǔ)，一般是固定的，連桿n上帶有末端執(zhí)行器。關(guān)節(jié)i連接連桿i和連桿i-1。一個連桿可以被視為一個剛體，確定與它相鄰的兩個關(guān)節(jié)的坐標軸之間的相對位置。一個連桿可以用兩個參數(shù)描述，連桿長度和連桿扭轉(zhuǎn)，這兩個量定義了與它相關(guān)的兩個坐標軸在空間的相對位置。而第一連桿和最后一個連桿的參數(shù)沒有意義，一般選擇為0。一個關(guān)節(jié)用兩個參數(shù)描述，一是連桿的偏移，是指從一個連桿到下一個連桿沿的關(guān)節(jié)軸線的距離。二是關(guān)節(jié)角度，指一個關(guān)節(jié)相對于下一個關(guān)節(jié)軸的旋轉(zhuǎn)角度。為了便于描述的每一個關(guān)節(jié)的位置，我們在每一個關(guān)節(jié)設(shè)置一個坐標系，對于一個關(guān)節(jié)鏈，Denavit和Hartenberg提出了一種用矩陣表示各個關(guān)節(jié)之間關(guān)系的系統(tǒng)方法。對于轉(zhuǎn)動關(guān)節(jié)i，規(guī)定它的轉(zhuǎn)動平行于坐標軸zi-1，坐標軸xi-1對準從zi-1到zi的法線方向，如果zi-］與%相交，則xi-1取z」］xzi的方向。連桿，關(guān)節(jié)參數(shù)概括如下：連桿長度ai沿著xi軸從zi-1和zi軸之間的距離；?連桿扭轉(zhuǎn)ai從zi-1軸到zi軸相對xi-1軸夾角；連桿偏移di從坐標系i-1的原點沿著zi-1軸到*軸的距離；?關(guān)節(jié)角度0ixi-1軸和xi軸之間關(guān)于zi-1軸的夾角。

對于一個轉(zhuǎn)動關(guān)節(jié)。j是關(guān)節(jié)變量，q是常數(shù)。而移動關(guān)節(jié)d.是可變的次j是恒定的。為了統(tǒng)一，表示為運用Denavit-Hartenberg（DH）方法，可以將相鄰的兩個坐標系之間的變換關(guān)系表示為一個4x4的齊次變換矩陣i-1A=icos0isin0i0一sin0cos以iicos0cos以iisini-1A=icos0isin0i0一sin0cos以iicos0cos以iisin以i0sin0sin以一cos0sin以cos以i0acos0

asin0ii-1i其中0T表示坐標系i相對于世界坐標系0的位置與姿態(tài)，簡稱位姿。i2、正向和反向運動學(xué)對于一個n-軸剛性連接的機械臂，正向運動學(xué)的解給出的是最后一個連桿坐標系的位置和姿態(tài)。重復(fù)利用上式，得到0T=0AiAn-iA=K（q）n12n機械臂末端位姿在笛卡爾坐標系中有6個自由度，3個平移，3個旋轉(zhuǎn)。所以，一般來說具有6個自由度的機械臂可以使末端實現(xiàn)任意的位姿?？偟臋C械臂變換0T一般簡寫為Tn，對6個自由度的機械臂簡寫為T6。對于任意的機械臂，無論其它有多少個關(guān)節(jié)，具有什么結(jié)構(gòu)，正向運動學(xué)解都是可以得到的。在機械臂的路徑規(guī)劃中，用到的是反向運動學(xué)的解q=K-1（0T），它給出了特定的末端位姿對應(yīng)的機械臂的關(guān)節(jié)角度。一般來說，反向運動學(xué)的解不是唯一的，對具有某種結(jié)構(gòu)的機械臂，封閉解可能不存在。對于6自由度的機器人而言，運動學(xué)逆解非常復(fù)雜，一般沒有封閉解。只有在某些特殊情況下才可能得到封閉解。不過大多數(shù)工業(yè)機器人都滿足封閉解的兩個充分條件之一（Pieper準則）（1）三個相鄰關(guān)節(jié)軸交于一點（2）三個相鄰關(guān)節(jié)軸相互平行如果機械臂多于6個關(guān)節(jié)，稱關(guān)節(jié)為冗余的，這時解是欠定的。如果對于機械臂某個特別的位姿，解不存在，稱這個位姿為奇異位姿。機械臂的奇異性可能是由于機械臂中某些坐標軸的重合，或位置不能達到引起的。機械臂的奇異位姿分為兩類：（1）邊界奇異位姿，當(dāng)機械臂的關(guān)節(jié)全部展開或折起時，使得末端處于操作空間的邊界或邊界附近，雅克比矩陣奇異，機械臂的運動受到物理結(jié)構(gòu)的約束，這時機械臂的奇異位姿稱為邊界奇異位姿。（2）內(nèi)部奇異位姿，兩個或兩個以上的關(guān)節(jié)軸線重合時，機械臂各個關(guān)節(jié)的運動相互抵消，不產(chǎn)生操作運動，這時機械臂的奇異位姿稱為內(nèi)部奇異位姿。機械臂運動學(xué)逆解的方法可以分為兩類：封閉解和數(shù)值解、在進行逆解時總是力求得到封閉解。因為封閉解的計算速度快，效率高，便于實時控制。而數(shù)值解法不具有這些特點。機械臂運動學(xué)的封閉逆解可通過兩種途徑得到：代數(shù)法和幾何法。一般而言，非零連桿參數(shù)越多，到達某一目標的方式也越多，即運動學(xué)逆解的數(shù)目也越多。在從多重解中選擇解時，應(yīng)根據(jù)具體情況，在避免碰撞的前提下通常按"最短行程”準則來選擇。同時還應(yīng)當(dāng)兼顧"多移動小關(guān)節(jié)，少移動大關(guān)節(jié)"的原則。n個自由度的機械臂的末端位姿由n個關(guān)節(jié)變量所決定，這n個關(guān)節(jié)變量統(tǒng)稱為n維關(guān)節(jié)矢量，記為q。所有的關(guān)節(jié)矢量構(gòu)成的空間稱為關(guān)節(jié)空間。機械臂末端的位姿用6個變量描述，3個平移儀歸）和3個旋轉(zhuǎn)（①/o，①J，記x=（x,y,乙氣,氣,％），x是機械臂末端在基xyzxyz坐標空間中的坐標，所有的矢量X構(gòu)成的空間稱為操作空間或作業(yè)定向空間。工作空間是操作臂的末端能夠到達的空間范圍，即末端能夠到達的目標點集合。值得指出的是，工作空間應(yīng)該嚴格地區(qū)分為兩類：（1）靈活（工作）空間指機械臂末端能夠以任意方位到達的目標點集合。因此，在靈活空間的每個點上，手爪的指向可任意規(guī)定。⑵可達（工作）空間指機械臂末端至少在一個方位上能夠到達的目標點集合。機械臂各關(guān)節(jié)驅(qū)動器的位置組成的矢量稱為驅(qū)動矢量s，由這些矢量構(gòu)成的空間稱為驅(qū)動空間正向運動學(xué)驅(qū)動空間4-關(guān)節(jié)空間4-工作空間運動學(xué)逆解3、Jacobian矩陣機械臂的Jacobian矩陣表示機械臂的操作空間與關(guān)節(jié)空間之間速度的線性映射關(guān)系，對于一個n軸的機械臂，機械臂末端在基坐標系中的速度是X=Jq其中x是6個元素的向量。??對于6個關(guān)節(jié)機械臂Jacobian矩陣是方陣，如果它是可逆的，則可以由機械臂的末端速度求出各個關(guān)節(jié)的速度。Jacobian矩陣在機械臂的奇異位姿上是不可逆的。在實際應(yīng)用中，當(dāng)機械臂的末端位置接近奇異位置時，Jacobian矩陣是病態(tài)的，可能導(dǎo)致關(guān)節(jié)速度不能正確地得到。上式解決的是正速度問題，即已知q和q求末端執(zhí)行器的速度x。對于逆速度解問題，由上??

式可以得到速度逆解公式為q—J-1x,注意到此時需要求雅可比矩陣的逆，由線性方程組理論知上式對任意的X,q都有解的必要條件是雅可比矩陣的秩rank(J)=6，這意味著機械臂的自由度數(shù)n>6o這也說明了具有冗余自由度的機械臂，在末端位姿固定的條件下，能使關(guān)節(jié)在一個較大的關(guān)節(jié)空間的子空間中運動，有效地避開障礙或奇異位姿，并把關(guān)節(jié)位移限制在允許范圍內(nèi)，從而具有更大的運動靈活性。雅可比矩陣可以看成是從關(guān)節(jié)空間到操作空間運動速度的傳動比同時也可用來表示兩空間之間力的傳遞關(guān)系。對于冗余自由度機械臂，其雅可比矩陣是長方矩陣，因J滿秩且方程個數(shù)少于未知數(shù)個數(shù)，所以有無窮多個解，這時，一般是求其中的最小范數(shù)解，或采用加權(quán)最小范數(shù)解也就是說使|\qTD^最小的解旗中D是對稱正定加權(quán)矩陣。此時的解是使機械臂在能量消耗最小的情況下的解。這時，逆速度問題便轉(zhuǎn)為：求q滿足q—J-1x且使L—1qTDq最小。實際上等同于求性能-2指標L在約束條件q—J-1X下的極值。應(yīng)用Lagrange乘子法，以上極值為題的解是q—D-1Jt(JD-iJt)-ix，當(dāng)D=I時，雅可比矩陣是J+-Jt(JJt)-i,稱為雅可比矩陣的偽逆。下面通過一個兩自由度的平面機械臂說明雅可比矩陣的特性，根據(jù)右圖中的幾何關(guān)系容易求得x—lc+1cc—cos(0),c—cos(0+0)11212111212y—1s+1ss—sin(0),s—sin(0+0)11212111212兩邊微分后寫成矩陣形式dxdydx801d01dxdF2d02」d0d012e「dx］「-1s-1s-1s］「d0即IIdxdydx801d01dxdF2d02」d0d012e「dx］「-1s-1s-1s］「d0即II—I11212212II1IdyII1c+1c1c|ld0112122122.—ls—ls—ls.,__,，由J=11212212可以看出，J陣的值隨末端位置的不同而不同，即。1和02的lC+1clc1211212212改變會導(dǎo)致J的變化。對于關(guān)節(jié)空間的某些位姿，機械臂的雅可比矩陣的秩減少，這些位姿稱為機械臂的奇異位姿。上例機械臂雅可比矩陣的行列式為：det(J)=〈孕諷氣)，當(dāng)02=0°或02=180°時，機械臂的雅可比行列式為0，矩陣的秩為1，這時機械臂處于奇異位姿。機械臂在操作空間的自由度將減少。如果機械臂的雅可比J是滿秩的方陣，相應(yīng)的關(guān)節(jié)速度即可求出，即Q=J-1x，上例平面1「lcls2R機械臂的逆雅可比矩陣J-1=占_*"_ls2—]s，顯然，當(dāng)02趨于0°(或122L1121211212」180°)時，機械臂接近奇異位姿，相應(yīng)的關(guān)節(jié)速度將趨于無窮大。為了補償機器人末端執(zhí)行器位姿與目標物體之間的誤差以及解決兩個不同坐標系之間的微位移關(guān)系問題，需要討論機器人連桿在作微小運動時的位姿變化。假設(shè)一變換的元素是某個變量的函數(shù)對該變換的微分就是該變換矩陣各元素對該變量的偏導(dǎo)數(shù)所組成的變換矩陣乘以該變量的微分。例如給定變換T為：一tttt一11121314T=tttt21222324tttt31323334tttt1-41424344若它的元素是變量x的函數(shù)，則變換T的微分為:

dT—dt—12dxdt——-dxdt—32dxdtdt—13dxdt—23dxdtdt—14dxdt―24dxdt—34dxdxdt—43dxdt下面討論機械臂的微分運動，設(shè)機械臂某一連桿相對于基坐標系的位姿為T,dT—dt—12dxdt——-dxdt—32dxdtdt—13dxdt—23dxdtdt—14dxdt―24dxdt—34dxdxdt—43dxdt所以有一dT—[Trans(d,d,d)Rot(k,dO)-144]T根據(jù)齊次變換的對稱性，若微運動是相對某個連桿坐標系i(動系)進行的(右乘)，則T+dT可以表示為T+dT=T-Trans(d,d,d)Rot(k,do)所以有一dT—T[Trans(d,d,d)Rot(k,dO)-144"令△-Trans(d,d,d)Rot(k,do)-1為微分算子-則相對基系有dT=A0T,相對i系xyz4x4有dT=TAi。這里A的下標不同是由于微運動相對不同坐標系進行的。在機械臂運動學(xué)中微分變換分為微分平移和微分旋轉(zhuǎn)兩類。微分平移變換與一般平移變換一樣，其變換矩陣為:100dx010dyTrans(dx,dy,dz)—001dz0001

由于微分旋轉(zhuǎn)時0-0，所以sine-de,cose-1將它們代入旋轉(zhuǎn)變換通式中得微分旋轉(zhuǎn)表達式:「1-kdekde0-kdez1y-kde0Rot(k,de)=zx-kdekde10yLox001_于是得到微分算子A=Trans(d,d,d)Rot(k,de)-1，即xyz4x4「0-kdekdedxkdez一yA=0-kdedyzkdex-kde0dzyxL0000微分旋轉(zhuǎn)與有限旋轉(zhuǎn)相比，有一些特殊的性質(zhì)，下面分別說明。(1)微分旋轉(zhuǎn)的無序性，當(dāng)e—0時，有sine-de,cose—1.若令6x=dex,6y=dey,5z=d0z，則繞三個坐標軸的微分旋轉(zhuǎn)矩陣分別為「1000「「106y0-「1-6z00_01-6x001006z100zRot(x,6x)=Rot(y,6y)=Rot(z,6z)=06x10-6y010001000010001_0001略去2次項，得到「106y0-「106y0"6x6y1-6x001-6x0「.Rot(x,6x)Rot(y,6y):=-6y6x10=-6y6x10l0001_l0001_「16x6y6y0-「106y0"01-6x001-6x0xRot(y,6y)Rot(x,6x)=-6y6x10—=-6y6x10L0001_一0001_兩者結(jié)果相同，可見這里左乘與右乘等效。結(jié)論：微分旋轉(zhuǎn)其結(jié)果與轉(zhuǎn)動次序無關(guān)，這是與有限轉(zhuǎn)動(一般旋轉(zhuǎn))的一個重要區(qū)別。(2)微分旋轉(zhuǎn)的可加性，考慮兩個微分旋轉(zhuǎn)復(fù)合后的效果

Rot(x,8x)Rot(y,8Rot(x,8x)Rot(y,8y)Rot(z,8z)=-8z8y01-8x08x10若Rot(6x,6y,6z)和Rot(6x’，阿，6Z）表示兩個不同的微分旋轉(zhuǎn)，則兩次連續(xù)轉(zhuǎn)動的結(jié)果為：Rot(8x,8y,8z)Rot(8x\8y\8z')=-(8z+8z')8y+8y'01-(8x+8x')08x+8x'100018z+8z^-(8y+8y')由等效轉(zhuǎn)軸和等效轉(zhuǎn)角與Rot(x,8x)Rot(y,8y)Rot(z,8z)等效，有Rot(k,d。)=Rot(x,8x)Rot(y,8y)Rot(z,8z)kd。kd。z-kd。y0-kd。一z1kd。x0kd。y-kd。0]0_18z-8z18y-8x0-0x10—=-8y8x10010001所以有kxde=6x，kyde=6y，kzde=6z，將它們代入△得一0-88d一8z0-8ydxA=zxy-880dyxz0000可見，微分變換由兩個部分組成6微分轉(zhuǎn)動矢量，d微分平移矢量，合稱為微分運動矢量,可表示為D=(d,d,d,8,8,8)tTOC\o"1-5"\h\zxyzxyz10501，相對固定系的微分平移矢量d=[100.5]，~001例：已知一個坐標系A(chǔ)=100010000微分旋轉(zhuǎn)矢量6=[00.110501，相對固定系的微分平移矢量d=[100.5]，微分旋轉(zhuǎn)矢量6=[00.10]，求微分變換dA。-0-88d--000.11"800-8d0000A=zxy=-880d-0.1000.5yxz_0000_0000~000.11-"00110"-00.101-000010050000...dA=AA=——-0.1000.5010000-0.1-0.5_0000_0001_0000_下面討論兩坐標系之間的微分關(guān)系，設(shè)第一個坐標系為i系，第二個坐標系為j系不失一般性，假定j系就是固定的0系。因為A=00

8z

-8y0因為A=00

8z

-8y0-8z08x05y-8x00dx

dy

dz

0所以A0T=0TA0iiinoaxxxnoayyynoazzz00008zi-8y0，A=0T-1A00T，整理得到PxPyPz1-8z8ydxiii0-8xdyii8x0dzii000=y((8xp=y((8xp)+d)=o((8xP)+d)=咄8xp)+d)ix8廣oddddxnnn(P乂n)(pxn)(pxn)dxixyzxyz0dyooo(P乂o)(pxo)(pxo)dyixyzxyz0dzaaa(pxa)(pxa)(pxa)dzi=xyzxyz一08x000nn一n8xixyz08y000ooo8yixyz08zJ000aaa8zil_xyz0iyiz對于任何三維矢量p=[px,py,?』,其反對稱矩陣s(p)定義為：0-ppzyp0-pz0-ppyxs(p)=xn0axxxn0axxxn0ayyyn0azzz0r=idi5i」0Rti0-0Rts(0p)0Rti類似地，任意兩坐標系｛A｝和｛B｝之間廣義速度的坐標變換為：bRA0-bRS(AP)bRAbRA0-bRS(AP)bRAaVaVaR,=BA^L0-aRS(BP)BVaRBB300110100501000001BO例：已知一個坐標系A(chǔ)=，相對固定系的微分平移矢量d=[100.5]，微分旋轉(zhuǎn)矢量S=[00.10]，求A系中等價的微分平移矢量dA和微分旋轉(zhuǎn)矢量皓解：將d=[100.5]和6=[00.10]代入得到d=[。0A5?廣遺5L5廣氣5dx=〃』(5xp)+d)dy=0^((5xp)+d)d_=ag(5xp)+d)5.1L5=[0.10°LoA4、機械臂軌跡規(guī)劃機械臂的軌跡規(guī)劃可以在關(guān)節(jié)空間也可以在笛卡爾空間中進行或者說機械臂軌跡規(guī)劃是指在關(guān)節(jié)空間或者笛卡爾空間中研究機械臂軌跡生成方法。簡言之，機械臂軌跡規(guī)劃是運動學(xué)逆解的實際應(yīng)用，它描述了機械臂在多維空間中的運動路線。在知道末端位姿的前提下，通過運動學(xué)逆解得到各個關(guān)節(jié)在相應(yīng)時刻的轉(zhuǎn)動量或者平移量合理的規(guī)劃指的是規(guī)劃出的角位移曲線、角速度曲線以及角加速度曲線，可以有效地減少了機械臂在運動過程中的沖擊和振動，使機械臂的工作壽命得以延長。得到d=[。0A5?廣遺5L5廣氣5機械臂的關(guān)節(jié)角位移變化率比較小，能夠有效地防止了機械臂工作時的振動和沖擊。機械臂關(guān)節(jié)角速度和角加速度變化均平順連續(xù)，從而有效避免了機械部件的磨損，能夠保證整個機械臂系統(tǒng)的長期、穩(wěn)定的運行，滿足機械臂的工作要求。5、robotics工具箱中的相關(guān)函數(shù)link建立一個連桿對象,例如對于本次競賽的機械臂，根據(jù)連桿參數(shù)得到L{1}=link([pi/20012000]);L{2}=link([pi/200000]);L{3}=link([-pi/200140.80pi]);L{4}=link([-pi/271.8000pi/2]);L{5}=link([+pi/271.8000pi]);L{6}=link([-pi/20000pi/2]);L{7}=link([000129.600]);robot建立一個機械臂對象R=robot(L)noname(7axis,RRRRRRR)grav=[0.000.009.81]standardD&HparametersalphaAthetaDR/P1.57080120R(std)1.5708.00R(std)-1.57080140.8R(std)-1.570871.80R(std)1.570871.80R(std)-1.570800R(std)00129.6R(std)drivebot用滑塊控制的機械臂圖形drivebot(R,ones(1,7)*pi)plot機械臂的圖形顯示plot(R,[pi/2pi/200000])fkine串聯(lián)機械臂正向運動學(xué)計算tr=fkine(ROBOT,Q)ROBOT表示機械臂對象，Q機械臂關(guān)節(jié)坐標值。tr=fkine(R,[000pi/2000])tr=0.0000-0.00001.0000129.6000-0.00001.00000.0000-0.0000-1.0000-0.00000.0000-20.80000001.0000ikine串聯(lián)機械臂逆向運動學(xué)計算q二ikine(ROBOT,T)q=ikine(ROBOT,T,Q)q=ikine(ROBOT,T,Q,M)輸入變量ROBOT表示機械臂對象，T機械臂末端變換矩陣。輸出變量q機械臂關(guān)節(jié)的角度(單位是弧度)，一般來說逆運動學(xué)的解不是唯一的，取決于初始值Q，缺省時是0向量。如果機械臂的自由度(DOF)小于6，由于解空間的維數(shù)大于機械臂的自由度，這時需要第4個輸入量M來確定笛卡爾坐標(手腕對應(yīng)的坐標系)中的哪些量在求解中被忽略。M中有6個元素，分別表示沿著x,y,z方向的平移和相對于x軸，y軸，z軸的旋轉(zhuǎn)，值是0(忽略)或1。非零元素的個數(shù)應(yīng)該等于機械臂的自由度。例如，對典型的有5個自由度的機械臂，一般是忽略相對手腕坐標的轉(zhuǎn)動，這時M=[111110]。另外一種用法是qt=ikine(ROBOT,TG)qt=ikine(ROBOT,TG,Q)qt=ikine(ROBOT,TG,Q,M)輸入變量ROBOT表示機械臂對象，TG是4x4xN機械臂末端變換矩陣。輸出變量qt是一組(N個)TG對應(yīng)的關(guān)節(jié)坐標。一行對應(yīng)一個輸入變換，每一步的初始值取上一步的值。求解使用機械臂Jacobian矩陣的偽逆，這是數(shù)值求解方法，對于特定機械臂逆運動學(xué)解(如果可能)應(yīng)該盡量使用解析解。但是這種方法可以得到奇異點上的解，零空間中的關(guān)節(jié)角度可以任取。q=ikine(R,tr)q=0.00000.00000.00000.7854-0.0000-0.78540.0000注意：對于機械臂末端的一個位置與姿態(tài)，逆運動學(xué)計算不是唯一的，驗證tr=fkine(R,q)tr=0.0000-0.00001.0000129.6000-0.00001.00000.0000-0.0000-1.0000-0.00000.0000-20.80000001.0000transl計算平移變換tr=transl(X,Y,Z)返回機械臂末端坐標X,Y,Z對應(yīng)的齊次表換矩陣tr=transl(129.6,0,20.8)tr=1.000000129.600001.000000001.000020.80000001.0000[XYZ]'=transl(T)返回齊次表換表示中的平移值，作為一個3兀素的列向量xyz=transl(tr)xyz=129.6000020.8000ctraj計算工作空間中兩點T0,T1之間的軌跡tc=ctraj(T0,T1,N)tc=ctraj(T0,T1,R)返回從T0到T1笛卡爾坐標系的軌跡TCN表示軌跡中的點數(shù)。在第1中情況下，軌跡中的點在T0到T1中等距離分配。在第2中情況下，向量R給出軌跡中每個點的距離，R中的元素取值為[01]。一個軌跡是4x4xN矩陣，最后一個下標表示點索引。旋轉(zhuǎn)插值使用四元球形線性插值。tr0=fkine(R,[0000000])tr0=1.00000.0000-0.00001.0000-0.00000.0000-0.00000.0000-0.0000-0.00001.0000108.80000001.0000tr1=fkine(R,[pi/4pi/60pi/3000])tr1=0.6124-0.70710.353695.60080.61240.70710.353695.6008-0.5000-0.00000.8660110.30050001.0000tc(:,:,1)=1.000000-0.000001.000000.0000001.0000108.80000001.0000tc(:,:,2)=0.8976-0.38220.219847.80040.35710.92260.145847.8004-0.2585-0.05230.9646109.55030001.0000tc(:,:,3)=0.6124-0.70710.353695.60080.61240.70710.353695.6008-0.5000-0.00000.8660110.30050001.0000transl(tc)ans=-0.00000.0000108.800047.800447.8004109.550395.600895.6008110.3005jtraj計算關(guān)節(jié)中兩點Q0,Q1之間的軌跡[QQDQDD]=jtraj(Q0,Q1,N)[QQDQDD]=jtraj(Q0,Q1,N,QD0,QD1)[QQDQDD]=jtraj(Q0,Q1,T)[QQDQDD]=jtraj(Q0,Q1,T,QD0,QD1)軌跡中的點數(shù)是N,或者是一個時間向量T。插值使用7次多項式，邊界速度由QD0,QD1指定，缺省時邊界速度和加速度為0。q0=[pipipipipipipi];q1=[pipi/2000pi/20];tr0=fkine(R,[pipipipipipipi]);tr1=fkine(R,[pipi/2000pi/20]);[QT,QD,QDD]=jtraj(q0,q1,30);figuresubplot(2,2,1),plot(R,QT)subplot(2,2,2),plot(QT),gridon,legendCq1','q2','q3','q4','q5','q6','q7','Location','NorthWest')subplot(2,2,3),plot(QD),gridonsubplot(2,2,4),plot(QDD),gridon%注意：其中有一些曲線重合jacob0計算機械臂在基坐標系中Jacobian矩陣J二jacob0(ROBOT,Q)tr2jac計算機械臂在基坐標系中Jacobian矩陣J=TR2JAC(T)diff2tr微分表示轉(zhuǎn)換為齊次變換tr=diff2tr(D)返回表示微分平移與旋轉(zhuǎn)的齊次變換矩陣，矩陣中包含一個反對稱的旋轉(zhuǎn)子矩陣。tr2diff轉(zhuǎn)換為齊次變換轉(zhuǎn)換為微分表示D=tr2diff⑴D=tr2diff(T1,T2)第一種形式將齊次表換矩陣表示轉(zhuǎn)換為6-元素向量微分表示。第二種形式返回6-元素向量，表示從T1到T2的在基坐標系中需要的微分移動。J=jacob0(R,q1)%Jacobiananddifferentialmotiondemonstration%Adifferentialmotioncanberepresentedbya6-elementvectorwithelements%[dxdydzdrxdrydrz]%wherethefirst3elementsareadifferentialtranslation,andthelast3%areadifferentialrotation.Whendealingwithinfinitisimalrotations,%theorderbecomesunimportant.Thedifferentialmotioncouldbewritten%intermsofcompoundedtransforms%transl(dx,dy,dz)*trotx(drx)*troty(dry)*trotz(drz)%butamoredirectapproachistousethefunctiondiff2tr()D=[.1.20-.2.1.1]'diff2tr(D)T=fkine(R,q1)%thenthedifferentialmotioninthesecondframewouldbegivenbyDT=tr2jac(T)*D;DQ=pinv(J)*DT;vel=[100000]';%translationalmotionintheXdirectionqvel=pinv(J)*vel;ans=-0.00000.0000-0.00000.0039-0.0000-0.00390.0000%這是計算工作空間軌跡和求逆解的另外一種方法。但是，如果Jacobian矩陣奇異時%會失效。如果機械臂的自由度大于6，即是冗余的，采用Jacobian矩陣偽逆計算，或%對Jacobian矩陣進行奇異值分解。附錄rpy角與euler角（1）rpy角rpy角是描述船舶航行時的姿態(tài)的一種方法，將船的行駛方向作為Z軸測繞Z軸旋轉(zhuǎn)稱為滾動（Roll）角a，將繞Y軸（與海面平行）方向的旋轉(zhuǎn)稱為俯仰（Pitch）角p，取X軸與海面垂直方向，將繞X軸的旋轉(zhuǎn)稱為偏轉(zhuǎn)（Yaw）角丫。機械臂末端的定義類似，故習(xí)慣上稱為rpy角。描述運動坐標系的規(guī)則是：首先使運動坐標系的初始方位與固定坐標系重合，將運動坐標系繞固定坐標系X軸轉(zhuǎn)動y，再將運動坐標系繞固定坐標系Y軸轉(zhuǎn)動p，最后將運動坐標系