InvenSense製慣性センサでのDigital Motion Processorの使い方

2012/12/20

センサ

計器に搭載しているInvenSenseの加速度・ジャイロセンサですが、Digital Motion Processorと呼ばれる機能を使うと、センサの生データだけではなく6(9)軸分のセンサデータから計算したQuaternionによる姿勢出力を得ることも可能です。
Quaternion出力が得られれば、(逆)三角関数の計算数回程度で演算能力の低いマイコンでも姿勢角を得ることができます。

以下にDMPの使い方の参考になる情報源等を紹介しておきます。
DMP出力を利用するためには、センサ内蔵のプロセッサにコードを流し込む必要があります。
初期化コードは最大で3 kByteほどのようです。

1. I2Cdevlibで公開されているライブラリ

多数のI2Cデバイス用のライブラリを公開しているI2Cdevlibのライブラリを利用するのが今のところ最も簡単なDMPの使い方だと思います。
基本的にArduino向けに書かれたものですが、I2Cの足回りを変更すれば他のマイコンでも使えます。
ただし、必要に応じてCに移植する必要はあります。

ここで公開されているコードはInvenSense製の評価キットのI2Cバスの信号を解析した成果のようで、生データも公開されています。

このライブラリを計器に組み込み、バーグラフにピッチ角を表示したものを下に示します。


I2Cの足回り等のデバイス依存部をPSoC 3/5用に移植しC++で動作確認後、公式にサポートされているCに移植しました。
PSoC CreatorはオフィシャルにはC++には対応していませんが、問題なくコンパイルは行えるようです。

2. InvenSenseのDeveloper's Cornerで公開されているライブラリ

InvenSenseもDMPを使うためのライブラリを公開していますが、基本的にはI2Cdevlibのものと中身は同じです。
I2Cdevlibにはない最新版(ver. 5.1)のコードはMSP430用のものしかないので、マイコン用のライブラリよりドキュメント類や可視化プログラムのほうが有用です。
近日中にver. 5.1のデバイス非依存なコードがリリースされるとの情報がForumでは確認はできていますが、現時点では利用できないようです。

Developer's Cornerからダウンロードできる資料によると、Quaternionはジャイロの積分値を加速度センサで補正して求めているようです。
また、計算は固定小数点で行われているようです。

公開されている可視化プログラムのTeaPotデモを実行した例を下に示します。

静置試験開始時
10分後

姿勢角が正しく取得できていること、10分間の静置の後も重力加速度による補正が効かないヨー角のドリフトが少ないことが確認できています。
この実験は地磁気センサなしで行ったものですが、地磁気補正を入れればさらにヨー角の安定性は増すと思われます。

3. MotionFit SDKのバージョンに関する注意

プロセッサに流し込むコードにはver. 2.0, ver. 4.1, ver. 5.1の3種類があるようです。
これらの区別は、256バイトごとに区切られるメモリバンクの先頭を比較することで行えます。
Bank 4の先頭がver. 2.0, ver. 4.1, ver. 5.1ではそれぞれ0xB4, 0x96, 0xD8となっています。
ver. 5.1より古いものでは加速度センサ・ジャイロセンサのゲインは指定された2g/2000dpsでないとならないらしく、それ以外の設定では異常な姿勢角が出力されることが確認できています。

現時点では一般のマイコンでver. 5.1でDMPを利用できるライブラリはないので、MSP430とMPU-6000をつないだ評価キットもどきを作って、I2Cバスの信号を解析しようとも考えています。

センサの組み合わせ基本的に加速度とジャイロなので、大きな加速度がかかるような環境下では正しい姿勢角が計算できないはずです。
加速度がかかった場合にどの程度の姿勢角のずれが生じるのかをテストして、(特に人力飛行機用として)どの程度使えるかを確認するつもりです。