機器人的空間描述與坐標變換

1、1 第二章 機器人的空間描述和坐標變換2.1 位姿和坐標系描述2.2平移和旋轉(zhuǎn)坐標系映射2.3平移和旋轉(zhuǎn)齊次坐標變換2.4物體的變換和變換方程2.5通用旋轉(zhuǎn)變換2zyxApppp2-1圖 位置表示2.1位置方位表示與坐標系描述位置方位表示與坐標系描述 n1.位置描述位置描述 矢量矢量 Ap 表示箭頭指向點的位置矢量，其表示箭頭指向點的位置矢量，其中右上角標中右上角標“A”表示該點是用表示該點是用A坐標系描述坐標系描述的。的。（2-2） n2.方位描述方位描述 坐標系坐標系B與機械手末端工具固連，工具的姿態(tài)與機械手末端工具固連，工具的姿態(tài)可以由坐標系可以由坐標系B的方向來描述。而坐標系的方向來描

2、述。而坐標系B的方的方向可以用沿三個坐標軸的單位矢量來表示向可以用沿三個坐標軸的單位矢量來表示333231232221131211rrrrrrrrrRBABABAABZYX圖2-2方位表示（2-1） 旋轉(zhuǎn)矩陣旋轉(zhuǎn)矩陣 描述坐標系描述坐標系B的姿態(tài)，矢量的姿態(tài)，矢量 描述坐標系描述坐標系B的原點位的原點位置。置。3BoAABRBp3.位姿描述位姿描述 固連坐標系把剛體位姿描述問題轉(zhuǎn)化為坐標系的描述問題。圖固連坐標系把剛體位姿描述問題轉(zhuǎn)化為坐標系的描述問題。圖2-3中坐標系中坐標系B可以在固定坐標系可以在固定坐標系A(chǔ)中描述為中描述為（2-3）RABBoAP41.平移坐標變換平移坐標變換圖圖2-3平

3、移變換平移變換 BP為坐標系為坐標系B描述的某一空間位描述的某一空間位置，我們也可以用置，我們也可以用AP（坐標系（坐標系A(chǔ)）描）描述同一空間位置。因為兩個坐標系具有述同一空間位置。因為兩個坐標系具有相同的姿態(tài)，同一個點在不同坐標系下相同的姿態(tài)，同一個點在不同坐標系下的描述滿足以下關(guān)系的描述滿足以下關(guān)系A(chǔ)BABoPPP （2-4）2.2平移和旋轉(zhuǎn)坐標系映平移和旋轉(zhuǎn)坐標系映射射 旋轉(zhuǎn)坐標變換的任務(wù)是已知坐標系旋轉(zhuǎn)坐標變換的任務(wù)是已知坐標系B描述的描述的一個點的位置矢量一個點的位置矢量BP和和旋轉(zhuǎn)矩陣旋轉(zhuǎn)矩陣 ，求在坐標，求在坐標系系A(chǔ)下描述同一個點的位置矢量下描述同一個點的位置矢量AP。 52.

4、旋轉(zhuǎn)坐標變旋轉(zhuǎn)坐標變換換RABABT BxAABT ByAABT BzApppXPYPZP （2-5）將（將（2-5）式寫成矩陣形式得：）式寫成矩陣形式得：PPZYXPBABBTABTABTABAR（2-6）圖2-4旋轉(zhuǎn)變換 式（式（2-6）即為我們要求的旋轉(zhuǎn)變換關(guān)系，該變換是通過兩個坐）即為我們要求的旋轉(zhuǎn)變換關(guān)系，該變換是通過兩個坐標系之間的旋轉(zhuǎn)變換實現(xiàn)的。標系之間的旋轉(zhuǎn)變換實現(xiàn)的。 63.復(fù)合變換復(fù)合變換圖圖2-5復(fù)合變換復(fù)合變換 如果兩個坐標系之間即存在平移如果兩個坐標系之間即存在平移又存在旋轉(zhuǎn)，如何計算同一個空間點又存在旋轉(zhuǎn)，如何計算同一個空間點在兩個坐標系下描述的變換關(guān)系？在兩個坐標

