第三章-機(jī)器人運動學(xué)

第三章機(jī)器人運動學(xué)§3.1機(jī)器人運動方程的表示

機(jī)器人的機(jī)械手看作是一系列由關(guān)節(jié)連接起來的連桿構(gòu)成的。為機(jī)械手的每一連桿建立一個坐標(biāo)系，并用齊次變換來描述這些坐標(biāo)系間的相對位置和姿態(tài)。通常把描述一個連桿與下一個連桿間相對關(guān)系的齊次變換叫做A矩陣。一個A矩陣就是一個描述連桿坐標(biāo)系間相對平移和旋轉(zhuǎn)的齊次變換。如果A1表示第一個連桿對于基系的位置和姿態(tài)，A2表示第二個連桿相對于第一個連桿的位置和姿態(tài)，則第二個連桿在基系中的位置和姿態(tài)可由下列矩陣的乘積給出

T2=

A1A2同理，若A3表示第三個連桿相對于第二個連桿的位置和姿態(tài)，則有

T3=

A1A2A3稱這些A矩陣的乘積為T矩陣，其前置上標(biāo)若為0，則可省略。對于六連桿機(jī)械手，有下列T矩陣

T6=

A1A2A3A4A5A6機(jī)械手的運動方向原點由矢量p表示。接近矢量a：z軸設(shè)在手指接近物體的方向，稱為接近矢量

方向矢量o：y軸設(shè)在兩手指的連線方向，稱為方位矢量

法線矢量n：x軸由右手系確定，即n=o

a

，稱為法向矢量。手爪坐標(biāo)系因此，變換T6具有下列元素。

六連桿機(jī)械手的T矩陣（T6）可由指定其16個元素的數(shù)值來決定。在這16個元素中，只有12個元素具有實際含義。

機(jī)械手由一串用轉(zhuǎn)動或平移關(guān)節(jié)連接的剛體(桿件)組成。每一對關(guān)節(jié)–桿件構(gòu)成一個自由度。桿件的編號由手臂的固定基座開始，固定基座可看成桿件0，第一個運動體是桿件1，依次類推，最后一個桿件與工具相連；關(guān)節(jié)1處于連接桿件1和基座之間，每個桿件至多與另外兩個桿件相聯(lián)，而不構(gòu)成閉環(huán)。桿件i的長度ai，是桿件上兩個關(guān)節(jié)軸線的最短距離；桿件i的扭轉(zhuǎn)角αi

，是兩個關(guān)節(jié)軸線的夾角。

3.1.1Denavt-Hartenberg(D-H)表示法AiAi+1任何桿件i都可以用兩個尺度表征，如上圖所示，AiAi-1兩個桿件的相對位置由兩個參數(shù)決定：兩桿間的距離di：關(guān)節(jié)軸上兩個法線的距離夾角θi：關(guān)節(jié)軸上兩個法線的夾角

為描述相鄰桿件間平移和轉(zhuǎn)動的關(guān)系。Denavt和Hartenberg(1955)提出了一種為關(guān)節(jié)鏈中的每一桿件建立附體坐標(biāo)系的矩陣方法。D-H方法是為每個關(guān)節(jié)處的桿件坐標(biāo)系建立44齊次變換矩陣，表示它與前一桿件坐標(biāo)系的關(guān)系。這樣逐次變換，用“手部坐標(biāo)”表示的末端執(zhí)行器可被變換并用機(jī)座坐標(biāo)表示。坐標(biāo)系的建立有兩種方式：固聯(lián)坐標(biāo)系后置固聯(lián)坐標(biāo)系前置3.1.1Denavt-Hartenberg(D-H)表示法

n關(guān)節(jié)機(jī)器人需建立n+1個坐標(biāo)系，其中參考(機(jī)座)坐標(biāo)系為O0x0y0z0，機(jī)械手末端的坐標(biāo)系為Onxnynzn，第i關(guān)節(jié)上的坐標(biāo)系為Oi-1xi-1yi-1zi-1。坐標(biāo)系Si置于連桿Li的遠(yuǎn)離基座的關(guān)節(jié)上，故稱固聯(lián)坐標(biāo)系后置。確定和建立每個坐標(biāo)系應(yīng)根據(jù)下面3條規(guī)則:

