機(jī)器人的數(shù)學(xué)基礎(chǔ)

1、1提綱提綱2.1 2.1 位置和姿態(tài)的表示位置和姿態(tài)的表示2.2 2.2 坐標(biāo)變換坐標(biāo)變換2.3 2.3 齊次坐標(biāo)變換齊次坐標(biāo)變換2.4 2.4 物體的變換及逆變換物體的變換及逆變換2.5 2.5 通用旋轉(zhuǎn)變換通用旋轉(zhuǎn)變換2Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.1 2.1 位置和姿態(tài)的表示位置和姿態(tài)的表示1.位置描述 在直角坐標(biāo)系A(chǔ)中,空間任意一點(diǎn)p的位置(Position)可用3x1列向量(位置矢量)表示:2.方位描述 空間物體B的方位(Orientation)可由某個(gè)固接于此物體的坐標(biāo)系B的三個(gè)單位主矢量xB,yB,zB相對(duì)于參考坐標(biāo)系A(chǔ)的方向余弦組成的3x3矩陣描述. TzyxApppP

2、3Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.1 2.1 位置和姿態(tài)的表示位置和姿態(tài)的表示 上述矩陣稱為旋轉(zhuǎn)矩陣,它是正交的.即 若坐標(biāo)系B可由坐標(biāo)系A(chǔ),通過(guò)繞A的某一坐標(biāo)軸獲得,則繞x,y,z三軸的旋轉(zhuǎn)矩陣分別為10000),(00100),(00001),(cssczcsscycsscxRRR11BBRABTABAB333231232221131211rrrrrrrrrABR4Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.1 2.1 位置和姿態(tài)的表示位置和姿態(tài)的表示 這些旋轉(zhuǎn)變換可以通過(guò)右圖推導(dǎo)這是繞Z軸的旋轉(zhuǎn). 其它兩軸只要把坐標(biāo)次序調(diào)換可得上頁(yè)結(jié)果.pBpApBpBpApBpBpAzzyxyyxxc

3、ossinsincospBpBpBpApApAzyxzyx1000cossin0sincos5Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.1 2.1 位置和姿態(tài)的表示位置和姿態(tài)的表示旋轉(zhuǎn)矩陣的幾何意義:1) 可以表示固定于剛體上的坐標(biāo)系B對(duì)參考坐標(biāo)系的姿態(tài)矩陣.2) 可作為坐標(biāo)變換矩陣.它使得坐標(biāo)系B中的點(diǎn)的坐標(biāo) 變換成A中點(diǎn)的坐標(biāo) .3) 可作為算子,將B中的矢量或物體變換到A中.RABRABpBpARAB6Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.1 2.1 位置和姿態(tài)的表示位置和姿態(tài)的表示3.位姿描述 剛體位姿(即位置和姿態(tài)),用剛體的方位矩陣和方位參考坐標(biāo)的原點(diǎn)位置矢量表示，即 0BAABpRB

4、 7Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.2 2.2 坐標(biāo)變換坐標(biāo)變換1.1. 平移坐標(biāo)變換平移坐標(biāo)變換 坐標(biāo)系A(chǔ)和B具有相同的方位,但原點(diǎn)不重合.則點(diǎn)P在兩個(gè)坐標(biāo)系中的位置矢 量 滿 足 下 式 :0BABAPPP8Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.2 2.2 坐標(biāo)變換坐標(biāo)變換2.2.旋轉(zhuǎn)變換旋轉(zhuǎn)變換 坐標(biāo)系A(chǔ)和B有相同的原點(diǎn)但方位不同,則點(diǎn)P的在兩個(gè)坐標(biāo)系中的位置矢量有如下關(guān)系:PRPBABATABABBARRR19旋轉(zhuǎn)矩陣-舉例例1 已知轉(zhuǎn)動(dòng)坐標(biāo)系OUVW中的兩點(diǎn)aUVW(4，3，2) T和bUVW(6，2，4) T，若OUVW系統(tǒng)繞OZ 軸轉(zhuǎn)動(dòng)了60。，試求參考坐標(biāo)系中的相應(yīng)點(diǎn)ax

