第五章機(jī)器人運(yùn)動(dòng)學(xué)

第五章機(jī)器人運(yùn)動(dòng)學(xué)1第一頁，共九十二頁，編輯于2023年，星期五運(yùn)動(dòng)學(xué)正問題桿件參數(shù)的意義坐標(biāo)系的建立原則桿件坐標(biāo)系間的變換過程-相鄰關(guān)節(jié)坐標(biāo)系的齊次變換機(jī)器人的運(yùn)動(dòng)學(xué)方程第二頁，共九十二頁，編輯于2023年，星期五桿件參數(shù)的意義-和

li

:

關(guān)節(jié)Ai軸和Ai+1軸線公法線的長(zhǎng)度:關(guān)節(jié)i軸線與i+1軸線在垂直于li平面內(nèi)的夾角

串聯(lián)關(guān)節(jié)，每個(gè)桿件最多與2個(gè)桿件相連，如Ai與Ai-1和Ai+1相連。由運(yùn)動(dòng)學(xué)的觀點(diǎn)來看，桿件的作用僅在于它能保持其兩端關(guān)節(jié)間的形態(tài)不變。這種形態(tài)由兩個(gè)參數(shù)決定，一是桿件的長(zhǎng)度

li（），一個(gè)是桿件的扭轉(zhuǎn)角AiAi+1第三頁，共九十二頁，編輯于2023年，星期五桿件參數(shù)的意義-和

:是從第i-1坐標(biāo)系的原點(diǎn)到Zi－１軸和Ｘi軸的交點(diǎn)沿Ｚi-1軸測(cè)量的距離:繞Zi-1軸由Ｘi-1軸轉(zhuǎn)向Ｘi軸的關(guān)節(jié)角

確定桿件相對(duì)位置關(guān)系，由另外2個(gè)參數(shù)決定，一個(gè)是桿件的距離：，一個(gè)是桿件的回轉(zhuǎn)角：

AiAi+1Ai-1第四頁，共九十二頁，編輯于2023年，星期五坐標(biāo)系的建立原則AiAi+1Ai-1為右手坐標(biāo)系原點(diǎn)Oi：設(shè)在Li與Ai+1軸線的交點(diǎn)上Zi軸：與Ai+1關(guān)節(jié)軸重合，指向任意Xi軸：與公法線Li重合，指向沿Li由Ai軸線指向Ai+1軸線Yi軸：按右手定則Li—沿xi軸，zi-1軸與xi軸交點(diǎn)到0i的距離αi—繞xi軸，由zi-1轉(zhuǎn)向zidi—沿zi-1軸，zi-1軸和xi交點(diǎn)至∑0i–1坐標(biāo)系原點(diǎn)的距離θi—繞zi-1軸，由xi-1轉(zhuǎn)向xi第五頁，共九十二頁，編輯于2023年，星期五

桿件坐標(biāo)系間的變換過程

-相鄰關(guān)節(jié)坐標(biāo)系的齊次變換將xi-1軸繞zi-1軸轉(zhuǎn)i角度，將其與xi軸平行；沿zi-1軸平移距離di，使xi-1軸與xi軸共線；沿xi軸平移距離li(ai)

，使兩坐標(biāo)系原點(diǎn)重合；繞xi軸轉(zhuǎn)i角度，zi-1軸與zi軸重合；兩坐標(biāo)系i-1與i完全重合．AiAi+1Ai-1第六頁，共九十二頁，編輯于2023年，星期五第七頁，共九十二頁，編輯于2023年，星期五機(jī)器人的運(yùn)動(dòng)學(xué)方程

D-H變換矩陣第八頁，共九十二頁，編輯于2023年，星期五特殊情況坐標(biāo)系的建立原則Oi—Ai與Ai+1關(guān)節(jié)軸線的交點(diǎn)Zi—Ai+1軸線Xi—Zi和Zi-1構(gòu)成的面的法線Yi—右手定則xiyi兩個(gè)關(guān)節(jié)軸相交oizi-1第九頁，共九十二頁，編輯于2023年，星期五兩個(gè)關(guān)節(jié)軸線平行先建立∑0i-1然后建立∑0i+1最后建立∑0i

