微分幾何在機器人領(lǐng)域的應用（二）深入理解三維空間變換

空間幾何變換

空間中的幾何變換分為多類(lèi)，從簡(jiǎn)單，到逐漸復雜的變換，分別有如下幾種：

1.   等距變換（Isometries）。等距變換下點(diǎn)到點(diǎn)的歐式距離保持不變。剛體變換是典型的等距變換。

2.   相似變換（Similarity）。在等距變換的基礎上加上一個(gè)各向同性的縮放。矩陣表示上需要在旋轉矩陣部分乘以一個(gè)非零系數s。

3.   仿射變換（Affine）。是一個(gè)非奇異的線(xiàn)性變換加上一個(gè)平移向量組成的變換。

4.   投影變換（Projective）。任意非奇異的4×4矩陣所構成的變換。

變換的分類(lèi)和特征如下圖所示。

三維剛體的空間變換屬于種情況。如果物體不變形，那么剛體變換涵蓋物理世界中的所有情況。剛體變換包含三個(gè)平移自由度和三個(gè)旋轉自由度，總共6個(gè)自由度。應用剛體變換，點(diǎn)到點(diǎn)的距離保持不變，同時(shí)矢量的點(diǎn)積和叉積保持不變。平移自由度易于理解，故本文重點(diǎn)討論旋轉分量，即旋轉矩陣R。

旋轉矩陣

在理解高維理論時(shí)，我們一般采用降維的方式理解，由易到難。首先回到二維空間的變換。二維平面中，剛體變換有三個(gè)自由度，x, y 和旋轉角θ。用矩陣的形式表示：

其中

分別為旋轉矩陣和平移向量?？梢钥吹叫D矩陣只有一個(gè)自由度，因其只有一個(gè)變量θ。

旋轉矩陣R的性質(zhì)：

1. 旋轉矩陣的逆矩陣是它的轉置矩陣，故旋轉矩陣是正交矩陣。（如果不理解逆矩陣和轉置矩陣，請首先惡補線(xiàn)性代數）。

2. 一個(gè)矩陣是旋轉矩陣，當且僅當它是正交矩陣，且它的行列式是1。正交矩陣的行列式是±1。讀者可思考行列式為-1的情況對應什么變換。

二維旋轉矩陣可用旋轉角唯yi表示。正角表示逆時(shí)針旋轉。

如下圖表示的是當θ=20°的情況。

二位旋轉矩陣的許多性質(zhì)在三維空間中同樣滿(mǎn)足。

讓我們回到三維空間。旋轉可以有三個(gè)旋轉組合而成。在右手（笛卡爾）坐標系下分別繞x，y， z軸旋轉。其旋轉矩陣分別對應為

任意旋轉矩陣可寫(xiě)作一定角度下的三個(gè)矩陣的乘積。

注意：矩陣乘法不符合交換律！故順序不同，得到的旋轉矩陣并不相同。

歐拉角

航空領(lǐng)域，一般定義飛機前后軸為x軸，沿x軸旋轉的角度一般稱(chēng)為Roll，中文稱(chēng)作翻滾角；兩翼方向稱(chēng)作Pitch，中文稱(chēng)作俯仰角；垂直地面的方向是航向角（Yaw），如下圖所示。作者覺(jué)得中文翻譯很符合愿意，更易于理解?？梢杂涀≡隈{駛飛機時(shí)，如何操縱翻滾角，俯仰角，航向角。Roll，Pitch，Yaw，又稱(chēng)作歐拉角。習慣上，三個(gè)歐拉角的方向是z-y-x，使用時(shí)需要特別重要，歐拉角順序錯了，旋轉矩陣也會(huì )發(fā)生變化。

程序實(shí)現：
程序使用基于C++的Eigen庫[3]。注意，Eigen庫是一個(gè)僅包含頭文件的基礎矩陣庫，沒(méi)有靜態(tài)或動(dòng)態(tài)庫。使用時(shí)僅需要把相關(guān)的目錄include就可以了。

再次注意：三個(gè)歐拉角的順序！

李群和李代數

三維旋轉矩陣是直觀(guān)的表示方法，但旋轉矩陣有9個(gè)變量，只有3個(gè)自由度，故信息是冗余的。旋轉矩陣在工程使用更好的表達方法。根據定義，所有的剛體變換屬于一個(gè)群（李qun，Lie Group）。剛體變換又稱(chēng)作特殊歐式變換（special  Euclidean  transformation），通常寫(xiě)作SE（3）。李群中的變換滿(mǎn)足如下特性。詳細性質(zhì)可參見(jiàn)李群和李代數的資料。如果只限于3D視覺(jué)或機器人學(xué)，只需記住其主要特性：

?封閉性
?相關(guān)性
?單位矩陣
?可逆

剛體變換的組合和逆變換均屬于剛體變換。
單純的旋轉變換稱(chēng)作特殊正角變換（special orthogonal transformation)，通常寫(xiě)作SO（3）。旋轉矩陣都是正交矩陣。
李代數通過(guò)指數映射，將旋轉矩陣的9個(gè)變量轉換為3個(gè)變量，結合三個(gè)平移向量，總共6個(gè)變量，對應6個(gè)自由度。李代數表示法在三維重建（SFM）、VR、SLAM等位姿估計領(lǐng)域應用的較多。李代數有基于Eigen的Sophus庫[4]可使用，方便完成指數映射。

