IMG_0106

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

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

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

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

しかしながら、ジャイロセンサの角度取得は計算過程で積分処理を行うため、計算結果には誤差が発生します。 これは一般的に「バイアス誤差」や「ドリフト」と呼ばれます。 角度計測においては、この誤差を減らす工夫が必要となります。

MPU-6050 | InvenSense

バイアス誤差が少ない角度情報を得る手段として、InvenSenseのMPU6050の内部計算機能であるDMP(Digital Motion Processor)を用いてる方法があります。 DMPを用いることで、センサーフュージョン(複数の種類のセンサの値から総合的に角度を計算する)による角度計算をICに委ねることができます。

実験回路

IMU実体配線図

実験回路の実体配線図

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

使用したマイコンボードの詳細です。 日本ではスイッチサイエンスさんで購入できます。 類似のArduinoボードでも全然動くと思います。

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つの悪い設計として書き留めておきます。

実装にあたっては、GY-521ボードの回路図を参考にしました。

Arduino Playground - MPU-6050 のページより確認できます。

GY-521 Schematic jpg