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

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

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

Py

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

Py

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

Py

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

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

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

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

v＝[abc

0]T

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

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

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

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

n＝[nx

ny

nz0]T，

o＝[ox

oy

oz0]T，

a＝[ax

ay

az0]T

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

下圖表示固聯(lián)于剛體的坐標系{B}位于OB點，xb=10,yb=5,zb=0。ZB軸與畫面垂直，坐標軸{B}相對固定坐標系{A}有一個30°的偏轉(zhuǎn)，試寫出表示剛體位姿的坐標系{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

坐標系{B}的位置陣列：

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

圖表示手部抓握物體Q，物體為邊長2個單位的正立方體，寫出表達該手部位姿的矩陣式。解：因為物體Q形心與手部坐標系O’X’Y’Z’的坐標原點Q’相重合，所以手部位置的（4×1）列陣為：P＝[1111]T手部坐標系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

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

{A”}坐標系是動系{A}沿自身坐標系作平移變換得來的，因此算子右乘，{A”}的矩陣表達式為第二十三頁，共五十四頁，2022年，8月28日2023/2/423物體Q的齊次坐標變換平移算子為故有第二十四頁，共五十四頁，2022年，8月28日2023/2/4244.2.2旋轉(zhuǎn)的齊次變換首先，介紹點在空間直角坐標系中的旋轉(zhuǎn)。如圖所示，空間某一點A，坐標為（x,y,z），當(dāng)它繞Z軸旋轉(zhuǎn)θ角后至A’點，坐標為（x’,y’,z’），A’點和A點的坐標關(guān)系為或用矩陣表示為：推導(dǎo)？第二十五頁，共五十四頁，2022年，8月28日2023/2/425返回第二十六頁，共五十四頁，2022年，8月28日2023/2/426用齊次坐標表示A點繞Z軸的旋轉(zhuǎn)變換過程為式中：表示齊次坐標變換時繞Z軸的旋轉(zhuǎn)算子，算子左乘表示相對于固定坐標系進行變換，算子的內(nèi)容為：也可簡寫為式中：第二十七頁，共五十四頁，2022年，8月28日2023/2/427同理，可寫出繞X軸旋轉(zhuǎn)的算子和繞Y軸旋轉(zhuǎn)的算子與平移變換一樣，旋轉(zhuǎn)變換算子不僅適用于點的旋轉(zhuǎn)變換，也適用于矢量、坐標系、物體等的旋轉(zhuǎn)變換計算。規(guī)則：若相對固定坐標系進行變換，則算子左乘；若相對動坐標系進行變換，則算子右乘。第二十八頁，共五十四頁，2022年，8月28日2023/2/428例4-5已知坐標系中點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點。寫出手部坐標系{G2}、{G3}的矩陣表達式。第三十頁，共五十四頁，2022年，8月28日2023/2/430解：手臂繞定軸轉(zhuǎn)動是相對固定坐標系作旋轉(zhuǎn)變換，所以第三十一頁，共五十四頁，2022年，8月28日2023/2/431手部繞手腕軸旋轉(zhuǎn)是相對動坐標系作旋轉(zhuǎn)變換，所以第三十二頁，共五十四頁，2022年，8月28日2023/2/432點在空間直角坐標系中繞過原點任意軸的一般旋轉(zhuǎn)變換圖所示為點A繞任意過原點的單位矢量k旋轉(zhuǎn)θ角的情況，kX、kY、kZ分別為k矢量在固定參考系坐標軸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機器人坐標系的分配順序：按從機座到末端執(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，依此類推。坐標系分配：在每一個連桿上建立一個坐標系，該坐標系的Z軸與連桿末端關(guān)節(jié)的軸線重合。第三十七頁，共五十四頁，2022年，8月28日2023/2/4374.3.2連桿參數(shù)及連桿坐標系的建立1、連桿參數(shù)連桿參數(shù)包括連桿尺寸參數(shù)和連桿關(guān)系參數(shù)兩組。1）連桿尺寸參數(shù)連桿i兩端有關(guān)節(jié)i和i+1。該連桿尺寸可以用兩個量來描述：連桿長度ai

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

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

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

T3=A1A2A3

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

T6=A1A2A3A4A5A6

等式右邊表示了從固定參考系到手部坐標系的各連桿坐標系之間的變換矩陣的連乘。結(jié)果為手部坐標系相對于固定參考系的位姿。稱該式為機器人運動學(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固定坐標系{0}和連桿1、連桿2、連桿3的坐標系{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的坐標系{1}相對于固定坐標系{0}的齊次變換矩陣；A2表示連桿2的坐標系{2}相對于連桿1坐標系{1}的齊次變換矩陣；A3表示連桿3的坐標系即手部坐標系{3}相對于連桿2坐標系{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表示手部坐標系{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é)方程式求解出運動