MATLAB で Nintendo Switch の Joy-Con に接続する ～加速度・ジャイロセンサのデータ取得と姿勢推定～

この記事は？

前回投稿した
　MATLAB で Nintendo Switch の Joy-Con に接続する　～ランプの点灯とボタン入力の取得まで～
の続きの記事です。

前回は接続とボタン情報の取得を行いました。今回は Joy-Con のセンサデータを取得していきたいと思います。

前回のおさらい

前回紹介（＆修正）した hidapi を使って Joy-Con(L) に接続していきます。

hid = hidapi(0,1406,8198,64,64);
hid.open;

この時、前回 PC と接続したあと Switch に接続し直した方は注意が必要です。PC に 1 度接続した Joy-Con を Switch に戻すと、Switch 側で接続の再設定が行われるようで、うまく PC と接続できなくなってしまいます。

赤枠の部分が「接続済み」ではなく「ペアリング済み」になっている場合は正しく接続できていません。一度右クリックからデバイスの削除を行い、もう一度新しいデバイスとして追加しましょう。

再接続した場合は、念のため enumerate 関数で確認してから open するようにしましょう。

加速度センサのデータを取得する

それでは接続した Joy-Con(L) の加速度センサのデータを取得していきましょう。加速度やジャイロなどのセンサのデータを取得するには、先にセンサデータを有効にする必要があります。センサデータを有効にするためには、まずサブコマンド 0x40(=64)に 0x01(=1) を送ってセンサを有効にし、その後サブコマンド 0x03(=3) に 0x30(=48) を送って全データを定期的に送信してもらうようにします。これは前回作った sendSubCommand メソッドがそのまま使えますね。

hid.sendSubCommand(64,1,0);
pause(0.02);
hid.sendSubCommand(3,48,1);

センサを有効にしてから反映されるまで少し時間間隔が必要らしいので、20ms 待ってから送信の設定を行っています。これで加速度センサとジャイロセンサの値を送信してもらえるようになりました。

それでは早速加速度センサの値を取得してみましょう。加速度センサは 3 軸ともに int16 型のデータで、unit8 のデータ 2 つに分割されて送られてきます。データの場所は、X 方向が 14,15、Y 方向が 16,17、Z 方向が 18,19 番目です。ただし、分割されたデータは HighByte, LowByte の順に並んでいるので、結合する際には注意が必要です。

function accelX = getAccelX(hid,rmsg)
    hByteBe = uint16(rmsg(14));
    lByteBe = uint16(rmsg(15));
    uint16Le = bitor(bitshift(lByteBe,8),hByteBe);
    int16Le = typecast(uint16Le,'int16')-350;
    accelX = double(int16Le)*0.000244*9.8;
end

早速 X 方向の加速度データを取り出すメソッドを作ってみました。MATLAB ではシフト演算子 >>, << がないので、代わりにビット演算系の関数を使います。また、typecast 関数は bit の並びを維持したまま別のデータ型に変換でき、小さいデータ型に分割されているデータをもとに戻す際にとても便利なのでお勧めです。

ちなみに、最後に 350 を引いているのはオフセットを補正するため、0.000244*9.8 をかけているのは単位を m/s^2 に直すためです。これらの数値はそれぞれ 6-Axis and Stick device parameters と 6-Axis sensor information に記載されています。

同様に、Y と Z についても作ってみます。

function accelY = getAccelY(hid,rmsg)
    hByteBe = uint16(rmsg(16));
    lByteBe = uint16(rmsg(17));
    uint16Le = bitor(bitshift(lByteBe,8),hByteBe);
    int16Le = typecast(uint16Le,'int16');
    accelY = double(int16Le)*0.000244*9.8;
end

function accelZ = getAccelZ(hid,rmsg)
    hByteBe = uint16(rmsg(18));
    lByteBe = uint16(rmsg(19));
    uint16Le = bitor(bitshift(lByteBe,8),hByteBe);
    int16Le = typecast(uint16Le,'int16');
    accelZ = double(int16Le)*0.000244*9.8;
end

