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

《機(jī)械臂運(yùn)動(dòng)學(xué)》由會(huì)員分享，可在線閱讀，更多相關(guān)《機(jī)械臂運(yùn)動(dòng)學(xué)（27頁(yè)珍藏版）》請(qǐng)?jiān)谘b配圖網(wǎng)上搜索。

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è)連 桿

2、可以用兩個(gè)參數(shù)描述, 連桿長(zhǎng)度和連桿扭轉(zhuǎn), 這兩個(gè)量定義了與它相關(guān)的兩個(gè)坐標(biāo)軸在空 間的相對(duì)位置。而第一連桿和最后一個(gè)連桿的參數(shù)沒(méi)有意義,一般選擇為 0。一個(gè)關(guān)節(jié)用兩 個(gè)參數(shù)描述, 一是連桿的偏移, 是指從一個(gè)連桿到下一個(gè)連桿沿的關(guān)節(jié)軸線的距離。 二是關(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-i，坐標(biāo)軸Xi-i對(duì)準(zhǔn)從Zi-i到乙的法線方向，如果 Zi-i

3、與 Zi相交，則Xi-1取Zi-1 Xzi的方向。連桿，關(guān)節(jié)參數(shù)概括如下： 連桿長(zhǎng)度 ai 沿著 xi 軸從 Zi-i 和 Zi 軸之間的距離 ; 連桿扭轉(zhuǎn)ai從Zi-1軸到Zi軸相對(duì)Xi-1軸夾角； 連桿偏移d i從坐標(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é) Qi是關(guān)節(jié)變量，di是常數(shù)。而移動(dòng)關(guān)節(jié) di是可變的，也是恒定的。為了 統(tǒng)一，表示為 ■ i轉(zhuǎn)動(dòng)關(guān)節(jié) q = 移動(dòng)關(guān)節(jié) 運(yùn)用Denavit-Hartenberg （ DH ）方法，可以將相鄰的兩個(gè)坐標(biāo)系之間的變換關(guān)系表示為 一個(gè)4x

4、4的齊次變換矩陣 - Cosq -sind cos% si nd si n 閃 a cosq' i . sin耳 cosQ COS^i 一cosd sins ai sin 4 A = 0 si n^i cos% di 0 0 0 1 一 上式表示出了坐標(biāo)系i相對(duì)于坐標(biāo)系i-1的關(guān)系。 即 0 0 i A八 T = li A A 0 其中T表示坐標(biāo)系i相對(duì)于世界坐標(biāo)系 0的位置與姿態(tài)，簡(jiǎn)稱位姿。 2、正向和反向運(yùn)動(dòng)學(xué) 對(duì)于一個(gè)n-軸剛性連接的機(jī)械臂，正向運(yùn)動(dòng)學(xué)的解給出的是最后

5、一個(gè)連桿坐標(biāo)系的位置和 姿態(tài)。重復(fù)利用上式，得到 七=0幾人川心代=K（q） 機(jī)械臂末端位姿在笛卡爾坐標(biāo)系中有 6個(gè)自由度，3個(gè)平移，3個(gè)旋轉(zhuǎn)。所以，一般來(lái)說(shuō)具 有6個(gè)自由度的機(jī)械臂可以使末端實(shí)現(xiàn)任意的位姿。 總的機(jī)械臂變換 In —般簡(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ī)劃中，用到的是反向運(yùn)動(dòng)學(xué)的解 q = K’CTn），它給出了特定的末端位 姿對(duì)應(yīng)的機(jī)械臂的關(guān)節(jié)角度。 一般來(lái)說(shuō)，反向運(yùn)動(dòng)學(xué)的解不是唯一的， 對(duì)具有某種結(jié)構(gòu)的機(jī) 械臂，封閉解可能不存在。 對(duì)于 6 自由度

6、的機(jī)器人而言，運(yùn)動(dòng)學(xué)逆解非常復(fù)雜，一般沒(méi)有封閉解。只有在某些特殊情 況下才可能得到封閉解。 不過(guò)，大多數(shù)工業(yè)機(jī)器人都滿足封閉解的兩個(gè)充分條件之一 （ Pieper 準(zhǔn)則） （1）三個(gè)相鄰關(guān)節(jié)軸交于一點(diǎn) （2）三個(gè)相鄰關(guān)節(jié)軸相互平行 如果機(jī)械臂多于 6 個(gè)關(guān)節(jié)，稱關(guān)節(jié)為冗余的，這時(shí)解是欠定的。如果對(duì)于機(jī)械臂某個(gè)特別 的位姿， 解不存在， 稱這個(gè)位姿為奇異位姿。 機(jī)械臂的奇異性可能是由于機(jī)械臂中某些坐標(biāo) 軸的重合，或位置不能達(dá)到引起的。 機(jī)械臂的奇異位姿分為兩類： （1） 邊界奇異位姿，當(dāng)機(jī)械臂的關(guān)節(jié)全部展開(kāi)或折起時(shí)，使得末端處于操作空間的邊界或邊 界附近， 雅克比矩陣奇異， 機(jī)械臂的

7、運(yùn)動(dòng)受到物理結(jié)構(gòu)的約束， 這時(shí)機(jī)械臂的奇異位姿稱為 邊界奇異位姿。 （2） 內(nèi)部奇異位姿，兩個(gè)或兩個(gè)以上的關(guān)節(jié)軸線重合時(shí)，機(jī)械臂各個(gè)關(guān)節(jié)的運(yùn)動(dòng)相互抵消， 不產(chǎn)生操作運(yùn)動(dòng)，這時(shí)機(jī)械臂的奇異位姿稱為內(nèi)部奇異位姿。 機(jī)械臂運(yùn)動(dòng)學(xué)逆解的方法可以分為兩類： 封閉解和數(shù)值解、 在進(jìn)行逆解時(shí)總是力求得到封閉 解。因?yàn)榉忾]解的計(jì)算速度快，效率高，便于實(shí)時(shí)控制。而數(shù)值解法不具有這些特點(diǎn)。機(jī)械 臂運(yùn)動(dòng)學(xué)的封閉逆解可通過(guò)兩種途徑得到：代數(shù)法和幾何法。 一般而言，非零連桿參數(shù)越多，到達(dá)某一目標(biāo)的方式也越多，即運(yùn)動(dòng)學(xué)逆解的數(shù)目也越多。 在從多重解中選擇解時(shí)， 應(yīng)根據(jù)具體情況， 在避免碰撞的前提下通常按“ 最短行程

