M5StickCのIMU(AHRS)研究その1 標準ライブラリ

概要
標準スケッチ
軸を考える
原理について
補正する方針を決める
1. 立ち上がりについて
2. オフセットについて
補正版スケッチ
まとめ

概要

M5StikCのIMUを使った姿勢測定のAHRSを研究してみました。過去はオフセットがひどかったのでちょっとしか使ったことがなかったのですが、オフセットの補正なども研究しました。

標準スケッチ

#include <M5StickC.h>
hw_timer_t *timer;
QueueHandle_t xQueue;
TaskHandle_t taskHandle;
// タイマー割り込み
void IRAM_ATTR onTimer() {
  int8_t data;
  // キューを送信
  xQueueSendFromISR(xQueue, &data, 0);
}
// 実際のタイマー処理用タスク
void task(void *pvParameters) {
  while (1) {
    int8_t data;
    // タイマー割り込みがあるまで待機する
    xQueueReceive(xQueue, &data, portMAX_DELAY);
    // 実際の処理
    float pitch;
    float roll;
    float yaw;
    M5.IMU.getAhrsData(&pitch, &roll, &yaw);
    Serial.printf(" %f,%f,%f\n", pitch, roll, yaw);
    Serial.flush();
  }
}
void setup() {
  M5.begin();
  M5.IMU.Init();
  // キュー作成
  xQueue = xQueueCreate(1, sizeof(int8_t));
  // Core1の優先度5でタスク起動
  xTaskCreateUniversal(
    task,           // タスク関数
    "task",         // タスク名(あまり意味はない)
    8192,           // スタックサイズ
    NULL,           // 引数
    5,              // 優先度(大きい方が高い)
    &taskHandle,    // タスクハンドル
    APP_CPU_NUM     // 実行するCPU(PRO_CPU_NUM or APP_CPU_NUM)
  );
  // 4つあるタイマーの1つめを利用
  // 1マイクロ秒ごとにカウント(どの周波数でも)
  // true:カウントアップ
  timer = timerBegin(0, getApbFrequency() / 1000000, true);
  // タイマー割り込み設定
  timerAttachInterrupt(timer, &onTimer, true);
  // マイクロ秒単位でタイマーセット
  timerAlarmWrite(timer, 10 * 1000, true);
  // タイマー開始
  timerAlarmEnable(timer);
}
void loop() {
  delay(1);
}

基本となるスケッチです。タイマーを使って10ミリ秒間隔で取得する例です。IMUはある程度正確な周期で呼び出さないと精度がでないようですので、高精度タイマーを使ってみました。

処理は初期化して、10ミリ秒ごとにIMUから姿勢情報を取得してシリアルに出力しています。

机においていると、こんな感じのグラフになります。青がピッチ、赤がロール、緑がヨーになります。青と赤はちょっと立ち上がり安定していないですね。緑は元気に増え続けています。

軸を考える

ローリング（Wikipedia）

飛行機の場合、ロールは進行方向に対して回転することです。右にロールした場合は右側に傾き、右に曲がることです。

ピッチング（Wikipedia）

ピッチは上下方向の動きで、ピッチを上げると飛行機が上昇します。

ヨーイング（Wikipedia）

ヨーは横方向の動きになります。ラダーで横方向に移動する動きです。

さて、M5StickCの軸を見てみると、ロールとピッチが逆の動きになっているように思えます。公式サンプルを見ればわかるのですが、縦画面ではなく横画面になっていると思います。

つまり上記のように右に倒しているので、ロールだと思うとピッチが変化します。

横画面で判定しているので、ピッチが変化します。個人的にはM5マークが手前にくる縦画面で判定するのが正しく思えます。。。

これは使っているライブラリと軸が違うのでずれているように思えます、、、

公式ライブラリの場合には、横にしてピッチを持ち上げる(画面を手前に傾ける)とプラスでした。ロールは左側に回転させるとプラスになりました。ヨーも左回りに回転させるとプラスになっています。