5、yz和bxyz。 解 uvwZxyzuvwZxyzbRbaRa0060,60, Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)10旋轉(zhuǎn)矩陣-舉例例2 已知參考坐標(biāo)系OXYZ中的兩點(diǎn)aXYZ(4，3，2) T和bXYZ(6，2，4) T，若OUVW系統(tǒng)繞OZ 軸轉(zhuǎn)動(dòng)了60。，試求轉(zhuǎn)動(dòng)坐標(biāo)系中的相應(yīng)點(diǎn)aUVW和bUVW。 解 xyzTZxyzxyzTZuvwbRbaRa0060,60, Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)11 合成旋轉(zhuǎn)矩陣合成旋轉(zhuǎn)矩陣: :例例1：在動(dòng)坐標(biāo)中有一固定點(diǎn)：在動(dòng)坐標(biāo)中有一固定點(diǎn) ，相對(duì)固，相對(duì)固定參考坐標(biāo)系定參考坐標(biāo)系 做如下運(yùn)動(dòng)：做如下運(yùn)動(dòng)： R（x, 90）；）； R(z, 9

6、0)； R(y,90)。求點(diǎn)。求點(diǎn) 在固定參考坐標(biāo)系在固定參考坐標(biāo)系 下的位置。下的位置。 TuvwPo321OxyzuvwPoOxyz解解1：用畫圖的簡(jiǎn)單方法：用畫圖的簡(jiǎn)單方法 Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)12解解2：用分步計(jì)算的方法：用分步計(jì)算的方法 R（x, 90） R（z, 90） R（y, 90） 2313210101-00001P21323110000101-0 P312213001-010100 P（2-14） （2-15） （2-16） Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)13 上述計(jì)算方法非常繁瑣，可以通過(guò)一系列計(jì)算得到上述上述計(jì)算方法非常繁瑣，可以通過(guò)一系列計(jì)算得到上述

7、結(jié)果。將式（結(jié)果。將式（2-14）（）（2-15）（）（2-16）聯(lián)寫為如下形式：）聯(lián)寫為如下形式： wvuzyxPPPRPPP33R3x3為二者之間的關(guān)系矩陣，我們令：為二者之間的關(guān)系矩陣，我們令： ),(),(),RR33xRzRy（定義定義1： 當(dāng)動(dòng)坐標(biāo)系當(dāng)動(dòng)坐標(biāo)系 繞固定坐標(biāo)系繞固定坐標(biāo)系 各坐標(biāo)軸順序有限各坐標(biāo)軸順序有限次轉(zhuǎn)動(dòng)時(shí)，其合成旋轉(zhuǎn)矩陣為各基本旋轉(zhuǎn)矩陣依旋轉(zhuǎn)順序次轉(zhuǎn)動(dòng)時(shí)，其合成旋轉(zhuǎn)矩陣為各基本旋轉(zhuǎn)矩陣依旋轉(zhuǎn)順序左乘左乘。注意：旋轉(zhuǎn)矩陣間不可以交換注意：旋轉(zhuǎn)矩陣間不可以交換 uvwOOxyzRobotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)14旋轉(zhuǎn)次序?qū)ψ儞Q結(jié)果的影響Robotics 數(shù)學(xué)基礎(chǔ)

8、數(shù)學(xué)基礎(chǔ)15合成旋轉(zhuǎn)矩陣為了表示繞為了表示繞OXYZOXYZ坐標(biāo)系各軸的一連串有限轉(zhuǎn)動(dòng)，可把基本旋轉(zhuǎn)矩陣連坐標(biāo)系各軸的一連串有限轉(zhuǎn)動(dòng)，可把基本旋轉(zhuǎn)矩陣連乘起來(lái)。由于矩陣乘法不可交換，故完成轉(zhuǎn)動(dòng)的次序是重要的。例如，乘起來(lái)。由于矩陣乘法不可交換，故完成轉(zhuǎn)動(dòng)的次序是重要的。例如，先繞先繞OXOX軸轉(zhuǎn)軸轉(zhuǎn) 角，然后繞角，然后繞OZOZ袖轉(zhuǎn)袖轉(zhuǎn) 角，再繞角，再繞OYOY轉(zhuǎn)轉(zhuǎn) 角；表示這種轉(zhuǎn)動(dòng)的角；表示這種轉(zhuǎn)動(dòng)的旋轉(zhuǎn)矩陣為旋轉(zhuǎn)矩陣為 如果轉(zhuǎn)動(dòng)的次序變化為，先繞OY轉(zhuǎn)角繞OX軸轉(zhuǎn)角，然后繞OZ袖轉(zhuǎn)角，再繞OX軸轉(zhuǎn)角；表示這種轉(zhuǎn)動(dòng)的旋轉(zhuǎn)矩陣為 Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)16除繞OXYZ參考系的坐標(biāo)軸

9、轉(zhuǎn)動(dòng)外，OUVW坐標(biāo)系也可以繞它自己的坐標(biāo)軸轉(zhuǎn)動(dòng)。這時(shí)，合成旋轉(zhuǎn)矩陣可按下述簡(jiǎn)單規(guī)則求得：1. 兩坐標(biāo)系最初重合，因此旋轉(zhuǎn)矩陣是一個(gè)33單位矩陣I3。2如果OUVW坐標(biāo)系繞OXYZ坐標(biāo)系的一坐標(biāo)軸轉(zhuǎn)動(dòng)，則可對(duì)上述旋轉(zhuǎn)矩陣左乘相應(yīng)的基本旋轉(zhuǎn)矩陣。3如果OUVW坐標(biāo)系繞自己的一坐標(biāo)鈾轉(zhuǎn)動(dòng)，則可對(duì)上述旋轉(zhuǎn)矩陣右乘相應(yīng)的基本旋轉(zhuǎn)矩陣合成旋轉(zhuǎn)矩陣規(guī)則先繞先繞OY軸轉(zhuǎn)軸轉(zhuǎn) 角，角，然后繞然后繞OW袖轉(zhuǎn)袖轉(zhuǎn)角，再繞角，再繞OU轉(zhuǎn)轉(zhuǎn)角；表示這種轉(zhuǎn)角；表示這種轉(zhuǎn)動(dòng)的旋轉(zhuǎn)矩陣為動(dòng)的旋轉(zhuǎn)矩陣為Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)17Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.2 2.2 坐標(biāo)變換坐標(biāo)變換3.3.復(fù)合變換復(fù)合

