機器人原理及控制技術(shù)：3-3第3章-位姿描述和齊次變換

1、第三章 位姿描述和齊次變換3.3 齊次坐標(biāo)及齊次坐標(biāo)變換一、齊次坐標(biāo) 所謂齊次坐標(biāo)就是將一個原本是n維的向量用一個n+1維向量來表示。例如，二維點(x,y)的齊次坐標(biāo)表示為(hx,hy,h)。由此可以看出，一個向量的齊次表示是不唯一的，齊次坐標(biāo)的h取不同的值都表示的是同一個點，比如齊次坐標(biāo)(8,4,2)、(4,2,1)表示的都是二維點(4,2)。 不同時為0的任意4個數(shù)稱為 三維空間點的齊次坐標(biāo)。齊次坐標(biāo)與點的笛卡爾坐標(biāo)的關(guān)系為：a= , b= , c= ，w為比例系數(shù) 顯然，齊次坐標(biāo)表達并不是唯一的，隨w值的不同而不同。在計算機圖學(xué)中，w 作為通用比例因子，它可取任意正值，但在機器人的運動分

2、析中，總是取w=1 。1、矢量的齊次坐標(biāo)三維空間矢量式中i, j, k為x, y, z 軸上的單位矢量，V的齊次坐標(biāo)表示式為例：可以表示為： V=3 4 5 1T 或 V=6 8 10 2T 或 V=-12 -16 -20 -4T 幾個特定意義的齊次坐標(biāo)：0, 0, 0, 1T坐標(biāo)原點矢量的齊次坐標(biāo)， 1 0 0 0TOX軸線上無窮遠(yuǎn)點；0 1 0 0TOY軸線上無窮遠(yuǎn)點； 0 0 1 0TOZ軸線上無窮遠(yuǎn)點；機器人研究所6第3節(jié) 坐標(biāo)變換平移坐標(biāo)變換 (Translation Transform)坐標(biāo)系B與A方向相同，但原點不重合。圖4 平移變換 此式稱為平移方程。其中 是B系原點OB在A系

3、中的表示。PY1X1Z1Y2X2Z2Y3X3Z3三坐標(biāo)的直角坐標(biāo)機器人適用的機器人類型舉例（有平移關(guān)節(jié)）YXZ機器人研究所9第3節(jié) 坐標(biāo)變換旋轉(zhuǎn)坐標(biāo)變換 (Rotation Transform)B與A有共同的坐標(biāo)原點，但方位不同。圖5 旋轉(zhuǎn)變換P機器人研究所10第3節(jié) 坐標(biāo)變換復(fù)合變換 (Composite Transform) 圖6 復(fù)合變換P機器人研究所11第4節(jié) 齊次坐標(biāo)變換齊次坐標(biāo)和齊次變換坐標(biāo)變換 ，式中對于點 是非齊次的，將其等價為齊次變換形式：等價于齊次變換直角坐標(biāo)齊次坐標(biāo)機器人研究所12第4節(jié) 齊次坐標(biāo)變換齊次坐標(biāo)和齊次變換 稱為齊次變換矩陣，對它有以下物理理解：描述坐標(biāo)系 B

4、 相對于坐標(biāo)系 A 的位姿；代表同一點 P 在兩個坐標(biāo)系 A 和 B 中描述之間的映射關(guān)系；表示同一坐標(biāo)系中，點 P 運動前后的位姿關(guān)系。二、笛卡爾坐標(biāo)系的齊次坐標(biāo)變換笛卡爾坐標(biāo)系 中的點 向另一坐標(biāo)系 變換，變換后的坐標(biāo)系 由下式計算： 式中 ：坐標(biāo)系 的原點在坐標(biāo)系 的坐標(biāo)；：坐標(biāo)系 的 軸對坐標(biāo)系 的3個方向余弦；：坐標(biāo)系 的 軸對坐標(biāo)系 的3個方向余弦；：坐標(biāo)系 的 軸對坐標(biāo)系 的3個方向余弦；若 是 系的齊次坐標(biāo)； 是 系的齊次坐標(biāo)，則將此式寫成矩陣 形式，有上式T是一個44階矩陣，稱為笛卡爾坐標(biāo)系的齊次變換矩陣，它溝通了兩個坐標(biāo)系的關(guān)系，表示了在坐標(biāo)系 中的點 ，經(jīng)T變換后變成了坐

