3軸地磁気センサーから方位（角度）の計算方法

　3軸の地磁気センサーから方位（角度）を計算する方法です。検証にはMPU9250＋Arduinoを使用してます。

概要

　3軸地磁気センサーは磁気強度を3軸で検出することができます。地球の磁力線を検出して方位を算出します。

　センサーからの取得値はμT（テスラ）などに単位変換できますが、方位の算出は三角関数などを使用して比から算出できますので、基本的には検出した生値をそのまま使用しても問題ないかと思います。

　（重力）加速度を検出して姿勢角を算出できる加速度センサーと考え方は少し似ているかもしれませんね。

　　常に地面と水平な状態でセンサーを使用する場合の方位（角度）の算出方法です。いろいろ算出方法があるかと思いますが一例です。

▼例▼

　例えば、上図のようにセンサーが磁力線に対して角度θだけ回転していたとします。地磁気の大きさをμT、センサーのx、y軸でそれぞれ検出する磁気をMx、Myとすると、単純な三角関数から

　となって、少し式をいじると

　こんな感じで、検出値から逆正接をとれば回転角が分かります。

　方位を算出するときは北を「0°」にして時計回りに方位（角度）を算出する場合がある？？かと思います。その場合は、x軸、y軸や角度方向（±）、位相など、、注意が必要です。

　ここら辺は考えるとややこしくて頭痛くなってくるので、私の場合は現物で試して合わせ込んでます。

　以下のプロットは、水平上で地磁気センサーをくるくる回したときのx値とy値を生値のまま散布図化したものです。

　それなりな円軌跡を描きます。ただ中心はずれてますので中心を合わせるように生値をキャリブレーションしないと正確な角度はでないです。

▼横軸を（角度）にしたプロット▼

　横軸を方位（角度）にして、x軸、y軸の値をそれぞれプロットしてます。このグラフはキャリブレーションした値を使用してます。この軌跡みても逆正接から方位（角度）が算出できるのがわかるかと思います。

　次に3軸の傾きがある（センサーが地面に水平でない）場合です。こちらもいろいろ算出方法があるかと思いますが一例です。

　今回はMPU9250（3軸ジャイロ、3軸加速度、3軸地磁気）センサーを使用してますので、ジャイロ、加速度から別途算出したセンサーの姿勢角を使います。

　センサーの姿勢角（傾き）がわかっていれば、傾いた状態で検出した磁気ベクトルを回転行列で座標系を水平に戻してから方位を計算すればいいかと思います。

　▼x軸廻り、y軸廻りはそれぞれ▼

の行列で表現されますので、

▼こんな感じで座標系を戻します▼
　

　回転する順番と角度方向（±）は注意が必要です。で座標系を元に戻してからあとは、水平のときと同じように計算すれば方位（角度）が計算できるかと思います。

以下参考にセンサーをぐるぐる廻したときのx、y、z座標の3次元散布図をとってみました。

　概ね球面で検出されます。2次元同様中心がずれていますので、方位（角度）算出にはキャリブレーションした値が必要となります。スマホとかで八の字に回して補正しているのはこのキャリブレーションことかもしれませんね。それぞれの軸で最大最小とって中心合わせてるのでしょうかね。

　地磁気センサーとGPSの組合せでを自作ドローンの自己位置推定をと考えているのですが、処理が結構重そうで、きつくなりそうな感じはしますね・・。