三維旋轉&坐標系變換

旋轉三維向量

圖中 $\mathbf{u}$為單位向量，表示轉軸。將 $\mathbf{x}$繞 $\mathbf{u}$逆時針旋轉角度 $\varphi$得到 ${\mathbf{x}}^{\prime }$。可將 $\mathbf{x}$分解為沿轉軸的分量 ${\mathbf{x}}_{\parallel }$和垂直轉軸的分量 ${\mathbf{x}}_{\perp }$。 ${\mathbf{x}}_{\parallel }$在轉動時不變， ${\mathbf{x}}_{\perp }$在平面上旋轉角度 $\varphi$得到 ${\mathbf{x}}_{\perp }^{\prime }$，可得

$$\begin{array}{}\text{(1)}& {\mathbf{x}}^{\prime }={\mathbf{x}}_{\parallel }+{\mathbf{x}}_{\perp }\cos \varphi +(\mathbf{u}\times \mathbf{x})\sin \varphi \end{array}$$

有

$$\mathbf{u}\times \mathbf{x}=[\mathbf{u}{]}_{\times }\mathbf{x}$$

其中

$$[\mathbf{u}{]}_{\times }\triangleq \left[\begin{array}{ccc}0& -u_3& u_2\\ u_3& 0& -u_1\\ -u_2& u_1& 0\end{array}\right]$$

為反對稱矩陣。

那么式 $\text{(1)}$可寫成：

$$\begin{array}{rl}{\mathbf{x}}^{\prime }& =\mathbf{u}{\mathbf{u}}^T\mathbf{x}+(\mathbf{x}-\mathbf{u}{\mathbf{u}}^T\mathbf{x})\cos \varphi +[\mathbf{u}{]}_{\times }\mathbf{x}\sin \varphi \\ & =((1-\cos \varphi )\mathbf{u}{\mathbf{u}}^{\mathrm{\top }}+\cos \varphi \mathbf{I}+\sin \varphi [\mathbf{u}{]}_{\times })\mathbf{x}\end{array}$$

因此旋轉作用可用一個矩陣表示。

旋轉群

旋轉是線性變換，將 $\mathbf{v}$變為 $r(\mathbf{v})$。其保持：

1. 向量長度
2. 兩向量的內積
3. 相對方向 $\mathbf{u}\times \mathbf{v}=\mathbf{w}⟺r(\mathbf{u})\times r(\mathbf{v})=r(\mathbf{w})$

1和2是等價的。1推2可由 $\Vert r(\mathbf{u}\mathbf{-}\mathbf{v})\Vert =\Vert \mathbf{u}\mathbf{-}\mathbf{v}\Vert$得到，2推1可由 $r(\mathbf{u})\cdot r(\mathbf{u})=\mathbf{u}\cdot \mathbf{u}$得到。

因此可定義旋轉群：

$$SO(3):\{{\mathbb{R}}^3\to {\mathbb{R}}^3\mid \mathrm{\forall }\mathbf{v},\mathbf{w}\in {\mathbb{R}}^3,\Vert r(\mathbf{v})\Vert =\Vert \mathbf{v}\Vert ,r(\mathbf{v})\times r(\mathbf{w})=r(\mathbf{v}\times \mathbf{w})\}$$

旋轉矩陣

算符 $r$是線性的，可用矩陣 $R$表示，

$$r(\mathbf{v})=\mathbf{R}\mathbf{v}$$

由 $\Vert \mathbf{R}\mathbf{v}\Vert =\Vert \mathbf{v}\Vert$，即

$$(\mathbf{R}\mathbf{v}{)}^{\mathrm{\top }}(\mathbf{R}\mathbf{v})={\mathbf{v}}^{\mathrm{\top }}{\mathbf{R}}^{\mathrm{\top }}\mathbf{R}\mathbf{v}={\mathbf{v}}^{\mathrm{\top }}\mathbf{v}$$

可得：

$${\mathbf{R}}^{\mathrm{\top }}\mathbf{R}=\mathbf{I}$$

即旋轉矩陣是正交矩陣。

由旋轉性質3，對於向量 $\mathbf{u},\mathbf{v},\mathbf{w}$組成的六面體，旋轉前后的有向體積應該相等，即

