IMG_0106
TADAです。今回はArduinoとMPU6050 DMPを使ったIMUについて書きます。

RoboCupJuniorにおいては,ロボットの方向を検出するための方位センサには,地磁気センサを使ったコンパスが主に使われているようです。しかし,コンパスセンサは試合会場の地場の影響を強く受けるため,地磁気環境の悪い屋内などでは,戦況が大きく不利になる事があります。

我々が観測できる地磁気は,身の回りの磁性源(例えばコンセントを流れる電流が引き起こす磁束など)に比べると,その大きさは大変小さいものです。特に屋内では,エレベータや配電盤などの,大電力を扱う機器が多数存在しますので,地磁気がほぼ観測不能なレベルまで乱されてしまう場合もあります。

コンパスを用いた方位センサの代替策として,ジャイロセンサを用いた慣性計測があります。
この原理を用いたセンサは,IMU(Internal Measurement Unit : 慣性計測装置)と呼ばれ,地球上の静止状態での慣性を原点とした絶対角度を得ます。

ただし,ジャイロセンサの観測量は角度の時間1回微分であるため,角度の追従においては積分計算の過程で誤差が発生します。これは一般的に”バイアス誤差”や”ドリフト”と呼ばれ,角度計測においては,この誤差を減らす工夫が必要となります。

MPU-6050 | InvenSense

今回は,InvenSenseのMPU6050の内部計算機能であるDMP(Digital Motion Processor)を用いて,センサーフュージョン(複数の種類のセンサの値から総合的に角度を計算する)による角度計算をする方法を紹介します。

実験回路

IMU実体配線図
実験回路の実体配線図です。

ロボットに搭載する場合は,モジュールのGND・Vcc・TXをそれぞれロボットのメインマイコンに接続します。DMPの計算用マイコンとロボットのメインマイコンを統合するのはお勧めしません。計算量が変化しないような実装(例えばLED光らせるとか)くらいなら大丈夫ですが,基本的にDMP監視以外の実装は控えておくと無難です。

使用したマイコンは以下です。
日本ではスイッチサイエンスさんで購入できます。
Pololu - A-Star 32U4 Micro

上記回路図におけるDMPのサンプルプログラムは以下からダウンロードできます。
MPU6050IMU.zip (ダウンロード)

UARTデータ出力

サンプルプログラム中のUART出力の部分です。

Serial1.write((uint8_t)(ypr[0] / PI * 128.0));

(uint8_t) 0~255 でYaw軸角度が出力されます。
出力周期はMPU6050のDMP周期に依存します(100Hz)

データとしてはypr[]が -PI ~ 0 ~ PI の値を取ります。
ここで,PIは円周率を示します。角度のラジアン表記の説明は今回は割愛します。
参考:ロボティクスにおける角度情報の取り扱い方法 - MyEnigma

データのプロトコルは自由に作ることができると思うので,必要な情報を選んで出力すると良いと思います。MPU6050.h/cppに実装されている機能も使用可能ですので,角速度と角度を別に送ることも出来ます(PD制御が簡単に実装できる)。

キャリブレーション

MPU初動
上のグラフは,モジュールを静止させた状態での,電源を投入した直後から20秒後までのDMPの出力を監視したものです。
オフセットとは,センサーの初期状態でのバイアス誤差を打ち消す量のことです。
ここではキャリブレーションとは,このオフセット量を適用する事を言います。

オフセットを適用する事で,電源投入直後の挙動が安定します。
ただし,グラフから分かるように,電源投入直後の2.5秒間は不確実なデータが吐き出されてくるので,オフセットの有無に関わらず,開始2.5秒間は静止させておく事が望ましいかと思います。
配布したサンプルプログラムには,void setup()内部に2.5秒間の待機処理が実装さています。


キャリブレーションの参考になる動画を見つけたので貼っておきます。

動画説明欄のリンクよりキャリブレーションプログラムをダウンロードします。
DMP関係のソースコードの元はGithubに公開されているので,そちらで探しても入手できます。
LIBRARIES AND SKETCHES(ダウンロード)

void imu_attachSensorOfset(int16_t XG, int16_t YG, int16_t ZG, int16_t ZA); 

サンプルプログラム中の,オフセットを適用する関数です。
void setup()内部で実行されていますので,Calibrationのプログラムを実行して得られた値のうち,ジャイロX,Y,Zおよび加速度Zの値を与えて,オフセットを適用してやります。引数の順番等は,動画を参照するかソースコードを読めばすぐ分かると思いますが,配布プログラム中にも引数との対象関係が(分かりにくいですが)書かれています。

DMPを使った自作IMUモジュール


以上で紹介したIMUは,2016年せとうちオープンからRoboCupJunior用に使い続けています。実用に十分耐える事が確認できたので,2017年度に基板化に挑戦しました。

これの基板を設計したのは,EAGLEを使い始めてからまだ間もない頃だったので,非常に出来が悪いと思っています。本来考慮されるべき箇所が全然考慮されていないので「たまたま動いてる」と思った方がいいかもしれません。

ダウンロード
MPU6050IMU_EAGLE_Files.zip 

基板は,オシロスコープで波形を確認する限りはマトモな波形が走っています(データは出せませんが)
あくまで「一応動いている」ということで,何か参考にでもなればと思います。

I2Cのプルアップ抵抗は,ATmega328pの内部プルアップで動いているので,設計ミスでは無いです。
ただ,本来はきちんと立ち上がり波形を測定しながら適切な抵抗値でプルアップするべきなので,これも1つの悪い設計として書き留めておきます。
29
実装に当たって,GY-521ボードの回路図を参考にしました。
Arduino Playground - MPU-6050 のページより確認できます。
GY-521 Schematic jpg

以上です。
質問はコメントかメッセージ(公開されません)にて受け付けています。