10、變換 一般情況原點(diǎn)既不重和,方位也不同.這時(shí)有: (2-13)0BABABAPPRP18Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.2 2.2 坐標(biāo)變換坐標(biāo)變換例例2.12.1 已知坐標(biāo)系B的初始位姿與A重合,首先B相對(duì)于A的ZA軸轉(zhuǎn)30,再沿A的XA軸移動(dòng)12單位,并沿A的YA軸移動(dòng)6單位.求位置矢量APB0和旋轉(zhuǎn)矩陣BAR.設(shè)點(diǎn)p在B坐標(biāo)系中的位置為BP=3,7,0,求它在坐標(biāo)系A(chǔ)中的位置.0612;1000866.05 .005 .0866.0)30,(00BAABzRpR0562.13908.1106120562. 7902. 00BABABAppRp19 開始20 一般來(lái)說(shuō)，n維空間的齊次

11、坐標(biāo)表示是一個(gè)（n+1）維空間實(shí)體。有一個(gè)特定的投影附加于n維空間，也可以把它看作一個(gè)附加于每個(gè)矢量的特定坐標(biāo)比例系數(shù)。 kcj bi av zy x TwwzyxV式中式中i, j, k為為x, y, z 軸上的單位矢量，軸上的單位矢量，a= , b= , c= ，w為比例為比例系數(shù)系數(shù) wxwywz 顯然，齊次坐標(biāo)表達(dá)并不是唯一的，隨顯然，齊次坐標(biāo)表達(dá)并不是唯一的，隨w值的不同而不同。在計(jì)算機(jī)圖學(xué)中，值的不同而不同。在計(jì)算機(jī)圖學(xué)中，w 作為作為通用比例因子通用比例因子，它可取任意正值，但，它可取任意正值，但在機(jī)器人的運(yùn)動(dòng)分析中，總是取在機(jī)器人的運(yùn)動(dòng)分析中，總是取w=1 。列矩陣列矩陣Rob

12、otics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.3 齊次坐標(biāo)變換齊次坐標(biāo)變換 - 齊次坐標(biāo)齊次坐標(biāo)21例：kjiV543可以表示為：可以表示為： V=3 4 5 1V=3 4 5 1T T 或或 V=6 8 10 2V=6 8 10 2T T 或或 V=-12 -16 -20 -4V=-12 -16 -20 -4T T Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.3 齊次坐標(biāo)變換齊次坐標(biāo)變換 - 齊次坐標(biāo)齊次坐標(biāo)22 齊次坐標(biāo)與三維直角坐標(biāo)的區(qū)別 V點(diǎn)在OXYZ坐標(biāo)系中表示是唯一的（x、y、z） 而在齊次坐標(biāo)中表示可以是多值的。不同的表示方法代表的V點(diǎn)在空間位置上不變。 xyzzzxV圖2-2oRobotics 數(shù)

13、學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.3 齊次坐標(biāo)變換齊次坐標(biāo)變換 - 齊次坐標(biāo)齊次坐標(biāo)23 幾個(gè)特定意義的齊次坐標(biāo)： 0, 0, 0, nT坐標(biāo)原點(diǎn)矢量的齊次坐標(biāo)，n為任意非零比例系數(shù) 1 0 0 0T指向無(wú)窮遠(yuǎn)處的OX軸 0 1 0 0T指向無(wú)窮遠(yuǎn)處的OY軸 0 0 1 0T指向無(wú)窮遠(yuǎn)處的OZ軸 這樣，利用齊次坐標(biāo)不僅可以規(guī)定點(diǎn)的位置，還可以用來(lái)規(guī)定矢量的方向。第四個(gè)元素非零時(shí)，代表點(diǎn)的位置；第四個(gè)元素為零時(shí)，代表方向。Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.3 齊次坐標(biāo)變換齊次坐標(biāo)變換 - 齊次坐標(biāo)齊次坐標(biāo)24Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.3 2.3 齊次坐標(biāo)變換齊次坐標(biāo)變換1.1.齊次變換齊次變換

14、(2-13)式可以寫為: (2-14)P點(diǎn)在A和B中的位置矢量分別增廣為:而齊次變換公式和變換矩陣變?yōu)? (2-15,16)11010PPRPBBAABATBBBBTAAAAzyxzyx1,1PP10,0BAABABBABAPRTPTP25Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.3 2.3 齊次坐標(biāo)變換齊次坐標(biāo)變換2.2.平移齊次坐標(biāo)變換平移齊次坐標(biāo)變換 AA分別沿B的X、Y、Z坐標(biāo)軸平移a、b、c距離的平移齊次變換矩陣寫為：用非零常數(shù)乘以變換矩陣的每個(gè)元素，不改變特性。例例2-3:求矢量2i+3j+2k被矢量4i-3j+7k平移得到的新矢量.1000100010001),(cbacbaTran

15、s19061232100071003010400126Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.3 2.3 齊次坐標(biāo)變換齊次坐標(biāo)變換3.3.旋轉(zhuǎn)齊次坐標(biāo)變換旋轉(zhuǎn)齊次坐標(biāo)變換將上式增廣為齊次式：10000),(00100),(00001),(cssczcsscycsscxRRR100001000000),(100000001000),(100000000001),(cssczcsscycsscxRRR27Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.3 2.3 齊次坐標(biāo)變換齊次坐標(biāo)變換 引入齊次變換后，連續(xù)的變換可以變成矩陣的連乘形式。計(jì)算簡(jiǎn)化。 例2-4 ：U=7i+3j+2k,繞Z軸轉(zhuǎn)90度后，再繞Y軸

16、轉(zhuǎn)90度。例2-5：在上述基礎(chǔ)上再平移（4，-3，7）。1273; ; ;1237; ; ;1000010000010010)90,(zR001032010077( ,90);100023000111R y 100426010374(4, 3,7);0017310000111Trans 28舉例說(shuō)明：舉例說(shuō)明：例例1：動(dòng)坐標(biāo)系：動(dòng)坐標(biāo)系0起始位置與固定參考坐標(biāo)系起始位置與固定參考坐標(biāo)系0重合重合,動(dòng)坐標(biāo)動(dòng)坐標(biāo)系系0做如下運(yùn)動(dòng)：做如下運(yùn)動(dòng)：R(Z,90) R（y,90） Trans(4，-3, 7)，求合成矩陣，求合成矩陣 解解1：用畫圖的方法：用畫圖的方法： ozyx74-3owuvvuwzy

17、xoo(o)xyzuvwzyxuwo(o) vRobotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.3 2.3 齊次坐標(biāo)變換齊次坐標(biāo)變換相對(duì)變換相對(duì)變換29解解2：用計(jì)算的方法：用計(jì)算的方法 TTrans(4, -3, 7) R(y, 90 ) R(Z,90 )00141003 01070001 以上均以固定坐標(biāo)系多軸為變換基準(zhǔn)，因此矩陣左乘。以上均以固定坐標(biāo)系多軸為變換基準(zhǔn)，因此矩陣左乘。如果我們做如下變換，也可以得到相同的結(jié)果：如果我們做如下變換，也可以得到相同的結(jié)果： 例例2：先平移：先平移Trans (4,-3,7)；繞當(dāng)前；繞當(dāng)前 軸轉(zhuǎn)動(dòng)軸轉(zhuǎn)動(dòng)90；繞當(dāng)前繞當(dāng)前 軸轉(zhuǎn)動(dòng)軸轉(zhuǎn)動(dòng)90；求合成旋轉(zhuǎn)矩陣。；

18、求合成旋轉(zhuǎn)矩陣。 vw （2-202-20）Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.3 2.3 齊次坐標(biāo)變換齊次坐標(biāo)變換相對(duì)變換相對(duì)變換30解解1：用畫圖的方法：用畫圖的方法 zyxo(o)vwuzyxoowuvozyxowvuxyzoowuv解解2：用計(jì)算的方法：用計(jì)算的方法 o00141003TTrans(4, -3, 7) R(y, 90 ) R(Z,90 )01070001o（2-212-21）Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.3 2.3 齊次坐標(biāo)變換齊次坐標(biāo)變換相對(duì)變換相對(duì)變換31Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.3 2.3 齊次坐標(biāo)變換齊次坐標(biāo)變換 由矩陣乘法沒(méi)有交換性，可知

19、變換次序?qū)Y(jié)果影響很大。1000701030014100)90,()90,()7 , 3, 4(zRotyRotTrans32式（式（2-202-20）和式（）和式（2-212-21）無(wú)論在形式上，還是在結(jié)果上都是一致的。因此）無(wú)論在形式上，還是在結(jié)果上都是一致的。因此我們有如下的結(jié)論：我們有如下的結(jié)論：動(dòng)坐標(biāo)系在固定坐標(biāo)系中的齊次變換有動(dòng)坐標(biāo)系在固定坐標(biāo)系中的齊次變換有2 2種情況：種情況：定義定義1 1：如果所有的變換都是相對(duì)于固定坐標(biāo)系中各坐標(biāo)軸旋轉(zhuǎn)或平移，：如果所有的變換都是相對(duì)于固定坐標(biāo)系中各坐標(biāo)軸旋轉(zhuǎn)或平移，則依次左乘，稱為則依次左乘，稱為絕對(duì)變換絕對(duì)變換。定義定義2 2：如果動(dòng)坐

20、標(biāo)系相對(duì)于自身坐標(biāo)系的當(dāng)前坐標(biāo)軸旋轉(zhuǎn)或平移，則齊：如果動(dòng)坐標(biāo)系相對(duì)于自身坐標(biāo)系的當(dāng)前坐標(biāo)軸旋轉(zhuǎn)或平移，則齊次變換為依次右乘，稱為次變換為依次右乘，稱為相對(duì)變換相對(duì)變換。 結(jié)果均為為動(dòng)坐標(biāo)系在固定坐標(biāo)中的位姿（位置結(jié)果均為為動(dòng)坐標(biāo)系在固定坐標(biāo)中的位姿（位置+ +姿態(tài)）。相對(duì)于固定坐姿態(tài)）。相對(duì)于固定坐標(biāo)系，標(biāo)系，軸。軸相當(dāng)于軸，軸相對(duì)于軸，軸相當(dāng)于ZYXwv 也就是說(shuō)，動(dòng)坐標(biāo)系繞自身坐標(biāo)軸做齊次變換，也就是說(shuō)，動(dòng)坐標(biāo)系繞自身坐標(biāo)軸做齊次變換，要達(dá)到繞固定坐標(biāo)系相等要達(dá)到繞固定坐標(biāo)系相等的結(jié)果，就應(yīng)該用相反的順序。的結(jié)果，就應(yīng)該用相反的順序。 Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.3 2.3 齊次

21、坐標(biāo)變換齊次坐標(biāo)變換相對(duì)變換相對(duì)變換33Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.3 2.3 齊次坐標(biāo)變換齊次坐標(biāo)變換繞固定軸繞固定軸x-y-zx-y-z旋轉(zhuǎn)旋轉(zhuǎn) RPYRPY角角( ,)( ,)( , )Rot zRot yRot x34Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.3 2.3 齊次坐標(biāo)變換齊次坐標(biāo)變換z-y-xz-y-x歐拉角歐拉角 ( ,)( ,)( , )Rot zRot yRot x35Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.3 2.3 齊次坐標(biāo)變換齊次坐標(biāo)變換z-y-zz-y-z歐拉角歐拉角36 的姿態(tài)轉(zhuǎn)矩陣部分確定了的坐標(biāo)原點(diǎn)，旋置矢量部分確定了參考坐標(biāo)系的位姿，位坐標(biāo)系對(duì)連的

22、可以用來(lái)描述與剛體固剛體位姿的描述。BBABTAB ) 1 PPAPPBTAB的坐標(biāo)中的同一個(gè)空間點(diǎn)成坐標(biāo)系變換點(diǎn)的坐標(biāo)中的。它使坐標(biāo)系可以作為坐標(biāo)變換矩陣AB)2 1AB1AB221AB1111AB221ABAB ,)3pTpTppPTABppABBPBpTpppTTABAAABAAAA，于是有點(diǎn)隨剛體到達(dá)了表示的運(yùn)動(dòng)，此時(shí)個(gè)用發(fā)生一相對(duì)于令剛體重合，所以與坐標(biāo)系坐標(biāo)系，且開始時(shí)和一個(gè)固連的坐標(biāo)系上有一點(diǎn)這實(shí)際上相當(dāng)于剛體，即產(chǎn)生新矢量作用于矢量可以作為算子。Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.3 2.3 齊次坐標(biāo)變換矩陣的幾何意義齊次坐標(biāo)變換矩陣的幾何意義37習(xí)題習(xí)題1 1：O O 與與

