旋轉矩陣、變換矩陣、旋轉向量、歐拉角、四元數

視覺SLAM十四講(三)——三維空間剛體運動(上)

• 三維空間剛體運動的描述方法有：旋轉矩陣、變換矩陣、旋轉向量、歐拉角和四元數，接下來將逐一介紹它們

一、旋轉矩陣

1. 點、向量、座標系
   * 點——存在於三維空間之中，點和點組成向量，點本身由原點指向它的向量所描述
   * 向量——帶指向性的箭頭，可以進行加法減法等運算，定義座標系後，向量可以由 R3 $R^3$當中的三個數表示， 如何理解這句話呢。如下圖所示：
   
   在代數中，我們用一組基底和向量 a $a$ 在每個座標軸上的投影來表示一個向量，對於 a $a$，通過某種線性組合，可以表示爲 a=axe1+aye2+aze3 $a=a_x{e}_1+a_y{e}_2+a_z{e}_3$
   而上面那句話的意思是在矩陣運算中， a $a$ 可以表示爲 ⎡⎣⎢axayaz⎤⎦⎥ $\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]$，因爲 (e1,e2,e3)⎡⎣⎢axayaz⎤⎦⎥=axe1+aye2+aze3 $(e_1,e_2,e_3)\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=a_x{e}_1+a_y{e}_2+a_z{e}_3$
   * 座標系——三個正交的軸，構成線性空間的一組基，分爲左手系和右手系
   * 向量的運算可以由座標運算來表達：加減法，內積，外積
2. 問題的出現——一個最簡單的情況，機器人從某一點直線運動到另一點，假設機器人是質點，並且和目標點處於同一平面上，分別以機器人和目標點建立座標系，在移動過程中機器人的座標系位置一直在變，要計算與目標點的距離，就需要描述座標系之間如何變化
   * 進而——如何計算同一個向量在不同座標系裏的座標
   * 如果剛纔的機器人不是直線運動，而會有拐彎，這時座標系就會旋轉，因此描述整個運動過程就是三個軸的旋轉和原點間的平移，這就是所謂的歐式變換，保證同一個向量在各個座標系下的長度和夾角都不會發生變化，通過旋轉和平移兩部分組成

3. 問題解決
   * 平移是一個向量
   * 旋轉

   • 設某座標系(用三個方向上的單位向量表示) (e1,e2,e3) $(e_1,e_2,e_3)$ 發生了一次旋轉，變成了 (e′1,e′2,e′3) $(e_1^{{}^{\prime }},e_2^{{}^{\prime }},e_3^{{}^{\prime }})$
   • 對於某個固定的向量 a $a$(向量不隨座標系旋轉)，它的座標怎麼變化，其中 ⎡⎣⎢a1a2a3⎤⎦⎥ $\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]$ 是 a $a$ 在第一個座標系中的座標， ⎡⎣⎢⎢a′1a′2a′3⎤⎦⎥⎥ $\left[\begin{array}{c}{a}_1^{{}^{\prime }}\\ a_2^{{}^{\prime }}\\ a_3^{{}^{\prime }}\end{array}\right]$ 是 a $a$ 在另一個座標系中的座標，如圖，P爲向量 a $a$
     
   • 座標關係 [e1,e2,e3]⎡⎣⎢a1a2a3⎤⎦⎥=[e′1,e′2,e′3]⎡⎣⎢⎢a′1a′2a′3⎤⎦⎥⎥  $[e_1,e_2,e_3]\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=[e_1^{{}^{\prime }},e_2^{{}^{\prime }},e_3^{{}^{\prime }}]\left[\begin{array}{c}{a}_1^{{}^{\prime }}\\ a_2^{{}^{\prime }}\\ a_3^{{}^{\prime }}\end{array}\right]$，乘出來的就是向量本身
   • 左乘 ⎡⎣⎢⎢eT1eT2eT3⎤⎦⎥⎥ $\left[\begin{array}{c}{e}_1^T\\ e_2^T\\ e_3^T\end{array}\right]$,得： ⎡⎣⎢a1a2a3⎤⎦⎥=⎡⎣⎢⎢eT1e′1eT2e′1eT3e′1eT1e′2eT2e′2eT3e′2eT1e′3eT2e′3eT3e′3⎤⎦⎥⎥⎡⎣⎢⎢a′1a′2a′3⎤⎦⎥⎥Δ=Ra′ $\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{ccc}{e}_1^T{e}_1^{{}^{\prime }}& e_1^T{e}_2^{{}^{\prime }}& e_1^T{e}_3^{{}^{\prime }}\\ e_2^T{e}_1^{{}^{\prime }}& e_2^T{e}_2^{{}^{\prime }}& e_2^T{e}_3^{{}^{\prime }}\\ e_3^T{e}_1^{{}^{\prime }}& e_3^T{e}_2^{{}^{\prime }}& e_3^T{e}_3^{{}^{\prime }}\end{array}\right]\left[\begin{array}{c}{a}_1^{{}^{\prime }}\\ a_2^{{}^{\prime }}\\ a_3^{{}^{\prime }}\end{array}\right]\begin{array}{c}\mathrm{\Delta }\\ =\end{array}R{a}^{{}^{\prime }}$
   • 中間的矩陣 R $R$ 稱爲旋轉矩陣
   • 根據定義可以驗證
     * R $R$ 是一個正交矩陣(矩陣的逆是其轉置)
     * R $R$ 的行列式爲 +1 $+1$
   • 滿足這兩個性質的矩陣稱爲旋轉矩陣

   *旋轉矩陣描述了兩個座標的變換關係，ex： a1=R12a2 $a_1=R_{12}{a}_2$，反之 a2=R21a1 $a_2=R_{21}{a}_1$，於是 R21=R−112=RT12 $R_{21}=R_{12}^{-1}=R_{12}^T$， 進一步，三個座標系亦有 a3=R32a2=R32R21a1=R31a1 $a_3=R_{32}{a}_2=R_{32}{R}_{21}{a}_1=R_{31}{a}_1$
   * 加上平移 a′a3=R32a2=R32R21a1=R31a1 $a_3=R_{32}{a}_2=R_{32}{R}_{21}{a}_1=R_{31}{a}_1$
   * 加上平移 a′=Ra+t