8、 ”準(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)稱為 n 維關(guān)節(jié) 矢量，記為q。所有的關(guān)節(jié)矢量構(gòu)成的空間稱為 關(guān)節(jié)空間。機(jī)械臂末端的位姿用 6個(gè)變量描 述，3個(gè)平移（x,y,z）和3個(gè)旋轉(zhuǎn)（「X, - .y, - .z），記x= （x,y,z, -，x, - -y, - .z），x是機(jī)械臂末端在基 坐標(biāo)空間中的坐標(biāo)，所有的矢量 x構(gòu)成的空間稱為 操作空間或作業(yè)定向空間 。工作空間是 操作臂的末端能夠到達(dá)的空間范圍， 即末端能夠到達(dá)的目標(biāo)點(diǎn)集合。 值得指出的是，工作空 間應(yīng)該嚴(yán)格地區(qū)分

9、為兩類： （1） 靈活（工作）空間 指機(jī)械臂末端能夠以任意方位到達(dá)的目標(biāo)點(diǎn)集合。因此，在靈活空 間的每個(gè)點(diǎn)上，手爪的指向可任意規(guī)定。 （2） 可達(dá)（工作）空間 指機(jī)械臂末端至少在一個(gè)方位上能夠到達(dá)的目標(biāo)點(diǎn)集合。 機(jī)械臂各關(guān)節(jié)驅(qū)動(dòng)器的位置組成的矢量稱為驅(qū)動(dòng)矢量 s，由這些矢量構(gòu)成的空間稱為驅(qū)動(dòng)空 間。 正向運(yùn)動(dòng)學(xué) 3、Jacobian 矩陣 機(jī)械臂的Jacobian矩陣表示機(jī)械臂的操作空間與關(guān)節(jié)空間之間速度的線性映射關(guān)系，對(duì)于 一個(gè)n軸的機(jī)械臂，機(jī)械臂末端在基坐標(biāo)系中的速度是 x = jq其中x是6個(gè)元素的向量。 對(duì)于6個(gè)關(guān)節(jié)機(jī)械臂Jacobian矩陣是方陣，如果它是可

10、逆的，則可以由機(jī)械臂的末端速度 求出各個(gè)關(guān)節(jié)的速度。 Jacobian矩陣在機(jī)械臂的奇異位姿上是不可逆的。在實(shí)際應(yīng)用中， 當(dāng)機(jī)械臂的末端位置接近奇異位置時(shí)， Jacobia n矩陣是病態(tài)的，可能導(dǎo)致關(guān)節(jié)速度不能正 確地得到。 上式解決的是正速度問(wèn)題， 即已知q和q求末端執(zhí)行器的速度x。對(duì)于逆速度解問(wèn)題，由上 式可以得到速度逆解公式為 JJx，注意到此時(shí)需要求雅可比矩陣的逆，由線性方程組 理論知上式對(duì)任意的 X， q都有解的必要條件是雅可比矩陣的秩 rank(J)=6，這意味著機(jī)械 臂的自由度數(shù)n >6。 這也說(shuō)明了具有冗余自由度的機(jī)械臂， 在末端位姿固定的條件下， 能使關(guān)

11、節(jié)在一個(gè)較大的關(guān) 節(jié)空間的子空間中運(yùn)動(dòng)， 有效地避開(kāi)障礙或奇異位姿， 并把關(guān)節(jié)位移限制在允許范圍內(nèi)， 從 而具有更大的運(yùn)動(dòng)靈活性。 雅可比矩陣可以看成是從關(guān)節(jié)空間到操作空間運(yùn)動(dòng)速度的傳動(dòng)比， 同時(shí)也可用來(lái)表示兩空間 之間力的傳遞關(guān)系。 對(duì)于冗余自由度機(jī)械臂， 其雅可比矩陣是長(zhǎng)方矩陣， 因J滿秩且方程個(gè) 數(shù)少于未知數(shù)個(gè)數(shù)， 所以有無(wú)窮多個(gè)解， 這時(shí)，一般是求其中的最小范數(shù)解，或采用加權(quán)最 小范數(shù)解也就是說(shuō)使| qT Dq|最小的解，其中d是對(duì)稱正定加權(quán)矩陣。此時(shí)的解是使機(jī)械臂 在能量消耗最小的情況下的解。 這時(shí)，逆速度問(wèn)題便轉(zhuǎn)為：求 q滿足q = j」x且使l qTDq最小。實(shí)際上等