23、O O初始重合，初始重合，O O 作如下運(yùn)動(dòng)：繞作如下運(yùn)動(dòng)：繞Z Z軸轉(zhuǎn)動(dòng)軸轉(zhuǎn)動(dòng)3030 ；繞繞X X軸轉(zhuǎn)動(dòng)軸轉(zhuǎn)動(dòng)6060 ；繞；繞Y Y軸轉(zhuǎn)動(dòng)軸轉(zhuǎn)動(dòng)9090 。求。求T T。 100001000030cos30sin0030sin30cosR11000060cos60sin0060sin60cos000012R1000090cos090sin0010090sin090cos3R1000002/12/302/34/34/102/14/34/3123RRRT38習(xí)題習(xí)題2 2：O O 與與O O初始重合，初始重合，O O 作如下運(yùn)動(dòng)：繞作如下運(yùn)動(dòng)：繞X X軸轉(zhuǎn)動(dòng)軸轉(zhuǎn)動(dòng)9090；繞；繞w w軸轉(zhuǎn)動(dòng)

24、軸轉(zhuǎn)動(dòng)9090；繞；繞Y Y軸轉(zhuǎn)動(dòng)軸轉(zhuǎn)動(dòng)9090。求。求 T T；改變旋轉(zhuǎn)順序，動(dòng)系如何旋轉(zhuǎn)才能獲得相同；改變旋轉(zhuǎn)順序，動(dòng)系如何旋轉(zhuǎn)才能獲得相同的結(jié)果。的結(jié)果。 1000090cos90sin0090sin-09cos00001R1100001000090cos90sin0090sin90cos2R1000090cos090sin0010090sin090cos3R1000001001000001213RRRT解：解： 解：解： 繞繞Z Z軸轉(zhuǎn)動(dòng)軸轉(zhuǎn)動(dòng)9090； 繞繞X X軸轉(zhuǎn)動(dòng)軸轉(zhuǎn)動(dòng)9090； 繞繞Y Y軸轉(zhuǎn)動(dòng)軸轉(zhuǎn)動(dòng)9090。 解：解： 繞繞v v軸轉(zhuǎn)動(dòng)軸轉(zhuǎn)動(dòng)9090； 繞繞u u軸轉(zhuǎn)動(dòng)軸轉(zhuǎn)

