Help us understand the problem. What is going on with this article?

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"と表示される)
③約1秒後、本体を持ち上げてRoll, Pitch, Yaw方向にまんべんなくぐるぐる回す。
④もう一度M5ボタンを押す。

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

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

参考

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

hatt_takumi
Pythonエンジニアです。 会話ロボット、エンタメ、インタラクティブコンテンツなどの案件によくアサインしています。 webも勉強中
Why not register and get more from Qiita?
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
Comments
No comments
Sign up for free and join this conversation.
If you already have a Qiita account
Why do not you register as a user and use Qiita more conveniently?
You need to log in to use this function. Qiita can be used more conveniently after logging in.
You seem to be reading articles frequently this month. Qiita can be used more conveniently after logging in.
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
ユーザーは見つかりませんでした