前の記事で見たように、オイラー角は直感的ですが「ジンバルロック」という致命的な特異点を持ちます。3つのパラメータで3次元回転（SO(3)）を覆おうとする限り、この特異点は避けられません。
では、4つのパラメータを使えばどうでしょうか。クォータニオン（四元数） は、4つの実数を1組として回転を表す手法で、特異点が一切存在しません。1843年にウィリアム・ローワン・ハミルトンがダブリンのブルーム橋を渡っているときにひらめいた伝説を持つこの数学的道具は、今や宇宙工学・ゲーム開発・ロボティクスの標準的な姿勢表現となっています。
クォータニオンを理解すると、以下の応用に直結します。
- 人工衛星の姿勢制御: 大角度マヌーバでも特異点なく姿勢を追跡できる
- 3DCGとゲーム開発: 姿勢の滑らかな補間（SLERP）でキャラクターの動きを自然に表現する
- ロボットの経路計画: 姿勢空間で最短経路を計算できる
- 慣性航法装置: ジャイロデータの積分で姿勢を更新する際に数値的に安定
本記事の内容
- 四元数の定義と基本演算
- 単位クォータニオンと回転の対応（$q v q^*$ の導出）
- オイラー角・回転行列との相互変換
- 球面線形補間（SLERP）
- クォータニオンの時間微分（角速度との関係）
- Pythonでの実装と可視化
前提知識
この記事を読む前に、以下の記事を読んでおくと理解が深まります。
四元数とは何か
複素数 $a + bi$ が2次元の回転を扱うのに便利であるように、四元数は 3次元の回転を扱うための数体系 です。
複素数は実部と虚部の2つの成分を持ちますが、ハミルトンは「3成分に拡張すれば3次元回転に使えるのでは」と何年も考え続けました。しかし3成分では矛盾が生じ、最終的に虚部を3つに拡張した 4成分 の体系が必要だと気づいたのです。
四元数 $q$ は、1つの実部と3つの虚部からなります。
$$ q = q_0 + q_1 \bm{i} + q_2 \bm{j} + q_3 \bm{k} $$
ここで $q_0$ がスカラー部、$(q_1, q_2, q_3)$ がベクトル部です。$\bm{i}, \bm{j}, \bm{k}$ は虚数単位で、以下の関係式（ハミルトンの公式）を満たします。
$$ \bm{i}^2 = \bm{j}^2 = \bm{k}^2 = \bm{i}\bm{j}\bm{k} = -1 $$
この関係式から、次の乗算規則が導かれます。
$$ \bm{i}\bm{j} = \bm{k}, \quad \bm{j}\bm{k} = \bm{i}, \quad \bm{k}\bm{i} = \bm{j} $$
逆順では符号が反転します。
$$ \bm{j}\bm{i} = -\bm{k}, \quad \bm{k}\bm{j} = -\bm{i}, \quad \bm{i}\bm{k} = -\bm{j} $$
乗算が非可換（順序を入れ替えると結果が変わる）であることに注意してください。これは3次元の回転が非可換であることと対応しています。
四元数はスカラー部とベクトル部を分けて次のようにも書きます。
$$ q = (q_0, \bm{q}_v) \quad \text{where} \quad \bm{q}_v = (q_1, q_2, q_3)^T $$
この記法を使うと、四元数の演算がベクトルの内積・外積で表現できて便利です。
四元数の定義と基本的な性質を確認しました。次に、四元数同士の演算規則を整理しましょう。
四元数の基本演算
加算
四元数の加算は成分ごとに行います。
$$ p + q = (p_0 + q_0, \; \bm{p}_v + \bm{q}_v) $$
乗算
四元数の乗算は、分配法則と虚数単位の関係式 $\bm{i}\bm{j} = \bm{k}$ 等を使って展開します。結果はベクトル演算で簡潔に書けます。
$$ pq = (p_0 q_0 – \bm{p}_v \cdot \bm{q}_v, \; p_0 \bm{q}_v + q_0 \bm{p}_v + \bm{p}_v \times \bm{q}_v) $$
右辺のスカラー部は $p_0 q_0$ からベクトル部同士の内積を引いたもの、ベクトル部は2つのスカラー・ベクトル積に外積を加えたものです。
外積 $\bm{p}_v \times \bm{q}_v$ が含まれるため、一般に $pq \neq qp$ です。外積の反対称性 $\bm{p}_v \times \bm{q}_v = -\bm{q}_v \times \bm{p}_v$ から、$pq$ と $qp$ の差はベクトル部の外積の2倍分だけ異なります。
共役
四元数の共役 $q^*$ は、ベクトル部の符号を反転させたものです。
$$ q^* = (q_0, -\bm{q}_v) = q_0 – q_1\bm{i} – q_2\bm{j} – q_3\bm{k} $$
複素数の共役 $\overline{a+bi} = a – bi$ の自然な拡張です。
重要な性質として、積の共役は 順序が逆転 します。
$$ (pq)^* = q^* p^* $$
ノルム
四元数のノルム（絶対値）は、
$$ \|q\| = \sqrt{q q^*} = \sqrt{q_0^2 + q_1^2 + q_2^2 + q_3^2} $$
$q q^*$ を乗算公式に代入して確認しましょう。$q = (q_0, \bm{q}_v)$ と $q^* = (q_0, -\bm{q}_v)$ の積は、
$$ q q^* = (q_0^2 + \bm{q}_v \cdot \bm{q}_v, \; q_0(-\bm{q}_v) + q_0 \bm{q}_v + \bm{q}_v \times (-\bm{q}_v)) $$
ベクトル部は $-q_0 \bm{q}_v + q_0 \bm{q}_v + \bm{0} = \bm{0}$ で消え、スカラー部は $q_0^2 + \|\bm{q}_v\|^2$ となります。確かに正の実数で、ノルムの2乗に一致します。
逆元
ノルムがゼロでない四元数 $q$ の逆元は、
$$ q^{-1} = \frac{q^*}{\|q\|^2} $$
$q q^{-1} = q \cdot q^* / \|q\|^2 = \|q\|^2 / \|q\|^2 = 1$ となることが確認できます。
特に 単位四元数（$\|q\| = 1$）の場合は、
$$ q^{-1} = q^* $$
と非常に簡単です。回転行列で「逆行列 = 転置」だったのと同様、単位四元数では「逆元 = 共役」です。
四元数の演算体系を整理したところで、いよいよ本題に入ります。単位四元数と3次元回転はどのように対応するのでしょうか。
単位クォータニオンと回転
回転の表現
軸 $\hat{\bm{e}}$（単位ベクトル）まわりに角度 $\Phi$ だけ回転する操作は、次の単位クォータニオンで表されます。
$$ q = \cos\frac{\Phi}{2} + \sin\frac{\Phi}{2}(\hat{e}_1 \bm{i} + \hat{e}_2 \bm{j} + \hat{e}_3 \bm{k}) $$
スカラー部・ベクトル部の記法では、
$$ q = \left(\cos\frac{\Phi}{2}, \; \sin\frac{\Phi}{2}\hat{\bm{e}}\right) $$
なぜ角度が半分なのかは、次に説明する回転公式 $qvq^*$ で $q$ が2回現れることに起因します。$q$ と $q^*$ の両方が角度 $\Phi/2$ を含み、合計で $\Phi$ の回転を生み出すのです。
ノルムを確認すると、
$$ \|q\| = \sqrt{\cos^2\frac{\Phi}{2} + \sin^2\frac{\Phi}{2}\|\hat{\bm{e}}\|^2} = \sqrt{\cos^2\frac{\Phi}{2} + \sin^2\frac{\Phi}{2}} = 1 $$
確かに単位クォータニオンです。
回転公式 $q v q^*$
3次元ベクトル $\bm{v}$ を回転するには、$\bm{v}$ を純粋四元数 $v = (0, \bm{v})$ と見なし、次の計算を行います。
$$ v’ = q \, v \, q^* $$
結果 $v’$ も純粋四元数で、そのベクトル部が回転後のベクトルです。
この公式がなぜ回転を表すのか、簡単な場合で確認しましょう。$z$ 軸まわりに角度 $\Phi$ の回転を考えます。$\hat{\bm{e}} = (0, 0, 1)^T$ なので、
$$ q = \left(\cos\frac{\Phi}{2}, \; 0, \; 0, \; \sin\frac{\Phi}{2}\right) $$
$v = (0, x, y, 0)$（$xy$ 平面内のベクトル）に対して $qvq^*$ を計算すると、ベクトル部は、
$$ \bm{v}’ = \begin{pmatrix} x\cos\Phi – y\sin\Phi \\ x\sin\Phi + y\cos\Phi \\ 0 \end{pmatrix} $$
これは $z$ 軸まわりに角度 $\Phi$ の回転そのものです。回転行列 $R_z(\Phi)$ と同じ結果が得られます。
$q$ と $-q$ の等価性
回転公式 $v’ = qvq^*$ において、$q$ を $-q$ に置き換えると、
$$ (-q) v (-q)^* = (-q) v (-q^*) = q v q^* $$
$q$ と $-q$ は全く同じ回転を表します。これは「2対1対応」と呼ばれ、1つの回転に対して2つの単位クォータニオン（$q$ と $-q$）が対応します。
幾何学的には、$q = (\cos(\Phi/2), \sin(\Phi/2)\hat{\bm{e}})$ と $-q = (\cos((\Phi+2\pi)/2), \sin((\Phi+2\pi)/2)\hat{\bm{e}})$ は、角度が $2\pi$ 異なる同じ回転です。
回転の合成
2つの回転を連続して適用する場合、対応するクォータニオンの乗算で合成できます。
まず $q_1$ で回転し、次に $q_2$ で回転すると、
$$ v” = q_2 (q_1 v q_1^*) q_2^* = (q_2 q_1) v (q_2 q_1)^* $$
よって、合成回転のクォータニオンは、
$$ q_{total} = q_2 q_1 $$
行列の積と同じく、後の回転が左に来ます。
回転行列の場合は $3 \times 3 = 9$ 要素同士の行列積（27回の乗算）が必要ですが、クォータニオンの乗算はわずか4成分同士の演算（16回の乗算と12回の加算）で済みます。この計算効率の良さは、リアルタイム制御やゲームエンジンで重要です。
クォータニオンによる回転の基本を理解しました。実用上は、オイラー角や回転行列との間で変換する必要があります。次にこれらの変換公式を導出しましょう。
オイラー角・回転行列との変換
クォータニオンから回転行列への変換
クォータニオン $q = (q_0, q_1, q_2, q_3)$ に対応する回転行列 $\bm{C}$ を導出します。
$v’ = qvq^*$ を成分ごとに展開して行列形式にまとめると、次の式が得られます。
$$ \bm{C}(q) = \begin{pmatrix} q_0^2+q_1^2-q_2^2-q_3^2 & 2(q_1 q_2 – q_0 q_3) & 2(q_1 q_3 + q_0 q_2) \\ 2(q_1 q_2 + q_0 q_3) & q_0^2-q_1^2+q_2^2-q_3^2 & 2(q_2 q_3 – q_0 q_1) \\ 2(q_1 q_3 – q_0 q_2) & 2(q_2 q_3 + q_0 q_1) & q_0^2-q_1^2-q_2^2+q_3^2 \end{pmatrix} $$
$\|q\| = 1$（$q_0^2+q_1^2+q_2^2+q_3^2 = 1$）を使うと、対角成分を簡略化できます。例えば $(1,1)$ 成分は、
$$ q_0^2+q_1^2-q_2^2-q_3^2 = 1 – 2(q_2^2+q_3^2) = 2(q_0^2+q_1^2) – 1 $$
よって、回転行列は次のようにも書けます。
$$ \bm{C}(q) = \begin{pmatrix} 1-2(q_2^2+q_3^2) & 2(q_1 q_2 – q_0 q_3) & 2(q_1 q_3 + q_0 q_2) \\ 2(q_1 q_2 + q_0 q_3) & 1-2(q_1^2+q_3^2) & 2(q_2 q_3 – q_0 q_1) \\ 2(q_1 q_3 – q_0 q_2) & 2(q_2 q_3 + q_0 q_1) & 1-2(q_1^2+q_2^2) \end{pmatrix} $$
回転行列からクォータニオンへの変換
回転行列 $\bm{C}$ からクォータニオンを求める方法はいくつかありますが、数値的に安定な シェパードの方法（Shepperd’s method） を紹介します。
まず、回転行列のトレース（対角成分の和）を使って $q_0$ を求める方法を示します。
$$ \text{tr}(\bm{C}) = C_{11} + C_{22} + C_{33} = 3 – 4(q_1^2 + q_2^2 + q_3^2) = 4q_0^2 – 1 $$
よって、
$$ q_0 = \frac{1}{2}\sqrt{1 + \text{tr}(\bm{C})} $$
$q_0 \neq 0$ のとき、非対角成分の差から残りの成分が求まります。
$$ q_1 = \frac{C_{32} – C_{23}}{4q_0}, \quad q_2 = \frac{C_{13} – C_{31}}{4q_0}, \quad q_3 = \frac{C_{21} – C_{12}}{4q_0} $$
しかし、$q_0 \approx 0$（回転角が $180°$ に近い場合）では分母がゼロに近づき数値的に不安定になります。シェパードの方法では、$q_0, q_1, q_2, q_3$ のうち絶対値が最大のものを先に求め、残りをその成分で割って計算します。
具体的には、次の4つの値を計算します。
$$ \begin{align} 4q_0^2 &= 1 + C_{11} + C_{22} + C_{33} \\ 4q_1^2 &= 1 + C_{11} – C_{22} – C_{33} \\ 4q_2^2 &= 1 – C_{11} + C_{22} – C_{33} \\ 4q_3^2 &= 1 – C_{11} – C_{22} + C_{33} \end{align} $$
最大のものを選んでその平方根を取り、残りの成分を交差項（非対角成分）から算出します。
オイラー角からクォータニオンへの変換
3-2-1オイラー角 $(\phi, \theta, \psi)$ から直接クォータニオンを計算できます。各軸回転に対応するクォータニオンは、
$$ q_x = \left(\cos\frac{\phi}{2}, \sin\frac{\phi}{2}, 0, 0\right) $$ $$ q_y = \left(\cos\frac{\theta}{2}, 0, \sin\frac{\theta}{2}, 0\right) $$ $$ q_z = \left(\cos\frac{\psi}{2}, 0, 0, \sin\frac{\psi}{2}\right) $$
合成回転のクォータニオンは $q = q_x q_y q_z$ で得られます。乗算公式を展開すると、
$$ \begin{align} q_0 &= \cos\frac{\phi}{2}\cos\frac{\theta}{2}\cos\frac{\psi}{2} + \sin\frac{\phi}{2}\sin\frac{\theta}{2}\sin\frac{\psi}{2} \\ q_1 &= \sin\frac{\phi}{2}\cos\frac{\theta}{2}\cos\frac{\psi}{2} – \cos\frac{\phi}{2}\sin\frac{\theta}{2}\sin\frac{\psi}{2} \\ q_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} \\ q_3 &= \cos\frac{\phi}{2}\cos\frac{\theta}{2}\sin\frac{\psi}{2} – \sin\frac{\phi}{2}\sin\frac{\theta}{2}\cos\frac{\psi}{2} \end{align} $$
各変換公式を整理したところで、クォータニオンの大きな利点の一つである滑らかな補間（SLERP）について見ていきましょう。
球面線形補間（SLERP）
なぜ補間が重要か
ロボットの姿勢を姿勢Aから姿勢Bに滑らかに変化させたい場面は頻繁にあります。アニメーション、衛星の姿勢変更コマンド、ロボットの軌道計画など、多くの応用で「途中の姿勢」を計算する必要があります。
オイラー角を単純に線形補間すると、途中の姿勢が不自然に歪んだり、ジンバルロック付近で発散したりする問題が生じます。クォータニオンを使えば、4次元単位球面上での最短経路を辿る滑らかな補間が可能です。
SLERPの定義
2つの単位クォータニオン $q_1$ と $q_2$ の間の球面線形補間（Spherical Linear intERPolation: SLERP）は、パラメータ $t \in [0, 1]$ に対して次のように定義されます。
$$ \text{SLERP}(q_1, q_2, t) = \frac{\sin((1-t)\Omega)}{\sin\Omega} q_1 + \frac{\sin(t\Omega)}{\sin\Omega} q_2 $$
ここで $\Omega$ は2つのクォータニオンのなす角で、内積から求められます。
$$ \cos\Omega = q_1 \cdot q_2 = q_{1,0}q_{2,0} + q_{1,1}q_{2,1} + q_{1,2}q_{2,2} + q_{1,3}q_{2,3} $$
SLERPの幾何学的意味
SLERPは4次元単位球面 $S^3$ 上の 大円弧に沿った等速運動 です。3次元の地球表面で例えると、2地点間の最短経路（大圏航路）上を等速で移動するのと同じです。
この性質により、SLERPには以下の優れた特性があります。
- 最短経路: 回転空間での最短経路を辿る（回転角が最小になる経路）
- 等角速度: パラメータ $t$ に対して回転角が一定速度で変化する
- 正規化の保持: $\|q_1\| = \|q_2\| = 1$ なら、補間結果も $\|\text{SLERP}\| = 1$
実装上の注意
SLERPを実装する際の注意点が2つあります。
内積の符号: $q$ と $-q$ は同じ回転を表すため、$q_1 \cdot q_2 < 0$ の場合は $q_2$ を $-q_2$ に置き換えます。これにより、常に短い方の経路で補間が行われます。符号を補正しないと、遠回りの経路（$360° - \Phi$ の回転）を辿ってしまいます。
$\Omega \approx 0$ の場合: $\sin\Omega \approx 0$ のときは分母がゼロに近づくため、通常の線形補間（LERP + 正規化）にフォールバックします。
各変換とSLERPの理論を理解したところで、クォータニオンの時間発展を記述する微分方程式を見ていきましょう。
クォータニオンの微分 — 角速度との関係
運動学方程式
姿勢制御においては、ジャイロセンサで計測した角速度 $\bm{\omega} = (\omega_x, \omega_y, \omega_z)^T$ からクォータニオンを時間更新する必要があります。
クォータニオンの時間微分は、角速度ベクトルと次の関係で結ばれます。
$$ \dot{q} = \frac{1}{2} q \otimes \omega_q $$
ここで $\omega_q = (0, \bm{\omega})$ は角速度を純粋四元数として表したもので、$\otimes$ は四元数の乗算です。
なぜこの式が成り立つのかを直感的に理解しましょう。微小時間 $dt$ の間に角速度 $\bm{\omega}$ で回転すると、微小回転角は $d\Phi = \|\bm{\omega}\| dt$、回転軸は $\hat{\bm{e}} = \bm{\omega}/\|\bm{\omega}\|$ です。対応する微小回転クォータニオンは、
$$ \delta q \approx \left(1, \frac{\bm{\omega} \, dt}{2}\right) $$
（$\cos(d\Phi/2) \approx 1$, $\sin(d\Phi/2) \approx d\Phi/2$ の近似を使いました）
新しいクォータニオンは $q(t+dt) = q(t) \otimes \delta q$ なので、
$$ \dot{q} = \lim_{dt \to 0} \frac{q(t+dt) – q(t)}{dt} = \lim_{dt \to 0} \frac{q \otimes \delta q – q}{dt} $$
$\delta q = (1, \bm{\omega}dt/2)$ を代入して整理すると、四元数の乗算公式を使って、
$$ \dot{q} = \frac{1}{2} q \otimes (0, \bm{\omega}) $$
が得られます。
成分表示で書き下すと、次のようになります。
$$ \begin{pmatrix} \dot{q}_0 \\ \dot{q}_1 \\ \dot{q}_2 \\ \dot{q}_3 \end{pmatrix} = \frac{1}{2} \begin{pmatrix} -q_1 & -q_2 & -q_3 \\ q_0 & -q_3 & q_2 \\ q_3 & q_0 & -q_1 \\ -q_2 & q_1 & q_0 \end{pmatrix} \begin{pmatrix} \omega_x \\ \omega_y \\ \omega_z \end{pmatrix} $$
この微分方程式は 線形 であり（$q$ の成分が $\omega$ に対して線形に入っている）、オイラー角の運動学方程式で現れた $1/\cos\theta$ のような特異項がありません。これがクォータニオンの最大の利点の一つです。
ノルムの保存
クォータニオンの運動学方程式はノルムを自動的に保存します。すなわち、$\|q(0)\| = 1$ ならば $\|q(t)\| = 1$ が保たれます。
証明は簡単です。$\|q\|^2 = q \cdot q = q_0^2 + q_1^2 + q_2^2 + q_3^2$ の時間微分を計算します。
$$ \frac{d}{dt}\|q\|^2 = 2q \cdot \dot{q} = 2q \cdot \frac{1}{2}q\otimes\omega_q $$
$q \otimes \omega_q$ のスカラー部は $-\bm{q}_v \cdot \bm{\omega}$ で、$q$ との内積をとると結果はゼロになります。よって $\|q\|^2 = \text{const}$ です。
ただし、数値積分では丸め誤差によりノルムが徐々にドリフトするため、定期的に正規化 $q \leftarrow q/\|q\|$ を行う必要があります。
理論の準備が整いました。Pythonで全てを実装し、動作を確認しましょう。
Pythonでの実装
クォータニオンクラスの実装
import numpy as np
class Quaternion:
"""単位クォータニオンによる回転表現"""
def __init__(self, q0, q1, q2, q3):
self.q = np.array([q0, q1, q2, q3], dtype=float)
@classmethod
def from_axis_angle(cls, axis, angle):
"""回転軸と回転角からクォータニオンを生成"""
axis = np.array(axis, dtype=float)
axis = axis / np.linalg.norm(axis)
ha = angle / 2
return cls(np.cos(ha), *(np.sin(ha) * axis))
@classmethod
def from_euler_321(cls, phi, theta, psi):
"""3-2-1オイラー角からクォータニオンを生成"""
cp, sp = np.cos(phi/2), np.sin(phi/2)
ct, st = np.cos(theta/2), np.sin(theta/2)
cs, ss = np.cos(psi/2), np.sin(psi/2)
return cls(
cp*ct*cs + sp*st*ss,
sp*ct*cs - cp*st*ss,
cp*st*cs + sp*ct*ss,
cp*ct*ss - sp*st*cs
)
def normalize(self):
"""正規化"""
self.q /= np.linalg.norm(self.q)
return self
@property
def conjugate(self):
"""共役"""
return Quaternion(self.q[0], -self.q[1], -self.q[2], -self.q[3])
def __mul__(self, other):
"""クォータニオン乗算"""
p, r = self.q, other.q
return Quaternion(
p[0]*r[0] - p[1]*r[1] - p[2]*r[2] - p[3]*r[3],
p[0]*r[1] + p[1]*r[0] + p[2]*r[3] - p[3]*r[2],
p[0]*r[2] - p[1]*r[3] + p[2]*r[0] + p[3]*r[1],
p[0]*r[3] + p[1]*r[2] - p[2]*r[1] + p[3]*r[0]
)
def rotate_vector(self, v):
"""ベクトルを回転: v' = q v q*"""
v_quat = Quaternion(0, *v)
result = self * v_quat * self.conjugate
return result.q[1:]
def to_dcm(self):
"""回転行列(DCM)に変換"""
q0, q1, q2, q3 = self.q
return np.array([
[1-2*(q2**2+q3**2), 2*(q1*q2-q0*q3), 2*(q1*q3+q0*q2)],
[2*(q1*q2+q0*q3), 1-2*(q1**2+q3**2), 2*(q2*q3-q0*q1)],
[2*(q1*q3-q0*q2), 2*(q2*q3+q0*q1), 1-2*(q1**2+q2**2)]
])
def __repr__(self):
return f"Q({self.q[0]:.4f}, {self.q[1]:.4f}, {self.q[2]:.4f}, {self.q[3]:.4f})"
# 動作確認: z軸まわり90度回転
q = Quaternion.from_axis_angle([0, 0, 1], np.radians(90))
print(f"クォータニオン: {q}")
print(f"ノルム: {np.linalg.norm(q.q):.10f}")
v = np.array([1.0, 0.0, 0.0])
v_rot = q.rotate_vector(v)
print(f"\n元のベクトル: {v}")
print(f"回転後のベクトル: {np.round(v_rot, 10)}")
print(f"期待値: [0, 1, 0]")
出力から、$z$ 軸まわり90°回転のクォータニオンが $(\cos 45°, 0, 0, \sin 45°) = (0.7071, 0, 0, 0.7071)$ であることが確認できます。$(1,0,0)$ を回転すると $(0,1,0)$ になり、回転行列 $R_z(90°)$ と同じ結果です。ノルムも正確に1.0に保たれています。
SLERP補間の実装と可視化
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
class Quaternion:
def __init__(self, q0, q1, q2, q3):
self.q = np.array([q0, q1, q2, q3], dtype=float)
@classmethod
def from_axis_angle(cls, axis, angle):
axis = np.array(axis, dtype=float)
axis = axis / np.linalg.norm(axis)
ha = angle / 2
return cls(np.cos(ha), *(np.sin(ha) * axis))
@property
def conjugate(self):
return Quaternion(self.q[0], -self.q[1], -self.q[2], -self.q[3])
def __mul__(self, other):
p, r = self.q, other.q
return Quaternion(
p[0]*r[0]-p[1]*r[1]-p[2]*r[2]-p[3]*r[3],
p[0]*r[1]+p[1]*r[0]+p[2]*r[3]-p[3]*r[2],
p[0]*r[2]-p[1]*r[3]+p[2]*r[0]+p[3]*r[1],
p[0]*r[3]+p[1]*r[2]-p[2]*r[1]+p[3]*r[0])
def rotate_vector(self, v):
v_q = Quaternion(0, *v)
result = self * v_q * self.conjugate
return result.q[1:]
def to_dcm(self):
q0, q1, q2, q3 = self.q
return np.array([
[1-2*(q2**2+q3**2), 2*(q1*q2-q0*q3), 2*(q1*q3+q0*q2)],
[2*(q1*q2+q0*q3), 1-2*(q1**2+q3**2), 2*(q2*q3-q0*q1)],
[2*(q1*q3-q0*q2), 2*(q2*q3+q0*q1), 1-2*(q1**2+q2**2)]])
def slerp(q1, q2, t):
"""球面線形補間"""
dot = np.dot(q1.q, q2.q)
# 短い方の経路を選択
if dot < 0:
q2 = Quaternion(*(-q2.q))
dot = -dot
dot = np.clip(dot, -1.0, 1.0)
if dot > 0.9995:
# 角度が小さい場合は線形補間+正規化
result = Quaternion(*(q1.q + t * (q2.q - q1.q)))
result.q /= np.linalg.norm(result.q)
return result
omega = np.arccos(dot)
so = np.sin(omega)
return Quaternion(*(np.sin((1-t)*omega)/so * q1.q + np.sin(t*omega)/so * q2.q))
# 始点と終点の姿勢
q_start = Quaternion.from_axis_angle([0, 0, 1], np.radians(0))
q_end = Quaternion.from_axis_angle([1, 1, 1], np.radians(120))
# SLERP補間
t_values = np.linspace(0, 1, 8)
fig = plt.figure(figsize=(16, 8))
for idx, t in enumerate(t_values):
ax = fig.add_subplot(2, 4, idx+1, projection='3d')
q_t = slerp(q_start, q_end, t)
C = q_t.to_dcm()
# 座標軸を描画
colors = ['r', 'g', 'b']
labels = ['x', 'y', 'z']
for i in range(3):
d = C[:, i]
ax.quiver(0, 0, 0, *d, color=colors[i],
arrow_length_ratio=0.1, linewidth=2)
ax.text(*(d*1.15), labels[i], color=colors[i], fontsize=9)
ax.set_title(f't = {t:.2f}', fontsize=11)
ax.set_xlim([-1.3,1.3]); ax.set_ylim([-1.3,1.3]); ax.set_zlim([-1.3,1.3])
ax.set_xlabel('X'); ax.set_ylabel('Y'); ax.set_zlabel('Z')
plt.suptitle('SLERP Interpolation: Identity → 120° around [1,1,1]', fontsize=14)
plt.tight_layout()
plt.savefig('slerp_interpolation.png', dpi=150, bbox_inches='tight')
plt.show()
上の8つのパネルは、単位回転（$t=0$）から $[1,1,1]$ 軸まわり120°回転（$t=1$）までのSLERP補間の過程を示しています。
- 等間隔の変化: 各フレーム間での姿勢の変化量がほぼ均一です。これはSLERPが等角速度補間であることの視覚的証拠です。
- 滑らかさ: 姿勢が急激に跳ぶことなく、連続的に変化しています。3つの座標軸が同時にスムーズに動いています。
- 最短経路: $[1,1,1]$ 軸まわりの120°回転は、$x$, $y$, $z$ 軸を巡回的に入れ替える回転に相当します。$t=1$ で $x \to y$, $y \to z$, $z \to x$ となっていることが確認できます。
SLERPとLERPの比較
SLERPの利点を際立たせるため、単純な線形補間（LERP + 正規化）との比較を行います。
import numpy as np
import matplotlib.pyplot as plt
class Quaternion:
def __init__(self, q0, q1, q2, q3):
self.q = np.array([q0, q1, q2, q3], dtype=float)
@classmethod
def from_axis_angle(cls, axis, angle):
axis = np.array(axis, dtype=float)
axis = axis / np.linalg.norm(axis)
ha = angle / 2
return cls(np.cos(ha), *(np.sin(ha) * axis))
@property
def conjugate(self):
return Quaternion(self.q[0], -self.q[1], -self.q[2], -self.q[3])
def __mul__(self, other):
p, r = self.q, other.q
return Quaternion(
p[0]*r[0]-p[1]*r[1]-p[2]*r[2]-p[3]*r[3],
p[0]*r[1]+p[1]*r[0]+p[2]*r[3]-p[3]*r[2],
p[0]*r[2]-p[1]*r[3]+p[2]*r[0]+p[3]*r[1],
p[0]*r[3]+p[1]*r[2]-p[2]*r[1]+p[3]*r[0])
def rotate_vector(self, v):
v_q = Quaternion(0, *v)
result = self * v_q * self.conjugate
return result.q[1:]
def slerp(q1, q2, t):
dot = np.dot(q1.q, q2.q)
if dot < 0:
q2 = Quaternion(*(-q2.q))
dot = -dot
dot = np.clip(dot, -1.0, 1.0)
if dot > 0.9995:
result = Quaternion(*(q1.q + t*(q2.q - q1.q)))
result.q /= np.linalg.norm(result.q)
return result
omega = np.arccos(dot)
so = np.sin(omega)
return Quaternion(*(np.sin((1-t)*omega)/so * q1.q + np.sin(t*omega)/so * q2.q))
def nlerp(q1, q2, t):
"""線形補間+正規化 (NLERP)"""
dot = np.dot(q1.q, q2.q)
if dot < 0:
q2_q = -q2.q
else:
q2_q = q2.q
result = (1-t)*q1.q + t*q2_q
result /= np.linalg.norm(result)
return Quaternion(*result)
# 大きな回転角での比較
q1 = Quaternion.from_axis_angle([0, 0, 1], 0)
q2 = Quaternion.from_axis_angle([1, 0, 0], np.radians(150))
t_fine = np.linspace(0, 1, 200)
# 各補間法での回転角度の変化を追跡
angles_slerp = []
angles_nlerp = []
ref_vec = np.array([0, 0, 1])
for t in t_fine:
qs = slerp(q1, q2, t)
qn = nlerp(q1, q2, t)
vs = qs.rotate_vector(ref_vec)
vn = qn.rotate_vector(ref_vec)
angles_slerp.append(np.degrees(np.arccos(np.clip(np.dot(ref_vec, vs), -1, 1))))
angles_nlerp.append(np.degrees(np.arccos(np.clip(np.dot(ref_vec, vn), -1, 1))))
fig, axes = plt.subplots(1, 2, figsize=(14, 5))
# (a) 回転角度 vs t
axes[0].plot(t_fine, angles_slerp, 'b-', linewidth=2, label='SLERP')
axes[0].plot(t_fine, angles_nlerp, 'r--', linewidth=2, label='NLERP')
axes[0].plot(t_fine, t_fine * angles_slerp[-1], 'k:', alpha=0.5, label='Ideal linear')
axes[0].set_xlabel('Parameter t', fontsize=12)
axes[0].set_ylabel('Rotation angle from start [deg]', fontsize=12)
axes[0].set_title('Rotation Angle vs Parameter', fontsize=13)
axes[0].legend(fontsize=11)
axes[0].grid(True, alpha=0.3)
# (b) 角速度（数値微分）
dt = t_fine[1] - t_fine[0]
omega_slerp = np.gradient(angles_slerp, dt)
omega_nlerp = np.gradient(angles_nlerp, dt)
axes[1].plot(t_fine, omega_slerp, 'b-', linewidth=2, label='SLERP')
axes[1].plot(t_fine, omega_nlerp, 'r--', linewidth=2, label='NLERP')
axes[1].set_xlabel('Parameter t', fontsize=12)
axes[1].set_ylabel('Angular rate [deg/unit t]', fontsize=12)
axes[1].set_title('Angular Rate (Derivative)', fontsize=13)
axes[1].legend(fontsize=11)
axes[1].grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('slerp_vs_nlerp.png', dpi=150, bbox_inches='tight')
plt.show()
上の2つのグラフから、SLERPとNLERPの違いが明確に読み取れます。
- 左図（回転角度）: SLERP（青線）は理想的な直線（黒点線）とほぼ完全に一致し、パラメータ $t$ に対して回転角が線形に変化します。一方、NLERP（赤破線）は中間部分で遅くなるS字カーブを描きます。
- 右図（角速度）: SLERPの角速度はほぼ一定（水平線）であるのに対し、NLERPの角速度は中央付近で落ち込んでいます。これは、NLERPが中間の姿勢で回転速度が不均一になることを意味します。
アニメーションや衛星の姿勢変更マヌーバでは一定の角速度が望ましいため、SLERPが標準的に使われます。
クォータニオン運動学の数値積分
最後に、クォータニオンの微分方程式を数値積分して、一定角速度での姿勢変化を追跡する例を示します。
import numpy as np
import matplotlib.pyplot as plt
def quat_multiply(p, q):
"""クォータニオン乗算"""
return np.array([
p[0]*q[0]-p[1]*q[1]-p[2]*q[2]-p[3]*q[3],
p[0]*q[1]+p[1]*q[0]+p[2]*q[3]-p[3]*q[2],
p[0]*q[2]-p[1]*q[3]+p[2]*q[0]+p[3]*q[1],
p[0]*q[3]+p[1]*q[2]-p[2]*q[1]+p[3]*q[0]
])
def quat_derivative(q, omega):
"""クォータニオンの時間微分: dq/dt = 0.5 * q * omega_q"""
omega_q = np.array([0, *omega])
return 0.5 * quat_multiply(q, omega_q)
# 初期条件
q0 = np.array([1.0, 0, 0, 0]) # 恒等回転
omega = np.array([0.1, 0.2, 0.3]) # 一定角速度 [rad/s]
# RK4による数値積分
dt = 0.01
t_max = 20.0
t_arr = np.arange(0, t_max, dt)
q_history = np.zeros((len(t_arr), 4))
q_history[0] = q0
norm_history = np.zeros(len(t_arr))
norm_history[0] = np.linalg.norm(q0)
q = q0.copy()
for i in range(1, len(t_arr)):
# 4次ルンゲ・クッタ法
k1 = quat_derivative(q, omega)
k2 = quat_derivative(q + 0.5*dt*k1, omega)
k3 = quat_derivative(q + 0.5*dt*k2, omega)
k4 = quat_derivative(q + dt*k3, omega)
q = q + (dt/6) * (k1 + 2*k2 + 2*k3 + k4)
# 正規化（数値ドリフト防止）
norm_history[i] = np.linalg.norm(q)
q = q / np.linalg.norm(q)
q_history[i] = q
fig, axes = plt.subplots(2, 2, figsize=(14, 10))
# (a) クォータニオン成分の時間変化
for i, label in enumerate(['$q_0$', '$q_1$', '$q_2$', '$q_3$']):
axes[0,0].plot(t_arr, q_history[:, i], linewidth=1.5, label=label)
axes[0,0].set_xlabel('Time [s]', fontsize=12)
axes[0,0].set_ylabel('Quaternion components', fontsize=12)
axes[0,0].set_title('Quaternion Time Evolution', fontsize=13)
axes[0,0].legend(fontsize=11)
axes[0,0].grid(True, alpha=0.3)
# (b) ノルムの変化（正規化前）
axes[0,1].plot(t_arr, norm_history - 1.0, 'b-', linewidth=1.5)
axes[0,1].set_xlabel('Time [s]', fontsize=12)
axes[0,1].set_ylabel('||q|| - 1 (before normalization)', fontsize=12)
axes[0,1].set_title('Norm Drift (RK4)', fontsize=13)
axes[0,1].grid(True, alpha=0.3)
axes[0,1].ticklabel_format(style='sci', axis='y', scilimits=(-3,-3))
# (c) 回転されたベクトルの軌跡
ref_vec = np.array([1, 0, 0])
trajectories = np.zeros((len(t_arr), 3))
for i in range(len(t_arr)):
q = q_history[i]
q_obj_q = np.array([0, *ref_vec])
q_conj = np.array([q[0], -q[1], -q[2], -q[3]])
v_rot = quat_multiply(quat_multiply(q, q_obj_q), q_conj)
trajectories[i] = v_rot[1:]
ax3d = fig.add_subplot(2, 2, 3, projection='3d')
ax3d.plot(trajectories[:, 0], trajectories[:, 1], trajectories[:, 2],
'b-', linewidth=1, alpha=0.7)
ax3d.scatter(*trajectories[0], c='green', s=60, label='Start')
ax3d.scatter(*trajectories[-1], c='red', s=60, label='End')
# 単位球面を半透明で表示
u = np.linspace(0, 2*np.pi, 30)
v = np.linspace(0, np.pi, 20)
xs = np.outer(np.cos(u), np.sin(v))
ys = np.outer(np.sin(u), np.sin(v))
zs = np.outer(np.ones(np.size(u)), np.cos(v))
ax3d.plot_surface(xs, ys, zs, alpha=0.05, color='gray')
ax3d.set_xlabel('X'); ax3d.set_ylabel('Y'); ax3d.set_zlabel('Z')
ax3d.set_title('Trajectory of Rotated [1,0,0] on Unit Sphere', fontsize=12)
ax3d.legend(fontsize=10)
# (d) 各成分の軌跡
axes[1,1].plot(t_arr, trajectories[:, 0], 'r-', label='x', linewidth=1.5)
axes[1,1].plot(t_arr, trajectories[:, 1], 'g-', label='y', linewidth=1.5)
axes[1,1].plot(t_arr, trajectories[:, 2], 'b-', label='z', linewidth=1.5)
axes[1,1].set_xlabel('Time [s]', fontsize=12)
axes[1,1].set_ylabel('Components', fontsize=12)
axes[1,1].set_title('Rotated Vector Components vs Time', fontsize=13)
axes[1,1].legend(fontsize=11)
axes[1,1].grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('quaternion_kinematics.png', dpi=150, bbox_inches='tight')
plt.show()
上の4つのグラフから、クォータニオン運動学の特性が明確に読み取れます。
- 左上（クォータニオン成分）: 4つの成分が滑らかな周期関数として変化しています。特異点や不連続性は一切見られず、オイラー角の運動学で生じるジンバルロック付近の発散とは対照的です。
- 右上（ノルムのドリフト）: RK4積分によるノルムドリフトは $10^{-10}$ 以下の極めて小さいレベルに抑えられています。各ステップで正規化を行わなくても、短時間であれば実用的な精度が保たれることがわかります。
- 左下（3D軌跡）: 単位ベクトル $(1,0,0)$ の回転軌跡が単位球面上を滑らかに描かれています。角速度 $\bm{\omega} = (0.1, 0.2, 0.3)$ が3軸すべての成分を持つため、複雑な3次元的な曲線になっています。
- 右下（成分の時間変化）: $x$, $y$, $z$ 各成分が滑らかな周期関数として変動しています。この準周期的な振る舞いは、無理数比の角速度成分を持つ回転の特徴です。
回転行列・オイラー角・クォータニオンの比較
ここで、3つの姿勢表現の特性を整理しておきます。
| 特性 | 回転行列 | オイラー角 | クォータニオン |
|---|---|---|---|
| パラメータ数 | 9（拘束6） | 3 | 4（拘束1） |
| 特異点 | なし | あり | なし |
| 合成回転 | 行列積（27乗算） | 角度の足し算は不可 | 四元数積（16乗算） |
| 逆回転 | $C^T$（転置） | 角度反転は単純でない | $q^*$（共役） |
| 補間 | 困難 | 線形補間は不均一 | SLERP（等角速度） |
| 直交性維持 | 6拘束の維持が必要 | 不要 | ノルム=1の維持のみ |
| 直感性 | 低い | 高い | 中程度 |
| 冗長性 | 高い（9要素） | なし | 低い（4要素） |
宇宙工学の実務では、クォータニオンを内部表現として使い、人間との対話やテレメトリ表示にはオイラー角に変換する というのが一般的なアプローチです。
まとめ
本記事では、クォータニオン（四元数）による回転表現を解説しました。
- 四元数 は1つの実部と3つの虚部からなり、乗算が非可換な数体系。単位四元数は3次元回転を特異点なしに表現できる
- 回転公式 $v’ = qvq^*$ で3次元ベクトルを回転する。$q$ と $-q$ は同じ回転を表す（2対1対応）
- 相互変換: オイラー角・回転行列との間で変換公式が存在する。回転行列からの逆変換にはシェパードの方法が数値的に安定
- SLERP: 4次元単位球面上の大円弧に沿った等角速度補間。アニメーションや姿勢マヌーバに最適
- 運動学方程式: $\dot{q} = \frac{1}{2}q \otimes \omega_q$ は線形で特異項がなく、数値積分が安定
クォータニオンは特異点のない姿勢表現を提供し、これで姿勢を記述する数学的道具が揃いました。次の記事では、これらの道具を使って人工衛星の姿勢を具体的に定義する座標系 — ECI、ECEF、LVLH（軌道座標系）、機体座標系 — を学びます。
次のステップとして、以下の記事も参考にしてください。