Ai-1AiAi+1Ai+2li-1oi-1xi-1yi-1zi-1ABCDoi（xi）（yi）zixiyioi+1xi+1yi+1zi+1di+1li+1di它們之間有無數(shù)條公垂線，這時(shí)可挑選與前一關(guān)節(jié)的公垂線共線的一條公垂線。第十頁，共九十二頁，編輯于2023年，星期五舉例：Stanford機(jī)器人第十一頁，共九十二頁，編輯于2023年，星期五A1A2A3A4A5A6d1z1x1y1O1d2z2x2y2O2z3y3O3y4z4x4O4z5y5x5O5d3z6x6y6O6d6z0y0x0O0為右手坐標(biāo)系原點(diǎn)Oi：Ai與Ai+1關(guān)節(jié)軸線的交點(diǎn)Zi軸：與Ai+1關(guān)節(jié)軸重合，指向任意Xi軸：Zi和Zi-1構(gòu)成的面的法線Yi軸：按右手定則Li—沿xi軸，zi-1軸與xi軸交點(diǎn)到0i的距離αi—繞xi軸，由zi-1轉(zhuǎn)向zidi—沿zi-1軸，zi-1軸和xi交點(diǎn)至∑0i–1坐標(biāo)系原點(diǎn)的距離θi—繞zi-1軸，由xi-1轉(zhuǎn)向xix3第十二頁，共九十二頁，編輯于2023年，星期五解：第十三頁，共九十二頁，編輯于2023年，星期五第十四頁，共九十二頁，編輯于2023年，星期五第十五頁，共九十二頁，編輯于2023年，星期五第十六頁，共九十二頁，編輯于2023年，星期五y1x1z1z2x2y2x3z0x0y0z4y4z3y3x4第十七頁，共九十二頁，編輯于2023年，星期五第十八頁，共九十二頁，編輯于2023年，星期五第十九頁，共九十二頁，編輯于2023年，星期五第二十頁，共九十二頁，編輯于2023年，星期五第二十一頁，共九十二頁，編輯于2023年，星期五運(yùn)動(dòng)學(xué)逆問題多解性，剔除多余解原則根據(jù)關(guān)節(jié)運(yùn)動(dòng)空間合適的解選擇一個(gè)與前一采樣時(shí)間最接近的解根據(jù)避障要求得選擇合適的解逐級(jí)剔除多余解可解性所有具有轉(zhuǎn)動(dòng)和移動(dòng)關(guān)節(jié)的系統(tǒng)，在一個(gè)單一串聯(lián)中總共有6個(gè)（或小于6個(gè)）自由度時(shí)，是可解的，一般是數(shù)值解，它不是解析表達(dá)式，而是利用數(shù)值迭代原理求解，它的計(jì)算量要比解析解大如若干個(gè)關(guān)節(jié)軸線相交和或多個(gè)關(guān)節(jié)軸線等于0或90°的情況下，具有6個(gè)自由度的機(jī)器人可得到解析解第二十二頁，共九十二頁，編輯于2023年，星期五例題：試求立方體中心在機(jī)座坐標(biāo)系∑0中的位置該手爪從上方把物體抓起，同時(shí)手爪的開合方向與物體的Y軸同向，那么，求手爪相對(duì)于∑0的姿態(tài)是什么？

在機(jī)器人工作臺(tái)上加裝一電視攝像機(jī)，攝像機(jī)可見到固聯(lián)著6DOF關(guān)節(jié)機(jī)器人的機(jī)座坐標(biāo)系原點(diǎn)，它也可以見到被操作物體（立方體）的中心，如果在物體中心建一局部坐標(biāo)系，則攝像機(jī)所見到的這個(gè)物體可由齊次變換矩陣T1來表示，如果攝像機(jī)所見到的機(jī)座坐標(biāo)系為矩陣T2表示。xyz第二十三頁，共九十二頁，編輯于2023年，星期五解1：因此物體位于機(jī)座坐標(biāo)系的（11，10，1）T處，它的X，Y，Z軸分別與機(jī)座坐標(biāo)系的-Y，X，Z軸平行。xyzz機(jī)y機(jī)z物y物x物oO機(jī)O物第二十四頁，共九十二頁，編輯于2023年，星期五解2：xyzz機(jī)y機(jī)z物y物x物oO機(jī)O物第二十五頁，共九十二頁，編輯于2023年，星期五xyzz機(jī)y機(jī)z物y物x物oO機(jī)O物第二十六頁，共九十二頁，編輯于2023年，星期五用未知的逆變換逐次左乘，由乘得的矩陣方程的元素決定未知數(shù)，即用逆變換把一個(gè)未知數(shù)由矩陣方程的右邊移到左邊求解這個(gè)未知數(shù)把下一個(gè)未知數(shù)移到左邊重復(fù)上述過程，直到解出所有解運(yùn)動(dòng)學(xué)逆問題解法Paul等人提出的方法:第二十七頁，共九十二頁，編輯于2023年，星期五Paul等人提出的方法第二十八頁，共九十二頁，編輯于2023年，星期五斯坦福機(jī)器人運(yùn)動(dòng)學(xué)逆問題解第二十九頁，共九十二頁，編輯于2023年，星期五式中：

