質問をすることでしか得られない、回答やアドバイスがある。

15分調べてもわからないことは、質問しよう!

ただいまの
回答率

90.46%

  • Arduino

    695questions

    Arduinoは、AVRマイコン、単純なI/O(入出力)ポートを備えた基板、C言語を元としたArduinoのプログラム言語と、それを実装した統合開発環境から構成されたシステムです。

MPU6050を用いて角度(yaw角)を算出したい

解決済

回答 3

投稿 編集

  • 評価
  • クリップ 0
  • VIEW 779

tannu

score 1

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

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

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

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

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

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

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

該当のソースコード

#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();
}

補足情報

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

*追記
角度算出式

  • 気になる質問をクリップする

    クリップした質問は、後からいつでもマイページで確認できます。

    またクリップした質問に回答があった際、通知やメールを受け取ることができます。

    クリップを取り消します

  • 良い質問の評価を上げる

    以下のような質問は評価を上げましょう

    • 質問内容が明確
    • 自分も答えを知りたい
    • 質問者以外のユーザにも役立つ

    評価が高い質問は、TOPページの「注目」タブのフィードに表示されやすくなります。

    質問の評価を上げたことを取り消します

  • 評価を下げられる数の上限に達しました

    評価を下げることができません

    • 1日5回まで評価を下げられます
    • 1日に1ユーザに対して2回まで評価を下げられます

    質問の評価を下げる

    teratailでは下記のような質問を「具体的に困っていることがない質問」、「サイトポリシーに違反する質問」と定義し、推奨していません。

    • プログラミングに関係のない質問
    • やってほしいことだけを記載した丸投げの質問
    • 問題・課題が含まれていない質問
    • 意図的に内容が抹消された質問
    • 広告と受け取られるような投稿

    評価が下がると、TOPページの「アクティブ」「注目」タブのフィードに表示されにくくなります。

    質問の評価を下げたことを取り消します

    この機能は開放されていません

    評価を下げる条件を満たしてません

    評価を下げる理由を選択してください

    詳細な説明はこちら

    上記に当てはまらず、質問内容が明確になっていない質問には「情報の追加・修正依頼」機能からコメントをしてください。

    質問の評価を下げる機能の利用条件

    この機能を利用するためには、以下の事項を行う必要があります。

質問への追記・修正、ベストアンサー選択の依頼

  • ikedas

    2019/03/26 21:17

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

    キャンセル

  • ozwk

    2019/03/26 22:57 編集

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

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

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

    キャンセル

  • ikedas

    2019/03/26 23:50 編集

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

    キャンセル

回答 3

checkベストアンサー

+4

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

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

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

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

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

投稿

  • 回答の評価を上げる

    以下のような回答は評価を上げましょう

    • 正しい回答
    • わかりやすい回答
    • ためになる回答

    評価が高い回答ほどページの上位に表示されます。

  • 回答の評価を下げる

    下記のような回答は推奨されていません。

    • 間違っている回答
    • 質問の回答になっていない投稿
    • スパムや攻撃的な表現を用いた投稿

    評価を下げる際はその理由を明確に伝え、適切な回答に修正してもらいましょう。

  • 2019/03/27 18:46

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

    キャンセル

+1

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

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

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

投稿

編集

  • 回答の評価を上げる

    以下のような回答は評価を上げましょう

    • 正しい回答
    • わかりやすい回答
    • ためになる回答

    評価が高い回答ほどページの上位に表示されます。

  • 回答の評価を下げる

    下記のような回答は推奨されていません。

    • 間違っている回答
    • 質問の回答になっていない投稿
    • スパムや攻撃的な表現を用いた投稿

    評価を下げる際はその理由を明確に伝え、適切な回答に修正してもらいましょう。

  • 2019/03/26 02:40

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

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

    キャンセル

-1

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

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

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

投稿

編集

  • 回答の評価を上げる

    以下のような回答は評価を上げましょう

    • 正しい回答
    • わかりやすい回答
    • ためになる回答

    評価が高い回答ほどページの上位に表示されます。

  • 回答の評価を下げる

    下記のような回答は推奨されていません。

    • 間違っている回答
    • 質問の回答になっていない投稿
    • スパムや攻撃的な表現を用いた投稿

    評価を下げる際はその理由を明確に伝え、適切な回答に修正してもらいましょう。

  • 2019/03/26 11:04

    角加速度?

    キャンセル

  • 2019/03/26 11:11

    ん?
    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.
    って書いてあったので。

    キャンセル

  • 2019/03/26 11:12

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

    キャンセル

  • 2019/03/26 11:15 編集

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

    キャンセル

  • 2019/03/26 11:26

    訂正と加筆しました。

    キャンセル

  • 2019/03/26 11:46 編集

    > yawも同じことで、

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

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

    キャンセル

  • 2019/03/26 12:47

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

    キャンセル

  • 2019/03/27 07:51

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

    キャンセル

15分調べてもわからないことは、teratailで質問しよう!

  • ただいまの回答率 90.46%
  • 質問をまとめることで、思考を整理して素早く解決
  • テンプレート機能で、簡単に質問をまとめられる

同じタグがついた質問を見る

  • Arduino

    695questions

    Arduinoは、AVRマイコン、単純なI/O(入出力)ポートを備えた基板、C言語を元としたArduinoのプログラム言語と、それを実装した統合開発環境から構成されたシステムです。