機器人學數(shù)學基礎(chǔ)學習教案

1、會計學1機器人學數(shù)學機器人學數(shù)學(shxu)基礎(chǔ)基礎(chǔ)第一頁，共46頁。2.1 2.1 位置和姿態(tài)的表示位置和姿態(tài)的表示2.2 2.2 坐標坐標(zubio)(zubio)變換變換2.3 2.3 齊次坐標齊次坐標(zubio)(zubio)變換變換2.4 2.4 旋轉(zhuǎn)矩陣旋轉(zhuǎn)矩陣機器人技術(shù)數(shù)學機器人技術(shù)數(shù)學(shxu)(shxu)基礎(chǔ)基礎(chǔ)Mathematic Preparation for Mathematic Preparation for RoboticsRobotics第1頁/共45頁第二頁，共46頁。inoa人們感興趣的是操作機末端執(zhí)行器相對于固定參考坐標數(shù)的空間幾何描述，也就是機器人的

2、運動學問題人們感興趣的是操作機末端執(zhí)行器相對于固定參考坐標數(shù)的空間幾何描述，也就是機器人的運動學問題機器人的運動學即是研究機器人手臂末端執(zhí)行器位置和姿態(tài)與關(guān)節(jié)機器人的運動學即是研究機器人手臂末端執(zhí)行器位置和姿態(tài)與關(guān)節(jié)(gunji)(gunji)變量空間之間的關(guān)系變量空間之間的關(guān)系第2頁/共45頁第三頁，共46頁。Where is my hand?Direct KinematicsHERE!How do I put my hand here?Inverse Kinematics: Choose these angles!第3頁/共45頁第四頁，共46頁。運 動 學 正 問 題關(guān) 節(jié) 角桿 件 參

3、 數(shù)末 端 執(zhí) 行 器運 動 學 正 問 題關(guān) 節(jié) 角桿 件 參 數(shù)第4頁/共45頁第五頁，共46頁。PUMA560HexapodFanuc manipulator第5頁/共45頁第六頁，共46頁。第6頁/共45頁第七頁，共46頁。第7頁/共45頁第八頁，共46頁。第8頁/共45頁第九頁，共46頁。D-H模型表示了對機器人連桿和關(guān)節(jié)進行建模的一種非常簡單的方法，可用于任何機器人構(gòu)型，而不管機器人的結(jié)構(gòu)順序和復雜程度如何。它也可用于表示已經(jīng)討論過的在任何坐標(zubio)中的變換，例如直角坐標(zubio)、圓柱坐標(zubio)、球坐標(zubio)、歐拉角坐標(zubio)及RPY坐標(zu

4、bio)等。另外，它也可以用于表示全旋轉(zhuǎn)的鏈式機器人、SCARA機器人或任何可能的關(guān)節(jié)和連桿組合。第9頁/共45頁第十頁，共46頁。a0vzyxzyxpcb0uEH圖2.1 點向量的描述第10頁/共45頁第十一頁，共46頁。第11頁/共45頁第十二頁，共46頁。 zy x TwwzyxV式中式中i, j, ki, j, k為為x, y, z x, y, z 軸上的單位矢量軸上的單位矢量(shling)(shling)，a= , b= , c= a= , b= , c= ，w w為比例系數(shù)為比例系數(shù) wxwywz 顯然，齊次坐標表達并不是唯一的，隨顯然，齊次坐標表達并不是唯一的，隨w值的不同而不

5、同。在計算機圖學中，值的不同而不同。在計算機圖學中，w 作為通用比例因子，它可取任意正值，但在機器人的運動分析中，總是取作為通用比例因子，它可取任意正值，但在機器人的運動分析中，總是取w=1 。列矩列矩陣陣第12頁/共45頁第十三頁，共46頁。在歐幾里得幾何空間里，兩條平行線永遠都不會相交。但是在投影空間中，如右圖中的兩條鐵軌在地平線處卻是會相交的，因為在無限遠處它們(t men)看起來相交于一點。第13頁/共45頁第十四頁，共46頁。 在歐幾里得（或稱笛卡爾）空間里描述2D/3D 幾何物體是很理想的，但在投影空間里面卻并不見得。 我們用 (x, y) 表示笛卡爾空間中的一個 2D 點，而處于

