三維空間位姿描述課件

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

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

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

4、它就是屬于SO（3）的特殊正交群，旋轉(zhuǎn)矩陣的相關(guān)性質(zhì)如下：根據(jù)上面的性質(zhì)結(jié)合二維坐標(biāo)中的情況，我們可以得到三維坐標(biāo)旋轉(zhuǎn)變換矩陣的表達(dá)式如下：可以看出：這些旋轉(zhuǎn)矩陣每一列都是單位長(zhǎng)度的向量，每一列都與其他一列正交。對(duì)于繞z軸的旋轉(zhuǎn)，情況特殊，因?yàn)槲覀兛梢灾苯訉⑵淇闯啥S空間繞著垂直于紙面向外（也就是垂直屏幕向外）的旋轉(zhuǎn)得到，所以二維旋轉(zhuǎn)和繞Z軸的三維旋轉(zhuǎn)是等價(jià)的，即上面坐標(biāo)變換中關(guān)于z軸的旋轉(zhuǎn)與二維空間旋轉(zhuǎn)變換R是等價(jià)的。MATLAB工具箱也提供了一些函數(shù)用來(lái)計(jì)算這些基本旋轉(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說(shuō)明：在正交旋轉(zhuǎn)矩陣中，從左到右的列向量代表了旋轉(zhuǎn)后的各個(gè)軸相對(duì)于當(dāng)前坐標(biāo)系的方向。上面的例子中，新的坐標(biāo)系的x軸仍在以前x軸方向（1,0,0），而新的y軸在以前z軸方向，即（0,0,1），而新的z軸在以前y軸反方向（0，-1,0）。X軸沒(méi)有變的原因在于該旋轉(zhuǎn)是繞x軸發(fā)生的。行向量相反，它們代表了當(dāng)前坐標(biāo)系的各個(gè)軸在新坐標(biāo)系中的方向。為了說(shuō)明旋轉(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)，如果次序不對(duì)得到的結(jié)果就完全不一樣。我們之前講歐拉旋轉(zhuǎn)定理時(shí)通過(guò)三維旋轉(zhuǎn)直觀地看到了這種差別。而現(xiàn)在我們通過(guò)矩陣變換重新演示上面的例子即如果改成先繞y軸旋轉(zhuǎn)再繞x軸旋轉(zhuǎn)將會(huì)得到如下結(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對(duì)比先繞x再繞y的旋轉(zhuǎn)順序所得到

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

8、?？柕な降奶攸c(diǎn)是繞3個(gè)不同的軸旋轉(zhuǎn)：XYZ XZY YZX YXZ ZXY或ZYX。這些序列統(tǒng)稱(chēng)為歐拉角，共有12種形式。ZYZ序列的歐拉角表示為：歐拉角是一個(gè)三維向量，表示為：如果想計(jì)算T=(0.1,0.2,0.3)的等價(jià)旋轉(zhuǎn)矩陣，可以在MATLAB中用函數(shù)這樣實(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ù)寫(xiě)成更簡(jiǎn)單的形式： 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如果求上面的問(wèn)題的逆命題，即求出給定旋轉(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、歐拉角對(duì)應(yīng)的旋轉(zhuǎn)矩陣仍與前一組是相同的。其實(shí)兩組不同的歐拉角對(duì)應(yīng)同一個(gè)旋轉(zhuǎn)矩陣，說(shuō)明從旋轉(zhuǎn)矩陣到歐拉角的映射不唯一，而工具箱的函數(shù)返回值始終為正。對(duì)于=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這個(gè)結(jié)果跟原來(lái)的值完全不同。實(shí)際上，從我們得到這種情況下的旋轉(zhuǎn)矩陣：最終化簡(jiǎn)的結(jié)果只是一個(gè)+的函數(shù)，對(duì)于逆運(yùn)算而言只能確定這個(gè)和的值。要得到其中的每個(gè)值，只能按慣例取=0。另一種廣泛使用的旋轉(zhuǎn)角順序是橫滾俯仰偏航角，即：這個(gè)表示方法用

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

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

13、個(gè)角度就能與另一個(gè)坐標(biāo)系姿態(tài)重合。為了找到繞著旋轉(zhuǎn)的軸，那就是我們要旋轉(zhuǎn)的軸必須與旋轉(zhuǎn)保持不變。任何在旋轉(zhuǎn)軸上的點(diǎn)都是旋轉(zhuǎn)的。這意味著旋轉(zhuǎn)軸必須是矩陣r的特征向量，旋轉(zhuǎn)矩陣有三個(gè)特征向量。而正交矩陣總有一個(gè)實(shí)特征值1，此時(shí)Rv=v化簡(jiǎn)為Rv=v，這意味著此時(shí)特征向量v不隨旋轉(zhuǎn)發(fā)生改變，而旋轉(zhuǎn)是以這個(gè)向量為軸發(fā)生的。反過(guò)來(lái)，使用羅德里格斯旋轉(zhuǎn)方程，我們可以從角度和向量計(jì)算出相應(yīng)的旋轉(zhuǎn)矩陣：運(yùn)用工具箱函數(shù)angvec2r可以計(jì)算相關(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ò)展，也稱(chēng)為超復(fù)數(shù)，記做一個(gè)標(biāo)量加上一個(gè)向量。四元數(shù)乘法不可交換，這種不可交換性正好符合坐標(biāo)系旋轉(zhuǎn)的情況。我們可以使用工具箱函數(shù)Quaternion的類(lèi)來(lái)實(shí)現(xiàn)四元數(shù)。構(gòu)造函數(shù)將傳遞的參數(shù)轉(zhuǎn)換成四元數(shù)： q=Quaternion(rpy2tr(0.1,0.2,0.3)q = 0.98186 我們用單位四元數(shù)來(lái)描述坐標(biāo)系的旋轉(zhuǎn)，它可以看成是繞單位向量n旋轉(zhuǎn)了角。該旋轉(zhuǎn)與四元數(shù)的關(guān)系為：?jiǎn)挝凰脑獢?shù)的性質(zhì)如下：我們可以使用工具箱函數(shù)來(lái)表示相關(guān)性質(zhì)：?jiǎn)挝凰脑獢?shù)： q.normans = 1.0000求一個(gè)四元數(shù)的共軛： q.inv()ans = 0.98186 求一個(gè)四元數(shù)乘以它的共軛： q

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

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