機器人學(xué)—數(shù)學(xué)基礎(chǔ)

1、王新慶 n機械設(shè)計與車輛工程系n 工科D414n工程軟件演示與講解教學(xué)法機器人運動學(xué)及其數(shù)學(xué)基礎(chǔ)參考教材n美付京遜機器人學(xué)n中南大學(xué)蔡自興機器人學(xué)n美理查德鮑爾機器人操作手?jǐn)?shù)學(xué)編程與控制參考教材n中南大學(xué)蔡自興n 中南大學(xué)教授，我國人工中南大學(xué)教授，我國人工智能和機器人領(lǐng)域著名專智能和機器人領(lǐng)域著名專家家n 中國人工智能學(xué)會智能機中國人工智能學(xué)會智能機器人專委會理事長器人專委會理事長n 曾與付京遜教授一起工作曾與付京遜教授一起工作過過一 機器人位置和姿態(tài)的描述Justin catch balln串聯(lián)機器人可以用一個開環(huán)關(guān)節(jié)鏈來建模n由數(shù)個驅(qū)動器驅(qū)動的轉(zhuǎn)動或移動關(guān)節(jié)串聯(lián)而成n一端固定在基座上，另

2、一端是自由的，安裝工具（末端執(zhí)行器），用以操縱物體，或完成各種任務(wù)運動學(xué)問題：nB,H坐標(biāo)系n位置：H的原點在B坐標(biāo)系中的坐標(biāo)表示n姿態(tài)：H坐標(biāo)系相對于B坐標(biāo)系的姿態(tài)inoa關(guān)節(jié)變量末端執(zhí)行器位置和姿態(tài)運動學(xué)研究的兩個問題Where is my hand?Direct KinematicsHERE!How do I put my hand here?Inverse Kinematics: Choose these angles!n軸線平行及相交n軸線異面n軸線平行時采用幾何分析方法n1955年丹納維特（Denavit）和哈頓伯格（Hartenberg）提出了一種采用矩陣代數(shù)方法解決機器人的運動

3、學(xué)問題D-H方法具有直觀的幾何意義能表達動力學(xué)、計算機視覺和比例變換問題n數(shù)學(xué)基礎(chǔ)是齊次變換二 數(shù)學(xué)基礎(chǔ)齊次坐標(biāo)和齊次變換2.1 點和面的齊次坐標(biāo)2.1.1 點的齊次坐標(biāo) n用n+1個變量表示n維空間的幾何元素。n 引入齊次坐標(biāo)的目的是為了表示幾何變換的旋轉(zhuǎn)、平移和縮放 顯然，齊次坐標(biāo)表達并不是唯一的，隨w值的不同而不同。在計算機圖學(xué)中，w 作為通用比例因子，它可取任意正值，但在機器人的運動分析中，總是取w=1 。kcj bi av 一個點矢：一個點矢：Tx y z Vw列矩陣列矩陣式中式中i, j, k為為x, y, z 軸上的單位矢量，軸上的單位矢量，a= , b= , c= ，w為比例系

4、數(shù)為比例系數(shù) wxwywz例1：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 n 齊次坐標(biāo)與三維直角坐標(biāo)的區(qū)別nV點在OXYZ坐標(biāo)系中表示是唯一的（a、b、c）n而在齊次坐標(biāo)中表示可以是多值的。不同的表示方法代表的V點在空間位置上不變。 n 幾個特定意義的齊次坐標(biāo)：n0 0 0 nT坐標(biāo)原點矢量的齊次坐標(biāo)，n為任意非零比例系數(shù) n1 0 0 0T 指向無窮遠處的OX軸n0 1 0 0T 指向無窮遠處的OY軸 n0 0 1 0T

