MPU6050を用いて角度（yaw角）を算出したい

Question

六軸センサMPU6050（GY521)を用いて角度算出を行いたいです

上記センサを用いて角度検出を行い，Arduino UNOを用いてシリアルモニタ上に表示しようとしています．最終的には検出した値を3dモデルと同期させることが目的です．
角度はモニタの回転角（相対角度？）ではなく，地面に対しての角度（絶対角度？）を取得を行いたいです．

また，madgwickフィルタを用いた補正についても行いたいので教えて下さると助かります．

それらを満たすソースコードを教えて頂けるとありがたいです．

x軸周りとy軸周りの角度は取得できていますが，z軸周りが取得できません

プログラムを１から自作するのは能力不足のため行えませんでしたので，GitHubよりソースコードを拝借いたしました．
コードの内容は殆ど把握できていないので引っ張ってきたものをそのまま使用しています．
下記のソースコードを試したところ，x軸(pitch)とy軸(roll)まわりの角度は地面に対する角度で取得できているのですが，z軸まわりだけはシリアルモニタを開いた時点からの回転角でしか表示できません．（シリアルモニタを表示した時点が0度）

以下にソースコードを載せますが，引用しただけですので，これを修正していただく，
もしくは１から作り変えてくださると大変助かります．

該当のソースコード

#include <Wire.h>
#include <MPU6050.h>

MPU6050 mpu;

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

