# 旋轉矩陣及四元數的空間轉換

###### tags: `articles`

**作者: [鄭聖文](http://wiki.csie.ncku.edu.tw/User/shengwen)**

**E-mail:** `[e-mail]`

**本文章以 `創用CC 姓名標示─非商業性─相同方式分享` 授權**


## 前言

旋轉矩陣及四元數是用來描述四軸飛行器、機械手臂或是各式機器人姿態的基本數學公式，本文是依照我的理解所撰寫的一篇導讀，若有錯誤，麻煩請您來信指正。

## 歐拉角及旋轉矩陣

歐拉角(Euler angle)是用於表示兩座標系之間相對應的角度關係，從一座標系至一座標系的轉換可以以一線性轉換進行描述。

給出沿著$x$軸旋轉$\phi$度的轉換:

$\pmatrix{x' \\ y' \\ z'} = \pmatrix{1 & 0 & 0 \\ 0 & cos\phi & -sin\phi \\ 0 & sin\phi & cos\phi}\pmatrix{x \\ y \\ z}$         

給出沿著$y$軸旋轉$\theta$度的轉換:

$\pmatrix{x' \\ y' \\ z'} = \pmatrix{cos\theta & 0 & sin\theta \\ 0 & 1 & 0 \\ -sin\theta & 0 & cos\theta}\pmatrix{x \\ y \\ z}$         

給出沿著$z$軸旋轉$\psi$度的轉換:
$\pmatrix{x' \\ y' \\ z'} = \pmatrix{cos\psi & -sin\psi & 0 \\sin\psi & cos\psi & 0 \\ 0 & 0 & 1}\pmatrix{x \\ y \\ z}$         

分別列出上面三組所使用到的轉換矩陣$R_x$, $R_y$以及$R_z$, 我們以下標表示旋轉軸:

$$R_x = \pmatrix{1 & 0 & 0 \\ 0 & cos\phi & -sin\phi \\ 0 & sin\phi & cos\phi}$$

$$R_y = \pmatrix{cos\theta & 0 & sin\theta \\ 0 & 1 & 0 \\ -sin\theta & 0 & cos\theta}$$

$$R_z = \pmatrix{cos\psi & -sin\psi & 0 \\sin\psi & cos\psi & 0 \\ 0 & 0 & 1}$$

接著可以定義由載體參考系(body frame of reference)旋轉至慣性參考系(Inertial frame of reference)的數學公式:

$$\pmatrix{x_i \\ y_i \\ z_i} = R_z R_y R_x \pmatrix{x_b \\ y_b \\ z_b} = R^i_b \pmatrix{x_b \\ y_b \\ z_b}$$

我們以$R_b^i$表示載體到慣性(body to interial)的轉換:

$$R_b^i = R_{z}R_{y}R_{x}$$

$$= \pmatrix{\cos\psi\cos\theta & -\sin\psi\cos\phi+\cos\psi\sin\theta\sin\phi & \sin\psi\sin\phi+\cos\psi\cos\phi\sin\theta \\ \sin\psi\cos\theta & \cos\psi\cos\phi+\sin\phi\sin\theta\sin\psi & -\cos\psi\sin\phi+\sin\theta\sin\psi\cos\phi \\ -\sin\theta & \cos\theta\sin\phi &
cos\theta\cos\phi}$$

旋轉矩陣並不具有交換律，以上我們以$(x-y-z)$的順序旋轉。

而若給定一已知旋轉矩陣，可由下式求出歐拉角:

$$\pmatrix{\phi \\ \theta \\ \psi} = \pmatrix{\tan^{-1}(\frac{r_{32}}{r_{33}}) \\ \sin^{-1}(-r_{31}) \\ \tan^{-1}(\frac{r_{21}}{r_{11}})} = \pmatrix{\tan^{-1}(\frac{\cos\theta\sin\phi}{cos\theta\cos\phi}) \\ \sin^{-1}(-(-\sin\theta)) \\ \tan^{-1}(\frac{\sin\psi\cos\theta}{\cos\psi\cos\theta})}$$

旋轉矩陣的逆轉換可由反矩陣取得:

$$\pmatrix{x_b \\ y_b \\ z_b} = R_x^{-1} R_y^{-1} R_z^{-1} \pmatrix{x_i \\ y_i \\ z_i} = R^b_i \pmatrix{x_i \\ y_i \\ z_i}$$

而因旋轉矩陣屬於**特殊旋轉群 SO(3)**，因此逆矩陣將等價於轉置矩陣:

$$R^b_i = R_{x}^{-1}R_{y}^{-1}R_{z}^{-1} = R_{x}^{\top}R_{y}^{\top}R_{z}^{\top}, \ \ \ \ \ since \ R \in SO(3)$$

## 定義四元數之基本運算

四元數是數學家 **威廉·哈密頓 (William Rowan Hamilton)** 於1843年提出的數學概念，其用途是作為物理學表達空間的工具。四元數是複數系的擴展，因此了解複數對於理解四元數有一定幫助。

四元數由一實數與三虛數組成，可以表示如下:

$$q \triangleq w+xi+yj+zk = \pmatrix{x \\ y \\ z \\ w}$$

如同虛數一樣，四元數具有其獨特的乘法關係:

$$i^2=j^2=k^2=-1$$

以及

$$ij=k,\ ji=-k$$

$$jk=i,\ kj=-i$$

$$ki=j,\ ik=-j$$

由上面六條規則可以發現，**四元數虛數之間相互乘法不具有交換性**

四元數的乘法關係可以整理為以下表格:

<center>

<img src="https://i.imgur.com/yMz9cIm.png" width="35%" height="35%"></a>

<!---
| $\cdot$ | $1$ | $\ \ i$ | $\ \ j$ | $\ \ k$ |
|---|---|----|----|----|
| $1$ | $1$ | $\ \ i$ | $\ \ j$ | $\ \ k$ |
| $i$ | $i$ | $-1$ | $\ \ k$ | $-j$ |
| $j$ | $j$ | $-k$ | $-1$ | $\ \ i$ |
| $k$ | $k$ | $\ \ j$ | $-i$ | $-1$ |
-->

</center>

我們首先定義四元數的加法:

$$q_1+q_2 = (w_1+w_2)+(x_1+x_2)i+(y_1+y_2)j+(z_1+z_2)k$$

接著定義乘法運算:

$$q_1q_2=(w_1+x_1i+y_1j+z_1k)(w_2+x_2i+y_2j+z_2k)=$$

$$(w_1w_2-x_1x_2-y_1y_2-z_1z_2)\ +$$

$$(w_1x_2+x_1w_2+y_1z_2-z_1y_2)i\ +$$

$$(w_1y_2+x_1z_2+y_1w_2+z_1x_2)j\ +$$

$$(w_1z_2+x_1y_2-y_1x_2+z_1w_2)k$$

上式可以從乘法表中導出。

可以將四元數的實數部及虛數部拆開寫作下式:

$$q=s+\vec{v}$$

因此乘法可寫作:

$$q_1q_2=s_1s_2-\vec{v_1}\vec{v_2}+s_1\vec{v_2}+s_2\vec{v_1}+\vec{v_1}\times\vec{v_2}$$

上式的$\times$為向量運算之外積運算。

而四元數乘法亦可以矩陣形式表示:

$$\pmatrix{x\\y\\z\\w}=\pmatrix{w_1&-z_1&y_1&x_1 \\ z_1&w_1&-x_1&y_1 \\ -y_1&x_1&w_1&z_1 \\ -x_1&-y_1&-z_1&w_1} \pmatrix{x_2 \\ y_2 \\ z_2 \\ w_2}$$

在稍後的章節中，會使用到四元數乘法的逆運算，而乘法逆運算必須由四元數的共軛值求得，因此我們首先定義四元數的共軛值為:

$$q^{*}=w-xi-yj-zk$$

而四元數的長度為:

$$|q|=\sqrt{q q^{*}}=\sqrt{w^2+x^2+y^2+z^2}$$

並且由於:

$$q^{-1}=\frac{1}{q}=\frac{q^{*}}{qq^{*}}=\frac{q^{*}}{|q|^2}$$

因此四元數的乘法逆運算可由下式求得:

$$q^{-1}=\frac{q^{*}}{|q|^2}$$

而當四元數為單位四元數時($\sqrt{w^2+x^2+y^2+z^2}=1$)，乘法逆運算等於共軛值:

$$q^{-1}=q^{*} \ \ \ \ if\ |q|=1$$

## 四元數的空間旋轉

根據已被證明之數學，若給出一四元數:

$$q=\cos(\frac{\theta}{2})+\sin(\frac{\theta}{2})\vec{n}$$

以及以四元數表達的一座標:

$$q_{space}=\pmatrix{x\\y\\z\\0}$$

則下式可以將點$q_{space}$相對原點旋轉$\theta$度:

$$q^{'}_{space}=qq_{space}q^{-1}$$

而$\vec{n}$是任意想要旋轉的軸。

$qq_{space}q^{-1}$可以想像為一四度空間上的旋轉，該運算除了會使原座標旋轉$\theta$度之外，也會在第四維空間上產生一位移，因此除了一次運算($q$)外，可以另一次逆運算($q^{-1}$)消除空間上的位移，但兩次運算皆會使座標旋轉$\theta$度，使最終旋轉的度數為$2\theta$度，因此我們必須選定原先的$\theta$角為$\frac{\theta}{2}$度。


接著將上式以矩陣形式描述:

$$\pmatrix{x^{"}\\y^{"}\\z^{"}\\w^{"}}=\pmatrix{1-2(y^2+z^2)&2(xy-yz)&2(wy+xz)&0 \\ 2(xy+wz)&1-2(x^2+z^2)&2(yz-wx)&0 \\ 2(xz-wy)&2(yz+wx)&1-2(x^2+y^2)&0 \\ 0&0&0&1}\pmatrix{x^{'}\\y^{'}\\z^{'}\\w^{'}}$$

亦可將行4列4省略記做下式:

$$\pmatrix{x^{"}\\y^{"}\\z^{"}}=\pmatrix{1-2(y^2+z^2)&2(xy-yz)&2(wy+xz) \\ 2(xy+wz)&1-2(x^2+z^2)&2(yz-wx) \\ 2(xz-wy)&2(yz+wx)&1-2(x^2+y^2)}\pmatrix{x^{'}\\y^{'}\\z^{'}}$$

接著擴展$q=\cos(\frac{\theta}{2})+\sin(\frac{\theta}{2})\vec{n}$使得我們可以一次描述$(x-y-z)$三軸的旋轉:

$$q=q_zq_yq_x=(\cos(\frac{\psi}{2})+\sin(\frac{\psi}{2})\vec{k})(\cos(\frac{\theta}{2})+\sin(\frac{\theta}{2})\vec{j})(\cos(\frac{\phi}{2})+\sin(\frac{\phi}{2})\vec{i})$$

$$\rightarrow \pmatrix{x\\y\\z\\w}=\pmatrix{\sin(\frac{\phi}{2})\cos(\frac{\theta}{2})cos(\frac{\psi}{2})-\cos(\frac{\phi}{2})\sin(\frac{\theta}{2})\sin(\frac{\psi}{2}) \\ \cos(\frac{\phi}{2})\sin(\frac{\theta}{2})cos(\frac{\psi}{2})+\sin(\frac{\phi}{2})\cos(\frac{\theta}{2})\sin(\frac{\psi}{2}) \\ \cos(\frac{\phi}{2})\cos(\frac{\theta}{2})sin(\frac{\psi}{2})-\sin(\frac{\phi}{2})\sin(\frac{\theta}{2})\cos(\frac{\psi}{2}) \\ \cos(\frac{\phi}{2})\cos(\frac{\theta}{2})cos(\frac{\psi}{2})+\sin(\frac{\phi}{2})\sin(\frac{\theta}{2})\sin(\frac{\psi}{2})}$$

注意，四元數乘法並不具備交換性，改變順序將會導致結果改變。

而數學上由於旋轉矩陣與四元數具有對稱性，因此由:

$$\pmatrix{\cos\psi\cos\theta & -\sin\psi\cos\phi+\cos\psi\sin\theta\sin\phi & \sin\psi\sin\phi+\cos\psi\cos\phi\sin\theta \\ \sin\psi\cos\theta & \cos\psi\cos\phi+\sin\phi\sin\theta\sin\psi & -\cos\psi\sin\phi+\sin\theta\sin\psi\cos\phi \\ -\sin\theta & \cos\theta\sin\phi &
cos\theta\cos\phi}$$

$$\pmatrix{\phi \\ \theta \\ \psi} = \pmatrix{\tan^{-1}(\frac{r_{32}}{r_{33}}) \\ \sin^{-1}(-r_{31}) \\ \tan^{-1}(\frac{r_{21}}{r_{11}})} = \pmatrix{\tan^{-1}(\frac{\cos\theta\sin\phi}{cos\theta\cos\phi}) \\ \sin^{-1}(-(-\sin\theta)) \\ \tan^{-1}(\frac{\sin\psi\cos\theta}{\cos\psi\cos\theta})}$$

可以給出:

$$\pmatrix{1-2(y^2+z^2)&2(xy-yz)&2(wy+xz) \\ 2(xy+wz)&1-2(x^2+z^2)&2(yz-wx) \\ 2(xz-wy)&2(yz+wx)&1-2(x^2+y^2)}$$

$$\pmatrix{\phi \\ \theta \\ \psi} = \pmatrix{\tan^{-1}(\frac{q_{32}}{q_{33}}) \\ \sin^{-1}(-q_{31}) \\ \tan^{-1}(\frac{q_{21}}{q_{11}})} = \pmatrix{\tan^{-1}(\frac{2(yz+wx)}{1-2(x^2+y^2)}) \\ \sin^{-1}(2(xz-wy)) \\ \tan^{-1}(\frac{2(xy+wz)}{1-2(y^2+z^2)})}$$

由上式便可將四元數轉換回歐拉角。

## 參考資料

[1] Conversion between quaternions and Euler angles, Wikipedia