MPU6050 の Digital Motion Processor の性能を試してみた（４）

前回の検討で、MPU6050をCalibrationをしてもオフセットが残ってしまう問題が残りました。何か解決策を考えなければなりません。





一般的なやり方は、センサーの出力値からローパスフィルターを使ってＤＣ成分を得て除去するというもの。Android のページでも紹介されています。

SensorEvent
https://developer.android.com/reference/android/hardware/SensorEvent.html


具体的には下記のようなアルゴリズムです。

 // alpha は 0.5 ～ 0.8 で調整
 lpf_signal = alpha * x[i-1] + (1 - alpha) * x[i]; // low pass filter
 hpf_signal = x[i] - lpf_signal; // distill high pass value using low pass value

しかし、これを実際のセンサーの出力（10msec間隔）に適用すると、ローパスフィルタの値がセンサーの出力値とほぼ重なってしまい、正しい加速度が算出できません。（赤がローパスフィルターの波形で、その下に青のセンサーからの出力波形が隠れています）




理想的には数十サンプルの平均値をとるようなローパスフィルターを実現したいところです。でも配列はメモリを食うので使いたくないし・・・。

ということで、下記のようなアルゴリズムを考えてみました。

#include 
#include 
#include "I2Cdev.h"
#include "MPU6050DMP.h"
extern "C" {
#include "user_interface.h"
}

#define INT_PIN 4

MPU6050DMP mpu6050;
bool dmpReady = false;
uint16_t  packetSize = 0;

volatile bool mpuInterrupt = false;
void dmpDataReady() {
    mpuInterrupt = true;
}

unsigned long getCurrentTimeMS() {
  return ESP.getCycleCount() / 80000L;
}

void setup() {

  Wire.begin(12, 14);  
  Wire.setClock(400000);
  Serial.begin(115200);
  pinMode(INT_PIN, INPUT);
  WiFi.disconnect();
  WiFi.mode(WIFI_OFF);
  WiFi.forceSleepBegin();

  if (mpu6050.testConnection()) {
    Serial.println("MPU6050 OK");
    attachInterrupt(INT_PIN, dmpDataReady, RISING);
  } else {
    Serial.println("MPU6050 NG");
    return;
  }
  
  mpu6050.initialize();
  // offset values for my sensor
  mpu6050.setXAccelOffset(565);
  mpu6050.setYAccelOffset(-5);
  mpu6050.setZAccelOffset(1432);
  mpu6050.setXGyroOffset(17);
  mpu6050.setYGyroOffset(550);
  mpu6050.setZGyroOffset(464);

  Serial.println("");
  Serial.println("");

  if (mpu6050.dmpInitialize() == 0) {
    Serial.println("MPU6050DMP OK");
    mpu6050.setDMPEnabled(true); 
    packetSize = mpu6050.dmpGetFIFOPacketSize();
    dmpReady = true;    
  } else {
    Serial.println("MPU6050DMP NG");
  }
}

uint16_t fifoCount  = 0;
uint8_t  fifoBuffer[64];
unsigned long preTime = 0;

unsigned int loop_counter = 0;

#define WSIZE 16 // change this value for your purpose

long sum_x = 0;
long sum_y = 0;
long sum_z = 0;
int adj_x = 0;
int adj_y = 0;
int adj_z = 0;


void loop() {
  
  if (!dmpReady) return;
  
  while (!mpuInterrupt);
  mpuInterrupt = false;

  while (fifoCount < packetSize) {
    fifoCount = mpu6050.getFIFOCount();
  }
  
  uint8_t mpuIntStatus = mpu6050.getIntStatus();
  
  if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
    mpu6050.resetFIFO();
    return;
  } else if ((mpuIntStatus & 0x02) == false) {
    return;
  }
  
  mpu6050.getFIFOBytes(fifoBuffer, packetSize);
  fifoCount -= packetSize;

  unsigned long curTime = getCurrentTimeMS();
  unsigned long duration = curTime - preTime;

  Quaternion q;
  VectorInt16 aa;
  VectorInt16 aaReal;
  VectorFloat gravity;
  
  mpu6050.dmpGetQuaternion(&q, fifoBuffer);
  mpu6050.dmpGetAccel(&aa, fifoBuffer);
  mpu6050.dmpGetGravity(&gravity, &q);
  mpu6050.dmpGetLinearAccel(&aaReal, &aa, &gravity);
  
  if (loop_counter < 2000) {
    if (loop_counter % 80 == 0)
      Serial.println("");
    ++loop_counter;
    Serial.print(".");
    return;
  } else if (loop_counter < 2000 + WSIZE) {
    sum_x += aaReal.x;
    sum_y += aaReal.y;
    sum_z += aaReal.z;
    if (loop_counter % 80 == 0)
      Serial.println("");
    ++loop_counter;
    Serial.print("*");
    return;
  } else {
    adj_x = sum_x / WSIZE;
    adj_y = sum_y / WSIZE;
    adj_z = sum_z / WSIZE;
    
    sum_x = sum_x - adj_x + aaReal.x;
    sum_y = sum_y - adj_y + aaReal.y;
    sum_z = sum_z - adj_z + aaReal.z;  
    
    /* 
    // High-pass values
    aaReal.x -= adj_x;
    aaReal.y -= adj_y;
    aaReal.z -= adj_z; 
    */   
  }
  
  Serial.print("areal\t");
  Serial.print(aaReal.x);
  Serial.print("\t");
  Serial.print(aaReal.y);
  Serial.print("\t");
  Serial.print(aaReal.z);
  Serial.print("\t");
  Serial.print(adj_x);
  Serial.print("\t");
  Serial.print(adj_y);
  Serial.print("\t");
  Serial.print(adj_z);
  Serial.print("\t");
  Serial.print(duration);
  Serial.println("");
  preTime = curTime;
}

アイディアは単純です。指定サンプル分の積算値を保持しておき、そこから平均値を出します。計算が終わったら、積算値から平均値を一つ分を減算して、直近の得られた値を積算値に加えるというものです。

このアルゴリズム適用した結果を以下に示します。赤がサンプル数分の平均値で、青がセンサーからの出力です。なかなか良い感じのデータが得られました。


16サンプル平均



32サンプル平均



64サンプル平均



128サンプル平均



このフィルターの欠点はオリジナルの波形に対して少し位相が遅れてしまうところです。位相の遅延を最低限にしつつ加速度の値をとれそうなのは、64か32あたりかな・・・。

適正なサンプル数は速度や距離を実測して決めたいと思います。
（＾＾）

---

MPU-6050搭載三軸加速度三軸ジャイロセンサモジュール

• 出版社/メーカー: スイッチサイエンス
• メディア: エレクトロニクス

ESPr Developer（ESP-WROOM-02開発ボード）

• 出版社/メーカー: スイッチサイエンス
• メディア: エレクトロニクス

実践ディジタル・フィルタ設計入門―JavaアプレットとC++プログラムを使った (ディジタル信号処理シリーズ)

• 作者: 岩田 利王
• 出版社/メーカー: CQ出版
• 発売日: 2004/10
• メディア: 単行本

---