6、無限遠處的點 (,) 在笛卡爾空間里是沒有意義(yy)的。投影空間里的兩條平行線會在無限遠處相交于一點，但笛卡爾空間里面無法搞定這個問題（因為無限遠處的點在笛卡爾空間里是沒有意義(yy)的），因此數(shù)學家想出齊次坐標這個點子來了。第14頁/共45頁第十五頁，共46頁。由 August Ferdinand Mbius 提出的齊次坐標（Homogeneous coordinates）讓我們能夠在投影空間里進行圖像和幾何處理，齊次坐標用 N + 1個分量(fn ling)來描述 N 維坐標。比如，2D 齊次坐標是在笛卡爾坐標(X, Y)的基礎(chǔ)上增加一個新分量(fn ling) w，變成(x, y, w

7、)，其中笛卡爾坐標系中的大X，Y 與齊次坐標中的小x，y有如下對應(yīng)關(guān)系：X = x/wY = y/w 笛卡爾坐標中的點 (1, 2) 在齊次坐標中就是 (1, 2, 1) 。如果這點移動到無限遠(,)處，在齊次坐標中就是 (1, 2, 0) ，這樣我們就避免了用沒意義的 來描述無限遠處的點。第15頁/共45頁第十六頁，共46頁。點 (1, 2, 3), (2, 4, 6) 和 (4, 8, 12) 對應(yīng)笛卡爾坐標中的同一點 (1/3, 2/3)。 任意數(shù)量積的(1a, 2a, 3a) 始終(shzhng)對應(yīng)于笛卡爾坐標中的同一點 (1/3, 2/3)。因此這些點是“齊次”的，因為他們始終(s

8、hzhng)對應(yīng)于笛卡爾坐標中的同一點。換句話說，齊次坐標描述縮放不變性（scale invariant）。第16頁/共45頁第十七頁，共46頁。kjiV543可以可以(ky)(ky)表示為：表示為： V=3 4 5 1T V=3 4 5 1T 或或 V=6 8 10 2T V=6 8 10 2T 或或 V=-12 -16 -20 -4T V=-12 -16 -20 -4T 第17頁/共45頁第十八頁，共46頁。xyzzzxV圖2-2o第18頁/共45頁第十九頁，共46頁。第19頁/共45頁第二十頁，共46頁。設(shè)固定參考坐標系直角坐標為設(shè)固定參考坐標系直角坐標為Oxyz，動坐標系為，動坐標系為

9、O uvw，研究，研究(ynji)旋轉(zhuǎn)變換情況。旋轉(zhuǎn)變換情況。xyzwvuPo(O)圖2-3 初始位置初始位置(wi zhi)時，動靜坐標系重合，時，動靜坐標系重合，O、O 重合，如圖。各軸對應(yīng)重合，設(shè)重合，如圖。各軸對應(yīng)重合，設(shè)P點是動坐標系點是動坐標系O uvw中的一點，且固定不變。則中的一點，且固定不變。則P點在點在O uvw中可表示為：中可表示為： wwuvuuuvwkPjPiPP 、 、 為坐標系為坐標系O uvw的單位矢量，則的單位矢量，則P點在點在oxyz中可表示為：中可表示為： uivjwkzzyyxxxyziPiPiPPuvwPxyzP第20頁/共45頁第二十一頁，共46頁。

