機器人的數(shù)學(xué)基礎(chǔ)齊次變換矩陣及其運算PPT教案

1、會計學(xué)1機器人的數(shù)學(xué)基礎(chǔ)齊次變換矩陣及其運機器人的數(shù)學(xué)基礎(chǔ)齊次變換矩陣及其運算算0001xxxxyyyyzzzznoapnoapFnoap由于各種原因，變換矩陣應(yīng)寫成方型形式，3*3或4*4均可. 為保證所表示的矩陣為方陣，如果在同一矩陣中既表示姿態(tài)又表示位置，那么可在矩陣中加入比例因子使之成為4*4矩陣。 上海電機學(xué)院 機械學(xué)院第1頁/共35頁變換可定義為空間的一個運動。 已知一直角坐標(biāo)系中的某點坐標(biāo)，那么該點在另一直角坐標(biāo)系中的坐標(biāo)可通過齊次坐標(biāo)變換來求得。變換可分為如下形式： 純平移 純旋轉(zhuǎn) 平移與旋轉(zhuǎn)的結(jié)合上海電機學(xué)院 機械學(xué)院第2頁/共35頁zzzyyyxxx110001000100

2、011zyxzyxzyx a=Trans(x, y, z)a 平移算子上海電機學(xué)院 機械學(xué)院第3頁/共35頁1000100010001),(Transzyxzyx上海電機學(xué)院 機械學(xué)院第4頁/共35頁1000110010011010A1000310030010010 A1000110020011010 A上海電機學(xué)院 機械學(xué)院第5頁/共35頁zzyxyyxxcossinsincos11000010000cossin00sincos1zyxzyx記為: a=Rot(z, )a 旋轉(zhuǎn)算子上海電機學(xué)院 機械學(xué)院第6頁/共35頁1000010000cossin00sincos),(zRot同理，繞x軸

3、、Y軸旋轉(zhuǎn)算子內(nèi)容為：繞Z軸旋轉(zhuǎn)算子內(nèi)容為：10000cossin00sincos00001),(xRot10000cos0sin00100sin0cos),(yRot上海電機學(xué)院 機械學(xué)院第7頁/共35頁如圖所示單操作手臂，并且手腕如圖所示單操作手臂，并且手腕也具有一個旋轉(zhuǎn)自由度。已知手也具有一個旋轉(zhuǎn)自由度。已知手部的起始位姿矩陣為部的起始位姿矩陣為G1.若手臂繞若手臂繞Z0軸旋轉(zhuǎn)軸旋轉(zhuǎn)90，則手，則手臂到達臂到達G2；若手臂不動，僅手；若手臂不動，僅手部繞手腕部繞手腕Z1軸轉(zhuǎn)軸轉(zhuǎn)90，則手部，則手部到達到達G3.寫出手部坐標(biāo)系寫出手部坐標(biāo)系G2、G3表達式。表達式。上海電機學(xué)院 機械學(xué)院第

4、8頁/共35頁上海電機學(xué)院 機械學(xué)院第9頁/共35頁復(fù)合變換是由固定參考坐標(biāo)系或當(dāng)前運動坐標(biāo)系的一系列沿軸平移和繞軸旋轉(zhuǎn)變換所組成的。任何變換都可以分解為按一定順序的一組平移和旋轉(zhuǎn)變換。 相對于固定坐標(biāo)系相對于動坐標(biāo)系算子左乘算子右乘上海電機學(xué)院 機械學(xué)院第10頁/共35頁Tu1237Rot( ,90 )Rot( ,90 )YZWU7001 001 0001 0010003 1 000001 02000100011 上海電機學(xué)院 機械學(xué)院第11頁/共35頁uzRotyRotTransE)90,()90,()7 , 3, 4( 上海電機學(xué)院 機械學(xué)院第12頁/共35頁上海電機學(xué)院 機械學(xué)院第13

5、頁/共35頁 齊次變換矩陣齊次變換矩陣 的數(shù)學(xué)意義：的數(shù)學(xué)意義：ABT （1）同一點在不同坐標(biāo)系）同一點在不同坐標(biāo)系B和和A中的變換；中的變換； （2）描述坐標(biāo)系）描述坐標(biāo)系B相對于坐標(biāo)系相對于坐標(biāo)系A(chǔ)的位置和方位；的位置和方位； （3）點的運動算子。）點的運動算子。pTpBABA1000401030011100TAB1000100010001),(Transzyxzyx1000010000cossin00sincos),(zRot上海電機學(xué)院 機械學(xué)院第14頁/共35頁4變換矩陣相乘變換矩陣相乘對于給定的坐標(biāo)系A(chǔ)、B、C，已知B相對A的描述為 ，C相對B的描述為 ，則pTpCBCBpTTpT

6、pCBCABBABA。從而定義復(fù)合變換 表示C相對于A的描述，是兩變換矩陣的乘積。注意：變換矩陣相乘不滿足“交換律”，變換矩陣的左乘和右乘的運動解釋不同。TTTBCABACTTTBCABACTABTBC上海電機學(xué)院 機械學(xué)院第15頁/共35頁101000CBBCBAABBCABACpRpRTTT復(fù)合變換可解釋為：TACTBCTABTBCTAC(1) 和 分別代表同一坐標(biāo)系C相對于A和B的描述。則 表示坐標(biāo)系C從 映射為 的變換。 (2)坐標(biāo)系C相對于A的描述 是這樣得到的：最初C與A重合，首先相對于A作運動 ，到達B,然后相對B作運動 ，到達最終位置C。TACTABTBC上海電機學(xué)院 機械學(xué)院

7、第16頁/共35頁如果知道坐標(biāo)系B相對于A的描述。希望得到A相對于B的描述，這是個齊次變換求逆問題。 對4*4矩陣直接求逆；利用齊次變換矩陣的特點，簡化矩陣求逆運算。求逆問題可以描述為：已知 ，求解 。TABTBA100BAABABpRT100ABBABApRT利用旋轉(zhuǎn)矩陣正交性 TABABR1R利用復(fù)合變換公式(2.13) ,求出 在B中描述。0BAp上海電機學(xué)院 機械學(xué)院第17頁/共35頁100BATABTABBApRRT0)(000ABBABABABppRp000BATABBABAABpRpRp上海電機學(xué)院 機械學(xué)院第18頁/共35頁 下面我們寫出變換矩陣的一般表達形式下面我們寫出變換矩

8、陣的一般表達形式 nx ox ax px ny oy ay py T = nz oz az pz 0 0 0 1 式中式中 n, o, a 是旋轉(zhuǎn)變換列向量，是旋轉(zhuǎn)變換列向量，p 是平移向量，其逆是是平移向量，其逆是 nx ny nz - p.n ox oy oz - p.o T- -1 = ax ay az - p.a 0 0 0 1 式中的式中的 “ . ” 表示向量的點積。表示向量的點積。上海電機學(xué)院 機械學(xué)院第19頁/共35頁 計算T矩陣的逆矩陣。0.500.86630.86605201050001T10.50.8660(3 0.52 0.8665 0)001(3 02 05 1)0.

9、8660.50(3 0.86620.55 0)0001T 0.50.86603.2300150.8660.501.5980001-0.5上海電機學(xué)院 機械學(xué)院第20頁/共35頁為了描述機器人的操作，必須建立機器人本身各連桿之間，機器人與周圍環(huán)境之間的運算關(guān)系。為此要規(guī)定各種坐標(biāo)系來描述機器人與環(huán)境的相對位姿關(guān)系。B代表基座坐標(biāo)系；W代表腕部坐標(biāo)系；T代表工具坐標(biāo)系；S代表工作站坐標(biāo)系；G代表目標(biāo)坐標(biāo)系；它們之間的位姿關(guān)系用相應(yīng)的齊次變換來描述。TBS描述工作站坐標(biāo)系相對于基座的位姿；TSG描述目標(biāo)坐標(biāo)系相對于S的位姿；TBW描述腕部W相對于基座B的位姿；上海電機學(xué)院 機械學(xué)院第21頁/共35頁

10、TGT對物體進行操作時，工具坐標(biāo)系T相對于目標(biāo)坐標(biāo)系G的位姿 直接影響操作效果。它是機器人控制和規(guī)劃的目標(biāo)。實際上，它與其他變換之間的關(guān)系類似于空間尺寸鏈，TGT則是封閉環(huán)。如圖所示，工具坐標(biāo)系T相對于基座坐標(biāo)系B的描述可用兩種變換矩陣的乘積來表示：TTTWTBWBTTTTTGTSGBSBT令上面兩式相等，則得變換方程TTTTTGTSGBSWTBW上海電機學(xué)院 機械學(xué)院第22頁/共35頁變換方程中的任一變換矩陣都可用其余的變換矩陣來表示。例如，為了對目標(biāo)物進行有效操作，工具坐標(biāo)系T相對于目標(biāo)坐標(biāo)系G的位姿 是預(yù)先規(guī)定的，需要改變 以達到這一目的，即通常規(guī)定 ，求 。TGTTBW1TTTTTWT

11、GTSGBSBW根據(jù)變換方程，可以立即求出TGTTBW上海電機學(xué)院 機械學(xué)院第23頁/共35頁kjikzyxkkk旋轉(zhuǎn)變換通式旋轉(zhuǎn)變換通式令是過原點的單位矢量，求繞k旋轉(zhuǎn)角的旋轉(zhuǎn)矩陣R(k,)。問題描述：令),(kRotRAB即R(k,)表示坐標(biāo)系B相對于參考系A(chǔ)的方位。 坐標(biāo)系坐標(biāo)系B由坐標(biāo)系由坐標(biāo)系A(chǔ)繞繞 軸旋轉(zhuǎn)軸旋轉(zhuǎn) 角得到。角得到。kkA上海電機學(xué)院 機械學(xué)院第24頁/共35頁kxAyAzAxByBzB AAyAxAz BBxByBz A B旋轉(zhuǎn)變換通式旋轉(zhuǎn)變換通式xxxABAByyyzzznokRRnoknok再定義兩坐標(biāo)系A(chǔ)和 B，分別與A和B固接，但要求(1)A和 B的z軸與k重

12、合。(2)旋轉(zhuǎn)之前A和 B重合， A和B也重合。上海電機學(xué)院 機械學(xué)院第25頁/共35頁 AAAABBABBRRRRcossin0sincos0001ABR又因為又因為所以可以得到：坐標(biāo)系B繞k軸相對于A旋轉(zhuǎn)角相當(dāng)于：坐標(biāo)系B相對于A的z軸旋轉(zhuǎn)角，保持其他關(guān)系不變。則TBBBBRRkxAyAzAxByBzB AAyAxAz BBxByBz A B坐標(biāo)系A(chǔ)經(jīng)過如下變換到坐標(biāo)系B:上海電機學(xué)院 機械學(xué)院第26頁/共35頁1cossin0sincos0001cossin0sincos0001xxxxxxAAABBABByyyyyyzzzzzzxxxxyzyyyxyzzzzxyznoknokRRRRn

13、oknoknoknoknoknnnnokooonokkkk把上式右端三矩陣相乘，并運用旋轉(zhuǎn)矩陣的正交性質(zhì)：onknkkoonkkoonn01上海電機學(xué)院 機械學(xué)院第27頁/共35頁1 c1 c1 c1 c1 c1 c1 c1 c1 cxxyxzzxyABxyzyyzyxxzyyzxzzk kck kk sk kk sRk kk sk kck kk sk kk sk kk sk kc 該式為一般旋轉(zhuǎn)齊次變換通式，概括了繞該式為一般旋轉(zhuǎn)齊次變換通式，概括了繞X X、Y Y、Z Z軸進行旋轉(zhuǎn)變換的情況。軸進行旋轉(zhuǎn)變換的情況。反之，當(dāng)給出某個旋轉(zhuǎn)齊次變換矩陣，則可求得反之，當(dāng)給出某個旋轉(zhuǎn)齊次變換矩陣，

14、則可求得k k及轉(zhuǎn)角及轉(zhuǎn)角。 變換算子公式不僅適用于點的旋轉(zhuǎn)，也適用于矢量、坐標(biāo)系、物體的旋轉(zhuǎn)變換算子公式不僅適用于點的旋轉(zhuǎn)，也適用于矢量、坐標(biāo)系、物體的旋轉(zhuǎn)。 左乘是相對固定坐標(biāo)系的變換；右乘是相對動坐標(biāo)系的變換。左乘是相對固定坐標(biāo)系的變換；右乘是相對動坐標(biāo)系的變換。 當(dāng)當(dāng)kx=1，ky=kz=0時時當(dāng)當(dāng)ky=1，kx=kz=0時時當(dāng)當(dāng)kz=1，kx=ky=0時時上海電機學(xué)院 機械學(xué)院第28頁/共35頁則可根據(jù) 求出其等效矢量k及等效轉(zhuǎn)角),(Rotk上海電機學(xué)院 機械學(xué)院第29頁/共35頁等效轉(zhuǎn)軸和等效轉(zhuǎn)角等效轉(zhuǎn)軸和等效轉(zhuǎn)角 給定旋轉(zhuǎn)矩陣給定旋轉(zhuǎn)矩陣 ，求對應(yīng)的等效旋轉(zhuǎn)軸，求對應(yīng)的等效旋轉(zhuǎn)

15、軸 和等效轉(zhuǎn)角和等效轉(zhuǎn)角ABRkxxxyyyzzznoaRnoanoa設(shè)設(shè) ，令令 1 c1 c1 c1 c1 c1 c1 c1 c1 cxxxyyyzzzxxyxzzxyxyzyyzyxxzyyzxzznoaRnoanoak kck kk sk kk sk kk sk kck kk sk kk sk kk sk kc上海電機學(xué)院 機械學(xué)院第30頁/共35頁得到：得到：方程兩邊矩陣的非對角元素成對相減，得到：方程兩邊矩陣的非對角元素成對相減，得到：2sin2sin2sinzyxxzyyxzoakanknok2221 cos3cosxyzxyznoakkk1cos12xyznoa兩邊平方后相加，

16、所以整理后得到：兩邊平方后相加，所以整理后得到：2221sin2zyxzyxoaanno 所以，所以，12cosxyznoa 上海電機學(xué)院 機械學(xué)院第31頁/共35頁所以：所以：方程兩邊矩陣的非對角元素成對相減，整理得到：方程兩邊矩陣的非對角元素成對相減，整理得到：2sin2sin2sinzyxxzyyxzoakanknok222tan1zyxzyxxyzoaannonoa （1）多值性：）多值性： 和和 值并不唯一，一般選取值并不唯一，一般選取 。（2）病態(tài)情況：當(dāng)）病態(tài)情況：當(dāng) 很小時，轉(zhuǎn)軸很小時，轉(zhuǎn)軸 不能確定，需要其它方法。不能確定，需要其它方法。注意注意：kk0 ,1 8 0上海電機學(xué)院 機械學(xué)院第32頁/共35頁例題：已知轉(zhuǎn)動變換矩陣1000001000010100)90,()90,(zRotyRot試求：等效轉(zhuǎn)軸與轉(zhuǎn)角?？梢宰C明，任何一組繞過原點的軸線的復(fù)合轉(zhuǎn)動總是等效于繞某一過原點的軸線的轉(zhuǎn)動R(k,).上海電機學(xué)院 機械學(xué)院第33頁/共35頁為什么說任一為什么說任一4