由兩端矩陣對(duì)應(yīng)元素相等可得：

第三十頁，共九十二頁，編輯于2023年，星期五作三角變換：

式中：

得到：

即有：

（）第三十一頁，共九十二頁，編輯于2023年，星期五由1,4和2,4元素對(duì)應(yīng)相等，得：

第三十二頁，共九十二頁，編輯于2023年，星期五式中第四列：

第三十三頁，共九十二頁，編輯于2023年，星期五式中第三列：

第三十四頁，共九十二頁，編輯于2023年，星期五第三十五頁，共九十二頁，編輯于2023年，星期五機(jī)器人末端操作器位姿的其它描述方法用矩陣表示剛性體的轉(zhuǎn)動(dòng)簡(jiǎn)化了許多運(yùn)算，但它需要9個(gè)元素來完全描述旋轉(zhuǎn)剛體的姿態(tài)，因此矩陣并不直接得出一組完備的廣義坐標(biāo)。一組廣義坐標(biāo)應(yīng)能描述轉(zhuǎn)動(dòng)剛體相對(duì)于參考坐標(biāo)的方向，被稱為歐拉角的三個(gè)角度，φ、θ、ψ就是這種廣義坐標(biāo)。有幾種不同的歐拉角表示方法，它們均可描述剛體相對(duì)于固定參考系的姿態(tài)。三種最常見的歐拉角類型列在表中

第三十六頁，共九十二頁，編輯于2023年，星期五3種最常見的歐拉角類型步1步2步3類型1繞OZ軸轉(zhuǎn)φ角繞當(dāng)前OU'軸轉(zhuǎn)θ角繞當(dāng)前OW″軸轉(zhuǎn)ψ角類型2繞OZ軸轉(zhuǎn)φ角繞當(dāng)前OV'軸轉(zhuǎn)θ角繞當(dāng)前OW″軸轉(zhuǎn)ψ角類型3繞OX軸轉(zhuǎn)φ角繞OY軸轉(zhuǎn)θ角繞OZ軸轉(zhuǎn)ψ角φφφu′v′w′①x(u)y(v)z(w)oθu"v"θw"②u???③ψψψv???W???類型1：表示法通常用于陀螺運(yùn)動(dòng)第三十七頁，共九十二頁，編輯于2023年，星期五類型2：所得的轉(zhuǎn)動(dòng)矩陣為右乘

第三十八頁，共九十二頁，編輯于2023年，星期五類型3：一般稱此轉(zhuǎn)動(dòng)的歐拉角為橫滾、俯仰和偏航角，這種形式主要用于航空工程中分析飛行器的運(yùn)動(dòng)，其旋轉(zhuǎn)矩陣為（這種方法也叫做橫滾、俯仰和偏航角表示方法）

第三十九頁，共九十二頁，編輯于2023年，星期五微動(dòng)矩陣和微動(dòng)齊次變換對(duì)象:微動(dòng)矩陣主要是描述機(jī)器人在微動(dòng)范圍內(nèi)各關(guān)節(jié)的位移運(yùn)動(dòng)關(guān)系定義:各關(guān)節(jié)當(dāng)角度移小于5°時(shí)，平移在0.1mm左右時(shí)，微動(dòng)矩陣大致可用第四十頁，共九十二頁，編輯于2023年，星期五微分變換與雅可比矩陣微分變換為了補(bǔ)償機(jī)器人末端執(zhí)行器位姿與目標(biāo)物體之間的誤差，以及解決兩個(gè)不同坐標(biāo)系之間的微位移關(guān)系問題，需要討論機(jī)器人桿件在作微小運(yùn)動(dòng)時(shí)的位姿變化。一.變換的微分

