三維空間位姿描述課件

1、機器人學(xué)、機器視覺與控制（2）三維空間位姿描述內(nèi)容: 三維空間姿態(tài)描述正交旋轉(zhuǎn)矩陣三角度數(shù)表示法奇異點及萬向節(jié)鎖雙向量表示法 繞任意向量旋轉(zhuǎn)單位四元數(shù)平移與旋轉(zhuǎn)組合三維空間位姿描述三維情況實際上是前一節(jié)討論的二維情況的延伸。我們在二維坐標(biāo)系上增加一個額外的坐標(biāo)軸，通常用z表示，它同時與x軸和y軸正交。Z軸的方向服從右手規(guī)則，并構(gòu)成右手坐標(biāo)系。歐拉旋轉(zhuǎn)定理：任何兩個獨立的正交坐標(biāo)系都可以通過一系列（不超過3次）相對于坐標(biāo)軸的旋轉(zhuǎn)聯(lián)系起來，但其中兩次旋轉(zhuǎn)不能繞同一條軸線。需要注意的是，在繞坐標(biāo)軸連續(xù)旋轉(zhuǎn)的情況下要注意旋轉(zhuǎn)的順序，旋轉(zhuǎn)角度相同而順序不同可能得到完全不一樣的結(jié)果。下圖是兩種不同順序的旋

2、轉(zhuǎn)，得到的結(jié)果完全不同。注意：1.我們可以用右手的三根指頭簡單便捷地表示空間坐標(biāo)系，具體方向如右圖。 2.這些旋轉(zhuǎn)都要遵循右手準(zhǔn)則，即按照右圖所示旋轉(zhuǎn)才是正方向。 在上節(jié)課中，我們介紹了姿勢和姿勢的概念，即一種描述一個坐標(biāo)系與另一個坐標(biāo)系的平移和定向的方法。我們使用的符號是，我們使用下標(biāo)字母B表明我們談?wù)撟鴺?biāo)系的姿態(tài)B相對于坐標(biāo)系A(chǔ) .所以我們讀這個符號為B對A，由此擴(kuò)展到三維空間如下：三維空間向量變換的表示方法三維空間的姿態(tài)變換的相關(guān)性質(zhì)如下，這些性質(zhì)跟二維空間的情形差不多對于多重的相對坐標(biāo)轉(zhuǎn)換同樣可以按照相應(yīng)的規(guī)則表示，下圖展示的是一個等效的變換，即紅線和藍(lán)線都表示從O到B的變換。正如在二

3、維情況下一樣，我們可以用相對于參考坐標(biāo)系的坐標(biāo)軸單位向量表示它們所在坐標(biāo)系的方向。每一個單位向量有3個元素，它們組成了33階正交矩陣：正交旋轉(zhuǎn)矩陣上式將一個相對于坐標(biāo)系B的向量旋轉(zhuǎn)為相對于坐標(biāo)系A(chǔ)的向量。分別繞x,y,z軸旋轉(zhuǎn)角后的標(biāo)準(zhǔn)正交矩陣可表示為：我們對這個公式的來源以及性質(zhì)做一些說明：我們從上一講得到的2D坐標(biāo)變換出發(fā)，將2維空間擴(kuò)展到三維空間之中，相應(yīng)可以得到一個新的33坐標(biāo)變換矩陣，它的三個列向量分別表示新的x,y,z坐標(biāo)變換。要得到最終的三維坐標(biāo)變換公式，關(guān)鍵是求出B相對于A的變換矩陣。從二維空間坐標(biāo)變換我們已經(jīng)知道：變換矩陣是一個屬于SO（2）的特殊正交群。現(xiàn)在對于三維空間而言

4、它就是屬于SO（3）的特殊正交群，旋轉(zhuǎn)矩陣的相關(guān)性質(zhì)如下：根據(jù)上面的性質(zhì)結(jié)合二維坐標(biāo)中的情況，我們可以得到三維坐標(biāo)旋轉(zhuǎn)變換矩陣的表達(dá)式如下：可以看出：這些旋轉(zhuǎn)矩陣每一列都是單位長度的向量，每一列都與其他一列正交。對于繞z軸的旋轉(zhuǎn)，情況特殊，因為我們可以直接將其看成二維空間繞著垂直于紙面向外（也就是垂直屏幕向外）的旋轉(zhuǎn)得到，所以二維旋轉(zhuǎn)和繞Z軸的三維旋轉(zhuǎn)是等價的，即上面坐標(biāo)變換中關(guān)于z軸的旋轉(zhuǎn)與二維空間旋轉(zhuǎn)變換R是等價的。MATLAB工具箱也提供了一些函數(shù)用來計算這些基本旋轉(zhuǎn)矩陣。比如繞X軸旋轉(zhuǎn)90度可以調(diào)用rotx()函數(shù)，調(diào)用后直接得到旋轉(zhuǎn)矩陣： R=rotx(pi/2)R = 1.0000

