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

 

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

 

 

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

　ここからが本題です。加速度センサーを1軸回転（平面上の回転）のような使い方をするのであれば上記の計算方法で良いかと思いますが、水平面に対する任意の傾きがある場合の計算方法を考えたいと思います。

 

　またジャイロのドリフト補正を目的としているため、絶対座標に対する角度ではなく、加速度センサーのローカル軸廻りの回転角度を考えます。

　まず、傾いた状態の各軸、回転をそれぞれを上記のように、加速度センサーからの取得値は各軸でそれぞれax、ay、az とします。

 

　xy平面を水平面（z軸が重力加速度）とした場合、x-local 軸が水平面となす角については

という基本的な三角関数から計算できます。y-local 軸についても同様の計算で可能です。ただ、これは水平面と x-local 軸のなす角であり、local 軸廻りの回転角度ではありません。

 

　水平面に対する任意の傾きについては2軸の回転角度で決めることができます。そこで、各2軸の計算順番（回転順番）の前提条件を決めて、軸廻りの角度として表現してみます。

 

　下図の場合で仮に、pitch ⇒ roll （y-local廻り ⇒ x-local廻り） の順番とします。

　まず最初の回転成分、 pitch 回転を考えます。回転軸は絶対座標軸のy軸 = y-local 軸と考えることができるため、x-local 軸はxz平面上に存在します。そのため、先ほど計算した水平面と x-local 軸のなす角の計算式がそのままpitch 回転角となります。

その後、roll 回転角の計算です。roll軸自体傾いていますが、傾き成分が、y軸、z軸に加味されていると考え、yz 平面上の回転として計算します。そのため roll回転角は

で計算できます。

 

　このように回転軸の計算順番で式を使い分けることによって、軸廻り角度で平面との傾きが、恐らく表現できると思います。

 

　また今回の計算では加速度センサーそのものが並進加速度を伴っている場合は計算できないため注意が必要です。各加速度の検出値がおおよそ

となっていることが前提となります。また、まとめた図と式の関係ですが象限や±は意識せずに記載してますので使用する際は適宜調整して下さい。

 

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

 

角度変換計算の検証

 

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

 

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

 

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

 

▼実際の動画です▼

 

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

【関連記事】

・ジャイロのドリフト補正と比較（カルマン、相補フィルター）
・カルマンフィルターと相補フィルターをProcessingでビジュアル比較してみました