回転・姿勢を表現する方法で四元数（クォータニオン）というものを聞いたことありますでしょうか？聞いたことはあるけどよくわからないので使っていないという方も多いと思います。極力数式少なめでどんなものか説明します。今回は第一弾で基礎編です。
対象者
- 四元数（クォータニオン）を聞いたことがない、聞いたことがあるけどなんとなく難しそうと感じている、もしくはオイラー角 or 回転行列で間に合っているので四元数を避けている人。
- 主に組込み系の人に向けた入門記事ですが易しい内容なので四元数に興味のある方であれば誰でも大丈夫です。
- 線形代数は必須ではないですがなんとなく行列をかけて座標変換ができるくらいの認識があると良いです。
背景と目的
数年前から3軸加速度、3軸ジャイロを合わせた6軸のセンサが四元数出力に対応しているものが出てきています。私が初めて出会ったのはInvensense(現TDK)製のMPU-6050でした。
オイラー角（ロール・ピッチ・ヨーとかヘディング・ピッチ・バンクとか）や回転行列は学生の間に触れたことがある方も多いかと思うのですが、四元数は私の場合、このセンサに触れるまで名前すら知りませんでした。
四元数は非常に便利でCG関連では既に多く扱われているのですが、組込み分野で適用したという例をあまり聞かないので、組込み分野でも採用事例を増やしたい、そのためにはまず知っている人を増やさなければ、というのがこの記事のモチベーションです。ちなみにROSの姿勢表現は四元数が標準のようなのでROSで聞いたことがあるよという人もいるかもしれません。
まずは、四元数がどんなもので、どこが便利で、どういうふうに使えるのかという感覚を掴んでもらうことが目的です。
扱う内容と扱わない内容
扱う内容
- 四元数でどんなことが実現できるのか→回転や姿勢の表現
- オイラー角、回転行列との違い
- 四元数の構造
- 正規化
- 四元数の計算
- 共役、逆数
- 外積、ベクトルの回転
- 差分
- 内積
- 補間
扱わない内容
- 数学的な詳細な内容（わかりやすさを優先します。興味を持てたら参考文献から詳細を調べてください。）
- 次回触れる予定の範囲
- 四元数を扱う上での注意点
四元数でどんなことが実現できるのか
動画
まずは、興味を持つために動いているものを見てみましょう。MPU-6050（3軸ジャイロ＋3軸加速度のセンサフュージョンから四元数を出力できるセンサ）で飛行機を回転させます。この例ではセンサMPU-6050上でセンサフュージョンを実施し、Arduinoでは計算結果の四元数を受け取ってそのままProcessing（PC上の描画ソフト）に渡しています。3Dを扱えるソフトの場合、四元数を扱えることが多い（Processingは標準では扱えないのですが）ので値を渡すとそのまま回転してくれます。難しい計算は一切していないですが、画面上の飛行機が、センサと同じ姿勢になるとわくわくしますね。
姿勢を表現することでできること
「四元数は姿勢を表現できます」。「姿勢」を言葉で説明するのが難しいのですが、上記の動画のように今センサがどちらを向いていてどの程度ひねられているのかを表現することができます。これを応用するとロボットアームの各関節の姿勢がわかるので姿勢を元に必要な軌道を描くように制御したり、クアッドコプター（よくドローンと呼ばれているやつ）の制御に使うことができます。他にはこの姿勢がどのように推移したかを使ってジェスチャ認識する、ということもできます。
姿勢と回転
「四元数では姿勢を表現できます」、と書きましたが、と同時に「四元数は回転を表現できます」。これだけ聞くと何を指しているのかよくわからないですね。
飛行機の姿勢は「飛行機の基準とする姿勢Oが存在して、飛行機が基準に対してどういう姿勢にあるか」を表現することに相当します。
先の動画では、姿勢Oを基準としてフレームごとに新しい姿勢に更新していました。これは、姿勢Oから新しい姿勢へと回転させたとも捉えることができます。
ここでアニメーションを作る場合を考えてみましょう。鉛直方向の軸を中心に飛行機を\(\pi/4\)[rad/frame] で回転してみます。フレームごとに\(\pi/4\)回転するので、8フレームで一周（\(2\pi\)回転）します。この場合は各フレームにおいて1つ前のフレームの姿勢を基準に、「回転軸周りに\(\pi/4\)回転」した姿勢が新しい時刻における姿勢になります。何を基準にするかで四元数は姿勢とも回転とも取れます。
回転行列、オイラー角、四元数の比較
ロボットの制御などをしている人たちはオイラー角や回転行列（例えばDH法等）が使われているのを知っていると思います。そこで四元数はオイラー角や回転行列と何が違うのか、という話になりますが基本的にできることは変わらず、等価に相互に変換可能です。ただし、細かいところでそれぞれ下記に挙げるような長所短所があります。
- 回転行列（3×3の行列）
- 長所 逆行列が使える、一意に表現可能
- 短所 回転行列はメモリを余計に使用する
- オイラー角（ロール・ピッチ・ヨー、ヘディング・ピッチ・バンクなど）
- 長所 直感的、メモリ量が少ない
- 短所 ジンバルロックが発生する
- 四元数
- 長所 姿勢間の補間が容易、一意に表現可能（正確には常に2つある）、メモリ量が少ない
- 欠点 少し理解しづらい？
後述しますが、四元数を使用する上で一番大きいと考えているのが姿勢間の補間、および合成が容易な点です。次節から、四元数の中身の説明に入っていきます。
四元数の構造
まず、受け入れるべき事実としてすべての姿勢は、「とある回転軸とその軸周りの回転角」で表現可能です。どれだけ複雑な回転を組み合わせた結果の姿勢でも同じです。私は四元数に触れるまでこの事実を知りませんでした。これは受け入れてください。
上記事実から「回転軸 \(\boldsymbol{v}\) と軸周りの回転角 \(\theta\) 」で姿勢を表現するシンプルな方法があります。これを数学上の都合で少しだけいじったものが四元数です。四元数\(q\)はスカラー \(w\) とベクトル \(\boldsymbol{v} = (x,y,z)\) からなり、下記のように表現します。
$$q = [w,\boldsymbol{v}] = [w, x, y, z]$$
補足）ROSのQuaternionクラスの引数ではスカラー \(w\)が最後に来るようですので、\(w\)の位置はその場に合わせて解釈してください。本稿では上記の \(w\)が最初に来る表記を用います。
正規化
四元数の定義としてはこれで十分ですが、回転を扱う上では少し制限をつけます。この制限によって回転が綺麗に表現できるようになります。
- 回転軸のベクトルは大きさが1の\(\boldsymbol{n}\)とします。\(||\boldsymbol{n}|| = 1\)
- ベクトル\(\boldsymbol{n}\)周りに角\(\theta\)回転する四元数\(q\)を下式で書き直します。
$$
\begin{align}
q &= \left[\cos\frac{\theta}{2},\ \sin\frac{\theta}{2} \boldsymbol{n} \right] \\
&= \left[\cos\frac{\theta}{2},\ \sin\frac{\theta}{2} n_x,\ \sin\frac{\theta}{2} n_y,\ \sin\frac{\theta}{2} n_z \right] \\
&= [ w, x, y, z]
\end{align}
$$
上記より各要素\(w,x,y,z\)は\([-1, 1]\)の範囲になります。上記の制限は結果として\(q\)のノルム（絶対値）を下記の様に定義した時、四元数のノルムが1となるようにしたことになります。
$$||q|| = \sqrt{w^2+x^2+y^2+z^2} = \sqrt{\cos^2(\theta/2) + \sin^2(\theta/2) ||\boldsymbol{n}||^2} = 1$$
上記のように正規化された四元数のことを単位四元数と呼びます。姿勢や回転をあらわす場面で出てくる四元数は特に言及がなければ単位四元数だと思ってください。
また、上記定義から、回転を行わない場合は\(\theta=0\)で、\(q=[1,0,0,0]\)となります。この四元数を恒等四元数と呼びます。
四元数の計算
数式はあまり出さないようにと言いつつ、いくつか言葉を定義する都合上もう少し出てきます。中身はもういいよという方はまとめまで読み飛ばしてください。ただし、四元数を3次元ベクトルの回転に使える、回転を結合できる、回転と回転の間を補間できるという3点を抑えてください。この3点がわかればライブラリとにらめっこすれば四元数を何となく使えると思います。
符号反転
「回転軸 \(\boldsymbol{v}\) と軸周りの回転角 \(\theta\)」の考え方に戻ると、「回転軸が\( -\boldsymbol{v}\)」で「回転角が\(-\theta\)」も同じ回転を表すことになります。これは四元数においても同じなので下記の\(-q\)は\(q\)と同じ回転を表します。鏡に写ったような四元数ですね。結果として同じ回転・姿勢を表現しますが、四元数としては区別（同一ではない）されます。
$$-q = [-w, -\boldsymbol{v}]$$
共役と逆数
四元数\(q\)の共役\(q^*\)はベクトル部分の符号反転することで得られます。
$$q^* = [w,v]^* = [w, -v] = [w, -x, -y, -z]$$
四元数\(q\)の逆数\(q^{-1}\)は共役を正規化することで得られます。
$$q^{-1} = \frac{q^*}{||q||}$$
これは回転軸だけを反対にして、同じ回転角だけ回すと\(q\)の逆の操作になると考えると感覚的にも理解できますね。ちなみに正規化されている前提であれば、\(||q||=1\)ですので、分母は不要になります。
外積
2つの四元数\(q_1, q_2\)の外積です。ベクトルの回転で使用する基本的な演算です。
$$
q_1 q_2 = [w_1, \boldsymbol{v_1}] [w_2, \boldsymbol{v_2}] = [w_1w_2 -\boldsymbol{v_1} \cdot \boldsymbol{v_2},\ w_1\boldsymbol{v_2}+w_2\boldsymbol{v_1}+\boldsymbol{v_2} \times \boldsymbol{v_1} ]
$$
本によって定義が少し違います。最後の項の\(\boldsymbol{v_2} \times \boldsymbol{v_1}\)の前後が違うことがあります。ライブラリを使用する場合でも定義がどちらか意識したほうが良いことがあるかもしれません。
外積は結合可能ですが交換可能ではありません。交換可能でないことは先の式の最後の項にベクトルの外積が入っていることからわかりますね。
$$
(ab)c = a(bc)\\
ab \neq ba
$$
ベクトルの回転
3次元空間における点\((p_x,p_y,p_z)\)があったとします。この点に対応する正規化されていない仮の四元数を\(p = [0,p_x,p_y,p_z]\)とおきます。点\(p\)に単位四元数\(q\)による回転を加えて点\(p’\)を得るには下式を使用します。四元数で両側から挟み込みます。シンプルですね。
$$p’ = q^{-1}pq$$
回転の連結
2つの単位四元数\(a,b\)を順に適用したい場合は下式のようになります。
$$
\begin{align}
p’ &= b^{-1} (a^{-1} p a) b \\
&= (b^{-1} a^{-1}) p (a b) \\
&= (ab)^{-1} p (ab)
\end{align}
$$
四元数\(a,b\)の外積を計算することで、回転を連結した四元数を得ることが出来ました。ただし、外積の定義が違う場合はこの結果は少し違うものになります。
差分
姿勢\(a\)から姿勢\(b\)に回転するにはどうしたらよいでしょうか。四元数\(a,b\)の差分\(d\)を考えます。つまり回転\(a\)に回転\(d\)を適用すると回転\(b\)になるようにすればよいわけです。式を立て、両辺に\(a^{-1}\)をかけると下記のようになります。
$$
\begin{align}
ad &= b \\
a^{-1}(ad) &= a^{-1}b \\
d &= a^{-1}b
\end{align}
$$
内積
外積の次は内積です。四元数\(q_1, q_2\)の内積は下記で表されます。
$$
\begin{align}
q_1 \cdot q_2 &= w_1 w_2 + \boldsymbol{v_1} \cdot\boldsymbol{v_2} \\
&= w_1 w_2 + x_1 x_2 + y_1 y_2 + z_1 z_2
\end{align}
$$
単位四元数同士の内積の結果は \([-1, 1]\)を取ります。内積の絶対値が大きいほど\(a\)と\(b\)の姿勢が似ているという性質があります。
補間
飛行機をある姿勢\(a\)からある姿勢\(b\)へと変化させたいとします。この時、差分\(d\)の計算の仕方はわかっていますが、少しずつ近づけるようなアニメーションを作りたい場合はこれでは不十分です。姿勢\(a\)と姿勢\(b\)を補間する関数にslerpがあります。四元数が扱える環境では大抵slerp関数が用意されていますのでこれを使いましょう。
定義は省略します。イメージとしては、数直線上の点\(a,b\)の間を時刻\(0\leq t \leq 1\)で移動する点を補間する際に\((1-t)a + tb\)のような式を使いますがこれに似ています。四元数の場合は1次元ではないので弧を描くような補間がされます。
まとめ
四元数の構造とどのような計算が行えるかを見て、なんとなく四元数がつかめたと思います。四元数はオイラー角と比較すると直感的ではなくわかりにくく感じるところもあるかもしれませんが、補間ができるなどの利点からよく使用されますので理解しておいて損はないと思います。次回は応用編として実際に四元数を使う上で気をつけるべき点などについて解説します。
参考文献
- 実例で学ぶゲーム3D数学
- 私は四元数をこの本で学びました。本全体の中で四元数を扱っているのはごく一部ではあるのですが、今回紹介した内容の他に、四元数とオイラー角・回転行列の相互変換式の導出なども載っています。四元数を応用する場合はこれを1冊おいておくと便利だと思います。その他にも3Dに関連した面白い内容が扱われているので、四元数以外の部分でも役に立つかもしれません。