①zi-1軸沿著第i關(guān)節(jié)的運動軸；②xi軸垂直于zi-1軸和zi軸并指向離開zi-1軸的方向③yi軸按右手坐標(biāo)系的要求建立。

按照這些規(guī)則，第0號坐標(biāo)系在機(jī)座上的位置和方向可任選，只要z0軸沿著第i關(guān)節(jié)運動軸。第n坐標(biāo)系可放在手的任何部位、只要xn軸與zn-1軸垂直。固聯(lián)坐標(biāo)系后置轉(zhuǎn)動關(guān)節(jié)連桿四參數(shù)示意圖9機(jī)器人機(jī)械手上坐標(biāo)系的配置取決于機(jī)械手連桿連接的類型。有兩種連接——轉(zhuǎn)動關(guān)節(jié)和棱柱聯(lián)軸節(jié)。現(xiàn)在來考慮棱柱聯(lián)軸節(jié)（平動關(guān)節(jié)）的情況。下圖示出其特征參數(shù)。棱柱關(guān)節(jié)連桿四參數(shù)示意圖3.1.2幾何參數(shù)定義θi：繞zi-1軸（右手規(guī)則）由xi-1軸向xi軸的關(guān)節(jié)角；di：從第i—1坐標(biāo)系的原點到zi-1軸和xi軸的交點沿zi-1軸的距離；ai：從zi-1和xi軸的交點到第i

