姿勢計算(Atitude Calculation)

 

磁気センサーと加速度センサーを使うと、傾斜補正だけでなく、端末の姿勢を計算することができます。

 

端末の姿勢は、基準となる絶対座標系に対して、端末の座標系がどれだけ回転しているか相対量で表されます。

相対量の姿勢は、値で表すのに以下の3つの方法があります。

(1) 行列 ( = 3つのベクトル )で表現

 センサーの座標系のx軸,y軸,z軸を、絶対座標系での値で3つのベクトルvx,vy,vzで表現することができます。

 3つのベクトルを組み合わせることにより3×3の行列で姿勢を表現します。

 絶対座標からの回転を表す回転行列にもなっています。

(2) オイラー角 (roll角, pitch角, yaw角)

 絶対座標のx軸,y軸,z軸についてそれぞれどれだけの角度が回転したのか3つの角度で姿勢を表現します。

(3) 四元数(クォータニオン) または回転ベクトル

 絶対座標からの回転を、回転ベクトルまたは四元数という特殊な複素数で表現することができます。

これらの3つは、それぞれ相互に変換可能です。

姿勢の行列を計算する方法を以下に示します。

まず、センサーの座標系で計測した値から、逆に絶対座標への変化量を求めます。

最初に、加速度の計測値からz軸のベクトルを求めることができます。

次に、加速度と地磁気から、2つのベクトルに直交なベクトルを求めます。これによりx軸が求まります。

最後に、先ほど求めたz軸とx軸の2つのベクトルから、直交なベクトルを求めます。これにより

y軸が求まります。

3つのベクトルをvx,vy,vzとすると、この3ベクトルより次のように行列を構成します。

この行列は、センサー座標から絶対座標への変化量の回転行列となっています。

絶対座標からセンサー座標への変化量に直すために逆行列を計算します。

以上で、姿勢を表す行列、M'を得ることができます。

 

※ 行列から四元数、またはオイラー角への変換は、参考文献をご参照ください。