5、標(biāo)系 中的點X意義：左上角的33矩陣 是兩個坐標(biāo)系之間的旋轉(zhuǎn)變換矩陣，它描述了姿態(tài)關(guān)系；右上角的31矩陣 是兩個坐標(biāo)系之間的平移變換矩陣，它描述了位置關(guān)系，所以齊次坐標(biāo)變換矩陣又稱為位姿矩陣。 例題已知坐標(biāo)系 向 系的齊次坐標(biāo)變換如下式：試決定 系的原點在 系中的位置。機器人研究所18例2.1 已知坐標(biāo)系B的初始位姿與A重合，首先B相對于坐標(biāo)系A(chǔ)的zA軸轉(zhuǎn)30，再沿A的xA軸移動12單位，并沿A的yA軸移動6單位。求位置矢量ApB0和旋轉(zhuǎn)矩陣 。假設(shè)點p在坐標(biāo)系B的描述為Bp=3,7,0T，求它在坐標(biāo)系A(chǔ)中的描述Ap。18第4節(jié) 齊次坐標(biāo)變換解：機器人研究所19例2.1 已知坐標(biāo)系B的初始位姿

6、與A重合，首先B相對于坐標(biāo)系A(chǔ)的zA軸轉(zhuǎn)30，再沿A的xA軸移動12單位，并沿A的yA軸移動6單位。求位置矢量ApB0和旋轉(zhuǎn)矩陣 。假設(shè)點p在坐標(biāo)系B的描述為Bp=3,7,0T，求它在坐標(biāo)系A(chǔ)中的描述Ap。19第4節(jié) 齊次坐標(biāo)變換解：機器人研究所20第4節(jié) 齊次坐標(biāo)變換例2.2 齊次變換矩陣描述坐標(biāo)系B相對于A的位姿，平移變換矩陣旋轉(zhuǎn)變換矩陣 表示繞過坐標(biāo)原點的軸k旋轉(zhuǎn)角度。 任何一個齊次坐標(biāo)變換矩陣均可分解為一個平移變換矩陣與一個旋轉(zhuǎn)變換矩陣的乘積，即： 機器人研究所24第3節(jié) 齊次坐標(biāo)變換平移齊次變換(Homogeneous Transformation of Translation)A分

7、別沿B的X、Y、Z坐標(biāo)軸平移a、b、c距離的平移齊次變換矩陣寫為：機器人研究所25第4節(jié) 齊次坐標(biāo)變換平移齊次變換(Homogeneous Transformation of Translation)對已知矢量 u=x, y, z, 1T 進行平移變換所得的矢量 v 為：機器人研究所26第4節(jié) 齊次坐標(biāo)變換旋轉(zhuǎn)齊次變換(Homogeneous Transformation of Rotation)機器人研究所27第4節(jié) 齊次坐標(biāo)變換復(fù)合變換給定坐標(biāo)系A(chǔ)，B和C，已知B相對A的描述為 ，C相對B的描述為 ，則有 同理可有：即一個坐標(biāo)系變換至另一坐標(biāo)系的齊次變換矩陣等于依次經(jīng)歷中間坐標(biāo)系各齊次變換

