M5StickCの6軸入力から姿勢を求める(カルマンフィルタ編)

この前のMakerFaireTokyoでM5StickCを買ったので試してみました。
6軸センサの入力からX軸とY軸の角度を求めます。

MPU6886

M5StickCには6軸センサが載っています。
古いロットのものはSH200Qで、新しいロットのものにはMPU6886が搭載されているようです。
自分が買ったのはMPU6886のロットでした。

M5ロゴを正面にした場合、上がY軸の+、右がX軸の+、手前側がZ軸の+方向です。

M5.MPU6886.Init();

で初期化して

M5.MPU6886.getGyroData(&gyroX,&gyroY,&gyroZ);
M5.MPU6886.getAccelData(&accX,&accY,&accZ);

のように変数をポインタで渡すことでデータを取得できます。

getGyroDataやgetAccelDataはセンサの分解能設定によるスケール補正をかけてくれるので、ここで取得できる値の単位はdegree/sとG(≒9.8m/s^2)です。

Arduino用ライブラリの少し前のバージョンではMPU6886がMPU6866になっていたとかなんとか。

ジャイロに関しては平面に置いても0から少しずれるので、オフセット取って補正したほうが良さそうです。

カルマンフィルタ

難しくてちょっとわからない他のブログなど見ると詳しい解説がたくさん出てくるので、ここには詳しく書きません。

ノイズを含まれている複数のセンサからの入力を組み合わせ、現在の状態を推測することができるフィルタです。

今回はライブラリを使いました。
github.com
Arduinoのライブラリマネージャからインストールできます。

Kalman kalmanX;
Kalman kalmanY;

kalmanX.setAngle(getRoll());
kalmanY.setAngle(getPitch());

のようにインスタンスを作り初期角度を設定し、

kalAngleX = kalmanX.getAngle(roll, gyroX, dt);
kalAngleY = kalmanY.getAngle(pitch, gyroY, dt);

加速度から得られるX軸Y軸の角度、角加速度(degree/s)、前回呼び出してからの経過時間を渡せば、勝手に計算してくれます。便利。

プログラム

ということでできたプログラムがこちらです。
Arduinoです。

起動時にセンサのズレを補正するためのオフセットを測定しています、
カルマンフィルタを使っているので、補正必要ないかも。

実機で動かしてみるとかなり正確です。45度に傾けるときっちり45度近くの値が表示されます。

#include <Kalman.h>

#include <M5StickC.h>

//x, y, zの順
float acc[3];
float accOffset[3];
float gyro[3];
float gyroOffset[3];

float kalAngleX;
float kalAngleY;


Kalman kalmanX;
Kalman kalmanY;

long lastMs = 0;
long tick = 0;

void setup() {
  M5.begin();
  M5.Lcd.setRotation(3);
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setTextSize(1);
  M5.Lcd.setCursor(0, 0);
  M5.Lcd.println("    X       Y       Z");
  M5.MPU6886.Init();

  M5.Lcd.setCursor(0, 60);
  M5.Lcd.println("Calibrating...");
  delay(500);  
  calibration();

  M5.Lcd.setCursor(0, 60);
  M5.Lcd.printf("%7.2f %7.2f %7.2f", gyroOffset[0], gyroOffset[1], gyroOffset[2]);
  M5.Lcd.setCursor(0, 75);
  M5.Lcd.printf("%7.2f %7.2f %7.2f", accOffset[0]*1000, accOffset[1]*1000, accOffset[2]*1000);
  
  readGyro();
  kalmanX.setAngle(getRoll());
  kalmanY.setAngle(getPitch());

  lastMs = micros();
}

void calibration(){
  //補正値を求める
  float gyroSum[3];
  float accSum[3];

  for(int i = 0; i < 500; 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(2);
  }
  gyroOffset[0] = gyroSum[0]/500;
  gyroOffset[1] = gyroSum[1]/500;
  gyroOffset[2] = gyroSum[2]/500;
  accOffset[0] = accSum[0]/500;
  accOffset[1] = accSum[1]/500;
  accOffset[2] = accSum[2]/500 - 1.0;//重力加速度1G
}


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();
  
  kalAngleX = kalmanX.getAngle(roll, gyro[0], dt);
  kalAngleY = kalmanY.getAngle(pitch, gyro[1], dt);

  //20回に1回だけ描画
  tick++;
  if(tick % 20 == 0){
    tick = 0;
    draw();
  }
  
  delay(2);
}


void draw(){
  M5.Lcd.setCursor(0, 15);
  M5.Lcd.printf("%7.2f %7.2f %7.2f", gyro[0], gyro[1], gyro[2]);
  M5.Lcd.setCursor(140, 15);
  M5.Lcd.print("o/s");
  M5.Lcd.setCursor(0, 30);
  M5.Lcd.printf("%7.2f %7.2f %7.2f", acc[0] * 1000, acc[1] * 1000, acc[2] * 1000);
  M5.Lcd.setCursor(145, 30);
  M5.Lcd.print("mg");

  M5.Lcd.setCursor(0, 45);
  M5.Lcd.printf("%7.2f %7.2f", kalAngleX, kalAngleY);
  M5.Lcd.setCursor(140, 45);
  M5.Lcd.print("deg");
}


void readGyro(){
  M5.MPU6886.getGyroData(&gyro[0], &gyro[1], &gyro[2]);
  M5.MPU6886.getAccelData(&acc[0], &acc[1], &acc[2]);
}

void applyCalibration(){
  gyro[0] -= gyroOffset[0];
  gyro[1] -= gyroOffset[1];
  gyro[2] -= gyroOffset[2];
  acc[0] -= accOffset[0];
  acc[1] -= accOffset[1];
  acc[2] -= accOffset[2];
}

float getRoll(){
  return atan2(acc[1], acc[2]) * RAD_TO_DEG;
}

float getPitch(){
  return atan(-acc[0] / sqrt(acc[1]*acc[1] + acc[2]*acc[2])) * RAD_TO_DEG;
}

おわりに

M5StickCでカルマンフィルタを使う記事があまりなかったので書いてみました。

いろいろ調べていたらMadgwickフィルタなるものも見つけたので今度試してみたいです。
カルマンフィルタよりも処理が軽いとかなんとか？