假設(shè)一變換的元素是某個(gè)變量的函數(shù)，對(duì)該變換的微分就是該變換矩陣各元素對(duì)該變量的偏導(dǎo)數(shù)所組成的變換矩陣乘以該變量的微分。第四十一頁，共九十二頁，編輯于2023年，星期五若它的元素是變量x的函數(shù)，則T的微分為:例如給定變換T為：第四十二頁，共九十二頁，編輯于2023年，星期五二.微分運(yùn)動(dòng)所以得

設(shè)機(jī)器人某一桿件相對(duì)于基坐標(biāo)系的位姿為T，經(jīng)過微運(yùn)動(dòng)后該桿件相對(duì)基坐標(biāo)系的位姿變?yōu)門+dT，若這個(gè)微運(yùn)動(dòng)是相對(duì)于基坐標(biāo)系（靜系）進(jìn)行的(右乘)，總可以用微小的平移和旋轉(zhuǎn)來表示，即第四十三頁，共九十二頁，編輯于2023年，星期五根據(jù)齊次變換的相對(duì)性，若微運(yùn)動(dòng)是相對(duì)某個(gè)桿件坐標(biāo)系i（動(dòng)系）進(jìn)行的(右乘)，則T+dT可以表示為則相對(duì)基系有dT=Δ0T，相對(duì)i系有dT=TΔi。這里Δ的下標(biāo)不同是由于微運(yùn)動(dòng)相對(duì)不同坐標(biāo)系進(jìn)行的。所以得令第四十四頁，共九十二頁，編輯于2023年，星期五三.微分平移和微分旋轉(zhuǎn)由于微分旋轉(zhuǎn)θ→0，所以sinθ→dθ，cosθ→1，Versθ→0，將它們代入旋轉(zhuǎn)變換通式中得微分旋轉(zhuǎn)表達(dá)式:微分平移變換與一般平移變換一樣，其變換矩陣為:于是得第四十五頁，共九十二頁，編輯于2023年，星期五四.微分旋轉(zhuǎn)的無序性當(dāng)θ→0時(shí)，有sinθ→dθ，cosθ→1．若令δx=dθx，δy=dθy，δz=dθz，則繞三個(gè)坐標(biāo)軸的微分旋轉(zhuǎn)矩陣分別為略去高階無窮小量第四十六頁，共九十二頁，編輯于2023年，星期五兩者結(jié)果相同，可見這里左乘與右乘等效。同理可得結(jié)論：微分旋轉(zhuǎn)其結(jié)果與轉(zhuǎn)動(dòng)次序無關(guān)，這是與有限轉(zhuǎn)動(dòng)（一般旋轉(zhuǎn)）的一個(gè)重要區(qū)別。第四十七頁，共九十二頁，編輯于2023年，星期五若Rot（δx，δy，δz）和Rot（δx’，δy’，δz’）表示兩個(gè)不同的微分旋轉(zhuǎn)，則兩次連續(xù)轉(zhuǎn)動(dòng)的結(jié)果為：上式表明：任意兩個(gè)微分旋轉(zhuǎn)的結(jié)果為繞每個(gè)軸轉(zhuǎn)動(dòng)的元素的代數(shù)和，即微分旋轉(zhuǎn)是可加的。第四十八頁，共九十二頁，編輯于2023年，星期五kxdθ=δx，

kydθ=δy

，

