TADAです。 今回はArduinoとMPU6050 DMPを使ったIMUについて書きます。
RoboCupJuniorにおいては、ロボットの方向を検出するための方位センサには、地磁気センサを使ったコンパスが主に使われているようです。 しかし、コンパスセンサは試合会場の地場の影響を強く受けるため、地磁気環境の悪い屋内などでは、戦況が大きく不利になる事があります。
我々が観測できる地磁気は、身の回りの磁性源(例えばコンセントを流れる電流が引き起こす磁束など)に比べると、その大きさは大変小さいものです。 特に屋内では、エレベータや配電盤などの大電力を扱う機器が多数存在しますので、地球の地磁気はほぼ観測不能なレベルまで乱されてしまう場合が多いです。
コンパスを用いた方位センサの代替策として、ジャイロセンサを用いた慣性計測があります。 この原理を用いたセンサは、IMU(Inertial Measurement Unit : 慣性計測装置)と呼ばれ、地球上の静止状態での慣性を原点とした絶対角度を得ます。
しかしながら、ジャイロセンサの角度取得は計算過程で積分処理を行うため、計算結果には誤差が発生します。 これは一般的に「バイアス誤差」や「ドリフト」と呼ばれます。 角度計測においては、この誤差を減らす工夫が必要となります。
バイアス誤差が少ない角度情報を得る手段として、InvenSenseのMPU6050の内部計算機能であるDMP(Digital Motion Processor)を用いてる方法があります。 DMPを用いることで、センサーフュージョン(複数の種類のセンサの値から総合的に角度を計算する)による角度計算をICに委ねることができます。
実験回路
実験回路の実体配線図です。 ロボットに搭載する場合は、モジュールのGND・Vcc・TXをそれぞれロボットのメインマイコンに接続します。 DMPの計算用マイコンとロボットのメインマイコンを統合するのはお勧めしません。 計算量が変化しないような実装(例えばLED光らせる程度)なら大丈夫だと思いますが、基本的にDMP監視以外の実装は控えておくと無難です。
使用したマイコンボードの詳細です。 日本ではスイッチサイエンスさんで購入できます。 類似のArduinoボードでも全然動くと思います。
上記回路図におけるDMPのサンプルプログラムは以下からダウンロードできます。
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制御が簡単に実装できる)。
キャリブレーション
上のグラフは、モジュールを静止させた状態において電源を投入した直後から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モジュール
500円玉サイズで3軸フュージョンされたYaw角をUARTで読めるモジュールを作った。
— TADA:Yunit Mavericks (@yxtada) 2017年2月4日
動画はインジケーターのテスト
向いてる方向のLEDが光って正面がわかるようになってます。イニシャライズ中は全点灯。
プロトコル次第で角速度も水平も取れます(つける予定) pic.twitter.com/k3DwXaNRBU
以上で紹介したIMUは、2016年せとうちオープンからRoboCupJunior用に使い続けています。実用に十分耐える事が確認できたので、2017年度に基板化に挑戦しました。
これの基板を設計したのは、EAGLEを使い始めてからまだ間もない頃だったので、非常に出来が悪いと思っています。 本来考慮されるべき箇所が全然考慮されていないので、「たまたま動いてる」と思った方がいいかもしれません。
ダウンロード MPU6050IMU_EAGLE_Files.zip
基板は、オシロスコープで波形を確認する限りはマトモな波形が走っています。 あくまで「一応動いている」ということで、何か参考にでもなればと思います。
I2Cのプルアップ抵抗は、ATmega328pの内部プルアップで動いているので、設計ミスでは無いです。 ただ、本来はきちんと立ち上がり波形を測定しながら適切な抵抗値でプルアップするべきなので、これも1つの悪い設計として書き留めておきます。
実装にあたっては、GY-521ボードの回路図を参考にしました。
Arduino Playground - MPU-6050 のページより確認できます。
GY-521 Schematic jpg
コメント
コメント一覧 (23)
UARTを使っています.SPIは配線数が増える,IICは通信エラーからの復帰が大変,と消去法的に選んでいます.
I2CもSPIもRS232Cもシリアル通信ですが,それぞれどのような決まりの上でデータをやり取りするかが違います。通信規格における決まりのことを,プロトコルと呼ぶようです。
UARTは一般的に,シリアル信号(主にRS232Cの規格)をディジタル回路で処理できるパラレル信号に変換する集積回路のことです。Arduinoであればマイコンの機能として搭載されています。
RS232Cを使う場合はUARTを使ってデータを自動でやり取りさせる事ができるので,非同期通信が比較的実装しやすいと思います。加えて,ノイズで信号が中断されても,データは失われますが通信の連続性は失われません。
以上ですが,あくまで個人の知識の範囲でお答えしていますので,間違い等あるかもしれません。ディジタル回路については,私も勉強中の身です。
TADAさんは他のサブマイコン(ライン処理用など)ともUARTを使って通信させているのですか?
僕は,電磁波によるノイズの心配が少ない場所ではRS232Cを,モータのすぐ真下などの,電磁ノイズの影響が心配される箇所の通信にはRS485をそれぞれ採用しています。
ただ,あまり通信箇所を増やすのは,開発コストを増やす事につながりますので,まずは通信はなるべく確実な方法(デジタルピン同士のパラレル接続>アナログ>シリアル)を選んで,うまくシステムを設計してみてください。
ありがとうございます。
MPU6050.h/cpp内部の634行目から637行目にある
void getRotation関数
int16_t getRotationX/getRotationY/getRotationZ関数(3種類)
などを使って取得できると思います。
byte data; // 受信データ
void setup() {
Serial.begin(115200);
Serial1.begin(115200);
}
void loop() {
while{
if ( Serial1.available() >= sizeof(byte) ) {
data = Serial1.read();
}
}
Serial.println(data);
}
何が原因でしょうか?
void loop()の内部でwhileが使われていますが,条件が定義されていませんので,このままだとコンパイルが通らないかと思います。
また,loop()関数自体が,while(1)と同様の動き(無限ループ)をしますので,loop()関数の内部にwhileを入れる必要はないかと思います。
Arduinoでは,ループ処理は基本的にloop関数に任せた方が無難です。バックグラウンドで,loop関数と同期して動くシリアルの処理などが存在します。
もしシリアルのバッファ待機をするのであれば
受け取るバイト数を (int)data_length という変数に定義して
while(Serial1.available < dala_length);
などとしてみるのはどうでしょう。
sizeof関数の使い方は
sizeof(byte)=1なので,特に問題ないかと思います。
ただ,配列の要素数を汎用的に把握するために使われることが多いかなと思います。
whileの条件式が書かれていないので何ともいえませんが
Serial.println(data);
まで到達していない可能性もあります。
正常にデータが受信できていれば,ifの条件はクリアするかと思います。
最後に余談ですが
Arduinoのbyteという型は,一般的なC/C++におけるbyteと全く違う型ですので,今後プログラムを書いていく上では,使わない事をお勧めしたいです。
int8_t (-127 ~ 0 ~ 127)
uint8_t (0 ~ 255) ← Arduinoのbyteと同じ型
の使用をお勧めします。
InvensenseのMPU6050の技術データを参照しても,DMPに関する情報はOverviewのみしか情報が出てきません。私の調べ方が悪いのかもしれませんが…。
Yaw軸の角度検出に加速度センサを用いているかは分かりませんが,仰る通り,原理的に考えてYaw軸の補正に加速度センサを使っているとは考えくいため,加速度センサは主にXY水平角の検出に使われているのだと思います。
ジャイロによるYaw軸の角度計算は,高いサンプリングレートで,かつ温度換算をしながら積分する事で,十分に誤差の小さい角度が得られますが
DMPは,それらの面倒な実装をICチップ内部で自動で行い,バッファリングしてI2Cで読み出せるようにしたもの,という認識をしています。DMPで温度センサが使われているかは情報が見つけられませんでしたが,MEMSのジャイロでYaw軸のドリフト誤差をあのレベルまで減らすためには,温度は無視できないかなと思っています。
私も勉強不足でして,あまり正確な返答ができず,すみません。
自分でドリフト補正すると複雑で演算もかかってしまうので、DMPを使うことでそれらを解決することが出来るのですね。
勉強になりました!
回転角度・角速度をどのように処理するのですか?
PD制御の詳しい方法を文章だけでお伝えするのは難しいです.
回転角度と角速度の2つのデータを用いる所までは合っています.あとは偏差「目的の値と現在の値の差」の意味が分かれば,それぞれの次元である係数をかけて閉ループを構成すれば,フィードバックな系を構成できます.
以上がヒントになれば良いのですが,詳しいことは今後記事にすることも検討します.
今は,これ以上の説明は難しいです.ごめんなさい.
SerialのTXとRXを正しく(TX同士RX同士を繋がないように注意)接続して,後は標準のSerial.read()関数が使えるかと思います.ボーレートをSerial.begin()で指定するときに,送信側と受信側のボーレートを同じ値にするように気をつけて下さい.
ブレッドボードくらいの環境なら,115.2[kbps] = 115200bpsまでは問題なく送受信できると思います.それ以上は高周波領域ならではのアナログ回路的な要素を検討しなくてはいけなくなるかな,と思います.
また,本文にてこの基板の出来は悪いと評価していますが,具体的にどこが良くないのでしょうか?
GNDビアを多めに通しているのは、基板のパターンを作るときにノイズの原因になるような浮いた(GNDに接続されていない)銅箔の浮島を作らないようにしつつ、2層のGNDがなるべく小さい抵抗(インピーダンス)で繋がるように数を確保しているかたちです。
実装が良くないのは回路図とパターンどちらにも言えることですが、回路図は主にI2Cのプルアップが無い等の根本的なところや、電源ラインのパスコンや保護用の実装がないこと。パターンについてはクロックやI2Cなどの本来ノイズがケアされていなければならないラインがGNDでしっかり囲われていない、クロストークが起こりうるような距離で並走してパターンが走っている箇所がある、ビアの上にシルクが乗っている部分がある、などです。いずれも基板サイズを大きく取れば解消しやすい問題なので、単純に小さく作りすぎたと言う事もできますね。
>>22 の回答にて「電源ラインの保護用の実装がない」と記述されていると思いますが,何を目的にどのような実装をしますか?
大電流は考えづらいと思うのでヒューズを挟むわけではないと思うのですが,ツェナーダイオードなどでサージ電圧をクラッピングするのでしょうか?
すみません返信が遅れてしまいました。
まさに書いていただいている通りで、ツェナーでのクランプや逆説保護用、もう少し安全を取るならマイコンと通信する信号ラインにも絶縁ICを挟んだりもすると思います。背景としては、ロボット全体がひとつのバッテリを共有していて、ある別の負荷が急激に電流を吸った場合、典型的にはモータの動き始めやキッカーの充電開始などが、今回であればIMUのような別のデバイスに影響することを懸念しているものです。おそらく最もイメージしやすいのはGNDの電位が変動する問題で、バッテリからデバイスまでの経路が別のデバイスと共有されていたときに、その経路の電圧降下が別のデバイスのGNDの電圧を変動させる現象が起きます。こうしたとき、本来意図していたよりも高い電圧が、GND以外のピン、つまりはVCCだけでなく信号線にも付加される瞬間が発生します。サッカーロボットはその性質上、電流を急激に吸うモータやキッカードリブラー等のデバイスが多いことと、小型に収めるために経路を共有しがちという2つの背景が重なって、この問題がよく起きると考えています。そこで、デバイスとしての安定性を少しでも良くすることを考えると、なるべく共通インピーダンスを減らすような結線で設計しつつ、各デバイスにはGNDが揺れるようなノイズや電源異常に対処する機能を持たせたほうが良い、と考えるようになります。