ESP32 (ESP-WROOM-32)でMPU6050を使おう！(i2C) (Arduino IDE)

ESP32弄り日記第二弾は、安くて有名なInvenSense製６軸センサ、MPU6050を使おう！です。

MPU6050には３軸加速度センサー、３軸ジャイロセンサーが積んであります。３＋３＝６軸センサーというわけです。

ESP32とMPU6050を用いて、角度計を作ってみましょう。
今回もArduino IDEを使って開発していきます。

私が使用したのは以下のモジュールです。

<br />

これでOKです。（めちゃくちゃ安い。ありがたい。）

まず配線ですが、ESP32は以前書いたように、SCL、SDAピンを対応している中から自由に選べます。

今回はSCLピンをGPIO25、SDAピンをGPIO26にしました。

するとブレッドボードはこんな具合になります。

f:id:tocknblog:20170311164837j:plain

いやぁ、、、わちゃわちゃしてるなぁ、、、

スケッチはこんな感じです。
（一応断っておきますが、私は素人です…こんな書き方はヤメロぉ！とかありましたらご指摘ください…）

#include <Wire.h>

#define MPU6050_ADDR         0x68 // MPU-6050 device address
#define MPU6050_SMPLRT_DIV   0x19 // MPU-6050 register address
#define MPU6050_CONFIG       0x1a
#define MPU6050_GYRO_CONFIG  0x1b
#define MPU6050_ACCEL_CONFIG 0x1c
#define MPU6050_WHO_AM_I     0x75
#define MPU6050_PWR_MGMT_1   0x6b

double offsetX = 0, offsetY = 0, offsetZ = 0;
double gyro_angle_x = 0, gyro_angle_y = 0, gyro_angle_z = 0;
float angleX, angleY, angleZ;
float interval, preInterval;
float acc_x, acc_y, acc_z, acc_angle_x, acc_angle_y;
float gx, gy, gz, dpsX, dpsY, dpsZ;

void culcRotation();

//I2c書き込み
void writeMPU6050(byte reg, byte data) {
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(reg);
  Wire.write(data);
  Wire.endTransmission();
}

//i2C読み込み
byte readMPU6050(byte reg) {
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(reg);
  Wire.endTransmission(true);
  Wire.requestFrom(MPU6050_ADDR, 1/*length*/); 
  byte data =  Wire.read();
  return data;
}

void setup() {

  Wire.begin(26, 25);
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(0x6B); // PWR_MGMT_1 register
  Wire.write(0); // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  
  Serial.begin(9600);
  delay(100);
  
  //正常に接続されているかの確認
  if (readMPU6050(MPU6050_WHO_AM_I) != 0x68) {
    Serial.println("\nWHO_AM_I error.");
    while (true) ;
  }

  //設定を書き込む
  writeMPU6050(MPU6050_SMPLRT_DIV, 0x00);   // sample rate: 8kHz/(7+1) = 1kHz
  writeMPU6050(MPU6050_CONFIG, 0x00);       // disable DLPF, gyro output rate = 8kHz
  writeMPU6050(MPU6050_GYRO_CONFIG, 0x08);  // gyro range: ±500dps
  writeMPU6050(MPU6050_ACCEL_CONFIG, 0x00); // accel range: ±2g
  writeMPU6050(MPU6050_PWR_MGMT_1, 0x01);   // disable sleep mode, PLL with X gyro

  //キャリブレーション
  Serial.print("Calculate Calibration");
  for(int i = 0; i < 3000; i++){
    
    int16_t raw_acc_x, raw_acc_y, raw_acc_z, raw_t, raw_gyro_x, raw_gyro_y, raw_gyro_z ;
    
    Wire.beginTransmission(MPU6050_ADDR);
    Wire.write(0x3B);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU6050_ADDR, 14, true);
  
    raw_acc_x = Wire.read() << 8 | Wire.read();
    raw_acc_y = Wire.read() << 8 | Wire.read();
    raw_acc_z = Wire.read() << 8 | Wire.read();
    raw_t = Wire.read() << 8 | Wire.read();
    raw_gyro_x = Wire.read() << 8 | Wire.read();
    raw_gyro_y = Wire.read() << 8 | Wire.read();
    raw_gyro_z = Wire.read() << 8 | Wire.read();
    dpsX = ((float)raw_gyro_x) / 65.5;
    dpsY = ((float)raw_gyro_y) / 65.5;
    dpsZ = ((float)raw_gyro_z) / 65.5;
    offsetX += dpsX;
    offsetY += dpsY;
    offsetZ += dpsZ;
    if(i % 1000 == 0){
      Serial.print(".");
    }
  }
  Serial.println();

  offsetX /= 3000;
  offsetY /= 3000;
  offsetZ /= 3000;

  Serial.print("offsetX : ");
  Serial.println(offsetX);
  Serial.print("offsetY : ");
  Serial.println(offsetY);
  Serial.print("offsetZ : ");
  Serial.println(offsetZ);
  
}