12、同于求性能 指標(biāo)L在約束條件 q °X下的極值。應(yīng)用 Lagrange 乘子法，以上極值為題的解是 q二D‘JT(JD」JT)」X，當(dāng)D=I時(shí)，雅可比矩陣是J ? = JT(JJT)_1，稱為雅可比矩陣的 F面通過(guò)一個(gè)兩自由度的平面機(jī)械臂說(shuō)明雅可比矩 偽逆。 陣的特性，根據(jù)右圖中的幾何關(guān)系容易求得 ^l1 c1 l2 c12 g 二 cos(m), g2 二 cos(m 二2) y =li3 丄晁 S =sin(m),$2 二 sin(弓 二2) I ^x_ "dx ]訊 ]dy. Jy 01 -X I 兩邊微分后寫(xiě)成矩陣形式 即 dx _ -l1s

13、^ _l2 s12 T2S12 ||_dy. HJ1C/ l2C12 LS.ILdl 簡(jiǎn)寫(xiě)成dx=Jd 0,式中J就稱為機(jī)械臂的雅可比(Jacobian )矩陣，它由函數(shù) x, y的偏微 分組成，反映了關(guān)節(jié)微小位移 d 0與機(jī)械臂末端微小運(yùn)動(dòng) dx之間的關(guān)系。將兩邊同除以dt dt得到：dx/dt=Jd 0/dt, 因此機(jī)械臂的雅可比矩陣也可以看做是操作空間中的速度與關(guān) 節(jié)空間中速度的線性變換。 dx/dt 稱為末端在操作空間中的廣義速度，簡(jiǎn)稱操作速度， d 0 /dt為關(guān)節(jié)速度?？梢钥闯?，雅可比矩陣的每一列表示其它關(guān)節(jié)不動(dòng)而某一關(guān)節(jié)以單位速度 運(yùn)動(dòng)產(chǎn)生的末端速度。 —12 S1

14、2 | 可以看出，J陣的值隨末端位置的不同而不同，即 01和02的 I2 C|2 對(duì)于關(guān)節(jié)空間的某些位姿， 機(jī)械臂的雅可比矩陣的秩減少，這些位姿 ll Si -^2 S12 由J二 |Ui Ci I2 C12 改變會(huì)導(dǎo)致J的變化。 稱為機(jī)械臂的奇異位姿。 上例機(jī)械臂雅可比矩陣的行列式為： det(J)二l1l2 sin(二2),當(dāng)02=0 或02=180。時(shí)，機(jī)械臂的雅可比行列式為 0,矩陣的秩為1，這時(shí)機(jī)械臂處于奇異位姿。機(jī) 械臂在操作空間的自由度將減少。 如果機(jī)械臂的雅可比 J是滿秩的方陣，相應(yīng)的關(guān)節(jié)速度即可求出，即 扌二J'x，上例平面 2R機(jī)械臂的逆雅可

15、比矩陣 J」二 丄［8 l1l2S^ l1C^l2C12 —l1S^ — l2S12 _ I2S2 ,顯然，當(dāng)02趨于0 ° (或 7 # 180 °)時(shí)，機(jī)械臂接近奇異位姿，相應(yīng)的關(guān)節(jié)速度將趨于無(wú)窮大。 以及解決兩個(gè)不同坐標(biāo)系之間的微 為了補(bǔ)償機(jī)器人末端執(zhí)行器位姿與目標(biāo)物體之間的誤差, 位移關(guān)系問(wèn)題，需要討論機(jī)器人連桿在作微小運(yùn)動(dòng)時(shí)的位姿變化。 假設(shè)一變換的元素是某個(gè)變量的函數(shù)， 對(duì)該變換的微分就是該變換矩陣各元素對(duì)該變量的偏 T為: 導(dǎo)數(shù)所組成的變換矩陣乘以該變量的微分。例如給定變換 tn t|2 t|3 t|4 t

16、21 t22 t23 t24 t31 t32 t33 t34 t42 t43 t44 若它的元素是變量x的函數(shù)，則變換T的微分為： 8 - 'ctn 盤(pán)21 點(diǎn)t|2 泳 戲22 盤(pán)13 次 Ct23 dT = ex ex 盤(pán)31 戲32 員33 ex ex 也1 戲42 戲43 一 ex ex -：t14 CX 孜24 盤(pán)44 cX 一 泳dx 盤(pán)34 ex T,經(jīng)過(guò)微運(yùn)動(dòng)后 F面討論機(jī)械臂的微分運(yùn)動(dòng)， 設(shè)機(jī)械臂某一連桿相對(duì)于基坐標(biāo)系的位姿為 該連桿相對(duì)

17、基坐標(biāo)系的位姿變?yōu)?T+dT，若這個(gè)微運(yùn)動(dòng)是相對(duì)于基坐標(biāo)系（靜系）進(jìn)行的（左 乘），總可以用微小的平移和旋轉(zhuǎn)來(lái)表示，即 T dT 二Trans（dx,dy,dz）Rot（k,dr）T 所以有 dT - ||Trans（dx,dy,dz）Rot（k,d" - I4 4 T 根據(jù)齊次變換的對(duì)稱性， 若微運(yùn)動(dòng)是相對(duì)某個(gè)連桿坐標(biāo)系 i （動(dòng)系）進(jìn)行的（右乘），則T+dT 可以表示為 T dT 二T Trans(dx,dy,dz)Rot(k,^) 所以有 dT =T Trans(dx,dy,dz)Rot(k,d" - I4 4 令厶二Trans(dx,dy,dz)R

