工業(yè)機器人運動學(xué)

工業(yè)機器人運動學(xué)第一頁，共五十四頁，2022年，8月28日第四章工業(yè)機器人運動學(xué)

4.1齊次坐標(biāo)及對象物的描述4.2齊次變換及運算4.3工業(yè)機器人連桿參數(shù)及其齊次變換矩陣4.4工業(yè)機器人運動學(xué)方程第二頁，共五十四頁，2022年，8月28日2023/2/42機器人運動學(xué)要研究的主要問題機器人實際上可以認(rèn)為是由一系列關(guān)節(jié)聯(lián)接起來的連桿所組成。我們把坐標(biāo)系固聯(lián)在機器人的每個關(guān)節(jié)上，可以用齊次變換來描述這些坐標(biāo)系之間的相對位置和方向。機器人運動學(xué)要研究的問題是：（1）正向運動學(xué)問題-運動分析已知各個關(guān)節(jié)和連桿的參數(shù)和運動變量，求解末端執(zhí)行器（手部）的位姿。（2）反向運動學(xué)問題-運動綜合在已知末端執(zhí)行器（手部）要到達的目標(biāo)位姿的情況下，如何求解各個關(guān)節(jié)的運動變量。第三頁，共五十四頁，2022年，8月28日2023/2/434.1齊次坐標(biāo)及對象物的描述4.1.1點的位置描述在選定的直角坐標(biāo)系{A}中，空間任一點P的位置可用3×1的位置矢量AP表示，其左上角代表選定的坐標(biāo)系式中：Px

Py

Pz是點P在坐標(biāo)系{A}中的三個位置分量。如圖所示。第四頁，共五十四頁，2022年，8月28日2023/2/444.1.2齊次坐標(biāo)如果用四個數(shù)組成的（4×1）列陣式中：Px

Py

Pz是點P在坐標(biāo)系{A}中的三個位置分量。如圖所示。表示三維空間直角坐標(biāo)系{A}中點P，則列陣[Px

Py

Pz1]T稱為三維空間點P的齊次坐標(biāo)。齊次坐標(biāo)將一個原本是n維的向量用一個n+1維向量來表示。說明：齊次坐標(biāo)的表示不是唯一的。我們將其各元素同乘一非零因子w后，仍然代表同一點P，即式中：a=wpx;b=wpy;c=wpz第五頁，共五十四頁，2022年，8月28日2023/2/454.1.3坐標(biāo)軸方向的描述如圖所示，i,j,

