Raspberry Piでジャイロを使ってみる：坂に対するXY軸の傾き（Python, Tkinter）

Marupeke-IKD

　前回ジャイロ基板の加速度センサーを用いて静止している時の水平角度、つまり傾斜具合を計算して表示してみました：

いわゆる「水平器」です。

　ジャイロ基板のZ軸(0,0,1)と基板の座標空間で示される地球の重力加速度のベクトルによって生じるGベクトルとの間の角度が水平角度になり、それを画面に表示する事で見える化を実現しました。

　ただ、傾きについてもう少し詳しい情報が欲しい場合があります。ジャイロ基板にはZ軸以外にX軸とY軸もあるので、それらの軸も水平面から何度傾いているか計算すれば、より詳しい傾斜の様子が測れます。

　そこで今回はジャイロ基板のX軸Y軸それぞれが水平面から何度傾いているか求めてみようと思います。

sinθがポイント

　下の図をご覧ください：

　水平面に対してGが真上を向いています。この基板が傾いて静止している時、基板にはGベクトルのみ影響し、それが基板の各軸ベクトルに分配されます。上図はそのX軸に分配された分を示しています。

　ジャイロのX軸と水平面との角度をθとすると、上図にあるようにθがGベクトルの方にも出てきます。今Gの大きさは加速度センサーの値から、

と求まります。そしてX軸に分配されたベクトルの大きさ（符号付き）はaxそのものなので、三角関数の定義から、

sinθで表現できるのが分かります。sinθが求まれば逆関数からθが求まります。人に優しいデグリー角するには、

と計算します。Pythonのmathライブラリにはラジアン角をデグリー角に直接変換するdegrees関数があるので、プログラムではそれを使いましょう。

　Y軸についても全く同様です。以上の理屈をコードに落としてみましょう。

X軸Y軸方向の坂の傾斜を表示してみる

　ベースは水平角度を表示した前回のコードを利用させて下さい：

　もちろんXY軸の傾斜角度は画面に表示します。ちょっと表示上の注意点としては、例えばX軸を単に「30°」と表記すると、それがX軸側の傾斜角度なのかX軸の回転角度なのか分かりにくいんですよね。そこで数学で勾配を意味する∇（ナブラ）を付けて「∇30.0°」と表示する事にします。

　前回同様に計算と表示をし続けるstartThread関数を以下のように更新します：

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

    fontXY = TkFont.Font( weight="bold", size=20 )
    angleTextX = tkinter.Label( root, text="", font = fontXY )
    angleTextX.place( x=350, y=200, anchor=tkinter.CENTER )
    angleTextY = tkinter.Label( root, text="", font = fontXY )
    angleTextY.place( x=200, y=50, 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
        degZ = math.degrees( math.acos( cosViaVertical ) )
        angleText["text"] = f"{degZ:.01f}" + "°"

        # XY軸傾斜角度を算出
        sinViaVerticalX = accX / gravLen
        sinViaVerticalY = accY / gravLen
        degX = math.degrees( math.asin( sinViaVerticalX ) )
        degY = math.degrees( math.asin( sinViaVerticalY ) )
        angleTextX["text"] = "∇" + f"{degX:.01f}" + "°"
        angleTextY["text"] = "∇" + f"{degY:.01f}" + "°"

　前回同様にフォントオブジェクトfontXYを作ります。水平角度よりもフォントサイズを小さくしたいので別フォントとしています。ラベル（angleTextX, angleTextY）にそのフォントを適用します。

　センサーからか加速度を得続けるwhile内で傾斜角度を表示しています。前回水平角度を表示したのと同様、XY軸の傾斜角度を先の理屈に従って計算して表示しています。

　上の実装で実行するとこういう感じの表示になります：

　画面右方向がX軸、手前から奥の方向がY軸です。軸が上向きになると値がプラスになるのを確認出来ます。なんからしくなってきました(^-^)

水平にするには

　X軸とY軸それぞれの傾斜角度がわかると、対象を水平にする動かし方がわかります。例えばX軸が+10°、Y軸が-5°傾いている場合、まずY軸のみを回転してX軸が0°になるようにします。次にX軸のみを回転してY軸の傾斜角度を0°にすると水平になります。

　気を付けたいのが「X軸とY軸を5°、-10°回転する訳ではない」という事です。傾斜角度は水平面に対する角度であって軸の回転角度ではありません。Y軸を何度か回転してX軸を0度にする、X軸を何度か回転してY軸を0度にする。そういうアナログな評価をするだけです。

　各軸個別で0度調整すれば水平になるとはいえ、そうなると「水平にするX軸、Y軸の回転角度をダイレクトに知りたい」となるわけです。それを計算するのはかなり大変なのですが、次にそれに挑戦してみましょう。


いいなと思ったら応援しよう！