kzdθ=δz所以有由等效轉(zhuǎn)軸和等效轉(zhuǎn)角與等效，有即第四十九頁，共九十二頁，編輯于2023年，星期五將它們代入Δ得因此Δ可以看成由和兩個(gè)矢量組成，叫微分轉(zhuǎn)動(dòng)矢量，叫微分平移矢量。分別表示為和合稱為微分運(yùn)動(dòng)矢量，可表示為第五十頁，共九十二頁，編輯于2023年，星期五解：例：已知一個(gè)坐標(biāo)系A(chǔ)，相對(duì)固定系的微分平移矢量，微分旋轉(zhuǎn)矢量，求微分變換dA。第五十一頁，共九十二頁，編輯于2023年，星期五五.兩坐標(biāo)系之間的微分關(guān)系因?yàn)閷⑺鼈兇肭懊娴姆匠态F(xiàn)在討論i系和j系之間的微分關(guān)系。不失一般性，假定j系就是固定系（基系）0系。第五十二頁，共九十二頁，編輯于2023年，星期五得其中上式簡(jiǎn)寫成第五十三頁，共九十二頁，編輯于2023年，星期五對(duì)于任何三維矢量，其反對(duì)稱矩陣定義為：相應(yīng)地，任意兩坐標(biāo)系{A}和{B}之間廣義速度的坐標(biāo)變換為：第五十四頁，共九十二頁，編輯于2023年，星期五例：知坐標(biāo)系A(chǔ)及相對(duì)于固定系的微分平移矢量，微分旋轉(zhuǎn)矢量，求A系中等價(jià)的微分平移矢量dA和微分旋轉(zhuǎn)矢量δA。解：因?yàn)橐阎?，可以根?jù)前面的公式求得dA和δA。也可根據(jù)與它一樣的另一組表達(dá)式（寫法不同）求解，即求得，代入第五十五頁，共九十二頁，編輯于2023年，星期五為了驗(yàn)證這一結(jié)果，先求ΔA再得dA驗(yàn)證的結(jié)果是與上例dA=ΔA的計(jì)算結(jié)果完全一樣。第五十六頁，共九十二頁，編輯于2023年，星期五

雅可比矩陣兩空間之間速度的線性映射關(guān)系—雅可比矩陣（簡(jiǎn)稱雅可比）。它可以看成是從關(guān)節(jié)空間到操作空間運(yùn)動(dòng)速度的傳動(dòng)比，同時(shí)也可用來表示兩空間之間力的傳遞關(guān)系。vxvy存在怎樣的關(guān)系第五十七頁，共九十二頁，編輯于2023年，星期五首先來看一個(gè)兩自由度的平面機(jī)械手，如圖3-17所示。圖3-17兩自由度平面機(jī)械手容易求得將其微分得寫成矩陣形式第五十八頁，共九十二頁，編輯于2023年，星期五假設(shè)關(guān)節(jié)速度為，手爪速度為。簡(jiǎn)寫成：