18、ot(k,dR-l4 4為微分算子，則相對(duì)基系有 dT= aot,相對(duì)i系 有dT=T Ai o這里△的下標(biāo)不同是由于微運(yùn)動(dòng)相對(duì)不同坐標(biāo)系進(jìn)行的。在機(jī)械臂運(yùn)動(dòng)學(xué)中 微分變換分為微分平移和微分旋轉(zhuǎn)兩類。 微分平移變換與一般平移變換一樣，其變換矩陣為 Trans(dx,dy, dz)二 0 0 dx 1 0 dy 0 1 dz 0 0 1 1 9 由于微分旋轉(zhuǎn)時(shí) 0^ 0，所以sin Q^d 0, cos 0^1將它們代入旋轉(zhuǎn)變換通式中得微分旋轉(zhuǎn) 表達(dá)式： Rot（k,dr）二 -1 kzd0 i-kyd6 0 -kzdr 1 kxdr 0 kydn

19、 -kxd v 1 01 0 于是得到微分算子.：=Trans（dx,dy,dz）Rot（k,dv）- ■ 0 -kzd6 kyd日 dx] kzd日 0 —kxd^ dy _kyd 日 kxd^ 0 dz 0 0 0 0 也= F面分別說(shuō)明。 I 4 4，即 微分旋轉(zhuǎn)與有限旋轉(zhuǎn)相比，有一些特殊的性質(zhì), (1 )微分旋轉(zhuǎn)的無(wú)序性，當(dāng) 0T 0時(shí)，有 sin 0f 0, cos 若令8x=d 0x, Sy=d Sz=d 0z,則繞三個(gè)坐標(biāo)軸的微分旋轉(zhuǎn)矩陣分別為 Rot（x,、.x） a ■1 0 0 衛(wèi) 0 1

20、、.x 0 0 -、x 1 0 01 0 0 1 略去2次項(xiàng)，得到 -Rot（x,、x）Rot（ y,、y） Rot（y,、y）Rot（x,、x）= 0 0 1 一 ' 0 0 - -1 0 b 01 1 -1 0 6y 01 y 1 丿X 0 0 1 0 -6 y 1 0 —d y 6x 1 0 0 0 0 1 _ 1 _ 0 0 0 1一 - 1 y 01 - ? 1 0 6y 01 0 1 -5x 0 0

21、 1 一 0 —0 y 6x 1 0 —0 y 6x 1 0 - 0 0 0 1 一 1 _ 0 0 0 1 一 Rot（y,、.y） a 結(jié)論: 微分旋轉(zhuǎn)其結(jié)果與轉(zhuǎn)動(dòng)次序無(wú)關(guān), 0 1 0 01 0 0 0 0 1 、y 0 1 -、z 1 0 ■1 0 -5y ■1 6z 0 Rot（Z,、.Z）= 01 0 0 1 兩者結(jié)果相同，可見(jiàn)這里左乘與右乘等效。 這是與 有限轉(zhuǎn)動(dòng)(一般旋轉(zhuǎn))的一個(gè)重要區(qū)別。 (2 )微分旋轉(zhuǎn)的可加性，考慮兩個(gè)微分旋轉(zhuǎn)復(fù)合后的效果

22、 11 1 --:：'z 、y 0 dz 1 -dx 0 Rot(x, 6x) Rot( y, 6y)Rot(z, 6z)= -by dx 1 0 .0 0 0 1一 若 Rot ( 8x, Sy, 8z) 和 Rot ( 3x' Sy' , Sz'） 表示兩個(gè)不同的微分旋轉(zhuǎn), 則兩次 12 #

23、連續(xù)轉(zhuǎn)動(dòng)的結(jié)果為: Rot(、x,、y,、z) RotC x：、y',、z')= 1 z :. z' _(、y :. y') 0 -（：Z 丄眉 z'） 1 、X丄心X' # # 即微分旋轉(zhuǎn)是可加的。 kyd日 01 - -1 」z 5y 01 -kxd 日 0 1 -5 x 0 1 0 -d y 1 0 0 1 一 1 0 0 0 1 一 kzd 0= Sz,將它們代入△得 上式表明：任意兩個(gè)微分旋轉(zhuǎn)的結(jié)果為繞每個(gè)軸轉(zhuǎn)動(dòng)的元素的

