微分幾何在機器人領域的應用（二）深刻理解三維空間變換

空間幾何變換

空間中的幾何變換分爲多類，從最簡單，到逐漸複雜的變換，分別有以下幾種。

1. 等距變換（Isometries）。等距變換下點到點的歐式距離保持不變。剛體變換是典型的等距變換。
2. 類似變換（Similarity）。在等距變換的基礎上加上一個各向同性的縮放。矩陣表示上須要在旋轉矩陣部分乘以一個係數s。
3. 仿射變換（Affine）。是一個非奇異的線性變換加上一個平移向量組成的變換。
4. 投影變換（Projective）。任意非奇異的4×4矩陣所構成的變換。

變換的分類和特徵以下圖所示:

三維剛體的空間變換屬於第一種狀況。若是問題不變形，那麼剛體變換涵蓋物理世界中的全部狀況。剛體變換包含三個平移自由度和三個旋轉自由度，總共6個自由度。應用剛體變換，點到點的距離保持不變，同時矢量的點積和叉積保持不變。平移自由度易於理解，故本文重點討論旋轉份量，即旋轉矩陣R。

旋轉矩陣：

 

 

    在理解高維理論時，咱們通常採用降維的方式理解，由易到難。首先回到二維空間的變換。二維平面中，剛體變換有三個自由度，x, y 和旋轉角θ。用矩陣的形式表示：

 

其中

 

　　分別爲旋轉矩陣和平移向量。能夠看到旋轉矩陣只有一個自由度，因其只有一個變量θ。

 

旋轉矩陣R的性質：

 

1. 旋轉矩陣的逆矩陣是它的轉置矩陣，故旋轉矩陣是正交矩陣。（若是不理解逆矩陣和轉置矩陣，請首先惡補線性代數）。
2. 一個矩陣是旋轉矩陣，當且僅當它是正交矩陣，且它的行列式是1。正交矩陣的行列式是±1。讀者可思考行列式爲-1的狀況對應什麼變換。

 

二維旋轉矩陣可用旋轉角惟一表示。正角表示逆時針旋轉。

 

 

 

　　以下圖表示的是當θ=20°的狀況。

 

 

 

　　 二位旋轉矩陣的許多性質在三維空間中一樣知足。

 

讓咱們回到三維空間。旋轉能夠有三個旋轉組合而成。在右手（笛卡爾）座標系下分別繞x, y, z軸旋轉。其旋轉矩陣分別對應爲

 

 

 

 

 

 

 

 

 

 

任意旋轉矩陣可寫做必定角度下的三個矩陣的乘積。

 

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

歐拉角

 

航空領域，通常定義飛機先後軸爲x軸，沿x軸旋轉的角度通常稱爲Roll，中文稱做翻滾角；兩翼方向稱做Pitch，中文稱做俯仰角；垂直地面的方向是航向角（Yaw），以下圖所示。我的以爲中文翻譯很符合願意，更易於理解。能夠記住在駕駛飛機時，如何操縱翻滾角，俯仰角，航向角。Roll，Pitch，Yaw，又稱做歐拉角。習慣上，三個歐拉角的方向是z-y-x，使用時須要特別重要，歐拉角順序錯了，旋轉矩陣也會發生變換。

 

 

程序實現：

 

程序使用基於C++的Eigen庫[3]。注意，Eigen庫是一個僅包含頭文件的基礎矩陣庫，沒有靜態或動態庫。使用時僅須要把相關的目錄include就能夠了。

 

再次注意：三個歐拉角的順序！

 

 1 #define _USE_MATH_DEFINES
 2 #include <math.h>
 3 
 4 #include <Eigen/Core>
 5 #include <Eigen/Dense>
 6 typedef Eigen::Vector3f            Geo3d;
 7 typedef Eigen::Matrix3f            GeoMat3;
 8 typedef Eigen::Matrix4f            GeoMat4;
 9 typedef GeoMat3                    RotMat;
