MadgwickフィルタとINS/GPS複合航法の比較

2021/12/24

センサ

人力飛行機 Advent Calendar 2021 12/24の記事です。


現在、Team 'F' Nextz Plus/Nextz Avantで運用している電装系では、姿勢角(ピッチ・ロール・ヨー)の推定にINS/GPS複合航法を利用しています。

INS/GPS複合航法では姿勢角の計算を10次元以上の状態空間を用いて行うため、姿勢推定をリアルタイムで行うためには計算能力の高いマイコンが必要です。

それに伴い電装系の消費電力が増加し、必要なバッテリ容量が大きくなり、重量も増加します。


計算能力の低いマイコンでも姿勢角が計算可能なアルゴリズムとして有名なものにMadgwickフィルタがあり、人力飛行機での採用例もいくつか見聞きしたことがあります。

Madgwickフィルタで十分な精度で姿勢角を求めることができれば、計算能力の高いマイコンを使う必要がなくなることから電装系の軽量化につながります。

また、アルゴリズムが比較的シンプルなので実装も簡単に行えます。


そこで、Madgwickフィルタを用いて姿勢角推定が十分な精度で行えるかどうか、実際のフライトデータを用いてその実力を評価してみました。

解析対象のデータは、速度競技機としては比較的長時間の飛行であり、飛行中に複数回の旋回を含む、2012年の速度日本記録樹立時のものを用いました。

加速度・ジャイロセンサは恒温槽・レートテーブルによる校正済みですが、地磁気センサはオフセットのみの簡易校正になっています。

加速度・ジャイロセンサのデータ取得間隔には多少のジッタがあるので、解析用に1次補間により8 ms刻みのデータを生成しました。

また、地磁気センサのデータは80 ms刻みでの取得なので、こちらも1次補間により8 ms刻みのデータを生成しました。


Madgwickフィルタにはジャイロの単純積分から求めた姿勢と、加速度または加速度+地磁気センサから求めた姿勢のどちらに重きを置くかに対応するパラメータβがあります。

β=0はジャイロセンサの単純積分で求めた姿勢角に対応し、βを大きくするにしたがい加速度・地磁気センサによる補正量が大きくなります。

このパラメータは姿勢推定対象のダイナミクスやジャイロセンサの特性によって調整が必要なので、複数のβについて解析を行っています。

解析はさまざまな姿勢推定アルゴリズムを集めたPythonパッケージAHRSを用いて行いました。


Madgwickフィルタに限らず、加速度・ジャイロセンサを用いて姿勢角を求める場合、重力以外に大きな力が姿勢推定の対象に加わらないことを仮定しています。

人力飛行機の場合、発進・旋回等で機体に加速度が加わるので、特にそのような条件下での姿勢推定精度を評価します。


異なるβに対する解析結果を下の図に示します。

姿勢角は地磁気センサを含める場合・含めない場合の両方の条件で計算しました。

図中の赤線が地磁気センサを含めない場合、青線が地磁気センサを含めた場合に対応します。

リファレンスとしてINS/GPS複合航法によるリアルタイム姿勢角推定値もプロット(図中の黒線)しています。

離陸時刻は82048ごろ、着陸時刻は82273ごろです。

Madgwickフィルタの収束に時間がかかる場合があるので、データ取得開始時からのフライトログをプロットしています。

今回はINS/GPS複合航法による姿勢が正しいものとしてMadgwickフィルタを評価します。











βが大きい場合、慣性センサのデータが得られた各時刻で計算した姿勢にノイズ・振動の影響が含まれることに対応して、得られる姿勢角にもノイズが多く含まれています。
βが0の場合、ジャイロセンサのみから姿勢角を推定するので、地磁気センサの有無による姿勢推定値の違いがなくなっています。
ピッチ・ロール・ヨー角のそれぞれ対してβを適当に調整すれば時間領域によってはそれなりの推定精度が出るようですが、フライト時間全域にわたって、またすべての姿勢角が高い精度でリファレンスに一致するβはありませんでした。
Madgwickフィルタの姿勢推定の原理から予想される通り、旋回時に加速度が加わった場合に姿勢角のずれが大きくなる傾向があります。
また、地磁気センサを推定に加えた場合、ヨー角の推定精度が向上することがわかります。
これは、重力加速度の方向(鉛直下向き)がヨー角を求める際のリファレンスになり得ないこととも符合する結果です。
プロット開始付近では、Madgwickフィルタが収束していく様子が見えますが、βを小さくするにつれ収束が遅くなっています。
これは、加速度・地磁気センサで一度に補正される量が小さくなっていくことに対応していると考えれば、βの増減と矛盾しない結果になります。
地磁気センサを姿勢推定に含めた場合に収束は遅くなっていますが、地磁気センサの校正が完全ではないために、正しく姿勢を収束させにくくなっていることが要因として考えられます。

以上の結果から、Madgwickフィルタでは特に旋回時には正しい姿勢を求めるのは困難なことがわかりました。
他にも加速度・ジャイロ・地磁気センサのセンサフュージョンで姿勢角を求める手法はいくつか存在するので、それらの評価も行い最終的に姿勢推定に用いるアルゴリズムを決定しようと思います。

しかしながら、加速度・ジャイロセンサのみを用いるアルゴリズムでは原理的に加速度環境下では姿勢推定の精度が悪化すること、地磁気センサは磁場環境の影響等で高い精度での校正が難しいことなどから、結局のところINS/GPSアルゴリズムを用いることになるのではないかと予想しています。

INS/GPSアルゴリズムを動かすにあたり十分な性能を持つマイコンボード(STM32H7@400 MHz + SDRAM 256 Mbit)はすでに作製済みですので、今後は姿勢推定アルゴリズムを含むファームウエアの実装を進めていきたいと思います。