機(jī)器人運(yùn)動學(xué)

創(chuàng)新設(shè)計作業(yè):設(shè)計一種類人教學(xué)機(jī)器人。要求機(jī)器人具有類似人的四肢，單片機(jī)控制。給出總體的設(shè)計方案、機(jī)械結(jié)構(gòu)和傳動方案、選擇合適的傳感器、控制方案。

第2章機(jī)器人運(yùn)動學(xué)

(KinematicsofRobots)引言機(jī)器人位置與姿態(tài)的描述機(jī)器人運(yùn)動學(xué)正問題機(jī)器人運(yùn)動學(xué)逆問題機(jī)器人的雅可比矩陣2§2.1引言（TheIntroduction）機(jī)器人運(yùn)動學(xué)正問題：定義逆問題：定義機(jī)器人動力學(xué)3基本概念（TheBasicConcepts）自由度：物體能夠?qū)ψ鴺?biāo)系進(jìn)行獨立運(yùn)動的數(shù)目稱為自由度(DOF,degreeoffreedom)。剛體具有6個自由度三個旋轉(zhuǎn)自由度R1,R2,R3三個平移自由度T1,T2,T3YXZR1R2R3T1T2T34機(jī)動度：DegreeofMobility關(guān)節(jié)：Joint連桿：Link自由度由機(jī)動度構(gòu)成,機(jī)動度不一定是自由度.123455個機(jī)動度，2個自由度5§2.2機(jī)器人位置與姿態(tài)的描述

(TheDescriptionofPositionandPosture)0EGBZ0Tnzyx一個物體與機(jī)械手?6位置與姿態(tài)的表示位置描述：位置矢量(positionvector)直角坐標(biāo)系{A},位置矢量Ap矩陣表示矢量和表示矢量的模，單位矢量xAyAzAoApAp7一、機(jī)器人坐標(biāo)系變換(CoordinateTransformation)

Ouvw：Puvw=（Pu,Pv,Pw）TOxyz：Pxyz=（Px,Py,Pz）T

當(dāng)Ouvw坐標(biāo)系繞一軸線轉(zhuǎn)動后，均可通過一個3x3旋轉(zhuǎn)矩陣R

將原坐標(biāo)Puvw變換到Oxyz系中的坐標(biāo)Pxyz，

即：

Pxyz=R

Puvw

8大家有疑問的，可以詢問和交流可以互相討論下，但要小聲點9由矢量分量的定義有：Puvw=puiu+pvjv+pwkw

pu、pv、pw分別表示P沿Ou、Ov、Ow軸的分量

Px=ix˙P=ix˙iupu+ix

˙jvpv+ix˙kwpw

Py=iy˙P=iy˙iupu+iy

˙jvpv+iy˙kwpw

Pz=iz˙P=iz˙iupu+iz

˙jvpv+iz˙kwpw

將上式寫成矩陣形式：

Px=ix˙iuix

˙jvix˙kwPu

Py=iy˙iuiy

˙jviy˙kwPv

Pz=iz˙iuiz

˙jviz˙kwPwPxyz=R

Puvw

同樣，也有Puvw=QPxyz，Q＝R－1

＝RT10

如果Ouvw坐標(biāo)系統(tǒng)繞Ox軸轉(zhuǎn)動α角，變換矩陣Rx,α稱為繞Ox軸轉(zhuǎn)動α角的旋轉(zhuǎn)矩陣，此時ix=iu，

ix˙iuix

˙jvix˙kw100Rx,α

=iy˙iuiy

˙jviy˙kw=0cosα

-sinα

iz˙iuiz

˙jviz˙kw0sinα

cosα

向量點乘：a?b=|a|?|b|?

cos(a)11類似地，繞Oy軸轉(zhuǎn)動φ角和繞Oz軸轉(zhuǎn)θ角的3×3旋轉(zhuǎn)矩陣分別為，

cosφ

0

sinφ

Ry,φ=

0

1

0-sinφ0cosφcosθ-sinθ

0

Rz,θ

=

sinθ

cosθ

0001

矩陣Rx,α、Ry,φ和Rz,φ稱為基本旋轉(zhuǎn)矩陣。

任何旋轉(zhuǎn)變換可以由有限個基本旋轉(zhuǎn)變換合成得到。12依次左乘（如果uvw對xyz旋轉(zhuǎn)）依次右乘（如果uvw繞自己的坐標(biāo)軸旋轉(zhuǎn)）

R=Rz,θRy,φRx,α13例題：求表示繞Oy軸轉(zhuǎn)φ角，然后繞Ow軸轉(zhuǎn)θ角，再繞Ou軸轉(zhuǎn)α角的合成旋轉(zhuǎn)矩陣。14例題：坐標(biāo)系{B}的初始位姿與參考坐標(biāo)系{A}相同，坐標(biāo)系{B}相對于{A}的zA軸旋轉(zhuǎn)30

，再沿{A}的xA軸移動12，沿{A}的yA軸移動6。求旋轉(zhuǎn)矩陣。解：15二、齊次坐標(biāo)和變換矩陣齊次坐標(biāo)是用n+l維坐標(biāo)來描述n維空間中的位置，其第n+1個分量（元素）ω稱為比例因子。

