2014年6月19日木曜日

加速度+ジャイロのGY-521(MPU-6050)を使ってみた -2- カルマンフィルター



こちらの記事ではarduino playgroundのスケッチを使い、加速度センサーから取り込んだ加速度から角度に変換していました。
ジャイロセンサーからの角速度を積分して回転角度になるのですが、ジャイロだけの数値を見ていると、センサーが静止していても少しずつ回転してしまいます。
正確な計測には、温度変化によるドリフトも考慮しないといけません。

それらの補正にはいろんな方法があるようですが、調べているとカルマンフィルターにたどり着きました。
ジャイロと加速度センサーの組み合わせで、ドリフトやノイズを取り除くようです。
カルマンフィルターとは

数学的には理解は難しいのですが、ライブラリがあるのでそれを利用します。

カルマンフィルターのライブラリのページからダウンロードしたものに、サンプルスケッチがあったので少し手を加え、processingに角度データを送信、3Dボックスを回転させるスケッチを書きました。
追記:(2015/2/28)ライブラリファイルの中の I2C.ino をarduino側スケッチと同じフォルダに置かないとエラーが出るようです。
Must be put I2C.ino in the library file in the same folder as the arduino side sketch.


◆arduino側スケッチ

/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.

 This software may be distributed and modified under the terms of the GNU
 General Public License version 2 (GPL2) as published by the Free Software
 Foundation and appearing in the file GPL2.TXT included in the packaging of
 this file. Please note that GPL2 Section 2[b] requires that all works based
 on this software must also be made publicly available under the terms of
 the GPL2 ("Copyleft").

 Contact information
 -------------------

 Kristian Lauszus, TKJ Electronics
 Web      :  http://www.tkjelectronics.com
 e-mail   :  kristianl@tkjelectronics.com
 */

#include <Wire.h>
#include "Kalman.h" // Source: https://github.com/TKJElectronics/KalmanFilter

#define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf
#define led 13
#define button 12

Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;

/* IMU Data */
double accX, accY, accZ;
double gyroX, gyroY, gyroZ;
int16_t tempRaw;

double gyroXangle, gyroYangle, gyroZangle; // Angle calculate using the gyro only
double compAngleX, compAngleY; // Calculated angle using a complementary filter
double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter

uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data

float roll, pitch;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz

  i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
  i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
  i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
  i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
  while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
  while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode

  while (i2cRead(0x75, i2cData, 1));
  if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
    Serial.print(F("Error reading sensor"));
    while (1);
  }

  delay(100); // Wait for sensor to stabilize

  /* Set kalman and gyro starting angle */
  while (i2cRead(0x3B, i2cData, 6));
  accX = (i2cData[0] << 8) | i2cData[1];
  accY = (i2cData[2] << 8) | i2cData[3];
  accZ = (i2cData[4] << 8) | i2cData[5];

  // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
  // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
  // It is then converted from radians to degrees
#ifdef RESTRICT_PITCH // Eq. 25 and 26
  roll  = atan2(accY, accZ) * RAD_TO_DEG;
  pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
  roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
  pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif

  kalmanX.setAngle(roll); // Set starting angle
  kalmanY.setAngle(pitch);
  gyroXangle = roll;
  gyroYangle = pitch;
  compAngleX = roll;
  compAngleY = pitch;

  pinMode(led, OUTPUT);
  digitalWrite(led, HIGH);
  pinMode(button, INPUT_PULLUP);

  timer = micros();
}