5、 指向無窮遠處的OZ軸 n0 0 0 0T 沒有意義n 2個常用的公式：個常用的公式：zzyyxxbabababakbabajbabaibababbbaaakjibaxyyxzxxzyzzyzyxzyx)()()(點乘點乘:叉乘叉乘:2.1.2 平面的齊次坐標(biāo)n平面齊次坐標(biāo)由行矩陣P=a b c d 來表示n當(dāng)點v=x y z wT處于平面P內(nèi)時，矩陣乘積PV=0，或記為0dwczbyaxwzyxdcbaPV如果定義一個常數(shù) m= ，則有： 222cba() ()xyzabcx ay bz cdijkijkwwwmmmw mw mw mm 可以把矢量 解釋為某個平面的外法線，此平面沿著法線方向

6、與坐標(biāo)原點的距離為(-d/m)。 )(kmcjmbiman 點和平面間的位置關(guān)系設(shè)一個平行于設(shè)一個平行于x、y軸，且在軸，且在z軸上的坐標(biāo)為單位距離的平軸上的坐標(biāo)為單位距離的平面面P可以表示為：可以表示為： 或或 有：有： PV= 1100P2200P v0 v0 v0 點在平面下方點在平面上點在平面上方與點矢與點矢 相仿，平面相仿，平面 也沒有意義也沒有意義 T00000000例如：點例如：點 V=10 20 1 1T 必定處于此平面內(nèi)，而點必定處于此平面內(nèi)，而點 V=0 0 2 1T處于平處于平 P 的上方，點的上方，點V=0 0 0 1T處于處于P平面下方，因為：平面下方，因為：1020

7、001010011 0 1120011000 -110001-100n2.1.3 平移變換平移變換n1、二維坐標(biāo)平移變換、二維坐標(biāo)平移變換 1111byaxyxabyxy1x1P(x1,y1)oo1沿坐標(biāo)軸平移（沿坐標(biāo)軸平移（a, b）。）。P位于位于O1坐標(biāo)系中，坐標(biāo)系中，O為絕對坐標(biāo)系為絕對坐標(biāo)系人（人（P）坐在汽車?yán)镞\動）坐在汽車?yán)镞\動11111001110011xxaxyTyby n2、三維坐標(biāo)平移變換、三維坐標(biāo)平移變換1100010001000111111111zyxcbazyxTzyx沿坐標(biāo)軸方向平移（沿坐標(biāo)軸方向平移（a,b,ca,b,c）車?yán)@盤山公路行駛車?yán)@盤山公路行駛n 平移

8、齊次變換矩陣平移齊次變換矩陣100010TTrans (a b c)0010001abcn 對任意向量對任意向量u=u=（x,y,z,wx,y,z,w）進行）進行T T變換后為：變換后為：100/010/001/00011axxawx wabyybwy wbVTuczzcwz wcww n 對已知任意向量對已知任意向量u=u=（x,y,z,wx,y,z,w）進行）進行T T變換變換實質(zhì)實質(zhì)是將是將u u與平移向量與平移向量(a,b,c,1)(a,b,c,1)相加相加2.2 旋轉(zhuǎn)矩陣及旋轉(zhuǎn)齊次變換旋轉(zhuǎn)矩陣及旋轉(zhuǎn)齊次變換2.2.1 2.2.1 旋轉(zhuǎn)矩陣旋轉(zhuǎn)矩陣 設(shè)固定參考坐標(biāo)系直角坐標(biāo)為設(shè)固定參考

9、坐標(biāo)系直角坐標(biāo)為Oxyz，動坐標(biāo)系為，動坐標(biāo)系為O uvw，研究旋轉(zhuǎn)變換情況。研究旋轉(zhuǎn)變換情況。xyzwvuPo(O)圖2-3 初始位置時，動靜坐標(biāo)系重合，初始位置時，動靜坐標(biāo)系重合，O、O 重合，如圖。各軸重合，如圖。各軸對應(yīng)重合，設(shè)對應(yīng)重合，設(shè)P點是動坐標(biāo)系點是動坐標(biāo)系O uvw中的一點，且固定不變。中的一點，且固定不變。則則P點在點在O uvw中可表示為：中可表示為： wwvvuuuvwkPjPiPP 、 、 為坐標(biāo)系為坐標(biāo)系O uvw的單位矢的單位矢量，則量，則P點在點在oxyz中可表示為：中可表示為： uivjwkzzyyxxxyzkPjPiPPxyzuvwPPn例：繞坐標(biāo)軸Z旋轉(zhuǎn)c