Y と Z のオフセットは 0 なので特に補正は行っていません。ちなみに、6-Axis and Stick device parameters には Z のオフセットが 4081 だと書いてありますが、実際に試してみたところこの数値は Z 方向にかかっている重力の影響が出ているだけで、オフセットではありませんでした。これを補正してしまうと、後述している加速度センサから姿勢の推定する方法が使えなくなってしまうので注意しましょう。

最後に全方向をまとめた加速度データを取得するメソッドを作れば完成です！

function accel = getAccel(hid,rmsg)
    accelX = hid.getAccelX(rmsg);
    accelY = hid.getAccelY(rmsg);
    accelZ = hid.getAccelZ(rmsg);
    accel = [accelX;accelY;accelZ];
end

実際に机の上に置いた状態で実行してみると、

>> rmsg = hid.read;
>> hid.getAccel(rmsg)

ans =

    0.0789
   -0.0335
    9.8326

結構いい精度が出てそうです。

ジャイロセンサのデータを取得する

それでは次にジャイロセンサのデータも取得します。とは言っても、加速度センサと場所が違うだけで形式は全く同じです。X 方向が 20,21、Y 方向が 22,23、Z 方向が 24,25 です。

function gyro = getGyro(hid,rmsg)
    gyroX = hid.getGyroX(rmsg);
    gyroY = hid.getGyroY(rmsg);
    gyroZ = hid.getGyroZ(rmsg);
    gyro = [gyroX;gyroY;gyroZ];
end
function gyroX = getGyroX(hid,rmsg)
    hByteBe = uint16(rmsg(20));
    lByteBe = uint16(rmsg(21));
    uint16Le = bitor(bitshift(lByteBe,8),hByteBe);
    int16Le = typecast(uint16Le,'int16');
    gyroX = double(int16Le)*0.06103/180*pi;
end
function gyroY = getGyroY(hid,rmsg)
    hByteBe = uint16(rmsg(22));
    lByteBe = uint16(rmsg(23));
    uint16Le = bitor(bitshift(lByteBe,8),hByteBe);
    int16Le = typecast(uint16Le,'int16');
    gyroY = double(int16Le)*0.06103/180*pi;
end
function gyroZ = getGyroZ(hid,rmsg)
    hByteBe = uint16(rmsg(24));
    lByteBe = uint16(rmsg(25));
    uint16Le = bitor(bitshift(lByteBe,8),hByteBe);
    int16Le = typecast(uint16Le,'int16');
    gyroZ = double(int16Le)*0.06103/180*pi;
end

最終行で 6-Axis sensor information に記載されている係数 0.06103 を使ってラジアン単位に変換しています。Joy-Con(L) を机に置いて実行してみると、

>> rmsg = hid.read;
>> hid.getGyro(rmsg)

ans =

    0.0511
   -0.0841
   -0.0415

そこそこの精度が出てそうです。1 方向に傾いているだけの状態であれば、これを積分することで Joy-Con の姿勢に変換できるはずです（※3 次元的に回転している場合は単純に積分するだけでは姿勢にはなりません）。早速簡単なコードで確認してみましょう。

angle = zeros(100,3);
dt = 0.1;
for i = 2:100
    rmsg = hid.read;
    angle(i,:) = angle(i-1,:) + hid.getGyro(rmsg)'*dt;
    pause(dt);
end
plot(1:100,angle);
legend("roll","pitch","yaw");

はい。まあ積分したら誤差が蓄積しちゃうので、当然ドリフトしちゃいますね。

拡張カルマンフィルタを用いた姿勢推定

一般的に、IMU から姿勢を推定する場合、ジャイロセンサの値を積分するだけでは上述の通りドリフトしてしまうので、重力の値を用いて加速度センサからも姿勢を計算し、それを用いて積分値を補正する拡張カルマンフィルタを設計することによって推定を行います。

拡張カルマンフィルタについてはいろいろな人が記事を書いているのでここでは詳細は省かせていただきますが、IMU の拡張カルマンフィルタの設計は下記のサイトがとてもわかりやすいと思います。

6軸IMU～拡張カルマンフィルタ

ということで、拡張カルマンフィルタを用いて Joy-Con の姿勢（roll, pitch のみ）を推定するコードを MATLAB で書いてみました。

拡張カルマンフィルタのコード