5、系下描述的變換關(guān)系？ 為了得到位置矢量為了得到位置矢量BP和和AP之之間的變換關(guān)系，我們建立一個中間的變換關(guān)系，我們建立一個中間坐標系間坐標系C。 PPPBABBCBCRRACAABACoBBoRPPPPP （2-7）（ 2-8）為了得到位置矢量為了得到位置矢量BP和和AP之間的變換關(guān)系，只需坐標系之間的變換關(guān)系，只需坐標系B 在坐標系在坐標系下下A的描述。的描述。 是是4 4矩陣，稱為齊次坐標變換矩陣?？梢岳斫鉃樽鴺讼稻仃嚕Q為齊次坐標變換矩陣?？梢岳斫鉃樽鴺讼礏在固定坐在固定坐標系標系A(chǔ)中的描述。中的描述。72.3齊次坐標變換 坐標變換（坐標變換（2-8）可以寫成以下形式）可以寫成以下形式

6、1101PPPBBoAABAR （2-9） 將位置矢量用將位置矢量用4 1矢量表示，增加矢量表示，增加1維的數(shù)值恒為維的數(shù)值恒為1，我們?nèi)匀挥迷覀內(nèi)匀挥迷瓉淼姆柋硎緛淼姆柋硎?維位置矢量并采用以下符號表示坐標變換矩陣維位置矢量并采用以下符號表示坐標變換矩陣10BoAABABRTP（2-10）PPBABAT（2-11）TAB 齊次坐標變換的主要作用是表達簡潔，同時在表示多個坐標變換齊次坐標變換的主要作用是表達簡潔，同時在表示多個坐標變換的時候比較方便。的時候比較方便。1.齊次變換齊次變換82.齊次變換算子齊次變換算子 在機器人學(xué)中還經(jīng)常用到下面的變換，如圖在機器人學(xué)中還經(jīng)常用到下面的變換

