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

tags: articles

作者: 鄭聖文

E-mail: [e-mail]

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

前言

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

歐拉角及旋轉矩陣

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

給出沿著

$x$軸旋轉

$\varphi$度的轉換:

$\left(\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ z^{\prime }\end{array}\right)=\left(\begin{array}{ccc}1& 0& 0\\ 0& cos\varphi & -sin\varphi \\ 0& sin\varphi & cos\varphi \end{array}\right)\left(\begin{array}{c}x\\ y\\ z\end{array}\right)$        

給出沿著

$y$軸旋轉

$\theta$度的轉換:

$\left(\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ z^{\prime }\end{array}\right)=\left(\begin{array}{ccc}cos\theta & 0& sin\theta \\ 0& 1& 0\\ -sin\theta & 0& cos\theta \end{array}\right)\left(\begin{array}{c}x\\ y\\ z\end{array}\right)$        

給出沿著

$z$軸旋轉

$\psi$度的轉換:

$\left(\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ z^{\prime }\end{array}\right)=\left(\begin{array}{ccc}cos\psi & -sin\psi & 0\\ sin\psi & cos\psi & 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{c}x\\ y\\ z\end{array}\right)$        

分別列出上面三組所使用到的轉換矩陣

$R_x$,

$R_y$以及

$R_z$, 我們以下標表示旋轉軸:

$$R_x=\left(\begin{array}{ccc}1& 0& 0\\ 0& cos\varphi & -sin\varphi \\ 0& sin\varphi & cos\varphi \end{array}\right)$$

$$R_y=\left(\begin{array}{ccc}cos\theta & 0& sin\theta \\ 0& 1& 0\\ -sin\theta & 0& cos\theta \end{array}\right)$$

$$R_z=\left(\begin{array}{ccc}cos\psi & -sin\psi & 0\\ sin\psi & cos\psi & 0\\ 0& 0& 1\end{array}\right)$$

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

$$\left(\begin{array}{c}{x}_i\\ y_i\\ z_i\end{array}\right)=R_z{R}_y{R}_x\left(\begin{array}{c}{x}_b\\ y_b\\ z_b\end{array}\right)=R_b^i\left(\begin{array}{c}{x}_b\\ y_b\\ z_b\end{array}\right)$$

我們以

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

$$R_b^i=R_z{R}_y{R}_x$$

$$=\left(\begin{array}{ccc}\cos \psi \cos \theta & -\sin \psi \cos \varphi +\cos \psi \sin \theta \sin \varphi & \sin \psi \sin \varphi +\cos \psi \cos \varphi \sin \theta \\ \sin \psi \cos \theta & \cos \psi \cos \varphi +\sin \varphi \sin \theta \sin \psi & -\cos \psi \sin \varphi +\sin \theta \sin \psi \cos \varphi \\ -\sin \theta & \cos \theta \sin \varphi & cos\theta \cos \varphi \end{array}\right)$$

旋轉矩陣並不具有交換律，以上我們以

$(x-y-z)$的順序旋轉。

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

$$\left(\begin{array}{c}\varphi \\ \theta \\ \psi \end{array}\right)=\left(\begin{array}{c}{\tan }^{-1}(\frac{r_{32}}{r_{33}})\\ {\sin }^{-1}(-r_{31})\\ {\tan }^{-1}(\frac{r_{21}}{r_{11}})\end{array}\right)=\left(\begin{array}{c}{\tan }^{-1}(\frac{\cos \theta \sin \varphi }{cos\theta \cos \varphi })\\ {\sin }^{-1}(-(-\sin \theta ))\\ {\tan }^{-1}(\frac{\sin \psi \cos \theta }{\cos \psi \cos \theta })\end{array}\right)$$

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

$$\left(\begin{array}{c}{x}_b\\ y_b\\ z_b\end{array}\right)=R_x^{-1}{R}_y^{-1}{R}_z^{-1}\left(\begin{array}{c}{x}_i\\ y_i\\ z_i\end{array}\right)=R_i^b\left(\begin{array}{c}{x}_i\\ y_i\\ z_i\end{array}\right)$$

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

$$R_i^b=R_x^{-1}{R}_y^{-1}{R}_z^{-1}=R_x^{\mathrm{\top }}{R}_y^{\mathrm{\top }}{R}_z^{\mathrm{\top }},sinceR\in SO(3)$$

定義四元數之基本運算

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

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

$$q\triangleq w+xi+yj+zk=\left(\begin{array}{c}x\\ y\\ z\\ w\end{array}\right)$$

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

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

以及

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

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

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

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

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

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

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

接著定義乘法運算:

$$q_1{q}_2=(w_1+x_{1}i+y_{1}j+z_{1}k)(w_2+x_{2}i+y_{2}j+z_{2}k)=$$

$$(w_1{w}_2-x_1{x}_2-y_1{y}_2-z_1{z}_2)+$$

$$(w_1{x}_2+x_1{w}_2+y_1{z}_2-z_1{y}_2)i+$$

$$(w_1{y}_2+x_1{z}_2+y_1{w}_2+z_1{x}_2)j+$$

$$(w_1{z}_2+x_1{y}_2-y_1{x}_2+z_1{w}_2)k$$

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

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

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

因此乘法可寫作:

$$q_1{q}_2=s_1{s}_2-\overrightarrow{v_1}\overrightarrow{v_2}+s_1\overrightarrow{v_2}+s_2\overrightarrow{v_1}+\overrightarrow{v_1}\times \overrightarrow{v_2}$$

上式的

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

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