24、代數(shù)和, 由等效轉(zhuǎn)軸和等效轉(zhuǎn)角與 Rot( xr x) Rot( yr y)Rot(z「z)等效，有 Rot(k, d 力二 Rot(x,、x)Rot(y,、y)Rot(z,、z) 1 -kzd v kzd 日 1 -kyd 日 kxd& -0 0 所以有 kxd 0= 8x, kyd 0= 8y , 0 丿z Hy dx I | 6 0 -氏 dy 」y我x 0 dz .0 0 0 0 一 # # 可見(jiàn)，微分變換由兩個(gè)部分組成 S微分轉(zhuǎn)動(dòng)矢量，d微分平移矢量 合稱為微分運(yùn)動(dòng)矢量， - 0 1 0 0 1 0 10] 5

25、 例：已知一個(gè)坐標(biāo)系 A = 0 1 0 0 - 0 0 0 1 一 可表示為 D = (dx, dy,dz r x^ y r z) 微分旋轉(zhuǎn)矢量S=[0 0.1 0 ]，求微分變換dA。 ，相對(duì)固定系的微分平移矢量 d= [1 0 0.5], 13 z -y 」z J dxi 一 0 0 0.1 1 1 0 」x dy 0 0 0 0 眞 0 dz -0.1 0 0 0.5 0 0 0 _ 1 0 0 0 0 一

26、 ■0 1 10 0 '0 0 0.1 0 0 0 二 dA = AA = -0.1 0 0 0 0 0 1 0 0 0 1 0 0.5 0 1 0 _0 0 0.1 0 0 0 0 0 -0.1 0 F面討論兩坐標(biāo)系之間的微分關(guān)系，設(shè)第一個(gè)坐標(biāo)系為 i系，第二個(gè)坐標(biāo)系為 1 1 0 -0.5 0 一 j系不失一般 性，假定j系就是固定的0系。 Ox ax 0T nz 「° Px Py Pz 1 -0 —0 z dx] '0 —° z

27、 dx ] dz 0 -5x dy ，也i — 岀 0 一o x dy -5 y 6x 0 dz 」Yi &Xi 0 dz -0 0 0 0 一 1 1 0 0 0 0 一 = 0TA,也i = 打， 整理得到 因?yàn)槎Y ay az 0 oy Oz 0 所以厶00T dix = nkC P) d) * = n」 diy =0L((「 P) d) Jy =o」 diz = aL((「 p) d) 、：iz = a」： dxi I |dyi d乙 ki 母j Ox ax

28、「° ny nz (P n)x (P n)y Oy Oz (P o)x (P 0)y ay az (P a)x (P a)y 0 0 nx ny 0 0 Ox Oy 0 0 ax ay a n)J_dx0 一 (P O)z (P a)z nz Oz az 對(duì)于任何三維矢量 p=[px, Py, Pz]，其反對(duì)稱矩陣S(P)定義為: 0 -Pz Py S(P)= I Pz 0 "Px '-Py Px 0 一 dy0 dz0

29、 15 nx Ox ax 0 _ i R = ny Oy a y Oz az 一 上式簡(jiǎn)寫(xiě)成 16 卩 1=；0Rt —0Rts(0Po)1 o 0rt 一 類似地，任意兩坐標(biāo)系 ｛A｝和｛B｝之間廣義速度的坐標(biāo)變換為: # # ；BvL；R -BRS(aPb°)1「v] ；avL£R -：RS(BPa。) 」B =」八 .1 A ，」A =」八 」B

30、0.1 0 ]，求A系中等價(jià)的微分平移矢量 dA和微分旋轉(zhuǎn)矢量 Sa。 「J -0 aR 」L j BR 八 J - 0 0 1 10] 1 0 0 5 例：已知一個(gè)坐標(biāo)系 A = 0 1 0 0 ，相對(duì)固定系的微分平移矢量 - 0 0 0 1 _ d= [1 0 0.5], 微分旋轉(zhuǎn)矢量S=[0 解：將d= [1 0 0.5]和 冃0 0.1 0 ]代入 dix = nkC P) d) * = n」 diy =0kC，P) d)=o」 diz =ak(、： p) d) 、：iz=a上 得到 dA -〔0 0 5.11 :

31、? A - 0.1 0 0丨 o 4、機(jī)械臂軌跡規(guī)劃 機(jī)械臂的軌跡規(guī)劃可以在關(guān)節(jié)空間也可以在笛卡爾空間中進(jìn)行， 或者說(shuō)機(jī)械臂軌跡規(guī)劃是指 在關(guān)節(jié)空間或者笛卡爾空間中研究機(jī)械臂軌跡生成方法。 簡(jiǎn)言之，機(jī)械臂軌跡規(guī)劃是運(yùn)動(dòng)學(xué) 逆解的實(shí)際應(yīng)用，它描述了機(jī)械臂在多維空間中的運(yùn)動(dòng)路線 o在知道末端位姿的前提下， 通過(guò)運(yùn)動(dòng)學(xué)逆解得到各個(gè)關(guān)節(jié)在相應(yīng)時(shí)刻的轉(zhuǎn)動(dòng)量或者平移量， 合理的規(guī)劃指的是規(guī)劃出的 角位移曲線、角速度曲線以及角加速度曲線， 可以有效地減少了機(jī)械臂在運(yùn)動(dòng)過(guò)程中的沖擊 和振動(dòng)，使機(jī)械臂的工作壽命得以延長(zhǎng)。 械臂可以分為點(diǎn)到點(diǎn)作業(yè) (Point-to-Point Motion ) 和連續(xù)路徑作

