センサー入力から姿勢制御できるまでをわかる範囲でまとめました。

1. さるでも分かりたい9dofで作
るクォータニオン姿勢制御
2015/10/18
丹野嘉信
http://ytanno.herokuapp.com/
tannoyoshinobu@gmail.com

2. 目次
• 9dof?クォータニオン?姿勢制御?
• 初期定義
• 処理の流れ
• プログラムから読む補正と解説
• 参考
• ＱandA

3. 色々説明
• 9dof ? 3次元 x (加速度 + 角速度 + 方位） = 9つ のデータ
• クォータニオン? Q = (w, x, y, z) からなる座標系
ジンバルロックというエラー回避、計算楽なので使用するといい
• ゴール
9dof のデータから回転後の物体表示を初見でもできるようになる

4. 初期定義
• 初期 Ｑinit = ( w, x, y, z ) = (q1, q2, q3, q4) = ( 1, 0, 0, 0)
• 初期 回転行列 Mt = 4x4の単位行列
• ジャイロセンサーの出力値 Sg = (gx, gy, gz);
• 加速度の出力値 Sa = (ax, ay, az ) ;
• 方位 Sm = (mx, my, mz);

5. 処理の流れ（一番大事）
1. 回転行列を使って物体表示
2. 9dofのデータ取得
3. 補正（色々ある） → 実はジャイロセンサーだけでも
値は出せるが他のセンサで補正しないと大体使えない
1. クォータニオンの更新
2. 更新したクォータニオンから回転行列に変換
(視点が固定、ＯｐｅｎＧＬの時は逆向き（共役な4のデータ）を変換
1－5の繰り返し

6. プログラムから読む補正の解説
• Madgwick AHRS
• 必要な初期パラメータ
samplePeriod（調整必要） , Algorithm gain beta（調整必要）, Ｑinit
• Ｑ更新に必要なデータ Sg Sa Sm

7. 変数設定
• 行列計算で使う計算を事前に用意

8. 加速度、方位からノルム補正

9. 地球の磁場の方向の計算
• 計算根拠は分からない

10. 補正のパラメータ計算
• 確率的勾配降下法周りだと思うけど時間かけて論文読まないとわか
らない

11. クォータニオンの更新
• 補正値消しても値はでる
（ジャイロセンサーだけでも理論上データはとれる）

12. 最後の処理
• クォータニオンの共役を見つけ、回転行列に変換、物体に反映

13. 参考 2番目に大事
• http://www.x-io.co.uk/ 神サイト C#のオープンソースもある
画像はこのサイトのもの
• http://myenigma.hatenablog.com/entry/20130719/1374304154
クォータニオンの分かりやすい解説
• http://www.ina111.org/archives/770
補正の解説
• http://marupeke296.com/DXG_No58_RotQuaternionTrans.html
クォータニオンから回転行列変換

14. QandA
• なぜ日本語？
-> 日本語で実用的な解説サイトがないから
• 磁場計算と補正式の具体的な説明がないんですけど？
-> 時間かけないと分からない。生活費ください
• ライブラリ使えばいいだろ。こんな解説に何の意味があるのか！？
->ある程度知らないと困るだろ！
• 他に補正アルゴリズム何あるの？
->MahonyAHRSってのがあった。あと、
->方位センサ抜きの補正方法もあるみたいです。