三維空間的旋轉

三維空間的旋轉

三維空間中兩個座標系之間的旋轉關係可以通過旋轉矩陣，旋轉向量（角軸），尤拉角，四元數表示。

1 旋轉矩陣

設某個單位正交基$(e_1,e_2,e_3)$經過一次旋轉變成了$(e_1^{{}^{\prime }},e_2^{{}^{\prime }},e_3^{{}^{\prime }})$,那麼對於同一個向量$\mathbf{a}$(注意該向量並沒有隨著座標系的旋轉而發生運動)，它在兩個座標系下的座標為 $(a_1,a_2,a_3{)}^T$ 和$(a_1^{‘},a_2^{‘},a_3^{‘}{)}^T$。根據座標系的定義，：

$$\left[\begin{array}{c}{e}_1,e_2,e_3\end{array}\right]\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{c}{e}_1^{{}^{\prime }},e_2^{{}^{\prime }},e_3^{{}^{\prime }}\end{array}\right]\left[\begin{array}{c}{a}_1^{{}^{\prime }}\\ a_2^{{}^{\prime }}\\ a_3^{{}^{\prime }}\end{array}\right]（1）$$

為描述兩個座標之間的關係，(1)左右兩邊同時乘以$(e_1^T,e_2^T,e_3^T{)}^T$，則有：

$$\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_1^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]=R{\mathbf{a}}^{{}^{\prime }}(2)$$

令：$R=\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_1^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]$（3）

矩陣$R$描述了座標系之間的旋轉，稱為旋轉矩陣。
旋轉矩陣是行列式為1的正交矩陣，旋轉矩陣的集合定義如下：

S O ( n ) = { R ∈ R n × n ∣ R R T = I , d e t ( R ) = I } ( 4 ) SO(n) = \{R \in \mathbb{R}^{n\times n} | RR^T = I, det(R) = I \} (4) SO(n)={R∈Rn×n∣RRT=I,det(R)=I}(4)
由於旋轉矩陣是正交陣，它的逆(即轉置)描述了一個相反的旋轉，則有${\mathbf{a}}^{‘}=R^{-1}\mathbf{a}=R^T\mathbf{a}$。顯然，$R^T$刻畫了一個相反的旋轉。

2 旋轉向量（角軸，李代數）$w$
座標系的任意旋轉都可以用一個旋轉軸和旋轉角來刻畫。使用一個向量，其方向與旋轉軸一致，而長度等於旋轉角，這種向量稱為旋轉向量（角-軸）。
2.1旋轉向量和旋轉矩陣
假如一個旋轉軸為$n$,角度為$\theta$的旋轉，它對應的旋轉向量為$\theta$$n$，旋轉向量$w=n\theta$ 到旋轉矩陣的轉換過程由羅德里格斯公式表明，轉換公式如下：
$$R=\cos \theta I+(1-\cos \theta )n{n}^T+\sin \theta {\left[\begin{array}{c}n\end{array}\right]}_X(5)$$

${\left[\begin{array}{cc}& \end{array}\right]}_X$：表示反對稱矩陣

$${\left[\begin{array}{c}W\end{array}\right]}_X=\left[\begin{array}{cccc}0& -W_3& -W_2& \\ W_3& 0& -W_1& \\ -W_2& W_1& 0& \end{array}\right](6)$$

對於轉角$\theta$，有：

$$\begin{gathered} tr(R)=cos\theta tr(I)+(1-cos\theta )tr(\mathbf{n}{\mathbf{n}}^T)+sin\theta tr([\mathbf{n}{]}_{\times }) \\ =3cos\theta +(1-cos\theta )=1+2cos\theta （7） \end{gathered}$$

因此 $\theta =arccos\frac{tr(R)-1}{2}$ （8）

計算轉軸 $\mathbf{n}$，由於旋轉軸上的向量在旋轉後不發生改變，有 $R\mathbf{n}=\mathbf{n}$。因此，轉軸 $\mathbf{n}$ 是矩陣 $R$ 特徵值為1對應的特徵向量。求解次方程，再歸一化，就得到了轉軸。
2.2 旋轉向量表示旋轉
對於三維空間點$p=\left[\begin{array}{ccc}x& y& z\end{array}\right]$
羅德里格斯旋轉公式：$${\mathbf{p}}_{rot}=\mathbf{p}cos\theta +(\mathbf{n}\times \mathbf{p})sin\theta +\mathbf{n}(\mathbf{n}\cdot \mathbf{p})(1-cos\theta )(9)$$
實際中，使用軸角表示法對三維點進行旋轉會分兩種情況：
$||\mathbf{w}||$ 相比0較大時。這個時候可以通過羅德里格斯旋轉公式計算
$||\mathbf{w}||$ 相比0較小時。根據羅德里格斯公式,
$R=cos||\mathbf{w}||I+\frac{[\mathbf{w}{]}_{\times }}{||\mathbf{w}||}sin(||\mathbf{w}||)+\frac{[\mathbf{w}{]}_{\times }^2}{||\mathbf{w}|{|}^2}(1-cos(||\mathbf{w}||))$
當$\mathbf{w}$較小時，$cos(||\mathbf{w}||)\approx 1,sin(||\mathbf{w}||)\approx ||\mathbf{w}||$。因此，$R\approx I+[\mathbf{w}{]}_{\times }$。於是，$R\mathbf{p}=\mathbf{p}+[\mathbf{w}{]}_{\times }\mathbf{p}$
注： 對於李代數 $\mathbf{w}=\theta \mathbf{n}$，其中$\mathbf{n}$為單位轉軸，因此 $||\mathbf{n}|{|}^2=1$，於是$||\mathbf{w}|{|}^2={\theta }^2||\mathbf{n}|{|}^2={\theta }^2$。因此，有 $\theta =||\mathbf{w}||$