5、 0 0 0 0.0000 -1.0000 0 1.0000 0.0000說明：在正交旋轉(zhuǎn)矩陣中，從左到右的列向量代表了旋轉(zhuǎn)后的各個軸相對于當(dāng)前坐標(biāo)系的方向。上面的例子中，新的坐標(biāo)系的x軸仍在以前x軸方向（1,0,0），而新的y軸在以前z軸方向，即（0,0,1），而新的z軸在以前y軸反方向（0，-1,0）。X軸沒有變的原因在于該旋轉(zhuǎn)是繞x軸發(fā)生的。行向量相反，它們代表了當(dāng)前坐標(biāo)系的各個軸在新坐標(biāo)系中的方向。為了說明旋轉(zhuǎn)的復(fù)合，我們接著上面的結(jié)果再繼續(xù)繞著y軸旋轉(zhuǎn)90度： R=rotx(pi/2)*roty(pi/2)R = 0.0000 0 1.0000 1.0000 0.0000 -0.00

6、00 -0.0000 1.0000 0.0000用trplot（R）命令作出兩次旋轉(zhuǎn)的圖形，左邊是繞著x軸旋轉(zhuǎn)90度的圖形，右邊是在此基礎(chǔ)上繼續(xù)繞著y軸旋轉(zhuǎn)90度的圖形注意：旋轉(zhuǎn)的次序很重要，即使同樣的旋轉(zhuǎn)，如果次序不對得到的結(jié)果就完全不一樣。我們之前講歐拉旋轉(zhuǎn)定理時通過三維旋轉(zhuǎn)直觀地看到了這種差別。而現(xiàn)在我們通過矩陣變換重新演示上面的例子即如果改成先繞y軸旋轉(zhuǎn)再繞x軸旋轉(zhuǎn)將會得到如下結(jié)果： R=roty(pi/2)*rotx(pi/2)R = 0.0000 1.0000 0.0000 0 0.0000 -1.0000 -1.0000 0.0000 0.0000對比先繞x再繞y的旋轉(zhuǎn)順序所得到

7、的結(jié)果不一樣，從矩陣變換的角度也說明了空間中旋轉(zhuǎn)順序的不可交換性。說明：正交矩陣有9個元素，但它們不是獨立的。每一列都是單位長度，這提供了3個約束。列向量之間相互正交，又提供了另外3個約束。9個元素加上6個約束，實際上只有3個獨立的值。因此，回憶一下歐拉旋轉(zhuǎn)定理，它指出任何旋轉(zhuǎn)都可以用不超過3次繞坐標(biāo)軸的旋轉(zhuǎn)表示。這意味著我們可以將任意地旋轉(zhuǎn)分解為一組繞三個坐標(biāo)軸轉(zhuǎn)動的角度，我們將在下面討論。三角度數(shù)表示法：旋轉(zhuǎn)定理要求繞3個軸依次旋轉(zhuǎn)，但不能繞同一軸線連續(xù)旋轉(zhuǎn)兩次。旋轉(zhuǎn)順序分為兩種：歐拉式和卡爾丹式。歐拉式：繞一個特定的軸重復(fù)旋轉(zhuǎn)，但不是連續(xù)的：XYX XZY YXY YZY ZXZ或ZYZ

8、?？柕な降奶攸c是繞3個不同的軸旋轉(zhuǎn)：XYZ XZY YZX YXZ ZXY或ZYX。這些序列統(tǒng)稱為歐拉角，共有12種形式。ZYZ序列的歐拉角表示為：歐拉角是一個三維向量，表示為：如果想計算T=(0.1,0.2,0.3)的等價旋轉(zhuǎn)矩陣，可以在MATLAB中用函數(shù)這樣實現(xiàn)： R=rotz(0.1)*roty(0.2)*rotz(0.3)R = 0.9021 -0.3836 0.1977 0.3875 0.9216 0.0198 -0.1898 0.0587 0.9801或者用eul2r函數(shù)寫成更簡單的形式： R=eul2r(0.1,0.2,0.3)R = 0.9021 -0.3836 0.197