dx=Jdθ。式中J就稱為機(jī)械手的雅可比（Jacobian）矩陣，它由函數(shù)x，y的偏微分組成，反映了關(guān)節(jié)微小位移dθ與手部（手爪）微小運(yùn)動(dòng)dx之間的關(guān)系。對(duì)dx=Jdθ兩邊同除以dt，得可以更一般的寫成。第五十九頁，共九十二頁，編輯于2023年，星期五因此機(jī)械手的雅可比矩陣定義為它的操作空間速度與關(guān)節(jié)空間速度的線性變換。（或v）稱為手爪在操作空間中的廣義速度，簡(jiǎn)稱操作速度，為關(guān)節(jié)速度。J若是6×n的偏導(dǎo)數(shù)矩陣，它的第i行第j列的元素為：式中，x代表操作空間，q代表關(guān)節(jié)空間。第六十頁，共九十二頁，編輯于2023年，星期五若令J1，J2分別為上例中雅可比矩陣的第一列矢量和第二列矢量，即可以看出，雅可比矩陣的每一列表示其它關(guān)節(jié)不動(dòng)而某一關(guān)節(jié)以單位速度運(yùn)動(dòng)產(chǎn)生的端點(diǎn)速度。由，可以看出，J陣的值隨手爪位置的不同而不同，即θ1和θ2的改變會(huì)導(dǎo)致J的變化。第六十一頁，共九十二頁，編輯于2023年，星期五對(duì)于關(guān)節(jié)空間的某些形位，機(jī)械手的雅可比矩陣的秩減少，這些形位稱為操作臂（機(jī)械手）的奇異形位。上例機(jī)械手雅可比矩陣的行列式為：det(J)=l1l2s2當(dāng)θ2=0°或θ2=180°時(shí)，機(jī)械手的雅可比行列式為0，矩陣的秩為1，因此處于奇異狀態(tài)。在奇異形位時(shí)，機(jī)械手在操作空間的自由度將減少。第六十二頁，共九十二頁，編輯于2023年，星期五只要知道機(jī)械手的雅可比J是滿秩的方陣，相應(yīng)的關(guān)節(jié)速度即可求出，即。上例平面2R機(jī)械手的逆雅可比于是得到與末端速度相應(yīng)的關(guān)節(jié)速度：顯然，當(dāng)θ2趨于0°（或180°）時(shí)，機(jī)械手接近奇異形位，相應(yīng)的關(guān)節(jié)速度將趨于無窮大。第六十三頁，共九十二頁，編輯于2023年，星期五微動(dòng)平移和微動(dòng)旋轉(zhuǎn)的齊次變換：平移：旋轉(zhuǎn)R，繞任意軸旋轉(zhuǎn)角。繞x,y,z軸的微分轉(zhuǎn)動(dòng)分別定義為。則有：第六十四頁，共九十二頁，編輯于2023年，星期五表示繞x,y,z軸的微分旋轉(zhuǎn)矩陣為：第六十五頁，共九十二頁，編輯于2023年，星期五如果兩個(gè)微分矩陣以不同的順序相乘則得到不同的結(jié)果。第六十六頁，共九十二頁，編輯于2023年，星期五第六十七頁，共九十二頁，編輯于2023年，星期五如果忽略高階微分，則結(jié)果完全相同。因此，在微分運(yùn)動(dòng)中，可認(rèn)為相乘的順序并不重要。第六十八頁，共九十二頁，編輯于2023年，星期五假設(shè)繞一般軸的微分運(yùn)動(dòng)是由繞三條坐標(biāo)軸的三個(gè)微分運(yùn)動(dòng)以任意的順序構(gòu)成的，因此繞一般軸的微分運(yùn)動(dòng)可以表示為：第六十九頁，共九十二頁，編輯于2023年，星期五如果忽略高階微分，則：第七十頁，共九十二頁，編輯于2023年，星期五第七十一頁，共九十二頁，編輯于2023年，星期五坐標(biāo)系的微分變換坐標(biāo)系的微分變換是微分平移和微分旋轉(zhuǎn)運(yùn)動(dòng)的合成。如果用T表示原始坐標(biāo)系，并假定由于微分變換所引起的坐標(biāo)系T的變化量用dT表示，則：微分平移和微分旋轉(zhuǎn)與順序無關(guān)。第七十二頁，共九十二頁，編輯于2023年，星期五因此微動(dòng)率△=微動(dòng)的齊次變換：dT=△?T

第七十三頁，共九十二頁，編輯于2023年，星期五己知變換矩陣轉(zhuǎn)動(dòng)：平移：求dT解：第七十四頁，共九十二頁，編輯于2023年，星期五反過來：如果我們要求Σ在Σ中的齊次交換矩陣為實(shí)際測(cè)得的為那么末端執(zhí)行器坐標(biāo)系要如何運(yùn)動(dòng)才能到達(dá)期望值？轉(zhuǎn)動(dòng)：平移：第七十五頁，共九十二頁，編輯于2023年，星期五等效微動(dòng)位移的求解前面研究的是動(dòng)坐標(biāo)系ΣOn在ΣOo中的b變換為T，相對(duì)于基準(zhǔn)坐標(biāo)系作微平移和微轉(zhuǎn)動(dòng)，來求微動(dòng)齊次交換。現(xiàn)在我們研究動(dòng)坐標(biāo)系ΣOn相對(duì)于自身坐標(biāo)系做了微位移或微轉(zhuǎn)動(dòng)，達(dá)到繞基準(zhǔn)坐標(biāo)同樣的效果則如何求解。

第七十六頁，共九十二頁，編輯于2023年，星期五dT=△?T（繞基準(zhǔn)坐標(biāo)系）=T?T△（繞動(dòng)坐標(biāo)系）左乘,繞基準(zhǔn)右乘,繞動(dòng)坐標(biāo)軸強(qiáng)調(diào)等效第七十七頁，共九十二頁，編輯于2023年，星期五設(shè)：有：第七十八頁，