M5StickC PlusのIMUを取得してみる

概要

M5Stickに触れる機会があり、それを用いて何かしらのものを作れないかという話になったので、 M5Stickの6軸IMUを取得してみることにした。 この処理が実装できると、M5Stickを搭載したモノを移動させたりすることでその動きをプログラム側に反映させることができるため、 M5Stickを埋め込んだ銃の様なものを作成し、プログラム側で的を用意し射的を再現できるのではないかと考えている。

本記事では、IMUのデータを安定して取得するために行なった実装とそれに伴い発生した課題への対処に関して記載する。

環境

• M5StickC Plus
• Arduino IDE
• MacBook Pro M1

※今回の記事ではArduino IDEの導入やM5Stickの設定は省略している。

実装

M5Stick

M5Stickに搭載されているIMUから情報を取得し、その情報をシリアル通信で送信する処理を実装する。 M5Stick上には現在取得している座標の情報がディスプレイ上で表示される様にする。 また、ボタン押下時にはIMUの取得情報をリセットする。

ライブラリ準備

M5UnifiedをArduino IDEのライブラリマネージャーから検索してインストールする。

コード1

setupと変数定義

// ライブラリーの導入
#include <M5Unified.h>

// ポインターの相対角度
float aimX = 0;
float aimY = 0;

// 送信間隔制御
unsigned long lastSendTime = 0;
const unsigned long sendInterval = 33;

// 時間管理
unsigned long lastUpdateTime = 0;

void setup() {
  // M5Unifiedの初期化
  auto cfg = M5.config();
  M5.begin(cfg);

  Serial.begin(115200);

  // IMUの初期化
  // 失敗時は動作をしない様にする
  if (!M5.Imu.begin()) {
    M5.Display.println("IMU init failed!");
    for (;;) { delay(100); }
  }

  lastUpdateTime = millis();
}

loop

void loop() {

  M5.update();

  // リセット用
  if (M5.BtnA.wasPressed()) {
    M5.Display.fillScreen(BLACK);
    M5.Display.setCursor(0, 40);
    M5.Display.setTextSize(1);
    M5.Display.println("RESET!");

    // 角度をリセット
    aimX = 0;
    aimY = 0;
    // 画面をクリア
    M5.Display.fillScreen(BLACK);
    delay(300);
  }

  float gyroX, gyroY, gyroZ;
  M5.Imu.getGyroData(&gyroX, &gyroY, &gyroZ);

  unsigned long currentTime = millis();
  float deltaTime = (currentTime - lastUpdateTime) / 1000.0f;
  lastUpdateTime = currentTime;

  aimX += gyroZ * deltaTime;
  aimY += gyroX * deltaTime;

  if (currentTime - lastSendTime >= sendInterval) {
    sendData();
    lastSendTime = currentTime;
  }

  displayData();
  delay(10);
}

データを送信する部分

外部システムとの連携用にJSON形式でデータを送信できる様にしておく。

<br>void sendData() {
  Serial.print("{\"aimX\":");
  Serial.print(aimX);
  Serial.print(",\"aimY\":");
  Serial.print(aimY);
  Serial.print(",\"timestamp\":");
  Serial.print(millis());
  Serial.println("}");
}

ディスプレイに表示する部分

M5Stickのディスプレイに表示するための実装。

<br>void displayData() {
  M5.Display.setCursor(0, 80);
  M5.Display.printf("aimX: %6.1f", aimX);
  M5.Display.setCursor(0, 100);
  M5.Display.printf("aimY: %6.1f", aimY);
}

実行1

実際に動作させると、M5Stickの画面には次の様な表示が出る。

X軸：-4.1 Y軸：116.9

画像の数値で固定されることはなく、常に値が加算され続ける。

キャリブレーション実装

実行1の様にM5Stickを動かしていなくても常に座標情報を更新し続ける現象が発生する。 これは、ドリフト現象というもので、この現象を完全に防ぐことはできないが、 ある程度のドリフトを防ぐためにキャリブレーションとデッドゾーンの設定をする。

次の処理を追加し、setup関数の中に記述する。

コード2

<br>// 変数を追加
// ...(省略)
// ジャイロセンサーのキャリブレーション用変数
float gyroX_offset = 0;
float gyroY_offset = 0;
float gyroZ_offset = 0;
const float deadZone = 5.0f;

// ...(省略)

void setup() {
  // ...(省略)
  calibrateSensors();
  lastUpdateTime = millis();
}

void loop() {
  // ...(省略)
  // -----以下を追加---------
  // ジャイロセンサーのオフセットを適用
  gyroX -= gyroX_offset;
  gyroY -= gyroY_offset;
  gyroZ -= gyroZ_offset;
  // ----------------------------

  // オフセット補正後の値を計算
  float correctedGyroZ = gyroZ - gyroZ_offset;
  float correctedGyroX = gyroX - gyroX_offset;

  if (abs(correctedGyroZ) < deadZone) {
    correctedGyroZ = 0;
  }
  if (abs(correctedGyroX) < deadZone) {
    correctedGyroX = 0;
  }

  aimX += correctedGyroZ * deltaTime;
  aimY += correctedGyroX * deltaTime;
  // -------------------------------------------------

  // ...(省略)
}

