M5StickC(MPU6886)+ENV Hatで姿勢推定する(Madgwickフィルタ編)

こちらの記事はM5StickC(MPU6886)+ENV Hatで6+3軸IMUの値を取得するの続きです。

※こちらの記事で使用しているM5StickCとENV Hatは、6軸IMUがMPU6886、3軸地磁気センサがBMM150という組み合わせです。SH200でも動くかと思いますが、動作未検証です。

最初に

前回、M5StickC(MPU6886)+ENV Hatで、角速度、加速度、地磁気の6+3軸IMUの値を補正して、概ね正しく取得することができた。今回はこれらの値を使っての姿勢推定を行い、Madgwickフィルタを使ってみる。

姿勢推定について

姿勢推定を行うには取得した値から行列計算を行う必要がある。
計算方法については、以下にまとめてくださっているのがとても参考になる。
動画もあってわかりやすいのでお勧めです。
http://watako-lab.com/2019/01/01/m5stack_sens_9axis/

行列変換については、詳しく書くと手間なのでここでは省く。
詳しく知りたい方は、上記以外にも調べればたくさん出てくると思うので、そちらを参照してください。

フィルタに関して

姿勢に関しては行列計算で求められるが、求めた値をそのまま使うとドリフトが発生する。ぐるぐる回したり振ったりすると、どんどん誤差を蓄積してずれていく。
フィルタを用いることで、これらを抑えることができる。
以下の三種がよくつかわれている模様。

• 相補フィルタ
• Kalmanフィルタ
• Madgwickフィルタ

相補フィルタに関しては今回使用しなかったので調べていないが、Madgwick、Kalmanに関してはArduinoのライブラリがある。MadgwickがKalmanの上互換みたいな記述が多いがそうなのだろうか。

また、本来誤差を取り除いてくれるのがフィルタの役割なので、生のセンサ値を突っ込んでもいける。と思うのだが、、そのまま突っ込んでもドリフトが止まらなかったため、事前に値のオフセットを行っている。

*私もフィルタの中身に関しては難しくて理解できていない派です。

ソースコード

ソースコードは以下。
上で行列計算が必要と述べたが、Madgwickのライブラリを使う場合、それらもライブラリ内でまとめてやってくれるので不要。
update()に角速度、加速度、地磁気の値それぞれを放り込んで、getRoll(),getPitch(),getYaw()で取得すればオーケー。

ポイントは

MadgwickFilter.begin(100);

でサンプルレートを設定した後、 loop()内にdelayを入れて、loop()が同じ周波数で回るように設定すること。

Madgwickのライブラリに関しては係数などは必要ないはずなのだが、何故か変位量が半分くらいになって取得されていたので、update自にgyroの値を0.49で割っている。Madgwickの方でもbetaという値がアルゴリズムゲインとして設定されていたが、privateだったので触るのはやめておいた。
ここに関してはよくわからない。。

また、processingでシリアルを受信してモデルを描画したかったので。bluetoothシリアルを使用している。

ソースコード

# include <M5StickC.h>
# include <Wire.h>
# include "bmm150.h"
# include "bmm150_defs.h"
# include "BluetoothSerial.h"
# include "Adafruit_Sensor.h"
# include <Adafruit_BMP280.h>
# include <MadgwickAHRS.h>

BluetoothSerial bluetooth;
Madgwick MadgwickFilter;

BMM150 bmm = BMM150();
bmm150_mag_data value_offset;
Adafruit_BMP280 bme;

float acc[3];
float accOffset[3];
float gyro[3];
float gyroOffset[3];
float mag[3];
float magOffset[3];
float magmax[3];
float magmin[3];
uint8_t setup_flag = 1;

float pitch = 0.0F;
float roll  = 0.0F;
float yaw   = 0.0F;

