六軸センサーから姿勢角度算出

姿勢制御

ドローンの姿勢を制御し水平に保つために、ドローンの姿勢を正確に検出する必要があります。
そのために、姿勢角を求めるセンサーを利用し、姿勢角を計算します。
ここで、MPU6050と呼ばれる６軸センサを使用しました。

MPU-6050では、ドローンのこれらの状態を数値で読み取ることができます。

ドローンの加速度（x方向、y方向、z方向）
ドローンの回転（ロール、ピッチ、ヨー）

電路図

姿勢算出

理論上、ジャイロのみまたは加速度センサーのみでも姿勢算出はできるのですが。

ジャイロセンサにはジャイロセンサの値を積分することで角度算出はできます。しかし、センサーをずっと静止させていても，ノイズや出力のズレやなどの影響で、出力値が外れていきます。

test_gryo_.ino

float pitch = pitch + YAxis * timeStep;
float roll  = roll  + XAxis * timeStep;
float yaw   = yaw   + ZAxis * timeStep;

加速度センサーには静止している状態で、時間が経っても出力のズレがないですが。yawの角度算出が困難であり、センサー自体に動いている場合に正確な角度（pitch、roll）も算出できません。

test_gryo_.ino

  float pitch = -(atan2(XAxis, sqrt(YAxis * YAxis + ZAxis * ZAxis)) * 180.0) / M_PI;
  float roll = (atan2(YAxis, ZAxis) * 180.0) / M_PI;

ジャイロからでも加速度センサーからでも姿勢算出はできるのですが、正確な数値とは言えません。ここで、ジャイロセンサーと加速度センサーを連動させる(センサフュージョン)ことで、補正を行ってより正確な姿勢角度を算出できます。

補正方法

ここで、Madgwickフィルタを利用して、より正確な姿勢角度を算出できます。

Madgwickフィルタは相補フィルターの一種、計算処理がほかのフィルタより軽いので，マイコンに対して優しいです．内容はちょっと難いので、興味があれば、このサイトを参考してください。

内容は分からなくでも大丈夫です、今回はMadgwickフィルタを使ってドリフト補正を行います。

ソースコード

ここで参考したライブラリのリンク：

test_Madgwick_.ino

//MPU6050 Madgwick filter Test
#include <Wire.h>
#include <MPU6050.h>
#include <MadgwickAHRS.h>
Madgwick MadgwickFilter;
MPU6050 mpu;

// Timers
unsigned long timer = 0;
float timeStep = 0.01;

// Pitch, Roll and Yaw values
float pitch = 0;
float roll = 0;
float yaw = 0;

void setup() 
{
  Serial.begin(115200);
  MadgwickFilter.begin(200); //100Hz
  // Initialize MPU6050
  while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
  {
    Serial.println("Could not find MPU6050 sensor");
    delay(500);
  }
  
  // Calibrate gyroscope. The calibration must be at rest.
  // If you don't want calibrate, comment this line.
  mpu.calibrateGyro();

  // Set threshold sensivty. Default 3.
  // If you don't want use threshold, comment this line or set 0.
  mpu.setThreshold(3);
}

void loop()
{
  timer = millis();

  // Read normalized values
  Vector acc = mpu.readNormalizeAccel();
  Vector gyr = mpu.readNormalizeGyro();

  // Calculate Pitch, Roll and Yaw
  pitch = pitch + gyr.YAxis * timeStep;
  roll = roll + gyr.XAxis * timeStep;
  yaw = yaw + gyr.ZAxis * timeStep;
  MadgwickFilter.updateIMU(gyr.XAxis, gyr.YAxis, gyr.ZAxis, acc.XAxis, acc.YAxis, acc.ZAxis);
  // Output raw
  Serial.print("補正前 Pitch = ");
  Serial.print(pitch);
  Serial.print(" Roll = ");
  Serial.print(roll);  
  Serial.print(" Yaw = ");
  Serial.println(yaw);
  
  Serial.print("補正後 Pitch = ");
  Serial.print(MadgwickFilter.getPitch());
  Serial.print(" Roll = ");
  Serial.print(MadgwickFilter.getRoll());
  Serial.print(" Yaw = ");
  Serial.println(MadgwickFilter.getYaw());

  // Wait to full timeStep period
  delay((timeStep * 1000) - (millis() - timer));
}