void loop() {
  /* Update all the values */
  while (i2cRead(0x3B, i2cData, 14));
  accX = ((i2cData[0] << 8) | i2cData[1]);// + 2000.0;
  accY = ((i2cData[2] << 8) | i2cData[3]);// + 3319.84;
  accZ = ((i2cData[4] << 8) | i2cData[5]);// + 664.48;
  tempRaw = (i2cData[6] << 8) | i2cData[7];
  gyroX = (i2cData[8] << 8) | i2cData[9];
  gyroY = (i2cData[10] << 8) | i2cData[11];
  gyroZ = (i2cData[12] << 8) | i2cData[13];

  double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
  timer = micros();

  // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
  // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
  // It is then converted from radians to degrees
#ifdef RESTRICT_PITCH // Eq. 25 and 26
  roll  = atan2(accY, accZ) * RAD_TO_DEG;//+++++++++++++++++++++++
  pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
  roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
  pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif

  double gyroXrate = gyroX / 131.0; // Convert to deg/s
  double gyroYrate = gyroY / 131.0; // Convert to deg/s
  double gyroZrate = gyroZ / 131.0; // Convert to deg/s

#ifdef RESTRICT_PITCH
  // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
  if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
    kalmanX.setAngle(roll);
    compAngleX = roll;
    kalAngleX = roll;
    gyroXangle = roll;
  } else
    kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter

  if (abs(kalAngleX) > 90)
    gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
  kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
#else
  // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
  if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
    kalmanY.setAngle(pitch);
    compAngleY = pitch;
    kalAngleY = pitch;
    gyroYangle = pitch;
  } else
    kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter

  if (abs(kalAngleY) > 90)
    gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
  kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
#endif

  gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
  gyroYangle += gyroYrate * dt;
  gyroZangle += gyroZrate * dt;
  //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate
  //gyroYangle += kalmanY.getRate() * dt;

  compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
  compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;

  // Reset the gyro angle when it has drifted too much
  if (gyroXangle < -180 || gyroXangle > 180)
    gyroXangle = kalAngleX;
  if (gyroYangle < -180 || gyroYangle > 180)
    gyroYangle = kalAngleY;

  if(digitalRead(button) == LOW){
    gyroZangle = 0;
  }

  Serial.print("!");
  Serial.write(highByte(int(round(roll))));
  Serial.write(lowByte(int(round(roll))));
  Serial.write(highByte(int(round(kalAngleX))));
  Serial.write(lowByte(int(round(kalAngleX))));
  Serial.write(highByte(int(round(pitch))));
  Serial.write(lowByte(int(round(pitch))));
  Serial.write(highByte(int(round(kalAngleY))));
  Serial.write(lowByte(int(round(kalAngleY))));
  Serial.write(highByte(int(round(gyroZangle))));
  Serial.write(lowByte(int(round(gyroZangle))));
}


◆processing側スケッチ
書き方が悪いようで、
processing側でシリアルポートを開放しなくなることがあります。
使用には注意してください。
Like how to write a sketch is bad,
You may not be able to open the serial port in the processing side.
Please be careful to use.


import processing.serial.*;

int roll,pitch;
int kalAngleX,kalAngleY;
int gyroZangle;

Serial myPort;
 
void setup() {
  size(800,400,P3D);
  print(Serial.list());
  myPort = new Serial(this, "COM3", 115200); //set your COM port
}
 
void draw() {
  background(0);
  pushMatrix();
  translate(width/3, height/2);
  rotateY( radians( gyroZangle ));
  rotateZ( radians( pitch ));
  rotateX( radians( -roll ));
  box_vertex();
  popMatrix();
  translate(width/3*2, height/2);
  rotateY( radians( gyroZangle ));
  rotateZ( radians( kalAngleY ));
  rotateX( radians( -kalAngleX ));
  box_vertex();
}

void mousePressed(){
  myPort.clear();
  exit();
}

void serialEvent(Serial p){
  if(myPort.available() >= 11){
    if(myPort.read() == '!'){
      roll      = (byte(myPort.read()) * 256 + myPort.read());
      kalAngleX = (byte(myPort.read()) * 256 + myPort.read());
      pitch     = (byte(myPort.read()) * 256 + myPort.read());
      kalAngleY = (byte(myPort.read()) * 256 + myPort.read());
      gyroZangle = (byte(myPort.read()) * 256 + myPort.read());
      print(roll + ", ");
      print(kalAngleX + ", ");
      print(pitch + ", ");
      print(kalAngleY + ", ");
      println(gyroZangle);
      myPort.write(255);
    }
  }
}