8、矩陣的連乘積。 機器人研究所2828例2.3 已知點 u=7, 3, 2T ，將 u繞 z 軸旋轉(zhuǎn)90得到點 v，再將點 v 繞 y軸旋轉(zhuǎn)90得到點w，求點v、w的坐標(biāo)。解：第4節(jié) 齊次坐標(biāo)變換旋轉(zhuǎn)變換機器人研究所2929例2.3 已知點 u=7, 3, 2T ，將 u繞 z 軸旋轉(zhuǎn)90得到點 v，再將點 v 繞 y軸旋轉(zhuǎn)90得到點w，求點v、w的坐標(biāo)。解：如果把上述兩變換組合在一起第4節(jié) 齊次坐標(biāo)變換旋轉(zhuǎn)變換機器人研究所30若改變旋轉(zhuǎn)次序，首先使 u 繞 y 軸旋轉(zhuǎn)90，再繞 z 軸旋轉(zhuǎn)90，會使 u 變換至與 w 不同的位置w1。第4節(jié) 齊次坐標(biāo)變換旋轉(zhuǎn)次序?qū)Y(jié)果的影響機器人研究所31第4

9、節(jié) 齊次坐標(biāo)變換例2.4 已知點u=7, 3, 2T ，將 u繞 z 軸旋轉(zhuǎn)90得到點 v，再將點 v 繞 y軸旋轉(zhuǎn)90得到點w，最后進行平移變換4, -3, 7T ，求最終的坐標(biāo)。解：將上述三個變換組合在一起平移變換和旋轉(zhuǎn)變換組合vwunzoyx機器人研究所32第4節(jié) 齊次坐標(biāo)變換例2.4 已知點u=7, 3, 2T ，將 u繞 z 軸旋轉(zhuǎn)90得到點 v，再將點 v 繞 y軸旋轉(zhuǎn)90得到點w，最后進行平移變換4, -3, 7T ，求最終的坐標(biāo)。解：將上述三個變換組合在一起平移變換和旋轉(zhuǎn)變換組合zoyxvwun機器人研究所33第5節(jié) 齊次變換的性質(zhì)1、變換過程的相對性繞固定坐標(biāo)系依次進行的坐標(biāo)

