機(jī)器人技術(shù) 二、齊次坐標(biāo)變換

1、齊次坐標(biāo)變換主講：吳海彬福州大學(xué)機(jī)械工程及自動(dòng)化學(xué)院第二講主要內(nèi)容引言點(diǎn)的向量表示單位向量點(diǎn)和向量的齊次表示坐標(biāo)系的位姿剛體的位姿平移變換旋轉(zhuǎn)變換一般變換相對(duì)參考坐標(biāo)系的變換相對(duì)自身坐標(biāo)系的變換引言 (Introduction) 機(jī)器人運(yùn)動(dòng)學(xué)解決的基本問(wèn)題： 正向運(yùn)動(dòng)學(xué) 逆向運(yùn)動(dòng)學(xué)機(jī)器人機(jī)構(gòu) 一個(gè)自由度情況 多個(gè)自由度情況 誤差的反饋點(diǎn)、向量和坐標(biāo)系的傳統(tǒng)表示坐標(biāo)軸的定義kcjbiaPzyxzyxcbaP或kcjbiaPzyxzyxcbaP或PzaonPaonPaonTzzzyyyyxxxx非方陣相乘結(jié)果的維數(shù)發(fā)生變化點(diǎn)、向量和坐標(biāo)系的齊次表示在三維向量中加入一比例因子w；其物理意義是，隨著W

2、的改變，向量的大小會(huì)發(fā)生變化，而方向不變；W大于1，向量的分量變大；W小于1，向量的分量變小；若W1，各分量大小不變；若W0，則表示一個(gè)無(wú)窮小的向量，其方向不變。第二章 機(jī)器人運(yùn)動(dòng)學(xué)zyxcbaPwzyxPwxaxwybywzcz其中齊次坐標(biāo)與傳統(tǒng)坐標(biāo)的關(guān)系點(diǎn)、向量和坐標(biāo)系的齊次表示第二章 機(jī)器人運(yùn)動(dòng)學(xué) 因此，習(xí)慣上用W1表示向量的長(zhǎng)度，用W0表示向量的方向，而且方向向量一般表示成單位向量的形式。形式如下：1zyxcbaP0222222222zyxzzyxyzyxxcbaccbabcbaaP例：有一向量P（3，5，2），請(qǐng)按如下要求表示成矩陣形式：1、比例因子為2；2、表示為方向的單位向量。點(diǎn)

3、、向量和坐標(biāo)系的齊次表示原點(diǎn)重合情況坐標(biāo)系的齊次表示是由坐標(biāo)系的三個(gè)方向向量和原點(diǎn)位置齊次坐標(biāo)組成：1000zzzzyyyyxxxxPaonPaonPaonF例：如圖所示為F坐標(biāo)系位于參考坐標(biāo)系中（3，5，7）的位置，它的n軸與x軸平行，o軸相對(duì)于y軸的角度為45度，a軸相對(duì)于z的角度為45度。請(qǐng)寫(xiě)出該坐標(biāo)的齊次表達(dá)形式。點(diǎn)、向量和坐標(biāo)系的齊次表示第二章 機(jī)器人運(yùn)動(dòng)學(xué)剛體的表示 一個(gè)剛體在空間的表示可以這樣實(shí)現(xiàn)：通過(guò)在它上面固連一個(gè)坐標(biāo)系，再將該固連的坐標(biāo)系在空間表示出來(lái)。由于這個(gè)坐標(biāo)系一直固連在該剛體上，所以該剛體相對(duì)于坐標(biāo)系的位姿是已知的。因此，只要這個(gè)坐標(biāo)系可以在空間表示出來(lái)，那么這個(gè)剛

4、體相對(duì)于固定坐標(biāo)系的位姿也就已知了。由此可知，剛體在參考坐標(biāo)系的表示與坐標(biāo)系是完全一樣的。1000zzzzyyyyxxxxobjectPaonPaonPaonF圖約束變量點(diǎn)、向量和坐標(biāo)系的齊次表示第二章 機(jī)器人運(yùn)動(dòng)學(xué)由剛體（坐標(biāo)系）在參考坐標(biāo)系的齊次矩陣表達(dá)可知，該矩陣有12個(gè)變量，但描述剛體位姿只需要6個(gè)變量（自由度）就足夠了，因此，齊次矩陣中12個(gè)變量之間并不是相互獨(dú)立的，而是有約束的，約束條件為：1、三個(gè)方向向量相互垂直；2、每個(gè)單位向量的長(zhǎng)度均為1。即：0on0an0oa1n1o1a 已知兩個(gè)向量 a = ax i + ay j + az k b = bx i + by j + bz