32、業(yè) (Continuous-Path Motion ) 。點(diǎn)到點(diǎn)的運(yùn)動(dòng)指的是機(jī)械臂在運(yùn)動(dòng)過(guò)程中，只要求在某些點(diǎn)上有準(zhǔn)確的位置和 姿態(tài)，相鄰的點(diǎn)不做要求。連續(xù)運(yùn)動(dòng)要求機(jī)械臂嚴(yán)格的沿特定的曲線運(yùn)動(dòng)。 機(jī)械臂的關(guān)節(jié)角位移變化率比較小， 能夠有效地防止了機(jī)械臂工作時(shí)的振動(dòng)和沖擊。 機(jī)械臂 關(guān)節(jié)角速度和角加速度變化均平順連續(xù)， 從而有效避免了機(jī)械部件的磨損，能夠保證整個(gè) 機(jī)械臂系統(tǒng)的長(zhǎng)期、穩(wěn)定的運(yùn)行，滿足機(jī)械臂的工作要求。 5、 robotics 工具箱中的相關(guān)函數(shù) link 建立一個(gè)連桿對(duì)象 ,例如對(duì)于本次競(jìng)賽的機(jī)械臂，根據(jù)連桿參數(shù)得到 L{1}=link([pi/2 0 0 120

33、 0 0]); L{2}=link([pi/2 0 0 0 0 0]); L{3}=link([-pi/2 0 0 140.8 0 pi]); L{4}=link([-pi/2 71.8 0 0 0 pi/2 ]); L{5}=link([+pi/2 71.8 0 0 0 pi]); L{6}=link([-pi/2 0 0 0 0 pi/2]); L{7}=link([0 0 0 129.6 0 0]); robot 建立一個(gè)機(jī)械臂對(duì)象 R= robot(L) noname (7 axis, RRRRRRR)

34、 grav = [0.00 0.00 9.81] standard D&H parameters alpha A theta D R/P 1.5708 0 120 R (std) 1.5708. 0 0 R (std) -1.5708 0 140.8 R (std) -1.5708 71.8 0 R (std) 1.5708 71.8 0 R (std) -1.5708 0 0 R (std) 0 0 129.6 R (std) drivebot 用滑塊控制的機(jī)械臂圖形 drivebot(R,ones(

35、1,7)*pi) plot 機(jī)械臂的圖形顯示 plot(R,[pi/2 pi/2 0 0 0 0 0]) fkine 串聯(lián)機(jī)械臂正向運(yùn)動(dòng)學(xué)計(jì)算 tr =fkine (ROBOT, Q) ROBOT 表示機(jī)械臂對(duì)象， Q 機(jī)械臂關(guān)節(jié)坐標(biāo)值。 tr =fkine (R, [0 0 0 pi/2 0 0 0]) tr = 0.0000 -0.0000 1.0000 129.6000 -0.0000 1.0000 0.0000 -0.0000 -1.0000 -0.0000 0.0000 -20.8000 0 0 0 1.0000 ikine 串

36、聯(lián)機(jī)械臂逆向運(yùn)動(dòng)學(xué)計(jì)算 q = ikine(ROBOT, T) q = ikine(ROBOT, T, Q) q = ikine(ROBOT, 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，由于解空間的維數(shù)大于機(jī)械 臂的自由度，這時(shí)需要第 4 個(gè)輸入量 M 來(lái)確定笛卡爾坐標(biāo) (手腕對(duì)應(yīng)的坐標(biāo)系 )中的哪些量 在求解中被忽略。 M 中有 6 個(gè)元素，分別表示沿著 x,y,z 方向的平移和相

37、對(duì)于 x 軸， y 軸， z 軸的旋轉(zhuǎn)，值是 0( 忽略 )或 1 。非零元素的個(gè)數(shù)應(yīng)該等于機(jī)械臂的自由度。例如，對(duì)典型 的有 5 個(gè)自由度的機(jī)械臂，一般是忽略相對(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è)輸入變換，每一步的初始值取上一步的 值。

38、求解使用機(jī)械臂 Jacobian 矩陣的偽逆，這是數(shù)值求解方法，對(duì)于特定機(jī)械臂逆運(yùn)動(dòng)學(xué) 解( 如果可能 )應(yīng)該盡量使用解析解。但是這種方法可以得到奇異點(diǎn)上的解，零空間中的關(guān)節(jié) 角度可以任取。 q=ikine(R,tr) q = 0.0000 0.0000 0.0000 0.7854 -0.0000 -0.7854 0.0000 注意：對(duì)于機(jī)械臂末端的一個(gè)位置與姿態(tài)，逆運(yùn)動(dòng)學(xué)計(jì)算不是唯一的，驗(yàn)證 tr=fkine(R,q) tr = 0.0000 -0.0000 1.0000 129.6000 -0.0000 1.0000 0.0000 -0.0000 -1.0000

39、 -0.0000 0.0000 -20.8000 0 0 0 1.0000 transl 計(jì)算平移變換 tr= transl (X, Y, Z) 返回機(jī)械臂末端坐標(biāo) X, Y, Z 對(duì)應(yīng)的齊次表?yè)Q矩陣 tr=transl(129.6,0,20.8) tr = 1.0000 0 0 129.6000 0 1.0000 0 0 0 0 1.0000 20.8000 0 0 0 1.0000 [X Y Z]' = transl(T) 返回齊次表?yè)Q表示中的平移值，作為一個(gè) 3 元素的列向量 xyz=transl(tr)'