10 
11 GeoMat3 RotationMatrixFromEulers(float rx, float ry, float rz)
12 {
13 　　Eigen::AngleAxisf quat = Eigen::AngleAxisf(rz, Geo3d::UnitZ()) * Eigen::AngleAxisf(ry, Geo3d::UnitY()) * Eigen::AngleAxisf(rx, Geo3d::UnitX());
14　　 return quat.matrix();
15 }
16 
17 int main(int argc, char *argv[])
18 {
19     auto rot = RotationMatrixFromEulers(M_PI/12, -M_PI/3, M_PI/2);
20     std::cout << 「Rotation Matrix: 「 << rot << std::endl;
21 
22     // to euler angles. (2, 1, 0) means, rz, ry, rx
23     Geo3d euler_angles = rot. eulerAngles(2, 1, 0);
24     std::cout <<」Euler angles: 「 << euler_angles.transpose() << std::endl;
25     return 1;
26 }

 李羣和李代數

 

三維旋轉矩陣是最直觀的表示方法，但旋轉矩陣有9個變量，只有3個自由度，故信息是冗餘的。旋轉矩陣在工程使用更好的表達方法。根據定義，全部的剛體變換屬於一個羣（李羣，Lie Group）。剛體變換又稱做特殊歐式變換（special Euclidean transformation），一般寫做SE（3）。李羣中的變換知足以下特性。詳細性質可參見李羣和李代數的資料。若是隻限於3D視覺或機器人學，只需記住其主要特性：

• 封閉性

 

• 相關性

 

•  單位矩陣
• 可逆

 

剛體變換的組合和逆變換均屬於剛體變換。

 

單純的旋轉變換稱做特殊正角變換（special orthogonal transformation), 一般寫做SO（3）。旋轉矩陣都是正交矩陣。

 

李代數經過指數映射，將旋轉矩陣的9個變量轉換爲3個變量，結合三個平移向量，總共6個變量，對應6個自由度。李代數表示法在三維重建（SFM）、VR、SLAM等位姿估計領域應用的較多。李代數有基於Eigen的Sophus庫[4]可以使用，方便完成指數映射。

 

羅德里格斯旋轉公式（Rodriguez’s Rotation Formula）

 

旋轉矩陣有一個更有效的表達方法，即由一個單位向量和一個旋轉角生成。每個旋轉矩陣都可轉化爲向量和角（又稱軸-角）的表達方式。根據公式，單位向量用表示，旋轉的角度是θ，那麼相應的旋轉矩陣是

 

 

 

此矩陣可用簡化爲以下公式，

 

 

 

　　具體點符號定義可參見相關文獻。單純環繞x，y或z軸旋轉而成的旋轉矩陣是羅德里格斯公式的特殊形式。讀者能夠把上式中的單位向量替換爲（0,0,1）進行驗證。雖然公式複雜，但程序實踐比較方便。利用Eigen庫中的Eigen::AngleAxisf（旋轉向量）能夠直接得到。

 

四元數

    四元素可看做一種特殊的複數，由一個實部和三個虛部構成。四元素的表示方法同旋轉矩陣、歐拉角表示方法是等價的。根據羅德里格斯旋轉公式，任何一個旋轉均可以表達成軸角的表達法。四元素能夠更方便的表達出旋轉軸和旋轉角。單位歐拉向量可表示爲

 

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

 

 

 

 

四元素分爲實部和虛部，實部只跟旋轉角有關。虛部有單位向量和旋轉角共同計算得來。

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

同時，四元數更易於作線性插值（Slerp）。實際實驗中，使用四元素作旋轉矩陣的計算更加方便。使用Eigen庫時，四元素的使用更爲方便。

 

 

總結

 

• 　　剛體的空間變換由平移和旋轉兩部分組成。平移部分易於理解，旋轉部分通常由直觀的3×3矩陣表示。

 

•  　　  旋轉矩陣有不少特性（正交矩陣、單位矩陣），但其由9個元素，但只有3個自由度，故數學上的表示是冗餘的。

 

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

 

•  　　 本文的幾乎全部變換都容易實現，可直接使用三方庫如Eigen[3]，相似的還要OpenCV等。但如要深刻理解，最好本身實戰。

 

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

 

參考文獻：

 

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

 

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

 

3. Eigen, http://eigen.tuxfamily.org/

 

4. Sophus, https://github.com/strasdat/Sophus