剛體在三維空間轉動的姿態確定之四元數法

阿新 • • 發佈：2019-01-01

最近做三維空間的機器人直立控制，遇到如何根據三軸加速度計和三軸陀螺儀各自三軸測得的分量值確定機器人姿態的問題。

         經過幾天的查閱文獻和思考，姑且將機器人直立控制分為姿態確定和運動控制兩部分。運動控制可以細分為直立控制、速度控制和方向控制。目前速度控制和方向控制都不成問題，難點就在與直立控制。不過由於之前模型建立的比較精準，加上速度和方向控制都已經調好，所以直立控制部分剩餘的主要工作量就是姿態確定問題。

目前先研究一下四元數法。四元數的三引數法選用的是尤拉角，這裡先貼一些尤拉角的資料。都是從網上搜集的資料。

Computer Graphics: 四元數與旋轉

SoRoMan：探討：物體繞任意向量的旋轉-四元數法VS.旋轉矩陣法的效能比較

基維百科：

萊昂哈德·尤拉用尤拉角來描述剛體在三維歐幾里得空間的取向。對於任何一個參考系，一個剛體的取向，是依照順序，從這參考系，做三個尤拉角的旋轉而設定的。所以，剛體的取向可以用三個基本旋轉矩陣來決定。換句話說，任何關於剛體旋轉的旋轉矩陣是由三個基本旋轉矩陣複合而成的。

靜態的定義

三個尤拉角： ( $\alpha,\ \beta,\ \gamma$ ) 。藍色的軸是 xyz-軸，紅色的軸是 XYZ-座標軸。綠色的線是交點線 (N) 。

對於在三維空間裡的一個參考系，任何座標系的取向，都可以用三個尤拉角來表現。參考系又稱為實驗室參考系，是靜止不動的。而座標系則固定於剛體，隨著剛體的旋轉而旋轉。

參閱右圖。設定 xyz-軸為參考系的參考軸。稱 xy-平面與 XY-平面的相交為交點線，用英文字母（Ｎ）代表。zxz 順規的尤拉角可以靜態地這樣定義:

$\alpha$ 是 x-軸與交點線的夾角，
$\beta$ 是 z-軸與Z-軸的夾角，
$\gamma$ 是交點線與X-軸的夾角。

很可惜地，對於夾角的順序和標記，夾角的兩個軸的指定，並沒有任何常規。科學家對此從未達成共識。每當用到尤拉角時，我們必須明確的表示出夾角的順序，指定其參考軸。

實際上，有許多方法可以設定兩個座標系的相對取向。尤拉角方法只是其中的一種。此外，不同的作者會用不同組合的尤拉角來描述，或用不同的名字表示同樣的尤拉角。因此，使用尤拉角前，必須先做好明確的定義。

[編輯]角值範圍

$\alpha,\ \gamma$ 值從 0 至 $2\pi$ 弧度。
$\beta$ 值從 0 至 $\pi$ 弧度。

對應於每一個取向，設定的一組尤拉角都是獨特唯一的；除了某些例外：

兩組尤拉角的 $\alpha$ ，一個是 0 ，一個是 $2\pi$ ，而 $\beta$ 與 $\gamma$ 分別相等，則此兩組尤拉角都描述同樣的取向。

兩組尤拉角的 $\gamma$ ，一個是 0 ，一個是 $2\pi$ ，而 $\alpha$ 與 $\beta$ 分別相等，則此兩組尤拉角都描述同樣的取向。

[編輯]旋轉矩陣

前面提到，設定剛體取向的旋轉矩陣 $[\mathbf{R}]$ 是由三個基本旋轉矩陣合成的：

$[\mathbf{R}] = \begin{bmatrix}\cos \gamma & \sin \gamma & 0 \\-\sin \gamma & \cos \gamma & 0 \\0 & 0 & 1 \end{bmatrix} \begin{bmatrix}1 & 0 & 0 \\0 & \cos \beta & \sin \beta \\0 & -\sin \beta & \cos \beta \end{bmatrix} \begin{bmatrix}\cos \alpha & \sin \alpha & 0 \\-\sin \alpha & \cos \alpha & 0 \\0 & 0 & 1 \end{bmatrix}$

單獨分開作用，每個矩陣各自代表繞著其轉動軸的旋轉；但是，當它們照次序相乘，

最裡面的(最右的) 矩陣代表繞著 z 軸的旋轉。
最外面的(最左的) 矩陣代表繞著 Z 軸的旋轉。
在中間的矩陣代表繞著交點線的旋轉。

經過一番運算,

