HMC5883L(GY-271)という電子コンパスを借りた。これをArduino Nanoにつないで使ってみる。
Kinectやってたときも思ったが、センシングにおけるチャレンジは、ひたすらキャリブレーションとノイズ除去なんだなぁと思う。つまり、センサーから生の値を拾うのは非常に簡単なのだが、そこから意味のある値を拾い出すのが格段に難しい。機器やプログラミングの知識よりも数学の知識がものを言う。
HMC5883Lの使い方とキャリブレーションについてはここのページが詳しい。
Advanced hard and soft iron magnetometer calibration for dummies - DIY Drones
このページからMagMasterというサンプルプログラム一式をダウンロードする。その中にあるArduino_Code.inoというのが最も初歩的なプログラムで、これを見ればまずHMC5883Lから生の値を読み出す方法がわかる。これをArduinoに流し込んで実行し、シリアル出力を付属のMagViewer.exeで見てみると、デモビデオに示されているようなそこそこきれいな球状のポイントクラウドが得られる。しかし、ゼロ点がずれていて、このままではアプリケーションでは使えないことがわかるだろう。
上記のページではこれをどうキャリブレーションするかも詳しく解説されている。ただ、センサーを東西南北に対して正確に置き、その方向を変えながら幾つもの値を採取する方法のようで、そんな几帳面なのは私の性には合わない。それよりも、球に近い形のポイントクラウドが得られているのだから、それにフィットする(誤差の最も少ない)球の方程式を求める最適化問題を解けばいいんじゃね？と思うわけだ。というわけで、Octave(MatLabのクローン)を使ってやってみよう。
まず、先ほどのArduino_Code.inoを用いて、センサーをぐるんぐるん回転させて生成したシリアル出力をファイルに保存する。これをOctaveで読んでみて、ちゃんと球状にデータが取れていることを確認する。
global points
points = csvread("MagData.csv");
scatter3(points(:,1), points(:,2), points(:,3));
このデータに対する誤差が最小になるような球の方程式のパラメタを求める。超適当だが下記のようなコードでできた。sphere_errorが目的関数で、xが決定したいパラメタ(中心点と半径)である。目的関数とxの初期値を与えてsdp (sequential quadratic programming)を呼ぶと、sphere_errorが最小になるxを求めてくれる。
function retval = sphere_errors(p, x)
retval = sqrt( (p(:,1).-x(1)).^2+(p(:,2).-x(2)).^2+(p(:,3).-x(3)).^2).-x(4)
endfunction
function retval = sphere_error(x)
global points
retval = sum(sphere_errors(points, x).^2)
endfunction
x0 = [mean(points(:,1)), mean(points(:,2)), mean(points(:,3)), 100]
result = sqp(x0, @sphere_error)
これで中心点(154.50, -274.99, -139.04)で半径562.08という結果が出た。この中心点をオフセットとして減算すれば、磁力線ベクトルとしてまともに使える値が手に入る。ただ、リアルタイムで再計算できるものではなくコードに決め打ちになってしまい環境の変化には対応できないが、まあ仕方がない。
これで得た磁力線ベクトルを水平面に射影すればコンパスのようなものができあがる。ただし、これをZ軸の値を無視するといった手抜きで実現しようとすると、センサーを水平に保たないと正しい値が取れなくなり、これが意外と難しい。磁力線って水平に対して結構傾いているので(私の場所だと50度くらい)、センサーの傾きが変わると方向に激しく影響が出てしまうのだ。そこで、加速度センサーMPU6050を併用して水平面の変動をトラックするようにしたらそれなりに実用性のありそうなものになった。これについてはまた別の機会に。
