続06) Arduino始めてみた - 方位センサー (GY-271/HMC5883L)

サッカーでは、自分のゴールと相手のゴールを区別し無くてはならない。でないとオウンゴールを量産する。 ということで、ロボカップジュニアでは、ゴールを青色と黄色に塗り分けて敵と味方を区別している。
ゴールの色を識別する方法が人間のやり方に近いけど、色を認識するセンサーは精度と反応速度の面からあまり主流ではなく、代わりにその場の磁場を読み取ってマシンの向きを検知する方位センサー(地磁気センサー)が主流である。
方位センサーで何が分かるかというと大雑把に言うと、画像のXY軸が地面に平面方向で方角がわかり、Z軸で地面に対する傾きがわかる、でいいかな。
また、現実的には建物内の電力線にから電磁波は撒き散らされているので地磁気センサーから取得したデータをそのまま使うことができない。補正する必要がある。 後述する。

GY-271
私のマシン用には、いくつかの候補の中から安価なGY-271を購入した。
GY-271は3軸の地磁気センサーHMC5833Lに電源チップやI2C通信機能などを加えてコンパクトに使いやすくしたもの。
重要なのはHMC5833Lである。GY-271の中央にある黒い正方形の箱がそれ。
詳しく知りたい場合はこちら(英語)を参照する。
サンプルプログラムを単体試験で使用した。
また、データシートはこちら(PDF/英語)。
データの取得間隔は15Hz　(67msec周期)。
1枚目の画像がGY-271を取り付けた図。緑色のボードの裏側に取り付けた。間に合わせのやっつけです。
環境電磁波からの影響を避けるためにもう少し高いところに設置したかったが、手持ちの材料ではこれがイッパイイッパイ。

Arduinoに接続
GY-271には5本の端子が出ている。また、I2C通信なので信号はSCL/SDAに接続する。
それぞれの接続先は次のとおりとなった。

• VCC　→　5V　(＋)
• GND　→　Ground　(−)
• SCL　→　A5　(SCL)
• SDA　→　A4　(SDA)
• DRDY　→　接続しない 

2枚目はGY-271を裏から見た画像。
ボードに端子用の穴を開けている。

方位を知る
GY-271から取得できるデータは、X軸、Y軸、Z軸の値である。
XとYは北を向くと数字が小さくなる。
参考までに、地磁気には偏位があるので(磁気偏角)、地域によって方位が偏る。
国土地理院のページを参照。

さて、方位センサーでXとYの値がわかると、中心(X0,Y0) = (0,0)からの角度が分かる。
(X0,Y0)を基準として、XYの値から角度を求めるには、タンジェントの逆関数を使えばよい。
具体的には次の式となる。

角度(ラジアン) = atan2(y,x);
// atan2(y, x) : arc tangent of y/x

または、角度(°)だと、

角度(°) = atan2(y, x) * (180 / PI);
// PI = π = 3.14159265...

もしくは、Arduinoに用意されている次の定義を使ってもOK。

角度(°) = atan2(y, x) * RAD_TO_DEG;
// RAD_TO_DEG = (180 / PI) : ラジアンから度(°)への変換

地磁気データを補正
しかし、地磁気というのは、周囲に電磁波があると影響される。
建物の中などで電力線が電磁波をまき散らしていると、地磁気センサーで取得する値が大きくずれてくる、というのが現実である。

追記
以下のサイトが非常に参考になった(ほとんど真似っ子である)。
demura.net - e-gadgetやTJ3から使える地磁気センサGY-271(HMC5883L使用)



2枚目の画像が、マシンを回転させて、その時のGY-271の値を読み取り、グラフにプロットしたもの。横軸がXで、縦軸がY軸である。

3F-1の◯(青色)と3F-2の◯(緑色)は自宅の3階のPCデスク上 、2Fの◯(黄色)はその真下の2階である。
また、取得したXYのそれぞれ最高値と最小値から円の中心を求めてある。
すると、どうでしょう、円の中心は(0,0)ではなく、大きずれているのが分かる。
しかし、このプロットされた値をそのまま使うと角度がおおよそ260°〜315°くらいの範囲となり、マシンがどこを向いても常に一つの方位をむいているように見えて、結局値からはマシンがどこを向いているのか分からない。
よって、中心点を(0,0)に補正する必要がある(引き算ですね)。

角度補正(°) = atan2(y - centerY, x - centerX) * (180 / PI);

また、別の日に取得しした、自宅3階と大会会場の中心点の記録が残っていたのでがプロットする。3階3F-3がＸ(橙色)で、大会会場が＋(赤色)である。
グラフからも分かるように3F-3Ｘと大会会場＋は3F-1◯、3F-2◯と2F◯とは中心点がずれている。
その都度場所に合わせた補正が必要になることがわかる。
(HMC5833Lのデータシートを読むと、そもそもこのセンサーの値は大丈夫かという気もするが、実用上はグラフを見ての通り、方位を求められるのでなんとかなると考えている)

実践
以上の準備を経て、マシンを試合コートに設置した時に相手ゴールに正面を向け、電源を入れると、そこを正面=「0°」とするように補正し、常に正面を向くようにスケッチ(プログラム)を仕上げることとした。
まず、試合前の準備として、コート上の地磁気の補正のためにマシンをスピンさせてXYの中心値を求める。
上記の結果、本番のスケッチでは地磁気センサーに関して次の動きを行う。

1. 宣言
1. 中心値を定義する 
2. setup()
1. 補正1(中心)：中心のズレの補正を行う。
2. 補正2(角度)：電源を入れた時(つまり初期値)、正面の角度を「0°」に補正する。
3. loop()
1. 正面軸(0°)とのズレを常に監視して、ずれたら「0°」を向くようにマシンを左右にスピンする。