センサのお話

こんばんは．更新がすっかり遅れてしまいました．
機体はボードを搭載して一通りのデバッグは完了しており，
今度の日曜日には飛行試験を行う予定です．

今日はセンサの特性とデータ処理について書きたいと思います．
IMUには３軸ジャイロと３軸加速度センサが搭載されています．ジャイロは角速度を検出するセンサです．これらをdsPICのADコンバータでサンプリングし，移動平均フィルタで平滑化しておきます．
さらに加速度センサはまだノイズが見られたのでさらにローパスフィルタ（IIR）をかけています．

さて，今求めたいのは機体の姿勢です．一口に姿勢といっても表現方法はいろいろありますが，
ここではピッチ角，ロール角，ヨー角とします．また本来は機体固定座標系での角速度から機体の姿勢を求めるには非線形の微分方程式を数値積分して回転行列を求める必要がありますが，ここでは初期姿勢から大きく変わらないとし，姿勢角は近似的に角速度の成分を積分したものに等しいとします．

例として，ロール角を求める場合を考えます．これには以下の２通りの方法がありますが，どちらも欠点がありそのままでは実用的とはいえません．

１．ジャイロから得た角速度を積分する．
この方法は数値積分に頼るため誤差が時間とともにたまっていきます．またジャイロにはオフセットが時間とともにランダムに変動するドリフトという厄介な特性があるため単純な積分ではすぐに発散してしまい使い物になりません．

２．加速度センサの値から重力の方向を求めて計算する．
この方法はiphoneなどで使われています．しかし手の平で使うiphoneとちがって空間を飛び回る飛行機には重力以外にも機体自身の加速度や姿勢の変化に伴う遠心，コリオリ力も加わり，当然加速度センサはこれらをすべて拾ってしまうため誤差が大きくなりあまり良くありません．

そこで，１と２のいいとこ取りを考えるわけです．センサフュージョンというやつですね．それを実現するアルゴリズムが，カルマンフィルタといわれるものです．詳しく説明すると長くなるので参考サイトのＵＲＬを貼っておきます．書いてる本人も詳細な理論はまだまだ勉強中ですが・・・．

実際に適用した結果は下の画像の通りです．青が方法１，赤が方法２，緑がカルマンフィルタを用いた結果です．なお，参考サイトではゲインを毎回計算していますが，ここではオフラインで良さそうな値を求めて固定しています．（チューニングは他のメンバーの人たちがやってくれました）

 
 前半の全てのグラフが振動している部分は機体を左右にロールさせています．後半の赤いグラフだけが振動している部分は，機体を水平にしたまま左右に動かしています．また最初と最後はどちらも水平に静止させた状態です．赤のグラフは平行移動も検出してしまい，また青のグラフは積分誤差のため最後に水平に戻っていませんが，緑のグラフはロール運動のみを検出し，かつ最後はきちんと水平に戻っていることがわかります．

参考URL: Kalman filtering of IMU data
http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data