根據(jù)上述對桿件參數(shù)及坐標(biāo)系的定義，描述串聯(lián)機(jī)器人相鄰坐標(biāo)系之間的關(guān)節(jié)關(guān)系可歸結(jié)如下4個參數(shù)：坐標(biāo)系原點沿xi軸的偏移距離(是zi-1軸和zi兩軸間的最小距離）i

：繞xi軸（右手規(guī)則）由zi-1軸轉(zhuǎn)向zi軸的偏角。

對于轉(zhuǎn)動關(guān)節(jié)，di、ai、αi是關(guān)節(jié)參數(shù)，θi是關(guān)節(jié)變量。移動關(guān)節(jié)的關(guān)節(jié)參數(shù)是θi、ai、αi，di是關(guān)節(jié)變量。3.1.2幾何參數(shù)定義

將第i個坐標(biāo)系表示的點ri在i-1坐標(biāo)系表示，需建立i坐標(biāo)系和i—1坐標(biāo)系的齊次變換矩陣，需經(jīng)過以下交換：(1)將坐標(biāo)系Oi-1xi-1yi-1zi-1繞zi-1軸轉(zhuǎn)θi角，使xi-1軸與xi平行并指向同一方向；(2)將坐標(biāo)系Oi-1xi-1yi-1zi-1沿zi-1軸平移距離di

，使xi-1軸與Oixiyizi的xi軸重合；(3)將坐標(biāo)系Oi-1xi-1yi-1zi-1沿xi軸平移距離ai

，使兩坐標(biāo)系的原點重合；(4)將坐標(biāo)系Oi-1xi-1yi-1zi-1繞xi軸轉(zhuǎn)αi角，使兩坐標(biāo)系完全重合。3.1.3建立i坐標(biāo)系和i-1坐標(biāo)系的齊次變換矩陣

i坐標(biāo)系和i—l坐標(biāo)系的齊次變換矩陣i-1Ai可以根據(jù)矩陣的合成規(guī)則得到，i-1Ai稱為相鄰坐標(biāo)系i和i—1的D-H變換矩陣。即固聯(lián)坐標(biāo)系后置（zi位于i+1關(guān)節(jié)軸上），變換公式對于在第i坐標(biāo)系中的點ri在第i—1坐標(biāo)系中表示為:

確定第i坐標(biāo)系相對于機(jī)座坐標(biāo)系的位置的齊次變換矩陣i-1Ti是各齊次變換矩陣Ai的連乘積，可表示成固聯(lián)坐標(biāo)系前置

連桿Li的固聯(lián)坐標(biāo)系Si的zi軸置于i關(guān)節(jié)的旋轉(zhuǎn)（或移動）軸上，即坐標(biāo)系Si置于連桿Li的靠近基座的關(guān)節(jié)上，故稱固聯(lián)坐標(biāo)系前置。zi-1與zi的公垂線為xi-1,zi與zi+1的公垂線為xi軸,

αi-1:為zi-1與zi的交錯角；αi:繞xi軸（右手規(guī)則）由zi軸轉(zhuǎn)向zi+1軸的偏角；ai-1：從第i-1坐標(biāo)系原點到xi-1軸和zi的交點沿xi-1軸的偏移距離ai：從第i坐標(biāo)系原點到xi軸和zi+1的交點沿xi軸的偏移距離θi

:繞zi軸由xi-1軸向xi軸的關(guān)節(jié)角；di：從xi-1

軸和zi軸的交點到第i坐標(biāo)系的原點沿zi軸的距離固聯(lián)坐標(biāo)系前置（zi位于i關(guān)節(jié)軸上），變換公式

例建立右圖所示機(jī)器人相鄰坐標(biāo)系間的轉(zhuǎn)換矩陣解：建立的坐標(biāo)系如右圖，這是二維坐標(biāo)系(在三維空間中，各坐標(biāo)系的z軸垂直于紙面)，其相鄰坐標(biāo)系的變換矩陣為式中，c12=cos(1+2)，s12=sin(1+2)容易驗證上式的正確性，即：末端位置為[；]T,姿態(tài)為1+2

；例建立下圖所示PUMA機(jī)器人相鄰坐標(biāo)系間的轉(zhuǎn)換矩陣。

第三章機(jī)器人運動學(xué)PUMA560是屬于關(guān)節(jié)式機(jī)器人，6個關(guān)節(jié)都是轉(zhuǎn)動關(guān)節(jié)。前3個關(guān)節(jié)確定手腕參考點的位置，后3個關(guān)節(jié)確定手腕的方位。PUMA機(jī)器人的連桿及關(guān)節(jié)參數(shù)表

第三章機(jī)器人運動學(xué)連桿iθi

αi-1

ai-1

di1θ1(90°)0°002θ2(0°)-90°0d23θ3(-90°)0°a204θ4(0°)-90°a3d45θ5(0°)90°006θ6(0°)-90°00式中：ci=cosisi=sini[例]確定下圖所示機(jī)器人的位置和姿態(tài)解：用D—H法建立坐標(biāo)系轉(zhuǎn)換矩陣，首先列出各連軒及關(guān)節(jié)參數(shù)，如下表所示。斯坦福機(jī)器人及其坐標(biāo)系圖斯坦福機(jī)器人的連桿及關(guān)節(jié)參數(shù)表

機(jī)器人運動學(xué)正問題是已知機(jī)器人各關(guān)節(jié)、各連桿參數(shù)及各關(guān)節(jié)變量，求機(jī)器人手端坐標(biāo)在基礎(chǔ)坐標(biāo)中的位置和姿態(tài)?！?/p>

3.2機(jī)器人運動學(xué)正問題將表中的參數(shù)分別代入i坐標(biāo)系和i—l坐標(biāo)系的齊次變換矩陣i-1Ai可得如下變換矩陣：由手端坐標(biāo)逐一向基礎(chǔ)坐標(biāo)變換，其過程如下nx=c1[c2(c4c5c6-s4s6)-s2s5c6]-s1(s4c5c6+c4s6)ny=s1[c2(c4c5c6-s4s6)-s2s5c6]+c1(s4c5c6+c4s6)nz=-s2(c4c5c6-s4s6)-c2s5c6ox=c1[-c2(c4c5s6+s4c6)+s2s5s6]-s1(-s4c5s6+c4c6)oy=c1[-c2(c4c5s6+s4c6)+s2s5s6]-s1(-s4c5s6+c4c6)oz=s2(c4c5s6+s4c6)+c2s5s6ax=c1(c2c4s5+s2c5)-s1s4s5ay=s1(c2c4s5+s2c5)+c1s4s5az=-s2c4s5+c2c5px=-c1s2d3-s1d2py=s1s2d3+c1d2pz=c2d3§

3.2機(jī)器人運動學(xué)正問題

機(jī)器人運動學(xué)逆問題，是已知滿足某工作要求時末端執(zhí)行器的位置和姿態(tài)，以及各連桿的結(jié)構(gòu)參數(shù)，求關(guān)節(jié)變量。對于通用機(jī)器人，求解各關(guān)節(jié)相應(yīng)位置的工作由機(jī)器人系統(tǒng)程序完成。目前，已經(jīng)能夠?qū)σ话憬Y(jié)構(gòu)的六自由度串聯(lián)機(jī)器人進(jìn)行逆運動學(xué)求解。但是，要獲得顯式解，只有滿足下列兩個充分條件之一：