25、動(dòng)9090； 繞繞w w軸轉(zhuǎn)動(dòng)軸轉(zhuǎn)動(dòng)9090。 39習(xí)題習(xí)題3 3： 矢量矢量 在在O O 中表示為中表示為 ，O O 相對(duì)于相對(duì)于O O的的齊次變換為：齊次變換為： Pkjip2230100011002000110010T oo0O0O0 20X0 90Y0 0)3 0 2) 00 1) pppp中的矢量在求此時(shí)，軸移動(dòng)的沿軸轉(zhuǎn)的繞當(dāng)中的矢量在求的位置和姿態(tài)在畫出：求解：解：1 1） 40解：解：2 2） 13238 T00ppoo解：解：3 3） 1000090cos090sin0010090sin090cosR110000100001020001T 1r10001001020001211

26、00 TRT11oorT1823231223100010010200012110100pTp41Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.4 2.4 物體的變換及物體的變換及 逆變換逆變換1.1.物體位置描述物體位置描述 物體物體可以由固定于其自身坐標(biāo)系上的若干特征點(diǎn)描述。物體的變換也可通過(guò)這些特征點(diǎn)的變換獲得。42Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.4 2.4 物體的變換及逆變換物體的變換及逆變換1.1.物體位置描述物體位置描述11111144000011111144664411111100220044000011111110000010000141001000001000014100)90,

