Help us understand the problem. What is going on with this article?

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

More than 1 year has passed since last update.

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

今回用いたセンサーはLSM6DS33
ライブラリはこちら

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軸に加速度が発生してしまう)ため角度計算おかしくなってしまいます。

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

参考

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

shidash
Why not register and get more from Qiita?
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
Comments
No comments
Sign up for free and join this conversation.
If you already have a Qiita account
Why do not you register as a user and use Qiita more conveniently?
You need to log in to use this function. Qiita can be used more conveniently after logging in.
You seem to be reading articles frequently this month. Qiita can be used more conveniently after logging in.
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
ユーザーは見つかりませんでした