$$\left|\begin{array}{ccc}Ru& Rv& Rw\end{array}\right|=det(R)\left|\begin{array}{ccc}u& v& w\end{array}\right|=\left|\begin{array}{ccc}u& v& w\end{array}\right|$$

得 $det(R)=1$。

這就構成了 $SO(3)$群(Special Orthogonal group)，其中的special就是指 $det(R)=1$。

歐拉定理

是說存在向量 $\mathbf{u}$，使得旋轉前后不變：

$$\mathbf{R}\mathbf{u}=\mathbf{u}$$

$\mathbf{u}$就是轉軸方向。

證明：
只要證明 $\mathbf{R}$有為1的特征值即可。

$$\begin{array}{rl}det(\mathbf{R}-\mathbf{I})=det((\mathbf{R}-\mathbf{I}{)}^{\mathsf{T}})& =det({\mathbf{R}}^{\mathsf{T}}-\mathbf{I})=det({\mathbf{R}}^{-1}-{\mathbf{R}}^{-1}\mathbf{R})\\ & =det({\mathbf{R}}^{-1}(\mathbf{I}-\mathbf{R}))=det\left({\mathbf{R}}^{-1}\right)det(-(\mathbf{R}-\mathbf{I}))\\ & =-det(\mathbf{R}-\mathbf{I})⟹det(\mathbf{R}-\mathbf{I})=0.\end{array}$$

指數映射

$$\frac{d}{dt}(R^{T}R)={\stackrel{\dot{}}{R}}^{T}R+R^T\stackrel{\dot{}}{R}=0$$

得：

$$R^T\stackrel{\dot{}}{R}=-(R^T\stackrel{\dot{}}{R}{)}^T$$

即 $R^T\stackrel{\dot{}}{R}$是反對稱矩陣。三維的反對稱矩陣集合用 $\mathfrak{s}\mathfrak{o}(3)$表示，稱為 $SO(3)$的李代數。

那么有

$$R^T\stackrel{\dot{}}{R}=[\mathit{\omega }{]}_{\times }$$

得到：

$$\stackrel{\dot{}}{R}=R[\mathit{\omega }{]}_{\times }$$

當 $R=I$ 時， $\stackrel{\dot{}}{R}=[\mathit{\omega }{]}_{\times }$，即李代數是在幺元處的切空間。

如果 $\mathit{\omega }$為常數，上述方程解得：

$$R(t)=R(0)e^{[\mathit{\omega }{]}_{\times }t}$$

其中矩陣的指數按泰勒級數定義。

這稱為指數映射：

$$\exp :\mathfrak{s}\mathfrak{o}(3)\to SO(3);[\mathit{\varphi }{]}_{\times }\mapsto \exp ([\mathit{\varphi }{]}_{\times })=e^{[\mathit{\varphi }{]}_{\times }}$$

還可以定義"大寫的"指數映射：

$$\text{Exp}:{\mathbb{R}}^3\to SO(3);\mathit{\varphi }\mapsto \text{Exp}(\mathit{\varphi })=e^{[\mathit{\varphi }{]}_{\times }}$$

如果繞轉軸 $\mathbf{u}$轉了角度 $\varphi$，那么旋轉矩陣：

$$\mathbf{R}=e^{\varphi [\mathbf{u}{]}_{\times }}$$

上式按泰勒展開后得：

$$\mathbf{R}=\mathbf{I}+\sin \varphi [\mathbf{u}{]}_{\times }+(1-\cos \varphi )[\mathbf{u}{]}_{\times }^2$$

這就是Rodrigues旋轉公式。推導過程用到了

$$[\mathbf{a}{]}_{\times }^2=\mathbf{a}{\mathbf{a}}^{\mathrm{\top }}-{\mathbf{a}}^{\mathrm{\top }}\mathbf{a}\mathbf{I}$$

根據這個式子，又可寫成：

$$\begin{array}{}\text{(2)}& \mathbf{R}=\cos \varphi \mathbf{I}+\sin \varphi [\mathbf{u}{]}_{\times }+(1-\cos \varphi )\mathbf{u}{\mathbf{u}}^{\mathrm{\top }}\end{array}$$