$[\mathbf{R}] = \begin{bmatrix}\cos\alpha\cos\gamma-\cos\beta\sin\alpha\sin\gamma & \sin\alpha\cos\gamma+\cos\beta\cos\alpha\sin\gamma & \sin\beta\sin\gamma\\-\cos\alpha\sin\gamma-\cos\beta\sin\alpha\cos\gamma & -\sin\alpha\sin\gamma+\cos\beta\cos\alpha\cos\gamma & \sin\beta\cos\gamma \\ \sin\beta\sin\alpha & -\sin\beta\cos\alpha & \cos\beta \end{bmatrix}$

$[\mathbf{R}]$ 的逆矩陣是：

$[\mathbf{R}]^{-1}= \begin{bmatrix}\cos\alpha\cos\gamma-\cos\beta\sin\alpha\sin\gamma & -\cos\alpha\sin\gamma-\cos\beta\sin\alpha\cos\gamma & \sin\beta\sin\alpha\\ \sin\alpha\cos\gamma+\cos\beta\cos\alpha\sin\gamma & -\sin\alpha\sin\gamma+\cos\beta\cos\alpha\cos\gamma & -\sin\beta\cos\alpha\\ \sin\beta\sin\gamma & \sin\beta\cos\gamma & \cos\beta \end{bmatrix}$

[編輯]別種順序

在經典力學裡，時常用 zxz 順規來設定尤拉角；照著第二個轉動軸的軸名，簡稱為 x 順規。另外，還有別種尤拉角組。合法的尤拉角組中，唯一的限制是，任何兩個連續的旋轉，必須繞著不同的轉動軸旋轉。因此，一共有 12 種順規。例如，y 順規，第二個轉動軸是 y-軸，時常用在量子力學，核子物理學，粒子物理學。另外，還有一種順規，xyz 順規，是用在航空航天工程學；參閱Tait-Bryan angles。

[編輯]動態的定義

我們也可以給予尤拉角兩種不同的動態定義。一種是繞著固定於剛體的座標軸的三個旋轉的複合；另外一種是繞著實驗室參考軸的三個旋轉的複合。用動態的定義，我們能更瞭解，尤拉角在物理上的含義與應用。特別注意，以下的描述, XYZ 座標軸是旋轉的剛體座標軸；而 xyz 座標軸是靜止不動的實驗室參考軸。

A) 繞著 XYZ 座標軸旋轉：最初，兩個座標系統 xyz 與 XYZ 的座標軸都是重疊著的。開始先繞著 Z-軸旋轉 $\alpha\,$ 角值。然後，繞著 X-軸旋轉 $\beta\,$ 角值。最後，繞著 Z-軸作角值 $\gamma\,$ 的旋轉。

B) 繞著 xyz 座標軸旋轉：最初，兩個座標系統 xyz 與 XYZ 的座標軸都是重疊著的。開始先繞著 z-軸旋轉 $\gamma\,$ 角值。然後，繞著 x-軸旋轉 $\beta\,$ 角值。最後，繞著 z-軸作角值 $\alpha\,$ 的旋轉。

參閱尤拉角圖，定義 A 與靜態定義的相等，這可以直接用幾何製圖方法來核對。

定義 A 與定義 B 的相等可以用旋轉矩陣來證明：

思考任何一點 $P_1\,$ ，在 xyz 與 XYZ 座標系統的座標分別為 $\mathbf{r}_1\,$ 與 $\mathbf{R}_1\,$ 。定義角算符 $Z(\alpha)\,$ 為繞著 Z-軸旋轉 $\alpha\,$ 角值。那麼，定義 A 可以表述如下:

$\mathbf{R}_1=Z(\gamma)\circ X(\beta)\circ Z(\alpha)\circ \mathbf{r}_1\,$ 。

用旋轉矩陣表示,

$Z(\alpha)= \begin{bmatrix}\cos \alpha & \sin \alpha & 0 \\-\sin \alpha & \cos \alpha & 0 \\0 & 0 & 1 \end{bmatrix}\,$ ，

$X(\beta)=\begin{bmatrix}1 & 0 & 0 \\0 & \cos \beta & \sin \beta \\0 & -\sin \beta & \cos \beta \end{bmatrix} \,$ ，

$Z(\gamma)=\begin{bmatrix}\cos \gamma & \sin \gamma & 0 \\-\sin \gamma & \cos \gamma & 0 \\0 & 0 & 1 \end{bmatrix}\,$ 。

思考任何一點 $P_2\,$ ，在 xyz 與 XYZ 座標系統的座標分別為 $\mathbf{r}_2\,$ 與 $\mathbf{R}_2\,$ 。定義角算符 $z(\alpha)\,$ 為繞著 z-軸旋轉 $\alpha\,$ 角值。則定義 B 可以表述如下:

$\mathbf{r}_2=z(\alpha)\circ x(\beta)\circ z(\gamma)\circ \mathbf{R}_2\,$ 。

用旋轉矩陣表示,

$z(\alpha)= \begin{bmatrix}\cos \alpha & -\sin \alpha & 0 \\\sin \alpha & \cos \alpha & 0 \\0 & 0 & 1 \end{bmatrix}\,$ ，

$x(\beta)=\begin{bmatrix}1 & 0 & 0 \\0 & \cos \beta & -\sin \beta \\0 & \sin \beta & \cos \beta \end{bmatrix} \,$ ，

$z(\gamma)=\begin{bmatrix}\cos \gamma & -\sin \gamma & 0 \\\sin \gamma & \cos \gamma & 0 \\0 & 0 & 1 \end{bmatrix}\,$ 。

假設， $\mathbf{r}_1=\mathbf{r}_2\,$ ．那麼，

$\mathbf{r}_1=z(\alpha)\circ x(\beta)\circ z(\gamma)\circ \mathbf{R}_2\,$ 。

乘以逆算符，

$z^{-1}(\gamma)\circ x^{-1}(\beta)\circ z^{-1}(\alpha)\circ \mathbf{r}_1=z^{-1}(\gamma)\circ x^{-1}(\beta)\circ z^{-1}(\alpha)\circ z(\alpha)\circ x(\beta)\circ z(\gamma)\circ \mathbf{R}_2\,$ 。

但是，從旋轉矩陣可以觀察出，

$z^{-1}(\alpha)=Z(\alpha)\,$ ，

$x^{-1}(\beta)=X(\beta)\,$ ，

$z^{-1}(\gamma)=Z(\gamma)\,$ 。

所以，

$Z(\gamma)\circ X(\beta)\circ Z(\alpha)\circ \mathbf{r}_1=\mathbf{R}_2\,$ ，

$\mathbf{R}_1=\mathbf{R}_2\,$ 。

定義 A 與定義 B 是相等的。

[編輯]尤拉角性質

尤拉角在SO(3)上，形成了一個座標卡(chart) ；SO(3)是在三維空間裡的旋轉的特殊正交群。這座標卡是平滑的，除了一個極座標式的奇點在 $\beta=0$ 　。

類似的三個角的分解也可以應用到SU(2)；複數二維空間裡旋轉的特殊酉群；這裡， $\beta$ 值在０與 $2\pi$ 之間。這些角也稱為尤拉角。

[編輯]應用

尤拉角廣泛地被應用於經典力學中的剛體研究，與量子力學中的角動量研究。

在剛體的問題上, xyz 座標系是全域性座標系， XYZ 座標系是區域性座標系。全域性座標系是不動的；而區域性座標系牢嵌於剛體內。關於動能的演算，通常用區域性座標系比較簡易；因為，慣性張量不隨時間而改變。如果將慣性張量（有九個分量，其中六個是獨立的）對角線化，那麼，會得到一組主軸，以及一個轉動慣量（只有三個分量）。

在量子力學裡，詳盡的描述SO(3)的形式，對於精準的演算，是非常重要的，並且幾乎所有研究都採用尤拉角為工具。在早期的量子力學研究，對於抽象群理論方法(稱為Gruppenpest)，物理學家與化學家仍舊持有極尖銳的反對態度的時候；對尤拉角的信賴，在基本理論研究來說，是必要的。

尤拉角的哈爾測度有一個簡單的形式 $\sin\beta\ d\alpha d\beta d\gamma$ 通常在前面添上歸一化因子 $1/8\pi^2$ 。例如，我們要生成均勻隨機取向，使 $\alpha, \ \gamma$ 從 $0$ 至 $2\pi$ 均勻的選隨機值；使 $\beta=\arccos(z)$ ， $z$ 從 $-1$ 至 $1$ 均勻的選隨機值。

單位四元數，又稱尤拉引數，提供另外一種方法來表述三維旋轉。這與特殊酉群的描述是等價的。四元數方法用在大多數的演算會比較快捷，概念上比較容易理解，並能避免一些技術上的問題，如萬向節鎖 (gimbal lock) 現象。因為這些原因，許多高速度三維圖形程式製作都使用四元數。

剛體在三維空間轉動的姿態確定之四元數法

Computer Graphics: 四元數與旋轉

SoRoMan：探討：物體繞任意向量的旋轉-四元數法VS.旋轉矩陣法的效能比較