(1)3個相鄰關(guān)節(jié)軸交于一點；

(2)3個相鄰關(guān)節(jié)軸平行?！?.3機(jī)器人運動學(xué)逆問題nx=c1[c2(c4c5c6-s4s6)-s2s5c6]-s1(s4c5c6+c4s6)ny=s1[c2(c4c5c6-s4s6)-s2s5c6]+c1(s4c5c6+c4s6)nz=-s2(c4c5c6-s4s6)-c2s5c6ox=c1[-c2(c4c5s6+s4c6)+s2s5s6]-s1(-s4c5s6+c4c6)oy=c1[-c2(c4c5s6+s4c6)+s2s5s6]-s1(-s4c5s6+c4c6)oz=s2(c4c5s6+s4c6)+c2s5s6

ax=c1(c2c4s5+s2c5)-s1s4s5ay=s1(c2c4s5+s2c5)+c1s4s5az=-s2c4s5+c2c5px=-c1s2d3-s1d2py=s1s2d3+c1d2pz=c2d3

（1）

[例]

已知上圖所示機(jī)器人位置和姿態(tài)，即已知式①矩陣中各元素的值，試確定機(jī)器人各關(guān)節(jié)變量。

解：用左乘式（1）得（2）方程式（2）的左端為（3）將它表示為其中（4）（4）中3行4列元素為常數(shù)，利用式（3）對應(yīng)元素的相等關(guān)系可得即為了解此類方程，作如下三角代換式中（5）將（5）代入（4）得即簡化為由于0d2/r1，說明角度（-1）在0-范圍內(nèi)：0（-1）求出1

：式中，正負(fù)號對應(yīng)于1的兩個可能解。

根據(jù)機(jī)器人運動連續(xù)性及回避障礙的需要，確定一個1

，從而式(2)左邊已知。由式(3)的1行4列及2行4列和式(2)對應(yīng)元素相等，列出由于d30，可求出2

可求出d3

解：用依次左乘式（2）得以下4個方程式：（6）（9）（8）（7）計算式(8)得（10）式中

由式(10)第3行3列為0，可得：即可求出

由式(10)第1行3列和第2行3列，可得：可求出5

由式(9)可得：

類似有：可求出6

利用雅可比矩陣可以建立起機(jī)器人手端在基礎(chǔ)坐標(biāo)中的速度與各關(guān)節(jié)速度間的關(guān)系，以及手部與外界接觸力與對應(yīng)各關(guān)節(jié)力間的關(guān)系，因此機(jī)器人雅可比矩陣在機(jī)器人技術(shù)中占有重要地位。3.4.1雅可比矩陣的定義對于一個n自由度機(jī)器人，其關(guān)節(jié)變量向量可寫為§3.4機(jī)器人的雅可比矩陣設(shè)機(jī)器人手部在基礎(chǔ)坐標(biāo)中的位置和姿態(tài)為P，則其中前3個元素表示位置，后3個元素表示姿態(tài)。它們都是n個關(guān)節(jié)變量的函數(shù)，也可寫為：為了求手部在基礎(chǔ)坐標(biāo)中的速度，可對上式求導(dǎo)(3.4-1)(3.4-2)(3.4-3)§3.4機(jī)器人的雅可比矩陣簡寫成(3.4-4)式(3.4-3)或式(3.4-4)表示手部在基礎(chǔ)坐標(biāo)中的速度與關(guān)節(jié)速度間的關(guān)系，聯(lián)結(jié)它們的紐帶為矩陣J，稱其為雅可比矩陣，它的展開式為(3.4-5)式(3.4-5)為機(jī)器人運動學(xué)正問題，即已知各關(guān)節(jié)速度求手端速度。3.4.2雅可比矩陣的矢量積求法