這就是式 $\text{(1)}$。

從 $\mathbf{R}$得到 $\varphi$和 $\mathbf{u}$

由 $\text{(2)}$可得：

$$tr(\mathbf{R})=2\cos \varphi +1$$

即：

$$\varphi =\arccos \left(\frac{tr(\mathbf{R})-1}{2}\right)$$

又有

$$\mathbf{R}-{\mathbf{R}}^T=2\sin \varphi [\mathbf{u}{]}_{\times }$$

因此

$$\mathbf{u}=\frac{(\mathbf{R}-{\mathbf{R}}^T{)}^{\vee }}{2\sin \varphi }$$

其中 ${\bullet }^{\vee }$是 $[\bullet {]}_{\times }$的逆，即 $([{\mathbf{v}}_{\times }{]}^{\vee })=\mathbf{v}$.

如果 $\sin \varphi =0$，分兩種情況：

1. $\cos \varphi =1$，這時候 $\mathbf{R}=\mathbf{I}$，轉軸未定義
2. $\cos \varphi =-1$，這時 $\mathbf{R}+\mathbf{I}=2\mathbf{u}{\mathbf{u}}^T$，這個矩陣的每一列都平行於 $\mathbf{u}$，只要對非0的一列歸一化即可。這時候 $-\mathbf{u}$也滿足條件，但是不影響，因為繞 $\pm \mathbf{u}$轉180度效果是一樣的。

四元數

四元數.md

旋轉公式為：

$$\begin{array}{}\text{(3)}& {\mathbf{x}}^{\prime }=\mathbf{q}\otimes \mathbf{x}\otimes {\mathbf{q}}^{\ast }\end{array}$$

旋轉要求

$$\Vert \mathbf{x}\Vert =\Vert {\mathbf{x}}^{\prime }\Vert =\Vert \mathbf{q}{\Vert }^2\Vert \mathbf{x}\Vert$$

因此 $\Vert \mathbf{q}{\Vert }^2=1$，即 $\mathbf{q}$是單位四元數：

$${\mathbf{q}}^{\ast }\otimes \mathbf{q}=1=\mathbf{q}\otimes {\mathbf{q}}^{\ast }$$

這與 $R^{T}R=I=R{R}^T$的條件類似。

還可以看到，自動保持了相對方向：

$$\begin{array}{rl}r(v)\times r(w)& =(q\otimes v\otimes q^{\ast })\times (q\otimes w\otimes q^{\ast })\\ & =\frac{1}{2}((q\otimes v\otimes q^{\ast })\otimes (q\otimes w\otimes q^{\ast })-(q\otimes w\otimes q^{\ast })\otimes (q\otimes v\otimes q^{\ast }))\\ & =\frac{1}{2}(q\otimes v\otimes w\otimes q^{\ast }-q\otimes w\otimes v\otimes q^{\ast })\\ & =\frac{1}{2}(q\otimes (v\otimes w-w\otimes v)\otimes q^{\ast })\\ & =q\otimes (v\times w)\otimes q^{\ast }\\ & =r(v\times w)\end{array}$$

其中第2和第5個等號是因為：

$${\mathbf{p}}_v\otimes {\mathbf{q}}_v-{\mathbf{q}}_v\otimes {\mathbf{p}}_v=2{\mathbf{p}}_v\times {\mathbf{q}}_v$$

指數映射

$$\frac{d(q^{\ast }\otimes q)}{dt}={\stackrel{\dot{}}{q}}^{\ast }\otimes q+q^{\ast }\otimes \stackrel{\dot{}}{q}=0$$

得：

$$q^{\ast }\otimes \stackrel{\dot{}}{q}=-({\stackrel{\dot{}}{q}}^{\ast }\otimes q)=-(q^{\ast }\otimes \stackrel{\dot{}}{q}{)}^{\ast }$$

即 $q^{\ast }\otimes \stackrel{\dot{}}{q}$是虛四元數。令：

$$q^{\ast }\otimes \stackrel{\dot{}}{q}=\mathrm{\Omega }$$

兩邊左乘 $q$，得到：

$$\stackrel{\dot{}}{q}=q\otimes \mathrm{\Omega }$$

