三維空間剛體運動2：旋轉向量表示旋轉

本篇繼續參照高翔老師《視覺SLAM十四講從理論到實踐》，講解三維空間剛體運動。博文將原第三講分爲四部分來講解：1、旋轉矩陣和變換矩陣；2、旋轉向量表示旋轉；3、歐拉角表示旋轉；4、四元數表示變換。本文相對於原文會適當精簡，同時爲便於理解，會加入一些註解和補充知識點，本篇爲第二部分：旋轉向量表示旋轉，另外三部分請參照博主的其他博文。

1.定義

對於矩陣表示方式至少有以下兩個缺點：
1.$SO(3)$的旋轉矩陣有9個量，但一次旋轉只有3個自由度，因此這種表達方式是冗餘的，同理$SE(4)$也是。那麼，是否有更緊湊的表示呢？
2.旋轉矩陣和變換矩陣自身帶有約束：它必須是正交矩陣且行列式爲。當想估計或優化一個旋轉矩陣或變換矩陣時，這些約束會使得求解變的更困難。
因此，希望有一種方式能夠緊湊的描述旋轉和平移。

旋轉向量：事實上，任意旋轉都可以用一個旋轉軸和一個旋轉角來刻畫，於是，我們可以使用一個向量，其方向$n$與旋轉軸一致，其長度等於旋轉角$\Theta$，那麼向量$\Theta n$也可以描述這個旋轉，這種向量稱爲旋轉向量（或軸角/角軸，Axis-Angle），只需一個三維向量即可描述旋轉。同樣，對於變換矩陣，使用一個旋轉向量和一個平移向量即可表達一次變換，此時變量維數正好是六維。

2.羅德里格斯公式

2.1 定義

羅德里格斯公式：旋轉向量和旋轉矩陣有什麼聯繫嗎？事實上，從旋轉向量到旋轉矩陣的轉換過程由羅德里格斯公式（Rodrigues’s Formula）表示。因爲任意旋轉都可以由一個旋轉軸$n$和一個旋轉角$\Theta$刻畫，故羅德里格斯公式具體形式如下：$R= \cos \theta I+(1-\cos \theta )nn^{T}+\sin \theta n^{\wedge }\tag{1.1}$
符號$^{\wedge }$是向量到反對稱矩陣的轉換符，見第一篇博客的公式（1.4）。

2.2 推導

首先，理解圖1。定義向量$k$是旋轉軸的單位矢量，爲表示方便，使用$k$代表$n$。圖中初始向量$v$以$k$爲軸旋轉角$\theta$得到$v_{rot}$。向量$w$爲$k\times v$的方向上的單位向量，向量$v_{\perp }$和$v_{rot\perp }$分別是$v$和$v_{rot}$的垂直於平面的向量。向量$a$和$b$分別是$v_{rot}$在$w$和$v_{\perp }$方向上的分量。
圖1.1：旋轉向量3D圖（推薦數學繪圖軟件：geogebra）

所謂推導旋轉方程，實則求出一個旋轉矩陣，使得$V_{rot}=Rv$，所以我們要做的就是找出$v_{rot}$與$v$，並用矩陣來表示。
此公式有2種形式，故而也有2種推導方法，兩者推導方法的不同主要在$v_{\perp }$的表示上。具體推導過程如下。

2.2.1 推導一

推導一使用單位向量$k$表示，推導過程如下：

1. 對$v$進行向量分解：$v = v_{\perp } + v_{||}$

2. 根據向量減法可得：$v_{\perp } = v - v_{||}$

3. 由旋轉過程平行向量不變得：$v_{rot||} = v_{||}$

4. $v_{||}$其實就是$v$在$u$上的正交投影（Orthogonal Projection），根據正交投影公式：$\begin{aligned} v_{||} &= proj_{k}(v) &= \frac{k\cdot v}{k\cdot k}k &= \frac{k\cdot v}{||k||^{2}}k &= (k\cdot v)k \end{aligned}$
   其中$u\cdot k=||k||^{2}, ||k||=1,(v\cdot k)$爲標量，所以再乘向量$k$得到一個矢量

5. 爲計算方便，對$v_{rot\perp}$ 進行向量分解：$v_{rot\perp} =a+b$

6. 由圖中的向量關係經推導可得：$a=\sin \theta k\times v,b=\cos \theta v_{\perp }$，$a,b$的推導在後邊

7. 綜上可得：
   $v_{rot}=v_{rot\perp}+v_{rot||}$
           $=a+b+v_{||}$
           $=\sin \theta k\times v +\cos \theta v_{\perp } + (v\cdot k)k$
           $=\sin \theta k\times v +\cos \theta (v - v_{||}) + (v\cdot k)k$
           $=\sin \theta k\times v +\cos \theta (v - (v\cdot k)k ) + (v\cdot k)k$
           $=\cos \theta v+(1-\cos \theta )(v\cdot k)k + \sin \theta k\times v$
   顯然：到此步，我們還無法將其用矩陣來表示，所以需要對$(v\cdot k)k$ 和 $k\times v$ 進行矩陣轉換，由點積的交換律和結合律得：$(v\cdot k)k=k\cdot(v\cdot k)=k\cdot (k^{T} v)=k\cdot k^{T} v$其中的向量都是列向量，點積展開規則爲：$x\cdot y = [x, y] =x^{T}y$。
   對於$k\times v$可用叉乘矩陣來化簡爲$Kv$：$\begin{bmatrix} (k\times v)_{x}\\ (k\times v)_{y}\\ (k\times v)_{z} \end{bmatrix}= \begin{bmatrix} k_{y}v_{z}- k_{z}v_{y}\\ k_{z}v_{x}- k_{x}v_{z}\\ k_{x}v_{y}- k_{y}v_{x} \end{bmatrix}= \begin{bmatrix} 0 & -k_{z} & k_{y}\\ k_{z} & 0 & -k_{x}\\ k_{y} & k_{x} & 0 \end{bmatrix}\begin{bmatrix} v_{x}\\ v_{y}\\ v_{z} \end{bmatrix}= Kv$
   故$v_{rot}=\cos \theta v+(1-\cos \theta )(v\cdot k)k + \sin \theta k\times v$
              $=\cos \theta v+(1-\cos \theta )kk^{T}v + \sin \theta K v$
              $=(\cos \theta I +(1-\cos \theta )kk^{T} + \sin \theta K )v$
   故旋轉矩陣$R= \cos \theta I +(1-\cos \theta )kk^{T} + \sin \theta K. \tag{1.2}$其中$I$爲單位矩陣。