void calibrateSensors() {
  // サンプリングを取得する回数 (回数を多くすると精度も上がるが初期化処理に時間がかかる)
  const int calibrationSamples = 1000;
  // 合算用の変数
  float gyroX_sum = 0, gyroY_sum = 0, gyroZ_sum = 0;

  M5.Display.setCursor(0, 80);
  M5.Display.print("Calibrating...");
  M5.Display.setCursor(0,100);
  M5.Display.print("Keep Stick STABLE!");

  for (int i = 0; i < calibrationSamples; i++) {
    float gx, gy, gz;
    M5.Imu.getGyroData(&gx, &gy, &gz);

    gyroX_sum += gx;
    gyroY_sum += gy;
    gyroZ_sum += gz;

    if (i % 100 == 0) {
        M5.Display.setCursor(0, 120);
        M5.Display.printf("Progress: %3d%%", (i * 100) / calibrationSamples);
    }
    delay(5);
  }

  gyroX_offset = gyroX_sum / calibrationSamples;
  gyroY_offset = gyroY_sum / calibrationSamples;
  gyroZ_offset = gyroZ_sum / calibrationSamples;

  M5.Display.fillScreen(BLACK);
  M5.Display.setCursor(0,80);
  M5.Display.print("Calibration Done!");
  delay(1500);
}

実行2

実行1と同様に動作させると、M5Stickの画面には次の様な表示が出る。

X軸：1.4 Y軸：7.2

キャリブレーションを行い、デッドゾーンを設定したため、ドリフトはある程度抑えられるようになった。 ただ、ドリフトしている値がそもそもデッドゾーンより大きくなっているので依然としてドリフトが発生している。

Madgwickフィルター実装

Madgwickフィルターとは

Madgwickフィルターとは、ジャイロセンサーのデータを組み合わせて姿勢を推定するフィルターである。 ジャイロセンサーのデータはドリフトが発生しやすいため、加速度センサーのデータを組み合わせて姿勢を推定する。 フィルタを利用することでこのドリフトを抑制してくれる。 詳しくはこちら

コード3

ライブラリの導入

MadgwickAHRSをArduinoIDEのライブラリマネージャーから検索してインストールする。

// ...(省略)
// ライブラリーの追加導入
#include <MadgwickAHRS.h>

// Madgwickフィルターのインスタンス
Madgwick MadgwickFilter;

float accX_offset = 0;
float accY_offset = 0;
float accZ_offset = 0;

void setup(){
  // ...(省略)
  // キャリブレーションを消す
  //calibrateSensors();
  // ...(省略)
}

void loop() {
  // ...(省略)
  float gyroX, gyroY, gyroZ;
  // 追加
  float accX, accY, accZ;

  M5.Imu.getGyroData(&gyroX, &gyroY, &gyroZ);
  // 追加
  M5.Imu.getAccelData(&accX, &accY, &accZ);

  // オフセット補正などは消す
  // gyroX -= gyroX_offset;
  // gyroY -= gyroY_offset;
  // gyroZ -= gyroZ_offset;
  // accX -= accX_offset;
  // accY -= accY_offset;
  // accZ -= accZ_offset;

  // Madgwickフィルターでセンサーデータを処理
  MadgwickFilter.updateIMU(gyroX, gyroY, gyroZ, accX, accY, accZ);

  // 姿勢角度を取得
  float roll = -MadgwickFilter.getRoll();
  float pitch = MadgwickFilter.getPitch();
  float yaw = MadgwickFilter.getYaw();

  // デッドゾーン処理（角度ベースで調整）
  aimX = pitch;
  aimY = roll;

  // ...(省略)
}

実行3

実行2と同様に動作させると、M5Stickの画面には次の様な表示が出る。

X軸：0.1 Y軸：-0.5

実行結果比較表

ドリフトが抑えられていることがわかる。 また、フィルターを通してピッチとロールから角度を取得しているため、 利用しているうちに誤差が修正されていき、精度が向上していく（らしい）。

あとがき

実装1では明らかにM5Stickに触っていない状態でも値が更新され続けていた。 この状態では利用に耐えないため、実際に実装するとなった場合には、 キャリブレーションとデッドゾーンの設定をすることは間違いなく必要になることがわかった。

Madgwickフィルターであるが、実際に画面上で見る限りではIMUから取得したデータを利用している場合と差異があまりなかった。 Madgwickフィルターの仕様に関して理解しきれていないのでその様な結果になっている可能性がある。

取得したデータを実際に利用するにはハード側でどの様な動きをする想定なのかを基に利用する必要がある。 従って、値を活用する術も調べていく。