機器人學(xué)第二章(數(shù)學(xué)基礎(chǔ))

1、機器人運動學(xué)機器人運動學(xué)第二章第二章 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.1 2.1 引言引言 機器人位置和姿態(tài)的描述機器人位置和姿態(tài)的描述 機器人可以用一個開環(huán)關(guān)節(jié)鏈來建模機器人可以用一個開環(huán)關(guān)節(jié)鏈來建模 由數(shù)個驅(qū)動器驅(qū)動的轉(zhuǎn)動或移動關(guān)節(jié)串聯(lián)而成由數(shù)個驅(qū)動器驅(qū)動的轉(zhuǎn)動或移動關(guān)節(jié)串聯(lián)而成 一端固定在基座上，另一端是自由的，安裝工具，用以一端固定在基座上，另一端是自由的，安裝工具，用以操縱物體操縱物體inoa 人們感興趣的是操作機末端執(zhí)行人們感興趣的是操作機末端執(zhí)行器相對于固定參考坐標數(shù)的空間器相對于固定參考坐標數(shù)的空間幾何描述，也就是機器人的運動幾何描述，也就是機器人的運動學(xué)問題學(xué)問題 機器人的運動學(xué)即是研究

2、機器人機器人的運動學(xué)即是研究機器人手臂末端執(zhí)行器位置和姿態(tài)與關(guān)手臂末端執(zhí)行器位置和姿態(tài)與關(guān)節(jié)變量空間之間的關(guān)系節(jié)變量空間之間的關(guān)系運動學(xué)研究的問題運動學(xué)研究的問題Where is my hand?Direct KinematicsHERE!How do I put my hand here?Inverse Kinematics: Choose these angles!a0vzyxzyxpcb0uEH圖2.1 點向量的描述數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ) 2.2.1 點向量（Point vectors） 點向量描述空間的一個點在某個坐標系的空點向量描述空間的一個點在某個坐標系的空間位置。同一個點在不同坐標系的

3、描述及位置向間位置。同一個點在不同坐標系的描述及位置向量的值也不同。如圖量的值也不同。如圖2.1中，點中，點p在在E坐標系上表示坐標系上表示為為 Ev，在，在H坐標系上表示為坐標系上表示為 Hu，且，且v u。一個點。一個點向量可表示為向量可表示為 v = ai + bj + ck 通常用一個（通常用一個（n + 1）維列矩陣表示，即除）維列矩陣表示，即除 x、y、z 三個方向上的分量外，再加一個比例因子三個方向上的分量外，再加一個比例因子 w ，即即 v = x y z w T 其中其中 a = x/w, b = y/w, c = z/w。 已知兩個向量已知兩個向量 a = ax i + a

4、y j + az k b = bx i + by j + bz k （2.1） 向量的點積是標量。用向量的點積是標量。用“ ”來定義向量點積，即來定義向量點積，即 a b = ax bx + ay by + az bz （2.2 ） 向量的叉積是一個垂直于由叉積的兩個向量構(gòu)成的平面的向量。用向量的叉積是一個垂直于由叉積的兩個向量構(gòu)成的平面的向量。用“”表示叉積，即表示叉積，即 a b = ( ay bz az by ) i + ( az bx ax bz ) j + ( ax by ay by ) k （ 2.3） 可用行列式表示為可用行列式表示為 i j k a b = ax ay az （

5、2.4） bx by bz旋轉(zhuǎn)矩陣旋轉(zhuǎn)矩陣 設(shè)固定參考坐標系直角坐標為設(shè)固定參考坐標系直角坐標為Oxyz，動坐標系為，動坐標系為O uvw，研究旋轉(zhuǎn)變換情況。研究旋轉(zhuǎn)變換情況。xyzwvuPo(O)圖2-3 初始位置時，動靜坐標系重合，初始位置時，動靜坐標系重合，O、O 重合，如圖。各軸重合，如圖。各軸對應(yīng)重合，設(shè)對應(yīng)重合，設(shè)P點是動坐標系點是動坐標系O uvw中的一點，且固定不變。中的一點，且固定不變。則則P點在點在O uvw中可表示為：中可表示為： wwuvuuuvwkPjPiPP 、 、 為坐標系為坐標系O uvw的單位矢的單位矢量，則量，則P點在點在oxyz中可表示為：中可表示為： u

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