當 $q=1$時， $\stackrel{\dot{}}{q}=\mathrm{\Omega }$，可見虛四元數構成了單位四元數球 $S^3$的切空間。

如果 $\mathrm{\Omega }$為常數，上式解得 $q(t)=q(0)\otimes e^{\mathrm{\Omega }t}$（可代回驗證），這就引出了指數映射。

如果繞轉軸 $\mathbf{u}$轉了角度 $\varphi$，定義“大寫的”指數映射：

$$\begin{array}{}\text{(4)}& \mathbf{q}\triangleq \text{Exp}(\varphi \mathbf{u})=e^{\varphi \mathbf{u}/2}=\cos \frac{\varphi }{2}+\mathbf{u}\sin \frac{\varphi }{2}\end{array}$$

旋轉作用

將式 $\text{(4)}$代入式 $\text{(3)}$， 推導可得式 $\text{(1)}$，這就驗證了 $\text{(3)}$的正確性。

在證明中有一步 $\mathbf{u}\otimes \mathbf{x}\otimes \mathbf{u}=\mathbf{x}({\mathbf{u}}^T\mathbf{u})-2\mathbf{u}({\mathbf{u}}^T\mathbf{x})$用到了 $(a\times b)\times c=-a(c\cdot b)+b(c\cdot a)$

四元數到旋轉矩陣的轉換

由

$$\mathbf{q}\otimes \mathbf{x}\otimes {\mathbf{q}}^{\ast }=[{\mathbf{q}}^{\ast }{]}_R[\mathbf{q}{]}_L\left[\begin{array}{c}0\\ \mathbf{x}\end{array}\right]=\left[\begin{array}{c}0\\ \mathbf{R}\mathbf{x}\end{array}\right]$$

可以得到：

$$\mathbf{R}=(q_w^2-{\mathbf{q}}_v^{\mathrm{\top }}{\mathbf{q}}_v)\mathbf{I}+2{\mathbf{q}}_v{\mathbf{q}}_v^{\mathrm{\top }}+2q_w[{\mathbf{q}}_v{]}_{\times }$$

旋轉合成

假設旋轉2作用於旋轉1之后。

對於旋轉矩陣，

$${\mathbf{R}}_2({\mathbf{R}}_1\mathbf{x})=({\mathbf{R}}_2{\mathbf{R}}_1)\mathbf{x}$$

對於四元數，

$${\mathbf{q}}_2\otimes ({\mathbf{q}}_1\otimes \mathbf{x}\otimes {\mathbf{q}}_1^{\ast })\otimes {\mathbf{q}}_2^{\ast }=({\mathbf{q}}_2\otimes {\mathbf{q}}_1)\otimes \mathbf{x}\otimes ({\mathbf{q}}_2\otimes {\mathbf{q}}_1{)}^{\ast }$$

因此，后旋轉的都是乘在左邊。

---

坐標系變換

方向余弦矩陣（DCM）

用G表示global，或者n系；L表示local，或者b系。

${\mathbf{r}}_G,{\mathbf{r}}_L$分別為向量 $\mathbf{r}$在坐標系 $G，L$中的坐標，由

$$\mathbf{r}=\left(\begin{array}{ccc}{\mathbf{i}}_G& {\mathbf{j}}_G& {\mathbf{k}}_G\end{array}\right){\mathbf{r}}_G=\left(\begin{array}{ccc}{\mathbf{i}}_L& {\mathbf{j}}_L& {\mathbf{k}}_L\end{array}\right){\mathbf{r}}_L$$

得：

$$\begin{array}{rl}{\mathbf{r}}_G& =\left(\begin{array}{c}{\mathbf{i}}_G^{⊺}\\ {\mathbf{j}}_G^{⊺}\\ {\mathbf{k}}_G^{⊺}\end{array}\right)\left(\begin{array}{ccc}{\mathbf{i}}_L& {\mathbf{j}}_L& {\mathbf{k}}_L\end{array}\right){\mathbf{r}}_L\\ & \triangleq R_{GL}{\mathbf{r}}_L\end{array}$$

表示從 $L$到 $G$的坐標變換。

由 $R_{GL}$的定義還可得基變換：