27、()90,()0,0,4(zRotyRotTransT43Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.4 2.4 物體的變換及逆變換物體的變換及逆變換2.2.齊次坐標(biāo)的復(fù)合變換齊次坐標(biāo)的復(fù)合變換B相對(duì)于A: ABT; C相對(duì)于B: BCT;則C相對(duì)于A:0000010101AABCBCAABBBBCCABABABCBCBTT TRpRpR RR pp44Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.4 2.4 物體的變換及逆變換物體的變換及逆變換3.3.齊次坐標(biāo)的逆變換齊次坐標(biāo)的逆變換B相對(duì)于A: ABT; A相對(duì)于B: BAT;兩者互為逆矩陣.求逆的辦法:1.直接求ABT-12.簡(jiǎn)化方法0ppRpTpRR

28、pRT000100)(1010ABBABABABABBATABTABABBABA45Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.4 2.4 物體的變換及逆變換物體的變換及逆變換3.3.齊次坐標(biāo)的逆變換齊次坐標(biāo)的逆變換一般,若則1000zzzzyyyyxxxxpaonpaonpaonT10001apopnpTzyxzyxzyxaaaooonnnTzyxTzyxTzyxTzyxaaaooonnnpppaonp,46Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.4 2.4 物體的變換物體的變換 及逆變換及逆變換3.3.變換方程初步B:基坐標(biāo)系T:工具坐標(biāo)系S:工作臺(tái)坐標(biāo)系G:目標(biāo)坐標(biāo)系 或工件坐標(biāo)系滿足方程TTT

29、TGTSGBSBT47習(xí)題習(xí)題4 4： 如圖所示，如圖所示，1 1）寫出）寫出 、 、 、 ；2 2）求）求 T01T12T23T34T04100011-00301-03.5-001T011000101-03001-0100T121000000155354-0054530 T23100001-0000103.5001- T34解：解：1 1） o0 x0y0z433.511o1x1y1z2o2x2y2z3o3x3y3z4o4x4y4z48解解2 2）：根據(jù)定義）：根據(jù)定義2 2，繞自身旋轉(zhuǎn)，右乘，繞自身旋轉(zhuǎn)，右乘100050.6-0.8-000.8-0.600001- T T TT T3423

30、12010449Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.5 2.5 通用旋轉(zhuǎn)變換通用旋轉(zhuǎn)變換1.通用旋轉(zhuǎn)變換公式求:繞從原點(diǎn)出發(fā)的f旋轉(zhuǎn)角時(shí)的旋轉(zhuǎn)矩陣.S:物體上固接的坐標(biāo)系T:參考坐標(biāo)系C:Z軸與f重合的輔助坐標(biāo)系xTYTZTTCSzSf, ZcO),(fRot50Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.5 2.5 通用旋轉(zhuǎn)變換通用旋轉(zhuǎn)變換在S上取一點(diǎn)p,其坐標(biāo)為向量P,它繞T中直線f旋轉(zhuǎn)角。1）將S上p點(diǎn)坐標(biāo)變換到T中，其坐標(biāo)為2）直接計(jì)算繞f旋轉(zhuǎn)的坐標(biāo)為， 目前上式在T無(wú)法直接求。采取如下步驟：3）建立輔助坐標(biāo)系C，使其Z軸與f重合。這樣問(wèn)題 變?yōu)槔@ZC旋轉(zhuǎn)。將S中的點(diǎn)p變換到C中，變換 為