7、jkijiiiPPPy（2-7） uvwxyzwzvzzwvyywxvxxPRpkkjkikkjjjijkijiii:R y則旋轉(zhuǎn)矩陣為：定義反過來：反過來： xyzuvwPRP1RRRdet*1T1RRRdet因此是正交矩陣，的行列式，由于為的伴隨矩陣，為RRRR由剛體的等距變換可知：uvwTuvwxyzTxyzpppp將上式代入，可得：IRRTR為正交矩陣為正交矩陣。 由圖可知，由圖可知， 在在y y軸上的投影為軸上的投影為 ， 在在z z軸上的投影軸上的投影為為 , , 在在y y軸上的投影為軸上的投影為 ， 在在z z軸上的投影為軸上的投影為 ，所以有：，所以有： vjcosyjsin

8、zksinyjwkcoszkvjwkwzvzzwvyywxvxxkkjkikkjjjijkijiiiy)R(x,xyzouvwUVWO圖2-5cossin0sincos0001uxii方向余弦陣方向余弦陣同理：同理： cos0sin010sin0cos)y,R（1000cossin0sin-cos)z,R （cossin0sincos0001)R(x,三個基本旋轉(zhuǎn)矩陣三個基本旋轉(zhuǎn)矩陣: : xyzouvwUWOxyzouvwUWOv 丹納維特（丹納維特（DenavitDenavit）和哈頓貝格（）和哈頓貝格（HartenbergHartenberg） 于于19551955年提出了一種矩陣代數(shù)方

9、法解決機器人年提出了一種矩陣代數(shù)方法解決機器人的運動學(xué)問題的運動學(xué)問題D-HD-H方法方法 具有直觀的幾何意義具有直觀的幾何意義 能表達動力學(xué)、計算機視覺和比例變換問題能表達動力學(xué)、計算機視覺和比例變換問題 其數(shù)學(xué)基礎(chǔ)即是齊次變換其數(shù)學(xué)基礎(chǔ)即是齊次變換2.2 2.2 點齊次坐標點齊次坐標2.2.1 2.2.1 點的齊次坐標點的齊次坐標 一般來說，一般來說，n維空間的齊次坐標表示是一個（維空間的齊次坐標表示是一個（n+1）維空間）維空間實體。有一個特定的投影附加于實體。有一個特定的投影附加于n維空間，也可以把它看作維空間，也可以把它看作一個附加于每個矢量的特定坐標一個附加于每個矢量的特定坐標比例

10、系數(shù)。比例系數(shù)。 kcj bi av zy x TwwzyxV式中式中i, j, k為為x, y, z 軸上的單位矢量，軸上的單位矢量，a= , b= , c= ，w為比例系數(shù)為比例系數(shù) wxwywz 顯然，齊次坐標表達并不是唯一的，隨顯然，齊次坐標表達并不是唯一的，隨w值的不同而不同。在計算機圖學(xué)中，值的不同而不同。在計算機圖學(xué)中，w 作為通用比例因子，它可取任意正值，但作為通用比例因子，它可取任意正值，但在機器人的運動分析中，總是取在機器人的運動分析中，總是取w=1 。列矩陣列矩陣 例例 ：kjiV543可以表示為：可以表示為： V=3 4 5 1V=3 4 5 1T T 或或 V=6 8

11、 10 2V=6 8 10 2T T 或或 V=-12 -16 -20 -4V=-12 -16 -20 -4T T 齊次坐標與三維直角坐標的區(qū)別齊次坐標與三維直角坐標的區(qū)別 V點在點在OXYZ坐標系中表坐標系中表示是唯一的（示是唯一的（x、y、z） 而在齊次坐標中表示可而在齊次坐標中表示可以是多值的。不同的表以是多值的。不同的表示方法代表的示方法代表的V點在空間點在空間位置上不變。位置上不變。 xyzzzxV圖2-2o2.2 2.2 旋轉(zhuǎn)齊次變換旋轉(zhuǎn)齊次變換 用齊次坐標變換來表示式旋轉(zhuǎn)變換：用齊次坐標變換來表示式旋轉(zhuǎn)變換： 110000001wvuzyxPPPRPPP1100000011zyx

12、wvuPPPRPPP 2.2.3 2.2.3 合成旋轉(zhuǎn)矩陣合成旋轉(zhuǎn)矩陣: :例例1：在動坐標中有一固定點：在動坐標中有一固定點 ，相對固定參，相對固定參考坐標系考坐標系 做如下運動：做如下運動： R（x, 90）；）； R(z, 90)； R(y,90)。求點。求點 在固定參考坐標系在固定參考坐標系 下下的位置。的位置。 TuvwPo1321OxyzuvwPoOxyz解解1：用畫圖的簡單方法：用畫圖的簡單方法 解解2：用分步計算的方法：用分步計算的方法 R（x, 90） R（z, 90） R（y, 90） 123113211000001001-000001P121312311000010000

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

14、動時，其合成旋轉(zhuǎn)矩陣為各基本旋轉(zhuǎn)矩陣依旋轉(zhuǎn)順序左乘左乘。注意：注意：旋轉(zhuǎn)矩陣間不可以交換旋轉(zhuǎn)矩陣間不可以交換 uvwOOxyz 平移齊次變換矩陣平移齊次變換矩陣1000100010001c) b (a TransHcba注意：注意：平移矩陣間可以交換，平移矩陣間可以交換， 平移和旋轉(zhuǎn)矩陣間不可以交換平移和旋轉(zhuǎn)矩陣間不可以交換 zyxoo w u v abc因此對向量 u = x y z w T，經(jīng)H變換為向量v可表示為 x + aw x / w + a y + bw y / w + b z + cw z / w + c w 12.2.4 2.2.4 相對變換相對變換 舉例說明：舉例說明：例例1

15、：動坐標系動坐標系0起始位置與固定參考坐標系起始位置與固定參考坐標系0重合重合,動坐標系動坐標系0做如下運動：做如下運動：R(Z,90) R（y,90） Trans(4，-3, 7)，求合成矩陣，求合成矩陣 解解1：用畫圖的方法：用畫圖的方法： ozyx74-3owuvvuwzyxoo(o)xyzuvwzyxuwo(o) v解解2：用計算的方法：用計算的方法 根據(jù)定義根據(jù)定義1，我們有：，我們有：1000701030014100 )R(Z,90 )90 R(y, 7) , 3 ,Trans(4T 以上均以固定坐標系多軸為變換基準，因此矩陣左乘。以上均以固定坐標系多軸為變換基準，因此矩陣左乘。如

16、果我們做如下變換，也可以得到相同的結(jié)果：如果我們做如下變換，也可以得到相同的結(jié)果： 例例2：先平移先平移Trans (4,-3,7)；繞當(dāng)前繞當(dāng)前 軸轉(zhuǎn)動軸轉(zhuǎn)動90； 繞當(dāng)前繞當(dāng)前 軸轉(zhuǎn)動軸轉(zhuǎn)動90；求合成旋轉(zhuǎn)矩陣。；求合成旋轉(zhuǎn)矩陣。 vw （2-202-20）解解1：用畫圖的方法：用畫圖的方法 zyxo(o)vwuzyxoowuvozyxowvuxyzoowuv解解2：用計算的方法：用計算的方法 1000701030014100)R(Z,90 )90 R(y, 7) , 3 ,Trans(4Too式（式（2-202-20）和式（）和式（2-212-21）無論在形式上，還是在結(jié)果上都是）無論在

17、形式上，還是在結(jié)果上都是一致的。因此我們有如下的結(jié)論：一致的。因此我們有如下的結(jié)論：動坐標系在固定坐標系中的齊次變換有動坐標系在固定坐標系中的齊次變換有2 2種情況：種情況：定義定義1 1：如果所有的變換都是相對于固定坐標系中各坐標軸旋如果所有的變換都是相對于固定坐標系中各坐標軸旋轉(zhuǎn)或平移，則依次左乘，稱為絕對變換。轉(zhuǎn)或平移，則依次左乘，稱為絕對變換。定義定義2 2：如果動坐標系相對于自身坐標系的當(dāng)前坐標軸旋轉(zhuǎn)或如果動坐標系相對于自身坐標系的當(dāng)前坐標軸旋轉(zhuǎn)或平移，則齊次變換為依次右乘，稱為相對變換。平移，則齊次變換為依次右乘，稱為相對變換。 結(jié)果均為動坐標系在固定坐標中的位姿（位置結(jié)果均為動坐

18、標系在固定坐標中的位姿（位置+ +姿態(tài)）。相姿態(tài)）。相對于固定坐標系，對于固定坐標系，軸。軸相當(dāng)于軸，軸相對于軸，軸相當(dāng)于ZYXwv 也就是說，動坐標系繞自身坐標軸做齊次變換，也就是說，動坐標系繞自身坐標軸做齊次變換，要達到繞固要達到繞固定坐標系相等的結(jié)果，就應(yīng)該用相反的順序。定坐標系相等的結(jié)果，就應(yīng)該用相反的順序。 機器人用到相對變換的機器人用到相對變換的時候比較多時候比較多 例如機械手抓一個杯子，例如機械手抓一個杯子，如右圖所示，手爪需要如右圖所示，手爪需要轉(zhuǎn)動一個角度才抓的牢，轉(zhuǎn)動一個角度才抓的牢，相對于固定坐標系表達相對于固定坐標系表達太麻煩，可以直接根據(jù)太麻煩，可以直接根據(jù)手爪的坐標

19、系表示手爪的坐標系表示 但也要知道在但也要知道在O中的位中的位姿，就用右乘的概念。姿，就用右乘的概念。 xyzoH2.2.5 2.2.5 繞通過原點的任意軸旋轉(zhuǎn)的齊次變換繞通過原點的任意軸旋轉(zhuǎn)的齊次變換 有時動坐標系有時動坐標系OO 可能繞過原點可能繞過原點O O的而分量分別為的而分量分別為rx、ry、rz的任意單位矢量的任意單位矢量r 轉(zhuǎn)動轉(zhuǎn)動角。角。研究這種轉(zhuǎn)動的好處是可用研究這種轉(zhuǎn)動的好處是可用OO 繞某軸繞某軸r 的一次轉(zhuǎn)動代的一次轉(zhuǎn)動代替繞替繞OO各坐標軸的數(shù)次轉(zhuǎn)動各坐標軸的數(shù)次轉(zhuǎn)動為推導(dǎo)此旋轉(zhuǎn)矩陣，可作下述變換：為推導(dǎo)此旋轉(zhuǎn)矩陣，可作下述變換：a.繞繞X 軸轉(zhuǎn)軸轉(zhuǎn)角，角， 使使r

20、軸處于軸處于XZ平面內(nèi)平面內(nèi)a.繞繞Y 軸轉(zhuǎn)軸轉(zhuǎn)-角，使角，使r 軸與軸與OZ軸重合軸重合a.繞繞OZ軸轉(zhuǎn)動軸轉(zhuǎn)動角角b.繞繞Y 軸轉(zhuǎn)軸轉(zhuǎn)角角c.繞繞X 軸轉(zhuǎn)軸轉(zhuǎn)-角角XYZrxryrzABCDBO51243rA由上圖容易求出：由上圖容易求出：2z2yyrrrsin2z2yzrrrcosxxrrrrOCsin2z2y2z2yrrrrrOBCBcos由定義由定義1和定義和定義2，上述，上述5次旋轉(zhuǎn)的合成旋轉(zhuǎn)矩陣為：次旋轉(zhuǎn)的合成旋轉(zhuǎn)矩陣為：cossin0sin-cos0001cos0sin010sin-0cos1000cossin0sin-coscos0sin-010sin0coscossin0s

21、incos0001RRRRRR,x,y, z,y,x, r（2-252-25）帶入式帶入式（2-252-25），得），得cos)cos(1rsinr)cos(1rrsinr)cos(1rrsinr)cos(1rrsinr)cos(1rrcos)cos(1rsinr)cos(1rrsinr)cos(1rrcos)cos(1rR2zxzyyzxxzyyzx2yzyxzyx2x, r2.2.6 2.2.6 齊次交換矩陣的幾何意義齊次交換矩陣的幾何意義 設(shè)設(shè)T= T= ，有一個手爪，已知其在，有一個手爪，已知其在OO的位置，設(shè)一個的位置，設(shè)一個該坐標系該坐標系OO ，已知，已知， ，那么，那么OO 在

22、在OO中的齊次坐中的齊次坐標變換為標變換為 ，如果手爪轉(zhuǎn)了一個角度，如果手爪轉(zhuǎn)了一個角度， 則：則：1000321twzzztwyyytwxxx111cbao1000100010001T 1111cba1000pppTzyyyxxxzzzyxwwwT T反映了反映了O O 在在O O中的位置和姿態(tài)，即表示了該坐標系原點中的位置和姿態(tài)，即表示了該坐標系原點和各坐標軸單位矢量在固定坐標系中的位置和姿態(tài)。和各坐標軸單位矢量在固定坐標系中的位置和姿態(tài)。該矩陣可以由該矩陣可以由4 4個子矩陣組成，寫成如下形式：個子矩陣組成，寫成如下形式：比例系數(shù)透視矩陣位置矢量旋轉(zhuǎn)矩陣11311333wfPRTzzzy

23、yyxxxwww33R為姿態(tài)矩陣，表示動坐標系為姿態(tài)矩陣，表示動坐標系OO 在固定參考在固定參考坐標系坐標系OO中的姿態(tài)，即表示中的姿態(tài)，即表示OO 各坐標軸單各坐標軸單位矢量在位矢量在OO各軸上的投影各軸上的投影 為位置矢量矩陣，代表動坐標系為位置矢量矩陣，代表動坐標系OO 坐標坐標原點在固定參考坐標系原點在固定參考坐標系OO中的位置中的位置 TzyxpppP13為透視變換矩陣，在視覺中進行圖像計算，為透視變換矩陣，在視覺中進行圖像計算，一般置為一般置為0 0 000f31為比例系數(shù)為比例系數(shù) 1 11w如果需要求解如果需要求解OO在在OO 中的位置和姿態(tài)，此時的齊次變換矩中的位置和姿態(tài)，此

24、時的齊次變換矩陣為陣為 ，即求逆矩陣：，即求逆矩陣： 1T1000)(RT13T331PRT kpjpippzyx其中：其中：這些式子以后經(jīng)常遇到，這些式子以后經(jīng)常遇到，在機器人計算中，所要在機器人計算中，所要求的就是齊次變換矩陣求的就是齊次變換矩陣習(xí)題習(xí)題1 1：O O 與與O O初始重合，初始重合，O O 作如下運動：作如下運動：繞繞Z Z軸轉(zhuǎn)動軸轉(zhuǎn)動3030 ；繞繞X X軸轉(zhuǎn)動軸轉(zhuǎn)動6060 ；繞繞Y Y軸轉(zhuǎn)動軸轉(zhuǎn)動9090 。求。求T T。 100001000030cos30sin0030sin30cosR11000060cos60sin0060sin60cos000012R10000

25、90cos090sin0010090sin090cos3R1000002/12/302/34/34/102/14/34/3123RRRT習(xí)題習(xí)題2 2：OO 與與OO初始重合，初始重合，OO 作如下運動：作如下運動：繞繞X X軸轉(zhuǎn)動軸轉(zhuǎn)動9090 ；繞繞w w軸轉(zhuǎn)動軸轉(zhuǎn)動9090 ；繞繞Y Y軸轉(zhuǎn)動軸轉(zhuǎn)動9090 。求。求 T T；改變旋轉(zhuǎn)順序，如改變旋轉(zhuǎn)順序，如何旋轉(zhuǎn)才能獲得相同的結(jié)果。何旋轉(zhuǎn)才能獲得相同的結(jié)果。 1000090cos90sin0090sin-09cos00001R1100001000090cos90sin0090sin90cos2R1000090cos090sin00100

26、90sin090cos3R1000001001000001213RRRT解解： 解解： 繞繞Z Z（w w）軸轉(zhuǎn)動）軸轉(zhuǎn)動9090 ； 繞繞X X軸轉(zhuǎn)動軸轉(zhuǎn)動9090 ； 繞繞Y Y軸轉(zhuǎn)動軸轉(zhuǎn)動9090 。 變換方程變換方程（Transform equations） 研究一下圖描述的研究一下圖描述的一個物體與機械手一個物體與機械手情情況，機械手用變換況，機械手用變換 Z 相對于基坐標系被定位。相對于基坐標系被定位。機械手的端點用變換機械手的端點用變換 ZT6 來描述，而末端執(zhí)行器來描述，而末端執(zhí)行器用變換用變換 T6E 來描述。物體用變換來描述。物體用變換 B 相對于基坐相對于基坐標系被定位。

27、最后，機械手末端抓手用變換標系被定位。最后，機械手末端抓手用變換 BG相對于物體被定位。末端抓手位置的描述有兩種相對于物體被定位。末端抓手位置的描述有兩種方式，一種是相對于物體的描述，一種是相對于方式，一種是相對于物體的描述，一種是相對于機械手的描述。由于兩種方式描述的是同一個機械手的描述。由于兩種方式描述的是同一個點，我們可以把這個描述等同起來，得到點，我們可以把這個描述等同起來，得到 Z ZT6 T6E = B BG 這個方程可以用有向變換圖來表示。圖的每這個方程可以用有向變換圖來表示。圖的每一段弧表示一個變換。從它的定義的坐標系一段弧表示一個變換。從它的定義的坐標系向外指向。向外指向。 用用 Z- -1左乘和用左乘和用E- -1右乘方程，得到右乘方程，得到 T6 = Z- -1 B G E- -10EGBZT6zyx一個物體與機械手有向變換圖GBET6Z0例題：例題：試求立方體中心在機座坐標系試求立方體中心在機座坐標系0中的位置中的