9、7 0.3875 0.9216 0.0198 -0.1898 0.0587 0.9801如果求上面的問題的逆命題，即求出給定旋轉(zhuǎn)矩陣的歐拉角，可以用tr2eul函數(shù)： gamma=tr2eul(R)gamma = 0.1000 0.2000 0.3000如果為負(fù)值，比如下面的情況： R=eul2r(0.1,-0.2,0.3)R = 0.9021 -0.3836 -0.1977 0.3875 0.9216 -0.0198 0.1898 -0.0587 0.9801反函數(shù)為： tr2eul(R)ans = -3.0416 0.2000 -2.8416我們發(fā)現(xiàn)其結(jié)果跟之前的旋轉(zhuǎn)角度完全不一樣，但這組

10、歐拉角對應(yīng)的旋轉(zhuǎn)矩陣仍與前一組是相同的。其實兩組不同的歐拉角對應(yīng)同一個旋轉(zhuǎn)矩陣，說明從旋轉(zhuǎn)矩陣到歐拉角的映射不唯一，而工具箱的函數(shù)返回值始終為正。對于=0的情況 R=eul2r(0.1,0,0.3)R = 0.9211 -0.3894 0 0.3894 0.9211 0 0 0 1.0000反函數(shù)返回的角度值為： tr2eul(R)ans = 0 0 0.4000這個結(jié)果跟原來的值完全不同。實際上，從我們得到這種情況下的旋轉(zhuǎn)矩陣：最終化簡的結(jié)果只是一個+的函數(shù)，對于逆運算而言只能確定這個和的值。要得到其中的每個值，只能按慣例取=0。另一種廣泛使用的旋轉(zhuǎn)角順序是橫滾俯仰偏航角，即：這個表示方法用

11、于描述船舶、飛機和車輛姿態(tài)時非常直觀。這個表示方法指分別繞x,y,z軸的旋轉(zhuǎn)。這個xyz角序列也是專業(yè)上的卡爾丹角，也稱泰特布萊恩角或?qū)Ш浇恰τ诤娇蘸偷孛孳囕v而言，通常定義x軸為前進(jìn)的方向、z軸垂直向下、y軸指向右手方向。奇異點及萬向節(jié)鎖之前討論的三旋轉(zhuǎn)角度表示方法中，一個根本的問題在于奇異點。當(dāng)中間的旋轉(zhuǎn)軸平行于第一個或第三個旋轉(zhuǎn)軸時這種情況就會發(fā)生，對于萬向節(jié)鎖同樣存在這個問題。圖中是用于導(dǎo)航的陀螺儀，其核心是三個互相正交的框架，它們能使安裝在其中的穩(wěn)定體相對宇宙靜止。因為無論飛船怎樣飛行，陀螺儀內(nèi)部的穩(wěn)定平臺都不會受到額外的力矩。而通過測量這些萬向框架的軸相對于穩(wěn)定平臺的轉(zhuǎn)動角度就可以

12、確定飛船的航行姿態(tài)。如果陀螺儀中間的內(nèi)萬向框架與外萬向框架的軸對齊，即它們的旋轉(zhuǎn)軸平行，此時陀螺儀只有兩個有效的旋轉(zhuǎn)軸，而不是原來的三個，我們稱之為丟失了一個自由度。雙向量表示法對于關(guān)節(jié)臂式機器人，按照圖式的方法，我們定義一個坐標(biāo)系。機器人兩指之間為姿態(tài)向量，往外的a向量為接近向量，第三個向量可由a向量與o向量正交得到。使用工具箱函數(shù)，我們可以通過a向量和o向量計算出n向量。例如執(zhí)行以下運算： a=1 0 0; o=0 1 0; oa2r(o,a)ans = 0 0 1 0 1 0 -1 0 0繞任意向量旋轉(zhuǎn)對于空間中兩個任意姿態(tài)的坐標(biāo)系，總可以在空間中找到某個軸，使其中一個坐標(biāo)系繞該軸旋轉(zhuǎn)一

