加速度センサーから軸廻り角度への変換計算

　ジャイロ値のドリフト補正を行うため、加速度センサーからの角度計算方法を調べました。いろいろ算出方法があるようですが、調べた内容をまとめておきます。

1軸回転の場合

　▲上図のようにまずは座標系をx、y、z、加速度センサーの軸をそれぞれx、y、z-local またその軸回りをroll、pitch、yaw、検出される加速度をax、ay、azとします。

　1軸回転は単純な三角関数で計算可能です。上図のようにy軸回り（pitch）の回転角度は

　もしくは

　で算出可能です。

　ただこれだと検出角度の範囲によって、加速度センサーの分解能の影響が出てしまいます。そこで式を少しいじって、

のように2軸の加速度値 ax、az から計算するようにします。これですと分解能の影響を極力無くすことができるのと、2値からの計算になるため少し信頼性が高まります。

　あと tan では検出範囲が -π/2<Θ<π/2 になってしまうので言語関数の atan2 関数を使用します。atan2 関数では -π<Θ>π の範囲で計算が可能です。例えば上記の場合、atan2（az,ax）というような感じになります。

　ここからが本題です。加速度センサーを1軸回転（平面上の回転）のような使い方をするのであれば上記の計算方法で良いかと思いますが、水平面に対する任意の傾きがある場合の計算方法を考えたいと思います。（⊿2018/4：以前はイメージで説明していたのですが、もう少しましな説明に書き直しました）

　1軸回転の場合は単純でしたが、3軸（2軸）回転となると少しややこしいです。回転行列（ベクトル）で考えてみます。

　まず、回転前のセンサー姿勢を「A」、回転後センサーの姿勢を「a」、回転行列「R」として、

　こんな感じでおきます。

　ここで、回転前のセンサー姿勢「A」を、水平状態でこのようにします。

　センサーの各軸を直行座標系の単位ベクトルと見立ててます。、長さは「１g（１）」としてますがどうせ消えるのでなんでもいいです。

　回転行列「R」は、オイラー角ZYXの回転順として、

と置きます。

　最初の式にそれぞれ代入して展開していくと、

　こんな感じで回転後のセンサー姿勢が計算できます。ここではz軸回転は計算する意味があまり無いため無視してます。

　ここで展開した行列式から、roll角を導くように式を整理していきます。回転後のセンサー姿勢となっているため、それぞれ対応するベクトル軸のz値がそのまま（重力）加速度値となるはず、、。

　こんな感じで計算すると、加速度値から「roll角」が算出できます。

　さらに、pitch角については

　こんな感じで纏めていくと、各軸の（重力）加速度値から「pitch角」が計算できます。pitch角はそのまま「-sinθ_p」からでも計算できますが、複数の加速度値からの方が信頼性が高そうなのでこうしてます。

　加速度センサーそのものが動的加速度を伴っている場合、正確な計算はできないため注意が必要です。各加速度の検出値がおおよそ

となっているのが前提です。

　さらに加速度センサーからの角度算出ではz軸（yaw軸）廻りの回転角度算出が困難です。。z軸回転では加速度に変化がない（また遠心力でx、y軸に加速度が発生してしまう）ため角度計算おかしくなってしまいます。

　途中間違ってないか心配です・・・。表現がおかしなところがあるかもしれませんがすみません・・・。

　検証にはArduino/Genuino 101の内蔵6軸センサーを使用してます。加速度センサーが内蔵されているため、基本的には外付け部品無しで検証できるので便利です。

　検証といっても傾き角度を計測するのは困難ですので、Processingを使用してビジュアルで見てみたいと思います。

　水平面に対する傾きを、加速度センサーの値から計算。その変換値をProcessing側でビジュアル化してます。

▼確認の動画です▼

　加速度センサーからの取得値のみで動きを可視化してます。割と正確に加速度から傾き角度の算出ができいるようです。