P=(ωPx,ωPy,

ωPz,ω)T

在機(jī)器人學(xué)的應(yīng)用中，一般將比例因子取為1。

機(jī)器人系統(tǒng)運(yùn)動分析中，齊次變換矩陣寫成以下形式：

T=R3×3

P3×1

=

旋轉(zhuǎn)矩陣3×3

位置矢量3×1

O1×3

I1×1O1×3

116若三維空間的位置矢量P表示成齊次坐標(biāo)，即

P=pxpypz1T,

1

0

0

0

cosβ

0sinβ

0

Tx，α

=

0cosα

-sinα

0Ty，β=

01

0

00sinαcosα

0-sinβ0cosβ0

00010001cosθ

-sinθ

0

0

100

dx

Tz，θ

=sinθcosθ

0

0Ttran=

01

0

dy

00

1

000

1dz

00010001Pxyz=TPuvw17課前提問：（1）什么是機(jī)器人運(yùn)動學(xué)的正問題和逆問題？（2）機(jī)器人的坐標(biāo)變換矩陣的一般形式是什么？（3）連續(xù)的變換矩陣，什么情況下依次左乘、什么情況下依次右乘？（4）什么是齊次坐標(biāo)和齊次變換？18§2.3機(jī)器人運(yùn)動學(xué)正問題

(TheForwardKinematicProblem)Denavit–Hartenberg(D-H)表示法191.坐標(biāo)系的建立：

n關(guān)節(jié)機(jī)器人需建立n+1個坐標(biāo)系，其中參考(機(jī)座)坐標(biāo)系為O0x0y0z0,，機(jī)械手末端的坐標(biāo)系為Onxnynzn20

串聯(lián)桿型機(jī)械手是由一系列通過連桿與其活動關(guān)節(jié)連接在一起所組成。如圖所示，任何一個連桿都可以用兩個量來描述：一個是公共垂線距離an，另一個是與an垂直的平面上兩個軸的夾角αn，習(xí)慣上稱an為連桿長度，αn稱為連桿的扭轉(zhuǎn)角。21

如圖所示，在每個關(guān)節(jié)軸上有兩個連桿與之相連，即關(guān)節(jié)軸有兩個公垂線與之垂直，每一個連桿一個。兩個相連的連桿的相對位置用dn和θn確定，dn是沿著n關(guān)節(jié)軸兩個垂線的距離，θn是在垂直這個關(guān)節(jié)軸的平面上兩個被測垂線之間的夾角，dn和θn分別稱作連桿之間的距離及夾角。xn-122

為了描述連桿之間的關(guān)系，我們對每個連桿賦一個坐標(biāo)系。

轉(zhuǎn)動關(guān)節(jié)：關(guān)節(jié)變量為θn。連桿n的坐標(biāo)原點設(shè)在關(guān)節(jié)n和關(guān)節(jié)n+1軸之間的公共垂線與關(guān)節(jié)n+1軸的交點上。在關(guān)節(jié)軸相交的情況下（無公垂線），這個原點就在兩個關(guān)節(jié)軸的相交點上（an＝0）。如果兩個關(guān)節(jié)軸平行（有無數(shù)條公垂線），則原點的選擇要使下一個連桿的關(guān)節(jié)距離為0（dn＝0），連桿n的z軸與n+1關(guān)節(jié)軸在一條直線上。x軸與任何存在的公共垂線成一條直線，并且沿著這條垂線從n關(guān)節(jié)指向n+1關(guān)節(jié)。在相交關(guān)節(jié)的情況下，x軸的方向平行或者逆平行zn-1×zn的向量叉積，應(yīng)該注意，這個條件對于沿著關(guān)節(jié)n和n+1之間垂線的x軸同樣滿足。當(dāng)xn-1和xn平行，且有相同的指向時，則對于第n個轉(zhuǎn)動關(guān)節(jié)θn＝0。

表連桿參數(shù)23

確定和建立每個坐標(biāo)系的原則：

(1)zi軸沿著第i關(guān)節(jié)的運(yùn)動軸;

(2)xi軸垂直于zi-1軸和zi軸并指向離開zi-1軸的方向;

(3)yi軸按右手坐標(biāo)系的要求建立。

按照這些規(guī)則，第0號坐標(biāo)在機(jī)座上的位置和方向可任選，只要z0軸沿著第1關(guān)節(jié)運(yùn)動軸。第n坐標(biāo)系可放在手的任何部位，只要xn軸與zn-1軸垂直。