$$\left(\begin{array}{ccc}{\mathbf{i}}_L& {\mathbf{j}}_L& {\mathbf{k}}_L\end{array}\right)=\left(\begin{array}{ccc}{\mathbf{i}}_G& {\mathbf{j}}_G& {\mathbf{k}}_G\end{array}\right)R_{GL}$$

對於一個與 $L$系固連的向量 $\mathbf{r}$，經過主動旋轉 $R$，得到 ${\mathbf{r}}^{\prime }$，那么：

$${\mathbf{r}}_G^{\prime }=R{\mathbf{r}}_G$$

而 ${\mathbf{r}}_L^{\prime }={\mathbf{r}}_G$，因此

$${\mathbf{r}}_G^{\prime }=R{\mathbf{r}}_L^{\prime }$$

而按定義：

$${\mathbf{r}}_G^{\prime }=R_{GL}{\mathbf{r}}_L^{\prime }$$

因此坐標變換矩陣與主動旋轉矩陣 $R$的關系是：

$$R_{GL}=R$$

歐拉角

intrinsic rotation是指繞當前坐標系（而不是某個固定坐標系）的軸轉動。

$G$繞某軸逆時針旋轉 $\theta$，得到 $L$。對於 $R_{LG}$，根據定義得出如下結果1：

繞 $X$軸旋轉

$$R_x=\left[\begin{array}{ccc}1& 0& 0\\ 0& c_{\theta }& s_{\theta }\\ 0& -s_{\theta }& c_{\theta }\end{array}\right]$$

繞 $Y$軸旋轉

$$R_y=\left[\begin{array}{ccc}{c}_{\theta }& 0& -s_{\theta }\\ 0& 1& 0\\ s_{\theta }& 0& c_{\theta }\end{array}\right]$$

繞 $Z$軸旋轉

$$R_z=\left[\begin{array}{ccc}{c}_{\theta }& s_{\theta }& 0\\ -s_{\theta }& c_{\theta }& 0\\ 0& 0& 1\end{array}\right]$$

如果按 $Z\to Y\to X$的順序轉動，

$$R_{LG}=R_x{R}_y{R}_z=\left[\begin{array}{ccc}{c}_y{c}_z& c_y{s}_z& -s_y\\ s_x{s}_y{c}_z-c_x{s}_z& s_x{s}_y{s}_z+c_x{c}_z& s_x{c}_y\\ c_x{s}_y{c}_z+s_x{s}_z& c_x{s}_y{s}_z-s_x{c}_z& c_x{c}_y\end{array}\right]$$

可得歐拉角和DCM的轉換關系：

$$\begin{gathered} \text{tan}({\theta }_z)=\frac{R_{12}}{R_{11}} \\ \text{sin}({\theta }_y)=-R_{13} \\ \text{tan}({\theta }_x)=\frac{R_{23}}{R_{33}} \end{gathered}$$

若是小角度轉動，

$$\begin{array}{}\text{(5)}& R=\left[\begin{array}{ccc}1& {\theta }_z& -{\theta }_y\\ -{\theta }_z& 1& {\theta }_x\\ {\theta }_y& -{\theta }_x& 1\end{array}\right]=I-[\mathit{\theta }{]}_{\times }\end{array}$$

可見小角度轉動與轉動順序無關（即只與繞XYZ各軸轉過的角度有關。如果只有倆軸，比如Z->X->Z，那么未出現的軸角度為0，出現兩次的軸的角度相加），等效於一次轉動：角度為 $\theta =\sqrt{{\theta }_x^2+{\theta }_y^2+{\theta }_z^2}$，轉軸為 $\frac{1}{\theta }\left(\begin{array}{c}{\theta }_x\\ {\theta }_y\\ {\theta }_z\end{array}\right)$，也可以從Rodrigues旋轉公式做小角近似看出來：

$${\mathbf{R}}_{LG}={\mathbf{R}}^T\approx \mathbf{I}-\varphi [\mathbf{u}{]}_{\times }$$

Extrinsic rotation

設固定坐標系為 $A$。坐標系 $B$繞 $A$的 $\mathbf{u}$軸轉動角度 $\varphi$，這個轉動用 $R_{\varphi \mathbf{u}}$表示，得坐標系 $C$。求 $R_{CA}$。

