第三章-機器人運動學

第三章機器人運動學§3.1機器人運動方程的表示

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

T2=

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

T3=

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

T6=

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

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

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

a

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

連桿Li的固聯(lián)坐標系Si的zi軸置于i關節(jié)的旋轉(zhuǎn)（或移動）軸上，即坐標系Si置于連桿Li的靠近基座的關節(jié)上，故稱固聯(lián)坐標系前置。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坐標系原點到xi-1軸和zi的交點沿xi-1軸的偏移距離ai：從第i坐標系原點到xi軸和zi+1的交點沿xi軸的偏移距離θi

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

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

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

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

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

第三章機器人運動學連桿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[例]確定下圖所示機器人的位置和姿態(tài)解：用D—H法建立坐標系轉(zhuǎn)換矩陣，首先列出各連軒及關節(jié)參數(shù)，如下表所示。斯坦福機器人及其坐標系圖斯坦福機器人的連桿及關節(jié)參數(shù)表

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

3.2機器人運動學正問題將表中的參數(shù)分別代入i坐標系和i—l坐標系的齊次變換矩陣i-1Ai可得如下變換矩陣：由手端坐標逐一向基礎坐標變換，其過程如下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機器人運動學正問題

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

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

(2)3個相鄰關節(jié)軸平行?！?.3機器人運動學逆問題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）

[例]

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

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

：式中，正負號對應于1的兩個可能解。

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

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

可求出d3

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

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

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

由式(9)可得：

類似有：可求出6

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

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

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

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

設某時刻僅此關節(jié)運動，其余的關節(jié)靜止不動，利用bi-1將zi-1軸上的角速度轉(zhuǎn)化到基礎坐標中僅旋轉(zhuǎ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個關節(jié)為移動關節(jié)時，qi＝di，，由于移動關節(jié)的平移不對手部產(chǎn)生角速度，所以此時

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

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

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

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

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

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

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

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

TiiTn，

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

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

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

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