LoginSignup
14
9

More than 3 years have passed since last update.

@hatt_takumi(匠 服部)

M5StickC(MPU6886)+ENV Hatで6+3軸IMUの値を取得する

Last updated at Posted at 2020-04-21

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

imu1.jpg

最初に

M5StickCを使用して姿勢推定を行おうとしたら思ったより難しかった。
MPU6886+BMM150の情報が少なく、苦労したので記事にしておく。

正直、API叩けば一発で取得できるぐらいのものだろうと考えていたが、そんな高レベルなものはなかった。状況に合わせて補正、演算、フィルタリングなど自分で行わないといけない。
(一応getAhrsData()というものがあるが、使ってみたら微妙だった。。)

これから事前知識なしにIMUを触ろうとしている人は、以下の情報だけでも事前知識として頭に入れておくと、作業のはかどり具合が違うと思います。

  • 生値をそのまま使うと、ジャイロの特性上"ドリフト"という現象が発生する
  • 6軸IMUだけではYaw角が算出できない
  • 地磁気センサの初期位置は原点からずれている

姿勢推定の流れ

姿勢推定するためには、
①各センサのキャリブレーション→各値のオフセット量を求める
②センサから値を取得し、①で求めた量をオフセット→ドリフトを抑える
③行列計算を用いてRoll,Pitch,Yawを算出
④フィルタを使用して振動などによるノイズを取る→ここでもドリフトを抑える
という流れが王道っぽい。
フィルタはKalman, madgwickがよく使用されており、Arduinoのライブラリもある。
今回は、ひとまず②までを記事にしておく。

ソースコードと使用方法

ソースコード

ソースコードは以下
ENV Hat周りのヘッダファイルが必要なので、Arduinoのサンプルライブラリ内にある
スケッチ例>M5StickC>Hat>ENV
からもってくるとよい。
6886を使うライブラリはIMUとMPU6886の二種類があるが、特にこだわらなければIMUのほうでいいと思う。
IMUの方が中でMPU6886とSH200の違いを吸収してくれているので、後々のコード移植等もスムーズに行えそう。MPU6886を使う部分に関しては、中身はほぼ同じであった。

コメント 2020-04-22 143904.png

[参考]
https://lang-ship.com/reference/unofficial/M5StickC/Class/IMU/
https://lang-ship.com/reference/unofficial/M5StickC/Class/MPU6886/

ソースコード
#include <M5StickC.h>
#include <Wire.h>
#include <Kalman.h>
#include "bmm150.h"
#include "bmm150_defs.h"
#include "Adafruit_Sensor.h"
#include <Adafruit_BMP280.h>

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 t,t0;
float magOffset[3];
float magmax[3];
float magmin[3];
uint8_t setup_flag = 1;

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;

void setup() {
  M5.begin();
  Wire.begin(0,26);
  M5.IMU.Init();
  Serial.begin(115200);
  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");

  t=0; t0=0;
  magOffset[0]=0;
  magOffset[1]=0;
  magOffset[2]=0;
  if(bmm.initialize() == BMM150_E_ID_NOT_CONFORM) {
    Serial.println("Chip ID can not read!");
    while(1);
  } else {
    Serial.println("Initialize done!");
    delay(1);
  }
  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 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 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(1);
  }
  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 loop() {
  M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
  M5.IMU.getAccelData(&accX,&accY,&accZ);
  M5.IMU.getAhrsData(&pitch,&roll,&yaw);

  applycalibration();

//debug gyro
  M5.Lcd.setCursor(0, 20);
  M5.Lcd.printf("%6.2f  %6.2f  %6.2f      ", gyroX, gyroY, gyroZ);//deg
  M5.Lcd.setCursor(140, 20);
  M5.Lcd.print("o/s");

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

//debug accel
  M5.Lcd.setCursor(0, 40);
  M5.Lcd.printf(" %5.2f   %5.2f   %5.2f   ", accX, accY, accZ);
  M5.Lcd.setCursor(140, 40);
  M5.Lcd.print("G");

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

//debug mag
  float heading = atan2(mag[0], mag[1]);
  if(heading < 0)
    heading += 2*PI;
  if(heading > 2*PI)
    heading -= 2*PI;
  bmm.read_mag_data();
  float heading_raw = atan2(bmm.raw_mag_data.raw_datax, bmm.raw_mag_data.raw_datay);
  if(heading_raw < 0)
    heading_raw += 2*PI;
  if(heading_raw > 2*PI)
    heading_raw -= 2*PI;

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

  delay(100);

  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("   ");
 }
}

使用方法

書き込み後はLCDにTOPの画像のような表示がされているはず。
上から順に２行ずつ、角速度、ジャイロ、方位を示しており、それぞれが上が生値、下がオフセット値となっている。
起動後はキャリブレーションを行う必要があり、以下の手順で行う。
＊地磁気センサは環境の影響を受けやすいようなので、必ずM5StickCをPCから少し離して行うこと。

①画像のように、M5のボタンが上向きとなるように、机の上などの水平な面に置く。
②本体を揺らさないよう、M5ボタンを押す。(LCD右下に"CAL"と表示される)
③約１秒後、本体を持ち上げてRoll, Pitch, Yaw方向にまんべんなくぐるぐる回す。
④もう一度M5ボタンを押す。

以上でキャリブレーションは完了である。
机にM5ボタンを上向きに置いたときに、角速度のオフセット値が0近傍で振れていて、ジャイロのX,Yが0近傍、Zが1近傍で振れていれば概ねうまくオフセット出来ている。

方位に関しては、スマホアプリのコンパスなどと比較するとよい。
今のところ、偏角などは考慮していないです。

参考

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

14
9
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
14
9