手部速度的前3個元素表示手的線速度，后3個元素表示手的角速度，所以將寫成分塊形式(3.4-6)其中JLi和JAi分別表示第i個關(guān)節(jié)變量引起的三維線速度系數(shù)和三維角速度系數(shù)。只要求JLi和JAi(i＝l，2，…，n)，即可確定雅可比矩陣J。3.4.2雅可比矩陣的矢量積求法3.4.2.1JLi的求法(1)第i個關(guān)節(jié)為移動關(guān)節(jié)時，qi＝di，，如左圖所示

設(shè)某時刻僅此關(guān)節(jié)運動，其余的關(guān)節(jié)靜止不動，由式（3.4-6)可得（3.4-7)

設(shè)bi-1為zi-1軸上的單位矢量，利用它可將局部坐標(biāo)下的平移速度di轉(zhuǎn)換成基礎(chǔ)坐標(biāo)下的速度：（3.4-8)比較式(3.4-7)或式(3.4-8)，并注意到，可得僅平移關(guān)節(jié)產(chǎn)生的線速度（3.4-9)3.4.2雅可比矩陣的矢量積求法(2)第i個關(guān)節(jié)為轉(zhuǎn)動關(guān)節(jié)時，，如下圖所示

設(shè)某時刻僅此關(guān)節(jié)運動，其余的關(guān)節(jié)靜止不動，利用bi-1將zi-1軸上的角速度轉(zhuǎn)化到基礎(chǔ)坐標(biāo)中僅旋轉(zhuǎn)關(guān)節(jié)產(chǎn)生的線速度而左圖中的矢量ri-1,e起于oi-1，止于on，所以由ωi產(chǎn)生的線速度為：所以（3.4-10)（3.4-11)把式(3.4-10)代入式(3.4-11)得:因，所以（3.4-12)3.4.2雅可比矩陣的矢量積求法3.4.2.2JAi的求法(1)第i個關(guān)節(jié)為移動關(guān)節(jié)時，qi＝di，，由于移動關(guān)節(jié)的平移不對手部產(chǎn)生角速度，所以此時

JAi=0(2)第i個關(guān)節(jié)為轉(zhuǎn)動關(guān)節(jié)時，，由式（3.4-10)得：所以此時（3.4-13)（3.4-14)綜合式（3.4-9)、（3.4-12)、（3.4-13)、（3.4-14)得第i個關(guān)節(jié)為移動關(guān)節(jié)時第i個關(guān)節(jié)為轉(zhuǎn)動關(guān)節(jié)時（3.4-15)（3.4-16)3.4.2雅可比矩陣的矢量積求法3.4.2.3求bi-1和ri-1,e用b表示zi-1軸上的單位向量把它轉(zhuǎn)換在基礎(chǔ)坐標(biāo)系中，即為向量間的關(guān)系圖如左圖所示，用O、Oi-1、On分別表示基礎(chǔ)坐標(biāo)系、i-l號坐標(biāo)及手部坐標(biāo)系的原點。用矢量表示在各自坐標(biāo)系中的原點。把ri-1,e用齊次坐標(biāo)表示，令所以（3.4-17)（3.4-18)由式(3.4-18)可方便地確定ri-1,e3.4.2雅可比矩陣的矢量積求法3.4.3.1機(jī)器人的微分運動微分平移的齊次變換為：通用旋轉(zhuǎn)變換矩陣3.4.3雅可比矩陣的微分變換求法微分變化相對基系進(jìn)行微分變化相對坐標(biāo)系T進(jìn)行其中當(dāng)微分運動相對于基系進(jìn)行時為當(dāng)微分運動相對于坐標(biāo)系{T}進(jìn)行時為當(dāng)對基系進(jìn)行微分變化時當(dāng)對坐標(biāo)系{T}進(jìn)行微分變化時3.4.3雅可比矩陣的微分變換求法繞矢量

