「Pythonを使ってアルプスのセンサネットワークモジュールでモーションデータの取得」でIoT Smart Moduleの加速度センサと地磁気センサからモーションデータを取得しましたが、このモーションデータからピッチ、ロール、ヘディングを計算します。

加速度センサと地磁気センサからピッチ、ロール、ヘディングの計算式

アルプスIoT Smart Moduleの加速度センサから加速度データ（Ax,Ay,Az）と、地磁気センサから地磁気データ（Mx,My,Mz）が入力されます。なお、本来は取得したデータに対して校正したり、正規化の必要がありますが、取得したデータをそのまま使用します。

最初にピッチとロールを次の計算で求めます。

Pitch = ρ = arcsin(-Ax)
Roll = γ = arcsin(Ay / cosρ )

次にヘディングを次の計算式で求めます。

Mx1 = Mxcosρ + Mzsinρ
My1 = Mxsinγsinρ + Mycosγ - Mzsinγcosρ
Heading = ψ = arctan(My1/Mx1)        Mx1>0 で My1>=0
            = 180° + arctan(My1/Mx1) Mx1<0 
            = 360° + arctan(My1/Mx1) Mx1>0 で My1<=0
            = 90°                   Mx1=0 で My1<0
            = 270°                  Mx1=0 で My1>0

使用する計算式について

アルプスIoT Smart Moduleのモーションセンサのセンサ感度軸については次のように公開されていますが、加速度センサと地磁気センサからピッチ、ロール、ヘディングの計算式については公開されていません。

センサ感度軸

このため、tilt compensated electronic compass「LSM303DLH」の計算式を使用してピッチ、ロール、ヘディングを求めます。LSM303DLHのデータシートを「Using LSM303DLH for a tilt compensated electronic compass」に示します。

次の図のように、ヘディングはXb軸と水平面上の磁北間の角度、ピッチはXb軸と水平面間の角度、ロールはYb軸と水平面間の角度で、それぞれ定義されます。

Body coordinates and attitude angles

異なる回転手順は異なる回転マトリックとなるため、航空機の慣習に従い、次のようにヘディングψ、ピッチρ、ロールγの順とします。

ヘディングψ、ピッチρ、ロールγの計算順序

次に示す（Ax1,Ay1,Az1)は、加速度センサ校正パラメータを加速度センサから取得したデータに適用した正規化された値です。このマトリックスからピッチとロールを求めた式を今回使用するピッチとロールの計算式にします。

ピッチとロールの回転マトリックス

ピッチとロールの回転により、次のように（Xb,Yb,Zb)からX”b,Y”b,Z”b)に回転させます。（Mx,My,Mz)は、新規の位置(X”b,Y”b,Z”b)で、地磁気センサ校正パラメータを地磁気センサから取得したデータに適用した正規化された値です。このマトリックスからヘッディングを求めた式を今回使用するヘッディングの計算式にします。。

motion04

ピッチ、ロール、ヘディングの計算ソフト

ピッチ、ロール、ヘディングを計算するソフトを次に示します。

# -*- coding: utf-8 -*- 

import math


def computeAngle(GeoMagnetic_X,GeoMagnetic_Y,GeoMagnetic_Z,Acceleration_X,Acceleration_Y,Acceleration_Z):
    Pitch = math.asin(-Acceleration_X)
    Roll = math.asin(Acceleration_Y/math.cos(Pitch))

    Mx = GeoMagnetic_X*math.cos(Pitch)+GeoMagnetic_Z*math.sin(Pitch)
    print ('cos:{0:.7f} sin:{1:.7f}'.format(math.cos(Pitch),math.sin(Pitch) ))
    
    My=GeoMagnetic_X*math.sin(Roll)*math.sin(Pitch)+GeoMagnetic_Y*math.cos(Roll)-GeoMagnetic_Z*math.sin(Roll)*math.cos(Pitch)
    print ('Mx:{0:.7f} My:{1:.7f}'.format(Mx,My ))
    Heading  = math.atan(My/Mx)  
    
    return math.degrees(Roll),math.degrees(Pitch),math.degrees(Heading)
        
def main():
    GeoMagnetic_X = -31.8
    GeoMagnetic_Y = 18.6
    GeoMagnetic_Z = 45.3
    Acceleration_X = 0.027
    Acceleration_Y = -0.016
    Acceleration_Z = -1.006
    Roll,Pitch,Heading = computeAngle(GeoMagnetic_X,GeoMagnetic_Y,GeoMagnetic_Z,Acceleration_X,Acceleration_Y,Acceleration_Z)
    print ('Roll:{0:.3f} Pitch:{1:.3f} Heading:{2:.3f}'.format(Roll,Pitch,Heading ))
    
if __name__ == "__main__":
    main()

ピッチ、ロール、ヘディングの計算結果

計算ソフトを実行すると次のような結果が得られました。

$ python actioncal.py
cos:0.9996354 sin:-0.0270000
Mx:-33.0115068 My:19.3086747
Roll:-0.917 Pitch:-1.547 Heading:-30.324

この計算結果とexcelで計算した結果とを比較した表を次に示します。正しく計算されていることが確認できます。

excelでピッチ、ロール、ヘディングの計算