k分別是直角坐標(biāo)系中X、Y、Z坐標(biāo)軸的單位向量。若用齊次坐標(biāo)來描述X、Y、Z軸的方向，則即規(guī)定：（1）若（4×1）列陣[abc

w]T中第四個元素為零，且a2+b2+c2=1，則表示某軸(矢量）的方向，其中abc是該軸的單位向量在各坐標(biāo)軸上的分量。（2）若（4×1）列陣[abc

w]T中第四個元素不為零，則表示空間某點的位置。如原點位置表示為v＝[0001]T。第六頁，共五十四頁，2022年，8月28日2023/2/46類似地，一個空間矢量的方向表示也用（4×1）列陣可表達為

v＝[abc

0]T

其中abc是單位方向矢量在各坐標(biāo)軸上的分量，即該單位向量的方向余弦。

a＝cosα，b＝cosβ，c＝cosγ第七頁，共五十四頁，2022年，8月28日2023/2/47例4-1

用齊次坐標(biāo)寫出圖中矢量u、v、w的方向列陣。解：矢量u：cosα＝0，cosβ＝0.707，cosγ＝0.707

u=[00.7070.7070]T

矢量v：cosα＝0.707，cosβ＝0，cosγ＝0.707

v=[0.70700.7070]T

矢量w：cosα＝0.5，cosβ＝0.5，cosγ＝0.707

w=[0.50.50.7070]T第八頁，共五十四頁，2022年，8月28日2023/2/484.1.4動坐標(biāo)系位姿的描述在機器人坐標(biāo)系中，當(dāng)連桿運動時，位置和姿態(tài)固定不變的坐標(biāo)系稱為固定坐標(biāo)系或靜系；跟隨連桿運動的坐標(biāo)系稱為動坐標(biāo)系或簡稱動系。動坐標(biāo)系位姿的描述就是對動坐標(biāo)系原點位置的描述和對動坐標(biāo)系各坐標(biāo)軸方向的描述。設(shè)有一動坐標(biāo)系O’X’Y’Z’原點為O’，該坐標(biāo)系在固定坐標(biāo)系OXYZ中的位置可用齊次坐標(biāo)形式的一個（4×1）列陣表示：第九頁，共五十四頁，2022年，8月28日2023/2/49

令n、o、a分別為X’Y’Z’坐標(biāo)軸的單位方向矢量，則每個單位方向矢量在固定坐標(biāo)系中各坐標(biāo)軸上的分量為動坐標(biāo)系各坐標(biāo)軸的方向余弦，用齊次坐標(biāo)形式的（4×1）列陣分別表示為：

n＝[nx

ny

nz0]T，

o＝[ox

oy

oz0]T，

a＝[ax

ay

az0]T

因此，圖中動坐標(biāo)系的位姿可用下面的（4×4）矩陣來描述：第十頁，共五十四頁，2022年，8月28日2023/2/4101、剛體位姿的描述在剛體上的任一點建立一個動坐標(biāo)系，該動坐標(biāo)系與剛體固聯(lián)在一起，則對剛體位姿的描述就是對動坐標(biāo)系原點位置的描述以及對動坐標(biāo)系各坐標(biāo)軸方向的描述。也就是說，對剛體Q位姿的描述就是對固聯(lián)在剛體Q的坐標(biāo)系O’X’Y’Z’位姿的描述。第十一頁，共五十四頁，2022年，8月28日2023/2/411例4-2

下圖表示固聯(lián)于剛體的坐標(biāo)系{B}位于OB點，xb=10,yb=5,zb=0。ZB軸與畫面垂直，坐標(biāo)軸{B}相對固定坐標(biāo)系{A}有一個30°的偏轉(zhuǎn)，試寫出表示剛體位姿的坐標(biāo)系{B}的（4×4）的矩陣表達式。解：XB的方向列陣：

n＝[cos30°cos60°cos90°0]T

＝[0.8660.5000.0000]T

YB的方向列陣：

o＝[cos120°cos30°cos90°0]T

＝[-0.5000.8660.0000]T

ZB的方向列陣：

a＝[cos90°cos90°cos0°0]T

=[0.0000.0001.0000]T

坐標(biāo)系{B}的位置陣列：

p＝[10.05.00.01]T第十二頁，共五十四頁，2022年，8月28日2023/2/412坐標(biāo)系{B}的位姿為：第十三頁，共五十四頁，2022年，8月28日2023/2/4132、手部位置和姿態(tài)的表示機器人手部的位置和姿態(tài)也可以用固聯(lián)在手部的坐標(biāo)系{B}的位姿來表示，如圖所示。坐標(biāo)系{B}可以這樣來確定：取手部的中心點為原點OB；關(guān)節(jié)軸為ZB軸，ZB軸的單位方向矢量a稱為接近矢量，指向朝外；二手指的連線為YB軸，YB軸的單位方向矢量o稱為姿態(tài)（方向）矢量，指向可任意選定；XB軸與YB軸及ZB軸相垂直，指向符合右手法則，XB軸的單位方向矢量n稱為法向（線）矢量。第十四頁，共五十四頁，2022年，8月28日2023/2/414手部的位置矢量為固定參考系原點指向手部坐標(biāo)系{B}原點的矢量p，手部的方向矢量為n、o、a。于是手部的位姿可用（4×4）矩陣表示為第十五頁，共五十四頁，2022年，8月28日2023/2/415例4-3

圖表示手部抓握物體Q，物體為邊長2個單位的正立方體，寫出表達該手部位姿的矩陣式。解：因為物體Q形心與手部坐標(biāo)系O’X’Y’Z’的坐標(biāo)原點Q’相重合，所以手部位置的（4×1）列陣為：P＝[1111]T手部坐標(biāo)系X’軸的方向可用單位矢量n來表示：n：α＝90°β＝180°γ＝90°

nx＝cosα＝0

ny＝cosβ＝－1

nz＝cosγ＝0

n=[0-100]T第十六頁，共五十四頁，2022年，8月28日2023/2/416

同理，手部坐標(biāo)系Y’軸與Z’軸的方向可分別用單位矢量o和a表示：

o：

ox＝-1oy＝0oz＝0

a：

ax＝0ay＝0az＝-1則手部位姿可用矩陣表達為第十七頁，共五十四頁，2022年，8月28日2023/2/4174.2齊次變換及運算

剛體的運動是由平移和轉(zhuǎn)動組成的。為了能用同一矩陣表示轉(zhuǎn)動和平移，有必要引入（4×4）的齊次坐標(biāo)變換矩陣。4.2.1平移的齊次變換首先，介紹點在空間直角坐標(biāo)系中的平移。如圖所示，空間某一點A，坐標(biāo)為（x,y,z），當(dāng)它平移至A’點后，坐標(biāo)為（x’,y’,z’），其中：第十八頁，共五十四頁，2022年，8月28日2023/2/418可用右面形式表示：也可以簡寫為式中，表示齊次坐標(biāo)變換的平移算子，且記?。喝羲阕幼蟪?，表示坐標(biāo)系變換是相對固定坐標(biāo)系進行的，假如相對動坐標(biāo)系進行坐標(biāo)變換，則算子應(yīng)該右乘。第十九頁，共五十四頁，2022年，8月28日2023/2/419例4-4如圖所示，動坐標(biāo)系{A}相對于固定坐標(biāo)系的X0Y0Z0軸作（-1,2,2）平移后到{A’};動坐標(biāo)系{A}相對于自身坐標(biāo)系（即動系）的X、Y、Z軸分別作（-1,2,2）平移后到{A”};物體Q相對于固定坐標(biāo)系作(2,6,0)平移后到Q’。已知：寫出{A’}、{A”}以及物體Q’的矩陣表達式。第二十頁，共五十四頁，2022年，8月28日2023/2/420第二十一頁，共五十四頁，2022年，8月28日2023/2/421解：動坐標(biāo)系{A}的兩個平移坐標(biāo)變換算子均為{A’}坐標(biāo)系是動系{A}沿固定坐標(biāo)系作平移變換得來的，因此算子左乘，{A’}的矩陣表達式為第二十二頁，共五十四頁，2022年，8月28日2023/2/422

{A”}坐標(biāo)系是動系{A}沿自身坐標(biāo)系作平移變換得來的，因此算子右乘，{A”}的矩陣表達式為第二十三頁，共五十四頁，2022年，8月28日2023/2/423物體Q的齊次坐標(biāo)變換平移算子為故有第二十四頁，共五十四頁，2022年，8月28日2023/2/4244.2.2旋轉(zhuǎn)的齊次變換首先，介紹點在空間直角坐標(biāo)系中的旋轉(zhuǎn)。如圖所示，空間某一點A，坐標(biāo)為（x,y,z），當(dāng)它繞Z軸旋轉(zhuǎn)θ角后至A’點，坐標(biāo)為（x’,y’,z’），A’點和A點的坐標(biāo)關(guān)系為或用矩陣表示為：推導(dǎo)？第二十五頁，共五十四頁，2022年，8月28日2023/2/425返回第二十六頁，共五十四頁，2022年，8月28日2023/2/426用齊次坐標(biāo)表示A點繞Z軸的旋轉(zhuǎn)變換過程為式中：表示齊次坐標(biāo)變換時繞Z軸的旋轉(zhuǎn)算子，算子左乘表示相對于固定坐標(biāo)系進行變換，算子的內(nèi)容為：也可簡寫為式中：第二十七頁，共五十四頁，2022年，8月28日2023/2/427同理，可寫出繞X軸旋轉(zhuǎn)的算子和繞Y軸旋轉(zhuǎn)的算子與平移變換一樣，旋轉(zhuǎn)變換算子不僅適用于點的旋轉(zhuǎn)變換，也適用于矢量、坐標(biāo)系、物體等的旋轉(zhuǎn)變換計算。規(guī)則：若相對固定坐標(biāo)系進行變換，則算子左乘；若相對動坐標(biāo)系進行變換，則算子右乘。第二十八頁，共五十四頁，2022年，8月28日2023/2/428例4-5已知坐標(biāo)系中點U的位置矢量u=[7321]T,將此點繞Z軸旋轉(zhuǎn)90°，再繞Y軸旋轉(zhuǎn)90°，如圖所示，求旋轉(zhuǎn)變換后所得的點W。解：第二十九頁，共五十四頁，2022年，8月28日2023/2/429例4-6如圖所示單臂操作手，手腕也具有一個自由度。已知手部起始位姿矩陣為若手臂繞Z0軸旋轉(zhuǎn)＋90°，則手部到達G2點；若手臂不動，僅手部繞手腕Z1軸轉(zhuǎn)＋90°，則手部到達G3點。寫出手部坐標(biāo)系{G2}、{G3}的矩陣表達式。第三十頁，共五十四頁，2022年，8月28日2023/2/430解：手臂繞定軸轉(zhuǎn)動是相對固定坐標(biāo)系作旋轉(zhuǎn)變換，所以第三十一頁，共五十四頁，2022年，8月28日2023/2/431手部繞手腕軸旋轉(zhuǎn)是相對動坐標(biāo)系作旋轉(zhuǎn)變換，所以第三十二頁，共五十四頁，2022年，8月28日2023/2/432點在空間直角坐標(biāo)系中繞過原點任意軸的一般旋轉(zhuǎn)變換圖所示為點A繞任意過原點的單位矢量k旋轉(zhuǎn)θ角的情況，kX、kY、kZ分別為k矢量在固定參考系坐標(biāo)軸X、Y、Z上的三個分量，且可以證得，繞任意過原點的單位矢量k轉(zhuǎn)θ角的旋轉(zhuǎn)算子為式中：第三十三頁，共五十四頁，2022年，8月28日2023/2/433上式稱為一般旋轉(zhuǎn)齊次變換通式，它概括了繞X軸、Y軸及Z軸進行旋轉(zhuǎn)齊次變換的各種特殊情況，例如：當(dāng)kX=1，即kY=kZ=0時，則由上式可得到繞X軸的旋轉(zhuǎn)算子；當(dāng)kY=1，即kX=kZ=0時，則由上式可得到繞Y軸的旋轉(zhuǎn)算子；當(dāng)kZ=1，即kX=kY=0時，則由上式可得到繞Z軸的旋轉(zhuǎn)算子。式中：第三十四頁，共五十四頁，2022年，8月28日2023/2/4344.2.3平移加旋轉(zhuǎn)的齊次變換平移變換和旋轉(zhuǎn)變換可以組合在一個齊次變換中，如例4-5中點W繞Z轉(zhuǎn)90°繞Y軸轉(zhuǎn)90°后若還要作4i-3j+7k的平移，則如圖所示，只要左乘上平移變換算子即可得到最后E點的列陣表達式第三十五頁，共五十四頁，2022年，8月28日2023/2/4354.3工業(yè)機器人連桿參數(shù)及其齊次變換矩陣4.3.1機器人坐標(biāo)系的分配順序：按從機座到末端執(zhí)行器的順序，由低到高依次為各關(guān)節(jié)和各連桿編號。連桿編號：機座的編號為連桿0，與機座相連的連桿編號為連桿1，依此類推。第三十六頁，共五十四頁，2022年，8月28日2023/2/436關(guān)節(jié)編號：機座與連桿1的關(guān)節(jié)編號為關(guān)節(jié)1，連桿1與連桿2的連接關(guān)節(jié)編號為2，依此類推。坐標(biāo)系分配：在每一個連桿上建立一個坐標(biāo)系，該坐標(biāo)系的Z軸與連桿末端關(guān)節(jié)的軸線重合。第三十七頁，共五十四頁，2022年，8月28日2023/2/4374.3.2連桿參數(shù)及連桿坐標(biāo)系的建立1、連桿參數(shù)連桿參數(shù)包括連桿尺寸參數(shù)和連桿關(guān)系參數(shù)兩組。1）連桿尺寸參數(shù)連桿i兩端有關(guān)節(jié)i和i+1。該連桿尺寸可以用兩個量來描述：連桿長度ai

：兩個關(guān)節(jié)軸線沿公垂線的距離（恒正）連桿扭角αi

：垂直于ai的平面內(nèi)兩個軸線的夾角（有正負(fù)，方向i到i+1）第三十八頁，共五十四頁，2022年，8月28日2023/2/438

2）連桿關(guān)系參數(shù)連桿i-1和連桿i通過關(guān)節(jié)i相連。其相對位置可以用兩個參數(shù)來描述：連桿間距離di：沿關(guān)節(jié)i軸線兩個公垂線的距離連桿間轉(zhuǎn)角θi：垂直于關(guān)節(jié)i軸線的平面內(nèi)兩個公垂線的夾角第三十九頁，共五十四頁，2022年，8月28日2023/2/439總結(jié)：每個連桿可以由四個參數(shù)所描述：其中兩個描述連桿尺寸；另外兩個描述連桿與相鄰連桿的聯(lián)接關(guān)系。對于旋轉(zhuǎn)關(guān)節(jié)，θi是關(guān)節(jié)變量，其它三個參數(shù)固定不變；對于移動關(guān)節(jié)，di是關(guān)節(jié)變量，其它三個參數(shù)固定不變。第四十頁，共五十四頁，2022年，8月28日2023/2/4402、連桿坐標(biāo)系連桿i坐標(biāo)系的建立按下面規(guī)則進行：原點：關(guān)節(jié)i軸線和關(guān)節(jié)i+1的軸線的公垂線與關(guān)節(jié)i+1的軸線相交點(兩種情況：1平行；2不平行)

Z軸：與關(guān)節(jié)i+1的軸線重合

X軸：與連桿i的公垂線重合，且方向從關(guān)節(jié)i指向i+1。

Y軸：按右手法則確定第四十一頁，共五十四頁，2022年，8月28日2023/2/4414.3.3連桿坐標(biāo)系間變換矩陣連桿坐標(biāo)系之間的相對關(guān)系可以用坐標(biāo)系之間的平移和旋轉(zhuǎn)來表達。坐標(biāo)系i-1經(jīng)過以下四步變換可與坐標(biāo)系i相重合：1)繞Zi–1軸旋轉(zhuǎn)θi角，使Xi–1軸轉(zhuǎn)到與Xi同一平面內(nèi)。2)沿Zi–1軸平移一距離di，把Xi–1移到與Xi同一直線上。3)沿Xi軸平移一距離ai，把連桿i–1的坐標(biāo)系移動到使其原點與連桿i坐標(biāo)系原點重合的地方（原點和X軸已重合）。4)繞Xi旋轉(zhuǎn)αi角，使Zi–1轉(zhuǎn)到與Zi同一直線上。第四十二頁，共五十四頁，2022年，8月28日2023/2/442可以用變換矩陣Ai來衡量兩個坐標(biāo)系之間的相對位姿。對于一個確定的機器人，它是i或di的函數(shù)。用一個變換矩陣Ai來綜合表示上述四次變換，由于后一次變換都是相對動坐標(biāo)系進行的，因此在運算中變換算子應(yīng)該右乘。第四十三頁，共五十四頁，2022年，8月28日2023/2/4434.4工業(yè)機器人運動學(xué)方程4.4.1機器人運動學(xué)方程將機器人的每一個連桿建立一個坐標(biāo)系，并用齊次變換來描述這些坐標(biāo)系間的相對關(guān)系，也叫相對位姿。通常把描述一個連桿坐標(biāo)系與下一個連桿坐標(biāo)系間相對關(guān)系的齊次變換矩陣叫做A變換矩陣或A矩陣。如果A1矩陣表示第一個連桿坐標(biāo)系相對于固定坐標(biāo)系的位姿，A2矩陣表示第二個連桿坐標(biāo)系相對于第一個連桿坐標(biāo)系的位姿，那么第二個連桿坐標(biāo)系在固定坐標(biāo)系中的位姿可用A1和A2的乘積來表示。