24z0z2z3z4x3x2x0x4021z5x5z6x66435z1x1252、幾何參數(shù)的定義描述串聯(lián)機(jī)器人相鄰坐標(biāo)系之間的關(guān)節(jié)關(guān)系可歸納為如下4個參數(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坐標(biāo)系原點沿xi軸的偏置距離;

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

263、建立i坐標(biāo)系和i-1坐標(biāo)系的齊次變換矩陣：27第i坐標(biāo)系相對于機(jī)座齊坐標(biāo)系的次變換矩陣是各齊次變換矩陣i-1Ai的連乘積：4、得出機(jī)器人手爪到機(jī)座的變換矩陣280Tn=0A11A2....n-1Ann為手的法向矢量，o為手的滑動矢量，a為手的接近矢量，p為手的位置矢量29例題1：建立二稈機(jī)構(gòu)的末端的變換矩陣同理：30最后得到的變換矩陣為：31z0z2z3z4x3x2x0x4021z5x5z6x66435

D-H參數(shù)表：

z1x1例題2:PUMA機(jī)器人的坐標(biāo)變換矩陣32z0z2z3z4x3x2x0x4021z5x5z6x66435

D-H參數(shù)表：

z1x13334例題3:斯坦福機(jī)械手一、建立坐標(biāo)系二、D-H參數(shù)表三、i-1Ai坐標(biāo)變換矩陣0123456Z0X0Z1X1Z2X2Z3X3Z4X4Z5X5Z6X635363738斯坦福機(jī)械手A變換的積如下所示，這些是從連桿6開始，然后逐步回到基坐標(biāo)。

C6

-S600S6C6005T6=0010（3.44）

0001

C5C6-C5S6S50S5C6-S5S6-C50

4T6=S6C600（3.45）

0001

C4C5C6-S4S6

-C4C5S6-S4C6C4S50S4C5C6+C4S6

-S4C5S6+C4C6S4S503T6=

-S5C6S5S6C50（3.46）

000139C4C5C6-S4S6

-C4C5S6-S4C6C4S50S4C5C6+C4S6

-S4C5S6+C4C6S4S502T6=-S5C6S5S6C5d3

（3.47）

0001

C2(C4C5C6-S4S6)-S2S5C6-C2(C4C5S6+S4C6)+S2S5S6

S2(C4C5C6-S4S6)+C2S5C6-S2(C4C5S6+S4C6)-C2S5S6

1T6=S4C5C6+C4C6-S4C5S6+C4C6

00

C2C4S5+S2C5S2d3S2C4S5-C2C5

-C2d3S4S5d2（3.48）

0140410Tn=0A11A2....n-1An42課前提問：（1）PUMA機(jī)器人的坐標(biāo)變換矩陣D-H參數(shù)？（2）矩陣的逆矩陣如何求解？43定義：若|A|≠0,則方陣A可逆，且A*稱為A的伴隨陣，由行列式|A|的方陣各個元素的代數(shù)余子式所Aij構(gòu)成。另一種求法：行列式變換44

§2.4機(jī)器人運(yùn)動學(xué)逆問題

(TheInverseKinematicProblem)450EGBZ0Tnzyx一個物體與機(jī)械手?有向變換圖GBET6Z??0Z：機(jī)器人基座相對于基礎(chǔ)坐標(biāo)系0Tn：手部端點相對于機(jī)器人基座Z：末端執(zhí)行器相對于手部端點物體用變換B：物體參考坐標(biāo)系相對于基礎(chǔ)坐標(biāo)系G：末端執(zhí)行器對物體的抓持位置相對于物體參考坐標(biāo)系

Z0TnE=BG

這個方程可以用有向變換圖來表示。圖的每一段弧表示一個變換。從它的定義的坐標(biāo)系向外指向。用Z-1左乘和用E-1右乘方程，得到

0Tn=Z-1BGE-1機(jī)器人坐標(biāo)變換關(guān)系：46

從有向變換圖上我們可以直接得到上述結(jié)果，從0Tn弧線的尾部開始，沿著圖形順時針依次列出各個變換，直到0Tn弧的箭頭為止。在逆變換時，我們從0T6弧的箭頭開始，按逆時針方向依次列出各個變換，直到T6弧的起始點為止，則可得到0Tn的逆

0Tn-1=EG-1B-1Z

作為進(jìn)一步的例子，假設(shè)一個物體B的位置不知道，但機(jī)械手移動，使得末端抓手正好定位在物體上面。然后用G-1右乘式（2.61）求出B。或者在有向變換圖中從B的尾部沿著逆時針方向到達(dá)弧B的箭頭，直接得到同樣結(jié)果。

B=Z0TnEG-1

同樣，我們可以用有向變換圖求出變換的連接組。例如

Z0Tn=BGE-1BGE0T6Z圖2.20有向變換圖的另一種形式474849

5051同時可確定d3為

用依次左乘方程式可得以下4個方程式52計算得式中53由式中第3行第3列為0可得即解得54由式第1行3列和第2行3列可得解得由第4個方程式可得55類似的有,第1行2列和2行2列對應(yīng)元素相等得可得56§2.5機(jī)器人的雅可比矩陣

(JacobianMatrix)意義：手端在基礎(chǔ)坐標(biāo)中的速度與各關(guān)節(jié)速度間的關(guān)系，以及手部與外界接觸力與對應(yīng)各關(guān)節(jié)力間的關(guān)系。一、雅可比矩陣的定義：

n自由度機(jī)器人，其關(guān)節(jié)變量向量可寫為

Q＝

(q1,q2,…,qn)T

，手部在基礎(chǔ)坐標(biāo)中的位置和姿態(tài)為P,則：

P

＝(xeyezeθex

θey

θez

)T