13、個角度就能與另一個坐標(biāo)系姿態(tài)重合。為了找到繞著旋轉(zhuǎn)的軸，那就是我們要旋轉(zhuǎn)的軸必須與旋轉(zhuǎn)保持不變。任何在旋轉(zhuǎn)軸上的點都是旋轉(zhuǎn)的。這意味著旋轉(zhuǎn)軸必須是矩陣r的特征向量，旋轉(zhuǎn)矩陣有三個特征向量。而正交矩陣總有一個實特征值1，此時Rv=v化簡為Rv=v，這意味著此時特征向量v不隨旋轉(zhuǎn)發(fā)生改變，而旋轉(zhuǎn)是以這個向量為軸發(fā)生的。反過來，使用羅德里格斯旋轉(zhuǎn)方程，我們可以從角度和向量計算出相應(yīng)的旋轉(zhuǎn)矩陣：運用工具箱函數(shù)angvec2r可以計算相關(guān)的旋轉(zhuǎn)矩陣： R=angvec2r(pi/2,1 0 0)R = 1.0000 0 0 0 0.0000 -1.0000 0 1.0000 0.0000單位四元數(shù)四元數(shù)

14、是復(fù)數(shù)的一種擴(kuò)展，也稱為超復(fù)數(shù)，記做一個標(biāo)量加上一個向量。四元數(shù)乘法不可交換，這種不可交換性正好符合坐標(biāo)系旋轉(zhuǎn)的情況。我們可以使用工具箱函數(shù)Quaternion的類來實現(xiàn)四元數(shù)。構(gòu)造函數(shù)將傳遞的參數(shù)轉(zhuǎn)換成四元數(shù)： q=Quaternion(rpy2tr(0.1,0.2,0.3)q = 0.98186 我們用單位四元數(shù)來描述坐標(biāo)系的旋轉(zhuǎn)，它可以看成是繞單位向量n旋轉(zhuǎn)了角。該旋轉(zhuǎn)與四元數(shù)的關(guān)系為：單位四元數(shù)的性質(zhì)如下：我們可以使用工具箱函數(shù)來表示相關(guān)性質(zhì)：單位四元數(shù)： q.normans = 1.0000求一個四元數(shù)的共軛： q.inv()ans = 0.98186 求一個四元數(shù)乘以它的共軛： q

15、*q.inv()ans = 1 結(jié)果是一個單位四元數(shù)，表示一個無效旋轉(zhuǎn)關(guān)于四元數(shù)的相關(guān)性質(zhì)可在MATLAB中通過help Quaternion命令查詢相關(guān)文檔平移與旋轉(zhuǎn)組合三維空間相對位姿表示，即兩個坐標(biāo)系之間位置和姿態(tài)的變化，我們曾經(jīng)討論了幾種不同的姿態(tài)表示法，現(xiàn)在我們需要將它們與平移變換相結(jié)合，創(chuàng)造出完整的相對位姿的表示方法。這其中包含2個分量，一個平動分量和一個轉(zhuǎn)動分量。我們可以把平移分量表示為矢量，然后用一組3個歐拉角表示轉(zhuǎn)動分量?；蛘?，我們可以用一個矢量加上一組3個滾轉(zhuǎn)、俯仰、偏航角來表示它?；蛘呶覀兛梢园阉硎緸橐粋€向量加上四元數(shù)。第四個選項是將它表示為一個齊次變換。這其中有兩種最

16、實用的表示方法：四元數(shù)向量對和44齊次變換矩陣。由于四元數(shù)的相關(guān)性質(zhì)上面已經(jīng)討論過，我們接下來著重討論44齊次變換矩陣齊次變換矩陣表示旋轉(zhuǎn)和轉(zhuǎn)換之前已經(jīng)討論過二維空間的變換，三維只需要在此基礎(chǔ)上增加Z軸并對矩陣進(jìn)行擴(kuò)展：化成向量形式：具體的變換如右圖所示：齊次變換矩陣的相關(guān)性質(zhì)如下：44齊次變換在機器人學(xué)和計算機視覺中都非常有用。我們可以在MATLAB中通過以下函數(shù)實現(xiàn)變換的合成： T=transl(1,0,0)*trotx(pi/2)*transl(0,1,0)T = 1.0000 0 0 1.0000 0 0.0000 -1.0000 0.0000 0 1.0000 0.0000 1.0000 0 0 0 1.0000函數(shù)transl創(chuàng)建了一個有平移但無旋轉(zhuǎn)的相對姿態(tài)，而函數(shù)trotx則返回一個繞x軸旋轉(zhuǎn)/2的44齊次變換矩陣：旋轉(zhuǎn)部分與rotx（pi/2）相同