7、，如圖2-8，矢量，矢量AP1沿矢量沿矢量AQ平移至的平移至的AQ終點，得一矢量終點，得一矢量AP2。已知。已知AP1和和AQ求求AP2的過程稱之為的過程稱之為平移變換，與前面不同，這里只涉及單一坐標系。平移變換，與前面不同，這里只涉及單一坐標系。圖圖2-6平移算子平移算子QPPAAA12 （2-12）可以采用齊次變換矩陣表示平移變換可以采用齊次變換矩陣表示平移變換12)(PQPAAATrans（2-13）)( QATrans稱為平移算子，其表達式為稱為平移算子，其表達式為1)(0QQAAITrans （2-14） 其中其中I是是3 3單位矩陣。例如若單位矩陣。例如若AQ=ai+bj+ck，其

8、中其中i、j和和k分別表示坐標系分別表示坐標系A(chǔ)三個坐標軸的三個坐標軸的單位矢量，則平移算子表示為單位矢量，則平移算子表示為1000100010001),(cbacbaTrans 9同樣，我們可以研究矢量在同一坐標系下的旋轉(zhuǎn)同樣，我們可以研究矢量在同一坐標系下的旋轉(zhuǎn)變換，如圖變換，如圖2-9，AP1繞繞Z軸轉(zhuǎn)軸轉(zhuǎn) 角得到角得到AP2。則。則圖圖2-7旋轉(zhuǎn)算子旋轉(zhuǎn)算子12),(PPAAzRot（2-20）Rot(z, )稱為旋轉(zhuǎn)算子，其表達式為稱為旋轉(zhuǎn)算子，其表達式為100001000000),(cssczRot （2-21）同理，可以得到繞同理，可以得到繞X軸和軸和Y軸的旋轉(zhuǎn)算子軸的旋轉(zhuǎn)算子1

9、00000000001),(csscxRot100000001000),(csscyRot 10 定義了平移算子和旋轉(zhuǎn)算子以后，可以將它們復(fù)合實現(xiàn)復(fù)雜的映射定義了平移算子和旋轉(zhuǎn)算子以后，可以將它們復(fù)合實現(xiàn)復(fù)雜的映射關(guān)系。變換算子與前面介紹的坐標變換矩陣形式完全相同，因為所有描關(guān)系。變換算子與前面介紹的坐標變換矩陣形式完全相同，因為所有描述均在同一坐標系下，所以不需上下標描述（坐標系）。述均在同一坐標系下，所以不需上下標描述（坐標系）。21AAPT P （2-23）TABABRBoAPPPBABAT21AAPT P齊次坐標變換總結(jié)： 表示坐標系表示坐標系B在坐標系在坐標系A(chǔ)下的描述，下的描述，

10、的各列是坐標系的各列是坐標系B三個坐標軸方向的單位矢量，三個坐標軸方向的單位矢量， 而表示坐標系而表示坐標系B原點位置原點位置。2. 它是不同坐標系間的坐標變換。如它是不同坐標系間的坐標變換。如3 3. .它是同一坐標系內(nèi)的變換算子。它是同一坐標系內(nèi)的變換算子。 齊次坐標變換是復(fù)雜空間變換的基礎(chǔ)，必須認真理解和掌握。具體應(yīng)齊次坐標變換是復(fù)雜空間變換的基礎(chǔ)，必須認真理解和掌握。具體應(yīng)用的關(guān)鍵是理解它代表的是上面三種含義的哪一種，而不是簡單的套用用的關(guān)鍵是理解它代表的是上面三種含義的哪一種，而不是簡單的套用公式！公式！1. 它是坐標系的描述。它是坐標系的描述。如圖如圖2-10表示的三個坐標系，已知

11、坐標系表示的三個坐標系，已知坐標系A(chǔ)、B和和C之間的變換矩陣之間的變換矩陣 和和 位置位置矢量矢量CP，求在坐標系，求在坐標系A(chǔ)下表示同一個點的下表示同一個點的位置矢量位置矢量AP。 113.復(fù)合變換復(fù)合變換 復(fù)合變換主要有兩種應(yīng)用形式，一種是建立了多個坐標系描述機器人復(fù)合變換主要有兩種應(yīng)用形式，一種是建立了多個坐標系描述機器人的位姿，任務(wù)是確定不同坐標系下對同一個量描述之間的關(guān)系；另一種是的位姿，任務(wù)是確定不同坐標系下對同一個量描述之間的關(guān)系；另一種是一個空間點在同一個坐標系內(nèi)順序經(jīng)過多次平移或旋轉(zhuǎn)變換，任務(wù)是確定一個空間點在同一個坐標系內(nèi)順序經(jīng)過多次平移或旋轉(zhuǎn)變換，任務(wù)是確定多次變換后點的

12、位置。多次變換后點的位置。圖圖2-10 復(fù)合坐標變換復(fù)合坐標變換 TABTBCPPCBCBTPPPCBCABBABATTT （2-24）（2-25）TTTBCABAC 根據(jù)坐標變換的定義得根據(jù)坐標變換的定義得（2-26）12(a) ZY順序旋轉(zhuǎn)順序旋轉(zhuǎn) (b) Y Z順序旋轉(zhuǎn)順序旋轉(zhuǎn)圖圖2-11旋轉(zhuǎn)順序?qū)πD(zhuǎn)順序?qū)ψ儞Q結(jié)果影響變換結(jié)果影響例例2-3已知點已知點u=7i+3j+2k，先對它進行繞，先對它進行繞Z軸旋轉(zhuǎn)軸旋轉(zhuǎn)90o的變換得點的變換得點v，再對點，再對點v進行繞進行繞Y軸旋轉(zhuǎn)軸旋轉(zhuǎn)90o的變換得的變換得點點w，求，求v和和w。127312371000010000010010)90,(

13、uvozRot137212731000000100100100)90,(vwoyRot如果只關(guān)心最后的變換結(jié)果，可以按下式計算如果只關(guān)心最后的變換結(jié)果，可以按下式計算( ,90 )( ,90 )( ,90 )oooRot yRot yRot zwvu001072100037010023000111 計算結(jié)果與前面的相同，稱計算結(jié)果與前面的相同，稱R= Rot(y,90o) Rot(z,90o) 為復(fù)合旋轉(zhuǎn)算子。為復(fù)合旋轉(zhuǎn)算子。 13注：固定坐標系變換，矩陣乘的順序注：固定坐標系變換，矩陣乘的順序“自右向左自右向左” 如果改變旋轉(zhuǎn)順序，先對它進行繞如果改變旋轉(zhuǎn)順序，先對它進行繞y軸旋轉(zhuǎn)軸旋轉(zhuǎn)90

14、o，再繞，再繞z軸旋轉(zhuǎn)軸旋轉(zhuǎn)90o，結(jié)，結(jié)果如圖果如圖2-11b所示。比較圖所示。比較圖2-11a和圖和圖2-11b可以發(fā)現(xiàn)最后的結(jié)果并不相同，可以發(fā)現(xiàn)最后的結(jié)果并不相同，即旋轉(zhuǎn)順序影響變換結(jié)果。即旋轉(zhuǎn)順序影響變換結(jié)果。從數(shù)學(xué)角度解釋就是矩陣乘法不滿足交換率，從數(shù)學(xué)角度解釋就是矩陣乘法不滿足交換率， Rot(y,90o) Rot(z,90o) Rot(z,90o) Rot(y,90o)。和和 ，求求 和和給定給定 計算計算142.4物體的變換和變換方程物體的變換和變換方程TABTBA已知坐標系已知坐標系B相對坐標系相對坐標系A(chǔ)的描述的描述求求坐標系坐標系A(chǔ)相對坐標系相對坐標系B的描述的描述一種

15、直接的方法是矩陣求逆，另一種方法是根據(jù)變換矩陣的特點直一種直接的方法是矩陣求逆，另一種方法是根據(jù)變換矩陣的特點直接得出逆變換。后一種方法更簡單方便。接得出逆變換。后一種方法更簡單方便。即齊次變換的求逆問題。即齊次變換的求逆問題。TABTBA等價為：已知等價為：已知RABBoAPRBAAoBP 是坐標系是坐標系B的原點在的原點在坐標系坐標系B中的描述，顯然為零矢量。中的描述，顯然為零矢量。由由（2-28）式得）式得15根據(jù)前面的討論，旋轉(zhuǎn)矩陣關(guān)系為根據(jù)前面的討論，旋轉(zhuǎn)矩陣關(guān)系為TABABBARRR1 （2-27）將坐標變換用于坐標系將坐標變換用于坐標系B的原點得的原點得AoBBoABABoBRP

16、PP（2-28）BoBPBBAAT AAoABoBBoRR PPP（2-29）逆變換可以直接用正變換的旋轉(zhuǎn)矩陣和平移矩陣表示逆變換可以直接用正變換的旋轉(zhuǎn)矩陣和平移矩陣表示10BoATABTABBARRTP（2-30）16A沿沿xA平移平移3個單位，再繞新的個單位，再繞新的zA 軸軸轉(zhuǎn)轉(zhuǎn)180o得得 B18018001001801800010001001ABcsRsc因此因此1003010000100001ABTB沿沿zB平移平移2個單位，然后繞個單位，然后繞yB軸轉(zhuǎn)軸轉(zhuǎn)90o再繞新再繞新xB軸轉(zhuǎn)軸轉(zhuǎn)150o得得 C3122331122223122900901000 0 1 1000010015

17、01500 1 0 009009001501501 0 0 0100BCcsRcsscsc 圖圖2-12楔形塊角點坐標楔形塊角點坐標系系例例2-4，如圖如圖2-12給出的楔形塊角點坐標系，求齊次坐標變換給出的楔形塊角點坐標系，求齊次坐標變換,ABABCCTTT,33112222331122221003000301000000001010021002000100010001AABCBCTT T 因此因此A沿沿xA和和zA平移平移3和和2，然后繞，然后繞yA軸轉(zhuǎn)軸轉(zhuǎn)90，再繞新，再繞新xA軸轉(zhuǎn)軸轉(zhuǎn) - -30得得C也可以按以下方法計算也可以按以下方法計算31223311222231229 009

18、010001003 03 09 009 003 03 00011000010001000100ACcsRcsscsc 17 事實上，對于像本例題這種簡單的情況，可以直接利用齊次坐標變換事實上，對于像本例題這種簡單的情況，可以直接利用齊次坐標變換的定義得到變換矩陣。即直接寫出坐標系的定義得到變換矩陣。即直接寫出坐標系C坐標軸矢量在坐標系坐標軸矢量在坐標系A(chǔ)下表下表示得旋轉(zhuǎn)矩陣，平移矢量為坐標系示得旋轉(zhuǎn)矩陣，平移矢量為坐標系C的原點在坐標系的原點在坐標系A(chǔ)下的矢量表示。下的矢量表示。18變換方程變換方程 圖圖2-13表示了多個坐標系的關(guān)系圖，可以用兩種不表示了多個坐標系的關(guān)系圖，可以用兩種不同的方

19、式得到世界坐標系同的方式得到世界坐標系U下坐標系下坐標系D的描述。的描述。TTTADUAUDTTTTCDBCUBUD （2-31） （2-32）由（由（2-31）和（）和（2-32）可以得到變換方程）可以得到變換方程圖圖2-13坐標變換序列坐標變換序列 可以利用變換方程可以利用變換方程（2-33）求解其中任意一個未知變換。例如，假設(shè)求解其中任意一個未知變換。例如，假設(shè)除除 以外其余變換均為已知，則該未知變換可以用下式計算以外其余變換均為已知，則該未知變換可以用下式計算TUB11UUACBBADDCTT T TT在坐標系的圖形表示方法中，從一個坐標系原點指向另一個坐標系原點在坐標系的圖形表示方法

20、中，從一個坐標系原點指向另一個坐標系原點的箭頭表示坐標系的描述關(guān)系。的箭頭表示坐標系的描述關(guān)系。 TTTBCUBUC（2-35）1UUDDCAACTT TT（2-36） 19例例2-5假設(shè)已知圖機械臂末端工具坐標系假設(shè)已知圖機械臂末端工具坐標系T相對基座相對基座坐標系坐標系B的描述，還已知工作臺坐標系的描述，還已知工作臺坐標系S相對基座相對基座坐標系坐標系B的描述，并且已知螺栓坐標系的描述，并且已知螺栓坐標系G相對工作相對工作臺坐標系臺坐標系S的描述。計算螺栓相對機械臂工具坐標的描述。計算螺栓相對機械臂工具坐標系的位姿。系的位姿。 解：添加從工具坐標系解：添加從工具坐標系T原點到螺栓坐標系原點

21、到螺栓坐標系G原點原點的箭頭，可以得到如下變換方程的箭頭，可以得到如下變換方程TTTTTGBTSGBS （2-37）螺栓相對機械臂工具坐標系的位姿描述為螺栓相對機械臂工具坐標系的位姿描述為TTTTSGBSBTTG1 （2-38）20 xyzffffijk1.繞任意軸旋轉(zhuǎn)變換繞任意軸旋轉(zhuǎn)變換下面討論繞任意軸下面討論繞任意軸 f 旋轉(zhuǎn)矩陣，軸在坐標系旋轉(zhuǎn)矩陣，軸在坐標系A(chǔ)下表示為下表示為以以 f 為為 Z 軸建立與軸建立與A固連的坐標系固連的坐標系C用用n、o和和f表示坐標系表示坐標系C三個坐標三個坐標軸的單位矢量，在坐標系軸的單位矢量，在坐標系A(chǔ)下表示為下表示為圖圖2-18繞任意軸旋轉(zhuǎn)變換繞任意

22、軸旋轉(zhuǎn)變換 xyzxyzxyznnnooofffnijkoijkfijkxxxACyyyzzznofRnofnof 因為固連的坐標系因為固連的坐標系C與與A固連，所以繞固連，所以繞 f旋轉(zhuǎn)旋轉(zhuǎn)等價于繞等價于繞ZC旋轉(zhuǎn)。為此我們先將旋轉(zhuǎn)。為此我們先將Ap在坐標系在坐標系C下下表示，再繞表示，再繞ZC旋轉(zhuǎn)旋轉(zhuǎn) 角，最后再把旋轉(zhuǎn)得到的矢量角，最后再把旋轉(zhuǎn)得到的矢量用坐標系用坐標系A(chǔ)表示。表示。CATACAACRRppp1( , )( , )ATACCCRot zRot zRpppAp1 = Rot(f, ) Ap2.52.5通用旋轉(zhuǎn)變換通用旋轉(zhuǎn)變換21再將再將Cp1在坐標系在坐標系A(chǔ)下表示下表示11

23、( , )AAATAACCCCRR Rot zRppp0( , )( , )0001xyzxxxAATCCyyyxyzzzzxyzxxyyzzxxxyyyxxyyzzzzzxyznnnnofcsRotR Rot zRnofscooonoffffn co sn co sn co snofnnofn so cn so cn so cnofffff111222333oanoanoa因此因此123xxxxxxxxxxxyyxxyxyxyxzzxxzxzxznn n cn o sn o so o cf fnn n cn o sn o so o cf fnn n cn o sn o so o cf f其中

24、一個矢量其中一個矢量上式中的上式中的n和和o各分量是未知的，需要用各分量是未知的，需要用 f 的各分量表示的各分量表示 22根據(jù)坐標系的右手規(guī)則知根據(jù)坐標系的右手規(guī)則知n o = f，叉積可以按下式計算，叉積可以按下式計算()()()xyzyzzyzxxzxyyxxyznnnn on on on on on ooooijkn oijk(),(),()yzzyxxyxzyxyyxzn on ofn on ofn on of再根據(jù)旋轉(zhuǎn)矩陣的正交性可以得再根據(jù)旋轉(zhuǎn)矩陣的正交性可以得 1,0 xxxxxxxyxyxyn no of fn no of f( , ),1xxxyzxzyxyzyyyzxxzyyzxzzf f vcf f vf sf f vf sRotf f vf sf f vcf f vf svcf f vf sf f vf sf f vc f123xxxxxxxxxxxyyxxyxyxyxzzxxzxzxznn n cn o sn o so o cf fnn n cn o sn o so o cf fnn n cn o sn