M5STACKのATOM MatrixでG-Bowlもどき ～６～

前回見つけたこちらのページの内容を手計算するコードにしてみました。

2つの3次元実数値ベクトルxとyが与えられた時、xからyへ回転する行列を求めたい

コードをNumPyやulabを使わない形に変換してみて驚いたのは、回転行列を求めているのに一切三角関数が出てこない点でした。全ての計算は四則演算のみで行われています。

def rotation_matrix_3d(frm, to=[0, 0, 1.0]):
    global r_matrix
    to_minus = [-to[0], -to[1], -to[2]]
    if frm == to:
        r_matrix = [[1.0,0,0],[0,1.0,0],[0,0,1.0]]
    if frm == to_minus:
        r_matrix = [[-1.0,0,0],[0,-1.0,0],[0,0,-1.0]]

    s = [frm[0]+to[0], frm[1]+to[1], frm[2]+to[2]]
    s_dot = s[0]**2 + s[1]**2 + s[2]**2
#    return 2.0 * np.outer(s, s) / np.dot(s, s) - np.identity(3)
    a11 = 2.0 * s[0]*s[0] / s_dot - 1.0
    a12 = 2.0 * s[0]*s[1] / s_dot
    a13 = 2.0 * s[0]*s[2] / s_dot
    a21 = 2.0 * s[1]*s[0] / s_dot
    a22 = 2.0 * s[1]*s[1] / s_dot - 1.0
    a23 = 2.0 * s[1]*s[2] / s_dot
    a31 = 2.0 * s[2]*s[0] / s_dot
    a32 = 2.0 * s[2]*s[1] / s_dot
    a33 = 2.0 * s[2]*s[2] / s_dot - 1.0
    r_matrix = [[a11,a12,a13],[a21,a22,a23],[a31,a32,a33]]

特にこのコードの検証はしていませんが、それっぽく動いているので合っているということにしますw。

あと今回嵌ったのは重力加速度の取り扱いでした。グローバル座標系でZ軸が上向きで加速度センサーが静止している時(重力加速度のみの時)は、重力の向きは下向きなのでZの値としては"-1"の値が得られると思っていたのですが実際に得られる値は"+1"でした。

その辺を詳しく書いてあったデータシートがあったので図を貼っておきます。

加速度検出軸

上の図はMPU6886のデータシートのものではありませんが、加速度センサーとしては上の図のようになるのが一般的なのでしょう。そんなわけで上のコードでは３次元ベクトルを回転させる先の初期値としてto=[0, 0, 1.0]としました。

言い忘れてましたが、今回実際にATOM Matrixを使うときのMPU6886の軸とグローバル座標系の軸が異なっているので、事前に軸の入れ替えをしたあとで加速度の計算は行うことにしました。ATOM Matrixに挿さるケーブルが下に来る状態の時に加速度センサーから得られるデータが上の図の軸の向きになるような軸の入れ替えをしています。

軸の入れ替え

あとはATOM Matrixのスイッチが押されたら起動時に行っているのと同じ軸の初期化を行うようにしました。ATOM Matrixのスイッチは5x5のLEDになっている面がスイッチになっていて押すことができます。

スイッチを使うためのコードは以下の通りです。

SW_GPIO = const(39)
sw = Pin(SW_GPIO, Pin.IN, Pin.PULL_UP)

これでやりたかったことは全て盛り込めたので、今度こそ今回作ったG-Bowlもどきを実際の車につけて走ってみようと思います。