M5StickCで姿勢角度を測る

昔ながらに、カメラを三脚に取り付けて、複数の画像をつなげて手動でパノラマ撮影をしようとおもいました。
カメラを水平回転させるときに三脚に角度メモリがついていなかったので、6-Axisセンサ（6軸ジャイロセンサ）を使ったら幸せになるのではないかと思い6-Axisセンサ基板を買いました。
が、そもそも表示器として使おうとしていたM5StickCにa-Axisセンサが付いているではありませんか＿|￣|○
というわけで、M5StckCの内蔵6-Axisセンサを使うことにしました。

#準備
##M5StickCのUSBシリアル・ドライバのインストール
M5STACKのドキュメントページ「Tutorial&Quick-Start」に載っている通り進めていきました。

###M5StickCがビックリマーク
私のPC Windows10 Pro環境では、M5StickCのUSB仮想シリアルポートがめでたく！マークになりました。

###FTDIドライバのインストールで解決
Tutorialの「USB Driver problem」に書かれていた通り実行し、別途ドライバをインストールすることで、無事にM5StickcがPCに接続できました。

#素の6-AXISセンサの出力と問題点
さて、とりあえずセンサーを動かしてみました。
スゲードリフトです。M5StickCを触らなくとも、どんどん値が変化してしまいます。

gyro.ino

#include <M5StickC.h>
float accX = 0.0F;
float accY = 0.0F;
float accZ = 0.0F;
float gyroX = 0.0F;
float gyroY = 0.0F;
float gyroZ = 0.0F;
float pitch = 0.0F;
float roll  = 0.0F;
float yaw   = 0.0F;
int mode = -1;
void setup() {
  M5.begin();
  M5.IMU.Init();
  M5.Lcd.setTextSize(2);
  M5.Lcd.setRotation(1);
}
void loop() {
  M5.update();
  // データ取得
  M5.IMU.getGyroData(&gyroX, &gyroY, &gyroZ);
  M5.IMU.getAccelData(&accX, &accY, &accZ);
  M5.IMU.getAhrsData(&pitch, &roll, &yaw);
  // モードチェンジ
  if ( mode == -1 || M5.BtnA.wasReleased() ) {
    mode++;
    mode = mode % 3;
    // タイトル出力
    M5.Lcd.setCursor(0, 0);
    if ( mode == 0 ) {
      M5.Lcd.printf("Gyro");
    } else if ( mode == 1 ) {
      M5.Lcd.printf("Acc");
    } else if ( mode == 2 ) {
      M5.Lcd.printf("Pitch");
    }
  }
  // データ出力
  M5.Lcd.setCursor(0, 30);
  if ( mode == 0 ) {
    M5.Lcd.printf("X:%.2f\nY:%.2f\nZ:%.2f", gyroX, gyroY, gyroZ);
  } else if ( mode == 1 ) {
    M5.Lcd.printf("X:%.2f\nY:%.2f\nZ:%.2f", accX, accY, accZ);
  } else if ( mode == 2 ) {
    M5.Lcd.printf("X:%.2f\nY:%.2f\nZ:%.2f", pitch, roll, yaw);
  }
  delay(500);
}

#ドリフトの解決方法
先人の方々のお知恵を拝借しながら問題解決をしていきましょう。

##センサ出力のオフセットを計測して出力補正する
まずは、ジャイロセンサー単独の測定を精度を上げる試みです。
数値を眺めていると、M5StickCの電源を入れた直後のドリフトが大きく、少しずつ変化が小さくなっているようです。どうやら温度変化によるドリフトですね。
従って、温度変化によるドリフトの傾きは補正しきれませんが、一定時間のドリフトの傾きで出力値を補正します。

生データと比較して、かなりドリフトの速度は低下したのですが、予想した通りドリフトは収まりません。

calibration

void calibration(){
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setTextSize(1);
  M5.Lcd.setCursor(0, 0);
  M5.Lcd.println("Calibrating...\n");
  delay(500);

  //補正値を求める
  float gyroSum[3];
  float accSum[3];
  int COUNTER = 500;
  for(int i = 0; i < COUNTER; i++){
    readGyro();
    gyroSum[0] += gyro[0];
    gyroSum[1] += gyro[1];
    gyroSum[2] += gyro[2];
    accSum[0] += acc[0];
    accSum[1] += acc[1];
    accSum[2] += acc[2];
    delay(10);
  }
  gyroOffset[0] = gyroSum[0]/COUNTER;
  gyroOffset[1] = gyroSum[1]/COUNTER;
  gyroOffset[2] = gyroSum[2]/COUNTER;
  accOffset[0] = accSum[0]/COUNTER;
  accOffset[1] = accSum[1]/COUNTER;
  accOffset[2] = accSum[2]/COUNTER - 1.0;//重力加速度1G

  M5.Lcd.println("    X       Y       Z\n");
  M5.Lcd.printf("%7.2f %7.2f %7.2f\n", gyroOffset[0], gyroOffset[1], gyroOffset[2]);
  M5.Lcd.printf("%7.2f %7.2f %7.2f\n", accOffset[0]*1000, accOffset[1]*1000, accOffset[2]*1000);
  delay(2000);  
  M5.Lcd.fillScreen(BLACK);

  readGyro();
  kalmanX.setAngle(getRoll());
  kalmanY.setAngle(getPitch());
  kalAngleZ = 0;
  lastMs = micros();
}

##カルマンフィルタを使って平滑化する
なんで６軸のセンサー（ジャイロ＋加速度）なんだろう、という疑問がここで解けました。
つまり両方使わないとダメなんですね。
ということで、加速度センサーを使ってジャイロの値をカルマンフィルタしてやります。

おっ！
Ｘ，Ｙ軸の値がおちつきました。
しかし、Ｚ軸はだめです。といかカルマンフィルタがかけられません。

kalman

#include <Kalman.h>
Kalman kalmanX;
Kalman kalmanY;
Kalman kalmanZ;

void loop() {
  // put your main code here, to run repeatedly:
  readGyro();
  applyCalibration();
  float dt = (micros() - lastMs) / 1000000.0;
  lastMs = micros();
  float roll = getRoll();
  float pitch = getPitch();
  float yaw = getYaw();
  
  kalAngleX = kalmanX.getAngle(roll, gyro[0], dt);
  kalAngleY = kalmanY.getAngle(pitch, gyro[1], dt);
  kalAngleZ += gyro[2] * dt;
  kalAngleZ = fmod(kalAngleZ, 360);
  //kalAngleZ = kalmanZ.getAngle(yaw, gyro[2], dt);

  //100回に1回だけ描画
  tick++;
  if(tick % 100 == 0){
    tick = 0;
    draw();

    if(digitalRead(M5_BUTTON_HOME) == LOW){
      calibration();
    }
  }
  
  delay(2);
}

##水平角の補正に関する課題
重力を利用した加速度センサーでできるのはここまででしょう。
これ以上は、磁気コンパスを含むセンサーに変えて、水平方向の加速度変化でカルマンフィルタでフィルタしてやらないとダメそうです。

#ソースコード
GitHubにまとめておきました。
https://github.com/NoriMasaTot/Arduino.git

#参考リンク
失念してしまいました。調べて追記します。

#蛇足
準備が整ってきたんですが、このご時世で出かけることが少なくなったこともあり、肝心のパノラマ撮影の熱は冷めちゃいました...