10、當動坐標系當動坐標系O uvw繞繞O點回轉(zhuǎn)點回轉(zhuǎn)(huzhun)時，求時，求P點在固定坐標系點在固定坐標系oxyz中的位置中的位置 yzxo(O)uvwPPwPvPu圖2-4已知：已知：P點在點在O uvw中是不變的仍然中是不變的仍然(rngrn)成立，由于成立，由于O uvw回轉(zhuǎn)，則：回轉(zhuǎn)，則： wwuvuuuvwkPjPiPP)(PwwvvuuxxuvwxkPjPiPiiP)(PwwvvuuyyuvwykPjPiPjjP)(PwwvvuuzzuvwzkPjPiPjjP用矩陣用矩陣(j zhn)表示為表示為: wvwzvzzwvyywxvxxzyxPPPkkjkikkjjjijkijiiiP

11、PPy（2-7） 第21頁/共45頁第二十二頁，共46頁。uvwxyzwzvzzwvyywxvxxPRpkkjkikkjjjijkijiii:R y則旋轉(zhuǎn)矩陣為：定義反過來：反過來： xyzuvwPRP1RRRdet*1T1RRRdet因此是正交矩陣，的行列式，由于為的伴隨矩陣，為RRRR由剛體的等距變換(binhun)可知：uvwTuvwxyzTxyzpppp將上式代入，可得：IRRTR為正交矩陣為正交矩陣(j zhn)。第22頁/共45頁第二十三頁，共46頁。 由圖可知，由圖可知， 在在y y軸上的投影軸上的投影(tuyng)(tuyng)為為 ， 在在z z軸上的投影軸上的投影(tuyn

12、g)(tuyng)為為 , , 在在y y軸上的投影軸上的投影(tuyng)(tuyng)為為 ， 在在z z軸上的投影軸上的投影(tuyng)(tuyng)為為 ，所以有：，所以有： vjcosyjsinzksinyjwkcoszkvjwkwzvzzwvyywxvxxkkjkikkjjjijkijiiiy)R(x,xyzouvwUVWO圖2-5cossin0sincos0001uxii方向方向(fngxing)余弦陣余弦陣第23頁/共45頁第二十四頁，共46頁。同理：同理： cos0sin010sin0cos)y,R（1000cossin0sin-cos)z,R （cossin0sincos

