クォータニオンと回転行列

はじめに

ドローンやロボットの制御において、姿勢表現は非常に重要です。オイラー角は直感的である反面、ジンバルロックという問題があり、連続的で安定な回転表現には適していません。一方、クォータニオンはこの問題を克服し、滑らかで連続的な回転表現を可能にします。本記事ではクォータニオンの基本、クォータニオン積、共役クォータニオンや逆元、そしてクォータニオンを用いた回転行列（方向余弦行列）の導出を丁寧に解説します。

1. クォータニオンの基本

クォータニオンはスカラー部とベクトル部からなる拡張された複素数で、次のように表されます。

$\mathbf{\tilde{q}} = q_0 + \mathbf{q}, \quad \mathbf{q} = q_1\mathbf{i} + q_2\mathbf{j} + q_3\mathbf{k}$

成分表示は、

$\mathbf{\tilde{q}} = (q_0, q_1, q_2, q_3)$

のように、最初の成分をスカラー部 ${ q_0 }$ 、後半の3成分 ${ (q_1, q_2, q_3) }$ をベクトル部として表します。

2. ベクトルの積とクォータニオンの積

クォータニオン同士の積にはベクトルの内積と外積を用います

内積（スカラー積）

2つのベクトル ${\mathbf{a}, \mathbf{b}}$ の内積は、スカラー値を返し、次のように定義されます。

$\mathbf{a}\cdot\mathbf{b} = a_1 b_1 + a_2 b_2 + a_3 b_3$

外積（ベクトル積）

ベクトルの外積はベクトル値を返し、以下で定義されます。

$\mathbf{a}\times\mathbf{b} = \begin{vmatrix} \mathbf{i} & \mathbf{j} & \mathbf{k} \\ a_1 & a_2 & a_3 \\ b_1 & b_2 & b_3 \end{vmatrix}$

クォータニオン積

クォータニオン積では、ベクトル部に関して以下のようなベクトル積を用いて定義します。

$\mathbf{a}\mathbf{b} = -\mathbf{a}\cdot\mathbf{b} + \mathbf{a}\times\mathbf{b}$

これを用いてクォータニオン ${\mathbf{\tilde{q}}_1=(q_{01},\mathbf{q}_1)}$ と ${\mathbf{\tilde{q}}_2=(q_{02},\mathbf{q}_2)}$ の積は、

$\mathbf{\tilde{q}}_1\mathbf{\tilde{q}}_2=(q_{01}q_{02}-\mathbf{q}_1\cdot\mathbf{q}_2)+(q_{01}\mathbf{q}_2+q_{02}\mathbf{q}_1+\mathbf{q}_1\times\mathbf{q}_2)$

で与えられます。

3. 共役クォータニオンと逆元

クォータニオンの共役 ${\mathbf{\tilde{q}}^*}$ は

$\mathbf{\tilde{q}}^*=(q_0,-q_1,-q_2,-q_3)$

と定義されます。

逆元は共役を用いて次のように表せます。

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

4. クォータニオンによる回転

回転クォータニオンの定義

回転軸となる単位ベクトル ${\mathbf{n}=(n_1,n_2,n_3)}$ と回転角 ${\theta}$ を用いて、回転クォータニオンは次のように定義されます。

$\mathbf{\tilde{q}}=\left(\cos\frac{\theta}{2},n_1\sin\frac{\theta}{2},n_2\sin\frac{\theta}{2},n_3\sin\frac{\theta}{2}\right)$

これによりベクトルを滑らかに回転させることができます。

クォータニオンによるベクトルの回転

あるベクトル ${\mathbf{v}=(0,v_1,v_2,v_3)}$ をクォータニオンで回転させるには、

$\mathbf{v}'=\mathbf{\tilde{q}}\mathbf{v}\mathbf{\tilde{q}}^{*}$

とします。

回転行列（方向余弦行列）

クォータニオン ${\mathbf{\tilde{q}}=(q_0,q_1,q_2,q_3)}$ から構成される回転行列 ${R}$ は、ドローンやロボットの姿勢を表し、物体座標系から慣性座標系への座標変換を行う行列として次式で表されます。

$R=\begin{bmatrix} q_0^2+q_1^2-q_2^2-q_3^2 & 2(q_1q_2-q_0q_3) & 2(q_1q_3+q_0q_2)\\ 2(q_1q_2+q_0q_3) & q_0^2-q_1^2+q_2^2-q_3^2 & 2(q_2q_3-q_0q_1)\\ 2(q_1q_3-q_0q_2) & 2(q_2q_3+q_0q_1) & q_0^2-q_1^2-q_2^2+q_3^2 \end{bmatrix}$

ジンバルロックは起きない

以上の、クォータニオンを用いた物体の姿勢を表す回転行列（方向余弦行列）はオイラー角を用いた姿勢表現におけるジンバルロックは起きません。というのはクォータニオンから回転行列は一意（一対一）に求まりますし、回転行列からクォータニオンも一意にもとまり特異点を持ちません。

姿勢計算はクォータニオンで表示はオイラー角で

ただし、「クォータニオンが姿勢（回転）の表現として直感的でわかりやすいか？」という問題があります。通常はジンバルロックなどを起こすオイラー角を姿勢の推定やそれを制御に使うことを避けなければならないので、姿勢計算はクォータニオンを使うのが今やデフォルトだと考えます。