void loop() {

  calcRotation();

  Serial.print("angleX : ");
  Serial.print(angleX);
  Serial.print("    angleY : ");
  Serial.print(angleY);
  Serial.print("    angleZ : ");
  Serial.println(angleZ);
 
}

//加速度、ジャイロから角度を計算
void calcRotation(){

  int16_t raw_acc_x, raw_acc_y, raw_acc_z, raw_t, raw_gyro_x, raw_gyro_y, raw_gyro_z ;
  
  //レジスタアドレス0x3Bから、計14バイト分のデータを出力するようMPU6050へ指示
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_ADDR, 14, true);
  
  //出力されたデータを読み込み、ビットシフト演算
  raw_acc_x = Wire.read() << 8 | Wire.read();
  raw_acc_y = Wire.read() << 8 | Wire.read();
  raw_acc_z = Wire.read() << 8 | Wire.read();
  raw_t = Wire.read() << 8 | Wire.read();
  raw_gyro_x = Wire.read() << 8 | Wire.read();
  raw_gyro_y = Wire.read() << 8 | Wire.read();
  raw_gyro_z = Wire.read() << 8 | Wire.read();
  
  //単位Gへ変換
  acc_x = ((float)raw_acc_x) / 16384.0;
  acc_y = ((float)raw_acc_y) / 16384.0;
  acc_z = ((float)raw_acc_z) / 16384.0;
  
  //加速度センサーから角度を算出
  acc_angle_y = atan2(acc_x, acc_z + abs(acc_y)) * 360 / -2.0 / PI;
  acc_angle_x = atan2(acc_y, acc_z + abs(acc_x)) * 360 / 2.0 / PI;

  dpsX = ((float)raw_gyro_x) / 65.5; // LSB sensitivity: 65.5 LSB/dps @ ±500dps
  dpsY = ((float)raw_gyro_y) / 65.5;
  dpsZ = ((float)raw_gyro_z) / 65.5;
  
  //前回計算した時から今までの経過時間を算出
  interval = millis() - preInterval;
  preInterval = millis();
  
  //数値積分
  gyro_angle_x += (dpsX - offsetX) * (interval * 0.001);
  gyro_angle_y += (dpsY - offsetY) * (interval * 0.001);
  gyro_angle_z += (dpsZ - offsetZ) * (interval * 0.001);
  
  //相補フィルター
  angleX = (0.996 * gyro_angle_x) + (0.004 * acc_angle_x);
  angleY = (0.996 * gyro_angle_y) + (0.004 * acc_angle_y);
  angleZ = gyro_angle_z;
  gyro_angle_x = angleX;
  gyro_angle_y = angleY;
  gyro_angle_z = angleZ;
}

まず最初のsetup()内のfor文ですが、これでジャイロセンサーのドリフト誤差を抑える為の計算をしています。

ドリフト誤差とはなんぞ？という方のために…
ジャイロセンサとは、つまりは角速度センサなのであります。角速度から角度を得るには…そう、積分ですよね！
プログラム内では、毎回毎回ジャイロセンサからの出力値と時間（秒）を掛け合わせたものを足していき、それが出力する角度としています。

理想的なジャイロセンサならば静止状態の時の出力値はゼロなのですが、世の中そうは行きません。普通は、静止状態でも常にゼロ以外の小さな値を出力し続けてしまいます。
放っておくと、どうなるでしょうか。
イエス、どんどん角度が増えて（減って）いってしまう！！！
これをドリフト誤差といいます。

それを防ぐために、静止状態で出力し続ける値を1000回位読み取って平均値を取り、その値を積分時に生の値から引いているのです。

さて次は、一番下の関数calcRotation()に注目してください。

その一部分

  raw_acc_x = Wire.read() << 8 | Wire.read();
  raw_acc_y = Wire.read() << 8 | Wire.read();
  raw_acc_z = Wire.read() << 8 | Wire.read();
  raw_t = Wire.read() << 8 | Wire.read();
  raw_gyro_x = Wire.read() << 8 | Wire.read();
  raw_gyro_y = Wire.read() << 8 | Wire.read();
  raw_gyro_z = Wire.read() << 8 | Wire.read();

ここ、何だこれって思いませんか？私は思います。

MPU6050は、各センサの出力値を

x軸加速度、y軸加速度、z軸加速度、温度、x軸角速度、y軸角速度、z軸角速度

の順番に出力しますが、さらにその各データは上位８bit→下位８bitの順に出力されています。つまり計14個のデータですね。
そこで、上記の部分ではビットシフト演算を行い、正しい値へ変換しているのです。