void setup() 
{
  Serial.begin(115200);

  Serial.println("Initialize MPU6050");

  while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
  {
    Serial.println("Could not find a valid MPU6050 sensor, check wiring!");
    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(1);

  // Check settings
  checkSettings();
}

void loop()
{
  // Read normalized values 
  Vector normAccel = mpu.readNormalizeAccel();
  Vector normGyro = mpu.readNormalizeGyro();

  // Calculate Pitch & Roll
  pitch = -(atan2(normAccel.XAxis, sqrt(normAccel.YAxis*normAccel.YAxis + normAccel.ZAxis*normAccel.ZAxis))*180.0)/M_PI;
  roll = (atan2(normAccel.YAxis, normAccel.ZAxis)*180.0)/M_PI;
  
  //Ignore the gyro if our angular velocity does not meet our threshold
  if (normGyro.ZAxis > 1 || normGyro.ZAxis < -1) {
    normGyro.ZAxis /= 100;
    yaw += normGyro.ZAxis;
  }

   //Keep our angle between 0-359 degrees
  if (yaw < 0)
    yaw += 360;
  else if (yaw > 359)
    yaw -= 360;
    
  // Output
  Serial.print("Pitch = ");
  Serial.print(pitch);
  Serial.print("\tRoll = ");
  Serial.print(roll);
  Serial.print("\tYaw = ");
  Serial.print(yaw);
  
  Serial.println();
  
  delay(10);
}

void checkSettings()
{
  Serial.println();
  
  Serial.print(" * Sleep Mode:        ");
  Serial.println(mpu.getSleepEnabled() ? "Enabled" : "Disabled");
  
  Serial.print(" * Clock Source:      ");
  switch(mpu.getClockSource())
  {
    case MPU6050_CLOCK_KEEP_RESET:     Serial.println("Stops the clock and keeps the timing generator in reset"); break;
    case MPU6050_CLOCK_EXTERNAL_19MHZ: Serial.println("PLL with external 19.2MHz reference"); break;
    case MPU6050_CLOCK_EXTERNAL_32KHZ: Serial.println("PLL with external 32.768kHz reference"); break;
    case MPU6050_CLOCK_PLL_ZGYRO:      Serial.println("PLL with Z axis gyroscope reference"); break;
    case MPU6050_CLOCK_PLL_YGYRO:      Serial.println("PLL with Y axis gyroscope reference"); break;
    case MPU6050_CLOCK_PLL_XGYRO:      Serial.println("PLL with X axis gyroscope reference"); break;
    case MPU6050_CLOCK_INTERNAL_8MHZ:  Serial.println("Internal 8MHz oscillator"); break;
  }
  
  Serial.print(" * Gyroscope:         ");
  switch(mpu.getScale())
  {
    case MPU6050_SCALE_2000DPS:        Serial.println("2000 dps"); break;
    case MPU6050_SCALE_1000DPS:        Serial.println("1000 dps"); break;
    case MPU6050_SCALE_500DPS:         Serial.println("500 dps"); break;
    case MPU6050_SCALE_250DPS:         Serial.println("250 dps"); break;
  } 
  
  Serial.print(" * Gyroscope offsets: ");
  Serial.print(mpu.getGyroOffsetX());
  Serial.print(" / ");
  Serial.print(mpu.getGyroOffsetY());![イメージ説明](d3ea6c978f9b7305cff2783c90dcc219.png)
  Serial.print(" / ");
  Serial.println(mpu.getGyroOffsetZ());
  
  Serial.println();
}

補足情報

当方プログラミングに関して初心者なので最低限の知識しか持ち合わせていないと思います．
ソースコード等最低限の情報さえご教授いただければ後は自分で勉強させていただきます．
よろしくお願いします．

＊追記

Comments

[ikedas]

プログラム中で、pitch、roll、yawがどういう風に計算されているかを、普通の数式を使って説明できますか。できるのなら質問本文に書いてもらえますか (atan2やsqrtの意味が分からなければマニュアル等で調べてください)。

[tannu]

ご指摘ありがとうございます．質問本文に少し説明を追加させて頂きました．
恥ずかしながら，プログラムの内容については理解が及んでいません．
アークタンジェントによって座標の偏角を求めていたり，平方根の計算をしているのかなという程度の認識でおりました．申し訳ありません．

[ikedas]

そこまで理解しているのなら、プログラムの中身を見て数式で説明してみましょうよ。Madgwick filterの解説ならもっと複雑な数式が出てきますよ。

[tannu]

お返事ありがとうございます
理解が足りていなく少しあいまいな部分がありますが，自分なりに分かる部分を説明させていただきます．
pitch角及びroll角は回転行列を使用して算出したのかと思います．
回転前の行列式(単位行列に重力成分を掛けたもの？）にそれぞれの軸周りの回転行列を掛け合わせたものと回転後の行列式（成分はセンサの加速度から取り出したもの）との恒等式について解いたものではないかと思います．
式については本文最後に追記します．(数式なのでここだと見にくいかと）

yaw角に関しては，センサのgyroの値が1より大きいか-1未満の場合，gyroの値を100で割って，その値を加算し続けているんでしょうか？（あまり自信がありませんが，，）

間違っていましたらご指摘の方お願いできればと思います．

[ozwk]

結局質問はyaw角を水平面に対する絶対角度で取りたいということですか?
だとしたら無理というかそんなものは無いですけど。

[tannu]

yaw角という使い方が正しいか分かりませんが，水平面に対して鉛直方向の軸周りの角度を取りたいということです．

[ikedas]

わたしも「そんなものは無い」とおもいます。回答にも書いたとおり、それぞれの軸がどれだけの角度ずれたかを、加速度センサだけで知ることはできません。
ただ、pitchやrollと同様に、加速度ベクトルのX-Y座標面への射影とX軸、Y軸との成す角や、加速度ベクトルがZ-X座標面やY-Z座標面と成す角を考えることはできるでしょう。そういったものに意味があるかどうかは、数式を作って考えてみては。

[ozwk]

> 水平面に対して鉛直方向の軸周りの角度を取りたい

では水平面に対して鉛直軸周りの回転で
例えば0度どこに置くんですか？

加速度センサーでも角速度センサでも定められないでしょ？

[ikedas]

それは、ほかの値は2軸以上の軸の間の関係から導かれてるんだから、yawだって同じことができるでしょう (わたしは「真の値は求められない」という立場ですが)。ジャイロセンサの初期値に意味はないし、センサのZ軸まわりの回転量を採用するのがおかしいのは当然として。加筆

Answer 1

MPU6050は６軸加速度(縦、横、奥行きの３方向の直線加速度と、縦軸、横軸、奥行き軸の３回転軸の回転加速度）のセンサーです。

回転加速度が全てゼロの状態では３方向の直線加速度から垂直軸（重力加速度の方向）に対する角度を得ることが可能です。

「地面に対しての角度（絶対角度？）」が何を指すのかよくわかりませんが、"絶対値"というのは何らかの基準からの乖離としてしか観測できません。

重力加速度の方向は３軸の直線加速度から得ることができますから、回転加速度が全てゼロの状態では絶対値が得られます。
６軸加速度センサーでは、これが限界です。

６軸に加えて、３軸の磁気センサーを備えた９軸センサ(例：MPU-9250)であれば、北の方向が判りますから、北の方向を基準にしたヨー角が計算できるはずです。

Comments

[tannu]

ご回答ありがとうございます．
大変よくわかりました．重力加速度の方向と同じ向きの軸の基準を作りたいので9軸センサの使用を検討してみます．
知識不足で拙い質問にもご丁寧にありがとうございました．

Answer 2

回答ではないですが、、
Arduinoは、シリアルモニタを開いたときに、CPUにリセットがかかるようになっています
ということで、正確には、
シリアルモニタを開いたときからの相対角度、ではなく、リセット時点からの相対角度、ということになりますね

これが具合悪い場合には、基板上からコンデンサを一つ外すと、リセットはかからなくなります

＃ざっとぐぐって見た感じだとZ軸は相対角度しか取れない感じですね

Comments

[tannu]

ご回答ありがとうございます

リセットの件初めて知りました，ありがとうございます．
リセットを行わずに角度を検出し続ける方法も検討してみます．
（恐らく長く使い続けていると角度誤差が大きくなる可能性もあるので難しそうですが，，，）

Answer 3

pitchとrollが意味しているものは、あくまでも静置時のセンサ自身を基準としたX軸・Y軸のまわりでの角加速度角度です。ですから初期の角度はありません (あるように思えるのは、センサを水平面に設置して作業しているための錯覚です)。yawも同じことで、初期の角度はありません。

「鉛直方向に対する角度を計っているではないか」と思うかもしれませんが、加速度センサについては、センサが運動しているときはそうではありません。等価原理により、センサの運動による加速度と重力加速度とを区別できないからです。

MadgwickフィルタについてはこちらのQiitaの記事が内容も正確なようです。四元数 (クォータニオン) がでてきますが、これは論理的には回転行列と同じことです (解説は適当にさがしてください。四元数のこのような応用は当初から知られていましたが、最近はUnityで採用していたりと、はやっていますね)。

Comments

[ozwk]

角加速度?

[ikedas]

ん?
https://www.electronicwings.com/arduino/mpu6050-interfacing-with-arduino-uno
> mpu.readNormalizeAccel()
> This function gives the values of the acceleration in the X, Y and Z directions in g units.
って書いてあったので。

[ikedas]

あっほんとだ。訂正します。

[ozwk]

もっと言えばある時刻のpitchとrollに関してはその時刻の加速度から算出していて
積分とかしてるわけじゃないので初期状態が水平かどうか関係なく求まります

[ikedas]

訂正と加筆しました。

[ozwk]

> yawも同じことで、

とありますが
yawの初期値を定められない理由とpitch,rollの初期値が定められない理由は全く別ですよね。

pitch,rollはセンサーが静止していればセンサの値から一意に定まりますが(ジンバルロック除く)
yawは定まらないので。

[ikedas]

静止しているという条件をとっぱらうと、3つの軸に違いはないです。わたしはこれ以上うまい説明ができませんねえ。

[ikedas]

>ozwkさん
Z軸方向が特別だと考えると、センサが90度 (以上) 傾いた場合にその特別な軸が入れ替わることになりますよね。
ジャイロセンサを積んでいるということは、並進運動に準ずる緩やかな方向転換だけでなく、回転運動など激しい方向転換も用途として想定しているはずです。質問に挙げられたコードはかなり虫のいい想定をしていると言えます。
思うに、固定した直交座標の上で考えているのが問題なのかと。