$$\left(\begin{array}{c}x\\ y\\ z\\ w\end{array}\right)=\left(\begin{array}{cccc}{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\end{array}\right)\left(\begin{array}{c}{x}_2\\ y_2\\ z_2\\ w_2\end{array}\right)$$

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

$$q^{\ast }=w-xi-yj-zk$$

而四元數的長度為:

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

並且由於:

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

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

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

而當四元數為單位四元數時(

$\sqrt{w^2+x^2+y^2+z^2}=1$)，乘法逆運算等於共軛值:

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

四元數的空間旋轉

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

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

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

$$q_{space}=\left(\begin{array}{c}x\\ y\\ z\\ 0\end{array}\right)$$

則下式可以將點

$q_{space}$相對原點旋轉

$\theta$度:

$$q_{space}^{{}^{\prime }}=q{q}_{space}{q}^{-1}$$

而

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

$q{q}_{space}{q}^{-1}$可以想像為一四度空間上的旋轉，該運算除了會使原座標旋轉

$\theta$度之外，也會在第四維空間上產生一位移，因此除了一次運算(

$q$)外，可以另一次逆運算(

$q^{-1}$)消除空間上的位移，但兩次運算皆會使座標旋轉

$\theta$度，使最終旋轉的度數為

$2\theta$度，因此我們必須選定原先的

$\theta$角為

$\frac{\theta }{2}$度。

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

$$\left(\begin{array}{c}{x}^{"}\\ y^{"}\\ z^{"}\\ w^{"}\end{array}\right)=\left(\begin{array}{cccc}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\end{array}\right)\left(\begin{array}{c}{x}^{{}^{\prime }}\\ y^{{}^{\prime }}\\ z^{{}^{\prime }}\\ w^{{}^{\prime }}\end{array}\right)$$

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

$$\left(\begin{array}{c}{x}^{"}\\ y^{"}\\ z^{"}\end{array}\right)=\left(\begin{array}{ccc}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)\end{array}\right)\left(\begin{array}{c}{x}^{{}^{\prime }}\\ y^{{}^{\prime }}\\ z^{{}^{\prime }}\end{array}\right)$$

接著擴展

$q=\cos (\frac{\theta }{2})+\sin (\frac{\theta }{2})\overrightarrow{n}$使得我們可以一次描述

$(x-y-z)$三軸的旋轉:

$$q=q_z{q}_y{q}_x=(\cos (\frac{\psi }{2})+\sin (\frac{\psi }{2})\overrightarrow{k})(\cos (\frac{\theta }{2})+\sin (\frac{\theta }{2})\overrightarrow{j})(\cos (\frac{\varphi }{2})+\sin (\frac{\varphi }{2})\overrightarrow{i})$$

$$\to \left(\begin{array}{c}x\\ y\\ z\\ w\end{array}\right)=\left(\begin{array}{c}\sin (\frac{\varphi }{2})\cos (\frac{\theta }{2})cos(\frac{\psi }{2})-\cos (\frac{\varphi }{2})\sin (\frac{\theta }{2})\sin (\frac{\psi }{2})\\ \cos (\frac{\varphi }{2})\sin (\frac{\theta }{2})cos(\frac{\psi }{2})+\sin (\frac{\varphi }{2})\cos (\frac{\theta }{2})\sin (\frac{\psi }{2})\\ \cos (\frac{\varphi }{2})\cos (\frac{\theta }{2})sin(\frac{\psi }{2})-\sin (\frac{\varphi }{2})\sin (\frac{\theta }{2})\cos (\frac{\psi }{2})\\ \cos (\frac{\varphi }{2})\cos (\frac{\theta }{2})cos(\frac{\psi }{2})+\sin (\frac{\varphi }{2})\sin (\frac{\theta }{2})\sin (\frac{\psi }{2})\end{array}\right)$$

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

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

$$\left(\begin{array}{ccc}\cos \psi \cos \theta & -\sin \psi \cos \varphi +\cos \psi \sin \theta \sin \varphi & \sin \psi \sin \varphi +\cos \psi \cos \varphi \sin \theta \\ \sin \psi \cos \theta & \cos \psi \cos \varphi +\sin \varphi \sin \theta \sin \psi & -\cos \psi \sin \varphi +\sin \theta \sin \psi \cos \varphi \\ -\sin \theta & \cos \theta \sin \varphi & cos\theta \cos \varphi \end{array}\right)$$

$$\left(\begin{array}{c}\varphi \\ \theta \\ \psi \end{array}\right)=\left(\begin{array}{c}{\tan }^{-1}(\frac{r_{32}}{r_{33}})\\ {\sin }^{-1}(-r_{31})\\ {\tan }^{-1}(\frac{r_{21}}{r_{11}})\end{array}\right)=\left(\begin{array}{c}{\tan }^{-1}(\frac{\cos \theta \sin \varphi }{cos\theta \cos \varphi })\\ {\sin }^{-1}(-(-\sin \theta ))\\ {\tan }^{-1}(\frac{\sin \psi \cos \theta }{\cos \psi \cos \theta })\end{array}\right)$$

可以給出:

$$\left(\begin{array}{ccc}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)\end{array}\right)$$

$$\left(\begin{array}{c}\varphi \\ \theta \\ \psi \end{array}\right)=\left(\begin{array}{c}{\tan }^{-1}(\frac{q_{32}}{q_{33}})\\ {\sin }^{-1}(-q_{31})\\ {\tan }^{-1}(\frac{q_{21}}{q_{11}})\end{array}\right)=\left(\begin{array}{c}{\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)})\end{array}\right)$$

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

參考資料

[1] Conversion between quaternions and Euler angles, Wikipedia