三維空間剛體運動2：旋轉向量與羅德里格斯公式

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

1.定義

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

旋轉向量：事實上，任意旋轉都可以用一個旋轉軸和一個旋轉角來刻畫，於是，我們可以使用一個向量$u$（爲方便表達，上下文統一採用符號$u$表示單位向量，其他符號$n,k$等也是可以的），其方向與旋轉軸一致，其長度等於旋轉角$\theta$，那麼向量$\theta u$也可以描述這個旋轉，這種向量稱爲旋轉向量（或軸角/角軸，Axis-Angle），只需一個三維向量即可描述旋轉。

在三維空間中定義一個方向只需要用到兩個量就可以了（與任意兩個座標軸之間的夾角，比如地球的經緯度就可以確定方向），因此第三個量可以用來定義長度，表示旋轉角度。同樣，對於變換矩陣，使用一個旋轉向量和一個平移向量即可表達一次變換，此時變量維數正好是六維。

2.羅德里格斯公式

2.1 定義

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

2.2 推導

首先，理解下圖。定義$u$是旋轉軸的單位向量，$v$爲旋轉向量，$w$爲$u\times v$方向上的單位向量。圖中$v$繞$u$旋轉角度$\theta$得到$v_{rot}$。將$v$分解爲平行於旋轉軸$u$以及正交於$u$的兩個分量：$v_{∥}$和$v_{⊥}$。將$v_{rot}$分解爲平行於旋轉軸$u$以及正交於$u$的兩個分量：$v_{rot||}$和$v_{rot\perp}$。向量$a$和$b$分別是$v_{rot}$在$w$和$v_{\perp }$方向上的分量。
圖2.1：旋轉向量3D圖（數學繪圖軟件推薦geogebra）

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

2.2.1 推導一

推導一推導過程如下：

1. $v$：對$v$進行向量分解：$v = v_{\perp } + v_{||}$，根據向量減法可得：$v_{\perp } = v - v_{||}\tag{2.3}$對$v_{rot||}$進行向量分解$v_{rot}=v_{rot||}+v_{rot\perp } \tag{2.4}$下邊分別推導$v_{rot||}$和$v_{rot\perp}$。
2. $v_{rot||}$：由於旋轉過程平行向量的不變性可得$v_{rot||} = v_{||}$，$v_{||}$其實就是$v$在$u$上的正交投影（Orthogonal Projection），根據正交投影公式：$\begin{aligned} v_{||} &= proj_{u}(v) &= \frac{u\cdot v}{u\cdot u}u &= \frac{u\cdot v}{||u||^{2}}u &= (u\cdot v)u \end{aligned}\tag{2.5}$其中$u\cdot u=||u||^{2}$，$||u||=1$，點積$(v\cdot u)$爲標量，所以再乘向量$u$得到一個矢量。
3. $v_{rot\perp}$：下面畫出$v_{rot\perp}$的俯視圖：
   圖2.2：旋轉向量俯視圖
   

我們需要處理正交於$u$的$v_{⊥}$，因爲這兩個向量是正交的，這個旋轉可以看做是平面內的一個旋轉，因爲旋轉不改變$v_{⊥}$的長度，所以路徑是一個圓。現在，3D的旋轉被轉化爲了2D平面上的旋轉。由於在這個平面上只有一個向量$v_{⊥}$，用它來表示一個旋轉是不夠的，我們還需要構造一個同時正交於$u$ 和$v_{⊥}$的向量$w$，這個可以通過叉乘來獲得： $w=u\times v_{⊥}\tag{2.6}$注意叉乘的順序，因爲我們使用的是右手座標系統，按照右手定則你可以發現這 個新的向量$w$指向$v_{⊥}$逆時針旋轉$(π/2)$後的方向，並且和$v_{⊥}$一樣也處於正交於$u$ 的平面內。因爲$∥u∥$ = 1，我們可以發現：$\begin{aligned} ||w|| &= ||u\times v_{⊥}|| \\ &= ||u||\cdot ||v_{⊥}||\cdot \sin(\frac{\pi}{2})\\ &=||v_{⊥}|| \end{aligned}\tag{2.7}$也就是說，$w$和$v_{⊥}$的模長是相同的，所以，$w$也位於圓上。有了這個新的向量$w$， 就相當於在平面內有了兩個座標軸。我們現在可以把$v_{rot||}$投影到$w$和$v_{⊥}$上，將其分解爲向量$a$和$b$，使用一些三角知識可以得到：$\begin{aligned} v_{rot⊥} &= a + b \\ &= \sin \theta w + \cos \theta v_{⊥} \\ &= \sin \theta (u\times v_{⊥}) + \cos \theta v_{⊥} \end{aligned}\tag{2.8}$因爲叉乘遵守分配律，且$u$平行於$v_{||}$，故：$\begin{aligned} u\times v_{⊥} &=u\times (v-v_{||})\\ &=u\times v - u \times v_{||}\\ &=u \times v\end{aligned}\tag{2.9}$另外，向量$a$和$b$還有另外一種證法，稍顯繁瑣，有興趣的同學請參見後兩小節。

