Raspberry Piでジャイロを使ってみる：水平角度を求めて表示（Python, Tkinter）

　前回まででジャイロ基板にある加速度センサーから得られる加速度ベクトルをGUIに表示する所までできました。GUIで表示する事で基板の傾き具合を目で確認できるのが便利です。

　ところで、基板から来る加速度ベクトルには常に値が入り続けています。これは地球の重力加速度をちゃんと検知しているからです。加速度センサーはその重力加速度を自身のXYZ軸に分配して返してくれます。ですからもし基板が重力加速度の方向に対して水平になっていれば、XY軸方向の値はゼロになり、Z軸のみ「9.8(m/s²)」くらいの値に収まります。

　今回はこの加速度センサーの性質を利用して「水平器」を作ってみようと思います。水平器は物の傾き具合を調べる機器です。ホームセンターのDIYコーナーなどにこういう泡が入った機器が売ってます：

これを水平を取りたい物の上に置いて、この泡が線の間に来るように物の傾きを調整すれば水平になります。これのデジタル版を作ろうって話です。

Z軸の加速度ベクトルの大きさから角度が求まる

　以下の図をご覧ください：

　ジャイロ基板を傾けると、基板のXYZ軸に対して重力加速度が分配されて検出されます。という事は逆にXYZ軸に検出されている加速度ベクトルの大きさを合成すると、元の重力加速度の大きさになるはずです。つまり上の図の重力加速度の大きさgは、

で求められます。いわゆる三平方の定理ですね。

　それを踏まえて以下の図をご覧ください：

　基板が傾いている場合、Z軸の加速度の大きさは重力加速度のよりも目減りします。これは左図のように重力加速度の矢印からZ軸方向へ垂線を下ろした先になります。ここに直角三角形が出来るので、右図のように傾きの大きさθは三角関数のcosを用いて、

こう表せますよね。これはcosの定義そのものです。そしてありがたい事に重力加速度の大きさgも、Z軸側の加速度ベクトルの（符号付きの）大きさzも加速度センサーは検出してくれているんですね。上式をそれを意識して書き変えると：

こうなるわけです。cosθの値が分かれば、θは逆関数で求まります：

　プログラム上でarccosはラジアン角で出力されるので、人に優しいデグリー角にするには、

と変換します。もっとも、Pythonのmathライブラリにはdegrees関数が用意されていて、ラジアン角を入れるとデグリー角を返してくれます。便利(^-^)

　ちなみにですが、上で求めたθは重力加速度の方向とZ軸の加速度ベクトル間の角度ですが、これは水平角度ど同じになります。

水平角度を表示してみよう

　では上の理屈を踏まえて水平角度を表示してみます。Python＋Tkinterを今回も使って行きます。また同じ事の説明を避けるため、前回までの実装をベースとさせて下さい。

　加速度を取得しているスレッド関数を以下のように書き変えます：

import tkinter.font as TkFont
import math

・・・（中略）

# 更新スレッド作成
def startThread():
    # 水平面からの角度を表示するテキストを追加
    font = TkFont.Font( weight="bold", size=32 )
    angleText = tkinter.Label( root, text="", font = font )
    angleText.place( x=200, y=300, anchor=tkinter.CENTER )

    # センサーから加速度を取得しグラフに表示（移動平均値）
    aveNum = 100
    accs = [ average.MoveAverage( aveNum ), average.MoveAverage( aveNum ),average.MoveAverage( aveNum ) ]
    while True:
        vals = tuple( sensor.acceleration )
        accX = accs[ 0 ].add( vals[ 0 ] )
        accY = accs[ 1 ].add( vals[ 1 ] )
        accZ = accs[ 2 ].add( vals[ 2 ] )
        levelGraph.setPosition( accX, accY )

        # 水平角度を算出
        gravLen = math.sqrt( accX ** 2 + accY ** 2 + accZ ** 2 )  # 重力加速度の絶対値
        cosViaVertical = accZ / gravLen
        deg = math.degrees( math.acos( cosViaVertical ) )

        angleText["text"] = f"{deg:.01f}" + "°"

各所細かく説明しますね。

水平角度表示用のラベル

    # 水平面からの角度を表示するテキストを追加
    font = TkFont.Font( weight="bold", size=32 )
    angleText = tkinter.Label( root, text="", font = font )
    angleText.place( x=200, y=300, anchor=tkinter.CENTER )

　水平角度はテキストで直接数値を表示した方が分かり良いですし微調整が厳密に利きます。Tkinterのテキスト表示はLabelクラスが管理しています。ただデフォルトでは割と小さい文字なので、大きくするためにフォント情報を変更する必要があります。tkinter.font.Fontオブジェクトにフォント情報を埋め込み、それをラベル生成時に渡します。

　ラベルオブジェクトはtkinter.Labelのコンストラクタで作成します。第1引数に親レイヤーを渡します。これを水平グラフ内のCanvasにしても良いのですが、今回は親レイヤーに直接表示する事にしました。

　Tkinterで作成したオブジェクトを表示するにはpack関数など配置用の関数を呼ぶ必要があります。placeもその一つで指定の座標に固定で置いてくれます。anchorは表示物のピボット位置を指定する物です。tkinter.CENTERにすると真ん中をピボットにしてくれます。

　ちなみに後で出てきますが、ラベルの文字列を変更するにはラベルオブジェクトに対して、

angleText["text"] = "Hello World!"

このように"text"をキーとして辞書指定すると実現できます。

水平角度を算出

        # 水平角度を算出
        gravLen = math.sqrt( accX ** 2 + accY ** 2 + accZ ** 2 )  # 重力加速度の絶対値
        cosViaVertical = accZ / gravLen
        deg = math.degrees( math.acos( cosViaVertical ) )

　表示する水平角度は前述の理屈で算出しています。gravLenが重力加速度の大きさです。これは地球上では9.8(m/s²)ですが、地域や環境によって異なるため、9.8と決め打ちしてはいけません。ちゃんと加速度センサーが返す各軸の加速度の値から算出する必要があります。

　cosViaVerticalには重力加速度の方向（地球の垂直軸）に対するセンサーのZ軸にかかる重力加速度の大きさの割合、つまりcosθが格納されます。それを人に優しいデグリー角度に変換しています。Pythonのmath.degrees関数にラジアン角を入れるだけなのでお手軽です。

　最後に、

        angleText["text"] = f"{deg:.01f}" + "°"

求めたデグリー角に"°"を付けて角度な文字列を作ってラベルを更新しています。f"～"というのは浮動小数点文字列を意味してくれる書式です。中のdegを浮動小数点として扱ってくれます。「:.01f」というのは「小数点以下1桁まで表示してね」という意味です。

　これで実際に表示してみたのがこちらです：

いい感じで角度が取れているみたいです(^-^)。

終わりに

　という事で今回は加速度センサーで水平器を作ってみました。水平器は一般には「水平か、水平じゃないか」を取る物です。ただ時には「どっちの方向に傾斜しているのか？」を知りたい事もあります。要は板の上にボールを置いた時に転がる方向です。数学的には「勾配」と呼ばれている物です。次は加速度センサーの情報から勾配を算出して表示してみましょう。…と言っても、実はもう表示はされているんですけどね(^-^;

ではまた(^-^)/