31、：4）在C中繞Z軸旋轉(zhuǎn)有：5）將C中坐標(biāo)變換回T中有，pTST),(pfRotTST pTSCTTT),(pzRTSCTTT),(pzTRTSCTTCTT51Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.5 2.5 通用旋轉(zhuǎn)變換通用旋轉(zhuǎn)變換步驟2）和5）中的結(jié)果應(yīng)該相同，即：由于C的Z軸與f重合,所以),(),(pfRotpzTRTSTSCTTCTTT1),(),(),(TTTCTCCTTCzTRzTRfRot10000001000010000001000000zyxzyxzyxzzzyyyxxxaaaooonnncsscaonaonaonzzyyxxfafafa52Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)

32、2.5 2.5 通用旋轉(zhuǎn)變換通用旋轉(zhuǎn)變換根據(jù)坐標(biāo)軸的正交性, ,有令 ,則ona1222zyxzyxyxzyxzxzyxzyzyxaaafnoonafnoonafnoonacos1)(vers1000000),(cversffsfversffsfversffsfversffcversffsfversffsfversffsfversffcversfffRotzzxzyyzxxyzyyzyxyxzzxyxx53Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.5 2.5 通用旋轉(zhuǎn)變換通用旋轉(zhuǎn)變換2.等效轉(zhuǎn)角與轉(zhuǎn)軸給出任一旋轉(zhuǎn)變換,能夠由上式求得進(jìn)行等效旋轉(zhuǎn)角的轉(zhuǎn)軸.已知旋轉(zhuǎn)變換R,R,令令R=R=Rot(f,

33、),即有將上式對(duì)角線元素相加,并簡(jiǎn)化得000000000 10001xxxx xy xzz xyyyyx yzy yzyxzzzx zyy zxz znoaf f verscf f versf sf f versf snoaf f versf sf f verscf f versf snoaf f versf sf f versf sf f versc ccversfffaonzyxzyx213)(222) 1(21xyxaonc54Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.5 2.5 通用旋轉(zhuǎn)變換通用旋轉(zhuǎn)變換非對(duì)角元素成對(duì)相減,有平方后有設(shè) ,sfonsfnasfaozxyyzxxyz22222

34、221)()()(xyzxyzonnaaoso18001)()()(tan222zyxxyzxyzaononnaaosonfsnafsaofxyzzxyyzx2/)(2/)(2/)(55Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.5 2.5 通用旋轉(zhuǎn)變換通用旋轉(zhuǎn)變換例2-7 一坐標(biāo)系B與參考系重合,現(xiàn)將其繞通過(guò)原點(diǎn)的軸 轉(zhuǎn)30,求轉(zhuǎn)動(dòng)后的B.以 ,代入算式,有Tf0707. 0707. 0ozyxfff0 .300 . 0707. 010000866. 0354. 0354. 00354. 0933. 0067. 00354. 0067. 0933. 0)30,(okRot56Robotics 數(shù)學(xué)

35、基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.5 2.5 通用旋轉(zhuǎn)變換通用旋轉(zhuǎn)變換一般情況,若f不通過(guò)原點(diǎn),而過(guò)q點(diǎn)(qx,qy,qz),則齊次變換矩陣為:其中,1000),(CcversffsfversffsfversffBsfversffcversffsfversffAsfversffsfversffcversfffRotzzxzyyzxxyzyyzyxyxzzxyxxzyxzzxzyyzxxyzyyzyxyxzzxyxxzyxqqqcversffsfversffsfversffsfversffcversffsfversffsfversffsfversffcversffqqqCBA57Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)

36、2.5 2.5 通用旋轉(zhuǎn)變換通用旋轉(zhuǎn)變換例2-8 一坐標(biāo)系B與參考系重合,現(xiàn)將其繞通過(guò)q=1,2,3T的軸 轉(zhuǎn)30,求轉(zhuǎn)動(dòng)后的B.以 ,代入算式,有Tf0707. 0707. 03210 .300 . 0707. 0zyxozyxqqqfff100004. 0866. 0354. 0354. 013. 1354. 0933. 0067. 013. 1354. 0067. 0933. 0)30,(okRot58Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)MatlabMatlab使用與矩陣計(jì)算使用與矩陣計(jì)算Matlab是美國(guó)Mathworks公司推出的數(shù)值計(jì)算軟件.在數(shù)值計(jì)算及科學(xué)研究中,是其它語(yǔ)言無(wú)法相比