羅德里格斯旋轉公式

（Rodriguez’s Rotation Formula）

旋轉矩陣有一個(gè)更有效的表達方法，即由一個(gè)單位向量和一個(gè)旋轉角生成。每一個(gè)旋轉矩陣均可轉化為向量和角（又稱(chēng)軸-角）的表達方式。根據公式，單位向量用表示，旋轉的角度是θ，那么相應的旋轉矩陣是：

此矩陣可簡(jiǎn)化為如下公式：

具體點(diǎn)符號定義可參見(jiàn)相關(guān)文獻。單純環(huán)繞x，y或z軸旋轉而成的旋轉矩陣是羅德里格斯公式的特殊形式。讀者可以把上式中的單位向量替換為（0,0,1）進(jìn)行驗證。雖然公式復雜，但程序實(shí)踐比較方便。利用Eigen庫中的Eigen::AngleAxisf（旋轉向量）可以直接獲得。

四元數(Quternions)

四元素可看作一種特殊的復數，由一個(gè)實(shí)部和三個(gè)虛部構成。四元素的表示方法同旋轉矩陣、歐拉角表示方法是等價(jià)的。根據羅德里格斯旋轉公式，任何一個(gè)旋轉都可以表達成軸角的表達法。四元素可以更方便的表達出旋轉軸和旋轉角。單位歐拉向量可表示為：

根據歐拉公式的擴展，四元素可表示為

四元素分為實(shí)部和虛部，實(shí)部只跟旋轉角有關(guān)。虛部有單位向量和旋轉角共同計算得來(lái)。

四元數的求逆可采用復數的共軛（即虛部取反）方式求得

同時(shí)，四元數更易于做線(xiàn)性插值（Slerp）。實(shí)際實(shí)驗中，使用四元素做旋轉矩陣的計算更加方便。使用Eigen庫時(shí)，四元素的使用更為方便。

總結

剛體的空間變換由平移和旋轉兩部分組成。平移部分易于理解，旋轉部分一般由直觀(guān)的3×3矩陣表示。

旋轉矩陣有很多特性（正交矩陣、單位矩陣），但其由9個(gè)元素，但只有3個(gè)自由度，故數學(xué)上的表示是冗余的。

在機器人領(lǐng)域，使用多的除旋轉矩陣外，還有旋轉向量、歐拉角、四元素等。

本文的幾乎所有變換都容易實(shí)現，可直接使用三方庫如Eigen[3]，類(lèi)似的還要OpenCV等。但如要深入理解，hao自己實(shí)戰。

思考：二維空間剛體變換有3個(gè)自由度，三維有6個(gè)自由度，四維空間呢？n維空間呢？

參考文獻：

1. Multiple View Geometry in Computer Vision (2nd Edition), Richard Hartley and Andrew Zisserman.

2. An Invitationto 3-D Vision From Images to Models, Yi Ma, Jana Kosecka, Stefano Soatto and Shankar Sastry.

3. Eigen, eigen.tuxfamily.org/.

本文屬于純原創(chuàng )文章，轉載請注明杭州藍芯科技有限公司