40、 xyz = 129.6000 0 20.8000 ctraj 計(jì)算工作空間中兩點(diǎn) T0,T1 之間的軌跡 tc= ctraj(T0, T1, N) tc = ctraj(T0, T1, R) 返回從 T0 到 T1 笛卡爾坐標(biāo)系的軌跡 TC N 表示軌跡中的點(diǎn)數(shù)。在第 1 中情況下，軌跡 20 # 中的點(diǎn)在 T0 到 T1 中等距離分配。在第 2 中情況下，向量 R 給出軌跡中每個(gè)點(diǎn)的距離， R 中的元素取值為 [0 1] 。一個(gè)軌跡是 4x4xN 矩陣，最后一個(gè)下標(biāo)表示點(diǎn)索引。旋轉(zhuǎn)插值使用 # # 四元球形線性插

41、值。 tr0=fkine(R,[0 0 0 0 0 0 0]) tr0 = 1.0000 -0.0000 -0.0000 -0.0000 0.0000 1.0000 0.0000 0.0000 -0.0000 -0.0000 1.0000 108.8000 0 0 0 1.0000 tr1=fkine(R,[pi/4 pi/6 0 pi/3 0 0 0]) tr1 = 0.6124 -0.7071 0.3536 95.6008 0.6124 0.7071 0.3536 95.6008 -0.5000 -0.0000 0.8660

42、 110.3005 0 0 0 1.0000 tc(:,:,1) = 1.0000 0 0 -0.0000 0 1.0000 0 0.0000 0 0 1.0000 108.8000 0 0 0 1.0000 tc(:,:,2) = 0.8976 -0.3822 0.2198 47.8004 0.3571 0.9226 0.1458 47.8004 -0.2585 -0.0523 0.9646 109.5503 0 0 0 1.0000 tc(:,:,3) = 0.6124 -

43、0.7071 0.3536 95.6008 0.6124 0.7071 0.3536 95.6008 # -0.5000 -0.0000 0.8660 110.3005 21 0 0 0 1.0000 transl(tc) ans = -0.0000 0.0000 108.8000 47.8004 47.8004 109.5503 95.6008 95.6008 110.3005 jtraj 計(jì)算關(guān)節(jié)中兩點(diǎn) Q0,Q1 之間的軌跡 [Q QD QDD] = jtraj(Q0, Q1, N) [Q QD QDD] = jtraj (Q0, Q1, N

44、, QD0, QD1) [Q QD QDD] = jtraj (Q0, Q1, T) [Q QD QDD] = jtraj (Q0, Q1, T, QD0, QD1) QD0, QD1 軌跡中的點(diǎn)數(shù)是 N，或者是一個(gè)時(shí)間向量 T。插值使用7次多項(xiàng)式，邊界速度由 指定，缺省時(shí)邊界速度和加速度為 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]=

45、jtraj(q0,q1,30); figure subplot(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 on subplot(2,2,4),plot(QDD),grid on %注意：其中

46、有一些曲線重合 jacob0 計(jì)算機(jī)械臂在基坐標(biāo)系中 Jacobian 矩陣 J = jacob0(ROBOT, Q) tr2jac 計(jì)算機(jī)械臂在基坐標(biāo)系中 Jacobian 矩陣 J = TR2JAC(T) diff2tr 微分表示轉(zhuǎn)換為齊次變換 tr = diff2tr(D) tr2diff 轉(zhuǎn)換為齊次變換轉(zhuǎn)換為微分表示 返回表示微分平移與旋轉(zhuǎn)的齊次變換矩陣 ,矩陣中包含一個(gè)反對(duì)稱的旋轉(zhuǎn)子矩陣。 D =tr2diff(T) D = tr2diff(T1, T2) 第一種形式將齊次表?yè)Q矩陣表示轉(zhuǎn)換為 6- 元素向量微分表示。 第二種形式返回 6-元素向量，表示從

47、 T1 到 T2 的在基坐標(biāo)系中需要的微分移動(dòng)。 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 r

48、otation. 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

49、-.2 .1 .1]' diff2tr(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.0000 0.0000 -0.0000 0.0039 -0.0000 -0.0039 0.

50、0000 % 這是計(jì)算工作空間軌跡和求逆解的另外一種方法。但是，如果 Jacobian 矩陣奇異時(shí) % 會(huì)失效。如果機(jī)械臂的自由度大于 6，即是冗余的，采用 Jacobian 矩陣偽逆計(jì)算，或 % 對(duì) Jacobian 矩陣進(jìn)行奇異值分解。 附錄 rpy 角與 euler 角 （1 ） rpy 角 rpy角是描述船舶航行時(shí)的姿態(tài)的一種方法， 將船的行駛方向作為 Z軸，則繞Z軸旋轉(zhuǎn)稱為 滾動(dòng)（Roll ）角冬將繞丫軸（與海面平行）方向的旋轉(zhuǎn)稱為俯仰（Pitch ）角取X軸與海 面垂直方向，將繞 X軸的旋轉(zhuǎn)稱為偏轉(zhuǎn)（Yaw ）角丫。機(jī)械臂末端的定義類似，故習(xí)慣上稱 為rpy角。

