地磁気センサでそこそこの精度の電子コンパスを作ってみたという話。

12116 ワード
C++ コンパス mbed 電子工作 センサー C++ テキストリンク

今回使用した機器

・マイコン: mbed LPC1768
・地磁気センサ: AE-BMX055

そもそも電子コンパスとは何か？

　地磁気はその名の通り地球によって生じる磁場のことを指します。方位磁針も電子コンパスもこの磁場を利用しているわけですが、利用の仕方は全く異なります。
　方位磁針は単純に磁場の向きに沿って針を向けているだけですが、電子コンパスは地磁気のベクトルをx成分・y成分・z成分に分解し、それらの値を元に方位を算出しています。より汎用性・正確性を求めるならばこれら3成分を全て考慮に入れる必要がありますが、今回は煩雑さを回避するため電子コンパスは常に水平面上にあると仮定してx成分・y成分のみ考慮することとします。
　原理的に考えれば以下の図のようになります。

つまりθを求めるためには単純に
θ=arctan(y_mag/x_mag)
とすればいいわけです。

厄介なこと

　しかし、地磁気というのは非常に微弱なため周りの磁場に大きく影響を受けます。例えばスピーカーなどが近くにあるとスピーカー内部の磁石が大きな影響を及ぼすのです。試しに何も処理しない状態でこの地磁気センサをぐるっと1周させてみましょう。

#include "mbed.h"
#include "BMX055.h"
BMX055 bmx(p9,p10);
Serial pc(USBTX, USBRX);

int main(){
    while(1){
        bmx.getMag();
        double x_mag = bmx.magnet[0];
        double y_mag = bmx.magnet[1];
        pc.printf("%f,%f",x_mag,y_mag);
        wait(0.5);
    }
}

x_magとy_magの散布図をとると次のようになります。データポイント数は15個です。

確かに綺麗な円を描いているため一見成功したかに見えます。しかし、円の中心付近は原点とは程遠い場所にあります。簡単に言えば基準点がずれている状態になります。この状態で上記のθを求める式に値を代入しても正確な値には全くなり得ないことは容易に想像がつきます。

基準点の修正方法

　一番簡単な方法は一回このようにグラフに表して「だいたいxを+50、yを-80くらいしとけばいいか...」とやる方法でしょう。しかしいつでもグラフ化できるとは限りませんし、周辺磁場が変わったらいちいち設定し直さなくてはなりません。そこで上のコードに次のようなfix関数を付け加えます。

#include "mbed.h"
#include "BMX055.h"
BMX055 bmx(p9,p10);
Serial pc(USBTX, USBRX);

double fix_value[2];

void fix(){
    double max_buffer[2];
    double min_buffer[2];
    for(int i=0;i<30;i++){
        for(int v=0;v<2;v++){
            bmx.getMag();
            double tmp = bmx.magnet[v];
            if(i == 0){
                max_buffer[v] = tmp;
                min_buffer[v] = tmp;
            }
            else{
                if(tmp > max_buffer[v]){
                    max_buffer[v] = tmp;
                }
                if(tmp < min_buffer[v]){
                    min_buffer[v] = tmp;
                }
            }
        }
        wait(0.5);
    }
    for(int i=0;i<2;i++){
        fix_value[i] = (max_buffer[i]+min_buffer[i])/2;
    }
}

int main(){
    fix();//fix関数を呼び出してズレを算出
    while(1){
        bmx.getMag();
        double x_mag = bmx.magnet[0];
        double y_mag = bmx.magnet[1];
        pc.printf("%f,%f,%f,%f\n",x_mag,y_mag,x_mag-fix_value[0],y_mag-fix_value[1]);
        wait(0.5);
    }
}

具体的に中身を解説することはしませんが、簡単にいうとfix関数ではx_mag・y_magそれぞれの最大値と最小値を求めてそれを1/2することで簡易的に円の中心の値を求めています。このように実装すればセンサを起動させてから最初の15秒間だけセンサをぐるっと1周回しさえすれば正確な計測値を得ることができます。
散布図に表してみましょう。

青い点が修正する前、オレンジの点が修正した後の値です。修正後の方はかなり原点に近くなっていることがわかります。

　最後はこれをもとに方位を算出するだけです。様々な実装方法があると思いますが、私は以下のcompass関数を使っています。

double compass(double x_mag, double y_mag){
    double theta = atan(y_mag/x_mag)*180/3.1415;
    if(x_mag > 0){
        if(y_mag < 0){
            theta = theta + 360;
        }
    }
    else{
        theta = theta + 180;
    }
    return theta;
}

最後に

　以上のようにして地磁気センサを使ってそこそこの電子コンパスを製作することができました。より高度な技術を使えば自動的に誤差を修正するようなこともできるそうです。すげえ。