旋轉(zhuǎn)等價于分別繞坐標(biāo)系的三個軸旋轉(zhuǎn)即代入上式得：3.4.3雅可比矩陣的微分變換求法可把微分平移和旋轉(zhuǎn)變換看成是由微分平移矢量d和微分旋轉(zhuǎn)矢量構(gòu)成的，它們分別為3.4.3雅可比矩陣的微分變換求法

用列矢量D來包含微分平移矢量d和微分旋轉(zhuǎn)矢量，并稱為剛體或坐標(biāo)系的微分運動矢量或或同理有：3.4.3雅可比矩陣的微分變換求法

要求得機(jī)械手的雅可比矩陣，就需要建立不同坐標(biāo)系間的微分變化的變換關(guān)系。當(dāng)兩坐標(biāo)系等價時，有變換后得由等式左邊求出得：由定義有：令上述兩式各元素分別相等，可求出：3.4.3雅可比矩陣的微分變換求法分別為微分坐標(biāo)變換{T}的列矢量。微分運動矢量TD和D的關(guān)系如下：3.4.3雅可比矩陣的微分變換求法3.4.3.2雅可比矩陣的微分變換法3.4.3雅可比矩陣的微分變換求法

對于轉(zhuǎn)動關(guān)節(jié)i，連桿i相對連桿i-1繞坐標(biāo)系{i}的zi軸作微分運動dθi，其微分運動矢量為：代入下式，得手相應(yīng)的微分運動矢量為：3.4.3雅可比矩陣的微分變換求法

對于移動關(guān)節(jié)i，連桿i沿zi軸相對連桿i-1作微分運動ddi，其微分運動矢量為：手相應(yīng)的微分運動矢量為：3.4.3雅可比矩陣的微分變換求法可得雅可比矩陣J的第i列如下：對于轉(zhuǎn)動關(guān)節(jié)i對于移動關(guān)節(jié)i是iTn的四個列矢量。其中3.4.3雅可比矩陣的微分變換求法

上述求雅可比矩陣TJ的方法是構(gòu)造性的，只要知道各連桿變換Ti，就可自動生成雅可比，而不必求解方程。步驟如下：①計算各連桿變換T1，T2，…,Tn②計算各連桿至末端連桿的變換Tn，n-2Tn=

n-2Tn-1Tn，…,I-1Tn=

TiiTn，

…，0Tn=T11Tn.③計算雅可比矩陣TJ的各列元素，第i列TJi由iTn決定。T1T2T3T4T5T65T64T63T62T61T6TJ6TJ5TJ4TJ3TJ2TJ1TJi由iTn之間的關(guān)系§3.4機(jī)器人的雅可比矩陣[例3.4-1]

建立右圖的雅可比矩陣。解可以簡寫為機(jī)械臂末端的速度§3.4.4雅可比矩陣的逆3.4.4雅可比矩陣的逆

對于在三維空間運動的n關(guān)節(jié)機(jī)器人，其雅可比矩陣的階數(shù)為6n。當(dāng)n＝6時，J是66方陣，可直接求其逆。當(dāng)n6時，J不是方陣．此時若用雅可比矩陣的逆就用其偽逆，用J+表示偽逆。（3.4-19)3.4.5雅可比矩陣的應(yīng)用3.4.5.1分離速度控制由可得（3.4-20)

由式(3.4-20)可見，當(dāng)已知手端速度向量，可通過左乘雅可比逆矩陣計算出機(jī)器人的關(guān)節(jié)速度向量，所以式(3.4-20)為運動學(xué)逆問題的速度關(guān)系式，是對機(jī)器人進(jìn)行速度控制的基本關(guān)系式。采用計算機(jī)控制時，把速度表示位置增量的形式，故將式(3.4-20)寫為（3.4