3 尤拉角
把旋轉分解成3次繞不同軸旋轉，每次旋轉的角度構成尤拉角。旋轉時，旋轉軸有先後順序，可以先繞X軸，再繞Y軸，最後繞Z軸，即XYZ軸的旋轉，同理可以定義ZYX,ZYX等。尤拉角在攝影測量中應用比較廣泛，常見的轉角系統有rpy角度系統(ZYX），OPK角度系統(XYZ)。
尤拉角的一個重大缺點是會碰到萬向鎖問題。
3.1 尤拉角和旋轉矩陣
opk角度系統

4 四元數
單位四元數可以表示三維空間的旋轉。
四元數：$\mathbf{q}=(c,\mathbf{v})=(q_0,q_1,q_2,q_3)=q_0+q_1\mathbf{i}+q_2\mathbf{j}+q_3\mathbf{k}$
三個虛部滿足以下關係式：
$$\begin{gathered} i^2=j^2=k^2=-1 \\ ij=k,ji=-k, \\ jk=i,kj=-i, \\ ki=j,ik=-j \end{gathered}$$
4.1 四元數和旋轉向量
假設旋轉繞單位向量 $\mathbf{n}=(n_x,x_y,n_z{)}^T$ 進行了角度為 $\theta$ 的旋轉，則該旋轉的四元數定義為:

$\mathbf{q}=(cos\frac{\theta }{2},n_{x}sin\frac{\theta }{2},n_{y}sin\frac{\theta }{2},n_{z}sin\frac{\theta }{2}{)}^T$ (9)

令 $\theta =\theta +2\pi$，則 $\mathbf{q}=(-cos\frac{\theta }{2},-n_x\sin \frac{\theta }{2},-n_{y}sin\frac{\theta }{2},-n_{z}sin\frac{\theta }{2}{)}^T=-\mathbf{q}$。即 $\mathbf{q}$ 和 $-\mathbf{q}$ 表示同一個旋轉。

$$\begin{gathered} theta=2arccos{q}_0, \\ (n_x,n_y,n_z{)}^T=\frac{1}{sin\frac{\theta }{2}}(q_1,q_2,q_3{)}^T=\frac{1}{||\mathbf{n}||}(q_1,q_2,q_3{)}^T(10) \end{gathered}$$

4.2 四元數和旋轉矩陣

$$R=\left[\begin{array}{ccc}{R}_{11}& R_{12}& R_{13}\\ R_{21}& R_{22}& R_{23}\\ R_{31}& R_{32}& R_{33}\end{array}\right](11)$$

則旋轉矩陣到四元數的轉換如下:

$q_0=\frac{\sqrt{tr(R)+1}}{2},q_1=\frac{R_{23}-R_{32}}{4q_0},q_2=\frac{R_{31}-R_{13}}{4q_0},q_3=\frac{R_{12}-R21}{4q_0}$ (12)

$$R=\left[\begin{array}{ccc}1-2q_2^2-2q_3^2& 2q_1{q}_2-2q_0{q}_3& 2q_1{q}_3+2q_0{q}_2\\ 2q_1{q}_2+2q_0{q}_3& 1-2q_1^2-2_3^2& 2q_2{q}_3-2q_0{q}_1\\ 2q_1{q}_3-2q_0{q}_2& 2q_2{q}_3+2q_0{q}_1& 1-2q_1^2-2q_2^2\end{array}\right](13)$$

4.3 用四元數表示旋轉
假設一個三維空間點$p=\left[\begin{array}{ccc}x& y& z\end{array}\right]\cap \in {\mathbb{R}}^3$,三維點$p$經過旋轉之後變成$p^{{}^{\prime }}$
首先，把三維空間點用一個虛四元數來描述：$p=\left[\begin{array}{cccc}0& x& y& z\end{array}\right]=\left[\begin{array}{cc}0& v\end{array}\right]$
那麼旋轉後的點$p^{{}^{\prime }}$表示為：$p^{{}^{\prime }}=qp{p}^{-1}$ (14)

參考：
[1]: https://aibluefisher.github.io/2019/04/16/rotation/
[2]:視覺SLAM十四講 從理論到實踐