4. 綜上可得：
   $\begin{aligned} v_{rot} &= v_{rot\perp}+v_{rot||} \\ &=a+b+v_{||} \\ &=\sin \theta u\times v_{⊥} +\cos \theta v_{\perp } + (u\cdot v)u \\ &=\sin \theta u\times v +\cos \theta (v - v_{||}) + (v\cdot u)u \\ &=\sin \theta u\times v +\cos \theta (v - (v\cdot u)u ) + (v\cdot u)u \\ &=\cos \theta v+(1-\cos \theta )(v\cdot u)u + \sin \theta u\times v \end{aligned}\tag{2.10}$
5. 顯然：到此步，我們還無法將其用矩陣來表示，所以需要對$(v\cdot u)u$ 和 $u\times v$ 進行矩陣轉換，由點積的交換律和結合律得：$(v\cdot u)u=u\cdot(v\cdot u)=u\cdot (u^{T} v)=u\cdot u^{T} v\tag{2.11}$其中的向量都是列向量，點積展開規則爲：$x\cdot y = [x, y] =x^{T}y$。
   對於$u\times v$可用叉乘矩陣來化簡爲$Uv$：$u\times v =\begin{bmatrix} (u\times v)_{x}\\ (u\times v)_{y}\\ (u\times v)_{z} \end{bmatrix}= \begin{bmatrix} u_{y}v_{z}- u_{z}v_{y}\\ u_{z}v_{x}- u_{x}v_{z}\\ u_{x}v_{y}- u_{y}v_{x} \end{bmatrix}= \begin{bmatrix} 0 & -u_{z} & u_{y}\\ u_{z} & 0 & -u_{x}\\ u_{y} & u_{x} & 0 \end{bmatrix}\begin{bmatrix} v_{x}\\ v_{y}\\ v_{z} \end{bmatrix}= Uv\tag{2.12}$其中$U= \begin{bmatrix} 0 & -u_{z} & u_{y}\\ u_{z} & 0 & -u_{x}\\ u_{y} & u_{x} & 0 \end{bmatrix}\tag{2.13}$
6. 將$(v\cdot u)u$ 和 $u\times v$ 轉換的矩陣代入式（2.10）得：$\begin{aligned} v_{rot}&=\cos \theta v+(1-\cos \theta )(v\cdot u)u+ \sin \theta u\times v\\ &=\cos \theta v+(1-\cos \theta )uu^{T}v + \sin \theta K v\\ &=(\cos \theta I +(1-\cos \theta )uu^{T} + \sin \theta U)v \end{aligned}\tag{2.14}$故旋轉矩陣$R= \cos \theta I +(1-\cos \theta )uu^{T} + \sin \theta U. \tag{2.15}$其中$I$爲單位矩陣。

2.2.2 推導二

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

2.2.3 推導向量$a$和$b$

此處單獨推導羅德里格斯公式的向量$a$和$b$，僅做參考，也可以忽略不看。$a$和$b$是由$v_{rot\perp }$正交分解得到的矢量，既有大小又有方向，所以在求解時，我們要對其大小和方向分別求解。
$a$大小
設$\theta_{1}= \pi -\theta$，$\theta_{2}$是向量$v$和$u$的夾角，$u$爲單位向量，則對於$a$的大小$|a|$有：
$\begin{aligned}|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||u| \end{aligned}\tag{2.19}$由三角公式$|a\times b| = \sin \theta |a||b|$知：$\sin \theta_{2}|v||u|=|u \times v|$，所以：
$|a|=\sin \theta |u \times v|\tag{2.20}$

$a$方向
由叉乘方向可得$a$的單位方向向量爲：$u \times v /|u \times v|\tag{2.21}$

綜上可得：
$\begin{aligned} a&=(u \times v /|u \times v|)|a|\\ &=(u \times v /|u \times v|)\sin \theta |u \times v|\\ &=\sin \theta u\times v \end{aligned}\tag{2.22}$

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

$b$方向
由於$b$的方向與$v_{\perp }$方向相反，可得$b$的單位方向向量爲：$-v_{\perp }/|v_{\perp }|\tag{2.24}$

綜上可得：
$\begin{aligned} b &=(-v_{\perp }/|v_{\perp }|)|b|\\ &=\cos \theta v_{\perp } \end{aligned}\tag{2.25}$

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

3.旋轉矩陣到向量

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

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

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