T2=A1A2第四十四頁，共五十四頁，2022年，8月28日2023/2/444同理，若A3矩陣表示第三個連桿坐標(biāo)系相對于第二個連桿坐標(biāo)系的位姿，則有

T3=A1A2A3

如此類推，對于六連桿機器人，有下列T6矩陣：

T6=A1A2A3A4A5A6

等式右邊表示了從固定參考系到手部坐標(biāo)系的各連桿坐標(biāo)系之間的變換矩陣的連乘。結(jié)果為手部坐標(biāo)系相對于固定參考系的位姿。稱該式為機器人運動學(xué)方程。該式的計算結(jié)果T6是一個如下的（4×4）矩陣：式中：前三列表示手部的姿態(tài)，第四列表示手部的位置。第四十五頁，共五十四頁，2022年，8月28日2023/2/4454.4.2正向運動學(xué)及實例正向運動學(xué)主要解決機器人運動學(xué)方程的建立及手部位姿的求解問題，下面結(jié)合實例介紹建立運動學(xué)方程的方法。1、平面關(guān)節(jié)型機器人的運動學(xué)方程右圖所示為具有一個肩關(guān)節(jié)、一個肘關(guān)節(jié)和一個腕關(guān)節(jié)的SCARA機器人。此機器人的機械結(jié)構(gòu)特點是三個關(guān)節(jié)軸線是相互平行的。第四十六頁，共五十四頁，2022年，8月28日2023/2/446固定坐標(biāo)系{0}和連桿1、連桿2、連桿3的坐標(biāo)系{1}、{2}、{3}分別如圖所示，坐落在關(guān)節(jié)1、關(guān)節(jié)2、關(guān)節(jié)3和手部中心、各連桿參數(shù)為：連桿轉(zhuǎn)角（變量）兩連桿間距離d連桿長度a連桿扭角α11010002201000330200第四十七頁，共五十四頁，2022年，8月28日2023/2/447該機器人的運動學(xué)方程為

