[Arduino] IMUから速度と角度を求める

最近ほんの少しArduinoを触る機会があったのですが, 少し悩んだのでまとめてみます。

今回用いたセンサーは[LSM6DS33]
(https://www.pololu.com/product/2736)。
ライブラリはこちら

IMUセンサーでは3軸の加速度と角速度を得ることができます。今回はこれらの値を用い, 速度と角度を求めます。

速度

加速度が分かっているので，$v=v_0 + at$ の式のように計算します。
しかし加速度には重力加速度も含まれているため, 影響を取り除く必要があります。

• 重力加速度を取り除く

下記サイトにかかれている通りにローパスフィルターを用い重力加速度の成分を分離します。
https://developer.android.com/reference/android/hardware/SensorEvent.html

コードにすると以下の通りです。

gravity[0] = alpha * gravity[0] + (1 - alpha) * event.values[0];
gravity[1] = alpha * gravity[1] + (1 - alpha) * event.values[1];
gravity[2] = alpha * gravity[2] + (1 - alpha) * event.values[2];

角度

カルマンフィルタと呼ばれる有名なフィルタを用います。
理論を理解するのは難しいですが, ライブラリを使うだけなら簡単です。
https://github.com/TKJElectronics/KalmanFilter

/*set*/
kalmanX.setAngle(roll); // Set starting angle
kalmanY.setAngle(pitch);

/*get*/
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);

コード

ここまでの内容を実装したコードを下記に示します。

#include <Wire.h>
#include <LSM6.h>
#include <Kalman.h>

//IMU
LSM6 imu;//生データ
double accX, accY, accZ;
double gyroX, gyroY, gyroZ;

//Filter
//Angle
Kalman kalmanX;
Kalman kalmanY;

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

uint32_t timer;

//Velocity
double velX, velY, velZ = 0;
double comVelX, comVelY, comVelZ = 0;

double comAccX,comAccY,comAccZ=0;  
double gravityX, gravityY, gravityZ=0;

const double alpha = 0.8;//lowpassfilter

//Log
char report[80];

void setup()
{
  Serial.begin(9600);
  Wire.begin();

  if (!imu.init())
  {
    Serial.println("Failed to detect and initialize IMU!");
    while (1);
  }
  imu.enableDefault();

  pinMode(sound_pin, OUTPUT);

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

  imu.read();
  
  accX = imu.a.x;
  accY = imu.a.y;
  accZ = imu.a.z;
  gyroX = imu.g.x;
  gyroY = imu.g.y;
  gyroZ = imu.g.z;

  double roll  = atan2(accY, accZ) * RAD_TO_DEG;
  double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;

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

  timer = micros();
}

void loop()
{
  imu.read();

  accX = imu.a.x;
  accY = imu.a.y;
  accZ = imu.a.z;
  gyroX = imu.g.x;
  gyroY = imu.g.y;
  gyroZ = imu.g.z;

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

  /*角度を求める kalman Filter*/
  double roll  = atan2(accY, accZ) * RAD_TO_DEG;
  double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
  
  double gyroXrate = gyroX / 131.0; // Convert to deg/s
  double gyroYrate = gyroY / 131.0; // Convert to deg/s

  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);

  gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
  gyroYangle += gyroYrate * 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;

  /*速度を求める*/
  // 重力加速度を求める
  gravityX = alpha * gravityX + (1 - alpha) * accX;
  gravityY = alpha * gravityY + (1 - alpha) * accY;
  gravityZ = alpha * gravityZ + (1 - alpha) * accZ;

  // 補正した加速度
  comAccX = accX - gravityX;
  comAccY = accY - gravityY;
  comAccZ = accZ - gravityZ;

  if(abs(comAccX) < 100)comAccX = 0;
  if(abs(comAccY) < 100)comAccY = 0;
  if(abs(comAccZ) < 100)comAccZ = 0;

  velX = velX + comAccX*dt;
  velY = velY + comAccY*dt;
  velZ = velZ + comAccZ*dt;

  /*print*/
  Serial.print(velX); Serial.print("\t");
  Serial.print(velY); Serial.print("\t");
  Serial.print(velZ); Serial.print("\t");
  Serial.print(comAccX); Serial.print("\t");
  Serial.print(comAccY); Serial.print("\t");
  Serial.print(comAccZ); Serial.print("\t");
    
  Serial.print(roll); Serial.print("\t");
  Serial.print(gyroXangle); Serial.print("\t");
  Serial.print(compAngleX); Serial.print("\t");
  Serial.print(kalAngleX); Serial.print("\t");

  Serial.print("\t");

  Serial.print(pitch); Serial.print("\t");
  Serial.print(gyroYangle); Serial.print("\t");
  Serial.print(compAngleY); Serial.print("\t");
  Serial.print(kalAngleY); Serial.print("\t");

  Serial.print("\r\n");

  delay(2);
}

補足

• row(y軸回り) と pitch(x軸回り)の角度しか求められない？

z軸回転を求めるのは難しいみたいです。

加速度センサーからの角度算出ではz軸（yaw軸）廻りの回転角度算出が困難です。z軸回転では加速度に変化がない（また遠心力でx、y軸に加速度が発生してしまう）ため角度計算おかしくなってしまいます。

加速度センサーから軸廻り角度への変換計算

参考

• https://theccontinuum.com/2012/09/24/arduino-imu-pitch-roll-from-accelerometer/
• https://garchiving.com/angle-from-acceleration/
• https://developer.android.com/reference/android/hardware/SensorEvent.html

随時必要があれば, 追記・修正します。