10、ossinABBpppxxysincosABBpppyxyABppzz2.2.2 三個基本旋轉(zhuǎn)矩陣和合成旋轉(zhuǎn)矩陣 n寫成矩陣形式00001ABppABppABppxcsxyscyzz()A BZn圖中動坐標(biāo)系換成uvw00001zwcskksc方向余弦陣方向余弦陣由圖由圖2-52-5可知，可知， 在在x x軸上的投影為軸上的投影為 ， 在在y y軸上的投影軸上的投影為為 , , 在在x x軸上的投影為軸上的投影為 ， 在在y y軸上的投影為軸上的投影為 uicosuisinvjvjuisinuivjcosvjn繞Z軸的基本旋轉(zhuǎn)矩陣對對x軸投影軸投影對對y軸投影軸投影對對z軸投影軸投影同理：同理

11、： cos0sin010sin0cos)y,R（1000cossin0sin-cos)z,R （ssin0sincos0001)R(x,co三個基本旋轉(zhuǎn)矩陣三個基本旋轉(zhuǎn)矩陣: : xyzouvwUWO總結(jié)總結(jié)三個基本旋轉(zhuǎn)矩陣三個基本旋轉(zhuǎn)矩陣 ( , )R z即動坐標(biāo)系即動坐標(biāo)系 求求 的旋轉(zhuǎn)矩陣，也就是的旋轉(zhuǎn)矩陣，也就是求出坐標(biāo)系求出坐標(biāo)系 中各軸單位矢量中各軸單位矢量 在固定坐標(biāo)系在固定坐標(biāo)系中各軸的投影分量，很容易得到在兩個坐標(biāo)系重合時，有：中各軸的投影分量，很容易得到在兩個坐標(biāo)系重合時，有：OvwOZ，繞軸轉(zhuǎn)動 角，vwOwvkji,Oxyz( , )R z100010001R 當(dāng)動坐標(biāo)

12、系當(dāng)動坐標(biāo)系O uvw繞繞O點回轉(zhuǎn)時，求點回轉(zhuǎn)時，求P點在固定坐標(biāo)系點在固定坐標(biāo)系oxyz中的位置中的位置 yzxo(O)uvwPPwPvPu圖2-4已知：已知：P點在點在O uvw中是不變的仍然中是不變的仍然成立，由于成立，由于O uvw回轉(zhuǎn)，則：回轉(zhuǎn)，則： wwvvuuuvwkPjPiPPxwwvvuuxuvwxikPjPiPiP)(PywwvvuuyuvwyjkPjPiPjP)(PzwwvvuuzuvwzkkPjPiPkP)(P用矩陣表示為用矩陣表示為: wvwzvzzwvyywxvxxzyxPPPkkjkikkjjjijkijiiiPPPy（2-7） uvwxyzwzvzzwvyywx

13、vxxPRpkkjkikkjjjijkijiii:R y則旋轉(zhuǎn)矩陣為：定義反過來：反過來： xyzuvwPRP1RRRdet*1T1RRRdet因此是正交矩陣，的行列式，由于為的伴隨矩陣，為RRRR2.2.2 旋轉(zhuǎn)齊次變換 用齊次坐標(biāo)變換來表示式（用齊次坐標(biāo)變換來表示式（2-7） 110000001wvuzyxPPPRPPP1100000011zyxwvuPPPRPPP00001zwcskkscn以繞Z軸的基本旋轉(zhuǎn)矩陣為例驗證yxxvxwyyvwzzvzwi ii ji kRj ijjj kk ik jk k n 合成旋轉(zhuǎn)矩陣合成旋轉(zhuǎn)矩陣: :例例1：在動坐標(biāo)中有一固定點：在動坐標(biāo)中有一固定點

14、 ，相對固定參，相對固定參考坐標(biāo)系考坐標(biāo)系 做如下運動：做如下運動： R（x, 90）；）； R(z, 90)； R(y,90)。求運動后點。求運動后點 在固定參考坐標(biāo)系在固定參考坐標(biāo)系 下的位置。下的位置。 TuvwPo1321OxyzuvwPoOxyz解解1：用畫圖的簡單方法：用畫圖的簡單方法 n塑料塊演示塑料塊演示解解2：用分步計算的方法：用分步計算的方法 R（x, 90） R（z, 90） R（y, 90） 123113211000001001-000001P12131231100001000001001-0 P1312121310000001-00100100 P（2-14） （2-

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

16、可以交換注意：旋轉(zhuǎn)矩陣間不可以交換 ，平移矩陣間可以交換平移矩陣間可以交換uvwOOxyz2.2.4 相對變換 舉例說明：舉例說明：例例1：動坐標(biāo)系：動坐標(biāo)系0起始位置與固定參考坐標(biāo)系起始位置與固定參考坐標(biāo)系0重合重合,動坐標(biāo)系動坐標(biāo)系0做如下運動：做如下運動：R(Z,90) R（y,90） Trans(4，-3, 7)，求合成矩陣，求合成矩陣 解解1：用畫圖的方法：用畫圖的方法： ozyx74-3owuvvuwzyxoo(o)xyzuvwzyxuwo(o) v解解2：用計算的方法：用計算的方法 根據(jù)定義根據(jù)定義1，我們有：，我們有：TTrans(4, -3, 7) R(Y, 90 ) R(Z

17、,90 )(start)00141003 01070001 以上均以固定坐標(biāo)系各軸為變換基準(zhǔn)，因此矩陣左乘。以上均以固定坐標(biāo)系各軸為變換基準(zhǔn)，因此矩陣左乘。如果我們做如下變換，也可以得到相同的結(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：用計算的方法：用計算的方法 o00141003T()Trans(4,

18、 -3, 7) R(v , 90 ) R(w ,90 )01070001ostart（2-212-21）00141003TTrans(4, -3, 7) R(y, 90 ) R(Z,90 )(start)(220)01070001 o00141003T()Trans(4, -3, 7) R(v , 90 ) R(w ,90 )01070001(221)ostart式（式（2-202-20）和式（）和式（2-212-21）無論在形式上，還是在結(jié)果上都是）無論在形式上，還是在結(jié)果上都是一致的。因此我們有如下的結(jié)論：一致的。因此我們有如下的結(jié)論：動坐標(biāo)系在固定坐標(biāo)系中的齊次變換有動坐標(biāo)系在固定坐標(biāo)系

19、中的齊次變換有2 2種情況：種情況：定義定義1 1：如果所有的變換都是相對于：如果所有的變換都是相對于固定坐標(biāo)系固定坐標(biāo)系中各坐標(biāo)軸旋中各坐標(biāo)軸旋轉(zhuǎn)或平移，則依次轉(zhuǎn)或平移，則依次左乘，稱為絕對變換左乘，稱為絕對變換。結(jié)果均為動坐標(biāo)系在固定坐標(biāo)中的位姿（位置結(jié)果均為動坐標(biāo)系在固定坐標(biāo)中的位姿（位置+ +姿態(tài)）。相姿態(tài)）。相對于固定坐標(biāo)系，對于固定坐標(biāo)系，軸。軸相當(dāng)于軸，軸相對于軸，軸相當(dāng)于ZYXwv 也就是說，動坐標(biāo)系繞自身坐標(biāo)軸做齊次變換，要達到繞固也就是說，動坐標(biāo)系繞自身坐標(biāo)軸做齊次變換，要達到繞固定坐標(biāo)系相等的結(jié)果，就應(yīng)該用相反的順序。定坐標(biāo)系相等的結(jié)果，就應(yīng)該用相反的順序。 定義定義2

20、2：如果動坐標(biāo)系相對于：如果動坐標(biāo)系相對于自身坐標(biāo)系自身坐標(biāo)系的當(dāng)前坐標(biāo)軸旋轉(zhuǎn)或的當(dāng)前坐標(biāo)軸旋轉(zhuǎn)或平移，則齊次變換為依次平移，則齊次變換為依次右乘，稱為相對變換右乘，稱為相對變換。 右乘的意義：n機器人用到相對變換的時候比較多n例如機械手抓一個杯子，如右圖所示，手爪需要轉(zhuǎn)動一個角度才抓的牢，相對于固定坐標(biāo)系表達太麻煩，可以直接根據(jù)手爪的坐標(biāo)系表示xyzoH2.2.5 齊次變換矩陣的幾何意義 設(shè)，有一個手爪，即動坐標(biāo)系設(shè)，有一個手爪，即動坐標(biāo)系OO ，已知，已知， 初始位置初始位置重合，那么重合，那么OO 在在OO中的齊次坐標(biāo)變換為：中的齊次坐標(biāo)變換為： ，如果手爪轉(zhuǎn)了一個角度，如果手爪轉(zhuǎn)了一個

21、角度， 則：則：111cbao1000100010001T 1111cba1000pppTzyyyxxxzzzyxwwwT反映了反映了O O 在在O O中的位置和姿態(tài)，即表示了該坐標(biāo)系原中的位置和姿態(tài)，即表示了該坐標(biāo)系原點和各坐標(biāo)軸單位矢量在固定坐標(biāo)系中的位置和姿態(tài)。點和各坐標(biāo)軸單位矢量在固定坐標(biāo)系中的位置和姿態(tài)。該矩陣可以由該矩陣可以由4 4個子矩陣組成，寫成如下形式：個子矩陣組成，寫成如下形式：比例系數(shù)透視矩陣位置矢量旋轉(zhuǎn)矩陣11311333wfPRTzzzyyyxxxwww33R為姿態(tài)矩陣（旋轉(zhuǎn)矩陣），表示動坐標(biāo)系為姿態(tài)矩陣（旋轉(zhuǎn)矩陣），表示動坐標(biāo)系OO 在固定參考坐標(biāo)系在固定參考坐標(biāo)系

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

23、（）（ kpjpippzyxkjizyxkvjvivvzyxkwjwiwwzyx其中：其中：這些式子以后經(jīng)常遇到，這些式子以后經(jīng)常遇到，在機器人計算中，所要在機器人計算中，所要求的就是齊次變換矩陣求的就是齊次變換矩陣 知識點： 1.點和面的齊次坐標(biāo)和齊次變換2.三個基本旋轉(zhuǎn)矩陣3.絕對變換：如果所有的變換都是相對于固定坐標(biāo)系中各坐標(biāo)軸旋轉(zhuǎn)或平移，則依次左乘，稱為絕對變換。4.相對變換：如果動坐標(biāo)系相對于自身坐標(biāo)系的當(dāng)前坐標(biāo)軸旋轉(zhuǎn)或平移，則齊次變換為依次右乘，稱為相對變換。知識點： 三個基本旋轉(zhuǎn)矩陣cos0sin010sin0cos)y,R（1000cossin0sin-cos)z,R （ssi

24、n0sincos0001)R(x,co例題例題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。 zcos30sin3000sin30cos3000R0010000110000cos60sin6000sin60cos6000001xRcos900sin9000100sin900cos9000001yR3 / 43/ 41/ 201/ 43 / 43 / 203 / 21/ 2000001yxzTR R R例題例題2 2：OO 與與OO初始重合，初始重合

25、，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é)果。 x10000cos90-sin900R0sin90cos9000001cos90sin9000sin90cos900000100001wRcos900sin9000100sin900cos9000001yR1000001001000001yxwRTR R解解： 解解： 繞繞Z（w）?軸轉(zhuǎn)動軸轉(zhuǎn)動90； 繞繞X軸轉(zhuǎn)動軸轉(zhuǎn)動90； 繞繞Y軸轉(zhuǎn)動軸轉(zhuǎn)動90。 例題例題3 3： 矢量矢量 在在OO 中表示為中表示為 ，OO 相對于相對于OO的的齊次變換為：齊次變換為： Pkjip2230