（優(yōu)選）第三章工業(yè)機器人運動學運動學方程ppt講解當前第1頁\共有39頁\編于星期五\8點主要內(nèi)容

數(shù)學基礎——齊次坐標變換機器人運動學方程的建立（正運動學）機器人逆運動學分析當前第2頁\共有39頁\編于星期五\8點二、運動學方程的建立（運動學正問題）2.1引言2.8T6的說明2.2姿態(tài)描述2.9各種A矩陣的說明2.3歐拉角2.10根據(jù)A矩陣來確定T6

2.4搖擺、俯仰和偏轉(zhuǎn)2.11斯坦福機械手的運動方程2.5位置的確定2.12肘機械手的運動方程2.6圓柱坐標2.13小結(jié)2.7球坐標

當前第3頁\共有39頁\編于星期五\8點2.1引言(Introduction)本章，我們采用齊次變換來描述在各種坐標系中機械手的位置與方向。首先介紹各種正交坐標系的齊次變換。然后介紹在非正交關節(jié)坐標系中描述機械手末端的齊次變換。注意，對任何數(shù)目關節(jié)的各種機械手均可以這樣進行。描述一個連桿與下一個連桿之間關系的齊次變換稱A矩陣。A矩陣是描述連桿坐標系之間的相對平移和旋轉(zhuǎn)的齊次變換。連續(xù)變換的若干A矩陣的積稱為T矩陣，對于一個六連桿（六自由度）機械手有

T6=A1A2A3A4A5A6

（2.1）六連桿的機械手有六個自由度，其中三個自由度用來確定位置，三個自由度用來確定方向。Ｔ６表示機械手在基坐標中的位置與方向。則變換矩陣Ｔ６有下列元素

nxoxaxpxnyoyaypyT6=nzozazpz

（2.2）0001當前第4頁\共有39頁\編于星期五\8點

如圖2.1所示，機器人的末端執(zhí)行器（手爪）的姿態(tài)（方向）由n、o、a三個旋轉(zhuǎn)矢量描述，其坐標位置由平移矢量p描述，這就構(gòu)成了式（2.2）中的變換矩陣T。由于n、o、a三個旋轉(zhuǎn)矢量是正交矢量，所以有n=o×a圖2.1末端執(zhí)行器的描述當前第5頁\共有39頁\編于星期五\8點2.2姿態(tài)描述(SpecificationofOrientation)

對式（2.2）中16個元素一一賦值就可確定T6。假定機械手可以到達要求的位置，而單位旋轉(zhuǎn)矢量o和a正交，即o·o＝1

（2.3）a·a＝1（2.4）o·a＝0（2.5）a形成單位向量

aa（2.6）|a|構(gòu)成與o和a正交的nno×a（2.7）

在o和a形成的平面上旋轉(zhuǎn)o，使得o與n和a正交

oa×n（2.8）單位向量o是oo（2.9）|o|根據(jù)數(shù)學基礎給出的一般性的旋轉(zhuǎn)矩陣Ｒot(k,θ)，它把機械手末端的姿態(tài)規(guī)定為繞k軸旋轉(zhuǎn)θ角。當前第6頁\共有39頁\編于星期五\8點2.３歐拉角(EulerAngles)