void setup() {
  MadgwickFilter.begin(100); //100Hz
  M5.begin();
  Wire.begin(0,26);
  M5.IMU.Init();
  Serial.begin(115200);
  bluetooth.begin("M5StickC");
  calibrate6886();
  pinMode(M5_BUTTON_HOME, INPUT);
  
  M5.Lcd.setRotation(3);
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setTextSize(1);
  M5.Lcd.setCursor(40, 0);
  M5.Lcd.println("IMU TEST");
  M5.Lcd.setCursor(0, 10);
  M5.Lcd.println("  X       Y       Z");
  M5.Lcd.setCursor(0, 50);
  M5.Lcd.println("  Roll    Pitch   Yaw");

  if(bmm.initialize() == BMM150_E_ID_NOT_CONFORM) {
    Serial.println("Chip ID can not read!");
    while(1);
  } else {
    Serial.println("Initialize done!");
    delay(0);
  }
  if (!bme.begin(0x76)){  
    Serial.println("Could not find a valid BMP280 sensor, check wiring!");
    while (1);
  }
  calibrateENV(10);
}

void calibrate6886(){
  float gyroSum[3];
  float accSum[3];
  int counter = 500;
  for(int i = 0; i < counter; i++){
    M5.IMU.getGyroData(&gyro[0],&gyro[1],&gyro[2]);
    M5.IMU.getAccelData(&acc[0],&acc[1],&acc[2]);
    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]/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ボタンが上向きで行う想定
}

void calibrateENV(uint32_t timeout)
{
  int16_t value_x_min = 0;
  int16_t value_x_max = 0;
  int16_t value_y_min = 0;
  int16_t value_y_max = 0;
  int16_t value_z_min = 0;
  int16_t value_z_max = 0;
  uint32_t timeStart = 0;

  bmm.read_mag_data();  
  value_x_min = bmm.raw_mag_data.raw_datax;
  value_x_max = bmm.raw_mag_data.raw_datax;
  value_y_min = bmm.raw_mag_data.raw_datay;
  value_y_max = bmm.raw_mag_data.raw_datay;
  value_z_min = bmm.raw_mag_data.raw_dataz;
  value_z_max = bmm.raw_mag_data.raw_dataz;
  delay(100);

  timeStart = millis();
  
  while((millis() - timeStart) < timeout)
  {
    bmm.read_mag_data();
    if(value_x_min > bmm.raw_mag_data.raw_datax)
    {
      value_x_min = bmm.raw_mag_data.raw_datax;
    } 
    else if(value_x_max < bmm.raw_mag_data.raw_datax)
    {
      value_x_max = bmm.raw_mag_data.raw_datax;
    }
    if(value_y_min > bmm.raw_mag_data.raw_datay)
    {
      value_y_min = bmm.raw_mag_data.raw_datay;
    } 
    else if(value_y_max < bmm.raw_mag_data.raw_datay)
    {
      value_y_max = bmm.raw_mag_data.raw_datay;
    }
    if(value_z_min > bmm.raw_mag_data.raw_dataz)
    {
      value_z_min = bmm.raw_mag_data.raw_dataz;
    } 
    else if(value_z_max < bmm.raw_mag_data.raw_dataz)
    {
      value_z_max = bmm.raw_mag_data.raw_dataz;
    }
    if(digitalRead(M5_BUTTON_HOME) == LOW){
      setup_flag = 1;
      while(digitalRead(M5_BUTTON_HOME) == LOW);
      break;
    }
    delay(100);
  }
  value_offset.x = value_x_min + (value_x_max - value_x_min)/2;
  value_offset.y = value_y_min + (value_y_max - value_y_min)/2;
  value_offset.z = value_z_min + (value_z_max - value_z_min)/2;
}

void applycalibration(){
  M5.IMU.getGyroData(&gyro[0],&gyro[1],&gyro[2]);
  M5.IMU.getAccelData(&acc[0],&acc[1],&acc[2]);
  gyro[0] -= gyroOffset[0];
  gyro[1] -= gyroOffset[1];
  gyro[2] -= gyroOffset[2];
  acc[0] -= accOffset[0];
  acc[1] -= accOffset[1];
  acc[2] -= accOffset[2];

  bmm150_mag_data value;
  bmm.read_mag_data();
  value.x = bmm.raw_mag_data.raw_datax - value_offset.x;
  value.y = bmm.raw_mag_data.raw_datay - value_offset.y;
  value.z = bmm.raw_mag_data.raw_dataz - value_offset.z;
  mag[0] = value.x;
  mag[1] = value.y;
  mag[2] = value.z;
}