原理について

主に加速度センサーを利用すると、地面方向に重力の1Gがかかっているので地面の方向がわかります。この値を使ってロールとピッチは計算します。動きがある場合には加速度センサーがぶれてしまうので、これにジャイロセンサーの回転量も計算することで、補正しています。

ヨーは電子コンパスと呼ばれる機能で、本来は磁気センサーを使ってどっちの方角を向いているのかを測定します。ただしM5StickCのIMUは6軸と呼ばれる加速度とジャイロセンサーのみ搭載しているセンサーになり、磁気センサーがありません。そのため本来は方角を測定することができません。しかし、ジャイロの回転した方向を元に擬似的に計算することができます。とはいえ、誤差がかなりありますので、このグラフのようにどんどんずれていきます。

補正する方針を決める

立ち上がりについて

最初の100データ(1秒間)はデータがおかしかったので使わない方が良さそうです。150データぐらい捨てれば大丈夫だと思いますが、安全に200データ(2秒間)ぐらい読み捨てることにします。

オフセットについて

ロールとピッチは停止させている限りは数値が一定でしたので、一定の期間データを取得してから、ずれている値を補正してあげれば大丈夫そうです。

ヨーは、原理的に正しい値になりません。厳密な値にはならないので、かなり限定して使う値になるように補正したいと思います。

補正版スケッチ

#include <M5StickC.h>
hw_timer_t *timer;
QueueHandle_t xQueue;
TaskHandle_t taskHandle;
// タイマー割り込み
void IRAM_ATTR onTimer() {
  int8_t data;
  // キューを送信
  xQueueSendFromISR(xQueue, &data, 0);
}
// 実際のタイマー処理用タスク
void task(void *pvParameters) {
  uint8_t calibration = 0;        // キャリブレーションの状態(0:初期化直後, 1:データ取得中, 2:完了)
  uint16_t calibrationCount = 0;  // データ取得数
  float pitchSum = 0;             // ピッチの累計数
  float rollSum = 0;              // ロールの累計数
  float yawSum = 0;               // ヨーの累計数
  float pitchOffset = 0;          // ピッチのオフセット
  float rollOffset = 0;           // ロールのオフセット
  float yawOffset = 0;            // ヨーのオフセット
  float yawOld = 0;               // 前回のヨー
  float yaw2 = 0;                 // 補正後のヨー
  while (1) {
    int8_t data;
    float pitch;
    float roll;
    float yaw;
    // タイマー割り込みがあるまで待機する
    xQueueReceive(xQueue, &data, portMAX_DELAY);
    M5.IMU.getAhrsData(&pitch, &roll, &yaw);
    if (calibration == 0) {
      // 最初の200個は読み捨てる
      calibrationCount++;
      if (200 <= calibrationCount) {
        calibration = 1;
        calibrationCount = 0;
        yawOld = yaw;
      }
    } else if (calibration == 1) {
      // 一定時間データを取得してオフセットを計算する
      float gyroX = 0.0F;
      float gyroY = 0.0F;
      float gyroZ = 0.0F;
      M5.IMU.getGyroData(&gyroX, &gyroY, &gyroZ);
      float gyro = abs(gyroX) + abs(gyroY) + abs(gyroZ);
      if (20 < gyro) {
        // 振動があった場合には再度キャリブレーション
        calibrationCount = 0;
        pitchSum = 0;
        rollSum = 0;
        yawSum = 0;
        Serial.printf("Calibration Init!!!!! %f\n", gyro);
      } else {
        // 累計を保存
        pitchSum += pitch;
        rollSum += roll;
        yawSum += (yawOld - yaw); // ヨーは差分を累計する
        yawOld = yaw;
        calibrationCount++;
        if (500 <= calibrationCount) {
          // 一定数溜まったらオフセット計算
          calibration = 2;
          pitchOffset = pitchSum / calibrationCount;
          rollOffset = rollSum / calibrationCount;
          yawOffset = yawSum / calibrationCount;
          Serial.printf(" pitchSum = %f\n", pitchSum);
          Serial.printf(" rollSum = %f\n", rollSum);
          Serial.printf(" yawSum = %f\n", yawSum);
          Serial.printf(" pitchOffset = %f\n", pitchOffset);
          Serial.printf(" rollOffset = %f\n", rollOffset);
          Serial.printf(" yawOffset = %f\n", yawOffset);
        }
      }
    } else {
      // 補正
      pitch -= pitchOffset;
      roll -= rollOffset;
      float yawDelta = yawOld - yaw;
      yawOld = yaw;
      if (-200 < yawDelta && yawDelta < 200) {
        // 急激な変化は補正しない
        yaw2 -= yawDelta - yawOffset;
        // 10%減少させて原点に戻す
        yaw2 = yaw2 * 0.9;
      }
      // 出力
      Serial.printf(" %f,%f,%f\n", pitch, roll, yaw2);
      Serial.flush();
    }
  }
}
void setup() {
  M5.begin();
  M5.IMU.Init();
  // キュー作成
  xQueue = xQueueCreate(1, sizeof(int8_t));
  // Core1の優先度5でタスク起動
  xTaskCreateUniversal(
    task,           // タスク関数
    "task",         // タスク名(あまり意味はない)
    8192,           // スタックサイズ
    NULL,           // 引数
    5,              // 優先度(大きい方が高い)
    &taskHandle,    // タスクハンドル
    APP_CPU_NUM     // 実行するCPU(PRO_CPU_NUM or APP_CPU_NUM)
  );
  // 4つあるタイマーの1つめを利用
  // 1マイクロ秒ごとにカウント(どの周波数でも)
  // true:カウントアップ
  timer = timerBegin(0, getApbFrequency() / 1000000, true);
  // タイマー割り込み設定
  timerAttachInterrupt(timer, &onTimer, true);
  // マイクロ秒単位でタイマーセット
  timerAlarmWrite(timer, 10 * 1000, true);
  // タイマー開始
  timerAlarmEnable(timer);
}
void loop() {
  // メイン処理は無し
  delay(1);
}