void box_vertex(){
  scale(-0.5);
  beginShape(QUADS);
    fill(255, 0, 0);
    vertex(-100,  100,  100);
    vertex( 100,  100,  100);
    vertex( 100, -100,  100);
    vertex(-100, -100,  100);
    fill(0, 255, 0); 
    vertex( 100,  100,  100);
    vertex( 100,  100, -100);
    vertex( 100, -100, -100);
    vertex( 100, -100,  100);
    fill(0, 0, 255); 
    vertex( 100,  100, -100);
    vertex(-100,  100, -100);
    vertex(-100, -100, -100);
    vertex( 100, -100, -100);
    fill(255, 255, 0); 
    vertex(-100,  100, -100);
    vertex(-100,  100,  100);
    vertex(-100, -100,  100);
    vertex(-100, -100, -100);
    fill(0, 255, 255); 
    vertex(-100,  100, -100);
    vertex( 100,  100, -100);
    vertex( 100,  100,  100);
    vertex(-100,  100,  100);
    fill(255, 0, 255); 
    vertex(-100, -100, -100);
    vertex( 100, -100, -100);
    vertex( 100, -100,  100);
    vertex(-100, -100,  100);
  endShape();
}

箱の3Dデータはこちらからお借りしました。
Yasushi Noguchi Class

追記:秋月AE-ATmega基板を使ったarduino互換機+USBシリアル変換モジュールAE-UM232R(FT232RL)で上記 スケッチを実行したところ、115200bpsではスムーズな受信ができませんでした。互換機などを使っていておかしい時は9600bpsなど速度を落と してみてください。
レイテンシの影響のようです。
デバイスマネージャで該当するシリアルポートのプロパティ「ポートの設定」「詳細設定」にある「待ち時間」(レイテンシ)を16から1に変更すると、115200でも問題なく動きました。


D12のボタンは、ずれたジャイロZ軸のリセット。
スケッチ内のシリアルポートの番号は環境に合わせてください。

processingのスケッチは、描画中のウィンドウをクリックするか、✕ボタン、停止ボタンで終了してください。
私だけかもしれませんが、実行中に ctrl+Rで再実行しようとするとおかしくなり、ログオフ、再ログインまで治りません。
シリアルまわりのスケッチの書き方が悪いのでしょうが、なぜなんでしょう・・・




で適用前と適用後の比較動画。


センサーのZ軸を中心とした回転の動き(ヨーの動き)はジャイロのみで、両方のボックスにフィルターはかかっていません。
どちらも少しづつずれていきます。

Z軸ジャイロの補正はどうすればいいのでしょうか?
地磁気センサーとの組み合わせ?数学的な手法?難しい・・・

加速度センサーだけからの傾斜角の算出では、プルプルと震えています。
また、指で弾くと激しく動きますが、カルマンフィルターを通すとかなり安定しています。

ただ問題が。
ピッチ(飛行機で言う機首の上げ下げ)は360度問題ないのですが、ロールの数値が90度近くになるとボックスの動きがおかしくなります。
またグルングルン回した後はZ軸が大きくずれます。
センサーの使用用途によっては、この出力では不十分かもしれません。

解決法を求めて、その3へ続く・・・


環境:arduinoUNO、arduinoIDE1.0.5、Processing2.2.1(64)、win7(64)


7 件のコメント:

  1. はじめまして。「大工の茂吉」といいます。Arduino初心者です。
    よく読ませていただいてます。
    「倒立振子、ジャイロ、カルマンフィルタ」のワードに興味大です。

    返信削除
    返信
    1. コメントありがとうございます。
      ライブラリやセンサーの実験だけでなく、
      それらを使った応用にまで持って行きたいところですが、
      先は長そうです

      削除
  2. REありがとうございます。
    小生もGY-521を注文しました。参考にさせていただきたいと思います。
    ところで、CNCもやっています「合板製CNC」で検索すると見つかります。

    返信削除
    返信
    1. MDF製で設計し、組み立てたものの現在放置中です。
      プリント基板の作成や、アルミの削りだしとかすばらしい。
      再開するときには参考にさせていただきます。

      削除
  3. I can't found the function i2cWrite declaration.......

    返信削除
    返信
    1. I2C.ino in the unzipped library, it seems need to put in the folder there is a sketch of the arduino side.

      削除
  4. i2cRead failed......how can fix it?
    thanks

    返信削除