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

 

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）というような感じになります。

 

 

3軸（2軸）回転の場合（水平面に対する任意の傾き）

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

 

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

　

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

　こんな感じでおきます。

 

　ここで、回転前のセンサー姿勢「A」を、傾いていない状態でこのようにします。
　座標軸方向の単位ベクトルで、長さは「１g（１）」としておきます。（水平の状態）

 

　で回転後のセンサー姿勢「a」は、回転後の加速度値を得たいため、
　このようにします。回転後の各ベクトルz成分がそのまま加速度値となります。

 

　絶対座標系からみた回転行列「R」は、オイラー角ZYXの回転順として、
　と置きます。

 

　最初の式にそれぞれ代入して展開していくと、
　こんな感じになります。ここではz軸回転は計算する意味があまり無いため無視してます。

 

　ここで展開した行列式から、roll角を導くように式を整理していくと、
　こんな感じで計算すると、加速度値から「roll角」が算出できます。

 

　さらに、pitch角については

　こんな感じで纏めていくと、加速度値から「pitch角」が計算できます。

 

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

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

 

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

 

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

 

角度変換計算の検証

 

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

 

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

 

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

 

▼実際の動画です▼

 

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

【関連記事】

・6軸センサーから角度の算出・補正方法

・ジャイロのドリフト補正と比較（カルマン、相補フィルター）