首先

$$R_{CA}=R_{BD}$$

其中 $D$為 $A$繞 $\mathbf{u}$軸轉動角度 $-\varphi$得到。這個結論對於二維轉動很直觀，對於三維其實也容易看出。按定義，只要證明兩對坐標系的基的內積相等即可。把基分解到平行於轉軸和垂直於轉軸。平行部分的內積顯然不變，垂直部分由二維情況可知也不變，且平行於垂直部分內積為0。因此得證。

$$R_{BD}=R_{BA}{R}_{AD}=R_{BA}{R}_{DA}^{-1}=R_{BA}{R}_{-\varphi \mathbf{u}}^{-1}=R_{BA}{R}_{\varphi \mathbf{u}}$$

因此得到結論，繞固定坐標系的軸轉動， $R$是乘在右邊的。

四元數

和DCM類比有

$$\begin{array}{}\text{(6)}& {\mathbf{x}}_G={\mathbf{q}}_{GL}\otimes {\mathbf{x}}_L\otimes {\mathbf{q}}_{GL}^{\ast }\end{array}$$

其中 ${\mathbf{q}}_{GL}$等於主動旋轉 $\mathbf{q}$。

運動方程

推導的要點是從當前 $L$系做擾動得到新的 $L$系，而陀螺儀的測量值剛好是在角速度在 $L$系的值。

四元數

由 $\text{(6)}$可得， ${\mathbf{q}}_{G{L}_2}={\mathbf{q}}_{G{L}_1}{\mathbf{q}}_{L_1{L}_2}$，即擾動是乘在右邊的（L表示local）：

$$q(t+\mathrm{\Delta }t)=q(t)\otimes \mathrm{\Delta }{q}_L=q(t)\otimes \text{Exp}(\mathrm{\Delta }{\mathit{\varphi }}_L)$$

$$\begin{array}{rl}\stackrel{\dot{}}{q}& \triangleq \underset{\mathrm{\Delta }t\to 0}{lim}\frac{q(t+\mathrm{\Delta }t)-q(t)}{\mathrm{\Delta }t}\\ & =\underset{\mathrm{\Delta }t\to 0}{lim}\frac{q\otimes \mathrm{\Delta }q-q}{\mathrm{\Delta }t}\\ & =\underset{\mathrm{\Delta }t\to 0}{lim}\frac{q\otimes (\left[\begin{array}{c}\cos \frac{\mathrm{\Delta }\varphi }{2}\\ \mathbf{u}\sin \frac{\mathrm{\Delta }\varphi }{2}\end{array}\right]-\left[\begin{array}{c}1\\ 0\end{array}\right])}{\mathrm{\Delta }t}\\ & =\underset{\mathrm{\Delta }t\to 0}{lim}\frac{q\otimes (\left[\begin{array}{c}1\\ \mathrm{\Delta }\mathit{\varphi }/2\end{array}\right]-\left[\begin{array}{c}1\\ 0\end{array}\right])}{\mathrm{\Delta }t}(小角近似)\\ & =\underset{\mathrm{\Delta }t\to 0}{lim}\frac{q\otimes \left[\begin{array}{c}0\\ \mathrm{\Delta }\mathit{\varphi }/2\end{array}\right]}{\mathrm{\Delta }t}\\ & =\frac{1}{2}q\otimes {\mathit{\omega }}_L\end{array}$$

方向余弦矩陣

由 $\text{(5)}$得：

$$R_{L_2{L}_1}=I-[\mathit{\theta }{]}_{\times }$$

因此

$$R_{L_1{L}_2}=R_{L_2{L}_1}^T=I+[\mathit{\theta }{]}_{\times }$$

由 $R_{G{L}_2}=R_{GL1}{R}_{L_1{L}_2}$，推導可得：

$${\stackrel{\dot{}}{R}}_{GL}=R_{GL}[{\mathit{\omega }}_L{]}_{\times }$$

參考資料

Quaternion kinematics for the error-state Kalman filter

---

1. $s_{\theta }$放置位置記憶方法：放在轉動軸的上一列（循環）。 ↩