% joy-con と接続し、センサデータを受信できるよう設定
hid = hidapi(0,1406,8198,64,64);
hid.open;
hid.sendSubCommand(64,1,0);
pause(0.02);
hid.sendSubCommand(3,48,1);

% アニメーション準備
figure
p = plot(NaN,NaN,NaN,NaN);
xlabel("time");
ylabel("degree");
legend("roll","pitch","Location","northwest");
t = 0;

% カルマンフィルタ初期化
roll = 0;
pitch = 0;
yaw = 0;
dt = 0.1;
xEst = [roll;pitch];
PEst = eye(2)*0.0174*dt^2;
Q = eye(2)*0.0174*dt^2;
R = eye(2)*dt^2;
H = eye(2);


while 1
    % データを受信
    rmsg = hid.read;

    % 加速度から角度を計算（観測値）
    accel = hid.getAccel(rmsg);
    ax = accel(1);
    ay = accel(2);
    az = accel(3);
    rollAcc = atan2(ay,az);
    pitchAcc = -atan2(ax,sqrt(ay^2+az^2));
    y = [rollAcc;pitchAcc];

    % ジャイロから角度を計算（予測値）
    gyro = hid.getGyro(rmsg);
    gx = gyro(1);
    gy = gyro(2);
    gz = gyro(3);
    droll = gx + sin(roll)*tan(pitch)*gy + cos(roll)*tan(pitch)*gz;
    dpitch = cos(roll)*gy - sin(roll)*gz;
    dyaw = sin(roll)/cos(pitch)*gy + cos(roll)/cos(pitch)*gz;

    rollGyro = roll + droll*dt;
    pitchGyro = pitch + dpitch*dt;
    yawGyro = yaw + dyaw*dt;

    xPred = [rollGyro;pitchGyro];

    % ヤコビ行列の計算
    F = zeros(2,2);
    F(1,1) = 1 + (gy*cos(roll)*tan(pitch)-gz*sin(roll)*tan(pitch))*dt;
    F(1,2) = (gy*sin(roll)/(cos(pitch)^2)+gz*cos(roll)/(cos(pitch)^2))*dt;
    F(2,1) = (-gy*sin(roll)+gz*cos(roll))*dt;
    F(2,2) = 1;

    % 更新
    PPred = F*PEst*F'+Q;
    K = PPred*H'/(H*PPred*H'+R);
    xEst = xPred + K*(y-H*xPred);
    PEst = (eye(2)-K*H)*PPred;

    roll = xEst(1);
    pitch = xEst(2);
    yaw = yawGyro;

    % プロット
    p(1).XData(end+1) = t;
    p(1).YData(end+1) = roll*180/pi;
    p(2).XData(end+1) = t;
    p(2).YData(end+1) = pitch*180/pi;
    %p(3).XData(end+1) = t;
    %p(3).YData(end+1) = yaw*180/pi;

    t = t + dt;
    pause(dt);
end

Joy-Con を止めているときはちゃんとドリフトせずに値が止まっています。表示されている値も違和感のない値になっていますね。

いつの間にか Joy-Con の話からカルマンフィルタの話になってしまいましたが、とりあえずこれで Joy-Con の姿勢も推定できるようになりました！

参考資料

今回の記事を書くにあたり、以下のページを参考にさせていただきました。

Joy-ConにPythonからBluetooth接続をして6軸センサーと入力情報を取得する
(Joy-Con のセンサデータに関する仕様全般)

6-Axis and Stick device parameters
(ジャイロセンサの値のオフセット値)

6-Axis sensor information
(ジャイロセンサの向き、単位変換)

6軸IMU～拡張カルマンフィルタ
(IMU から姿勢を推定する際の拡張カルマンフィルタの設計方法)

拡張カルマンフィルタを使用した自己位置推定MATLAB, Pythonサンプルプログラム
(拡張カルマンフィルタを MATLAB で実装する方法)

終わりに

ということで、今回は MATLAB から Joy-Con のセンサデータを取得し、姿勢を推定することに成功しました！これで Joy-Con を使った操縦を行うことができるようになったので、次回は Simulink と連携して何かのモデルを Joy-Con で操縦してみようかなと思います！

LGTM していただけると、その分だけ次回の記事が豪華になります（たぶん）。