37、的.其主要特點(diǎn)有:1.1.語(yǔ)言簡(jiǎn)潔緊湊語(yǔ)言簡(jiǎn)潔緊湊, ,使用方便靈活使用方便靈活, ,庫(kù)含數(shù)極其豐富庫(kù)含數(shù)極其豐富. .2.2.具有非常多的矩陣函數(shù)具有非常多的矩陣函數(shù), ,矩陣計(jì)算異常方便矩陣計(jì)算異常方便. .3.3.具有多種功能的工具包具有多種功能的工具包. .4.4.具有與具有與FORTRANFORTRAN、C C等同樣多的運(yùn)算符和結(jié)構(gòu)控制指令的同等同樣多的運(yùn)算符和結(jié)構(gòu)控制指令的同 時(shí)，語(yǔ)法限制卻不嚴(yán)格，使程序設(shè)計(jì)很自由時(shí)，語(yǔ)法限制卻不嚴(yán)格，使程序設(shè)計(jì)很自由. .5.5.圖形功能強(qiáng)大圖形功能強(qiáng)大, ,數(shù)據(jù)可視化好數(shù)據(jù)可視化好. .6.6.原程序和庫(kù)函數(shù)代碼公開原程序和庫(kù)函數(shù)代碼公開. .

38、但但. .程序執(zhí)行效率較低程序執(zhí)行效率較低. .本節(jié)主要介紹其矩陣計(jì)算在機(jī)器人分析中的應(yīng)用本節(jié)主要介紹其矩陣計(jì)算在機(jī)器人分析中的應(yīng)用. .59Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)MatlabMatlab使用與矩陣計(jì)算使用與矩陣計(jì)算矩陣的輸入:1)矩陣的直接輸入.(操作) 以 作為首尾,行分隔用”;”,元素分隔用”,”或空格.2)矩陣編輯器.(操作) 先在工作區(qū)定義矩陣,用編輯器修改矩陣.3)用函數(shù)創(chuàng)建矩陣,如.(操作) zeros(m,n):zeros(m,n):零矩陣零矩陣 ones(m,n):ones(m,n):全部元素都為全部元素都為1 1的矩陣的矩陣 eye(m,n):eye(m,n):

39、單位陣單位陣 randn(m,n):randn(m,n):正態(tài)分布的隨機(jī)矩陣正態(tài)分布的隨機(jī)矩陣 vander(A):vander(A):由矩陣由矩陣A A產(chǎn)生的產(chǎn)生的VandermondeVandermonde矩陣矩陣60Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)MatlabMatlab使用與矩陣計(jì)算使用與矩陣計(jì)算矩陣的計(jì)算矩陣的計(jì)算.(操作)1)1)加減加減2)2)轉(zhuǎn)置轉(zhuǎn)置3)3)乘法乘法4)4)除法與線性方程組除法與線性方程組5)5)逆逆6)6)冪和指數(shù)冪和指數(shù)61Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)MatlabMatlab使用與矩陣計(jì)算使用與矩陣計(jì)算例: 計(jì)算:100001000000)60,(1

40、00000001000)45,(100000000001)45,(2123232102222222222222222zRotyRotxRotoo)45,()45,()60,()60,()45,()45,(ooooooxRyRzRzRyRxR62Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)習(xí)題習(xí)題:2.3:2.3坐標(biāo)系坐標(biāo)系BB初始與初始與AA重合重合, ,讓讓BB繞繞Z ZB B旋轉(zhuǎn)旋轉(zhuǎn)角角; ;然后再然后再繞繞X XB B轉(zhuǎn)轉(zhuǎn)角角. .求把求把B BP P變?yōu)樽優(yōu)锳 AP P的旋轉(zhuǎn)矩陣的旋轉(zhuǎn)矩陣. .cscsccsssscc063Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)習(xí)題習(xí)題:2.3:2.3變化變化坐標(biāo)系

41、坐標(biāo)系BB初始與初始與AA重合重合, ,讓讓BB繞繞Z ZB B旋轉(zhuǎn)旋轉(zhuǎn)角角; ;然后再然后再繞繞X XA A轉(zhuǎn)轉(zhuǎn)角角. .求把求把B BP P變?yōu)樽優(yōu)锳 AP P的旋轉(zhuǎn)矩陣的旋轉(zhuǎn)矩陣. .cscssscccssc064Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)習(xí)題習(xí)題:2.3:2.3變化變化坐標(biāo)系坐標(biāo)系BB初始與初始與AA重合重合, ,讓讓BB繞繞Z ZB B旋轉(zhuǎn)旋轉(zhuǎn)角角; ;然后再然后再繞繞X XA A轉(zhuǎn)轉(zhuǎn)角角. .求把求把B BP P變?yōu)樽優(yōu)锳 AP P的旋轉(zhuǎn)矩陣的旋轉(zhuǎn)矩陣. .cscssscccssccssccssczRotxRot01000000001),(),(65Robotics 數(shù)學(xué)基礎(chǔ)