姿態(tài)變更常用繞x,y或z軸的一系列旋轉(zhuǎn)來確定。歐拉角描述方法是：先繞z軸旋轉(zhuǎn)?，然后繞新的y(即y/)軸旋轉(zhuǎn)θ，最后繞更新的z(z//)軸旋轉(zhuǎn)ψ（見圖2.2）歐拉變換Euler(?,θ,ψ)可以通過連乘三個旋轉(zhuǎn)矩陣來求得Euler(?,θ,ψ)＝Ｒot(z,?)Ｒot(y,θ)Ｒot(z,ψ)（2.10）在一系列旋轉(zhuǎn)中，旋轉(zhuǎn)的次序是重要的。應注意，旋轉(zhuǎn)序列如果按相反的順序進行，則是繞基坐標中的軸旋轉(zhuǎn)：繞z軸旋轉(zhuǎn)ψ，接著繞y軸旋轉(zhuǎn)θ，最后再一次繞z軸旋轉(zhuǎn)?，結(jié)果如圖2.3所示，它與圖2.2是一致的。當前第7頁\共有39頁\編于星期五\8點xx’x’’x’’’yy’’y’θ?θ?ψψψθz’’’z’’z’zy’’’圖2.2歐拉角?0xx’x’’x’’’yy’’y’θ?θ?ψψψθz’’’z’’z’zy’’’圖2.3基于基坐標的歐拉角?0?θ當前第8頁\共有39頁\編于星期五\8點2.4搖擺、俯仰和偏轉(zhuǎn)(Roll,PitchandYaw)

搖擺、俯仰和偏轉(zhuǎn)為另一種旋轉(zhuǎn)。如圖2.4所示，就像水中航行的一條小船一樣，繞著它前進的方向（z軸）旋轉(zhuǎn)?稱為搖擺，繞著它的橫向中軸（y軸）旋轉(zhuǎn)θ稱為俯仰，繞著它甲板的垂直向上的方向（x軸）旋轉(zhuǎn)ψ

稱為偏轉(zhuǎn)。借助于這種旋轉(zhuǎn)來描述機械手的末端執(zhí)行器如圖3.5所示。規(guī)定旋轉(zhuǎn)的次序為

RPY(?,θ,ψ)＝Ｒot(z,?)Ｒot(y,θ)Ｒot(x,ψ)（2.12）

即，繞x軸旋轉(zhuǎn)ψ，接著繞y軸旋轉(zhuǎn)θ，最后繞z軸旋轉(zhuǎn)?

，這個變換如下

cosθ0sinθ0100001000cosψ–sinψ0RPY(?,θ,ψ)=Ｒot(z,?)–sinθ0cosθ00sinψcosψ0

（2.13）00010001

cos?–sin?00cosθsinθsinψsinθcosψ0sin?cos?000cosψ–sinψ0RPY(?,θ,ψ)=0010

-sinθcosθsinψcosθcosψ0

（2.14）00010001

當前第9頁\共有39頁\編于星期五\8點圖2.4搖擺、俯仰和偏轉(zhuǎn)角圖2.5機械手的末端執(zhí)行器的搖擺、俯仰和偏轉(zhuǎn)當前第10頁\共有39頁\編于星期五\8點RPY(?，θ，ψ)=cos?cosθcos?sinθsinψ–sin?cosψcos?sinθcosψ+sin?sinψ0sin?cosθsin?sinθsinψ+cos?cosψsin?sinθcosψ–cos?sinψ0

-sinθcosθsinψcosθcosψ00

0

01

（2.15）當前第11頁\共有39頁\編于星期五\8點2.5位置的確定(SpecificationofPosition)

一旦方向被確定之后，用一個相應的p向量的位移變換可得到機器人末端執(zhí)行器在基坐標中的位置：

100px

010py

T6=

001pz

（2.16）0001

旋轉(zhuǎn)變換矩陣當前第12頁\共有39頁\編于星期五\8點2.6圓柱坐標(CylindricalCoordinates)

如圖2.6所示，在圓柱坐標中確定機械手的位置是沿x軸平移r，接著繞z軸旋轉(zhuǎn)α，最后沿著z軸平移z。Cyl(z,α,r)=Trans(0,0,z)Rot(z,α)Trans(r,0,0)

cosα-sinα00100rsinαcosα000100Cyl(z,α,r)=Trans(0,0,z)0010001000010001（2.17）1000cosα-sinα0rcosα0100sinαcosα0rsinαCyl(z,α,r)=001z001000010001

（2.18）zaCzyxBAoαrn圖2.6圓柱坐標注意：圓柱坐標只能繞z軸旋轉(zhuǎn)當前第13頁\共有39頁\編于星期五\8點cosα-sinα0rcosαsinαcosα0rsinαCyl(z,α,r)=001z（2.19）0001

如用一個繞z軸旋轉(zhuǎn)-α的變換矩陣右乘式（2.19）,結(jié)果如下

cosα-sinα0rcosαcos(-α)-sin(-α)0

0sinαcosα0rsinαsin(-α)cos(-α)0

0Cyl(z,α,r)=

001z0000（2.20）00110001cosα-sinα0rcosαcosαsinα

00sinαcosα0rsinα-sinαcosα00Cyl(z,α,r)=001z0000

（2.21）00010001100rcosα010rsinαCyl(z,α,r)=001z（2.22）0001

上式表明平移矢量未變，旋轉(zhuǎn)矩陣為單位陣，此時末端坐標的姿態(tài)未變，而只是改變了它的空間位置。當前第14頁\共有39頁\編于星期五\8點2.7球坐標(SphericalCoordinates)

如圖2.7所示，用球坐標來確定位置向量的方法是：沿著z軸平移γ，然后繞y軸旋轉(zhuǎn)β，最后繞z軸旋轉(zhuǎn)α。Sph(α,β,γ)=Rot(z,α)Rot(y,β)Trans(0,0,γ)（2.23）cosβ0sinβ0100001001100Sph(α,β,γ)=Rot(z,α)-sinβ0cosβ0001γ00010001

（2.24）αaonzyx圖2.7球坐標γβ

當前第15頁\共有39頁\編于星期五\8點cosα-sinα00cosβ0sinβrsinβsinαcosα000100Sph(α,β,γ)=0010-sinβ0cosβrcosβ（2.25）00010001

cosαcosβ-sinαcosαsinβγcosαsinβsinαcosβcosαsinαsinβγsinαsinβSph(α,β,γ)=-sinβ0cosβγcosβ（2.26）0001

同樣，如果不希望改變末端坐標的姿態(tài)，而只是改變其空間位置，我們可以用Rot(y,-β)和Rot(z,-α)右乘式（2.26）Sph(α,β,γ)=Rot(z,α)Rot(y,β)Trans(0,0,γ)Rot(y,-β)Rot(z,-α)（2.27）100γcosαsinβ010γsinαsinβSph(α,β,γ)=001γcosβ（2.28）0001

當前第16頁\共有39頁\編于星期五\8點2.7T6的確定(SpecificationofT6)

T6可以用旋轉(zhuǎn)和平移的方法來確定。

T6=[平移][旋轉(zhuǎn)]

（2.29）

表2.1各種平移與旋轉(zhuǎn)的表達式

[Translation]Eqn[Rotation]Eqn

px,py,pzoxoyozaxay

az

Rot(k,θ)2.32Cyl(z,α,r)2.22Euler(?,θ,ψ)2.11Sph(α,β,γ)2.26RPY(?,θ,ψ)2.12

我們已經(jīng)研究過的各種平移與旋轉(zhuǎn)的式子，總結(jié)在表2.1中。如果我們使用Cyl和Sph的非旋轉(zhuǎn)的形式，那么矩陣積（2.29）僅僅是一個平移變換。

當前第17頁\共有39頁\編于星期五\8點2.9各種A矩陣的確定(SpecificationofmatricesA)

現(xiàn)在考慮方程(2.1)右邊各A矩陣的確定。串聯(lián)桿型機械手是由一系列通過連桿與其活動關節(jié)連接在一起所組成。如圖2.8所示，任何一個連桿都可以用兩個量來描述：一個是公共垂線距離an，另一個是與an垂直的平面上兩個軸的夾角αn，習慣上稱an為連桿長度，αn稱為連桿的扭轉(zhuǎn)角。圖2.8連桿的長度與扭轉(zhuǎn)角當前第18頁\共有39頁\編于星期五\8點

如圖2.9所示，在每個關節(jié)軸上有兩個連桿與之相連，即關節(jié)軸有兩個公垂線與之垂直，每一個連桿一個。兩個相連的連桿的相對位置用dn和θn確定，dn是沿著n關節(jié)軸兩個垂線的距離，θn是在垂直這個關節(jié)軸的平面上兩個被測垂線之間的夾角，dn和θn分別稱作連桿之間的距離及夾角。圖2.9連桿參數(shù)xn-1當前第19頁\共有39頁\編于星期五\8點為了描述連桿之間的關系，我們對每個連桿賦一個坐標系。

轉(zhuǎn)動關節(jié)：關節(jié)變量為θn。連桿n的坐標原點設在關節(jié)n和關節(jié)n+1軸之間的公共垂線與關節(jié)n+1軸的交點上。在關節(jié)軸相交的情況下（無公垂線），這個原點就在兩個關節(jié)軸的相交點上（an＝0）。如果兩個關節(jié)軸平行（有無數(shù)條公垂線），則原點的選擇要使下一個連桿的關節(jié)距離為0（dn＝0），連桿n的z軸與n+1關節(jié)軸在一條直線上。x軸與任何存在的公共垂線成一條直線，并且沿著這條垂線從n關節(jié)指向n+1關節(jié)。在相交關節(jié)的情況下，x軸的方向平行或者逆平行zn-1×zn的向量叉積，應該注意，這個條件對于沿著關節(jié)n和n+1之間垂線的x軸同樣滿足。當xn-1和xn平行，且有相同的指向時，則對于第n個轉(zhuǎn)動關節(jié)θn＝0。連桿本身的參數(shù)連桿長度an連桿兩個軸的公垂線距離（x方向）連桿扭轉(zhuǎn)角αn連桿兩個軸的夾角（x軸的扭轉(zhuǎn)角）連桿之間的參數(shù)連桿之間的距離dn相連兩連桿公垂線距離（z方向平移距）連桿之間的夾角θn相連兩連桿公垂線的夾角（z軸旋轉(zhuǎn)角）表2.2連桿參數(shù)當前第20頁\共有39頁\編于星期五\8點棱形關節(jié)：關節(jié)變量為dn。關節(jié)軸的方向就是關節(jié)的運動方向。與轉(zhuǎn)動關節(jié)不同，軸的運動方向被確定了，但在空間的位置并沒有確定（見圖2.10）。對于棱形關節(jié)，連桿長度an沒有意義，所以被設置為0。棱形關節(jié)坐標的z軸（zn）與下一個連桿的軸在一條直線上，x軸（xn）平行或逆平行棱形關節(jié)軸的方向（zn-1）與zn的叉積。對于棱形關節(jié)，當dn=0時，定義為0位置（即坐標原點）。因此棱形關節(jié)坐標原點與上一個關節(jié)（n-1）坐標原點重合，上一個關節(jié)的z軸（zn-1）與棱形關節(jié)的軸向相同，其關節(jié)長度an-1為上一個關節(jié)的軸線與zn-1的公垂線長度，xn-1軸向為公垂線向下一個關節(jié)延伸的方向。圖2.10棱型關節(jié)的連桿參數(shù)an-1當前第21頁\共有39頁\編于星期五\8點根據(jù)上述模式用下列旋轉(zhuǎn)和位移我們可以建立相鄰的n-1和n坐標系之間的關系：繞zn-1旋轉(zhuǎn)一個角度θn沿zn-1位移一個距離dn沿著被旋轉(zhuǎn)的xn-1即xn位移an繞xn旋轉(zhuǎn)的扭轉(zhuǎn)角為αn這四個齊次變換的積為A矩陣，即An=Rot(z,θ)Trans(0,0,d)Trans(a,0,0)Rot(x,α)（2.30）cosθ-sinθ00100a1000sinθcosθ0001000cosα-sinα0An=0010001d0sinαcosα0（2.31）000100010001

cosθ-sinθcosαsinθsinαacosθsinθcosθcosα-cosθsinαasinθAn=0sinαcosαd（2.32）0001當前第22頁\共有39頁\編于星期五\8點

對于棱形關節(jié)，an=0，則式（2.32）A矩陣簡化為

cosθ-sinθcosαsinθsinα0sinθcosθcosα-cosθsinα0An=0sinαcosαd（2.33）0001

一旦給機械手各連桿坐標系都賦了值，各種固定的連桿參數(shù)可以確定：對于后面是旋轉(zhuǎn)關節(jié)的連桿參數(shù)為d,a和α，對于后面是棱形關節(jié)的連桿參數(shù)為θ和α。根據(jù)這些參數(shù)，α的正弦和余弦也可以求出。這樣，對于旋轉(zhuǎn)關節(jié)，A矩陣變成了關節(jié)變量θ的函數(shù)?；蛟诶庑侮P節(jié)的情況下，變成了d的函數(shù)。一旦這些值給出，對于六個Ai變換矩陣的值就可以確定。當前第23頁\共有39頁\編于星期五\8點2.10根據(jù)A矩陣來確定T6

(SpecificationofT6inTermsoftheAmatrices)

機械手的坐標變換圖如圖2.11所示，機械手的末端（即連桿坐標系6）相對于連桿坐標系n-1的描述用n-1T6表示，即：

n-1T6=AnAn+1???A6（2.34）0zA1A2A3A4A5A60EX0T61T62T63T64T65T6圖2.11機械手的坐標變換圖當前第24頁\共有39頁\編于星期五\8點

機械手的末端相對于基坐標系（用T6表示）用下式給出

T6=A1A2A3A4A5A6（2.35）如果機械手用變換矩陣Z與參考坐標系相聯(lián)系，機械手末端執(zhí)行器用E來描述，末端執(zhí)行器的位置和方向相對參考坐標系用X來描述，如圖2.11所示有X=ZT6E（2.36）由此可以得到T6的表達式T6=Z-1XE-1（2.37）當前第25頁\共有39頁\編于星期五\8點2.11斯坦福機械手的運動方程

(KinematicEquationsfortheStanfordManipulator)

斯坦福機械手及其各關節(jié)坐標的設置如圖2.12所示。將角θ的正弦和余弦簡化

sinθi=Sicosθi=Cisin(θi+θj)=Sijcos(θi+θj)=Cij注：將所有關節(jié)x軸的方向設置一致，可簡化坐標變換。圖2.12斯坦福機械手坐標系當前第26頁\共有39頁\編于星期五\8點表2.2斯坦福機械手連桿參數(shù)

LinkVariableαadcosαsinα

1θ1

-90°

000-12θ290°

0d20

13d30°0d3104θ4

-90°

000-15θ590°00016θ60°0010當前第27頁\共有39頁\編于星期五\8點斯坦福機械手的A變換如下：

C10-S10S10C10A1=0-100（2.38）0001

C20S20S20-C20A2=010d2（2.39）0001

10000100A3=001d3（2.40）0001

當前第28頁\共有39頁\編于星期五\8點C40-S40S40C40A4=0-100（2.41）0001

C50S50S50-C50A5=0100（2.42）0001

C6

-S600S6C600A6=0010（2.43）0001當前第29頁\共有39頁\編于星期五\8點斯坦福機械手A變換的積如下所示，這些是從連桿6開始，然后逐步回到基坐標。

C6

-S600S6C6005T6=0010（2.44）0001

C5C6-C5S6S50S5C6-S5S6-C50

4T6=S6C600（2.45）0001

C4C5C6-S4S6

-C4C5S6-S4C6C4S50S4C5C6+C4S6

-S4C5S6+C4C6S4S503T6=

-S5C6S5S6C50（2.46）0001當前第30頁\共有39頁\編于星期五\8點C4C5C6-S4S6

-C4C5S6-S4C6C4S50S4C5C6+C4S6

-S4C5S6+C4C6S4S502T6=-S5C6S5S6C5d3（2.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（2.48）01當前第31頁\共有39頁\編于星期五\8點nxoxaxpxnyoyaypyT6=nzozazpz（2.49）0001其中nx=C1[C2(C4C5C6-S4S6)-S2S5C6]-S1(S4C5S6+C4S6)ny=S1[C2(C4C5C6-S4S6)-S2S5C6]+C1(S4C5S6+C4S6)nz=-S2(C4C5C6-S4S6)-C2S5C6ox=C1[-C2(C4C5S6+S4C6)+S2S5C6]-S1(-S4C5S6+C4S6)oy=S1[-C2(C4C5C6+S4C6)+S2S5S6]+C1(-S4C5S6+C4S6)oz=S2(C4C5C6+S4C6)+C2S5S6ax=C1(C2C4S5+S2C5)–S1S4C5ay=S1(C2C4S5+S2C5)+C1S4S5az=–S2C4S5+C2C5px=C1S2d3–S1d2py=S1S2d3+C1d2pz=C2d3當前第32頁\共有39頁\編于星期五\8點2.12肘機械手的運動方程

(KineamticEquationsforanElbowManipulator)

肘機械手及其各關節(jié)坐標的設置如圖2.13所示。z1z0z2z3z4z5、z6xa3a4a2圖2.13肘機械手的坐標系x0y0當前第33頁\共有39頁\編于星期五\8點

表2.3肘機械手的連桿參數(shù)

LinkVariableαadcosαsinα1θ190°

00012θ20a20

103θ30a30

104θ4

-90°

a400-15θ590°00016θ600010注：在以下的T矩陣中用變量θ23=θ2+θ3和θ234=θ23+θ4來進行簡化。當前第34頁\共有39頁\編于星期五\8點肘機械手的A變換如下：C10S10S10-C1

0A1=0100（2.50）0001

C2

-S20C2S2S2C20S2a2A2=0110（2.51）0001

C3

-S30C3a3S3C30S3a3A3=0010（2.52）0001當前第35頁\共有39頁\編于星期五\8點C40-S4C4a4S40C4S4a4A4=0-100（2.53）0001

C50S50S50-C50A5=0-100（2.54）0001

C6

-S600S6C600A6=0010（2.55）0001當前第36頁\共有39頁\編于星期五\8點

為了得到T6，我們從連桿6開始來算A矩陣的積，逐步往回計算到基坐標。

C6

-S600S6C6005T6=0010（2.55）