13、0001)R(x,三個基本三個基本(jbn)(jbn)旋轉(zhuǎn)矩陣旋轉(zhuǎn)矩陣: : xyzouvwUWOxyzouvwUWOv第24頁/共45頁第二十五頁，共46頁。用齊次坐標變換用齊次坐標變換(binhun)來表示式旋轉(zhuǎn)變換來表示式旋轉(zhuǎn)變換(binhun)： 110000001wvuzyxPPPRPPP1100000011zyxwvuPPPRPPP第25頁/共45頁第二十六頁，共46頁。合成合成(hchng)(hchng)旋轉(zhuǎn)矩陣旋轉(zhuǎn)矩陣: :例例1：在動坐標中有一固定點：在動坐標中有一固定點 ，相對固定參考坐標系，相對固定參考坐標系 做如下做如下(rxi)運動：運動： R（x, 90）；）； R

14、(z, 90)； R(y,90)。求點。求點 在固定參考坐標系在固定參考坐標系 下的位置。下的位置。 TuvwPo1321OxyzuvwPoOxyz解解1：用畫圖：用畫圖(hu t)的簡單方法的簡單方法 第26頁/共45頁第二十七頁，共46頁。解解2：用分步計算：用分步計算(j sun)的方法的方法 R（x, 90） R（z, 90） R（y, 90） 123113211000001001-000001P12131231100001000001001-0 P1312121310000001-00100100 P（2-14） （2-15） （2-16） 第27頁/共45頁第二十八頁，共46頁。

15、上述計算方法非常上述計算方法非常(fichng)繁瑣，可以通過一系列計算得到上述結(jié)果。將式（繁瑣，可以通過一系列計算得到上述結(jié)果。將式（2-14）（）（2-15）（）（2-16）聯(lián)寫為如下形式：）聯(lián)寫為如下形式： 1144wvuzyxPPPRPPPR4x4為二者之間的關(guān)系矩陣為二者之間的關(guān)系矩陣(j zhn)，我們令：，我們令： ),(),(),RR44xRzRy（定義定義1： 當動坐標系當動坐標系 繞固定坐標系繞固定坐標系 各坐標軸順序有限次轉(zhuǎn)動時，其合成旋轉(zhuǎn)各坐標軸順序有限次轉(zhuǎn)動時，其合成旋轉(zhuǎn)(xunzhun)矩陣為各基本旋轉(zhuǎn)矩陣為各基本旋轉(zhuǎn)(xunzhun)矩陣依旋轉(zhuǎn)矩陣依旋轉(zhuǎn)(xunz

16、hun)順序左乘。順序左乘。注意：旋轉(zhuǎn)注意：旋轉(zhuǎn)(xunzhun)矩陣間不可以交換矩陣間不可以交換 uvwOOxyz第28頁/共45頁第二十九頁，共46頁。 平移齊次變換平移齊次變換(binhun)(binhun)矩陣矩陣1000100010001c) b (a TransHcba注意：平移矩陣間可以注意：平移矩陣間可以(ky)交換，交換， 平移和旋轉(zhuǎn)矩陣間不可以平移和旋轉(zhuǎn)矩陣間不可以(ky)交換交換 zyxoo w u v abc因此對向量 u = x y z w T，經(jīng)H變換(binhun)為向量v可表示為 x + aw x / w + a y + bw y / w + b z + cw

17、z / w + c w 1第29頁/共45頁第三十頁，共46頁。舉例說明：舉例說明：例例1：動坐標系：動坐標系0起始位置與固定參考坐標系起始位置與固定參考坐標系0重合重合,動坐標系動坐標系0做如下運動：做如下運動：R(Z,90) R（y,90） Trans(4，-3, 7)，求合成，求合成(hchng)矩陣矩陣 解解1：用畫圖：用畫圖(hu t)的方法：的方法： ozyx74-3owuvvuwzyxoo(o)xyzuvwzyxuwo(o) v第30頁/共45頁第三十一頁，共46頁。解解2：用計算：用計算(j sun)的方法的方法 根據(jù)定義根據(jù)定義(dngy)1，我們有：，我們有：1000701

18、030014100 )R(Z,90 )90 R(y, 7) , 3 ,Trans(4T 以上均以固定坐標系多軸為變換基準以上均以固定坐標系多軸為變換基準(jzhn)，因此矩陣左乘。，因此矩陣左乘。如果我們做如下變換，也可以得到相同的結(jié)果：如果我們做如下變換，也可以得到相同的結(jié)果： 例例2：先平移：先平移Trans (4,-3,7)；繞當前；繞當前 軸轉(zhuǎn)動軸轉(zhuǎn)動90； 繞當前繞當前 軸轉(zhuǎn)動軸轉(zhuǎn)動90；求合成旋轉(zhuǎn)矩陣。；求合成旋轉(zhuǎn)矩陣。 vw （2-202-20）第31頁/共45頁第三十二頁，共46頁。解解1：用畫圖：用畫圖(hu t)的方法的方法 zyxo(o)vwuzyxoowuvozyxow

19、vuxyzoowuv解解2：用計算：用計算(j sun)的方法的方法 1000701030014100)R(Z,90 )90 R(y, 7) , 3 ,Trans(4Too第32頁/共45頁第三十三頁，共46頁。式（式（2-202-20）和式（）和式（2-212-21）無論在形式上，還是在結(jié)果上都是一致的。因此我們有如下的結(jié)論：）無論在形式上，還是在結(jié)果上都是一致的。因此我們有如下的結(jié)論：動坐標系在固定坐標系中的齊次變換有動坐標系在固定坐標系中的齊次變換有2 2種情況：種情況：定義定義1 1：如果所有的變換都是相對于固定坐標系中各坐標軸旋轉(zhuǎn)或平移，則依次：如果所有的變換都是相對于固定坐標系中各

20、坐標軸旋轉(zhuǎn)或平移，則依次(yc)(yc)左乘，稱為絕對變換。左乘，稱為絕對變換。定義定義2 2：如果動坐標系相對于自身坐標系的當前坐標軸旋轉(zhuǎn)或平移，則齊次變換為依次：如果動坐標系相對于自身坐標系的當前坐標軸旋轉(zhuǎn)或平移，則齊次變換為依次(yc)(yc)右乘，稱為相對變換。右乘，稱為相對變換。 結(jié)果均為動坐標系在固定坐標中的位姿（位置結(jié)果均為動坐標系在固定坐標中的位姿（位置+ +姿態(tài)姿態(tài)(zti)(zti)）。相對于固定坐標系，）。相對于固定坐標系，軸。軸相當于軸，軸相對于軸，軸相當于ZYXwv 也就是說，動坐標系繞自身坐標軸做齊次變換，要達到繞固定坐標系相等的結(jié)果，就應(yīng)該也就是說，動坐標系繞自身

21、坐標軸做齊次變換，要達到繞固定坐標系相等的結(jié)果，就應(yīng)該(ynggi)(ynggi)用相反的順序。用相反的順序。 第33頁/共45頁第三十四頁，共46頁。xyzoH第34頁/共45頁第三十五頁，共46頁。XYZrxryrzABCDBO51243rA第35頁/共45頁第三十六頁，共46頁。由上圖容易由上圖容易(rngy)求出：求出：2z2yyrrrsin2z2yzrrrcosxxrrrrOCsin2z2y2z2yrrrrrOBCBcos由定義由定義(dngy)1和定義和定義(dngy)2，上述，上述5次旋轉(zhuǎn)的合成旋轉(zhuǎn)次旋轉(zhuǎn)的合成旋轉(zhuǎn)矩陣為：矩陣為：cossin0sin-cos0001cos0sin

22、010sin-0cos1000cossin0sin-coscos0sin-010sin0coscossin0sincos0001RRRRRR,x,y, z,y,x, r（2-252-25）第36頁/共45頁第三十七頁，共46頁。帶入式帶入式（2-252-25），得），得cos)cos(1rsinr)cos(1rrsinr)cos(1rrsinr)cos(1rrsinr)cos(1rrcos)cos(1rsinr)cos(1rrsinr)cos(1rrcos)cos(1rR2zxzyyzxxzyyzx2yzyxzyx2x, r第37頁/共45頁第三十八頁，共46頁。設(shè)設(shè)T= T= ，有一個手爪，

23、已知其在，有一個手爪，已知其在OO的位置，設(shè)一個的位置，設(shè)一個該坐標系該坐標系OO ，已知，已知， ，那么，那么OO 在在OO中的齊次坐中的齊次坐標變換為標變換為 ，如果，如果(rgu)(rgu)手爪轉(zhuǎn)了一個角度，手爪轉(zhuǎn)了一個角度， 則：則：1000321twzzztwyyytwxxx111cbao1000100010001T 1111cba1000pppTzyyyxxxzzzyxwww第38頁/共45頁第三十九頁，共46頁。T T反映了反映了OO 在在OO中的位置和姿態(tài)中的位置和姿態(tài)(zti)(zti)，即表示了該坐標系原點和各坐標軸單位矢量在固定坐標系中的位置和姿態(tài)，即表示了該坐標系原點和

24、各坐標軸單位矢量在固定坐標系中的位置和姿態(tài)(zti)(zti)。該矩陣可以由該矩陣可以由4 4個子矩陣組成，寫成如下形式：個子矩陣組成，寫成如下形式：比例系數(shù)透視矩陣位置矢量旋轉(zhuǎn)矩陣11311333wfPRTzzzyyyxxxwww33R為姿態(tài)矩陣，表示動坐標系為姿態(tài)矩陣，表示動坐標系OO 在固定參考坐標系在固定參考坐標系OO中的姿態(tài)，即表示中的姿態(tài)，即表示OO 各坐標軸單位矢量各坐標軸單位矢量(shling)(shling)在在OO各軸上的投影各軸上的投影 為位置矢量矩陣，代表動坐標系為位置矢量矩陣，代表動坐標系OO 坐標原點在固定參考坐標原點在固定參考(cnko)(cnko)坐標系坐標系OO中的位置中的位置 TzyxpppP13為透視變換矩陣，在視覺中進行圖像計算，一般置為為透視變換矩陣，在視覺中進行圖像計算，一般置為0 0 000f31為比例系數(shù)為比例系數(shù) 1 11w第39頁/共45頁第四十頁，共46頁。如果需要求解如果需要求解OO在在OO 中的位置和姿態(tài)，此時的齊次變換中的位置和姿態(tài)，此時的齊次變換(binhun)(binhun)矩陣為矩陣為 ，即求逆矩陣：，即求逆矩陣： 1T1000)(RT13T331PRT kpjpippzyx其中其中(qzhng)(qzhng)：這些式子以后經(jīng)常遇到，在機器人計算中，所要求這些式子以后經(jīng)常遇到，在機器人計算中，所要