剛體姿態運動學03 旋轉矩陣與坐標轉換

旋轉矩陣（Spatial Rotation Matrix）

簡單回顧上一篇推導的羅德里格旋轉公式（Rodrigues’ rotation formula）。
其功能在於：給定一個自由向量 $\boldsymbol{\rho}_1$，以及轉動軸 $\mathbf{u}$ 與角度 $\phi$，即可求得以其末端為中心旋轉後的新向量 $\boldsymbol{\rho}_2$：

\[\boldsymbol{\rho}_2 = (\boldsymbol{\rho}_1\cdot\mathbf{u})\mathbf{u}(1-\cos\phi) + \cos\phi\,\boldsymbol{\rho}_1 + \sin\phi\,\mathbf{u}\times\boldsymbol{\rho}_1 \tag{1}\]

本篇將羅德里格旋轉公式改寫為矩陣形式，使其可表示為

\[\boldsymbol{\rho}_2 = \mathbf{R}(\mathbf{u},\phi)\boldsymbol{\rho}_1\]

如此一來，透過矩陣乘法即可大幅簡化計算流程。

---

向量外積的矩陣表示

考慮兩向量

\[\mathbf{a} = \begin{bmatrix} a_x & a_y & a_z \end{bmatrix}^T,\quad \mathbf{b} = \begin{bmatrix} b_x & b_y & b_z \end{bmatrix}^T\]

其外積可寫為：

\[\begin{aligned} \mathbf{a}\times\mathbf{b} &= \begin{vmatrix} \mathbf{i} & \mathbf{j} & \mathbf{k} \\ a_x & a_y & a_z \\ b_x & b_y & b_z \end{vmatrix} \\ &= \begin{bmatrix} a_y b_z - a_z b_y \\ b_x a_z - b_z a_x \\ b_y a_x - b_x a_y \end{bmatrix} \\ &= \begin{bmatrix} 0 & -a_z & a_y \\ a_z & 0 & -a_x \\ -a_y & a_x & 0 \end{bmatrix} \begin{bmatrix} b_x \\ b_y \\ b_z \end{bmatrix} = [\mathbf{a}\times]\mathbf{b} \end{aligned} \tag{2a}\]

其中 $[\mathbf{a}\times]$ 為由 $\mathbf{a}$ 生成的反對稱矩陣。

---

向量內積的矩陣表示

向量內積亦可表示為矩陣乘法：

\[\begin{aligned} (\mathbf{a}\cdot\mathbf{b})\mathbf{b} &= \mathbf{b}(\mathbf{b}^T\mathbf{a}) = (\mathbf{b}\mathbf{b}^T)\mathbf{a} \\ &= \begin{bmatrix} b_x^2 & b_x b_y & b_x b_z \\ b_y b_x & b_y^2 & b_y b_z \\ b_z b_x & b_z b_y & b_z^2 \end{bmatrix} \begin{bmatrix} a_x \\ a_y \\ a_z \end{bmatrix} \end{aligned} \tag{2b}\]

---

羅德里格公式的矩陣形式

利用式 (2a)、(2b)，可將式 (1) 改寫為：

\[\boldsymbol{\rho}_2 = \mathbf{u}\mathbf{u}^T\boldsymbol{\rho}_1(1-\cos\phi) + \mathbf{I}\boldsymbol{\rho}_1\cos\phi + [\mathbf{u}\times]\boldsymbol{\rho}_1\sin\phi\]

整理後得到：

\[\boldsymbol{\rho}_2 = \underbrace{ \begin{bmatrix} u_x^2(1-\cos\phi)+\cos\phi & u_xu_y(1-\cos\phi)-u_z\sin\phi & u_xu_z(1-\cos\phi)+u_y\sin\phi \\ u_yu_x(1-\cos\phi)+u_z\sin\phi & u_y^2(1-\cos\phi)+\cos\phi & u_yu_z(1-\cos\phi)-u_x\sin\phi \\ u_zu_x(1-\cos\phi)-u_y\sin\phi & u_zu_y(1-\cos\phi)+u_x\sin\phi & u_z^2(1-\cos\phi)+\cos\phi \end{bmatrix} }_{\mathbf{R}(\mathbf{u},\phi)} \boldsymbol{\rho}_1\]

因此可簡寫為：

\[\boldsymbol{\rho}_2 = \mathbf{R}(\mathbf{u},\phi)\boldsymbol{\rho}_1 \tag{3}\]

其中 $\mathbf{R}(\mathbf{u},\phi)\in\mathbb{R}^{3\times3}$ 稱為空間旋轉矩陣（Spatial Rotation Matrix）。

---

旋轉矩陣的特性

特性 1：正交性

\[\mathbf{R}^{-1}(\mathbf{u},\phi) = \mathbf{R}(\mathbf{u},-\phi) = \mathbf{R}^T(\mathbf{u},\phi)\]

---

特性 2：列向量正交

若將旋轉矩陣三個列向量定義為 $\mathbf{n}_1,\mathbf{n}_2,\mathbf{n}_3$，則：

\[\mathbf{n}_i\cdot\mathbf{n}_j= \begin{cases} 0,& i\neq j \\ 1,& i=j \end{cases}\]

且滿足右手定則：

\[\mathbf{n}_1\times\mathbf{n}_2=\mathbf{n}_3,\quad \mathbf{n}_2\times\mathbf{n}_3=\mathbf{n}_1,\quad \mathbf{n}_3\times\mathbf{n}_1=\mathbf{n}_2\]

---

坐標轉換（Coordinate Transformation）

自由向量不依附於坐標系。
若兩坐標系僅存在旋轉關係，則可使用旋轉矩陣進行向量轉換。

設 $R_b$ 由 $R_0$ 繞 $\mathbf{u}$ 旋轉 $\phi$ 而得，則：

\[\mathbf{w}_0 = \mathbf{R}(\mathbf{u},\phi)\mathbf{w}_b\]

因此：

\[\mathbf{w}_b = \mathbf{R}^T(\mathbf{u},\phi)\mathbf{w}_0\]

亦可簡記為：

\[\mathbf{w}_b = \mathbf{R}_0^b\mathbf{w}_0\]

---

參考資料

1. 林昱廷，
   飛彈姿態運動之適應控制研究，
   碩士論文，機械工程學系，國立臺灣科技大學，2021。