10、系轉(zhuǎn)換，各齊次變換矩陣按“從右向左”依次相乘原則進行運算（右乘）. 3 2 1 坐標(biāo)系的運動方式：B的初始方位與坐標(biāo)系A(chǔ)重合，首先使B繞 xA旋轉(zhuǎn) 角，再繞 yA轉(zhuǎn) 角，最后繞zA轉(zhuǎn) 角。機器人研究所34第5節(jié) 齊次變換的性質(zhì)1、變換過程的相對性繞動坐標(biāo)系依次進行的齊次變換，按“從左向右”的原則依次相乘(左乘）。 坐標(biāo)系的運動方式：B的初始方位與坐標(biāo)系A(chǔ)重合，首先使B繞 zB旋轉(zhuǎn) 角，再繞 yB轉(zhuǎn) 角，最后繞 xB轉(zhuǎn) 角。機器人研究所35相對于固定坐標(biāo)系運動相對于活動坐標(biāo)系運動第5節(jié) 齊次變換的性質(zhì)1、變換過程的相對性結(jié)論：1）變換順序從右至左，運動是相對于固定參考系而言的；2）變換順序從左至

11、右，運動是相對于運動坐標(biāo)系而言的。方式1：方式2：2、變換過程的可逆性機器人研究所37第5節(jié) 齊次變換的性質(zhì)2、變換過程的可逆性齊次變換過程是可逆的，逆變換就是使被變換的動坐標(biāo)系返回到固定坐標(biāo)系中。例如： 將被變換坐標(biāo)系變回到原來的坐標(biāo)系時，可以用變換T的逆 來實現(xiàn)。 例如X=TC 使C變換為X，若用X求C則為 X= TC=C=C式中為單位矩陣。2、變換過程的可逆性6、逆變換齊次變換的逆變換：齊次變換矩陣對于給定的 求等價于給定 和 計算6、逆變換若齊次坐標(biāo)變換矩陣為：6、逆變換6、逆變換機器人研究所43第5節(jié) 齊次變換的性質(zhì)從逆方向去看圖，固定系的 x軸與動系的 z 軸方向一致，故x軸在動系

12、中可表示為0, 0, 1, 0T，同樣固定系的 y 軸可表示為1, 0, 0, 0 T，z軸可表示為0, 1, 0, 0 T，而固定系的原點可表示為3, -7, -4 , 1 T 。2、變換過程的可逆性T表示B與A之間的變換，也即B在A中的描述；下面從另一角度分析一下A在B中的描述。機器人研究所44第5節(jié) 齊次變換的性質(zhì)2、變換過程的可逆性于是，A在B系中的描述為：容易驗證機器人研究所45第5節(jié) 齊次變換的性質(zhì)2、變換過程的可逆性齊次變換逆變換的公式：已知變換矩陣為:其逆變換矩陣為：機器人研究所46第5節(jié) 齊次變換的性質(zhì)2、變換過程的可逆性例題：已知齊次矩陣為：求 A-1機器人研究所47第5節(jié)

13、 齊次變換的性質(zhì)2、變換過程的可逆性例題：已知齊次矩陣為：求 A-1解： -Pn = -01-02-(-1)3=3 -Po = -01-12-03= -2 -Pa = -11-02-03= -1則有第1節(jié) 位置和姿態(tài)的表示第2節(jié) 坐標(biāo)變換第3節(jié) 齊次坐標(biāo)變換第4節(jié) 齊次變換的性質(zhì)第5節(jié) 旋轉(zhuǎn)變換通式第三章 位姿描述和齊次變換機器人研究所49第6節(jié) 旋轉(zhuǎn)變換通式1、旋轉(zhuǎn)變換通式一般旋轉(zhuǎn)變換指旋轉(zhuǎn)軸線不與參考坐標(biāo)系中的任何軸線重合，而是參考系中過原點的某一矢量，這一矢量的方向用單位矢量 表示。令 是過A系原點的單位矢量，求繞K旋轉(zhuǎn)角到B系的旋轉(zhuǎn)矩陣R(K,)，即 機器人研究所50第6節(jié) 旋轉(zhuǎn)變換通

14、式1、旋轉(zhuǎn)變換通式設(shè)K是某坐標(biāo)系C的Z軸的單位向量，并設(shè)：這樣，繞矢量k旋轉(zhuǎn)就等于繞坐標(biāo)系C的Z軸旋轉(zhuǎn)，即 Rot(K,)=Rot(ZC,) 機器人研究所51第6節(jié) 旋轉(zhuǎn)變換通式1、旋轉(zhuǎn)變換通式如果被旋轉(zhuǎn)的坐標(biāo)系以參考坐標(biāo)系描述時，記為Y,以坐標(biāo)系C為參考系時記為X，Y與X的關(guān)系為：繞k軸旋轉(zhuǎn)Y等效于繞坐標(biāo)系C的Z軸旋轉(zhuǎn)X,即將 代入得：機器人研究所52第6節(jié) 旋轉(zhuǎn)變換通式1、旋轉(zhuǎn)變換通式當(dāng)kx=1, ky=kz=0時，即 K為 x 軸，此時其中：機器人研究所53第6節(jié) 旋轉(zhuǎn)變換通式2、 等效轉(zhuǎn)軸與等效轉(zhuǎn)角任何一組經(jīng)過有限次基本旋轉(zhuǎn)變換后的復(fù)合旋轉(zhuǎn)總可以等效成繞某一過原點的軸線轉(zhuǎn)角的單一旋轉(zhuǎn)。對于給定的旋轉(zhuǎn)矩陣機器人研究所54第6節(jié) 旋轉(zhuǎn)變換通式2、 等效轉(zhuǎn)軸與等效轉(zhuǎn)角球等效轉(zhuǎn)軸 K 和等效轉(zhuǎn)角，即解下面的方程組。機器人研究所55機器人研究所56第5節(jié) 旋轉(zhuǎn)變換通式2、 等效轉(zhuǎn)軸與等效轉(zhuǎn)角例題：求復(fù)合變換 的等效轉(zhuǎn)軸K和轉(zhuǎn)角。解：1. 計算旋轉(zhuǎn)矩陣 機器人研究所57第6節(jié) 旋