2.2.2 推導二

與推導一相比，推導二的不同主要在於用叉乘去表示一些數據。用叉乘來表示v⊥可得：$v_{\perp } =-k\times (k\times v)$。
所以聯立推導一中各式得：
$v_{rot}=v_{rot\perp}+v_{rot||}$
        $=a+b+v_{||}$
        $=\sin \theta k\times v +\cos \theta v_{\perp } + v-v_{\perp }$
        $=\sin \theta k\times v -\cos \theta k\times (k\times v) + v + k\times (k\times v)$
        $=v + (1 - \cos \theta )k\times (k\times v) + \sin \theta k\times v$
        $=v + (1 - \cos \theta )K^{2}v+ \sin \theta Kv$(叉乘矩陣表示)
        $=(I + (1 - \cos \theta )K^{2}+ \sin \theta K)v$
        $=Rv$
從而得出第二種表達式$R= I +(1-\cos \theta )K^{2} + \sin \theta K. \tag{1.3}$顯然，第二種表達式更爲簡便，在計算的過程中涉及的參數更少，所以這也是在進行旋轉操作時常用的公式。

2.2.3 推導$a$

向量$a$和$b$是由$v_{rot\perp }$正交分解得到的矢量，既有大小又有方向，所以在求解時，我們要對其大小和方向分別求解。
大小
設$\theta_{1}= \pi -\theta$，$\theta_{2}$是向量$v$和$k$的夾角，$k$爲單位向量，則對於$a$的大小$|a|$有：
$|a|=\sin \theta_{1}|v_{rot\perp }|=\sin \theta_{1}|v_{\perp }|$
      $=\sin (\pi -\theta)|v_{\perp }|=\sin \theta|v_{\perp }|$
      $=\sin \theta \sin \theta_{2}|v|$
      $=\sin \theta \sin \theta_{2}|v||k|$
      $=\sin \theta |k \times v|$
由公式$|a\times b| = \sin \theta |a||b|$知$\sin \theta_{2}|v||k|=|k\times v|$，所以：
$|a|=\sin \theta |k \times v|$

方向
由叉乘方向可得$a$的單位方向向量爲：$k\times v /|k \times v|$，綜上可得：
$a=(k\times v /|k \times v|)|a|$
    $=(k\times v /|k \times v|)\sin \theta |k \times v|$
    $=\sin \theta k\times v$

2.2.4 推導$b$

大小
由圖得，$\theta_{1}$爲$b$和$v_{rot\perp }$的夾角，則：
$|b|=\cos \theta_{1}|v_{rot\perp }|=\cos (\pi -\theta)|v_{\perp }|= -\cos \theta|v_{\perp }|$

方向
由於$b$的方向與$v_{\perp }$方向相反，可得$b$的單位方向向量爲：$-v_{\perp }/|v_{\perp }|$，綜上可得：
$b=(-v_{\perp }/|v_{\perp }|)|b|$
    $=\cos \theta v_{\perp }$

至此，羅德里格斯公式的證明全部結束。此外，如果讀者希望獲得關於Oxyz座標系的旋轉變換關係，可以參考這篇博客：圖形變換之旋轉變換公式推導。
羅德里格斯公式反應的是旋轉向量到旋轉矩陣的轉換關係，如果已知旋轉矩陣$R$，如果推導旋轉向量$v$呢？下邊給出旋轉矩陣到旋轉向量的反向轉換關係。

3.旋轉矩陣到向量

這裏計算從一個旋轉矩陣到旋轉向量的轉換。對於旋轉角$\theta$，取旋轉矩陣$R$兩邊的跡，有：$\begin{aligned} tr(R) &= \cos \theta tr(I)+(1-\cos \theta )tr(nn^{T})+\sin \theta tr(n^{\wedge })\\ &=3\cos \theta + (1- \cos \theta) \\ &=1+2\cos \theta. \tag{1.4} \end{aligned}$因此：$\theta = \arccos \frac{tr(R)-1}{2} .\tag{1.5}$關於轉軸$n$，旋轉軸上的向量在旋轉後不發生改變，說明：$Rn=n.\tag{1.6}$因此，轉軸$n$是矩陣$R$特徵值1對應的特徵向量。求解此方程，再歸一化，就得到了旋轉軸$n$。由$v=\theta n$得到旋轉向量$v$。

至此，推導結束。實踐部分代碼放到第三部分一起演示。

參考：
1.《視覺SLAM十四講：從理論到實踐》，高翔、張濤等著，中國工信出版社
2. 羅德里格斯公式推導