Arduinoで地磁気センサ（LSM303D）を使って方位角を計算してみる
卒業を間近に控えた機械系学生の筆者、ちょっと暇になってきたので技術系の何かをアウトプットしてみたくなってきた。新しい知識を仕入れたり、やってみたことをブログに書いてみることに。
今回はArduinoを使って地磁気センサ（LSM303D）からごく簡単に方位角を取得するまでをまとめてみようと思った。
今回の記事の要点は以下の通り。
- マイコンはArduino Megaを使う（Arduinoシリーズなら他のやつでもいいと思う）
- LSM303Dのライブラリのインストール
- とりあえず値を読んでみる
- ２軸の取得値からシンプルな方位角の計算方法
- 自分の研究に使ってみた
地磁気センサ（LSM303D）を使うことになった背景
筆者はメカトロ系の研究室に所属しており、卒業研究的なアレでドローンの運動制御について研究していた。その船首方位の維持のために方位角を知る必要があった。
ロボコンで活動している友人にオススメの地磁気センサを教えてもらい、購入したのが今回扱うLSM303Dだった。
これがそう↓
ざっくり説明すると３軸の加速度と磁束密度を測れるセンサですね。
用意するもの
Arduino Mega
ArduinoとPCと接続するUSBケーブル
LSM303D （付属のピン含む）
ジャンパー線（4本）
ブレッドボード
早速使ってみる
単に研究で使ってたのが丁度これだっただけなので特に深い意味はなかったり。別に他のArduinoシリーズでもいいんじゃないでしょうか。Unoとか。
弊研究室にはArduino UnoとMega、あと去年買ったGnuino101が自由に使えるマイコンとしてあります。
ライブラリのインストールとスケッチ例
Arduino IDEを起動 バージョンは1.8.5を使ってます。
まずはLSM303DをArduinoで手軽に扱うためのライブラリを追加します。
ライブラリの管理をクリックするとこんな画面が出るので、LSM303Dで検索をかけてみます。
それっぽいライブラリが出てきましたね。私は既にインストール済みなので赤く囲った部分に[インストール]が出てきてますが、初めてなら右のほうに出ると思います。
バージョンはとりあえず最新のものを入れました。
インストールが完了したのでスケッチ例：Serial を使ってみます。
今回使ったスケッチ例：Serial の中身はこんな感じ
ライブラリをインストールしたので、<LSM303.h>がインクルードできるようになったり色々してます。まあ最初なので宣言とか細けぇことは気にすんなよの精神でとりあえず使ってみます。
メインループを見てみると、どうやら compass.read(); の一文で３軸の加速度と磁束密度の値を取得できているようです。便利ですねぇ。
取得した計６つの値をスペース区切りで100msごとにプロットする感じですね。
compass.a.x がセンサのx軸方向の加速度を表しています。aはaccelerationの頭文字でしょう。
同様に
compass.m.xがセンサのx軸方向の磁束密度を表しています。magneticのmを取ってるのかな。
配線
ArduinoとLSM303Dをジャンパー線で繋ぎます。まずは次の画像のように付属のピンを半田付けしてブレッドボードに挿しましょう。コネクタとか使って直接マイコンに繋いでもいいです。
必要な配線は非常にシンプル。センサのVINをArduino megaの5Vピンに、GNDは同じくGNDピンに、I2Cを使うのでSDAとSCLをArduino megaのSDA、SCLに対応しているデジタルI/Oピン（20，21）にそれぞれ繋ぎます。
値を読み取ってみる
配線が完了してプログラムも書き込み完了したら、早速シリアルモニタを開いてみましょう。
こんな感じに表示されたらとりあえず成功です。
何も表示されて無い場合は接触不良か配線ミスの可能性があるので確認してください。
実行結果は左からx軸、y軸、z軸の加速度、そして磁束密度ですね。
Excelにこのシリアルモニタの結果をコピペして、テキストウィザードを使うとこんな感じにできます。このままグラフ化なんかもできちゃいますね。
方位角を計算してみる
センサが常に水平であるものとして、X軸とY軸の磁束密度の取得値(x , y)から簡易的に方位角を算出します。私は言語関数のatan2()を使いました。
atan2()は２つの引数から逆正接を計算して-πから+πまでの範囲の値を返します。Ｘ軸とＹ軸で表現される2次元座標系で表現するとこんな感じでしょうか。
ｘとｙの大きさの比でθが決まります。単位はラジアン(rad)なので180/πを掛ければ度(degree)に変換できますね。普通のatan()では±πの範囲しか扱えないため、こちらは何かと便利です。
ここで、改めてLSM303Dを見てみると、長手方向にＸ軸、横方向にＹ軸を取っています。上図とは逆ですね。センサのＸ軸をドローンの向きと合わせて使いたかったこともあり、X軸とＹ軸を反転させて考えることにしました。
このように考えることで、θ＝atan2(x,y)で磁北を基準にドローンの向き（Ｘ軸）がいくら傾いているかを簡単に求められます。例えばＸ軸の値が最大で、Ｙ軸の値が0ならば、θ＝０でドローンは磁北を向いているということになります。
ちなみに、「磁北」と北極点のある方向である「真北」との間には地軸のズレの分、偏角と言う何度かの差があります。その地域の緯度と経度によって偏角の大きさは違うようです。日本だとだいたい4度から7度くらいだったかな？
一般的に「北」は「真北」のことを指すと思います。私の場合はドローンの向きを一定方向に保ちたいだけなので、磁北でも真北でもどっちでもいいです。
実際にドローンに地磁気センサを搭載し、ほぼ水平にぐるぐると回転させてみました。そのときのプロットがこんな感じ。
右下のあたりが少し変なのは気にしないでください。データを取ってるときにちょっと手が滑りました。
うーん。円の中心が明らかに原点からズレています。このまま方位角を計算すると妙な方向を指してしまうので困りました。というのも、地磁気センサは磁束密度を測っているので、周囲に磁性体等があると、わりと簡単に値が狂ってしまいます。コンパスと一緒ですね。
今回の場合はマイコン、モータ類が近くにあったのが原因です。このような値のドリフトを補正する必要がありますね。センサ周りの環境が変わらないのであれば、円の中心のズレの分だけ取得値を加算または減算してやればいいです。この場合だと、Ｘ軸の取得値を-200くらい、Ｙ軸の取得値に+2000くらいしてやれば補正できそうです。ただし、電子回路の配置を変えたりすると、この補正もやり直しすることになりそうです。地磁気に関わるスマホアプリなんかを使うときに、スマホをぐるぐる回してくださいという風な指示が出ることがあるかと思いますが、それはこのようなドリフトを補正するためのものなんでしょうか。
私は地磁気センサだけをアンテナ状の棒の先に避難させるような形にすることで、周辺環境の影響を軽減することにしました。次の図がセンサを避難させた後のプロット図です。
いい感じになりましたね！これならほとんどそのまま方位角の計算に使っても大丈夫そうです。
研究に使ってみた
地磁気センサから簡易的に方位角を計算できるようになったので、ドローンの向きの維持に使ってみました。
— たくまつ (@takk_mcmc) 2019年2月5日
シンプルにドローンの方向を維持するように比例制御をしただけでしたが、思ったよりいい感じに機能してくれました。よかったです。
厳密にはセンサの水平が常に保たれているわけではないので、3次元的なセンサの傾きを考慮して地磁気の取得値を補正してやるのが理想的と言えます。そのためにLSM303Dは加速度の値も同時に取得できるようになっているのですが...ドローン自身の運動による加速度や、波やモータによる振動がセンサに影響するため、加速度の取得値だけで3次元的なセンサの傾きを精度よく知るのはちょっと厳しそう。センサが静止しているわけではない以上、加速度センサとジャイロセンサを併用した3次元的な角度推定手法を用いた方がよさげな気がします。となると最初から9軸のセンサを買っておけばよかったかも...。傾きに応じた補正手法に関しては他の詳しい方のブログ等でも解説されているのでここでは詳しく述べませんが、そのうち書けたらいいな。書くならもう少しちゃんと勉強しなくてはいけませんね。
最後まで読んでいただきありがとうございました。