5、k 向量的點(diǎn)積是標(biāo)量。用“ ”來(lái)定義向量點(diǎn)積，即 a b = ax bx + ay by + az bz 向量的叉積是一個(gè)垂直于由叉積的兩個(gè)向量構(gòu)成的平面的向量。用“”表示叉積，即 a b = ( ay bz az by ) i + ( az bx ax bz ) j + ( ax by ay bx ) k 可用行列式表示為 i j k a b = ax ay az bx by bz例題點(diǎn)、向量和坐標(biāo)系的齊次表示第二章 機(jī)器人運(yùn)動(dòng)學(xué)對(duì)于下列坐標(biāo)系，求解所缺元素的值，并用矩陣來(lái)表示這個(gè)坐標(biāo)系。100020?3?707. 05?0?Fkajaiaooonnnkjizyxzyxzyxaon注：三個(gè)點(diǎn)

6、積約束條件可以用叉積代替，即：進(jìn)一步有齊次變換矩陣 變換定義為空間的一個(gè)運(yùn)動(dòng)； 當(dāng)空間的一個(gè)坐標(biāo)系（向量、剛體、運(yùn)動(dòng)坐標(biāo)系）相對(duì)于固定的參考坐標(biāo)系運(yùn)動(dòng)時(shí)，這一運(yùn)動(dòng)可以用類(lèi)似于表示坐標(biāo)系的方式來(lái)表示； 變換有如下幾種形式： 純平移， 純旋轉(zhuǎn)， 平移和旋轉(zhuǎn)的結(jié)合。第二章 機(jī)器人運(yùn)動(dòng)學(xué)純平移齊次變換矩陣第二章 機(jī)器人運(yùn)動(dòng)學(xué)特點(diǎn)：運(yùn)動(dòng)過(guò)程中姿態(tài)不變，坐標(biāo)方向單位向量保持同一方向不變。),(1000100010001zyxzyxdddTransdddT變換矩陣可表示為100010001000100010001zzzzzyyyyyxxxxxzzzzyyyyxxxxzyxnewdPaondPaondPaon

7、PaonPaonPaondddF變換過(guò)程為：注：相對(duì)固定坐標(biāo)系的平移，變換矩陣左乘，公式為oldzyxnewFdddTransF),(例純旋轉(zhuǎn)(相對(duì)坐標(biāo)繞參考坐標(biāo)X軸）齊次變換矩陣第二章 機(jī)器人運(yùn)動(dòng)學(xué)nxPPsincos21aoyPPllPcossin43aozPPllPaonzyxPPPPPPcossin0sincos0001例必須從原點(diǎn)開(kāi)始變換！純旋轉(zhuǎn)齊次變換矩陣第二章 機(jī)器人運(yùn)動(dòng)學(xué)noaxyzPxRotP),(cossin0sincos0001),(xRotcos0sin010sin0cos),(yRot1000cossin0sincos),(zRot也就相當(dāng)于旋轉(zhuǎn)變換前在固定參考坐標(biāo)系

8、的初始位置。式中noaPPTPRRUU圖、例注：相對(duì)固定坐標(biāo)系的旋轉(zhuǎn)，變換矩陣左乘，公式為繞x軸旋轉(zhuǎn)可簡(jiǎn)寫(xiě)成其中同理純旋轉(zhuǎn)例題齊次變換矩陣第二章 機(jī)器人運(yùn)動(dòng)學(xué)旋轉(zhuǎn)坐標(biāo)系中有一點(diǎn)P（2，3，4），此坐標(biāo)系繞參考坐標(biāo)系x軸旋轉(zhuǎn)90度。求旋轉(zhuǎn)后該點(diǎn)相對(duì)于參考坐標(biāo)系的坐標(biāo)。復(fù)合變換齊次變換矩陣第二章 機(jī)器人運(yùn)動(dòng)學(xué)特點(diǎn)：既有平移，又有旋轉(zhuǎn)，而且可以多次。假設(shè)坐標(biāo)系（n，o，a）相對(duì)于參考坐標(biāo)系（x，y，z）依次進(jìn)行如下變換：1、繞x軸旋轉(zhuǎn) 角；2、平移 ；3、再繞y軸旋轉(zhuǎn) 角。321lllnoaxyzPxRotlllTransyRotP),(),(),(321注：矩陣的順序不能變； 相對(duì)固定坐標(biāo)系的平移

9、和旋轉(zhuǎn)，變換矩陣左乘。例復(fù)合變換例題齊次變換矩陣相對(duì)坐標(biāo)系的齊次矩陣固連在坐標(biāo)系（n，o，a）上的點(diǎn)P（7，3，2）經(jīng)歷如下變換，求出變換后該點(diǎn)相對(duì)于參考坐標(biāo)系的坐標(biāo)。1、繞z軸旋轉(zhuǎn)90度；2、接著繞y軸旋轉(zhuǎn)90度；3、接著再平移（4，-3，7）。復(fù)合變換例題齊次變換矩陣第二章 機(jī)器人運(yùn)動(dòng)學(xué)假設(shè)（n，o，a）坐標(biāo)系上的點(diǎn)P（7，3，2）也經(jīng)歷相同變換，但變換順序按如下進(jìn)行，求出變換后該點(diǎn)相對(duì)于參考坐標(biāo)系的坐標(biāo)。1、繞z軸旋轉(zhuǎn)90度；2、接著平移（4，-3，7）；3、接著再繞y軸旋轉(zhuǎn)90度。相對(duì)動(dòng)坐標(biāo)系的變換齊次變換矩陣第二章 機(jī)器人運(yùn)動(dòng)學(xué)相對(duì)運(yùn)動(dòng)坐標(biāo)系的變換與相對(duì)固定參考坐標(biāo)系不同，這時(shí)需要右

10、乘變換矩陣而不是左乘。相對(duì)自身的運(yùn)動(dòng)即是相對(duì)動(dòng)坐標(biāo)。相對(duì)動(dòng)坐標(biāo)是指動(dòng)坐標(biāo)系本身相對(duì)自身的運(yùn)動(dòng)，而不是動(dòng)坐標(biāo)系中的點(diǎn)相對(duì)動(dòng)坐標(biāo)系的運(yùn)動(dòng)。如果在一個(gè)變換過(guò)程中，既有相對(duì)固定坐標(biāo)系的變換，也有相對(duì)于動(dòng)坐標(biāo)系的變換，則應(yīng)先寫(xiě)出第一個(gè)變換因子，在根據(jù)變換的具體過(guò)程，依次左乘或右乘變換因子，最后乘以被變換的對(duì)象（點(diǎn)或坐標(biāo)）。相對(duì)動(dòng)坐標(biāo)系的變換例題齊次變換矩陣第二章 機(jī)器人運(yùn)動(dòng)學(xué)假設(shè)與上例相同的點(diǎn)現(xiàn)在進(jìn)行相同的變換，但所有變換都是相對(duì)當(dāng)前運(yùn)動(dòng)坐標(biāo)系的，具體變換如下，求變換完成后該點(diǎn)相對(duì)于參考坐標(biāo)系的坐標(biāo)。1、繞a軸旋轉(zhuǎn)90度；2、然后沿n、o、a軸平移（4，-3，7）；3、接著繞o軸旋轉(zhuǎn)90度。相對(duì)動(dòng)坐標(biāo)系

11、的變換例題齊次變換矩陣第二章 機(jī)器人運(yùn)動(dòng)學(xué)坐標(biāo)系B繞x軸旋轉(zhuǎn)90度，然后沿當(dāng)前坐標(biāo)系a軸做了3英寸的平移，然后再繞z軸旋轉(zhuǎn)90度，最后沿當(dāng)前坐標(biāo)系o軸做5英寸的平移。1、寫(xiě)出描述該運(yùn)動(dòng)的方程；2、求坐標(biāo)系中的點(diǎn)P（1，5，4）相對(duì)于參考坐標(biāo)系的最終位置。提示：先求 ，再求BUTPTPBBUU變換矩陣的逆第二章 機(jī)器人運(yùn)動(dòng)學(xué)鉆孔點(diǎn)位置的描述：EPPUEHHRRUEUTTTTTT式中：只有 是未知的，其它都可以通過(guò)傳感器獲得，或本身就是已知的。因此，通過(guò)求逆陣就可以求得 。HRTHRT求矩陣逆例題變換矩陣的逆第二章 機(jī)器人運(yùn)動(dòng)學(xué)在一個(gè)具有六自由度的機(jī)器人的第五個(gè)連桿上裝有照相機(jī)，照相機(jī)觀(guān)察物體并測(cè)

12、定它相對(duì)于照相機(jī)坐標(biāo)系的位置，然后根據(jù)以下數(shù)據(jù)來(lái)確定末端執(zhí)行器要到達(dá)物體所必須完成的運(yùn)動(dòng)。10005001001031005camT10004100000100105HT1000401020012100objcamT1000310000100001EHTobjcamcamRobjEEHHRTTTTTTT5555objET提示：根據(jù)求 ，這可以用于測(cè)距變換矩陣的逆求逆陣的步驟：第二章 機(jī)器人運(yùn)動(dòng)學(xué)1、計(jì)算矩陣的行列式；2、將矩陣轉(zhuǎn)置；3、將轉(zhuǎn)置矩陣的每個(gè)元素用它的子行列式（伴隨矩陣）代替；4、用轉(zhuǎn)換后的矩陣除以行列式AAA*1即cossin0sincos0001),(xRot例：求的逆陣。滿(mǎn)足T

13、AA1的矩陣稱(chēng)為酉矩陣。齊次矩陣的逆變換矩陣的逆第二章 機(jī)器人運(yùn)動(dòng)學(xué) 對(duì)于4X4齊次變換矩陣，可以將矩陣分成兩部分求逆。其旋轉(zhuǎn)部分仍是酉矩陣，只需要簡(jiǎn)單的轉(zhuǎn)置；矩陣的位置部分是向量P分別與n、o、a向量點(diǎn)積的取反。10001aPaaaoPooonPnnnTzyxzyxzyx1000zzzzyyyyxxxxPaonPaonPaonT即的逆陣為1000501025 . 00866. 03866. 005 . 0T例：求的逆陣。 圖2.12所示為點(diǎn)A繞任意過(guò)原點(diǎn)的單位矢量此旋轉(zhuǎn)角的情況。kx，ky，kz分別為此矢量在固定參考系坐標(biāo)軸X、Y、Z上的三個(gè)分量， 可以證得，繞任意過(guò)原點(diǎn)的單位矢量k轉(zhuǎn)角的旋

14、轉(zhuǎn)齊次變換公式為 式(2-18)稱(chēng)為一般旋轉(zhuǎn)齊次變換通式，它概括了繞X軸、Y軸、Z軸進(jìn)行旋轉(zhuǎn)齊次變換的各種特殊情況，例如： 當(dāng)kx=1，即ky=kz=0時(shí)，則由式(2-18)可得到式(2-16)； 當(dāng)ky=1，即kx=kz=0時(shí)，則由式(2-18)可得到式(2-17)； 當(dāng)kz=1，即kx=ky=0時(shí)，則由式(2-18)可得到式(2-15)。 反之，若給出某個(gè)旋轉(zhuǎn)齊次矩陣則可根據(jù)式 (2-18)求出其等效矢量k及等效轉(zhuǎn)角式中：當(dāng)取0到180。之間的值時(shí)，式中的符號(hào)取+號(hào)；當(dāng)轉(zhuǎn)角時(shí)很小時(shí)，公式很難確定轉(zhuǎn)軸；當(dāng)接近0。或180。時(shí)，轉(zhuǎn)軸完全不確定。 與平移變換一樣，旋轉(zhuǎn)變換算子公式(2-15)、 (2-16)、(2-17)以及一般旋轉(zhuǎn)變換算子公式(2-18)，不僅僅適用于點(diǎn)的旋轉(zhuǎn)變換,而且也適用于矢量、坐標(biāo)系、物體等