四元数入門の後編、応用編です。前回の基礎編では四元数がどのように使えて、どんな構造をしているか、どんな計算ができるかを解説しました。今回は私がこれまで四元数を応用してくる中で出会った落とし穴や、実際のセンサを使う上での注意点などについてご紹介します。
大きく問題を下記の3つに分けて解説していきます。
- センサの利用に起因する問題
- 座標系に起因する問題
- 四元数に起因する問題
1. センサの利用に起因する問題
ジャイロのドリフト問題
基礎編の動画のようにMPU-6050は優秀なのでDMPが内部で精度の良い四元数を計算してくれます。いったんこのことを忘れて、DMPがない世界を想像してみましょう。
私達の手元にあるのは3軸の加速度と3軸のジャイロ（角速度）です。これを元にどうやって姿勢を推定するか考えてみます。
まずは加速度を使ってみましょう。静止している状態であれば3軸加速度だけで重力方向を特定することができます。その場でゆっくり動かすだけであればどちらが下を向いているかを特定できます。あくまで下がどちらを向いているかだけで、水平方向はどちらを向いているかはわかりません。また、センサそのものに重力以外の加速度が加わるとそもそもどちらを向いているのかわからなくなります。ただし、一度静止してしまえばどれだけ複雑に動いたあとでも誤差がたまることなく確実に下方向がわかります。
ジャイロだけを使った場合はどうでしょう。角速度 [rad/sec]がわかるので、初期の姿勢だけ決めてしまえば角速度をサンプリングごとに積分していけば角 [rad]が出そうです。ですが、これは実際にはうまく行きません。
現実には各センサの出力値には誤差があります。切り捨てた桁による誤差、数値計算誤差、温度変化による誤差など。一番厄介なのは温度変化やキャリブレーション不足によるバイアスが入るような誤差（センサ値に常に少しだけ＋の誤差があるなど）で、これを単純に積分すると誤差がどんどん溜まって、静止していても本来の角度からずれていきます。これがジャイロセンサのドリフト問題です。
いくらセンサの出力の精度を挙げたとしても誤差はなくならないので、これは避けられない問題と言えます。
センサフュージョンと各種フィルタ（カルマン、拡張カルマン、Madgwick）
加速度はある時点における状態を、角速度は変化を追いかけることが得意です。「複数のセンサを用いて、それぞれのセンサの長所を活かそう」というのがセンサフュージョンの考え方です。ここでよく出てくるアルゴリズムはカルマンフィルタや拡張カルマンフィルタ(EKF)です。
とはいっても、センサフュージョンは魔法ではないので（まるで魔法のようでもあるのですが）情報が足りなければそれなりにしか姿勢を推定することはできません。
たとえば、3軸加速度、3軸ジャイロの6軸センサに対してカルマンフィルタを使ったとしても北はどちらかわかりません。なぜならジャイロでは方角はわからないから。カルマンフィルタは複数の情報を補完しあい、誤差を推定するような計算をします。方角の例でいくと、一旦推定がずれてしまうとそれを補正するための情報がなければずれたままになります。結局、6軸のセンサフュージョンではドリフトのようなものは場合によっては発生しえます。
ここに3軸の地磁気センサが加わって9軸のセンサになると話が変わります。カルマンフィルタなどで方角がずれない推定が作れるようになります。地磁気でなくても、例えばGPSやカメラであったり、深度センサ、電波強度であったりその他の情報をうまく取り入れることでズレをなくすことができます。
ちなみに（拡張）カルマンフィルタ以外で性能の良い6軸、9軸の姿勢推定に使われるフィルタとしてMadgwickフィルタを紹介しておきます。Madgwickフィルタでは四元数をアルゴリズムのベースにおいています。カルマンフィルタベースのアルゴリズムと比較して良い結果が得られているようです。6軸、9軸両方でアルゴリズムが用意されており、低いサンプリングレートでも有効、演算数が少ない（つまり計算リソースの少ない組込み環境で適用可能）、調整が必要なパラメータが6軸で1個、9軸で2個と少ないという特長があります。論文（internal report）とソースコードも公開されていますので、興味のある方は調べてみてください。
磁界を用いた場合の欠点
では、「コストが許せば常に6軸よりも9軸が良い」ということになりそうですが、これは注意が必要です。周り（地下も含む）に何もない場合は問題ありませんが、室内や建物・人工物が近くにある場合は磁界が安定しないため、かえって不安定になることがあります。9軸センサを使用する場合は、「ジャイロに変化が見られないのに、磁界が急激に変化した場合は磁界を無視する」というような対処が必要になります。9軸を採用した開発中のクアッドコプターがコンパスの影響で墜落するということは現実に発生します。
室内など方角が必要ないケースや磁界の変化が想定される場合は磁界を使用せずに6軸センサを使用する方が良いかもしれません。室内利用の場合、北がどちらかが問題になることは少ないと思います。例えば全天球のコンテンツ（Ricoh Thetaで撮影したような画像・動画）を視聴する場合は前後左右がどちらかが分かっていれば北がどちらかは必要ありません。6軸センサと十分にドリフトが少ないフィルタがあれば、基準の方角を決めて相対的な方角がわかっていれば十分でしょう。もしくは環境が許せばその他のセンサを使用することもよいでしょう。
Madgwickフィルタのinternal reportではMagnetic distortion compensationの節があるので磁界の問題にも対応出来ているのかもしれません。
蛇足ですが、MicrosoftのHoloLensのように複数人で仮想環境を共有する場合は6軸センサだけでは足りません。何らかの方法で方角を一致させる必要があります。Oculus RiftやHTC Vive等のように外部環境にセンサやマーカーを配置するケースはわかりやすいのですが、HoloLensはどうやっているのでしょうね。
センサ外計算 vs センサ内計算（消費電力、サンプリングレート、カスタマイズ性）
ここまでで6軸もしくは9軸センサと適切なフィルタを用いれば姿勢が推定できることがわかりました。ここで一度忘れていたInvensenseのDMP（センサフュージョンをセンサ内で計算してくれるプロセッサ）のことを思い出しましょう。
センサフュージョンはセンサ内（InvensenseのDMP方式）とセンサ外（従来？方式）どちらで計算するのがよいでしょうか。いくつかの観点で見てみましょう。
- 消費電力
- センサ内が有利です。センサフュージョンの計算中はメインプロセッサは他のことができますし、やることがなければスリープしておくことができます。姿勢の推定計算はサンプリングレートが高いほど負荷が高くなりますので、センサ内に任せられるなら任せたいところです。
- 低サンプリングレート時の姿勢推定の精度
- センサ内が有利です。ちょっと説明が必要ですが、個人的にはこれがとても大きいと感じています。メインプロセッサの計算リソースが貧弱で、メインプロセッサがセンサから5Hzでしかセンサ値を取得できないとします。5Hzのサンプリングレートで姿勢推定を行うとMadgwickフィルタを持ってしても誤差が大きくなってきます。ところがInvensenseのMPU-6050の場合はセンサ内で200Hzでセンサフュージョンの計算を実施していて、結果だけを5Hzで出力することが可能です。
- カスタマイズ性
- センサ外が有利です。生のセンサ値を取得して好きなフィルタを適用することができます。加速度・ジャイロ以外の追加のセンサをフィルタに組み込むことができます。
- もし、DMPなどのセンサ内で計算した姿勢と、外部センサの値を組み合わせたフィルタが作れればさらに良いと考えます。
- また、Invensenseの新しいセンサの場合、特定のジェスチャが発生した時にイベントを起こすなどのカスタマイズができるようですが、このあたりはよく知らないので、興味のある方は最新の情報を参照してください。また他のメーカーにも同様にセンサ内部でセンサフュージョンできるセンサはあります。安価に試せる例としてMPU-6050は適当ですが、製品搭載する場合は各メーカーのカタログを見るなどして適切なセンサを使用するようにしましょう。
2. 座標系に起因する問題
座標系には気をつけろ（右手系or左手系、前方はx or y、z-up or z-down）
「数学よりの分野では右手座標系、CGよりの分野では左手座標系が多い」・・・、らしいのですが、それぞれで使用される座標系は確認が必要です。右手系・左手系に加えて、z-up, z-downなどもあります。 前方はxでしょうかyでしょうか、前方がzのこともあるかもしれません、よく考えずにセンサを配置してしまって気がついたらロボットの前方が負の方向ということもあるかもしれません。
これは、センサだけではなくて、たとえばROSやUnity 3D、Unreal Engine、Processing等を使うならその環境の座標系が必要ですし、センサを複数載せたら右手系と左手系が混ざっていたとか、カオスな状況が発生しうるので特に環境の立ち上げ時は座標系に十分に気を配ってください。変換は可能ですが、なるべく頭を悩ませたくないですよね。
オイラー角はひとつじゃない
四元数に直接は関係ないのですが、相互に変換して利用する人もいるかと思いますのでここに記載します。座標系に次いでオイラー角も鬼門です。はっきり言ってややこしいので私はオイラー角が嫌いです。まず、ロール・ピッチ・ヨー(RPY)とヘディング・ピッチ・バンク(HPB)と言葉が違うこともあります。
動画（拝借）を使って説明します。
私がよく使う言葉だと上記動画の各回転を、「ヘディング・ピッチ・バンク」の順に回転させたと呼んでいます。ただし動画はz-downですが、私がよく使う系はz-upです（既にカオス）。違う言葉を使うと「ロール」は「バンク」に対応し、「ヨー」は「ヘディング」と対応します。ラジコン飛行機なんかをやっている人はロール・ピッチ・ヨーの言葉のほうが馴染みが深いと思います。
別の表記だと回転させた軸を使ってz-y-xだという言い方もできます。そしてオイラー角は伝統的にはz-x-zがオイラー角らしいです（軸のとり方のパターンが増えて更にカオス）。
結局同じ回転をあらわすのだから相互変換はできるのですが、「回転を適用する軸の順序が違うだけだから簡単に変換できるだろう」と考えていると死にます。オイラー角は各軸の適用順序が変わると全く異なる結果になります。変換する場合は一旦回転行列を経由することで変換できます。
オイラー角のカオスさを体感するために、ROSのtf2のQuaternionクラスのAPIを覗いてみましょう。オイラー角関係の setXXX メソッドだけで3種類あります（おそらくsetEulerが標準的？）。
| void | setEuler (const tf2Scalar &yaw, const tf2Scalar &pitch, const tf2Scalar &roll) |
| Set the quaternion using Euler angles. | |
| void | setEulerZYX (const tf2Scalar &yaw, const tf2Scalar &pitch, const tf2Scalar &roll) __attribute__((deprecated)) |
| Set the quaternion using euler angles. | |
| void | setRPY (const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw) |
| Set the quaternion using fixed axis RPY. |
上記APIの最後のsetRPYを見るとfixed axisとなっているので、回転軸を慣性座標系（動画で描かれている座標軸）に固定するパターンがあるようです。ちなみに動画の場合はオブジェクト座標系（動画には見えませんが飛行機のパイロットから見える座標系と思ってください）の軸周りに回転させています。
長々書きましたが、とにかくオイラー角を使う場合は「座標系が複数ある問題」に「オイラー角の軸の適用順序問題」が掛け合わされるので、本やウェブ検索で見つけた変換式をそのまま引っ張ってこれないことが多いので要注意です。
一方で、特定の問題においては特定の順序のオイラー角を使うと問題がシンプルになるケースがあります。具体例はここで挙げることができないのですが、うまくはまると便利なケースもある憎いやつです（嫌いですけど・・・）。
またオイラー角には、別途ジンバルロックの問題があるので、オイラー角を使いたい場合は調べておいてください。四元数を使うことでジンバルロックから開放されます。
動作確認
4元数はひと目で正しいかどうかを判断するのが難しいです。「正しく描画できるかどうかわからないデバッグ中のソフト」で描画するまでセンサが正しく動いているかどうかわからない、というのでは困ります。
センサの座標軸がどうなっているかの確認はとても重要なのできちんと動作確認をしましょう。軸の確認方法を動画にしてみました。字幕が必須ですので字幕を有効にした上で見てください。
動画のようにまずは恒等四元数（ \(q=[w,x,y,z]=[1,0,0,0]\) ）となるポジションを探しましょう。まずセンサを水平にすることは確実なので、この状態で鉛直方向の軸周りに回転させると見つかります。符号反転した\([-1,0,0,0]\)が見つかることもありますが、この場合はもう一周回すと恒等四元数が見つかると思います。
次に各軸周りに回転させます。軸がどこにあるかはセンサの向きからわかるはずです。右手系の場合は右手の法則の向き（右手で親指を軸のプラス方向につきだして他の指を丸めた時の親指以外の指の方向）が回転のプラス方向になります。これを3本の軸に対して実施すればそれぞれの軸がX,Y,Zのどの軸か（関係ない軸の値は0で変動しない）、軸がどちらを向いているか（先の右手の法則）がわかります。左手系なら回転のプラス方向が逆になります。
この動画を見ると四元数のイメージがつきやすいかもしれません。四元数は一見とっつきにくいですが、単一の軸回りに回転させる分には値の変化はわかりやすいです。
Invensenseの仕様書はかなり調べたのですが、四元数でどの座標系を採用しているか明記してありません。ちゃんと契約したらなにかドキュメントが出てくるのかもしれませんがそのあたりはよくわかりません（もしかすると左手系にカスタマイズできるかもしれないですし）。とりあえず、動画の場合は右手系のz-upだということがわかりました。
3. 四元数に起因する問題
正規化は忘れずに
正規化されていると思って計算していると、計算誤差の蓄積で正規化された状態からずれてきて期待した結果が得られないことがあります。時々正規化する、もしくは使用しているライブラリを確認して、計算ごとに正規化を挟んでいることを確認しましょう。近い値が出てなんとなく動作する分、地味にやっかいな問題です。
四元数は１個の数と考えること
四元数は \(q=[w,x,y,z]\)となっているので、x成分やz成分だけ取り出して・・・、みたいなことを考えたくなりますが、4つで1セットです。特定の成分だけ取り出してもほとんど意味をなしません。
あえて意味を見出すなら\(q=[w, \boldsymbol{v}]\)としたときの \(w, \boldsymbol{v}\)であれば、どの程度回転させるのか\(w\)と回転軸\(\boldsymbol{v}\)とに分割するとある程度の回転の目安になります。
外積の定義と結合の順序
外積には定義が2種類あります。これによって回転を結合する際の順番が入れ替わるので、自分が扱っているライブラリがどちらの順序で外積を行っているかを確認する必要があります。基礎編は 実例で学ぶゲーム3D数学
で主に採用している定義にならいました。「主に」というのは、式を綺麗にする都合でわざと一般的な定義から外していて、その旨も書籍の中で説明してあります。書籍の中では一般的な定義と、基礎編と同じ少し外れた定義の両方が紹介されています。
制御への適用
制御理論をかじったことのある人向けの情報です。四元数を制御に適用することができます。ですが、私がよく理解できていませんので説明できません。
A quaternion-based attitude tracking controller for robotic systems , James Biggs, September 2015 のペーパーで紹介されているようなので興味のある人は見てください。わかったら教えてください・・・。\(q\)が現在の姿勢、\(q_c\)が目標姿勢、\(q_e\)がその差分（差分については前編で説明済）で、\(q_e\)を\(w\)と\(\boldsymbol{v}\)に分けて入力値に反映させてるような気がします。
四元数の歴史とその先へ
※この節は四元数を使う上で必要ありません。
2次元平面上での回転は、複素平面を考えると\(e^{i\theta}=\cos(\theta)+i \sin(\theta)\) このように書けます。オイラーの公式ですね。オイラー角でも名前が出てくるオイラーさんですが、この時点では四元数は発明されていません。のちに、もっと美しく表現したいと考えたハミルトンさんが複素数の世界に\(i\)だけでなく、\(j,k\)を導入すれば綺麗に回転を表せることを発見しました。
このハミルトンさんの19世紀の発見が、世紀を超えた今日の3Dグラフィックで酷使され、多くの人の目に触れるようになっているわけですから面白いですね。
ただ、四元数を使って色々なアルゴリズムを考えていると、四元数の持っている情報でもっと面白いことができると思うのだけれど、うまくいかなくて別の空間に持っていけないかなと感じることがありました。
調べていると幾何学的代数（Geometric Algebra）というものがあって、5次元空間で更に面白い計算ができそうだということがわかってきました。完全に「四元数入門」の範囲外ですが更に先へと進みたい方は幾何学的代数の世界へ一緒に行きましょう。
「幾何学と代数系 Geometric Algebra（森北出版）」が幾何学的代数について解説するおそらく執筆時点での唯一の和書で、ありがたいことにとてもわかりやすいと思います。読み進めると段々と代数系（四元数代数を含む）が統一されていきます。
最後に
これで、「組込み技術者のための四元数（クォータニオン）入門」を終えます。調べているとROS界隈で標準的に使われているようなので執筆を開始した時に考えていたよりも組込み系で四元数が使われているのだということがわかりました。
「四元数（クォータニオン）という名前のよくわからないもの」、「四元数の値は見ても全くわからない数値列」という認識が本連載で少しでも緩和されると幸いです。