起動して、200データ読み捨ててから、キャリブレーションを行っています。キャリブレーション中にジャイロが一定値以上発生した場合には、停止していないと判定して、再度キャリブレーションをします。

ロールとピッチは単純にオフセット値を計算して、補正することでなんとかなりそうでした。ヨーはいろいろ補正をしてみて、グラフの傾きがかなり減ったのですが完全に安定させることはできませんでした。

そのため、差分を積み上げていく方式にして、さらに10%単位で原点に近づける処理をいれています。

上記のようなグラフになりました。とはいえ、ヨーは0になるように補正しているので、ちょっとずるいです。ヨー軸に回転させると回転方向がわかります。

ただし、移動を止めるとすぐに原点に戻る動きになります。ロールとピッチは比較的正確にどの角度になっているかが推定でき、ヨーはその瞬間にどっちに回転しているかがわかるデータになりました。

ただし、ちょっと動かしていると予想外の動きがあります。上のグラフは机の上においてヨー軸方向に回転しているので、ロールとピッチは変化していないはずです。グラフ上変化しているのはおそらくジャイロなどの補正が効きすぎているのだと思います。

補正値はライブラリの中で固定が指定されているので、変更することはできません。そこで次回以降は標準ライブラリではなく、IMUの加速度、ジャイロのデータを元にAHRSライブラリを直接呼び出して、もう少し補正できるかを研究したいと思います。

まとめ

個人的にあまりIMUは使わないので、研究不足でした。それなりに利用者がいると思いますので、もう少しだけ突っ込んで研究してみたいと思います。

M5SLiteでもIMUにAHRSを組み込もうと少し作業したのですが、ロールとピッチの軸が気になってそこで止まっています。互換性なくなってしまいますが、補正してから軸の入れ替えをしたほうがいいのかな？