51、描述運(yùn)動(dòng)坐標(biāo)系的規(guī)則是：首先使運(yùn)動(dòng)坐標(biāo)系的初始方位與固定坐標(biāo)系重合， 將運(yùn)動(dòng)坐標(biāo)系 繞固定坐標(biāo)系 X軸轉(zhuǎn)動(dòng)y,再將運(yùn)動(dòng)坐標(biāo)系繞固定坐標(biāo)系 丫軸轉(zhuǎn)動(dòng)3,最后將運(yùn)動(dòng)坐標(biāo)系繞 固定坐標(biāo)系Z軸轉(zhuǎn)動(dòng)a。因?yàn)槿无D(zhuǎn)動(dòng)都是相對(duì)固定坐標(biāo)系的，所以相應(yīng)的旋轉(zhuǎn)矩陣 XYZ旋轉(zhuǎn)的rpy方 法。下面討論逆問(wèn)題：從給定的旋轉(zhuǎn)矩陣得到繞固定軸 XYZ旋轉(zhuǎn)的rpy角丫3 a令 rpy（,：,：）二rot（ z, ： ）rot（y, ： ）rot（x,） 「g -sa 0 O［「cB 0 S0 0V1 0 0 0〕 s g 0 0 0 1 0 0 0 & -sY 0 —0

52、0 1 0 -sP 0 C0 0 0 S? cY 0 「0 0 0 0 0 ―。 0 0 1 一 將三個(gè)矩陣相乘得到 「c^cP casPs? 一尹 c gsP c 01 sPs? -EC? 少 sPc? 0 rpy（j, * d 創(chuàng)s? cPc? 0 -0 0 0 1 一 它表示繞固定坐標(biāo)系的三個(gè)軸依次旋轉(zhuǎn)得到的旋轉(zhuǎn)矩陣，稱為繞固定軸 Ox ax 01 .V R \ ny Oy ay 0 rpy（「H）= c nz oz az 0 .0 0 0 1 一

53、上式中有3個(gè)未知數(shù)，9個(gè)方程，其中6個(gè)不獨(dú)立，因此可利用其中 3個(gè)解出未知數(shù)。 cos（ ■）= n： - ny，如果cos（ 3）不為零，則可以得到 P =atan2( -nz，Jn； +n：),口 =atan2(ox, nx), Y =atan2(ay, az) (2 )繞運(yùn)動(dòng)系ZYX轉(zhuǎn)動(dòng)的euler角 描述運(yùn)動(dòng)坐標(biāo)系的規(guī)則是： 運(yùn)動(dòng)坐標(biāo)系的初始方位與參考坐標(biāo)系重合， 首先將運(yùn)動(dòng)坐標(biāo)系繞 Z軸轉(zhuǎn)動(dòng)a,再將運(yùn)動(dòng)坐標(biāo)系繞 Y軸轉(zhuǎn)動(dòng)3,最后將運(yùn)動(dòng)坐標(biāo)系繞 X軸轉(zhuǎn)動(dòng)丫。這種描述方法 中各次的轉(zhuǎn)動(dòng)都是相對(duì)運(yùn)動(dòng)系的，而不是相對(duì)固定坐標(biāo)系的。相應(yīng)的旋轉(zhuǎn)矩陣為 euler（： , :, ）

54、 = rot（x^ ）rot（y, ：）rot（z,） Pca -sa 0 0「cB 0 sP 0］「1 0 0 01 S- d 0 0 0 1 0 0 0 & -s/ 0 0 0 1 0 -sP 0 cP 0 0 s? c 0 「° 0 0 1 一 -0 0 0 1一 0 0 0 1 一 將三個(gè)矩陣相乘得到 25 # euler（-知：，） c：c： S^cP I -sP g S - S - S--^ c — c=c cP s? 01

55、 0 結(jié)果與繞固定軸XYZ旋轉(zhuǎn)相同, 且旋轉(zhuǎn)角度對(duì)應(yīng)相等。因此，用 另外一種常用的euler角方法是 這是因?yàn)槔@固定軸旋轉(zhuǎn)的順序與繞運(yùn)動(dòng)軸旋轉(zhuǎn)的順序相反, ZYX euler角與XYZ rpy角的描述方法是等價(jià)的。 ZYZ方法，首先使運(yùn)動(dòng)坐標(biāo)系與參考坐標(biāo)系重合，將運(yùn)動(dòng) -0 -St 0 0「cB 0 sP 0］「C? -s 0 01 s 0 0 0 1 0 0 s? c 0 0 0 0 1 0 1-sP 0 cP 0 0 0 1 0 「° 0 0 1 一 ?0 0 0 1一

56、 0 0 0 1一 坐標(biāo)系繞Z軸轉(zhuǎn)動(dòng)a,再將運(yùn)動(dòng)坐標(biāo)系繞 Y軸轉(zhuǎn)動(dòng)3,最后將運(yùn)動(dòng)坐標(biāo)系繞 Z軸轉(zhuǎn)動(dòng)丫。 euler（： , ：, ） =rot（z, ： ）rot （y, ： ）rot（z,） 將三個(gè)矩陣相乘得到 C-：心 C -S-^S euler（:,：,）= 0 _C-：：S：S _S-：iC -s-：：sE“s ' c-^c 01 0 0 1 26 :=atan2(. a： 如果sin( 3)不為零，ZYZ方式euler角逆解是 2 + ay, az),a =atan2(0z,nz)7 =atan2(ay, _ax) 27