機械臂運動學基礎

機械臂運動學基礎1 、機械臂的運動學模型機械臂運動學研究的是機械臂運動，而不考慮產(chǎn)生運動的力。運動學研究機械臂的位置，速度和加速度。 機械臂的運動學的研究涉及到的幾何和基于時間的內(nèi)容，特別是各個關節(jié)彼此之間的關系以及隨時間變化規(guī)律。典型的機械臂由一些串行連接的關節(jié)和連桿組成。每個關節(jié)具有一個自由度，平移或旋轉。對于具有 n 個關節(jié)的機械臂，關節(jié)的編號從1 到 n ，有 n +1 個連桿，編號從 0到 n。連桿0 是機械臂的基礎， 一般是固定的， 連桿 n 上帶有末端執(zhí)行器。 關節(jié) i 連接連桿 i和連桿 i-1 。一個連桿可以被視為一個剛體，確定與它相鄰的兩個關節(jié)的坐標軸之間的相對位置。一個連桿可以用兩個參數(shù)描述， 連桿長度和連桿扭轉， 這兩個量定義了與它相關的兩個坐標軸在空間的相對位置。而第一連桿和最后一個連桿的參數(shù)沒有意義，一般選擇為0 。一個關節(jié)用兩個參數(shù)描述， 一是連桿的偏移， 是指從一個連桿到下一個連桿沿的關節(jié)軸線的距離。二是關節(jié)角度，指一個關節(jié)相對于下一個關節(jié)軸的旋轉角度。為了便于描述的每一個關節(jié)的位置，我們在每一個關節(jié)設置一個坐標系，對于一個關節(jié)鏈，Denavit 和 Hartenberg提出了一種用矩陣表示各個關節(jié)之間關系的系統(tǒng)方法。對于轉動關節(jié) i，規(guī)定它的轉動平行于坐標軸zi-1 ，坐標軸 xi-1 對準從 zi-1 到 zi 的法線方向，如果 zi-1 與z相交，則 xi-1取 zi-1z的方向。連桿，關節(jié)參數(shù)概括如下：ii連桿長度 a i沿著 xi 軸從 zi-1和 zi 軸之間的距離 ;連桿扭轉 i從 zi-1 軸到 zi 軸相對 xi-1 軸夾角 ;連桿偏移 d i從坐標系 i-1 的原點沿著 zi-1 軸到 xi 軸的距離 ;關節(jié)角度 ixi-1 軸和 xi 軸之間關于 zi-1 軸的夾角。1對于一個轉動關節(jié)i 是關節(jié)變量， d i 是常數(shù)。而移動關節(jié)d i 是可變的， i 是恒定的。為了統(tǒng)一，表示為qii轉動關節(jié)di移動關節(jié)運用Denavit-Hartenberg（ DH ）方法，可以將相鄰的兩個坐標系之間的變換關系表示為一個 4x4 的齊次變換矩陣cos isin i cosisini siniai cos ii 1 Asin icos i cos icos isin iai sin ii0sin icosdii0001上式表示出了坐標系i 相對于坐標系i-1 的關系。即0Ti0Ti 1 i 1 Ai其中 0Ti 表示坐標系 i 相對于世界坐標系 0的位置與姿態(tài)，簡稱位姿。2 、正向和反向運動學對于一個n- 軸剛性連接的機械臂，正向運動學的解給出的是最后一個連桿坐標系的位置和姿態(tài)。重復利用上式,得到0Tn0 A1 1 A2n 1 AnK (q)機械臂末端位姿在笛卡爾坐標系中有6 個自由度， 3 個平移， 3 個旋轉。所以，一般來說具有 6 個自由度的機械臂可以使末端實現(xiàn)任意的位姿?？偟臋C械臂變換0Tn 一般簡寫為Tn ，對 6 個自由度的機械臂簡寫為T6 。對于任意的機械臂，無論其它有多少個關節(jié)，具有什么結構，正向運動學解都是可以得到的。在機械臂的路徑規(guī)劃中，用到的是反向運動學的解qK 1( 0Tn ) ，它給出了特定的末端位姿對應的機械臂的關節(jié)角度。一般來說， 反向運動學的解不是唯一的，對具有某種結構的機械臂，封閉解可能不存在。2對于6 自由度的機器人而言，運動學逆解非常復雜，一般沒有封閉解。只有在某些特殊情況下才可能得到封閉解。不過，大多數(shù)工業(yè)機器人都滿足封閉解的兩個充分條件之一（ Pieper準則）（ 1 ）三個相鄰關節(jié)軸交于一點（ 2 ）三個相鄰關節(jié)軸相互平行如果機械臂多于6 個關節(jié)，稱關節(jié)為冗余的，這時解是欠定的。如果對于機械臂某個特別的位姿， 解不存在， 稱這個位姿為奇異位姿。機械臂的奇異性可能是由于機械臂中某些坐標軸的重合，或位置不能達到引起的。機械臂的奇異位姿分為兩類：(1) 邊界奇異位姿，當機械臂的關節(jié)全部展開或折起時，使得末端處于操作空間的邊界或邊界附近， 雅克比矩陣奇異，機械臂的運動受到物理結構的約束，這時機械臂的奇異位姿稱為邊界奇異位姿。(2) 內(nèi)部奇異位姿，兩個或兩個以上的關節(jié)軸線重合時，機械臂各個關節(jié)的運動相互抵消，不產(chǎn)生操作運動，這時機械臂的奇異位姿稱為內(nèi)部奇異位姿。機械臂運動學逆解的方法可以分為兩類： 封閉解和數(shù)值解、 在進行逆解時總是力求得到封閉解。因為封閉解的計算速度快，效率高，便于實時控制。而數(shù)值解法不具有這些特點。機械臂運動學的封閉逆解可通過兩種途徑得到：代數(shù)法和幾何法。一般而言，非零連桿參數(shù)越多，到達某一目標的方式也越多，即運動學逆解的數(shù)目也越多。在從多重解中選擇解時， 應根據(jù)具體情況， 在避免碰撞的前提下通常按“ 最短行程 ”準則來選擇。同時還應當兼顧“ 多移動小關節(jié)，少移動大關節(jié) ”的原則。n 個自由度的機械臂的末端位姿由n 個關節(jié)變量所決定，這n 個關節(jié)變量統(tǒng)稱為n 維關節(jié)3矢量， 記為 q 。所有的關節(jié)矢量構成的空間稱為關節(jié)空間 。機械臂末端的位姿用6 個變量描述， 3 個平移 (x,y,z) 和 3 個旋轉 (x,y,z) ，記 x= (x,y,z,x,y,z)， x 是機械臂末端在基坐標空間中的坐標，所有的矢量x 構成的空間稱為操作空間或作業(yè)定向空間。工作空間是操作臂的末端能夠到達的空間范圍，即末端能夠到達的目標點集合。值得指出的是， 工作空間應該嚴格地區(qū)分為兩類：(1) 靈活（工作）空間指機械臂末端能夠以任意方位到達的目標點集合。因此，在靈活空間的每個點上，手爪的指向可任意規(guī)定。(2) 可達（工作）空間指機械臂末端至少在一個方位上能夠到達的目標點集合。機械臂各關節(jié)驅動器的位置組成的矢量稱為驅動矢量s，由這些矢量構成的空間稱為驅動空間。正向運動學驅動空間關節(jié)空間工作空間運動學逆解3 、 Jacobian矩陣機械臂的 Jacobian矩陣表示機械臂的操作空間與關節(jié)空間之間速度的線性映射關系，對于一個 n 軸的機械臂，機械臂末端在基坐標系中的速度是xJq 其中 x 是 6 個元素的向量。對于 6 個關節(jié)機械臂Jacobian矩陣是方陣，如果它是可逆的，則可以由機械臂的末端速度求出各個關節(jié)的速度。Jacobian矩陣在機械臂的奇異位姿上是不可逆的。在實際應用中，當機械臂的末端位置接近奇異位置時，Jacobian矩陣是病態(tài)的，可能導致關節(jié)速度不能正確地得到。上式解決的是正速度問題，即已知 q 和 q 求末端執(zhí)行器的速度x 。對于逆速度解問題，由上4式可以得到速度逆解公式為qJ 1x ，注意到此時需要求雅可比矩陣的逆，由線性方程組理論知上式對任意的x ， q 都有解的必要條件是雅可比矩陣的秩rank(J)=6，這意味著機械臂的自由度數(shù)n 6 。這也說明了具有冗余自由度的機械臂，在末端位姿固定的條件下，能使關節(jié)在一個較大的關節(jié)空間的子空間中運動，有效地避開障礙或奇異位姿，并把關節(jié)位移限制在允許范圍內(nèi)，從而具有更大的運動靈活性。雅可比矩陣可以看成是從關節(jié)空間到操作空間運動速度的傳動比，同時也可用來表示兩空間之間力的傳遞關系。對于冗余自由度機械臂，其雅可比矩陣是長方矩陣，因 J 滿秩且方程個數(shù)少于未知數(shù)個數(shù)，所以有無窮多個解，這時，一般是求其中的最小范數(shù)解，或采用加權最小范數(shù)解也就是說使qT Dq 最小的解， 其中 D 是對稱正定加權矩陣。此時的解是使機械臂在能量消耗最小的情況下的解。這時，逆速度問題便轉為：求q 滿足 qJ 1x 且使 L1 qT Dq 最小。實際上等同于求性能2指標L 在約束條件qJ 1 x下的極值。應用Lagrange乘子法，以上極值為題的解是qD 1J T (JD 1 J T ) 1 x ，當 D= I 時，雅可比矩陣是JJ T ( JJ T ) 1 ，稱為雅可比矩陣的偽逆。下面通過一個兩自由度的平面機械臂說明雅可比矩陣的特性，根據(jù)右圖中的幾何關系容易求得xl1 c1l2 c12c1cos( 1), c12cos(yl1 s1l 2 s12s1sin( 1 ), s12sin(1 2 )12 )兩邊微分后寫成矩陣形式xxdx12ddyyyd1dxl1 s1 l2 s12l2 s12d即l1 c1 l 2 c12l2 c12d2dy12125簡寫成 dx=Jd ， 式中 J 就稱為機械臂的雅可比（ Jacobian ）矩陣，它由函數(shù)x，y 的偏微分組成， 反映了關節(jié)微小位移d 與機械臂末端微小運動 dx 之間的關系。 將兩邊同除以dtdt 得到：dx/dt=Jd/dt,因此機械臂的雅可比矩陣也可以看做是操作空間中的速度與關節(jié)空間中速度的線性變換。dx/dt 稱為末端在操作空間中的廣義速度，簡稱操作速度，d /dt為關節(jié)速度。 可以看出， 雅可比矩陣的每一列表示其它關節(jié)不動而某一關節(jié)以單位速度運動產(chǎn)生的末端速度。由 Jl1 s1 l2 s12l2 s12 可以看出， J 陣的值隨末端位置的不同而不同，即1 和2 的l1 c1 l 2 c12l2 c12改變會導致J 的變化。 對于關節(jié)空間的某些位姿，機械臂的雅可比矩陣的秩減少，這些位姿稱為機械臂的奇異位姿。 上例機械臂雅可比矩陣的行列式為：det(J )2 =0 l1l2 sin( 2 ) ，當 或 =180 時，機械臂的雅可比行列式為0 ，矩陣的秩為1 ，這時機械臂處于奇異位姿。機2械臂在操作空間的自由度將減少。如果機械臂的雅可比 J是滿秩的方陣，相應的關節(jié)速度即可求出，即J 1 x ，上例平面2R 機械臂的逆雅可比矩陣J 11l2 c12l2 s12l1c1 l 2c12，顯然，當 2 趨于 0 （或l1l 2 s2l1s1 l 2s12180 ）時，機械臂接近奇異位姿，相應的關節(jié)速度將趨于無窮大。為了補償機器人末端執(zhí)行器位姿與目標物體之間的誤差， 以及解決兩個不同坐標系之間的微位移關系問題，需要討論機器人連桿在作微小運動時的位姿變化。假設一變換的元素是某個變量的函數(shù)， 對該變換的微分就是該變換矩陣各元素對該變量的偏導數(shù)所組成的變換矩陣乘以該變量的微分。例如給定變換T 為：t11t12t13t14t21t 22t23t24Tt32t33t34t31t41t 42t43t44若它的元素是變量x 的函數(shù)，則變換T 的微分為 :6t11t12t13t14xxxxt21t22t23t 24dTxxxxdxt31t32t33t34xxxxt41t42t43t 44xxxx下面討論機械臂的微分運動，設機械臂某一連桿相對于基坐標系的位姿為T ，經(jīng)過微運動后該連桿相對基坐標系的位姿變?yōu)門+dT ，若這個微運動是相對于基坐標系（靜系） 進行的 (左乘) ，總可以用微小的平移和旋轉來表示，即TdTTrans(dx , dy , dz ) Rot(k ,d)T所以有dTTrans(d x, d y , dz ) Rot(k , d )I 44T根據(jù)齊次變換的對稱性， 若微運動是相對某個連桿坐標系i（動系） 進行的 ( 右乘 )，則 T+dT可以表示為TdTT Trans( dx , d y ,d z) Rot(k , d)所以有dTT Trans( dx ,d y , dz) Rot(k , d)I 44令Trans(dx, d y , d z)Rot( k , d) I 44 為微分算子，則相對基系有dT= 0T ，相對 i 系有 dT=T i 。 這里 的下標不同是由于微運動相對不同坐標系進行的。在機械臂運動學中微分變換分為微分平移和微分旋轉兩類。微分平移變換與一般平移變換一樣，其變換矩陣為:100dx010dyTrans (dx, dy, dz)001dz00017由于微分旋轉時 0，所以 sin d， cos 1將它們代入旋轉變換通式中得微分旋轉表達式 :1kzdky d0Rot(k , d )kzd1kxd0kydkxd100001于是得到微分算子Trans(dx ,d y , dz)Rot( k , d ) I 44 ，即0kzdkyddxkzd0kxddykydkx d0dz0000微分旋轉與有限旋轉相比，有一些特殊的性質，下面分別說明。（1 ）微分旋轉的無序性，當 0 時，有 sin d， cos 1 若令 x=d x，y=d y ，z=d z，則繞三個坐標軸的微分旋轉矩陣分別為100010y01z 0001x 00100z100Rot(x, x)x10Rot( y, y)y 010Rot(z, z)01000000100010001略去 2 次項，得到10y010y0Rot(x,x y1x001x0x)Rot( y, y)x10yx10y000100011x yy010y0Rot ( y,01x001x0y) Rot(x, x)x10yx10y00010001兩者結果相同， 可見這里左乘與右乘等效。結論：微分旋轉其結果與轉動次序無關，這是與有限轉動（一般旋轉）的一個重要區(qū)別。（2 ）微分旋轉的可加性，考慮兩個微分旋轉復合后的效果81zy0z1x0Rot( x, x) Rot( y, y)Rot( z, z)x10y0001若 Rot （ x，y ，z） 和 Rot （x， y ， z）表示兩個不同的微分旋轉，則兩次連續(xù)轉動的結果為：1( zz)yy 0zz1( xx )0Rot( x, y, z) Rot( x , y , z )y )xx 10( y0001上式表明：任意兩個微分旋轉的結果為繞每個軸轉動的元素的代數(shù)和，即微分旋轉是可加的。由等效轉軸和等效轉角與Rot( x,x) Rot( y,y)Rot( z,z) 等效，有Rot(k , d )Rot( x,x) Rot( y,y)Rot( z, z)1kzdky d01zy0kz d1kxd0z1x 0k ydkxd10yx1000010001所以有 kxd = x， kyd = y ， kzd = z， 將它們代入得0zydxz0xd yyx0d z0000可見，微分變換由兩個部分組成微分轉動矢量 ，d 微分平移矢量 ,合稱為微分運動矢量，可表示為 D (d x, dy , dz , x,y , z )T001101005例：已知一個坐標系A，相對固定系的微分平移矢量d= 100.5 ，01000001微分旋轉矢量=00.10 ，求微分變換dA 。90zydx000.11z0xdy0000yx0dz0.1000.500000000000.110011000.101dAA0000100500000.1000.50100000.10.5000000010000下面討論兩坐標系之間的微分關系，設第一個坐標系為i系，第二個坐標系為j 系不失一般性，假定 j 系就是固定的 0系。nxoxaxpx0nyoyaypyiTnzozazpz00010zy dx0ziyidxi因為z0x dy， izi0xidyi0yx0dzyixi0dzi00000000所以00iT0iT i ，i0iT100iT ，整理得到dixn (p)d )diyo (p)d )diza (p)d )ixiyiznoadxinxnynz( p n)x( p n) y( pn) zdx0dyioxoyoz( p o)x( p o ) y( po) zdy0dziaxayaz( p a)x( p a) y( pa) zdz0xi000nxnynzx0yi000oxoyozy0z000axayazzi0對于任何三維矢量p = p x, p y, p z,其反對稱矩陣s(p)定義為：0pzpys( p)pz0pxpypx010記nxoxax0i Rnyoyaynzozaz上式簡寫成di0RT0RT s( 0p)iii 0i00i RT類似地，任意兩坐標系A 和 B之間廣義速度的坐標變換為：BVAB RAB RS( A PBO )AV，AVBARBA RS( B PAO )BVB0ABRAA0BA RB00110100500.5 ，例：已知一個坐標系A10，相對固定系的微分平移矢量d= 1000001微分旋轉矢量 =00.1 0 ，求 A 系中等價的微分平移矢量d A 和微分旋轉矢量A 。解：將 d= 10 0.5 和 =00.1 0 代入dixn (p)d )ixdiyo (p)d )iydiza (p)d )iz得到 dA 0 0 5.1TTA0.1 0 0。noa4 、機械臂軌跡規(guī)劃機械臂的軌跡規(guī)劃可以在關節(jié)空間也可以在笛卡爾空間中進行，或者說機械臂軌跡規(guī)劃是指在關節(jié)空間或者笛卡爾空間中研究機械臂軌跡生成方法。簡言之， 機械臂軌跡規(guī)劃是運動學逆解的實際應用，它描述了機械臂在多維空間中的運動路線。 在知道末端位姿的前提下，通過運動學逆解得到各個關節(jié)在相應時刻的轉動量或者平移量，合理的規(guī)劃指的是規(guī)劃出的角位移曲線、 角速度曲線以及角加速度曲線，可以有效地減少了機械臂在運動過程中的沖擊和振動，使機械臂的工作壽命得以延長。11械臂可以分為點到點作業(yè)(Point-to-Point Motion )和連續(xù)路徑作業(yè)(Continuous-PathMotion )。點到點的運動指的是機械臂在運動過程中，只要求在某些點上有準確的位置和姿態(tài)，相鄰的點不做要求。連續(xù)運動要求機械臂嚴格的沿特定的曲線運動。機械臂的關節(jié)角位移變化率比較小，能夠有效地防止了機械臂工作時的振動和沖擊。機械臂關節(jié)角速度和角加速度變化均平順連續(xù)，從而有效避免了機械部件的磨損，能夠保證整個機械臂系統(tǒng)的長期、穩(wěn)定的運行，滿足機械臂的工作要求。5 、 robotics工具箱中的相關函數(shù)link建立一個連桿對象,例如對于本次競賽的機械臂，根據(jù)連桿參數(shù)得到L1=link(pi/20012000);L2=link(pi/200000);L3=link(-pi/200140.80pi);L4=link(-pi/271.8000pi/2 );L5=link(+pi/271.8000pi);L6=link(-pi/20000pi/2);L7=link(000129.600);robot建立一個機械臂對象R= robot(L)noname (7 axis, RRRRRRR)grav = 0.00 0.00 9.81standard D&H parametersalphaAthetaDR/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/2 pi/2 0 0 0 0 0)12fkine串聯(lián)機械臂正向運動學計算tr =fkine (ROBOT, Q)ROBOT 表示機械臂對象，Q 機械臂關節(jié)坐標值。tr =fkine (R, 0 0 0 pi/2 0 0 0)tr =0.0000-0.00001.0000129.6000-0.00001.00000.0000-0.0000-1.0000-0.00000.0000-20.80000001.0000ikine串聯(lián)機械臂逆向運動學計算q = ikine(ROBOT, T)q = ikine(ROBOT, T, Q)q = ikine(ROBOT, T, Q, M)輸入變量ROBOT 表示機械臂對象，T 機械臂末端變換矩陣。輸出變量q 機械臂關節(jié)的角度(單位是弧度 )，一般來說逆運動學的解不是唯一的，取決于初始值 Q ，缺省時是0 向量。如果機械臂的自由度(DOF) 小于 6 ，由于解空間的維數(shù)大于機械臂的自由度，這時需要第4 個輸入量M 來確定笛卡爾坐標(手腕對應的坐標系)中的哪些量在求解中被忽略。M 中有 6 個元素，分別表示沿著x,y,z 方向的平移和相對于x 軸， y 軸，z 軸的旋轉，值是0( 忽略 )或 1 。非零元素的個數(shù)應該等于機械臂的自由度。例如，對典型的有 5 個自由度的機械臂，一般是忽略相對手腕坐標的轉動，這時M = 1 1 1 1 1 0 。另外一種用法是qt = ikine(ROBOT, TG)qt = ikine (ROBOT, TG, Q)qt = ikine (ROBOT, TG, Q, M)13輸入變量ROBOT 表示機械臂對象，TG 是 4x4xN機械臂末端變換矩陣。輸出變量qt 是一組 (N 個 )TG 對應的關節(jié)坐標。一行對應一個輸入變換，每一步的初始值取上一步的值。求解使用機械臂Jacobian矩陣的偽逆，這是數(shù)值求解方法，對于特定機械臂逆運動學解( 如果可能 )應該盡量使用解析解。但是這種方法可以得到奇異點上的解，零空間中的關節(jié)角度可以任取。q=ikine(R,tr)q =0.00000.00000.00000.7854-0.0000-0.78540.0000注意：對于機械臂末端的一個位置與姿態(tài)，逆運動學計算不是唯一的，驗證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 對應的齊次表換矩陣tr=transl(129.6,0,20.8)tr =1.000000129.600001.000000001.000020.80000001.0000X Y Z = transl(T)返回齊次表換表示中的平移值，作為一個3 元素的列向量xyz=transl(tr)14xyz =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中的元素取值為 0 1 。一個軌跡是 4x4xN矩陣，最后一個下標表示點索引。旋轉插值使用四元球形線性插值。tr0=fkine(R,0 0 0 0 0 0 0)tr0 =1.0000-0.0000-0.0000-0.00000.00001.00000.00000.0000-0.0000-0.00001.0000108.80000001.0000tr1=fkine(R,pi/4 pi/6 0 pi/3 0 0 0)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.600815-0.5000-0.00000.8660110.30050001.0000transl(tc)ans =-0.00000.0000108.800047.800447.8004109.550395.600895.6008110.3005jtraj計算關節(jié)中兩點Q0,Q1之間的軌跡Q QD QDD = jtraj(Q0, Q1, N)Q QD QDD = jtraj (Q0, Q1, N, QD0, QD1)Q QD QDD = jtraj (Q0, Q1, T)Q QD QDD = jtraj (Q0, Q1, T, QD0, QD1)軌跡中的點數(shù)是N ，或者是一個時間向量T。插值使用7 次多項式， 邊界速度由QD0, QD1指定，缺省時邊界速度和加速度為0。q0=pi pi pi pi pi pi pi;q1=pi pi/2 0 0 0 pi/2 0;tr0=fkine(R,pi pi pi pi pi pi pi);tr1=fkine(R,pi pi/2 0 0 0 pi/2 0);QT,QD,QDD=jtraj(q0,q1,30);figuresubplot(2,2,1),plot(R,QT)subplot(2,2,2),plot(QT),grid on,legend(q1,q2,q3,q4,q5,q6,q7,Location, NorthWest)subplot(2,2,3),plot(QD),grid onsubplot(2,2,4),plot(QDD),grid on% 注意：其中有一些曲線重合jacob0計算機械臂在基坐標系中Jacobian矩陣J = jacob0(ROBOT, Q)tr2jac計算機械臂在基坐標系中Jacobian矩陣J = TR2JAC(T)16diff2tr微分表示轉換為齊次變換tr = diff2tr(D)返回表示微分平移與旋轉的齊次變換矩陣,矩陣中包含一個反對稱的旋轉子矩陣。tr2diff轉換為齊次變換轉換為微分表示D =tr2diff(T)D = tr2diff(T1, T2)第一種形式將齊次表換矩陣表示轉換為6- 元素向量微分表示。第二種形式返回6- 元素向量，表示從T1 到 T2 的在基坐標系中需要的微分移動。J = jacob0(R, q1)% Jacobian and differential motion demonstration% A differential motion can be represented by a 6-element vector with elements% dx dy dz drx dry drz% where the first 3 elements are a differential translation, and the last 3% are a differential rotation.When dealing with infinitisimal rotations,% the order becomes unimportant.The differential motion could be written% in terms of compounded transforms% transl(dx,dy,dz) * trotx(drx) * troty(dry) * trotz(drz)% but a more direct approach is to use the function diff2tr() D = .1 .2 0 -.2 .1 .1diff2tr(D)T=fkine(R,q1)% then the differential motion in the second frame would be given by DT = tr2jac(T) * D;DQ= pinv(J) * DT;vel = 1 0 0 0 0 0; % translational motion in the X direction qvel = pinv(J) * vel;ans =-0.00000.0000-0.00000.0039-0.0000-0.00390.0000% 這是計算工作空間軌跡和求逆解的另外一種方法。但是，如果Jacobian 矩陣奇異時% 會失效。如果機械臂的自由度大于6，即是冗余的，采用 Jacobian 矩陣偽逆計算，或% 對 Jacobian 矩陣進行奇異值分解。17附錄rpy 角與 euler角（ 1 ）rpy 角rpy 角是描述船舶航行時的姿態(tài)的一種方法，滾動（ Roll ）角 ，將繞 Y 軸 (與海面平行面垂直方向，將繞X 軸的旋轉稱為偏轉（將船的行駛方向作為Z 軸，則繞 Z 軸旋轉稱為)方向的旋轉稱為俯仰（Pitch ）角 ，取 X 軸與海Yaw ）角 。機械臂末端的定義類似，故習慣上稱為 rpy 角。描述運動坐標系的規(guī)則是：首先使運動坐標系的初始方位與固定坐標系重合，將運動坐標系繞固定坐標系X 軸轉動 ，再將運動坐標系繞固定坐標系Y 軸轉動 ，最后將運動坐標系繞固定坐標系Z 軸轉動 。因為三次轉動都是相對固定坐標系的，所以相應的旋轉矩陣rpy ( , , ) rot ( z,) rot ( y,)rot ( x,)cs00c0s01000sc0001000cs00010s0c00sc0000100010001將三個矩陣相乘得到c cc ssscc s c0s cs ssccs s c0rpy ( , , )c sc c0s0001它表示繞固定坐標系的三個軸依次旋轉得到的旋轉矩陣，稱為繞固定軸XYZ 旋轉的 rpy 方法。下面討論逆問題：從給定的旋轉矩陣得到繞固定軸XYZ 旋轉的 rpy 角。令nxoxax0rpy( , ,nyoyay0)ozaz0nz0001上式中有 3個未知數(shù)， 9 個方程，其中6 個不獨立，因此可利用其中3 個解出未知數(shù)。cos( )nx2ny2 ，如果 cos( )不為零，則可以得到18a tan 2( n,n2n2),a tan 2(ox, nx),a tan 2(ay, a )zxyz（ 2 ）繞運動系 ZYX 轉動的 euler 角描述運動坐標系的規(guī)則是：運動坐標系的初始方位與參考坐標系重合，首先將運動坐標系繞Z 軸轉動 ，再將運動坐標系繞Y 軸轉動 ，最后將運動坐標系繞X 軸轉動 。這種描述方法中各次的轉動都是相對運動系的，而不是相對固定坐標系的。相應的旋轉矩陣為euler( , , ) rot ( x,)rot ( y,)rot (z,)cs00c0s01000sc0001000cs0001 0s0c00sc0000100010001將三個矩陣相乘得到c cc s ss cc s c0s cs s sc cs s c0euler ( , , )csc c0s0001結果與繞固定軸XYZ 旋轉相同， 這是因為繞固定軸旋轉的順序與繞運動軸旋轉的順序相反，且旋轉角度對應相等。因此，用ZYX euler角與 XYZ rpy角的描述方法是等價的。另外一種常用的euler角方法是ZYZ 方法，首先使運動坐標系與參考坐標系重合，將運動坐標系繞Z 軸轉動 ，再將運動坐標系繞Y 軸轉動 ，最后將運動坐標系繞Z 軸轉動 。euler( , ) rot (