ただし、人間が計算結果を確認する際はオイラー角で表示した方が若干わかりやすいので、クォータニオンで作られた回転行列からオイラー角に変換して表示することがあります。この場合も、落とし穴としては、表示の際にオイラー角でいうジンバルロックの条件が揃っていれば、オイラー角変換はうまくいかず、おかしな結果が出ます。しかし、これは内部で姿勢計算が間違っているわけではなく、オイラー角が内在している問題によってひきおこされている問題で、本質的なものではないということになります。

まとめ

クォータニオンはスカラー部とベクトル部で構成される。
内積、外積を用いてクォータニオン積を定義可能。
クォータニオンはジンバルロックを回避し、回転を安定的に表現できる。
回転軸と回転角に基づき回転クォータニオンを定義し、回転行列を構成できる。
姿勢計算はクォータニオンでおこない、表示はオイラー角で行う。（ただしジンバルロック状態の表示は難しい）

ようやく、姿勢を表す三つの表現

方向余弦行列（回転行列）
オイラー角
クォータニオン

が全て揃いました。今後は、それぞれの相互変換、そしていよいよ姿勢の推定というお話になっていきます。

付録：回転行列の導出過程

以下、クォータニオンを用いた回転行列の導出を詳しく解説すます。

クォータニオンは以下のように定義します：

$\tilde{q} = q_0 + \mathbf{q}$

また、回転させたいベクトル ${\mathbf{r}}$ を、スカラー部がゼロのクォータニオン ${\tilde{r}}$ として表します：

$\tilde{r} = 0 + \mathbf{r}$

クォータニオンを用いた回転操作は、以下の式で表されます：

$\mathbf{r}' = \tilde{q} \tilde{r} \tilde{q}^*$

ここで、 ${\tilde{q}^*}$ は ${\tilde{q}}$ の逆元であり、ノルムが1（単位クォータニオン）であれば、

$\tilde{q}^* = q_0 - \mathbf{q}$

と表せます。

クォータニオンの積の展開

まず、左側の積 ${\tilde{q} \tilde{r}}$ を展開します。

$(q_0 + \mathbf{q})(0 + \mathbf{r})$

クォータニオンの積の公式を用いると、

$\tilde{q} \tilde{r} = (- \mathbf{q} \cdot \mathbf{r}) + (q_0 \mathbf{r} + \mathbf{q} \times \mathbf{r})$

スカラー部： ${- \mathbf{q} \cdot \mathbf{r}}$
ベクトル部： ${q_0 \mathbf{r} + \mathbf{q} \times \mathbf{r}}$

次に、これに ${\tilde{q}^*}$ を掛けます。

$(- \mathbf{q} \cdot \mathbf{r} + q_0 \mathbf{r} + \mathbf{q} \times \mathbf{r})(q_0 - \mathbf{q})$

クォータニオンの積の公式を適用し、スカラー部とベクトル部をそれぞれ計算すると：

スカラー部分の計算

$(- \mathbf{q} \cdot \mathbf{r}) q_0 + (q_0 \mathbf{r} + \mathbf{q} \times \mathbf{r}) \cdot (-\mathbf{q})$

$= -q_0 (\mathbf{q} \cdot \mathbf{r}) - \mathbf{q} \cdot (q_0 \mathbf{r} + \mathbf{q} \times \mathbf{r})$

$= -q_0 (\mathbf{q} \cdot \mathbf{r}) - q_0 (\mathbf{q} \cdot \mathbf{r}) - \mathbf{q} \cdot (\mathbf{q} \times \mathbf{r})$

$= -2 q_0 (\mathbf{q} \cdot \mathbf{r}) + 0$

（最後の項はベクトル三重積の性質よりゼロ）

ベクトル部分の計算

$(-\mathbf{q} \cdot \mathbf{r}) (-\mathbf{q}) + q_0 \mathbf{r} q_0 + q_0 (\mathbf{q} \times \mathbf{r}) - (\mathbf{q} \times \mathbf{r}) \times \mathbf{q}$

ここで、ベクトル三重積の公式

$\mathbf{a} \times (\mathbf{b} \times \mathbf{c}) = (\mathbf{a} \cdot \mathbf{c}) \mathbf{b} - (\mathbf{a} \cdot \mathbf{b}) \mathbf{c}$

を適用すると、

$- (\mathbf{q} \times \mathbf{r}) \times \mathbf{q} = -((\mathbf{q} \cdot \mathbf{q}) \mathbf{r} - (\mathbf{q} \cdot \mathbf{r}) \mathbf{q})$

$= -|\mathbf{q}|^2 \mathbf{r} + (\mathbf{q} \cdot \mathbf{r}) \mathbf{q}$

これを整理すると、

$q_0^2 \mathbf{r} - |\mathbf{q}|^2 \mathbf{r} + 2 (\mathbf{q} \cdot \mathbf{r}) \mathbf{q} + 2 q_0 (\mathbf{q} \times \mathbf{r})$

この結果、回転後のベクトル ${\mathbf{r}'}$ の最終的な式が得られます。

$\mathbf{r}' = (q_0^2 - |\mathbf{q}|^2) \mathbf{r} + 2 (\mathbf{q} \cdot \mathbf{r}) \mathbf{q} + 2 q_0 (\mathbf{q} \times \mathbf{r})$

回転行列の導出

クォータニオンの成分 ${(q_0, q_1, q_2, q_3)}$ を展開し、ベクトル ${\mathbf{r} = (r_1, r_2, r_3)}$ の各成分を代入して整理すると、次の回転行列 ${R}$ を得ることができます。

$R = \begin{bmatrix} 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{bmatrix}$