void loop() {
  applycalibration();

  M5.Lcd.setCursor(0, 20);
  M5.Lcd.printf("%6.2f  %6.2f  %6.2f      ", gyro[0], gyro[1], gyro[2]);//deg
  M5.Lcd.setCursor(140, 20);
  M5.Lcd.print("o/s");

  M5.Lcd.setCursor(0, 30);
  M5.Lcd.printf(" %5.2f   %5.2f   %5.2f   ", acc[0], acc[1], acc[2]);
  M5.Lcd.setCursor(140, 30);
  M5.Lcd.print("G");

  float heading = atan2(mag[0], mag[1]);
  if(heading < 0)
    heading += 2*PI;
  if(heading > 2*PI)
    heading -= 2*PI;

  M5.Lcd.setCursor(0, 40);
  M5.Lcd.printf("headingDegrees: %2.1f", heading * 180/M_PI);

  MadgwickFilter.update(gyro[0]/0.49, gyro[1]/0.49, gyro[2]/0.49, acc[0], acc[1], acc[2], mag[0], mag[1], mag[2]);

  roll = MadgwickFilter.getRoll();
  pitch = MadgwickFilter.getPitch();
  yaw   = MadgwickFilter.getYaw();

  M5.Lcd.setCursor(0, 60);
  M5.Lcd.printf("%6.2f, %6.2f, %6.2f\r\n", roll, pitch, yaw);

  bluetooth.printf("%6.2f, %6.2f, %6.2f\r\n", roll, pitch, yaw);
  
  delay(10);

  if(!setup_flag){
     setup_flag = 1;
     if(bmm.initialize() == BMM150_E_ID_NOT_CONFORM) {
    while(1);
  } else {
    delay(0);
  }
  if (!bme.begin(0x76)){  
    while (1);
  }
  calibrate6886();
  calibrateENV(100000);
 }
 if(digitalRead(M5_BUTTON_HOME) == LOW){
  setup_flag = 0;
  while(digitalRead(M5_BUTTON_HOME) == LOW);
 }
 M5.Lcd.setCursor(140, 70);
 if (setup_flag==0) {
   M5.Lcd.printf("CAL");
 } else {
   M5.Lcd.printf("   ");
 }
}

使用方法

使用するに際して、キャリブレーションを行う必要がある。
方法については以下を参照してください。
M5StickC(MPU6886)+ENV Hatで6+3軸IMUの値を取得する

＊地磁気センサは環境の影響を受けやすいようなので、必ずM5StickCをPCから少し離して行うこと。
①画像のように、M5のボタンが上向きとなるように、机の上などの水平な面に置く。
②本体を揺らさないよう、M5ボタンを押す。(LCD右下に"CAL"と表示される)
③約１秒後、本体を持ち上げてRoll, Pitch, Yaw方向にまんべんなくぐるぐる回す。
④もう一度M5ボタンを押す。

キャリブレーション後は、M5ボタンが上になるように水平な面においてしばらく待つと、RollとPitchが0に収束していくが、時間が結構かかる。Madgwickは収束が遅いと聞いたが、ここのことだろうか？Yawに関してはM5ボタンが東を向く方向が0となる。

収束後は以下のような制度で利用できる。
(有線放送と子供の声が入ってます)

いいかんじ！
M5StickCとENV Hatで姿勢推定
(Madgwickフィルタ)#M5StickC #M5Stack#エンジニアと繋がりたい pic.twitter.com/vyYzrs8H7i

— たくみ@スカジャンのエンジニア (@hatt_takumi) April 24, 2020

参考

https://lang-ship.com/reference/unofficial/M5StickC/
http://watako-lab.com/2019/01/01/m5stack_sens_9axis/
http://kako.com/blog/?p=46574
https://ambidata.io/samples/m5stack/rotatem5stack/
https://lang-ship.com/blog/work/m5stickc-imu-mpu6886/
https://qiita.com/AceZeami/items/3760317d754798d5c06c
https://qiita.com/shidash/items/ad735faf23c6148f981a
https://qiita.com/foka22ok/items/53d5271a21313e9ddcbd
https://qiita.com/Google_Homer/items/d0cdb2975491a5ea8129
https://qiita.com/coppercele/items/e4d71537a386966338d0
https://shiker.hatenablog.com/entry/2019/08/24/004637