T3=A1A2A3

式中，A1表示連桿1的坐標(biāo)系{1}相對于固定坐標(biāo)系{0}的齊次變換矩陣；A2表示連桿2的坐標(biāo)系{2}相對于連桿1坐標(biāo)系{1}的齊次變換矩陣；A3表示連桿3的坐標(biāo)系即手部坐標(biāo)系{3}相對于連桿2坐標(biāo)系{2}的齊次變換矩陣。于是有第四十八頁，共五十四頁，2022年，8月28日2023/2/448因此，可以寫出式中：c123＝cos(θ1+θ2+θ3）s123＝sin(θ1+θ2+θ3）

c12＝cos(θ1+θ2）s12＝sin(θ1+θ2）

c1＝cosθ1

s1＝sinθ1

第四十九頁，共五十四頁，2022年，8月28日2023/2/449T3表示手部坐標(biāo)系{3}(即手部）的位置和姿態(tài)。于是可寫出位置（4×1）列陣為第五十頁，共五十四頁，2022年，8月28日2023/2/450表示手部姿態(tài)的方向矢量n、o、a分別為當(dāng)轉(zhuǎn)角變量θ1、θ2、

θ3、給定時，可以算出具體的數(shù)值。設(shè)θ1＝30°θ2＝－60°θ3＝－30°，則可根據(jù)平面關(guān)節(jié)型機器人運動學(xué)方程式求解出運動