齊次坐標(biāo)變換.ppt

第二章齊次坐標(biāo)變換Chapter HomogeneousTransformation 2 1引言2 2點(diǎn)向量和平面的描述2 3變換2 4平移變換2 5旋轉(zhuǎn)變換2 6坐標(biāo)系2 7相對(duì)變換2 8物體的描述2 9逆變換2 10一般性旋轉(zhuǎn)變換2 11等價(jià)旋轉(zhuǎn)角與旋轉(zhuǎn)軸2 12擴(kuò)展與縮小2 13透視變換2 14變換方程2 15小結(jié) 2 1引言 Introduction 機(jī)器人操作涉及到各物體之間的關(guān)系和各物體與機(jī)械手之間的關(guān)系 這一章將給出描述這些關(guān)系必須的表達(dá)方法 類(lèi)似這種表示方法在計(jì)算機(jī)圖形學(xué)中已經(jīng)解決 在計(jì)算機(jī)圖形學(xué)和計(jì)算機(jī)視覺(jué)中 物體之間的關(guān)系是用齊次坐標(biāo)變換來(lái)描述的 在本課程我們將采用齊次坐標(biāo)變換來(lái)描述機(jī)械手各關(guān)節(jié)坐標(biāo)之間 各物體之間以及各物體與機(jī)械手之間的關(guān)系 本章首先介紹向量和平面的表示方法 然后引出向量和平面的坐標(biāo)變換 這些變換基本上是由平移和旋轉(zhuǎn)組成 因此可以用坐標(biāo)系來(lái)描述各種物體和機(jī)械手的空間位置和姿態(tài) 稍后還要介紹逆變換 逆變換是運(yùn)動(dòng)學(xué)求解的基礎(chǔ) 2 2點(diǎn)向量和平面的描述 Notationofpointvectorsandplanes 2 2 1點(diǎn)向量 Pointvectors 點(diǎn)向量描述空間的一個(gè)點(diǎn)在某個(gè)坐標(biāo)系的空間位置 同一個(gè)點(diǎn)在不同坐標(biāo)系的描述及位置向量的值也不同 如圖2 1中 點(diǎn)p在E坐標(biāo)系上表示為Ev 在H坐標(biāo)系上表示為Hu 且v u 一個(gè)點(diǎn)向量可表示為v ai bj ck通常用一個(gè) n 1 維列矩陣表示 即除x y z三個(gè)方向上的分量外 再加一個(gè)比例因子w 即v xyzw T其中a x w b y w c z w 改變比例因子w 則分量a b c的數(shù)值相應(yīng)改變 但描述的還是同一個(gè)點(diǎn)向量 如v 3i 4j 5k可表示為v 3451 T 68102 T 3 4 5 1 T在向量中增加一個(gè)比例因子w是為了方便坐標(biāo)變換中的矩陣運(yùn)算 已知兩個(gè)向量a axi ayj azkb bxi byj bzk 2 1 向量的點(diǎn)積是標(biāo)量 用 來(lái)定義向量點(diǎn)積 即a b axbx ayby azbz 2 2 向量的叉積是一個(gè)垂直于由叉積的兩個(gè)向量構(gòu)成的平面的向量 用 表示叉積 即a b aybz azby i azbx axbz j axby ayby k 2 3 可用行列式表示為ijka b axayaz 2 4 bxbybz 2 2 2平面 Planes 平面可用一個(gè)行矩陣表示 即p abcd 2 5 它表示了平面p的法線(xiàn)方向 且距坐標(biāo)原點(diǎn)的距離為 d m 其中m 2 6 如圖2 2所示 如果將x y平面沿z軸正方向平移一個(gè)單位距離 構(gòu)成平面p 則p 001 1 即a 0 b 0 c 1 d 1 m 1平面p上任一點(diǎn)v為v xy11 T 它與平面p的點(diǎn)乘為零 即p v 0平面p上方任一點(diǎn)v 如v 0021 T 它與平面p的點(diǎn)乘為一個(gè)正數(shù) 即p v 1平面p下方任一點(diǎn)v 如v 0001 T 它與平面p的點(diǎn)乘為一個(gè)負(fù)數(shù) 即p v 1注意 平面 0000 無(wú)定義 H空間的變換是由4 4矩陣來(lái)完成的 它可以表示平移 旋轉(zhuǎn) 擴(kuò)展和透視等各種變換 如已知點(diǎn)u 在平面p上 它的變換v 在平面q上 用矩陣積表示為v Hu 2 7 其中H為4 4變換矩陣 u和v為4 1的點(diǎn)列向量 相應(yīng)的平面p到q的變換是q pH 1 2 8 其中H 1為H的逆陣 p和q為1 4的平面行向量 經(jīng)變換后的平面向量q與點(diǎn)向量v的點(diǎn)乘為q v pH 1 Hu p u 2 9 與變換前平面p與點(diǎn)u的點(diǎn)乘相等 證明了變換的等效性 2 3變換 Transformation 2 4平移變換 Translationtransformation 用向量h ai bj ck進(jìn)行平移 其相應(yīng)的H變換矩陣是100a010bH Trans abc 001c 2 10 0001因此對(duì)向量u xyzw T 經(jīng)H變換為向量v可表示為x awx w ay bwy w bv z cw z w c 2 11 w1可見(jiàn) 平移實(shí)際上是對(duì)已知向量u xyzw T與平移向量h abc1 T相加 例2 1 對(duì)點(diǎn)向量u 2321 T進(jìn)行平移 平移向量為h 4 371 T 則平移后的向量為v 6091 T 或100426010 330v H u 00172 9000111點(diǎn)向量的平移過(guò)程如圖2 3所示 對(duì)平面的平移則用H 1進(jìn)行變換 如對(duì)平面p 100 2 進(jìn)行H變換為平面q 則根據(jù)變換原理有100 40103q pH 1 100 2 001 70001 100 6 平面p 100 2 是y z平面沿x正方向移動(dòng)2個(gè)單位形成的平面 圖2 3 點(diǎn)u 2321 T是平面p上的一個(gè)點(diǎn) 它們的點(diǎn)乘p u 0 經(jīng)H變換后的平面q 100 6 是y z平面沿x正方向移動(dòng)6個(gè)單位形成的平面 點(diǎn)v 6091 T是平面q上一個(gè)點(diǎn) 平面q與點(diǎn)v的點(diǎn)乘也應(yīng)是零 即q v 0 說(shuō)明變換前后的結(jié)果不變 證明H變換是正確的 2 5旋轉(zhuǎn)變換 Rotationtransformation 如圖2 4所示 繞x y z軸旋轉(zhuǎn)一個(gè) 角的相應(yīng)變換是10000cos sin 0Rot x 0sin cos 0 2 12 0001cos 0sin 00100Rot y sin 0cos 0 2 13 0001cos sin 00sin cos 00Rot z 0010 2 14 0001 注意 角旋轉(zhuǎn)的正方向遵循右手螺旋法則 如圖2 4所示 例2 2 點(diǎn)u 7i 3j 2k 它繞z軸旋轉(zhuǎn)90 為v 經(jīng)式 2 14 變換得到 sin 1 cos 0 0 1007 3100037v Rot z 90 00102 2000111起始點(diǎn)u和終點(diǎn)v如圖2 5所示 如將v點(diǎn)再繞y軸旋轉(zhuǎn)90 得到w 用式 2 13 變換得到0010 32010077w Rot y 90 10002 3000111結(jié)果如圖2 6所示 如果將上述兩次旋轉(zhuǎn)結(jié)合起來(lái) 寫(xiě)成一個(gè)表達(dá)式得到w Rot y 90 v Rot y 90 Rot z 90 u用兩個(gè)變換矩陣Rot y 90 Rot z 90 和起始點(diǎn)u代入上式計(jì)算的結(jié)果與前面分兩次計(jì)算的結(jié)果相同 為此 先將點(diǎn)u繞z軸旋轉(zhuǎn)90 然后再繞y軸旋轉(zhuǎn)90 我們得到00100 100720100100037w Rot y 90 Rot z 90 u 100000102 30001000111如果按著逆序旋轉(zhuǎn) 首先繞y軸旋轉(zhuǎn)90 然后再繞z軸旋轉(zhuǎn)90 其結(jié)果為0 10000107 31000010032w Rot z 90 Rot y 90 u 0100 10002 70001000111逆序旋轉(zhuǎn)的結(jié)果如圖2 7所示 顯然 變換的順序不同 其結(jié)果也不同 這從矩陣相乘是不可交換的 AB BA 也可以得到證明 如對(duì)經(jīng)過(guò)兩次旋轉(zhuǎn)變換得到的點(diǎn)向量w再進(jìn)行一次平移 平移向量為h 4 371 T 則可得到如圖2 8所示的點(diǎn)向量n 變換過(guò)程如下100426010 374n Trans 4 3 7 w 00173 10000111 2 6坐標(biāo)系 Coordinateframes 齊次變換矩陣H由四個(gè)列向量組成 它的前三個(gè)列向量稱(chēng)為方向向量 由式 2 12 到式 2 14 的旋轉(zhuǎn)變換 分別繞x y z軸旋轉(zhuǎn) 角 確定 第四個(gè)列向量稱(chēng)為平移向量 它的平移分量 沿x y z軸的平移量 由式 2 10 第四列的前三個(gè)元素確定 如0014100 3H Trans 4 3 7 Rot y 90 Rot z 90 0107 2 15 0001坐標(biāo)系的原點(diǎn) 即零向量 0001 T的H變換是 4 371 T 相當(dāng)于將原點(diǎn)按平移向量的各個(gè)分量進(jìn)行平移的結(jié)果 如圖2 9所示 如果對(duì)x y z軸的單位向量進(jìn)行H變換 分別得到 4 271 T 4 381 T和 5 371 T 這四個(gè)向量在圖2 9中標(biāo)出 并形成了一個(gè)新坐標(biāo)系 這個(gè)新坐標(biāo)系的x y z軸的方向分別是 0 1 0 0 T 0 0 1 0 T和 1 0 0 0 T 它是由單位向量的H變換減去這個(gè)坐標(biāo)原點(diǎn)的向量得到的 這些方向向量相應(yīng)于變換矩陣的前三列 見(jiàn)式 2 15 可見(jiàn) H變換矩陣描述了一個(gè)坐標(biāo)系繞原參考坐標(biāo)系旋轉(zhuǎn)和對(duì)參考坐標(biāo)系平移的三個(gè)軸的方向和原點(diǎn)的位置 見(jiàn)圖2 9 如圖2 10所示 當(dāng)對(duì)一個(gè)向量n進(jìn)行式 2 15 給出的H變換時(shí) 原向量n可以被認(rèn)為是在新坐標(biāo)系描述的那個(gè)向量u 即被變換了的向量u就是相對(duì)于參考坐標(biāo)系描述的同一個(gè)向量n 2 7相對(duì)變換 Relativetransformation 我們剛剛描述的旋轉(zhuǎn)和平移都是相對(duì)于一個(gè)固定的坐標(biāo)系而進(jìn)行的 這樣 在已給的例子里0014100 3Trans 4 3 7 Rot y 90 Rot z 90 0107 2 16 0001坐標(biāo)系首先繞參考坐標(biāo)系z(mì)軸旋轉(zhuǎn)90 然后繞y軸旋轉(zhuǎn)90 最后平移4i 3j 7k 如圖2 9所示 如果以相反次序從左到右來(lái)進(jìn)行這些操作 首先對(duì)坐標(biāo)平移4i 3j 7k 然后將它繞當(dāng)前坐標(biāo)系的y軸旋轉(zhuǎn)90 此時(shí)當(dāng)前坐標(biāo)系的y軸與參考坐標(biāo)系的y軸是相同的 然后再繞著新坐標(biāo)系 當(dāng)前的 坐標(biāo)系的z軸旋轉(zhuǎn)90 所得結(jié)果與前面的方法相同 見(jiàn)圖2 11 一般的情況下 如果我們用一個(gè)旋轉(zhuǎn)和 或平移變換矩陣右乘一個(gè)坐標(biāo)系的變換 那么產(chǎn)生的平移和 或旋轉(zhuǎn)是相對(duì)于前一個(gè)變換的坐標(biāo)系 當(dāng)前坐標(biāo)系 的軸來(lái)說(shuō)的 如果我們用一個(gè)描述平移和 或旋轉(zhuǎn)的變換矩陣左乘一個(gè)坐標(biāo)系的變換 那么產(chǎn)生的平移和 或旋轉(zhuǎn)是相對(duì)于基坐標(biāo)系來(lái)說(shuō)的 例2 3 給一個(gè)坐標(biāo)系C和一個(gè)變換T T為繞z軸旋轉(zhuǎn)90 并在x軸方向上平移10個(gè)單位 當(dāng)變換是相對(duì)于基坐標(biāo)系產(chǎn)生時(shí) 我們用T左乘C得到新的位置x為0 1010100200010100000 11010020 x TC 00100100 0100 2 17 000100010001當(dāng)變換是相對(duì)于當(dāng)前坐標(biāo)系C軸產(chǎn)生時(shí) 我們用T右乘C得到新的位置y為100200 10100 103000 110100000 110y CT 01000010 1000 2 18 000100010001結(jié)果如圖2 12所示 2 8物體的描述 Objectrepresentation 變換可用來(lái)描述物體的位置與方向 方位 如圖2 13所示的楔形物體用六個(gè)角點(diǎn)來(lái)描述 這六個(gè)角點(diǎn)是相對(duì)于物體所在的參考坐標(biāo)系的 如果把物體繞z軸旋轉(zhuǎn)90 然后繞y軸旋轉(zhuǎn)90 接著沿x方向平移4個(gè)單位 我們可以描述這個(gè)變換為00141000Trans 4 0 0 Rot y 90 Rot z 90 01000001這個(gè)變換表示了對(duì)參考坐標(biāo)系的旋轉(zhuǎn)和平移操作 變換后物體的六個(gè)角點(diǎn)為44664400141 1 111 11 1 111 11000000044000044 01000002201111110001111111變換后該物體在坐標(biāo)上的方位如圖2 13所示 從圖2 13可以看出 由于楔形物體的角點(diǎn)與它所在的坐標(biāo)系有固定的關(guān)系 因此沒(méi)有必要對(duì)所有的角點(diǎn)進(jìn)行變換 只要對(duì)物體所在的坐標(biāo)系進(jìn)行變換 就可得到變換后的各個(gè)角點(diǎn)在基坐標(biāo)中的位置 將這些角點(diǎn)用直線(xiàn)連接起來(lái)就可得到楔形物體的邊緣 它與逐點(diǎn)變換的結(jié)果完全相同 見(jiàn)圖2 14 2 9逆變換 Inversetransformation 所謂逆變換就是將被變換的坐標(biāo)系返回到原來(lái)的坐標(biāo)系 在數(shù)學(xué)上就是求變換矩陣的逆 下面我們寫(xiě)出變換矩陣的一般表達(dá)形式nxoxaxpxnyoyaypyT nzozazpz 2 19 0001式中n o a是旋轉(zhuǎn)變換列向量 p是平移向量 其逆是nxnynz p noxoyoz p oT 1 axayaz p a 2 20 0001式中的 表示向量的點(diǎn)積 這個(gè)結(jié)果很容易用式2 19右乘式2 20是單位矩陣來(lái)證明 2 10一般性旋轉(zhuǎn)變換 Generalrotationtransformation 前面我們介紹的旋轉(zhuǎn)變換都是繞x y z軸旋轉(zhuǎn)的旋轉(zhuǎn)變換 這些變換都有一個(gè)簡(jiǎn)單的幾何解釋 例如 在繞z軸旋轉(zhuǎn)的情況下 表示z軸保持恒定 x軸和y軸將如圖2 15所示那樣變化 如圖2 16所示 給出一個(gè)變換矩陣C 它繞任意向量k旋轉(zhuǎn) 我們把k當(dāng)作C坐標(biāo)系的z軸單位向量 nxoxax0nyoyay0C nzozaz0 2 21 0001k axi ayj azk 2 22 繞k旋轉(zhuǎn)就相等于繞C坐標(biāo)系的z軸旋轉(zhuǎn) Rot k Rot Cz 2 23 如果我們給一個(gè)坐標(biāo)系T 它在參考坐標(biāo)系里被描述 它在C坐標(biāo)系里用X描述 這樣T CX 2 24 其中X描述T相對(duì)C的位姿 求X 我們得到X C 1T 2 25 T繞k旋轉(zhuǎn)就等于 繞 坐標(biāo)系的z軸旋轉(zhuǎn)Rot k CRot z X 2 26 Rot k CRot z C 1T 2 27 這樣Rot k CRot z C 1 2 28 展開(kāi)式 2 28 我們發(fā)現(xiàn)CRot z C 1僅是k的函數(shù) 用C 1右乘Rot z 我們得到cos sin 00nxnynz0sin cos 00oxoyoz0Rot z C 1 0010axayaz000010001nxcos oxsin nycos oysin nzcos ozsin 0nxcos oxsin nycos oysin nzcos ozsin 0 axayaz0 2 29 0001再用C左乘nxoxax0nyoyay0C nzozaz0 2 30 0001 得到CRot z C 1 nxnxcos nxoxsin nxoxsin oxoxcos axaxnynxcos nyoxsin nxoysin oyoxcos ayaxnznxcos nzoxsin nxozsin ozoxcos azax0nxnycos nxoysin nyoxsin oyoxcos axaynynycos nyoysin nyoysin oyoycos ayaynznycos nzoysin nyozsin oyozcos azay0nxnzcos nxozsin nzoxsin ozoxcos axaz0nynzcos nyozsin nzoysin ozoycos ayaz0nznzcos nzozsin nzozsin ozozcos azaz0 2 31 01 應(yīng)用下列關(guān)系進(jìn)行簡(jiǎn)化 C坐標(biāo)系任意的行或列與其他行或列的點(diǎn)積為零 因?yàn)檫@些向量是正交的 C坐標(biāo)系任意的行或列與其自身的點(diǎn)積為I 因?yàn)樗鼈兪菃挝涣?z向量是x和y向量的叉積 a n o 它有下列分量ax nyoz nzoyay nzox nxozaz nxoy nyox正矢Vers 1 cos 簡(jiǎn)寫(xiě)成Vers 且kx ax ky ay kz az 由此可得到簡(jiǎn)化式為Rot k kxkxVers cos kykxVers kzsin kzkxVers kysin 0kxkyVers kzsin kykyVers cos kzkyVers kzxsin 0kxkzVers kysin kykzVers kxsin kzkzVers cos 0 2 32 0001上式是一般性的旋轉(zhuǎn)變換的重要結(jié)論 從這個(gè)結(jié)論可以得出每一個(gè)基本旋轉(zhuǎn)變換 例如 Rot x 就是Rot k 當(dāng)kx 1 ky 0 kz 0的情況 將這些值代入式 2 32 得到10000cos sin 0Rot x 0sin cos 0 2 33 0001這個(gè)結(jié)果與以前一樣 2 11等價(jià)旋轉(zhuǎn)角與旋轉(zhuǎn)軸 Equivalentangleandaxisofrotation 任給一個(gè)旋轉(zhuǎn)變換 從 2 32 方程得到一個(gè)軸 繞這個(gè)軸旋轉(zhuǎn)的等價(jià)旋轉(zhuǎn)角可由如下方法得到 已知一個(gè)旋轉(zhuǎn)變換Rnxoxax0nyoyay0R nzozaz0 2 34 0001令R和式 2 32 的Rot k 相等 并將對(duì)角線(xiàn)各項(xiàng)相加得到nx oy az 1 k2xVers cos k2yVers cos k2zVers cos 1 2 35 nx oy az k2x k2y k2z Vers 3cos 1 2cos 2 36 由此可得到旋轉(zhuǎn)角的余弦是cos 1 2 nx oy az 1 2 37 對(duì)非對(duì)角線(xiàn)項(xiàng)相減 我們得到oz ay 2kxsin 2 38 ax nz 2kysin 2 39 ny ox 2kzsin 2 40 把式 2 38 到式 2 40 兩邊平方并相加有 oz ay 2 ax nz 2 ny ox 2 4sin2 2 41 我們得到了sin 的表達(dá)式sin 1 2 oz ay 2 ax nz 2 ny ox 2 2 42 規(guī)定這個(gè)旋轉(zhuǎn)是繞k正方向旋轉(zhuǎn) 當(dāng)0 180 時(shí) 在上式中取十號(hào)是合理的 這個(gè)旋轉(zhuǎn)角 被唯一定義為tan oz ay 2 ax nz 2 ny ox 2 nx oy az 1 2 43 k的各分量為kx oz ay 2sin 2 44 ky ax nz 2sin 2 45 kz ny ox 2sin 2 46 注意 當(dāng)旋轉(zhuǎn)角 較小或接近180 時(shí) 上述三個(gè)式子的分子和分母都很小 所計(jì)算的k值是不精確的 為此可繼續(xù)根據(jù)式 2 32 和式 2 33 對(duì)應(yīng)元素以及它們的代數(shù)和相等的關(guān)系來(lái)求出k的各個(gè)分量 2 12擴(kuò)展與縮小 Stretchingandscaling 一個(gè)變換Ta0000b00T 00c0 2 47 0001將沿著x軸以a因子 沿著y軸以b因子 沿著z軸c因子均勻擴(kuò)展著各種物體 假定在一個(gè)物體上任意一個(gè)點(diǎn)xi yj zk 它的變換是axa000 xby0b00ycz 00c0z 2 48 100011這個(gè)正好表示出所說(shuō)的擴(kuò)展 這樣 一個(gè)正方體可以由這個(gè)變換變成長(zhǎng)方體 變換ss0000s00s 00s0 2 49 0001將以s為比例因子來(lái)擴(kuò)展或縮小任一物體 2 13透視變換 Perspectivetransformation 假設(shè)由一個(gè)簡(jiǎn)單透鏡把一個(gè)物體形成的像如圖2 17所示 透鏡的軸沿著y的方向 焦距為f 物體上的一個(gè)點(diǎn)x y z成象為x y z y 表示象距 它隨著物距y而變化 如果在通過(guò)y 而垂直于y的平面 照相機(jī)的底片 上畫(huà)出各個(gè)點(diǎn) 那么就形成了一個(gè)透視像 射線(xiàn)穿過(guò)透鏡中心不偏轉(zhuǎn) 則z y z y 2 50 x y x y 2 51 根據(jù)平行透鏡的軸的射線(xiàn)通過(guò)焦點(diǎn) 我們可以寫(xiě)出z f z y f 2 52 x f x y f 2 53 x y 和z 是負(fù)數(shù) 而f是正數(shù) 用式 2 50 和式 2 52 消去y 得z f z z y z f 2 54 求出x x 1 y f 2 55 y y 1 y f 2 56 z z 1 y f 2 57 齊次變換p能導(dǎo)出同樣結(jié)果 變換p是10000100p 0010 2 58 0 1 f01任何一點(diǎn)xi yj zk變換為x1000 xy0100yz 0010z 2 59 1 y f0 1 f011用比例因子1 y f除得到的象點(diǎn)x y z 有 x 1 y f i y 1 y f j z 1 y f k 2 60 這個(gè)結(jié)果與前面利用透視原理的結(jié)果完全相同 在p變換的第二列最底一元素為 1 f 則導(dǎo)出一個(gè)沿著y軸的一透視變換 如果 1 f是第三列最底一項(xiàng) 那就是沿z軸的透視變換 2 14變換方程 Transformequations 研究一下圖2 18描述的一個(gè)物體與機(jī)械手情況 機(jī)械手用變換Z相對(duì)于基坐標(biāo)系被定位 機(jī)械手的端點(diǎn)用變換ZT6來(lái)描述 而末端執(zhí)行器用變換