そしてその下、「単位Gへ変換」という部分ですが、MPU6050は出力した加速度センサのデータを、16384で割ることで単位Gへ変換することが出来ます。
ただしこれは設定で変更することが出来ます。
ちゃっかりその設定を、最初のsetup()内で行っていました。

  //設定を書き込む
  writeMPU6050(MPU6050_SMPLRT_DIV, 0x00);   // sample rate: 8kHz/(7+1) = 1kHz
  writeMPU6050(MPU6050_CONFIG, 0x00);       // disable DLPF, gyro output rate = 8kHz
  writeMPU6050(MPU6050_GYRO_CONFIG, 0x08);  // gyro range: ±500dps
  writeMPU6050(MPU6050_ACCEL_CONFIG, 0x00); // accel range: ±2g
  writeMPU6050(MPU6050_PWR_MGMT_1, 0x01);   // disable sleep mode, PLL with X gyro

ここです。（上から３，４行目）
今回は加速度センサは±2G、ジャイロセンサは±500dps(degree per second)まで計測できる設定にしています。
ここの設定によって、取得した生のデータから変換するための計算式が変わってくるのです。
詳しくはデータシートを見てネ！

その下

  //加速度センサーから角度を算出
  acc_angle_y = atan2(acc_x, acc_z + abs(acc_y)) * 360 / -2.0 / PI;
  acc_angle_x = atan2(acc_y, acc_z + abs(acc_x)) * 360 / 2.0 / PI;

こちらは書いてある通り、逆三角関数を用いて角度を計算しています。
選ぶ軸と正負には気をつけてね！！

  dpsX = ((float)raw_gyro_x) / 65.5; // LSB sensitivity: 65.5 LSB/dps @ ±500dps
  dpsY = ((float)raw_gyro_y) / 65.5;
  dpsZ = ((float)raw_gyro_z) / 65.5;

ジャイロセンサーから取得した生のデータを、dps (degree per second)へ変換しています。加速度のときと同じですね。

  //前回計算した時から今までの経過時間を算出
  interval = millis() - preInterval;
  preInterval = millis();
  
  gyro_angle_x += (dpsX - offsetX) * (interval * 0.001);
  gyro_angle_y += (dpsY - offsetY) * (interval * 0.001);
  gyro_angle_z += (dpsZ - offsetZ) * (interval * 0.001);

ここでは数値積分を行っています。最初に計算したoffset達がここで役立つわけなんです！

millis()は、ESP32の起動から今までの経過時間を単位ミリ秒で取得できます。interval * 0.001は、ミリ秒から秒へ変換しているわけです。

そして最後の

  //相補フィルター
  angleX = (0.996 * gyro_angle_x) + (0.004 * acc_angle_x);
  angleY = (0.996 * gyro_angle_y) + (0.004 * acc_angle_y);
  angleZ = gyro_angle_z;
  gyro_angle_x = angleX;
  gyro_angle_y = angleY;
  gyro_angle_z = angleZ;

ここでは相補フィルターというものを使い、加速度センサーから求めた角度と、ジャイロセンサーから求めた角度を組み合わせています。
なぜそんなことをする必要があるのか、それは、互いの欠点を打ち消し合い、長所を活かしているのです。

まずジャイロセンサーには前述の通り、ドリフト誤差という欠点があります。一応このプログラムではそれを出来る限り抑えるようにしていますが、それでもやはりドリフトしてしまいます。

そして加速度センサーは、角度を加速度で求めているというのが欠点になります。なぜなら、静止状態なら良いのですが、対象が動く物体だったらどうでしょう。激しく動けば動くほど、あらゆる軸に加速度がかかり、角度がブレてしまうのです。

まとめると、

ジャイロセンサーは長時間運用に弱いが、瞬間的ならばほぼ正確な値を出力する。

加速度センサーは長時間運用に強いが、瞬間的なもの（外力）には弱い

そこで

　angleX = (0.996 * gyro_angle_x) + (0.004 * acc_angle_x);
  angleY = (0.996 * gyro_angle_y) + (0.004 * acc_angle_y);

この式なのです。
瞬間的にはジャイロセンサーの値を適用するが、ほぼ静止するとじわじわと加速度センサーの値へ近づいていくのがわかると思います。
これが相補フィルターなのです。（多分）
他にもNASAの？何かの？姿勢制御に使われた？カルマンフィルターなどがあるそうなのですが？、よくわかりませんでした。
こういったセンサーを組み合わせることを、センサフュージョンというらしいです。かっこいい！

説明は大体こんな感じでしょうか。

f:id:tocknblog:20170311175829p:plain

ちゃんと書き込めれば、こんな感じになると思います。

何か質問、ご指摘等ありましたら、気軽にコメントしてください！

以上！

<br />

とっくんラボ

電子工作、プログラミングで日々の発見や成果をよろづに

ESP32 (ESP-WROOM-32)でMPU6050を使おう！(i2C) (Arduino IDE)